ARGoS
3
A parallel, multi-engine simulator for swarm robotics
|
00001 00007 #ifndef CQUATERNION_H 00008 #define CQUATERNION_H 00009 00010 #include <argos3/core/utility/math/vector3.h> 00011 00012 namespace argos { 00013 00014 class CQuaternion { 00015 00016 public: 00017 CQuaternion() { 00018 m_fValues[0] = 1.0; 00019 m_fValues[1] = 0.0; 00020 m_fValues[2] = 0.0; 00021 m_fValues[3] = 0.0; 00022 } 00023 00024 CQuaternion(const CQuaternion& c_quaternion) { 00025 *this = c_quaternion; 00026 } 00027 00028 CQuaternion(Real f_real, 00029 Real f_img1, 00030 Real f_img2, 00031 Real f_img3) { 00032 m_fValues[0] = f_real; 00033 m_fValues[1] = f_img1; 00034 m_fValues[2] = f_img2; 00035 m_fValues[3] = f_img3; 00036 } 00037 00038 CQuaternion(const CRadians& c_radians, 00039 const CVector3& c_vector3) { 00040 FromAngleAxis(c_radians, c_vector3); 00041 } 00042 00043 inline CQuaternion(const CVector3& c_vector1, 00044 const CVector3& c_vector2) { 00045 BetweenTwoVectors(c_vector1, c_vector2); 00046 } 00047 00048 inline Real GetW() const { 00049 return m_fValues[0]; 00050 } 00051 00052 inline Real GetX() const { 00053 return m_fValues[1]; 00054 } 00055 00056 inline Real GetY() const { 00057 return m_fValues[2]; 00058 } 00059 00060 inline Real GetZ() const { 00061 return m_fValues[3]; 00062 } 00063 00064 inline CQuaternion Conjugate() const { 00065 return CQuaternion(m_fValues[0], 00066 -m_fValues[1], 00067 -m_fValues[2], 00068 -m_fValues[3]); 00069 } 00070 00071 inline CQuaternion Inverse() const { 00072 return CQuaternion(m_fValues[0], 00073 -m_fValues[1], 00074 -m_fValues[2], 00075 -m_fValues[3]); 00076 } 00077 00078 inline Real Length() const { 00079 return ::sqrt(SquareLength()); 00080 } 00081 00082 inline Real SquareLength() const { 00083 return 00084 Square(m_fValues[0]) + 00085 Square(m_fValues[1]) + 00086 Square(m_fValues[2]) + 00087 Square(m_fValues[3]); 00088 } 00089 00090 inline CQuaternion& Normalize() { 00091 Real fInvLength = 1.0 / Length(); 00092 m_fValues[0] *= fInvLength; 00093 m_fValues[1] *= fInvLength; 00094 m_fValues[2] *= fInvLength; 00095 m_fValues[3] *= fInvLength; 00096 return *this; 00097 } 00098 00099 inline CQuaternion& FromAngleAxis(const CRadians& c_angle, 00100 const CVector3& c_vector) { 00101 CRadians cHalfAngle = c_angle * 0.5; 00102 Real fSin, fCos; 00103 #ifdef ARGOS_SINCOS 00104 SinCos(cHalfAngle, fSin, fCos); 00105 #else 00106 fSin = Sin(cHalfAngle); 00107 fCos = Cos(cHalfAngle); 00108 #endif 00109 m_fValues[0] = fCos; 00110 m_fValues[1] = c_vector.GetX() * fSin; 00111 m_fValues[2] = c_vector.GetY() * fSin; 00112 m_fValues[3] = c_vector.GetZ() * fSin; 00113 return *this; 00114 } 00115 00116 inline void ToAngleAxis(CRadians& c_angle, 00117 CVector3& c_vector) const { 00118 Real fSquareLength = 00119 Square(m_fValues[1]) + 00120 Square(m_fValues[2]) + 00121 Square(m_fValues[3]); 00122 if(fSquareLength > 0.0f) { 00123 c_angle = 2.0f * ACos(m_fValues[0]); 00124 Real fInvLength = 1.0f / ::sqrt(fSquareLength); 00125 c_vector.Set(m_fValues[1] * fInvLength, 00126 m_fValues[2] * fInvLength, 00127 m_fValues[3] * fInvLength); 00128 } 00129 else { 00130 /* By default, to ease the support of robot orientation, no rotation refers to the Z axis */ 00131 c_angle = CRadians::ZERO; 00132 c_vector = CVector3::Z; 00133 } 00134 } 00135 00136 inline CQuaternion& FromEulerAngles(const CRadians& c_z_angle, 00137 const CRadians& c_y_angle, 00138 const CRadians& c_x_angle) { 00139 (*this) = CQuaternion(c_x_angle, CVector3::X) * 00140 CQuaternion(c_y_angle, CVector3::Y) * 00141 CQuaternion(c_z_angle, CVector3::Z); 00142 return (*this); 00143 } 00144 00145 inline void ToEulerAngles(CRadians& c_z_angle, 00146 CRadians& c_y_angle, 00147 CRadians& c_x_angle) const { 00148 /* With the ZYX convention, gimbal lock happens when 00149 cos(y_angle) = 0, that is when y_angle = +- pi/2 00150 In this condition, the Z and X axis overlap and we 00151 lose one degree of freedom. It's a problem of the 00152 Euler representation of rotations that is not 00153 present when we deal with quaternions. 00154 For reasons of speed, we consider gimbal lock 00155 happened when fTest > 0.499 and when fTest < -0.499. 00156 */ 00157 /* Computed to understand if we have gimbal lock or not */ 00158 Real fTest = 00159 m_fValues[1] * m_fValues[3] + 00160 m_fValues[0] * m_fValues[2]; 00161 00162 if(fTest > 0.499f) { 00163 /* Gimbal lock */ 00164 c_x_angle = CRadians::ZERO; 00165 c_y_angle = CRadians::PI_OVER_TWO; 00166 c_z_angle = ATan2(2.0f * (m_fValues[1] * m_fValues[2] + m_fValues[0] * m_fValues[3]), 00167 1.0f - 2.0f * (m_fValues[1] * m_fValues[1] + m_fValues[3] * m_fValues[3])); 00168 } 00169 else if(fTest < -0.499f) { 00170 /* Gimbal lock */ 00171 c_x_angle = CRadians::ZERO; 00172 c_y_angle = -CRadians::PI_OVER_TWO; 00173 c_z_angle = ATan2(2.0f * (m_fValues[1] * m_fValues[2] + m_fValues[0] * m_fValues[3]), 00174 1.0f - 2.0f * (m_fValues[1] * m_fValues[1] + m_fValues[3] * m_fValues[3])); 00175 } 00176 else { 00177 /* Normal case */ 00178 Real fSqValues[4] = { 00179 Square(m_fValues[0]), 00180 Square(m_fValues[1]), 00181 Square(m_fValues[2]), 00182 Square(m_fValues[3]) 00183 }; 00184 00185 c_x_angle = ATan2(2.0 * (m_fValues[0] * m_fValues[1] - m_fValues[2] * m_fValues[3]), 00186 fSqValues[0] - fSqValues[1] - fSqValues[2] + fSqValues[3]); 00187 c_y_angle = ASin(2.0 * (m_fValues[1] * m_fValues[3] + m_fValues[0] * m_fValues[2])); 00188 c_z_angle = ATan2(2.0 * (m_fValues[0] * m_fValues[3] - m_fValues[1] * m_fValues[2]), 00189 fSqValues[0] + fSqValues[1] - fSqValues[2] - fSqValues[3]); 00190 } 00191 } 00192 00193 inline CQuaternion& BetweenTwoVectors(const CVector3& c_vector1, 00194 const CVector3& c_vector2) { 00195 m_fValues[0] = ::sqrt(c_vector1.SquareLength() * c_vector2.SquareLength()) + c_vector1.DotProduct(c_vector2); 00196 CVector3 cCrossProd(c_vector1); 00197 cCrossProd.CrossProduct(c_vector2); 00198 m_fValues[1] = cCrossProd.GetX(); 00199 m_fValues[2] = cCrossProd.GetY(); 00200 m_fValues[3] = cCrossProd.GetZ(); 00201 Normalize(); 00202 return *this; 00203 } 00204 00205 inline bool operator==(const CQuaternion& c_quaternion) { 00206 return (m_fValues[0] == c_quaternion.m_fValues[0] && 00207 m_fValues[1] == c_quaternion.m_fValues[1] && 00208 m_fValues[2] == c_quaternion.m_fValues[2] && 00209 m_fValues[3] == c_quaternion.m_fValues[3]); 00210 } 00211 00212 inline CQuaternion& operator=(const CQuaternion& c_quaternion) { 00213 if (&c_quaternion != this) { 00214 m_fValues[0] = c_quaternion.m_fValues[0]; 00215 m_fValues[1] = c_quaternion.m_fValues[1]; 00216 m_fValues[2] = c_quaternion.m_fValues[2]; 00217 m_fValues[3] = c_quaternion.m_fValues[3]; 00218 } 00219 return *this; 00220 } 00221 00222 inline CQuaternion& operator+=(const CQuaternion& c_quaternion) { 00223 m_fValues[0] += c_quaternion.m_fValues[0]; 00224 m_fValues[1] += c_quaternion.m_fValues[1]; 00225 m_fValues[2] += c_quaternion.m_fValues[2]; 00226 m_fValues[3] += c_quaternion.m_fValues[3]; 00227 return *this; 00228 } 00229 00230 inline CQuaternion& operator-=(const CQuaternion& c_quaternion) { 00231 m_fValues[0] -= c_quaternion.m_fValues[0]; 00232 m_fValues[1] -= c_quaternion.m_fValues[1]; 00233 m_fValues[2] -= c_quaternion.m_fValues[2]; 00234 m_fValues[3] -= c_quaternion.m_fValues[3]; 00235 return *this; 00236 } 00237 00238 inline CQuaternion& operator*=(const CQuaternion& c_quaternion) { 00239 Real newv[4]; 00240 newv[0] = m_fValues[0] * c_quaternion.m_fValues[0] - 00241 m_fValues[1] * c_quaternion.m_fValues[1] - 00242 m_fValues[2] * c_quaternion.m_fValues[2] - 00243 m_fValues[3] * c_quaternion.m_fValues[3]; 00244 newv[1] = m_fValues[0] * c_quaternion.m_fValues[1] + 00245 m_fValues[1] * c_quaternion.m_fValues[0] + 00246 m_fValues[2] * c_quaternion.m_fValues[3] - 00247 m_fValues[3] * c_quaternion.m_fValues[2]; 00248 newv[2] = m_fValues[0] * c_quaternion.m_fValues[2] - 00249 m_fValues[1] * c_quaternion.m_fValues[3] + 00250 m_fValues[2] * c_quaternion.m_fValues[0] + 00251 m_fValues[3] * c_quaternion.m_fValues[1]; 00252 newv[3] = m_fValues[0] * c_quaternion.m_fValues[3] + 00253 m_fValues[1] * c_quaternion.m_fValues[2] - 00254 m_fValues[2] * c_quaternion.m_fValues[1] + 00255 m_fValues[3] * c_quaternion.m_fValues[0]; 00256 m_fValues[0] = newv[0]; 00257 m_fValues[1] = newv[1]; 00258 m_fValues[2] = newv[2]; 00259 m_fValues[3] = newv[3]; 00260 return *this; 00261 } 00262 00263 inline CQuaternion operator+(const CQuaternion& c_quaternion) const { 00264 CQuaternion result(*this); 00265 result += c_quaternion; 00266 return result; 00267 } 00268 00269 inline CQuaternion operator-(const CQuaternion& c_quaternion) const { 00270 CQuaternion result(*this); 00271 result -= c_quaternion; 00272 return result; 00273 } 00274 00275 inline CQuaternion operator*(const CQuaternion& c_quaternion) const { 00276 CQuaternion result(*this); 00277 result *= c_quaternion; 00278 return result; 00279 } 00280 00288 inline friend std::ostream& operator<<(std::ostream& c_os, const CQuaternion& c_quaternion) { 00289 CRadians cZAngle, cYAngle, cXAngle; 00290 c_quaternion.ToEulerAngles(cZAngle, cYAngle, cXAngle); 00291 c_os << ToDegrees(cZAngle).GetValue() << "," 00292 << ToDegrees(cYAngle).GetValue() << "," 00293 << ToDegrees(cXAngle).GetValue(); 00294 return c_os; 00295 } 00296 00304 inline friend std::istream& operator>>(std::istream& c_is, CQuaternion& c_quaternion) { 00305 Real fValues[3]; 00306 ParseValues<Real>(c_is, 3, fValues, ','); 00307 c_quaternion.FromEulerAngles(ToRadians(CDegrees(fValues[0])), 00308 ToRadians(CDegrees(fValues[1])), 00309 ToRadians(CDegrees(fValues[2]))); 00310 return c_is; 00311 } 00312 00313 private: 00314 00315 Real m_fValues[4]; 00316 }; 00317 00318 } 00319 00320 #endif