14 files changed, 857 insertions, 529 deletions

diff --git a/src/control/CarCtrl.cpp b/src/control/CarCtrl.cpp
index c6d90dd9..5a2def05 100644
--- a/src/control/CarCtrl.cpp
+++ b/src/control/CarCtrl.cpp
@@ -1080,11 +1080,11 @@ void CCarCtrl::SlowCarDownForCarsSectorList(CPtrList& lst, CVehicle* pVehicle, f
 void CCarCtrl::SlowCarDownForOtherCar(CEntity* pOtherEntity, CVehicle* pVehicle, float* pSpeed, float curSpeed)
 {
 	CVector forwardA = pVehicle->GetForward();
-	((CVector2D)forwardA).Normalise();
+	((CVector2D)forwardA).NormaliseSafe();
 	if (DotProduct2D(pOtherEntity->GetPosition() - pVehicle->GetPosition(), forwardA) < 0.0f)
 		return;
 	CVector forwardB = pOtherEntity->GetForward();
-	((CVector2D)forwardB).Normalise();
+	((CVector2D)forwardB).NormaliseSafe();
 	forwardA.z = forwardB.z = 0.0f;
 	CVehicle* pOtherVehicle = (CVehicle*)pOtherEntity;
 	/* why is the argument CEntity if it's always CVehicle anyway and is casted? */
@@ -1337,7 +1337,7 @@ void CCarCtrl::WeaveForOtherCar(CEntity* pOtherEntity, CVehicle* pVehicle, float
 	  pVehicle->GetModelInfo()->GetColModel()->boundingSphere.radius < distance)
 		return;
 	CVector2D forward = pVehicle->GetForward();
-	forward.Normalise();
+	forward.NormaliseSafe();
 	float forwardAngle = CGeneral::GetATanOfXY(forward.x, forward.y);
 	float angleDiff = angleBetweenVehicles - forwardAngle;
 	float lenProjection = ABS(pOtherCar->GetColModel()->boundingBox.max.y * sin(angleDiff));
@@ -2276,7 +2276,7 @@ float CCarCtrl::FindMaxSteerAngle(CVehicle* pVehicle)
 void CCarCtrl::SteerAICarWithPhysicsFollowPath(CVehicle* pVehicle, float* pSwerve, float* pAccel, float* pBrake, bool* pHandbrake)
 {
 	CVector2D forward = pVehicle->GetForward();
-	forward.Normalise();
+	forward.NormaliseSafe();
 	CCarPathLink* pCurrentLink = &ThePaths.m_carPathLinks[pVehicle->AutoPilot.m_nCurrentPathNodeInfo];
 	CCarPathLink* pNextLink = &ThePaths.m_carPathLinks[pVehicle->AutoPilot.m_nNextPathNodeInfo];
 	CVector2D currentPathLinkForward(pCurrentLink->GetDirX() * pVehicle->AutoPilot.m_nCurrentDirection,
@@ -2410,7 +2410,7 @@ void CCarCtrl::SteerAICarWithPhysicsHeadingForTarget(CVehicle* pVehicle, CPhysic
 {
 	*pHandbrake = false;
 	CVector2D forward = pVehicle->GetForward();
-	forward.Normalise();
+	forward.NormaliseSafe();
 	float angleToTarget = CGeneral::GetATanOfXY(targetX - pVehicle->GetPosition().x, targetY - pVehicle->GetPosition().y);
 	float angleForward = CGeneral::GetATanOfXY(forward.x, forward.y);
 	if (pVehicle->AutoPilot.m_nDrivingStyle == DRIVINGSTYLE_AVOID_CARS)
@@ -2497,7 +2497,7 @@ void CCarCtrl::SteerAICarWithPhysicsTryingToBlockTarget_Stop(CVehicle* pVehicle,
 void CCarCtrl::SteerAIBoatWithPhysicsHeadingForTarget(CBoat* pBoat, float targetX, float targetY, float* pSwerve, float* pAccel, float* pBrake)
 {
 	CVector2D forward(pBoat->GetForward());
-	forward.Normalise();
+	forward.NormaliseSafe();
 	CVector2D distanceToTarget = CVector2D(targetX, targetY) - pBoat->GetPosition();
 	float angleToTarget = CGeneral::GetATanOfXY(distanceToTarget.x, distanceToTarget.y);
 	float angleForward = CGeneral::GetATanOfXY(forward.x, forward.y);
@@ -2733,7 +2733,7 @@ bool CCarCtrl::GenerateOneEmergencyServicesCar(uint32 mi, CVector vecPos)
 	pVehicle->AutoPilot.m_nTempAction = TEMPACT_NONE;
 	pVehicle->AutoPilot.m_nDrivingStyle = DRIVINGSTYLE_AVOID_CARS;
 	CVector2D direction = vecPos - spawnPos;
-	direction.Normalise();
+	direction.NormaliseSafe();
 	pVehicle->GetForward() = CVector(direction.x, direction.y, 0.0f);
 	pVehicle->GetRight() = CVector(direction.y, -direction.x, 0.0f);
 	pVehicle->GetUp() = CVector(0.0f, 0.0f, 1.0f);
diff --git a/src/math/Matrix.cpp b/src/math/Matrix.cpp
new file mode 100644
index 00000000..3e77548d
--- /dev/null
+++ b/src/math/Matrix.cpp
@@ -0,0 +1,552 @@
+#include "common.h"
+
+CMatrix::CMatrix(void)
+{
+	m_attachment = nil;
+	m_hasRwMatrix = false;
+}
+
+CMatrix::CMatrix(CMatrix const &m)
+{
+	m_attachment = nil;
+	m_hasRwMatrix = false;
+	*this = m;
+}
+
+CMatrix::CMatrix(RwMatrix *matrix, bool owner)
+{
+	m_attachment = nil;
+	Attach(matrix, owner);
+}
+
+CMatrix::~CMatrix(void)
+{
+	if (m_hasRwMatrix && m_attachment)
+		RwMatrixDestroy(m_attachment);
+}
+
+void
+CMatrix::Attach(RwMatrix *matrix, bool owner)
+{
+#ifdef FIX_BUGS
+	if (m_attachment && m_hasRwMatrix)
+#else
+	if (m_hasRwMatrix && m_attachment)
+#endif
+		RwMatrixDestroy(m_attachment);
+	m_attachment = matrix;
+	m_hasRwMatrix = owner;
+	Update();
+}
+
+void
+CMatrix::AttachRW(RwMatrix *matrix, bool owner)
+{
+	if (m_hasRwMatrix && m_attachment)
+		RwMatrixDestroy(m_attachment);
+	m_attachment = matrix;
+	m_hasRwMatrix = owner;
+	UpdateRW();
+}
+
+void
+CMatrix::Detach(void)
+{
+	if (m_hasRwMatrix && m_attachment)
+		RwMatrixDestroy(m_attachment);
+	m_attachment = nil;
+}
+
+void
+CMatrix::Update(void)
+{
+	m_matrix = *m_attachment;
+}
+
+void
+CMatrix::UpdateRW(void)
+{
+	if (m_attachment) {
+		*m_attachment = m_matrix;
+		RwMatrixUpdate(m_attachment);
+	}
+}
+
+void
+CMatrix::operator=(CMatrix const &rhs)
+{
+	m_matrix = rhs.m_matrix;
+	if (m_attachment)
+		UpdateRW();
+}
+
+void
+CMatrix::CopyOnlyMatrix(CMatrix *other)
+{
+	m_matrix = other->m_matrix;
+}
+
+CMatrix &
+CMatrix::operator+=(CMatrix const &rhs)
+{
+	m_matrix.right.x += rhs.m_matrix.right.x;
+	m_matrix.up.x += rhs.m_matrix.up.x;
+	m_matrix.at.x += rhs.m_matrix.at.x;
+	m_matrix.right.y += rhs.m_matrix.right.y;
+	m_matrix.up.y += rhs.m_matrix.up.y;
+	m_matrix.at.y += rhs.m_matrix.at.y;
+	m_matrix.right.z += rhs.m_matrix.right.z;
+	m_matrix.up.z += rhs.m_matrix.up.z;
+	m_matrix.at.z += rhs.m_matrix.at.z;
+	m_matrix.pos.x += rhs.m_matrix.pos.x;
+	m_matrix.pos.y += rhs.m_matrix.pos.y;
+	m_matrix.pos.z += rhs.m_matrix.pos.z;
+	return *this;
+}
+
+void
+CMatrix::SetUnity(void)
+{
+	m_matrix.right.x = 1.0f;
+	m_matrix.right.y = 0.0f;
+	m_matrix.right.z = 0.0f;
+	m_matrix.up.x = 0.0f;
+	m_matrix.up.y = 1.0f;
+	m_matrix.up.z = 0.0f;
+	m_matrix.at.x = 0.0f;
+	m_matrix.at.y = 0.0f;
+	m_matrix.at.z = 1.0f;
+	m_matrix.pos.x = 0.0f;
+	m_matrix.pos.y = 0.0f;
+	m_matrix.pos.z = 0.0f;
+}
+
+void
+CMatrix::ResetOrientation(void)
+{
+	m_matrix.right.x = 1.0f;
+	m_matrix.right.y = 0.0f;
+	m_matrix.right.z = 0.0f;
+	m_matrix.up.x = 0.0f;
+	m_matrix.up.y = 1.0f;
+	m_matrix.up.z = 0.0f;
+	m_matrix.at.x = 0.0f;
+	m_matrix.at.y = 0.0f;
+	m_matrix.at.z = 1.0f;
+}
+
+void
+CMatrix::SetScale(float s)
+{
+	m_matrix.right.x = s;
+	m_matrix.right.y = 0.0f;
+	m_matrix.right.z = 0.0f;
+
+	m_matrix.up.x = 0.0f;
+	m_matrix.up.y = s;
+	m_matrix.up.z = 0.0f;
+
+	m_matrix.at.x = 0.0f;
+	m_matrix.at.y = 0.0f;
+	m_matrix.at.z = s;
+
+	m_matrix.pos.x = 0.0f;
+	m_matrix.pos.y = 0.0f;
+	m_matrix.pos.z = 0.0f;
+}
+
+void
+CMatrix::SetTranslate(float x, float y, float z)
+{
+	m_matrix.right.x = 1.0f;
+	m_matrix.right.y = 0.0f;
+	m_matrix.right.z = 0.0f;
+
+	m_matrix.up.x = 0.0f;
+	m_matrix.up.y = 1.0f;
+	m_matrix.up.z = 0.0f;
+
+	m_matrix.at.x = 0.0f;
+	m_matrix.at.y = 0.0f;
+	m_matrix.at.z = 1.0f;
+
+	m_matrix.pos.x = x;
+	m_matrix.pos.y = y;
+	m_matrix.pos.z = z;
+}
+
+void
+CMatrix::SetRotateXOnly(float angle)
+{
+	float c = Cos(angle);
+	float s = Sin(angle);
+
+	m_matrix.right.x = 1.0f;
+	m_matrix.right.y = 0.0f;
+	m_matrix.right.z = 0.0f;
+
+	m_matrix.up.x = 0.0f;
+	m_matrix.up.y = c;
+	m_matrix.up.z = s;
+
+	m_matrix.at.x = 0.0f;
+	m_matrix.at.y = -s;
+	m_matrix.at.z = c;
+}
+
+void
+CMatrix::SetRotateYOnly(float angle)
+{
+	float c = Cos(angle);
+	float s = Sin(angle);
+
+	m_matrix.right.x = c;
+	m_matrix.right.y = 0.0f;
+	m_matrix.right.z = -s;
+
+	m_matrix.up.x = 0.0f;
+	m_matrix.up.y = 1.0f;
+	m_matrix.up.z = 0.0f;
+
+	m_matrix.at.x = s;
+	m_matrix.at.y = 0.0f;
+	m_matrix.at.z = c;
+}
+
+void
+CMatrix::SetRotateZOnly(float angle)
+{
+	float c = Cos(angle);
+	float s = Sin(angle);
+
+	m_matrix.right.x = c;
+	m_matrix.right.y = s;
+	m_matrix.right.z = 0.0f;
+
+	m_matrix.up.x = -s;
+	m_matrix.up.y = c;
+	m_matrix.up.z = 0.0f;
+
+	m_matrix.at.x = 0.0f;
+	m_matrix.at.y = 0.0f;
+	m_matrix.at.z = 1.0f;
+}
+
+void
+CMatrix::SetRotateX(float angle)
+{
+	SetRotateXOnly(angle);
+	m_matrix.pos.x = 0.0f;
+	m_matrix.pos.y = 0.0f;
+	m_matrix.pos.z = 0.0f;
+}
+
+
+void
+CMatrix::SetRotateY(float angle)
+{
+	SetRotateYOnly(angle);
+	m_matrix.pos.x = 0.0f;
+	m_matrix.pos.y = 0.0f;
+	m_matrix.pos.z = 0.0f;
+}
+
+void
+CMatrix::SetRotateZ(float angle)
+{
+	SetRotateZOnly(angle);
+	m_matrix.pos.x = 0.0f;
+	m_matrix.pos.y = 0.0f;
+	m_matrix.pos.z = 0.0f;
+}
+
+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::RotateX(float x)
+{
+	float c = Cos(x);
+	float s = Sin(x);
+
+	float ry = m_matrix.right.y;
+	float rz = m_matrix.right.z;
+	float uy = m_matrix.up.y;
+	float uz = m_matrix.up.z;
+	float ay = m_matrix.at.y;
+	float az = m_matrix.at.z;
+	float py = m_matrix.pos.y;
+	float pz = m_matrix.pos.z;
+
+	m_matrix.right.y = c * ry - s * rz;
+	m_matrix.right.z = c * rz + s * ry;
+	m_matrix.up.y = c * uy - s * uz;
+	m_matrix.up.z = c * uz + s * uy;
+	m_matrix.at.y = c * ay - s * az;
+	m_matrix.at.z = c * az + s * ay;
+	m_matrix.pos.y = c * py - s * pz;
+	m_matrix.pos.z = c * pz + s * py;
+}
+
+void
+CMatrix::RotateY(float y)
+{
+	float c = Cos(y);
+	float s = Sin(y);
+
+	float rx = m_matrix.right.x;
+	float rz = m_matrix.right.z;
+	float ux = m_matrix.up.x;
+	float uz = m_matrix.up.z;
+	float ax = m_matrix.at.x;
+	float az = m_matrix.at.z;
+	float px = m_matrix.pos.x;
+	float pz = m_matrix.pos.z;
+
+	m_matrix.right.x = c * rx - s * rz;
+	m_matrix.right.z = c * rz + s * rx;
+	m_matrix.up.x = c * ux - s * uz;
+	m_matrix.up.z = c * uz + s * ux;
+	m_matrix.at.x = c * ax - s * az;
+	m_matrix.at.z = c * az + s * ax;
+	m_matrix.pos.x = c * px - s * pz;
+	m_matrix.pos.z = c * pz + s * px;
+}
+
+void
+CMatrix::RotateZ(float z)
+{
+	float c = Cos(z);
+	float s = Sin(z);
+
+	float ry = m_matrix.right.y;
+	float rx = m_matrix.right.x;
+	float uy = m_matrix.up.y;
+	float ux = m_matrix.up.x;
+	float ay = m_matrix.at.y;
+	float ax = m_matrix.at.x;
+	float py = m_matrix.pos.y;
+	float px = m_matrix.pos.x;
+
+	m_matrix.right.x = c * rx - s * ry;
+	m_matrix.right.y = c * ry + s * rx;
+	m_matrix.up.x = c * ux - s * uy;
+	m_matrix.up.y = c * uy + s * ux;
+	m_matrix.at.x = c * ax - s * ay;
+	m_matrix.at.y = c * ay + s * ax;
+	m_matrix.pos.x = c * px - s * py;
+	m_matrix.pos.y = c * py + s * px;
+
+}
+
+void
+CMatrix::Rotate(float x, float y, float z)
+{
+	float cX = Cos(x);
+	float sX = Sin(x);
+	float cY = Cos(y);
+	float sY = Sin(y);
+	float cZ = Cos(z);
+	float sZ = Sin(z);
+	
+	float rx = m_matrix.right.x;
+	float ry = m_matrix.right.y;
+	float rz = m_matrix.right.z;
+	float ux = m_matrix.up.x;
+	float uy = m_matrix.up.y;
+	float uz = m_matrix.up.z;
+	float ax = m_matrix.at.x;
+	float ay = m_matrix.at.y;
+	float az = m_matrix.at.z;
+	float px = m_matrix.pos.x;
+	float py = m_matrix.pos.y;
+	float pz = m_matrix.pos.z;
+
+	float x1 = cZ * cY - (sZ * sX) * sY;
+	float x2 = (cZ * sX) * sY + sZ * cY;
+	float x3 = -cX * sY;
+	float y1 = -sZ * cX;
+	float y2 = cZ * cX;
+	float y3 = sX;
+	float z1 = (sZ * sX) * cY + cZ * sY;
+	float z2 = sZ * sY - (cZ * sX) * cY;
+	float z3 = cX * cY;
+
+	m_matrix.right.x = x1 * rx + y1 * ry + z1 * rz;
+	m_matrix.right.y = x2 * rx + y2 * ry + z2 * rz;
+	m_matrix.right.z = x3 * rx + y3 * ry + z3 * rz;
+	m_matrix.up.x = x1 * ux + y1 * uy + z1 * uz;
+	m_matrix.up.y = x2 * ux + y2 * uy + z2 * uz;
+	m_matrix.up.z = x3 * ux + y3 * uy + z3 * uz;
+	m_matrix.at.x = x1 * ax + y1 * ay + z1 * az;
+	m_matrix.at.y = x2 * ax + y2 * ay + z2 * az;
+	m_matrix.at.z = x3 * ax + y3 * ay + z3 * az;
+	m_matrix.pos.x = x1 * px + y1 * py + z1 * pz;
+	m_matrix.pos.y = x2 * px + y2 * py + z2 * pz;
+	m_matrix.pos.z = x3 * px + y3 * py + z3 * pz;
+}
+
+CMatrix &
+CMatrix::operator*=(CMatrix const &rhs)
+{
+	// TODO: VU0 code
+	*this = *this * rhs;
+	return *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
+operator*(const CMatrix &m1, const CMatrix &m2)
+{
+	// TODO: VU0 code
+	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;
+}
+
+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;
+}
+
+CMatrix
+Invert(const CMatrix &matrix)
+{
+	CMatrix inv;
+	return Invert(matrix, inv);
+}
+
+void
+CCompressedMatrixNotAligned::CompressFromFullMatrix(CMatrix &other)
+{
+	m_rightX = 127.0f * other.GetRight().x;
+	m_rightY = 127.0f * other.GetRight().y;
+	m_rightZ = 127.0f * other.GetRight().z;
+	m_upX = 127.0f * other.GetForward().x;
+	m_upY = 127.0f * other.GetForward().y;
+	m_upZ = 127.0f * other.GetForward().z;
+	m_vecPos = other.GetPosition();
+}
+
+void
+CCompressedMatrixNotAligned::DecompressIntoFullMatrix(CMatrix &other)
+{
+	other.GetRight().x = m_rightX / 127.0f;
+	other.GetRight().y = m_rightY / 127.0f;
+	other.GetRight().z = m_rightZ / 127.0f;
+	other.GetForward().x = m_upX / 127.0f;
+	other.GetForward().y = m_upY / 127.0f;
+	other.GetForward().z = m_upZ / 127.0f;
+	other.GetUp() = CrossProduct(other.GetRight(), other.GetForward());
+	other.GetPosition() = m_vecPos;
+	other.Reorthogonalise();
+}
\ No newline at end of file
diff --git a/src/math/Matrix.h b/src/math/Matrix.h
index e2d6b0e0..d8f6388d 100644
--- a/src/math/Matrix.h
+++ b/src/math/Matrix.h
@@ -7,104 +7,30 @@ public:
 	RwMatrix *m_attachment;
 	bool m_hasRwMatrix;	// are we the owner?
 
-	CMatrix(void){
-		m_attachment = nil;
-		m_hasRwMatrix = false;
-	}
-	CMatrix(CMatrix const &m){
-		m_attachment = nil;
-		m_hasRwMatrix = false;
-		*this = m;
-	}
-	CMatrix(RwMatrix *matrix, bool owner = false){
-		m_attachment = nil;
-		Attach(matrix, owner);
-	}
+	CMatrix(void);
+	CMatrix(CMatrix const &m);
+	CMatrix(RwMatrix *matrix, bool owner = false);
 	CMatrix(float scale){
 		m_attachment = nil;
 		m_hasRwMatrix = false;
 		SetScale(scale);
 	}
-	~CMatrix(void){
-		if(m_hasRwMatrix && m_attachment)
-			RwMatrixDestroy(m_attachment);
-	}
-	void Attach(RwMatrix *matrix, bool owner = false){
-#ifdef FIX_BUGS
-		if(m_attachment && m_hasRwMatrix)
-#else
-		if(m_hasRwMatrix && m_attachment)
-#endif
-			RwMatrixDestroy(m_attachment);
-		m_attachment = matrix;
-		m_hasRwMatrix = owner;
-		Update();
-	}
-	void AttachRW(RwMatrix *matrix, bool owner = false){
-		if(m_hasRwMatrix && m_attachment)
-			RwMatrixDestroy(m_attachment);
-		m_attachment = matrix;
-		m_hasRwMatrix = owner;
-		UpdateRW();
-	}
-	void Detach(void){
-		if(m_hasRwMatrix && m_attachment)
-			RwMatrixDestroy(m_attachment);
-		m_attachment = nil;
-	}
-	void Update(void){
-		m_matrix = *m_attachment;
-	}
-	void UpdateRW(void){
-		if(m_attachment){
-			*m_attachment = m_matrix;
-			RwMatrixUpdate(m_attachment);
-		}
-	}
-	void operator=(CMatrix const &rhs){
-		m_matrix = rhs.m_matrix;
-		if(m_attachment)
-			UpdateRW();
-	}
-	CMatrix& operator+=(CMatrix const &rhs){
-		m_matrix.right.x += rhs.m_matrix.right.x;
-		m_matrix.up.x += rhs.m_matrix.up.x;
-		m_matrix.at.x += rhs.m_matrix.at.x;
-		m_matrix.right.y += rhs.m_matrix.right.y;
-		m_matrix.up.y += rhs.m_matrix.up.y;
-		m_matrix.at.y += rhs.m_matrix.at.y;
-		m_matrix.right.z += rhs.m_matrix.right.z;
-		m_matrix.up.z += rhs.m_matrix.up.z;
-		m_matrix.at.z += rhs.m_matrix.at.z;
-		m_matrix.pos.x += rhs.m_matrix.pos.x;
-		m_matrix.pos.y += rhs.m_matrix.pos.y;
-		m_matrix.pos.z += rhs.m_matrix.pos.z;
-		return *this;
-	}
-	CMatrix& operator*=(CMatrix const &rhs);
+	~CMatrix(void);
+	void Attach(RwMatrix *matrix, bool owner = false);
+	void AttachRW(RwMatrix *matrix, bool owner = false);
+	void Detach(void);
+	void Update(void);
+	void UpdateRW(void);
+	void operator=(CMatrix const &rhs);
+	CMatrix &operator+=(CMatrix const &rhs);
+	CMatrix &operator*=(CMatrix const &rhs);
 
 	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; }
 
-	void SetTranslate(float x, float y, float z){
-		m_matrix.right.x = 1.0f;
-		m_matrix.right.y = 0.0f;
-		m_matrix.right.z = 0.0f;
-
-		m_matrix.up.x = 0.0f;
-		m_matrix.up.y = 1.0f;
-		m_matrix.up.z = 0.0f;
-
-		m_matrix.at.x = 0.0f;
-		m_matrix.at.y = 0.0f;
-		m_matrix.at.z = 1.0f;
-
-		m_matrix.pos.x = x;
-		m_matrix.pos.y = y;
-		m_matrix.pos.z = z;
-	}
+	void SetTranslate(float x, float y, float z);
 	void SetTranslate(const CVector &trans){ SetTranslate(trans.x, trans.y, trans.z); }
 	void Translate(float x, float y, float z){
 		m_matrix.pos.x += x;
@@ -113,23 +39,7 @@ public:
 	}
 	void Translate(const CVector &trans){ Translate(trans.x, trans.y, trans.z); }
 
-	void SetScale(float s){
-		m_matrix.right.x = s;
-		m_matrix.right.y = 0.0f;
-		m_matrix.right.z = 0.0f;
-
-		m_matrix.up.x = 0.0f;
-		m_matrix.up.y = s;
-		m_matrix.up.z = 0.0f;
-
-		m_matrix.at.x = 0.0f;
-		m_matrix.at.y = 0.0f;
-		m_matrix.at.z = s;
-
-		m_matrix.pos.x = 0.0f;
-		m_matrix.pos.y = 0.0f;
-		m_matrix.pos.z = 0.0f;
-	}
+	void SetScale(float s);
 	void Scale(float scale)
 	{
 		float *pFloatMatrix = (float*)&m_matrix;
@@ -143,66 +53,9 @@ public:
 	}
 
 
-	void SetRotateXOnly(float angle){
-		float c = Cos(angle);
-		float s = Sin(angle);
-
-		m_matrix.right.x = 1.0f;
-		m_matrix.right.y = 0.0f;
-		m_matrix.right.z = 0.0f;
-
-		m_matrix.up.x = 0.0f;
-		m_matrix.up.y = c;
-		m_matrix.up.z = s;
-
-		m_matrix.at.x = 0.0f;
-		m_matrix.at.y = -s;
-		m_matrix.at.z = c;
-	}
-	void SetRotateX(float angle){
-		SetRotateXOnly(angle);
-		m_matrix.pos.x = 0.0f;
-		m_matrix.pos.y = 0.0f;
-		m_matrix.pos.z = 0.0f;
-	}
-	void SetRotateYOnly(float angle){
-		float c = Cos(angle);
-		float s = Sin(angle);
-
-		m_matrix.right.x = c;
-		m_matrix.right.y = 0.0f;
-		m_matrix.right.z = -s;
-
-		m_matrix.up.x = 0.0f;
-		m_matrix.up.y = 1.0f;
-		m_matrix.up.z = 0.0f;
-
-		m_matrix.at.x = s;
-		m_matrix.at.y = 0.0f;
-		m_matrix.at.z = c;
-	}
-	void SetRotateY(float angle){
-		SetRotateYOnly(angle);
-		m_matrix.pos.x = 0.0f;
-		m_matrix.pos.y = 0.0f;
-		m_matrix.pos.z = 0.0f;
-	}
-	void SetRotateZOnly(float angle){
-		float c = Cos(angle);
-		float s = Sin(angle);
-
-		m_matrix.right.x = c;
-		m_matrix.right.y = s;
-		m_matrix.right.z = 0.0f;
-
-		m_matrix.up.x = -s;
-		m_matrix.up.y = c;
-		m_matrix.up.z = 0.0f;
-
-		m_matrix.at.x = 0.0f;
-		m_matrix.at.y = 0.0f;
-		m_matrix.at.z = 1.0f;
-	}
+	void SetRotateXOnly(float angle);
+	void SetRotateYOnly(float angle);
+	void SetRotateZOnly(float angle);
 	void SetRotateZOnlyScaled(float angle, float scale) {
 		float c = Cos(angle);
 		float s = Sin(angle);
@@ -219,12 +72,9 @@ public:
 		m_matrix.at.y = 0.0f;
 		m_matrix.at.z = scale;
 	}
-	void SetRotateZ(float angle){
-		SetRotateZOnly(angle);
-		m_matrix.pos.x = 0.0f;
-		m_matrix.pos.y = 0.0f;
-		m_matrix.pos.z = 0.0f;
-	}
+	void SetRotateX(float angle);
+	void SetRotateY(float angle);
+	void SetRotateZ(float angle);
 	void SetRotate(float xAngle, float yAngle, float zAngle);
 	void Rotate(float x, float y, float z);
 	void RotateX(float x);
@@ -232,34 +82,9 @@ public:
 	void RotateZ(float z);
 
 	void Reorthogonalise(void);
-	void CopyOnlyMatrix(CMatrix *other){
-		m_matrix = other->m_matrix;
-	}
-	void SetUnity(void) {
-		m_matrix.right.x = 1.0f;
-		m_matrix.right.y = 0.0f;
-		m_matrix.right.z = 0.0f;
-		m_matrix.up.x = 0.0f;
-		m_matrix.up.y = 1.0f;
-		m_matrix.up.z = 0.0f;
-		m_matrix.at.x = 0.0f;
-		m_matrix.at.y = 0.0f;
-		m_matrix.at.z = 1.0f;
-		m_matrix.pos.x = 0.0f;
-		m_matrix.pos.y = 0.0f;
-		m_matrix.pos.z = 0.0f;
-	}
-	void ResetOrientation(void) {
-		m_matrix.right.x = 1.0f;
-		m_matrix.right.y = 0.0f;
-		m_matrix.right.z = 0.0f;
-		m_matrix.up.x = 0.0f;
-		m_matrix.up.y = 1.0f;
-		m_matrix.up.z = 0.0f;
-		m_matrix.at.x = 0.0f;
-		m_matrix.at.y = 0.0f;
-		m_matrix.at.z = 1.0f;
-	}
+	void CopyOnlyMatrix(CMatrix *other);
+	void SetUnity(void);
+	void ResetOrientation(void);
 	void SetTranslateOnly(float x, float y, float z) {
 		m_matrix.pos.x = x;
 		m_matrix.pos.y = y;
@@ -268,11 +93,12 @@ public:
 	void SetTranslateOnly(const CVector& pos) {
 		SetTranslateOnly(pos.x, pos.y, pos.z);
 	}
+	void CheckIntegrity(){}
 };
 
 
 CMatrix &Invert(const CMatrix &src, CMatrix &dst);
-CVector operator*(const CMatrix &mat, const CVector &vec);
+CMatrix Invert(const CMatrix &matrix);
 CMatrix operator*(const CMatrix &m1, const CMatrix &m2);
 inline CVector MultiplyInverse(const CMatrix &mat, const CVector &vec)
 {
@@ -283,15 +109,6 @@ inline CVector MultiplyInverse(const CMatrix &mat, const CVector &vec)
 		mat.m_matrix.at.x * v.x + mat.m_matrix.at.y * v.y + mat.m_matrix.at.z * v.z);
 }
 
-const CVector Multiply3x3(const CMatrix &mat, const CVector &vec);
-const CVector Multiply3x3(const CVector &vec, const CMatrix &mat);
-
-inline CMatrix
-Invert(const CMatrix &matrix)
-{
-	CMatrix inv;
-	return Invert(matrix, inv);
-}
 
 
 class CCompressedMatrixNotAligned
@@ -304,28 +121,8 @@ class CCompressedMatrixNotAligned
 	int8 m_upY;
 	int8 m_upZ;
 public:
-	void CompressFromFullMatrix(CMatrix &other)
-	{
-		m_rightX = 127.0f * other.GetRight().x;
-		m_rightY = 127.0f * other.GetRight().y;
-		m_rightZ = 127.0f * other.GetRight().z;
-		m_upX = 127.0f * other.GetForward().x;
-		m_upY = 127.0f * other.GetForward().y;
-		m_upZ = 127.0f * other.GetForward().z;
-		m_vecPos = other.GetPosition();
-	}
-	void DecompressIntoFullMatrix(CMatrix &other)
-	{
-		other.GetRight().x = m_rightX / 127.0f;
-		other.GetRight().y = m_rightY / 127.0f;
-		other.GetRight().z = m_rightZ / 127.0f;
-		other.GetForward().x = m_upX / 127.0f;
-		other.GetForward().y = m_upY / 127.0f;
-		other.GetForward().z = m_upZ / 127.0f;
-		other.GetUp() = CrossProduct(other.GetRight(), other.GetForward());
-		other.GetPosition() = m_vecPos;
-		other.Reorthogonalise();
-	}
+	void CompressFromFullMatrix(CMatrix &other);
+	void DecompressIntoFullMatrix(CMatrix &other);
 };
 
 class CCompressedMatrix : public CCompressedMatrixNotAligned
diff --git a/src/math/Quaternion.cpp b/src/math/Quaternion.cpp
new file mode 100644
index 00000000..b0e782e2
--- /dev/null
+++ b/src/math/Quaternion.cpp
@@ -0,0 +1,177 @@
+#include "common.h"
+#include "Quaternion.h"
+
+void
+CQuaternion::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;
+	}
+}
+
+void
+CQuaternion::Slerp(const CQuaternion &q1, const CQuaternion &q2, float theta, float invSin, float t)
+{
+	if (theta == 0.0f)
+		*this = q2;
+	else {
+		float w1, w2;
+		if (theta > PI / 2) {
+			theta = PI - theta;
+			w1 = Sin((1.0f - t) * theta) * invSin;
+			w2 = -Sin(t * theta) * invSin;
+		} else {
+			w1 = Sin((1.0f - t) * theta) * invSin;
+			w2 = Sin(t * theta) * invSin;
+		}
+		// TODO: VU0 code
+		*this = w1 * q1 + w2 * q2;
+	}
+}
+
+void
+CQuaternion::Multiply(const CQuaternion &q1, const CQuaternion &q2)
+{
+	x = (q2.z * q1.y) - (q1.z * q2.y) + (q1.x * q2.w) + (q2.x * q1.w);
+	y = (q2.x * q1.z) - (q1.x * q2.z) + (q1.y * q2.w) + (q2.y * q1.w);
+	z = (q2.y * q1.x) - (q1.y * q2.x) + (q1.z * q2.w) + (q2.z * q1.w);
+	w = (q2.w * q1.w) - (q2.x * q1.x) - (q2.y * q1.y) - (q2.z * q1.z);
+}
+
+void
+CQuaternion::Get(RwV3d *axis, float *angle)
+{
+	*angle = Acos(w);
+	float s = Sin(*angle);
+
+	axis->x = x * (1.0f / s);
+	axis->y = y * (1.0f / s);
+	axis->z = z * (1.0f / s);
+}
+
+void
+CQuaternion::Set(RwV3d *axis, float angle)
+{
+	float halfCos = Cos(angle * 0.5f);
+	float halfSin = Sin(angle * 0.5f);
+	x = axis->x * halfSin;
+	y = axis->y * halfSin;
+	z = axis->z * halfSin;
+	w = halfCos;
+}
+
+void
+CQuaternion::Get(RwMatrix *matrix)
+{
+	float x2 = x + x;
+	float y2 = y + y;
+	float z2 = z + z;
+
+	float x_2x = x * x2;
+	float x_2y = x * y2;
+	float x_2z = x * z2;
+	float y_2y = y * y2;
+	float y_2z = y * z2;
+	float z_2z = z * z2;
+	float w_2x = w * x2;
+	float w_2y = w * y2;
+	float w_2z = w * z2;
+
+	matrix->right.x = 1.0f - (y_2y + z_2z);
+	matrix->up.x = x_2y - w_2z;
+	matrix->at.x = x_2z + w_2y;
+	matrix->right.y = x_2y + w_2z;
+	matrix->up.y = 1.0f - (x_2x + z_2z);
+	matrix->at.y = y_2z - w_2x;
+	matrix->right.z = x_2z - w_2y;
+	matrix->up.z = y_2z + w_2x;
+	matrix->at.z = 1.0f - (x_2x + y_2y);
+}
+
+void
+CQuaternion::Set(const RwMatrix &matrix)
+{
+	float f, s, m;
+
+	f = matrix.up.y + matrix.right.x + matrix.at.z;
+	if (f >= 0.0f) {
+		s = Sqrt(f + 1.0f);
+		w = 0.5f * s;
+		m = 0.5f / s;
+		x = (matrix.up.z - matrix.at.y) * m;
+		y = (matrix.at.x - matrix.right.z) * m;
+		z = (matrix.right.y - matrix.up.x) * m;
+		return;
+	}
+
+	f = matrix.right.x - matrix.up.y - matrix.at.z;
+	if (f >= 0.0f) {
+		s = Sqrt(f + 1.0f);
+		x = 0.5f * s;
+		m = 0.5f / s;
+		y = (matrix.up.x + matrix.right.y) * m;
+		z = (matrix.at.x + matrix.right.z) * m;
+		w = (matrix.up.z - matrix.at.y) * m;
+		return;
+	}
+
+	f = matrix.up.y - matrix.right.x - matrix.at.z;
+	if (f >= 0.0f) {
+		s = Sqrt(f + 1.0f);
+		y = 0.5f * s;
+		m = 0.5f / s;
+		w = (matrix.at.x - matrix.right.z) * m;
+		x = (matrix.up.x - matrix.right.y) * m;
+		z = (matrix.at.y + matrix.up.z) * m;
+		return;
+	}
+
+	f = matrix.at.z - (matrix.up.y + matrix.right.x);
+	s = Sqrt(f + 1.0f);
+	z = 0.5f * s;
+	m = 0.5f / s;
+	w = (matrix.right.y - matrix.up.x) * m;
+	x = (matrix.at.x + matrix.right.z) * m;
+	y = (matrix.at.y + matrix.up.z) * m;
+}
+
+void
+CQuaternion::Get(float *f1, float *f2, float *f3)
+{
+	RwMatrix matrix;
+
+	Get(&matrix);
+	*f3 = Atan2(matrix.right.y, matrix.up.y);
+	if (*f3 < 0.0f)
+		*f3 += TWOPI;
+	float s = Sin(*f3);
+	float c = Cos(*f3);
+	*f1 = Atan2(-matrix.at.y, s * matrix.right.y + c * matrix.up.y);
+	if (*f1 < 0.0f)
+		*f1 += TWOPI;
+	*f2 = Atan2(-(matrix.right.z * c - matrix.up.z * s), matrix.right.x * c - matrix.up.x * s);
+	if (*f2 < 0.0f)
+		*f2 += TWOPI;
+}
+
+void
+CQuaternion::Set(float f1, float f2, float f3)
+{
+	float c1 = Cos(f1 * 0.5f);
+	float c2 = Cos(f2 * 0.5f);
+	float c3 = Cos(f3 * 0.5f);
+	float s1 = Sin(f1 * 0.5f);
+	float s2 = Sin(f2 * 0.5f);
+	float s3 = Sin(f3 * 0.5f);
+	x = ((c2 * c1) * s3) - ((s2 * s1) * c3);
+	y = ((s1 * c2) * c3) + ((s2 * c1) * s3);
+	z = ((s2 * c1) * c3) - ((s1 * c2) * s3);
+	w = ((c2 * c1) * c3) + ((s2 * s1) * s3);
+}
\ No newline at end of file
diff --git a/src/math/Quaternion.h b/src/math/Quaternion.h
index dac49362..a5a34626 100644
--- a/src/math/Quaternion.h
+++ b/src/math/Quaternion.h
@@ -10,18 +10,8 @@ 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;
-		}
-	}
+	void Normalise(void);
+	void Multiply(const CQuaternion &q1, const CQuaternion &q2);
 
 	const CQuaternion &operator+=(CQuaternion const &right) {
 		x += right.x;
@@ -60,8 +50,12 @@ public:
 	}
 
 	void Slerp(const CQuaternion &q1, const CQuaternion &q2, float theta, float invSin, float t);
+	void Get(RwV3d *axis, float *angle);
 	void Set(RwV3d *axis, float angle);
 	void Get(RwMatrix *matrix);
+	void Set(const RwMatrix &matrix);
+	void Set(float f1, float f2, float f3);
+	void Get(float *f1, float *f2, float *f3);
 };
 
 inline float
