intern/moto/include/MT_Matrix3x3.inl

   1 #include "MT_Optimize.h"
   2
   3 GEN_INLINE MT_Quaternion MT_Matrix3x3::getRotation() const {
   4     static int  next[3] = { 1, 2, 0 };
   5
   6     MT_Quaternion result;
   7
   8     MT_Scalar trace = m_el[0][0] + m_el[1][1] + m_el[2][2];
   9
  10     if (trace > 0.0)
  11     {
  12         MT_Scalar s = sqrt(trace + MT_Scalar(1.0));
  13         result[3] = s * MT_Scalar(0.5);
  14         s = MT_Scalar(0.5) / s;
  15
  16         result[0] = (m_el[2][1] - m_el[1][2]) * s;
  17         result[1] = (m_el[0][2] - m_el[2][0]) * s;
  18         result[2] = (m_el[1][0] - m_el[0][1]) * s;
  19     }
  20     else
  21     {
  22         int i = 0;
  23         if (m_el[1][1] > m_el[0][0])
  24             i = 1;
  25         if (m_el[2][2] > m_el[i][i])
  26             i = 2;
  27
  28         int j = next[i];
  29         int k = next[j];
  30
  31         MT_Scalar s = sqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + MT_Scalar(1.0));
  32
  33         result[i] = s * MT_Scalar(0.5);
  34
  35         s = MT_Scalar(0.5) / s;
  36
  37         result[3] = (m_el[k][j] - m_el[j][k]) * s;
  38         result[j] = (m_el[j][i] + m_el[i][j]) * s;
  39         result[k] = (m_el[k][i] + m_el[i][k]) * s;
  40     }
  41     return result;
  42 }
  43
  44 GEN_INLINE MT_Matrix3x3& MT_Matrix3x3::operator*=(const MT_Matrix3x3& m) {
  45     setValue(m.tdot(0, m_el[0]), m.tdot(1, m_el[0]), m.tdot(2, m_el[0]),
  46              m.tdot(0, m_el[1]), m.tdot(1, m_el[1]), m.tdot(2, m_el[1]),
  47              m.tdot(0, m_el[2]), m.tdot(1, m_el[2]), m.tdot(2, m_el[2]));
  48     return *this;
  49 }
  50
  51 GEN_INLINE MT_Scalar MT_Matrix3x3::determinant() const {
  52     return MT_triple((*this)[0], (*this)[1], (*this)[2]);
  53 }
  54
  55 GEN_INLINE MT_Matrix3x3 MT_Matrix3x3::absolute() const {
  56     return
  57         MT_Matrix3x3(MT_abs(m_el[0][0]), MT_abs(m_el[0][1]), MT_abs(m_el[0][2]),
  58                      MT_abs(m_el[1][0]), MT_abs(m_el[1][1]), MT_abs(m_el[1][2]),
  59                      MT_abs(m_el[2][0]), MT_abs(m_el[2][1]), MT_abs(m_el[2][2]));
  60 }
  61
  62 GEN_INLINE MT_Matrix3x3 MT_Matrix3x3::transposed() const {
  63     return MT_Matrix3x3(m_el[0][0], m_el[1][0], m_el[2][0],
  64                         m_el[0][1], m_el[1][1], m_el[2][1],
  65                         m_el[0][2], m_el[1][2], m_el[2][2]);
  66 }
  67
  68 GEN_INLINE void MT_Matrix3x3::transpose() {
  69         *this = transposed();
  70 }
  71
  72 GEN_INLINE MT_Matrix3x3 MT_Matrix3x3::adjoint() const {
  73     return
  74         MT_Matrix3x3(cofac(1, 1, 2, 2), cofac(0, 2, 2, 1), cofac(0, 1, 1, 2),
  75                      cofac(1, 2, 2, 0), cofac(0, 0, 2, 2), cofac(0, 2, 1, 0),
  76                      cofac(1, 0, 2, 1), cofac(0, 1, 2, 0), cofac(0, 0, 1, 1));
  77 }
  78
  79 GEN_INLINE MT_Matrix3x3 MT_Matrix3x3::inverse() const {
  80     MT_Vector3 co(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1));
  81     MT_Scalar det = MT_dot((*this)[0], co);
  82     MT_assert(!MT_fuzzyZero2(det));
  83     MT_Scalar s = MT_Scalar(1.0) / det;
  84     return
  85         MT_Matrix3x3(co[0] * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s,
  86                      co[1] * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s,
  87                      co[2] * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s);
  88 }
  89
  90 GEN_INLINE void MT_Matrix3x3::invert() {
  91         *this = inverse();
  92 }
  93
  94 GEN_INLINE MT_Vector3 operator*(const MT_Matrix3x3& m, const MT_Vector3& v) {
  95     return MT_Vector3(MT_dot(m[0], v), MT_dot(m[1], v), MT_dot(m[2], v));
  96 }
  97
  98 GEN_INLINE MT_Vector3 operator*(const MT_Vector3& v, const MT_Matrix3x3& m) {
  99     return MT_Vector3(m.tdot(0, v), m.tdot(1, v), m.tdot(2, v));
 100 }
 101
 102 GEN_INLINE MT_Matrix3x3 operator*(const MT_Matrix3x3& m1, const MT_Matrix3x3& m2) {
 103     return
 104         MT_Matrix3x3(m2.tdot(0, m1[0]), m2.tdot(1, m1[0]), m2.tdot(2, m1[0]),
 105                      m2.tdot(0, m1[1]), m2.tdot(1, m1[1]), m2.tdot(2, m1[1]),
 106                      m2.tdot(0, m1[2]), m2.tdot(1, m1[2]), m2.tdot(2, m1[2]));
 107 }
 108
 109 GEN_INLINE MT_Matrix3x3 MT_multTransposeLeft(const MT_Matrix3x3& m1, const MT_Matrix3x3& m2) {
 110     return MT_Matrix3x3(
 111         m1[0][0] * m2[0][0] + m1[1][0] * m2[1][0] + m1[2][0] * m2[2][0],
 112         m1[0][0] * m2[0][1] + m1[1][0] * m2[1][1] + m1[2][0] * m2[2][1],
 113         m1[0][0] * m2[0][2] + m1[1][0] * m2[1][2] + m1[2][0] * m2[2][2],
 114         m1[0][1] * m2[0][0] + m1[1][1] * m2[1][0] + m1[2][1] * m2[2][0],
 115         m1[0][1] * m2[0][1] + m1[1][1] * m2[1][1] + m1[2][1] * m2[2][1],
 116         m1[0][1] * m2[0][2] + m1[1][1] * m2[1][2] + m1[2][1] * m2[2][2],
 117         m1[0][2] * m2[0][0] + m1[1][2] * m2[1][0] + m1[2][2] * m2[2][0],
 118         m1[0][2] * m2[0][1] + m1[1][2] * m2[1][1] + m1[2][2] * m2[2][1],
 119         m1[0][2] * m2[0][2] + m1[1][2] * m2[1][2] + m1[2][2] * m2[2][2]);
 120 }
 121
 122 GEN_INLINE MT_Matrix3x3 MT_multTransposeRight(const MT_Matrix3x3& m1, const MT_Matrix3x3& m2) {
 123     return
 124         MT_Matrix3x3(m1[0].dot(m2[0]), m1[0].dot(m2[1]), m1[0].dot(m2[2]),
 125                      m1[1].dot(m2[0]), m1[1].dot(m2[1]), m1[1].dot(m2[2]),
 126                      m1[2].dot(m2[0]), m1[2].dot(m2[1]), m1[2].dot(m2[2]));
 127
 128 }