extern/bullet/LinearMath/SimdQuaternion.h

   1 /*
   2
   3 Copyright (c) 2005 Gino van den Bergen / Erwin Coumans <www.erwincoumans.com>
   4
   5 Permission is hereby granted, free of charge, to any person or organization
   6 obtaining a copy of the software and accompanying documentation covered by
   7 this license (the "Software") to use, reproduce, display, distribute,
   8 execute, and transmit the Software, and to prepare derivative works of the
   9 Software, and to permit third-parties to whom the Software is furnished to
  10 do so, all subject to the following:
  11
  12 The copyright notices in the Software and this entire statement, including
  13 the above license grant, this restriction and the following disclaimer,
  14 must be included in all copies of the Software, in whole or in part, and
  15 all derivative works of the Software, unless such copies or derivative
  16 works are solely in the form of machine-executable object code generated by
  17 a source language processor.
  18
  19 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  20 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  21 FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT
  22 SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
  23 FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
  24 ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
  25 DEALINGS IN THE SOFTWARE.
  26 */
  27
  28
  29 #ifndef SIMD__QUATERNION_H_
  30 #define SIMD__QUATERNION_H_
  31
  32 #include "SimdVector3.h"
  33
  34 class SimdQuaternion : public SimdQuadWord {
  35 public:
  36         SimdQuaternion() {}
  37
  38         //              template <typename SimdScalar>
  39         //              explicit Quaternion(const SimdScalar *v) : Tuple4<SimdScalar>(v) {}
  40
  41         SimdQuaternion(const SimdScalar& x, const SimdScalar& y, const SimdScalar& z, const SimdScalar& w)
  42                 : SimdQuadWord(x, y, z, w)
  43         {}
  44
  45         SimdQuaternion(const SimdVector3& axis, const SimdScalar& angle)
  46         {
  47                 setRotation(axis, angle);
  48         }
  49
  50         SimdQuaternion(const SimdScalar& yaw, const SimdScalar& pitch, const SimdScalar& roll)
  51         {
  52                 setEuler(yaw, pitch, roll);
  53         }
  54
  55         void setRotation(const SimdVector3& axis, const SimdScalar& angle)
  56         {
  57                 SimdScalar d = axis.length();
  58                 assert(d != SimdScalar(0.0));
  59                 SimdScalar s = sinf(angle * SimdScalar(0.5)) / d;
  60                 setValue(axis.x() * s, axis.y() * s, axis.z() * s,
  61                         cosf(angle * SimdScalar(0.5)));
  62         }
  63
  64         void setEuler(const SimdScalar& yaw, const SimdScalar& pitch, const SimdScalar& roll)
  65         {
  66                 SimdScalar halfYaw = SimdScalar(yaw) * SimdScalar(0.5);
  67                 SimdScalar halfPitch = SimdScalar(pitch) * SimdScalar(0.5);
  68                 SimdScalar halfRoll = SimdScalar(roll) * SimdScalar(0.5);
  69                 SimdScalar cosYaw = cosf(halfYaw);
  70                 SimdScalar sinYaw = sinf(halfYaw);
  71                 SimdScalar cosPitch = cosf(halfPitch);
  72                 SimdScalar sinPitch = sinf(halfPitch);
  73                 SimdScalar cosRoll = cosf(halfRoll);
  74                 SimdScalar sinRoll = sinf(halfRoll);
  75                 setValue(cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw,
  76                         cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw,
  77                         sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw,
  78                         cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw);
  79         }
  80
  81         SimdQuaternion& operator+=(const SimdQuaternion& q)
  82         {
  83                 m_x += q.x(); m_y += q.y(); m_z += q.z(); m_unusedW += q[3];
  84                 return *this;
  85         }
  86
  87         SimdQuaternion& operator-=(const SimdQuaternion& q)
  88         {
  89                 m_x -= q.x(); m_y -= q.y(); m_z -= q.z(); m_unusedW -= q[3];
  90                 return *this;
  91         }
  92
  93         SimdQuaternion& operator*=(const SimdScalar& s)
  94         {
  95                 m_x *= s; m_y *= s; m_z *= s; m_unusedW *= s;
  96                 return *this;
  97         }
  98
  99
 100         SimdQuaternion& operator*=(const SimdQuaternion& q)
 101         {
 102                 setValue(m_unusedW * q.x() + m_x * q[3] + m_y * q.z() - m_z * q.y(),
 103                         m_unusedW * q.y() + m_y * q[3] + m_z * q.x() - m_x * q.z(),
 104                         m_unusedW * q.z() + m_z * q[3] + m_x * q.y() - m_y * q.x(),
 105                         m_unusedW * q[3] - m_x * q.x() - m_y * q.y() - m_z * q.z());
 106                 return *this;
 107         }
 108
 109         SimdScalar dot(const SimdQuaternion& q) const
 110         {
 111                 return m_x * q.x() + m_y * q.y() + m_z * q.z() + m_unusedW * q[3];
 112         }
 113
 114         SimdScalar length2() const
 115         {
 116                 return dot(*this);
 117         }
 118
 119         SimdScalar length() const
 120         {
 121                 return sqrtf(length2());
 122         }
 123
 124         SimdQuaternion& normalize()
 125         {
 126                 return *this /= length();
 127         }
 128
 129         SIMD_FORCE_INLINE SimdQuaternion
 130         operator*(const SimdScalar& s) const
 131         {
 132                 return SimdQuaternion(x() * s, y() * s, z() * s, m_unusedW * s);
 133         }
 134
 135
 136
 137         SimdQuaternion operator/(const SimdScalar& s) const
 138         {
 139                 assert(s != SimdScalar(0.0));
 140                 return *this * (SimdScalar(1.0) / s);
 141         }
 142
 143
 144         SimdQuaternion& operator/=(const SimdScalar& s)
 145         {
 146                 assert(s != SimdScalar(0.0));
 147                 return *this *= SimdScalar(1.0) / s;
 148         }
 149
 150
 151         SimdQuaternion normalized() const
 152         {
 153                 return *this / length();
 154         }
 155
 156         SimdScalar angle(const SimdQuaternion& q) const
 157         {
 158                 SimdScalar s = sqrtf(length2() * q.length2());
 159                 assert(s != SimdScalar(0.0));
 160                 return acosf(dot(q) / s);
 161         }
 162
 163         SimdScalar getAngle() const
 164         {
 165                 SimdScalar s = 2.f * acosf(m_unusedW);
 166                 return s;
 167         }
 168
 169
 170
 171         SimdQuaternion inverse() const
 172         {
 173                 return SimdQuaternion(m_x, m_y, m_z, -m_unusedW);
 174         }
 175
 176         SIMD_FORCE_INLINE SimdQuaternion
 177         operator+(const SimdQuaternion& q2) const
 178         {
 179                 const SimdQuaternion& q1 = *this;
 180                 return SimdQuaternion(q1.x() + q2.x(), q1.y() + q2.y(), q1.z() + q2.z(), q1[3] + q2[3]);
 181         }
 182
 183         SIMD_FORCE_INLINE SimdQuaternion
 184         operator-(const SimdQuaternion& q2) const
 185         {
 186                 const SimdQuaternion& q1 = *this;
 187                 return SimdQuaternion(q1.x() - q2.x(), q1.y() - q2.y(), q1.z() - q2.z(), q1[3] - q2[3]);
 188         }
 189
 190         SIMD_FORCE_INLINE SimdQuaternion operator-() const
 191         {
 192                 const SimdQuaternion& q2 = *this;
 193                 return SimdQuaternion( - q2.x(), - q2.y(),  - q2.z(),  - q2[3]);
 194         }
 195
 196         SIMD_FORCE_INLINE SimdQuaternion farthest( const SimdQuaternion& qd) const
 197         {
 198                 SimdQuaternion diff,sum;
 199                 diff = *this - qd;
 200                 sum = *this + qd;
 201                 if( diff.dot(diff) > sum.dot(sum) )
 202                         return qd;
 203                 return (-qd);
 204         }
 205
 206         SimdQuaternion slerp(const SimdQuaternion& q, const SimdScalar& t) const
 207         {
 208                 SimdScalar theta = angle(q);
 209                 if (theta != SimdScalar(0.0))
 210                 {
 211                         SimdScalar d = SimdScalar(1.0) / sinf(theta);
 212                         SimdScalar s0 = sinf((SimdScalar(1.0) - t) * theta);
 213                         SimdScalar s1 = sinf(t * theta);
 214                         return SimdQuaternion((m_x * s0 + q.x() * s1) * d,
 215                                 (m_y * s0 + q.y() * s1) * d,
 216                                 (m_z * s0 + q.z() * s1) * d,
 217                                 (m_unusedW * s0 + q[3] * s1) * d);
 218                 }
 219                 else
 220                 {
 221                         return *this;
 222                 }
 223         }
 224
 225
 226
 227 };
 228
 229
 230
 231 SIMD_FORCE_INLINE SimdQuaternion
 232 operator-(const SimdQuaternion& q)
 233 {
 234         return SimdQuaternion(-q.x(), -q.y(), -q.z(), -q[3]);
 235 }
 236
 237
 238
 239
 240 SIMD_FORCE_INLINE SimdQuaternion
 241 operator*(const SimdQuaternion& q1, const SimdQuaternion& q2) {
 242         return SimdQuaternion(q1[3] * q2.x() + q1.x() * q2[3] + q1.y() * q2.z() - q1.z() * q2.y(),
 243                 q1[3] * q2.y() + q1.y() * q2[3] + q1.z() * q2.x() - q1.x() * q2.z(),
 244                 q1[3] * q2.z() + q1.z() * q2[3] + q1.x() * q2.y() - q1.y() * q2.x(),
 245                 q1[3] * q2[3] - q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z());
 246 }
 247
 248 SIMD_FORCE_INLINE SimdQuaternion
 249 operator*(const SimdQuaternion& q, const SimdVector3& w)
 250 {
 251         return SimdQuaternion( q[3] * w.x() + q.y() * w.z() - q.z() * w.y(),
 252                 q[3] * w.y() + q.z() * w.x() - q.x() * w.z(),
 253                 q[3] * w.z() + q.x() * w.y() - q.y() * w.x(),
 254                 -q.x() * w.x() - q.y() * w.y() - q.z() * w.z());
 255 }
 256
 257 SIMD_FORCE_INLINE SimdQuaternion
 258 operator*(const SimdVector3& w, const SimdQuaternion& q)
 259 {
 260         return SimdQuaternion( w.x() * q[3] + w.y() * q.z() - w.z() * q.y(),
 261                 w.y() * q[3] + w.z() * q.x() - w.x() * q.z(),
 262                 w.z() * q[3] + w.x() * q.y() - w.y() * q.x(),
 263                 -w.x() * q.x() - w.y() * q.y() - w.z() * q.z());
 264 }
 265
 266 SIMD_FORCE_INLINE SimdScalar
 267 dot(const SimdQuaternion& q1, const SimdQuaternion& q2)
 268 {
 269         return q1.dot(q2);
 270 }
 271
 272
 273 SIMD_FORCE_INLINE SimdScalar
 274 length(const SimdQuaternion& q)
 275 {
 276         return q.length();
 277 }
 278
 279 SIMD_FORCE_INLINE SimdScalar
 280 angle(const SimdQuaternion& q1, const SimdQuaternion& q2)
 281 {
 282         return q1.angle(q2);
 283 }
 284
 285
 286 SIMD_FORCE_INLINE SimdQuaternion
 287 inverse(const SimdQuaternion& q)
 288 {
 289         return q.inverse();
 290 }
 291
 292 SIMD_FORCE_INLINE SimdQuaternion
 293 slerp(const SimdQuaternion& q1, const SimdQuaternion& q2, const SimdScalar& t)
 294 {
 295         return q1.slerp(q2, t);
 296 }
 297
 298
 299 #endif
 300
 301
 302