/*
 ******************************************************************************
 * Title: ConcaveDistance
 * Project: ColDetection Library
 ******************************************************************************
 * File: ConcaveDistance.cpp
 * Author: Romain Rodriguez <Romain.Rodriguez@inrialpes.fr>
 * Created: 2003-01-15
 * Last update: 2003-05-20
 ******************************************************************************
 * Description: 
 * Compute distance between non-convex polyhedras
 ******************************************************************************
 * Copyright (c) 2003, INRIA CYBERMOVE
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public License
 * as published by the Free Software Foundation; either version 2.1
 * of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 ******************************************************************************
 */

#include "vtkConcaveDistance.h"
#include "vtkVector3f.h"
#include "vtkObjectFactory.h"

vtkCxxRevisionMacro(vtkConcaveDistance, "$Revision: 0.1 $");
vtkStandardNewMacro(vtkConcaveDistance);
vtkConcaveDistance::vtkConcaveDistance(vtkDObject* obj1, vtkDObject* obj2)
{
  // Vertices
  _ptsA = obj1 -> mesh -> vertices();
  _ptsB = obj2 -> mesh -> vertices();

  // Facets
  _triA = obj1 -> mesh -> facets();
  _triB = obj2 -> mesh -> facets();
  
  // Transformation Matrix
  transA = obj1 -> trans;
  transB = obj2 -> trans;

  // Bounding Box Binary Tree
  bbARoot = obj1 -> mesh -> getBoundingBoxTree();
  bbBRoot = obj2 -> mesh -> getBoundingBoxTree();

  // Vector of Points that minimize the distance
  tabVectorRes = new vtkVector3f[2];

  // Vector of neighbors
  computeTriangleRing();
}

vtkConcaveDistance::~vtkConcaveDistance(void)
{
  delete[] triangleRing;
  delete[] tabVectorRes;
}

double vtkConcaveDistance::getDistance(void)
{
  distanceMin = MAX_REAL;

  vtkVector3f::setXYZ(tabVectorRes[0], 0, 0, 0);
  vtkVector3f::setXYZ(tabVectorRes[1], 0, 0, 0);

  bbAMinVector.clear();
  bbBMinVector.clear();

  tabCoordVectorA.clear();
  tabCoordVectorB.clear();

  recurSearchMin(bbARoot, bbBRoot);

  // Explore bbAMinvector & bbBMinvector
  return searchCoupleMin();
}

void vtkConcaveDistance::recurSearchMin(BoundingBoxTree* bbA, 
				     BoundingBoxTree* bbB)
{
  double d  = distanceBBoxAsSphere(bbA, bbB);
  
  if (d > distanceMin) return;
  
  if (bbA -> node -> type == TREE) {
    if (bbB -> node -> type == TREE) { // bbA TREE & bbB TREE
      if (bbA == bbARoot) {
	if (bbB == bbBRoot) {
	  // bbA & bbB roots : explore the biggest volume
	  if ((bbA -> node -> box -> volume) < (bbB -> node -> box -> volume))
	    exploreB(bbA, bbB);
	  else 
	    exploreA(bbA, bbB);
	} else { // bbA root & bbB non root : explore B
	  exploreB(bbA, bbB);
	}
      } else { // bbA non root & bbB root : explore A
	exploreA(bbA, bbB);
      }
    } else { // bbA TREE & bbB LEAF
      exploreA(bbA, bbB);
    }
  } else {
    if (bbB -> node -> type == TREE) { // bbA LEAF & bbB TREE
      exploreB(bbA, bbB);
    } else { // bbA LEAF & bbB LEAF
      distanceMin = d;
      bbAMinVector.push_back(bbA);
      bbBMinVector.push_back(bbB);
    }
  }
}

