8 files changed, 321 insertions, 280 deletions

diff --git a/src/control/Replay.cpp b/src/control/Replay.cpp
index 2e87d1a7..2dd66333 100644
--- a/src/control/Replay.cpp
+++ b/src/control/Replay.cpp
@@ -305,7 +305,7 @@ void CReplay::RecordThisFrame(void)
 #endif
 	tGeneralPacket* general = (tGeneralPacket*)&Record.m_pBase[Record.m_nOffset];
 	general->type = REPLAYPACKET_GENERAL;
-	general->camera_pos.CopyOnlyMatrix(&TheCamera.GetMatrix());
+	general->camera_pos.CopyOnlyMatrix(TheCamera.GetMatrix());
 	general->player_pos = FindPlayerCoors();
 	general->in_rcvehicle = CWorld::Players[CWorld::PlayerInFocus].m_pRemoteVehicle ? true : false;
 	Record.m_nOffset += sizeof(*general);
diff --git a/src/core/Camera.cpp b/src/core/Camera.cpp
index 7a831068..9d169716 100644
--- a/src/core/Camera.cpp
+++ b/src/core/Camera.cpp
@@ -3629,9 +3629,17 @@ CCamera::CalculateDerivedValues(void)
 bool
 CCamera::IsPointVisible(const CVector &center, const CMatrix *mat)
 {
-	RwV3d c;
-	c = center;
-	RwV3dTransformPoints(&c, &c, 1, &mat->m_matrix);
+#ifdef GTA_PS2
+	CVuVector c;
+	TransformPoint(c, *mat, center);
+#else
+	CVector c = center;
+	#ifdef FIX_BUGS
+		c = *mat * center;
+	#else
+		RwV3dTransformPoints(&c, &c, 1, (RwMatrix*)mat);
+	#endif
+#endif
 	if(c.y < CDraw::GetNearClipZ()) return false;
 	if(c.y > CDraw::GetFarClipZ()) return false;
 	if(c.x*m_vecFrustumNormals[0].x + c.y*m_vecFrustumNormals[0].y > 0.0f) return false;
@@ -3644,9 +3652,17 @@ CCamera::IsPointVisible(const CVector &center, const CMatrix *mat)
 bool
 CCamera::IsSphereVisible(const CVector &center, float radius, const CMatrix *mat)
 {
-	RwV3d c;
-	c = center;
-	RwV3dTransformPoints(&c, &c, 1, &mat->m_matrix);
+#ifdef GTA_PS2
+	CVuVector c;
+	TransformPoint(c, *mat, center);
+#else
+	CVector c = center;
+	#ifdef FIX_BUGS
+		c = *mat * center;
+	#else
+		RwV3dTransformPoints(&c, &c, 1, (RwMatrix*)mat);
+	#endif
+#endif
 	if(c.y + radius < CDraw::GetNearClipZ()) return false;
 	if(c.y - radius > CDraw::GetFarClipZ()) return false;
 	if(c.x*m_vecFrustumNormals[0].x + c.y*m_vecFrustumNormals[0].y > radius) return false;
@@ -3664,11 +3680,24 @@ CCamera::IsSphereVisible(const CVector &center, float radius)
 }
 
 bool
-CCamera::IsBoxVisible(RwV3d *box, const CMatrix *mat)
+#ifdef GTA_PS2
+CCamera::IsBoxVisible(CVuVector *box, const CMatrix *mat)
+#else
+CCamera::IsBoxVisible(CVector *box, const CMatrix *mat)
+#endif
 {
 	int i;
 	int frustumTests[6] = { 0 };
-	RwV3dTransformPoints(box, box, 8, &mat->m_matrix);
+#ifdef GTA_PS2
+	TransformPoints(box, 8, *mat, box);
+#else
+	#ifdef FIX_BUGS
+		for (i = 0; i < 8; i++)
+			box[i] = *mat * box[i];
+	#else
+		RwV3dTransformPoints(box, box, 8, (RwMatrix*)mat);
+	#endif
+#endif
 
 	for(i = 0; i < 8; i++){
 		if(box[i].y < CDraw::GetNearClipZ()) frustumTests[0]++;
diff --git a/src/core/Camera.h b/src/core/Camera.h
index ca1bd135..d7293e20 100644
--- a/src/core/Camera.h
+++ b/src/core/Camera.h
@@ -641,7 +641,11 @@ public:
 	bool IsPointVisible(const CVector &center, const CMatrix *mat);
 	bool IsSphereVisible(const CVector &center, float radius, const CMatrix *mat);
 	bool IsSphereVisible(const CVector &center, float radius);
-	bool IsBoxVisible(RwV3d *box, const CMatrix *mat);
+#ifdef GTA_PS2
+	bool IsBoxVisible(CVuVector *box, const CMatrix *mat);
+#else
+	bool IsBoxVisible(CVector *box, const CMatrix *mat);
+#endif
 };
 
 VALIDATE_SIZE(CCamera, 0xE9D8);
diff --git a/src/entities/Entity.cpp b/src/entities/Entity.cpp
index 709f91c1..4885d631 100644
--- a/src/entities/Entity.cpp
+++ b/src/entities/Entity.cpp
@@ -407,7 +407,11 @@ CEntity::GetIsOnScreen(void)
 bool
 CEntity::GetIsOnScreenComplex(void)
 {
-	RwV3d boundBox[8];
+#ifdef GTA_PS2
+	CVuVector boundBox[8];
+#else
+	CVector boundBox[8];
+#endif
 
 	if(TheCamera.IsPointVisible(GetBoundCentre(), &TheCamera.GetCameraMatrix()))
 		return true;
diff --git a/src/math/Matrix.cpp b/src/math/Matrix.cpp
index a8b1b182..bee3ffba 100644
--- a/src/math/Matrix.cpp
+++ b/src/math/Matrix.cpp
@@ -60,14 +60,20 @@ CMatrix::Detach(void)
 void
 CMatrix::Update(void)
 {
-	m_matrix = *m_attachment;
+	right = m_attachment->right;
+	forward = m_attachment->up;
+	up = m_attachment->at;
+	pos = m_attachment->pos;
 }
 
 void
 CMatrix::UpdateRW(void)
 {
 	if (m_attachment) {
-		*m_attachment = m_matrix;
+		m_attachment->right = right;
+		m_attachment->up = forward;
+		m_attachment->at = up;
+		m_attachment->pos = pos;
 		RwMatrixUpdate(m_attachment);
 	}
 }
@@ -75,104 +81,96 @@ CMatrix::UpdateRW(void)
 void
 CMatrix::operator=(CMatrix const &rhs)
 {
-	m_matrix = rhs.m_matrix;
+	memcpy(this, &rhs, sizeof(f));
 	if (m_attachment)
 		UpdateRW();
 }
 
 void
-CMatrix::CopyOnlyMatrix(CMatrix *other)
+CMatrix::CopyOnlyMatrix(const CMatrix &other)
 {
-	m_matrix = other->m_matrix;
+	memcpy(this, &other, sizeof(f));
 }
 
 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;
+	right += rhs.right;
+	forward += rhs.forward;
+	up += rhs.up;
+	pos += rhs.pos;
 	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;
+	right.x = 1.0f;
+	right.y = 0.0f;
+	right.z = 0.0f;
+	forward.x = 0.0f;
+	forward.y = 1.0f;
+	forward.z = 0.0f;
+	up.x = 0.0f;
+	up.y = 0.0f;
+	up.z = 1.0f;
+	pos.x = 0.0f;
+	pos.y = 0.0f;
+	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;
+	right.x = 1.0f;
+	right.y = 0.0f;
+	right.z = 0.0f;
+	forward.x = 0.0f;
+	forward.y = 1.0f;
+	forward.z = 0.0f;
+	up.x = 0.0f;
+	up.y = 0.0f;
+	up.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;
+	right.x = s;
+	right.y = 0.0f;
+	right.z = 0.0f;
 
-	m_matrix.up.x = 0.0f;
-	m_matrix.up.y = s;
-	m_matrix.up.z = 0.0f;
+	forward.x = 0.0f;
+	forward.y = s;
+	forward.z = 0.0f;
 
-	m_matrix.at.x = 0.0f;
-	m_matrix.at.y = 0.0f;
-	m_matrix.at.z = s;
+	up.x = 0.0f;
+	up.y = 0.0f;
+	up.z = s;
 
-	m_matrix.pos.x = 0.0f;
-	m_matrix.pos.y = 0.0f;
-	m_matrix.pos.z = 0.0f;
+	pos.x = 0.0f;
+	pos.y = 0.0f;
+	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;
+	right.x = 1.0f;
+	right.y = 0.0f;
+	right.z = 0.0f;
 
-	m_matrix.up.x = 0.0f;
-	m_matrix.up.y = 1.0f;
-	m_matrix.up.z = 0.0f;
+	forward.x = 0.0f;
+	forward.y = 1.0f;
+	forward.z = 0.0f;
 
-	m_matrix.at.x = 0.0f;
-	m_matrix.at.y = 0.0f;
-	m_matrix.at.z = 1.0f;
+	up.x = 0.0f;
+	up.y = 0.0f;
+	up.z = 1.0f;
 
-	m_matrix.pos.x = x;
-	m_matrix.pos.y = y;
-	m_matrix.pos.z = z;
+	pos.x = x;
+	pos.y = y;
+	pos.z = z;
 }
 
 void
@@ -181,17 +179,17 @@ 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;
+	right.x = 1.0f;
+	right.y = 0.0f;
+	right.z = 0.0f;
 
-	m_matrix.up.x = 0.0f;
-	m_matrix.up.y = c;
-	m_matrix.up.z = s;
+	forward.x = 0.0f;
+	forward.y = c;
+	forward.z = s;
 
-	m_matrix.at.x = 0.0f;
-	m_matrix.at.y = -s;
-	m_matrix.at.z = c;
+	up.x = 0.0f;
+	up.y = -s;
+	up.z = c;
 }
 
 void
@@ -200,17 +198,17 @@ 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;
+	right.x = c;
+	right.y = 0.0f;
+	right.z = -s;
 
-	m_matrix.up.x = 0.0f;
-	m_matrix.up.y = 1.0f;
-	m_matrix.up.z = 0.0f;
+	forward.x = 0.0f;
+	forward.y = 1.0f;
+	forward.z = 0.0f;
 
-	m_matrix.at.x = s;
-	m_matrix.at.y = 0.0f;
-	m_matrix.at.z = c;
+	up.x = s;
+	up.y = 0.0f;
+	up.z = c;
 }
 
 void
