Changeset 4545 in orxonox.OLD for orxonox/trunk/src/lib/collision_detection

Timestamp:

Jun 7, 2005, 11:15:57 PM (19 years ago)

Author:

patrick

Message:

orxonox/trunk: further improved the vector class, now it can be used like an array , finished calculation of the covariance matrix

File:

• orxonox/trunk/src/lib/collision_detection/obb_tree_node.cc (modified) (2 diffs)

orxonox/trunk/src/lib/collision_detection/obb_tree_node.cc

@@ -57 +57 @@
   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 +75 @@
       /* 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];
+            }
+        }
+    }
 }