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