00001 #ifndef __INSTANTIO_MATRIX4_H
00002 #define __INSTANTIO_MATRIX4_H
00003
00004 #ifdef _MSC_VER
00005 # pragma once
00006 #endif
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026 #include "InstantIODef.h"
00027 #include "TypeName.h"
00028 #include <cmath>
00029 #include <iostream>
00030 #ifdef _MSC_VER
00031 # pragma warning (push)
00032 # pragma warning (disable: 4351)
00033 #endif
00034
00035
00036 namespace InstantIO
00037 {
00038
00039
00062 template <class T>
00063 class Matrix4
00064 {
00065 public:
00066
00068 enum MatrixElementIndex
00069 {
00070 M_00 = 0, M_01 = 1, M_02 = 2, M_03 = 3,
00071 M_10 = 4, M_11 = 5, M_12 = 6, M_13 = 7,
00072 M_20 = 8, M_21 = 9, M_22 = 10, M_23 = 11,
00073 M_30 = 12, M_31 = 13, M_32 = 14, M_33 = 15
00074 };
00075
00077 enum QuaternionElementIndex { Q_X = 0, Q_Y = 1, Q_Z = 2, Q_W = 3 };
00078
00080 enum VectorElementIndex { V_X = 0, V_Y = 1, V_Z = 2 };
00081
00083 static const T epsilon;
00084
00089 inline Matrix4(): matrix_()
00090 { setIdentity(); }
00091
00097 inline Matrix4(const Matrix4 &obj): matrix_()
00098 {
00099 matrix_[M_00] = obj.matrix_[M_00]; matrix_[M_01] = obj.matrix_[M_01];
00100 matrix_[M_02] = obj.matrix_[M_02]; matrix_[M_03] = obj.matrix_[M_03];
00101 matrix_[M_10] = obj.matrix_[M_10]; matrix_[M_11] = obj.matrix_[M_11];
00102 matrix_[M_12] = obj.matrix_[M_12]; matrix_[M_13] = obj.matrix_[M_13];
00103 matrix_[M_20] = obj.matrix_[M_20]; matrix_[M_21] = obj.matrix_[M_21];
00104 matrix_[M_22] = obj.matrix_[M_22]; matrix_[M_23] = obj.matrix_[M_23];
00105 matrix_[M_30] = obj.matrix_[M_30]; matrix_[M_31] = obj.matrix_[M_31];
00106 matrix_[M_32] = obj.matrix_[M_32]; matrix_[M_33] = obj.matrix_[M_33];
00107 }
00108
00116 inline Matrix4(const T matrix[16], bool transposed = false): matrix_()
00117 {
00118 if (transposed == true)
00119 {
00120 matrix_[M_00] = matrix[M_00]; matrix_[M_01] = matrix[M_10];
00121 matrix_[M_02] = matrix[M_20]; matrix_[M_03] = matrix[M_30];
00122 matrix_[M_10] = matrix[M_01]; matrix_[M_11] = matrix[M_11];
00123 matrix_[M_12] = matrix[M_21]; matrix_[M_13] = matrix[M_31];
00124 matrix_[M_20] = matrix[M_02]; matrix_[M_21] = matrix[M_12];
00125 matrix_[M_22] = matrix[M_22]; matrix_[M_23] = matrix[M_32];
00126 matrix_[M_30] = matrix[M_03]; matrix_[M_31] = matrix[M_13];
00127 matrix_[M_32] = matrix[M_23]; matrix_[M_33] = matrix[M_33];
00128 }
00129 else
00130 {
00131 matrix_[M_00] = matrix[M_00]; matrix_[M_01] = matrix[M_01];
00132 matrix_[M_02] = matrix[M_02]; matrix_[M_03] = matrix[M_03];
00133 matrix_[M_10] = matrix[M_10]; matrix_[M_11] = matrix[M_11];
00134 matrix_[M_12] = matrix[M_12]; matrix_[M_13] = matrix[M_13];
00135 matrix_[M_20] = matrix[M_20]; matrix_[M_21] = matrix[M_21];
00136 matrix_[M_22] = matrix[M_22]; matrix_[M_23] = matrix[M_23];
00137 matrix_[M_30] = matrix[M_30]; matrix_[M_31] = matrix[M_31];
00138 matrix_[M_32] = matrix[M_32]; matrix_[M_33] = matrix[M_33];
00139 }
00140 }
00141
00153 inline Matrix4(T tx, T ty, T tz, T qx, T qy, T qz, T qw)
00154 : matrix_()
00155 {
00156 setTransform(tx, ty, tz, qx, qy, qz, qw);
00157 }
00158
00173 inline Matrix4(T tx, T ty, T tz, T qx, T qy, T qz, T qw, T sx, T sy, T sz)
00174 : matrix_()
00175 {
00176 setTransform(tx, ty, tz, qx, qy, qz, qw);
00177 applyScale(sx, sy, sz);
00178 }
00179
00185 inline const Matrix4 &operator=(const Matrix4 &obj)
00186 {
00187 matrix_[M_00] = obj.matrix_[M_00]; matrix_[M_01] = obj.matrix_[M_01];
00188 matrix_[M_02] = obj.matrix_[M_02]; matrix_[M_03] = obj.matrix_[M_03];
00189 matrix_[M_10] = obj.matrix_[M_10]; matrix_[M_11] = obj.matrix_[M_11];
00190 matrix_[M_12] = obj.matrix_[M_12]; matrix_[M_13] = obj.matrix_[M_13];
00191 matrix_[M_20] = obj.matrix_[M_20]; matrix_[M_21] = obj.matrix_[M_21];
00192 matrix_[M_22] = obj.matrix_[M_22]; matrix_[M_23] = obj.matrix_[M_23];
00193 matrix_[M_30] = obj.matrix_[M_30]; matrix_[M_31] = obj.matrix_[M_31];
00194 matrix_[M_32] = obj.matrix_[M_32]; matrix_[M_33] = obj.matrix_[M_33];
00195 return obj;
00196 }
00197
00204 inline const T *operator=(const T matrix[16])
00205 {
00206 matrix_[M_00] = matrix[M_00]; matrix_[M_01] = matrix[M_01];
00207 matrix_[M_02] = matrix[M_02]; matrix_[M_03] = matrix[M_03];
00208 matrix_[M_10] = matrix[M_10]; matrix_[M_11] = matrix[M_11];
00209 matrix_[M_12] = matrix[M_12]; matrix_[M_13] = matrix[M_13];
00210 matrix_[M_20] = matrix[M_20]; matrix_[M_21] = matrix[M_21];
00211 matrix_[M_22] = matrix[M_22]; matrix_[M_23] = matrix[M_23];
00212 matrix_[M_30] = matrix[M_30]; matrix_[M_31] = matrix[M_31];
00213 matrix_[M_32] = matrix[M_32]; matrix_[M_33] = matrix[M_33];
00214 return matrix;
00215 }
00216
00218 void setIdentity();
00219
00226 void setTransform(T tx, T ty, T tz);
00227
00235 void setTransform(T qx, T qy, T qz, T qw);
00236
00247 void setTransform(T tx, T ty, T tz, T qx, T qy, T qz, T qw);
00248
00249
00264 inline void setTransform(T tx, T ty, T tz, T qx, T qy, T qz, T qw, T sx, T sy, T sz)
00265 {
00266 setTransform(tx, ty, tz, qx, qy, qz, qw);
00267 applyScale(sx, sy, sz);
00268 }
00269
00276 template <class TranslationT, class QuaternionT, class ScaleT>
00277 inline void setTransform(const TranslationT &t,const QuaternionT &q, const ScaleT &s)
00278 {
00279 setTransform(t, q);
00280 applyScale(s);
00281 }
00282
00283
00289 template <class TranslationT, class QuaternionT>
00290 inline void setTransform(const TranslationT &t, const QuaternionT &q)
00291 {
00292 setRotationFromQuaternion(q);
00293 setTranslation(t);
00294 matrix_[M_30] = 0.f;
00295 matrix_[M_31] = 0.f;
00296 matrix_[M_32] = 0.f;
00297 matrix_[M_33] = 1.f;
00298 }
00299
00306 template <class Vec3fT>
00307 void setTranslationAxisAngle(const Vec3fT &translation, const Vec3fT &axis, T angle)
00308 {
00309 T qx, qy, qz, qw;
00310 T length = static_cast<T>(sqrtf(axis[0] * axis[0] + axis[1] * axis[1] + axis[2] * axis[2]));
00311 T angle2 = angle / 2.f;
00312 T sangle2l = static_cast<T>(sin(angle2) / length);
00313 qx = sangle2l * axis[V_X];
00314 qy = sangle2l * axis[V_Y];
00315 qz = sangle2l * axis[V_Z];
00316 qw = static_cast<T>(cos(angle2));
00317 setTransform(translation[V_X], translation[V_Y], translation[V_Z], qx, qy, qz, qw);
00318 }
00319
00326 template <class ArrayT>
00327 inline void setMatrixFromArray(const ArrayT &array, bool transposed = false)
00328 {
00329 if (transposed == true)
00330 {
00331 matrix_[M_00] = array[M_00]; matrix_[M_01] = array[M_10];
00332 matrix_[M_02] = array[M_20]; matrix_[M_03] = array[M_30];
00333 matrix_[M_10] = array[M_01]; matrix_[M_11] = array[M_11];
00334 matrix_[M_12] = array[M_21]; matrix_[M_13] = array[M_31];
00335 matrix_[M_20] = array[M_02]; matrix_[M_21] = array[M_12];
00336 matrix_[M_22] = array[M_22]; matrix_[M_23] = array[M_32];
00337 matrix_[M_30] = array[M_03]; matrix_[M_31] = array[M_13];
00338 matrix_[M_32] = array[M_23]; matrix_[M_33] = array[M_33];
00339 }
00340 else
00341 {
00342 matrix_[M_00] = array[M_00]; matrix_[M_01] = array[M_01];
00343 matrix_[M_02] = array[M_02]; matrix_[M_03] = array[M_03];
00344 matrix_[M_10] = array[M_10]; matrix_[M_11] = array[M_11];
00345 matrix_[M_12] = array[M_12]; matrix_[M_13] = array[M_13];
00346 matrix_[M_20] = array[M_20]; matrix_[M_21] = array[M_21];
00347 matrix_[M_22] = array[M_22]; matrix_[M_23] = array[M_23];
00348 matrix_[M_30] = array[M_30]; matrix_[M_31] = array[M_31];
00349 matrix_[M_32] = array[M_32]; matrix_[M_33] = array[M_33];
00350 }
00351 }
00352
00359 template <class MatrixT>
00360 inline void setMatrixFromMatrix(const MatrixT &matrix, bool transposed = false)
00361 {
00362 if (transposed == true)
00363 {
00364 matrix_[M_00] = matrix[0][0]; matrix_[M_01] = matrix[1][0];
00365 matrix_[M_02] = matrix[2][0]; matrix_[M_03] = matrix[3][0];
00366 matrix_[M_10] = matrix[0][1]; matrix_[M_11] = matrix[1][1];
00367 matrix_[M_12] = matrix[2][1]; matrix_[M_13] = matrix[3][1];
00368 matrix_[M_20] = matrix[0][2]; matrix_[M_21] = matrix[1][2];
00369 matrix_[M_22] = matrix[2][2]; matrix_[M_23] = matrix[3][2];
00370 matrix_[M_30] = matrix[0][3]; matrix_[M_31] = matrix[1][3];
00371 matrix_[M_32] = matrix[2][3]; matrix_[M_33] = matrix[3][3];
00372 }
00373 else
00374 {
00375 matrix_[M_00] = matrix[0][0]; matrix_[M_01] = matrix[0][1];
00376 matrix_[M_02] = matrix[0][2]; matrix_[M_03] = matrix[0][3];
00377 matrix_[M_10] = matrix[1][0]; matrix_[M_11] = matrix[1][1];
00378 matrix_[M_12] = matrix[1][2]; matrix_[M_13] = matrix[1][3];
00379 matrix_[M_20] = matrix[2][0]; matrix_[M_21] = matrix[2][1];
00380 matrix_[M_22] = matrix[2][2]; matrix_[M_23] = matrix[2][3];
00381 matrix_[M_30] = matrix[3][0]; matrix_[M_31] = matrix[3][1];
00382 matrix_[M_32] = matrix[3][2]; matrix_[M_33] = matrix[3][3];
00383 }
00384 }
00385
00393 void setRotationFromQuaternion(T qx, T qy, T qz, T qw);
00394
00399 template<class QuaternionT>
00400 inline void setRotationFromQuaternion(const QuaternionT &q)
00401 {
00402 setRotationFromQuaternion(q[Q_X], q[Q_Y], q[Q_Z], q[Q_W]);
00403 }
00404
00411 template <class RotationMatrixT>
00412 inline void setRotationFromRotationMatrix(const RotationMatrixT &r, bool transposed = false)
00413 {
00414 if (transposed == true)
00415 {
00416 matrix_[M_00] = r[0][0];
00417 matrix_[M_01] = r[1][0];
00418 matrix_[M_02] = r[2][0];
00419
00420 matrix_[M_10] = r[0][1];
00421 matrix_[M_11] = r[1][1];
00422 matrix_[M_12] = r[2][1];
00423
00424 matrix_[M_20] = r[0][2];
00425 matrix_[M_21] = r[1][2];
00426 matrix_[M_22] = r[2][2];
00427
00428 }
00429 else
00430 {
00431 matrix_[M_00] = r[0][0];
00432 matrix_[M_10] = r[1][0];
00433 matrix_[M_20] = r[2][0];
00434
00435 matrix_[M_01] = r[0][1];
00436 matrix_[M_11] = r[1][1];
00437 matrix_[M_21] = r[2][1];
00438
00439 matrix_[M_02] = r[0][2];
00440 matrix_[M_12] = r[1][2];
00441 matrix_[M_22] = r[2][2];
00442 }
00443 }
00444
00449 template<class TranslationT>
00450 inline void setTranslation(const TranslationT &translation)
00451 {
00452 matrix_[M_03] = translation[V_X];
00453 matrix_[M_13] = translation[V_Y];
00454 matrix_[M_23] = translation[V_Z];
00455 }
00456
00463 void setTranslation(T tx, T ty, T tz);
00464
00469 template <class ScaleT>
00470 inline void applyScale(const ScaleT &s)
00471 {
00472 applyScale(s[V_X], s[V_Y], s[V_Z]);
00473 }
00474
00481 void applyScale(T sx, T sy, T sz);
00482
00488 inline T operator[](MatrixElementIndex i) const { return matrix_[i]; }
00489
00495 inline T &operator[](MatrixElementIndex i) { return matrix_[i]; }
00496
00502 inline const T *getValue() const { return matrix_; }
00503
00509 inline T *getValue() { return matrix_; }
00510
00525 void getTransform(T &tx, T &ty, T &tz, T &qx, T &qy, T &qz, T &qw, T &sx, T &sy, T &sz) const;
00526
00538 void getTransform(T &tx, T &ty, T &tz, T &qx, T &qy, T &qz, T &qw) const;
00539
00547 template <class TranslationT, class QuaternionT, class ScaleT>
00548 inline void getTransform(TranslationT &t, QuaternionT &q, ScaleT &s) const
00549 {
00550 T tx, ty, tz, qx, qy, qz, qw, sx, sy, sz;
00551 getTransform(tx, ty, tz, qx, qy, qz, qw, sx, sy, sz);
00552 t[V_X] = tx; t[V_Y] = ty; t[V_Z] = tz;
00553 s[V_X] = sx; s[V_Y] = sy; s[V_Z] = sz;
00554 q[Q_W] = qw; q[Q_X] = qx; q[Q_Y] = qy; q[Q_Z] = qz;
00555 }
00556
00563 template <class TranslationT, class QuaternionT>
00564 inline void getTransform(TranslationT &t, QuaternionT &q) const
00565 {
00566 T tx, ty, tz, qx, qy, qz, qw;
00567 getTransform(tx, ty, tz, qx, qy, qz, qw);
00568 t[V_X] = tx; t[V_Y] = ty; t[V_Z] = tz;
00569 q[Q_W] = qw; q[Q_X] = qx; q[Q_Y] = qy; q[Q_Z] = qz;
00570 }
00571
00578 template <class ArrayT>
00579 inline void getMatrixAsArray(ArrayT &matrix, bool transposed = false) const
00580 {
00581 if (transposed == true)
00582 {
00583 matrix[M_00] = matrix_[M_00]; matrix[M_01] = matrix_[M_10];
00584 matrix[M_02] = matrix_[M_20]; matrix[M_03] = matrix_[M_30];
00585 matrix[M_10] = matrix_[M_01]; matrix[M_11] = matrix_[M_11];
00586 matrix[M_12] = matrix_[M_21]; matrix[M_13] = matrix_[M_31];
00587 matrix[M_20] = matrix_[M_02]; matrix[M_21] = matrix_[M_12];
00588 matrix[M_22] = matrix_[M_22]; matrix[M_23] = matrix_[M_32];
00589 matrix[M_30] = matrix_[M_03]; matrix[M_31] = matrix_[M_13];
00590 matrix[M_32] = matrix_[M_23]; matrix[M_33] = matrix_[M_33];
00591 }
00592 else
00593 {
00594 matrix[M_00] = matrix_[M_00]; matrix[M_01] = matrix_[M_01];
00595 matrix[M_02] = matrix_[M_02]; matrix[M_03] = matrix_[M_03];
00596 matrix[M_10] = matrix_[M_10]; matrix[M_11] = matrix_[M_11];
00597 matrix[M_12] = matrix_[M_12]; matrix[M_13] = matrix_[M_13];
00598 matrix[M_20] = matrix_[M_20]; matrix[M_21] = matrix_[M_21];
00599 matrix[M_22] = matrix_[M_22]; matrix[M_23] = matrix_[M_23];
00600 matrix[M_30] = matrix_[M_30]; matrix[M_31] = matrix_[M_31];
00601 matrix[M_32] = matrix_[M_32]; matrix[M_33] = matrix_[M_33];
00602 }
00603 }
00604
00611 template <class MatrixT>
00612 inline void getMatrixAsMatrix(MatrixT &matrix, bool transposed = false) const
00613 {
00614 if (transposed == true)
00615 {
00616 matrix[0][0] = matrix_[M_00]; matrix[0][1] = matrix_[M_10];
00617 matrix[0][2] = matrix_[M_20]; matrix[0][3] = matrix_[M_30];
00618 matrix[1][0] = matrix_[M_01]; matrix[1][1] = matrix_[M_11];
00619 matrix[1][2] = matrix_[M_21]; matrix[1][3] = matrix_[M_31];
00620 matrix[2][0] = matrix_[M_02]; matrix[2][1] = matrix_[M_12];
00621 matrix[2][2] = matrix_[M_22]; matrix[2][3] = matrix_[M_32];
00622 matrix[3][0] = matrix_[M_03]; matrix[3][1] = matrix_[M_13];
00623 matrix[3][2] = matrix_[M_23]; matrix[3][3] = matrix_[M_33];
00624 }
00625 else
00626 {
00627 matrix[0][0] = matrix_[M_00]; matrix[0][1] = matrix_[M_01];
00628 matrix[0][2] = matrix_[M_02]; matrix[0][3] = matrix_[M_03];
00629 matrix[1][0] = matrix_[M_10]; matrix[1][1] = matrix_[M_11];
00630 matrix[1][2] = matrix_[M_12]; matrix[1][3] = matrix_[M_13];
00631 matrix[2][0] = matrix_[M_20]; matrix[2][1] = matrix_[M_21];
00632 matrix[2][2] = matrix_[M_22]; matrix[2][3] = matrix_[M_23];
00633 matrix[3][0] = matrix_[M_30]; matrix[3][1] = matrix_[M_31];
00634 matrix[3][2] = matrix_[M_32]; matrix[3][3] = matrix_[M_33];
00635 }
00636 }
00637
00642 inline operator T *() { return &matrix_[0]; }
00643
00648 inline operator const T *() const { return &matrix_[0]; }
00649
00654 void scale(T s);
00655
00660 void add(const Matrix4 &m);
00661
00667 void addScaled(const Matrix4 &m, T s);
00668
00672 void negate();
00673
00678 void multLeft(const Matrix4 &m);
00679
00684 void mult(const Matrix4 &m);
00685
00690 template <class Vec3fT>
00691 void multVec(Vec3fT &v)
00692 {
00693 Vec3fT r;
00694 r[0] =
00695 v[0] * matrix_[M_00] +
00696 v[1] * matrix_[M_10] +
00697 v[2] * matrix_[M_20];
00698 r[1] =
00699 v[0] * matrix_[M_01] +
00700 v[1] * matrix_[M_11] +
00701 v[2] * matrix_[M_21];
00702 r[2] =
00703 v[0] * matrix_[M_02] +
00704 v[1] * matrix_[M_12] +
00705 v[2] * matrix_[M_22];
00706 v = r;
00707 }
00708
00713 template <class Vec3fT>
00714 void multPnt(Vec3fT &v)
00715 {
00716 Vec3fT r;
00717 r[0] =
00718 v[0] * matrix_[M_00] +
00719 v[1] * matrix_[M_10] +
00720 v[2] * matrix_[M_20] +
00721 matrix_[M_30];
00722 r[1] =
00723 v[0] * matrix_[M_01] +
00724 v[1] * matrix_[M_11] +
00725 v[2] * matrix_[M_21] +
00726 matrix_[M_31];
00727 r[2] =
00728 v[0] * matrix_[M_02] +
00729 v[1] * matrix_[M_12] +
00730 v[2] * matrix_[M_22] +
00731 matrix_[M_32];
00732 v = r;
00733 }
00734
00739 T normInfinity();
00740
00745 T det () const;
00746
00752 bool getInverse(Matrix4 &result) const;
00753
00759 bool sqrt();
00760
00766 void setExpOf(const Matrix4 &m);
00767
00773 bool setLogOf(const Matrix4 &matrix);
00774
00779 void print(std::ostream &target);
00780
00781 private:
00782
00784 T matrix_[16];
00785 };
00786
00787
00788 template <class T>
00789 const T Matrix4<T>::epsilon = static_cast<T>(0.000001);
00790
00791
00792 template <class T>
00793 void Matrix4<T>::setIdentity()
00794 {
00795 matrix_[M_01] = matrix_[M_02] = matrix_[M_03] = 0.f;
00796 matrix_[M_10] = matrix_[M_12] = matrix_[M_13] = 0.f;
00797 matrix_[M_20] = matrix_[M_21] = matrix_[M_23] = 0.f;
00798 matrix_[M_30] = matrix_[M_31] = matrix_[M_32] = 0.f;
00799 matrix_[M_00] = matrix_[M_11] = matrix_[M_22] = matrix_[M_33] = 1.f;
00800 }
00801
00802
00803 template <class T>
00804 void Matrix4<T>::setTransform(T tx, T ty, T tz)
00805 {
00806 setIdentity();
00807 setTranslation(tx, ty, tz);
00808 }
00809
00810
00811 template <class T>
00812 void Matrix4<T>::setTransform(T qx, T qy, T qz, T qw)
00813 {
00814
00815 setTranslation(0.f, 0.f, 0.f);
00816 setRotationFromQuaternion(qx, qy, qz, qw);
00817 matrix_[M_30] = 0.f;
00818 matrix_[M_31] = 0.f;
00819 matrix_[M_32] = 0.f;
00820 matrix_[M_33] = 1.f;
00821 }
00822
00823
00824 template <class T>
00825 void Matrix4<T>::setTransform(T tx, T ty, T tz, T qx, T qy, T qz, T qw)
00826 {
00827
00828 setRotationFromQuaternion(qx, qy, qz, qw);
00829 setTranslation(tx, ty, tz);
00830 matrix_[M_30] = 0.f;
00831 matrix_[M_31] = 0.f;
00832 matrix_[M_32] = 0.f;
00833 matrix_[M_33] = 1.f;
00834 }
00835
00836
00837 template <class T>
00838 void Matrix4<T>::setTranslation(T tx, T ty, T tz)
00839 {
00840 matrix_[M_03] = tx;
00841 matrix_[M_13] = ty;
00842 matrix_[M_23] = tz;
00843 }
00844
00845
00846 template <class T>
00847 void Matrix4<T>::setRotationFromQuaternion(T qx, T qy, T qz, T qw)
00848 {
00849 matrix_[M_00] = 1.0f - 2.0f * (qy * qy + qz * qz);
00850 matrix_[M_10] = 2.0f * (qx * qy + qz * qw);
00851 matrix_[M_20] = 2.0f * (qz * qx - qy * qw);
00852 matrix_[M_30] = 0.0f;
00853
00854 matrix_[M_01] = 2.0f * (qx * qy - qz * qw);
00855 matrix_[M_11] = 1.0f - 2.0f * (qz * qz + qx * qx);
00856 matrix_[M_21] = 2.0f * (qy * qz + qx * qw);
00857 matrix_[M_31] = 0.0f;
00858
00859 matrix_[M_02] = 2.0f * (qz * qx + qy * qw);
00860 matrix_[M_12] = 2.0f * (qy * qz - qx * qw);
00861 matrix_[M_22] = 1.0f - 2.0f * (qy * qy + qx * qx);
00862 matrix_[M_32] = 0.0f;
00863
00864 matrix_[M_03] = 0.0f;
00865 matrix_[M_13] = 0.0f;
00866 matrix_[M_23] = 0.0f;
00867 matrix_[M_33] = 1.0f;
00868 }
00869
00870
00871 template <class T>
00872 void Matrix4<T>::applyScale(T sx, T sy, T sz)
00873 {
00874 matrix_[M_00] *= sx; matrix_[M_10] *= sx; matrix_[M_20] *=sx;
00875 matrix_[M_01] *= sy; matrix_[M_11] *= sy; matrix_[M_21] *=sy;
00876 matrix_[M_02] *= sz; matrix_[M_12] *= sz; matrix_[M_22] *=sz;
00877 }
00878
00879
00880 template <class T>
00881 void Matrix4<T>::getTransform(T &tx, T &ty, T &tz, T &qx, T &qy, T &qz, T &qw, T &sx, T &sy, T &sz) const
00882 {
00883 tx = matrix_[M_03];
00884 ty = matrix_[M_13];
00885 tz = matrix_[M_23];
00886 T r00 = matrix_[M_00], r01 = matrix_[M_01], r02 = matrix_[M_02];
00887 T r10 = matrix_[M_10], r11 = matrix_[M_11], r12 = matrix_[M_12];
00888 T r20 = matrix_[M_20], r21 = matrix_[M_21], r22 = matrix_[M_22];
00889 sx = sqrtf(r00 * r00 + r10 * r10 + r20 * r20);
00890 sy = sqrtf(r01 * r01 + r11 * r11 + r21 * r21);
00891 sz = sqrtf(r02 * r02 + r12 * r12 + r22 * r22);
00892 T sc = 1.f / sx;
00893 r00 *= sc; r10 *= sc; r20 *= sc;
00894 sc = 1.f / sy;
00895 r01 *= sc; r11 *= sc; r21 *= sc;
00896 sc = 1.f / sz;
00897 r02 *= sc; r12 *= sc; r22 *= sc;
00898 T trace = r00 + r11 + r22;
00899 if (trace > epsilon)
00900 {
00901 T sf = 0.5f / sqrtf(trace + 1.f);
00902 qw = 0.25f / sf;
00903 qx = (r21 - r12) * sf;
00904 qy = (r02 - r20) * sf;
00905 qz = (r10 - r01) * sf;
00906 }
00907 else
00908 {
00909 if ((r00 > r11) && (r00 > r22))
00910 {
00911 T sf = 0.5f / sqrtf(1.f + r00 - r11 - r22);
00912 qx = 0.25f / sf;
00913 qy = (r01 + r10) * sf;
00914 qz = (r02 + r20) * sf;
00915 qw = (r12 - r21) * sf;
00916 }
00917 else if (r11 > r22)
00918 {
00919 T sf = 0.5f / sqrtf(1.f + r11 - r00 - r22);
00920 qx = (r01 + r10) * sf;
00921 qy = 0.25f / sf;
00922 qz = (r12 + r21) * sf;
00923 qw = (r02 - r20) * sf;
00924 }
00925 else
00926 {
00927 T sf = 0.5f / sqrtf(1.f + r22 - r00 - r11);
00928 qx = (r02 + r20) * sf;
00929 qy = (r12 + r21) * sf;
00930 qz = 0.25f / sf;
00931 qw = (r01 - r10) * sf;
00932 }
00933 }
00934 }
00935
00936
00937 template <class T>
00938 void Matrix4<T>::getTransform(T &tx, T &ty, T &tz, T &qx, T &qy, T &qz, T &qw) const
00939 {
00940 if (
00941 (fabs(1.f - (matrix_[M_00] * matrix_[M_00] + matrix_[M_01] * matrix_[M_01] + matrix_[M_02] * matrix_[M_02])) > epsilon) ||
00942 (fabs(1.f - (matrix_[M_10] * matrix_[M_10] + matrix_[M_11] * matrix_[M_11] + matrix_[M_12] * matrix_[M_12])) > epsilon) ||
00943 (fabs(1.f - (matrix_[M_20] * matrix_[M_20] + matrix_[M_21] * matrix_[M_21] + matrix_[M_22] * matrix_[M_22])) > epsilon))
00944 {
00945
00946
00947 T sx, sy, sz;
00948 getTransform(tx, ty, tz, qx, qy, qz, qw, sx, sy, sz);
00949 return;
00950 }
00951 tx = matrix_[M_03];
00952 ty = matrix_[M_13];
00953 tz = matrix_[M_23];
00954 T trace = matrix_[M_00] + matrix_[M_11] + matrix_[M_22];
00955 if (trace > epsilon)
00956 {
00957 T sf = 0.5f / sqrtf(trace + 1.f);
00958 qw = 0.25f / sf;
00959 qx = (matrix_[M_21] - matrix_[M_12]) * sf;
00960 qy = (matrix_[M_02] - matrix_[M_20]) * sf;
00961 qz = (matrix_[M_10] - matrix_[M_01]) * sf;
00962 }
00963 else
00964 {
00965 if ((matrix_[M_00] > matrix_[M_11]) && (matrix_[M_00] > matrix_[M_22]))
00966 {
00967 T sf = 0.5f / sqrtf(1.f + matrix_[M_00] - matrix_[M_11] - matrix_[M_22]);
00968 qx = 0.25f / sf;
00969 qy = (matrix_[M_01] + matrix_[M_10]) * sf;
00970 qz = (matrix_[M_02] + matrix_[M_20]) * sf;
00971 qw = (matrix_[M_12] - matrix_[M_21]) * sf;
00972 }
00973 else if (matrix_[M_11] > matrix_[M_22])
00974 {
00975 T sf = 1.f / sqrtf(1.f + matrix_[M_11] - matrix_[M_00] - matrix_[M_22]);
00976 qx = (matrix_[M_01] + matrix_[M_10]) * sf;
00977 qy = 0.25f / sf;
00978 qz = (matrix_[M_12] + matrix_[M_21]) * sf;
00979 qw = (matrix_[M_02] - matrix_[M_20]) * sf;
00980 }
00981 else
00982 {
00983 T sf = 0.5f / sqrtf(1.f + matrix_[M_22] - matrix_[M_00] - matrix_[M_11]);
00984 qx = (matrix_[M_02] + matrix_[M_20]) * sf;
00985 qy = (matrix_[M_12] + matrix_[M_21]) * sf;
00986 qz = 0.25f / sf;
00987 qw = (matrix_[M_01] - matrix_[M_10]) * sf;
00988 }
00989 }
00990 }
00991
00992
00993 template <class T>
00994 void Matrix4<T>::scale(T s)
00995 {
00996 for (int i = 0; i < 16; ++i)
00997 matrix_[i] *= s;
00998 }
00999
01000
01001 template <class T>
01002 void Matrix4<T>::add(const Matrix4 &m)
01003 {
01004 for (int i = 0; i < 16; ++i)
01005 matrix_[i] += m.matrix_[i];
01006 }
01007
01008
01009 template <class T>
01010 void Matrix4<T>::addScaled(const Matrix4 &m, T s)
01011 {
01012 for (int i = 0; i < 16; ++i)
01013 matrix_[i] += s * m.matrix_[i];
01014 }
01015
01016
01017 template <class T>
01018 void Matrix4<T>::negate()
01019 {
01020 for (int i = 0; i < 16; ++i)
01021 matrix_[i] *= -1.f;
01022 }
01023
01024
01025 template <class T>
01026 void Matrix4<T>::multLeft(const Matrix4 &m)
01027 {
01028
01029 Matrix4 tmp;
01030 for (int r = 0; r < 16; r += 4)
01031 for (int c = 0; c < 4; ++c)
01032 tmp.matrix_[r+c] = m.matrix_[r ] * matrix_[ c] +
01033 m.matrix_[r + 1] * matrix_[ 4 + c] +
01034 m.matrix_[r + 2] * matrix_[ 8 + c] +
01035 m.matrix_[r + 3] * matrix_[12 + c];
01036 for (int i = 0; i < 16; ++i)
01037 matrix_[i] = tmp.matrix_[i];
01038 }
01039
01040
01041 template <class T>
01042 void Matrix4<T>::mult(const Matrix4 &m)
01043 {
01044 Matrix4 tmp;
01045 for (int r = 0; r < 16; r += 4)
01046 for (int c = 0; c < 4; ++c)
01047 tmp.matrix_[r + c] = matrix_[r ] * m.matrix_[ c] +
01048 matrix_[r + 1] * m.matrix_[ 4 + c] +
01049 matrix_[r + 2] * m.matrix_[ 8 + c] +
01050 matrix_[r + 3] * m.matrix_[12 + c];
01051 for (int i = 0; i < 16; ++i)
01052 matrix_[i] = tmp.matrix_[i];
01053 }
01054
01055
01056 template <class T>
01057 T Matrix4<T>::normInfinity()
01058 {
01059 T t, n = 0.0f;
01060 for (int i = 0; i < 16; ++i)
01061 {
01062 t = fabs(matrix_[i]);
01063 if (t > n)
01064 n = t;
01065 }
01066 return n;
01067 }
01068
01069
01070 template <class T>
01071 static inline T det3(T a1, T a2, T a3, T b1, T b2, T b3, T c1, T c2, T c3)
01072 {
01073 return
01074 (a1 * b2 * c3) + (a2 * b3 * c1) + (a3 * b1 * c2) -
01075 (a1 * b3 * c2) - (a2 * b1 * c3) - (a3 * b2 * c1);
01076 }
01077
01078
01079 template <class T>
01080 T Matrix4<T>::det() const
01081 {
01082 T a1, a2, a3, a4,
01083 b1, b2, b3, b4,
01084 c1, c2, c3, c4,
01085 d1, d2, d3, d4;
01086
01087
01088 a1 = matrix_[M_00];
01089 b1 = matrix_[M_01];
01090 c1 = matrix_[M_02];
01091 d1 = matrix_[M_03];
01092
01093 a2 = matrix_[M_10];
01094 b2 = matrix_[M_11];
01095 c2 = matrix_[M_12];
01096 d2 = matrix_[M_13];
01097
01098 a3 = matrix_[M_20];
01099 b3 = matrix_[M_21];
01100 c3 = matrix_[M_22];
01101 d3 = matrix_[M_23];
01102
01103 a4 = matrix_[M_30];
01104 b4 = matrix_[M_31];
01105 c4 = matrix_[M_32];
01106 d4 = matrix_[M_33];
01107
01108 return (a1 * det3(b2, b3, b4, c2, c3, c4, d2, d3, d4) -
01109 b1 * det3(a2, a3, a4, c2, c3, c4, d2, d3, d4) +
01110 c1 * det3(a2, a3, a4, b2, b3, b4, d2, d3, d4) -
01111 d1 * det3(a2, a3, a4, b2, b3, b4, c2, c3, c4));
01112 }
01113
01114
01115 template <class T>
01116 bool Matrix4<T>::getInverse(Matrix4 &result) const
01117 {
01118 T rDet;
01119
01120 T a1, a2, a3, a4,
01121 b1, b2, b3, b4,
01122 c1, c2, c3, c4,
01123 d1, d2, d3, d4;
01124
01125
01126 a1 = matrix_[M_00];
01127 b1 = matrix_[M_01];
01128 c1 = matrix_[M_02];
01129 d1 = matrix_[M_03];
01130
01131 a2 = matrix_[M_10];
01132 b2 = matrix_[M_11];
01133 c2 = matrix_[M_12];
01134 d2 = matrix_[M_13];
01135
01136 a3 = matrix_[M_20];
01137 b3 = matrix_[M_21];
01138 c3 = matrix_[M_22];
01139 d3 = matrix_[M_23];
01140
01141 a4 = matrix_[M_30];
01142 b4 = matrix_[M_31];
01143 c4 = matrix_[M_32];
01144 d4 = matrix_[M_33];
01145
01146 rDet = det();
01147
01148 if (fabs(rDet) < 1E-30)
01149 {
01150 result.setIdentity();
01151 return false;
01152 }
01153
01154 rDet = 1.f / rDet;
01155
01156
01157 result[M_00] = det3(b2, b3, b4, c2, c3, c4, d2, d3, d4) * rDet;
01158 result[M_10] = - det3(a2, a3, a4, c2, c3, c4, d2, d3, d4) * rDet;
01159 result[M_20] = det3(a2, a3, a4, b2, b3, b4, d2, d3, d4) * rDet;
01160 result[M_30] = - det3(a2, a3, a4, b2, b3, b4, c2, c3, c4) * rDet;
01161
01162 result[M_01] = - det3(b1, b3, b4, c1, c3, c4, d1, d3, d4) * rDet;
01163 result[M_11] = det3(a1, a3, a4, c1, c3, c4, d1, d3, d4) * rDet;
01164 result[M_21] = - det3(a1, a3, a4, b1, b3, b4, d1, d3, d4) * rDet;
01165 result[M_31] = det3(a1, a3, a4, b1, b3, b4, c1, c3, c4) * rDet;
01166
01167 result[M_02] = det3(b1, b2, b4, c1, c2, c4, d1, d2, d4) * rDet;
01168 result[M_12] = - det3(a1, a2, a4, c1, c2, c4, d1, d2, d4) * rDet;
01169 result[M_22] = det3(a1, a2, a4, b1, b2, b4, d1, d2, d4) * rDet;
01170 result[M_32] = - det3(a1, a2, a4, b1, b2, b4, c1, c2, c4) * rDet;
01171
01172 result[M_03] = - det3(b1, b2, b3, c1, c2, c3, d1, d2, d3) * rDet;
01173 result[M_13] = det3(a1, a2, a3, c1, c2, c3, d1, d2, d3) * rDet;
01174 result[M_23] = - det3(a1, a2, a3, b1, b2, b3, d1, d2, d3) * rDet;
01175 result[M_33] = det3(a1, a2, a3, b1, b2, b3, c1, c2, c3) * rDet;
01176
01177 return true;
01178 }
01179
01180
01181 template <class T>
01182 bool Matrix4<T>::sqrt()
01183 {
01184 Matrix4 iX;
01185 Matrix4 Y;
01186 Matrix4 iY;
01187 T g;
01188 T ig;
01189 Y.setIdentity();
01190 for (int i = 0; i < 6; ++i)
01191 {
01192 getInverse(iX);
01193
01194 Y.getInverse(iY);
01195
01196 g = fabsf(powf(det() * Y.det(), -0.125f));
01197 ig = 1.0f / g;
01198
01199 scale (g );
01200 addScaled(iY, ig);
01201 scale (0.5f );
01202
01203 Y.scale (g );
01204 Y.addScaled(iX, ig);
01205 Y.scale (0.5f );
01206 }
01207
01208 return true;
01209 }
01210
01211
01212 template <class T>
01213 void Matrix4<T>::setExpOf(const Matrix4& m)
01214 {
01215 const int q = 6;
01216
01217 Matrix4 A(m);
01218 Matrix4 D;
01219 Matrix4 N;
01220
01221 int j = 1;
01222 int k;
01223
01224 T c = 1.0;
01225
01226 j += int(logf(A.normInfinity() / 0.693f));
01227
01228 if (j < 0)
01229 j = 0;
01230
01231 A.scale(1.0f / T(1 << j));
01232
01233 setIdentity();
01234
01235 for (k = 1; k <= q; ++k)
01236 {
01237 c *= T(q - k + 1) / T(k * (2 *q - k + 1));
01238
01239 multLeft(A);
01240
01241 N.addScaled(*this, c);
01242
01243 if (k % 2)
01244 D.addScaled(*this, -c);
01245 else
01246 D.addScaled(*this, c);
01247 }
01248
01249
01250 D.getInverse(*this);
01251 mult(N);
01252 for (k = 0; k < j; ++k)
01253 mult(*this);
01254 }
01255
01256
01257 template <class T>
01258 bool Matrix4<T>::setLogOf(const Matrix4 &matrix)
01259 {
01260 const int maxiter = 12;
01261 int k = 0;
01262 int i = 0;
01263 const T eps = 1e-12f;
01264 Matrix4 A(matrix),Z;
01265
01266
01267
01268 Z=A;
01269
01270 Z[M_00] -= 1.f;
01271 Z[M_11] -= 1.f;
01272 Z[M_22] -= 1.f;
01273 Z[M_33] -= 1.f;
01274
01275 while (Z.normInfinity() > 0.5f)
01276 {
01277 A.sqrt();
01278
01279
01280 Z=A;
01281 Z[M_00] -= 1.f;
01282 Z[M_11] -= 1.f;
01283 Z[M_22] -= 1.f;
01284 Z[M_33] -= 1.f;
01285 ++k;
01286 }
01287
01288 A[M_00] -= 1.f;
01289 A[M_11] -= 1.f;
01290 A[M_22] -= 1.f;
01291 A[M_33] -= 1.f;
01292 A.negate();
01293
01294 *this=A;
01295
01296 Z=A;
01297 i = 1;
01298 while (Z.normInfinity() > eps && i < maxiter)
01299 {
01300 Z.mult(A);
01301 ++i;
01302 addScaled(Z, 1.f / T(i));
01303 }
01304
01305 scale(-1.0f * T(1 << k));
01306
01307 return (i < maxiter);
01308 }
01309
01310
01311 template <class T>
01312 void Matrix4<T>::print(std::ostream &target)
01313 {
01314 target << matrix_[M_00] << ' ' << matrix_[M_01] << ' ' << matrix_[M_02] << ' ' << matrix_[M_03] << '\n'
01315 << matrix_[M_10] << ' ' << matrix_[M_11] << ' ' << matrix_[M_12] << ' ' << matrix_[M_13] << '\n'
01316 << matrix_[M_20] << ' ' << matrix_[M_21] << ' ' << matrix_[M_22] << ' ' << matrix_[M_23] << '\n'
01317 << matrix_[M_30] << ' ' << matrix_[M_31] << ' ' << matrix_[M_32] << ' ' << matrix_[M_33] << std::endl;
01318 }
01319
01320
01321 typedef Matrix4<double> Matrix4d;
01322
01323
01324 typedef Matrix4<float> Matrix4f;
01325
01326
01333 template <>
01334 class INSTANTIO_DLLMAPPING TypeName<Matrix4d>
01335 {
01336 public:
01337
01342 static const char *getName();
01343
01344 private:
01345
01347 TypeName();
01348
01350 TypeName(const TypeName &);
01351
01353 const TypeName &operator=(const TypeName &);
01354 };
01355
01356
01363 template <>
01364 class INSTANTIO_DLLMAPPING TypeName<Matrix4f>
01365 {
01366 public:
01367
01372 static const char *getName();
01373
01374 private:
01375
01377 TypeName();
01378
01380 TypeName(const TypeName &);
01381
01383 const TypeName &operator=(const TypeName &);
01384 };
01385
01386
01387 template <class T>
01388 std::ostream &operator <<(std::ostream &os, const Matrix4<T> &obj)
01389 {
01390 for (int i = 0; i < 4; ++i)
01391 {
01392 for (int j = 0; j < 4; ++j)
01393 os << obj[j * 4 + i] << ' ';
01394 os << std::endl;
01395 }
01396 return os;
01397 }
01398
01399
01400 template <class T>
01401 std::istream &operator >>(std::istream &is, Matrix4<T> &obj)
01402 {
01403 for (int i = 0; i < 4; ++i)
01404 for (int j = 0; j < 4; ++j)
01405 if (is.good())
01406 is >> obj[j * 4 + i];
01407 return is;
01408 }
01409
01410
01411 }
01412
01413
01414 #ifdef _MSC_VER
01415 # pragma warning (pop)
01416 #endif
01417
01418 #endif // __INSTANTIO_MATRIX4_H