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

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

Location:

orxonox/trunk/src/lib/collision_detection

Files:

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

orxonox/trunk/src/lib/collision_detection/Makefile.am

@@ -25 +25 @@
                      bv_tree_node.h \
                      bounding_volume.h \
-                     bounding_sphere.h
+                     bounding_sphere.h \
+                     lin_alg.h
 

orxonox/trunk/src/lib/collision_detection/Makefile.in

@@ -208 +208 @@
                      bv_tree_node.h \
                      bounding_volume.h \
-                     bounding_sphere.h
+                     bounding_sphere.h \
+                     lin_alg.h
 
 all: all-am

orxonox/trunk/src/lib/collision_detection/lin_alg.h

@@ +1 @@
+/*!
+    \file lin_alg.h
+    \brief Definition of some important linear algebra formulas
+
+    compute the eigenpairs (eigenvalues and eigenvectors) of a real symmetric matrix "A" by the Jacobi method
+ */
+
+#include "abstract_model.h"
+
+#include <stdio.h>
+#include <math.h>
+
+#define NDIM 3
+
+
+typedef float MatrixX[3][3];
+
+//
+// class "EVJacobi" for computing the eigenpairs
+// (members)
+//   ndim  int    ...  dimension
+//       "ndim" must satisfy 1 < ndim < NDIM
+//   ("NDIM" is given above).
+//   a     double [NDIM][NDIM] ...  matrix A
+//   aa    double ...  the square root of
+//                  (1/2) x (the sum of the off-diagonal elements squared)
+//   ev    double [NDIM] ...  eigenvalues
+//   evec  double [NDIM][NDIM] ... eigenvectors
+//       evec[i][k], i=1,2,...,ndim are the elements of the eigenvector
+//       corresponding to the k-th eigenvalue ev[k]
+//   vec   double [NDIM][NDIM] ... the 2-dimensional array where the matrix elements are stored
+//   lSort     int      ...
+//       If lSort = 1, sort the eigenvalues d(i) in the descending order, i.e.,
+//                       ev[1] >= ev[2] >= ... >= ev[ndim], and
+//       if lSort = 0, in the ascending order, i.e.,
+//                       ev[1] <= ev[2] <= ... <= ev[ndim].
+//   lMatSize  int      ...  If 1 < ndim < NDIM, lMatSize = 1
+//                            otherwise, lMatSize = 0
+//   p     int [NDIM]    ...  index vector for sorting the eigenvalues
+// (public member functions)
+//   setMatrix          void ...  give the matrix A
+//   getEigenValue      void ...  get the eigenvalues
+//   getEigenVector     void ...  get the eigenvectors
+//   sortEigenpair      void ...  sort the eigenpairs
+// (private member functions)
+//   ComputeEigenpair   void ...  compute the eigenpairs
+//   matrixUpdate       void ...  each step of the Jacobi method, i.e.,
+//                          update of the matrix A by Givens' transform.
+//   getP               void ...  get the index vector p, i.e., sort the eigenvalues.
+//   printMatrix        void ...  print the elements of the matrix A.
+//
+
+class EVJacobi
+{
+  public:
+    void setMatrix(int, double [][NDIM], int, int);
+    void getEigenValue(double []);
+    void getEigenVector(double [][NDIM]);
+    void sortEigenpair(int);
+
+  private:
+    void ComputeEigenpair(int);
+    void matrixUpdate(void);
+    void getP(void);
+    void printMatrix(void);
+
+  private:
+    double a[NDIM][NDIM], aa, ev[NDIM], evec[NDIM][NDIM], vec[NDIM][NDIM];
+    int ndim, lSort, p[NDIM], lMatSize;
+};
+
+//------------public member function of the class "EVJacobi"------------------------------
+//
+// give the dimension "ndim" and the matrix "A" and compute the eigenpairs
+// (input)
+// ndim0    int      ... dimension
+// a0     double[][NDIM]  matrix A
+// lSort0  int      ... lSort
+//       If lSort = 1, sort the eigenvalues d(i) in the descending order, i.e.,
+//                       ev[1] >= ev[2] >= ... >= ev[ndim], and
+//       if lSort = 0, in the ascending order, i.e.,
+//                       ev[1] <= ev[2] <= ... <= ev[ndim].
+// l_print  int      ...
+//       If l_print = 1, print the matrices during the iterations.
+//
+void EVJacobi::setMatrix(int ndim0, double a0[][NDIM], int lSort0, int l_print)
+{
+  ndim = ndim0;
+  if (ndim < NDIM && ndim > 1)
+  {
+    lMatSize = 1;
+    lSort = lSort0;
+    for (int i=1; i<=ndim; ++i)
+      for (int j=1; j<=ndim; ++j)
+        a[i][j] = a0[i][j];
+    //
+    aa = 0.0;
+    for (int i=1; i<=ndim; ++i)
+      for (int j=1; j<=i-1; ++j)
+        aa += a[i][j]*a[i][j];
+    aa = sqrt(aa);
+    //
+    ComputeEigenpair(l_print);
+    getP();
+  }
+  else
+  {
+    lMatSize = 0;
+    printf("ndim = %d\n", ndim);
+    printf("ndim must satisfy 1 < ndim < NDIM=%d\n", NDIM);
+  }
+}
+//
+// get the eigenvalues
+// (input)
+// ev0[NDIM] double ...  the array where the eigenvalues are written
+void EVJacobi::getEigenValue(double ev0[])
+{
+  for (int k=1; k<=ndim; ++k) ev0[k] = ev[p[k]];
+}
+//
+// get the eigenvectors
+// (input)
+// evec0[NDIM][NDIM] double ...  the two-dimensional array
+//   where the eigenvectors are written in such a way that
+//   evec0[k][i], i=1,2,...,ndim are the elements of the eigenvector
+//   corresponding to the k-th eigenvalue ev0[k]
+//
+void EVJacobi::getEigenVector(double evec0[][NDIM])
+{
+  for (int k=1; k<=ndim; ++k)
+    for (int i=1; i<=ndim; ++i)
+      evec0[k][i] = evec[p[k]][i];
+}
+//
+// sort the eigenpairs
+// (input)
+// lSort0  int
+//   If lSort0 = 1, the eigenvalues are sorted in the descending order, i.e.,
+//      ev0[1] >= ev0[2] >= ... >= ev0[ndim]
+//   and if lSort0 = 0, in the ascending order, i.e.,
+//      ev0[1] <= ev0[2] <= ... <= ev0[ndim]
+//
+void EVJacobi::sortEigenpair(int lSort0)
+{
+  lSort = lSort0;
+  getP();
+}
+//-------private member function of the class "EVJacobi"-----
+//
+// compute the eigenpairs
+// (input)
+// l_print  int
+//    If l_print = 1, print the matrices during the iterations.
+//
+void EVJacobi::ComputeEigenpair(int l_print)
+{
+  if (lMatSize==1)
+  {
+    if (l_print==1)
+    {
+      printf("step %d\n", 0);
+      printMatrix();
+      printf("\n");
+    }
+    //
+    double eps = 1.0e-15, epsa = eps * aa;
+    int kend = 1000, l_conv = 0;
+    //
+    for (int i=1; i<=ndim; ++i)
+      for (int j=1; j<=ndim; ++j)
+        vec[i][j] = 0.0;
+    for (int i=1; i<=ndim; ++i)
+      vec[i][i] = 1.0;
+    //
+    for (int k=1; k<=kend; ++k)
+    {
+      matrixUpdate();
+      double a1 = 0.0;
+      for (int i=1; i<=ndim; ++i)
+        for (int j=1; j<=i-1; ++j)
+          a1 += a[i][j] * a[i][j];
+      a1 = sqrt(a1);
+      if (a1 < epsa)
+      {
+        if (l_print==1)
+        {
+          printf("converged at step %d\n", k);
+          printMatrix();
+          printf("\n");
+        }
+        l_conv = 1;
+        break;
+      }
+      if (l_print==1)
+        if (k%10==0)
+      {
+        printf("step %d\n", k);
+        printMatrix();
+        printf("\n");
+      }
+    }
+    //
+    if (l_conv == 0) printf("Jacobi method not converged.\n");
+    for (int k=1; k<=ndim; ++k)
+    {
+      ev[k] = a[k][k];
+      for (int i=1; i<=ndim; ++i) evec[k][i] = vec[i][k];
+    }
+  }
+}
+//
+void EVJacobi::printMatrix(void)
+{
+  for (int i=1; i<=ndim; ++i)
+  {
+    for (int j=1; j<=ndim; ++j) printf("%8.1e ",a[i][j]);
+    printf("\n");
+  }
+}
+//
+void EVJacobi::matrixUpdate(void)
+{
+  double a_new[NDIM][NDIM], vec_new[NDIM][NDIM];
+  //
+  int p=2, q=1;
+  double amax = fabs(a[p][q]);
+  for (int i=3; i<=ndim; ++i)
+    for (int j=1; j<=i-1; ++j)
+      if (fabs(a[i][j]) > amax)
+  {
+    p = i;
+    q = j;
+    amax = fabs(a[i][j]);
+  }
+  //
+        // Givens' rotation by Rutishauser's rule
+  //
+  double z, t, c, s, u;
+  z = (a[q][q]  - a[p][p]) / (2.0 * a[p][q]);
+  t = fabs(z) + sqrt(1.0 + z*z);
+  if (z < 0.0) t = - t;
+  t = 1.0 / t;
+  c = 1.0 / sqrt(1.0 + t*t);
+  s = c * t;
+  u = s / (1.0 + c);
+  //
+  for (int i=1; i<=ndim; ++i)
+    for (int j=1; j<=ndim; ++j)
+      a_new[i][j] = a[i][j];
+  //
+  a_new[p][p] = a[p][p] - t * a[p][q];
+  a_new[q][q] = a[q][q] + t * a[p][q];
+  a_new[p][q] = 0.0;
+  a_new[q][p] = 0.0;
+  for (int j=1; j<=ndim; ++j)
+    if (j!=p && j!=q)
+  {
+    a_new[p][j] = a[p][j] - s * (a[q][j] + u * a[p][j]);
+    a_new[j][p] = a_new[p][j];
+    a_new[q][j] = a[q][j] + s * (a[p][j] - u * a[q][j]);
+    a_new[j][q] = a_new[q][j];
+  }
+  //
+  for (int i=1; i<=ndim; ++i)
+  {
+    vec_new[i][p] = vec[i][p] * c - vec[i][q] * s;
+    vec_new[i][q] = vec[i][p] * s + vec[i][q] * c;
+    for (int j=1; j<=ndim; ++j)
+      if (j!=p && j!=q) vec_new[i][j] = vec[i][j];
+  }
+  //
+  for (int i=1; i<=ndim; ++i)
+    for (int j=1; j<=ndim; ++j)
+  {
+    a[i][j] = a_new[i][j];
+    vec[i][j] = vec_new[i][j];
+  }
+}
+//
+// sort the eigenpairs
+//   If l_print=1, sort the eigenvalues in the descending order, i.e.,
+//      ev[1] >= ev[2] >= ... >= ev[ndim], and
+//   if l_print=0, in the ascending order, i.e.,
+//      ev[1] <= ev[2] <= ... <= ev[ndim].
+//
+void EVJacobi::getP(void)
+{
+  for (int i=1; i<=ndim; ++i) p[i] = i;
+  //
+  if (lSort==1)
+  {
+    for (int k=1; k<=ndim; ++k)
+    {
+      double emax = ev[p[k]];
+      for (int i=k+1; i<=ndim; ++i)
+      {
+        if (emax < ev[p[i]])
+        {
+          emax = ev[p[i]];
+          int pp = p[k];
+          p[k] = p[i];
+          p[i] = pp;
+        }
+      }
+    }
+  }
+  if (lSort==0)
+  {
+    for (int k=1; k<=ndim; ++k)
+    {
+      double emin = ev[p[k]];
+      for (int i=k+1; i<=ndim; ++i)
+      {
+        if (emin > ev[p[i]])
+        {
+          emin = ev[p[i]];
+          int pp = p[k];
+          p[k] = p[i];
+          p[i] = pp;
+        }
+      }
+    }
+  }
+}
+
+
+
+
+
+
+
+//  void jacobi(Matrix A, int n, sVec3D d, Matrix V, int *nRot)
+// {
+//   sVec3D  B, Z;
+//   double  c, g, h, s, sm, t, tau, theta, tresh;
+//   int     i, j, ip, iq;
+//
+//   void *vmblock1 = NULL;
+//
+//   //allocate vectors B, Z
+//   vmblock1 = vminit();
+//   //B = (REAL *) vmalloc(vmblock1, VEKTOR,  100, 0);
+//   //Z = (REAL *) vmalloc(vmblock1, VEKTOR,  100, 0);
+//
+//    //initialize V to identity matrix
+//   for(int i = 1; i <= n; i++)
+//   {
+//     for(int j = 1; j <= n; j++)
+//         V[i][j] = 0;
+//     V[i][i] = 1;
+//   }
+//
+//   for(int i = 1; i <= n; i++)
+//   {
+//     B[i] = A[i][i];
+//     D[i] = B[i];
+//     Z[i] = 0;
+//   }
+//
+//   *nRot = 0;
+//   for(int i = 1; i<=50; i++)
+//   {
+//     sm = 0;
+//     for(int k = 1; k < n; k++)    //sum off-diagonal elements
+//       for (int l = k + 1; l <= n; k++)
+//         sm = sm + fabs(A[k][l]);
+//     if ( sm == 0 )
+//     {
+//       //vmfree(vmblock1);
+//       return;       //normal return
+//     }
+//     if (i < 4)
+//       tresh = 0.2 * sm * sm;
+//     else
+//       tresh = 0;
+//     for(int k = 1; k < n; k++)
+//     {
+//       for (iq=ip+1; iq<=N; iq++) {
+//         g=100*fabs(A[ip][iq]);
+// // after 4 sweeps, skip the rotation if the off-diagonal element is small
+//         if ((i > 4) && (fabs(D[ip])+g == fabs(D[ip])) && (fabs(D[iq])+g == fabs(D[iq])))
+//           A[ip][iq]=0;
+//         else if (fabs(A[ip][iq]) > tresh) {
+//           h=D[iq]-D[ip];
+//           if (fabs(h)+g == fabs(h))
+//             t=A[ip][iq]/h;
+//           else {
+//             theta=0.5*h/A[ip][iq];
+//             t=1/(fabs(theta)+sqrt(1.0+theta*theta));
+//             if (theta < 0)  t=-t;
+//           }
+//           c=1.0/sqrt(1.0+t*t);
+//           s=t*c;
+//           tau=s/(1.0+c);
+//           h=t*A[ip][iq];
+//           Z[ip] -= h;
+//           Z[iq] += h;
+//           D[ip] -= h;
+//           D[iq] += h;
+//           A[ip][iq]=0;
+//           for (j=1; 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);
+//           }
+//           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=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=1; 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;
+//         } //end ((i.gt.4)...else if
+//       } // main iq loop
+//     } // main ip loop
+//     for (ip=1; ip<=N; ip++) {
+//       B[ip] += Z[ip];
+//       D[ip]=B[ip];
+//       Z[ip]=0;
+//     }
+//   } //end of main i loop
+//   printf("\n 50 iterations !\n");
+//   vmfree(vmblock1);
+//   return;  //too many iterations
+// }
+

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));