4 files changed, 174 insertions, 122 deletions

diff --git a/src/math/Matrix.h b/src/math/Matrix.h
index 05a6eb03..bfe85afa 100644
--- a/src/math/Matrix.h
+++ b/src/math/Matrix.h
@@ -125,6 +125,24 @@ public:
 		m_matrix.pos.y = 0.0f;
 		m_matrix.pos.z = 0.0f;
 	}
+	void Scale(float scale)
+	{
+		// GTA treats this as 4x4 floats
+		m_matrix.right.x *= scale;
+		m_matrix.right.y *= scale;
+		m_matrix.right.z *= scale;
+		m_matrix.up.x *= scale;
+		m_matrix.up.y *= scale;
+		m_matrix.up.z *= scale;
+		m_matrix.at.x *= scale;
+		m_matrix.at.y *= scale;
+		m_matrix.at.z *= scale;
+		m_matrix.pos.x *= scale;
+		m_matrix.pos.y *= scale;
+		m_matrix.pos.z *= scale;
+		m_matrix.flags = 0;
+	}
+
 
 	void SetRotateXOnly(float angle){
 		float c = Cos(angle);
@@ -192,40 +210,10 @@ public:
 		m_matrix.pos.y = 0.0f;
 		m_matrix.pos.z = 0.0f;
 	}
-	void SetRotate(float xAngle, float yAngle, float zAngle) {
-		float cX = Cos(xAngle);
-		float sX = Sin(xAngle);
-		float cY = Cos(yAngle);
-		float sY = Sin(yAngle);
-		float cZ = Cos(zAngle);
-		float sZ = Sin(zAngle);
-
-		m_matrix.right.x = cZ * cY - (sZ * sX) * sY;
-		m_matrix.right.y = (cZ * sX) * sY + sZ * cY;
-		m_matrix.right.z = -cX * sY;
+	void SetRotate(float xAngle, float yAngle, float zAngle);
+	void Rotate(float x, float y, float z);
 
-		m_matrix.up.x = -sZ * cX;
-		m_matrix.up.y = cZ * cX;
-		m_matrix.up.z = sX;
-
-		m_matrix.at.x = (sZ * sX) * cY + cZ * sY;
-		m_matrix.at.y = sZ * sY - (cZ * sX) * cY;
-		m_matrix.at.z = cX * cY;
-
-		m_matrix.pos.x = 0.0f;
-		m_matrix.pos.y = 0.0f;
-		m_matrix.pos.z = 0.0f;
-	}
-	void Reorthogonalise(void){
-		CVector &r = GetRight();
-		CVector &f = GetForward();
-		CVector &u = GetUp();
-		u = CrossProduct(r, f);
-		u.Normalise();
-		r = CrossProduct(f, u);
-		r.Normalise();
-		f = CrossProduct(u, r);
-	}
+	void Reorthogonalise(void);
 	void CopyOnlyMatrix(CMatrix *other){
 		m_matrix = other->m_matrix;
 	}
@@ -245,35 +233,13 @@ public:
 	}
 };
 
-inline CMatrix&
-Invert(const CMatrix &src, CMatrix &dst)
-{
-	// GTA handles this as a raw 4x4 orthonormal matrix
-	// and trashes the RW flags, let's not do that
-	// actual copy of librw code:
-	RwMatrix *d = &dst.m_matrix;
-	const RwMatrix *s = &src.m_matrix;
-	d->right.x = s->right.x;
-	d->right.y = s->up.x;
-	d->right.z = s->at.x;
-	d->up.x = s->right.y;
-	d->up.y = s->up.y;
-	d->up.z = s->at.y;
-	d->at.x = s->right.z;
-	d->at.y = s->up.z;
-	d->at.z = s->at.z;
-	d->pos.x = -(s->pos.x*s->right.x +
-	               s->pos.y*s->right.y +
-	               s->pos.z*s->right.z);
-	d->pos.y = -(s->pos.x*s->up.x +
-	               s->pos.y*s->up.y +
-	               s->pos.z*s->up.z);
-	d->pos.z = -(s->pos.x*s->at.x +
-	               s->pos.y*s->at.y +
-	               s->pos.z*s->at.z);
-	d->flags = rwMATRIXTYPEORTHONORMAL;
-	return dst;
-}
+
+CMatrix &Invert(const CMatrix &src, CMatrix &dst);
+CVector operator*(const CMatrix &mat, const CVector &vec);
+CMatrix operator*(const CMatrix &m1, const CMatrix &m2);
+CVector MultiplyInverse(const CMatrix &mat, const CVector &vec);
+CVector Multiply3x3(const CMatrix &mat, const CVector &vec);
+CVector Multiply3x3(const CVector &vec, const CMatrix &mat);
 
 inline CMatrix
 Invert(const CMatrix &matrix)
