4 files changed, 43 insertions, 8 deletions

diff --git a/src/math/Quaternion.h b/src/math/Quaternion.h
index fb37dc10..1d04bdff 100644
--- a/src/math/Quaternion.h
+++ b/src/math/Quaternion.h
@@ -10,6 +10,18 @@ public:
 
 	float Magnitude(void) const { return Sqrt(x*x + y*y + z*z + w*w); }
 	float MagnitudeSqr(void) const { return x*x + y*y + z*z + w*w; }
+	void Normalise(void) {
+		float sq = MagnitudeSqr();
+		if(sq == 0.0f)
+			w = 1.0f;
+		else{
+			float invsqrt = RecipSqrt(sq);
+			x *= invsqrt;
+			y *= invsqrt;
+			z *= invsqrt;
+			w *= invsqrt;
+		}
+	}
 
 	const CQuaternion &operator+=(CQuaternion const &right) {
 		x += right.x;
diff --git a/src/math/Vector.h b/src/math/Vector.h
index 1274a4b2..44e646e9 100644
--- a/src/math/Vector.h
+++ b/src/math/Vector.h
@@ -9,6 +9,11 @@ public:
 #ifdef RWCORE_H
 	CVector(const RwV3d &v) : x(v.x), y(v.y), z(v.z) {}
 
+	RwV3d toRwV3d(void) const {
+		RwV3d vecRw = { this->x, this->y, this->z };
+		return vecRw;
+	}
+
 	operator RwV3d (void) const {
 		RwV3d vecRw = { this->x, this->y, this->z };
 		return vecRw;
@@ -46,6 +51,13 @@ public:
 		y *= invsqrt;
 		z *= invsqrt;
 	}
+	
+	void Normalise2D(void) {
+		float sq = MagnitudeSqr2D();
+		float invsqrt = RecipSqrt(sq);
+		x *= invsqrt;
+		y *= invsqrt;
+	}
 
 	const CVector &operator+=(CVector const &right) {
 		x += right.x;
diff --git a/src/math/Vector2D.h b/src/math/Vector2D.h
index 1e4d698f..0885a5d2 100644
--- a/src/math/Vector2D.h
+++ b/src/math/Vector2D.h
@@ -11,15 +11,18 @@ public:
 	float Magnitude(void) const { return Sqrt(x*x + y*y); }
 	float MagnitudeSqr(void) const { return x*x + y*y; }
 
-	void Normalise(void){
+	void Normalise(void);
+
+	void NormaliseSafe(void) {
 		float sq = MagnitudeSqr();
 		if(sq > 0.0f){
 			float invsqrt = RecipSqrt(sq);
 			x *= invsqrt;
 			y *= invsqrt;
 		}else
-			x = 1.0f;
+			y = 1.0f;
 	}
+
 	const CVector2D &operator+=(CVector2D const &right) {
 		x += right.x;
 		y += right.y;
diff --git a/src/math/math.cpp b/src/math/math.cpp
index 4f74fac9..eeb9d3fa 100644
--- a/src/math/math.cpp
+++ b/src/math/math.cpp
@@ -1,10 +1,23 @@
 #include "common.h"
-#include "patcher.h"
+
 #include "Quaternion.h"
 
 // TODO: move more stuff into here
 
 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);
@@ -191,8 +204,3 @@ CQuaternion::Get(RwMatrix *matrix)
 	matrix->up.z = y_2z + w_2x;
 	matrix->at.z = 1.0f - (x_2x + y_2y);
 }
-
-STARTPATCHES
-	InjectHook(0x4BA1C0, &CQuaternion::Slerp, PATCH_JUMP);
-	InjectHook(0x4BA0D0, &CQuaternion::Get, PATCH_JUMP);
-ENDPATCHES