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 (dot(*this) < 0)
00240 s = tfScalar(2.) * tfAcos(m_floats[3]);
00241 else
00242 s = tfScalar(2.) * tfAcos(-m_floats[3]);
00243
00244 return s;
00245 }
00246
00248 Vector3 getAxis() const
00249 {
00250 tfScalar s_squared = tfScalar(1.) - tfPow(m_floats[3], tfScalar(2.));
00251 if (s_squared < tfScalar(10.) * TFSIMD_EPSILON)
00252 return Vector3(1.0, 0.0, 0.0);
00253 tfScalar s = tfSqrt(s_squared);
00254 return Vector3(m_floats[0] / s, m_floats[1] / s, m_floats[2] / s);
00255 }
00256
00258 Quaternion inverse() const
00259 {
00260 return Quaternion(-m_floats[0], -m_floats[1], -m_floats[2], m_floats[3]);
00261 }
00262
00265 TFSIMD_FORCE_INLINE Quaternion
00266 operator+(const Quaternion& q2) const
00267 {
00268 const Quaternion& q1 = *this;
00269 return Quaternion(q1.x() + q2.x(), q1.y() + q2.y(), q1.z() + q2.z(), q1.m_floats[3] + q2.m_floats[3]);
00270 }
00271
00274 TFSIMD_FORCE_INLINE Quaternion
00275 operator-(const Quaternion& q2) const
00276 {
00277 const Quaternion& q1 = *this;
00278 return Quaternion(q1.x() - q2.x(), q1.y() - q2.y(), q1.z() - q2.z(), q1.m_floats[3] - q2.m_floats[3]);
00279 }
00280
00283 TFSIMD_FORCE_INLINE Quaternion operator-() const
00284 {
00285 const Quaternion& q2 = *this;
00286 return Quaternion( - q2.x(), - q2.y(), - q2.z(), - q2.m_floats[3]);
00287 }
00289 TFSIMD_FORCE_INLINE Quaternion farthest( const Quaternion& qd) const
00290 {
00291 Quaternion diff,sum;
00292 diff = *this - qd;
00293 sum = *this + qd;
00294 if( diff.dot(diff) > sum.dot(sum) )
00295 return qd;
00296 return (-qd);
00297 }
00298
00300 TFSIMD_FORCE_INLINE Quaternion nearest( const Quaternion& qd) const
00301 {
00302 Quaternion diff,sum;
00303 diff = *this - qd;
00304 sum = *this + qd;
00305 if( diff.dot(diff) < sum.dot(sum) )
00306 return qd;
00307 return (-qd);
00308 }
00309
00310
00315 Quaternion slerp(const Quaternion& q, const tfScalar& t) const
00316 {
00317 tfScalar theta = angleShortestPath(q) / tfScalar(2.0);
00318 if (theta != tfScalar(0.0))
00319 {
00320 tfScalar d = tfScalar(1.0) / tfSin(theta);
00321 tfScalar s0 = tfSin((tfScalar(1.0) - t) * theta);
00322 tfScalar s1 = tfSin(t * theta);
00323 if (dot(q) < 0)
00324 return Quaternion((m_floats[0] * s0 + -q.x() * s1) * d,
00325 (m_floats[1] * s0 + -q.y() * s1) * d,
00326 (m_floats[2] * s0 + -q.z() * s1) * d,
00327 (m_floats[3] * s0 + -q.m_floats[3] * s1) * d);
00328 else
00329 return Quaternion((m_floats[0] * s0 + q.x() * s1) * d,
00330 (m_floats[1] * s0 + q.y() * s1) * d,
00331 (m_floats[2] * s0 + q.z() * s1) * d,
00332 (m_floats[3] * s0 + q.m_floats[3] * s1) * d);
00333
00334 }
00335 else
00336 {
00337 return *this;
00338 }
00339 }
00340
00341 static const Quaternion& getIdentity()
00342 {
00343 static const Quaternion identityQuat(tfScalar(0.),tfScalar(0.),tfScalar(0.),tfScalar(1.));
00344 return identityQuat;
00345 }
00346
00347 TFSIMD_FORCE_INLINE const tfScalar& getW() const { return m_floats[3]; }
00348
00349
00350 };
00351
00352
00354 TFSIMD_FORCE_INLINE Quaternion
00355 operator-(const Quaternion& q)
00356 {
00357 return Quaternion(-q.x(), -q.y(), -q.z(), -q.w());
00358 }
00359
00360
00361
00363 TFSIMD_FORCE_INLINE Quaternion
00364 operator*(const Quaternion& q1, const Quaternion& q2) {
00365 return Quaternion(q1.w() * q2.x() + q1.x() * q2.w() + q1.y() * q2.z() - q1.z() * q2.y(),
00366 q1.w() * q2.y() + q1.y() * q2.w() + q1.z() * q2.x() - q1.x() * q2.z(),
00367 q1.w() * q2.z() + q1.z() * q2.w() + q1.x() * q2.y() - q1.y() * q2.x(),
00368 q1.w() * q2.w() - q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z());
00369 }
00370
00371 TFSIMD_FORCE_INLINE Quaternion
00372 operator*(const Quaternion& q, const Vector3& w)
00373 {
00374 return Quaternion( q.w() * w.x() + q.y() * w.z() - q.z() * w.y(),
00375 q.w() * w.y() + q.z() * w.x() - q.x() * w.z(),
00376 q.w() * w.z() + q.x() * w.y() - q.y() * w.x(),
00377 -q.x() * w.x() - q.y() * w.y() - q.z() * w.z());
00378 }
00379
00380 TFSIMD_FORCE_INLINE Quaternion
00381 operator*(const Vector3& w, const Quaternion& q)
00382 {
00383 return Quaternion( w.x() * q.w() + w.y() * q.z() - w.z() * q.y(),
00384 w.y() * q.w() + w.z() * q.x() - w.x() * q.z(),
00385 w.z() * q.w() + w.x() * q.y() - w.y() * q.x(),
00386 -w.x() * q.x() - w.y() * q.y() - w.z() * q.z());
00387 }
00388
00390 TFSIMD_FORCE_INLINE tfScalar
00391 dot(const Quaternion& q1, const Quaternion& q2)
00392 {
00393 return q1.dot(q2);
00394 }
00395
00396
00398 TFSIMD_FORCE_INLINE tfScalar
00399 length(const Quaternion& q)
00400 {
00401 return q.length();
00402 }
00403
00405 TFSIMD_FORCE_INLINE tfScalar
00406 angle(const Quaternion& q1, const Quaternion& q2)
00407 {
00408 return q1.angle(q2);
00409 }
00410
00412 TFSIMD_FORCE_INLINE tfScalar
00413 angleShortestPath(const Quaternion& q1, const Quaternion& q2)
00414 {
00415 return q1.angleShortestPath(q2);
00416 }
00417
00419 TFSIMD_FORCE_INLINE Quaternion
00420 inverse(const Quaternion& q)
00421 {
00422 return q.inverse();
00423 }
00424
00430 TFSIMD_FORCE_INLINE Quaternion
00431 slerp(const Quaternion& q1, const Quaternion& q2, const tfScalar& t)
00432 {
00433 return q1.slerp(q2, t);
00434 }
00435
00436 TFSIMD_FORCE_INLINE Vector3
00437 quatRotate(const Quaternion& rotation, const Vector3& v)
00438 {
00439 Quaternion q = rotation * v;
00440 q *= rotation.inverse();
00441 return Vector3(q.getX(),q.getY(),q.getZ());
00442 }
00443
00444 TFSIMD_FORCE_INLINE Quaternion
00445 shortestArcQuat(const Vector3& v0, const Vector3& v1)
00446 {
00447 Vector3 c = v0.cross(v1);
00448 tfScalar d = v0.dot(v1);
00449
00450 if (d < -1.0 + TFSIMD_EPSILON)
00451 {
00452 Vector3 n,unused;
00453 tfPlaneSpace1(v0,n,unused);
00454 return Quaternion(n.x(),n.y(),n.z(),0.0f);
00455 }
00456
00457 tfScalar s = tfSqrt((1.0f + d) * 2.0f);
00458 tfScalar rs = 1.0f / s;
00459
00460 return Quaternion(c.getX()*rs,c.getY()*rs,c.getZ()*rs,s * 0.5f);
00461 }
00462
00463 TFSIMD_FORCE_INLINE Quaternion
00464 shortestArcQuatNormalize2(Vector3& v0,Vector3& v1)
00465 {
00466 v0.normalize();
00467 v1.normalize();
00468 return shortestArcQuat(v0,v1);
00469 }
00470
00471 }
00472 #endif
00473
00474
00475
00476