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

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


Vector Matrix::eigenValues() const
{
  Vector eigVl;

  float a = 0;
  float b = 0;

  float c[3];

  // c[0] is the determinante of mat
  c[0] = this->m11 * this->m22 * this->m33 +
      2* this->m12 * this->m13 * this->m23 -
      this->m11 * this->m23 * this->m23 -
      this->m22 * this->m13 * this->m13 -
      this->m33 * this->m12 * this->m12;

  // c[1] is the trace of a
  c[1] = this->m11 * this->m22 -
      this->m12 * this->m12 +
      this->m11 * this->m33 -
      this->m13 * this->m13 +
      this->m22 * this->m33 -
      this->m23 * this->m23;

  // c[2] is the sum of the diagonal elements
  c[2] = this->m11 +
      this->m22 +
      this->m33;


  // 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;

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

    eigVl.x = c[2]/3.0 + 2 * pow(p, 1/3.0) * cos(psi/3.0);
    eigVl.y = c[2]/3.0 - pow(p, 1/3.0) * (cos(psi/3.0)
        + sqrt(3.0) * sin(psi/3.0));
    eigVl.z = c[2]/3.0 - pow(p, 1/3.0) * (cos(psi/3.0)
        - sqrt(3.0) * sin(psi/3.0));

  }
  // 2 Distinct Roots
  else if (Q == 0)
  {
    eigVl.x = c[2]/3.0 + pow(b/2.0, 1.0/3.0);
    eigVl.y = c[2]/3.0 + pow(b/2.0, 1.0/3.0);
    eigVl.z = c[2]/3.0 + 2* pow(b/2.0, 1.0/3.0);
  }
  // 1 Root (not calculating anything.)
  else if (Q > 0)
  {
    printf("A is multiple of Identity matrix (lambda * I3))\n");
    eigVl.x = eigVl.y = eigVl.z = 1;
  }
  printf("%f %f %f\n", eigVl.x, eigVl.y, eigVl.z);
  return eigVl;

}

void Matrix::eigenVectors(Vector& a, Vector& b, Vector& c) const
{
  Vector eigVl = this->eigenValues();

  float eigVc[9];
  // EigenVectors
  for (int i = 0; i < 3; ++i)
  {
    printf (":: i = %d\n", i);
    Matrix M = *this -  Matrix::identity() * eigVl.x;
    Vector m1, m2, m3;

    M.getTransposed().toVectors(m1, m2, m3);

    Vector u1, u2, u3;

    u1 = m2.cross(m3); u1 /= u1.len();
    u2 = m3.cross(m1); u2 /= u2.len();
    u3 = m1.cross(m2); u3 /= u3.len();

    printf("%f, %f, %f\n", u1.x, u1.y, u1.z);
    printf("%f, %f, %f\n", u2.x, u2.y, u2.z);
    printf("%f, %f, %f\n", u3.x, u3.y, u3.z);

//     u1 = M*u1;
//     u2 = M*u2;
//     u3 = M*u3;
    //
//     printf("%f, %f, %f\n", u1.x, u1.y, u1.z);
//     printf("%f, %f, %f\n", u2.x, u2.y, u2.z);
//     printf("%f, %f, %f\n", u3.x, u3.y, u3.z);
//     printf("\n\n");
  }

  Vector eigVc[3];
  /* eigenvec test */
  for(int i = 0; i < 3; i++)
  {
    eigVc[i].x =-1/this->m13*(this->m33 - eigValue[i]) + (this->m32*(-this->m31*this->m32 + this->m12*this->m33 - this->m12*eigValue[i])) /
        this->m13*(-this->m13*this->m22 - this->m12*this->m23 + this->m13*eigValue[i]);
    eigVc[i].y = -( -this->m13*this->m23 + this->m12*this->m33 - this->m12*eigValue[i]) /
        (-this->m31*this->m22 + this->m12*this->m23 + this->m13*eigValue[i]);
    eigVc[i].z = 1.0f;


    printf("home brewn: %f, %f, %f\n", eigVc[i].x, eigVc[i].y, eigVc[i].z);

  }



  this->debug();
}


void Matrix::debug() const
{
  printf("input: | %f | %f | %f |\n", this->m11, this->m12, this->m13 );
  printf("       | %f | %f | %f |\n", this->m21, this->m22, this->m23 );
  printf("       | %f | %f | %f |\n", this->m31, this->m32, this->m33 );

}