/*
 ******************************************************************************
 * Title: EnGJK
 * Project: ColDetection Library
 ******************************************************************************
 * File: EnGJK.cpp
 * Author: Romain Rodriguez <Romain.Rodriguez@inrialpes.fr>
 * Created: 2003-01-08
 * Last update: 2003-05-20
 ******************************************************************************
 * Description: 
 * Enhanced GJK
 ******************************************************************************
 * 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 "vtkEnGJK.h"
#include "vtkVector3f.h"
vtkCxxRevisionMacro(vtkEnGJK, "$Revision: 0.1 $");

vtkEnGJK::vtkEnGJK(vtkDObject* obj1,
	     vtkDObject* obj2)
{
  init(obj1 -> mesh -> nbVertices,
       obj1 -> mesh -> vertices(),
       obj1 -> mesh -> neighborsOfVertices(),
       obj1 -> trans,
       obj2 -> mesh -> nbVertices,
       obj2 -> mesh -> vertices(),
       obj2 -> mesh -> neighborsOfVertices(),
       obj2 -> trans);
}

vtkEnGJK::vtkEnGJK(unsigned nptsA,
	     vtkVector3f* verticesA,
	     const std::vector<unsigned>* neighborsOfVerticesA,
	     double* transA[16],
	     unsigned nptsB,
	     vtkVector3f* verticesB,
	     const std::vector<unsigned>* neighborsOfVerticesB,
	     double* transB[16])
{
  init(nptsA,
       verticesA,
       neighborsOfVerticesA,
       transA,
       nptsB,
       verticesB,
       neighborsOfVerticesB,
       transB);
}

vtkEnGJK::~vtkEnGJK(void)
{
  delete memory;
  delete[] tabVectorRes;
}

void vtkEnGJK::init(unsigned nptsA,
		 vtkVector3f* verticesA,
		 const std::vector<unsigned>* neighborsOfVerticesA,
		 double* transA[16],
		 unsigned nptsB,
		 vtkVector3f* verticesB,
		 const std::vector<unsigned>* neighborsOfVerticesB,
		 double* transB[16])
{
  gjkDist = ZERO;
  
  gjkPtsA = verticesA;
  gjkPtsB = verticesB;
  
  this -> nptsA = nptsA;
  this -> nptsB = nptsB;
  
  neighborA = neighborsOfVerticesA;
  neighborB = neighborsOfVerticesB;
  
  this -> transA = transA;
  this -> transB = transB;

  memory = new distMemory;
  memory -> nptsSimplexA = 1;
  memory -> nptsSimplexB = 1;
  memory -> indexSimplexA[0] = 0;
  memory -> indexSimplexB[0] = 0;

  vtkVector3f::copy(memory -> wptA, gjkPtsA[0]);
  vtkVector3f::copy(memory -> wptB, gjkPtsB[0]);

  tabVectorRes = new vtkVector3f[2];
}

double vtkEnGJK::getDistance(void)
{
  updateTabVectorRes();


  enGJKDistance();

  return sqrt(gjkDist);
}

void vtkEnGJK::updateTabVectorRes(void)
{
  matrix4fXformvtkVector3f(tabVectorRes[0], * transA, memory -> wptA);
  matrix4fXformvtkVector3f(tabVectorRes[1], * transB, memory -> wptB);
}

bool vtkEnGJK::enGJKDistance(void) 
{
  bool projectOnB = true;
  int ia, ib;
  int k;
  double upperBound, lowerBound;
  vtkVector3f wptAinB, wptBinA, tcsoWpt;
  vtkVector3f wVector, wVectorMin;
  int sweepPointA = 0;
  int sweepPointB = 0;

  // Inverse the transformation matrix
  Matrix4f invTransA, invTransB;
  matrix4fInverse(invTransA, * transA);
  matrix4fInverse(invTransB, * transB);

  // begin
  while (true) {
    // normal case. start ping-pong 

    // ping
    if (projectOnB) {

      
      matrix4fXformvtkVector3f(wptAinB, * transA, memory -> wptA);
      
      // solve the minimum distance between the witness 
      // point from A and the object B. store in std::vector tcsoWpt
      // this vector will be called the sweeping direction
      ia = projectPointOnConvex(wptAinB,
				gjkPtsB,
				neighborB,
				memory -> indexSimplexB,
				& (memory -> nptsSimplexB),
				tcsoWpt,
				& upperBound,
				transB);
      // if collision return
      if (ia == COLLISION) {
	updateTabVectorRes();
	gjkDist = ZERO;
	return true;
      }

      // else get the witness point in B
      matrix4fXformvtkVector3f(memory -> wptB, invTransB, tcsoWpt + wptAinB);
      
      // reverse the vector sweeping direction
      tcsoWpt *= - ONE;

      // every things take place in A 
      // transform the reversed sweeping vector to the A frame
      vtkVector3f::copy(wVector, tcsoWpt);
	
      sweepPointA = convexSupportFunction(gjkPtsA,
					  neighborA,
					  memory -> indexSimplexA[0],
					  wVector,
					  transA);
      
      // expression of the sweeping point of A in B
      matrix4fXformvtkVector3f(wVectorMin, * transA, gjkPtsA[sweepPointA]);
      
      k = memory -> indexSimplexB[0];
      
      // objectB[k] is also on the support plane defined with tcsoWpt
      vtkVector3f tmpB;
      matrix4fXformvtkVector3f(tmpB, * transB, gjkPtsB[k]);
      wVector = tmpB - wVectorMin;

      lowerBound = - (wVector * tcsoWpt);

      // exit with the lower bound as result the distance is too 
      // big to be of interest
      if (lowerBound > INTERACTION_DISTANCE) {
	updateTabVectorRes();
	gjkDist = lowerBound;
	return false;
      }

      if ((upperBound - lowerBound) < (RELATIVE_PRECISION * upperBound)) {
	// we got the projection with a precision good enought
	updateTabVectorRes();
	gjkDist = (upperBound + lowerBound) * HALF;
	return false;
      } else {
	// improve the precision by projecting from B to A
	projectOnB = false;
      }
    } else {

      // pong
      // project on A
      
      // next computation take place in A
      matrix4fXformvtkVector3f(wptBinA, * transB, memory -> wptB);

      // project on A
      ib = projectPointOnConvex(wptBinA,
				gjkPtsA,
				neighborA, 
				memory -> indexSimplexA,
				& (memory -> nptsSimplexA),
				tcsoWpt,
				& upperBound,
				transA);
      
      if (ib == COLLISION) {
	updateTabVectorRes();
	gjkDist = ZERO ;
	return true;
      }

      // else get the witness point in A
      matrix4fXformvtkVector3f(memory -> wptA, invTransA, tcsoWpt + wptBinA);

      tcsoWpt *= - ONE;
      
      // every things take place in B
      vtkVector3f::copy(wVector, tcsoWpt);

      sweepPointB = convexSupportFunction(gjkPtsB,
					  neighborB,
					  memory -> indexSimplexB[0],
					  wVector,
					  transB);
      
      // expression of the sweeping point of B in A
      matrix4fXformvtkVector3f(wVectorMin, * transB, gjkPtsB[sweepPointB]);
      
      k = memory -> indexSimplexA[0];
	
      // objectA[k] is also on the support plane defined with tcsoWpt
      vtkVector3f tmpA;
      matrix4fXformvtkVector3f(tmpA, * transA, gjkPtsA[k]);
      wVector = tmpA - wVectorMin;

      lowerBound = - (wVector * tcsoWpt);
      
      // exit with the lower bound as result the distance is too
      // big to be of interest
      if (lowerBound > INTERACTION_DISTANCE) {
	updateTabVectorRes();
	gjkDist = lowerBound;
	return false;
      }

      if (upperBound - lowerBound < RELATIVE_PRECISION * upperBound) {
	// we got the projection with a precision good enought
	updateTabVectorRes();
	gjkDist = (upperBound + lowerBound) * HALF;
	return false;
      } else {
	// improve the precision by projecting from A to B
	projectOnB = true;
      }
    }
  }
}

int vtkEnGJK::convexSupportFunction(const vtkVector3f pts[],
				 const std::vector<unsigned>* topo,
				 int start, 
				 const vtkVector3f& direction,
				 double* trans[16])
{
  int p = -1;
  int minp = start;
  unsigned neighbour;
  double minv, thisv;
  
  // this is the starting point. we want to find the sweeping point
  // for a given sweeping direction. we use the hill-climbing method 
  // proposed by cameron. we need a reference point

  vtkVector3f tmp;
  matrix4fXformvtkVector3f(tmp, * trans, pts[minp]);
  minv = tmp * direction;

  while (p != minp) {
    // get the neighbourhood of the reference point
    p = minp;
    
    const std::vector<unsigned>& neighborhood = topo[p];
    
    for (unsigned i = 0 ; i < neighborhood.size() ; i++) {
      // for each neighbor, check the dot product with direction
      neighbour = neighborhood[i];
      
      vtkVector3f tmpLoop;
      matrix4fXformvtkVector3f(tmpLoop, * trans, pts[neighbour]);
      thisv = tmpLoop * direction;
      
      // if the dot product is good, store the reference point and
      // reiterate. stop when the reference point does not change
      if (thisv < minv && minv != thisv) {
	minv = thisv;
	minp = neighbour;
      }
    }
  }
  
  return p;
}

int vtkEnGJK::projectOriginOnSimplex(vtkVector3f& simplexWpt,
				  vtkVector3f simplex[4],
				  int simplexIndices[4],
				  int* nbSimplexPoint)
{
  vtkVector3f v01, v12, v02, a01, a12, a20, vn;
  double lengthSquare, num;
  int geometricHash;
  double normVnSquare, scale ;
  int i, j, k;
  double nV12, nV02, nV01, d;
  vtkVector3f workingSimplex[4];
  int workingIndices[4];
  double upper, lower;
  int result;

  switch (* nbSimplexPoint) {
  case 1:
    /* vertex to vertex */
    vtkVector3f::copy(simplexWpt, simplex[0]);
    return OKAY;
    
  case 2:
    /* vertex to edge   */
    v01 = simplex[1] - simplex[0];
    
    /* the minimum is on one vertex */
    if ((v01 * simplex[1]) <= ZERO) {
      * nbSimplexPoint = 1;
      vtkVector3f::copy(simplexWpt, simplex[1]);
      vtkVector3f::copy(simplex[0], simplex[1]);
      simplexIndices[0] = simplexIndices[1];
      return OKAY;
    }
			
    /* the minimum is on the other vertex */
    if ((v01 * simplex[0]) >= ZERO) {
      *nbSimplexPoint = 1;
      vtkVector3f::copy(simplexWpt, simplex[0]);
      return OKAY;
    }
    
    /* the minimum is on the edge */
    
    /* compute the lenght square of the edge */
			
    lengthSquare = v01 * v01;
    
    if (lengthSquare < MIN_EDGE_LENTH_SQUARE) {
      /* we know the absolute error to have in order
	 to stay within the relative
	 error is bigger than 
	 PROJECTION-RELATIVE-PRECISION x TOO_NEAR  */
					
      /* we use the middle of this small segment as the nearest point */
      simplexWpt = simplex[0] + simplex[1];
      simplexWpt *= HALF;
      return EDGE_APPROXIMATION;
    } else {
      /* see the formula on function definition */
      num = v01 * simplex[0];
      num /= lengthSquare;
      v01 *= num;
      simplexWpt = simplex[0] - v01;
      return OKAY;
    }
    
  case 3:
    /* vertex triangle */
    
    /* compute the vector vn normal to the plane */
    
    v01 = simplex[1] - simplex[0];
    v02 = simplex[2] - simplex[0];
    vn = v01 ^ v02;
    
    /* to test where is the center */ 
    a01 = simplex[0] ^ simplex[1];
    a12 = simplex[1] ^ simplex[2];
    a20 = simplex[2] ^ simplex[0];
    
    geometricHash = 0;
			
    if ((vn * a01) < 0) geometricHash = 1;
    if ((vn * a12) < 0) geometricHash += 2;
    if ((vn * a20) < 0) geometricHash += 4;
			
    switch (geometricHash) {

    case 0:

      /* inside the triangle */
      normVnSquare = vn * vn;
      if (normVnSquare >= MIN_TRIANGLE_SURFACE) {
	/* standard case the minimum is on the face */
	scale = (vn * simplex[0]) / normVnSquare;
	vtkVector3f::copy(simplexWpt, vn);
	simplexWpt *= scale;
	return OKAY;
      } else {
	/* degenerate case */
	/* check the largest edge */
	v12 = simplex[2] - simplex[1];
	nV12 = v12 * v12;
	nV02 = v02 * v02;
	nV01 = v01 * v01;
	
	if (nV12 > nV02) {
	  if (nV12 > nV01) {
	    
	    /* v12 is the bigest vector */
	    vtkVector3f::copy(simplex[0], simplex[2]);
	    simplexIndices[0] = simplexIndices[2];
	  }
	  /* else */
	  /* v01 is the bigest vector */
	} else {
	  if (nV02 > nV01) {
	    /* v02 is the bigest vector */
	    
	    vtkVector3f::copy(simplex[1], simplex[2]);
	    simplexIndices[1] = simplexIndices[2];
	  }
	  /* else */
	  /* v01 is the bigest vector */
	}
	* nbSimplexPoint = 2;
      }
      break;
      
    case 1: /* edge 01 is canditate */
      
      * nbSimplexPoint = 2;
      break;
      
    case 2: /* edge 12 is a canditate */
      
      vtkVector3f::copy(simplex[0], simplex[2]);
      simplexIndices[0] = simplexIndices[2];
      * nbSimplexPoint = 2;
      break;
      
    case 4: /* edge 02 is a canditate */
      
      vtkVector3f::copy(simplex[1], simplex[2]);
      simplexIndices[1] = simplexIndices[2];
      * nbSimplexPoint = 2;
      break;
      
    case 3: /* vertex 1 is candidate */
      
      d = simplex[1] * simplex[1];
      /* check edge 01 */
      if ((simplex[0] * simplex[1]) < d) {
	* nbSimplexPoint = 2;
	break;
      } else {
	/* check edge 21 */
	if ((simplex[2] * simplex[1]) < d) {
	  vtkVector3f::copy(simplex[0], simplex[2]);
	  simplexIndices[0] = simplexIndices[2];
	  * nbSimplexPoint = 2;
	  break;
	} else {
	  /* vertex 1 is the minimum */
	  *nbSimplexPoint = 1;
	  vtkVector3f::copy(simplexWpt, simplex[1]);
	  simplexIndices[0] = simplexIndices[1];
	  return OKAY;
	}
      }
      
    case 6: /* vertex 2 is candidate */
      
      d = (simplex[2] * simplex[2]);
      /* check edge 02 */
      if ((simplex[0] * simplex[2]) < d) {
	vtkVector3f::copy(simplex[1], simplex[2]);
	simplexIndices[1] = simplexIndices[2];
	* nbSimplexPoint = 2;
	break;
      } else {
	/* check edge 21 */
	if ((simplex[1] * simplex[2]) < d) {
	  vtkVector3f::copy(simplex[0], simplex[2]);
	  simplexIndices[0] = simplexIndices[2];
	  * nbSimplexPoint = 2;
	  break;
	} else {
	  /* vertex 2 is the minimum */
	  *nbSimplexPoint = 1;
	  vtkVector3f::copy(simplexWpt, simplex[2]);
	  simplexIndices[0] = simplexIndices[2];
	  return OKAY;
	}
      }
      
      case 5: /* vertex 0 is candidate */
	
	d = simplex[0] * simplex[0];
	/* check edge 02 */
	if ((simplex[0] * simplex[2]) < d) {
	  vtkVector3f::copy(simplex[1], simplex[2]);
	  simplexIndices[1] = simplexIndices[2];
	  * nbSimplexPoint = 2;
	  break;
	} else {
	  /* check edge 01 */
	  if ((simplex[0] * simplex[1]) < d) {
	    * nbSimplexPoint = 2;
	    break;
	  } else {
	    /* vertex 0 is the minimum */
	    * nbSimplexPoint = 1;
	    vtkVector3f::copy(simplexWpt, simplex[0]);
	    return OKAY;
	  }
	}
    }
    
    return projectOriginOnSimplex(simplexWpt,
				  simplex,
				  simplexIndices,
				  nbSimplexPoint);
    
  case 4: /* vertex tetrahedron */
    
    /* select one vertex */
    for (i = 0 ; i < 4 ; i++) {
      /* create the simplex corresponding to the face which do not contains
	 this vertex */
      for (j = 0 ; j < 3 ; j++) {
	k = (i + 1 + j) % 4;
	vtkVector3f::copy(workingSimplex[j], simplex[k]);
	workingIndices[j] = simplexIndices[k];
      }
      /* call  projectOriginOnSimplex on this simplex */
      * nbSimplexPoint = 3;
      result = projectOriginOnSimplex(simplexWpt,
				      workingSimplex,
				      workingIndices,
				      nbSimplexPoint);
      
      /* check if the selected vertex is further from the simplexWpt */
      upper = simplexWpt * simplexWpt;
      lower = simplexWpt * simplex[i];
      
      if (upper - lower <= ZERO) {
	/* if yes exit with this simplex */
	for (j = 0 ; j < * nbSimplexPoint ; j++) {
	  vtkVector3f::copy(simplex[j], workingSimplex[j]);
	  simplexIndices[j] = workingIndices[j];
	}
	return result;
      }
    }
    /* if we are here all vertices are necessary to compute the distance */
    return COLLISION;
    
  default:
    SPY("Error in EnGJK::projectOriginOnSimplex()\n");
    return COLLISION;
  }
}