diff --git a/src/math/Rect.cpp b/src/math/Rect.cpp
new file mode 100644
index 00000000..de6320ad
--- /dev/null
+++ b/src/math/Rect.cpp
@@ -0,0 +1,17 @@
+#include "common.h"
+
+CRect::CRect(void)
+{
+	left = 1000000.0f;
+	top = 1000000.0f;
+	right = -1000000.0f;
+	bottom = -1000000.0f;
+}
+
+CRect::CRect(float l, float t, float r, float b)
+{
+	left = l;
+	top = t;
+	right = r;
+	bottom = b;
+}
\ No newline at end of file
diff --git a/src/math/Rect.h b/src/math/Rect.h
index 326bb479..fa8d8de4 100644
--- a/src/math/Rect.h
+++ b/src/math/Rect.h
@@ -8,18 +8,8 @@ public:
 	float right;    // x max
 	float top;      // y min
 
-	CRect(void){
-		left = 1000000.0f;
-		top = 1000000.0f;
-		right = -1000000.0f;
-		bottom = -1000000.0f;
-	}
-	CRect(float l, float t, float r, float b){
-		left = l;
-		top = t;
-		right = r;
-		bottom = b;
-	}
+	CRect(void);
+	CRect(float l, float t, float r, float b);
 	void ContainPoint(CVector const &v){
 		if(v.x < left) left = v.x;
 		if(v.x > right) right = v.x;
diff --git a/src/math/Vector.cpp b/src/math/Vector.cpp
new file mode 100644
index 00000000..42e1828e
--- /dev/null
+++ b/src/math/Vector.cpp
@@ -0,0 +1,46 @@
+#include "common.h"
+
+void
+CVector::Normalise(void)
+{
+	float sq = MagnitudeSqr();
+	if (sq > 0.0f) {
+		float invsqrt = RecipSqrt(sq);
+		x *= invsqrt;
+		y *= invsqrt;
+		z *= invsqrt;
+	} else
+		x = 1.0f;
+}
+
+CVector
+CrossProduct(const CVector &v1, const CVector &v2)
+{
+	return CVector(v1.y * v2.z - v1.z * v2.y, v1.z * v2.x - v1.x * v2.z, v1.x * v2.y - v1.y * v2.x);
+}
+
+CVector
+Multiply3x3(const CMatrix &mat, const CVector &vec)
+{
+	// TODO: VU0 code
+	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);
+}
+
+CVector
+operator*(const CMatrix &mat, const CVector &vec)
+{
+	// TODO: VU0 code
+	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);
+}
diff --git a/src/math/Vector.h b/src/math/Vector.h
index 5918a5d1..7ee01149 100644
--- a/src/math/Vector.h
+++ b/src/math/Vector.h
@@ -24,24 +24,7 @@ public:
 	float MagnitudeSqr(void) const { return x*x + y*y + z*z; }
 	float Magnitude2D(void) const { return Sqrt(x*x + y*y); }
 	float MagnitudeSqr2D(void) const { return x*x + y*y; }
