Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: orxonox.OLD/trunk/src/lib/collision_detection/lin_alg.h @ 5617

Last change on this file since 5617 was 5493, checked in by patrick, 19 years ago

orxonox/trunk: the other jacobi functions doesn't work neither. there is only the possibility to implement it by myself or to search the web for other 2 hours…

File size: 15.3 KB
RevLine 
[4627]1/*!
[5039]2 * @file lin_alg.h
[4836]3  *  Definition of some important linear algebra formulas
[4627]4
5    compute the eigenpairs (eigenvalues and eigenvectors) of a real symmetric matrix "A" by the Jacobi method
6 */
7
[4628]8
[5488]9
[4628]10/************************************************************
11* This subroutine computes all eigenvalues and eigenvectors *
12* of a real symmetric square matrix A(N,N). On output, ele- *
13* ments of A above the diagonal are destroyed. D(N) returns *
14* the eigenvalues of matrix A. V(N,N) contains, on output,  *
15* the eigenvectors of A by columns. THe normalization to    *
16* unity is made by main program before printing results.    *
17* NROT returns the number of Jacobi matrix rotations which  *
18* were required.                                            *
19* --------------------------------------------------------- *
20* Ref.:"NUMERICAL RECIPES IN FORTRAN, Cambridge University  *
21*       Press, 1986, chap. 11, pages 346-348".              *
22*                                                           *
23*                         C++ version by J-P Moreau, Paris. *
24************************************************************/
[5488]25void JacobI(float **A, float *D, float **V, int *NROT) {
26
27int N = 3;
28
[4628]29float  *B, *Z;
[5428]30double  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;
[5490]31int     i = 0, j = 0, ip = 0, iq = 0;
[4628]32
[5428]33  //allocate vectors B, Z
[5491]34  B = new float[N];
35  Z = new float[N];
[4628]36
[5489]37  // initialize V to identity matrix
38  for( ip = 0; ip < N; ip++) {
39    for( iq = 0; iq < N; iq++)
[5488]40      V[ip][iq] = 0.0f;
41    V[ip][ip] = 1.0f;
[4628]42  }
[5489]43  // initialize B,D to the diagonal of A
44  for( ip = 0; ip < N; ip++) {
[5488]45    B[ip] = A[ip][ip];
46    D[ip] = B[ip];
[5489]47    Z[ip] = 0.0f;
[4628]48  }
[5488]49
50  *NROT = 0;
51  // make maximaly 50 iterations
52  for( i = 1; i <= 50; i++) {
[5490]53    sm = 0.0f;
[5488]54
[5490]55    // sum off-diagonal elements
56    for( ip = 0; ip < N - 1; ip++)
57      for( iq = ip + 1; iq < N; iq++)
58        sm += fabs(A[ip][iq]);
[5488]59
[5490]60    // is it already a diagonal matrix?
61    if( sm == 0)
[4630]62    {
[5489]63      delete[] B;
64      delete[] Z;
[5490]65      return;
[4628]66    }
[5490]67    // just adjust this on the first 3 sweeps
68    if( i < 4)
[5491]69      tresh = 0.2 * sm / (N * N) ;
[5490]70    else
71      tresh = 0.0f;
72
73    for( ip = 0; ip < (N-1); ip++) {
74      for( iq = ip + 1; iq < N; iq++) {
75
76        g = 100.0f * fabs(A[ip][iq]);
77        // after 4 sweeps, skip the rotation if the off-diagonal element is small
78        if( (i > 4) && ( fabs(D[ip]) + g == fabs(D[ip]) ) && ( fabs(D[iq]) + g == fabs(D[iq]) ) )
79          A[ip][iq] = 0.0f;
80        else if( fabs(A[ip][iq]) > tresh) {
81          h = D[iq] - D[ip];
82          if (fabs(h) + g == fabs(h))
83            t = A[ip][iq] / h;
[4628]84          else {
[5490]85            theta = 0.5f * h / A[ip][iq];
86            t = 1.0f / (fabs(theta) + sqrt(1.0f + theta * theta));
87            if( theta < 0.0f)
88              t = -t;
[4628]89          }
[5490]90          c = 1.0f / sqrt(1.0f + t * t);
91          s = t * c;
92          tau = s / (1.0f + c);
[5491]93          h = t * A[ip][iq];
[4628]94          Z[ip] -= h;
95          Z[iq] += h;
96          D[ip] -= h;
97          D[iq] += h;
[5491]98          A[ip][iq] = 0.0f;
99
100          for( j = 0; j < (ip - 1); j++) {
101            g = A[j][ip];
102            h = A[j][iq];
103            A[j][ip] = g - s * (h + g * tau);
104            A[j][iq] = h + s * (g - h * tau);
[4628]105          }
[5491]106          for( j = (ip + 1); j < (iq - 1); j++) {
107            g = A[ip][j];
108            h = A[j][iq];
109            A[ip][j] = g - s * (h + g * tau);
110            A[j][iq] = h + s * (g - h * tau);
[4628]111          }
[5491]112          for( j = (iq + 1); j < N; j++) {
113            g = A[ip][j];
114            h = A[iq][j];
115            A[ip][j] = g - s * (h + g * tau);
116            A[iq][j] = h + s * (g - h * tau);
[4628]117          }
[5491]118          for( j = 0; j < N; j++) {
119            g = V[j][ip];
120            h = V[j][iq];
121            V[j][ip] = g - s * (h + g * tau);
122            V[j][iq] = h + s * (g - h * tau);
[4628]123          }
[5491]124          *NROT += 1;
[4628]125        } //end ((i.gt.4)...else if
126      } // main iq loop
127    } // main ip loop
[5491]128    for( ip = 0; ip < N; ip++) {
[4628]129      B[ip] += Z[ip];
[5491]130      D[ip] = B[ip];
131      Z[ip] = 0.0f;
[4628]132    }
133  } //end of main i loop
[5489]134  delete[] B;
135  delete[] Z;
[4628]136  return;  //too many iterations
137}
138
139
140
141
142
[4627]143#include "abstract_model.h"
144
145#include <stdio.h>
146#include <math.h>
147
148#define NDIM 3
149
150
151typedef float MatrixX[3][3];
152
153//
154// class "EVJacobi" for computing the eigenpairs
155// (members)
156//   ndim  int    ...  dimension
157//       "ndim" must satisfy 1 < ndim < NDIM
158//   ("NDIM" is given above).
159//   a     double [NDIM][NDIM] ...  matrix A
160//   aa    double ...  the square root of
161//                  (1/2) x (the sum of the off-diagonal elements squared)
162//   ev    double [NDIM] ...  eigenvalues
163//   evec  double [NDIM][NDIM] ... eigenvectors
164//       evec[i][k], i=1,2,...,ndim are the elements of the eigenvector
165//       corresponding to the k-th eigenvalue ev[k]
166//   vec   double [NDIM][NDIM] ... the 2-dimensional array where the matrix elements are stored
167//   lSort     int      ...
168//       If lSort = 1, sort the eigenvalues d(i) in the descending order, i.e.,
169//                       ev[1] >= ev[2] >= ... >= ev[ndim], and
170//       if lSort = 0, in the ascending order, i.e.,
171//                       ev[1] <= ev[2] <= ... <= ev[ndim].
172//   lMatSize  int      ...  If 1 < ndim < NDIM, lMatSize = 1
173//                            otherwise, lMatSize = 0
174//   p     int [NDIM]    ...  index vector for sorting the eigenvalues
175// (public member functions)
176//   setMatrix          void ...  give the matrix A
177//   getEigenValue      void ...  get the eigenvalues
178//   getEigenVector     void ...  get the eigenvectors
179//   sortEigenpair      void ...  sort the eigenpairs
180// (private member functions)
181//   ComputeEigenpair   void ...  compute the eigenpairs
182//   matrixUpdate       void ...  each step of the Jacobi method, i.e.,
183//                          update of the matrix A by Givens' transform.
184//   getP               void ...  get the index vector p, i.e., sort the eigenvalues.
185//   printMatrix        void ...  print the elements of the matrix A.
186//
187
188class EVJacobi
189{
190  public:
191    void setMatrix(int, double [][NDIM], int, int);
192    void getEigenValue(double []);
193    void getEigenVector(double [][NDIM]);
194    void sortEigenpair(int);
195
196  private:
197    void ComputeEigenpair(int);
[4746]198    void matrixUpdate();
199    void getP();
200    void printMatrix();
[4627]201
202  private:
203    double a[NDIM][NDIM], aa, ev[NDIM], evec[NDIM][NDIM], vec[NDIM][NDIM];
204    int ndim, lSort, p[NDIM], lMatSize;
205};
206
207//------------public member function of the class "EVJacobi"------------------------------
208//
209// give the dimension "ndim" and the matrix "A" and compute the eigenpairs
210// (input)
211// ndim0    int      ... dimension
212// a0     double[][NDIM]  matrix A
213// lSort0  int      ... lSort
214//       If lSort = 1, sort the eigenvalues d(i) in the descending order, i.e.,
215//                       ev[1] >= ev[2] >= ... >= ev[ndim], and
216//       if lSort = 0, in the ascending order, i.e.,
217//                       ev[1] <= ev[2] <= ... <= ev[ndim].
218// l_print  int      ...
219//       If l_print = 1, print the matrices during the iterations.
220//
221void EVJacobi::setMatrix(int ndim0, double a0[][NDIM], int lSort0, int l_print)
222{
223  ndim = ndim0;
224  if (ndim < NDIM && ndim > 1)
225  {
226    lMatSize = 1;
227    lSort = lSort0;
228    for (int i=1; i<=ndim; ++i)
229      for (int j=1; j<=ndim; ++j)
230        a[i][j] = a0[i][j];
231    //
232    aa = 0.0;
233    for (int i=1; i<=ndim; ++i)
234      for (int j=1; j<=i-1; ++j)
235        aa += a[i][j]*a[i][j];
236    aa = sqrt(aa);
237    //
238    ComputeEigenpair(l_print);
239    getP();
240  }
241  else
242  {
243    lMatSize = 0;
244    printf("ndim = %d\n", ndim);
245    printf("ndim must satisfy 1 < ndim < NDIM=%d\n", NDIM);
246  }
247}
248//
249// get the eigenvalues
250// (input)
251// ev0[NDIM] double ...  the array where the eigenvalues are written
252void EVJacobi::getEigenValue(double ev0[])
253{
254  for (int k=1; k<=ndim; ++k) ev0[k] = ev[p[k]];
255}
256//
257// get the eigenvectors
258// (input)
259// evec0[NDIM][NDIM] double ...  the two-dimensional array
260//   where the eigenvectors are written in such a way that
261//   evec0[k][i], i=1,2,...,ndim are the elements of the eigenvector
262//   corresponding to the k-th eigenvalue ev0[k]
263//
264void EVJacobi::getEigenVector(double evec0[][NDIM])
265{
266  for (int k=1; k<=ndim; ++k)
267    for (int i=1; i<=ndim; ++i)
268      evec0[k][i] = evec[p[k]][i];
269}
270//
271// sort the eigenpairs
272// (input)
273// lSort0  int
274//   If lSort0 = 1, the eigenvalues are sorted in the descending order, i.e.,
275//      ev0[1] >= ev0[2] >= ... >= ev0[ndim]
276//   and if lSort0 = 0, in the ascending order, i.e.,
277//      ev0[1] <= ev0[2] <= ... <= ev0[ndim]
278//
279void EVJacobi::sortEigenpair(int lSort0)
280{
281  lSort = lSort0;
282  getP();
283}
284//-------private member function of the class "EVJacobi"-----
285//
286// compute the eigenpairs
287// (input)
288// l_print  int
289//    If l_print = 1, print the matrices during the iterations.
290//
291void EVJacobi::ComputeEigenpair(int l_print)
292{
293  if (lMatSize==1)
294  {
295    if (l_print==1)
296    {
297      printf("step %d\n", 0);
298      printMatrix();
299      printf("\n");
300    }
301    //
302    double eps = 1.0e-15, epsa = eps * aa;
303    int kend = 1000, l_conv = 0;
304    //
305    for (int i=1; i<=ndim; ++i)
306      for (int j=1; j<=ndim; ++j)
307        vec[i][j] = 0.0;
308    for (int i=1; i<=ndim; ++i)
309      vec[i][i] = 1.0;
310    //
311    for (int k=1; k<=kend; ++k)
312    {
313      matrixUpdate();
314      double a1 = 0.0;
315      for (int i=1; i<=ndim; ++i)
316        for (int j=1; j<=i-1; ++j)
317          a1 += a[i][j] * a[i][j];
318      a1 = sqrt(a1);
319      if (a1 < epsa)
320      {
321        if (l_print==1)
322        {
323          printf("converged at step %d\n", k);
324          printMatrix();
325          printf("\n");
326        }
327        l_conv = 1;
328        break;
329      }
330      if (l_print==1)
331        if (k%10==0)
332      {
333        printf("step %d\n", k);
334        printMatrix();
335        printf("\n");
336      }
337    }
338    //
339    if (l_conv == 0) printf("Jacobi method not converged.\n");
340    for (int k=1; k<=ndim; ++k)
341    {
342      ev[k] = a[k][k];
343      for (int i=1; i<=ndim; ++i) evec[k][i] = vec[i][k];
344    }
345  }
346}
347//
[4746]348void EVJacobi::printMatrix()
[4627]349{
350  for (int i=1; i<=ndim; ++i)
351  {
352    for (int j=1; j<=ndim; ++j) printf("%8.1e ",a[i][j]);
353    printf("\n");
354  }
355}
356//
[4746]357void EVJacobi::matrixUpdate()
[4627]358{
359  double a_new[NDIM][NDIM], vec_new[NDIM][NDIM];
360  //
361  int p=2, q=1;
362  double amax = fabs(a[p][q]);
363  for (int i=3; i<=ndim; ++i)
364    for (int j=1; j<=i-1; ++j)
365      if (fabs(a[i][j]) > amax)
366  {
367    p = i;
368    q = j;
369    amax = fabs(a[i][j]);
370  }
371  //
372        // Givens' rotation by Rutishauser's rule
373  //
374  double z, t, c, s, u;
375  z = (a[q][q]  - a[p][p]) / (2.0 * a[p][q]);
376  t = fabs(z) + sqrt(1.0 + z*z);
377  if (z < 0.0) t = - t;
378  t = 1.0 / t;
379  c = 1.0 / sqrt(1.0 + t*t);
380  s = c * t;
381  u = s / (1.0 + c);
382  //
383  for (int i=1; i<=ndim; ++i)
384    for (int j=1; j<=ndim; ++j)
385      a_new[i][j] = a[i][j];
386  //
387  a_new[p][p] = a[p][p] - t * a[p][q];
388  a_new[q][q] = a[q][q] + t * a[p][q];
389  a_new[p][q] = 0.0;
390  a_new[q][p] = 0.0;
391  for (int j=1; j<=ndim; ++j)
392    if (j!=p && j!=q)
393  {
394    a_new[p][j] = a[p][j] - s * (a[q][j] + u * a[p][j]);
395    a_new[j][p] = a_new[p][j];
396    a_new[q][j] = a[q][j] + s * (a[p][j] - u * a[q][j]);
397    a_new[j][q] = a_new[q][j];
398  }
399  //
400  for (int i=1; i<=ndim; ++i)
401  {
402    vec_new[i][p] = vec[i][p] * c - vec[i][q] * s;
403    vec_new[i][q] = vec[i][p] * s + vec[i][q] * c;
404    for (int j=1; j<=ndim; ++j)
405      if (j!=p && j!=q) vec_new[i][j] = vec[i][j];
406  }
407  //
408  for (int i=1; i<=ndim; ++i)
409    for (int j=1; j<=ndim; ++j)
410  {
411    a[i][j] = a_new[i][j];
412    vec[i][j] = vec_new[i][j];
413  }
414}
415//
416// sort the eigenpairs
417//   If l_print=1, sort the eigenvalues in the descending order, i.e.,
418//      ev[1] >= ev[2] >= ... >= ev[ndim], and
419//   if l_print=0, in the ascending order, i.e.,
420//      ev[1] <= ev[2] <= ... <= ev[ndim].
421//
[4746]422void EVJacobi::getP()
[4627]423{
424  for (int i=1; i<=ndim; ++i) p[i] = i;
425  //
426  if (lSort==1)
427  {
428    for (int k=1; k<=ndim; ++k)
429    {
430      double emax = ev[p[k]];
431      for (int i=k+1; i<=ndim; ++i)
432      {
433        if (emax < ev[p[i]])
434        {
435          emax = ev[p[i]];
436          int pp = p[k];
437          p[k] = p[i];
438          p[i] = pp;
439        }
440      }
441    }
442  }
443  if (lSort==0)
444  {
445    for (int k=1; k<=ndim; ++k)
446    {
447      double emin = ev[p[k]];
448      for (int i=k+1; i<=ndim; ++i)
449      {
450        if (emin > ev[p[i]])
451        {
452          emin = ev[p[i]];
453          int pp = p[k];
454          p[k] = p[i];
455          p[i] = pp;
456        }
457      }
458    }
459  }
460}
461
462
463
[5493]464// void jacobi(float **A, float *D, float **V, int *nRot) {
[4627]465//
[5493]466// int n = 3;
[4627]467//
[5493]468// float   *B, *Z;
469// 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;
470// int     i = 0, j = 0, ip = 0, iq = 0;
471//
472//   //void *vmblock1 = NULL;
473//
[4627]474//   //allocate vectors B, Z
[5493]475//   //vmblock1 = vminit();
[4628]476//   //B = (float *) vmalloc(vmblock1, VEKTOR,  100, 0);
477//   //Z = (float *) vmalloc(vmblock1, VEKTOR,  100, 0);
[5493]478//   B = new float[n+1];
479//   Z = new float[n+1];
[4627]480//
481//    //initialize V to identity matrix
482//   for(int i = 1; i <= n; i++)
483//   {
484//     for(int j = 1; j <= n; j++)
485//         V[i][j] = 0;
486//     V[i][i] = 1;
487//   }
488//
489//   for(int i = 1; i <= n; i++)
490//   {
491//     B[i] = A[i][i];
492//     D[i] = B[i];
493//     Z[i] = 0;
494//   }
495//
496//   *nRot = 0;
497//   for(int i = 1; i<=50; i++)
498//   {
499//     sm = 0;
500//     for(int k = 1; k < n; k++)    //sum off-diagonal elements
501//       for (int l = k + 1; l <= n; k++)
502//         sm = sm + fabs(A[k][l]);
503//     if ( sm == 0 )
504//     {
505//       //vmfree(vmblock1);
[5493]506//       delete[] B;
507//       delete[] Z;
[4627]508//       return;       //normal return
509//     }
510//     if (i < 4)
511//       tresh = 0.2 * sm * sm;
512//     else
513//       tresh = 0;
514//     for(int k = 1; k < n; k++)
515//     {
[5493]516//       for (iq=ip+1; iq<=n; iq++) {
[4627]517//         g=100*fabs(A[ip][iq]);
518// // after 4 sweeps, skip the rotation if the off-diagonal element is small
519//         if ((i > 4) && (fabs(D[ip])+g == fabs(D[ip])) && (fabs(D[iq])+g == fabs(D[iq])))
520//           A[ip][iq]=0;
521//         else if (fabs(A[ip][iq]) > tresh) {
522//           h=D[iq]-D[ip];
523//           if (fabs(h)+g == fabs(h))
524//             t=A[ip][iq]/h;
525//           else {
526//             theta=0.5*h/A[ip][iq];
527//             t=1/(fabs(theta)+sqrt(1.0+theta*theta));
528//             if (theta < 0)  t=-t;
529//           }
530//           c=1.0/sqrt(1.0+t*t);
531//           s=t*c;
532//           tau=s/(1.0+c);
533//           h=t*A[ip][iq];
534//           Z[ip] -= h;
535//           Z[iq] += h;
536//           D[ip] -= h;
537//           D[iq] += h;
538//           A[ip][iq]=0;
539//           for (j=1; j<ip; j++) {
540//             g=A[j][ip];
541//             h=A[j][iq];
542//             A[j][ip] = g-s*(h+g*tau);
543//             A[j][iq] = h+s*(g-h*tau);
544//           }
545//           for (j=ip+1; j<iq; j++) {
546//             g=A[ip][j];
547//             h=A[j][iq];
548//             A[ip][j] = g-s*(h+g*tau);
549//             A[j][iq] = h+s*(g-h*tau);
550//           }
[5493]551//           for (j=iq+1; j<=n; j++) {
[4627]552//             g=A[ip][j];
553//             h=A[iq][j];
554//             A[ip][j] = g-s*(h+g*tau);
555//             A[iq][j] = h+s*(g-h*tau);
556//           }
[5493]557//           for (j=1; j<=n; j++) {
[4627]558//             g=V[j][ip];
559//             h=V[j][iq];
560//             V[j][ip] = g-s*(h+g*tau);
561//             V[j][iq] = h+s*(g-h*tau);
562//           }
[5493]563//           *nRot=*nRot+1;
[4627]564//         } //end ((i.gt.4)...else if
565//       } // main iq loop
566//     } // main ip loop
[5493]567//     for (ip=1; ip<=n; ip++) {
[4627]568//       B[ip] += Z[ip];
569//       D[ip]=B[ip];
570//       Z[ip]=0;
571//     }
572//   } //end of main i loop
573//   printf("\n 50 iterations !\n");
[5493]574//   //vmfree(vmblock1);
575//   delete[] Z;
576//   delete[] B;
[4627]577//   return;  //too many iterations
578// }
579
Note: See TracBrowser for help on using the repository browser.