/*
 ******************************************************************************
 * Title: MeshCol
 * Project: ColDetection Library
 ******************************************************************************
 * File: MeshCol.cpp
 * Author: Romain Rodriguez <Romain.Rodriguez@inrialpes.fr>
 * Created: 2003-01-13
 * Last update: 2003-05-20
 ******************************************************************************
 * Description: 
 * Collision detection of 2 general mesh
 ******************************************************************************
 * 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 "vtkMeshCol.h"
vtkCxxRevisionMacro(vtkMeshCol, "$Revision: 0.1 $");
vtkMeshCol::vtkMeshCol(vtkDObject* obj1, vtkDObject* obj2, bool search): vtkHBBInteraction(obj1, obj2)
{
  _ptsA = obj1 -> mesh -> vertices();
  _ptsB = obj2 -> mesh -> vertices();

  _triA = obj1 -> mesh -> facets();
  _triB = obj2 -> mesh -> facets();

  neighborsA = obj1 -> mesh -> neighborsOfFacets();
  neighborsB = obj2 -> mesh -> neighborsOfFacets();

  collision = false;
  searchSurface = search;
}

vtkMeshCol::~vtkMeshCol(void)
{
}

void vtkMeshCol::getCollisionInfo(std::vector<CollisionPair>& collisionInfo)
{
	collisionInfo=collisionFacets;
}

bool vtkMeshCol::verifyCollision(void)
{
  /** initialize all lists */
  collision = false;
  collisionFacets.clear();
  
  if (checkCollision()) {
    
    /** variables */
    vtkVector3f a0, a1, a2, b0, b1, b2, n1, n2;
    CollisionPair pair;
    planeEq p1, p2;
    
    for (unsigned k = 0 ; k < svColl.size() ; k++) {
      unsigned f1 = svColl[k].leafA -> node -> id;
      unsigned f2 = svColl[k].leafB -> node -> id;
      
      matrix4fXformvtkVector3f(a0, * transA, _ptsA[(_triA[f1]).a]);
      matrix4fXformvtkVector3f(a1, * transA, _ptsA[(_triA[f1]).b]);
      matrix4fXformvtkVector3f(a2, * transA, _ptsA[(_triA[f1]).c]);
      
      matrix4fXformvtkVector3f(b0, * transB, _ptsB[(_triB[f2]).a]);
      matrix4fXformvtkVector3f(b1, * transB, _ptsB[(_triB[f2]).b]);
      matrix4fXformvtkVector3f(b2, * transB, _ptsB[(_triB[f2]).c]);
      
      vtkVector3f::makeTriNormal(n1, a0, a1, a2);
      vtkVector3f::makeTriNormal(n2, b0, b1, b2);

      if (facetFacetCollisionTest(a0, a1, a2, b0, b1, b2, n1, n2)) {
	makePlaneEq(p1, a0, a1, a2);
	makePlaneEq(p2, b0, b1, b2);
	
	pair.F1 = f1;
	pair.P1 = p2;
	
	pair.F2 = f2; 
	pair.P2 = p1;
	
	collisionFacets.push_back(pair);
	
	collision = true;
      }
    }
  }
  if (searchSurface && (collisionFacets.size() > 0)) {
    search();
  }
  return collision;
}

