Index: orxonox/trunk/src/lib/collision_detection/obb_tree_node.cc
===================================================================
--- orxonox/trunk/src/lib/collision_detection/obb_tree_node.cc	(revision 4659)
+++ orxonox/trunk/src/lib/collision_detection/obb_tree_node.cc	(revision 4660)
@@ -339,5 +339,5 @@
   vectors
   */
-  Vector**              axis = new Vector*[3];                //!< the references to the obb axis
+  Vector*              axis = new Vector[3];                //!< the references to the obb axis
 
   coMat[1][1] = box->covarianceMatrix[0][0]; coMat[1][2] = box->covarianceMatrix[0][1]; coMat[1][3] = box->covarianceMatrix[0][2];
@@ -361,13 +361,14 @@
 //   }
 
-  axis[0] = new Vector(eigvMat[1][1], eigvMat[2][1], eigvMat[3][1]);
-  axis[1] = new Vector(eigvMat[1][2], eigvMat[2][2], eigvMat[3][2]);
-  axis[2] = new Vector(eigvMat[1][3], eigvMat[2][3], eigvMat[3][3]);
+  axis[0].x = eigvMat[1][1]; axis[0].y = eigvMat[2][1]; axis[0].z = eigvMat[3][1];
+  axis[1].x = eigvMat[1][2]; axis[1].y = eigvMat[2][2]; axis[1].z = eigvMat[3][2];
+  axis[2].x = eigvMat[1][3]; axis[2].y = eigvMat[2][3]; axis[2].z = eigvMat[3][3];
   box->axis = axis;
+
   PRINTF(3)("-- 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(3)("eigenvector: %f, %f, %f\n", box->axis[0].x, box->axis[0].y, box->axis[0].z);
+  PRINTF(3)("eigenvector: %f, %f, %f\n", box->axis[1].x, box->axis[1].y, box->axis[1].z);
+  PRINTF(3)("eigenvector: %f, %f, %f\n", box->axis[2].x, box->axis[2].y, box->axis[2].z);
 }
 
@@ -380,7 +381,7 @@
   float*              halfLength = new float[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);
+  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];
@@ -415,49 +416,50 @@
 
   /* get the maximal dimensions of the body in all directions */
-//   maxLength[0] = 0.0f;
-//   minLength[0] = 0.0f;
-//   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] = 0.0f;
-//   minLength[1] = 0.0f;
-//   for(int j = 0; j < length; ++j)
-//   {
-//     tmpLength = p0.distancePoint(vertices[j]);
-//     if( tmpLength > maxLength[1])
-//       maxLength[1] = tmpLength;
-//     else if( tmpLength < minLength[1])
-//       minLength[1] = tmpLength;
-//   }
-//
-//   maxLength[2] = 0.0f;
-//   minLength[2] = 0.0f;
-//   for(int j = 0; j < length; ++j)
-//   {
-//     tmpLength = p0.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 center[3];
-//   float offset[3];
-//   for(int i = 0; i < 3; ++i)
-//     {
-//       center[i] = (maxLength[i] - minLength[i]) / 2.0f; // min length is negative
-//       offset[i] = halfLength[i] - center[i];
-//       box->center[i] +=  *box->axis[i] * offset[i];            // update the new center vector
-//       PRINTF(0)("Center Translation Operation: halfLength old: %f, new: %f\n", halfLength[i], center[i]);
-//       halfLength[i] = center[i];
-//     }
+   maxLength[0] = 0.0f;
+   minLength[0] = 0.0f;
+   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] = 0.0f;
+   minLength[1] = 0.0f;
+   for(int j = 0; j < length; ++j)
+   {
+     tmpLength = p0.distancePoint(vertices[j]);
+     if( tmpLength > maxLength[1])
+       maxLength[1] = tmpLength;
+     else if( tmpLength < minLength[1])
+       minLength[1] = tmpLength;
+   }
+
+   maxLength[2] = 0.0f;
+   minLength[2] = 0.0f;
+   for(int j = 0; j < length; ++j)
+   {
+     tmpLength = p0.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 center[3];
+   float offset[3];
+   for(int i = 0; i < 3; ++i)
+     {
+       center[i] = (maxLength[i] - minLength[i]) / 2.0f; // min length is negative
+       offset[i] = halfLength[i] - center[i];
+       //PRINTF(0)("Center Orig: %f, %f, %f ---- new: %f, %f, %f", box->center[]);
+       *box->center +=  (box->axis[i] * offset[i]);            // update the new center vector
+       PRINTF(0)("Center Translation Operation: halfLength old: %f, new: %f\n", halfLength[i], center[i]);
+       halfLength[i] = center[i];
+     }
 
 
@@ -473,5 +475,5 @@
   {
     //if( box->halfLength[i] == 0.0)
-      PRINTF(0)("length[%i] = %f\n", i, box->halfLength[i]);
+    PRINTF(3)("length[%i] = %f\n", i, box->halfLength[i]);
   }
 }
@@ -507,6 +509,7 @@
   float               tmpDist;                             //!< temporary distance
   int                 vertexIndex;
-  Plane               middlePlane(*box->axis[axisIndex], *box->center); //!< the middle plane
-
+  Plane               middlePlane(box->axis[axisIndex], *box->center); //!< the middle plane
+
+  vertexIndex = 0;
   for(int i = 0; i < box->numOfVertices; ++i)
   {
@@ -528,6 +531,6 @@
   tList<sVec3D>      partition2;                           //!< the vertex partition 2
 
-
-  this->separationPlane = new Plane(*box->axis[axisIndex], box->vertices[vertexIndex]);  //!< separation plane
+  printf("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;
@@ -655,17 +658,17 @@
       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->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->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]);
+      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();
     }
@@ -678,66 +681,66 @@
     {
     Vector cen = *this->bvElement->center;
-    Vector** axis = this->bvElement->axis;
+    Vector* axis = this->bvElement->axis;
     float* len = this->bvElement->halfLength;
 
     /* draw bounding box */
     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]);
+    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_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]);
+    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_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]);
+    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_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]);
+    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();
     }
@@ -750,6 +753,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 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];
