Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

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


Ignore:
Timestamp:
Nov 4, 2005, 5:50:53 PM (19 years ago)
Author:
patrick
Message:

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

File:
1 edited

Legend:

Unmodified
Added
Removed
  • trunk/src/lib/collision_detection/lin_alg.h

    r5489 r5490  
    2929float  *B, *Z;
    3030double  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;
    31 int     i=0,j=0,ip=0,iq=0;
     31int     i = 0, j = 0, ip = 0, iq = 0;
    3232
    3333  //allocate vectors B, Z
     
    5454  // make maximaly 50 iterations
    5555  for( i = 1; i <= 50; i++) {
    56     sm = 0;
    57 
    58     for( ip = 0; ip < 2; ip++)    //sum off-diagonal elements
    59       for( iq = ip + 1; iq < 3; iq++)
    60         sm=sm+fabs(A[ip][iq]);
    61 
    62     if(sm == 0)
     56    sm = 0.0f;
     57
     58    // sum off-diagonal elements
     59    for( ip = 0; ip < N - 1; ip++)
     60      for( iq = ip + 1; iq < N; iq++)
     61        sm += fabs(A[ip][iq]);
     62
     63    // is it already a diagonal matrix?
     64    if( sm == 0)
    6365    {
    64       //free(B);
    65       //free(Z);
    6666      delete[] B;
    6767      delete[] Z;
    68       return;       //normal return
    69     }
    70       tresh=0.2*sm*sm;
    71     for (ip=0; ip<2; ip++) {
    72       for (iq=ip+1; iq<3; iq++) {
    73         g=100*fabs(A[ip][iq]);
    74 // after 4 sweeps, skip the rotation if the off-diagonal element is small
    75         if ((i > 4) && (fabs(D[ip])+g == fabs(D[ip])) && (fabs(D[iq])+g == fabs(D[iq])))
    76           A[ip][iq]=0;
    77         else if (fabs(A[ip][iq]) > tresh) {
    78           h=D[iq]-D[ip];
    79           if (fabs(h)+g == fabs(h))
    80             t=A[ip][iq]/h;
     68      return;
     69    }
     70    // just adjust this on the first 3 sweeps
     71    if( i < 4)
     72      tresh = 0.2 * sm / (N*N) ;
     73    else
     74      tresh = 0.0f;
     75
     76    for( ip = 0; ip < (N-1); ip++) {
     77      for( iq = ip + 1; iq < N; iq++) {
     78
     79        g = 100.0f * fabs(A[ip][iq]);
     80        // after 4 sweeps, skip the rotation if the off-diagonal element is small
     81        if( (i > 4) && ( fabs(D[ip]) + g == fabs(D[ip]) ) && ( fabs(D[iq]) + g == fabs(D[iq]) ) )
     82          A[ip][iq] = 0.0f;
     83        else if( fabs(A[ip][iq]) > tresh) {
     84          h = D[iq] - D[ip];
     85          if (fabs(h) + g == fabs(h))
     86            t = A[ip][iq] / h;
    8187          else {
    82             theta=0.5*h/A[ip][iq];
    83             t=1/(fabs(theta)+sqrt(1.0+theta*theta));
    84             if (theta < 0)  t=-t;
     88            theta = 0.5f * h / A[ip][iq];
     89            t = 1.0f / (fabs(theta) + sqrt(1.0f + theta * theta));
     90            if( theta < 0.0f)
     91              t = -t;
    8592          }
    86           c=1.0/sqrt(1.0+t*t);
    87           s=t*c;
    88           tau=s/(1.0+c);
     93          c = 1.0f / sqrt(1.0f + t * t);
     94          s = t * c;
     95          tau = s / (1.0f + c);
    8996          h=t*A[ip][iq];
    9097          Z[ip] -= h;
Note: See TracChangeset for help on using the changeset viewer.