rotationmatrix3.cpp
Go to the documentation of this file.
1 
9 #include "rotationmatrix3.h"
10 
11 #include <argos3/core/utility/math/quaternion.h>
12 #include <argos3/core/utility/math/vector3.h>
13 
14 namespace argos {
15 
16  /****************************************/
17  /****************************************/
18 
19  CRotationMatrix3::operator CQuaternion() const {
20  Real fTrace = 1.0f + m_pfValues[0] + m_pfValues[4] + m_pfValues[8];
21  if (fTrace > 0.00001f) {
22  Real fS = Sqrt(fTrace) * 2.0f;
23  return CQuaternion(
24  fS / 4.0f,
25  (m_pfValues[7] - m_pfValues[5]) / fS,
26  (m_pfValues[2] - m_pfValues[6]) / fS,
27  (m_pfValues[3] - m_pfValues[1]) / fS);
28  }
29  else if (m_pfValues[0] > m_pfValues[4] && m_pfValues[0] > m_pfValues[8]) {
30  Real fS = Sqrt(1.0f + m_pfValues[0] - m_pfValues[4] - m_pfValues[8]) * 2.0f;
31  return CQuaternion(
32  (m_pfValues[7] - m_pfValues[5]) / fS,
33  fS / 4.0f,
34  (m_pfValues[3] + m_pfValues[1]) / fS,
35  (m_pfValues[2] + m_pfValues[6]) / fS);
36  }
37  else if (m_pfValues[4] > m_pfValues[8]) {
38  Real fS = Sqrt(1.0f + m_pfValues[4] - m_pfValues[0] - m_pfValues[8]) * 2.0f;
39  return CQuaternion(
40  (m_pfValues[2] - m_pfValues[6]) / fS,
41  (m_pfValues[3] + m_pfValues[1]) / fS,
42  fS / 4.0f,
43  (m_pfValues[7] + m_pfValues[5]) / fS);
44  }
45  else {
46  Real fS = Sqrt(1.0f + m_pfValues[8] - m_pfValues[0] - m_pfValues[4]) * 2.0f;
47  return CQuaternion(
48  (m_pfValues[3] - m_pfValues[1]) / fS,
49  (m_pfValues[2] + m_pfValues[6]) / fS,
50  (m_pfValues[7] + m_pfValues[5]) / fS,
51  fS / 4.0f);
52  }
53  }
54 
55  /****************************************/
56  /****************************************/
57 
59  m_pfValues[0] = c_matrix.m_pfValues[0];
60  m_pfValues[1] = c_matrix.m_pfValues[1];
61  m_pfValues[2] = c_matrix.m_pfValues[2];
62  m_pfValues[3] = c_matrix.m_pfValues[3];
63  m_pfValues[4] = c_matrix.m_pfValues[4];
64  m_pfValues[5] = c_matrix.m_pfValues[5];
65  m_pfValues[6] = c_matrix.m_pfValues[6];
66  m_pfValues[7] = c_matrix.m_pfValues[7];
67  m_pfValues[8] = c_matrix.m_pfValues[8];
68  }
69 
70  /****************************************/
71  /****************************************/
72 
73  void CRotationMatrix3::SetFromAngles(const CRadians& c_z_angle, const CRadians& c_y_angle, const CRadians& c_x_angle) {
74  Real fSinX = Sin(c_x_angle);
75  Real fCosX = Cos(c_x_angle);
76  Real fSinY = Sin(c_y_angle);
77  Real fCosY = Cos(c_y_angle);
78  Real fSinZ = Sin(c_z_angle);
79  Real fCosZ = Cos(c_z_angle);
80  m_pfValues[0] = (fCosZ * fCosY);
81  m_pfValues[1] = (fCosZ * fSinY * fSinX) - (fCosX * fSinZ);
82  m_pfValues[2] = (fSinZ * fSinX) + (fCosZ * fCosX * fSinY);
83  m_pfValues[3] = (fCosY * fSinZ);
84  m_pfValues[4] = (fCosZ * fCosX) + (fSinZ * fSinY * fSinX);
85  m_pfValues[5] = (fCosX * fSinZ * fSinY) - (fCosZ * fSinX);
86  m_pfValues[6] = -fSinY;
87  m_pfValues[7] = (fCosY * fSinX);
88  m_pfValues[8] = (fCosY * fCosX);
89  }
90 
91  /****************************************/
92  /****************************************/
93 
94  void CRotationMatrix3::SetFromValues(Real f_value0, Real f_value1, Real f_value2,
95  Real f_value3, Real f_value4, Real f_value5,
96  Real f_value6, Real f_value7, Real f_value8) {
97  m_pfValues[0] = f_value0;
98  m_pfValues[1] = f_value1;
99  m_pfValues[2] = f_value2;
100  m_pfValues[3] = f_value3;
101  m_pfValues[4] = f_value4;
102  m_pfValues[5] = f_value5;
103  m_pfValues[6] = f_value6;
104  m_pfValues[7] = f_value7;
105  m_pfValues[8] = f_value8;
106  }
107 
108  /****************************************/
109  /****************************************/
110 
112  return CVector3(m_pfValues[0]*c_vector.m_fX + m_pfValues[1]*c_vector.m_fY + m_pfValues[2]*c_vector.m_fZ,
113  m_pfValues[3]*c_vector.m_fX + m_pfValues[4]*c_vector.m_fY + m_pfValues[5]*c_vector.m_fZ,
114  m_pfValues[6]*c_vector.m_fX + m_pfValues[7]*c_vector.m_fY + m_pfValues[8]*c_vector.m_fZ);
115  }
116 
117  /****************************************/
118  /****************************************/
119 }
A 3D vector class.
Definition: vector3.h:29
#define Sqrt
Definition: general.h:64
float Real
Collects all ARGoS code.
Definition: datatypes.h:39
Real Cos(const CRadians &c_radians)
Computes the cosine of the passed value in radians.
Definition: angles.h:595
void SetFromValues(Real f_value0, Real f_value1, Real f_value2, Real f_value3, Real f_value4, Real f_value5, Real f_value6, Real f_value7, Real f_value8)
It defines the basic type CRadians, used to store an angle value in radians.
Definition: angles.h:42
void SetFromAngles(const CRadians &c_x_angle, const CRadians &c_y_angle, const CRadians &c_z_angle)
void SetFromMatrix(const CMatrix< 3, 3 > &c_matrix)
Real Sin(const CRadians &c_radians)
Computes the sine of the passed value in radians.
Definition: angles.h:586
Real m_pfValues[ROWS *COLS]
Definition: matrix.h:222
CVector3 operator*(const CVector3 &c_vector) const
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12