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

Timestamp:

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

Author:

patrick

Message:

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

File:

• orxonox/trunk/src/lib/collision_detection/lin_alg.h (modified) (2 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