Index: branches/collision_detection/src/lib/collision_detection/obb_tree_node.cc
===================================================================
--- branches/collision_detection/src/lib/collision_detection/obb_tree_node.cc	(revision 5718)
+++ branches/collision_detection/src/lib/collision_detection/obb_tree_node.cc	(revision 5825)
@@ -43,5 +43,5 @@
  */
 OBBTreeNode::OBBTreeNode (const OBBTree& tree, unsigned int depth)
-  : BVTreeNode()
+    : BVTreeNode()
 {
   this->setClassID(CL_OBB_TREE_NODE, "OBBTreeNode");
@@ -139,25 +139,25 @@
 
 
-//     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");
-//     }
+    //     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");
+    //     }
 
   }
@@ -174,56 +174,55 @@
 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");
-//     }
-//   }
+  //   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)
-{
-  const sVec3D* verticesList;
+    const int* triangleIndexes, unsigned int length)
+{
 
   PRINTF(3)("Created OBBox\n");
@@ -251,5 +250,6 @@
 
     /* finding the facelet surface via cross-product */
-    t1 = p - q; t2 = p - r;
+    t1 = p - q;
+    t2 = p - r;
     facelet[i] = 0.5f * fabs( t1.cross(t2).len() );
     /* update the entire convex hull surface */
@@ -263,5 +263,4 @@
   /* take the average of the centroid sum */
   center /= face;
-  PRINTF(3)("-- Calculated Center\n");
 
 
@@ -282,19 +281,22 @@
 
         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];
+                           q[j] * q[k] + r[j] * r[k]) - center[j] * center[k];
       }
     }
   }
-  PRINTF(3)("-- Calculated Covariance\n");
-
-
-  PRINTF(3)("\nVertex Data:\n");
-  for(int i = 0; i < length; i++)
-    PRINTF(3)(" Vertex[%i]: %f, %f, %f\n", i, box.vertices[i][0], box.vertices[i][1], box.vertices[i][2]);
+
   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"); }
+  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)
   {
@@ -304,17 +306,19 @@
   }
   box.center = center;
-
-  PRINTF(3)("-- Written Result to OBB\n");
 }
 
 
 void OBBTreeNode::calculateBoxEigenvectors(OBB& box, const modelInfo& modInfo,
-                                           const int* triangleIndexes, unsigned int length)
-{}
-
-void OBBTreeNode::calculateBoxEigenvectors(OBB& box, const sVec3D* verticesList, unsigned int length)
-{
-  PRINTF(3)("Calculated attributes1\n");
-  /* now getting spanning vectors of the sub-space:
+    const int* triangleIndexes, unsigned int length)
+{
+
+  PRINTF(3)("Calculate the Box Eigenvectors\n");
+
+  const sVec3D*  verticesList;
+  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.
@@ -322,41 +326,15 @@
   vectors
   */
-  Vector axis[3];                //!< the references to the obb axis
-
-  Matrix covMat(  box.covarianceMatrix  );
+
+  /* calculate the axis */
   covMat.getEigenVectors(axis[0], axis[1], axis[2] );
-
-
-  /* new jacobi tests */
-//  JacobI(OBBTreeNode::coMat, OBBTreeNode::eigvlMat, OBBTreeNode::eigvMat, OBBTreeNode::rotCount);
-//  PRINTF(3)("-- Done Jacobi Decomposition\n");
-
-
-//   PRINTF(0)("Jacobi\n");
-//   for(int j = 0; j < 3; ++j)
-//   {
-//     printf(" |");
-//     for(int k = 0; k < 3; ++k)
-//     {
-//       printf(" \t%f ", OBBTreeNode::OBBTreeNode::eigvMat[j][k]);
-//     }
-//     printf(" |\n");
-//   }
-
-/*  axis[0].x = OBBTreeNode::eigvMat[0][0]; axis[0].y = OBBTreeNode::eigvMat[1][0]; axis[0].z = OBBTreeNode::eigvMat[2][0];
-  axis[1].x = OBBTreeNode::eigvMat[0][1]; axis[1].y = OBBTreeNode::eigvMat[1][1]; axis[1].z = OBBTreeNode::eigvMat[2][1];
-  axis[2].x = OBBTreeNode::eigvMat[0][2]; axis[2].y = OBBTreeNode::eigvMat[1][2]; axis[2].z = OBBTreeNode::eigvMat[2][2];
-  axis[0].normalize();
-  axis[1].normalize();
-  axis[2].normalize();*/
   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);
+  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);
 }
 
@@ -386,74 +364,74 @@
   halfLength[0] = -1.0f;
   for(int j = 0; j < length; ++j)
