// %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 "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;
}

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