bool vtkMeshCol::facetFacetCollisionTest(const vtkVector3f& a0,
				      const vtkVector3f& a1,
				      const vtkVector3f& a2,
				      const vtkVector3f& b0, 
				      const vtkVector3f& b1,
				      const vtkVector3f& b2, 
				      const vtkVector3f& n1,
				      const vtkVector3f& n2)
{
  int index = 0;
  vtkVector3f cross;
  double isect1[2], isect2[2];
  double d1, d2, aaa, bbb, ccc;
  double du0, du1, du2, dv0, dv1, dv2;
  double du0du1, du0du2, du1du2;
  double dv0dv1, dv0dv2, dv1dv2;
  double vp0 = ZERO, vp1 = ZERO, vp2 = ZERO;
  double up0 = ZERO, up1 = ZERO, up2 = ZERO;
  double a, b, c, x0, x1, d, e, f, y0, y1;
  double xx, yy, xxyy, tmp;
  
  /****************************************************************/
 /*    Calculating Intersection of Facets under collision        */
 /****************************************************************/
 
 // compute plane equation of triangle(V0,V1,V2)
 d1 = -1 * (n1 * a0);
 
 // put U0,U1,U2 into plane equation 1 to compute signed distances to
 // the plane
 du0 = (n1 * b0) + d1;
 du1 = (n1 * b1) + d1;
 du2 = (n1 * b2) + d1;
 
 // coplanarity robustness check 
 if (IS_ALMOST_ZERO(du0)) du0 = ZERO;
 if (IS_ALMOST_ZERO(du1)) du1 = ZERO;
 if (IS_ALMOST_ZERO(du2)) du2 = ZERO;
 du0du1 = du0 * du1;
 du0du2 = du0 * du2;
 du1du2 = du1 * du2;
 
 //  same sign on all of them + not equal 0 ?  no intersection occurs 
 if (du0du1 > 0 && du0du2 > 0) return false;  
 
 // compute plane of triangle (U0,U1,U2) 
 d2 = -1 * (n2 * b0);
 
 // put V0,V1,V2 into plane equation 2 to compute signed distances to
 // the plane
 dv0 = (n2 * a0) + d2;
 dv1 = (n2 * a1) + d2;
 dv2 = (n2 * a2) + d2;
 
 // coplanarity robustness check 
 if (IS_ALMOST_ZERO(dv0)) dv0 = ZERO;
 if (IS_ALMOST_ZERO(dv1)) dv1 = ZERO;
 if (IS_ALMOST_ZERO(dv2)) dv2 = ZERO;
 dv0dv1 = dv0 * dv1;
 dv0dv2 = dv0 * dv2;
 dv1dv2 = dv1 * dv2;
 
 // same sign on all of them + not equal 0 ? no intersection occurs 
 if (dv0dv1 > 0 && dv0dv2 > 0) return false;  
 
 /************************************************************/
 /* at this point we have verified that the planes formed by */
 /* the 2 triangles intersect but we are not sure if the     */
 /* intersection is due to coplanarity or not                */
 /************************************************************/
 
 // compute direction of intersection line 
 cross.crossProduct(n1, n2);
 
 // compute and index to the largest component of D 
 aaa = ABS(cross.x); bbb = ABS(cross.y); ccc = ABS(cross.z);
 if (bbb > aaa) {
   aaa = bbb;
   index = 1;
 }
 if (ccc > aaa) {
   aaa = ccc;
   index = 2;
 }
 
 // this is the simplified projection onto the line of intersection
 switch (index) {
 case 0:
   vp0 = a0.x; vp1 = a1.x; vp2 = a2.x; up0 = b0.x; up1 = b1.x; up2 = b2.x;
   break;
 case 1:
   vp0 = a0.y; vp1 = a1.y; vp2 = a2.y; up0 = b0.y; up1 = b1.y; up2 = b2.y;
   break;
 case 2:
   vp0 = a0.z; vp1 = a1.z; vp2 = a2.z; up0 = b0.z; up1 = b1.z; up2 = b2.z;
   break;
 }
 
 // compute interval for triangle 1
 if (dv0dv1 > 0) {
   a = vp2; b = (vp0 - vp2) * dv2; c = (vp1 - vp2) * dv2;
   x0 = dv2 - dv0; x1 = dv2 -dv1;
 } else if (dv0dv2 > 0) { 
   a = vp1; b = (vp0 - vp1) * dv1; c = (vp2 - vp1) * dv1;
   x0 = dv1 - dv0; x1 = dv1 - dv2;
 } else if (dv1dv2 > 0 || dv0 != 0) {
   a = vp0; b = (vp1 - vp0) * dv0; c = (vp2 - vp0) * dv0;
   x0 = dv0 - dv1; x1 = dv0 - dv2;
 } else if (dv1 != 0) {
   a = vp1; b = (vp0 - vp1) * dv1; c = (vp2 - vp1) * dv1;
   x0 = dv1 - dv0; x1 = dv1 - dv2;
 } else if (dv2 != 0) {
   a = vp2; b = (vp0 - vp2) * dv2; c = (vp1 - vp2) * dv2;
   x0 = dv2 - dv0; x1 = dv2 - dv1;
 } else return false;  //coplanar facets
 
 // compute interval for triangle 2 
 if (du0du1 > 0) {
   d = up2; e = (up0 - up2) * du2; f = (up1 - up2) * du2;
   y0 = du2 - du0; y1 = du2 - du1;
 } else if (du0du2 > 0) { 
   d = up1; e = (up0 - up1) * du1; f = (up2 - up1) * du1;
   y0 = du1 - du0; y1 = du1 - du2;
 } else if (du1du2 > 0 || du0 != 0) { 
   d = up0; e = (up1 - up0) * du0; f = (up2 - up0) * du0;
   y0 = du0 - du1; y1 = du0 - du2;
 } else if (du1 != 0) {
   d = up1; e = (up0 - up1) * du1; f = (up2 - up1) * du1;
   y0 = du1 - du0; y1 = du1 - du2;
 } else if (du2 != 0) {
   d = up2; e = (up0 - up2) * du2; f = (up1 - up2) * du2;
   y0 = du2 - du0; y1 = du2 - du1;
 } else return false;  //coplanar facets
 
 // arrange interval and compare. if they overlap find endpoints and
 // return true. else return false
 xx = x0 * x1;
 yy = y0 * y1;
 xxyy = xx * yy;
 tmp = a * xxyy;
 isect1[0] = tmp + b * x1 * yy;
 isect1[1] = tmp + c * x0 * yy;
 tmp = d * xxyy;
 isect2[0] = tmp + e * xx * y1;
 isect2[1] = tmp + f * xx * y0;
 
 if (isect1[0] > isect1[1]) { 
   tmp = isect1[0]; isect1[0] = isect1[1]; isect1[1] = tmp;
 }
 if (isect2[0] > isect2[1]) {
   tmp = isect2[0]; isect2[0] = isect2[1]; isect2[1] = tmp;
 }
 if (isect1[1] < isect2[0] || isect2[1] < isect1[0]) return false;  
 
 return true;
}