-    {
-      tmpLength = fabs(p0.distancePoint(vertices[j]));
-      if( tmpLength > halfLength[0])
-        halfLength[0] = tmpLength;
-    }
+  {
+    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;
-    }
+  {
+    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;
-    }
+  {
+    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];
-     }
+  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];
+  }
 
 
@@ -489,5 +467,5 @@
   }
 
-   PRINTF(3)("longest axis is: nr %i with a half-length of: %f\n", axisIndex, aLength);
+  PRINTF(3)("longest axis is: nr %i with a half-length of: %f\n", axisIndex, aLength);
 
 
@@ -526,5 +504,6 @@
   for(int i = 0; i < box.numOfVertices; ++i)
   {
-    if( i == vertexIndex) continue;
+    if( i == vertexIndex)
+      continue;
     tmpDist = this->separationPlane.distancePoint(box.vertices[i]);
     if( tmpDist > 0.0)
@@ -561,9 +540,9 @@
   }
   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]);
-//   }
+  //   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();
@@ -590,9 +569,9 @@
   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]);
-//   }
+  //   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]);
+  //   }
 }
 
@@ -605,5 +584,5 @@
   /* 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( unlikely(treeNode == NULL)) return;
 
 
@@ -649,6 +628,6 @@
 bool OBBTreeNode::overlapTest(const OBB& boxA, const OBB& boxB, const WorldEntity& nodeA, const WorldEntity& nodeB) const
 {
-//   if( boxB == NULL || boxA == NULL)
-//     return false;
+  //   if( boxB == NULL || boxA == NULL)
+  //     return false;
 
   /* first check all axis */
@@ -671,12 +650,12 @@
   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);
+  //   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);
 
 
@@ -759,6 +738,6 @@
 
   /* FIXME: there is no collision mark set now */
-//   boxA.bCollided = true; /* use this ONLY(!!!!) for drawing operations */
-//   boxB.bCollided = true;
+  //   boxA.bCollided = true; /* use this ONLY(!!!!) for drawing operations */
+  //   boxB.bCollided = true;
 
 
@@ -865,97 +844,25 @@
     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]);
+      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],
@@ -964,10 +871,52 @@
                  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();
 
-      glBegin(GL_QUADS);
+      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],
@@ -976,16 +925,46 @@
                  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);
+
+
+      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);
     }
 
@@ -1000,19 +979,19 @@
         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);
+      /* 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);
 
     }
@@ -1038,10 +1017,7 @@
 void OBBTreeNode::debug() const
 {
-
-  /*
-  for(int i = 0; i < length; i++)
-  {
-  PRINTF(3)("vertex %i: %f, %f, %f\n", i, verticesList[i][0], verticesList[i][1], verticesList[i][2]);
-}
-  */
-}
+  PRINT(0)("========OBBTreeNode::debug()=====\n");
+  PRINT(0)(" Current depth: %i", this->depth);
+  PRINT(0)(" ");
+  PRINT(0)("=================================\n");
+}
Index: branches/collision_detection/src/lib/collision_detection/obb_tree_node.h
===================================================================
--- branches/collision_detection/src/lib/collision_detection/obb_tree_node.h	(revision 5718)
+++ branches/collision_detection/src/lib/collision_detection/obb_tree_node.h	(revision 5825)
@@ -21,5 +21,6 @@
 
 //! A class that represents a bounding volume tree
-class OBBTreeNode : public BVTreeNode {
+class OBBTreeNode : public BVTreeNode
+{
 
 
@@ -29,5 +30,8 @@
 
     /*  this function returns the bounding volume of this tree node @return: returns the BV */
-    virtual inline const BoundingVolume* getBV() const { return (BoundingVolume*)this->bvElement; }
+    virtual inline const BoundingVolume* getBV() const
+    {
+      return (BoundingVolume*)this->bvElement;
+    }
 
     virtual void spawnBVTree(const sVec3D *verticesList, unsigned int length);
@@ -40,5 +44,4 @@
 
   private:
-    void calculateBoxEigenvectors(OBB& box, const sVec3D* verticesList, unsigned int length);
     void calculateBoxAxis(OBB& box, const sVec3D* verticesList, unsigned int length);
 
@@ -62,4 +65,7 @@
     const OBBTree*      obbTree;                    //!< reference to the obb tree
 
+    const modelInfo*    modelInf;                   //!< pointer to the models modelInfo object
+    const int*          triangleIndexes;            //!< indexes to the used model triangles
+
     const sVec3D*       vertices;                   //!< pointer to the vertices data
     int                 numOfVertices;              //!< number of vertices in vertices data
