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

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

maj quaternion 3dmgx3

File size: 3.0 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
24using std::max;
25
26namespace flair {
27namespace core {
28
29RotationMatrix::RotationMatrix() {
30  for (int i = 0; i < 3; i++) {
31    for (int j = 0; j < 3; j++) {
32      if (i == j) {
33        m[i][j] = 1;
34      } else {
35        m[i][j] = 0;
36      }
37    }
38  }
39}
40
41RotationMatrix::~RotationMatrix() {}
42
43void RotationMatrix::ToEuler(Euler &euler) const {
44  euler.roll = atanf(m[1][2] / m[2][2]);
45  euler.pitch = asinf(-m[0][2]);
46  euler.yaw = atan2f(m[0][1], m[0][0]);
47}
48
49Euler RotationMatrix::ToEuler(void) const {
50  Euler euler;
51  ToEuler(euler);
52  return euler;
53}
54
55//from 3dmgx3-25 manual
56void RotationMatrix::ToQuaternion(Quaternion &quaternion) const {
57  float test1 = m[0][0] + m[1][1] + m[2][2];
58  float test2 = m[0][0] - m[1][1] - m[2][2];
59  float test3 = -m[0][0] + m[1][1] - m[2][2];
60  float test4 = -m[0][0] - m[1][1] + m[2][2];
61
62  if(test1==max(max(test1,test2),max(test3,test4))) {
63    float s=2*sqrtf(1+test1);
64    quaternion.q0 = s/4.;
65    quaternion.q1 = ( m[1][2] - m[2][1] ) / s;
66    quaternion.q2 = ( m[2][0] - m[0][2] ) / s;
67    quaternion.q3 = ( m[0][1] - m[1][0] ) / s;
68  }
69
70  if(test2==max(max(test1,test2),max(test3,test4))) {
71    float s=2*sqrtf(1+test2);
72    quaternion.q0 = ( m[2][1] - m[1][2] ) / s;
73    quaternion.q1 = -s/4.;
74    quaternion.q2 = -( m[1][0] + m[0][1] ) / s;
75    quaternion.q3 = -( m[0][2] + m[2][0] ) / s;
76  }
77
78  if(test3==max(max(test1,test2),max(test3,test4))) {
79    float s=2*sqrtf(1+test3);
80    quaternion.q0 = (m[0][2] - m[2][0] ) / s;
81    quaternion.q1 = -( m[1][0] + m[0][1] ) / s;
82    quaternion.q2 = -s/4.;
83    quaternion.q3 = -( m[2][1] + m[1][2] ) / s;
84  }
85
86  if(test4==max(max(test1,test2),max(test3,test4))) {
87    float s=2*sqrtf(1+test4);
88    quaternion.q0 = (m[1][0] - m[0][1] ) / s;
89    quaternion.q1 = -( m[0][2] + m[2][0] ) / s;
90    quaternion.q2 = -( m[2][1] + m[1][2] ) / s;
91    quaternion.q3 = -s/4.;
92  }
93  quaternion.Normalize();
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.