@@ -219,26 +217,26 @@ 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;
+	right.x = c;
+	right.y = s;
+	right.z = 0.0f;
 
-	m_matrix.up.x = -s;
-	m_matrix.up.y = c;
-	m_matrix.up.z = 0.0f;
+	forward.x = -s;
+	forward.y = c;
+	forward.z = 0.0f;
 
-	m_matrix.at.x = 0.0f;
-	m_matrix.at.y = 0.0f;
-	m_matrix.at.z = 1.0f;
+	up.x = 0.0f;
+	up.y = 0.0f;
+	up.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;
+	pos.x = 0.0f;
+	pos.y = 0.0f;
+	pos.z = 0.0f;
 }
 
 
@@ -246,18 +244,18 @@ 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;
+	pos.x = 0.0f;
+	pos.y = 0.0f;
+	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;
+	pos.x = 0.0f;
+	pos.y = 0.0f;
+	pos.z = 0.0f;
 }
 
 void
@@ -270,21 +268,21 @@ CMatrix::SetRotate(float xAngle, float yAngle, float zAngle)
 	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;
+	right.x = cZ * cY - (sZ * sX) * sY;
+	right.y = (cZ * sX) * sY + sZ * cY;
+	right.z = -cX * sY;
 
-	m_matrix.up.x = -sZ * cX;
-	m_matrix.up.y = cZ * cX;
-	m_matrix.up.z = sX;
+	forward.x = -sZ * cX;
+	forward.y = cZ * cX;
+	forward.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;
+	up.x = (sZ * sX) * cY + cZ * sY;
+	up.y = sZ * sY - (cZ * sX) * cY;
+	up.z = cX * cY;
 
