src/apps/haiku3d/Quaternion.h

   1 /*
   2  * Copyright 2008 Haiku Inc. All rights reserved.
   3  * Distributed under the terms of the MIT License.
   4  *
   5  * Authors:
   6  *              Alexandre Deckner
   7  *
   8  */
   9
  10 /*
  11  *
  12  * This is a refactored and stripped down version of
  13  * bullet-2.66 src\LinearMath\btQuaternion.h
  14  * The dependancies on base class btQuadWord have been removed for
  15  * simplification.
  16  * Added gl matrix conversion method.
  17  *
  18  */
  19
  20 /*
  21 Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans
  22         http://continuousphysics.com/Bullet/
  23
  24 This software is provided 'as-is', without any express or implied warranty.
  25 In no event will the authors be held liable for any damages arising from the
  26 use of this software.
  27 Permission is granted to anyone to use this software for any purpose,
  28 including commercial applications, and to alter it and redistribute it freely,
  29 subject to the following restrictions:
  30
  31 1. The origin of this software must not be misrepresented; you must not claim
  32  that you wrote the original software. If you use this software in a product,
  33  an acknowledgment in the product documentation would be appreciated but is
  34  not required.
  35 2. Altered source versions must be plainly marked as such, and must not be
  36  misrepresented as being the original software.
  37 3. This notice may not be removed or altered from any source distribution.
  38 */
  39 #ifndef __QUATERNION_H__
  40 #define __QUATERNION_H__
  41
  42 #include "Vector3.h"
  43 #include <SupportDefs.h>
  44
  45 class Quaternion {
  46 protected:
  47         float   m_x;
  48         float   m_y;
  49         float   m_z;
  50         float   m_w;
  51 public:
  52         Quaternion() {}
  53
  54         Quaternion(const Quaternion& q)
  55         {
  56                 *((Quaternion*)this) = q;
  57         }
  58
  59         Quaternion(const float& x, const float& y, const float& z, const float& w)
  60         {
  61                 m_x = x, m_y = y, m_z = z, m_w = w;
  62         }
  63
  64         Quaternion(const Vector3& axis, const float& angle)
  65         {
  66                 setRotation(axis, angle);
  67         }
  68
  69         Quaternion(const float& yaw, const float& pitch, const float& roll)
  70         {
  71                 setEuler(yaw, pitch, roll);
  72         }
  73
  74         inline const float& x() const { return m_x; }
  75
  76
  77         inline const float& y() const { return m_y; }
  78
  79
  80         inline const float& z() const { return m_z; }
  81
  82
  83         inline const float& w() const { return m_w; }
  84
  85
  86         void setValue(const float& x, const float& y, const float& z)
  87         {
  88                 m_x = x;
  89                 m_y = y;
  90                 m_z = z;
  91                 m_w = 0.f;
  92         }
  93
  94
  95         void setValue(const float& x, const float& y, const float& z, const float& w)
  96         {
  97                 m_x = x;
  98                 m_y = y;
  99                 m_z = z;
 100                 m_w = w;
 101         }
 102
 103
 104         void setRotation(const Vector3& axis, const float& angle)
 105         {
 106                 float d = axis.length();
 107                 assert(d != 0.0f);
 108                 float s = sin(angle * 0.5f) / d;
 109                 setValue(axis.x() * s, axis.y() * s, axis.z() * s,
 110                         cos(angle * 0.5f));
 111         }
 112
 113
 114         void setEuler(const float& yaw, const float& pitch, const float& roll)
 115         {
 116                 float halfYaw = yaw * 0.5f;
 117                 float halfPitch = pitch * 0.5f;
 118                 float halfRoll = roll * 0.5f;
 119                 float cosYaw = cos(halfYaw);
 120                 float sinYaw = sin(halfYaw);
 121                 float cosPitch = cos(halfPitch);
 122                 float sinPitch = sin(halfPitch);
 123                 float cosRoll = cos(halfRoll);
 124                 float sinRoll = sin(halfRoll);
 125                 setValue(cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw,
 126                         cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw,
 127                         sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw,
 128                         cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw);
 129         }
 130
 131
 132         Quaternion& operator+=(const Quaternion& q)
 133         {
 134                 m_x += q.x(); m_y += q.y(); m_z += q.z(); m_w += q.m_w;
 135                 return *this;
 136         }
 137
 138
 139         Quaternion& operator-=(const Quaternion& q)
 140         {
 141                 m_x -= q.x(); m_y -= q.y(); m_z -= q.z(); m_w -= q.m_w;
 142                 return *this;
 143         }
 144
 145
 146         Quaternion& operator*=(const float& s)
 147         {
 148                 m_x *= s; m_y *= s; m_z *= s; m_w *= s;
 149                 return *this;
 150         }
 151
 152
 153         Quaternion& operator*=(const Quaternion& q)
 154         {
 155                 setValue(m_w * q.x() + m_x * q.m_w + m_y * q.z() - m_z * q.y(),
 156                         m_w * q.y() + m_y * q.m_w + m_z * q.x() - m_x * q.z(),
 157                         m_w * q.z() + m_z * q.m_w + m_x * q.y() - m_y * q.x(),
 158                         m_w * q.m_w - m_x * q.x() - m_y * q.y() - m_z * q.z());
 159                 return *this;
 160         }
 161
 162
 163         float dot(const Quaternion& q) const
 164         {
 165                 return m_x * q.x() + m_y * q.y() + m_z * q.z() + m_w * q.m_w;
 166         }
 167
 168
 169         float length2() const
 170         {
 171                 return dot(*this);
 172         }
 173
 174
 175         float length() const
 176         {
 177                 return sqrt(length2());
 178         }
 179
 180
 181         Quaternion& normalize()
 182         {
 183                 return *this /= length();
 184         }
 185
 186
 187         inline Quaternion
 188         operator*(const float& s) const
 189         {
 190                 return Quaternion(x() * s, y() * s, z() * s, m_w * s);
 191         }
 192
 193
 194         Quaternion operator/(const float& s) const
 195         {
 196                 assert(s != 0.0f);
 197                 return *this * (1.0f / s);
 198         }
 199
 200
 201         Quaternion& operator/=(const float& s)
 202         {
 203                 assert(s != 0.0f);
 204                 return *this *= 1.0f / s;
 205         }
 206
 207
 208         Quaternion normalized() const
 209         {
 210                 return *this / length();
 211         }
 212
 213
 214         float angle(const Quaternion& q) const
 215         {
 216                 float s = sqrt(length2() * q.length2());
 217                 assert(s != 0.0f);
 218                 return acos(dot(q) / s);
 219         }
 220
 221
 222         float getAngle() const
 223         {
 224                 float s = 2.0f * acos(m_w);
 225                 return s;
 226         }
 227
 228
 229         Quaternion inverse() const
 230         {
 231                 return Quaternion(m_x, m_y, m_z, -m_w);
 232         }
 233
 234
 235         inline Quaternion
 236         operator+(const Quaternion& q2) const
 237         {
 238                 const Quaternion& q1 = *this;
 239                 return Quaternion(q1.x() + q2.x(), q1.y() + q2.y(), q1.z() + q2.z(),
 240                         q1.m_w + q2.m_w);
 241         }
 242
 243
 244         inline Quaternion
 245         operator-(const Quaternion& q2) const
 246         {
 247                 const Quaternion& q1 = *this;
 248                 return Quaternion(q1.x() - q2.x(), q1.y() - q2.y(), q1.z() - q2.z(),
 249                         q1.m_w - q2.m_w);
 250         }
 251
 252
 253         inline Quaternion operator-() const
 254         {
 255                 const Quaternion& q2 = *this;
 256                 return Quaternion( - q2.x(), - q2.y(),  - q2.z(),  - q2.m_w);
 257         }
 258
 259
 260         inline Quaternion farthest( const Quaternion& qd) const
 261         {
 262                 Quaternion diff, sum;
 263                 diff = *this - qd;
 264                 sum = *this + qd;
 265                 if (diff.dot(diff) > sum.dot(sum))
 266                         return qd;
 267                 return (-qd);
 268         }
 269
 270
 271         Quaternion slerp(const Quaternion& q, const float& t) const
 272         {
 273                 float theta = angle(q);
 274                 if (theta != 0.0f) {
 275                         float d = 1.0f / sin(theta);
 276                         float s0 = sin((1.0f - t) * theta);
 277                         float s1 = sin(t * theta);
 278                         return Quaternion((m_x * s0 + q.x() * s1) * d,
 279                                 (m_y * s0 + q.y() * s1) * d,
 280                                 (m_z * s0 + q.z() * s1) * d,
 281                                 (m_w * s0 + q.m_w * s1) * d);
 282                 } else {
 283                         return *this;
 284                 }
 285         }
 286
 287
 288         void toOpenGLMatrix(float m[4][4])
 289         {
 290                 float wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;
 291
 292                 // calculate coefficients
 293                 x2 = m_x + m_x; y2 = m_y + m_y;
 294                 z2 = m_z + m_z;
 295                 xx = m_x * x2; xy = m_x * y2; xz = m_x * z2;
 296                 yy = m_y * y2; yz = m_y * z2; zz = m_z * z2;
 297                 wx = m_w * x2; wy = m_w * y2; wz = m_w * z2;
 298
 299
 300                 m[0][0] = 1.0 - (yy + zz); m[1][0] = xy - wz;
 301                 m[2][0] = xz + wy; m[3][0] = 0.0;
 302
 303                 m[0][1] = xy + wz; m[1][1] = 1.0 - (xx + zz);
 304                 m[2][1] = yz - wx; m[3][1] = 0.0;
 305
 306
 307                 m[0][2] = xz - wy; m[1][2] = yz + wx;
 308                 m[2][2] = 1.0 - (xx + yy); m[3][2] = 0.0;
 309
 310
 311                 m[0][3] = 0; m[1][3] = 0;
 312                 m[2][3] = 0; m[3][3] = 1;
 313         }
 314 };
 315
 316
 317 inline Quaternion
 318 operator-(const Quaternion& q)
 319 {
 320         return Quaternion(-q.x(), -q.y(), -q.z(), -q.w());
 321 }
 322
 323
 324 inline Quaternion
 325 operator*(const Quaternion& q1, const Quaternion& q2) {
 326         return Quaternion(
 327                 q1.w() * q2.x() + q1.x() * q2.w() + q1.y() * q2.z() - q1.z() * q2.y(),
 328                 q1.w() * q2.y() + q1.y() * q2.w() + q1.z() * q2.x() - q1.x() * q2.z(),
 329                 q1.w() * q2.z() + q1.z() * q2.w() + q1.x() * q2.y() - q1.y() * q2.x(),
 330                 q1.w() * q2.w() - q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z());
 331 }
 332
 333
 334 inline Quaternion
 335 operator*(const Quaternion& q, const Vector3& w)
 336 {
 337         return Quaternion( q.w() * w.x() + q.y() * w.z() - q.z() * w.y(),
 338                 q.w() * w.y() + q.z() * w.x() - q.x() * w.z(),
 339                 q.w() * w.z() + q.x() * w.y() - q.y() * w.x(),
 340                 -q.x() * w.x() - q.y() * w.y() - q.z() * w.z());
 341 }
 342
 343
 344 inline Quaternion
 345 operator*(const Vector3& w, const Quaternion& q)
 346 {
 347         return Quaternion( w.x() * q.w() + w.y() * q.z() - w.z() * q.y(),
 348                 w.y() * q.w() + w.z() * q.x() - w.x() * q.z(),
 349                 w.z() * q.w() + w.x() * q.y() - w.y() * q.x(),
 350                 -w.x() * q.x() - w.y() * q.y() - w.z() * q.z());
 351 }
 352
 353
 354 inline float
 355 dot(const Quaternion& q1, const Quaternion& q2)
 356 {
 357         return q1.dot(q2);
 358 }
 359
 360
 361 inline float
 362 length(const Quaternion& q)
 363 {
 364         return q.length();
 365 }
 366
 367
 368 inline float
 369 angle(const Quaternion& q1, const Quaternion& q2)
 370 {
 371         return q1.angle(q2);
 372 }
 373
 374
 375 inline Quaternion
 376 inverse(const Quaternion& q)
 377 {
 378         return q.inverse();
 379 }
 380
 381
 382 inline Quaternion
 383 slerp(const Quaternion& q1, const Quaternion& q2, const float& t)
 384 {
 385         return q1.slerp(q2, t);
 386 }
 387
 388
 389 // Game Programming Gems 2.10. make sure v0,v1 are normalized
 390 inline Quaternion
 391 shortestArcQuat(const Vector3& v0, const Vector3& v1)
 392 {
 393         Vector3 c = v0.cross(v1);
 394         float  d = v0.dot(v1);
 395
 396         if (d < -1.0 + FLT_EPSILON)
 397                 return Quaternion(0.0f, 1.0f, 0.0f, 0.0f); // just pick any vector
 398
 399         float  s = sqrt((1.0f + d) * 2.0f);
 400         float rs = 1.0f / s;
 401
 402         return Quaternion(c.x() * rs, c.y() * rs, c.z() * rs, s * 0.5f);
 403 }
 404
 405
 406 inline Quaternion
 407 shortestArcQuatNormalize2(Vector3& v0, Vector3& v1)
 408 {
 409         v0.normalize();
 410         v1.normalize();
 411         return shortestArcQuat(v0, v1);
 412 }
 413 #endif
 414
 415
 416