source: orxonox.OLD/branches/collision_detection/src/lib/collision_detection/obb_tree_node.cc @ 5867

/*
   orxonox - the future of 3D-vertical-scrollers

   Copyright (C) 2004 orx

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; either version 2, or (at your option)
   any later version.

### File Specific:
   main-programmer: Patrick Boenzli
   co-programmer: ...
*/

#define DEBUG_SPECIAL_MODULE DEBUG_MODULE_COLLISION_DETECTION

#include "obb_tree_node.h"
#include "list.h"
#include "obb.h"
#include "obb_tree.h"
#include "matrix.h"
#include "abstract_model.h"
#include "world_entity.h"

#include "color.h"

#include "debug.h"
#include "glincl.h"



using namespace std;

float**  OBBTreeNode::coMat = NULL;
float**  OBBTreeNode::eigvMat = NULL;
float*   OBBTreeNode::eigvlMat = NULL;
int*     OBBTreeNode::rotCount = NULL;
GLUquadricObj* OBBTreeNode_sphereObj = NULL;

/**
 *  standard constructor
 */
OBBTreeNode::OBBTreeNode (const OBBTree& tree, unsigned int depth)
    : BVTreeNode()
{
  this->setClassID(CL_OBB_TREE_NODE, "OBBTreeNode");

  this->obbTree = &tree;
  this->depth = depth;

  this->nodeLeft = NULL;
  this->nodeRight = NULL;
  this->bvElement = NULL;

  this->tmpVert1 = NULL;
  this->tmpVert2 = NULL;

  if( OBBTreeNode::coMat == NULL)
  {
    OBBTreeNode::coMat = new float*[4];
    for(int i = 0; i < 4; i++)
      OBBTreeNode::coMat[i] = new float[4];
  }
  if( OBBTreeNode::eigvMat == NULL)
  {
    OBBTreeNode::eigvMat = new float*[4];
    for( int i = 0; i < 4; i++)
      OBBTreeNode::eigvMat[i] = new float[4];
  }
  if( OBBTreeNode::eigvlMat == NULL)
  {
    OBBTreeNode::eigvlMat = new float[4];
  }
  if( OBBTreeNode::rotCount == NULL)
    OBBTreeNode::rotCount = new int;

  if( OBBTreeNode_sphereObj == NULL)
    OBBTreeNode_sphereObj = gluNewQuadric();
}


/**
 *  standard deconstructor
 */
OBBTreeNode::~OBBTreeNode ()
{
  if( this->nodeLeft)
  {
    delete this->nodeLeft;
    this->nodeLeft = NULL;
  }
  if( this->nodeRight)
  {
    delete this->nodeRight;
    this->nodeRight = NULL;
  }
  if( this->bvElement)
    delete this->bvElement;
  this->bvElement = NULL;

  if (this->tmpVert1 != NULL)
    delete this->tmpVert1;
  if (this->tmpVert2 != NULL)
    delete this->tmpVert2;
}


/**
 *  creates a new BVTree or BVTree partition
 * @param depth: how much more depth-steps to go: if == 1 don't go any deeper!
 * @param modInfo: model informations from the abstrac model
 *
 * this function creates the Bounding Volume tree from a modelInfo struct and bases its calculations
 * on the triangle informations (triangle soup not polygon soup)
 */
void OBBTreeNode::spawnBVTree(const modelInfo& modelInf, const int* triangleIndexes, unsigned int length)
{
  sVec3D* verticesList;

  PRINTF(3)("OBB Depth: %i, tree index: %i, numVertices: %i\n", depth, treeIndex, length);
  printf("OBBTreeNode::spawnBVTree()\n");
  this->depth = depth;

  this->bvElement = new OBB();
  this->bvElement->modelInf = &modelInf;
  this->bvElement->triangleIndexes = triangleIndexes;
  this->bvElement->numTriangles = length;

  /* create the boxes in three steps */
  this->calculateBoxCovariance(*this->bvElement, modelInf, triangleIndexes, length);
  this->calculateBoxEigenvectors(*this->bvElement, modelInf, triangleIndexes, length);
  this->calculateBoxAxis(*this->bvElement, modelInf, triangleIndexes, length);

  /* do we need to descent further in the obb tree?*/
  if( likely( this->depth > 0))
  {
    //this->forkBox(*this->bvElement);


    //     if(this->tmpLen1 > 2)
    //     {
    //       OBBTreeNode* node1 = new OBBTreeNode();
    //       this->nodeLeft = node1;
    //       this->nodeLeft->spawnBVTree(depth - 1, this->tmpVert1, this->tmpLen1);
    //     }
    //     else
    //     {
    //       PRINTF(3)("Aboarding tree walk: less than 3 vertices left\n");
    //     }
    //
    //     if( this->tmpLen2 > 2)
    //     {
    //       OBBTreeNode* node2 = new OBBTreeNode();
    //       this->nodeRight = node2;
    //       this->nodeRight->spawnBVTree(depth - 1, this->tmpVert2, this->tmpLen2);
    //     }
    //     else
    //     {
    //       PRINTF(3)("Abording tree walk: less than 3 vertices left\n");
    //     }

  }
}


/**
 *  creates a new BVTree or BVTree partition
 * @param depth: how much more depth-steps to go: if == 1 don't go any deeper!
 * @param verticesList: the list of vertices of the object - each vertices triple is interpreted as a triangle
 *
 * this function creates an Bounding Volume tree from a vertices soup (no triangle data)
 */
