source: orxonox.OLD/branches/scriptimprovements/src/lib/collision_detection/aabb_tree_node.cc @ 10558

/*
   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
*/

#define DEBUG_SPECIAL_MODULE 3/* DEBUG_MODULE_COLLISION_DETECTION*/

#include "aabb_tree_node.h"
#include "aabb.h"
#include "bv_tree.h"

#include "collision_tube.h"

#include "matrix.h"
#include "model.h"
#include "world_entity.h"
#include "plane.h"

#include "color.h"
#include "glincl.h"

#include <list>
#include <vector>
#include "debug.h"

GLUquadricObj* AABBTreeNode_sphereObj = NULL;

ObjectListDefinition(AABBTreeNode);
/**
 *  standard constructor
 * @param tree: reference to the obb tree
 * @param depth: the depth of the obb tree to generate
 */
AABBTreeNode::AABBTreeNode (const OBBTree& tree, AABBTreeNode* prev, int depth)
    : BVTreeNode()
{
  this->registerObject(this, AABBTreeNode::_objectList);

  this->obbTree = &tree;
  this->nodePrev = prev;
  this->depth = depth;
  this->nextID = 0;

  this->init();
}


/**
 *  standard constructor
 * @param depth: the depth of the obb tree to generate
 */
AABBTreeNode::AABBTreeNode(int depth)
{
  this->depth = depth;

  this->init();
}

/**
 *  init funciton
 */
void AABBTreeNode::init()
{
  this->nodeLeft = NULL;
  this->nodeRight = NULL;
  this->bvElement = NULL;

  this->triangleIndexList1 = NULL;
  this->triangleIndexList2 = NULL;

  this->modelInf = NULL;
  this->triangleIndexes = NULL;

  if( AABBTreeNode_sphereObj == NULL)
    AABBTreeNode_sphereObj = gluNewQuadric();

  this->owner = NULL;
}

/**
 *  standard deconstructor
 */
AABBTreeNode::~AABBTreeNode ()
{
  if( this->nodeLeft)
    delete this->nodeLeft;
  if( this->nodeRight)
    delete this->nodeRight;

  if( this->bvElement)
    delete this->bvElement;
}



void AABBTreeNode::spawnBVTree(Model* model)
{

  const modelInfo* modelInf = model->getModelInfo();

  int* triangleIndexes = new int[modelInf->numTriangles];
  for(unsigned int i = 0; i < modelInf->numTriangles; ++i)
    triangleIndexes[i] = i;

  this->spawnBVTree(*modelInf, triangleIndexes, modelInf->numTriangles);
}


