Changeset 7365 in orxonox.OLD for branches/cd/src/lib/collision_detection/obb_tree_node.cc

Timestamp:

Apr 25, 2006, 3:17:20 PM (18 years ago)

Author:

bensch

Message:

orxonox/branches/cd: merged the new collision-detection back.
merged and collissions resolved.

File:

• branches/cd/src/lib/collision_detection/obb_tree_node.cc (modified) (28 diffs)

branches/cd/src/lib/collision_detection/obb_tree_node.cc

@@ -11 +11 @@
 ### File Specific:
    main-programmer: Patrick Boenzli
-   co-programmer: ...
 */
 
-#define DEBUG_SPECIAL_MODULE DEBUG_MODULE_COLLISION
+#define DEBUG_SPECIAL_MODULE DEBUG_MODULE_COLLISION_DETECTION
 
 #include "obb_tree_node.h"
-#include "list.h"
+#include "obb_tree.h"
 #include "obb.h"
-#include "obb_tree.h"
+
 #include "matrix.h"
 #include "model.h"
@@ -26 +25 @@
 
 #include "color.h"
-
+#include "glincl.h"
+
+#include <list>
+#include <vector>
 #include "debug.h"
-#include "glincl.h"
 
 
@@ -34 +35 @@
 using namespace std;
 
-OBBTree*  OBBTreeNode::obbTree = NULL;
-
-float**  OBBTreeNode::coMat = NULL;
-float**  OBBTreeNode::eigvMat = NULL;
-float*   OBBTreeNode::eigvlMat = NULL;
-int*     OBBTreeNode::rotCount = NULL;
+
 GLUquadricObj* OBBTreeNode_sphereObj = NULL;
+
 
 /**
  *  standard constructor
+ * @param tree: reference to the obb tree
+ * @param depth: the depth of the obb tree to generate
  */
-OBBTreeNode::OBBTreeNode ()
+OBBTreeNode::OBBTreeNode (const OBBTree& tree, OBBTreeNode* prev, int depth)
+    : BVTreeNode()
 {
   this->setClassID(CL_OBB_TREE_NODE, "OBBTreeNode");
+
+  this->obbTree = &tree;
+  this->nodePrev = prev;
+  this->depth = depth;
+  this->nextID = 0;
+
   this->nodeLeft = NULL;
   this->nodeRight = NULL;
   this->bvElement = 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;
+  this->triangleIndexList1 = NULL;
+  this->triangleIndexList2 = NULL;
+
+  this->modelInf = NULL;
+  this->triangleIndexes = NULL;
 
   if( OBBTreeNode_sphereObj == NULL)
     OBBTreeNode_sphereObj = gluNewQuadric();
+
+  this->owner = NULL;
+
+  /* debug ids */
+  if( this->nodePrev)
+    this->treeIndex = 100 * this->depth + this->nodePrev->getID();
+  else
+    this->treeIndex = 0;
 }
 
@@ -82 +83 @@
 {
   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->triangleIndexList1 != NULL)
+//     delete [] this->triangleIndexList1;
+//   if( this->triangleIndexList2 != NULL)
+//     delete [] this->triangleIndexList2;
 }
 
@@ -105 +105 @@
  * on the triangle informations (triangle soup not polygon soup)
  */
