ARGoS  3
A parallel, multi-engine simulator for swarm robotics
rotationmatrix3.cpp
Go to the documentation of this file.
1 
9 #include "rotationmatrix3.h"
10 #include <argos3/core/utility/math/quaternion.h>
11 #include <argos3/core/utility/math/vector3.h>
12 
13 namespace argos {
14 
16  {
17  const Real trace = 1.0f + m_pfValues[0] + m_pfValues[4] + m_pfValues[8];
18 
19  if (trace > 0.00001f) {
20  const Real s = Sqrt(trace) * 2.0f;
21  return CQuaternion(
22  s / 4.0f,
23  (m_pfValues[7] - m_pfValues[5]) / s,
24  (m_pfValues[2] - m_pfValues[6]) / s,
25  (m_pfValues[3] - m_pfValues[1]) / s);
26  }
27  else if (m_pfValues[0] > m_pfValues[4] && m_pfValues[0] > m_pfValues[8]) {
28  const Real s = Sqrt(1.0f + m_pfValues[0] - m_pfValues[4] - m_pfValues[8]) * 2.0f;
29  return CQuaternion(
30  (m_pfValues[7] - m_pfValues[5]) / s,
31  s / 4.0f,
32  (m_pfValues[3] + m_pfValues[1]) / s,
33  (m_pfValues[2] + m_pfValues[6]) / s);
34  }
35  else if (m_pfValues[4] > m_pfValues[8]) {
36  const Real s = Sqrt(1.0f + m_pfValues[4] - m_pfValues[0] - m_pfValues[8]) * 2.0f;
37  return CQuaternion(
38  (m_pfValues[2] - m_pfValues[6]) / s,
39  (m_pfValues[3] + m_pfValues[1]) / s,
40  s / 4.0f,
41  (m_pfValues[7] + m_pfValues[5]) / s);
42  }
43  else {
44  const Real s = Sqrt(1.0f + m_pfValues[8] - m_pfValues[0] - m_pfValues[4]) * 2.0f;
45  return CQuaternion(
46  (m_pfValues[3] - m_pfValues[1]) / s,
47  (m_pfValues[2] + m_pfValues[6]) / s,
48  (m_pfValues[7] + m_pfValues[5]) / s,
49  s / 4.0f);
50  }
51  }
52 
53  /****************************************/
54  /****************************************/
55 
57  m_pfValues[0] = c_matrix.m_pfValues[0];
58  m_pfValues[1] = c_matrix.m_pfValues[1];
59  m_pfValues[2] = c_matrix.m_pfValues[2];
60  m_pfValues[3] = c_matrix.m_pfValues[3];
61  m_pfValues[4] = c_matrix.m_pfValues[4];
62  m_pfValues[5] = c_matrix.m_pfValues[5];
63  m_pfValues[6] = c_matrix.m_pfValues[6];
64  m_pfValues[7] = c_matrix.m_pfValues[7];
65  m_pfValues[8] = c_matrix.m_pfValues[8];
66  }
67 
68  /****************************************/
69  /****************************************/
70 
72  /* This code is adapted from the Bullet source code */
73  Real d = c_quaternion.SquareLength();
74  Real s = 2.0f / d;
75  Real xs = c_quaternion.GetX() * s, ys = c_quaternion.GetY() * s, zs = c_quaternion.GetZ() * s;
76  Real wx = c_quaternion.GetW() * xs, wy = c_quaternion.GetW() * ys, wz = c_quaternion.GetW() * zs;
77  Real xx = c_quaternion.GetX() * xs, xy = c_quaternion.GetX() * ys, xz = c_quaternion.GetX() * zs;
78  Real yy = c_quaternion.GetY() * ys, yz = c_quaternion.GetY() * zs, zz = c_quaternion.GetZ() * zs;
79 
80  /* Set values */
81  m_pfValues[0] = 1.0f - (yy + zz);
82  m_pfValues[1] = xy - wz;
83  m_pfValues[2] = xz + wy;
84  m_pfValues[3] = xy + wz;
85  m_pfValues[4] = 1.0f - (xx + zz);
86  m_pfValues[5] = yz - wx;
87  m_pfValues[6] = xz - wy;
88  m_pfValues[7] = yz + wx;
89  m_pfValues[8] = 1.0f - (xx + yy);
90  }
91 
92  /****************************************/
93  /****************************************/
94 
95  void CRotationMatrix3::SetFromAngles(const CRadians& c_z_angle, const CRadians& c_y_angle, const CRadians& c_x_angle) {
96  Real fSinX = Sin(c_x_angle);
97  Real fCosX = Cos(c_x_angle);
98  Real fSinY = Sin(c_y_angle);
99  Real fCosY = Cos(c_y_angle);
100  Real fSinZ = Sin(c_z_angle);
101  Real fCosZ = Cos(c_z_angle);
102 
103  m_pfValues[0] = (fCosZ * fCosY);
104  m_pfValues[1] = (fCosZ * fSinY * fSinX) - (fCosX * fSinZ);
105  m_pfValues[2] = (fSinZ * fSinX) + (fCosZ * fCosX * fSinY);
106  m_pfValues[3] = (fCosY * fSinZ);
107  m_pfValues[4] = (fCosZ * fCosX) + (fSinZ * fSinY * fSinX);
108  m_pfValues[5] = (fCosX * fSinZ * fSinY) - (fCosZ * fSinX);
109  m_pfValues[6] = -fSinY;
110  m_pfValues[7] = (fCosY * fSinX);
111  m_pfValues[8] = (fCosY * fCosX);
112  }
113 
114  /****************************************/
115  /****************************************/
116 
117  void CRotationMatrix3::SetFromValues(Real f_value0, Real f_value1, Real f_value2,
118  Real f_value3, Real f_value4, Real f_value5,
119  Real f_value6, Real f_value7, Real f_value8) {
120  m_pfValues[0] = f_value0;
121  m_pfValues[1] = f_value1;
122  m_pfValues[2] = f_value2;
123  m_pfValues[3] = f_value3;
124  m_pfValues[4] = f_value4;
125  m_pfValues[5] = f_value5;
126  m_pfValues[6] = f_value6;
127  m_pfValues[7] = f_value7;
128  m_pfValues[8] = f_value8;
129  }
130 
131  /****************************************/
132  /****************************************/
133 
135  return CVector3(m_pfValues[0]*c_vector.m_fX + m_pfValues[1]*c_vector.m_fY + m_pfValues[2]*c_vector.m_fZ,
136  m_pfValues[3]*c_vector.m_fX + m_pfValues[4]*c_vector.m_fY + m_pfValues[5]*c_vector.m_fZ,
137  m_pfValues[6]*c_vector.m_fX + m_pfValues[7]*c_vector.m_fY + m_pfValues[8]*c_vector.m_fZ);
138  }
139 
140  /****************************************/
141  /****************************************/
142 }
Sqrt
#define Sqrt
Definition: general.h:64
argos::CRotationMatrix3::SetFromAngles
void SetFromAngles(const CRadians &c_x_angle, const CRadians &c_y_angle, const CRadians &c_z_angle)
Definition: rotationmatrix3.cpp:95
argos::CQuaternion::GetW
Real GetW() const
Definition: quaternion.h:48
argos::Cos
Real Cos(const CRadians &c_radians)
Computes the cosine of the passed value in radians.
Definition: angles.h:595
argos
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12
argos::CVector3
A 3D vector class.
Definition: vector3.h:29
argos::CRadians
It defines the basic type CRadians, used to store an angle value in radians.
Definition: angles.h:42
argos::CRotationMatrix3::SetFromValues
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)
Definition: rotationmatrix3.cpp:117
argos::CRotationMatrix3::ToQuaternion
CQuaternion ToQuaternion() const
Definition: rotationmatrix3.cpp:15
argos::Sin
Real Sin(const CRadians &c_radians)
Computes the sine of the passed value in radians.
Definition: angles.h:586
rotationmatrix3.h
argos::CQuaternion::SquareLength
Real SquareLength() const
Definition: quaternion.h:108
argos::CQuaternion::GetY
Real GetY() const
Definition: quaternion.h:56
argos::CQuaternion::GetZ
Real GetZ() const
Definition: quaternion.h:60
argos::CQuaternion
Definition: quaternion.h:14
argos::CQuaternion::GetX
Real GetX() const
Definition: quaternion.h:52
argos::CMatrix< DIM, DIM >::m_pfValues
Real m_pfValues[ROWS *COLS]
Definition: matrix.h:214
argos::CMatrix
Definition: matrix.h:20
argos::CRotationMatrix3::operator*
CVector3 operator*(const CVector3 &c_vector) const
Definition: rotationmatrix3.cpp:134
argos::CRotationMatrix3::SetFromMatrix
void SetFromMatrix(const CMatrix< 3, 3 > &c_matrix)
Definition: rotationmatrix3.cpp:56
argos::CRotationMatrix3::SetFromQuaternion
void SetFromQuaternion(const CQuaternion &c_quaternion)
Definition: rotationmatrix3.cpp:71
Real
float Real
Collects all ARGoS code.
Definition: datatypes.h:39