-	void Normalise(void) {
-		float sq = MagnitudeSqr();
-		if(sq > 0.0f){
-			float invsqrt = RecipSqrt(sq);
-			x *= invsqrt;
-			y *= invsqrt;
-			z *= invsqrt;
-		}else
-			x = 1.0f;
-	}
-	
-	void Normalise(float norm) {
-		float sq = MagnitudeSqr();
-		float invsqrt = RecipSqrt(norm, sq);
-		x *= invsqrt;
-		y *= invsqrt;
-		z *= invsqrt;
-	}
+	void Normalise(void);
 	
 	void Normalise2D(void) {
 		float sq = MagnitudeSqr2D();
@@ -124,17 +107,16 @@ DotProduct(const CVector &v1, const CVector &v2)
 	return v1.x*v2.x + v1.y*v2.y + v1.z*v2.z;
 }
 
-inline const CVector
-CrossProduct(const CVector &v1, const CVector &v2)
-{
-	return CVector(
-		v1.y*v2.z - v1.z*v2.y,
-		v1.z*v2.x - v1.x*v2.z,
-		v1.x*v2.y - v1.y*v2.x);
-}
+CVector CrossProduct(const CVector &v1, const CVector &v2);
 
 inline float
 Distance(const CVector &v1, const CVector &v2)
 {
 	return (v2 - v1).Magnitude();
-}
\ No newline at end of file
+}
+
+class CMatrix;
+
+CVector Multiply3x3(const CMatrix &mat, const CVector &vec);
+CVector Multiply3x3(const CVector &vec, const CMatrix &mat);
+CVector operator*(const CMatrix &mat, const CVector &vec);
\ No newline at end of file
diff --git a/src/math/Vector2D.h b/src/math/Vector2D.h
index 0885a5d2..0235dbe5 100644
--- a/src/math/Vector2D.h
+++ b/src/math/Vector2D.h
@@ -11,7 +11,13 @@ 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) {
+		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();
@@ -20,7 +26,7 @@ public:
 			x *= invsqrt;
 			y *= invsqrt;
 		}else
