TITANIC: Finished DVector class

6 files changed, 70 insertions, 29 deletions

diff --git a/engines/titanic/star_control/dmatrix.cpp b/engines/titanic/star_control/dmatrix.cpp
index 1808b13dc2..86ccd3d866 100644
--- a/engines/titanic/star_control/dmatrix.cpp
+++ b/engines/titanic/star_control/dmatrix.cpp
@@ -120,9 +120,9 @@ void DMatrix::fn3(CStarControlSub26 *sub26) {
 	error("TODO: DMatrix::fn3 %d", (int)v);
 }
 
-const DMatrix *DMatrix::fn4(DMatrix &dest, const DMatrix &m1, const DMatrix &m2) {
+DMatrix DMatrix::fn4(const DMatrix &m) {
 	// TODO
-	return nullptr;
+	return DMatrix();
 }
 
 } // End of namespace Titanic
diff --git a/engines/titanic/star_control/dmatrix.h b/engines/titanic/star_control/dmatrix.h
index c3490770fb..793abadffd 100644
--- a/engines/titanic/star_control/dmatrix.h
+++ b/engines/titanic/star_control/dmatrix.h
@@ -60,7 +60,7 @@ public:
 	void fn1(DMatrix &m);
 	void fn3(CStarControlSub26 *sub26);
 
-	const DMatrix *fn4(DMatrix &dest, const DMatrix &m1, const DMatrix &m2);
+	DMatrix fn4(const DMatrix &m);
 };
 
 } // End of namespace Titanic
diff --git a/engines/titanic/star_control/dvector.cpp b/engines/titanic/star_control/dvector.cpp
index 9cd610fc0a..17409c6811 100644
--- a/engines/titanic/star_control/dvector.cpp
+++ b/engines/titanic/star_control/dvector.cpp
@@ -21,17 +21,19 @@
  */
 
 #include "titanic/star_control/dvector.h"
