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

Timestamp:

Oct 29, 2005, 2:38:25 PM (20 years ago)

Author:

bensch

Message:

orxonox/trunk: Jacobi-reloaded

File:

• trunk/src/lib/collision_detection/lin_alg.h (modified) (6 diffs)

trunk/src/lib/collision_detection/lin_alg.h

@@ -32 +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;
+  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;
   }
@@ -44 +44 @@
   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++)
+    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)
@@ -53 +53 @@
       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++) {
+    for (ip=0; ip<2; ip++) {
+      for (iq=ip+1; iq<3; iq++) {
         g=100*fabs(A[ip][iq]);
 // after 4 sweeps, skip the rotation if the off-diagonal element is small
@@ -81 +78 @@
           D[iq] += h;
           A[ip][iq]=0;
-          for (j=1; j<ip; j++) {
+          for (j=0; j<ip; j++) {
             g=A[j][ip];
             h=A[j][iq];
@@ -99 +96 @@
             A[iq][j] = h+s*(g-h*tau);
           }
-          for (j=1; j<=N; j++) {
+          for (j=0; j<3; j++) {
             g=V[j][ip];
             h=V[j][iq];
@@ -109 +106 @@
       } // main iq loop
     } // main ip loop
-    for (ip=1; ip<=N; ip++) {
+    for (ip=0; ip<3; ip++) {
       B[ip] += Z[ip];
       D[ip]=B[ip];