Changeset 4700 in orxonox.OLD for orxonox/trunk/src/lib/collision_detection/obb_tree_node.cc

Timestamp:

Jun 26, 2005, 12:37:38 PM (19 years ago)

Author:

patrick

Message:

orxonox/trunk: 3 axis get checked:) 12 to go…

File:

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

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

@@ -22 +22 @@
 #include "vector.h"
 #include "abstract_model.h"
+#include "p_node.h"
 
 #include <math.h>
@@ -604 +605 @@
 
 
-void OBBTreeNode::collideWith(BVTreeNode* treeNode)
-{
-  PRINTF(0)("collideWith");
+void OBBTreeNode::collideWith(BVTreeNode* treeNode, PNode* nodeA, PNode* nodeB)
+{
+  PRINTF(0)("collideWith\n");
   /* if the obb overlap, make subtests: check which node is realy overlaping  */
-  if( this->overlapTest(this->bvElement, ((OBBTreeNode*)treeNode)->bvElement))
+  if( this->overlapTest(this->bvElement, ((OBBTreeNode*)treeNode)->bvElement, nodeA, nodeB))
   {
     /* check if left node overlaps */
     if( unlikely( this->nodeLeft != NULL))
-      if( this->overlapTest(this->nodeLeft->bvElement, ((OBBTreeNode*)treeNode)->bvElement))
-        this->nodeLeft->collideWith(((OBBTreeNode*)treeNode)->nodeLeft);
+      if( this->overlapTest(this->nodeLeft->bvElement, ((OBBTreeNode*)treeNode)->bvElement, nodeA, nodeB))
+        this->nodeLeft->collideWith(((OBBTreeNode*)treeNode)->nodeLeft, nodeA, nodeB);
     /* check if right node overlaps */
     if( unlikely( this->nodeRight != NULL))
-      if(this->overlapTest(this->nodeRight->bvElement, ((OBBTreeNode*)treeNode)->bvElement))
-        this->nodeLeft->collideWith(((OBBTreeNode*)treeNode)->nodeRight);
-  }
-}
-
-
-
-bool OBBTreeNode::overlapTest(OBB* boxA, OBB* boxB)
-{
+      if(this->overlapTest(this->nodeRight->bvElement, ((OBBTreeNode*)treeNode)->bvElement, nodeA, nodeB))
+        this->nodeLeft->collideWith(((OBBTreeNode*)treeNode)->nodeRight, nodeA, nodeB);
+  }
+}
+
+
+
+bool OBBTreeNode::overlapTest(OBB* boxA, OBB* boxB, PNode* nodeA, PNode* nodeB)
+{
+  
+  
   /* first check all axis */
-  float r = 0.0f;
-  Vector l = boxA->axis[0];
-  for(int i = 0; i < 3; ++i)
-  {
-    r += boxA->halfLength[i] * boxA->axis[i].dot(l);
-  }
-
-  printf("r = %f\n", r);
-  /* now check all orthogonals from the axis */
+  Vector t = nodeA->getAbsCoor() + *boxA->center - ( nodeB->getAbsCoor() + *boxB->center);
+  float rA = 0.0f;
+  float rB = 0.0f;
+  Vector l;
+
+  for(int j = 0; j < 3; ++j)
+    {
+      rA = 0.0f;
+      rB = 0.0f;
+      l = boxA->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(0)("s = %f, rA+rB = %f\n", fabs(t.dot(l)), rA+rB);
+
+      if( (rA + rB) < fabs(t.dot(l)))
+        PRINTF(0)(" - The Boxes are disjoint!\n");
+      else
+        PRINTF(0)(" - The Boxes are not disjoint\n");
+  
+      PRINTF(0)("rA = %f, rB = %f\n", rA, rB);
+
+
+      /* now check all orthogonals from the axis */
+
+    }
+  return false;
 }