TITANIC: Add 4x4 inverse to Daffine and use it

Very important for StarCamera:lockMarker2 is an inverse of the
difference between locked star1 and about to be star2.

Before it was calculating the col4 values by doing a
new col4 = -inv(R)*col4. col4 represents the x,y,z
position of the vector. This calculation is not correct
in the most general sense and is only valid for a single
rotation and translation.

For any more than one rotation and translation the upper left
3x3 inverse is still the transpose of the previous 3x3
portion there since that is just the rotation part, but the
translation part is now R2T1 + T2, which can't be undone by
simply multiplying by the inverse of R2. This gets more complicated
for lots of rotations so I've added a general 4x4 inverse
calculation and just pulled of the column 4 values.

The inverse implementation I used was from the mesa 3d library and
that has an MIT license so its okay to use in GPL.

2 files changed, 210 insertions, 10 deletions

diff --git a/engines/titanic/star_control/daffine.cpp b/engines/titanic/star_control/daffine.cpp
index 5b935838fa..616042d715 100644
--- a/engines/titanic/star_control/daffine.cpp
+++ b/engines/titanic/star_control/daffine.cpp
@@ -22,6 +22,7 @@
 
 #include "titanic/star_control/daffine.h"
 #include "titanic/star_control/fmatrix.h" // includes FVector
+#include "titanic/star_control/matrix_inv.h"
 #include "titanic/star_control/matrix_transform.h"
 
 namespace Titanic {
@@ -115,11 +116,26 @@ void DAffine::setRotationMatrix(Axis axis, double angleDeg) {
 	}
 }
 