-			y = 1.0f;
+			x = 1.0f;
 	}
 
 	const CVector2D &operator+=(CVector2D const &right) {
diff --git a/src/math/math.cpp b/src/math/math.cpp
index 4792fdd0..8cb56dab 100644
--- a/src/math/math.cpp
+++ b/src/math/math.cpp
@@ -1,6 +1,5 @@
 #include "common.h"
 
-#include "Quaternion.h"
 #include "VuVector.h"
 
 // TODO: move more stuff into here
@@ -117,235 +116,3 @@ void TransformPoints(CVuVector *out, int n, const CMatrix &mat, const CVuVector
 	}
 #endif
 }
-
-
-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);
-	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::RotateX(float x)
-{
-	Rotate(x, 0.0f, 0.0f);
-}
-
-void
-CMatrix::RotateY(float y)
-{
-	Rotate(0.0f, y, 0.0f);
-}
-
-void
-CMatrix::RotateZ(float z)
-{
-	Rotate(0.0f, 0.0f, z);
-}
-
-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)
-{
-	// TODO: VU0 code
-	// 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)
-{
-	// TODO: VU0 code
-	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)
-{
-	// TODO: VU0 code
-	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;
-}
-
-CMatrix&
-CMatrix::operator*=(CMatrix const &rhs)
-{
-	// TODO: VU0 code
-	*this = *this * rhs;
-	return *this;
-}
-
-const CVector
-Multiply3x3(const CMatrix &mat, const CVector &vec)
-{
-	// TODO: VU0 code
-	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);
-}
-
-const 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)
-		*this = q2;
-	else{
-		float w1, w2;
-		if(theta > PI/2){
-			theta = PI - theta;
-			w1 = Sin((1.0f - t) * theta) * invSin;
-			w2 = -Sin(t * theta) * invSin;
-		}else{
-			w1 = Sin((1.0f - t) * theta) * invSin;
-			w2 = Sin(t * theta) * invSin;
-		}
-		// TODO: VU0 code
-		*this = w1*q1 + w2*q2;
-	}
-}
-
-void
-CQuaternion::Set(RwV3d *axis, float angle)
-{
-	float halfCos = Cos(angle*0.5f);
-	float halfSin = Sin(angle*0.5f);
-	x = axis->x*halfSin;
-	y = axis->y*halfSin;
-	z = axis->z*halfSin;
-	w = halfCos;
-}
-
-void
-CQuaternion::Get(RwMatrix *matrix)
-{
-	float x2 = x+x;
-	float y2 = y+y;
-	float z2 = z+z;
-
-	float x_2x = x * x2;
-	float x_2y = x * y2;
-	float x_2z = x * z2;
-	float y_2y = y * y2;
-	float y_2z = y * z2;
-	float z_2z = z * z2;
-	float w_2x = w * x2;
-	float w_2y = w * y2;
-	float w_2z = w * z2;
-
-	matrix->right.x = 1.0f - (y_2y + z_2z);
-	matrix->up.x = x_2y - w_2z;
-	matrix->at.x = x_2z + w_2y;
-	matrix->right.y = x_2y + w_2z;
-	matrix->up.y = 1.0f - (x_2x + z_2z);
-	matrix->at.y = y_2z - w_2x;
-	matrix->right.z = x_2z - w_2y;
-	matrix->up.z = y_2z + w_2x;
-	matrix->at.z = 1.0f - (x_2x + y_2y);
-}
diff --git a/src/render/Glass.cpp b/src/render/Glass.cpp
index ee36bad3..c52d49bd 100644
--- a/src/render/Glass.cpp
+++ b/src/render/Glass.cpp
@@ -700,7 +700,7 @@ CGlass::WindowRespondsToExplosion(CEntity *entity, CVector point)
 
 	if ( fDistToGlass < 10.0f )
 	{
-		distToGlass.Normalise(0.3f);
+		distToGlass *= (0.3f / fDistToGlass); // normalise
 		WindowRespondsToCollision(object, 10000.0f, distToGlass, object->GetPosition(), true);
 	}
 	else
