diff options
Diffstat (limited to 'extern/bullet/LinearMath/SimdQuaternion.h')
| -rw-r--r-- | extern/bullet/LinearMath/SimdQuaternion.h | 302 |
1 files changed, 302 insertions, 0 deletions
diff --git a/extern/bullet/LinearMath/SimdQuaternion.h b/extern/bullet/LinearMath/SimdQuaternion.h new file mode 100644 index 00000000000..6a8513ae0d9 --- /dev/null +++ b/extern/bullet/LinearMath/SimdQuaternion.h @@ -0,0 +1,302 @@ +/* + +Copyright (c) 2005 Gino van den Bergen / Erwin Coumans <www.erwincoumans.com> + +Permission is hereby granted, free of charge, to any person or organization +obtaining a copy of the software and accompanying documentation covered by +this license (the "Software") to use, reproduce, display, distribute, +execute, and transmit the Software, and to prepare derivative works of the +Software, and to permit third-parties to whom the Software is furnished to +do so, all subject to the following: + +The copyright notices in the Software and this entire statement, including +the above license grant, this restriction and the following disclaimer, +must be included in all copies of the Software, in whole or in part, and +all derivative works of the Software, unless such copies or derivative +works are solely in the form of machine-executable object code generated by +a source language processor. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT +SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE +FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE, +ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER +DEALINGS IN THE SOFTWARE. +*/ + + +#ifndef SIMD__QUATERNION_H_ +#define SIMD__QUATERNION_H_ + +#include "SimdVector3.h" + +class SimdQuaternion : public SimdQuadWord { +public: + SimdQuaternion() {} + + // template <typename SimdScalar> + // explicit Quaternion(const SimdScalar *v) : Tuple4<SimdScalar>(v) {} + + SimdQuaternion(const SimdScalar& x, const SimdScalar& y, const SimdScalar& z, const SimdScalar& w) + : SimdQuadWord(x, y, z, w) + {} + + SimdQuaternion(const SimdVector3& axis, const SimdScalar& angle) + { + setRotation(axis, angle); + } + + SimdQuaternion(const SimdScalar& yaw, const SimdScalar& pitch, const SimdScalar& roll) + { + setEuler(yaw, pitch, roll); + } + + void setRotation(const SimdVector3& axis, const SimdScalar& angle) + { + SimdScalar d = axis.length(); + assert(d != SimdScalar(0.0)); + SimdScalar s = sinf(angle * SimdScalar(0.5)) / d; + setValue(axis.x() * s, axis.y() * s, axis.z() * s, + cosf(angle * SimdScalar(0.5))); + } + + void setEuler(const SimdScalar& yaw, const SimdScalar& pitch, const SimdScalar& roll) + { + SimdScalar halfYaw = SimdScalar(yaw) * SimdScalar(0.5); + SimdScalar halfPitch = SimdScalar(pitch) * SimdScalar(0.5); + SimdScalar halfRoll = SimdScalar(roll) * SimdScalar(0.5); + SimdScalar cosYaw = cosf(halfYaw); + SimdScalar sinYaw = sinf(halfYaw); + SimdScalar cosPitch = cosf(halfPitch); + SimdScalar sinPitch = sinf(halfPitch); + SimdScalar cosRoll = cosf(halfRoll); + SimdScalar sinRoll = sinf(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); + } + + SimdQuaternion& operator+=(const SimdQuaternion& q) + { + m_x += q.x(); m_y += q.y(); m_z += q.z(); m_unusedW += q[3]; + return *this; + } + + SimdQuaternion& operator-=(const SimdQuaternion& q) + { + m_x -= q.x(); m_y -= q.y(); m_z -= q.z(); m_unusedW -= q[3]; + return *this; + } + + SimdQuaternion& operator*=(const SimdScalar& s) + { + m_x *= s; m_y *= s; m_z *= s; m_unusedW *= s; + return *this; + } + + + SimdQuaternion& operator*=(const SimdQuaternion& q) + { + setValue(m_unusedW * q.x() + m_x * q[3] + m_y * q.z() - m_z * q.y(), + m_unusedW * q.y() + m_y * q[3] + m_z * q.x() - m_x * q.z(), + m_unusedW * q.z() + m_z * q[3] + m_x * q.y() - m_y * q.x(), + m_unusedW * q[3] - m_x * q.x() - m_y * q.y() - m_z * q.z()); + return *this; + } + + SimdScalar dot(const SimdQuaternion& q) const + { + return m_x * q.x() + m_y * q.y() + m_z * q.z() + m_unusedW * q[3]; + } + + SimdScalar length2() const + { + return dot(*this); + } + + SimdScalar length() const + { + return sqrtf(length2()); + } + + SimdQuaternion& normalize() + { + return *this /= length(); + } + + SIMD_FORCE_INLINE SimdQuaternion + operator*(const SimdScalar& s) const + { + return SimdQuaternion(x() * s, y() * s, z() * s, m_unusedW * s); + } + + + + SimdQuaternion operator/(const SimdScalar& s) const + { + assert(s != SimdScalar(0.0)); + return *this * (SimdScalar(1.0) / s); + } + + + SimdQuaternion& operator/=(const SimdScalar& s) + { + assert(s != SimdScalar(0.0)); + return *this *= SimdScalar(1.0) / s; + } + + + SimdQuaternion normalized() const + { + return *this / length(); + } + + SimdScalar angle(const SimdQuaternion& q) const + { + SimdScalar s = sqrtf(length2() * q.length2()); + assert(s != SimdScalar(0.0)); + return acosf(dot(q) / s); + } + + SimdScalar getAngle() const + { + SimdScalar s = 2.f * acosf(m_unusedW); + return s; + } + + + + SimdQuaternion inverse() const + { + return SimdQuaternion(m_x, m_y, m_z, -m_unusedW); + } + + SIMD_FORCE_INLINE SimdQuaternion + operator+(const SimdQuaternion& q2) const + { + const SimdQuaternion& q1 = *this; + return SimdQuaternion(q1.x() + q2.x(), q1.y() + q2.y(), q1.z() + q2.z(), q1[3] + q2[3]); + } + + SIMD_FORCE_INLINE SimdQuaternion + operator-(const SimdQuaternion& q2) const + { + const SimdQuaternion& q1 = *this; + return SimdQuaternion(q1.x() - q2.x(), q1.y() - q2.y(), q1.z() - q2.z(), q1[3] - q2[3]); + } + + SIMD_FORCE_INLINE SimdQuaternion operator-() const + { + const SimdQuaternion& q2 = *this; + return SimdQuaternion( - q2.x(), - q2.y(), - q2.z(), - q2[3]); + } + + SIMD_FORCE_INLINE SimdQuaternion farthest( const SimdQuaternion& qd) const + { + SimdQuaternion diff,sum; + diff = *this - qd; + sum = *this + qd; + if( diff.dot(diff) > sum.dot(sum) ) + return qd; + return (-qd); + } + + SimdQuaternion slerp(const SimdQuaternion& q, const SimdScalar& t) const + { + SimdScalar theta = angle(q); + if (theta != SimdScalar(0.0)) + { + SimdScalar d = SimdScalar(1.0) / sinf(theta); + SimdScalar s0 = sinf((SimdScalar(1.0) - t) * theta); + SimdScalar s1 = sinf(t * theta); + return SimdQuaternion((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[3] * s1) * d); + } + else + { + return *this; + } + } + + + +}; + + + +SIMD_FORCE_INLINE SimdQuaternion +operator-(const SimdQuaternion& q) +{ + return SimdQuaternion(-q.x(), -q.y(), -q.z(), -q[3]); +} + + + + +SIMD_FORCE_INLINE SimdQuaternion +operator*(const SimdQuaternion& q1, const SimdQuaternion& q2) { + return SimdQuaternion(q1[3] * q2.x() + q1.x() * q2[3] + q1.y() * q2.z() - q1.z() * q2.y(), + q1[3] * q2.y() + q1.y() * q2[3] + q1.z() * q2.x() - q1.x() * q2.z(), + q1[3] * q2.z() + q1.z() * q2[3] + q1.x() * q2.y() - q1.y() * q2.x(), + q1[3] * q2[3] - q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z()); +} + +SIMD_FORCE_INLINE SimdQuaternion +operator*(const SimdQuaternion& q, const SimdVector3& w) +{ + return SimdQuaternion( q[3] * w.x() + q.y() * w.z() - q.z() * w.y(), + q[3] * w.y() + q.z() * w.x() - q.x() * w.z(), + q[3] * 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 SimdQuaternion +operator*(const SimdVector3& w, const SimdQuaternion& q) +{ + return SimdQuaternion( w.x() * q[3] + w.y() * q.z() - w.z() * q.y(), + w.y() * q[3] + w.z() * q.x() - w.x() * q.z(), + w.z() * q[3] + w.x() * q.y() - w.y() * q.x(), + -w.x() * q.x() - w.y() * q.y() - w.z() * q.z()); +} + +SIMD_FORCE_INLINE SimdScalar +dot(const SimdQuaternion& q1, const SimdQuaternion& q2) +{ + return q1.dot(q2); +} + + +SIMD_FORCE_INLINE SimdScalar +length(const SimdQuaternion& q) +{ + return q.length(); +} + +SIMD_FORCE_INLINE SimdScalar +angle(const SimdQuaternion& q1, const SimdQuaternion& q2) +{ + return q1.angle(q2); +} + + +SIMD_FORCE_INLINE SimdQuaternion +inverse(const SimdQuaternion& q) +{ + return q.inverse(); +} + +SIMD_FORCE_INLINE SimdQuaternion +slerp(const SimdQuaternion& q1, const SimdQuaternion& q2, const SimdScalar& t) +{ + return q1.slerp(q2, t); +} + + +#endif + + + |