void OBBTreeNode::spawnBVTree(const sVec3D *verticesList, unsigned int length)
{
  //   PRINTF(3)("\n");
  //   PRINTF(3)("OBB Depth: %i, tree index: %i, numVertices: %i\n", depth, treeIndex, length);
  //   this->depth = depth;
  //
  //
  //   this->bvElement = new OBB();
  //   this->bvElement->vertices = verticesList;
  //   this->bvElement->numOfVertices = length;
  //   PRINTF(3)("Created OBBox\n");
  //   this->calculateBoxCovariance(this->bvElement, verticesList, length);
  //   PRINTF(3)("Calculated attributes1\n");
  //   this->calculateBoxEigenvectors(this->bvElement, verticesList, length);
  //   PRINTF(3)("Calculated attributes2\n");
  //   this->calculateBoxAxis(this->bvElement, verticesList, length);
  //   PRINTF(3)("Calculated attributes3\n");
  //
  //
  //
  //   if( likely( this->depth > 0))
  //   {
  //     this->forkBox(this->bvElement);
  //
  //
  //     if(this->tmpLen1 > 2)
  //     {
  //       OBBTreeNode* node1 = new OBBTreeNode(this->obbTree);
  //       this->nodeLeft = node1;
  //       this->nodeLeft->spawnBVTree(depth - 1, this->tmpVert1, this->tmpLen1);
  //     }
  //     else
  //     {
  //       PRINTF(3)("Aboarding tree walk: less than 3 vertices left\n");
  //     }
  //
  //     if( this->tmpLen2 > 2)
  //     {
  //       OBBTreeNode* node2 = new OBBTreeNode(this->obbTree);
  //       this->nodeRight = node2;
  //       this->nodeRight->spawnBVTree(depth - 1, this->tmpVert2, this->tmpLen2);
  //     }
  //     else
  //     {
  //       PRINTF(3)("Abording tree walk: less than 3 vertices left\n");
  //     }
  //   }
}


void OBBTreeNode::calculateBoxCovariance(OBB& box, const modelInfo& modelInf, const int* triangleIndexes, unsigned int length)
{

  PRINTF(3)("Created OBBox\n");

  float     facelet[length];                         //!< surface area of the i'th triangle of the convex hull
  float     face = 0.0f;                             //!< surface area of the entire convex hull
  Vector    centroid[length];                        //!< centroid of the i'th convex hull
  Vector    center;                                  //!< the center of the entire hull
  Vector    p, q, r;                                 //!< holder of the polygon data, much more conveniant to work with Vector than sVec3d
  Vector    t1, t2;                                  //!< temporary values
  float     covariance[3][3] = {0,0,0, 0,0,0, 0,0,0};//!< the covariance matrix
  sVec3D*   tmpVec = NULL;                           //!< a temp saving place for sVec3Ds


  /* fist compute all the convex hull face/facelets and centroids */
  for( int i = 0; i < length ; ++i)
  {
    tmpVec = (sVec3D*)(&modelInf.pVertices[modelInf.pTriangles[triangleIndexes[i]].indexToVertices[0]]);
    p = *tmpVec;
    tmpVec = (sVec3D*)(&modelInf.pVertices[modelInf.pTriangles[triangleIndexes[i]].indexToVertices[1]]);
    q = *tmpVec;
    tmpVec = (sVec3D*)(&modelInf.pVertices[modelInf.pTriangles[triangleIndexes[i]].indexToVertices[2]]);
    r = *tmpVec;

    /* finding the facelet surface via cross-product */
    t1 = p - q;
    t2 = p - r;
    facelet[i] = 0.5f * fabs( t1.cross(t2).len() );
    /* update the entire convex hull surface */
    face += facelet[i];

    /* calculate the cetroid of the hull triangles */
    centroid[i] = (p + q + r) * 1/3;
    /* now calculate the centroid of the entire convex hull, weighted average of triangle centroids */
    center += centroid[i] * facelet[i];
  }
  /* take the average of the centroid sum */
  center /= face;


  /* now calculate the covariance matrix - if not written in three for-loops,
     it would compute faster: minor */
  for( int j = 0; j < 3; ++j)
  {
    for( int k = 0; k < 3; ++k)
    {
      for( int i = 0; i + 3 < length; i+=3)
      {
        tmpVec = (sVec3D*)(&modelInf.pVertices[modelInf.pTriangles[triangleIndexes[i]].indexToVertices[0]]);
        p = *tmpVec;
        tmpVec = (sVec3D*)(&modelInf.pVertices[modelInf.pTriangles[triangleIndexes[i]].indexToVertices[1]]);
        q = *tmpVec;
        tmpVec = (sVec3D*)(&modelInf.pVertices[modelInf.pTriangles[triangleIndexes[i]].indexToVertices[2]]);
        r = *tmpVec;

        covariance[j][k] = facelet[i] / (12.0f * face) * (9.0f * centroid[i][j] * centroid[i][k] + p[j] * p[k] +
                           q[j] * q[k] + r[j] * r[k]) - center[j] * center[k];
      }
    }
  }

  PRINTF(3)("\nOBB Covariance Matrix:\n");
  for(int j = 0; j < 3; ++j)
  {
    PRINTF(3)(" |");
    for(int k = 0; k < 3; ++k)
    {
      PRINTF(3)(" \b%f ", covariance[j][k]);
    }
    PRINTF(3)(" |\n");
  }
  PRINTF(3)("OBB Center: %f, %f, %f\n", center.x, center.y, center.z);

  /* write back the covariance matrix data to the object oriented bouning box */
  for(int i = 0; i < 3; ++i)
  {
    box.covarianceMatrix[i][0] = covariance[i][0];
    box.covarianceMatrix[i][1] = covariance[i][1];
    box.covarianceMatrix[i][2] = covariance[i][2];
  }
  box.center = center;
}



/**
 *  calculate the eigenvectors for the object oriented box
 * @param box: reference to the box
 * @param modelInf: the model info structure of the model
 * @param tirangleIndexes: an array with the indexes of the triangles inside this
 * @param length: the length of the indexes array
 */
