diff options
Diffstat (limited to 'extern/bullet2/src/LinearMath/btQuaternion.h')
| -rw-r--r-- | extern/bullet2/src/LinearMath/btQuaternion.h | 164 |
1 files changed, 121 insertions, 43 deletions
diff --git a/extern/bullet2/src/LinearMath/btQuaternion.h b/extern/bullet2/src/LinearMath/btQuaternion.h index 264751b33e7..cbeca2681cc 100644 --- a/extern/bullet2/src/LinearMath/btQuaternion.h +++ b/extern/bullet2/src/LinearMath/btQuaternion.h @@ -17,39 +17,56 @@ subject to the following restrictions: #ifndef SIMD__QUATERNION_H_ #define SIMD__QUATERNION_H_ + #include "btVector3.h" +#include "btQuadWord.h" -///The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatrix3x3, btVector3 and btTransform. +/**@brief The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatrix3x3, btVector3 and btTransform. */ class btQuaternion : public btQuadWord { public: + /**@brief No initialization constructor */ btQuaternion() {} // template <typename btScalar> // explicit Quaternion(const btScalar *v) : Tuple4<btScalar>(v) {} - + /**@brief Constructor from scalars */ btQuaternion(const btScalar& x, const btScalar& y, const btScalar& z, const btScalar& w) : btQuadWord(x, y, z, w) {} - + /**@brief Axis angle Constructor + * @param axis The axis which the rotation is around + * @param angle The magnitude of the rotation around the angle (Radians) */ btQuaternion(const btVector3& axis, const btScalar& angle) { setRotation(axis, angle); } - + /**@brief Constructor from Euler angles + * @param yaw Angle around Y unless BT_EULER_DEFAULT_ZYX defined then Z + * @param pitch Angle around X unless BT_EULER_DEFAULT_ZYX defined then Y + * @param roll Angle around Z unless BT_EULER_DEFAULT_ZYX defined then X */ btQuaternion(const btScalar& yaw, const btScalar& pitch, const btScalar& roll) { +#ifndef BT_EULER_DEFAULT_ZYX setEuler(yaw, pitch, roll); +#else + setEulerZYX(yaw, pitch, roll); +#endif } - + /**@brief Set the rotation using axis angle notation + * @param axis The axis around which to rotate + * @param angle The magnitude of the rotation in Radians */ void setRotation(const btVector3& axis, const btScalar& angle) { btScalar d = axis.length(); - assert(d != btScalar(0.0)); + btAssert(d != btScalar(0.0)); btScalar s = btSin(angle * btScalar(0.5)) / d; setValue(axis.x() * s, axis.y() * s, axis.z() * s, btCos(angle * btScalar(0.5))); } - + /**@brief Set the quaternion using Euler angles + * @param yaw Angle around Y + * @param pitch Angle around X + * @param roll Angle around Z */ void setEuler(const btScalar& yaw, const btScalar& pitch, const btScalar& roll) { btScalar halfYaw = btScalar(yaw) * btScalar(0.5); @@ -66,122 +83,165 @@ public: sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw, cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); } - - btQuaternion& operator+=(const btQuaternion& q) + /**@brief Set the quaternion using euler angles + * @param yaw Angle around Z + * @param pitch Angle around Y + * @param roll Angle around X */ + void setEulerZYX(const btScalar& yaw, const btScalar& pitch, const btScalar& roll) { - m_x += q.x(); m_y += q.y(); m_z += q.z(); m_unusedW += q.m_unusedW; + btScalar halfYaw = btScalar(yaw) * btScalar(0.5); + btScalar halfPitch = btScalar(pitch) * btScalar(0.5); + btScalar halfRoll = btScalar(roll) * btScalar(0.5); + btScalar cosYaw = btCos(halfYaw); + btScalar sinYaw = btSin(halfYaw); + btScalar cosPitch = btCos(halfPitch); + btScalar sinPitch = btSin(halfPitch); + btScalar cosRoll = btCos(halfRoll); + btScalar sinRoll = btSin(halfRoll); + setValue(sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw, //x + cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw, //y + cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, //z + cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); //formerly yzx + } + /**@brief Add two quaternions + * @param q The quaternion to add to this one */ + SIMD_FORCE_INLINE btQuaternion& operator+=(const btQuaternion& q) + { + m_floats[0] += q.x(); m_floats[1] += q.y(); m_floats[2] += q.z(); m_floats[3] += q.m_floats[3]; return *this; } + /**@brief Subtract out a quaternion + * @param q The quaternion to subtract from this one */ btQuaternion& operator-=(const btQuaternion& q) { - m_x -= q.x(); m_y -= q.y(); m_z -= q.z(); m_unusedW -= q.m_unusedW; + m_floats[0] -= q.x(); m_floats[1] -= q.y(); m_floats[2] -= q.z(); m_floats[3] -= q.m_floats[3]; return *this; } + /**@brief Scale this quaternion + * @param s The scalar to scale by */ btQuaternion& operator*=(const btScalar& s) { - m_x *= s; m_y *= s; m_z *= s; m_unusedW *= s; + m_floats[0] *= s; m_floats[1] *= s; m_floats[2] *= s; m_floats[3] *= s; return *this; } - + /**@brief Multiply this quaternion by q on the right + * @param q The other quaternion + * Equivilant to this = this * q */ btQuaternion& operator*=(const btQuaternion& q) { - setValue(m_unusedW * q.x() + m_x * q.m_unusedW + m_y * q.z() - m_z * q.y(), - m_unusedW * q.y() + m_y * q.m_unusedW + m_z * q.x() - m_x * q.z(), - m_unusedW * q.z() + m_z * q.m_unusedW + m_x * q.y() - m_y * q.x(), - m_unusedW * q.m_unusedW - m_x * q.x() - m_y * q.y() - m_z * q.z()); + setValue(m_floats[3] * q.x() + m_floats[0] * q.m_floats[3] + m_floats[1] * q.z() - m_floats[2] * q.y(), + m_floats[3] * q.y() + m_floats[1] * q.m_floats[3] + m_floats[2] * q.x() - m_floats[0] * q.z(), + m_floats[3] * q.z() + m_floats[2] * q.m_floats[3] + m_floats[0] * q.y() - m_floats[1] * q.x(), + m_floats[3] * q.m_floats[3] - m_floats[0] * q.x() - m_floats[1] * q.y() - m_floats[2] * q.z()); return *this; } - + /**@brief Return the dot product between this quaternion and another + * @param q The other quaternion */ btScalar dot(const btQuaternion& q) const { - return m_x * q.x() + m_y * q.y() + m_z * q.z() + m_unusedW * q.m_unusedW; + return m_floats[0] * q.x() + m_floats[1] * q.y() + m_floats[2] * q.z() + m_floats[3] * q.m_floats[3]; } + /**@brief Return the length squared of the quaternion */ btScalar length2() const { return dot(*this); } + /**@brief Return the length of the quaternion */ btScalar length() const { return btSqrt(length2()); } + /**@brief Normalize the quaternion + * Such that x^2 + y^2 + z^2 +w^2 = 1 */ btQuaternion& normalize() { return *this /= length(); } + /**@brief Return a scaled version of this quaternion + * @param s The scale factor */ SIMD_FORCE_INLINE btQuaternion operator*(const btScalar& s) const { - return btQuaternion(x() * s, y() * s, z() * s, m_unusedW * s); + return btQuaternion(x() * s, y() * s, z() * s, m_floats[3] * s); } - + /**@brief Return an inversely scaled versionof this quaternion + * @param s The inverse scale factor */ btQuaternion operator/(const btScalar& s) const { - assert(s != btScalar(0.0)); + btAssert(s != btScalar(0.0)); return *this * (btScalar(1.0) / s); } - + /**@brief Inversely scale this quaternion + * @param s The scale factor */ btQuaternion& operator/=(const btScalar& s) { - assert(s != btScalar(0.0)); + btAssert(s != btScalar(0.0)); return *this *= btScalar(1.0) / s; } - + /**@brief Return a normalized version of this quaternion */ btQuaternion normalized() const { return *this / length(); } - + /**@brief Return the angle between this quaternion and the other + * @param q The other quaternion */ btScalar angle(const btQuaternion& q) const { btScalar s = btSqrt(length2() * q.length2()); - assert(s != btScalar(0.0)); + btAssert(s != btScalar(0.0)); return btAcos(dot(q) / s); } - + /**@brief Return the angle of rotation represented by this quaternion */ btScalar getAngle() const { - btScalar s = btScalar(2.) * btAcos(m_unusedW); + btScalar s = btScalar(2.) * btAcos(m_floats[3]); return s; } - + /**@brief Return the inverse of this quaternion */ btQuaternion inverse() const { - return btQuaternion(-m_x, -m_y, -m_z, m_unusedW); + return btQuaternion(-m_floats[0], -m_floats[1], -m_floats[2], m_floats[3]); } + /**@brief Return the sum of this quaternion and the other + * @param q2 The other quaternion */ SIMD_FORCE_INLINE btQuaternion operator+(const btQuaternion& q2) const { const btQuaternion& q1 = *this; - return btQuaternion(q1.x() + q2.x(), q1.y() + q2.y(), q1.z() + q2.z(), q1.m_unusedW + q2.m_unusedW); + return btQuaternion(q1.x() + q2.x(), q1.y() + q2.y(), q1.z() + q2.z(), q1.m_floats[3] + q2.m_floats[3]); } + /**@brief Return the difference between this quaternion and the other + * @param q2 The other quaternion */ SIMD_FORCE_INLINE btQuaternion operator-(const btQuaternion& q2) const { const btQuaternion& q1 = *this; - return btQuaternion(q1.x() - q2.x(), q1.y() - q2.y(), q1.z() - q2.z(), q1.m_unusedW - q2.m_unusedW); + return btQuaternion(q1.x() - q2.x(), q1.y() - q2.y(), q1.z() - q2.z(), q1.m_floats[3] - q2.m_floats[3]); } + /**@brief Return the negative of this quaternion + * This simply negates each element */ SIMD_FORCE_INLINE btQuaternion operator-() const { const btQuaternion& q2 = *this; - return btQuaternion( - q2.x(), - q2.y(), - q2.z(), - q2.m_unusedW); + return btQuaternion( - q2.x(), - q2.y(), - q2.z(), - q2.m_floats[3]); } - + /**@todo document this and it's use */ SIMD_FORCE_INLINE btQuaternion farthest( const btQuaternion& qd) const { btQuaternion diff,sum; @@ -192,6 +252,10 @@ public: return (-qd); } + /**@brief Return the quaternion which is the result of Spherical Linear Interpolation between this and the other quaternion + * @param q The other quaternion to interpolate with + * @param t The ratio between this and q to interpolate. If t = 0 the result is this, if t=1 the result is q. + * Slerp interpolates assuming constant velocity. */ btQuaternion slerp(const btQuaternion& q, const btScalar& t) const { btScalar theta = angle(q); @@ -200,10 +264,10 @@ public: btScalar d = btScalar(1.0) / btSin(theta); btScalar s0 = btSin((btScalar(1.0) - t) * theta); btScalar s1 = btSin(t * theta); - return btQuaternion((m_x * s0 + q.x() * s1) * d, - (m_y * s0 + q.y() * s1) * d, - (m_z * s0 + q.z() * s1) * d, - (m_unusedW * s0 + q.m_unusedW * s1) * d); + return btQuaternion((m_floats[0] * s0 + q.x() * s1) * d, + (m_floats[1] * s0 + q.y() * s1) * d, + (m_floats[2] * s0 + q.z() * s1) * d, + (m_floats[3] * s0 + q.m_floats[3] * s1) * d); } else { @@ -211,13 +275,19 @@ public: } } - SIMD_FORCE_INLINE const btScalar& getW() const { return m_unusedW; } + static const btQuaternion& getIdentity() + { + static const btQuaternion identityQuat(btScalar(0.),btScalar(0.),btScalar(0.),btScalar(1.)); + return identityQuat; + } + + SIMD_FORCE_INLINE const btScalar& getW() const { return m_floats[3]; } }; - +/**@brief Return the negative of a quaternion */ SIMD_FORCE_INLINE btQuaternion operator-(const btQuaternion& q) { @@ -226,7 +296,7 @@ operator-(const btQuaternion& q) - +/**@brief Return the product of two quaternions */ SIMD_FORCE_INLINE btQuaternion operator*(const btQuaternion& q1, const btQuaternion& q2) { return btQuaternion(q1.w() * q2.x() + q1.x() * q2.w() + q1.y() * q2.z() - q1.z() * q2.y(), @@ -253,6 +323,7 @@ operator*(const btVector3& w, const btQuaternion& q) -w.x() * q.x() - w.y() * q.y() - w.z() * q.z()); } +/**@brief Calculate the dot product between two quaternions */ SIMD_FORCE_INLINE btScalar dot(const btQuaternion& q1, const btQuaternion& q2) { @@ -260,25 +331,32 @@ dot(const btQuaternion& q1, const btQuaternion& q2) } +/**@brief Return the length of a quaternion */ SIMD_FORCE_INLINE btScalar length(const btQuaternion& q) { return q.length(); } +/**@brief Return the angle between two quaternions*/ SIMD_FORCE_INLINE btScalar angle(const btQuaternion& q1, const btQuaternion& q2) { return q1.angle(q2); } - +/**@brief Return the inverse of a quaternion*/ SIMD_FORCE_INLINE btQuaternion inverse(const btQuaternion& q) { return q.inverse(); } +/**@brief Return the result of spherical linear interpolation betwen two quaternions + * @param q1 The first quaternion + * @param q2 The second quaternion + * @param t The ration between q1 and q2. t = 0 return q1, t=1 returns q2 + * Slerp assumes constant velocity between positions. */ SIMD_FORCE_INLINE btQuaternion slerp(const btQuaternion& q1, const btQuaternion& q2, const btScalar& t) { |