-	m_matrix.pos.x = 0.0f;
-	m_matrix.pos.y = 0.0f;
-	m_matrix.pos.z = 0.0f;
+	pos.x = 0.0f;
+	pos.y = 0.0f;
+	pos.z = 0.0f;
 }
 
 void
@@ -293,23 +291,23 @@ 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;
+	float ry = right.y;
+	float rz = right.z;
+	float uy = forward.y;
+	float uz = forward.z;
+	float ay = up.y;
+	float az = up.z;
+	float py = pos.y;
+	float pz = pos.z;
+
+	right.y = c * ry - s * rz;
+	right.z = c * rz + s * ry;
+	forward.y = c * uy - s * uz;
+	forward.z = c * uz + s * uy;
+	up.y = c * ay - s * az;
+	up.z = c * az + s * ay;
+	pos.y = c * py - s * pz;
+	pos.z = c * pz + s * py;
 }
 
 void
@@ -318,23 +316,23 @@ 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;
+	float rx = right.x;
+	float rz = right.z;
+	float ux = forward.x;
+	float uz = forward.z;
+	float ax = up.x;
+	float az = up.z;
+	float px = pos.x;
+	float pz = pos.z;
+
+	right.x = c * rx + s * rz;
+	right.z = c * rz - s * rx;
+	forward.x = c * ux + s * uz;
+	forward.z = c * uz - s * ux;
+	up.x = c * ax + s * az;
+	up.z = c * az - s * ax;
+	pos.x = c * px + s * pz;
+	pos.z = c * pz - s * px;
 }
 
 void