void vtkMeshCol::search(void)
{
  planeEq plan;
  unsigned f;
  double d1, d2, d3;

  CS1.clear();
  CS2.clear();
  OB1.clear(); 
  OB2.clear();
  IB1.clear(); 
  IB2.clear();
  
  for (unsigned j = 0; j < collisionFacets.size(); j++) {
    // begin construction for first object
    f = collisionFacets[j].F1; 
    if (index(CS1, f) == -1) {
      CS1.push_back(f);
      OB1.push_back(f);
    }
    plan = collisionFacets[j].P1;
    const std::vector<unsigned>& tab1 = neighborsA[f];

    for (unsigned k = 0; k < tab1.size() ; k++) {     
      vtkVector3f tmpD1;
      matrix4fXformvtkVector3f(tmpD1, * transA, _ptsA[(_triA[tab1[k]]).a]);
      vtkVector3f tmpD2;
      matrix4fXformvtkVector3f(tmpD2, * transA, _ptsA[(_triA[tab1[k]]).b]);
      vtkVector3f tmpD3;
      matrix4fXformvtkVector3f(tmpD3, * transA, _ptsA[(_triA[tab1[k]]).c]);
      
      d1 = dist2Plane(plan, tmpD1);     
      d2 = dist2Plane(plan, tmpD2);
      d3 = dist2Plane(plan, tmpD3);

      if (d1 > ZERO || d2 > ZERO || d3 > ZERO) 
        OB1.push_back(tab1[k]);
      else if (d1 < ZERO && d2 < ZERO && d3 < ZERO) 
        IB1.push_back(tab1[k]); 
    }
    // begin construction for second object
    f = collisionFacets[j].F2; 
    if (index(CS2, f) == -1) {
      CS2.push_back(f);
      OB2.push_back(f);
    }
    plan = collisionFacets[j].P2;
    const std::vector<unsigned>& tab2 = neighborsB[f];

    for (unsigned k = 0 ; k < tab2.size(); k++) {     
      vtkVector3f tmpD1;
      matrix4fXformvtkVector3f(tmpD1, * transB, _ptsB[(_triB[tab2[k]]).a]);
      vtkVector3f tmpD2;
      matrix4fXformvtkVector3f(tmpD2, * transB, _ptsB[(_triB[tab2[k]]).b]);
      vtkVector3f tmpD3;
      matrix4fXformvtkVector3f(tmpD3, * transB, _ptsB[(_triB[tab2[k]]).c]);
      
      d1 = dist2Plane(plan, tmpD1);
      d2 = dist2Plane(plan, tmpD2);
      d3 = dist2Plane(plan, tmpD3);

      if (d1 > ZERO || d2 > ZERO || d3 > ZERO) 
        OB2.push_back(tab2[k]);
      else if (d1 < ZERO && d2 < ZERO && d3 < ZERO) 
        IB2.push_back(tab2[k]); 
    }
  }
  
  // search contact elements for first object
//  unsigned j;
  if (IB1.size() > 0) {
    for (unsigned j = 0 ; j < IB1.size() ; j++)
      if (index(OB1, IB1[j]) == -1)       
        contactSurface(IB1[j], neighborsA, CS1, OB1);
  }
  
  // search contact elements for second object
  if (IB2.size() > 0) {
    for (unsigned j = 0 ; j < IB2.size() ; j++)
      if (index(OB2, IB2[j]) == -1)       
        contactSurface(IB2[j], neighborsB, CS2, OB2);
  }

}

