Changeset 4708 in orxonox.OLD for orxonox/trunk/src/lib

Timestamp:

Jun 26, 2005, 6:51:31 PM (19 years ago)

Author:

patrick

Message:

orxonox/trunk: the cd now supports rotations in the detection

File:

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

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

@@ -643 +643 @@
 {
   /* first check all axis */
-  Vector t = nodeA->getAbsCoor() + *boxA->center - ( nodeB->getAbsCoor() + *boxB->center);
+  Vector t;
   float rA = 0.0f;
   float rB = 0.0f;
   Vector l;
+  Vector rotAxisA[3];
+  Vector rotAxisB[3];
+
+  rotAxisA[0] =  nodeA->getAbsDir().apply(boxA->axis[0]);
+  rotAxisA[1] =  nodeA->getAbsDir().apply(boxA->axis[1]);
+  rotAxisA[2] =  nodeA->getAbsDir().apply(boxA->axis[2]);
+
+  rotAxisB[0] =  nodeB->getAbsDir().apply(boxB->axis[0]);
+  rotAxisB[1] =  nodeB->getAbsDir().apply(boxB->axis[1]);
+  rotAxisB[2] =  nodeB->getAbsDir().apply(boxB->axis[2]);
+
+  t = nodeA->getAbsCoor() + nodeA->getAbsDir().apply(*boxA->center) - ( nodeB->getAbsCoor() + nodeB->getAbsDir().apply(*boxB->center));
+
+//   printf("\n");
+//   printf("(%f, %f, %f) -> (%f, %f, %f)\n", boxA->axis[0].x, boxA->axis[0].y, boxA->axis[0].z, rotAxisA[0].x, rotAxisA[0].y, rotAxisA[0].z);
+//   printf("(%f, %f, %f) -> (%f, %f, %f)\n", boxA->axis[1].x, boxA->axis[1].y, boxA->axis[1].z, rotAxisA[1].x, rotAxisA[1].y, rotAxisA[1].z);
+//   printf("(%f, %f, %f) -> (%f, %f, %f)\n", boxA->axis[2].x, boxA->axis[2].y, boxA->axis[2].z, rotAxisA[2].x, rotAxisA[2].y, rotAxisA[2].z);
+//
+//   printf("(%f, %f, %f) -> (%f, %f, %f)\n", boxB->axis[0].x, boxB->axis[0].y, boxB->axis[0].z, rotAxisB[0].x, rotAxisB[0].y, rotAxisB[0].z);
+//   printf("(%f, %f, %f) -> (%f, %f, %f)\n", boxB->axis[1].x, boxB->axis[1].y, boxB->axis[1].z, rotAxisB[1].x, rotAxisB[1].y, rotAxisB[1].z);
+//   printf("(%f, %f, %f) -> (%f, %f, %f)\n", boxB->axis[2].x, boxB->axis[2].y, boxB->axis[2].z, rotAxisB[2].x, rotAxisB[2].y, rotAxisB[2].z);
+
 
   /* All 3 axis of the object A */
@@ -653 +675 @@
     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));
+    l = rotAxisA[j];
+
+    rA += fabs(boxA->halfLength[0] * rotAxisA[0].dot(l));
+    rA += fabs(boxA->halfLength[1] * rotAxisA[1].dot(l));
+    rA += fabs(boxA->halfLength[2] * rotAxisA[2].dot(l));
+
+    rB += fabs(boxB->halfLength[0] * rotAxisB[0].dot(l));
+    rB += fabs(boxB->halfLength[1] * rotAxisB[1].dot(l));
+    rB += fabs(boxB->halfLength[2] * rotAxisB[2].dot(l));
 
     PRINTF(3)("s = %f, rA+rB = %f\n", fabs(t.dot(l)), rA+rB);
@@ -677 +699 @@
     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));
+    l = rotAxisB[j];
+
+    rA += fabs(boxA->halfLength[0] * rotAxisA[0].dot(l));
+    rA += fabs(boxA->halfLength[1] * rotAxisA[1].dot(l));
+    rA += fabs(boxA->halfLength[2] * rotAxisA[2].dot(l));
+
+    rB += fabs(boxB->halfLength[0] * rotAxisB[0].dot(l));
+    rB += fabs(boxB->halfLength[1] * rotAxisB[1].dot(l));
+    rB += fabs(boxB->halfLength[2] * rotAxisB[2].dot(l));
 
     PRINTF(3)("s = %f, rA+rB = %f\n", fabs(t.dot(l)), rA+rB);
@@ -705 +727 @@
       rA = 0.0f;
       rB = 0.0f;
-      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));
+      l = rotAxisA[j].cross(rotAxisB[k]);
+
+      rA += fabs(boxA->halfLength[0] * rotAxisA[0].dot(l));
+      rA += fabs(boxA->halfLength[1] * rotAxisA[1].dot(l));
+      rA += fabs(boxA->halfLength[2] * rotAxisA[2].dot(l));
+
+      rB += fabs(boxB->halfLength[0] * rotAxisB[0].dot(l));
+      rB += fabs(boxB->halfLength[1] * rotAxisB[1].dot(l));
+      rB += fabs(boxB->halfLength[2] * rotAxisB[2].dot(l));
 
       PRINTF(3)("s = %f, rA+rB = %f\n", fabs(t.dot(l)), rA+rB);
@@ -731 +753 @@
   return true;
 }
+
+