Reimplemented matrix inversion.

3 files changed, 45 insertions, 5 deletions

diff --git a/assimp.cpp b/assimp.cpp
index e50532d..828baf3 100755
--- a/assimp.cpp
+++ b/assimp.cpp
@@ -105,7 +105,7 @@ class FiskPattern : public Pattern {
 				if(patt_id2 == marker_info[j].id) {
 					arGetTransMat(&marker_info[j], patt_center, patt_width, m2_mat.m);
 					m2_visible = true;
-					m2_vec = m1_mat.inverse() * m2_mat.getTranslation();
+					m2_vec = m1_mat.invtr() * m2_mat.getTranslation();
 				}
 			}
 			
diff --git a/hcoord.cpp b/hcoord.cpp
index 86e9f26..b7c3322 100644
--- a/hcoord.cpp
+++ b/hcoord.cpp
@@ -1,6 +1,5 @@
 #include "hcoord.h"
 #include <SDL/SDL_opengl.h>
-#include <AR/ar.h>
 
 HVector::HVector() {
 	w = 1.0;
@@ -18,9 +17,49 @@ HMatrix::HMatrix() {
 	m[3][3] = 1.0;
 }
 
-HMatrix HMatrix::inverse() {
+HMatrix HMatrix::invtr() {
 	HMatrix res;
-	arUtilMatInv(m, res.m);
+	
+	res.m[0][0] = m[0][0];
+	res.m[0][1] = m[1][0];
+	res.m[0][2] = m[2][0];
+	
+	res.m[1][0] = m[0][1];
+	res.m[1][1] = m[1][1];
+	res.m[1][2] = m[2][1];
+	
+	res.m[2][0] = m[0][2];
+	res.m[2][1] = m[1][2];
+	res.m[2][2] = m[2][2];
+	
+	res.m[0][3] = - res.m[0][0] * m[0][3] - res.m[0][1] * m[1][3] - res.m[0][2] * m[2][3];
+	res.m[1][3] = - res.m[1][0] * m[0][3] - res.m[1][1] * m[1][3] - res.m[1][2] * m[2][3];
+	res.m[2][3] = - res.m[2][0] * m[0][3] - res.m[2][1] * m[1][3] - res.m[2][2] * m[2][3];
+	
+	return res;
+}
+
+HMatrix HMatrix::invtrs() {
+	HMatrix res;
+	
+	double s = 1 / (m[0][0] * m[0][0] + m[0][1] * m[0][1] + m[0][2] * m[0][2]);
+	
+	res.m[0][0] = s * m[0][0];
+	res.m[0][1] = s * m[1][0];
+	res.m[0][2] = s * m[2][0];
+	
+	res.m[1][0] = s * m[0][1];
+	res.m[1][1] = s * m[1][1];
+	res.m[1][2] = s * m[2][1];
+	
+	res.m[2][0] = s * m[0][2];
+	res.m[2][1] = s * m[1][2];
+	res.m[2][2] = s * m[2][2];
+	
+	res.m[0][3] = - res.m[0][0] * m[0][3] - res.m[0][1] * m[1][3] - res.m[0][2] * m[2][3];
+	res.m[1][3] = - res.m[1][0] * m[0][3] - res.m[1][1] * m[1][3] - res.m[1][2] * m[2][3];
+	res.m[2][3] = - res.m[2][0] * m[0][3] - res.m[2][1] * m[1][3] - res.m[2][2] * m[2][3];
+	
 	return res;
 }
 
diff --git a/hcoord.h b/hcoord.h
index 075d180..b7ddbd7 100644
--- a/hcoord.h
+++ b/hcoord.h
@@ -16,7 +16,8 @@ class HMatrix {
 		
 		HMatrix();
 		
-		HMatrix inverse();
+		HMatrix invtr();
+		HMatrix invtrs();
 		
 		HVector operator*(const HVector& vec);