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

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

maj quaternion 3dmgx3

File size: 3.0 KB
RevLine 
[2]1// %flair:license{
[15]2// This file is part of the Flair framework distributed under the
3// CECILL-C License, Version 1.0.
[2]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"
[100]21#include "Quaternion.h"
[2]22#include "math.h"
23
[103]24using std::max;
25
[15]26namespace flair {
27namespace core {
[2]28
29RotationMatrix::RotationMatrix() {
[15]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 }
[2]37 }
[15]38 }
[2]39}
40
[15]41RotationMatrix::~RotationMatrix() {}
[2]42
43void RotationMatrix::ToEuler(Euler &euler) const {
[15]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]);
[2]47}
48
49Euler RotationMatrix::ToEuler(void) const {
[15]50 Euler euler;
51 ToEuler(euler);
52 return euler;
[2]53}
54
[103]55//from 3dmgx3-25 manual
[100]56void RotationMatrix::ToQuaternion(Quaternion &quaternion) const {
[103]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;
[101]68 }
[103]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 }
[101]93 quaternion.Normalize();
94}
[103]95
[100]96Quaternion RotationMatrix::ToQuaternion(void) const {
97 Quaternion quaternion;
98 ToQuaternion(quaternion);
99 return quaternion;
100}
101
[15]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 }
[2]109}
110
[15]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 }
[2]118}
119
120} // end namespace core
121} // end namespace flair
Note: See TracBrowser for help on using the repository browser.