source: flair-src/trunk/lib/FlairCore/src/RotationMatrix.cpp@ 101

// %flair:license{
// This file is part of the Flair framework distributed under the
// CECILL-C License, Version 1.0.
// %flair:license}
//  created:    2016/02/09
//  filename:   RotationMatrix.cpp
//
//  author:     Guillaume Sanahuja
//              Copyright Heudiasyc UMR UTC/CNRS 7253
//
//  version:    $Id: $
//
//  purpose:    Class defining a rotation matrix
//
//
/*********************************************************************/

#include "RotationMatrix.h"
#include "Object.h"
#include "Euler.h"
#include "Quaternion.h"
#include "math.h"

namespace flair {
namespace core {

RotationMatrix::RotationMatrix() {
  for (int i = 0; i < 3; i++) {
    for (int j = 0; j < 3; j++) {
      if (i == j) {
        m[i][j] = 1;
      } else {
        m[i][j] = 0;
      }
    }
  }
}

RotationMatrix::~RotationMatrix() {}

void RotationMatrix::ToEuler(Euler &euler) const {
  euler.roll = atanf(m[1][2] / m[2][2]);
  euler.pitch = asinf(-m[0][2]);
  euler.yaw = atan2f(m[0][1], m[0][0]);
}

Euler RotationMatrix::ToEuler(void) const {
  Euler euler;
  ToEuler(euler);
  return euler;
}

//from
//http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
void RotationMatrix::ToQuaternion(Quaternion &quaternion) const {
  float trace = m[0][0] + m[1][1] + m[2][2];
  if( trace > 0 ) {
    float s = 0.5f / sqrtf(trace+ 1.0f);
    quaternion.q0 = 0.25f / s;
    quaternion.q1 = ( m[2][1] - m[1][2] ) * s;
    quaternion.q2 = ( m[0][2] - m[2][0] ) * s;
    quaternion.q3 = ( m[1][0] - m[0][1] ) * s;
  } else {
    if ( m[0][0] > m[1][1] && m[0][0] > m[2][2] ) {
      float s = 2.0f * sqrtf( 1.0f + m[0][0] - m[1][1] - m[2][2]);
      quaternion.q0 = (m[2][1] - m[1][2] ) / s;
      quaternion.q1 = 0.25f * s;
      quaternion.q2 = (m[0][1] + m[1][0] ) / s;
      quaternion.q3 = (m[0][2] + m[2][0] ) / s;
    } else if (m[1][1] > m[2][2]) {
      float s = 2.0f * sqrtf( 1.0f + m[1][1] - m[0][0] - m[2][2]);
      quaternion.q0 = (m[0][2] - m[2][0] ) / s;
      quaternion.q1 = (m[0][1] + m[1][0] ) / s;
      quaternion.q2 = 0.25f * s;
      quaternion.q3 = (m[1][2] + m[2][1] ) / s;
    } else {
      float s = 2.0f * sqrtf( 1.0f + m[2][2] - m[0][0] - m[1][1] );
      quaternion.q0 = (m[1][0] - m[0][1] ) / s;
      quaternion.q1 = (m[0][2] + m[2][0] ) / s;
      quaternion.q2 = (m[1][2] + m[2][1] ) / s;
      quaternion.q3 = 0.25f * s;
    }
  }
  quaternion.Normalize();
}
/*
void RotationMatrix::ToQuaternion(Quaternion &quaternion) const {
                quaternion.q0 = 0.5f * sqrtf(1.0f + m[0][0] + m[1][1] + m[2][2]);
                quaternion.q1 = 0.5f * sqrtf(1.0f + m[0][0] - m[1][1] - m[2][2]);
                quaternion.q2 = 0.5f * sqrtf(1.0f - m[0][0] + m[1][1] - m[2][2]);
                quaternion.q3 = 0.5f * sqrtf(1.0f - m[0][0] - m[1][1] + m[2][2]);
                //Printf("%f %f %f\n", m[0][0] , m[1][1] , m[2][2]);
                //Printf("%f %f %f\n",1.0f + m[0][0] - m[1][1] - m[2][2],1.0f - m[0][0] + m[1][1] - m[2][2],1.0f - m[0][0] - m[1][1] + m[2][2]);
}
*/
Quaternion RotationMatrix::ToQuaternion(void) const {
  Quaternion quaternion;
  ToQuaternion(quaternion);
  return quaternion;
}

float &RotationMatrix::operator()(size_t row, size_t col) {
  if (row < 3 && col < 3) {
    return m[row][col];
  } else {
    Printf("RotationMatrix: index (%i,%i) out of bound\n", row, col);
    return m[2][2];
  }
}

const float &RotationMatrix::operator()(size_t row, size_t col) const {
  if (row < 3 && col < 3) {
    return m[row][col];
  } else {
    Printf("RotationMatrix: index (%i,%i) out of bound\n", row, col);
    return m[2][2];
  }
}

} // end namespace core
} // end namespace flair