Index: /orxonox/trunk/src/lib/collision_detection/obb_tree_node.cc
===================================================================
--- /orxonox/trunk/src/lib/collision_detection/obb_tree_node.cc	(revision 4544)
+++ /orxonox/trunk/src/lib/collision_detection/obb_tree_node.cc	(revision 4545)
@@ -57,11 +57,13 @@
   float     facelet[length];                         //!< surface area of the i'th triangle of the convex hull
   float     face;                                    //!< surface area of the entire convex hull
-  float     centroid[length];                        //!< centroid of the i'th convex hull  
+  Vector    centroid[length];                        //!< centroid of the i'th convex hull  
+  Vector    centre;                                  //!< the centre of the entire hull
   OBB*      obb = new OBB();                         //!< the new obb to add
   Vector    p, q, r;                                 //!< holder of the polygon data, much more conveniant to work with Vector than sVec3d
-  Vector    t1, t2;
-
-  /* fist compute all the convex hull face/facelets */
-  for(int i = 0; i < length; i+=3)
+  Vector    t1, t2;                                  //!< temporary values
+  float     covariance[3][3];                        //!< the covariance matrix
+    
+  /* fist compute all the convex hull face/facelets and centroids */
+  for(int i = 0; i < length; i+=3)          /* FIX-ME-QUICK: hops of 3, array indiscontinuity*/
     {
       p = *(verticesList + i);
@@ -73,10 +75,32 @@
       /* 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 */
+      centre += centroid[i] * facelet[i];
+    }
+  /* take the average of the centroid sum */
+  centre /= 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+=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]) - centre[j] * centre[k];
+	    }
+	}
+    }
 }
 
Index: /orxonox/trunk/src/lib/math/vector.h
===================================================================
--- /orxonox/trunk/src/lib/math/vector.h	(revision 4544)
+++ /orxonox/trunk/src/lib/math/vector.h	(revision 4545)
@@ -11,4 +11,5 @@
 #include <math.h>
 #include "compiler.h"
+#include "abstract_model.h"
 //! PI the circle-constant
 #define PI 3.14159265359f
@@ -26,4 +27,6 @@
   ~Vector () {}
 
+  /** \param index The index of the "array" \returns the x/y/z coordinate */
+  inline float operator[] (float index) const {if( index == 0) return this->x; if( index == 1) return this->y; if( index == 2) return this->z;}
   /**  \param v The vector to add \returns the addition between two vectors (this + v) */
   inline Vector operator+ (const Vector& v) const { return Vector(x + v.x, y + v.y, z + v.z); };
@@ -48,4 +51,6 @@
   /** \brief copy constructor \todo (i do not know it this is faster) \param v the vector to assign to this vector. \returns the vector v */
   inline const Vector& operator= (const Vector& v) { this->x = v.x; this->y = v.y; this->z = v.z; return *this; };
+  /** \brief copy constructor  \param v the sVec3D to assign to this vector. \returns the vector v */
+  inline const Vector& operator= (const sVec3D& v) { this->x = v[0]; this->y = v[1]; this->z = v[2]; }
   /** \param v: the other vector \return the dot product of the vectors */
   float dot (const Vector& v) const { return x*v.x+y*v.y+z*v.z; };
