00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #ifndef TF_QUATERNION_H_
00018 #define TF_QUATERNION_H_
00019
00020
00021 #include "Vector3.h"
00022 #include "QuadWord.h"
00023
00024 namespace tf
00025 {
00026
00028 class Quaternion : public QuadWord {
00029 public:
00031 Quaternion() {}
00032
00033
00034
00035
00037 Quaternion(const tfScalar& x, const tfScalar& y, const tfScalar& z, const tfScalar& w)
00038 : QuadWord(x, y, z, w)
00039 {}
00043 Quaternion(const Vector3& axis, const tfScalar& angle)
00044 {
00045 setRotation(axis, angle);
00046 }
00051 Quaternion(const tfScalar& yaw, const tfScalar& pitch, const tfScalar& roll) __attribute__((deprecated))
00052 {
00053 #ifndef TF_EULER_DEFAULT_ZYX
00054 setEuler(yaw, pitch, roll);
00055 #else
00056 setRPY(roll, pitch, yaw);
00057 #endif
00058 }
00062 void setRotation(const Vector3& axis, const tfScalar& angle)
00063 {
00064 tfScalar d = axis.length();
00065 tfAssert(d != tfScalar(0.0));
00066 tfScalar s = tfSin(angle * tfScalar(0.5)) / d;
00067 setValue(axis.x() * s, axis.y() * s, axis.z() * s,
00068 tfCos(angle * tfScalar(0.5)));
00069 }
00074 void setEuler(const tfScalar& yaw, const tfScalar& pitch, const tfScalar& roll)
00075 {
00076 tfScalar halfYaw = tfScalar(yaw) * tfScalar(0.5);
00077 tfScalar halfPitch = tfScalar(pitch) * tfScalar(0.5);
00078 tfScalar halfRoll = tfScalar(roll) * tfScalar(0.5);
00079 tfScalar cosYaw = tfCos(halfYaw);
00080 tfScalar sinYaw = tfSin(halfYaw);
00081 tfScalar cosPitch = tfCos(halfPitch);
00082 tfScalar sinPitch = tfSin(halfPitch);
00083 tfScalar cosRoll = tfCos(halfRoll);
00084 tfScalar sinRoll = tfSin(halfRoll);
00085 setValue(cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw,
00086 cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw,
00087 sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw,
00088 cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw);
00089 }
00094 void setRPY(const tfScalar& roll, const tfScalar& pitch, const tfScalar& yaw)
00095 {
00096 tfScalar halfYaw = tfScalar(yaw) * tfScalar(0.5);
00097 tfScalar halfPitch = tfScalar(pitch) * tfScalar(0.5);
00098 tfScalar halfRoll = tfScalar(roll) * tfScalar(0.5);
00099 tfScalar cosYaw = tfCos(halfYaw);
00100 tfScalar sinYaw = tfSin(halfYaw);
00101 tfScalar cosPitch = tfCos(halfPitch);
00102 tfScalar sinPitch = tfSin(halfPitch);
00103 tfScalar cosRoll = tfCos(halfRoll);
00104 tfScalar sinRoll = tfSin(halfRoll);
00105 setValue(sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw,
00106 cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw,
00107 cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw,
00108 cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw);
00109 }
00114 void setEulerZYX(const tfScalar& yaw, const tfScalar& pitch, const tfScalar& roll) __attribute__((deprecated))
00115 {
00116 setRPY(roll, pitch, yaw);
00117 }
00120 TFSIMD_FORCE_INLINE Quaternion& operator+=(const Quaternion& q)
00121 {
00122 m_floats[0] += q.x(); m_floats[1] += q.y(); m_floats[2] += q.z(); m_floats[3] += q.m_floats[3];
00123 return *this;
00124 }
00125
00128 Quaternion& operator-=(const Quaternion& q)
00129 {
00130 m_floats[0] -= q.x(); m_floats[1] -= q.y(); m_floats[2] -= q.z(); m_floats[3] -= q.m_floats[3];
00131 return *this;
00132 }
00133
00136 Quaternion& operator*=(const tfScalar& s)
00137 {
00138 m_floats[0] *= s; m_floats[1] *= s; m_floats[2] *= s; m_floats[3] *= s;
00139 return *this;
00140 }
00141
00145 Quaternion& operator*=(const Quaternion& q)
00146 {
00147 setValue(m_floats[3] * q.x() + m_floats[0] * q.m_floats[3] + m_floats[1] * q.z() - m_floats[2] * q.y(),
00148 m_floats[3] * q.y() + m_floats[1] * q.m_floats[3] + m_floats[2] * q.x() - m_floats[0] * q.z(),
00149 m_floats[3] * q.z() + m_floats[2] * q.m_floats[3] + m_floats[0] * q.y() - m_floats[1] * q.x(),
00150 m_floats[3] * q.m_floats[3] - m_floats[0] * q.x() - m_floats[1] * q.y() - m_floats[2] * q.z());
00151 return *this;
00152 }
00155 tfScalar dot(const Quaternion& q) const
00156 {
00157 return m_floats[0] * q.x() + m_floats[1] * q.y() + m_floats[2] * q.z() + m_floats[3] * q.m_floats[3];
00158 }
00159
00161 tfScalar length2() const
00162 {
00163 return dot(*this);
00164 }
00165
00167 tfScalar length() const
00168 {
00169 return tfSqrt(length2());
00170 }
00171
00174 Quaternion& normalize()
00175 {
00176 return *this /= length();
00177 }
00178
00181 TFSIMD_FORCE_INLINE Quaternion
00182 operator*(const tfScalar& s) const
00183 {
00184 return Quaternion(x() * s, y() * s, z() * s, m_floats[3] * s);
00185 }
00186
00187
00190 Quaternion operator/(const tfScalar& s) const
00191 {
00192 tfAssert(s != tfScalar(0.0));
00193 return *this * (tfScalar(1.0) / s);
00194 }
00195
00198 Quaternion& operator/=(const tfScalar& s)
00199 {
00200 tfAssert(s != tfScalar(0.0));
00201 return *this *= tfScalar(1.0) / s;
00202 }
00203
00205 Quaternion normalized() const
00206 {
00207 return *this / length();
00208 }
00211 tfScalar angle(const Quaternion& q) const
00212 {
00213 tfScalar s = tfSqrt(length2() * q.length2());
00214 tfAssert(s != tfScalar(0.0));
00215 return tfAcos(dot(q) / s);
00216 }
00219 tfScalar angleShortestPath(const Quaternion& q) const
00220 {
00221 tfScalar s = tfSqrt(length2() * q.length2());
00222 tfAssert(s != tfScalar(0.0));
00223 if (dot(q) < 0)
00224 return tfAcos(dot(-q) / s) * tfScalar(2.0);
00225 else
00226 return tfAcos(dot(q) / s) * tfScalar(2.0);
00227 }
00229 tfScalar getAngle() const
00230 {
00231 tfScalar s = tfScalar(2.) * tfAcos(m_floats[3]);
00232 return s;
00233 }
00234
00236 tfScalar getAngleShortestPath() const
00237 {
00238 tfScalar s;
00239 if (m_floats[3] < 0)
00240 s = tfScalar(2.) * tfAcos(-m_floats[3]);
00241 else
00242 s = tfScalar(2.) * tfAcos(m_floats[3]);
00243 return s;
00244 }
00245
00247 Vector3 getAxis() const
00248 {
00249 tfScalar s_squared = tfScalar(1.) - tfPow(m_floats[3], tfScalar(2.));
00250 if (s_squared < tfScalar(10.) * TFSIMD_EPSILON)
00251 return Vector3(1.0, 0.0, 0.0);
00252 tfScalar s = tfSqrt(s_squared);
00253 return Vector3(m_floats[0] / s, m_floats[1] / s, m_floats[2] / s);
00254 }
00255
00257 Quaternion inverse() const
00258 {
00259 return Quaternion(-m_floats[0], -m_floats[1], -m_floats[2], m_floats[3]);
00260 }
00261
00264 TFSIMD_FORCE_INLINE Quaternion
00265 operator+(const Quaternion& q2) const
00266 {
00267 const Quaternion& q1 = *this;
00268 return Quaternion(q1.x() + q2.x(), q1.y() + q2.y(), q1.z() + q2.z(), q1.m_floats[3] + q2.m_floats[3]);
00269 }
00270
00273 TFSIMD_FORCE_INLINE Quaternion
00274 operator-(const Quaternion& q2) const
00275 {
00276 const Quaternion& q1 = *this;
00277 return Quaternion(q1.x() - q2.x(), q1.y() - q2.y(), q1.z() - q2.z(), q1.m_floats[3] - q2.m_floats[3]);
00278 }
00279
00282 TFSIMD_FORCE_INLINE Quaternion operator-() const
00283 {
00284 const Quaternion& q2 = *this;
00285 return Quaternion( - q2.x(), - q2.y(), - q2.z(), - q2.m_floats[3]);
00286 }
00288 TFSIMD_FORCE_INLINE Quaternion farthest( const Quaternion& qd) const
00289 {
00290 Quaternion diff,sum;
00291 diff = *this - qd;
00292 sum = *this + qd;
00293 if( diff.dot(diff) > sum.dot(sum) )
00294 return qd;
00295 return (-qd);
00296 }
00297
00299 TFSIMD_FORCE_INLINE Quaternion nearest( const Quaternion& qd) const
00300 {
00301 Quaternion diff,sum;
00302 diff = *this - qd;
00303 sum = *this + qd;
00304 if( diff.dot(diff) < sum.dot(sum) )
00305 return qd;
00306 return (-qd);
00307 }
00308
00309
00314 Quaternion slerp(const Quaternion& q, const tfScalar& t) const
00315 {
00316 tfScalar theta = angleShortestPath(q) / tfScalar(2.0);
00317 if (theta != tfScalar(0.0))
00318 {
00319 tfScalar d = tfScalar(1.0) / tfSin(theta);
00320 tfScalar s0 = tfSin((tfScalar(1.0) - t) * theta);
00321 tfScalar s1 = tfSin(t * theta);
00322 if (dot(q) < 0)
00323 return Quaternion((m_floats[0] * s0 + -q.x() * s1) * d,
00324 (m_floats[1] * s0 + -q.y() * s1) * d,
00325 (m_floats[2] * s0 + -q.z() * s1) * d,
00326 (m_floats[3] * s0 + -q.m_floats[3] * s1) * d);
00327 else
00328 return Quaternion((m_floats[0] * s0 + q.x() * s1) * d,
00329 (m_floats[1] * s0 + q.y() * s1) * d,
00330 (m_floats[2] * s0 + q.z() * s1) * d,
00331 (m_floats[3] * s0 + q.m_floats[3] * s1) * d);
00332
00333 }
00334 else
00335 {
00336 return *this;
00337 }
00338 }
00339
00340 static const Quaternion& getIdentity()
00341 {
00342 static const Quaternion identityQuat(tfScalar(0.),tfScalar(0.),tfScalar(0.),tfScalar(1.));
00343 return identityQuat;
00344 }
00345
00346 TFSIMD_FORCE_INLINE const tfScalar& getW() const { return m_floats[3]; }
00347
00348
00349 };
00350
00351
00353 TFSIMD_FORCE_INLINE Quaternion
00354 operator-(const Quaternion& q)
00355 {
00356 return Quaternion(-q.x(), -q.y(), -q.z(), -q.w());
00357 }
00358
00359
00360
00362 TFSIMD_FORCE_INLINE Quaternion
00363 operator*(const Quaternion& q1, const Quaternion& q2) {
00364 return Quaternion(q1.w() * q2.x() + q1.x() * q2.w() + q1.y() * q2.z() - q1.z() * q2.y(),
00365 q1.w() * q2.y() + q1.y() * q2.w() + q1.z() * q2.x() - q1.x() * q2.z(),
00366 q1.w() * q2.z() + q1.z() * q2.w() + q1.x() * q2.y() - q1.y() * q2.x(),
00367 q1.w() * q2.w() - q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z());
00368 }
00369
00370 TFSIMD_FORCE_INLINE Quaternion
00371 operator*(const Quaternion& q, const Vector3& w)
00372 {
00373 return Quaternion( q.w() * w.x() + q.y() * w.z() - q.z() * w.y(),
00374 q.w() * w.y() + q.z() * w.x() - q.x() * w.z(),
00375 q.w() * w.z() + q.x() * w.y() - q.y() * w.x(),
00376 -q.x() * w.x() - q.y() * w.y() - q.z() * w.z());
00377 }
00378
00379 TFSIMD_FORCE_INLINE Quaternion
00380 operator*(const Vector3& w, const Quaternion& q)
00381 {
00382 return Quaternion( w.x() * q.w() + w.y() * q.z() - w.z() * q.y(),
00383 w.y() * q.w() + w.z() * q.x() - w.x() * q.z(),
00384 w.z() * q.w() + w.x() * q.y() - w.y() * q.x(),
00385 -w.x() * q.x() - w.y() * q.y() - w.z() * q.z());
00386 }
00387
00389 TFSIMD_FORCE_INLINE tfScalar
00390 dot(const Quaternion& q1, const Quaternion& q2)
00391 {
00392 return q1.dot(q2);
00393 }
00394
00395
00397 TFSIMD_FORCE_INLINE tfScalar
00398 length(const Quaternion& q)
00399 {
00400 return q.length();
00401 }
00402
00404 TFSIMD_FORCE_INLINE tfScalar
00405 angle(const Quaternion& q1, const Quaternion& q2)
00406 {
00407 return q1.angle(q2);
00408 }
00409
00411 TFSIMD_FORCE_INLINE tfScalar
00412 angleShortestPath(const Quaternion& q1, const Quaternion& q2)
00413 {
00414 return q1.angleShortestPath(q2);
00415 }
00416
00418 TFSIMD_FORCE_INLINE Quaternion
00419 inverse(const Quaternion& q)
00420 {
00421 return q.inverse();
00422 }
00423
00429 TFSIMD_FORCE_INLINE Quaternion
00430 slerp(const Quaternion& q1, const Quaternion& q2, const tfScalar& t)
00431 {
00432 return q1.slerp(q2, t);
00433 }
00434
00435 TFSIMD_FORCE_INLINE Vector3
00436 quatRotate(const Quaternion& rotation, const Vector3& v)
00437 {
00438 Quaternion q = rotation * v;
00439 q *= rotation.inverse();
00440 return Vector3(q.getX(),q.getY(),q.getZ());
00441 }
00442
00443 TFSIMD_FORCE_INLINE Quaternion
00444 shortestArcQuat(const Vector3& v0, const Vector3& v1)
00445 {
00446 Vector3 c = v0.cross(v1);
00447 tfScalar d = v0.dot(v1);
00448
00449 if (d < -1.0 + TFSIMD_EPSILON)
00450 {
00451 Vector3 n,unused;
00452 tfPlaneSpace1(v0,n,unused);
00453 return Quaternion(n.x(),n.y(),n.z(),0.0f);
00454 }
00455
00456 tfScalar s = tfSqrt((1.0f + d) * 2.0f);
00457 tfScalar rs = 1.0f / s;
00458
00459 return Quaternion(c.getX()*rs,c.getY()*rs,c.getZ()*rs,s * 0.5f);
00460 }
00461
00462 TFSIMD_FORCE_INLINE Quaternion
00463 shortestArcQuatNormalize2(Vector3& v0,Vector3& v1)
00464 {
00465 v0.normalize();
00466 v1.normalize();
00467 return shortestArcQuat(v0,v1);
00468 }
00469
00470 }
00471 #endif
00472
00473
00474
00475