IceMatrix4x4.cpp
Go to the documentation of this file.
00001 
00002 
00008 
00009 
00011 
00035 
00036 
00038 // Precompiled Header
00039 #include "Stdafx.h"
00040 
00041 using namespace IceMaths;
00042 
00044 
00053 
00054 ICEMATHS_API void IceMaths::InvertPRMatrix(Matrix4x4& dest, const Matrix4x4& src)
00055 {
00056         dest.m[0][0] = src.m[0][0];
00057         dest.m[1][0] = src.m[0][1];
00058         dest.m[2][0] = src.m[0][2];
00059         dest.m[3][0] = -(src.m[3][0]*src.m[0][0] + src.m[3][1]*src.m[0][1] + src.m[3][2]*src.m[0][2]);
00060 
00061         dest.m[0][1] = src.m[1][0];
00062         dest.m[1][1] = src.m[1][1];
00063         dest.m[2][1] = src.m[1][2];
00064         dest.m[3][1] = -(src.m[3][0]*src.m[1][0] + src.m[3][1]*src.m[1][1] + src.m[3][2]*src.m[1][2]);
00065 
00066         dest.m[0][2] = src.m[2][0];
00067         dest.m[1][2] = src.m[2][1];
00068         dest.m[2][2] = src.m[2][2];
00069         dest.m[3][2] = -(src.m[3][0]*src.m[2][0] + src.m[3][1]*src.m[2][1] + src.m[3][2]*src.m[2][2]);
00070 
00071         dest.m[0][3] = 0.0f;
00072         dest.m[1][3] = 0.0f;
00073         dest.m[2][3] = 0.0f;
00074         dest.m[3][3] = 1.0f;
00075 }
00076 
00078 // Compute the cofactor of the Matrix at a specified location
00080 float Matrix4x4::CoFactor(udword row, udword col) const
00081 {
00082         return   (( m[(row+1)&3][(col+1)&3]*m[(row+2)&3][(col+2)&3]*m[(row+3)&3][(col+3)&3] +
00083                                 m[(row+1)&3][(col+2)&3]*m[(row+2)&3][(col+3)&3]*m[(row+3)&3][(col+1)&3] +
00084                                 m[(row+1)&3][(col+3)&3]*m[(row+2)&3][(col+1)&3]*m[(row+3)&3][(col+2)&3])
00085                         -  (m[(row+3)&3][(col+1)&3]*m[(row+2)&3][(col+2)&3]*m[(row+1)&3][(col+3)&3] +
00086                                 m[(row+3)&3][(col+2)&3]*m[(row+2)&3][(col+3)&3]*m[(row+1)&3][(col+1)&3] +
00087                                 m[(row+3)&3][(col+3)&3]*m[(row+2)&3][(col+1)&3]*m[(row+1)&3][(col+2)&3])) * ((row + col) & 1 ? -1.0f : +1.0f);
00088 }
00089 
00091 // Compute the determinant of the Matrix
00093 float Matrix4x4::Determinant() const
00094 {
00095         return  m[0][0] * CoFactor(0, 0) +
00096                         m[0][1] * CoFactor(0, 1) +
00097                         m[0][2] * CoFactor(0, 2) +
00098                         m[0][3] * CoFactor(0, 3);
00099 }
00100 
00102 // Compute the inverse of the matrix
00104 Matrix4x4& Matrix4x4::Invert()
00105 {
00106         float Det = Determinant();
00107         Matrix4x4 Temp;
00108 
00109         if(fabsf(Det) < MATRIX4X4_EPSILON)
00110                 return  *this;          // The matrix is not invertible! Singular case!
00111 
00112         float IDet = 1.0f / Det;
00113 
00114         Temp.m[0][0] = CoFactor(0,0) * IDet;
00115         Temp.m[1][0] = CoFactor(0,1) * IDet;
00116         Temp.m[2][0] = CoFactor(0,2) * IDet;
00117         Temp.m[3][0] = CoFactor(0,3) * IDet;
00118         Temp.m[0][1] = CoFactor(1,0) * IDet;
00119         Temp.m[1][1] = CoFactor(1,1) * IDet;
00120         Temp.m[2][1] = CoFactor(1,2) * IDet;
00121         Temp.m[3][1] = CoFactor(1,3) * IDet;
00122         Temp.m[0][2] = CoFactor(2,0) * IDet;
00123         Temp.m[1][2] = CoFactor(2,1) * IDet;
00124         Temp.m[2][2] = CoFactor(2,2) * IDet;
00125         Temp.m[3][2] = CoFactor(2,3) * IDet;
00126         Temp.m[0][3] = CoFactor(3,0) * IDet;
00127         Temp.m[1][3] = CoFactor(3,1) * IDet;
00128         Temp.m[2][3] = CoFactor(3,2) * IDet;
00129         Temp.m[3][3] = CoFactor(3,3) * IDet;
00130 
00131         *this = Temp;
00132 
00133         return  *this;
00134 }
00135 


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sun Apr 2 2017 03:43:54