/*
 ******************************************************************************
 * Title: HBBInteraction
 * Project: ColDetection Library
 ******************************************************************************
 * File: HBBInteraction.cpp
 * Author: Romain Rodriguez <Romain.Rodriguez@inrialpes.fr>
 * Created: 2003-01-13
 * Last update: 2003-05-20
 ******************************************************************************
 * Description: 
 * Hierarchical Bounding Box Interaction
 ******************************************************************************
 * 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 "vtkHBBInteraction.h"

//     6------7
//    /|     /|        y
//   1------0 |        |
//   | |    | |        0--x
//   | 5----|-4       /
//   |/     |/       z
//   2------3
vtkCxxRevisionMacro(vtkHBBInteraction, "$Revision: 0.1 $");
int vtkHBBInteraction::HBBRING[] = {
  8, 12, 16, 20, 24, 28, 32, 36,
  1, 3, 7, -1,
  0, 6, 2, -1,
  3, 1, 5, -1,
  0, 2, 4, -1,
  7, 3, 5, -1,
  2, 6, 4, -1,
  7, 5, 1, -1,
  0, 4, 6, -1
};

vtkHBBInteraction::vtkHBBInteraction(vtkDObject* obj1,
			       vtkDObject* obj2)
{
  transA = obj1 -> trans;
  transB = obj2 -> trans;

  _bboxTA = obj1 -> mesh -> getBoundingBoxTree();
  _bboxTB = obj2 -> mesh -> getBoundingBoxTree();
}

vtkHBBInteraction::~vtkHBBInteraction(void)
{
}

bool vtkHBBInteraction::checkCollision(void)
{
  svColl.clear();
  recurCheckCollision();

  return (svColl.size() != 0);
}

void vtkHBBInteraction::recurCheckCollision(void)
{
  recurCheckCollisionSAT(_bboxTA, _bboxTB);
}

void vtkHBBInteraction::recurCheckCollisionSAT(BoundingBoxTree* bboxTA,
					    BoundingBoxTree* bboxTB)
{
 
  
  // First check collision
  if (checkBBoxCollision(bboxTA,  bboxTB)) {
    if (bboxTA -> node -> type == TREE) { // bboxTA is TREE
      if (bboxTB -> node -> type == TREE) { // bboxTB is TREE
	// then explore every sons
	recurCheckCollisionSAT(bboxTA -> lson, bboxTB -> lson);
	recurCheckCollisionSAT(bboxTA -> lson, bboxTB -> rson);
	recurCheckCollisionSAT(bboxTA -> rson, bboxTB -> lson);
	recurCheckCollisionSAT(bboxTA -> rson, bboxTB -> rson);
      } else { // bboxTB is a LEAF
	// then explore bboxTA's sons.
	recurCheckCollisionSAT(bboxTA -> lson, bboxTB);
	recurCheckCollisionSAT(bboxTA -> rson, bboxTB);
      }
    } else { // bboxTA is a LEAF, check bboxTB
      if (bboxTB -> node -> type == TREE) { // bboxTB is TREE
	// then explore  bboxTB's sons.
	recurCheckCollisionSAT(bboxTA, bboxTB -> lson);
	recurCheckCollisionSAT(bboxTA, bboxTB -> rson);
      } else { // both bboxTA and bboxTB are LEAF
	// Bingo ! insert this collision in the svColl
	HBBCollision aColl;
	aColl.leafA = bboxTA;
	aColl.leafB = bboxTB;
	svColl.push_back(aColl);
	
      }
    }	
  } 
}

bool vtkHBBInteraction::checkBBoxCollision(BoundingBoxTree* bboxTA,
					BoundingBoxTree* bboxTB)
{
  // Get the necessary vectors
  vtkVector3f AC(bboxTA -> node -> box -> center);
  vtkVector3f BC(bboxTB -> node -> box -> center);

  // Transformations
  transform(AC, * transA);
  transform(BC, * transB);

  vtkVector3f* axisA = new vtkVector3f[3];
  vtkVector3f::setXYZ(axisA[0], (* transA)[0], (* transA)[1], (* transA)[2]);
  vtkVector3f::setXYZ(axisA[1], (* transA)[4], (* transA)[5], (* transA)[6]);
  vtkVector3f::setXYZ(axisA[2], (* transA)[8], (* transA)[9], (* transA)[10]);

  vtkVector3f* axisB = new vtkVector3f[3];
  vtkVector3f::setXYZ(axisB[0], (* transB)[0], (* transB)[1], (* transB)[2]);
  vtkVector3f::setXYZ(axisB[1], (* transB)[4], (* transB)[5], (* transB)[6]);
  vtkVector3f::setXYZ(axisB[2], (* transB)[8], (* transB)[9], (* transB)[10]);

  // Center Difference
  vtkVector3f D;
  D = BC - AC;
  
  vtkVector3f* C = new vtkVector3f[3];
  vtkVector3f* absC = new vtkVector3f[3];
  double* AD = new double[3];
  float R0, R1, R, R01;
  
   // A0
  C[0].x = axisA[0] * axisB[0];
  C[0].y = axisA[0] * axisB[1];
  C[0].z = axisA[0] * axisB[2];
  AD[0] = axisA[0] * D;
  absC[0].x = ABS(C[0].x);
  absC[0].y = ABS(C[0].y);
  absC[0].z = ABS(C[0].z);
  R = ABS(AD[0]);
  R1 =
    bboxTB -> node -> box -> diagonal.x * absC[0].x +
    bboxTB -> node -> box -> diagonal.y * absC[0].y + 
    bboxTB -> node -> box -> diagonal.z * absC[0].z;
  R01 = bboxTA -> node -> box -> diagonal.x + R1;
  if (R > R01)
  {
	  delete[] axisA;
	  delete[] axisB;
	  delete[] C;
	  delete[] absC;
	  delete[] AD;
	  return false;
  }

  // A1
  C[1].x = axisA[1] * axisB[0];
  C[1].y = axisA[1] * axisB[1];
  C[1].z = axisA[1] * axisB[2];
  AD[1] = axisA[1] * D;
  absC[1].x = ABS(C[1].x);
  absC[1].y = ABS(C[1].y);
  absC[1].z = ABS(C[1].z);
  R = ABS(AD[1]);
  R1 =
    bboxTB -> node -> box -> diagonal.x * absC[1].x +
    bboxTB -> node -> box -> diagonal.y * absC[1].y +
    bboxTB -> node -> box -> diagonal.z * absC[1].z;
  R01 = bboxTA -> node -> box -> diagonal.y + R1;
  if (R > R01){
	 delete[] axisA;
	  delete[] axisB;
	  delete[] C;
	  delete[] absC;
	  delete[] AD;
	  return false;
  }
  
  // A2
  C[2].x = axisA[2] * axisB[0];
  C[2].y = axisA[2] * axisB[1];
  C[2].z = axisA[2] * axisB[2];
  AD[2] = axisA[2] * D;
  absC[2].x = ABS(C[2].x);
  absC[2].y = ABS(C[2].y);
  absC[2].z = ABS(C[2].z);
  R = ABS(AD[2]);
  R1 =
    bboxTB -> node -> box -> diagonal.x * absC[2].x + 
    bboxTB -> node -> box -> diagonal.y * absC[2].y +
    bboxTB -> node -> box -> diagonal.z * absC[2].z;
  R01 = bboxTA -> node -> box -> diagonal.z + R1;
  if (R > R01){
	  delete[] axisA;
	  delete[] axisB;
	  delete[] C;
	  delete[] absC;
	  delete[] AD;
	  return false;
  }
  
  // B0
  R = ABS(axisB[0] * D);
  R0 =
    bboxTA -> node -> box -> diagonal.x * absC[0].x + 
    bboxTA -> node -> box -> diagonal.y * absC[1].x +
    bboxTA -> node -> box -> diagonal.z * absC[2].x;
  R01 = R0 + bboxTB -> node -> box -> diagonal.x;
  if (R > R01) {
		delete[] axisA;
	  delete[] axisB;
	  delete[] C;
	  delete[] absC;
	  delete[] AD;
	  return false;
  }
  
  // B1
  R = ABS(axisB[1] * D);
  R0 = 
    bboxTA -> node -> box -> diagonal.x * absC[0].y +
    bboxTA -> node -> box -> diagonal.y * absC[1].y +
    bboxTA -> node -> box -> diagonal.z * absC[2].y;
  R01 = R0 + bboxTB -> node -> box -> diagonal.y;
  if (R > R01){
	  delete[] axisA;
	  delete[] axisB;
	  delete[] C;
	  delete[] absC;
	  delete[] AD;
	  return false;
  }
  
  
  // B2
  R = ABS(axisB[2] * D);
  R0 =
    bboxTA -> node -> box -> diagonal.x * absC[0].z + 
    bboxTA -> node -> box -> diagonal.y * absC[1].z +
    bboxTA -> node -> box -> diagonal.z * absC[2].z;
  R01 = R0 + bboxTB -> node -> box -> diagonal.z;
  if (R > R01) {
		delete[] axisA;
	  delete[] axisB;
	  delete[] C;
	  delete[] absC;
	  delete[] AD;
	  return false;  
  }
  
  // A0xB0
  R = ABS(AD[2] * C[1].x - AD[1] * C[2].x);
  R0 =
    bboxTA -> node -> box -> diagonal.y * absC[2].x + 
    bboxTA -> node -> box -> diagonal.z * absC[1].x;
  R1 =
    bboxTB -> node -> box -> diagonal.y * absC[0].z +
    bboxTB -> node -> box -> diagonal.z * absC[0].y;
  R01 = R0 + R1;
  if (R > R01) {
	  delete[] axisA;
	  delete[] axisB;
	  delete[] C;
	  delete[] absC;
	  delete[] AD;
	  return false;
  }
  
  // A0xB1
  R = ABS(AD[2] * C[1].y - AD[1] * C[2].y);
  R0 = 
    bboxTA -> node -> box -> diagonal.y * absC[2].y + 
    bboxTA -> node -> box -> diagonal.z * absC[1].y;
  R1 =
    bboxTB -> node -> box -> diagonal.x * absC[0].z +
    bboxTB -> node -> box -> diagonal.z * absC[0].x;
  R01 = R0 + R1;
  if (R > R01){
delete[] axisA;
	  delete[] axisB;
	  delete[] C;
	  delete[] absC;
	  delete[] AD;
	  return false;
  }
  
  // A0xB2
  R = ABS(AD[2] * C[1].z - AD[1] * C[2].z);
  R0 =
    bboxTA -> node -> box -> diagonal.y * absC[2].z +
    bboxTA -> node -> box -> diagonal.z * absC[1].z;
  R1 =
    bboxTB -> node -> box -> diagonal.x * absC[0].y +
    bboxTB -> node -> box -> diagonal.y * absC[0].x;
  R01 = R0 + R1;
  if (R > R01) {
	  delete[] axisA;
	  delete[] axisB;
	  delete[] C;
	  delete[] absC;
	  delete[] AD;
	  return false;
  }
  
  // A1xB0
  R = ABS(AD[0] * C[2].x - AD[2] * C[0].x);
  R0 =
    bboxTA -> node -> box -> diagonal.x * absC[2].x +
    bboxTA -> node -> box -> diagonal.z * absC[0].x;
  R1 =
    bboxTB -> node -> box -> diagonal.y * absC[1].z +
    bboxTB -> node -> box -> diagonal.z * absC[1].y;
  R01 = R0 + R1;
  if (R > R01){
	  delete[] axisA;
	  delete[] axisB;
	  delete[] C;
	  delete[] absC;
	  delete[] AD;
	  return false;
  }
  
  // A1xB1
  R = ABS(AD[0] * C[2].y - AD[2] * C[0].y);
  R0 =
    bboxTA -> node -> box -> diagonal.x * absC[2].y +
    bboxTA -> node -> box -> diagonal.z * absC[0].y;
  R1 =
    bboxTB -> node -> box -> diagonal.x * absC[1].z +
    bboxTB -> node -> box -> diagonal.z * absC[1].x;
  R01 = R0 + R1;
  if (R > R01) {
	  delete[] axisA;
	  delete[] axisB;
	  delete[] C;
	  delete[] absC;
	  delete[] AD;
	  return false;
  }
  
  // A1xB2
  R = ABS(AD[0] * C[2].z - AD[2] * C[0].z);
  R0 =
    bboxTA -> node -> box -> diagonal.x * absC[2].z +
    bboxTA -> node -> box -> diagonal.z * absC[0].z;
  R1 =
    bboxTB -> node -> box -> diagonal.x * absC[1].y +
    bboxTB -> node -> box -> diagonal.y * absC[1].x;
  R01 = R0 + R1;
  if (R > R01){
	  delete[] axisA;
	  delete[] axisB;
	  delete[] C;
	  delete[] absC;
	  delete[] AD;
	  return false;
  }
  
  // A2xB0
  R = ABS(AD[1] * C[0].x - AD[0] * C[1].x);
  R0 =
    bboxTA -> node -> box -> diagonal.x * absC[1].x +
    bboxTA -> node -> box -> diagonal.y * absC[0].x;
  R1 =
    bboxTB -> node -> box -> diagonal.y * absC[2].z + 
    bboxTB -> node -> box -> diagonal.z * absC[2].y;
  R01 = R0 + R1;
  if (R > R01) {
	  delete[] axisA;
	  delete[] axisB;
	  delete[] C;
	  delete[] absC;
	  delete[] AD;
	  return false;
  }
  
  // A2xB1
  R = ABS(AD[1] * C[0].y - AD[0] * C[1].y);
  R0 =
    bboxTA -> node -> box -> diagonal.x * absC[1].y +
    bboxTA -> node -> box -> diagonal.y * absC[0].y;
  R1 =
    bboxTB -> node -> box -> diagonal.x * absC[2].z + 
    bboxTB -> node -> box -> diagonal.z * absC[2].x;
  R01 = R0 + R1;
  if (R > R01) {
	 delete[] axisA;
	  delete[] axisB;
	  delete[] C;
	  delete[] absC;
	  delete[] AD;
	  return false;
  }
  
  // A2xB2
  R = ABS(AD[1] * C[0].z - AD[0] * C[1].z);
  R0 =
    bboxTA -> node -> box -> diagonal.x * absC[1].z +
    bboxTA -> node -> box -> diagonal.y * absC[0].z;
  R1 =
    bboxTB -> node -> box -> diagonal.x * absC[2].y +
    bboxTB -> node -> box -> diagonal.y * absC[2].x;
  R01 = R0 + R1;
  if (R > R01) {
  delete[] axisA;
	  delete[] axisB;
	  delete[] C;
	  delete[] absC;
	  delete[] AD;
	  return false;
  }

  // Collision found
  delete[] axisA;
  delete[] axisB;
  delete[] C;
  delete[] absC;
  delete[] AD;
  return true;
}

/* HBBInteraction.cpp ends here */