void vtkConcaveDistance::exploreA(BoundingBoxTree* bbA,
			       BoundingBoxTree* bbB)
{
  if (distanceBBoxAsSphere(bbA -> lson, bbBRoot) <
      distanceBBoxAsSphere(bbA -> rson, bbBRoot)) {
    // We explore before the A's left son
    recurSearchMin(bbA -> lson, bbB);
    recurSearchMin(bbA -> rson, bbB);
  } else { // We explore before the A's right son
    recurSearchMin(bbA -> rson, bbB);
    recurSearchMin(bbA -> lson, bbB);
  }
}

void vtkConcaveDistance::exploreB(BoundingBoxTree* bbA,
			       BoundingBoxTree* bbB)
{
  if (distanceBBoxAsSphere(bbARoot, bbB -> lson) <
      distanceBBoxAsSphere(bbARoot, bbB -> rson)) {
    // We explore before the B's left son
    recurSearchMin(bbA, bbB -> lson);
    recurSearchMin(bbA, bbB -> rson);
  } else { // We explore before the B's right son
    recurSearchMin(bbA, bbB -> rson);
    recurSearchMin(bbA, bbB -> lson);
  }
}

double vtkConcaveDistance::distanceBBoxAsSphere(BoundingBoxTree* bbA,
					     BoundingBoxTree* bbB)
{
  vtkVector3f cA, cB;
  
  matrix4fXformvtkVector3f(cA, * transA, bbA -> node -> box -> center);
  matrix4fXformvtkVector3f(cB, * transB, bbB -> node -> box -> center);
  
  double d = cA.distance(cB) - 
    (bbA -> node -> box -> diagonal).norm() -
    (bbB -> node -> box -> diagonal).norm();
  
  if (d < 0) return 0;
  return d;
}

double vtkConcaveDistance::distanceTriangleTriangle(int indA,
						 int indB)
{
  vtkVector3f* dPtsA = new vtkVector3f[3];
  vtkVector3f::copy(dPtsA[0], _ptsA[(_triA[indA]).a]);
  vtkVector3f::copy(dPtsA[1], _ptsA[(_triA[indA]).b]);
  vtkVector3f::copy(dPtsA[2], _ptsA[(_triA[indA]).c]);
  
  vtkVector3f* dPtsB = new vtkVector3f[3];
  vtkVector3f::copy(dPtsB[0], _ptsB[(_triB[indB]).a]);
  vtkVector3f::copy(dPtsB[1], _ptsB[(_triB[indB]).b]);
  vtkVector3f::copy(dPtsB[2], _ptsB[(_triB[indB]).c]);
  
  vtkEnGJK distTriangleTriangle(3, dPtsA, triangleRing, transA,
			     3, dPtsB, triangleRing, transB);

  double d = distTriangleTriangle.getDistance();

  tabCoordVectorA.push_back(distTriangleTriangle.getTabVectorRes()[0]);
  tabCoordVectorB.push_back(distTriangleTriangle.getTabVectorRes()[1]);
 
  delete[] dPtsA;
  delete[] dPtsB;

  return d;
}

void vtkConcaveDistance::computeTriangleRing(void)
{
  triangleRing = new std::vector<unsigned>[3];
 
  triangleRing[0].push_back(1);
  triangleRing[0].push_back(2);
  
  triangleRing[1].push_back(0);
  triangleRing[1].push_back(2);

  triangleRing[2].push_back(0);
  triangleRing[2].push_back(1);
}

double vtkConcaveDistance::searchCoupleMin(void)
{
  double min = MAX_REAL;
  unsigned i;
  for (i = 0 ; i < bbAMinVector.size() ; i++) {

    double d = distanceTriangleTriangle(bbAMinVector[i] -> node -> id,
					bbBMinVector[i] -> node -> id);
    if (d < min) {
      min = d;
      tabVectorRes[0] = tabCoordVectorA[i];
      tabVectorRes[1] = tabCoordVectorB[i];
    }
  }
  return min;
}

/* ConcaveDistance.cpp ends here */