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 409