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

Timestamp:

Jun 14, 2005, 1:39:31 AM (19 years ago)

Author:

patrick

Message:

orxonox/trunk: trying to implement my own jacobi decomposition alg since the other once didn't work… shit

File:

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

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

@@ -36 +36 @@
 #include "newmatio.h"
 
+#include "lin_alg.h"
+
 
 
@@ -101 +103 @@
   Vector    p, q, r;                                 //!< holder of the polygon data, much more conveniant to work with Vector than sVec3d
   Vector    t1, t2;                                  //!< temporary values
-  float     covariance[3][3];                        //!< the covariance matrix
+  double    covariance[3][3];                        //!< the covariance matrix
 
   this->numOfVertices = length;
@@ -191 +193 @@
   Vector**              axis = new Vector*[3];                //!< the references to the obb axis
 
-  C(1,1) = covariance[0][0];
-  C(1,2) = covariance[0][1];
-  C(1,3) = covariance[0][2];
-  C(2,1) = covariance[1][0];
-  C(2,2) = covariance[1][1];
-  C(2,3) = covariance[1][2];
-  C(3,1) = covariance[2][0];
-  C(3,2) = covariance[2][1];
-  C(3,3) = covariance[2][2];
+
+  double a[4][4];
+
+  a[0][0] = C(1,1) = covariance[0][0];
+  a[0][1] = C(1,2) = covariance[0][1];
+  a[0][2] = C(1,3) = covariance[0][2];
+  a[1][0] = C(2,1) = covariance[1][0];
+  a[1][1] = C(2,2) = covariance[1][1];
+  a[1][2] = C(2,3) = covariance[1][2];
+  a[2][0] = C(3,1) = covariance[2][0];
+  a[2][1] = C(3,2) = covariance[2][1];
+  a[2][2] = C(3,3) = covariance[2][2];
 
   Jacobi(C, D, V);                                            /* do the jacobi decomposition */
   PRINTF(0)("-- Done Jacobi Decomposition\n");
 
-//   printf("\nwe got a result! YES: \n");
-//
-//   for(int j = 1; j < 4; ++j)
-//   {
-//     printf(" |");
-//     for(int k = 1; k < 4; ++k)
-//     {
-//       printf(" \b%f ", V(j, k));
-//     }
-//     printf(" |\n");
-//   }
+
+  /* new jacobi tests */
+  double eigenvectors[3][3];
+  double eigval[3];
+
+  EVJacobi jac;
+  jac.setMatrix(2, covariance, 0, 0);
+  jac.getEigenVector(eigenvectors);
+
+
+  printf("Old Jacobi\n");
+  for(int j = 1; j < 4; ++j)
+  {
+    printf(" |");
+    for(int k = 1; k < 4; ++k)
+    {
+      printf(" \b%f ", V(j, k));
+    }
+    printf(" |\n");
+  }
+
+  printf("New Jacobi\n");
+  for(int j = 0; j < 3; ++j)
+  {
+    printf(" |");
+    for(int k = 0; k < 3; ++k)
+    {
+      printf(" \b%f ", eigenvectors[j][k]);
+    }
+    printf(" |\n");
+  }
 
   axis[0] = new Vector(V(1, 1), V(2, 1), V(3, 1));