void OBBTreeNode::calculateBoxEigenvectors(OBB& box, const modelInfo& modelInf,
    const int* triangleIndexes, unsigned int length)
{

  PRINTF(3)("Calculate the Box Eigenvectors\n");

  Vector         axis[3];                            //!< the references to the obb axis
  Matrix         covMat(  box.covarianceMatrix  );   //!< covariance matrix (in the matrix dataform)

  /*
  now getting spanning vectors of the sub-space:
  the eigenvectors of a symmertric matrix, such as the
  covarience matrix are mutually orthogonal.
  after normalizing them, they can be used as a the basis
  vectors
  */

  /* calculate the axis */
  covMat.getEigenVectors(axis[0], axis[1], axis[2] );
  box.axis[0] = axis[0];
  box.axis[1] = axis[1];
  box.axis[2] = axis[2];

  PRINTF(0)("-- Got Axis\n");
  PRINTF(0)("Eigenvector: %f, %f, %f\n", box.axis[0].x, box.axis[0].y, box.axis[0].z);
  PRINTF(0)("Eigenvector: %f, %f, %f\n", box.axis[1].x, box.axis[1].y, box.axis[1].z);
  PRINTF(0)("Eigenvector: %f, %f, %f\n", box.axis[2].x, box.axis[2].y, box.axis[2].z);
}




/**
 *  calculate the eigenvectors for the object oriented box
 * @param box: reference to the box
 * @param modelInf: the model info structure of the model
 * @param tirangleIndexes: an array with the indexes of the triangles inside this
 * @param length: the length of the indexes array
 */
void OBBTreeNode::calculateBoxAxis(OBB& box, const modelInfo& modInfo, const int* triangleIndexes, unsigned int length)
{

  sVec3D* verticesList;


  PRINTF(3)("Calculated attributes3\n");
  /* now get the axis length */
  Line                ax[3];                                 //!< the axis
  float               halfLength[3];                         //!< half length of the axis
  float               tmpLength;                             //!< tmp save point for the length
  Plane               p0(box.axis[0], box.center);        //!< the axis planes
  Plane               p1(box.axis[1], box.center);
  Plane               p2(box.axis[2], box.center);
  float               maxLength[3];
  float               minLength[3];


  /* get a bad bounding box */
  halfLength[0] = -1.0f;
  for(int j = 0; j < length; ++j)
  {
    tmpLength = fabs(p0.distancePoint(vertices[j]));
    if( tmpLength > halfLength[0])
      halfLength[0] = tmpLength;
  }

  halfLength[1] = -1.0f;
  for(int j = 0; j < length; ++j)
  {
    tmpLength = fabs(p1.distancePoint(vertices[j]));
    if( tmpLength > halfLength[1])
      halfLength[1] = tmpLength;
  }

  halfLength[2] = -1.0f;
  for(int j = 0; j < length; ++j)
  {
    tmpLength = fabs(p2.distancePoint(vertices[j]));
    if( tmpLength > halfLength[2])
      halfLength[2] = tmpLength;
  }



  /* get the maximal dimensions of the body in all directions */
  maxLength[0] = p0.distancePoint(vertices[0]);
  minLength[0] = p0.distancePoint(vertices[0]);
  for(int j = 0; j < length; ++j)
  {
    tmpLength = p0.distancePoint(vertices[j]);
    if( tmpLength > maxLength[0])
      maxLength[0] = tmpLength;
    else if( tmpLength < minLength[0])
      minLength[0] = tmpLength;
  }

  maxLength[1] = p1.distancePoint(vertices[0]);
  minLength[1] = p1.distancePoint(vertices[0]);
  for(int j = 0; j < length; ++j)
  {
    tmpLength = p1.distancePoint(vertices[j]);
    if( tmpLength > maxLength[1])
      maxLength[1] = tmpLength;
    else if( tmpLength < minLength[1])
      minLength[1] = tmpLength;
  }

  maxLength[2] = p2.distancePoint(vertices[0]);
  minLength[2] = p2.distancePoint(vertices[0]);
  for(int j = 0; j < length; ++j)
  {
    tmpLength = p2.distancePoint(vertices[j]);
    if( tmpLength > maxLength[2])
      maxLength[2] = tmpLength;
    else if( tmpLength < minLength[2])
      minLength[2] = tmpLength;
  }


  /* calculate the real centre of the body by using the axis length */
  float centerOffset[3];
  float newHalfLength[3];
  for(int i = 0; i < 3; ++i)
  {
    PRINTF(3)("max: %f, min: %f \n", maxLength[i], minLength[i]);
    centerOffset[i] = (maxLength[i] + minLength[i]) / 2.0f;       // min length is negatie
    newHalfLength[i] = (maxLength[i] - minLength[i]) / 2.0f;      // min length is negative
    box.center +=  (box.axis[i] * centerOffset[i]);            // update the new center vector
    halfLength[i] = newHalfLength[i];
  }



  box.halfLength[0] = halfLength[0];
  box.halfLength[1] = halfLength[1];
  box.halfLength[2] = halfLength[2];
  PRINTF(3)("-- Written Axis to obb\n");
  PRINTF(3)("-- Finished Calculating Attributes\n");
}



/**
  \brief this separates an ob-box in the middle
* @param box: the box to separate

  this will separate the box into to smaller boxes. the separation is done along the middle of the longest axis
 */
