Subversion Repository Public Repository

Divide-Framework

This repository has no backups
This repository's network speed is throttled to 100KB/sec

Diff Revisions 868 vs 869 for /trunk/Source Code/Core/Math/Headers/Quaternion.inl

Diff revisions: vs.
  @@ -191,7 +191,7 @@
191 191
192 192 template <typename T>
193 193 inline Quaternion<T>& Quaternion<T>::operator*=(const Quaternion<T>& rq) {
194 - (*this) = rq * (*this);
194 + (*this) = (*this) * rq;
195 195 return (*this);
196 196 }
197 197
  @@ -306,13 +306,12 @@
306 306
307 307 template <typename T>
308 308 void Quaternion<T>::fromAxisAngle(const vec3<T>& v, Angle::DEGREES<T> angle) {
309 - Angle::RADIANS<T> angleRad = Angle::to_RADIANS(angle);
309 + Angle::RADIANS<T> angleHAlfRad = Angle::to_RADIANS(angle) * 0.5f;
310 310
311 - angleRad *= 0.5f;
312 311 vec3<T> vn(v);
313 312 vn.normalize();
314 313
315 - _elements.set(vn * std::sin(angleRad), std::cos(angleRad));
314 + _elements.set(vn * std::sin(angleHAlfRad), std::cos(angleHAlfRad));
316 315 }
317 316
318 317 template <typename T>
  @@ -415,30 +414,33 @@
415 414 }
416 415
417 416 template <typename T>
418 - void Quaternion<T>::getMatrix(mat4<F32>& outMatrix) const {
417 + void Quaternion<T>::getMatrix(mat3<F32>& outMatrix) const {
419 418 const T& x = X();
420 419 const T& y = Y();
421 420 const T& z = Z();
422 421 const T& w = W();
423 -
424 - T xx = x * x;
425 - T xy = x * y;
426 - T xz = x * z;
427 - T xw = x * w;
428 - T yy = y * y;
429 - T yz = y * z;
430 - T yw = y * w;
431 - T zz = z * z;
432 - T zw = z * w;
433 - outMatrix.mat[0] = 1 - 2 * (yy + zz);
434 - outMatrix.mat[1] = 2 * (xy - zw);
435 - outMatrix.mat[2] = 2 * (xz + yw);
436 - outMatrix.mat[4] = 2 * (xy + zw);
437 - outMatrix.mat[5] = 1 - 2 * (xx + zz);
438 - outMatrix.mat[6] = 2 * (yz - xw);
439 - outMatrix.mat[8] = 2 * (xz - yw);
440 - outMatrix.mat[9] = 2 * (yz + xw);
441 - outMatrix.mat[10] = 1 - 2 * (xx + yy);
422 + T fTx = x + x;
423 + T fTy = y + y;
424 + T fTz = z + z;
425 + T fTwx = fTx*w;
426 + T fTwy = fTy*w;
427 + T fTwz = fTz*w;
428 + T fTxx = fTx*x;
429 + T fTxy = fTy*x;
430 + T fTxz = fTz*x;
431 + T fTyy = fTy*y;
432 + T fTyz = fTz*y;
433 + T fTzz = fTz*z;
434 +
435 + outMatrix.m[0][0] = 1.0f - (fTyy + fTzz);
436 + outMatrix.m[0][1] = fTxy - fTwz;
437 + outMatrix.m[0][2] = fTxz + fTwy;
438 + outMatrix.m[1][0] = fTxy + fTwz;
439 + outMatrix.m[1][1] = 1.0f - (fTxx + fTzz);
440 + outMatrix.m[1][2] = fTyz - fTwx;
441 + outMatrix.m[2][0] = fTxz - fTwy;
442 + outMatrix.m[2][1] = fTyz + fTwx;
443 + outMatrix.m[2][2] = 1.0f - (fTxx + fTyy);
442 444 }
443 445
444 446 template <typename T>
  @@ -449,7 +451,10 @@
449 451
450 452 template <typename T>
451 453 void Quaternion<T>::getEuler(vec3<Angle::RADIANS<T>>& euler) const {
452 - T heading = 0, attitude = 0, bank = 0;
454 + T& heading = euler.yaw;
455 + T& attitude = euler.roll;
456 + T& bank = euler.pitch;
457 +
453 458 const T& x = X();
454 459 const T& y = Y();
455 460 const T& z = Z();
  @@ -478,10 +483,109 @@
478 483 attitude = std::asin(2 * test / unit);
479 484 bank = std::atan2(x2 * w - y2 * z, -sqx + sqy - sqz + sqw);
480 485 }
481 - // Convert back from Z = pitch to Z = roll
482 - euler.yaw = heading;
483 - euler.pitch = bank;
484 - euler.roll = attitude;
486 + }
487 +
488 + template <typename T>
489 + void Quaternion<T>::fromAxes(const vec3<T>* axis) {
490 +
491 + mat3<T> rot;
492 + for (U8 col = 0; col < 3; col++) {
493 + rot.setCol(iCol, axis[iCol]);
494 + }
495 +
496 + fromMatrix(rot);
497 + }
498 +
499 + template <typename T>
500 + void Quaternion<T>::fromAxes(const vec3<T>& xAxis, const vec3<T>& yAxis, const vec3<T>& zAxis) {
501 +
502 + mat3<T> rot;
503 +
504 + rot.setCol(0, xAxis);
505 + rot.setCol(1, yAxis);
506 + rot.setCol(2, zAxis);
507 +
508 + fromMatrix(rot);
509 + }
510 +
511 + template <typename T>
512 + void Quaternion<T>::toAxes(vec3<T>* axis) const {
513 +
514 + mat3<T> rot
515 + getMatrix(rot);
516 +
517 + for (U8 col = 0; col < 3; col++) {
518 + axis[col].set(rot.getCol(col));
519 + }
520 + }
521 +
522 + template <typename T>
523 + void Quaternion<T>::toAxes(vec3<T>& xAxis, vec3<T>& yAxis, vec3<T>& zAxis) const {
524 + mat3<T> rot
525 + getMatrix(rot);
526 + xAxis.set(rot.getCol(0));
527 + yAxis.set(rot.getCol(1));
528 + zAxis.set(rot.getCol(2));
529 + }
530 +
531 + template <typename T>
532 + vec3<T> Quaternion<T>::xAxis() const {
533 + const T& x = X();
534 + const T& y = Y();
535 + const T& z = Z();
536 + const T& w = W();
537 +
538 + //T fTx = 2.0f*x;
539 + T fTy = 2.0f*y;
540 + T fTz = 2.0f*z;
541 + T fTwy = fTy*w;
542 + T fTwz = fTz*w;
543 + T fTxy = fTy*x;
544 + T fTxz = fTz*x;
545 + T fTyy = fTy*y;
546 + T fTzz = fTz*z;
547 +
548 + return vec3<T>(1.0f - (fTyy + fTzz), fTxy + fTwz, fTxz - fTwy);
549 + }
550 +
551 + template <typename T>
552 + vec3<T> Quaternion<T>::yAxis() const {
553 + const T& x = X();
554 + const T& y = Y();
555 + const T& z = Z();
556 + const T& w = W();
557 +
558 + T fTx = 2.0f*x;
559 + T fTy = 2.0f*y;
560 + T fTz = 2.0f*z;
561 + T fTwx = fTx*w;
562 + T fTwz = fTz*w;
563 + T fTxx = fTx*x;
564 + T fTxy = fTy*x;
565 + T fTyz = fTz*y;
566 + T fTzz = fTz*z;
567 +
568 + return vec3<T>(fTxy - fTwz, 1.0f - (fTxx + fTzz), fTyz + fTwx);
569 + }
570 +
571 + template <typename T>
572 + vec3<T> Quaternion<T>::zAxis() const {
573 + const T& x = X();
574 + const T& y = Y();
575 + const T& z = Z();
576 + const T& w = W();
577 +
578 + T fTx = 2.0f*x;
579 + T fTy = 2.0f*y;
580 + T fTz = 2.0f*z;
581 + T fTwx = fTx*w;
582 + T fTwy = fTy*w;
583 + T fTxx = fTx*x;
584 + T fTxz = fTz*x;
585 + T fTyy = fTy*y;
586 + T fTyz = fTz*y;
587 +
588 + return vec3<T>(fTxz + fTwy, fTyz - fTwx, 1.0f - (fTxx + fTyy));
485 589 }
486 590
487 591 template <typename T>
  @@ -556,7 +660,7 @@
