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

Timestamp:

Jun 17, 2005, 1:31:18 AM (19 years ago)

Author:

patrick

Message:

orxonox/trunk: playing with different center/covariance calculation algorithms. main problem of the hole calc process is to find an ideal center for the obb. will take a little sleep before doing this:)

File:

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

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

@@ -113 +113 @@
 
 
-    if(this->tmpLen1 > 2)
+    if(this->tmpLen1 > 0)
     {
       OBBTreeNode* node1 = new OBBTreeNode();
@@ -124 +124 @@
     }
 
-    if( this->tmpLen2 > 2)
+    if( this->tmpLen2 > 0)
     {
       OBBTreeNode* node2 = new OBBTreeNode();
@@ -149 +149 @@
   Vector    t1, t2;                                  //!< temporary values
   float     covariance[3][3];                        //!< the covariance matrix
+  int       mode = 3;                                //!< mode = 0: vertex soup, no connections, mode = 1: 3 following verteces build a triangle
 
   this->numOfVertices = length;
@@ -154 +155 @@
 
 
-
-
-  /* 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];
-    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)
-    {
-      for(int i = 0; i < length; i+=3)
+  if( likely(mode == 0))
+  {
+    /* 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];
+      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)
       {
-        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];
+        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]) - center[j] * center[k];
+        }
       }
     }
-  }
-  PRINTF(3)("-- Calculated Covariance\n");
-
-
-  PRINTF(0)("\nVertex Data:\n");
+    PRINTF(3)("-- Calculated Covariance\n");
+  }
+  else if( mode == 1)
+  {
+    for( int i = 0; i < 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 < 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 < 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 < 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 < 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(0)("vertex %i: %f, %f, %f\n", i, verticesList[i][0], verticesList[i][1], verticesList[i][2]);
-    PRINTF(0)("vertex %i: %f, %f, %f\n", i, box->vertices[i][0], box->vertices[i][1], box->vertices[i][2]);
-  }
+    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");
@@ -463 +562 @@
     if( !(drawMode & DRAW_SINGLE && depth != 0))
     {
-      glBegin(GL_LINE_STRIP);
+      //glBegin(GL_LINE_STRIP);
       for(int i = 0; i < this->bvElement->numOfVertices; ++i)
       {
         glPushMatrix();
-        glMatrixMode(GL_MODELVIEW);
-      //glVertex3f(this->bvElement->vertices[i][0], this->bvElement->vertices[i][1], this->bvElement->vertices[i][2]);
+        //glMatrixMode(GL_MODELVIEW);
+        //glVertex3f(this->bvElement->vertices[i][0], this->bvElement->vertices[i][1], this->bvElement->vertices[i][2]);
         glTranslatef(this->bvElement->vertices[i][0], this->bvElement->vertices[i][1], this->bvElement->vertices[i][2]);
         gluSphere(this->sphereObj, 1, 10, 10);
-        //PRINTF(0)("v(%f, %f, %f)\n", this->vertices[i][0], this->vertices[i][1], this->vertices[i][2]);
+        //PRINTF(0)("v(%f, %f, %f)\n", this->bvElement->vertices[i][0], this->bvElement->vertices[i][1], this->bvElement->vertices[i][2]);
         glPopMatrix();
       }
-      glEnd();
+      //glEnd();
     }
   }