@@ -343,23 +341,23 @@ 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;
+	float ry = right.y;
+	float rx = right.x;
+	float uy = forward.y;
+	float ux = forward.x;
+	float ay = up.y;
+	float ax = up.x;
+	float py = pos.y;
+	float px = 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;
+	right.x = c * rx - s * ry;
+	right.y = c * ry + s * rx;
+	forward.x = c * ux - s * uy;
+	forward.y = c * uy + s * ux;
+	up.x = c * ax - s * ay;
+	up.y = c * ay + s * ax;
+	pos.x = c * px - s * py;
+	pos.y = c * py + s * px;
 
 }
 
@@ -373,18 +371,18 @@ CMatrix::Rotate(float x, float y, float z)
 	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 rx = right.x;
+	float ry = right.y;
+	float rz = right.z;
+	float ux = forward.x;
+	float uy = forward.y;
+	float uz = forward.z;
+	float ax = up.x;
+	float ay = up.y;
+	float az = up.z;
+	float px = pos.x;
+	float py = pos.y;
+	float pz = pos.z;
 
 	float x1 = cZ * cY - (sZ * sX) * sY;
 	float x2 = (cZ * sX) * sY + sZ * cY;
@@ -396,18 +394,18 @@ CMatrix::Rotate(float x, float y, float z)
 	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;
+	right.x = x1 * rx + y1 * ry + z1 * rz;
+	right.y = x2 * rx + y2 * ry + z2 * rz;
+	right.z = x3 * rx + y3 * ry + z3 * rz;
+	forward.x = x1 * ux + y1 * uy + z1 * uz;
+	forward.y = x2 * ux + y2 * uy + z2 * uz;
+	forward.z = x3 * ux + y3 * uy + z3 * uz;
+	up.x = x1 * ax + y1 * ay + z1 * az;
+	up.y = x2 * ax + y2 * ay + z2 * az;
+	up.z = x3 * ax + y3 * ay + z3 * az;
+	pos.x = x1 * px + y1 * py + z1 * pz;
+	pos.y = x2 * px + y2 * py + z2 * pz;
+	pos.z = x3 * px + y3 * py + z3 * pz;
 }
 
 CMatrix &