+#include "titanic/star_control/dmatrix.h"
 #include "common/algorithm.h"
 
 namespace Titanic {
 
-void DVector::normalize() {
+double DVector::normalize() {
 	double hyp = sqrt(_x * _x + _y * _y + _z * _z);
 	assert(hyp);
 
 	_x *= 1.0 / hyp;
 	_y *= 1.0 / hyp;
 	_z *= 1.0 / hyp;
+	return hyp;
 }
 
 double DVector::getDistance(const DVector &src) {
@@ -39,25 +41,69 @@ double DVector::getDistance(const DVector &src) {
 }
 
 DVector *DVector::fn1(DVector &dest, const DMatrix &m) {
-	// TODO
-	return nullptr;
+	dest._x = m._row3._x * _z + m._row2._x * _y + _x * m._row1._x + m._row4._x;
+	dest._y = m._row2._y * _y + m._row3._y * _z + m._row1._y * _x + m._row4._z;
+	dest._z = m._row3._z * _z + m._row2._z * _y + m._row1._z * _x + m._row4._y;
+	return &dest;
 }
 
 void DVector::fn2(double val) {
-	// TODO
+	const double FACTOR = 2 * M_PI / 360.0;
+	double sinVal = sin(val * FACTOR);
+	double cosVal = cos(val * FACTOR);
+
+	_x = cosVal * _x - sinVal * _z;
+	_z = cosVal * _z + sinVal * _x;
 }
 
-void DVector::fn3(DVector &dest) {
-	// TODO
+DVector DVector::fn3() const {
+	DVector vector = *this;
+	DVector dest;
+	dest._x = vector.normalize();
+	dest._z = acos(vector._y);
+
+	if (ABS(vector._z) < 0.00001) {
+		if (vector._x < 0.0) {
+			dest._y = 2 * M_PI - (M_PI / 2.0);
+		} else {
+			dest._y = M_PI / 2.0;
+		}
+	} else {
+		dest._y = atan(vector._x / vector._z);
+		if (vector._x < 0.0)
+			dest._y += 2 * M_PI;
+	}
+
+	return dest;
 }
 
-const DMatrix *DVector::fn4(const DVector &v, DMatrix &m) {
-	// TODO
-	return nullptr;
+void DVector::fn4(const DVector &v, DMatrix &m) {
+	const double FACTOR = 180.0 / M_PI;
+	DMatrix matrix1, matrix2, matrix3, matrix4;
+	DMatrix dest;
+	DVector vector1 = fn3();
+
+	matrix1.setRotationMatrix(X_AXIS, vector1._y * FACTOR);
+	matrix2.setRotationMatrix(Y_AXIS, -(vector1._z * FACTOR));
+	matrix3 = matrix1.fn4(matrix2);
+	matrix3.fn1(matrix4);
+
+	vector1 = v.fn3();
+	matrix1.setRotationMatrix(X_AXIS, vector1._y * FACTOR);
+	matrix2.setRotationMatrix(Y_AXIS, -(vector1._z * FACTOR));
+	matrix3 = matrix1.fn4(matrix2);
+	matrix1.fn1(matrix4);
+
+	m = matrix4.fn4(matrix3);
 }
 
-void DVector::fn5(DMatrix &dest) {
-	// TODO
+DMatrix DVector::fn5() const {
+	const double FACTOR = 180.0 / M_PI;
+	DVector v1 = fn3();
+	DMatrix m1, m2;
+	m1.setRotationMatrix(X_AXIS, v1._y * FACTOR);
+	m2.setRotationMatrix(Y_AXIS, -(v1._z * FACTOR));
+	return m1.fn4(m2);
 }
 
 } // End of namespace Titanic
diff --git a/engines/titanic/star_control/dvector.h b/engines/titanic/star_control/dvector.h
index a447f253e1..fcb463b715 100644
--- a/engines/titanic/star_control/dvector.h
+++ b/engines/titanic/star_control/dvector.h
@@ -41,7 +41,7 @@ public:
 	DVector(double x, double y, double z) : _x(x), _y(y), _z(z) {}
 	DVector(const FVector &v) : _x(v._x), _y(v._y), _z(v._z) {}
 
-	void normalize();
+	double normalize();
 
 	/**
 	 * Returns the distance between this vector and the passed one
@@ -50,9 +50,9 @@ public:
 
 	DVector *fn1(DVector &dest, const DMatrix &m);
 	void fn2(double val);
-	void fn3(DVector &dest);
-	const DMatrix *fn4(const DVector &v, DMatrix &m);
-	void fn5(DMatrix &dest);
+	DVector fn3() const;
+	void fn4(const DVector &v, DMatrix &m);
+	DMatrix fn5() const;
 
 	/**
 	 * Returns true if the passed vector equals this one
diff --git a/engines/titanic/star_control/star_control_sub12.cpp b/engines/titanic/star_control/star_control_sub12.cpp
index 45cebec836..6b2f78c309 100644
--- a/engines/titanic/star_control/star_control_sub12.cpp
+++ b/engines/titanic/star_control/star_control_sub12.cpp
@@ -324,12 +324,10 @@ void CStarControlSub12::setViewportPosition(const FPoint &angles) {
 
 		tempV1 = _matrix._row2 - _matrix._row1;
 		diffV = tempV1;
-		diffV.fn5(m1);
-		sub.fn4(sub, m1, subX);
-		m1 = sub;
+		m1 = diffV.fn5();
+		m1 = m1.fn4(subX);
 		m1.fn1(subX);
-		subX.fn4(m2, subX, subY);
-		subX = m2;
+		subX = subX.fn4(subY);
 
 		FMatrix m3 = _sub13.getMatrix();
 		tempV2 = _sub13._position;
@@ -509,11 +507,9 @@ void CStarControlSub12::fn3(CStarControlSub13 *sub13, const FVector &v) {
 	DMatrix m2(0, tempV1);
 
 	tempV1 = v - _matrix._row1;
-	tempV1.fn5(m1);
+	m1 = tempV1.fn5();
 
-	DMatrix m3;
-	const DMatrix *m = m1.fn4(m3, m1, m2);
-	m1 = *m;
+	m1 = m1.fn4(m2);
 	m1.fn1(m2);
 
 	DVector tempV2 = _sub13._position;
diff --git a/engines/titanic/star_control/star_control_sub21.cpp b/engines/titanic/star_control/star_control_sub21.cpp
index a70460aa53..f1a9565ce6 100644
--- a/engines/titanic/star_control/star_control_sub21.cpp
+++ b/engines/titanic/star_control/star_control_sub21.cpp
@@ -46,9 +46,8 @@ void CStarControlSub21::proc10(const FVector &v1, const FVector &v2, const FVect
 	DVector vector2 = v2;
 	DMatrix matrix1, matrix2 = m, matrix3;
 	vector2.fn4(vector1, matrix1);
-	const DMatrix *matrixP = matrix1.fn4(matrix3, matrix1, matrix2);
+	FMatrix matrix4 = matrix1.fn4(matrix2);
 
-	FMatrix matrix4 = *matrixP;
 	_sub24.proc3(m, matrix4);
 	incLockCount();
 }