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

Timestamp:

Oct 29, 2005, 2:38:25 PM (19 years ago)

Author:

bensch

Message:

orxonox/trunk: Jacobi-reloaded

Location:

trunk/src/lib/collision_detection

Files:

• lin_alg.h (modified) (6 diffs)
• obb_tree_node.cc (modified) (2 diffs)

trunk/src/lib/collision_detection/lin_alg.h

@@ -32 +32 @@
   Z = (float *) calloc(100, 32);
 
-  for(ip=1; ip<=N; ip++) {  //initialize V to identity matrix
-    for (iq=1; iq<=N; iq++)  V[ip][iq]=0;
+  for(ip=0; ip<3; ip++) {  //initialize V to identity matrix
+    for (iq=0; iq<3; iq++)  V[ip][iq]=0;
     V[ip][ip]=1;
   }
@@ -44 +44 @@
   for (i=1; i<=50; i++) {
     sm=0;
-    for (ip=1; ip<N; ip++)    //sum off-diagonal elements
-      for (iq=ip+1; iq<=N; iq++)
+    for (ip=0; ip<2; ip++)    //sum off-diagonal elements
+      for (iq=ip+1; iq<3; iq++)
         sm=sm+fabs(A[ip][iq]);
     if (sm==0)
@@ -53 +53 @@
       return;       //normal return
     }
-    if (i < 4)
       tresh=0.2*sm*sm;
-    else
-      tresh=0;
-    for (ip=1; ip<N; ip++) {
-      for (iq=ip+1; iq<=N; iq++) {
+    for (ip=0; ip<2; ip++) {
+      for (iq=ip+1; iq<3; iq++) {
         g=100*fabs(A[ip][iq]);
 // after 4 sweeps, skip the rotation if the off-diagonal element is small
@@ -81 +78 @@
           D[iq] += h;
           A[ip][iq]=0;
-          for (j=1; j<ip; j++) {
+          for (j=0; j<ip; j++) {
             g=A[j][ip];
             h=A[j][iq];
@@ -99 +96 @@
             A[iq][j] = h+s*(g-h*tau);
           }
-          for (j=1; j<=N; j++) {
+          for (j=0; j<3; j++) {
             g=V[j][ip];
             h=V[j][iq];
@@ -109 +106 @@
       } // main iq loop
     } // main ip loop
-    for (ip=1; ip<=N; ip++) {
+    for (ip=0; ip<3; ip++) {
       B[ip] += Z[ip];
       D[ip]=B[ip];

trunk/src/lib/collision_detection/obb_tree_node.cc

@@ -360 +360 @@
   Vector*              axis = new Vector[3];                //!< the references to the obb axis
 
-  OBBTreeNode::coMat[1][1] = box->covarianceMatrix[0][0]; OBBTreeNode::coMat[1][2] = box->covarianceMatrix[0][1]; OBBTreeNode::coMat[1][3] = box->covarianceMatrix[0][2];
-  OBBTreeNode::coMat[2][1] = box->covarianceMatrix[1][0]; OBBTreeNode::coMat[2][2] = box->covarianceMatrix[1][1]; OBBTreeNode::coMat[2][3] = box->covarianceMatrix[1][2];
-  OBBTreeNode::coMat[3][1] = box->covarianceMatrix[2][0]; OBBTreeNode::coMat[3][2] = box->covarianceMatrix[2][1]; OBBTreeNode::coMat[3][3] = box->covarianceMatrix[2][2];
+  OBBTreeNode::coMat[0][0] = box->covarianceMatrix[0][0];
+  OBBTreeNode::coMat[0][1] = box->covarianceMatrix[0][1];
+  OBBTreeNode::coMat[0][2] = box->covarianceMatrix[0][2];
+
+  OBBTreeNode::coMat[1][0] = box->covarianceMatrix[1][0];
+  OBBTreeNode::coMat[1][1] = box->covarianceMatrix[1][1];
+  OBBTreeNode::coMat[1][2] = box->covarianceMatrix[1][2];
+
+  OBBTreeNode::coMat[2][0] = box->covarianceMatrix[2][0];
+  OBBTreeNode::coMat[2][1] = box->covarianceMatrix[2][1];
+  OBBTreeNode::coMat[2][2] = box->covarianceMatrix[2][2];
 
 //   OBBTreeNode::coMat[0][0] = box->covarianceMatrix[0][0];
@@ -393 +401 @@
 //   }
 
-  axis[0].x = OBBTreeNode::eigvMat[1][1]; axis[0].y = OBBTreeNode::eigvMat[2][1]; axis[0].z = OBBTreeNode::eigvMat[3][1];
-  axis[1].x = OBBTreeNode::eigvMat[1][2]; axis[1].y = OBBTreeNode::eigvMat[2][2]; axis[1].z = OBBTreeNode::eigvMat[3][2];
-  axis[2].x = OBBTreeNode::eigvMat[1][3]; axis[2].y = OBBTreeNode::eigvMat[2][3]; axis[2].z = OBBTreeNode::eigvMat[3][3];
+  axis[0].x = OBBTreeNode::eigvMat[0][0]; axis[0].y = OBBTreeNode::eigvMat[1][0]; axis[0].z = OBBTreeNode::eigvMat[2][0];
+  axis[1].x = OBBTreeNode::eigvMat[0][1]; axis[1].y = OBBTreeNode::eigvMat[1][1]; axis[1].z = OBBTreeNode::eigvMat[2][1];
+  axis[2].x = OBBTreeNode::eigvMat[0][2]; axis[2].y = OBBTreeNode::eigvMat[1][2]; axis[2].z = OBBTreeNode::eigvMat[2][2];
   axis[0].normalize();
   axis[1].normalize();