source: orxonox.OLD/trunk/src/lib/math/matrix.cc @ 5661

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

void eigVl(float mat[3][3])
{

  float eigValue[3];
  float eigVc[9];

  float a = 0;
  float b = 0;

  float c[3];

  // c[0] is the determinante of mat
  c[0] = mat[0][0] * mat[1][1] * mat[2][2] +
      2* mat[0][1] * mat[0][2] * mat[1][2] -
      mat[0][0] * mat[1][2] * mat[1][2] -
      mat[1][1] * mat[0][2] * mat[0][2] -
      mat[2][2] * mat[0][1] * mat[0][1];

  // c[1] is the trace of a
  c[1] = mat[0][0] * mat[1][1] -
      mat[0][1] * mat[0][1] +
      mat[0][0] * mat[2][2] -
      mat[0][2] * mat[0][2] +
      mat[1][1] * mat[2][2] -
      mat[1][2] * mat[1][2];

  // c[2] is the sum of the diagonal elements
  c[2] = mat[0][0] +
      mat[1][1] +
      mat[2][2];


  // Computing the roots:
  a = (3.0*c[1] - c[2]*c[2]) / 3.0;
  b = (-2.0*c[2]*c[2]*c[2] + 9.0*c[1]*c[2] - 27.0*c[0]) / 27.0;

  float Q = b*b/4.0 + a*a*a/27.0;

  if (Q < 0)
  {
    printf("good\n");
    float psi = atan2(sqrt(-Q), -b/2.0);
    float p = sqrt((b/2.0)*(b/2.0) - Q);

    eigValue[0] = c[2]/3.0 + 2 * pow(p, 1/3.0) * cos(psi/3.0);
    eigValue[1] = c[2]/3.0 - pow(p, 1/3.0) * (cos(psi/3.0)
        + sqrt(3.0) * sin(psi/3.0));
    eigValue[2] = c[2]/3.0 - pow(p, 1/3.0) * (cos(psi/3.0)
        - sqrt(3.0) * sin(psi/3.0));

  }
  else if (Q == 0)
  {
    eigValue[0] = c[2]/3.0 + pow(b/2.0, 1.0/3.0);
    eigValue[1] = c[2]/3.0 + pow(b/2.0, 1.0/3.0);
    eigValue[2] = c[2]/3.0 + 2* pow(b/2.0, 1.0/3.0);
  }
  else if (Q > 0)
  {
    printf("A is multiple of Identity matrix (lambda * I3))\n");
    eigValue[0] = eigValue[1] = eigValue[2] = 1;
  }

  printf("input: | %f | %f | %f |\n", mat[0][0], mat[0][1], mat[0][2] );
  printf("       | %f | %f | %f |\n", mat[1][0], mat[1][1], mat[1][2] );
  printf("       | %f | %f | %f |\n", mat[2][0], mat[2][1], mat[2][2] );

  printf("%f %f %f\n", eigValue[0], eigValue[1], eigValue[2]);




}