-//TODO: Check column 4 math
 DAffine DAffine::inverseTransform() const {
 	DAffine m;
 
+	// Create a 4x4 matrix so that the column 4
+	// for the inverse can be obtained,  
+	// it is not simply -inv(R)*_col4
+	// Load input matrix
+	double A[16]={_col1._x,_col1._y,_col1._z, 0.0,
+				  _col2._x,_col2._y,_col2._z, 0.0,
+				  _col3._x,_col3._y,_col3._z, 0.0,
+				  _col4._x,_col4._y,_col4._z, 1.0};
+	// Inverse matrix
+	double B[16]={};
+
+	// B contains inverse of A
+	matrix4Inverse(A,B);
+
 	// Inverse of rotation matrix is the transpose
+	// While B contains the inverse of the rotation 
+	// this method is more numerically accurate
 	m._col1._x = _col1._x;
 	m._col2._x = _col1._y;
 	m._col3._x = _col1._z;
@@ -130,15 +146,10 @@ DAffine DAffine::inverseTransform() const {
 	m._col2._z = _col3._y;
 	m._col3._z = _col3._z;
 
-	m._col4._x = -(_col4._x * m._col1._x
-		+ _col4._y * m._col2._x
-		+ _col4._z * m._col3._x);
-	m._col4._y = -(_col4._x * m._col1._y
-		+ _col4._y * m._col2._y
-		+ _col4._z * m._col3._y);
-	m._col4._z = -(_col4._x * m._col1._z
-		+ _col4._y * m._col2._z
-		+ _col4._z * m._col3._z);
+	m._col4._x = B[12];
+	m._col4._y = B[13];
+	m._col4._z = B[14];
+
 	return m;
 }
 
diff --git a/engines/titanic/star_control/matrix_inv.h b/engines/titanic/star_control/matrix_inv.h
new file mode 100644
index 0000000000..21eb1d3a6f
--- /dev/null
+++ b/engines/titanic/star_control/matrix_inv.h
@@ -0,0 +1,189 @@
+/* ScummVM - Graphic Adventure Engine
+ *
+ * ScummVM is the legal property of its developers, whose names
+ * are too numerous to list here. Please refer to the COPYRIGHT
+ * file distributed with this source distribution.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+ *
+ */
+
+/*
+* Files matrix_inv.h are a part of the MESA 3D Library (MIT License)
+*
+* Copyright (C) 1999-2007  Brian Paul   All Rights Reserved.
+*
+* Permission is hereby granted, free of charge, to any person obtaining a
+* copy of this software and associated documentation files (the "Software"),
+* to deal in the Software without restriction, including without limitation
+* the rights to use, copy, modify, merge, publish, distribute, sublicense,
+* and/or sell copies of the Software, and to permit persons to whom the
+* Software is furnished to do so, subject to the following conditions:
+*
+* The above copyright notice and this permission notice shall be included
+* in all copies or substantial portions of the Software.
+*
+* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
+* OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+* THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+* SOFTWARE.
+*
+*/
+
+#ifndef TITANIC_MATRIX_INV_H
+#define TITANIC_MATRIX_INV_H
+
+namespace Titanic {
+
+// 4x4 Matrix m is column major, e.x., m[3] is the row 4 column 1 element
+// Modified version of MESA 3D library function (MIT license)
+bool matrix4Inverse(const double m[16], double invOut[16])
+{
+    double temp_inv[16]; 
+    double determinant;
+    double determinant_inv;
+    int i;
+
+    temp_inv[0] = m[5]  * m[10] * m[15] - 
+             m[5]  * m[11] * m[14] - 
+             m[9]  * m[6]  * m[15] + 
+             m[9]  * m[7]  * m[14] +
+             m[13] * m[6]  * m[11] - 
+             m[13] * m[7]  * m[10];
+
+    temp_inv[4] = -m[4]  * m[10] * m[15] + 
+              m[4]  * m[11] * m[14] + 
+              m[8]  * m[6]  * m[15] - 
+              m[8]  * m[7]  * m[14] - 
+              m[12] * m[6]  * m[11] + 
+              m[12] * m[7]  * m[10];
+
+    temp_inv[8] = m[4]  * m[9] * m[15] - 
+             m[4]  * m[11] * m[13] - 
+             m[8]  * m[5] * m[15] + 
+             m[8]  * m[7] * m[13] + 
+             m[12] * m[5] * m[11] - 
+             m[12] * m[7] * m[9];
+
+    temp_inv[12] = -m[4]  * m[9] * m[14] + 
+               m[4]  * m[10] * m[13] +
+               m[8]  * m[5] * m[14] - 
+               m[8]  * m[6] * m[13] - 
+               m[12] * m[5] * m[10] + 
+               m[12] * m[6] * m[9];
+
+    temp_inv[1] = -m[1]  * m[10] * m[15] + 
+              m[1]  * m[11] * m[14] + 
+              m[9]  * m[2] * m[15] - 
+              m[9]  * m[3] * m[14] - 
+              m[13] * m[2] * m[11] + 
+              m[13] * m[3] * m[10];
+
+    temp_inv[5] = m[0]  * m[10] * m[15] - 
+             m[0]  * m[11] * m[14] - 
+             m[8]  * m[2] * m[15] + 
+             m[8]  * m[3] * m[14] + 
+             m[12] * m[2] * m[11] - 
+             m[12] * m[3] * m[10];
+
+    temp_inv[9] = -m[0]  * m[9] * m[15] + 
+              m[0]  * m[11] * m[13] + 
+              m[8]  * m[1] * m[15] - 
+              m[8]  * m[3] * m[13] - 
+              m[12] * m[1] * m[11] + 
+              m[12] * m[3] * m[9];
+
+    temp_inv[13] = m[0]  * m[9] * m[14] - 
+              m[0]  * m[10] * m[13] - 
+              m[8]  * m[1] * m[14] + 
+              m[8]  * m[2] * m[13] + 
+              m[12] * m[1] * m[10] - 
+              m[12] * m[2] * m[9];
+
+    temp_inv[2] = m[1]  * m[6] * m[15] - 
+             m[1]  * m[7] * m[14] - 
+             m[5]  * m[2] * m[15] + 
+             m[5]  * m[3] * m[14] + 
+             m[13] * m[2] * m[7] - 
+             m[13] * m[3] * m[6];
+
+    temp_inv[6] = -m[0]  * m[6] * m[15] + 
+              m[0]  * m[7] * m[14] + 
+              m[4]  * m[2] * m[15] - 
+              m[4]  * m[3] * m[14] - 
+              m[12] * m[2] * m[7] + 
+              m[12] * m[3] * m[6];
+
+    temp_inv[10] = m[0]  * m[5] * m[15] - 
+              m[0]  * m[7] * m[13] - 
+              m[4]  * m[1] * m[15] + 
+              m[4]  * m[3] * m[13] + 
+              m[12] * m[1] * m[7] - 
+              m[12] * m[3] * m[5];
+
+    temp_inv[14] = -m[0]  * m[5] * m[14] + 
+               m[0]  * m[6] * m[13] + 
+               m[4]  * m[1] * m[14] - 
+               m[4]  * m[2] * m[13] - 
+               m[12] * m[1] * m[6] + 
+               m[12] * m[2] * m[5];
+
+    temp_inv[3] = -m[1] * m[6] * m[11] + 
+              m[1] * m[7] * m[10] + 
+              m[5] * m[2] * m[11] - 
+              m[5] * m[3] * m[10] - 
+              m[9] * m[2] * m[7] + 
+              m[9] * m[3] * m[6];
+
+    temp_inv[7] = m[0] * m[6] * m[11] - 
+             m[0] * m[7] * m[10] - 
+             m[4] * m[2] * m[11] + 
+             m[4] * m[3] * m[10] + 
+             m[8] * m[2] * m[7] - 
+             m[8] * m[3] * m[6];
+
+    temp_inv[11] = -m[0] * m[5] * m[11] + 
+               m[0] * m[7] * m[9] + 
+               m[4] * m[1] * m[11] - 
+               m[4] * m[3] * m[9] - 
+               m[8] * m[1] * m[7] + 
+               m[8] * m[3] * m[5];
+
+    temp_inv[15] = m[0] * m[5] * m[10] - 
+              m[0] * m[6] * m[9] - 
+              m[4] * m[1] * m[10] + 
+              m[4] * m[2] * m[9] + 
+              m[8] * m[1] * m[6] - 
+              m[8] * m[2] * m[5];
+
+    determinant = m[0] * temp_inv[0] + m[1] * temp_inv[4] + m[2] * temp_inv[8] + m[3] * temp_inv[12];
+
+    if (determinant == 0)
+        return false;
+
+    determinant_inv = 1.0 / determinant;
+
+    for (i = 0; i < 16; i++)
+        invOut[i] = temp_inv[i] * determinant_inv;
+
+    return true;
+}
+
+} // End of namespace Titanic
+
+#endif /* TITANIC_MATRIX_INV_H */