556 660 } else if (d < (1e-6f - 1.0f)) {
557 661 if (!fallbackAxis.compare(VECTOR3_ZERO)) {
558 662 // rotate 180 degrees about the fallback axis
559 - q.fromAxisAngle(fallbackAxis, Angle::to_RADIANS(to_F32(M_PI)));
663 + q.fromAxisAngle(fallbackAxis, to_F32(M_PI));
560 664 } else {
561 665 // Generate an axis
562 666 vec3<T> axis;
  @@ -566,7 +670,7 @@
566 670 axis.cross(WORLD_Y_AXIS, v);
567 671
568 672 axis.normalize();
569 - q.fromAxisAngle(axis, Angle::to_RADIANS(to_F32(M_PI)));
673 + q.fromAxisAngle(axis, to_F32(M_PI));
570 674 }
571 675 } else {
572 676 F32 s = Divide::Sqrt((1 + d) * 2.0f);
  @@ -588,15 +692,15 @@
588 692 }
589 693
590 694 template <typename T>
591 - inline mat4<T> GetMatrix(const Quaternion<T>& q) {
592 - mat4<T> temp;
695 + inline mat3<T> GetMatrix(const Quaternion<T>& q) {
696 + mat3<T> temp;
593 697 q.getMatrix(temp);
594 698 return temp;
595 699 }
596 700
597 701 template <typename T>
598 702 inline vec3<Angle::RADIANS<T>> GetEuler(const Quaternion<T>& q) {
599 - vec3<T> euler;
703 + vec3<Angle::RADIANS<T>> euler;
600 704 q.getEuler(euler);
601 705 return euler;
602 706 }