source: orxonox.OLD/branches/spaceshipcontrol/src/lib/math/matrix.cc @ 5904

/*
   orxonox - the future of 3D-vertical-scrollers

   Copyright (C) 2004 orx

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; either version 2, or (at your option)
   any later version.

### File Specific:
   main-programmer: Benjamin Grauer
   co-programmer: Patrick Boenzli
*/
#include "matrix.h"
#include <math.h>

#ifdef DEBUG
#include "debug.h"
#else
#include <stdio.h>
#define PRINT(x) printf
#endif

/**
 * constructs a Matrix from all Parameters in a Row
 * @param m11 [0][0]
 * @param m12 [0][1]
 * @param m13 [0][2]
 * @param m21 [1][0]
 * @param m22 [1][1]
 * @param m23 [1][2]
 * @param m31 [2][0]
 * @param m32 [2][1]
 * @param m33 [2][2]
 */
Matrix::Matrix ( float m11, float m12, float m13,
             float m21, float m22, float m23,
             float m31, float m32, float m33 )
{
  this->m11 = m11; this->m12 = m12; this->m13 = m13;
  this->m21 = m21; this->m22 = m22; this->m23 = m23;
  this->m31 = m31; this->m32 = m32; this->m33 = m33;
};

/**
 * creates a Matrix out of an Array of floats with size [3][3]
 * @param m the Matrix stored in an Array
 */
Matrix::Matrix(const float m[3][3])
{
  this->m11 = m[0][0]; this->m12 = m[0][1]; this->m13 = m[0][2];
  this->m21 = m[1][0]; this->m22 = m[1][1]; this->m23 = m[1][2];
  this->m31 = m[2][0]; this->m32 = m[2][1]; this->m33 = m[2][2];
};


/**
 * adds a Matrix to this one returning the result
 * @param m the Matrix to add to this one
 * @returns a copy of this Matrix added m
 */
Matrix Matrix::operator+ (const Matrix& m) const
{
  return Matrix (this->m11 + m.m11, this->m12 + m.m12, this->m13 + m.m13,
                 this->m21 + m.m21, this->m22 + m.m22, this->m23 + m.m23,
                 this->m31 + m.m31, this->m32 + m.m32, this->m33 + m.m33);
}

/**
 * sustracts a Matrix from this one returning the result
 * @param m the Matrix to substract from this one
 * @returns a copy of this Matrix substracted m
 */
Matrix Matrix::operator- (const Matrix& m) const
{
  return Matrix (this->m11 - m.m11, this->m12 - m.m12, this->m13 - m.m13,
                 this->m21 - m.m21, this->m22 - m.m22, this->m23 - m.m23,
                 this->m31 - m.m31, this->m32 - m.m32, this->m33 - m.m33);
}

/**
 * multiplies each value of a copu of this Matrix by k
 * @param k the multiplication factor
 * @returns a copy of this Matrix multiplied by k
 */
Matrix Matrix::operator* (float k) const
{
  return Matrix(this->m11 * k, this->m12 * k, this->m13 * k,
                this->m21 * k, this->m22 * k, this->m23 * k,
                this->m31 * k, this->m32 * k, this->m33 * k);
}

/**
 * multiplies the Matrix by a Vector returning a Vector of the result
 * @param v the Vector the matrix will be multiplied with
 * @returns the result of the Multiplication
 */
Vector Matrix::operator* (const Vector& v) const
{
  return Vector (this->m11*v.x + this->m12*v.y + this->m13*v.z,
                 this->m21*v.x + this->m22*v.y + this->m23*v.z,
                 this->m31*v.x + this->m32*v.y + this->m33*v.z );
}

/**
 * @returns a Transposed copy of this Matrix
 */
Matrix Matrix::getTransposed() const
{
  return Matrix( this->m11, this->m21, this->m31,
                 this->m12, this->m22, this->m32,
                 this->m13, this->m23, this->m33);
}

/**
 * converts the Matrix into 3 Vector, and returns them in m1, m2 and m3
 * @param m1 the first Column of the Matrix as a Vector
 * @param m2 the second Column of the Matrix as a Vector
 * @param m3 the third Column of the Matrix as a Vector
 */
void Matrix::toVectors(Vector& m1, Vector& m2, Vector& m3) const
{
  m1 = Vector(this->m11, this->m21, this->m31);
  m2 = Vector(this->m12, this->m22, this->m32);
  m3 = Vector(this->m13, this->m23, this->m33);
}

