12 files changed, 1507 insertions, 0 deletions

diff --git a/src/math/Matrix.cpp b/src/math/Matrix.cpp
new file mode 100644
index 0000000..c0d909c
--- /dev/null
+++ b/src/math/Matrix.cpp
@@ -0,0 +1,531 @@
+#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)
+{
+	GetRight() = m_attachment->right;
+	GetForward() = m_attachment->up;
+	GetUp() = m_attachment->at;
+	GetPosition() = m_attachment->pos;
+}
+
+void
+CMatrix::UpdateRW(void)
+{
+	if (m_attachment) {
+		m_attachment->right = GetRight();
+		m_attachment->up = GetForward();
+		m_attachment->at = GetUp();
+		m_attachment->pos = GetPosition();
+		RwMatrixUpdate(m_attachment);
+	}
+}
+
+void
+CMatrix::operator=(CMatrix const &rhs)
+{
+	memcpy(this, &rhs, sizeof(f));
+	if (m_attachment)
+		UpdateRW();
+}
+
+void
+CMatrix::CopyOnlyMatrix(const CMatrix &other)
+{
+	memcpy(this, &other, sizeof(f));
+}
+
+CMatrix &
+CMatrix::operator+=(CMatrix const &rhs)
+{
+	GetRight() += rhs.GetRight();
+	GetForward() += rhs.GetForward();
+	GetUp() += rhs.GetUp();
+	GetPosition() += rhs.GetPosition();
+	return *this;
+}
+
+void
+CMatrix::SetUnity(void)
+{
+	rx = 1.0f;
+	ry = 0.0f;
+	rz = 0.0f;
+	fx = 0.0f;
+	fy = 1.0f;
+	fz = 0.0f;
+	ux = 0.0f;
+	uy = 0.0f;
+	uz = 1.0f;
+	px = 0.0f;
+	py = 0.0f;
+	pz = 0.0f;
+}
+
+void
+CMatrix::ResetOrientation(void)
+{
+	rx = 1.0f;
+	ry = 0.0f;
+	rz = 0.0f;
+	fx = 0.0f;
+	fy = 1.0f;
+	fz = 0.0f;
+	ux = 0.0f;
+	uy = 0.0f;
+	uz = 1.0f;
+}
+
+void
+CMatrix::SetScale(float s)
+{
+	rx = s;
+	ry = 0.0f;
+	rz = 0.0f;
+
+	fx = 0.0f;
+	fy = s;
+	fz = 0.0f;
+
+	ux = 0.0f;
+	uy = 0.0f;
+	uz = s;
+
+	px = 0.0f;
+	py = 0.0f;
+	pz = 0.0f;
+}
+
+void
+CMatrix::SetTranslate(float x, float y, float z)
+{
+	rx = 1.0f;
+	ry = 0.0f;
+	rz = 0.0f;
+
+	fx = 0.0f;
+	fy = 1.0f;
+	fz = 0.0f;
+
+	ux = 0.0f;
+	uy = 0.0f;
+	uz = 1.0f;
+
+	px = x;
+	py = y;
+	pz = z;
+}
+
+void
+CMatrix::SetRotateXOnly(float angle)
+{
+	float c = Cos(angle);
+	float s = Sin(angle);
+
+	rx = 1.0f;
+	ry = 0.0f;
+	rz = 0.0f;
+
+	fx = 0.0f;
+	fy = c;
+	fz = s;
+
+	ux = 0.0f;
+	uy = -s;
+	uz = c;
+}
+
+void
+CMatrix::SetRotateYOnly(float angle)
+{
+	float c = Cos(angle);
+	float s = Sin(angle);
+
+	rx = c;
+	ry = 0.0f;
+	rz = -s;
+
+	fx = 0.0f;
+	fy = 1.0f;
+	fz = 0.0f;
+
+	ux = s;
+	uy = 0.0f;
+	uz = c;
+}
+
+void
+CMatrix::SetRotateZOnly(float angle)
+{
+	float c = Cos(angle);
+	float s = Sin(angle);
+
+	rx = c;
+	ry = s;
+	rz = 0.0f;
+
+	fx = -s;
+	fy = c;
+	fz = 0.0f;
+
+	ux = 0.0f;
+	uy = 0.0f;
+	uz = 1.0f;
+}
+
+void
+CMatrix::SetRotateX(float angle)
+{
+	SetRotateXOnly(angle);
+	px = 0.0f;
+	py = 0.0f;
+	pz = 0.0f;
+}
+
+
+void
+CMatrix::SetRotateY(float angle)
+{
+	SetRotateYOnly(angle);
+	px = 0.0f;
+	py = 0.0f;
+	pz = 0.0f;
+}
+
+void
+CMatrix::SetRotateZ(float angle)
+{
+	SetRotateZOnly(angle);
+	px = 0.0f;
+	py = 0.0f;
+	pz = 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);
+
+	rx = cZ * cY - (sZ * sX) * sY;
+	ry = (cZ * sX) * sY + sZ * cY;
+	rz = -cX * sY;
+
+	fx = -sZ * cX;
+	fy = cZ * cX;
+	fz = sX;
+
+	ux = (sZ * sX) * cY + cZ * sY;
+	uy = sZ * sY - (cZ * sX) * cY;
+	uz = cX * cY;
+
+	px = 0.0f;
+	py = 0.0f;
+	pz = 0.0f;
+}
+
+void
+CMatrix::RotateX(float x)
+{
+	float c = Cos(x);
+	float s = Sin(x);
+
+	float ry = this->ry;
+	float rz = this->rz;
+	float uy = this->fy;
+	float uz = this->fz;
+	float ay = this->uy;
+	float az = this->uz;
+	float py = this->py;
+	float pz = this->pz;
+
+	this->ry = c * ry - s * rz;
+	this->rz = c * rz + s * ry;
+	this->fy = c * uy - s * uz;
+	this->fz = c * uz + s * uy;
+	this->uy = c * ay - s * az;
+	this->uz = c * az + s * ay;
+	this->py = c * py - s * pz;
+	this->pz = c * pz + s * py;
+}
+
+void
+CMatrix::RotateY(float y)
+{
+	float c = Cos(y);
+	float s = Sin(y);
+
+	float rx = this->rx;
+	float rz = this->rz;
+	float ux = this->fx;
+	float uz = this->fz;
+	float ax = this->ux;
+	float az = this->uz;
+	float px = this->px;
+	float pz = this->pz;
+
+	this->rx = c * rx + s * rz;
+	this->rz = c * rz - s * rx;
+	this->fx = c * ux + s * uz;
+	this->fz = c * uz - s * ux;
+	this->ux = c * ax + s * az;
+	this->uz = c * az - s * ax;
+	this->px = c * px + s * pz;
+	this->pz = c * pz - s * px;
+}
+
+void
+CMatrix::RotateZ(float z)
+{
+	float c = Cos(z);
+	float s = Sin(z);
+
+	float ry = this->ry;
+	float rx = this->rx;
+	float uy = this->fy;
+	float ux = this->fx;
+	float ay = this->uy;
+	float ax = this->ux;
+	float py = this->py;
+	float px = this->px;
+
+	this->rx = c * rx - s * ry;
+	this->ry = c * ry + s * rx;
+	this->fx = c * ux - s * uy;
+	this->fy = c * uy + s * ux;
+	this->ux = c * ax - s * ay;
+	this->uy = c * ay + s * ax;
+	this->px = c * px - s * py;
+	this->py = 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 = this->rx;
+	float ry = this->ry;
+	float rz = this->rz;
+	float ux = this->fx;
+	float uy = this->fy;
+	float uz = this->fz;
+	float ax = this->ux;
+	float ay = this->uy;
+	float az = this->uz;
+	float px = this->px;
+	float py = this->py;
+	float pz = this->pz;
+
+	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;
+
+	this->rx = x1 * rx + y1 * ry + z1 * rz;
+	this->ry = x2 * rx + y2 * ry + z2 * rz;
+	this->rz = x3 * rx + y3 * ry + z3 * rz;
+	this->fx = x1 * ux + y1 * uy + z1 * uz;
+	this->fy = x2 * ux + y2 * uy + z2 * uz;
+	this->fz = x3 * ux + y3 * uy + z3 * uz;
+	this->ux = x1 * ax + y1 * ay + z1 * az;
+	this->uy = x2 * ax + y2 * ay + z2 * az;
+	this->uz = x3 * ax + y3 * ay + z3 * az;
+	this->px = x1 * px + y1 * py + z1 * pz;
+	this->py = x2 * px + y2 * py + z2 * pz;
+	this->pz = 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;
+	out.rx = m1.rx * m2.rx + m1.fx * m2.ry + m1.ux * m2.rz;
+	out.ry = m1.ry * m2.rx + m1.fy * m2.ry + m1.uy * m2.rz;
+	out.rz = m1.rz * m2.rx + m1.fz * m2.ry + m1.uz * m2.rz;
+	out.fx = m1.rx * m2.fx + m1.fx * m2.fy + m1.ux * m2.fz;
+	out.fy = m1.ry * m2.fx + m1.fy * m2.fy + m1.uy * m2.fz;
+	out.fz = m1.rz * m2.fx + m1.fz * m2.fy + m1.uz * m2.fz;
+	out.ux = m1.rx * m2.ux + m1.fx * m2.uy + m1.ux * m2.uz;
+	out.uy = m1.ry * m2.ux + m1.fy * m2.uy + m1.uy * m2.uz;
+	out.uz = m1.rz * m2.ux + m1.fz * m2.uy + m1.uz * m2.uz;
+	out.px = m1.rx * m2.px + m1.fx * m2.py + m1.ux * m2.pz + m1.px;
+	out.py = m1.ry * m2.px + m1.fy * m2.py + m1.uy * m2.pz + m1.py;
+	out.pz = m1.rz * m2.px + m1.fz * m2.py + m1.uz * m2.pz + m1.pz;
+	return out;
+}
+
+CMatrix &
+Invert(const CMatrix &src, CMatrix &dst)
+{
+	// TODO: VU0 code
+	dst.f[3][0] = dst.f[3][1] = dst.f[3][2] = 0.0f;
+
+	dst.f[0][0] = src.f[0][0];
+	dst.f[0][1] = src.f[1][0];
+	dst.f[0][2] = src.f[2][0];
+
+	dst.f[1][0] = src.f[0][1];
+	dst.f[1][1] = src.f[1][1];
+	dst.f[1][2] = src.f[2][1];
+
+	dst.f[2][0] = src.f[0][2];
+	dst.f[2][1] = src.f[1][2];
+	dst.f[2][2] = src.f[2][2];
+
+
+	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];
+
+	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];
+
+	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];
+
+	dst.f[3][0] = -dst.f[3][0];
+	dst.f[3][1] = -dst.f[3][1];
+	dst.f[3][2] = -dst.f[3][2];
+
+	return dst;
+}
+
+void
+CMatrix::CopyToRwMatrix(RwMatrix* matrix)
+{
+	matrix->right = GetRight();
+	matrix->up = GetForward();
+	matrix->at = GetUp();
+	matrix->pos = GetPosition();
+	RwMatrixUpdate(matrix);
+}
+
+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
new file mode 100644
index 0000000..6da4c76
--- /dev/null
+++ b/src/math/Matrix.h
@@ -0,0 +1,170 @@
+#pragma once
+
+class CMatrix
+{
+public:
+#ifdef GTA_PS2
+	union
+	{
+		float f[4][4];
+		struct
+		{
+			float rx, ry, rz;
+			RwMatrix *m_attachment;
+			float fx, fy, fz;
+			bool m_hasRwMatrix;	// are we the owner?
+			float ux, uy, uz, uw;
+			float px, py, pz, pw;
+		};
+	};
+#else
+	union
+	{
+		float f[4][4];
+		struct
+		{
+			float rx, ry, rz, rw;
+			float fx, fy, fz, fw;
+			float ux, uy, uz, uw;
+			float px, py, pz, pw;
+		};
+	};
+
+	RwMatrix *m_attachment;
+	bool m_hasRwMatrix;	// are we the owner?
+#endif
+
+	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);
+	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*)&px; }
+	CVector &GetRight(void) { return *(CVector*)℞ }
+	CVector &GetForward(void) { return *(CVector*)&fx; }
+	CVector &GetUp(void) { return *(CVector*)&ux; }
+
+	const CVector &GetPosition(void) const { return *(CVector*)&px; }
+	const CVector &GetRight(void) const { return *(CVector*)℞ }
+	const CVector &GetForward(void) const { return *(CVector*)&fx; }
+	const CVector &GetUp(void) const { return *(CVector*)&ux; }
+
+
+	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){
+		px += x;
+		py += y;
+		pz += z;
+	}
+	void Translate(const CVector &trans){ Translate(trans.x, trans.y, trans.z); }
+
+	void SetScale(float s);
+	void Scale(float scale)
+	{
+		for (int i = 0; i < 3; i++)
+			for (int j = 0; j < 3; j++)
+				f[i][j] *= scale;
+	}
+	void Scale(float sx, float sy, float sz)
+	{
+		for (int i = 0; i < 3; i++){
+			f[i][0] *= sx;
+			f[i][1] *= sy;
+			f[i][2] *= sz;
+		}
+	}
+
+
+	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);
+
+		rx = c * scale;
+		ry = s * scale;
+		rz = 0.0f;
+
+		fx = -s * scale;
+		fy = c * scale;
+		fz = 0.0f;
+
+		ux = 0.0f;
+		uy = 0.0f;
+		uz = scale;
+	}
+	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);
+	void RotateY(float y);
+	void RotateZ(float z);
+
+	void Reorthogonalise(void);
+	void CopyOnlyMatrix(const CMatrix &other);
+	void SetUnity(void);
+	void ResetOrientation(void);
+	
+	void CopyToRwMatrix(RwMatrix* matrix);
+	
+	void SetTranslateOnly(float x, float y, float z) {
+		px = x;
+		py = y;
+		pz = z;
+	}
+	void SetTranslateOnly(const CVector& pos) {
+		SetTranslateOnly(pos.x, pos.y, pos.z);
+	}
+	void CheckIntegrity(){}
+};
+
+
+CMatrix &Invert(const CMatrix &src, CMatrix &dst);
+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.px, vec.y - mat.py, vec.z - mat.pz);
+	return CVector(
+		mat.rx * v.x + mat.ry * v.y + mat.rz * v.z,
+		mat.fx * v.x + mat.fy * v.y + mat.fz * v.z,
+		mat.ux * v.x + mat.uy * v.y + mat.uz * v.z);
+}
+
+
+
+class CCompressedMatrixNotAligned
+{
+	CVector m_vecPos;
+	int8 m_rightX;
+	int8 m_rightY;
+	int8 m_rightZ;
+	int8 m_upX;
+	int8 m_upY;
+	int8 m_upZ;
+public:
+	void CompressFromFullMatrix(CMatrix &other);
+	void DecompressIntoFullMatrix(CMatrix &other);
+};
+
+class CCompressedMatrix : public CCompressedMatrixNotAligned
+{
+	int _alignment; // no clue what would this align to
+};
\ No newline at end of file
diff --git a/src/math/Quaternion.cpp b/src/math/Quaternion.cpp
new file mode 100644
index 0000000..b0e782e
--- /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
new file mode 100644
index 0000000..47c94f7
--- /dev/null
+++ b/src/math/Quaternion.h
@@ -0,0 +1,95 @@
+#pragma once
+
+// TODO: actually implement this
+class CQuaternion
+{
+public:
+	float x, y, z, w;
+	CQuaternion(void) {}
+	CQuaternion(float x, float y, float z, float w) : x(x), y(y), z(z), w(w) {}
+
+	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);
+	void Multiply(const CQuaternion &q1, const CQuaternion &q2);
+	void Invert(void){	// Conjugate would have been a better name
+		x = -x;
+		y = -y;
+		z = -z;
+	}
+
+	const CQuaternion &operator+=(CQuaternion const &right) {
+		x += right.x;
+		y += right.y;
+		z += right.z;
+		w += right.w;
+		return *this;
+	}
+
+	const CQuaternion &operator-=(CQuaternion const &right) {
+		x -= right.x;
+		y -= right.y;
+		z -= right.z;
+		w -= right.w;
+		return *this;
+	}
+
+	const CQuaternion &operator*=(float right) {
+		x *= right;
+		y *= right;
+		z *= right;
+		w *= right;
+		return *this;
+	}
+
+	const CQuaternion &operator/=(float right) {
+		x /= right;
+		y /= right;
+		z /= right;
+		w /= right;
+		return *this;
+	}
+
+	CQuaternion operator-() const {
+		return CQuaternion(-x, -y, -z, -w);
+	}
+
+	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
+DotProduct(const CQuaternion &q1, const CQuaternion &q2)
+{
+	return q1.x*q2.x + q1.y*q2.y + q1.z*q2.z + q1.w*q2.w;
+}
+
+inline CQuaternion operator+(const CQuaternion &left, const CQuaternion &right)
+{
+	return CQuaternion(left.x + right.x, left.y + right.y, left.z + right.z, left.w + right.w);
+}
+
+inline CQuaternion operator-(const CQuaternion &left, const CQuaternion &right)
+{
+	return CQuaternion(left.x - right.x, left.y - right.y, left.z - right.z, left.w - right.w);
+}
+
+inline CQuaternion operator*(const CQuaternion &left, float right)
+{
+	return CQuaternion(left.x * right, left.y * right, left.z * right, left.w * right);
+}
+
+inline CQuaternion operator*(float left, const CQuaternion &right)
+{
+	return CQuaternion(left * right.x, left * right.y, left * right.z, left * right.w);
+}
+
+inline CQuaternion operator/(const CQuaternion &left, float right)
+{
+	return CQuaternion(left.x / right, left.y / right, left.z / right, left.w / right);
+}
diff --git a/src/math/Rect.cpp b/src/math/Rect.cpp
new file mode 100644
index 0000000..de6320a
--- /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
new file mode 100644
index 0000000..e9b2589
--- /dev/null
+++ b/src/math/Rect.h
@@ -0,0 +1,71 @@
+#pragma once
+
+class CRect
+{
+public:
+	float left;     // x min
+	float bottom;   // y max
+	float right;    // x max
+	float top;      // y min
+
+	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;
+		if(v.y < top) top = v.y;
+		if(v.y > bottom) bottom = v.y;
+	}
+	void ContainRect(const CRect &r){
+		if(r.left < left) left = r.left;
+		if(r.right > right) right = r.right;
+		if(r.top < top) top = r.top;
+		if(r.bottom > bottom) bottom = r.bottom;
+	}
+
+	bool IsPointInside(const CVector2D &p){
+		return p.x >= left &&
+			p.x <= right &&
+			p.y >= top &&
+			p.y <= bottom;
+	}
+	bool IsPointInside(const CVector2D &p, float extraRadius){
+		return p.x >= left-extraRadius &&
+			p.x <= right+extraRadius &&
+			p.y >= top-extraRadius &&
+			p.y <= bottom+extraRadius;
+	}
+
+	void Translate(float x, float y){
+		left += x;
+		right += x;
+		bottom += y;
+		top += y;
+	}
+
+	void Grow(float r) {
+		left -= r;
+		right += r;
+		top -= r;
+		bottom += r;
+	}
+
+	void Grow(float l, float r)
+	{
+		left -= l;
+		top -= l;
+		right += r;
+		bottom += r;
+	}
+
+	void Grow(float l, float r, float t, float b)
+	{
+		left -= l;
+		top -= t;
+		right += r;
+		bottom += b;
+	}
+
+	float GetWidth(void) { return right - left; }
+	float GetHeight(void) { return bottom - top; }
+};
diff --git a/src/math/Vector.cpp b/src/math/Vector.cpp
new file mode 100644
index 0000000..ee76e55
--- /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.rx * vec.x + mat.fx * vec.y + mat.ux * vec.z,
+	               mat.ry * vec.x + mat.fy * vec.y + mat.uy * vec.z,
+	               mat.rz * vec.x + mat.fz * vec.y + mat.uz * vec.z);
+}
+
+CVector
+Multiply3x3(const CVector &vec, const CMatrix &mat)
+{
+	return CVector(mat.rx * vec.x + mat.ry * vec.y + mat.rz * vec.z,
+	               mat.fx * vec.x + mat.fy * vec.y + mat.fz * vec.z,
+	               mat.ux * vec.x + mat.uy * vec.y + mat.uz * vec.z);
+}
+
+CVector
+operator*(const CMatrix &mat, const CVector &vec)
+{
+	// TODO: VU0 code
+	return CVector(mat.rx * vec.x + mat.fx * vec.y + mat.ux * vec.z + mat.px,
+	               mat.ry * vec.x + mat.fy * vec.y + mat.uy * vec.z + mat.py,
+	               mat.rz * vec.x + mat.fz * vec.y + mat.uz * vec.z + mat.pz);
+}
diff --git a/src/math/Vector.h b/src/math/Vector.h
new file mode 100644
index 0000000..0212845
--- /dev/null
+++ b/src/math/Vector.h
@@ -0,0 +1,129 @@
+#pragma once
+
+class CVector : public RwV3d
+{
+public:
+	CVector(void) {}
+	CVector(float x, float y, float z)
+	{
+		this->x = x;
+		this->y = y;
+		this->z = z;
+	}
+
+	CVector(const RwV3d &v)
+	{
+		x = v.x;
+		y = v.y;
+		z = v.z;
+	}
+	// (0,1,0) means no rotation. So get right vector and its atan
+	float Heading(void) const { return Atan2(-x, y); }
+	float Magnitude(void) const { return Sqrt(x*x + y*y + z*z); }
+	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);
+	
+	void Normalise2D(void) {
+		float sq = MagnitudeSqr2D();
+		float invsqrt = RecipSqrt(sq);
+		x *= invsqrt;
+		y *= invsqrt;
+	}
+
+	const CVector &operator+=(CVector const &right) {
+		x += right.x;
+		y += right.y;
+		z += right.z;
+		return *this;
+	}
+
+	const CVector &operator-=(CVector const &right) {
+		x -= right.x;
+		y -= right.y;
+		z -= right.z;
+		return *this;
+	}
+
+	const CVector &operator*=(float right) {
+		x *= right;
+		y *= right;
+		z *= right;
+		return *this;
+	}
+
+	const CVector &operator/=(float right) {
+		x /= right;
+		y /= right;
+		z /= right;
+		return *this;
+	}
+
+	CVector operator-() const {
+		return CVector(-x, -y, -z);
+	}
+
+	const bool operator==(CVector const &right) const {
+		return x == right.x && y == right.y && z == right.z;
+	}
+
+	const bool operator!=(CVector const &right) const {
+		return x != right.x || y != right.y || z != right.z;
+	}
+
+	bool IsZero(void) const { return x == 0.0f && y == 0.0f && z == 0.0f; }
+};
+
+inline CVector operator+(const CVector &left, const CVector &right)
+{
+	return CVector(left.x + right.x, left.y + right.y, left.z + right.z);
+}
+
+inline CVector operator-(const CVector &left, const CVector &right)
+{
+	return CVector(left.x - right.x, left.y - right.y, left.z - right.z);
+}
+
+inline CVector operator*(const CVector &left, float right)
+{
+	return CVector(left.x * right, left.y * right, left.z * right);
+}
+
+inline CVector operator*(float left, const CVector &right)
+{
+	return CVector(left * right.x, left * right.y, left * right.z);
+}
+
+inline CVector operator/(const CVector &left, float right)
+{
+	return CVector(left.x / right, left.y / right, left.z / right);
+}
+
+inline float
+DotProduct(const CVector &v1, const CVector &v2)
+{
+	return v1.x*v2.x + v1.y*v2.y + v1.z*v2.z;
+}
+
+CVector CrossProduct(const CVector &v1, const CVector &v2);
+
+inline float
+Distance(const CVector &v1, const CVector &v2)
+{
+	return (v2 - v1).Magnitude();
+}
+
+inline float
+Distance2D(const CVector &v1, const CVector &v2)
+{
+	float x = v2.x - v1.x;
+	float y = v2.y - v1.y;
+	return Sqrt(x*x + y*y);
+}
+
+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
new file mode 100644
index 0000000..deabd0b
--- /dev/null
+++ b/src/math/Vector2D.h
@@ -0,0 +1,104 @@
+#pragma once
+
+class CVector2D
+{
+public:
+	float x, y;
+	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 = RecipSqrt(sq);
+			x *= invsqrt;
+			y *= invsqrt;
+		}else
+			x = 1.0f;
+	}
+
+	const CVector2D &operator+=(CVector2D const &right) {
+		x += right.x;
+		y += right.y;
+		return *this;
+	}
+
+	const CVector2D &operator-=(CVector2D const &right) {
+		x -= right.x;
+		y -= right.y;
+		return *this;
+	}
+
+	const CVector2D &operator*=(float right) {
+		x *= right;
+		y *= right;
+		return *this;
+	}
+
+	const CVector2D &operator/=(float right) {
+		x /= right;
+		y /= right;
+		return *this;
+	}
+	CVector2D operator-(const CVector2D &rhs) const {
+		return CVector2D(x-rhs.x, y-rhs.y);
+	}
+	CVector2D operator+(const CVector2D &rhs) const {
+		return CVector2D(x+rhs.x, y+rhs.y);
+	}
+	CVector2D operator/(float t) const {
+		return CVector2D(x/t, y/t);
+	}
+	CVector2D operator-() const {
+		return CVector2D(-x, -y);
+	}
+};
+
+inline float
+DotProduct2D(const CVector2D &v1, const CVector2D &v2)
+{
+	return v1.x*v2.x + v1.y*v2.y;
+}
+
+inline float
+CrossProduct2D(const CVector2D &v1, const CVector2D &v2)
+{
+	return v1.x*v2.y - v1.y*v2.x;
+}
+
+inline float
+Distance2D(const CVector2D &v, float x, float y)
+{
+	return Sqrt((v.x-x)*(v.x-x) + (v.y-y)*(v.y-y));
+}
+
+inline float
+DistanceSqr2D(const CVector2D &v, float x, float y)
+{
+	return (v.x-x)*(v.x-x) + (v.y-y)*(v.y-y);
+}
+
+inline void
+NormalizeXY(float &x, float &y)
+{
+	float l  = Sqrt(x*x + y*y);
+	if(l != 0.0f){
+		x /= l;
+		y /= l;
+	}else
+		x = 1.0f;
+}
+
+inline CVector2D operator*(const CVector2D &left, float right)
+{
+	return CVector2D(left.x * right, left.y * right);
+}
+
+inline CVector2D operator*(float left, const CVector2D &right)
+{
+	return CVector2D(left * right.x, left * right.y);
+}
diff --git a/src/math/VuVector.h b/src/math/VuVector.h
new file mode 100644
index 0000000..30d62cf
--- /dev/null
+++ b/src/math/VuVector.h
@@ -0,0 +1,30 @@
+#pragma once
+
+class TYPEALIGN(16) CVuVector : public CVector
+{
+public:
+	float w;
+	CVuVector(void) {}
+	CVuVector(float x, float y, float z) : CVector(x, y, z) {}
+	CVuVector(float x, float y, float z, float w) : CVector(x, y, z), w(w) {}
+	CVuVector(const CVector &v) : CVector(v.x, v.y, v.z) {}
+	CVuVector(const RwV3d &v) : CVector(v) {}
+/*
+	void Normalise(void) {
+		float sq = MagnitudeSqr();
+		// TODO: VU0 code
+		if(sq > 0.0f){
+			float invsqrt = RecipSqrt(sq);
+			x *= invsqrt;
+			y *= invsqrt;
+			z *= invsqrt;
+		}else
+			x = 1.0f;
+	}
+*/
+};
+
+void TransformPoint(CVuVector &out, const CMatrix &mat, const CVuVector &in);
+void TransformPoint(CVuVector &out, const CMatrix &mat, const RwV3d &in);
+void TransformPoints(CVuVector *out, int n, const CMatrix &mat, const RwV3d *in, int stride);
+void TransformPoints(CVuVector *out, int n, const CMatrix &mat, const CVuVector *in);
diff --git a/src/math/math.cpp b/src/math/math.cpp
new file mode 100644
index 0000000..8cb56da
--- /dev/null
+++ b/src/math/math.cpp
@@ -0,0 +1,118 @@
+#include "common.h"
+
+#include "VuVector.h"
+
+// TODO: move more stuff into here
+
+
+void TransformPoint(CVuVector &out, const CMatrix &mat, const CVuVector &in)
+{
+#ifdef GTA_PS2
+	__asm__ __volatile__("\n\
+		lqc2    vf01,0x0(%2)\n\
+		lqc2    vf02,0x0(%1)\n\
+		lqc2    vf03,0x10(%1)\n\
+		lqc2    vf04,0x20(%1)\n\
+		lqc2    vf05,0x30(%1)\n\
+		vmulax.xyz	ACC,   vf02,vf01\n\
+		vmadday.xyz	ACC,   vf03,vf01\n\
+		vmaddaz.xyz	ACC,   vf04,vf01\n\
+		vmaddw.xyz	vf06,vf05,vf00\n\
+		sqc2    vf06,0x0(%0)\n\
+		": : "r" (&out) , "r" (&mat) ,"r" (&in): "memory");
+#else
+	out = mat * in;
+#endif
+}
+
+void TransformPoint(CVuVector &out, const CMatrix &mat, const RwV3d &in)
+{
+#ifdef GTA_PS2
+	__asm__ __volatile__("\n\
+		ldr	$8,0x0(%2)\n\
+		ldl	$8,0x7(%2)\n\
+		lw	$9,0x8(%2)\n\
+		pcpyld	$10,$9,$8\n\
+		qmtc2	$10,vf01\n\
+		lqc2    vf02,0x0(%1)\n\
+		lqc2    vf03,0x10(%1)\n\
+		lqc2    vf04,0x20(%1)\n\
+		lqc2    vf05,0x30(%1)\n\
+		vmulax.xyz	ACC,   vf02,vf01\n\
+		vmadday.xyz	ACC,   vf03,vf01\n\
+		vmaddaz.xyz	ACC,   vf04,vf01\n\
+		vmaddw.xyz	vf06,vf05,vf00\n\
+		sqc2    vf06,0x0(%0)\n\
+		": : "r" (&out) , "r" (&mat) ,"r" (&in): "memory");
+#else
+	out = mat * in;
+#endif
+}
+
+void TransformPoints(CVuVector *out, int n, const CMatrix &mat, const RwV3d *in, int stride)
+{
+#ifdef GTA_PS3
+	__asm__ __volatile__("\n\
+		paddub	$3,%4,$0\n\
+		lqc2    vf02,0x0(%2)\n\
+		lqc2    vf03,0x10(%2)\n\
+		lqc2    vf04,0x20(%2)\n\
+		lqc2    vf05,0x30(%2)\n\
+		ldr	$8,0x0(%3)\n\
+		ldl	$8,0x7(%3)\n\
+		lw	$9,0x8(%3)\n\
+		pcpyld	$10,$9,$8\n\
+		qmtc2	$10,vf01\n\
+	1:	vmulax.xyz	ACC,   vf02,vf01\n\
+		vmadday.xyz	ACC,   vf03,vf01\n\
+		vmaddaz.xyz	ACC,   vf04,vf01\n\
+		vmaddw.xyz	vf06,vf05,vf00\n\
+		add	%3,%3,$3\n\
+		ldr	$8,0x0(%3)\n\
+		ldl	$8,0x7(%3)\n\
+		lw	$9,0x8(%3)\n\
+		pcpyld	$10,$9,$8\n\
+		qmtc2	$10,vf01\n\
+		addi	%1,%1,-1\n\
+		addiu	%0,%0,0x10\n\
+		sqc2    vf06,-0x10(%0)\n\
+		bnez	%1,1b\n\
+		": : "r" (out) , "r" (n), "r" (&mat), "r" (in), "r" (stride): "memory");
+#else
+	while(n--){
+		*out = mat * *in;
+		in = (RwV3d*)((uint8*)in + stride);
+		out++;
+	}
+#endif
+}
+
+void TransformPoints(CVuVector *out, int n, const CMatrix &mat, const CVuVector *in)
+{
+#ifdef GTA_PS2
+	__asm__ __volatile__("\n\
+		lqc2    vf02,0x0(%2)\n\
+		lqc2    vf03,0x10(%2)\n\
+		lqc2    vf04,0x20(%2)\n\
+		lqc2    vf05,0x30(%2)\n\
+		lqc2    vf01,0x0(%3)\n\
+		nop\n\
+	1:	vmulax.xyz	ACC,   vf02,vf01\n\
+		vmadday.xyz	ACC,   vf03,vf01\n\
+		vmaddaz.xyz	ACC,   vf04,vf01\n\
+		vmaddw.xyz	vf06,vf05,vf00\n\
+		lqc2	vf01,0x10(%3)\n\
+		addiu	%3,%3,0x10\n\
+		addi	%1,%1,-1\n\
+		addiu	%0,%0,0x10\n\
+		sqc2    vf06,-0x10(%0)\n\
+		bnez	%1,1b\n\
+		": : "r" (out) , "r" (n), "r" (&mat) ,"r" (in): "memory");
+#else
+	while(n--){
+		*out = mat * *in;
+		in++;
+		out++;
+	}
+#endif
+}
diff --git a/src/math/maths.h b/src/math/maths.h
new file mode 100644
index 0000000..8d68bf6
--- /dev/null
+++ b/src/math/maths.h
@@ -0,0 +1,19 @@
+#pragma once
+
+// wrapper around float versions of functions
+// in gta they are in CMaths but that makes the code rather noisy
+
+inline float Sin(float x) { return sinf(x); }
+inline float Asin(float x) { return asinf(x); }
+inline float Cos(float x) { return cosf(x); }
+inline float Acos(float x) { return acosf(x); }
+inline float Tan(float x) { return tanf(x); }
+inline float Atan(float x) { return atanf(x); }
+inline float Atan2(float y, float x) { return atan2f(y, x); }
+inline float Abs(float x) { return fabs(x); }
+inline float Sqrt(float x) { return sqrtf(x); }
+inline float RecipSqrt(float x, float y) { return x/Sqrt(y); }
+inline float RecipSqrt(float x) { return RecipSqrt(1.0f, x); }
+inline float Pow(float x, float y) { return powf(x, y); }
+inline float Floor(float x) { return floorf(x); }
+inline float Ceil(float x) { return ceilf(x); }