@@ -282,64 +248,6 @@ Invert(const CMatrix &matrix)
 	return Invert(matrix, inv);
 }
 
-inline CVector
-operator*(const CMatrix &mat, const CVector &vec)
-{
-	return CVector(
-		mat.m_matrix.right.x * vec.x + mat.m_matrix.up.x * vec.y + mat.m_matrix.at.x * vec.z + mat.m_matrix.pos.x,
-		mat.m_matrix.right.y * vec.x + mat.m_matrix.up.y * vec.y + mat.m_matrix.at.y * vec.z + mat.m_matrix.pos.y,
-		mat.m_matrix.right.z * vec.x + mat.m_matrix.up.z * vec.y + mat.m_matrix.at.z * vec.z + mat.m_matrix.pos.z);
-}
-
-inline CMatrix
-operator*(const CMatrix &m1, const CMatrix &m2)
-{
-	CMatrix out;
-	RwMatrix *dst = &out.m_matrix;
-	const RwMatrix *src1 = &m1.m_matrix;
-	const RwMatrix *src2 = &m2.m_matrix;
-	dst->right.x = src1->right.x*src2->right.x + src1->up.x*src2->right.y + src1->at.x*src2->right.z;
-	dst->right.y = src1->right.y*src2->right.x + src1->up.y*src2->right.y + src1->at.y*src2->right.z;
-	dst->right.z = src1->right.z*src2->right.x + src1->up.z*src2->right.y + src1->at.z*src2->right.z;
-	dst->up.x    = src1->right.x*src2->up.x    + src1->up.x*src2->up.y    + src1->at.x*src2->up.z;
-	dst->up.y    = src1->right.y*src2->up.x    + src1->up.y*src2->up.y    + src1->at.y*src2->up.z;
-	dst->up.z    = src1->right.z*src2->up.x    + src1->up.z*src2->up.y    + src1->at.z*src2->up.z;
-	dst->at.x    = src1->right.x*src2->at.x    + src1->up.x*src2->at.y    + src1->at.x*src2->at.z;
-	dst->at.y    = src1->right.y*src2->at.x    + src1->up.y*src2->at.y    + src1->at.y*src2->at.z;
-	dst->at.z    = src1->right.z*src2->at.x    + src1->up.z*src2->at.y    + src1->at.z*src2->at.z;
-	dst->pos.x   = src1->right.x*src2->pos.x   + src1->up.x*src2->pos.y   + src1->at.x*src2->pos.z + src1->pos.x;
-	dst->pos.y   = src1->right.y*src2->pos.x   + src1->up.y*src2->pos.y   + src1->at.y*src2->pos.z + src1->pos.y;
-	dst->pos.z   = src1->right.z*src2->pos.x   + src1->up.z*src2->pos.y   + src1->at.z*src2->pos.z + src1->pos.z;
-	return out;
-}
-
-inline CVector
-MultiplyInverse(const CMatrix &mat, const CVector &vec)
-{
-	CVector v(vec.x - mat.m_matrix.pos.x, vec.y - mat.m_matrix.pos.y, vec.z - mat.m_matrix.pos.z);
-	return CVector(
-		mat.m_matrix.right.x * v.x + mat.m_matrix.right.y * v.y + mat.m_matrix.right.z * v.z,
-		mat.m_matrix.up.x * v.x + mat.m_matrix.up.y * v.y + mat.m_matrix.up.z * v.z,
-		mat.m_matrix.at.x * v.x + mat.m_matrix.at.y * v.y + mat.m_matrix.at.z * v.z);
-}
-
-inline CVector
-Multiply3x3(const CMatrix &mat, const CVector &vec)
-{
-	return CVector(
-		mat.m_matrix.right.x * vec.x + mat.m_matrix.up.x * vec.y + mat.m_matrix.at.x * vec.z,
-		mat.m_matrix.right.y * vec.x + mat.m_matrix.up.y * vec.y + mat.m_matrix.at.y * vec.z,
-		mat.m_matrix.right.z * vec.x + mat.m_matrix.up.z * vec.y + mat.m_matrix.at.z * vec.z);
-}
-
-inline CVector
-Multiply3x3(const CVector &vec, const CMatrix &mat)
-{
-	return CVector(
-		mat.m_matrix.right.x * vec.x + mat.m_matrix.right.y * vec.y + mat.m_matrix.right.z * vec.z,
-		mat.m_matrix.up.x * vec.x + mat.m_matrix.up.y * vec.y + mat.m_matrix.up.z * vec.z,
-		mat.m_matrix.at.x * vec.x + mat.m_matrix.at.y * vec.y + mat.m_matrix.at.z * vec.z);
-}
 
 class CCompressedMatrixNotAligned
 {
diff --git a/src/math/Vector.h b/src/math/Vector.h
index f794a57f..42087339 100644
--- a/src/math/Vector.h
+++ b/src/math/Vector.h
@@ -31,7 +31,7 @@ public:
 	void Normalise(void) {
 		float sq = MagnitudeSqr();
 		if(sq > 0.0f){
-			float invsqrt = 1.0f/Sqrt(sq); // CMaths::RecipSqrt
+			float invsqrt = RecipSqrt(sq);
 			x *= invsqrt;
 			y *= invsqrt;
 			z *= invsqrt;
@@ -71,6 +71,10 @@ public:
 		return CVector(-x, -y, -z);
 	}
 
+	const bool operator==(CVector const &right) {
+		return x == right.x && y == right.y && z == right.z;
+	}
+
 	bool IsZero(void) { return x == 0.0f && y == 0.0f && z == 0.0f; }
 };
 
diff --git a/src/math/Vector2D.h b/src/math/Vector2D.h
index e6b04c14..c8835ec0 100644
--- a/src/math/Vector2D.h
+++ b/src/math/Vector2D.h
@@ -7,13 +7,14 @@ public:
 	CVector2D(void) {}
 	CVector2D(float x, float y) : x(x), y(y) {}
 	CVector2D(const CVector &v) : x(v.x), y(v.y) {}
+	float Heading(void) const { return Atan2(-x, y); }
 	float Magnitude(void) const { return Sqrt(x*x + y*y); }
 	float MagnitudeSqr(void) const { return x*x + y*y; }
 
 	void Normalise(void){
 		float sq = MagnitudeSqr();
 		if(sq > 0.0f){
-			float invsqrt = 1.0f/Sqrt(sq);
+			float invsqrt = RecipSqrt(sq);
 			x *= invsqrt;
 			y *= invsqrt;
 		}else
diff --git a/src/math/math.cpp b/src/math/math.cpp
index c1199fcc..e8b7d933 100644
--- a/src/math/math.cpp
+++ b/src/math/math.cpp
@@ -5,6 +5,145 @@
 // TODO: move more stuff into here
 
 void
+CMatrix::SetRotate(float xAngle, float yAngle, float zAngle)
+{
+	float cX = Cos(xAngle);
+	float sX = Sin(xAngle);
+	float cY = Cos(yAngle);
+	float sY = Sin(yAngle);
+	float cZ = Cos(zAngle);
+	float sZ = Sin(zAngle);
+
+	m_matrix.right.x = cZ * cY - (sZ * sX) * sY;
+	m_matrix.right.y = (cZ * sX) * sY + sZ * cY;
+	m_matrix.right.z = -cX * sY;
+
+	m_matrix.up.x = -sZ * cX;
+	m_matrix.up.y = cZ * cX;
+	m_matrix.up.z = sX;
+
+	m_matrix.at.x = (sZ * sX) * cY + cZ * sY;
+	m_matrix.at.y = sZ * sY - (cZ * sX) * cY;
+	m_matrix.at.z = cX * cY;
+
+	m_matrix.pos.x = 0.0f;
+	m_matrix.pos.y = 0.0f;
+	m_matrix.pos.z = 0.0f;
+}
+
+void
+CMatrix::Rotate(float x, float y, float z)
+{
+	// TODO? do this directly without creating another matrix
+	CMatrix rot;
+	rot.SetRotate(x, y, z);
+	*this = rot * *this;
+}
+
+void
+CMatrix::Reorthogonalise(void)
+{
+	CVector &r = GetRight();
+	CVector &f = GetForward();
+	CVector &u = GetUp();
+	u = CrossProduct(r, f);
+	u.Normalise();
+	r = CrossProduct(f, u);
+	r.Normalise();
+	f = CrossProduct(u, r);
+}
+
+CMatrix&
+Invert(const CMatrix &src, CMatrix &dst)
+{
+	// GTA handles this as a raw 4x4 orthonormal matrix
+	// and trashes the RW flags, let's not do that
+	// actual copy of librw code:
+	RwMatrix *d = &dst.m_matrix;
+	const RwMatrix *s = &src.m_matrix;
+	d->right.x = s->right.x;
+	d->right.y = s->up.x;
+	d->right.z = s->at.x;
+	d->up.x = s->right.y;
+	d->up.y = s->up.y;
+	d->up.z = s->at.y;
+	d->at.x = s->right.z;
+	d->at.y = s->up.z;
+	d->at.z = s->at.z;
+	d->pos.x = -(s->pos.x*s->right.x +
+	               s->pos.y*s->right.y +
+	               s->pos.z*s->right.z);
+	d->pos.y = -(s->pos.x*s->up.x +
+	               s->pos.y*s->up.y +
+	               s->pos.z*s->up.z);
+	d->pos.z = -(s->pos.x*s->at.x +
+	               s->pos.y*s->at.y +
+	               s->pos.z*s->at.z);
+	d->flags = rwMATRIXTYPEORTHONORMAL;
+	return dst;
+}
+
+CVector
+operator*(const CMatrix &mat, const CVector &vec)
+{
+	return CVector(
+		mat.m_matrix.right.x * vec.x + mat.m_matrix.up.x * vec.y + mat.m_matrix.at.x * vec.z + mat.m_matrix.pos.x,
+		mat.m_matrix.right.y * vec.x + mat.m_matrix.up.y * vec.y + mat.m_matrix.at.y * vec.z + mat.m_matrix.pos.y,
+		mat.m_matrix.right.z * vec.x + mat.m_matrix.up.z * vec.y + mat.m_matrix.at.z * vec.z + mat.m_matrix.pos.z);
+}
+
+CMatrix
+operator*(const CMatrix &m1, const CMatrix &m2)
+{
+	CMatrix out;
+	RwMatrix *dst = &out.m_matrix;
+	const RwMatrix *src1 = &m1.m_matrix;
+	const RwMatrix *src2 = &m2.m_matrix;
+	dst->right.x = src1->right.x*src2->right.x + src1->up.x*src2->right.y + src1->at.x*src2->right.z;
+	dst->right.y = src1->right.y*src2->right.x + src1->up.y*src2->right.y + src1->at.y*src2->right.z;
+	dst->right.z = src1->right.z*src2->right.x + src1->up.z*src2->right.y + src1->at.z*src2->right.z;
+	dst->up.x    = src1->right.x*src2->up.x    + src1->up.x*src2->up.y    + src1->at.x*src2->up.z;
+	dst->up.y    = src1->right.y*src2->up.x    + src1->up.y*src2->up.y    + src1->at.y*src2->up.z;
+	dst->up.z    = src1->right.z*src2->up.x    + src1->up.z*src2->up.y    + src1->at.z*src2->up.z;
+	dst->at.x    = src1->right.x*src2->at.x    + src1->up.x*src2->at.y    + src1->at.x*src2->at.z;
+	dst->at.y    = src1->right.y*src2->at.x    + src1->up.y*src2->at.y    + src1->at.y*src2->at.z;
+	dst->at.z    = src1->right.z*src2->at.x    + src1->up.z*src2->at.y    + src1->at.z*src2->at.z;
+	dst->pos.x   = src1->right.x*src2->pos.x   + src1->up.x*src2->pos.y   + src1->at.x*src2->pos.z + src1->pos.x;
+	dst->pos.y   = src1->right.y*src2->pos.x   + src1->up.y*src2->pos.y   + src1->at.y*src2->pos.z + src1->pos.y;
+	dst->pos.z   = src1->right.z*src2->pos.x   + src1->up.z*src2->pos.y   + src1->at.z*src2->pos.z + src1->pos.z;
+	return out;
+}
+
+CVector
+MultiplyInverse(const CMatrix &mat, const CVector &vec)
+{
+	CVector v(vec.x - mat.m_matrix.pos.x, vec.y - mat.m_matrix.pos.y, vec.z - mat.m_matrix.pos.z);
+	return CVector(
+		mat.m_matrix.right.x * v.x + mat.m_matrix.right.y * v.y + mat.m_matrix.right.z * v.z,
+		mat.m_matrix.up.x * v.x + mat.m_matrix.up.y * v.y + mat.m_matrix.up.z * v.z,
+		mat.m_matrix.at.x * v.x + mat.m_matrix.at.y * v.y + mat.m_matrix.at.z * v.z);
+}
+
+CVector
+Multiply3x3(const CMatrix &mat, const CVector &vec)
+{
+	return CVector(
+		mat.m_matrix.right.x * vec.x + mat.m_matrix.up.x * vec.y + mat.m_matrix.at.x * vec.z,
+		mat.m_matrix.right.y * vec.x + mat.m_matrix.up.y * vec.y + mat.m_matrix.at.y * vec.z,
+		mat.m_matrix.right.z * vec.x + mat.m_matrix.up.z * vec.y + mat.m_matrix.at.z * vec.z);
+}
+
+CVector
+Multiply3x3(const CVector &vec, const CMatrix &mat)
+{
+	return CVector(
+		mat.m_matrix.right.x * vec.x + mat.m_matrix.right.y * vec.y + mat.m_matrix.right.z * vec.z,
+		mat.m_matrix.up.x * vec.x + mat.m_matrix.up.y * vec.y + mat.m_matrix.up.z * vec.z,
+		mat.m_matrix.at.x * vec.x + mat.m_matrix.at.y * vec.y + mat.m_matrix.at.z * vec.z);
+}
+
+
+void
 CQuaternion::Slerp(const CQuaternion &q1, const CQuaternion &q2, float theta, float invSin, float t)
 {
 	if(theta == 0.0f)