tMatrix cMathUtil::InvRigidMat(const tMatrix& mat)
{
tMatrix inv_mat = tMatrix::Zero();
inv_mat.block(0, 0, 3, 3) = mat.block(0, 0, 3, 3).transpose();
inv_mat.col(3) = -inv_mat * mat.col(3);
inv_mat(3, 3) = 1;
return inv_mat;
}
int main(int argc, const char * argv[])
{
// insert code here...
std::cout << "Hello, World!\n";
std::cout << "In Matrix\n";
Matrix mat(3,3);
mat.unit_matrix();
// mat[1][1] = 1;
// mat[1][2] = 2;
// mat[2][1] = 3;
// mat[2][2] = 4;
mat[1][1] = 1.0;
mat[1][2] = 2.0;
mat[1][3] = 1.0;
mat[2][1] = 2.0;
mat[2][2] = 1.0;
mat[2][3] = 0.0;
mat[3][1] = 1.0;
mat[3][2] = 1.0;
mat[3][3] = 2.0;
for(int i=1; i <= mat.row_size(); i++){
for(int j=1; j <= mat.column_size(); j++){
printf(" %f",mat[i][j]);
}
printf("\n");
}
std::cout << "\nInverse Matrix\n";
Matrix inv_mat(mat.inverse());
for(int i=1; i <= inv_mat.row_size(); i++){
for(int j=1; j <= inv_mat.column_size(); j++){
printf(" %f",inv_mat[i][j]);
}
printf("\n");
}
std::cout << "\nIn Matrix\n";
mat = mat * inv_mat;
for(int i=1; i <= mat.row_size(); i++){
for(int j=1; j <= mat.column_size(); j++){
cout << setw(15) << mat[i][j];
}
printf("\n");
}
return 0;
}
