[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] | 24 | using std::max;
|
---|
| 25 |
|
---|
[15] | 26 | namespace flair {
|
---|
| 27 | namespace core {
|
---|
[2] | 28 |
|
---|
| 29 | RotationMatrix::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] | 41 | RotationMatrix::~RotationMatrix() {}
|
---|
[2] | 42 |
|
---|
| 43 | void 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 |
|
---|
| 49 | Euler RotationMatrix::ToEuler(void) const {
|
---|
[15] | 50 | Euler euler;
|
---|
| 51 | ToEuler(euler);
|
---|
| 52 | return euler;
|
---|
[2] | 53 | }
|
---|
| 54 |
|
---|
[103] | 55 | //from 3dmgx3-25 manual
|
---|
[100] | 56 | void 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] | 96 | Quaternion RotationMatrix::ToQuaternion(void) const {
|
---|
| 97 | Quaternion quaternion;
|
---|
| 98 | ToQuaternion(quaternion);
|
---|
| 99 | return quaternion;
|
---|
| 100 | }
|
---|
| 101 |
|
---|
[15] | 102 | float &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] | 111 | const 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
|
---|