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 #ifndef __QUATERNION_H__ 32 #define __QUATERNION_H__ 33 34 #include "Vector3.h" 35 #include <SupportDefs.h> //pout FLT_EPSILON 36 37 class Quaternion { 38 protected: 39 float m_x; 40 float m_y; 41 float m_z; 42 float m_w; 43 public: 44 Quaternion() {} 45 46 Quaternion(const Quaternion& q) 47 { 48 *((Quaternion*)this) = q; 49 } 50 51 Quaternion(const float& x, const float& y, const float& z,const float& w) 52 { 53 m_x = x, m_y = y, m_z = z, m_w = w; 54 } 55 56 Quaternion(const Vector3& axis, const float& angle) 57 { 58 setRotation(axis, angle); 59 } 60 61 Quaternion(const float& yaw, const float& pitch, const float& roll) 62 { 63 setEuler(yaw, pitch, roll); 64 } 65 66 inline const float& x() const { return m_x; } 67 68 69 inline const float& y() const { return m_y; } 70 71 72 inline const float& z() const { return m_z; } 73 74 75 inline const float& w() const { return m_w; } 76 77 78 void setValue(const float& x, const float& y, const float& z) 79 { 80 m_x=x; 81 m_y=y; 82 m_z=z; 83 m_w = 0.f; 84 } 85 86 87 void setValue(const float& x, const float& y, const float& z,const float& w) 88 { 89 m_x=x; 90 m_y=y; 91 m_z=z; 92 m_w=w; 93 } 94 95 96 void setRotation(const Vector3& axis, const float& angle) 97 { 98 float d = axis.length(); 99 assert(d != 0.0f); 100 float s = sin(angle * 0.5f) / d; 101 setValue(axis.x() * s, axis.y() * s, axis.z() * s, 102 cos(angle * 0.5f)); 103 } 104 105 106 void setEuler(const float& yaw, const float& pitch, const float& roll) 107 { 108 float halfYaw = yaw * 0.5f; 109 float halfPitch = pitch * 0.5f; 110 float halfRoll = roll * 0.5f; 111 float cosYaw = cos(halfYaw); 112 float sinYaw = sin(halfYaw); 113 float cosPitch = cos(halfPitch); 114 float sinPitch = sin(halfPitch); 115 float cosRoll = cos(halfRoll); 116 float sinRoll = sin(halfRoll); 117 setValue(cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw, 118 cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, 119 sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw, 120 cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); 121 } 122 123 124 Quaternion& operator+=(const Quaternion& q) 125 { 126 m_x += q.x(); m_y += q.y(); m_z += q.z(); m_w += q.m_w; 127 return *this; 128 } 129 130 131 Quaternion& operator-=(const Quaternion& q) 132 { 133 m_x -= q.x(); m_y -= q.y(); m_z -= q.z(); m_w -= q.m_w; 134 return *this; 135 } 136 137 138 Quaternion& operator*=(const float& s) 139 { 140 m_x *= s; m_y *= s; m_z *= s; m_w *= s; 141 return *this; 142 } 143 144 145 Quaternion& operator*=(const Quaternion& q) 146 { 147 setValue(m_w * q.x() + m_x * q.m_w + m_y * q.z() - m_z * q.y(), 148 m_w * q.y() + m_y * q.m_w + m_z * q.x() - m_x * q.z(), 149 m_w * q.z() + m_z * q.m_w + m_x * q.y() - m_y * q.x(), 150 m_w * q.m_w - m_x * q.x() - m_y * q.y() - m_z * q.z()); 151 return *this; 152 } 153 154 155 float dot(const Quaternion& q) const 156 { 157 return m_x * q.x() + m_y * q.y() + m_z * q.z() + m_w * q.m_w; 158 } 159 160 161 float length2() const 162 { 163 return dot(*this); 164 } 165 166 167 float length() const 168 { 169 return sqrt(length2()); 170 } 171 172 173 Quaternion& normalize() 174 { 175 return *this /= length(); 176 } 177 178 179 inline Quaternion 180 operator*(const float& s) const 181 { 182 return Quaternion(x() * s, y() * s, z() * s, m_w * s); 183 } 184 185 186 Quaternion operator/(const float& s) const 187 { 188 assert(s != 0.0f); 189 return *this * (1.0f / s); 190 } 191 192 193 Quaternion& operator/=(const float& s) 194 { 195 assert(s != 0.0f); 196 return *this *= 1.0f / s; 197 } 198 199 200 Quaternion normalized() const 201 { 202 return *this / length(); 203 } 204 205 206 float angle(const Quaternion& q) const 207 { 208 float s = sqrt(length2() * q.length2()); 209 assert(s != 0.0f); 210 return acos(dot(q) / s); 211 } 212 213 214 float getAngle() const 215 { 216 float s = 2.0f * acos(m_w); 217 return s; 218 } 219 220 221 Quaternion inverse() const 222 { 223 return Quaternion(m_x, m_y, m_z, -m_w); 224 } 225 226 227 inline Quaternion 228 operator+(const Quaternion& q2) const 229 { 230 const Quaternion& q1 = *this; 231 return Quaternion(q1.x() + q2.x(), q1.y() + q2.y(), q1.z() + q2.z(), q1.m_w + q2.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(), q1.m_w - q2.m_w); 240 } 241 242 243 inline Quaternion operator-() const 244 { 245 const Quaternion& q2 = *this; 246 return Quaternion( - q2.x(), - q2.y(), - q2.z(), - q2.m_w); 247 } 248 249 250 inline Quaternion farthest( const Quaternion& qd) const 251 { 252 Quaternion diff,sum; 253 diff = *this - qd; 254 sum = *this + qd; 255 if( diff.dot(diff) > sum.dot(sum) ) 256 return qd; 257 return (-qd); 258 } 259 260 261 Quaternion slerp(const Quaternion& q, const float& t) const 262 { 263 float theta = angle(q); 264 if (theta != 0.0f) 265 { 266 float d = 1.0f / sin(theta); 267 float s0 = sin((1.0f - t) * theta); 268 float s1 = sin(t * theta); 269 return Quaternion((m_x * s0 + q.x() * s1) * d, 270 (m_y * s0 + q.y() * s1) * d, 271 (m_z * s0 + q.z() * s1) * d, 272 (m_w * s0 + q.m_w * s1) * d); 273 } 274 else 275 { 276 return *this; 277 } 278 } 279 280 281 void toOpenGLMatrix(float m[4][4]){ 282 283 float wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2; 284 285 // calculate coefficients 286 x2 = m_x + m_x; y2 = m_y + m_y; 287 z2 = m_z + m_z; 288 xx = m_x * x2; xy = m_x * y2; xz = m_x * z2; 289 yy = m_y * y2; yz = m_y * z2; zz = m_z * z2; 290 wx = m_w * x2; wy = m_w * y2; wz = m_w * z2; 291 292 293 m[0][0] = 1.0 - (yy + zz); m[1][0] = xy - wz; 294 m[2][0] = xz + wy; m[3][0] = 0.0; 295 296 m[0][1] = xy + wz; m[1][1] = 1.0 - (xx + zz); 297 m[2][1] = yz - wx; m[3][1] = 0.0; 298 299 300 m[0][2] = xz - wy; m[1][2] = yz + wx; 301 m[2][2] = 1.0 - (xx + yy); m[3][2] = 0.0; 302 303 304 m[0][3] = 0; m[1][3] = 0; 305 m[2][3] = 0; m[3][3] = 1; 306 } 307 }; 308 309 310 inline Quaternion 311 operator-(const Quaternion& q) 312 { 313 return Quaternion(-q.x(), -q.y(), -q.z(), -q.w()); 314 } 315 316 317 inline Quaternion 318 operator*(const Quaternion& q1, const Quaternion& q2) { 319 return Quaternion(q1.w() * q2.x() + q1.x() * q2.w() + q1.y() * q2.z() - q1.z() * q2.y(), 320 q1.w() * q2.y() + q1.y() * q2.w() + q1.z() * q2.x() - q1.x() * q2.z(), 321 q1.w() * q2.z() + q1.z() * q2.w() + q1.x() * q2.y() - q1.y() * q2.x(), 322 q1.w() * q2.w() - q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z()); 323 } 324 325 326 inline Quaternion 327 operator*(const Quaternion& q, const Vector3& w) 328 { 329 return Quaternion( q.w() * w.x() + q.y() * w.z() - q.z() * w.y(), 330 q.w() * w.y() + q.z() * w.x() - q.x() * w.z(), 331 q.w() * w.z() + q.x() * w.y() - q.y() * w.x(), 332 -q.x() * w.x() - q.y() * w.y() - q.z() * w.z()); 333 } 334 335 336 inline Quaternion 337 operator*(const Vector3& w, const Quaternion& q) 338 { 339 return Quaternion( w.x() * q.w() + w.y() * q.z() - w.z() * q.y(), 340 w.y() * q.w() + w.z() * q.x() - w.x() * q.z(), 341 w.z() * q.w() + w.x() * q.y() - w.y() * q.x(), 342 -w.x() * q.x() - w.y() * q.y() - w.z() * q.z()); 343 } 344 345 346 inline float 347 dot(const Quaternion& q1, const Quaternion& q2) 348 { 349 return q1.dot(q2); 350 } 351 352 353 inline float 354 length(const Quaternion& q) 355 { 356 return q.length(); 357 } 358 359 360 inline float 361 angle(const Quaternion& q1, const Quaternion& q2) 362 { 363 return q1.angle(q2); 364 } 365 366 367 inline Quaternion 368 inverse(const Quaternion& q) 369 { 370 return q.inverse(); 371 } 372 373 374 inline Quaternion 375 slerp(const Quaternion& q1, const Quaternion& q2, const float& t) 376 { 377 return q1.slerp(q2, t); 378 } 379 380 381 inline Quaternion 382 shortestArcQuat(const Vector3& v0, const Vector3& v1) // Game Programming Gems 2.10. make sure v0,v1 are normalized 383 { 384 Vector3 c = v0.cross(v1); 385 float d = v0.dot(v1); 386 387 if (d < -1.0 + FLT_EPSILON) 388 return Quaternion(0.0f, 1.0f, 0.0f, 0.0f); // just pick any vector 389 390 float s = sqrt((1.0f + d) * 2.0f); 391 float rs = 1.0f / s; 392 393 return Quaternion(c.x() * rs, c.y() * rs, c.z() * rs, s * 0.5f); 394 } 395 396 397 inline Quaternion 398 shortestArcQuatNormalize2(Vector3& v0, Vector3& v1) 399 { 400 v0.normalize(); 401 v1.normalize(); 402 return shortestArcQuat(v0, v1); 403 } 404 #endif 405 406 407 408