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

Timestamp:

Jun 14, 2005, 2:02:35 AM (19 years ago)

Author:

patrick

Message:

orxonox/trunk: FINALLY! MUAHHAHAHAA, i found a function to compute the eigenvec. just wanted to go to sleep, but…

Location:

orxonox/trunk/src/lib/collision_detection

Files:

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

orxonox/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
  */
+
+
+/************************************************************
+* This subroutine computes all eigenvalues and eigenvectors *
+* of a real symmetric square matrix A(N,N). On output, ele- *
+* ments of A above the diagonal are destroyed. D(N) returns *
+* the eigenvalues of matrix A. V(N,N) contains, on output,  *
+* the eigenvectors of A by columns. THe normalization to    *
+* unity is made by main program before printing results.    *
+* NROT returns the number of Jacobi matrix rotations which  *
+* were required.                                            *
+* --------------------------------------------------------- *
+* Ref.:"NUMERICAL RECIPES IN FORTRAN, Cambridge University  *
+*       Press, 1986, chap. 11, pages 346-348".              *
+*                                                           *
+*                         C++ version by J-P Moreau, Paris. *
+************************************************************/
+void JacobI(float **A,int N,float *D, float **V, int *NROT) {
+float  *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 = (float *) calloc(100, 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;
+    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=1; ip<N; ip++)    //sum off-diagonal elements
+      for (iq=ip+1; iq<=N; iq++)
+        sm=sm+fabs(A[ip][iq]);
+    if (sm==0) {
+      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++) {
+        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");
+  return;  //too many iterations
+}
+
+
+
+
 
 #include "abstract_model.h"
@@ -341 +457 @@
 //   //allocate vectors B, Z
 //   vmblock1 = vminit();
-//   //B = (REAL *) vmalloc(vmblock1, VEKTOR,  100, 0);
-//   //Z = (REAL *) vmalloc(vmblock1, VEKTOR,  100, 0);
+//   //B = (float *) vmalloc(vmblock1, VEKTOR,  100, 0);
+//   //Z = (float *) vmalloc(vmblock1, VEKTOR,  100, 0);
 //
 //    //initialize V to identity matrix

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

@@ -103 +103 @@
   Vector    p, q, r;                                 //!< holder of the polygon data, much more conveniant to work with Vector than sVec3d
   Vector    t1, t2;                                  //!< temporary values
-  double    covariance[3][3];                        //!< the covariance matrix
+  float     covariance[3][3];                        //!< the covariance matrix
 
   this->numOfVertices = length;
@@ -194 +194 @@
 
 
-  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];
+  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];
 
   Jacobi(C, D, V);                                            /* do the jacobi decomposition */
@@ -211 +209 @@
 
   /* new jacobi tests */
-  double eigenvectors[3][3];
-  double eigval[3];
-
-  EVJacobi jac;
-  jac.setMatrix(2, covariance, 0, 0);
-  jac.getEigenVector(eigenvectors);
-
+  float** a = new float*[4];
+  a[0] = new float[4]; a[1] = new float[4]; a[2] = new float[4]; a[3] = new float[4];
+
+  float** b = new float*[4];
+  b[0] = new float[4]; b[1] = new float[4]; b[2] = new float[4]; b[3] = new float[4];
+
+  float eigval[3];
+
+  int* rot = new int;
+
+
+  a[1][1] = covariance[0][0];
+  a[1][2] = covariance[0][1];
+  a[1][3] = covariance[0][2];
+  a[2][1] = covariance[1][0];
+  a[2][2] = covariance[1][1];
+  a[2][3] = covariance[1][2];
+  a[3][1] = covariance[2][0];
+  a[3][2] = covariance[2][1];
+  a[3][3] = covariance[2][2];
+
+
+//   EVJacobi jac;
+//   jac.setMatrix(2, covariance, 0, 0);
+//   jac.getEigenVector(eigenvectors);
+//
+
+  JacobI(a, 3, eigval, b, rot);
 
   printf("Old Jacobi\n");
@@ -231 +250 @@
 
   printf("New Jacobi\n");
-  for(int j = 0; j < 3; ++j)
+  for(int j = 1; j < 4; ++j)
   {
     printf(" |");
-    for(int k = 0; k < 3; ++k)
+    for(int k = 1; k < 4; ++k)
     {
-      printf(" \b%f ", eigenvectors[j][k]);
+      printf(" \b%f ", b[j][k]);
     }
     printf(" |\n");