void OBBTreeNode::forkBox(OBB& box)
{
  /* get the longest axis of the box */
  float               aLength = -1.0f;                     //!< the length of the longest axis
  int                 axisIndex = 0;                       //!< this is the nr of the longest axis

  for(int i = 0; i < 3; ++i)
  {
    if( aLength < box.halfLength[i])
    {
      aLength = box.halfLength[i];
      axisIndex = i;
    }
  }

  PRINTF(3)("longest axis is: nr %i with a half-length of: %f\n", axisIndex, aLength);


  /* get the closest vertex near the center */
  float               dist = 999999.0f;                    //!< the smallest distance to each vertex
  float               tmpDist;                             //!< temporary distance
  int                 vertexIndex;
  Plane               middlePlane(box.axis[axisIndex], box.center); //!< the middle plane

  vertexIndex = 0;
  for(int i = 0; i < box.numOfVertices; ++i)
  {
    tmpDist = fabs(middlePlane.distancePoint(box.vertices[i]));
    if( tmpDist < dist)
    {
      dist = tmpDist;
      vertexIndex = i;
    }
  }

  PRINTF(3)("\nthe clostest vertex is nr: %i, with a dist of: %f\n", vertexIndex ,dist);


  /* now definin the separation plane through this specified nearest point and partition
  the points depending on which side they are located
  */
  tList<const sVec3D>      partition1;                           //!< the vertex partition 1
  tList<const sVec3D>      partition2;                           //!< the vertex partition 2


  PRINTF(3)("vertex index: %i, of %i\n", vertexIndex, box.numOfVertices);
  this->separationPlane = Plane(box.axis[axisIndex], box.vertices[vertexIndex]);  //!< separation plane
  this->sepPlaneCenter = &box.vertices[vertexIndex];
  this->longestAxisIndex = axisIndex;

  for(int i = 0; i < box.numOfVertices; ++i)
  {
    if( i == vertexIndex)
      continue;
    tmpDist = this->separationPlane.distancePoint(box.vertices[i]);
    if( tmpDist > 0.0)
      partition1.add(&box.vertices[i]); /* positive numbers plus zero */
    else
      partition2.add(&box.vertices[i]); /* negatice numbers */
  }
  partition1.add(&box.vertices[vertexIndex]);
  partition2.add(&box.vertices[vertexIndex]);

  PRINTF(3)("\npartition1: got %i vertices/ partition 2: got %i vertices\n", partition1.getSize(), partition2.getSize());


  /* now comes the separation into two different sVec3D arrays */
  tIterator<const sVec3D>* iterator;                       //!< the iterator to go through the lists
  const sVec3D*      element;                              //!< the elements
  int                index;                                //!< index storage place
  sVec3D*            vertList1;                            //!< the vertex list 1
  sVec3D*            vertList2;                            //!< the vertex list 2

  vertList1 = new sVec3D[partition1.getSize()];
  vertList2 = new sVec3D[partition2.getSize()];

  iterator = partition1.getIterator();
  element = iterator->firstElement();
  index = 0;
  while( element != NULL)
  {
    vertList1[index][0] = element[0][0];
    vertList1[index][1] = element[0][1];
    vertList1[index][2] = element[0][2];
    ++index;
    element = iterator->nextElement();
  }
  delete iterator;
  //   PRINTF(0)("\npartition 1:\n");
  //   for(int i = 0; i < partition1.getSize(); ++i)
  //   {
  //     PRINTF(0)("v[%i][0] = %f,\tv[%i][1] = %f,\tv[%i][1] = %f\n", i, vertList1[i][0], i, vertList1[i][1], i, vertList1[i][2]);
  //   }

  iterator = partition2.getIterator();
  element = iterator->firstElement();
  index = 0;
  while( element != NULL)
  {
    vertList2[index][0] = element[0][0];
    vertList2[index][1] = element[0][1];
    vertList2[index][2] = element[0][2];
    ++index;
    element = iterator->nextElement();
  }

  if (this->tmpVert1 != NULL)
    delete[] this->tmpVert1;
  this->tmpVert1 = vertList1;
  if (this->tmpVert2 != NULL)
    delete[] this->tmpVert2;
  this->tmpVert2 = vertList2;
  this->tmpLen1 = partition1.getSize();
  this->tmpLen2 = partition2.getSize();

  delete iterator;

  //   PRINTF(0)("\npartition 2:\n");
  //   for(int i = 0; i < partition2.getSize(); ++i)
  //   {
  //     PRINTF(0)("v[%i][0] = %f,\tv[%i][1] = %f,\tv[%i][1] = %f\n", i, vertList2[i][0], i,  vertList2[i][1], i, vertList2[i][2]);
  //   }
}




void OBBTreeNode::collideWith(const BVTreeNode& treeNode, const WorldEntity& nodeA, const WorldEntity& nodeB) const
{
  PRINTF(3)("collideWith\n");
  /* if the obb overlap, make subtests: check which node is realy overlaping  */
  PRINTF(3)("Checking OBB %i vs %i: ", this->getIndex(), treeNode.getIndex());
  //   if( unlikely(treeNode == NULL)) return;


  if( this->overlapTest(*this->bvElement, *(((const OBBTreeNode*)&treeNode)->bvElement), nodeA, nodeB))
  {
    PRINTF(3)("collision @ lvl %i, object %s vs. %s, (%p, %p)\n", this->depth, nodeA.getClassName(), nodeB.getClassName(), this->nodeLeft, this->nodeRight);

    /* check if left node overlaps */
    if( likely( this->nodeLeft != NULL))
    {
      PRINTF(3)("Checking OBB %i vs %i: ", this->nodeLeft->getIndex(), treeNode.getIndex());
      if( this->overlapTest(*this->nodeLeft->bvElement, *(((const OBBTreeNode*)&treeNode)->bvElement), nodeA, nodeB))
      {
        this->nodeLeft->collideWith(*(((const OBBTreeNode*)&treeNode)->nodeLeft), nodeA, nodeB);
        this->nodeLeft->collideWith(*(((const OBBTreeNode*)&treeNode)->nodeRight), nodeA, nodeB);
      }
    }
    /* check if right node overlaps */
    if( likely( this->nodeRight != NULL))
    {
      PRINTF(3)("Checking OBB %i vs %i: ", this->nodeRight->getIndex(), treeNode.getIndex());
      if(this->overlapTest(*this->nodeRight->bvElement, *(((const OBBTreeNode*)&treeNode)->bvElement), nodeA, nodeB))
      {
        this->nodeRight->collideWith(*(((const OBBTreeNode*)&treeNode)->nodeLeft), nodeA, nodeB);
        this->nodeRight->collideWith(*(((const OBBTreeNode*)&treeNode)->nodeRight), nodeA, nodeB);
      }
    }

    /* so there is a collision and this is the last box in the tree (i.e. leaf) */
    /* FIXME: If we would choose || insead of && there would also be asymmetrical cases supported */
    if( unlikely(this->nodeRight == NULL && this->nodeLeft == NULL))
    {
      nodeA.collidesWith(nodeB, (((const OBBTreeNode*)&treeNode)->bvElement->center));

      nodeB.collidesWith(nodeA, this->bvElement->center);
    }

  }
}



