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

Timestamp:

Nov 4, 2005, 5:50:53 PM (18 years ago)

Author:

patrick

Message:

orxonox/trunk/src/lib/collision: heavy work on the jacobi function

File:

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

trunk/src/lib/collision_detection/lin_alg.h

@@ -29 +29 @@
 float  *B, *Z;
 double  c=0.0f, g=0.0f, h=0.0f, s=0.0f, sm=0.0f, t=0.0f, tau=0.0f, theta=0.0f, tresh=0.0f;
-int     i=0,j=0,ip=0,iq=0;
+int     i = 0, j = 0, ip = 0, iq = 0;
 
   //allocate vectors B, Z
@@ -54 +54 @@
   // make maximaly 50 iterations
   for( i = 1; i <= 50; i++) {
-    sm = 0;
-
-    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)
+    sm = 0.0f;
+
+    // sum off-diagonal elements
+    for( ip = 0; ip < N - 1; ip++)
+      for( iq = ip + 1; iq < N; iq++)
+        sm += fabs(A[ip][iq]);
+
+    // is it already a diagonal matrix?
+    if( sm == 0)
     {
-      //free(B);
-      //free(Z);
       delete[] B;
       delete[] Z;
-      return;       //normal return
-    }
-      tresh=0.2*sm*sm;
-    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
-        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;
+      return;
+    }
+    // just adjust this on the first 3 sweeps
+    if( i < 4)
+      tresh = 0.2 * sm / (N*N) ;
+    else
+      tresh = 0.0f;
+
+    for( ip = 0; ip < (N-1); ip++) {
+      for( iq = ip + 1; iq < N; iq++) {
+
+        g = 100.0f * 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.0f;
+        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;
+            theta = 0.5f * h / A[ip][iq];
+            t = 1.0f / (fabs(theta) + sqrt(1.0f + theta * theta));
+            if( theta < 0.0f)
+              t = -t;
           }
-          c=1.0/sqrt(1.0+t*t);
-          s=t*c;
-          tau=s/(1.0+c);
+          c = 1.0f / sqrt(1.0f + t * t);
+          s = t * c;
+          tau = s / (1.0f + c);
           h=t*A[ip][iq];
           Z[ip] -= h;