int vtkEnGJK::projectPointOnConvex(const vtkVector3f& point,
				const vtkVector3f object[], 
				const std::vector<unsigned>* neighbor,
				int simplexIndex[], 
				int* nbSimplexPoint,
				vtkVector3f& tcsoWpt,
				double* upperBound,
				double* trans[16])
{
  int k, i;
  double lowerBound;
  int sweepIndex = 0;
  vtkVector3f simplex[4];
  vtkVector3f workV;

  for (i = 0 ; i < * nbSimplexPoint ; i++) {
    k = simplexIndex[i];

    vtkVector3f tmp;
    matrix4fXformvtkVector3f(tmp, * trans, object[k]);
    simplex[i] = tmp - point;
  }
  
  while (1) {
    // find the distance with the existing simplex
    i = projectOriginOnSimplex(tcsoWpt, 
			       simplex,
			       simplexIndex,
			       nbSimplexPoint);
    
    // if in collision return
    if (i == COLLISION) return COLLISION;
    
    // there is a distance with the existing simplex
    // but check if the distance is too small to be of interest
    (* upperBound) = tcsoWpt * tcsoWpt;
    if ((* upperBound) < SECURITY_DISTANCE_SQUARE) return COLLISION;
    
    // distance is big enough. sweep the object along the distance 
    // direction to get a sweep point
    sweepIndex = convexSupportFunction(object, 
				       neighbor,
				       simplexIndex[0],
				       tcsoWpt,
				       trans);
    
    // check if the sweep point already exists in the existing
    // simplex. if it does, we cannot descent anymore. return
    for (i = 0 ; i < * nbSimplexPoint ; i++)
      if (sweepIndex == simplexIndex[i]) return NO_MORE_DESCENT;
    
    // new sweep point obtained. but is the simplex formed by
    // this new sweep point 'big' enough

    vtkVector3f tmpPtIndex;
    matrix4fXformvtkVector3f(tmpPtIndex, * trans, object[sweepIndex]);
    workV = tmpPtIndex - point;
    
    lowerBound = tcsoWpt * workV;
    
    if ((* upperBound) - lowerBound < 
	RELATIVE_PRECISION_FOR_PROJECTION * (* upperBound)) {
      // the new point obtained does not form a reasonable simplx
      // we got the projection with a precision good enough 
      return OKAY;
    } else {
      /* add the point we found in the simplex */
      simplexIndex[* nbSimplexPoint] = sweepIndex;
      simplex[* nbSimplexPoint] = tmpPtIndex - point;
      (* nbSimplexPoint) = (* nbSimplexPoint) + 1;
    }
  }
  
  SPY("Error in EnGJK::projectPointOnConvex()\n");

  return COLLISION;
}

/* EnGJK.cpp ends here */