TITANIC: Duplicate DAffine usage in lockMarker2 with FPose usage

6 files changed, 126 insertions, 38 deletions

diff --git a/engines/titanic/star_control/daffine.cpp b/engines/titanic/star_control/daffine.cpp
index 283df9cbd2..0f176b65af 100644
--- a/engines/titanic/star_control/daffine.cpp
+++ b/engines/titanic/star_control/daffine.cpp
@@ -138,7 +138,7 @@ DAffine DAffine::inverseTransform() const {
 	double B[16]={};
 
 	// B contains inverse of A
-	matrix4Inverse(A,B);
+	matrix4Inverse<double>(A,B);
 
 	// Inverse of rotation matrix is the transpose
 	// While B contains the inverse of the rotation 
diff --git a/engines/titanic/star_control/fpose.cpp b/engines/titanic/star_control/fpose.cpp
index bb508228fe..a076cbb288 100644
--- a/engines/titanic/star_control/fpose.cpp
+++ b/engines/titanic/star_control/fpose.cpp
@@ -36,9 +36,9 @@ void fposeProd(const FPose &a, const FPose &m, FPose &C) {
 	C._row3._x = m._row1._x * a._row3._x + a._row3._z * m._row3._x + a._row3._y * m._row2._x;
 	C._row3._y = a._row3._y * m._row2._y + a._row3._z * m._row3._y + a._row3._x * m._row1._y;
 	C._row3._z = a._row3._x * m._row1._z + a._row3._y * m._row2._z + a._row3._z * m._row3._z;
-	C._vector._x = m._row1._x * a._vector._x + a._vector._y * m._row2._x + a._vector._z * m._row3._x + m._vector._x;
-	C._vector._y = a._vector._z * m._row3._y + a._vector._y * m._row2._y + a._vector._x * m._row1._y + a._vector._y;
-	C._vector._z = a._vector._y * m._row2._z + a._vector._z * m._row3._z + a._vector._x * m._row1._z + m._vector._z;
+	C._vector._x = a._vector._x * m._row1._x + a._vector._y * m._row2._x + a._vector._z * m._row3._x + m._vector._x;
+	C._vector._y = a._vector._x * m._row1._y + a._vector._y * m._row2._y + a._vector._z * m._row3._y + m._vector._y;
+	C._vector._z = a._vector._x * m._row1._z + a._vector._y * m._row2._z + a._vector._z * m._row3._z + m._vector._z;
 }
 
 // Member functions
@@ -59,6 +59,29 @@ FPose::FPose(const FPose &s1, const FPose &s2) {
 	fposeProd(s1, s2, *this);
 }
 
+FPose::FPose(int mode, const FVector &src) {
+	switch (mode) {
+	case 0:
+		_row1._x = 1.0;
+		_row2._y = 1.0;
+		_row3._z = 1.0;
+		_vector = src;
+		break;
+
+	case 1:
+		_row1._x = src._x;
+		_row2._y = src._y;
+		_row3._z = src._z;
+		break;
+
+	default:
+		_row1._x = 1.0;
+		_row2._y = 1.0;
+		_row3._z = 1.0;
+		break;
+	}
+}
+
 void FPose::identity() {
 	FMatrix::identity();
 	_vector.clear();
@@ -114,6 +137,13 @@ void FPose::setRotationMatrix(Axis axis, float amount) {
 	_vector.clear();
 }
 
+void FPose::rotVectAxisY(double angleDeg) {
+	_row1.rotVectAxisY(angleDeg);
+	_row2.rotVectAxisY(angleDeg);
+	_row3.rotVectAxisY(angleDeg);
+	_vector.rotVectAxisY(angleDeg);
+}
+
 void FPose::copyFrom(const FPose &src) {
 	_row1 = src._row1;
 	_row2 = src._row2;
@@ -153,4 +183,14 @@ FPose FPose::inverseTransform() const {
 	return result;
 }
 
+FPose FPose::compose2(const FPose &m) {
+	FPose dm;
+	dm._row1 = _row1.MatProdRowVect(m);
+	dm._row2 = _row2.MatProdRowVect(m);
+	dm._row3 = _row3.MatProdRowVect(m);
+	dm._vector = _vector.MatProdRowVect(m);
+
+	return dm;
+}
+
 } // End of namespace Titanic
diff --git a/engines/titanic/star_control/fpose.h b/engines/titanic/star_control/fpose.h
index 5287ff0516..7a1dc7e5e1 100644
--- a/engines/titanic/star_control/fpose.h
+++ b/engines/titanic/star_control/fpose.h
@@ -38,7 +38,7 @@ public:
 	FPose();
 	FPose(Axis axis, float amount);
 	FPose(const FPose &src);
-
+	FPose(int mode, const FVector &src);
 	/**
 	 * This fpose is the fpose product of s1 (on the left) and s2 (on the right)
 	 */
@@ -55,6 +55,10 @@ public:
 	void setRotationMatrix(Axis axis, float val);
 
 	/**
+	 * Rotate this FPose about the Y axis
+	 */
+	void rotVectAxisY(double angleDeg);
+	/**
 	 * Copy from the specified source pose
 	 */
 	void copyFrom(const FPose &src);
@@ -68,6 +72,8 @@ public:
 	 * The inverse of rotation and the position vector
 	 */
 	FPose inverseTransform() const;
+
+	FPose compose2(const FPose &m);
 };
 
 /**
diff --git a/engines/titanic/star_control/fvector.cpp b/engines/titanic/star_control/fvector.cpp
index 4f0fd7db68..5e87ebc7e9 100644
--- a/engines/titanic/star_control/fvector.cpp
+++ b/engines/titanic/star_control/fvector.cpp
@@ -150,6 +150,16 @@ DAffine FVector::formRotXY() const {
 	return m1.compose(m2);
 }
 
+FPose FVector::formRotXY2() const {
+	FVector v1 = getAnglesAsVect();
+	FPose m1, m2;
+	m1.setRotationMatrix(X_AXIS, v1._y * Rad2Deg);
+	m2.setRotationMatrix(Y_AXIS, v1._z * Rad2Deg);
+	FPose m3;
+	fposeProd(m1,m2,m3);
+	return m3;
+}
+
 Common::String FVector::toString() const {
 	return Common::String::format("(%.3f,%.3f,%.3f)", _x, _y, _z);
 }
diff --git a/engines/titanic/star_control/fvector.h b/engines/titanic/star_control/fvector.h
index 18270253c3..201d19e33a 100644
--- a/engines/titanic/star_control/fvector.h
+++ b/engines/titanic/star_control/fvector.h
@@ -124,6 +124,7 @@ public:
 	 * based on the orientation of this vector
 	 */
 	DAffine formRotXY() const;
+	FPose formRotXY2() const;
 
 	/**
 	 * Returns true if the passed vector equals this one
diff --git a/engines/titanic/star_control/star_camera.cpp b/engines/titanic/star_control/star_camera.cpp
index a2dfa3835f..38ce492ce5 100644
--- a/engines/titanic/star_control/star_camera.cpp
+++ b/engines/titanic/star_control/star_camera.cpp
@@ -27,6 +27,7 @@
 #include "titanic/star_control/fmatrix.h"
 #include "titanic/star_control/fpoint.h"
 #include "titanic/star_control/marked_camera_mover.h"
+#include "titanic/star_control/matrix_inv.h"
 #include "titanic/star_control/unmarked_camera_mover.h"
 #include "titanic/star_control/error_code.h"
 #include "titanic/support/simple_file.h"
@@ -319,7 +320,7 @@ void CStarCamera::setViewportAngle(const FPoint &angles) {
 
 	case TWO_LOCKED: {
 		FVector tempV2;
-		DAffine m1, m2, sub;
+		DAffine m1;
 		FVector mrow1, mrow2, mrow3;
 		FVector tempV1, diffV, multV, multV2, tempV3, tempV7;
 
@@ -518,17 +519,41 @@ bool CStarCamera::lockMarker2(CViewport *viewport, const FVector &secondStarPosi
 	_isInLockingProcess = true;
 	FVector firstStarPosition = _lockedStarsPos._row1;
 	DAffine m2(0, firstStarPosition); // Identity matrix and col4 as the 1st stars position
+	FPose m3(0, firstStarPosition); // Identity matrix and row4 as the 1st stars position
 	FVector starDelta = secondStarPosition - firstStarPosition;
 	DAffine m1 = starDelta.formRotXY();
+	FPose m10 = starDelta.formRotXY2();
+	FPose m11;
+	fposeProd(m10,m3,m11);
 	m1 = m1.compose(m2);
 	m2 = m1.inverseTransform();
 	
+	float A[16]={m11._row1._x,m11._row1._y,m11._row1._z, 0.0,
+				 m11._row2._x,m11._row2._y,m11._row2._z, 0.0,
+				 m11._row3._x,m11._row3._y,m11._row3._z, 0.0,
+				 m11._vector._x,m11._vector._y,m11._vector._z, 1.0};
+	// Inverse matrix
+	float B[16]={};
+
+	// B contains inverse of A
+	matrix4Inverse<float>(A,B);
+	m10=m11.inverseTransform();
+	m10._vector._x=B[12];
+	m10._vector._y=B[13];
+	m10._vector._z=B[14];
+
 	FVector oldPos = _viewport._position;
-	DAffine m4;
-	m4._col1 = viewport->_position;
-	m4._col2 = FVector(0.0, 0.0, 0.0);
-	m4._col3 = FVector(0.0, 0.0, 0.0);
-	m4._col4 = FVector(0.0, 0.0, 0.0);
+	DAffine m5;
+	m5._col1 = viewport->_position;
+	m5._col2 = FVector(0.0, 0.0, 0.0);
+	m5._col3 = FVector(0.0, 0.0, 0.0);
+	m5._col4 = FVector(0.0, 0.0, 0.0);
+
+	FPose m4;
+	m4._row1 = viewport->_position;
+	m4._row2 = FVector(0.0, 0.0, 0.0);
+	m4._row3 = FVector(0.0, 0.0, 0.0);
+	m4._vector = FVector(0.0, 0.0, 0.0);
 
 	FMatrix newOr = viewport->getOrientation();
 	float yVal1 = newOr._row1._y * rowScale2;
@@ -536,58 +561,64 @@ bool CStarCamera::lockMarker2(CViewport *viewport, const FVector &secondStarPosi
 	float xVal1 = newOr._row2._x * rowScale2;
 	float yVal2 = newOr._row2._y * rowScale2;
 	float zVal2 = newOr._row2._z * rowScale2;
-	float zVal3 = zVal1 + m4._col1._z;
-	float yVal3 = yVal1 + m4._col1._y;
-	float xVal2 = newOr._row1._x * rowScale2 + m4._col1._x;
-	float zVal4 = zVal2 + m4._col1._z;
-	float yVal4 = yVal2 + m4._col1._y;
-	float xVal3 = xVal1 + m4._col1._x;
+	float zVal3 = zVal1 + m4._row1._z;
+	float yVal3 = yVal1 + m4._row1._y;
+	float xVal2 = newOr._row1._x * rowScale2 + m4._row1._x;
+	float zVal4 = zVal2 + m4._row1._z;
+	float yVal4 = yVal2 + m4._row1._y;
+	float xVal3 = xVal1 + m4._row1._x;
 
 	FVector tempV4(xVal2, yVal3, zVal3);
 	FVector tempV3(xVal3, yVal4, zVal4);
-	m4._col3 = tempV4;
+	m4._row3 = tempV4;
 
 	FVector tempV5;
 	tempV5._x = newOr._row3._x * rowScale2;
 	tempV5._y = newOr._row3._y * rowScale2;
-	m4._col2 = tempV3;
+	m4._row2 = tempV3;
 
-	tempV3._x = tempV5._x + m4._col1._x;
-	tempV3._y = tempV5._y + m4._col1._y;
-	tempV3._z = newOr._row3._z * rowScale2 + m4._col1._z;
-	m4._col4 = tempV3;
+	tempV3._x = tempV5._x + m4._row1._x;
+	tempV3._y = tempV5._y + m4._row1._y;
+	tempV3._z = newOr._row3._z * rowScale2 + m4._row1._z;
+	m4._vector = tempV3;
 
 
 	FVector viewPosition = oldPos.MatProdColVect(m2);
-	m4 = m4.compose2(m2);
+	FVector viewPosition2 = oldPos.MatProdRowVect(m10);
+	//m4 = m4.compose2(m2);
+	//fposeProd(m4,m10,m3);
+	m3 = m4.compose2(m10);
 
 	float minDistance;
-	FVector x1(viewPosition);
-	FVector x2(m4._col1);
-	// Find the angle of rotation for m4._col1 that gives the minimum distance to viewPosition
+	FVector x1(viewPosition2);
+	FVector x2(m3._row1);
+	// Find the angle of rotation for m4._row1 that gives the minimum distance to viewPosition
 	float minDegree = calcAngleForMinDist(x1,x2,minDistance);
 
-	m4.rotVectAxisY((double)minDegree);
-	m4 = m4.compose2(m1);
+	m3.rotVectAxisY((double)minDegree);
+	//m4 = m4.compose2(m1);
+	FPose m13;
+	//fposeProd(m3,m11,m13);
+	m13 = m3.compose2(m11);
 
-	m4._col3 -= m4._col1;
-	m4._col2 -= m4._col1;
-	m4._col4 -= m4._col1;
+	m13._row3 -= m13._row1;
+	m13._row2 -= m13._row1;
+	m13._vector -= m13._row1;
 
 
 
-	double unusedScale=0.0;
-	if (!m4._col2.normalize(unusedScale) ||
-			!m4._col3.normalize(unusedScale) ||
-			!m4._col4.normalize(unusedScale) ) {
+	float unusedScale=0.0;
+	if (!m13._row2.normalize(unusedScale) ||
+			!m13._row3.normalize(unusedScale) ||
+			!m13._vector.normalize(unusedScale) ) {
 		// Do the normalizations, put the scale amount in unusedScale,
 		// but if any of the normalizations are unsuccessful, crash
 		assert(unusedScale);
 	}
 
-	newOr.set(m4._col3, m4._col2, m4._col4);
+	newOr.set(m13._row3, m13._row2, m13._vector);
 
-	FVector newPos = m4._col1;
+	FVector newPos = m13._row1;
 	FMatrix oldOr = _viewport.getOrientation();
 
 	// WORKAROUND: set old position to new position (1st argument), this prevents