diff --git a/src/render/Skidmarks.cpp b/src/render/Skidmarks.cpp
index 5d521041..ad036d58 100644
--- a/src/render/Skidmarks.cpp
+++ b/src/render/Skidmarks.cpp
@@ -222,10 +222,11 @@ CSkidmarks::RegisterOne(uintptr id, CVector pos, float fwdX, float fwdY, bool *i
 
 		aSkidmarks[i].m_pos[aSkidmarks[i].m_last] = pos;
 
-		CVector2D dist = aSkidmarks[i].m_pos[aSkidmarks[i].m_last] - aSkidmarks[i].m_pos[aSkidmarks[i].m_last-1];
-		dist.NormaliseSafe();
+		CVector2D right(aSkidmarks[i].m_pos[aSkidmarks[i].m_last].y - aSkidmarks[i].m_pos[aSkidmarks[i].m_last - 1].y,
+		                aSkidmarks[i].m_pos[aSkidmarks[i].m_last - 1].x - aSkidmarks[i].m_pos[aSkidmarks[i].m_last].x);
+
+		right.NormaliseSafe();
 		fwd.NormaliseSafe();
-		CVector2D right(dist.y, -dist.x);
 		float turn = DotProduct2D(fwd, right);
 		turn = Abs(turn) + 1.0f;
 		aSkidmarks[i].m_side[aSkidmarks[i].m_last] = CVector(right.x, right.y, 0.0f) * turn * 0.125f;
diff --git a/src/render/WaterCannon.cpp b/src/render/WaterCannon.cpp
index 2ef10d77..2b34db37 100644
--- a/src/render/WaterCannon.cpp
+++ b/src/render/WaterCannon.cpp
@@ -140,8 +140,7 @@ void CWaterCannon::Render(void)
 			if ( !bInit )
 			{
 				CVector cp = CrossProduct(m_avecPos[pointB] - m_avecPos[pointA], TheCamera.GetForward());
-				cp.Normalise(0.05f);
-				norm = cp;
+				norm = cp * (0.05f / cp.Magnitude());
 				bInit = true;
 			}