3 files changed, 30 insertions, 21 deletions
diff --git a/src/math/Matrix.h b/src/math/Matrix.h
index e2d6b0e0..73870fe5 100644
--- a/src/math/Matrix.h
+++ b/src/math/Matrix.h
@@ -29,6 +29,15 @@ public:
 		if(m_hasRwMatrix && m_attachment)
 			RwMatrixDestroy(m_attachment);
 	}
+#ifdef RWCORE_H
+	operator RwMatrix (void) const {
+		return m_matrix;
+	}
+	
+	operator RwMatrix *(void) {
+		return &m_matrix;
+	}
+#endif
 	void Attach(RwMatrix *matrix, bool owner = false){
 #ifdef FIX_BUGS
 		if(m_attachment && m_hasRwMatrix)
@@ -83,7 +92,8 @@ public:
 	}
 	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; }
@@ -134,13 +144,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){
@@ -235,6 +250,15 @@ public:
 	void CopyOnlyMatrix(CMatrix *other){
 		m_matrix = other->m_matrix;
 	}
+	void CopyRwMatrix(RwMatrix *matrix){
+		m_matrix = *matrix;
+	}
+	
+	void CopyToRwMatrix(RwMatrix *matrix){
+		*matrix = m_matrix;
+		RwMatrixUpdate(matrix);
+	}
+	
 	void SetUnity(void) {
 		m_matrix.right.x = 1.0f;
 		m_matrix.right.y = 0.0f;
diff --git a/src/math/Vector2D.h b/src/math/Vector2D.h
index 0885a5d2..7bfccae6 100644
--- a/src/math/Vector2D.h
+++ b/src/math/Vector2D.h
@@ -11,9 +11,7 @@ public:
 	float Magnitude(void) const { return Sqrt(x*x + y*y); }
 	float MagnitudeSqr(void) const { return x*x + y*y; }
 
-	void Normalise(void);
-
-	void NormaliseSafe(void) {
+	void Normalise(void) {
 		float sq = MagnitudeSqr();
 		if(sq > 0.0f){
 			float invsqrt = RecipSqrt(sq);
diff --git a/src/math/math.cpp b/src/math/math.cpp
index 4792fdd0..d231b147 100644
--- a/src/math/math.cpp
+++ b/src/math/math.cpp
@@ -120,19 +120,6 @@ void TransformPoints(CVuVector *out, int n, const CMatrix &mat, const CVuVector
 
 
 void
-CVector2D::Normalise(void)
-{
-	float sq = MagnitudeSqr();
-	assert(sq != 0.0f);	// just be safe here
-	//if(sq > 0.0f){
-		float invsqrt = RecipSqrt(sq);
-		x *= invsqrt;
-		y *= invsqrt;
-	//}else
-	//	x = 1.0f;
-}
-
-void
 CMatrix::SetRotate(float xAngle, float yAngle, float zAngle)
 {
 	float cX = Cos(xAngle);