| /* |
| 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); |
| 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 |
| |
| |
| |
| |