4 files changed, 28 insertions, 41 deletions

diff --git a/src/math/Matrix.cpp b/src/math/Matrix.cpp
index a8b1b182..3ac3e2b9 100644
--- a/src/math/Matrix.cpp
+++ b/src/math/Matrix.cpp
@@ -458,62 +458,39 @@ CMatrix &
 Invert(const CMatrix &src, CMatrix &dst)
 {
 	// TODO: VU0 code
-	// GTA handles this as a raw 4x4 orthonormal matrix
-	// and trashes the RW flags, let's not do that
 	float (*scr_fm)[4] = (float (*)[4])&src.m_matrix;
 	float (*dst_fm)[4] = (float (*)[4])&dst.m_matrix;
 
 	dst_fm[3][0] = dst_fm[3][1] = dst_fm[3][2] = 0.0f;
-#ifndef FIX_BUGS
-	dst_fm[3][3] = scr_fm[3][3];
-#endif
 
 	dst_fm[0][0] = scr_fm[0][0];
 	dst_fm[0][1] = scr_fm[1][0];
 	dst_fm[0][2] = scr_fm[2][0];
-#ifndef FIX_BUGS
-	dst_fm[0][3] = scr_fm[3][0];
-#endif
+
 	dst_fm[1][0] = scr_fm[0][1];
 	dst_fm[1][1] = scr_fm[1][1];
 	dst_fm[1][2] = scr_fm[2][1];
-#ifndef FIX_BUGS
-	dst_fm[1][3] = scr_fm[3][1];
-#endif
+
 	dst_fm[2][0] = scr_fm[0][2];
 	dst_fm[2][1] = scr_fm[1][2];
 	dst_fm[2][2] = scr_fm[2][2];
-#ifndef FIX_BUGS
-	dst_fm[2][3] = scr_fm[3][2];
-#endif
+
 
 	dst_fm[3][0] += dst_fm[0][0] * scr_fm[3][0];
 	dst_fm[3][1] += dst_fm[0][1] * scr_fm[3][0];
 	dst_fm[3][2] += dst_fm[0][2] * scr_fm[3][0];
-#ifndef FIX_BUGS
-	dst_fm[3][3] += dst_fm[0][3] * scr_fm[3][0];
-#endif
 
 	dst_fm[3][0] += dst_fm[1][0] * scr_fm[3][1];
 	dst_fm[3][1] += dst_fm[1][1] * scr_fm[3][1];
 	dst_fm[3][2] += dst_fm[1][2] * scr_fm[3][1];
-#ifndef FIX_BUGS
-	dst_fm[3][3] += dst_fm[1][3] * scr_fm[3][1];
-#endif
 
 	dst_fm[3][0] += dst_fm[2][0] * scr_fm[3][2];
 	dst_fm[3][1] += dst_fm[2][1] * scr_fm[3][2];
 	dst_fm[3][2] += dst_fm[2][2] * scr_fm[3][2];
-#ifndef FIX_BUGS
-	dst_fm[3][3] += dst_fm[2][3] * scr_fm[3][2];
-#endif
 
 	dst_fm[3][0] = -dst_fm[3][0];
 	dst_fm[3][1] = -dst_fm[3][1];
 	dst_fm[3][2] = -dst_fm[3][2];
-#ifndef FIX_BUGS
-	dst_fm[3][3] = scr_fm[3][3] - dst_fm[3][3];
-#endif
 
 	return dst;
 }
diff --git a/src/math/Matrix.h b/src/math/Matrix.h
index d8f6388d..d32b1d93 100644
--- a/src/math/Matrix.h
+++ b/src/math/Matrix.h
@@ -25,7 +25,8 @@ public:
 	CMatrix &operator+=(CMatrix const &rhs);
 	CMatrix &operator*=(CMatrix const &rhs);
 
-	CVector &GetPosition(void){ return *(CVector*)&m_matrix.pos; }
+	const CVector &GetPosition(void) const { return *(CVector*)&m_matrix.pos; }
+	CVector& GetPosition(void) { return *(CVector*)&m_matrix.pos; }
 	CVector &GetRight(void) { return *(CVector*)&m_matrix.right; }
 	CVector &GetForward(void) { return *(CVector*)&m_matrix.up; }
 	CVector &GetUp(void) { return *(CVector*)&m_matrix.at; }
@@ -44,13 +45,18 @@ public:
 	{
 		float *pFloatMatrix = (float*)&m_matrix;
 		for (int i = 0; i < 3; i++)
-#ifdef FIX_BUGS // BUGFIX from VC
 			for (int j = 0; j < 3; j++)
-#else
-			for (int j = 0; j < 4; j++)
-#endif
 				pFloatMatrix[i * 4 + j] *= scale;
 	}
+	void Scale(float sx, float sy, float sz)
+	{
+		float *pFloatMatrix = (float*)&m_matrix;
+		for (int i = 0; i < 3; i++){
+			pFloatMatrix[i * 4 + 0] *= sx;
+			pFloatMatrix[i * 4 + 1] *= sy;
+			pFloatMatrix[i * 4 + 2] *= sz;
+		}
+	}
 
 
 	void SetRotateXOnly(float angle);
@@ -85,6 +91,15 @@ public:
 	void CopyOnlyMatrix(CMatrix *other);
 	void SetUnity(void);
 	void ResetOrientation(void);
+	void CopyRwMatrix(RwMatrix *matrix){
+		m_matrix = *matrix;
+	}
+	
+	void CopyToRwMatrix(RwMatrix *matrix){
+		*matrix = m_matrix;
+		RwMatrixUpdate(matrix);
+	}
+	
 	void SetTranslateOnly(float x, float y, float z) {
 		m_matrix.pos.x = x;
 		m_matrix.pos.y = y;
diff --git a/src/math/Vector.h b/src/math/Vector.h
index 776bfcfe..02128454 100644
--- a/src/math/Vector.h
+++ b/src/math/Vector.h
@@ -64,11 +64,11 @@ public:
 		return CVector(-x, -y, -z);
 	}
 
-	const bool operator==(CVector const &right) {
+	const bool operator==(CVector const &right) const {
 		return x == right.x && y == right.y && z == right.z;
 	}
 
-	const bool operator!=(CVector const &right) {
+	const bool operator!=(CVector const &right) const {
 		return x != right.x || y != right.y || z != right.z;
 	}
 
diff --git a/src/math/Vector2D.h b/src/math/Vector2D.h
index 0235dbe5..deabd0b1 100644
--- a/src/math/Vector2D.h
+++ b/src/math/Vector2D.h
@@ -13,14 +13,6 @@ public:
 
 	void Normalise(void) {
 		float sq = MagnitudeSqr();
-		// assert(sq != 0.0f);	// just be safe here
-		float invsqrt = RecipSqrt(sq);
-		x *= invsqrt;
-		y *= invsqrt;
-	}
-
-	void NormaliseSafe(void) {
-		float sq = MagnitudeSqr();
 		if(sq > 0.0f){
 			float invsqrt = RecipSqrt(sq);
 			x *= invsqrt;
@@ -61,6 +53,9 @@ public:
 	CVector2D operator/(float t) const {
 		return CVector2D(x/t, y/t);
 	}
+	CVector2D operator-() const {
+		return CVector2D(-x, -y);
+	}
 };
 
 inline float