bool OBBTreeNode::overlapTest(const OBB& boxA, const OBB& boxB, const WorldEntity& nodeA, const WorldEntity& nodeB) const
{
  //   if( boxB == NULL || boxA == NULL)
  //     return false;

  /* first check all axis */
  Vector t;
  float rA = 0.0f;
  float rB = 0.0f;
  Vector l;
  Vector rotAxisA[3];
  Vector rotAxisB[3];

  rotAxisA[0] =  nodeA.getAbsDir().apply(boxA.axis[0]);
  rotAxisA[1] =  nodeA.getAbsDir().apply(boxA.axis[1]);
  rotAxisA[2] =  nodeA.getAbsDir().apply(boxA.axis[2]);

  rotAxisB[0] =  nodeB.getAbsDir().apply(boxB.axis[0]);
  rotAxisB[1] =  nodeB.getAbsDir().apply(boxB.axis[1]);
  rotAxisB[2] =  nodeB.getAbsDir().apply(boxB.axis[2]);


  t = nodeA.getAbsCoor() + nodeA.getAbsDir().apply(boxA.center) - ( nodeB.getAbsCoor() + nodeB.getAbsDir().apply(boxB.center));

  //   printf("\n");
  //   printf("(%f, %f, %f) -> (%f, %f, %f)\n", boxA->axis[0].x, boxA->axis[0].y, boxA->axis[0].z, rotAxisA[0].x, rotAxisA[0].y, rotAxisA[0].z);
  //   printf("(%f, %f, %f) -> (%f, %f, %f)\n", boxA->axis[1].x, boxA->axis[1].y, boxA->axis[1].z, rotAxisA[1].x, rotAxisA[1].y, rotAxisA[1].z);
  //   printf("(%f, %f, %f) -> (%f, %f, %f)\n", boxA->axis[2].x, boxA->axis[2].y, boxA->axis[2].z, rotAxisA[2].x, rotAxisA[2].y, rotAxisA[2].z);
  //
  //   printf("(%f, %f, %f) -> (%f, %f, %f)\n", boxB->axis[0].x, boxB->axis[0].y, boxB->axis[0].z, rotAxisB[0].x, rotAxisB[0].y, rotAxisB[0].z);
  //   printf("(%f, %f, %f) -> (%f, %f, %f)\n", boxB->axis[1].x, boxB->axis[1].y, boxB->axis[1].z, rotAxisB[1].x, rotAxisB[1].y, rotAxisB[1].z);
  //   printf("(%f, %f, %f) -> (%f, %f, %f)\n", boxB->axis[2].x, boxB->axis[2].y, boxB->axis[2].z, rotAxisB[2].x, rotAxisB[2].y, rotAxisB[2].z);


  /* All 3 axis of the object A */
  for( int j = 0; j < 3; ++j)
  {
    rA = 0.0f;
    rB = 0.0f;
    l = rotAxisA[j];

    rA += fabs(boxA.halfLength[0] * rotAxisA[0].dot(l));
    rA += fabs(boxA.halfLength[1] * rotAxisA[1].dot(l));
    rA += fabs(boxA.halfLength[2] * rotAxisA[2].dot(l));

    rB += fabs(boxB.halfLength[0] * rotAxisB[0].dot(l));
    rB += fabs(boxB.halfLength[1] * rotAxisB[1].dot(l));
    rB += fabs(boxB.halfLength[2] * rotAxisB[2].dot(l));

    PRINTF(3)("s = %f, rA+rB = %f\n", fabs(t.dot(l)), rA+rB);

    if( (rA + rB) < fabs(t.dot(l)))
    {
      PRINTF(3)("no Collision\n");
      return false;
    }
  }

  /* All 3 axis of the object B */
  for( int j = 0; j < 3; ++j)
  {
    rA = 0.0f;
    rB = 0.0f;
    l = rotAxisB[j];

    rA += fabs(boxA.halfLength[0] * rotAxisA[0].dot(l));
    rA += fabs(boxA.halfLength[1] * rotAxisA[1].dot(l));
    rA += fabs(boxA.halfLength[2] * rotAxisA[2].dot(l));

    rB += fabs(boxB.halfLength[0] * rotAxisB[0].dot(l));
    rB += fabs(boxB.halfLength[1] * rotAxisB[1].dot(l));
    rB += fabs(boxB.halfLength[2] * rotAxisB[2].dot(l));

    PRINTF(3)("s = %f, rA+rB = %f\n", fabs(t.dot(l)), rA+rB);

    if( (rA + rB) < fabs(t.dot(l)))
    {
      PRINTF(3)("no Collision\n");
      return false;
    }
  }


  /* Now check for all face cross products */

  for( int j = 0; j < 3; ++j)
  {
    for(int k = 0; k < 3; ++k )
    {
      rA = 0.0f;
      rB = 0.0f;
      l = rotAxisA[j].cross(rotAxisB[k]);

      rA += fabs(boxA.halfLength[0] * rotAxisA[0].dot(l));
      rA += fabs(boxA.halfLength[1] * rotAxisA[1].dot(l));
      rA += fabs(boxA.halfLength[2] * rotAxisA[2].dot(l));

      rB += fabs(boxB.halfLength[0] * rotAxisB[0].dot(l));
      rB += fabs(boxB.halfLength[1] * rotAxisB[1].dot(l));
      rB += fabs(boxB.halfLength[2] * rotAxisB[2].dot(l));

      PRINTF(3)("s = %f, rA+rB = %f\n", fabs(t.dot(l)), rA+rB);

      if( (rA + rB) < fabs(t.dot(l)))
      {
        PRINTF(3)("keine Kollision\n");
        return false;
      }
    }
  }

  /* FIXME: there is no collision mark set now */
  //   boxA.bCollided = true; /* use this ONLY(!!!!) for drawing operations */
  //   boxB.bCollided = true;


  PRINTF(3)("Kollision!\n");
  return true;
}





