4 files changed, 45 insertions, 40 deletions

diff --git a/src/math/Matrix.cpp b/src/math/Matrix.cpp
index b11e8a1c..c0d909cb 100644
--- a/src/math/Matrix.cpp
+++ b/src/math/Matrix.cpp
@@ -453,63 +453,50 @@ 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
 	dst.f[3][0] = dst.f[3][1] = dst.f[3][2] = 0.0f;
-#ifndef FIX_BUGS
-	dst.f[3][3] = src.f[3][3];
-#endif
 
 	dst.f[0][0] = src.f[0][0];
 	dst.f[0][1] = src.f[1][0];
 	dst.f[0][2] = src.f[2][0];
-#ifndef FIX_BUGS
-	dst.f[0][3] = src.f[3][0];
-#endif
+
 	dst.f[1][0] = src.f[0][1];
 	dst.f[1][1] = src.f[1][1];
 	dst.f[1][2] = src.f[2][1];
-#ifndef FIX_BUGS
-	dst.f[1][3] = src.f[3][1];
-#endif
+
 	dst.f[2][0] = src.f[0][2];
 	dst.f[2][1] = src.f[1][2];
 	dst.f[2][2] = src.f[2][2];
-#ifndef FIX_BUGS
-	dst.f[2][3] = src.f[3][2];
-#endif
+
 
 	dst.f[3][0] += dst.f[0][0] * src.f[3][0];
 	dst.f[3][1] += dst.f[0][1] * src.f[3][0];
 	dst.f[3][2] += dst.f[0][2] * src.f[3][0];
-#ifndef FIX_BUGS
-	dst.f[3][3] += dst.f[0][3] * src.f[3][0];
-#endif
 
 	dst.f[3][0] += dst.f[1][0] * src.f[3][1];
 	dst.f[3][1] += dst.f[1][1] * src.f[3][1];
 	dst.f[3][2] += dst.f[1][2] * src.f[3][1];
-#ifndef FIX_BUGS
-	dst.f[3][3] += dst.f[1][3] * src.f[3][1];
-#endif
 
 	dst.f[3][0] += dst.f[2][0] * src.f[3][2];
 	dst.f[3][1] += dst.f[2][1] * src.f[3][2];
 	dst.f[3][2] += dst.f[2][2] * src.f[3][2];
-#ifndef FIX_BUGS
-	dst.f[3][3] += dst.f[2][3] * src.f[3][2];
-#endif
 
 	dst.f[3][0] = -dst.f[3][0];
 	dst.f[3][1] = -dst.f[3][1];
 	dst.f[3][2] = -dst.f[3][2];
-#ifndef FIX_BUGS
-	dst.f[3][3] = src.f[3][3] - dst.f[3][3];
-#endif
 
 	return dst;
 }
 
+void
+CMatrix::CopyToRwMatrix(RwMatrix* matrix)
+{
+	matrix->right = GetRight();
+	matrix->up = GetForward();
+	matrix->at = GetUp();
+	matrix->pos = GetPosition();
+	RwMatrixUpdate(matrix);
+}
+
 CMatrix
 Invert(const CMatrix &matrix)
 {
diff --git a/src/math/Matrix.h b/src/math/Matrix.h
index 6404b506..0adcf32c 100644
--- a/src/math/Matrix.h
+++ b/src/math/Matrix.h
@@ -3,6 +3,21 @@
 class CMatrix
 {
 public:
+#ifdef GTA_PS2
+	union
+	{
+		float f[4][4];
+		struct
+		{
+			float rx, ry, rz;
+			RwMatrix *m_attachment;
+			float fx, fy, fz;
+			bool m_hasRwMatrix;	// are we the owner?
+			float ux, uy, uz, uw;
+			float px, py, pz, pw;
+		};
+	};
+#else
 	union
 	{
 		float f[4][4];
@@ -17,6 +32,7 @@ public:
 
 	RwMatrix *m_attachment;
 	bool m_hasRwMatrix;	// are we the owner?
+#endif
 
 	CMatrix(void);
 	CMatrix(CMatrix const &m);
@@ -60,13 +76,17 @@ public:
 	void Scale(float scale)
 	{
 		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
 				f[i][j] *= scale;
 	}
+	void Scale(float sx, float sy, float sz)
+	{
+		for (int i = 0; i < 3; i++){
+			f[i][0] *= sx;
+			f[i][1] *= sy;
+			f[i][2] *= sz;
+		}
+	}
 
 
 	void SetRotateXOnly(float angle);
@@ -85,6 +105,9 @@ public:
 	void CopyOnlyMatrix(const CMatrix &other);
 	void SetUnity(void);
 	void ResetOrientation(void);
+	
+	void CopyToRwMatrix(RwMatrix* matrix);
+	
 	void SetTranslateOnly(float x, float y, float z) {
 		px = x;
 		py = 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