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

Timestamp:

Nov 4, 2005, 6:14:13 PM (18 years ago)

Author:

patrick

Message:

orxonox/trunk/src/lib/cd: rebuilt the jacobi function. There are still some false reads in the function. There perhaps another bug also

Location:

trunk/src/lib/collision_detection

Files:

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

trunk/src/lib/collision_detection/lin_alg.h

@@ -32 +32 @@
 
   //allocate vectors B, Z
-
-  //B = (float *) calloc(100, 32);
-  //Z = (float *) calloc(100, 32);
-  B = new float[N+1];
-  Z = new float[N+1];
+  B = new float[N];
+  Z = new float[N];
 
   // initialize V to identity matrix
@@ -70 +67 @@
     // just adjust this on the first 3 sweeps
     if( i < 4)
-      tresh = 0.2 * sm / (N*N) ;
+      tresh = 0.2 * sm / (N * N) ;
     else
       tresh = 0.0f;
@@ -94 +91 @@
           s = t * c;
           tau = s / (1.0f + c);
-          h=t*A[ip][iq];
+          h = t * A[ip][iq];
           Z[ip] -= h;
           Z[iq] += h;
           D[ip] -= h;
           D[iq] += h;
-          A[ip][iq]=0;
-          for (j=0; j<ip; j++) {
-            g=A[j][ip];
-            h=A[j][iq];
-            A[j][ip] = g-s*(h+g*tau);
-            A[j][iq] = h+s*(g-h*tau);
+          A[ip][iq] = 0.0f;
+
+          for( j = 0; j < (ip - 1); j++) {
+            g = A[j][ip];
+            h = A[j][iq];
+            A[j][ip] = g - s * (h + g * tau);
+            A[j][iq] = h + s * (g - h * tau);
           }
-          for (j=ip+1; j<iq; j++) {
-            g=A[ip][j];
-            h=A[j][iq];
-            A[ip][j] = g-s*(h+g*tau);
-            A[j][iq] = h+s*(g-h*tau);
+          for( j = (ip + 1); j < (iq - 1); j++) {
+            g = A[ip][j];
+            h = A[j][iq];
+            A[ip][j] = g - s * (h + g * tau);
+            A[j][iq] = h + s * (g - h * tau);
           }
-          for (j=iq+1; j<=N; j++) {
-            g=A[ip][j];
-            h=A[iq][j];
-            A[ip][j] = g-s*(h+g*tau);
-            A[iq][j] = h+s*(g-h*tau);
+          for( j = (iq + 1); j < N; j++) {
+            g = A[ip][j];
+            h = A[iq][j];
+            A[ip][j] = g - s * (h + g * tau);
+            A[iq][j] = h + s * (g - h * tau);
           }
-          for (j=0; j<3; j++) {
-            g=V[j][ip];
-            h=V[j][iq];
-            V[j][ip] = g-s*(h+g*tau);
-            V[j][iq] = h+s*(g-h*tau);
+          for( j = 0; j < N; j++) {
+            g = V[j][ip];
+            h = V[j][iq];
+            V[j][ip] = g - s * (h + g * tau);
+            V[j][iq] = h + s * (g - h * tau);
           }
-          *NROT=*NROT+1;
+          *NROT += 1;
         } //end ((i.gt.4)...else if
       } // main iq loop
     } // main ip loop
-    for (ip=0; ip<3; ip++) {
+    for( ip = 0; ip < N; ip++) {
       B[ip] += Z[ip];
-      D[ip]=B[ip];
-      Z[ip]=0;
+      D[ip] = B[ip];
+      Z[ip] = 0.0f;
     }
   } //end of main i loop
-//  printf("\n 50 iterations !\n");
-  //free(B);
-  //free(Z);
   delete[] B;
   delete[] Z;

trunk/src/lib/collision_detection/obb_tree_node.cc

@@ -390 +390 @@
 
 
-//   PRINTF(3)("Jacobi\n");
-//   for(int j = 1; j < 4; ++j)
+//   PRINTF(0)("Jacobi\n");
+//   for(int j = 0; j < 3; ++j)
 //   {
-//     PRINTF(3)(" |");
-//     for(int k = 1; k < 4; ++k)
+//     printf(" |");
+//     for(int k = 0; k < 3; ++k)
 //     {
-//       PRINTF(3)(" \b%f ", OBBTreeNode::OBBTreeNode::eigvMat[j][k]);
+//       printf(" \t%f ", OBBTreeNode::OBBTreeNode::eigvMat[j][k]);
 //     }
-//     PRINTF(3)(" |\n");
+//     printf(" |\n");
 //   }
 
@@ -409 +409 @@
   box->axis = axis;
 
-  PRINTF(3)("-- Got Axis\n");
-
-  PRINTF(3)("eigenvector: %f, %f, %f\n", box->axis[0].x, box->axis[0].y, box->axis[0].z);
-  PRINTF(3)("eigenvector: %f, %f, %f\n", box->axis[1].x, box->axis[1].y, box->axis[1].z);
-  PRINTF(3)("eigenvector: %f, %f, %f\n", box->axis[2].x, box->axis[2].y, box->axis[2].z);
+  PRINTF(0)("-- Got Axis\n");
+
+  PRINTF(0)("eigenvector: %f, %f, %f\n", box->axis[0].x, box->axis[0].y, box->axis[0].z);
+  PRINTF(0)("eigenvector: %f, %f, %f\n", box->axis[1].x, box->axis[1].y, box->axis[1].z);
+  PRINTF(0)("eigenvector: %f, %f, %f\n", box->axis[2].x, box->axis[2].y, box->axis[2].z);
 }