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 417