/**
 *  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 AABBTreeNode::spawnBVTree(const modelInfo& modelInf, const int* triangleIndexes, int length)
{
  PRINTF(4)("\n==============================Creating AABB Tree Node==================\n");
  PRINT(4)(" AABB Tree Infos: \n");
  PRINT(4)("\tDepth: %i \n\tTree Index: %i \n\tNumber of Triangles: %i\n", depth, this->treeIndex, length);
  this->depth = depth;

  this->bvElement = new AABB();
  this->bvElement->modelInf = &modelInf;
  this->bvElement->triangleIndexes = triangleIndexes;
  this->bvElement->triangleIndexesLength = length;

  /* create the bounding 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->triangleIndexLength1 >= 3)
    {
      this->nodeLeft = new AABBTreeNode(*this->obbTree, this, depth - 1);
      this->nodeLeft->spawnBVTree(modelInf, this->triangleIndexList1, this->triangleIndexLength1);
    }
    if( this->triangleIndexLength2 >= 3)
    {
      this->nodeRight = new AABBTreeNode(*this->obbTree, this, depth - 1);
      this->nodeRight->spawnBVTree(modelInf, this->triangleIndexList2, this->triangleIndexLength2);
    }
  }
}



/**
 *  calculate the box covariance matrix
 * @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 AABBTreeNode::calculateBoxCovariance(AABB& box, const modelInfo& modelInf, const int* triangleIndexes, int length)
{
  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

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

    /* 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) / 3.0f;
    /* now calculate the centroid of the entire convex hull, weighted average of triangle centroids */
    center += centroid[i] * facelet[i];
    /* the arithmetical center */
  }
  /* 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 < length; ++i)
      {
        p = (&modelInf.pVertices[modelInf.pTriangles[triangleIndexes[i]].indexToVertices[0]]);
        q = (&modelInf.pVertices[modelInf.pTriangles[triangleIndexes[i]].indexToVertices[1]]);
        r = (&modelInf.pVertices[modelInf.pTriangles[triangleIndexes[i]].indexToVertices[2]]);

        covariance[j][k] = facelet[i] * (9.0f * centroid[i][j] * centroid[i][k] + p[j] * p[k] +
                           q[j] * q[k] + r[j] * r[k]);
      }
      covariance[j][k] = covariance[j][k] / (12.0f * face) - center[j] * center[k];
    }
  }
  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;

  /* debug output section*/
  PRINTF(4)("\nOBB Covariance Matrix:\n");
  for(int j = 0; j < 3; ++j)
  {
    PRINT(4)("\t\t");
    for(int k = 0; k < 3; ++k)
    {
      PRINT(4)("%11.4f\t", covariance[j][k]);
    }
    PRINT(4)("\n");
  }
  PRINTF(4)("\nWeighteed AABB Center:\n\t\t%11.4f\t %11.4f\t %11.4f\n", center.x, center.y, center.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 AABBTreeNode::calculateBoxEigenvectors(AABB& box, const modelInfo& modelInf,
    const int* triangleIndexes, int length)
{
  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
  */


  // this is for axis aligned bouning boxes only
   box.axis[0] = Vector(1,0,0);
   box.axis[1] = Vector(0,1,0);
   box.axis[2] = Vector(0,0,1);

   PRINTF(4)("Eigenvectors:\n");
   PRINT(4)("\t\t%11.2f \t%11.2f \t%11.2f\n", box.axis[0].x, box.axis[0].y, box.axis[0].z);
   PRINT(4)("\t\t%11.2f \t%11.2f \t%11.2f\n", box.axis[1].x, box.axis[1].y, box.axis[1].z);
   PRINT(4)("\t\t%11.2f \t%11.2f \t%11.2f\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 AABBTreeNode::calculateBoxAxis(AABB& box, const modelInfo& modelInf, const int* triangleIndexes, int length)
{

  PRINTF(4)("Calculate Box Axis\n");
  /* now get the axis length */
  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);           //!< the axis planes
  Plane               p2(box.axis[2], box.center);           //!< the axis planes
  float               maxLength[3];                          //!< maximal lenth of the axis
  float               minLength[3];                          //!< minimal length of the axis
  const float*        tmpVec;                                //!< variable taking tmp vectors
  float               centerOffset[3];

  /* get the maximal dimensions of the body in all directions */
  /* for the initialisation the value just has to be inside of the polygon soup -> first vertices (rand) */
  for( int k = 0; k  < 3; k++)
  {
    tmpVec = (&modelInf.pVertices[modelInf.pTriangles[triangleIndexes[0]].indexToVertices[0]]);
    Plane* p;
    if( k == 0)
      p = &p0;
    else if( k == 1)
      p = &p1;
    else
      p = &p2;
    maxLength[k] = p->distancePoint(tmpVec);
    minLength[k] = p->distancePoint(tmpVec);

    for( int j = 0; j < length; ++j) {
      for( int i = 0; i < 3; ++i) {
        tmpVec = &modelInf.pVertices[modelInf.pTriangles[triangleIndexes[j]].indexToVertices[i]];
        tmpLength = p->distancePoint(tmpVec);

        if( tmpLength > maxLength[k])
          maxLength[k] = tmpLength;
        else if( tmpLength < minLength[k])
          minLength[k] = tmpLength;
      }
    }
  }



  /* calculate the real centre of the body by using the axis length */
  for( int i = 0; i < 3; ++i)
  {
    if( maxLength[i] > 0.0f && minLength[i] > 0.0f)  // both axis positiv
      centerOffset[i] = minLength[i] + (maxLength[i] - minLength[i]) / 2.0f;
    else if( maxLength[i] > 0.0f && maxLength[i] < 0.0f) // positiv and negativ
      centerOffset[i] = (maxLength[i] + minLength[i]) / 2.0f;
    else //both negativ
     centerOffset[i] = minLength[i] + (maxLength[i] - minLength[i]) / 2.0f;

    box.halfLength[i] = (maxLength[i] - minLength[i]) / 2.0f;
  }

  box.center += (box.axis[0] * centerOffset[0]);
  box.center += (box.axis[1] * centerOffset[1]);
  box.center += (box.axis[2] * centerOffset[2]);


  PRINTF(4)("\n");
  PRINT(4)("\tAxis halflength x: %11.2f (max: %11.2f, \tmin: %11.2f), offset: %11.2f\n", box.halfLength[0], maxLength[0], minLength[0], centerOffset[0]);
  PRINT(4)("\tAxis halflength y: %11.2f (max: %11.2f, \tmin: %11.2f), offset: %11.2f\n", box.halfLength[1], maxLength[1], minLength[1], centerOffset[1] );
  PRINT(4)("\tAxis halflength z: %11.2f (max: %11.2f, \tmin: %11.2f), offset: %11.2f\n", box.halfLength[2], maxLength[2], minLength[2], centerOffset[2]);
}



/**
 *  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 AABBTreeNode::forkBox(AABB& box)
{

  PRINTF(4)("Fork Box\n");
  PRINTF(4)("Calculating the longest Axis\n");
  /* get the longest axis of the box */
  float               longestAxis = -1.0f;                 //!< the length of the longest axis
  int                 longestAxisIndex = 0;                //!< this is the nr of the longest axis


  /* now get the longest axis of the three exiting */
  for( int i = 0; i < 3; ++i)
  {
    if( longestAxis < box.halfLength[i])
    {
      longestAxis = box.halfLength[i];
      longestAxisIndex = i;
    }
  }
  PRINTF(4)("\nLongest Axis is: Nr %i with a half-length of:%11.2f\n", longestAxisIndex, longestAxis);


  PRINTF(4)("Separating along the longest axis\n");
  /* get the closest vertex near the center */
  float               tmpDist;                             //!< variable to save diverse distances temporarily
  Plane               middlePlane(box.axis[longestAxisIndex], box.center); //!< the middle plane


  /* now definin the separation plane through this specified nearest point and partition
  the points depending on which side they are located
  */
  std::list<int>           partition1;                           //!< the vertex partition 1
  std::list<int>           partition2;                           //!< the vertex partition 2
  float*                   triangleCenter = new float[3];        //!< the center of the triangle
  const float*             a;                                    //!< triangle  edge a
  const float*             b;                                    //!< triangle  edge b
  const float*             c;                                    //!< triangle  edge c


  /* find the center of the box */
  this->separationPlane = Plane(box.axis[longestAxisIndex], box.center);
  this->sepPlaneCenter[0] = box.center.x;
  this->sepPlaneCenter[1] = box.center.y;
  this->sepPlaneCenter[2] = box.center.z;
  this->longestAxisIndex = longestAxisIndex;

  for( int i = 0; i < box.triangleIndexesLength; ++i)
  {
    /* first calculate the middle of the triangle */
    a = &box.modelInf->pVertices[box.modelInf->pTriangles[box.triangleIndexes[i]].indexToVertices[0]];
    b = &box.modelInf->pVertices[box.modelInf->pTriangles[box.triangleIndexes[i]].indexToVertices[1]];
    c = &box.modelInf->pVertices[box.modelInf->pTriangles[box.triangleIndexes[i]].indexToVertices[2]];

    triangleCenter[0] = (a[0] + b[0] + c[0]) / 3.0f;
    triangleCenter[1] = (a[1] + b[1] + c[1]) / 3.0f;
    triangleCenter[2] = (a[2] + b[2] + c[2]) / 3.0f;
    tmpDist = this->separationPlane.distancePoint(*((sVec3D*)triangleCenter));

    if( tmpDist > 0.0f)
      partition1.push_back(box.triangleIndexes[i]); /* positive numbers plus zero */
    else if( tmpDist < 0.0f)
      partition2.push_back(box.triangleIndexes[i]); /* negatice numbers */
    else {
      partition1.push_back(box.triangleIndexes[i]); /* 0.0f? unprobable... */
      partition2.push_back(box.triangleIndexes[i]);
    }
  }
  PRINTF(4)("\nPartition1: got \t%i Vertices \nPartition2: got \t%i Vertices\n", partition1.size(), partition2.size());


  /* now comes the separation into two different sVec3D arrays */
  int                index;                                //!< index storage place
  int*               triangleIndexList1;                   //!< the vertex list 1
  int*               triangleIndexList2;                   //!< the vertex list 2
  std::list<int>::iterator element;                        //!< the list iterator

  triangleIndexList1 = new int[partition1.size()];
  triangleIndexList2 = new int[partition2.size()];

  for( element = partition1.begin(), index = 0; element != partition1.end(); element++, index++)
    triangleIndexList1[index] = (*element);

  for( element = partition2.begin(), index = 0; element != partition2.end(); element++, index++)
    triangleIndexList2[index] = (*element);

  if( this->triangleIndexList1!= NULL)
    delete[] this->triangleIndexList1;
  this->triangleIndexList1 = triangleIndexList1;
  this->triangleIndexLength1 = partition1.size();

  if( this->triangleIndexList2 != NULL)
    delete[] this->triangleIndexList2;
  this->triangleIndexList2 = triangleIndexList2;
  this->triangleIndexLength2 = partition2.size();
}



/**
 * collides one tree with an other
 *  @param treeNode the other bv tree node
 *  @param nodeA  the worldentity belonging to this bv
 *  @param nodeB the worldentity belonging to treeNode
 */
void AABBTreeNode::collideWith(BVTreeNode* treeNode, WorldEntity* nodeA, WorldEntity* nodeB)
{
  if( unlikely(treeNode == NULL || nodeA == NULL || nodeB == NULL))
    return;

  PRINTF(4)("collideWith\n");
  PRINTF(5)("Checking AABB %i vs %i: ", this->getIndex(), treeNode->getIndex());

  // for now only collide with AABBTreeNodes
  this->collideWithOBB((AABBTreeNode*)treeNode, nodeA, nodeB);
}



/**
 * collides one obb tree with an other
 *  @param treeNode the other bv tree node
 *  @param nodeA  the worldentity belonging to this bv
 *  @param nodeB the worldentity belonging to treeNode
 */
void AABBTreeNode::collideWithOBB(AABBTreeNode* treeNode, WorldEntity* nodeA, WorldEntity* nodeB)
{

  if( this->overlapTest(this->bvElement, treeNode->bvElement, nodeA, nodeB))
  {
    PRINTF(5)("collision @ lvl %i, object %s::%s vs. %s::%s, (%p, %p)\n", this->depth, nodeA->getClassCName(), nodeA->getCName(), nodeB->getClassCName(), nodeA->getCName(), this->nodeLeft, this->nodeRight);


    // left node
    if( this->nodeLeft != NULL )
    {
      if( this->overlapTest(this->nodeLeft->bvElement, treeNode->bvElement, nodeA, nodeB))
      {
        if( treeNode->nodeLeft != NULL)
          this->nodeLeft->collideWith(treeNode->nodeLeft, nodeA, nodeB);
        if( treeNode->nodeRight != NULL)
          this->nodeLeft->collideWith(treeNode->nodeRight, nodeA, nodeB);
      }
    }

    // right node
    if( this->nodeRight != NULL )
    {
      if( this->overlapTest(this->nodeRight->bvElement, treeNode->bvElement, nodeA, nodeB))
      {
        if( treeNode->nodeLeft != NULL)
          this->nodeRight->collideWith(treeNode->nodeLeft, nodeA, nodeB);
        if( treeNode->nodeRight != NULL)
          this->nodeRight->collideWith(treeNode->nodeRight, nodeA, nodeB);
      }
    }


    // hybrid mode: we reached the end of this obbtree, now reach the end of the other tree
    if( this->nodeLeft == NULL && this->nodeRight == NULL)
    {
      if( treeNode->nodeLeft != NULL)
        this->collideWith(treeNode->nodeLeft, nodeA, nodeB);
      if( treeNode->nodeRight != NULL)
        this->collideWith(treeNode->nodeRight, nodeA, nodeB);
    }


    // now check if we reached the end of both trees
    if( unlikely((this->nodeRight == NULL && this->nodeLeft == NULL) &&
        (treeNode->nodeRight == NULL && treeNode->nodeLeft == NULL)) )
    {
      CoRe::CollisionTube::getInstance()->registerCollisionEvent( nodeA, nodeB, (BoundingVolume*)this->bvElement, (BoundingVolume*)treeNode->bvElement);
    }

  }
}


/**
 * this actualy checks if one obb box touches the other
 * @param boxA the box from nodeA
 * @param boxB the box from nodeB
 * @param nodeA the node itself
 * @param nodeB the node itself
 */
bool AABBTreeNode::overlapTest(AABB* boxA, AABB* boxB, WorldEntity* nodeA, WorldEntity* nodeB)
{
  //HACK remove this again
  this->owner = nodeA;
  //   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));

  /* 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(5)("s = %f, rA+rB = %f\n", fabs(t.dot(l)), rA+rB);

    if( (rA + rB) < fabs(t.dot(l)))
    {
      PRINTF(4)("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(5)("s = %f, rA+rB = %f\n", fabs(t.dot(l)), rA+rB);

    if( (rA + rB) < fabs(t.dot(l)))
    {
      PRINTF(4)("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(5)("s = %f, rA+rB = %f\n", fabs(t.dot(l)), rA+rB);

      if( (rA + rB) < fabs(t.dot(l)))
      {
        PRINTF(4)("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(4)("Kollision!\n");
  return true;
}


/**
 *
 * draw the BV tree - debug mode
 */
void AABBTreeNode::drawBV(int depth, int drawMode, const Vector& color,  bool top) const
{
  /* this function can be used to draw the triangles and/or the points only  */
  if( 1 /*drawMode & DRAW_MODEL || drawMode & DRAW_ALL*/)
  {
    if( depth == 0/*!(drawMode & DRAW_SINGLE && depth != 0)*/)
    {
      if( 1 /*drawMode & DRAW_POINTS*/)
      {
        glBegin(GL_POINTS);
        glColor3f(0.3, 0.8, 0.54);
        for(unsigned int i = 0; i < this->bvElement->modelInf->numVertices*3; i+=3)
          glVertex3f(this->bvElement->modelInf->pVertices[i],
                     this->bvElement->modelInf->pVertices[i+1],
                     this->bvElement->modelInf->pVertices[i+2]);
        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( 1 /*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( 1/*drawMode & DRAW_BV_AXIS || drawMode & DRAW_ALL*/)
  {
    if( 1/*drawMode & DRAW_SINGLE && depth != 0*/)
    {
      /* draw the obb axes */
      glBegin(GL_LINES);
      glColor3f(1.0, 0.0, 0.0);
      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]);

      glColor3f(0.0, 1.0, 0.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]);

      glColor3f(0.0, 0.0, 1.0);
      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( depth == 0 /*!(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 AABBTreeNode::debug() const
{
  PRINT(0)("========AABBTreeNode::debug()=====\n");
  PRINT(0)(" Current depth: %i", this->depth);
  PRINT(0)(" ");
  PRINT(0)("=================================\n");
}