@@ -436,21 +434,18 @@ 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;
+	out.right.x = m1.right.x * m2.right.x + m1.forward.x * m2.right.y + m1.up.x * m2.right.z;
+	out.right.y = m1.right.y * m2.right.x + m1.forward.y * m2.right.y + m1.up.y * m2.right.z;
+	out.right.z = m1.right.z * m2.right.x + m1.forward.z * m2.right.y + m1.up.z * m2.right.z;
+	out.forward.x = m1.right.x * m2.forward.x + m1.forward.x * m2.forward.y + m1.up.x * m2.forward.z;
+	out.forward.y = m1.right.y * m2.forward.x + m1.forward.y * m2.forward.y + m1.up.y * m2.forward.z;
+	out.forward.z = m1.right.z * m2.forward.x + m1.forward.z * m2.forward.y + m1.up.z * m2.forward.z;
+	out.up.x = m1.right.x * m2.up.x + m1.forward.x * m2.up.y + m1.up.x * m2.up.z;
+	out.up.y = m1.right.y * m2.up.x + m1.forward.y * m2.up.y + m1.up.y * m2.up.z;
+	out.up.z = m1.right.z * m2.up.x + m1.forward.z * m2.up.y + m1.up.z * m2.up.z;
+	out.pos.x = m1.right.x * m2.pos.x + m1.forward.x * m2.pos.y + m1.up.x * m2.pos.z + m1.pos.x;
+	out.pos.y = m1.right.y * m2.pos.x + m1.forward.y * m2.pos.y + m1.up.y * m2.pos.z + m1.pos.y;
+	out.pos.z = m1.right.z * m2.pos.x + m1.forward.z * m2.pos.y + m1.up.z * m2.pos.z + m1.pos.z;
 	return out;
 }
 
@@ -460,59 +455,56 @@ 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;
+	dst.f[3][0] = dst.f[3][1] = dst.f[3][2] = 0.0f;
 #ifndef FIX_BUGS
-	dst_fm[3][3] = scr_fm[3][3];
+	dst.f[3][3] = src.f[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];
+	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_fm[0][3] = scr_fm[3][0];
+	dst.f[0][3] = src.f[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];
+	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_fm[1][3] = scr_fm[3][1];
+	dst.f[1][3] = src.f[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];
+	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_fm[2][3] = scr_fm[3][2];
+	dst.f[2][3] = src.f[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];
+	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_fm[3][3] += dst_fm[0][3] * scr_fm[3][0];
+	dst.f[3][3] += dst.f[0][3] * src.f[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];
+	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_fm[3][3] += dst_fm[1][3] * scr_fm[3][1];
+	dst.f[3][3] += dst.f[1][3] * src.f[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];
+	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_fm[3][3] += dst_fm[2][3] * scr_fm[3][2];
+	dst.f[3][3] += dst.f[2][3] * src.f[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];
+	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_fm[3][3] = scr_fm[3][3] - dst_fm[3][3];
+	dst.f[3][3] = src.f[3][3] - dst.f[3][3];
 #endif
 
 	return dst;
diff --git a/src/math/Matrix.h b/src/math/Matrix.h
index d8f6388d..fdbd0b60 100644
--- a/src/math/Matrix.h
+++ b/src/math/Matrix.h
@@ -1,9 +1,22 @@
 #pragma once
 
+#include "VuVector.h"
+
 class CMatrix
 {
 public:
-	RwMatrix m_matrix;
+	union
+	{
+		float f[4][4];
+		struct
+		{
+			CVuVector right;
+			CVuVector forward;
+			CVuVector up;
+			CVuVector pos;
+		};
+	};
+
 	RwMatrix *m_attachment;
 	bool m_hasRwMatrix;	// are we the owner?
 
@@ -25,31 +38,30 @@ public:
 	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; }
+	CVector &GetPosition(void){ return pos; }
+	CVector &GetRight(void) { return right; }
+	CVector &GetForward(void) { return forward; }
+	CVector &GetUp(void) { return up; }
 
 	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;
-		m_matrix.pos.y += y;
-		m_matrix.pos.z += z;
+		pos.x += x;
+		pos.y += y;
+		pos.z += z;
 	}
 	void Translate(const CVector &trans){ Translate(trans.x, trans.y, trans.z); }
 
 	void SetScale(float s);
 	void Scale(float scale)
 	{
-		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;
+				f[i][j] *= scale;
 	}
 
 
