source: orxonox.OLD/branches/network/src/lib/collision_detection/lin_alg.h @ 5733

/*!
 * @file lin_alg.h
  *  Definition of some important linear algebra formulas

    compute the eigenpairs (eigenvalues and eigenvectors) of a real symmetric matrix "A" by the Jacobi method
 */


/**
 *  function that calculates the eigenvectors from a given symmertical 3x3 Matrix
 * @arg A: Matrix
 * @arg D: Eigenvectors
 */
void eigenVectors(float** matrix, float **vectors)
{
  /* map variables to other names, so they can be processed more easely */
  float a = matrix[0][0];
  float b = matrix[0][1];
  float c = matrix[0][2];
  float d = matrix[1][1];
  float e = matrix[1][2];
  float f = matrix[2][2];
  
  /* first eigenvector */
  vectors[0][0] = -1/c * (f - /*Root*/ (c*c*d - 2*b*c + a*e*e + b*b*f - a*d*f - 
                  b*b - c*c + a*d - e*e + a*f ));
}



/************************************************************
* 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, float *D, float **V, int *NROT) {

int N = 3;

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;

  //allocate vectors B, Z
  B = new float[N];
  Z = new float[N];

  // initialize V to identity matrix
  for( ip = 0; ip < N; ip++) {
    for( iq = 0; iq < N; iq++)
      V[ip][iq] = 0.0f;
    V[ip][ip] = 1.0f;
  }
  // initialize B,D to the diagonal of A
  for( ip = 0; ip < N; ip++) {
    B[ip] = A[ip][ip];
    D[ip] = B[ip];
    Z[ip] = 0.0f;
  }

  *NROT = 0;
  // make maximaly 50 iterations
  for( i = 1; i <= 50; i++) {
    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)
    {
      delete[] B;
      delete[] Z;
      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.5f * h / A[ip][iq];
            t = 1.0f / (fabs(theta) + sqrt(1.0f + theta * theta));
            if( theta < 0.0f)
              t = -t;
          }
          c = 1.0f / sqrt(1.0f + t * t);
          s = t * c;
          tau = s / (1.0f + c);
          h = t * A[ip][iq];
          Z[ip] -= h;
          Z[iq] += h;
          D[ip] -= h;
          D[iq] += h;
          A[ip][iq] = 0.0f;

          for( j = 0; j < (ip - 1); 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 - 1); 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 = 0; 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 += 1;
        } //end ((i.gt.4)...else if
      } // main iq loop
    } // main ip loop
    for( ip = 0; ip < N; ip++) {
      B[ip] += Z[ip];
      D[ip] = B[ip];
      Z[ip] = 0.0f;
    }
  } //end of main i loop
  delete[] B;
  delete[] Z;
  return;  //too many iterations
}





#include "abstract_model.h"

#include <stdio.h>
#include <math.h>

#define NDIM 3


typedef float MatrixX[3][3];

//
// class "EVJacobi" for computing the eigenpairs
// (members)
//   ndim  int    ...  dimension
//       "ndim" must satisfy 1 < ndim < NDIM
//   ("NDIM" is given above).
//   a     double [NDIM][NDIM] ...  matrix A
//   aa    double ...  the square root of
//                  (1/2) x (the sum of the off-diagonal elements squared)
//   ev    double [NDIM] ...  eigenvalues
//   evec  double [NDIM][NDIM] ... eigenvectors
//       evec[i][k], i=1,2,...,ndim are the elements of the eigenvector
//       corresponding to the k-th eigenvalue ev[k]
//   vec   double [NDIM][NDIM] ... the 2-dimensional array where the matrix elements are stored
//   lSort     int      ...
//       If lSort = 1, sort the eigenvalues d(i) in the descending order, i.e.,
//                       ev[1] >= ev[2] >= ... >= ev[ndim], and
//       if lSort = 0, in the ascending order, i.e.,
//                       ev[1] <= ev[2] <= ... <= ev[ndim].
//   lMatSize  int      ...  If 1 < ndim < NDIM, lMatSize = 1
//                            otherwise, lMatSize = 0
//   p     int [NDIM]    ...  index vector for sorting the eigenvalues
// (public member functions)
//   setMatrix          void ...  give the matrix A
//   getEigenValue      void ...  get the eigenvalues
//   getEigenVector     void ...  get the eigenvectors
//   sortEigenpair      void ...  sort the eigenpairs
// (private member functions)
//   ComputeEigenpair   void ...  compute the eigenpairs
//   matrixUpdate       void ...  each step of the Jacobi method, i.e.,
//                          update of the matrix A by Givens' transform.
//   getP               void ...  get the index vector p, i.e., sort the eigenvalues.
//   printMatrix        void ...  print the elements of the matrix A.
//

class EVJacobi
{
  public:
    void setMatrix(int, double [][NDIM], int, int);
    void getEigenValue(double []);
    void getEigenVector(double [][NDIM]);
    void sortEigenpair(int);

  private:
    void ComputeEigenpair(int);
    void matrixUpdate();
    void getP();
    void printMatrix();

  private:
    double a[NDIM][NDIM], aa, ev[NDIM], evec[NDIM][NDIM], vec[NDIM][NDIM];
    int ndim, lSort, p[NDIM], lMatSize;
};

//------------public member function of the class "EVJacobi"------------------------------
//
// give the dimension "ndim" and the matrix "A" and compute the eigenpairs
// (input)
// ndim0    int      ... dimension
// a0     double[][NDIM]  matrix A
// lSort0  int      ... lSort
//       If lSort = 1, sort the eigenvalues d(i) in the descending order, i.e.,
//                       ev[1] >= ev[2] >= ... >= ev[ndim], and
//       if lSort = 0, in the ascending order, i.e.,
//                       ev[1] <= ev[2] <= ... <= ev[ndim].
// l_print  int      ...
//       If l_print = 1, print the matrices during the iterations.
//
void EVJacobi::setMatrix(int ndim0, double a0[][NDIM], int lSort0, int l_print)
{
  ndim = ndim0;
  if (ndim < NDIM && ndim > 1)
  {
    lMatSize = 1;
    lSort = lSort0;
    for (int i=1; i<=ndim; ++i)
      for (int j=1; j<=ndim; ++j)
        a[i][j] = a0[i][j];
    //
    aa = 0.0;
    for (int i=1; i<=ndim; ++i)
      for (int j=1; j<=i-1; ++j)
        aa += a[i][j]*a[i][j];
    aa = sqrt(aa);
    //
    ComputeEigenpair(l_print);
    getP();
  }
  else
  {
    lMatSize = 0;
    printf("ndim = %d\n", ndim);
    printf("ndim must satisfy 1 < ndim < NDIM=%d\n", NDIM);
  }
}
//
// get the eigenvalues
// (input)
// ev0[NDIM] double ...  the array where the eigenvalues are written
void EVJacobi::getEigenValue(double ev0[])
{
  for (int k=1; k<=ndim; ++k) ev0[k] = ev[p[k]];
}
//
// get the eigenvectors
// (input)
// evec0[NDIM][NDIM] double ...  the two-dimensional array
//   where the eigenvectors are written in such a way that
//   evec0[k][i], i=1,2,...,ndim are the elements of the eigenvector
//   corresponding to the k-th eigenvalue ev0[k]
//
void EVJacobi::getEigenVector(double evec0[][NDIM])
{
  for (int k=1; k<=ndim; ++k)
    for (int i=1; i<=ndim; ++i)
      evec0[k][i] = evec[p[k]][i];
}
//
// sort the eigenpairs
// (input)
// lSort0  int
//   If lSort0 = 1, the eigenvalues are sorted in the descending order, i.e.,
//      ev0[1] >= ev0[2] >= ... >= ev0[ndim]
//   and if lSort0 = 0, in the ascending order, i.e.,
//      ev0[1] <= ev0[2] <= ... <= ev0[ndim]
//
void EVJacobi::sortEigenpair(int lSort0)
{
  lSort = lSort0;
  getP();
}
//-------private member function of the class "EVJacobi"-----
//
// compute the eigenpairs
// (input)
// l_print  int
//    If l_print = 1, print the matrices during the iterations.
//
void EVJacobi::ComputeEigenpair(int l_print)
{
  if (lMatSize==1)
  {
    if (l_print==1)
    {
      printf("step %d\n", 0);
      printMatrix();
      printf("\n");
    }
    //
    double eps = 1.0e-15, epsa = eps * aa;
    int kend = 1000, l_conv = 0;
    //
    for (int i=1; i<=ndim; ++i)
      for (int j=1; j<=ndim; ++j)
        vec[i][j] = 0.0;
    for (int i=1; i<=ndim; ++i)
      vec[i][i] = 1.0;
    //
    for (int k=1; k<=kend; ++k)
    {
      matrixUpdate();
      double a1 = 0.0;
      for (int i=1; i<=ndim; ++i)
        for (int j=1; j<=i-1; ++j)
          a1 += a[i][j] * a[i][j];
      a1 = sqrt(a1);
      if (a1 < epsa)
      {
        if (l_print==1)
        {
          printf("converged at step %d\n", k);
          printMatrix();
          printf("\n");
        }
        l_conv = 1;
        break;
      }
      if (l_print==1)
        if (k%10==0)
      {
        printf("step %d\n", k);
        printMatrix();
        printf("\n");
      }
    }
    //
    if (l_conv == 0) printf("Jacobi method not converged.\n");
    for (int k=1; k<=ndim; ++k)
    {
      ev[k] = a[k][k];
      for (int i=1; i<=ndim; ++i) evec[k][i] = vec[i][k];
    }
  }
}
//
void EVJacobi::printMatrix()
{
  for (int i=1; i<=ndim; ++i)
  {
    for (int j=1; j<=ndim; ++j) printf("%8.1e ",a[i][j]);
    printf("\n");
  }
}
//
void EVJacobi::matrixUpdate()
{
  double a_new[NDIM][NDIM], vec_new[NDIM][NDIM];
  //
  int p=2, q=1;
  double amax = fabs(a[p][q]);
  for (int i=3; i<=ndim; ++i)
    for (int j=1; j<=i-1; ++j)
      if (fabs(a[i][j]) > amax)
  {
    p = i;
    q = j;
    amax = fabs(a[i][j]);
  }
  //
        // Givens' rotation by Rutishauser's rule
  //
  double z, t, c, s, u;
  z = (a[q][q]  - a[p][p]) / (2.0 * a[p][q]);
  t = fabs(z) + sqrt(1.0 + z*z);
  if (z < 0.0) t = - t;
  t = 1.0 / t;
  c = 1.0 / sqrt(1.0 + t*t);
  s = c * t;
  u = s / (1.0 + c);
  //
  for (int i=1; i<=ndim; ++i)
    for (int j=1; j<=ndim; ++j)
      a_new[i][j] = a[i][j];
  //
  a_new[p][p] = a[p][p] - t * a[p][q];
  a_new[q][q] = a[q][q] + t * a[p][q];
  a_new[p][q] = 0.0;
  a_new[q][p] = 0.0;
  for (int j=1; j<=ndim; ++j)
    if (j!=p && j!=q)
  {
    a_new[p][j] = a[p][j] - s * (a[q][j] + u * a[p][j]);
    a_new[j][p] = a_new[p][j];
    a_new[q][j] = a[q][j] + s * (a[p][j] - u * a[q][j]);
    a_new[j][q] = a_new[q][j];
  }
  //
  for (int i=1; i<=ndim; ++i)
  {
    vec_new[i][p] = vec[i][p] * c - vec[i][q] * s;
    vec_new[i][q] = vec[i][p] * s + vec[i][q] * c;
    for (int j=1; j<=ndim; ++j)
      if (j!=p && j!=q) vec_new[i][j] = vec[i][j];
  }
  //
  for (int i=1; i<=ndim; ++i)
    for (int j=1; j<=ndim; ++j)
  {
    a[i][j] = a_new[i][j];
    vec[i][j] = vec_new[i][j];
  }
}
//
// sort the eigenpairs
//   If l_print=1, sort the eigenvalues in the descending order, i.e.,
//      ev[1] >= ev[2] >= ... >= ev[ndim], and
//   if l_print=0, in the ascending order, i.e.,
//      ev[1] <= ev[2] <= ... <= ev[ndim].
//
void EVJacobi::getP()
{
  for (int i=1; i<=ndim; ++i) p[i] = i;
  //
  if (lSort==1)
  {
    for (int k=1; k<=ndim; ++k)
    {
      double emax = ev[p[k]];
      for (int i=k+1; i<=ndim; ++i)
      {
        if (emax < ev[p[i]])
        {
          emax = ev[p[i]];
          int pp = p[k];
          p[k] = p[i];
          p[i] = pp;
        }
      }
    }
  }
  if (lSort==0)
  {
    for (int k=1; k<=ndim; ++k)
    {
      double emin = ev[p[k]];
      for (int i=k+1; i<=ndim; ++i)
      {
        if (emin > ev[p[i]])
        {
          emin = ev[p[i]];
          int pp = p[k];
          p[k] = p[i];
          p[i] = pp;
        }
      }
    }
  }
}



// void jacobi(float **A, float *D, float **V, int *nRot) {
//
// int n = 3;
//
// 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;
//
//   //void *vmblock1 = NULL;
//
//   //allocate vectors B, Z
//   //vmblock1 = vminit();
//   //B = (float *) vmalloc(vmblock1, VEKTOR,  100, 0);
//   //Z = (float *) vmalloc(vmblock1, VEKTOR,  100, 0);
//   B = new float[n+1];
//   Z = new float[n+1];
//
//    //initialize V to identity matrix
//   for(int i = 1; i <= n; i++)
//   {
//     for(int j = 1; j <= n; j++)
//         V[i][j] = 0;
//     V[i][i] = 1;
//   }
//
//   for(int i = 1; i <= n; i++)
//   {
//     B[i] = A[i][i];
//     D[i] = B[i];
//     Z[i] = 0;
//   }
//
//   *nRot = 0;
//   for(int i = 1; i<=50; i++)
//   {
//     sm = 0;
//     for(int k = 1; k < n; k++)    //sum off-diagonal elements
//       for (int l = k + 1; l <= n; k++)
//         sm = sm + fabs(A[k][l]);
//     if ( sm == 0 )
//     {
//       //vmfree(vmblock1);
//       delete[] B;
//       delete[] Z;
//       return;       //normal return
//     }
//     if (i < 4)
//       tresh = 0.2 * sm * sm;
//     else
//       tresh = 0;
//     for(int k = 1; k < n; k++)
//     {
//       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");
//   //vmfree(vmblock1);
//   delete[] Z;
//   delete[] B;
//   return;  //too many iterations
// }