Blender: MT_Matrix3x3.inl Source File

Blender V2.61 - r43446
00001 #include "MT_Optimize.h"
00002 
00003 GEN_INLINE MT_Quaternion MT_Matrix3x3::getRotation() const {
00004     static int  next[3] = { 1, 2, 0 };
00005 
00006     MT_Quaternion result;
00007    
00008     MT_Scalar trace = m_el[0][0] + m_el[1][1] + m_el[2][2];
00009     
00010     if (trace > 0.0) 
00011     {
00012         MT_Scalar s = sqrt(trace + MT_Scalar(1.0));
00013         result[3] = s * MT_Scalar(0.5);
00014         s = MT_Scalar(0.5) / s;
00015         
00016         result[0] = (m_el[2][1] - m_el[1][2]) * s;
00017         result[1] = (m_el[0][2] - m_el[2][0]) * s;
00018         result[2] = (m_el[1][0] - m_el[0][1]) * s;
00019     } 
00020     else 
00021     {
00022         int i = 0;
00023         if (m_el[1][1] > m_el[0][0])
00024             i = 1;
00025         if (m_el[2][2] > m_el[i][i])
00026             i = 2;
00027         
00028         int j = next[i];  
00029         int k = next[j];
00030         
00031         MT_Scalar s = sqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + MT_Scalar(1.0));
00032         
00033         result[i] = s * MT_Scalar(0.5);
00034         
00035         s = MT_Scalar(0.5) / s;
00036         
00037         result[3] = (m_el[k][j] - m_el[j][k]) * s;
00038         result[j] = (m_el[j][i] + m_el[i][j]) * s;
00039         result[k] = (m_el[k][i] + m_el[i][k]) * s;
00040     }
00041     return result;
00042 }
00043 
00044 GEN_INLINE MT_Matrix3x3& MT_Matrix3x3::operator*=(const MT_Matrix3x3& m) {
00045     setValue(m.tdot(0, m_el[0]), m.tdot(1, m_el[0]), m.tdot(2, m_el[0]),
00046              m.tdot(0, m_el[1]), m.tdot(1, m_el[1]), m.tdot(2, m_el[1]),
00047              m.tdot(0, m_el[2]), m.tdot(1, m_el[2]), m.tdot(2, m_el[2]));
00048     return *this;
00049 }
00050 
00051 GEN_INLINE MT_Scalar MT_Matrix3x3::determinant() const { 
00052     return MT_triple((*this)[0], (*this)[1], (*this)[2]);
00053 }
00054 
00055 GEN_INLINE MT_Matrix3x3 MT_Matrix3x3::absolute() const {
00056     return 
00057         MT_Matrix3x3(MT_abs(m_el[0][0]), MT_abs(m_el[0][1]), MT_abs(m_el[0][2]),
00058                      MT_abs(m_el[1][0]), MT_abs(m_el[1][1]), MT_abs(m_el[1][2]),
00059                      MT_abs(m_el[2][0]), MT_abs(m_el[2][1]), MT_abs(m_el[2][2]));
00060 }
00061 
00062 GEN_INLINE MT_Matrix3x3 MT_Matrix3x3::transposed() const {
00063     return MT_Matrix3x3(m_el[0][0], m_el[1][0], m_el[2][0],
00064                         m_el[0][1], m_el[1][1], m_el[2][1],
00065                         m_el[0][2], m_el[1][2], m_el[2][2]);
00066 }
00067 
00068 GEN_INLINE void MT_Matrix3x3::transpose() {
00069     *this = transposed();
00070 }
00071 
00072 GEN_INLINE MT_Matrix3x3 MT_Matrix3x3::adjoint() const {
00073     return 
00074         MT_Matrix3x3(cofac(1, 1, 2, 2), cofac(0, 2, 2, 1), cofac(0, 1, 1, 2),
00075                      cofac(1, 2, 2, 0), cofac(0, 0, 2, 2), cofac(0, 2, 1, 0),
00076                      cofac(1, 0, 2, 1), cofac(0, 1, 2, 0), cofac(0, 0, 1, 1));
00077 }
00078 
00079 GEN_INLINE MT_Matrix3x3 MT_Matrix3x3::inverse() const {
00080     MT_Vector3 co(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1));
00081     MT_Scalar det = MT_dot((*this)[0], co);
00082     MT_assert(!MT_fuzzyZero2(det));
00083     MT_Scalar s = MT_Scalar(1.0) / det;
00084     return 
00085         MT_Matrix3x3(co[0] * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s,
00086                      co[1] * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s,
00087                      co[2] * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s);
00088 }
00089 
00090 GEN_INLINE void MT_Matrix3x3::invert() {
00091     *this = inverse();
00092 }
00093 
00094 GEN_INLINE MT_Vector3 operator*(const MT_Matrix3x3& m, const MT_Vector3& v) {
00095     return MT_Vector3(MT_dot(m[0], v), MT_dot(m[1], v), MT_dot(m[2], v));
00096 }
00097 
00098 GEN_INLINE MT_Vector3 operator*(const MT_Vector3& v, const MT_Matrix3x3& m) {
00099     return MT_Vector3(m.tdot(0, v), m.tdot(1, v), m.tdot(2, v));
00100 }
00101 
00102 GEN_INLINE MT_Matrix3x3 operator*(const MT_Matrix3x3& m1, const MT_Matrix3x3& m2) {
00103     return 
00104         MT_Matrix3x3(m2.tdot(0, m1[0]), m2.tdot(1, m1[0]), m2.tdot(2, m1[0]),
00105                      m2.tdot(0, m1[1]), m2.tdot(1, m1[1]), m2.tdot(2, m1[1]),
00106                      m2.tdot(0, m1[2]), m2.tdot(1, m1[2]), m2.tdot(2, m1[2]));
00107 }
00108 
00109 GEN_INLINE MT_Matrix3x3 MT_multTransposeLeft(const MT_Matrix3x3& m1, const MT_Matrix3x3& m2) {
00110     return MT_Matrix3x3(
00111         m1[0][0] * m2[0][0] + m1[1][0] * m2[1][0] + m1[2][0] * m2[2][0],
00112         m1[0][0] * m2[0][1] + m1[1][0] * m2[1][1] + m1[2][0] * m2[2][1],
00113         m1[0][0] * m2[0][2] + m1[1][0] * m2[1][2] + m1[2][0] * m2[2][2],
00114         m1[0][1] * m2[0][0] + m1[1][1] * m2[1][0] + m1[2][1] * m2[2][0],
00115         m1[0][1] * m2[0][1] + m1[1][1] * m2[1][1] + m1[2][1] * m2[2][1],
00116         m1[0][1] * m2[0][2] + m1[1][1] * m2[1][2] + m1[2][1] * m2[2][2],
00117         m1[0][2] * m2[0][0] + m1[1][2] * m2[1][0] + m1[2][2] * m2[2][0],
00118         m1[0][2] * m2[0][1] + m1[1][2] * m2[1][1] + m1[2][2] * m2[2][1],
00119         m1[0][2] * m2[0][2] + m1[1][2] * m2[1][2] + m1[2][2] * m2[2][2]);
00120 }
00121 
00122 GEN_INLINE MT_Matrix3x3 MT_multTransposeRight(const MT_Matrix3x3& m1, const MT_Matrix3x3& m2) {
00123     return
00124         MT_Matrix3x3(m1[0].dot(m2[0]), m1[0].dot(m2[1]), m1[0].dot(m2[2]),
00125                      m1[1].dot(m2[0]), m1[1].dot(m2[1]), m1[1].dot(m2[2]),
00126                      m1[2].dot(m2[0]), m1[2].dot(m2[1]), m1[2].dot(m2[2]));
00127                      
00128 }