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

Last change on this file since 100 was 100, checked in by Sanahuja Guillaume, 6 years ago

passage quaternoin 3dmgx3

File size: 2.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
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
53void RotationMatrix::ToQuaternion(Quaternion &quaternion) const {
54 quaternion.q0 = 0.5f * sqrtf(1.0f + m[0][0] + m[1][1] + m[2][2]);
55 quaternion.q1 = 0.5f * sqrtf(1.0f + m[0][0] - m[1][1] - m[2][2]);
56 quaternion.q2 = 0.5f * sqrtf(1.0f - m[0][0] + m[1][1] - m[2][2]);
57 quaternion.q3 = 0.5f * sqrtf(1.0f - m[0][0] - m[1][1] + m[2][2]);
58}
59
60Quaternion RotationMatrix::ToQuaternion(void) const {
61 Quaternion quaternion;
62 ToQuaternion(quaternion);
63 return quaternion;
64}
65
66float &RotationMatrix::operator()(size_t row, size_t col) {
67 if (row < 3 && col < 3) {
68 return m[row][col];
69 } else {
70 Printf("RotationMatrix: index (%i,%i) out of bound\n", row, col);
71 return m[2][2];
72 }
73}
74
75const float &RotationMatrix::operator()(size_t row, size_t col) const {
76 if (row < 3 && col < 3) {
77 return m[row][col];
78 } else {
79 Printf("RotationMatrix: index (%i,%i) out of bound\n", row, col);
80 return m[2][2];
81 }
82}
83
84} // end namespace core
85} // end namespace flair
Note: See TracBrowser for help on using the repository browser.