/xbmc/screensavers/rsxs-0.9/src/vector.hh
C++ Header | 797 lines | 668 code | 101 blank | 28 comment | 19 complexity | da00a397ca2381f701e962ae1b9fcf17 MD5 | raw file
1/* 2 * Really Slick XScreenSavers 3 * Copyright (C) 2002-2006 Michael Chapman 4 * 5 * This program is free software; you can redistribute it and/or modify 6 * it under the terms of the GNU General Public License version 2 as 7 * published by the Free Software Foundation. 8 * 9 * This program is distributed in the hope that it will be useful, 10 * but WITHOUT ANY WARRANTY; without even the implied warranty of 11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 * GNU General Public License for more details. 13 * 14 * You should have received a copy of the GNU General Public License 15 * along with this program; if not, write to the Free Software 16 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 17 * 18 ***************************************************************************** 19 * 20 * This is a Linux port of the Really Slick Screensavers, 21 * Copyright (C) 2002 Terence M. Welsh, available from www.reallyslick.com 22 */ 23#ifndef _VECTOR_HH 24#define _VECTOR_HH 25 26#include <common.hh> 27 28#define R2D (180.0f / M_PI) 29#define D2R (M_PI / 180.0f) 30 31template <typename T> 32class VectorBase { 33protected: 34 T _v[3]; 35public: 36 VectorBase(const T* v) { 37 set(v[0], v[1], v[2]); 38 } 39 40 VectorBase(const T& x, const T& y, const T& z) { 41 set(x, y, z); 42 } 43 44 void set(const T& x, const T& y, const T& z) { 45 _v[0] = x; 46 _v[1] = y; 47 _v[2] = z; 48 } 49 50 const T& x() const { return _v[0]; } 51 T& x() { return _v[0]; } 52 const T& y() const { return _v[1]; } 53 T& y() { return _v[1]; } 54 const T& z() const { return _v[2]; } 55 T& z() { return _v[2]; } 56 57 const T* get() const { 58 return _v; 59 } 60 61 T lengthSquared() const { 62 return _v[0] * _v[0] + _v[1] * _v[1] + _v[2] * _v[2]; 63 } 64}; 65 66class Vector : public VectorBase<float> { 67public: 68 Vector(const float* v) : VectorBase<float>(v) {} 69 70 Vector(float x = 0.0f, float y = 0.0f, float z = 0.0f) 71 : VectorBase<float>(x, y, z) {} 72 73 float length() const { 74 return std::sqrt(lengthSquared()); 75 } 76 77 float normalize() { 78 float l = length(); 79 if (l == 0.0f) 80 return 0.0f; 81 _v[0] /= l; 82 _v[1] /= l; 83 _v[2] /= l; 84 return l; 85 } 86 87 Vector operator+(const Vector& v) const { 88 return Vector(_v[0] + v._v[0], _v[1] + v._v[1], _v[2] + v._v[2]); 89 } 90 91 Vector operator-(const Vector& v) const { 92 return Vector(_v[0] - v._v[0], _v[1] - v._v[1], _v[2] - v._v[2]); 93 } 94 95 Vector operator*(float f) const { 96 return Vector(_v[0] * f, _v[1] * f, _v[2] * f); 97 } 98 99 Vector operator/(float f) const { 100 return Vector(_v[0] / f, _v[1] / f, _v[2] / f); 101 } 102 103 Vector& operator+=(const Vector& v) { 104 _v[0] += v._v[0]; 105 _v[1] += v._v[1]; 106 _v[2] += v._v[2]; 107 return *this; 108 } 109 110 Vector& operator-=(const Vector& v) { 111 _v[0] -= v._v[0]; 112 _v[1] -= v._v[1]; 113 _v[2] -= v._v[2]; 114 return *this; 115 } 116 117 Vector& operator*=(float f) { 118 _v[0] *= f; 119 _v[1] *= f; 120 _v[2] *= f; 121 return *this; 122 } 123 124 Vector& operator/=(float f) { 125 _v[0] /= f; 126 _v[1] /= f; 127 _v[2] /= f; 128 return *this; 129 } 130 131 static float dot(const Vector& a, const Vector& b) { 132 return a.x() * b.x() + a.y() * b.y() + a.z() * b.z(); 133 } 134 135 static Vector cross(const Vector& a, const Vector& b) { 136 return Vector( 137 a.y() * b.z() - b.y() * a.z(), 138 a.z() * b.x() - b.z() * a.x(), 139 a.x() * b.y() - b.x() * a.y() 140 ); 141 }; 142}; 143 144class UVector : public VectorBase<unsigned int> { 145public: 146 UVector(const unsigned int* v) : VectorBase<unsigned int>(v) {} 147 148 UVector(unsigned int x = 0, unsigned int y = 0, unsigned int z = 0) 149 : VectorBase<unsigned int>(x, y, z) {} 150 151 UVector operator+(const UVector& v) const { 152 return UVector(_v[0] + v._v[0], _v[1] + v._v[1], _v[2] + v._v[2]); 153 } 154 155 UVector operator-(const UVector& v) const { 156 return UVector(_v[0] - v._v[0], _v[1] - v._v[1], _v[2] - v._v[2]); 157 } 158 159 UVector& operator+=(const UVector& v) { 160 _v[0] += v._v[0]; 161 _v[1] += v._v[1]; 162 _v[2] += v._v[2]; 163 return *this; 164 } 165 166 UVector& operator-=(const UVector& v) { 167 _v[0] -= v._v[0]; 168 _v[1] -= v._v[1]; 169 _v[2] -= v._v[2]; 170 return *this; 171 } 172}; 173 174class UnitVector : public Vector { 175protected: 176 using Vector::set; 177 using Vector::length; 178 using Vector::lengthSquared; 179 using Vector::normalize; 180 181 UnitVector(const float* v) { 182 set(v[0], v[1], v[2]); 183 } 184 185 UnitVector(float x, float y, float z) { 186 set(x, y, z); 187 } 188 189 friend class Vector; 190 friend class RotationMatrix; 191 friend class UnitQuat; 192public: 193 UnitVector(const Vector& v) { 194 float l = v.length(); 195 if (l == 0.0f) 196 set(1.0f, 0.0f, 0.0f); 197 else 198 set(v.x() / l, v.y() / l, v.z() / l); 199 } 200 201 UnitVector() { 202 set(1.0f, 0.0f, 0.0f); 203 } 204 205 static UnitVector of(const Vector& v) { 206 float l = v.length(); 207 if (l == 0.0f) 208 return UnitVector(); 209 return UnitVector( 210 v.x() / l, 211 v.y() / l, 212 v.z() / l 213 ); 214 } 215}; 216 217class Matrix { 218protected: 219 float _m[16]; 220public: 221 Matrix( 222 float m0 = 1.0f, float m1 = 0.0f, float m2 = 0.0f, float m3 = 0.0f, 223 float m4 = 0.0f, float m5 = 1.0f, float m6 = 0.0f, float m7 = 0.0f, 224 float m8 = 0.0f, float m9 = 0.0f, float m10 = 1.0f, float m11 = 0.0f, 225 float m12 = 0.0f, float m13 = 0.0f, float m14 = 0.0f, float m15 = 1.0f 226 ) { 227 _m[0] = m0; _m[1] = m1; _m[2] = m2; _m[3] = m3; 228 _m[4] = m4; _m[5] = m5; _m[6] = m6; _m[7] = m7; 229 _m[8] = m8; _m[9] = m9; _m[10] = m10; _m[11] = m11; 230 _m[12] = m12; _m[13] = m13; _m[14] = m14; _m[15] = m15; 231 } 232 233 float get(int i) const { 234 return _m[i]; 235 } 236 237 const float* get() const { 238 return _m; 239 } 240 241 Matrix transposed() const { 242 return Matrix( 243 _m[0], _m[4], _m[8], _m[12], 244 _m[1], _m[5], _m[9], _m[13], 245 _m[2], _m[6], _m[10], _m[14], 246 _m[3], _m[7], _m[11], _m[15] 247 ); 248 } 249 250 Vector transform(const Vector& v) const { 251 float w = _m[3] * v.x() + _m[7] * v.y() + _m[11] * v.z() + _m[15]; 252 return Vector( 253 (_m[0] * v.x() + _m[4] * v.y() + _m[8] * v.z() + _m[12]) / w, 254 (_m[1] * v.x() + _m[5] * v.y() + _m[9] * v.z() + _m[13]) / w, 255 (_m[2] * v.x() + _m[6] * v.y() + _m[10] * v.z() + _m[14]) / w 256 ); 257 } 258 259 Matrix operator*(const Matrix& m) const { 260 return Matrix( 261 _m[0] * m._m[0] + _m[1] * m._m[4] + 262 _m[2] * m._m[8] + _m[3] * m._m[12], 263 _m[0] * m._m[1] + _m[1] * m._m[5] + 264 _m[2] * m._m[9] + _m[3] * m._m[13], 265 _m[0] * m._m[2] + _m[1] * m._m[6] + 266 _m[2] * m._m[10] + _m[3] * m._m[14], 267 _m[0] * m._m[3] + _m[1] * m._m[7] + 268 _m[2] * m._m[11] + _m[3] * m._m[15] 269 , 270 _m[4] * m._m[0] + _m[5] * m._m[4] + 271 _m[6] * m._m[8] + _m[7] * m._m[12], 272 _m[4] * m._m[1] + _m[5] * m._m[5] + 273 _m[6] * m._m[9] + _m[7] * m._m[13], 274 _m[4] * m._m[2] + _m[5] * m._m[6] + 275 _m[6] * m._m[10] + _m[7] * m._m[14], 276 _m[4] * m._m[3] + _m[5] * m._m[7] + 277 _m[6] * m._m[11] + _m[7] * m._m[15] 278 , 279 _m[8] * m._m[0] + _m[9] * m._m[4] + 280 _m[10] * m._m[8] + _m[11] * m._m[12], 281 _m[8] * m._m[1] + _m[9] * m._m[5] + 282 _m[10] * m._m[9] + _m[11] * m._m[13], 283 _m[8] * m._m[2] + _m[9] * m._m[6] + 284 _m[10] * m._m[10] + _m[11] * m._m[14], 285 _m[8] * m._m[3] + _m[9] * m._m[7] + 286 _m[10] * m._m[11] + _m[11] * m._m[15] 287 , 288 _m[12] * m._m[0] + _m[13] * m._m[4] + 289 _m[14] * m._m[8] + _m[15] * m._m[12], 290 _m[12] * m._m[1] + _m[13] * m._m[5] + 291 _m[14] * m._m[9] + _m[15] * m._m[13], 292 _m[12] * m._m[2] + _m[13] * m._m[6] + 293 _m[14] * m._m[10] + _m[15] * m._m[14], 294 _m[12] * m._m[3] + _m[13] * m._m[7] + 295 _m[14] * m._m[11] + _m[15] * m._m[15] 296 ); 297 } 298}; 299 300class TranslationMatrix : public Matrix { 301protected: 302 float _dx, _dy, _dz; 303public: 304 TranslationMatrix(float dx = 0.0f, float dy = 0.0f, float dz = 0.0f) : 305 Matrix( 306 1.0f, 0.0f, 0.0f, 0.0f, 307 0.0f, 1.0f, 0.0f, 0.0f, 308 0.0f, 0.0f, 1.0f, 0.0f, 309 dz, dy, dz, 1.0f 310 ), _dx(dx), _dy(dy), _dz(dz) {} 311 312 TranslationMatrix inverted() const { 313 return TranslationMatrix(-_dx, -_dy, -_dz); 314 } 315 316 Vector transform(const Vector& v) const { 317 return v + Vector(_dx, _dy, _dz); 318 } 319 320 TranslationMatrix operator*(const TranslationMatrix& m) const { 321 return TranslationMatrix(_dx + m._dx, _dy + m._dy, _dz + m._dz); 322 } 323}; 324 325class ScalingMatrix : public Matrix { 326protected: 327 float _sx, _sy, _sz; 328public: 329 ScalingMatrix(float sx = 0.0f, float sy = 0.0f, float sz = 0.0f) : 330 Matrix( 331 sx, 0.0f, 0.0f, 0.0f, 332 0.0f, sy, 0.0f, 0.0f, 333 0.0f, 0.0f, sz, 0.0f, 334 0.0f, 0.0f, 0.0f, 1.0f 335 ), _sx(sx), _sy(sy), _sz(sz) {} 336 337 ScalingMatrix inverted() const { 338 return ScalingMatrix(-_sx, -_sy, -_sz); 339 } 340 341 ScalingMatrix transposed() const { 342 return ScalingMatrix(*this); 343 } 344 345 Vector transform(const Vector& v) const { 346 return Vector(_sx * v.x(), _sy * v.y(), _sz * v.z()); 347 } 348 349 ScalingMatrix operator*(const ScalingMatrix& m) const { 350 return ScalingMatrix(_sx + m._sx, _sy + m._sy, _sz + m._sz); 351 } 352}; 353 354class ShearingMatrix : public Matrix { 355protected: 356 float _sxy, _sxz, _syx, _syz, _szx, _szy; 357public: 358 ShearingMatrix( 359 float sxy = 0.0f, float sxz = 0.0f, 360 float syx = 0.0f, float syz = 0.0f, 361 float szx = 0.0f, float szy = 0.0f 362 ) : 363 Matrix( 364 1.0f, sxy, sxz, 0.0f, 365 syx, 1.0f, syz, 0.0f, 366 szx, szy, 1.0f, 0.0f, 367 0.0f, 0.0f, 0.0f, 1.0f 368 ), 369 _sxy(sxy), _sxz(sxz), _syx(syx), 370 _syz(syz), _szx(szx), _szy(szy) {} 371 372 ShearingMatrix transposed() const { 373 return ShearingMatrix(_syx, _szx, _sxy, _szy, _sxz, _syz); 374 } 375 376 Vector transform(const Vector& v) const { 377 return Vector( 378 v.x() + _syx * v.y() + _szx * v.z(), 379 v.y() + _sxy * v.x() + _szy * v.z(), 380 v.z() + _sxz * v.x() + _syz * v.y() 381 ); 382 } 383}; 384 385class RotationMatrix : public Matrix { 386protected: 387 RotationMatrix( 388 float m0, float m1, float m2, 389 float m4, float m5, float m6, 390 float m8, float m9, float m10 391 ) : Matrix( 392 m0, m1, m2, 0.0f, 393 m4, m5, m6, 0.0f, 394 m8, m9, m10, 0.0f, 395 0.0f, 0.0f, 0.0f, 1.0f 396 ) {} 397 398 RotationMatrix(const Vector& x, const Vector& y, const Vector& z) : 399 Matrix( 400 x.x(), x.y(), x.z(), 0.0f, 401 y.x(), y.y(), y.z(), 0.0f, 402 z.x(), z.y(), z.z(), 0.0f, 403 0.0f, 0.0f, 0.0f, 1.0f 404 ) {} 405 406 friend class UnitQuat; 407public: 408 RotationMatrix() : 409 Matrix( 410 1.0f, 0.0f, 0.0f, 0.0f, 411 0.0f, 1.0f, 0.0f, 0.0f, 412 0.0f, 0.0f, 1.0f, 0.0f, 413 0.0f, 0.0f, 0.0f, 1.0f 414 ) {} 415 416 // Emulate gluLookAt 417 static RotationMatrix lookAt( 418 const Vector& eye, const Vector& center, const Vector& up 419 ) { 420 Vector f(eye - center); 421 f.normalize(); 422 Vector x(Vector::cross(up, f)); 423 x.normalize(); 424 return RotationMatrix(x, Vector::cross(f, x), f); 425 } 426 427 static RotationMatrix heading(float a) { 428 float sa = std::sin(a); 429 float ca = std::cos(a); 430 431 return RotationMatrix( 432 1.0f, 0.0f, 0.0f, 433 0.0f, ca, -sa, 434 0.0f, sa, ca 435 ); 436 } 437 438 static RotationMatrix pitch(float a) { 439 float sa = std::sin(a); 440 float ca = std::cos(a); 441 442 return RotationMatrix( 443 ca, 0.0f, -sa, 444 0.0f, 1.0f, 0.0f, 445 sa, 0.0f, ca 446 ); 447 } 448 449 static RotationMatrix roll(float a) { 450 float sa = std::sin(a); 451 float ca = std::cos(a); 452 453 return RotationMatrix( 454 ca, -sa, 0.0f, 455 sa, ca, 0.0f, 456 0.0f, 0.0f, 1.0f 457 ); 458 } 459 460 RotationMatrix inverted() const { 461 return RotationMatrix( 462 _m[0], _m[4], _m[8], 463 _m[1], _m[5], _m[9], 464 _m[2], _m[6], _m[10] 465 ); 466 } 467 468 RotationMatrix transposed() const { 469 return RotationMatrix( 470 _m[0], _m[4], _m[8], 471 _m[1], _m[5], _m[9], 472 _m[2], _m[6], _m[10] 473 ); 474 } 475 476 Vector transform(const Vector& v) const { 477 return Vector( 478 _m[0] * v.x() + _m[4] * v.y() + _m[8] * v.z(), 479 _m[1] * v.x() + _m[5] * v.y() + _m[9] * v.z(), 480 _m[2] * v.x() + _m[6] * v.y() + _m[10] * v.z() 481 ); 482 } 483 484 UnitVector transform(const UnitVector& v) const { 485 return UnitVector( 486 _m[0] * v.x() + _m[4] * v.y() + _m[8] * v.z(), 487 _m[1] * v.x() + _m[5] * v.y() + _m[9] * v.z(), 488 _m[2] * v.x() + _m[6] * v.y() + _m[10] * v.z() 489 ); 490 } 491 492 RotationMatrix operator*(const RotationMatrix& m) const { 493 return RotationMatrix( 494 _m[0] * m._m[0] + _m[1] * m._m[4] + _m[2] * m._m[8], 495 _m[0] * m._m[1] + _m[1] * m._m[5] + _m[2] * m._m[9], 496 _m[0] * m._m[2] + _m[1] * m._m[6] + _m[2] * m._m[10] 497 , 498 _m[4] * m._m[0] + _m[5] * m._m[4] + _m[6] * m._m[8], 499 _m[4] * m._m[1] + _m[5] * m._m[5] + _m[6] * m._m[9], 500 _m[4] * m._m[2] + _m[5] * m._m[6] + _m[6] * m._m[10] 501 , 502 _m[8] * m._m[0] + _m[9] * m._m[4] + _m[10] * m._m[8], 503 _m[8] * m._m[1] + _m[9] * m._m[5] + _m[10] * m._m[9], 504 _m[8] * m._m[2] + _m[9] * m._m[6] + _m[10] * m._m[10] 505 ); 506 } 507}; 508 509class UnitQuat { 510protected: 511 float _q[4]; 512 unsigned int _renormalizeCount; 513 514 // These two are internal, since they do not guarantee length == 1 515 UnitQuat(float x, float y, float z, float w) : _renormalizeCount(0) { 516 _q[0] = x; 517 _q[1] = y; 518 _q[2] = z; 519 _q[3] = w; 520 } 521 522 void set(float x, float y, float z, float w) { 523 _q[0] = x; 524 _q[1] = y; 525 _q[2] = z; 526 _q[3] = w; 527 } 528public: 529 UnitQuat() : _renormalizeCount(0) { 530 _q[0] = _q[1] = _q[2] = 0.0f; 531 _q[3] = 1.0f; 532 } 533 534 UnitQuat(float a, const UnitVector& axis) : _renormalizeCount(0) { 535 a *= 0.5f; 536 float sinA = std::sin(a); 537 set( 538 sinA * axis.x(), 539 sinA * axis.y(), 540 sinA * axis.z(), 541 std::cos(a) 542 ); 543 } 544 545 UnitQuat(const RotationMatrix& r) : _renormalizeCount(0) { 546 float trace = r.get(0) + r.get(5) + r.get(10) + 1.0f; 547 if (trace > 0) { 548 float s = 2.0f * std::sqrt(trace); 549 set( 550 (r.get(9) - r.get(6)) / s, 551 (r.get(2) - r.get(8)) / s, 552 (r.get(4) - r.get(1)) / s, 553 s / 4.0f 554 ); 555 } else { 556 if (r.get(0) > r.get(5)) { 557 if (r.get(0) > r.get(10)) { 558 // r.get(0) is largest 559 float s = 2.0f * 560 std::sqrt(1.0f + r.get(0) - r.get(5) - r.get(10)); 561 set( 562 s / 4.0f, 563 (r.get(1) + r.get(4)) / s, 564 (r.get(2) + r.get(8)) / s, 565 (r.get(9) - r.get(6)) / s 566 ); 567 } else { 568 // r.get(10) is largest 569 float s = 2.0f * 570 std::sqrt(1.0f + r.get(10) - r.get(0) - r.get(5)); 571 set( 572 (r.get(2) + r.get(8)) / s, 573 (r.get(6) + r.get(9)) / s, 574 s / 4, 575 (r.get(4) - r.get(1)) / s 576 ); 577 } 578 } else { 579 if (r.get(5) > r.get(10)) { 580 // r.get(5) is largest 581 float s = 2.0f * 582 std::sqrt(1.0f + r.get(5) - r.get(0) - r.get(10)); 583 set( 584 (r.get(1) + r.get(4)) / s, 585 s / 4, 586 (r.get(6) + r.get(9)) / s, 587 (r.get(2) - r.get(8)) / s 588 ); 589 } else { 590 // r.get(10) is largest 591 float s = 2.0f * 592 std::sqrt(1.0f + r.get(10) - r.get(0) - r.get(5)); 593 set( 594 (r.get(2) + r.get(8)) / s, 595 (r.get(6) + r.get(9)) / s, 596 s / 4, 597 (r.get(4) - r.get(1)) / s 598 ); 599 } 600 } 601 } 602 } 603 604 operator RotationMatrix() const { 605 float wx = _q[3] * _q[0] * 2.0f; 606 float wy = _q[3] * _q[1] * 2.0f; 607 float wz = _q[3] * _q[2] * 2.0f; 608 float xx = _q[0] * _q[0] * 2.0f; 609 float xy = _q[0] * _q[1] * 2.0f; 610 float xz = _q[0] * _q[2] * 2.0f; 611 float yy = _q[1] * _q[1] * 2.0f; 612 float yz = _q[1] * _q[2] * 2.0f; 613 float zz = _q[2] * _q[2] * 2.0f; 614 615 return RotationMatrix( 616 1.0f - yy - zz, xy + wz, xz - wy, 617 xy - wz, 1.0f - xx - zz, yz + wx, 618 xz + wy, yz - wx, 1.0f - xx - yy 619 ); 620 } 621 622 void toAngleAxis(float* a, UnitVector* axis) const { 623 *a = 2.0f * std::acos(_q[3]); 624 float sinA = std::sqrt(1.0f - _q[3] * _q[3]); 625 if (std::abs(sinA) < 0.00001f) 626 axis->set(1.0f, 0.0f, 0.0f); 627 else 628 axis->set( 629 _q[0] / sinA, 630 _q[1] / sinA, 631 _q[2] / sinA 632 ); 633 } 634 635 UnitVector forward() const { 636 float wx = _q[3] * _q[0] * 2.0f; 637 float wy = _q[3] * _q[1] * 2.0f; 638 float xx = _q[0] * _q[0] * 2.0f; 639 float xz = _q[0] * _q[2] * 2.0f; 640 float yy = _q[1] * _q[1] * 2.0f; 641 float yz = _q[1] * _q[2] * 2.0f; 642 643 return UnitVector(-xz - wy, -yz + wx, -1.0f + xx + yy); 644 } 645 646 UnitVector backward() const { 647 float wx = _q[3] * _q[0] * 2.0f; 648 float wy = _q[3] * _q[1] * 2.0f; 649 float xx = _q[0] * _q[0] * 2.0f; 650 float xz = _q[0] * _q[2] * 2.0f; 651 float yy = _q[1] * _q[1] * 2.0f; 652 float yz = _q[1] * _q[2] * 2.0f; 653 654 return UnitVector(xz + wy, yz - wx, 1.0f - xx - yy); 655 } 656 657 UnitVector left() const { 658 float wy = _q[3] * _q[1] * 2.0f; 659 float wz = _q[3] * _q[2] * 2.0f; 660 float xy = _q[0] * _q[1] * 2.0f; 661 float xz = _q[0] * _q[2] * 2.0f; 662 float yy = _q[1] * _q[1] * 2.0f; 663 float zz = _q[2] * _q[2] * 2.0f; 664 665 return UnitVector(-1.0f + yy + zz, -xy - wz, -xz + wy); 666 } 667 668 UnitVector right() const { 669 float wy = _q[3] * _q[1] * 2.0f; 670 float wz = _q[3] * _q[2] * 2.0f; 671 float xy = _q[0] * _q[1] * 2.0f; 672 float xz = _q[0] * _q[2] * 2.0f; 673 float yy = _q[1] * _q[1] * 2.0f; 674 float zz = _q[2] * _q[2] * 2.0f; 675 676 return UnitVector(1.0f - yy - zz, xy + wz, xz - wy); 677 } 678 679 UnitVector up() const { 680 float wx = _q[3] * _q[0] * 2.0f; 681 float wz = _q[3] * _q[2] * 2.0f; 682 float xx = _q[0] * _q[0] * 2.0f; 683 float xy = _q[0] * _q[1] * 2.0f; 684 float yz = _q[1] * _q[2] * 2.0f; 685 float zz = _q[2] * _q[2] * 2.0f; 686 687 return UnitVector(xy - wz, 1.0f - xx - zz, yz + wx); 688 } 689 690 UnitVector down() const { 691 float wx = _q[3] * _q[0] * 2.0f; 692 float wz = _q[3] * _q[2] * 2.0f; 693 float xx = _q[0] * _q[0] * 2.0f; 694 float xy = _q[0] * _q[1] * 2.0f; 695 float yz = _q[1] * _q[2] * 2.0f; 696 float zz = _q[2] * _q[2] * 2.0f; 697 698 return UnitVector(-xy + wz, -1.0f + xx + zz, -yz + wx); 699 } 700 701 float getX() const { 702 return _q[0]; 703 } 704 705 float getY() const { 706 return _q[1]; 707 } 708 709 float getZ() const { 710 return _q[2]; 711 } 712 713 float getW() const { 714 return _q[3]; 715 } 716 717 void multiplyBy(const UnitQuat& q) { 718 float tempX = _q[0]; 719 float tempY = _q[1]; 720 float tempZ = _q[2]; 721 float tempW = _q[3]; 722 723 _q[0] = tempW * q._q[0] + q._q[3] * tempX 724 + tempY * q._q[2] - q._q[1] * tempZ; 725 _q[1] = tempW * q._q[1] + q._q[3] * tempY 726 + tempZ * q._q[0] - q._q[2] * tempX; 727 _q[2] = tempW * q._q[2] + q._q[3] * tempZ 728 + tempX * q._q[1] - q._q[0] * tempY; 729 _q[3] = tempW * q._q[3] 730 - tempX * q._q[0] 731 - tempY * q._q[1] 732 - tempZ * q._q[2]; 733 734 if (++_renormalizeCount == 5) { 735 float length = std::sqrt( 736 _q[0] * _q[0] + 737 _q[1] * _q[1] + 738 _q[2] * _q[2] + 739 _q[3] * _q[3] 740 ); 741 _q[0] /= length; 742 _q[1] /= length; 743 _q[2] /= length; 744 _q[3] /= length; 745 _renormalizeCount = 0; 746 } 747 } 748 749 void preMultiplyBy(const UnitQuat& q) { 750 float tempX = _q[0]; 751 float tempY = _q[1]; 752 float tempZ = _q[2]; 753 float tempW = _q[3]; 754 755 _q[0] = q._q[3] * tempX + tempW * q._q[0] 756 + q._q[1] * tempZ - tempY * q._q[2]; 757 _q[1] = q._q[3] * tempY + tempW * q._q[1] 758 + q._q[2] * tempX - tempZ * q._q[0]; 759 _q[2] = q._q[3] * tempZ + tempW * q._q[2] 760 + q._q[0] * tempY - tempX * q._q[1]; 761 _q[3] = q._q[3] * tempW 762 - q._q[0] * tempX 763 - q._q[1] * tempY 764 - q._q[2] * tempZ; 765 766 if (++_renormalizeCount == 5) { 767 float length = std::sqrt( 768 _q[0] * _q[0] + 769 _q[1] * _q[1] + 770 _q[2] * _q[2] + 771 _q[3] * _q[3] 772 ); 773 _q[0] /= length; 774 _q[1] /= length; 775 _q[2] /= length; 776 _q[3] /= length; 777 _renormalizeCount = 0; 778 } 779 } 780 781 static UnitQuat pitch(float a) { 782 a *= 0.5f; 783 return UnitQuat(std::sin(a), 0.0f, 0.0f, std::cos(a)); 784 } 785 786 static UnitQuat heading(float a) { 787 a *= 0.5f; 788 return UnitQuat(0.0f, std::sin(a), 0.0f, std::cos(a)); 789 } 790 791 static UnitQuat roll(float a) { 792 a *= 0.5f; 793 return UnitQuat(0.0f, 0.0f, std::sin(a), std::cos(a)); 794 } 795}; 796 797#endif // _VECTOR_HH