-void OBBTreeNode::spawnBVTree(const int depth, const modelInfo& modInfo)
-{
-  int length = 0;
-  sVec3D* verticesList;
-
-  PRINT(3)("\n");
-  this->treeIndex = this->obbTree->getID();
-  PRINTF(3)("OBB Depth: %i, tree index: %i, numVertices: %i\n", depth, treeIndex, length);
+void OBBTreeNode::spawnBVTree(const modelInfo& modelInf, const int* triangleIndexes, int length)
+{
+  PRINTF(3)("\n==============================Creating OBB Tree Node==================\n");
+  PRINT(3)(" OBB Tree Infos: \n");
+  PRINT(3)("\tDepth: %i \n\tTree Index: %i \n\tNumber of Triangles: %i\n", depth, this->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, modInfo);
-  PRINTF(3)("Calculated attributes1\n");
-  this->calculateBoxEigenvectors(this->bvElement, modInfo);
-  PRINTF(3)("Calculated attributes2\n");
-  this->calculateBoxAxis(this->bvElement,modInfo);
-  PRINTF(3)("Calculated attributes3\n");
-
-  /* if this is the first node, the vertices data are the original ones of the model itself, so dont delete them in cleanup */
-  if( this->treeIndex == 1)
-    this->bvElement->bOrigVertices = true;
-
+  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->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");
-//     }
-
-  }
-}
+    this->forkBox(*this->bvElement);
+
+    if( this->triangleIndexLength1 >= 3)
+    {
+      this->nodeLeft = new OBBTreeNode(*this->obbTree, this, depth - 1);
+      this->nodeLeft->spawnBVTree(modelInf, this->triangleIndexList1, this->triangleIndexLength1);
+    }
+    if( this->triangleIndexLength2 >= 3)
+    {
+      this->nodeRight = new OBBTreeNode(*this->obbTree, this, depth - 1);
+      this->nodeRight->spawnBVTree(modelInf, this->triangleIndexList2, this->triangleIndexLength2);
+    }
+  }
+}
+
 
 
 /**
- *  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)
+ *  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 OBBTreeNode::spawnBVTree(const int depth, const sVec3D *verticesList, unsigned int length)
-{
-  PRINT(3)("\n");
-  this->treeIndex = this->obbTree->getID();
-  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 this is the first node, the vertices data are the original ones of the model itself, so dont delete them in cleanup */
-  if( this->treeIndex == 1)
-    this->bvElement->bOrigVertices = true;
-
-  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");
-    }
-  }
-}
-
-
-void OBBTreeNode::calculateBoxCovariance(OBB* box, const modelInfo& modInfo)
-{}
-
-
-void OBBTreeNode::calculateBoxCovariance(OBB* box, const sVec3D* verticesList, unsigned int length)
+void OBBTreeNode::calculateBoxCovariance(OBB& box, const modelInfo& modelInf, const int* triangleIndexes, int length)
 {
   float     facelet[length];                         //!< surface area of the i'th triangle of the convex hull
@@ -235 +158 @@
   Vector    t1, t2;                                  //!< temporary values
   float     covariance[3][3] = {0,0,0, 0,0,0, 0,0,0};//!< the covariance matrix
-  int       mode = 0;                                //!< mode = 0: vertex soup, no connections, mode = 1: 3 following verteces build a triangle
-
-  this->numOfVertices = length;
-  this->vertices = verticesList;
-
-
-  if( likely(mode == 0))
-  {
-    /* fist compute all the convex hull face/facelets and centroids */
-    for( int i = 0; i+3 < length ; i+=3)          /* FIX-ME-QUICK: hops of 3, array indiscontinuity*/
-    {
-      p = verticesList[i];
-      q = verticesList[i + 1];
-      r = verticesList[i + 2];
-
-      t1 = p - q; t2 = p - r;
-
-      /* finding the facelet surface via cross-product */
-      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;
-    PRINTF(3)("-- Calculated Center\n");
-
-
-    /* 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)
+  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) / 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)
       {
-        for( int i = 0; i + 3 < length; i+=3)
-        {
-          p = verticesList[i];
-          q = verticesList[i + 1];
-          r = verticesList[i + 2];
-
-          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];
-        }
+        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] * (9.0f * centroid[i][j] * centroid[i][k] + p[j] * p[k] +
+                           q[j] * q[k] + r[j] * r[k]);
       }
-    }
-    PRINTF(3)("-- Calculated Covariance\n");
-  }
-  else if( mode == 1)
-  {
-    for( int i = 0; i + 3 < length; i+=3)          /* FIX-ME-QUICK: hops of 3, array indiscontinuity*/
-    {
-      p = verticesList[i];
-      q = verticesList[i + 1];
-      r = verticesList[i + 2];
-
-      centroid[i] = (p + q + r) / 3.0f;
-      center += centroid[i];
-    }
-    center /= length;
-
-    for( int j = 0; j < 3; ++j)
-    {
-      for( int k = 0; k < 3; ++k)
-      {
-        for( int i = 0; i + 3 < length; i+=3)
-        {
-          p = verticesList[i];
-          q = verticesList[i +1];
-          r = verticesList[i + 2];
-
-          covariance[j][k] = p[j] * p[k] + q[j] * q[k] + r[j] + r[k];
-        }
-        covariance[j][k] /= (3.0f * length);
-      }
-    }
-    PRINTF(3)("-- Calculated Covariance\n");
-  }
-  else if( mode == 2)
-  {
-    /* fist compute all the convex hull face/facelets and centroids */
-    for(int i = 0; i + 3 < length; i+=3)          /* FIX-ME-QUICK: hops of 3, array indiscontinuity*/
-    {
-      p = verticesList[i];
-      q = verticesList[i + 1];
-      r = verticesList[i + 2];
-
-      t1 = p - q; t2 = p - r;
-
-      /* finding the facelet surface via cross-product */
-      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;
-    PRINTF(3)("-- Calculated Center\n");
-
-    for( int j = 0; j < 3; ++j)
-    {
-      for( int k = 0; k < 3; ++k)
-      {
-        for( int i = 0; i + 3 < length; i+=3)
-        {
-          p = verticesList[i];
-          q = verticesList[i +1];
-          r = verticesList[i + 2];
-
-          covariance[j][k] = p[j] * p[k] + q[j] * q[k] + r[j] + r[k];
-        }
-        covariance[j][k] /= (3.0f * length);
-      }
-    }
-    PRINTF(3)("-- Calculated Covariance\n");
-  }
-  else
-  {
-    for( int i = 0; i < length; ++i)          /* FIX-ME-QUICK: hops of 3, array indiscontinuity*/
-    {
-      center += verticesList[i];
-    }
-    center /= length;
-
-    for( int j = 0; j < 3; ++j)
-    {
-      for( int k = 0; k < 3; ++k)
-      {
-        for( int i = 0; i + 3 < length; i+=3)
-        {
-          p = verticesList[i];
-          q = verticesList[i +1];
-          r = verticesList[i + 2];
-
-          covariance[j][k] = p[j] * p[k] + q[j] * q[k] + r[j] + r[k];
-        }
-        covariance[j][k] /= (3.0f * length);
-      }
-    }
-    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)("\nCovariance Matrix:\n");
+      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;
+
+
+  std::vector<int>           vertIndexVector;                           //!< vertex indexes list
+  int                        vertIndex;                                 //!< index to vertex
+  bool                       vertexFound;                               //!< vertex found flag
+  Vector                     arithCenter;                               //!< aritmetical center
+
+  /* calculate the arithmetical center of the box */
+
+  /* go thourgh all vertices, add only the used vertices indexes */
+//   for( int i = 0; i < length; ++i)
+//   {
+//     for(int j = 0; j < 3; ++j)
+//     {
+//       vertIndex = modelInf.pTriangles[triangleIndexes[i]].indexToVertices[j];
+//
+//       vertexFound = false;
+//       for( int i = 0; i < vertIndexVector.size(); i++)
+//       {
+//         if( vertIndexVector[i] == vertIndex)
+//           vertexFound = true;
+//       }
+//       if( !vertexFound)
+//         vertIndexVector.push_back(vertIndex);
+//     }
+//   }
+//   /* now realy calculate the center */
+//   for( int i = 0; i < vertIndexVector.size(); ++i)
+//   {
+//     tmpVec = (sVec3D*)(&modelInf.pVertices[vertIndexVector[i]]);
+//     arithCenter += *tmpVec;
+//   }
+//   box.arithCenter = arithCenter / vertIndexVector.size();
+
+
+
+  /* debug output section*/
+  PRINTF(3)("\nOBB Covariance Matrix:\n");
   for(int j = 0; j < 3; ++j)
   {
-    PRINT(3)(" |");
+    PRINT(3)("\t\t");
     for(int k = 0; k < 3; ++k)
     {
-      PRINT(3)(" \b%f ", covariance[j][k]);
-    }
-    PRINT(3)(" |\n");
-  }
-
-  PRINTF(3)("center: %f, %f, %f\n", center.x, center.y, center.z);
-
-
-  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;
-  PRINTF(3)("-- Written Result to obb\n");
-}
-
-
-void OBBTreeNode::calculateBoxEigenvectors(OBB* box, const modelInfo& modInfo)
-{}
-
-void OBBTreeNode::calculateBoxEigenvectors(OBB* box, const sVec3D* verticesList, unsigned int length)
-{
-
-  /* now getting spanning vectors of the sub-space:
+      PRINT(3)("%11.4f\t", covariance[j][k]);
+    }
+    PRINT(3)("\n");
+  }
+  PRINTF(3)("\nWeighteed OBB Center:\n\t\t%11.4f\t %11.4f\t %11.4f\n", center.x, center.y, center.z);
+//   PRINTF(3)("\nArithmetical OBB Center:\n\t\t%11.4f\t %11.4f\t %11.4f\n", box.arithCenter.x, box.arithCenter.y, box.arithCenter.z);
+
+  /* write back the covariance matrix data to the object oriented bouning box */
+}
+
+
+
+/**
+ *  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, int length)
+{
+
+  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.
@@ -427 +292 @@
   vectors
   */
-  Vector*              axis = new Vector[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 = axis;
-
-//   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);
-}
-
-
-void OBBTreeNode::calculateBoxAxis(OBB* box, const modelInfo& modInfo)
-{
-  this->calculateBoxAxis(box, (const sVec3D*)modInfo.pVertices, modInfo.numVertices);
-}
-
-
-
-void OBBTreeNode::calculateBoxAxis(OBB* box, const sVec3D* verticesList, unsigned int length)
-{
-
+  box.axis[0] = axis[0];
+  box.axis[1] = axis[1];
+  box.axis[2] = axis[2];
+
+  PRINTF(3)("Eigenvectors:\n");
+  PRINT(3)("\t\t%11.2f \t%11.2f \t%11.2f\n", box.axis[0].x, box.axis[0].y, box.axis[0].z);
+  PRINT(3)("\t\t%11.2f \t%11.2f \t%11.2f\n", box.axis[1].x, box.axis[1].y, box.axis[1].z);
+  PRINT(3)("\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 OBBTreeNode::calculateBoxAxis(OBB& box, const modelInfo& modelInf, const int* triangleIndexes, int length)
+{
+
+  PRINTF(3)("Calculate Box Axis\n");
   /* now get the axis length */
   Line                ax[3];                                 //!< the axis
-  float*              halfLength = new float[3];             //!< half length of 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;
-    }
-
+  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 sVec3D*       tmpVec;                                //!< variable taking tmp vectors
 
 
   /* 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 = halfLength;
-  PRINTF(3)("-- Written Axis to obb\n");
-  PRINTF(3)("-- Finished Calculating Attributes\n");
+  /* for the initialisation the value just has to be inside of the polygon soup -> first vertices (rand) */
+  tmpVec = (sVec3D*)(&modelInf.pVertices[modelInf.pTriangles[triangleIndexes[0]].indexToVertices[0]]);
+  maxLength[0] = p0.distancePoint(*tmpVec);
+  minLength[0] = p0.distancePoint(*tmpVec);
+  for( int j = 0; j < length; ++j)
+  {
+    for( int i = 0; i < 3; ++i)
+    {
+      tmpVec = (sVec3D*)(&modelInf.pVertices[modelInf.pTriangles[triangleIndexes[j]].indexToVertices[i]]);
+      tmpLength = p0.distancePoint(*tmpVec);
+      if( tmpLength > maxLength[0])
+        maxLength[0] = tmpLength;
+      else if( tmpLength < minLength[0])
+        minLength[0] = tmpLength;
+    }
+  }
+
+  /* for the initialisation the value just has to be inside of the polygon soup -> first vertices (rand) */
+  tmpVec = (sVec3D*)(&modelInf.pVertices[modelInf.pTriangles[triangleIndexes[0]].indexToVertices[0]]);
+  maxLength[1] = p1.distancePoint(*tmpVec);
+  minLength[1] = p1.distancePoint(*tmpVec);
+  for( int j = 0; j < length; ++j)
+  {
+    for( int i = 0; i < 3; ++i)
+    {
+      tmpVec = (sVec3D*)(&modelInf.pVertices[modelInf.pTriangles[triangleIndexes[j]].indexToVertices[i]]);
+      tmpLength = p1.distancePoint(*tmpVec);
+      if( tmpLength > maxLength[1])
+        maxLength[1] = tmpLength;
+      else if( tmpLength < minLength[1])
+        minLength[1] = tmpLength;
+    }
+  }
+
+  /* for the initialisation the value just has to be inside of the polygon soup -> first vertices (rand) */
+  tmpVec = (sVec3D*)(&modelInf.pVertices[modelInf.pTriangles[triangleIndexes[0]].indexToVertices[0]]);
+  maxLength[2] = p2.distancePoint(*tmpVec);
+  minLength[2] = p2.distancePoint(*tmpVec);
+  for( int j = 0; j < length; ++j)
+  {
+    for( int i = 0; i < 3; ++i)
+    {
+      tmpVec = (sVec3D*)(&modelInf.pVertices[modelInf.pTriangles[triangleIndexes[j]].indexToVertices[i]]);
+      tmpLength = p2.distancePoint(*tmpVec);
+      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];
+
+  for( int i = 0; i < 3; ++i)
+  {
+    centerOffset[i] = (maxLength[i] + minLength[i]) / 2.0f;       // min length is negatie
+    box.halfLength[i] = (maxLength[i] - minLength[i]) / 2.0f;      // min length is negative
+  }
+  box.center.x += centerOffset[0];
+  box.center.y += centerOffset[1];
+  box.center.z += centerOffset[2];
+
+  PRINTF(3)("\n");
+  PRINT(3)("\tAxis Length x: %f (max: %11.2f, \tmin: %11.2f)\n", halfLength[0], maxLength[0], minLength[0]);
+  PRINT(3)("\tAxis Length x: %f (max: %11.2f, \tmin: %11.2f)\n", halfLength[1], maxLength[1], minLength[1]);
+  PRINT(3)("\tAxis Length x: %f (max: %11.2f, \tmin: %11.2f)\n", halfLength[2], maxLength[2], minLength[2]);
+
+
+//   box.halfLength[0] = halfLength[0];
+//   box.halfLength[1] = halfLength[1];
+//   box.halfLength[2] = halfLength[2];
 }
 
@@ -570 +410 @@
 
 /**
-  \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
+ *  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)
-{
+void OBBTreeNode::forkBox(OBB& box)
+{
+
+  PRINTF(3)("Fork Box\n");
+  PRINTF(4)("Calculating the longest Axis\n");
   /* 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);
-
-
+  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(3)("\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               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);
+  float               tmpDist;                             //!< variable to save diverse distances temporarily
+  int                 vertexIndex;                         //!< index of the vertex near the center
+  Plane               middlePlane(box.axis[longestAxisIndex], box.center); //!< the middle plane
+  const sVec3D*       tmpVec;                              //!< temp simple 3D vector
 
 
@@ -616 +449 @@
   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 = new 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());
+  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(3)("\nPartition1: got \t%i Vertices \nPartition2: got \t%i Vertices\n", partition1.size(), partition2.size());
 
 
   /* 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();
-  }
-
-//   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();
-  }
-
-  this->tmpVert1 = vertList1;
-  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]);
-//   }
+  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();
 }
 
@@ -699 +519 @@
 void OBBTreeNode::collideWith(BVTreeNode* treeNode, WorldEntity* nodeA, WorldEntity* nodeB)
 {
+  if( unlikely(treeNode == NULL))
+    return;
+
   PRINTF(3)("collideWith\n");
   /* if the obb overlap, make subtests: check which node is realy overlaping  */
-  PRINT(3)("Checking OBB %i vs %i: ", this->getIndex(), treeNode->getIndex());
-  if( unlikely(treeNode == NULL)) return;
-
-  if( this->overlapTest(this->bvElement, ((OBBTreeNode*)treeNode)->bvElement, nodeA, nodeB))
+  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);
@@ -711 +535 @@
     if( likely( this->nodeLeft != NULL))
     {
-      PRINT(3)("Checking OBB %i vs %i: ", this->nodeLeft->getIndex(), treeNode->getIndex());
-      if( this->overlapTest(this->nodeLeft->bvElement, ((OBBTreeNode*)treeNode)->bvElement, nodeA, nodeB))
+      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(((OBBTreeNode*)treeNode)->nodeLeft, nodeA, nodeB);
-        this->nodeLeft->collideWith(((OBBTreeNode*)treeNode)->nodeRight, nodeA, nodeB);
+        this->nodeLeft->collideWith((((const OBBTreeNode*)treeNode)->nodeLeft), nodeA, nodeB);
+        this->nodeLeft->collideWith((((const OBBTreeNode*)treeNode)->nodeRight), nodeA, nodeB);
       }
     }
@@ -721 +545 @@
     if( likely( this->nodeRight != NULL))
     {
-      PRINT(3)("Checking OBB %i vs %i: ", this->nodeRight->getIndex(), treeNode->getIndex());
-      if(this->overlapTest(this->nodeRight->bvElement, ((OBBTreeNode*)treeNode)->bvElement, nodeA, nodeB))
+      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(((OBBTreeNode*)treeNode)->nodeLeft, nodeA, nodeB);
-       this->nodeRight->collideWith(((OBBTreeNode*)treeNode)->nodeRight, 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, *((OBBTreeNode*)treeNode)->bvElement->center);
-
-      nodeB->collidesWith(nodeA, *this->bvElement->center);
-    }
-
-  }
-}
-
-
-
-bool OBBTreeNode::overlapTest(OBB* boxA, OBB* boxB, WorldEntity* nodeA, WorldEntity* nodeB)
-{
+      nodeA->collidesWith(nodeB, (((const OBBTreeNode*)&treeNode)->bvElement->center));
+
+      nodeB->collidesWith(nodeA, this->bvElement->center);
+    }
+
+  }
+}
+
+
+
+bool OBBTreeNode::overlapTest(OBB& boxA, OBB& 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;
@@ -752 +582 @@
   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);
