From 7a63472ea29db5d7555016d98e6a45c48fbf3157 Mon Sep 17 00:00:00 2001 From: Vegard Storheil Eriksen Date: Thu, 8 Apr 2010 16:46:21 +0200 Subject: Reimplemented matrix inversion. --- hcoord.cpp | 45 ++++++++++++++++++++++++++++++++++++++++++--- 1 file changed, 42 insertions(+), 3 deletions(-) (limited to 'hcoord.cpp') 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 -#include 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; } -- cgit v1.2.3