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

Last change on this file since 101 was 101, checked in by Sanahuja Guillaume, 5 years ago

maj quaternion 3dmgx3

File size: 3.5 KB
Line 
1// %flair:license{
2// This file is part of the Flair framework distributed under the
3// CECILL-C License, Version 1.0.
4// %flair:license}
5//  created:    2016/02/09
6//  filename:   RotationMatrix.cpp
7//
8//  author:     Guillaume Sanahuja
9//              Copyright Heudiasyc UMR UTC/CNRS 7253
10//
11//  version:    $Id: $
12//
13//  purpose:    Class defining a rotation matrix
14//
15//
16/*********************************************************************/
17
18#include "RotationMatrix.h"
19#include "Object.h"
20#include "Euler.h"
21#include "Quaternion.h"
22#include "math.h"
23
24namespace flair {
25namespace core {
26
27RotationMatrix::RotationMatrix() {
28  for (int i = 0; i < 3; i++) {
29    for (int j = 0; j < 3; j++) {
30      if (i == j) {
31        m[i][j] = 1;
32      } else {
33        m[i][j] = 0;
34      }
35    }
36  }
37}
38
39RotationMatrix::~RotationMatrix() {}
40
41void RotationMatrix::ToEuler(Euler &euler) const {
42  euler.roll = atanf(m[1][2] / m[2][2]);
43  euler.pitch = asinf(-m[0][2]);
44  euler.yaw = atan2f(m[0][1], m[0][0]);
45}
46
47Euler RotationMatrix::ToEuler(void) const {
48  Euler euler;
49  ToEuler(euler);
50  return euler;
51}
52
53//from
54//http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
55void RotationMatrix::ToQuaternion(Quaternion &quaternion) const {
56  float trace = m[0][0] + m[1][1] + m[2][2];
57  if( trace > 0 ) {
58    float s = 0.5f / sqrtf(trace+ 1.0f);
59    quaternion.q0 = 0.25f / s;
60    quaternion.q1 = ( m[2][1] - m[1][2] ) * s;
61    quaternion.q2 = ( m[0][2] - m[2][0] ) * s;
62    quaternion.q3 = ( m[1][0] - m[0][1] ) * s;
63  } else {
64    if ( m[0][0] > m[1][1] && m[0][0] > m[2][2] ) {
65      float s = 2.0f * sqrtf( 1.0f + m[0][0] - m[1][1] - m[2][2]);
66      quaternion.q0 = (m[2][1] - m[1][2] ) / s;
67      quaternion.q1 = 0.25f * s;
68      quaternion.q2 = (m[0][1] + m[1][0] ) / s;
69      quaternion.q3 = (m[0][2] + m[2][0] ) / s;
70    } else if (m[1][1] > m[2][2]) {
71      float s = 2.0f * sqrtf( 1.0f + m[1][1] - m[0][0] - m[2][2]);
72      quaternion.q0 = (m[0][2] - m[2][0] ) / s;
73      quaternion.q1 = (m[0][1] + m[1][0] ) / s;
74      quaternion.q2 = 0.25f * s;
75      quaternion.q3 = (m[1][2] + m[2][1] ) / s;
76    } else {
77      float s = 2.0f * sqrtf( 1.0f + m[2][2] - m[0][0] - m[1][1] );
78      quaternion.q0 = (m[1][0] - m[0][1] ) / s;
79      quaternion.q1 = (m[0][2] + m[2][0] ) / s;
80      quaternion.q2 = (m[1][2] + m[2][1] ) / s;
81      quaternion.q3 = 0.25f * s;
82    }
83  }
84  quaternion.Normalize();
85}
86/*
87void RotationMatrix::ToQuaternion(Quaternion &quaternion) const {
88                quaternion.q0 = 0.5f * sqrtf(1.0f + m[0][0] + m[1][1] + m[2][2]);
89                quaternion.q1 = 0.5f * sqrtf(1.0f + m[0][0] - m[1][1] - m[2][2]);
90                quaternion.q2 = 0.5f * sqrtf(1.0f - m[0][0] + m[1][1] - m[2][2]);
91                quaternion.q3 = 0.5f * sqrtf(1.0f - m[0][0] - m[1][1] + m[2][2]);
92                //Printf("%f %f %f\n", m[0][0] , m[1][1] , m[2][2]);
93                //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]);
94}
95*/
96Quaternion RotationMatrix::ToQuaternion(void) const {
97  Quaternion quaternion;
98  ToQuaternion(quaternion);
99  return quaternion;
100}
101
102float &RotationMatrix::operator()(size_t row, size_t col) {
103  if (row < 3 && col < 3) {
104    return m[row][col];
105  } else {
106    Printf("RotationMatrix: index (%i,%i) out of bound\n", row, col);
107    return m[2][2];
108  }
109}
110
111const float &RotationMatrix::operator()(size_t row, size_t col) const {
112  if (row < 3 && col < 3) {
113    return m[row][col];
114  } else {
115    Printf("RotationMatrix: index (%i,%i) out of bound\n", row, col);
116    return m[2][2];
117  }
118}
119
120} // end namespace core
121} // end namespace flair
Note: See TracBrowser for help on using the repository browser.