/**
 * @returns the Determinant of this Matrix
 */
float Matrix::getDeterminant() const
{
  return this->m11*(this->m22*this->m33 - this->m23*this->m32) -
      this->m12*(this->m21*this->m33 - this->m23*this->m31) +
      this->m13*(this->m21*this->m32 - this->m22*this->m31);
}

/**
 * calculates an returns the EingenValues of this Matrix.
 * @param eigneValues the Values calculated in a Vector
 * @returns the Count of found eigenValues
 *
 * This Function calculates the EigenValues of a 3x3-Matrix explicitly.
 * the Returned value eigenValues has the Values stored in Vector form
 * The Vector will be filled upside down, meaning if the count of found
 * eingenValues is 1 the only value will be located in eigneValues.x
 */
int Matrix::getEigenValues(Vector& eigenValues) const
{
  int retVal = -1;
  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)
  {
    float psi = atan2(sqrt(-Q), -b/2.0);
    float p = sqrt((b/2.0)*(b/2.0) - Q);

    eigenValues.x = c[2]/3.0 + 2 * pow(p, 1/3.0) * cos(psi/3.0);
    eigenValues.y = c[2]/3.0 - pow(p, 1/3.0) * (cos(psi/3.0)
        + sqrt(3.0) * sin(psi/3.0));
    eigenValues.z = c[2]/3.0 - pow(p, 1/3.0) * (cos(psi/3.0)
        - sqrt(3.0) * sin(psi/3.0));
    retVal = 3;
  }
  // 2 Distinct Roots
  else if (Q == 0)
  {
    eigenValues.x = eigenValues.y = c[2]/3.0 + pow(b/2.0, 1.0/3.0);
    eigenValues.z = c[2]/3.0 + 2* pow(b/2.0, 1.0/3.0);
    retVal = 2;
  }
  // 1 Root (not calculating anything.)
  else if (Q > 0)
  {
    eigenValues.x = eigenValues.y = eigenValues.z = 1;
    retVal = 1;
  }
  return retVal;
}

/**
 * calculates and returns the EigenVectors of this function as Vectors.
 * @param eigVc1 the first eigenVector will be stored here.
 * @param eigVc2 the second eigenVector will be stored here.
 * @param eigVc3 the third eigenVector will be stored here.
 */
void Matrix::getEigenVectors(Vector& eigVc1, Vector& eigVc2, Vector& eigVc3) const
{
  Vector eigenValues;
  int eigenValuesCount = this->getEigenValues(eigenValues);

  if (eigenValuesCount == 2 || eigenValuesCount == 3)
  {
    /* eigenvec creation */
    eigVc1.x = -1/this->m13*(this->m33 - eigenValues.x) +
        (this->m32*(-this->m31*this->m32 + this->m12*this->m33 - this->m12*eigenValues.x)) /
        this->m13*(-this->m13*this->m22 - this->m12*this->m23 + this->m13*eigenValues.x);

    eigVc1.y = -( -this->m13*this->m23 + this->m12*this->m33 - this->m12*eigenValues.x) /
        (-this->m31*this->m22 + this->m12*this->m23 + this->m13*eigenValues.x);

    eigVc1.z = 1.0f;

    eigVc2.x = -1/this->m13*(this->m33 - eigenValues.y) +
        (this->m32*(-this->m31*this->m32 + this->m12*this->m33 - this->m12*eigenValues.y)) /
        this->m13*(-this->m13*this->m22 - this->m12*this->m23 + this->m13*eigenValues.y);

    eigVc2.y = -( -this->m13*this->m23 + this->m12*this->m33 - this->m12*eigenValues.y) /
        (-this->m31*this->m22 + this->m12*this->m23 + this->m13*eigenValues.y);

    eigVc2.z = 1.0f;

    eigVc3 = eigVc1.cross(eigVc2);
  }
  else if (eigenValuesCount == 1)
  {
    eigVc1 = Vector(1,0,0);
    eigVc2 = Vector(0,1,0);
    eigVc3 = Vector(0,0,1);
  }
  eigVc1.normalize();
  eigVc2.normalize();
  eigVc3.normalize();
}

/**
 * prints out some nice debug information
 */
void Matrix::debug() const
{
  printf("| %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 );

}