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

Timestamp:

Jun 26, 2005, 4:54:43 PM (19 years ago)

Author:

patrick

Message:

orxonox/trunk: some speed improvements of the cd

File:

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

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

@@ -365 +365 @@
   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];
+  axis[0].normalize();
+  axis[1].normalize();
+  axis[2].normalize();
   box->axis = axis;
 
@@ -607 +610 @@
 void OBBTreeNode::collideWith(BVTreeNode* treeNode, PNode* nodeA, PNode* nodeB)
 {
-  PRINTF(0)("collideWith\n");
+  PRINTF(3)("collideWith\n");
   /* if the obb overlap, make subtests: check which node is realy overlaping  */
-  PRINT(0)("Checking OBB %i vs %i: ", this->getIndex(), treeNode->getIndex());
+  PRINT(3)("Checking OBB %i vs %i: ", this->getIndex(), treeNode->getIndex());
   if( this->overlapTest(this->bvElement, ((OBBTreeNode*)treeNode)->bvElement, nodeA, nodeB))
   {
@@ -615 +618 @@
     if( likely( this->nodeLeft != NULL))
     {
-      PRINT(0)("Checking OBB %i vs %i: ", this->nodeLeft->getIndex(), treeNode->getIndex());
+      PRINT(3)("Checking OBB %i vs %i: ", this->nodeLeft->getIndex(), treeNode->getIndex());
       if( this->overlapTest(this->nodeLeft->bvElement, ((OBBTreeNode*)treeNode)->bvElement, nodeA, nodeB))
       {
@@ -625 +628 @@
     if( likely( this->nodeRight != NULL))
     {
-      PRINT(0)("Checking OBB %i vs %i: ", this->nodeRight->getIndex(), treeNode->getIndex());
+      PRINT(3)("Checking OBB %i vs %i: ", this->nodeRight->getIndex(), treeNode->getIndex());
       if(this->overlapTest(this->nodeRight->bvElement, ((OBBTreeNode*)treeNode)->bvElement, nodeA, nodeB))
       {
@@ -639 +642 @@
 bool OBBTreeNode::overlapTest(OBB* boxA, OBB* boxB, PNode* nodeA, PNode* nodeB)
 {
-
-
-
   /* first check all axis */
   Vector t = nodeA->getAbsCoor() + *boxA->center - ( nodeB->getAbsCoor() + *boxB->center);
@@ -650 +650 @@
   /* All 3 axis of the object A */
   for( int j = 0; j < 3; ++j)
+  {
+    rA = 0.0f;
+    rB = 0.0f;
+    l = boxA->axis[j];
+
+    rA += fabs(boxA->halfLength[0] * boxA->axis[0].dot(l));
+    rA += fabs(boxA->halfLength[1] * boxA->axis[1].dot(l));
+    rA += fabs(boxA->halfLength[2] * boxA->axis[2].dot(l));
+
+    rB += fabs(boxB->halfLength[0] * boxB->axis[0].dot(l));
+    rB += fabs(boxB->halfLength[1] * boxB->axis[1].dot(l));
+    rB += fabs(boxB->halfLength[2] * boxB->axis[2].dot(l));
+
+    PRINTF(3)("s = %f, rA+rB = %f\n", fabs(t.dot(l)), rA+rB);
+
+    if( (rA + rB) < fabs(t.dot(l)))
+    {
+      PRINT(3)("keine Kollision\n");
+      return false;
+    }
+  }
+
+  /* All 3 axis of the object B */
+  for( int j = 0; j < 3; ++j)
+  {
+    rA = 0.0f;
+    rB = 0.0f;
+    l = boxB->axis[j];
+
+    rA += fabs(boxA->halfLength[0] * boxA->axis[0].dot(l));
+    rA += fabs(boxA->halfLength[1] * boxA->axis[1].dot(l));
+    rA += fabs(boxA->halfLength[2] * boxA->axis[2].dot(l));
+
+    rB += fabs(boxB->halfLength[0] * boxB->axis[0].dot(l));
+    rB += fabs(boxB->halfLength[1] * boxB->axis[1].dot(l));
+    rB += fabs(boxB->halfLength[2] * boxB->axis[2].dot(l));
+
+    PRINTF(3)("s = %f, rA+rB = %f\n", fabs(t.dot(l)), rA+rB);
+
+    if( (rA + rB) < fabs(t.dot(l)))
+    {
+      PRINT(3)("keine Kollision\n");
+      return false;
+    }
+  }
+
+
+  /* Now check for all face cross products */
+
+  for( int j = 0; j < 3; ++j)
+  {
+    for(int k = 0; k < 3; ++k )
     {
       rA = 0.0f;
       rB = 0.0f;
-      l = boxA->axis[j];
-
-      /* \todo unroll all loops */
-      for(int i = 0; i < 3; ++i)
-        rA += fabs(boxA->halfLength[i] * boxA->axis[i].dot(l));
-      for(int i = 0; i < 3; ++i)
-        rB += fabs(boxB->halfLength[i] * boxB->axis[i].dot(l));
-
+      l = boxA->axis[j].cross(boxB->axis[k]);
+
+      rA += fabs(boxA->halfLength[0] * boxA->axis[0].dot(l));
+      rA += fabs(boxA->halfLength[1] * boxA->axis[1].dot(l));
+      rA += fabs(boxA->halfLength[2] * boxA->axis[2].dot(l));
+
+      rB += fabs(boxB->halfLength[0] * boxB->axis[0].dot(l));
+      rB += fabs(boxB->halfLength[1] * boxB->axis[1].dot(l));
+      rB += fabs(boxB->halfLength[2] * boxB->axis[2].dot(l));
 
       PRINTF(3)("s = %f, rA+rB = %f\n", fabs(t.dot(l)), rA+rB);
@@ -666 +719 @@
       if( (rA + rB) < fabs(t.dot(l)))
       {
-        PRINT(0)("keine Kollision\n");
-        PRINTF(3)(" - Keine Kollision in der Bahnfohstrasse! Passagiere der Linien 6, 14 sind gebeten zu fliegen!\n");
+        PRINT(3)("keine Kollision\n");
         return false;
       }
     }
-
-    /* All 3 axis of the object B */
-    for( int j = 0; j < 3; ++j)
-    {
-      rA = 0.0f;
-      rB = 0.0f;
-      l = boxB->axis[j];
-
-      for(int i = 0; i < 3; ++i)
-        rA += fabs(boxA->halfLength[i] * boxA->axis[i].dot(l));
-      for(int i = 0; i < 3; ++i)
-        rB += fabs(boxB->halfLength[i] * boxB->axis[i].dot(l));
-
-
-      PRINTF(3)("s = %f, rA+rB = %f\n", fabs(t.dot(l)), rA+rB);
-
-      if( (rA + rB) < fabs(t.dot(l)))
-      {
-        PRINT(0)("keine Kollision\n");
-        PRINTF(3)(" - Keine Kollision in der Bahnfohstrasse! Passagiere der Linien 6, 14 sind gebeten zu fliegen!\n");
-        return false;
-      }
-    }
-
-
-    /* Now check for all face cross products */
-
-      for( int j = 0; j < 3; ++j)
-      {
-        for(int k = 0; k < 3; ++k )
-        {
-          rA = 0.0f;
-          rB = 0.0f;
-          l = boxA->axis[j].cross(boxB->axis[k]);
-
-          for(int i = 0; i < 3; ++i)
-            rA += fabs(boxA->halfLength[i] * boxA->axis[i].dot(l));
-          for(int i = 0; i < 3; ++i)
-            rB += fabs(boxB->halfLength[i] * boxB->axis[i].dot(l));
-
-
-          PRINTF(3)("s = %f, rA+rB = %f\n", fabs(t.dot(l)), rA+rB);
-
-          if( (rA + rB) < fabs(t.dot(l)))
-          {
-            PRINT(0)("keine Kollision\n");
-            PRINTF(3)(" - Keine Kollision in der Bahnfohstrasse! Passagiere der Linien 6, 14 sind gebeten zu fliegen!\n");
-            return false;
-          }
-        }
-      }
-
-
-
-
-    boxA->bCollided = true; /* use this ONLY!!!! for drawing operations */
-    boxB->bCollided = true;
-    PRINT(0)("Kollision!\n");
-    return true;
+  }
+
+
+  boxA->bCollided = true; /* use this ONLY(!!!!) for drawing operations */
+  boxB->bCollided = true;
+  PRINT(3)("Kollision!\n");
+  return true;
 }