+  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);
 
 
@@ -779 +610 @@
     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));
+    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);
@@ -791 +622 @@
     if( (rA + rB) < fabs(t.dot(l)))
     {
-      PRINT(3)("no Collision\n");
+      PRINTF(3)("no Collision\n");
       return false;
     }
@@ -803 +634 @@
     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));
+    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);
@@ -815 +646 @@
     if( (rA + rB) < fabs(t.dot(l)))
     {
-      PRINT(3)("no Collision\n");
+      PRINTF(3)("no Collision\n");
       return false;
     }
@@ -831 +662 @@
       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));
+      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);
@@ -843 +674 @@
       if( (rA + rB) < fabs(t.dot(l)))
       {
-        PRINT(3)("keine Kollision\n");
+        PRINTF(3)("keine Kollision\n");
         return false;
       }
@@ -849 +680 @@
   }
 
-
-  boxA->bCollided = true; /* use this ONLY(!!!!) for drawing operations */
-  boxB->bCollided = true;
-  PRINT(3)("Kollision!\n");
+  /* 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;
 }
@@ -860 +693 @@
 
 
+
+
+
+
+
+/**
+ *
+ * draw the BV tree - debug mode
+ */
 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 */
+  /* this function can be used to draw the triangles and/or the points only  */
   if( drawMode & DRAW_MODEL || drawMode & DRAW_ALL)
   {
@@ -869 +710 @@
     {
       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();
-        }
+        for( 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( drawMode & DRAW_POINTS)
-        glEnd();
     }
   }
@@ -917 +751 @@
   if( drawMode & DRAW_BV_AXIS || drawMode & DRAW_ALL)
   {
-    if( !(drawMode & DRAW_SINGLE && depth != 0))
+    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]);
+      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();
     }
@@ -950 +786 @@
     }
 
-    if(this->nodeLeft == NULL || this->nodeRight == NULL)
+    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]);
+
+    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);
+      }
+
+      // debug out
+      if( this->obbTree->getOwner() != NULL)
+      {
+        PRINTF(0)("debug poly draw: depth: %i, mode: %i, entity-name: %s, class: %s\n", depth, drawMode, this->obbTree->getOwner()->getName(), this->obbTree->getOwner()->getClassName());
+      }
+      else
+        PRINTF(0)("debug poly draw: depth: %i, mode: %i\n", depth, drawMode);
+
+
+      /* 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],
@@ -1053 +829 @@
                  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],
@@ -1065 +883 @@
                  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);
+    }
   }
 
@@ -1089 +935 @@
         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);
 
     }
@@ -1127 +973 @@
 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");
+}