void vtkMeshCol::contactSurface(unsigned seed,
                             const std::vector<unsigned>* neighbors, 
                             std::vector<unsigned>& CS,
                             std::vector<unsigned>& OB)
{
  const std::vector<unsigned>& tab = neighbors[seed];

  for (unsigned k = 0 ; k < tab.size() ; k++) {
    if (index(OB, tab[k]) == -1) {
      CS.push_back(tab[k]);
      OB.push_back(tab[k]);
      contactSurface(tab[k], neighbors, CS, OB);
    }
  }

}

/** 
 * Calculate the volume of penetration and the center of mass
 * of this volume. force will be applied at this point.
 */
void vtkMeshCol::collisionVolume(double& colVol,
			      vtkVector3f& colCom,
			      const vtkVector3f* pts,
			      const vtkVector3ui* tri,
			      std::vector<unsigned>& CS)
{
  double height, area;
  vtkVector3f cross, proj;
  vtkVector3f v0, v1, v2;
  
  colVol = ZERO;
  colCom.zero();
  
  // calculate center of collision and volume of collision for object
  
  for (unsigned i = 0; i < CS.size(); i++ ) {
    v0 = pts[(tri[CS[i]]).a];
    colCom += v0;
    v1 = pts[(tri[CS[i]]).b];
    colCom += v1;
    v2 = pts[(tri[CS[i]]).c];
    colCom += v2;
    vtkVector3f::makeTriNormal(cross, v0, v1, v2);
    area = cross.norm();
    cross.normalize();
    proj = v1 - colCom;
    height = cross * proj;
    colVol += (height * area) / 6;
  }

  colCom /= CS.size() * 3;
}

/* MeshCol.cpp ends here */