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

Timestamp:

Nov 4, 2005, 5:28:42 PM (19 years ago)

Author:

patrick

Message:

orxonox/trunk/src/lib/cd: woking on the jacobi matrix decomposition algorithm

Location:

trunk/src/lib/collision_detection

Files:

• lin_alg.h (modified) (3 diffs)
• obb_tree_node.cc (modified) (1 diff)

trunk/src/lib/collision_detection/lin_alg.h

@@ -5 +5 @@
     compute the eigenpairs (eigenvalues and eigenvectors) of a real symmetric matrix "A" by the Jacobi method
  */
+
 
 
@@ -22 +23 @@
 *                         C++ version by J-P Moreau, Paris. *
 ************************************************************/
-void JacobI(float **A,int N,float *D, float **V, int *NROT) {
+void JacobI(float **A, float *D, float **V, int *NROT) {
+
+int N = 3;
+
 float  *B, *Z;
 double  c=0.0f, g=0.0f, h=0.0f, s=0.0f, sm=0.0f, t=0.0f, tau=0.0f, theta=0.0f, tresh=0.0f;
@@ -29 +33 @@
   //allocate vectors B, Z
 
-  B = (float *) calloc(100, 32);
-  Z = (float *) calloc(100, 32);
-
-  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;
-  }
-  for (ip=1; ip<=N; ip++) {
-    B[ip]=A[ip][ip];
-    D[ip]=B[ip];
-    Z[ip]=0;
-  }
-  *NROT=0;
-  for (i=1; i<=50; i++) {
-    sm=0;
-    for (ip=0; ip<2; ip++)    //sum off-diagonal elements
-      for (iq=ip+1; iq<3; iq++)
+  //B = (float *) calloc(100, 32);
+  //Z = (float *) calloc(100, 32);
+  B = new float[N];
+  Z = new float[N];
+
+
+  for( ip = 0; ip < 3; ip++) {  //initialize V to identity matrix
+    for( iq = 0; iq < 3; iq++)
+      V[ip][iq] = 0.0f;
+    V[ip][ip] = 1.0f;
+  }
+  for( ip = 1; ip <= N; ip++) {
+    B[ip] = A[ip][ip];
+    D[ip] = B[ip];
+    Z[ip] = 0;
+  }
+
+  *NROT = 0;
+  // make maximaly 50 iterations
+  for( i = 1; i <= 50; i++) {
+    sm = 0;
+
+    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)
+
+    if(sm == 0)
     {
       free(B);

trunk/src/lib/collision_detection/obb_tree_node.cc

@@ -386 +386 @@
 
   /* new jacobi tests */
-  JacobI(OBBTreeNode::coMat, 3, OBBTreeNode::eigvlMat, OBBTreeNode::eigvMat, OBBTreeNode::rotCount);
+  JacobI(OBBTreeNode::coMat, OBBTreeNode::eigvlMat, OBBTreeNode::eigvMat, OBBTreeNode::rotCount);
   PRINTF(3)("-- Done Jacobi Decomposition\n");