@@ -60,17 +72,17 @@ public:
 		float c = Cos(angle);
 		float s = Sin(angle);
 
-		m_matrix.right.x = c * scale;
-		m_matrix.right.y = s * scale;
-		m_matrix.right.z = 0.0f;
+		right.x = c * scale;
+		right.y = s * scale;
+		right.z = 0.0f;
 
-		m_matrix.up.x = -s * scale;
-		m_matrix.up.y = c * scale;
-		m_matrix.up.z = 0.0f;
+		forward.x = -s * scale;
+		forward.y = c * scale;
+		forward.z = 0.0f;
 
-		m_matrix.at.x = 0.0f;
-		m_matrix.at.y = 0.0f;
-		m_matrix.at.z = scale;
+		up.x = 0.0f;
+		up.y = 0.0f;
+		up.z = scale;
 	}
 	void SetRotateX(float angle);
 	void SetRotateY(float angle);
@@ -82,13 +94,13 @@ public:
 	void RotateZ(float z);
 
 	void Reorthogonalise(void);
-	void CopyOnlyMatrix(CMatrix *other);
+	void CopyOnlyMatrix(const 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;
-		m_matrix.pos.z = z;
+		pos.x = x;
+		pos.y = y;
+		pos.z = z;
 	}
 	void SetTranslateOnly(const CVector& pos) {
 		SetTranslateOnly(pos.x, pos.y, pos.z);
@@ -102,11 +114,11 @@ CMatrix Invert(const CMatrix &matrix);
 CMatrix operator*(const CMatrix &m1, const CMatrix &m2);
 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);
+	CVector v(vec.x - mat.pos.x, vec.y - mat.pos.y, vec.z - mat.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);
+		mat.right.x * v.x + mat.right.y * v.y + mat.right.z * v.z,
+		mat.forward.x * v.x + mat.forward.y * v.y + mat.forward.z * v.z,
+		mat.up.x * v.x + mat.up.y * v.y + mat.up.z * v.z);
 }
 
 
diff --git a/src/math/Vector.cpp b/src/math/Vector.cpp
index 42e1828e..3bc395d8 100644
--- a/src/math/Vector.cpp
+++ b/src/math/Vector.cpp
@@ -23,24 +23,24 @@ 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);
+	return CVector(mat.right.x * vec.x + mat.forward.x * vec.y + mat.up.x * vec.z,
+	               mat.right.y * vec.x + mat.forward.y * vec.y + mat.up.y * vec.z,
+	               mat.right.z * vec.x + mat.forward.z * vec.y + mat.up.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);
+	return CVector(mat.right.x * vec.x + mat.right.y * vec.y + mat.right.z * vec.z,
+	               mat.forward.x * vec.x + mat.forward.y * vec.y + mat.forward.z * vec.z,
+	               mat.up.x * vec.x + mat.up.y * vec.y + mat.up.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);
+	return CVector(mat.right.x * vec.x + mat.forward.x * vec.y + mat.up.x * vec.z + mat.pos.x,
+	               mat.right.y * vec.x + mat.forward.y * vec.y + mat.up.y * vec.z + mat.pos.y,
+	               mat.right.z * vec.x + mat.forward.z * vec.y + mat.up.z * vec.z + mat.pos.z);
 }
diff --git a/src/render/SpecialFX.cpp b/src/render/SpecialFX.cpp
index e3d2ffa6..13afadb6 100644
--- a/src/render/SpecialFX.cpp
+++ b/src/render/SpecialFX.cpp
@@ -587,7 +587,7 @@ C3dMarkers::PlaceMarker(uint32 identifier, uint16 type, CVector &pos, float size
 				pMarker->m_Color.alpha = (float)a * 0.4f * someSin + a;
 		}
 		if (pMarker->m_nRotateRate) {
-			RwV3d pos = pMarker->m_Matrix.m_matrix.pos;
+			CVector pos = pMarker->m_Matrix.GetPosition();
 			pMarker->m_Matrix.RotateZ(DEGTORAD(pMarker->m_nRotateRate * CTimer::GetTimeStep()));
 			pMarker->m_Matrix.GetPosition() = pos;
 		}