diff options
Diffstat (limited to 'extern/bullet2/LinearMath/btQuaternion.h')
| -rw-r--r-- | extern/bullet2/LinearMath/btQuaternion.h | 433 |
1 files changed, 433 insertions, 0 deletions
diff --git a/extern/bullet2/LinearMath/btQuaternion.h b/extern/bullet2/LinearMath/btQuaternion.h new file mode 100644 index 00000000000..15cf5f868d9 --- /dev/null +++ b/extern/bullet2/LinearMath/btQuaternion.h @@ -0,0 +1,433 @@ +/* +Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + + +#ifndef SIMD__QUATERNION_H_ +#define SIMD__QUATERNION_H_ + + +#include "btVector3.h" +#include "btQuadWord.h" + +/**@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(); + 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); + 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(cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw, + cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, + sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw, + cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); + } + /**@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) + { + 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_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_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_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_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_floats[3] * s); + } + + + /**@brief Return an inversely scaled versionof this quaternion + * @param s The inverse scale factor */ + btQuaternion operator/(const btScalar& s) const + { + 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) + { + 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()); + 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_floats[3]); + return s; + } + + /**@brief Return the axis of the rotation represented by this quaternion */ + btVector3 getAxis() const + { + btScalar s_squared = btScalar(1.) - btPow(m_floats[3], btScalar(2.)); + if (s_squared < btScalar(10.) * SIMD_EPSILON) //Check for divide by zero + return btVector3(1.0, 0.0, 0.0); // Arbitrary + btScalar s = btSqrt(s_squared); + return btVector3(m_floats[0] / s, m_floats[1] / s, m_floats[2] / s); + } + + /**@brief Return the inverse of this quaternion */ + btQuaternion inverse() const + { + 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_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_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_floats[3]); + } + /**@todo document this and it's use */ + SIMD_FORCE_INLINE btQuaternion farthest( const btQuaternion& qd) const + { + btQuaternion diff,sum; + diff = *this - qd; + sum = *this + qd; + if( diff.dot(diff) > sum.dot(sum) ) + return qd; + return (-qd); + } + + /**@todo document this and it's use */ + SIMD_FORCE_INLINE btQuaternion nearest( const btQuaternion& qd) const + { + btQuaternion diff,sum; + diff = *this - qd; + sum = *this + qd; + if( diff.dot(diff) < sum.dot(sum) ) + return qd; + 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); + if (theta != btScalar(0.0)) + { + btScalar d = btScalar(1.0) / btSin(theta); + btScalar s0 = btSin((btScalar(1.0) - t) * theta); + btScalar s1 = btSin(t * theta); + if (dot(q) < 0) // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp + 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 + 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 + { + return *this; + } + } + + 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) +{ + return btQuaternion(-q.x(), -q.y(), -q.z(), -q.w()); +} + + + +/**@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(), + q1.w() * q2.y() + q1.y() * q2.w() + q1.z() * q2.x() - q1.x() * q2.z(), + q1.w() * q2.z() + q1.z() * q2.w() + q1.x() * q2.y() - q1.y() * q2.x(), + q1.w() * q2.w() - q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z()); +} + +SIMD_FORCE_INLINE btQuaternion +operator*(const btQuaternion& q, const btVector3& w) +{ + return btQuaternion( q.w() * w.x() + q.y() * w.z() - q.z() * w.y(), + q.w() * w.y() + q.z() * w.x() - q.x() * w.z(), + q.w() * w.z() + q.x() * w.y() - q.y() * w.x(), + -q.x() * w.x() - q.y() * w.y() - q.z() * w.z()); +} + +SIMD_FORCE_INLINE btQuaternion +operator*(const btVector3& w, const btQuaternion& q) +{ + return btQuaternion( w.x() * q.w() + w.y() * q.z() - w.z() * q.y(), + w.y() * q.w() + w.z() * q.x() - w.x() * q.z(), + w.z() * q.w() + w.x() * q.y() - w.y() * q.x(), + -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) +{ + return q1.dot(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) +{ + return q1.slerp(q2, t); +} + +SIMD_FORCE_INLINE btVector3 +quatRotate(const btQuaternion& rotation, const btVector3& v) +{ + btQuaternion q = rotation * v; + q *= rotation.inverse(); + return btVector3(q.getX(),q.getY(),q.getZ()); +} + +SIMD_FORCE_INLINE btQuaternion +shortestArcQuat(const btVector3& v0, const btVector3& v1) // Game Programming Gems 2.10. make sure v0,v1 are normalized +{ + btVector3 c = v0.cross(v1); + btScalar d = v0.dot(v1); + + if (d < -1.0 + SIMD_EPSILON) + { + btVector3 n,unused; + btPlaneSpace1(v0,n,unused); + return btQuaternion(n.x(),n.y(),n.z(),0.0f); // just pick any vector that is orthogonal to v0 + } + + btScalar s = btSqrt((1.0f + d) * 2.0f); + btScalar rs = 1.0f / s; + + return btQuaternion(c.getX()*rs,c.getY()*rs,c.getZ()*rs,s * 0.5f); +} + +SIMD_FORCE_INLINE btQuaternion +shortestArcQuatNormalize2(btVector3& v0,btVector3& v1) +{ + v0.normalize(); + v1.normalize(); + return shortestArcQuat(v0,v1); +} + +#endif + + + + |