void OBBTreeNode::drawBV(int depth, int drawMode, const Vector& color,  bool top) const
{

  /* draw the model itself, there is some problem concerning this: the vertices are drawn multiple times */
  if( drawMode & DRAW_MODEL || drawMode & DRAW_ALL)
  {
    if( !(drawMode & DRAW_SINGLE && depth != 0))
    {
      if( drawMode & DRAW_POINTS)
        glBegin(GL_POINTS);
      for(int i = 0; i < this->bvElement->numOfVertices; ++i)
      {
        if( drawMode & DRAW_POINTS)
          glVertex3f(this->bvElement->vertices[i][0], this->bvElement->vertices[i][1], this->bvElement->vertices[i][2]);
        else
        {
          glPushMatrix();
          glTranslatef(this->bvElement->vertices[i][0], this->bvElement->vertices[i][1], this->bvElement->vertices[i][2]);
          gluSphere(OBBTreeNode_sphereObj, 0.1, 10, 10);
          glPopMatrix();
        }
      }
      if( drawMode & DRAW_POINTS)
        glEnd();
    }
  }

  if (top)
  {
    glPushAttrib(GL_ENABLE_BIT);
    glDisable(GL_LIGHTING);
    glDisable(GL_TEXTURE_2D);
  }
  glColor3f(color.x, color.y, color.z);


  /* draw world axes */
  if( drawMode & DRAW_BV_AXIS)
  {
    glBegin(GL_LINES);
    glColor3f(1.0, 0.0, 0.0);
    glVertex3f(0.0, 0.0, 0.0);
    glVertex3f(3.0, 0.0, 0.0);

    glColor3f(0.0, 1.0, 0.0);
    glVertex3f(0.0, 0.0, 0.0);
    glVertex3f(0.0, 3.0, 0.0);

    glColor3f(0.0, 0.0, 1.0);
    glVertex3f(0.0, 0.0, 0.0);
    glVertex3f(0.0, 0.0, 3.0);
    glEnd();
  }


  if( drawMode & DRAW_BV_AXIS || drawMode & DRAW_ALL)
  {
    if( !(drawMode & DRAW_SINGLE && depth != 0))
    {
      /* draw the obb axes */
      glBegin(GL_LINES);
      glColor3f(0.0, 0.4, 0.3);
      glVertex3f(this->bvElement->center.x, this->bvElement->center.y, this->bvElement->center.z);
      glVertex3f(this->bvElement->center.x + this->bvElement->axis[0].x * this->bvElement->halfLength[0],
                 this->bvElement->center.y + this->bvElement->axis[0].y * this->bvElement->halfLength[0],
                 this->bvElement->center.z + this->bvElement->axis[0].z * this->bvElement->halfLength[0]);

      glVertex3f(this->bvElement->center.x, this->bvElement->center.y, this->bvElement->center.z);
      glVertex3f(this->bvElement->center.x + this->bvElement->axis[1].x * this->bvElement->halfLength[1],
                 this->bvElement->center.y + this->bvElement->axis[1].y * this->bvElement->halfLength[1],
                 this->bvElement->center.z + this->bvElement->axis[1].z * this->bvElement->halfLength[1]);

      glVertex3f(this->bvElement->center.x, this->bvElement->center.y, this->bvElement->center.z);
      glVertex3f(this->bvElement->center.x + this->bvElement->axis[2].x * this->bvElement->halfLength[2],
                 this->bvElement->center.y + this->bvElement->axis[2].y * this->bvElement->halfLength[2],
                 this->bvElement->center.z + this->bvElement->axis[2].z * this->bvElement->halfLength[2]);
      glEnd();
    }
  }


  /* DRAW POLYGONS */
  if( drawMode & DRAW_BV_POLYGON || drawMode & DRAW_ALL || drawMode & DRAW_BV_BLENDED)
  {
    if (top)
    {
      glEnable(GL_BLEND);
      glBlendFunc(GL_SRC_ALPHA, GL_ONE);
    }

    if(this->nodeLeft == NULL || this->nodeRight == NULL)
      depth = 0;
    if( !(drawMode & DRAW_SINGLE && depth != 0))
    {
      Vector cen = this->bvElement->center;
      Vector* axis = this->bvElement->axis;
      float* len = this->bvElement->halfLength;

      if( this->bvElement->bCollided)
      {
        glColor4f(1.0, 1.0, 1.0, .5); // COLLISION COLOR
      }
      else if( drawMode & DRAW_BV_BLENDED)
      {
        glColor4f(color.x, color.y, color.z, .5);
      }

      /* draw bounding box */
      if( drawMode & DRAW_BV_BLENDED)
        glBegin(GL_QUADS);
      else
        glBegin(GL_LINE_LOOP);
      glVertex3f(cen.x + axis[0].x * len[0] + axis[1].x * len[1] + axis[2].x * len[2],
                 cen.y + axis[0].y * len[0] + axis[1].y * len[1] + axis[2].y * len[2],
                 cen.z + axis[0].z * len[0] + axis[1].z * len[1] + axis[2].z * len[2]);
      glVertex3f(cen.x + axis[0].x * len[0] + axis[1].x * len[1] - axis[2].x * len[2],
                 cen.y + axis[0].y * len[0] + axis[1].y * len[1] - axis[2].y * len[2],
                 cen.z + axis[0].z * len[0] + axis[1].z * len[1] - axis[2].z * len[2]);
      glVertex3f(cen.x + axis[0].x * len[0] - axis[1].x * len[1] - axis[2].x * len[2],
                 cen.y + axis[0].y * len[0] - axis[1].y * len[1] - axis[2].y * len[2],
                 cen.z + axis[0].z * len[0] - axis[1].z * len[1] - axis[2].z * len[2]);
      glVertex3f(cen.x + axis[0].x * len[0] - axis[1].x * len[1] + axis[2].x * len[2],
                 cen.y + axis[0].y * len[0] - axis[1].y * len[1] + axis[2].y * len[2],
                 cen.z + axis[0].z * len[0] - axis[1].z * len[1] + axis[2].z * len[2]);
      glEnd();

      if( drawMode & DRAW_BV_BLENDED)
        glBegin(GL_QUADS);
      else
        glBegin(GL_LINE_LOOP);
      glVertex3f(cen.x + axis[0].x * len[0] - axis[1].x * len[1] + axis[2].x * len[2],
                 cen.y + axis[0].y * len[0] - axis[1].y * len[1] + axis[2].y * len[2],
                 cen.z + axis[0].z * len[0] - axis[1].z * len[1] + axis[2].z * len[2]);
      glVertex3f(cen.x + axis[0].x * len[0] - axis[1].x * len[1] - axis[2].x * len[2],
                 cen.y + axis[0].y * len[0] - axis[1].y * len[1] - axis[2].y * len[2],
                 cen.z + axis[0].z * len[0] - axis[1].z * len[1] - axis[2].z * len[2]);
      glVertex3f(cen.x - axis[0].x * len[0] - axis[1].x * len[1] - axis[2].x * len[2],
                 cen.y - axis[0].y * len[0] - axis[1].y * len[1] - axis[2].y * len[2],
                 cen.z - axis[0].z * len[0] - axis[1].z * len[1] - axis[2].z * len[2]);
      glVertex3f(cen.x - axis[0].x * len[0] - axis[1].x * len[1] + axis[2].x * len[2],
                 cen.y - axis[0].y * len[0] - axis[1].y * len[1] + axis[2].y * len[2],
                 cen.z - axis[0].z * len[0] - axis[1].z * len[1] + axis[2].z * len[2]);
      glEnd();

      if( drawMode & DRAW_BV_BLENDED)
        glBegin(GL_QUADS);
      else
        glBegin(GL_LINE_LOOP);
      glVertex3f(cen.x - axis[0].x * len[0] - axis[1].x * len[1] + axis[2].x * len[2],
                 cen.y - axis[0].y * len[0] - axis[1].y * len[1] + axis[2].y * len[2],
                 cen.z - axis[0].z * len[0] - axis[1].z * len[1] + axis[2].z * len[2]);
      glVertex3f(cen.x - axis[0].x * len[0] - axis[1].x * len[1] - axis[2].x * len[2],
                 cen.y - axis[0].y * len[0] - axis[1].y * len[1] - axis[2].y * len[2],
                 cen.z - axis[0].z * len[0] - axis[1].z * len[1] - axis[2].z * len[2]);
      glVertex3f(cen.x - axis[0].x * len[0] + axis[1].x * len[1] - axis[2].x * len[2],
                 cen.y - axis[0].y * len[0] + axis[1].y * len[1] - axis[2].y * len[2],
                 cen.z - axis[0].z * len[0] + axis[1].z * len[1] - axis[2].z * len[2]);
      glVertex3f(cen.x - axis[0].x * len[0] + axis[1].x * len[1] + axis[2].x * len[2],
                 cen.y - axis[0].y * len[0] + axis[1].y * len[1] + axis[2].y * len[2],
                 cen.z - axis[0].z * len[0] + axis[1].z * len[1] + axis[2].z * len[2]);
      glEnd();

      if( drawMode & DRAW_BV_BLENDED)
        glBegin(GL_QUADS);
      else
        glBegin(GL_LINE_LOOP);
      glVertex3f(cen.x - axis[0].x * len[0] + axis[1].x * len[1] - axis[2].x * len[2],
                 cen.y - axis[0].y * len[0] + axis[1].y * len[1] - axis[2].y * len[2],
                 cen.z - axis[0].z * len[0] + axis[1].z * len[1] - axis[2].z * len[2]);
      glVertex3f(cen.x - axis[0].x * len[0] + axis[1].x * len[1] + axis[2].x * len[2],
                 cen.y - axis[0].y * len[0] + axis[1].y * len[1] + axis[2].y * len[2],
                 cen.z - axis[0].z * len[0] + axis[1].z * len[1] + axis[2].z * len[2]);
      glVertex3f(cen.x + axis[0].x * len[0] + axis[1].x * len[1] + axis[2].x * len[2],
                 cen.y + axis[0].y * len[0] + axis[1].y * len[1] + axis[2].y * len[2],
                 cen.z + axis[0].z * len[0] + axis[1].z * len[1] + axis[2].z * len[2]);
      glVertex3f(cen.x + axis[0].x * len[0] + axis[1].x * len[1] - axis[2].x * len[2],
                 cen.y + axis[0].y * len[0] + axis[1].y * len[1] - axis[2].y * len[2],
                 cen.z + axis[0].z * len[0] + axis[1].z * len[1] - axis[2].z * len[2]);
      glEnd();


      if( drawMode & DRAW_BV_BLENDED)
      {
        glBegin(GL_QUADS);
        glVertex3f(cen.x - axis[0].x * len[0] + axis[1].x * len[1] - axis[2].x * len[2],
                   cen.y - axis[0].y * len[0] + axis[1].y * len[1] - axis[2].y * len[2],
                   cen.z - axis[0].z * len[0] + axis[1].z * len[1] - axis[2].z * len[2]);
        glVertex3f(cen.x + axis[0].x * len[0] + axis[1].x * len[1] - axis[2].x * len[2],
                   cen.y + axis[0].y * len[0] + axis[1].y * len[1] - axis[2].y * len[2],
                   cen.z + axis[0].z * len[0] + axis[1].z * len[1] - axis[2].z * len[2]);
        glVertex3f(cen.x + axis[0].x * len[0] - axis[1].x * len[1] - axis[2].x * len[2],
                   cen.y + axis[0].y * len[0] - axis[1].y * len[1] - axis[2].y * len[2],
                   cen.z + axis[0].z * len[0] - axis[1].z * len[1] - axis[2].z * len[2]);
        glVertex3f(cen.x - axis[0].x * len[0] - axis[1].x * len[1] - axis[2].x * len[2],
                   cen.y - axis[0].y * len[0] - axis[1].y * len[1] - axis[2].y * len[2],
                   cen.z - axis[0].z * len[0] - axis[1].z * len[1] - axis[2].z * len[2]);
        glEnd();

        glBegin(GL_QUADS);
        glVertex3f(cen.x - axis[0].x * len[0] + axis[1].x * len[1] + axis[2].x * len[2],
                   cen.y - axis[0].y * len[0] + axis[1].y * len[1] + axis[2].y * len[2],
                   cen.z - axis[0].z * len[0] + axis[1].z * len[1] + axis[2].z * len[2]);
        glVertex3f(cen.x + axis[0].x * len[0] + axis[1].x * len[1] + axis[2].x * len[2],
                   cen.y + axis[0].y * len[0] + axis[1].y * len[1] + axis[2].y * len[2],
                   cen.z + axis[0].z * len[0] + axis[1].z * len[1] + axis[2].z * len[2]);
        glVertex3f(cen.x + axis[0].x * len[0] - axis[1].x * len[1] + axis[2].x * len[2],
                   cen.y + axis[0].y * len[0] - axis[1].y * len[1] + axis[2].y * len[2],
                   cen.z + axis[0].z * len[0] - axis[1].z * len[1] + axis[2].z * len[2]);
        glVertex3f(cen.x - axis[0].x * len[0] - axis[1].x * len[1] + axis[2].x * len[2],
                   cen.y - axis[0].y * len[0] - axis[1].y * len[1] + axis[2].y * len[2],
                   cen.z - axis[0].z * len[0] - axis[1].z * len[1] + axis[2].z * len[2]);
        glEnd();
      }


      if( drawMode & DRAW_BV_BLENDED)
        glColor3f(color.x, color.y, color.z);
    }

  }

  /* DRAW SEPARATING PLANE */
  if( drawMode & DRAW_SEPARATING_PLANE || drawMode & DRAW_ALL)
  {
    if( !(drawMode & DRAW_SINGLE && depth != 0))
    {
      if( drawMode & DRAW_BV_BLENDED)
        glColor4f(color.x, color.y, color.z, .6);

      /* now draw the separation plane */
      Vector a1 = this->bvElement->axis[(this->longestAxisIndex + 1)%3];
      Vector a2 = this->bvElement->axis[(this->longestAxisIndex + 2)%3];
      Vector c = this->bvElement->center;
      float l1 = this->bvElement->halfLength[(this->longestAxisIndex + 1)%3];
      float l2 = this->bvElement->halfLength[(this->longestAxisIndex + 2)%3];
      glBegin(GL_QUADS);
      glVertex3f(c.x + a1.x * l1 + a2.x * l2, c.y + a1.y * l1+ a2.y * l2, c.z + a1.z * l1 + a2.z * l2);
      glVertex3f(c.x - a1.x * l1 + a2.x * l2, c.y - a1.y * l1+ a2.y * l2, c.z - a1.z * l1 + a2.z * l2);
      glVertex3f(c.x - a1.x * l1 - a2.x * l2, c.y - a1.y * l1- a2.y * l2, c.z - a1.z * l1 - a2.z * l2);
      glVertex3f(c.x + a1.x * l1 - a2.x * l2, c.y + a1.y * l1- a2.y * l2, c.z + a1.z * l1 - a2.z * l2);
      glEnd();

      if( drawMode & DRAW_BV_BLENDED)
        glColor4f(color.x, color.y, color.z, 1.0);

    }
  }



  if (depth > 0)
  {
    if( this->nodeLeft != NULL)
      this->nodeLeft->drawBV(depth - 1, drawMode, Color::HSVtoRGB(Color::RGBtoHSV(color)+Vector(15.0,0.0,0.0)), false);
    if( this->nodeRight != NULL)
      this->nodeRight->drawBV(depth - 1, drawMode, Color::HSVtoRGB(Color::RGBtoHSV(color)+Vector(30.0,0.0,0.0)), false);
  }
  this->bvElement->bCollided = false;

  if (top)
    glPopAttrib();
}



void OBBTreeNode::debug() const
{
  PRINT(0)("========OBBTreeNode::debug()=====\n");
  PRINT(0)(" Current depth: %i", this->depth);
  PRINT(0)(" ");
  PRINT(0)("=================================\n");
}