22 files changed, 917 insertions, 588 deletions

diff --git a/src/collision/ColBox.cpp b/src/collision/ColBox.cpp
new file mode 100644
index 00000000..53cba88b
--- /dev/null
+++ b/src/collision/ColBox.cpp
@@ -0,0 +1,21 @@
+#include "common.h"
+#include "ColBox.h"
+
+void
+CColBox::Set(const CVector &min, const CVector &max, uint8 surf, uint8 piece)
+{
+	this->min = min;
+	this->max = max;
+	this->surface = surf;
+	this->piece = piece;
+}
+
+CColBox&
+CColBox::operator=(const CColBox& other)
+{
+	min = other.min;
+	max = other.max;
+	surface = other.surface;
+	piece = other.piece;
+	return *this;
+}
\ No newline at end of file
diff --git a/src/collision/ColBox.h b/src/collision/ColBox.h
new file mode 100644
index 00000000..ac2cd675
--- /dev/null
+++ b/src/collision/ColBox.h
@@ -0,0 +1,16 @@
+#pragma once
+
+#include "SurfaceTable.h"
+
+struct CColBox
+{
+	CVector min;
+	CVector max;
+	uint8 surface;
+	uint8 piece;
+
+	void Set(const CVector &min, const CVector &max, uint8 surf = SURFACE_DEFAULT, uint8 piece = 0);
+	CVector GetSize(void) { return max - min; }
+
+	CColBox& operator=(const CColBox &other);
+};
\ No newline at end of file
diff --git a/src/collision/ColLine.cpp b/src/collision/ColLine.cpp
new file mode 100644
index 00000000..c6247449
--- /dev/null
+++ b/src/collision/ColLine.cpp
@@ -0,0 +1,9 @@
+#include "common.h"
+#include "ColLine.h"
+
+void
+CColLine::Set(const CVector &p0, const CVector &p1)
+{
+	this->p0 = p0;
+	this->p1 = p1;
+}
\ No newline at end of file
diff --git a/src/collision/ColLine.h b/src/collision/ColLine.h
new file mode 100644
index 00000000..21587a06
--- /dev/null
+++ b/src/collision/ColLine.h
@@ -0,0 +1,14 @@
+#pragma once
+
+struct CColLine
+{
+	// NB: this has to be compatible with two CVuVectors
+	CVector p0;
+	int pad0;
+	CVector p1;
+	int pad1;
+
+	CColLine(void) { };
+	CColLine(const CVector &p0, const CVector &p1) { this->p0 = p0; this->p1 = p1; };
+	void Set(const CVector &p0, const CVector &p1);
+};
\ No newline at end of file
diff --git a/src/collision/ColModel.cpp b/src/collision/ColModel.cpp
new file mode 100644
index 00000000..650e6958
--- /dev/null
+++ b/src/collision/ColModel.cpp
@@ -0,0 +1,184 @@
+#include "common.h"
+#include "ColModel.h"
+#include "Game.h"
+
+CColModel::CColModel(void)
+{
+	numSpheres = 0;
+	spheres = nil;
+	numLines = 0;
+	lines = nil;
+	numBoxes = 0;
+	boxes = nil;
+	numTriangles = 0;
+	vertices = nil;
+	triangles = nil;
+	trianglePlanes = nil;
+	level = CGame::currLevel;
+	ownsCollisionVolumes = true;
+}
+
+CColModel::~CColModel(void)
+{
+	RemoveCollisionVolumes();
+	RemoveTrianglePlanes();
+}
+
+void
+CColModel::RemoveCollisionVolumes(void)
+{
+	if(ownsCollisionVolumes){
+		RwFree(spheres);
+		RwFree(lines);
+		RwFree(boxes);
+		RwFree(vertices);
+		RwFree(triangles);
+	}
+	numSpheres = 0;
+	numLines = 0;
+	numBoxes = 0;
+	numTriangles = 0;
+	spheres = nil;
+	lines = nil;
+	boxes = nil;
+	vertices = nil;
+	triangles = nil;
+}
+
+void
+CColModel::CalculateTrianglePlanes(void)
+{
+	// HACK: allocate space for one more element to stuff the link pointer into
+	trianglePlanes = (CColTrianglePlane*)RwMalloc(sizeof(CColTrianglePlane) * (numTriangles+1));
+	for(int i = 0; i < numTriangles; i++)
+		trianglePlanes[i].Set(vertices, triangles[i]);
+}
+
+void
+CColModel::RemoveTrianglePlanes(void)
+{
+	RwFree(trianglePlanes);
+	trianglePlanes = nil;
+}
+
+void
+CColModel::SetLinkPtr(CLink<CColModel*> *lptr)
+{
+	assert(trianglePlanes);
+	*(CLink<CColModel*>**)ALIGNPTR(&trianglePlanes[numTriangles]) = lptr;
+}
+
+CLink<CColModel*>*
+CColModel::GetLinkPtr(void)
+{
+	assert(trianglePlanes);
+	return *(CLink<CColModel*>**)ALIGNPTR(&trianglePlanes[numTriangles]);
+}
+
+void
+CColModel::GetTrianglePoint(CVector &v, int i) const
+{
+	v = vertices[i].Get();
+}
+
+CColModel&
+CColModel::operator=(const CColModel &other)
+{
+	int i;
+	int numVerts;
+
+	boundingSphere = other.boundingSphere;
+	boundingBox = other.boundingBox;
+
+	// copy spheres
+	if(other.numSpheres){
+		if(numSpheres != other.numSpheres){
+			numSpheres = other.numSpheres;
+			if(spheres)
+				RwFree(spheres);
+			spheres = (CColSphere*)RwMalloc(numSpheres*sizeof(CColSphere));
+		}
+		for(i = 0; i < numSpheres; i++)
+			spheres[i] = other.spheres[i];
+	}else{
+		numSpheres = 0;
+		if(spheres)
+			RwFree(spheres);
+		spheres = nil;
+	}
+
+	// copy lines
+	if(other.numLines){
+		if(numLines != other.numLines){
+			numLines = other.numLines;
+			if(lines)
+				RwFree(lines);
+			lines = (CColLine*)RwMalloc(numLines*sizeof(CColLine));
+		}
+		for(i = 0; i < numLines; i++)
+			lines[i] = other.lines[i];
+	}else{
+		numLines = 0;
+		if(lines)
+			RwFree(lines);
+		lines = nil;
+	}
+
+	// copy boxes
+	if(other.numBoxes){
+		if(numBoxes != other.numBoxes){
+			numBoxes = other.numBoxes;
+			if(boxes)
+				RwFree(boxes);
+			boxes = (CColBox*)RwMalloc(numBoxes*sizeof(CColBox));
+		}
+		for(i = 0; i < numBoxes; i++)
+			boxes[i] = other.boxes[i];
+	}else{
+		numBoxes = 0;
+		if(boxes)
+			RwFree(boxes);
+		boxes = nil;
+	}
+
+	// copy mesh
+	if(other.numTriangles){
+		// copy vertices
+		numVerts = 0;
+		for(i = 0; i < other.numTriangles; i++){
+			if(other.triangles[i].a > numVerts)
+				numVerts = other.triangles[i].a;
+			if(other.triangles[i].b > numVerts)
+				numVerts = other.triangles[i].b;
+			if(other.triangles[i].c > numVerts)
+				numVerts = other.triangles[i].c;
+		}
+		numVerts++;
+		if(vertices)
+			RwFree(vertices);
+		if(numVerts){
+			vertices = (CompressedVector*)RwMalloc(numVerts*sizeof(CompressedVector));
+			for(i = 0; i < numVerts; i++)
+				vertices[i] = other.vertices[i];
+		}
+
+		// copy triangles
+		if(numTriangles != other.numTriangles){
+			numTriangles = other.numTriangles;
+			if(triangles)
+				RwFree(triangles);
+			triangles = (CColTriangle*)RwMalloc(numTriangles*sizeof(CColTriangle));
+		}
+		for(i = 0; i < numTriangles; i++)
+			triangles[i] = other.triangles[i];
+	}else{
+		numTriangles = 0;
+		if(triangles)
+			RwFree(triangles);
+		triangles = nil;
+		if(vertices)
+			RwFree(vertices);
+		vertices = nil;
+	}
+	return *this;
+}
diff --git a/src/collision/ColModel.h b/src/collision/ColModel.h
new file mode 100644
index 00000000..7dcdfa4d
--- /dev/null
+++ b/src/collision/ColModel.h
@@ -0,0 +1,37 @@
+#pragma once
+
+#include "templates.h"
+#include "ColBox.h"
+#include "ColSphere.h"
+#include "ColLine.h"
+#include "ColPoint.h"
+#include "ColTriangle.h"
+
+struct CColModel
+{
+	CColSphere boundingSphere;
+	CColBox boundingBox;
+	int16 numSpheres;
+	int16 numLines;
+	int16 numBoxes;
+	int16 numTriangles;
+	int32 level;
+	bool ownsCollisionVolumes;	// missing on PS2
+	CColSphere *spheres;
+	CColLine *lines;
+	CColBox *boxes;
+	CompressedVector *vertices;
+	CColTriangle *triangles;
+	CColTrianglePlane *trianglePlanes;
+
+	CColModel(void);
+	~CColModel(void);
+	void RemoveCollisionVolumes(void);
+	void CalculateTrianglePlanes(void);
+	void RemoveTrianglePlanes(void);
+	CLink<CColModel*> *GetLinkPtr(void);
+	void SetLinkPtr(CLink<CColModel*>*);
+	void GetTrianglePoint(CVector &v, int i) const;
+
+	CColModel& operator=(const CColModel& other);
+};
\ No newline at end of file
diff --git a/src/collision/ColPoint.cpp b/src/collision/ColPoint.cpp
new file mode 100644
index 00000000..fbf9e8c3
--- /dev/null
+++ b/src/collision/ColPoint.cpp
@@ -0,0 +1,16 @@
+#include "common.h"
+#include "ColPoint.h"
+
+CColPoint&
+CColPoint::operator=(const CColPoint &other)
+{
+	point = other.point;
+	normal = other.normal;
+	surfaceA = other.surfaceA;
+	pieceA = other.pieceA;
+	surfaceB = other.surfaceB;
+	pieceB = other.pieceB;
+
+	// no depth?
+	return *this;
+}
diff --git a/src/collision/ColPoint.h b/src/collision/ColPoint.h
new file mode 100644
index 00000000..a15b2345
--- /dev/null
+++ b/src/collision/ColPoint.h
@@ -0,0 +1,34 @@
+#pragma once
+
+struct CColPoint
+{
+	CVector point;
+	int pad1;
+	// the surface normal on the surface of point
+	CVector normal;
+	int pad2;
+	uint8 surfaceA;
+	uint8 pieceA;
+	uint8 surfaceB;
+	uint8 pieceB;
+	float depth;
+
+	const CVector &GetNormal() { return normal; }
+	float GetDepth() { return depth; }
+	void Set(float depth, uint8 surfA, uint8 pieceA, uint8 surfB, uint8 pieceB) {
+		this->depth = depth;
+		this->surfaceA = surfA;
+		this->pieceA = pieceA;
+		this->surfaceB = surfB;
+		this->pieceB = pieceB;
+	}
+	void Set(uint8 surfA, uint8 pieceA, uint8 surfB, uint8 pieceB) {
+		this->surfaceA = surfA;
+		this->pieceA = pieceA;
+		this->surfaceB = surfB;
+		this->pieceB = pieceB;
+	}
+
+	CColPoint &operator=(const CColPoint &other);
+};
+
diff --git a/src/collision/ColSphere.cpp b/src/collision/ColSphere.cpp
new file mode 100644
index 00000000..9aac01e0
--- /dev/null
+++ b/src/collision/ColSphere.cpp
@@ -0,0 +1,11 @@
+#include "common.h"
+#include "ColSphere.h"
+
+void
+CColSphere::Set(float radius, const CVector &center, uint8 surf, uint8 piece)
+{
+	this->radius = radius;
+	this->center = center;
+	this->surface = surf;
+	this->piece = piece;
+}
\ No newline at end of file
diff --git a/src/collision/ColSphere.h b/src/collision/ColSphere.h
new file mode 100644
index 00000000..70e29763
--- /dev/null
+++ b/src/collision/ColSphere.h
@@ -0,0 +1,13 @@
+#pragma once
+
+#include "SurfaceTable.h"
+
+struct CColSphere
+{
+	// NB: this has to be compatible with a CVuVector
+	CVector center;
+	float radius;
+	uint8 surface;
+	uint8 piece;
+	void Set(float radius, const CVector &center, uint8 surf = SURFACE_DEFAULT, uint8 piece = 0);
+};
\ No newline at end of file
diff --git a/src/collision/ColTriangle.cpp b/src/collision/ColTriangle.cpp
new file mode 100644
index 00000000..9120fcff
--- /dev/null
+++ b/src/collision/ColTriangle.cpp
@@ -0,0 +1,41 @@
+#include "common.h"
+#include "ColTriangle.h"
+
+void
+CColTriangle::Set(const CompressedVector *, int a, int b, int c, uint8 surf, uint8 piece)
+{
+	this->a = a;
+	this->b = b;
+	this->c = c;
+	this->surface = surf;
+}
+
+#ifdef VU_COLLISION
+void
+CColTrianglePlane::Set(const CVector &va, const CVector &vb, const CVector &vc)
+{
+	CVector norm = CrossProduct(vc-va, vb-va);
+	norm.Normalise();
+	float d = DotProduct(norm, va);
+	normal.x = norm.x*4096.0f;
+	normal.y = norm.y*4096.0f;
+	normal.z = norm.z*4096.0f;
+	dist = d*128.0f;
+}
+#else
+void
+CColTrianglePlane::Set(const CVector &va, const CVector &vb, const CVector &vc)
+{
+	normal = CrossProduct(vc-va, vb-va);
+	normal.Normalise();
+	dist = DotProduct(normal, va);
+	CVector an(Abs(normal.x), Abs(normal.y), Abs(normal.z));
+	// find out largest component and its direction
+	if(an.x > an.y && an.x > an.z)
+		dir = normal.x < 0.0f ? DIR_X_NEG : DIR_X_POS;
+	else if(an.y > an.z)
+		dir = normal.y < 0.0f ? DIR_Y_NEG : DIR_Y_POS;
+	else
+		dir = normal.z < 0.0f ? DIR_Z_NEG : DIR_Z_POS;
+}
+#endif
\ No newline at end of file
diff --git a/src/collision/ColTriangle.h b/src/collision/ColTriangle.h
new file mode 100644
index 00000000..9e918e38
--- /dev/null
+++ b/src/collision/ColTriangle.h
@@ -0,0 +1,68 @@
+#pragma once
+
+#include "CompressedVector.h"
+
+enum Direction {
+	DIR_X_POS,
+	DIR_X_NEG,
+	DIR_Y_POS,
+	DIR_Y_NEG,
+	DIR_Z_POS,
+	DIR_Z_NEG,
+};
+
+struct CColTriangle
+{
+	uint16 a;
+	uint16 b;
+	uint16 c;
+	uint8 surface;
+
+	void Set(const CompressedVector *v, int a, int b, int c, uint8 surf, uint8 piece);
+};
+
+struct CColTrianglePlane
+{
+#ifdef VU_COLLISION
+	CompressedVector normal;
+	int16 dist;
+
+	void Set(const CVector &va, const CVector &vb, const CVector &vc);
+	void Set(const CompressedVector *v, CColTriangle &tri) { Set(v[tri.a].Get(), v[tri.b].Get(), v[tri.c].Get()); }
+	void GetNormal(CVector &n) const { n.x = normal.x/4096.0f; n.y = normal.y/4096.0f; n.z = normal.z/4096.0f; }
+	float CalcPoint(const CVector &v) const { CVector n; GetNormal(n); return DotProduct(n, v) - dist/128.0f; };
+#ifdef GTA_PS2
+	void Unpack(uint128 &qword) const {
+		__asm__ volatile (
+			"lh      $8, 0(%1)\n"
+			"lh      $9, 2(%1)\n"
+			"lh      $10, 4(%1)\n"
+			"lh      $11, 6(%1)\n"
+			"pextlw  $10, $8\n"
+			"pextlw  $11, $9\n"
+			"pextlw  $2, $11, $10\n"
+			"sq      $2, %0\n"
+			: "=m" (qword)
+			: "r" (this)
+			: "$8", "$9", "$10", "$11", "$2"
+		);
+	}
+#else
+	void Unpack(int32 *qword) const {
+		qword[0] = normal.x;
+		qword[1] = normal.y;
+		qword[2] = normal.z;
+		qword[3] = dist;
+	}
+#endif
+#else
+	CVector normal;
+	float dist;
+	uint8 dir;
+
+	void Set(const CVector &va, const CVector &vb, const CVector &vc);
+	void Set(const CompressedVector *v, CColTriangle &tri) { Set(v[tri.a].Get(), v[tri.b].Get(), v[tri.c].Get()); }
+	void GetNormal(CVector &n) const { n = normal; }
+	float CalcPoint(const CVector &v) const { return DotProduct(normal, v) - dist; };
+#endif
+};
\ No newline at end of file
diff --git a/src/core/Collision.cpp b/src/collision/Collision.cpp
index d8603cd8..41997e32 100644
--- a/src/core/Collision.cpp
+++ b/src/collision/Collision.cpp
@@ -23,303 +23,8 @@
 #include "Collision.h"
 #include "Frontend.h"
 
-
-// TODO: where do these go?
-
 #ifdef VU_COLLISION
-
-struct VuTriangle
-{
-	// Compressed int16 but unpacked
-#ifdef GTA_PS2
-	uint128 v0;
-	uint128 v1;
-	uint128 v2;
-	uint128 plane;
-#else
-	int32 v0[4];
-	int32 v1[4];
-	int32 v2[4];
-	int32 plane[4];
-#endif
-};
-
-#ifndef GTA_PS2
-static int16 vi01;
-static CVuVector vf01;
-static CVuVector vf02;
-static CVuVector vf03;
-
-CVuVector
-DistanceBetweenSphereAndLine(const CVuVector &center, const CVuVector &p0, const CVuVector &line)
-{
-	// center  VF12
-	// p0      VF14
-	// line    VF15
-	CVuVector ret;	// VF16
-	CVuVector p1 = p0+line;
-	CVuVector dist0 = center - p0;	// VF20
-	CVuVector dist1 = center - p1;	// VF25
-	float lenSq = line.MagnitudeSqr();	// VF21
-	float distSq0 = dist0.MagnitudeSqr();	// VF22
-	float distSq1 = dist1.MagnitudeSqr();
-	float dot = DotProduct(dist0, line);	// VF23
-	if(dot < 0.0f){
-		// not above line, closest to p0
-		ret = p0;
-		ret.w = distSq0;
-		return ret;
-	}
-	float t = dot/lenSq;	// param of nearest point on infinite line
-	if(t > 1.0f){
-		// not above line, closest to p1
-		ret = p1;
-		ret.w = distSq1;
-		return ret;
-	}
-	// closest to line
-	ret = p0 + line*t;
-	ret.w = (ret - center).MagnitudeSqr();
-	return ret;
-}
-inline int SignFlags(const CVector &v)
-{
-	int f = 0;
-	if(v.x < 0.0f) f |= 1;
-	if(v.y < 0.0f) f |= 2;
-	if(v.z < 0.0f) f |= 4;
-	return f;
-}
-#endif
-
-extern "C" void
-LineToTriangleCollision(const CVuVector &p0, const CVuVector &p1,
-	const CVuVector &v0, const CVuVector &v1, const CVuVector &v2,
-	const CVuVector &plane)
-{
-#ifdef GTA_PS2
-	__asm__ volatile (
-		".set noreorder\n"
-		"lqc2	vf12, 0x0(%0)\n"
-		"lqc2	vf13, 0x0(%1)\n"
-		"lqc2	vf14, 0x0(%2)\n"
-		"lqc2	vf15, 0x0(%3)\n"
-		"lqc2	vf16, 0x0(%4)\n"
-		"lqc2	vf17, 0x0(%5)\n"
-		"vcallms	Vu0LineToTriangleCollisionStart\n"
-		".set reorder\n"
-		:
-		: "r" (&p0), "r" (&p1), "r" (&v0), "r" (&v1), "r" (&v2), "r" (&plane)
-	);
-#else
-	float dot0 = DotProduct(plane, p0);
-	float dot1 = DotProduct(plane, p1);
-	float dist0 = plane.w - dot0;
-	float dist1 = plane.w - dot1;
-
-	// if points are on the same side, no collision
-	if(dist0 * dist1 > 0.0f){
-		vi01 = 0;
-		return;
-	}
-
-	CVuVector diff = p1 - p0;
-	float t = dist0/(dot1 - dot0);
-	CVuVector p = p0 + diff*t;
-	p.w = 0.0f;
-	vf01 = p;
-	vf03.x = t;
-
-	// Check if point is inside
-	CVector cross1 = CrossProduct(p-v0, v1-v0);
-	CVector cross2 = CrossProduct(p-v1, v2-v1);
-	CVector cross3 = CrossProduct(p-v2, v0-v2);
-	// Only check relevant directions
-	int flagmask = 0;
-	if(Abs(plane.x) > 0.5f) flagmask |= 1;
-	if(Abs(plane.y) > 0.5f) flagmask |= 2;
-	if(Abs(plane.z) > 0.5f) flagmask |= 4;
-	int flags1 = SignFlags(cross1) & flagmask;
-	int flags2 = SignFlags(cross2) & flagmask;
-	int flags3 = SignFlags(cross3) & flagmask;
-	// inside if on the same side of all edges
-	if(flags1 != flags2 || flags1 != flags3){
-		vi01 = 0;
-		return;
-	}
-	vi01 = 1;
-	vf02 = plane;
-	return;
-#endif
-}
-
-extern "C" void
-LineToTriangleCollisionCompressed(const CVuVector &p0, const CVuVector &p1, VuTriangle &tri)
-{
-#ifdef GTA_PS2
-	__asm__ volatile (
-		".set noreorder\n"
-		"lqc2	vf12, 0x0(%0)\n"
-		"lqc2	vf13, 0x0(%1)\n"
-		"lqc2	vf14, 0x0(%2)\n"
-		"lqc2	vf15, 0x10(%2)\n"
-		"lqc2	vf16, 0x20(%2)\n"
-		"lqc2	vf17, 0x30(%2)\n"
-		"vcallms	Vu0LineToTriangleCollisionCompressedStart\n"
-		".set reorder\n"
-		:
-		: "r" (&p0), "r" (&p1), "r" (&tri)
-	);
-#else
-	CVuVector v0, v1, v2, plane;
-	v0.x = tri.v0[0]/128.0f;
-	v0.y = tri.v0[1]/128.0f;
-	v0.z = tri.v0[2]/128.0f;
-	v0.w = tri.v0[3]/128.0f;
-	v1.x = tri.v1[0]/128.0f;
-	v1.y = tri.v1[1]/128.0f;
-	v1.z = tri.v1[2]/128.0f;
-	v1.w = tri.v1[3]/128.0f;
-	v2.x = tri.v2[0]/128.0f;
-	v2.y = tri.v2[1]/128.0f;
-	v2.z = tri.v2[2]/128.0f;
-	v2.w = tri.v2[3]/128.0f;
-	plane.x = tri.plane[0]/4096.0f;
-	plane.y = tri.plane[1]/4096.0f;
-	plane.z = tri.plane[2]/4096.0f;
-	plane.w = tri.plane[3]/128.0f;
-	LineToTriangleCollision(p0, p1, v0, v1, v2, plane);
-#endif
-}
-
-extern "C" void
-SphereToTriangleCollision(const CVuVector &sph,
-	const CVuVector &v0, const CVuVector &v1, const CVuVector &v2,
-	const CVuVector &plane)
-{
-#ifdef GTA_PS2
-	__asm__ volatile (
-		".set noreorder\n"
-		"lqc2	vf12, 0x0(%0)\n"
-		"lqc2	vf14, 0x0(%1)\n"
-		"lqc2	vf15, 0x0(%2)\n"
-		"lqc2	vf16, 0x0(%3)\n"
-		"lqc2	vf17, 0x0(%4)\n"
-		"vcallms	Vu0SphereToTriangleCollisionStart\n"
-		".set reorder\n"
-		:
-		: "r" (&sph), "r" (&v0), "r" (&v1), "r" (&v2), "r" (&plane)
-	);
-#else
-	float planedist = DotProduct(plane, sph) - plane.w;	// VF02
-	if(Abs(planedist) > sph.w){
-		vi01 = 0;
-		return;
-	}
-	// point on plane
-	CVuVector p = sph - planedist*plane;
-	p.w = 0.0f;
-	vf01 = p;
-	planedist = Abs(planedist);
-	// edges
-	CVuVector v01 = v1 - v0;
-	CVuVector v12 = v2 - v1;
-	CVuVector v20 = v0 - v2;
-	// VU code calculates normal again for some weird reason...
-	// Check sides of point
-	CVector cross1 = CrossProduct(p-v0, v01);
-	CVector cross2 = CrossProduct(p-v1, v12);
-	CVector cross3 = CrossProduct(p-v2, v20);
-	// Only check relevant directions
-	int flagmask = 0;
-	if(Abs(plane.x) > 0.1f) flagmask |= 1;
-	if(Abs(plane.y) > 0.1f) flagmask |= 2;
-	if(Abs(plane.z) > 0.1f) flagmask |= 4;
-	int nflags = SignFlags(plane) & flagmask;
-	int flags1 = SignFlags(cross1) & flagmask;
-	int flags2 = SignFlags(cross2) & flagmask;
-	int flags3 = SignFlags(cross3) & flagmask;
-	int testcase = 0;
-	CVuVector closest(0.0f, 0.0f, 0.0f);	// VF04
-	if(flags1 == nflags){
-		closest += v2;
-		testcase++;
-	}
-	if(flags2 == nflags){
-		closest += v0;
-		testcase++;
-	}
-	if(flags3 == nflags){
-		closest += v1;
-		testcase++;
-	}
-	if(testcase == 3){
-		// inside triangle - dist to plane already checked
-		vf02 = plane;
-		vf02.w = vf03.x = planedist;
-		vi01 = 1;
-	}else if(testcase == 1){
-		// outside two sides - closest to point opposide inside edge
-		vf01 = closest;
-		vf02 = sph - closest;
-		float distSq = vf02.MagnitudeSqr();
-		vi01 = sph.w*sph.w > distSq;
-		vf03.x = Sqrt(distSq);
-		vf02 *= 1.0f/vf03.x;
-	}else{
-		// inside two sides - closest to third edge
-		if(flags1 != nflags)
-			closest = DistanceBetweenSphereAndLine(sph, v0, v01);
-		else if(flags2 != nflags)
-			closest = DistanceBetweenSphereAndLine(sph, v1, v12);
-		else
-			closest = DistanceBetweenSphereAndLine(sph, v2, v20);
-		vi01 = sph.w*sph.w > closest.w;
-		vf01 = closest;
-		vf02 = sph - closest;
-		vf03.x = Sqrt(closest.w);
-		vf02 *= 1.0f/vf03.x;
-	}
-#endif
-}
-
-extern "C" void
-SphereToTriangleCollisionCompressed(const CVuVector &sph, VuTriangle &tri)
-{
-#ifdef GTA_PS2
-	__asm__ volatile (
-		".set noreorder\n"
-		"lqc2	vf12, 0x0(%0)\n"
-		"lqc2	vf14, 0x0(%1)\n"
-		"lqc2	vf15, 0x10(%1)\n"
-		"lqc2	vf16, 0x20(%1)\n"
-		"lqc2	vf17, 0x30(%1)\n"
-		"vcallms	Vu0SphereToTriangleCollisionCompressedStart\n"
-		".set reorder\n"
-		:
-		: "r" (&sph), "r" (&tri)
-	);
-#else
-	CVuVector v0, v1, v2, plane;
-	v0.x = tri.v0[0]/128.0f;
-	v0.y = tri.v0[1]/128.0f;
-	v0.z = tri.v0[2]/128.0f;
-	v0.w = tri.v0[3]/128.0f;
-	v1.x = tri.v1[0]/128.0f;
-	v1.y = tri.v1[1]/128.0f;
-	v1.z = tri.v1[2]/128.0f;
-	v1.w = tri.v1[3]/128.0f;
-	v2.x = tri.v2[0]/128.0f;
-	v2.y = tri.v2[1]/128.0f;
-	v2.z = tri.v2[2]/128.0f;
-	v2.w = tri.v2[3]/128.0f;
-	plane.x = tri.plane[0]/4096.0f;
-	plane.y = tri.plane[1]/4096.0f;
-	plane.z = tri.plane[2]/4096.0f;
-	plane.w = tri.plane[3]/128.0f;
-	SphereToTriangleCollision(sph, v0, v1, v2, plane);
-#endif
-}
+#include "VuCollision.h"
 
 inline int
 GetVUresult(void)
@@ -362,17 +67,6 @@ GetVUresult(CVuVector &point, CVuVector &normal, float &dist)
 
 #endif
 
-
-enum Direction
-{
-	DIR_X_POS,
-	DIR_X_NEG,
-	DIR_Y_POS,
-	DIR_Y_NEG,
-	DIR_Z_POS,
-	DIR_Z_NEG,
-};
-
 eLevelName CCollision::ms_collisionInMemory;
 CLinkList<CColModel*> CCollision::ms_colModelCache;
 
@@ -2412,11 +2106,12 @@ CCollision::ProcessColModels(const CMatrix &matrixA, CColModel &modelA,
 	assert(modelA.numLines <= MAXNUMLINES);
 
 	// From model A space to model B space
-	Invert(matrixB, matAB);
+	matAB = Invert(matrixB, matAB);
 	matAB *= matrixA;
 
 	CColSphere bsphereAB;	// bounding sphere of A in B space
-	bsphereAB.Set(modelA.boundingSphere.radius, matAB * modelA.boundingSphere.center);
+	bsphereAB.radius = modelA.boundingSphere.radius;
+	bsphereAB.center = matAB * modelA.boundingSphere.center;
 	if(!TestSphereBox(bsphereAB, modelB.boundingBox))
 		return 0;
 	// B to A space
@@ -2449,7 +2144,8 @@ CCollision::ProcessColModels(const CMatrix &matrixA, CColModel &modelA,
 	int numBoxesB = 0;
 	int numTrianglesB = 0;
 	for(i = 0; i < modelB.numSpheres; i++){
-		s.Set(modelB.spheres[i].radius, matBA * modelB.spheres[i].center);
+		s.radius = modelB.spheres[i].radius;
+		s.center = matBA * modelB.spheres[i].center;
 		if(TestSphereBox(s, modelA.boundingBox))
 			aSphereIndicesB[numSpheresB++] = i;
 	}
@@ -3037,254 +2733,4 @@ CCollision::DrawColModel_Coloured(const CMatrix &mat, const CColModel &colModel,
 	RwRenderStateSet(rwRENDERSTATEVERTEXALPHAENABLE, (void*)FALSE);
 	RwRenderStateSet(rwRENDERSTATEZWRITEENABLE, (void*)TRUE);
 	RwRenderStateSet(rwRENDERSTATEZTESTENABLE, (void*)TRUE);
-}
-
-
-/*
- * ColModel code
- */
-
-void
-CColSphere::Set(float radius, const CVector &center, uint8 surf, uint8 piece)
-{
-	this->radius = radius;
-	this->center = center;
-	this->surface = surf;
-	this->piece = piece;
-}
-
-void
-CColBox::Set(const CVector &min, const CVector &max, uint8 surf, uint8 piece)
-{
-	this->min = min;
-	this->max = max;
-	this->surface = surf;
-	this->piece = piece;
-}
-
-void
-CColLine::Set(const CVector &p0, const CVector &p1)
-{
-	this->p0 = p0;
-	this->p1 = p1;
-}
-
-void
-CColTriangle::Set(const CompressedVector *, int a, int b, int c, uint8 surf, uint8 piece)
-{
-	this->a = a;
-	this->b = b;
-	this->c = c;
-	this->surface = surf;
-}
-
-#ifdef VU_COLLISION
-void
-CColTrianglePlane::Set(const CVector &va, const CVector &vb, const CVector &vc)
-{
-	CVector norm = CrossProduct(vc-va, vb-va);
-	norm.Normalise();
-	float d = DotProduct(norm, va);
-	normal.x = norm.x*4096.0f;
-	normal.y = norm.y*4096.0f;
-	normal.z = norm.z*4096.0f;
-	dist = d*128.0f;
-}
-#else
-void
-CColTrianglePlane::Set(const CVector &va, const CVector &vb, const CVector &vc)
-{
-	normal = CrossProduct(vc-va, vb-va);
-	normal.Normalise();
-	dist = DotProduct(normal, va);
-	CVector an(Abs(normal.x), Abs(normal.y), Abs(normal.z));
-	// find out largest component and its direction
-	if(an.x > an.y && an.x > an.z)
-		dir = normal.x < 0.0f ? DIR_X_NEG : DIR_X_POS;
-	else if(an.y > an.z)
-		dir = normal.y < 0.0f ? DIR_Y_NEG : DIR_Y_POS;
-	else
-		dir = normal.z < 0.0f ? DIR_Z_NEG : DIR_Z_POS;
-}
-#endif
-
-CColModel::CColModel(void)
-{
-	numSpheres = 0;
-	spheres = nil;
-	numLines = 0;
-	lines = nil;
-	numBoxes = 0;
-	boxes = nil;
-	numTriangles = 0;
-	vertices = nil;
-	triangles = nil;
-	trianglePlanes = nil;
-	level = CGame::currLevel;
-	ownsCollisionVolumes = true;
-}
-
-CColModel::~CColModel(void)
-{
-	RemoveCollisionVolumes();
-	RemoveTrianglePlanes();
-}
-
-void
-CColModel::RemoveCollisionVolumes(void)
-{
-	if(ownsCollisionVolumes){
-		RwFree(spheres);
-		RwFree(lines);
-		RwFree(boxes);
-		RwFree(vertices);
-		RwFree(triangles);
-	}
-	numSpheres = 0;
-	numLines = 0;
-	numBoxes = 0;
-	numTriangles = 0;
-	spheres = nil;
-	lines = nil;
-	boxes = nil;
-	vertices = nil;
-	triangles = nil;
-}
-
-void
-CColModel::CalculateTrianglePlanes(void)
-{
-	// HACK: allocate space for one more element to stuff the link pointer into
-	trianglePlanes = (CColTrianglePlane*)RwMalloc(sizeof(CColTrianglePlane) * (numTriangles+1));
-	for(int i = 0; i < numTriangles; i++)
-		trianglePlanes[i].Set(vertices, triangles[i]);
-}
-
-void
-CColModel::RemoveTrianglePlanes(void)
-{
-	RwFree(trianglePlanes);
-	trianglePlanes = nil;
-}
-
-void
-CColModel::SetLinkPtr(CLink<CColModel*> *lptr)
-{
-	assert(trianglePlanes);
-	*(CLink<CColModel*>**)ALIGNPTR(&trianglePlanes[numTriangles]) = lptr;
-}
-
-CLink<CColModel*>*
-CColModel::GetLinkPtr(void)
-{
-	assert(trianglePlanes);
-	return *(CLink<CColModel*>**)ALIGNPTR(&trianglePlanes[numTriangles]);
-}
-
-void
-CColModel::GetTrianglePoint(CVector &v, int i) const
-{
-	v = vertices[i].Get();
-}
-
-CColModel&
-CColModel::operator=(const CColModel &other)
-{
-	int i;
-	int numVerts;
-
-	boundingSphere = other.boundingSphere;
-	boundingBox = other.boundingBox;
-
-	// copy spheres
-	if(other.numSpheres){
-		if(numSpheres != other.numSpheres){
-			numSpheres = other.numSpheres;
-			if(spheres)
-				RwFree(spheres);
-			spheres = (CColSphere*)RwMalloc(numSpheres*sizeof(CColSphere));
-		}
-		for(i = 0; i < numSpheres; i++)
-			spheres[i] = other.spheres[i];
-	}else{
-		numSpheres = 0;
-		if(spheres)
-			RwFree(spheres);
-		spheres = nil;
-	}
-
-	// copy lines
-	if(other.numLines){
-		if(numLines != other.numLines){
-			numLines = other.numLines;
-			if(lines)
-				RwFree(lines);
-			lines = (CColLine*)RwMalloc(numLines*sizeof(CColLine));
-		}
-		for(i = 0; i < numLines; i++)
-			lines[i] = other.lines[i];
-	}else{
-		numLines = 0;
-		if(lines)
-			RwFree(lines);
-		lines = nil;
-	}
-
-	// copy boxes
-	if(other.numBoxes){
-		if(numBoxes != other.numBoxes){
-			numBoxes = other.numBoxes;
-			if(boxes)
-				RwFree(boxes);
-			boxes = (CColBox*)RwMalloc(numBoxes*sizeof(CColBox));
-		}
-		for(i = 0; i < numBoxes; i++)
-			boxes[i] = other.boxes[i];
-	}else{
-		numBoxes = 0;
-		if(boxes)
-			RwFree(boxes);
-		boxes = nil;
-	}
-
-	// copy mesh
-	if(other.numTriangles){
-		// copy vertices
-		numVerts = 0;
-		for(i = 0; i < other.numTriangles; i++){
-			if(other.triangles[i].a > numVerts)
-				numVerts = other.triangles[i].a;
-			if(other.triangles[i].b > numVerts)
-				numVerts = other.triangles[i].b;
-			if(other.triangles[i].c > numVerts)
-				numVerts = other.triangles[i].c;
-		}
-		numVerts++;
-		if(vertices)
-			RwFree(vertices);
-		if(numVerts){
-			vertices = (CompressedVector*)RwMalloc(numVerts*sizeof(CompressedVector));
-			for(i = 0; i < numVerts; i++)
-				vertices[i] = other.vertices[i];
-		}
-
-		// copy triangles
-		if(numTriangles != other.numTriangles){
-			numTriangles = other.numTriangles;
-			if(triangles)
-				RwFree(triangles);
-			triangles = (CColTriangle*)RwMalloc(numTriangles*sizeof(CColTriangle));
-		}
-		for(i = 0; i < numTriangles; i++)
-			triangles[i] = other.triangles[i];
-	}else{
-		numTriangles = 0;
-		if(triangles)
-			RwFree(triangles);
-		triangles = nil;
-		if(vertices)
-			RwFree(vertices);
-		vertices = nil;
-	}
-	return *this;
-}
+}
\ No newline at end of file
diff --git a/src/collision/Collision.h b/src/collision/Collision.h
new file mode 100644
index 00000000..f4270bc5
--- /dev/null
+++ b/src/collision/Collision.h
@@ -0,0 +1,70 @@
+#pragma once
+
+#include "ColModel.h"
+#include "Game.h"	// for eLevelName
+#ifdef VU_COLLISION
+#include "VuVector.h"
+#endif
+
+struct CStoredCollPoly
+{
+#ifdef VU_COLLISION
+	CVuVector verts[3];
+#else
+	CVector verts[3];
+#endif
+	bool valid;
+};
+
+// If you spawn many tanks at once, you will see that collisions of two entity exceeds 32.
+#if defined(FIX_BUGS) && !defined(SQUEEZE_PERFORMANCE)
+#define MAX_COLLISION_POINTS 64
+#else
+#define MAX_COLLISION_POINTS 32
+#endif
+
+class CCollision
+{
+public:
+	static eLevelName ms_collisionInMemory;
+	static CLinkList<CColModel*> ms_colModelCache;
+#ifdef NO_ISLAND_LOADING
+	static bool bAlreadyLoaded;
+#endif
+
+	static void Init(void);
+	static void Shutdown(void);
+	static void Update(void);
+	static void LoadCollisionWhenINeedIt(bool changeLevel);
+	static void SortOutCollisionAfterLoad(void);
+	static void LoadCollisionScreen(eLevelName level);
+	static void DrawColModel(const CMatrix &mat, const CColModel &colModel);
+	static void DrawColModel_Coloured(const CMatrix &mat, const CColModel &colModel, int32 id);
+
+	static void CalculateTrianglePlanes(CColModel *model);
+
+	// all these return true if there's a collision
+	static bool TestSphereSphere(const CColSphere &s1, const CColSphere &s2);
+	static bool TestSphereBox(const CColSphere &sph, const CColBox &box);
+	static bool TestLineBox(const CColLine &line, const CColBox &box);
+	static bool TestVerticalLineBox(const CColLine &line, const CColBox &box);
+	static bool TestLineTriangle(const CColLine &line, const CompressedVector *verts, const CColTriangle &tri, const CColTrianglePlane &plane);
+	static bool TestLineSphere(const CColLine &line, const CColSphere &sph);
+	static bool TestSphereTriangle(const CColSphere &sphere, const CompressedVector *verts, const CColTriangle &tri, const CColTrianglePlane &plane);
+	static bool TestLineOfSight(const CColLine &line, const CMatrix &matrix, CColModel &model, bool ignoreSeeThrough);
+
+	static bool ProcessSphereSphere(const CColSphere &s1, const CColSphere &s2, CColPoint &point, float &mindistsq);
+	static bool ProcessSphereBox(const CColSphere &sph, const CColBox &box, CColPoint &point, float &mindistsq);
+	static bool ProcessLineBox(const CColLine &line, const CColBox &box, CColPoint &point, float &mindist);
+	static bool ProcessVerticalLineTriangle(const CColLine &line, const CompressedVector *verts, const CColTriangle &tri, const CColTrianglePlane &plane, CColPoint &point, float &mindist, CStoredCollPoly *poly);
+	static bool ProcessLineTriangle(const CColLine &line , const CompressedVector *verts, const CColTriangle &tri, const CColTrianglePlane &plane, CColPoint &point, float &mindist);
+	static bool ProcessLineSphere(const CColLine &line, const CColSphere &sphere, CColPoint &point, float &mindist);
+	static bool ProcessSphereTriangle(const CColSphere &sph, const CompressedVector *verts, const CColTriangle &tri, const CColTrianglePlane &plane, CColPoint &point, float &mindistsq);
+	static bool ProcessLineOfSight(const CColLine &line, const CMatrix &matrix, CColModel &model, CColPoint &point, float &mindist, bool ignoreSeeThrough);
+	static bool ProcessVerticalLine(const CColLine &line, const CMatrix &matrix, CColModel &model, CColPoint &point, float &mindist, bool ignoreSeeThrough, CStoredCollPoly *poly);
+	static int32 ProcessColModels(const CMatrix &matrixA, CColModel &modelA, const CMatrix &matrixB, CColModel &modelB, CColPoint *spherepoints, CColPoint *linepoints, float *linedists);
+	static bool IsStoredPolyStillValidVerticalLine(const CVector &pos, float z, CColPoint &point, CStoredCollPoly *poly);
+
+	static float DistToLine(const CVector *l0, const CVector *l1, const CVector *point);
+	static float DistToLine(const CVector *l0, const CVector *l1, const CVector *point, CVector &closest);
+};
diff --git a/src/collision/CompressedVector.h b/src/collision/CompressedVector.h
new file mode 100644
index 00000000..d54e49b1
--- /dev/null
+++ b/src/collision/CompressedVector.h
@@ -0,0 +1,36 @@
+#pragma once
+
+struct CompressedVector
+{
+#ifdef COMPRESSED_COL_VECTORS
+	int16 x, y, z;
+	CVector Get(void) const { return CVector(x, y, z)/128.0f; };
+	void Set(float x, float y, float z) { this->x = x*128.0f; this->y = y*128.0f; this->z = z*128.0f; };
+#ifdef GTA_PS2
+	void Unpack(uint128 &qword) const {
+		__asm__ volatile (
+			"lh      $8, 0(%1)\n"
+			"lh      $9, 2(%1)\n"
+			"lh      $10, 4(%1)\n"
+			"pextlw  $10, $8\n"
+			"pextlw  $2, $9, $10\n"
+			"sq      $2, %0\n"
+			: "=m" (qword)
+			: "r" (this)
+			: "$8", "$9", "$10", "$2"
+		);
+	}
+#else
+	void Unpack(int32 *qword) const {
+		qword[0] = x;
+		qword[1] = y;
+		qword[2] = z;
+		qword[3] = 0;	// junk
+	}
+#endif
+#else
+	float x, y, z;
+	CVector Get(void) const { return CVector(x, y, z); };
+	void Set(float x, float y, float z) { this->x = x; this->y = y; this->z = z; };
+#endif
+};
\ No newline at end of file
diff --git a/src/core/TempColModels.cpp b/src/collision/TempColModels.cpp
index f6796909..849eb01e 100644
--- a/src/core/TempColModels.cpp
+++ b/src/collision/TempColModels.cpp
@@ -1,7 +1,6 @@
 #include "common.h"
 
 #include "TempColModels.h"
-#include "SurfaceTable.h"
 
 CColModel CTempColModels::ms_colModelPed1;
 CColModel CTempColModels::ms_colModelPed2;
@@ -45,13 +44,13 @@ CTempColModels::Initialise(void)
 
 	int i;
 
-	ms_colModelBBox.boundingSphere.Set(2.0f, CVector(0.0f, 0.0f, 0.0f), SURFACE_DEFAULT, 0);
-	ms_colModelBBox.boundingBox.Set(CVector(-2.0f, -2.0f, -2.0f), CVector(2.0f, 2.0f, 2.0f), SURFACE_DEFAULT, 0);
+	ms_colModelBBox.boundingSphere.Set(2.0f, CVector(0.0f, 0.0f, 0.0f));
+	ms_colModelBBox.boundingBox.Set(CVector(-2.0f, -2.0f, -2.0f), CVector(2.0f, 2.0f, 2.0f));
 	ms_colModelBBox.level = LEVEL_GENERIC;
 
 	for (i = 0; i < ARRAY_SIZE(ms_colModelCutObj); i++) {
-		ms_colModelCutObj[i].boundingSphere.Set(2.0f, CVector(0.0f, 0.0f, 0.0f), SURFACE_DEFAULT, 0);
-		ms_colModelCutObj[i].boundingBox.Set(CVector(-2.0f, -2.0f, -2.0f), CVector(2.0f, 2.0f, 2.0f), SURFACE_DEFAULT, 0);
+		ms_colModelCutObj[i].boundingSphere.Set(2.0f, CVector(0.0f, 0.0f, 0.0f));
+		ms_colModelCutObj[i].boundingBox.Set(CVector(-2.0f, -2.0f, -2.0f), CVector(2.0f, 2.0f, 2.0f));
 		ms_colModelCutObj[i].level = LEVEL_GENERIC;
 	}
 
@@ -73,8 +72,8 @@ CTempColModels::Initialise(void)
 		s_aPedSpheres[i].piece = 0;
 	}
 
-	ms_colModelPed1.boundingSphere.Set(1.25f, CVector(0.0f, 0.0f, 0.0f), SURFACE_DEFAULT, 0);
-	ms_colModelPed1.boundingBox.Set(CVector(-0.35f, -0.35f, -1.0f), CVector(0.35f, 0.35f, 0.9f), SURFACE_DEFAULT, 0);
+	ms_colModelPed1.boundingSphere.Set(1.25f, CVector(0.0f, 0.0f, 0.0f));
+	ms_colModelPed1.boundingBox.Set(CVector(-0.35f, -0.35f, -1.0f), CVector(0.35f, 0.35f, 0.9f));
 	SET_COLMODEL_SPHERES(ms_colModelPed1, s_aPedSpheres);
 
 	// Ped 2 Spheres
@@ -92,8 +91,8 @@ CTempColModels::Initialise(void)
 		s_aPed2Spheres[i].piece = 0;
 	}
 
-	ms_colModelPed2.boundingSphere.Set(2.0f, CVector(0.0f, 0.0f, 0.0f), SURFACE_DEFAULT, 0);
-	ms_colModelPed2.boundingBox.Set(CVector(-0.7f, -0.7f, -1.2f), CVector(0.7f, 0.7f, 0.0f), SURFACE_DEFAULT, 0);
+	ms_colModelPed2.boundingSphere.Set(2.0f, CVector(0.0f, 0.0f, 0.0f));
+	ms_colModelPed2.boundingBox.Set(CVector(-0.7f, -0.7f, -1.2f), CVector(0.7f, 0.7f, 0.0f));
 
 	SET_COLMODEL_SPHERES(ms_colModelPed2, s_aPed2Spheres);
 
@@ -118,8 +117,8 @@ CTempColModels::Initialise(void)
 	s_aPedGSpheres[2].piece = 0;
 	s_aPedGSpheres[3].piece = 6;
 
-	ms_colModelPedGroundHit.boundingSphere.Set(2.0f, CVector(0.0f, 0.0f, 0.0f), SURFACE_DEFAULT, 0);
-	ms_colModelPedGroundHit.boundingBox.Set(CVector(-0.4f, -1.0f, -1.25f), CVector(0.4f, 1.2f, -0.5f), SURFACE_DEFAULT, 0);
+	ms_colModelPedGroundHit.boundingSphere.Set(2.0f, CVector(0.0f, 0.0f, 0.0f));
+	ms_colModelPedGroundHit.boundingBox.Set(CVector(-0.4f, -1.0f, -1.25f), CVector(0.4f, 1.2f, -0.5f));
 
 	SET_COLMODEL_SPHERES(ms_colModelPedGroundHit, s_aPedGSpheres);
 
@@ -142,8 +141,8 @@ CTempColModels::Initialise(void)
 		s_aDoorSpheres[i].piece = 0;
 	}
 
-	ms_colModelDoor1.boundingSphere.Set(1.5f, CVector(0.0f, -0.6f, 0.0f), SURFACE_DEFAULT, 0);
-	ms_colModelDoor1.boundingBox.Set(CVector(-0.3f, 0.0f, -0.6f), CVector(0.3f, -1.2f, 0.6f), SURFACE_DEFAULT, 0);
+	ms_colModelDoor1.boundingSphere.Set(1.5f, CVector(0.0f, -0.6f, 0.0f));
+	ms_colModelDoor1.boundingBox.Set(CVector(-0.3f, 0.0f, -0.6f), CVector(0.3f, -1.2f, 0.6f));
 
 	SET_COLMODEL_SPHERES(ms_colModelDoor1, s_aDoorSpheres);
 
@@ -162,8 +161,8 @@ CTempColModels::Initialise(void)
 		s_aBumperSpheres[i].piece = 0;
 	}
 
-	ms_colModelBumper1.boundingSphere.Set(2.2f, CVector(0.0f, -0.6f, 0.0f), SURFACE_DEFAULT, 0);
-	ms_colModelBumper1.boundingBox.Set(CVector(-1.2f, -0.3f, -0.2f), CVector(1.2f, 0.3f, 0.2f), SURFACE_DEFAULT, 0);
+	ms_colModelBumper1.boundingSphere.Set(2.2f, CVector(0.0f, -0.6f, 0.0f));
+	ms_colModelBumper1.boundingBox.Set(CVector(-1.2f, -0.3f, -0.2f), CVector(1.2f, 0.3f, 0.2f));
 
 	SET_COLMODEL_SPHERES(ms_colModelBumper1, s_aBumperSpheres);
 
@@ -182,8 +181,8 @@ CTempColModels::Initialise(void)
 		s_aPanelSpheres[i].piece = 0;
 	}
 
-	ms_colModelPanel1.boundingSphere.Set(1.4f, CVector(0.0f, 0.0f, 0.0f), SURFACE_DEFAULT, 0);
-	ms_colModelPanel1.boundingBox.Set(CVector(-0.3f, -0.6f, -0.15f), CVector(0.3f, 0.6f, 0.15f), SURFACE_DEFAULT, 0);
+	ms_colModelPanel1.boundingSphere.Set(1.4f, CVector(0.0f, 0.0f, 0.0f));
+	ms_colModelPanel1.boundingBox.Set(CVector(-0.3f, -0.6f, -0.15f), CVector(0.3f, 0.6f, 0.15f));
 
 	SET_COLMODEL_SPHERES(ms_colModelPanel1, s_aPanelSpheres);
 
@@ -202,8 +201,8 @@ CTempColModels::Initialise(void)
 		s_aBonnetSpheres[i].piece = 0;
 	}
 
-	ms_colModelBonnet1.boundingSphere.Set(1.7f, CVector(0.0f, 0.5f, 0.0f), SURFACE_DEFAULT, 0);
-	ms_colModelBonnet1.boundingBox.Set(CVector(-0.7f, -0.2f, -0.3f), CVector(0.7f, 1.2f, 0.3f), SURFACE_DEFAULT, 0);
+	ms_colModelBonnet1.boundingSphere.Set(1.7f, CVector(0.0f, 0.5f, 0.0f));
+	ms_colModelBonnet1.boundingBox.Set(CVector(-0.7f, -0.2f, -0.3f), CVector(0.7f, 1.2f, 0.3f));
 
 	SET_COLMODEL_SPHERES(ms_colModelBonnet1, s_aBonnetSpheres);
 
@@ -222,8 +221,8 @@ CTempColModels::Initialise(void)
 		s_aBootSpheres[i].piece = 0;
 	}
 
-	ms_colModelBoot1.boundingSphere.Set(1.4f, CVector(0.0f, -0.4f, 0.0f), SURFACE_DEFAULT, 0);
-	ms_colModelBoot1.boundingBox.Set(CVector(-0.7f, -0.9f, -0.3f), CVector(0.7f, 0.2f, 0.3f), SURFACE_DEFAULT, 0);
+	ms_colModelBoot1.boundingSphere.Set(1.4f, CVector(0.0f, -0.4f, 0.0f));
+	ms_colModelBoot1.boundingBox.Set(CVector(-0.7f, -0.9f, -0.3f), CVector(0.7f, 0.2f, 0.3f));
 
 	SET_COLMODEL_SPHERES(ms_colModelBoot1, s_aBootSpheres);
 
@@ -244,8 +243,8 @@ CTempColModels::Initialise(void)
 		s_aWheelSpheres[i].piece = 0;
 	}
 
-	ms_colModelWheel1.boundingSphere.Set(1.4f, CVector(0.0f, 0.0f, 0.0f), SURFACE_DEFAULT, 0);
-	ms_colModelWheel1.boundingBox.Set(CVector(-0.7f, -0.4f, -0.4f), CVector(0.7f, 0.4f, 0.4f), SURFACE_DEFAULT, 0);
+	ms_colModelWheel1.boundingSphere.Set(1.4f, CVector(0.0f, 0.0f, 0.0f));
+	ms_colModelWheel1.boundingBox.Set(CVector(-0.7f, -0.4f, -0.4f), CVector(0.7f, 0.4f, 0.4f));
 
 	SET_COLMODEL_SPHERES(ms_colModelWheel1, s_aWheelSpheres);
 
@@ -266,8 +265,8 @@ CTempColModels::Initialise(void)
 		s_aBodyPartSpheres1[i].piece = 0;
 	}
 
-	ms_colModelBodyPart1.boundingSphere.Set(0.7f, CVector(0.4f, 0.0f, 0.0f), SURFACE_DEFAULT, 0);
-	ms_colModelBodyPart1.boundingBox.Set(CVector(-0.3f, -0.3f, -0.3f), CVector(1.1f, 0.3f, 0.3f), SURFACE_DEFAULT, 0);
+	ms_colModelBodyPart1.boundingSphere.Set(0.7f, CVector(0.4f, 0.0f, 0.0f));
+	ms_colModelBodyPart1.boundingBox.Set(CVector(-0.3f, -0.3f, -0.3f), CVector(1.1f, 0.3f, 0.3f));
 
 	SET_COLMODEL_SPHERES(ms_colModelBodyPart1, s_aBodyPartSpheres1);
 
@@ -288,8 +287,8 @@ CTempColModels::Initialise(void)
 		s_aBodyPartSpheres2[i].piece = 0;
 	}
 
-	ms_colModelBodyPart2.boundingSphere.Set(0.5f, CVector(0.25f, 0.0f, 0.0f), SURFACE_DEFAULT, 0);
-	ms_colModelBodyPart2.boundingBox.Set(CVector(-0.2f, -0.2f, -0.2f), CVector(0.7f, 0.2f, 0.2f), SURFACE_DEFAULT, 0);
+	ms_colModelBodyPart2.boundingSphere.Set(0.5f, CVector(0.25f, 0.0f, 0.0f));
+	ms_colModelBodyPart2.boundingBox.Set(CVector(-0.2f, -0.2f, -0.2f), CVector(0.7f, 0.2f, 0.2f));
 
 	SET_COLMODEL_SPHERES(ms_colModelBodyPart2, s_aBodyPartSpheres2);
 
diff --git a/src/core/TempColModels.h b/src/collision/TempColModels.h
index 3e1dd5e1..3e1dd5e1 100644
--- a/src/core/TempColModels.h
+++ b/src/collision/TempColModels.h
diff --git a/src/collision/VuCollision.cpp b/src/collision/VuCollision.cpp
new file mode 100644
index 00000000..8828d2e1
--- /dev/null
+++ b/src/collision/VuCollision.cpp
@@ -0,0 +1,282 @@
+#include "common.h"
+#ifdef VU_COLLISION
+#include "VuVector.h"
+#include "VuCollision.h"
+
+#ifndef GTA_PS2
+int16 vi01;
+CVuVector vf01;
+CVuVector vf02;
+CVuVector vf03;
+
+CVuVector
+DistanceBetweenSphereAndLine(const CVuVector &center, const CVuVector &p0, const CVuVector &line)
+{
+	// center  VF12
+	// p0      VF14
+	// line    VF15
+	CVuVector ret;	// VF16
+	CVuVector p1 = p0+line;
+	CVuVector dist0 = center - p0;	// VF20
+	CVuVector dist1 = center - p1;	// VF25
+	float lenSq = line.MagnitudeSqr();	// VF21
+	float distSq0 = dist0.MagnitudeSqr();	// VF22
+	float distSq1 = dist1.MagnitudeSqr();
+	float dot = DotProduct(dist0, line);	// VF23
+	if(dot < 0.0f){
+		// not above line, closest to p0
+		ret = p0;
+		ret.w = distSq0;
+		return ret;
+	}
+	float t = dot/lenSq;	// param of nearest point on infinite line
+	if(t > 1.0f){
+		// not above line, closest to p1
+		ret = p1;
+		ret.w = distSq1;
+		return ret;
+	}
+	// closest to line
+	ret = p0 + line*t;
+	ret.w = (ret - center).MagnitudeSqr();
+	return ret;
+}
+inline int SignFlags(const CVector &v)
+{
+	int f = 0;
+	if(v.x < 0.0f) f |= 1;
+	if(v.y < 0.0f) f |= 2;
+	if(v.z < 0.0f) f |= 4;
+	return f;
+}
+#endif
+
+extern "C" void
+LineToTriangleCollision(const CVuVector &p0, const CVuVector &p1,
+	const CVuVector &v0, const CVuVector &v1, const CVuVector &v2,
+	const CVuVector &plane)
+{
+#ifdef GTA_PS2
+	__asm__ volatile (
+		".set noreorder\n"
+		"lqc2	vf12, 0x0(%0)\n"
+		"lqc2	vf13, 0x0(%1)\n"
+		"lqc2	vf14, 0x0(%2)\n"
+		"lqc2	vf15, 0x0(%3)\n"
+		"lqc2	vf16, 0x0(%4)\n"
+		"lqc2	vf17, 0x0(%5)\n"
+		"vcallms	Vu0LineToTriangleCollisionStart\n"
+		".set reorder\n"
+		:
+		: "r" (&p0), "r" (&p1), "r" (&v0), "r" (&v1), "r" (&v2), "r" (&plane)
+	);
+#else
+	float dot0 = DotProduct(plane, p0);
+	float dot1 = DotProduct(plane, p1);
+	float dist0 = plane.w - dot0;
+	float dist1 = plane.w - dot1;
+
+	// if points are on the same side, no collision
+	if(dist0 * dist1 > 0.0f){
+		vi01 = 0;
+		return;
+	}
+
+	CVuVector diff = p1 - p0;
+	float t = dist0/(dot1 - dot0);
+	CVuVector p = p0 + diff*t;
+	p.w = 0.0f;
+	vf01 = p;
+	vf03.x = t;
+
+	// Check if point is inside
+	CVector cross1 = CrossProduct(p-v0, v1-v0);
+	CVector cross2 = CrossProduct(p-v1, v2-v1);
+	CVector cross3 = CrossProduct(p-v2, v0-v2);
+	// Only check relevant directions
+	int flagmask = 0;
+	if(Abs(plane.x) > 0.5f) flagmask |= 1;
+	if(Abs(plane.y) > 0.5f) flagmask |= 2;
+	if(Abs(plane.z) > 0.5f) flagmask |= 4;
+	int flags1 = SignFlags(cross1) & flagmask;
+	int flags2 = SignFlags(cross2) & flagmask;
+	int flags3 = SignFlags(cross3) & flagmask;
+	// inside if on the same side of all edges
+	if(flags1 != flags2 || flags1 != flags3){
+		vi01 = 0;
+		return;
+	}
+	vi01 = 1;
+	vf02 = plane;
+	return;
+#endif
+}
+
+extern "C" void
+LineToTriangleCollisionCompressed(const CVuVector &p0, const CVuVector &p1, VuTriangle &tri)
+{
+#ifdef GTA_PS2
+	__asm__ volatile (
+		".set noreorder\n"
+		"lqc2	vf12, 0x0(%0)\n"
+		"lqc2	vf13, 0x0(%1)\n"
+		"lqc2	vf14, 0x0(%2)\n"
+		"lqc2	vf15, 0x10(%2)\n"
+		"lqc2	vf16, 0x20(%2)\n"
+		"lqc2	vf17, 0x30(%2)\n"
+		"vcallms	Vu0LineToTriangleCollisionCompressedStart\n"
+		".set reorder\n"
+		:
+		: "r" (&p0), "r" (&p1), "r" (&tri)
+	);
+#else
+	CVuVector v0, v1, v2, plane;
+	v0.x = tri.v0[0]/128.0f;
+	v0.y = tri.v0[1]/128.0f;
+	v0.z = tri.v0[2]/128.0f;
+	v0.w = tri.v0[3]/128.0f;
+	v1.x = tri.v1[0]/128.0f;
+	v1.y = tri.v1[1]/128.0f;
+	v1.z = tri.v1[2]/128.0f;
+	v1.w = tri.v1[3]/128.0f;
+	v2.x = tri.v2[0]/128.0f;
+	v2.y = tri.v2[1]/128.0f;
+	v2.z = tri.v2[2]/128.0f;
+	v2.w = tri.v2[3]/128.0f;
+	plane.x = tri.plane[0]/4096.0f;
+	plane.y = tri.plane[1]/4096.0f;
+	plane.z = tri.plane[2]/4096.0f;
+	plane.w = tri.plane[3]/128.0f;
+	LineToTriangleCollision(p0, p1, v0, v1, v2, plane);
+#endif
+}
+
+extern "C" void
+SphereToTriangleCollision(const CVuVector &sph,
+	const CVuVector &v0, const CVuVector &v1, const CVuVector &v2,
+	const CVuVector &plane)
+{
+#ifdef GTA_PS2
+	__asm__ volatile (
+		".set noreorder\n"
+		"lqc2	vf12, 0x0(%0)\n"
+		"lqc2	vf14, 0x0(%1)\n"
+		"lqc2	vf15, 0x0(%2)\n"
+		"lqc2	vf16, 0x0(%3)\n"
+		"lqc2	vf17, 0x0(%4)\n"
+		"vcallms	Vu0SphereToTriangleCollisionStart\n"
+		".set reorder\n"
+		:
+		: "r" (&sph), "r" (&v0), "r" (&v1), "r" (&v2), "r" (&plane)
+	);
+#else
+	float planedist = DotProduct(plane, sph) - plane.w;	// VF02
+	if(Abs(planedist) > sph.w){
+		vi01 = 0;
+		return;
+	}
+	// point on plane
+	CVuVector p = sph - planedist*plane;
+	p.w = 0.0f;
+	vf01 = p;
+	planedist = Abs(planedist);
+	// edges
+	CVuVector v01 = v1 - v0;
+	CVuVector v12 = v2 - v1;
+	CVuVector v20 = v0 - v2;
+	// VU code calculates normal again for some weird reason...
+	// Check sides of point
+	CVector cross1 = CrossProduct(p-v0, v01);
+	CVector cross2 = CrossProduct(p-v1, v12);
+	CVector cross3 = CrossProduct(p-v2, v20);
+	// Only check relevant directions
+	int flagmask = 0;
+	if(Abs(plane.x) > 0.1f) flagmask |= 1;
+	if(Abs(plane.y) > 0.1f) flagmask |= 2;
+	if(Abs(plane.z) > 0.1f) flagmask |= 4;
+	int nflags = SignFlags(plane) & flagmask;
+	int flags1 = SignFlags(cross1) & flagmask;
+	int flags2 = SignFlags(cross2) & flagmask;
+	int flags3 = SignFlags(cross3) & flagmask;
+	int testcase = 0;
+	CVuVector closest(0.0f, 0.0f, 0.0f);	// VF04
+	if(flags1 == nflags){
+		closest += v2;
+		testcase++;
+	}
+	if(flags2 == nflags){
+		closest += v0;
+		testcase++;
+	}
+	if(flags3 == nflags){
+		closest += v1;
+		testcase++;
+	}
+	if(testcase == 3){
+		// inside triangle - dist to plane already checked
+		vf02 = plane;
+		vf02.w = vf03.x = planedist;
+		vi01 = 1;
+	}else if(testcase == 1){
+		// outside two sides - closest to point opposide inside edge
+		vf01 = closest;
+		vf02 = sph - closest;
+		float distSq = vf02.MagnitudeSqr();
+		vi01 = sph.w*sph.w > distSq;
+		vf03.x = Sqrt(distSq);
+		vf02 *= 1.0f/vf03.x;
+	}else{
+		// inside two sides - closest to third edge
+		if(flags1 != nflags)
+			closest = DistanceBetweenSphereAndLine(sph, v0, v01);
+		else if(flags2 != nflags)
+			closest = DistanceBetweenSphereAndLine(sph, v1, v12);
+		else
+			closest = DistanceBetweenSphereAndLine(sph, v2, v20);
+		vi01 = sph.w*sph.w > closest.w;
+		vf01 = closest;
+		vf02 = sph - closest;
+		vf03.x = Sqrt(closest.w);
+		vf02 *= 1.0f/vf03.x;
+	}
+#endif
+}
+
+extern "C" void
+SphereToTriangleCollisionCompressed(const CVuVector &sph, VuTriangle &tri)
+{
+#ifdef GTA_PS2
+	__asm__ volatile (
+		".set noreorder\n"
+		"lqc2	vf12, 0x0(%0)\n"
+		"lqc2	vf14, 0x0(%1)\n"
+		"lqc2	vf15, 0x10(%1)\n"
+		"lqc2	vf16, 0x20(%1)\n"
+		"lqc2	vf17, 0x30(%1)\n"
+		"vcallms	Vu0SphereToTriangleCollisionCompressedStart\n"
+		".set reorder\n"
+		:
+		: "r" (&sph), "r" (&tri)
+	);
+#else
+	CVuVector v0, v1, v2, plane;
+	v0.x = tri.v0[0]/128.0f;
+	v0.y = tri.v0[1]/128.0f;
+	v0.z = tri.v0[2]/128.0f;
+	v0.w = tri.v0[3]/128.0f;
+	v1.x = tri.v1[0]/128.0f;
+	v1.y = tri.v1[1]/128.0f;
+	v1.z = tri.v1[2]/128.0f;
+	v1.w = tri.v1[3]/128.0f;
+	v2.x = tri.v2[0]/128.0f;
+	v2.y = tri.v2[1]/128.0f;
+	v2.z = tri.v2[2]/128.0f;
+	v2.w = tri.v2[3]/128.0f;
+	plane.x = tri.plane[0]/4096.0f;
+	plane.y = tri.plane[1]/4096.0f;
+	plane.z = tri.plane[2]/4096.0f;
+	plane.w = tri.plane[3]/128.0f;
+	SphereToTriangleCollision(sph, v0, v1, v2, plane);
+#endif
+}
+#endif
\ No newline at end of file
diff --git a/src/collision/VuCollision.h b/src/collision/VuCollision.h
new file mode 100644
index 00000000..29ca4cbf
--- /dev/null
+++ b/src/collision/VuCollision.h
@@ -0,0 +1,32 @@
+#pragma once
+
+
+struct VuTriangle
+{
+	// Compressed int16 but unpacked
+#ifdef GTA_PS2
+	uint128 v0;
+	uint128 v1;
+	uint128 v2;
+	uint128 plane;
+#else
+	int32 v0[4];
+	int32 v1[4];
+	int32 v2[4];
+	int32 plane[4];
+#endif
+};
+
+#ifndef GTA_PS2
+extern int16 vi01;
+extern CVuVector vf01;
+extern CVuVector vf02;
+extern CVuVector vf03;
+#endif
+
+extern "C" {
+void LineToTriangleCollision(const CVuVector &p0, const CVuVector &p1, const CVuVector &v0, const CVuVector &v1, const CVuVector &v2, const CVuVector &plane);
+void LineToTriangleCollisionCompressed(const CVuVector &p0, const CVuVector &p1, VuTriangle &tri);
+void SphereToTriangleCollision(const CVuVector &sph, const CVuVector &v0, const CVuVector &v1, const CVuVector &v2, const CVuVector &plane);
+void SphereToTriangleCollisionCompressed(const CVuVector &sph, VuTriangle &tri);
+}
diff --git a/src/core/vu0Collision.dsm b/src/collision/vu0Collision.dsm
index 657c8b81..657c8b81 100644
--- a/src/core/vu0Collision.dsm
+++ b/src/collision/vu0Collision.dsm
diff --git a/src/core/vu0Collision_1.s b/src/collision/vu0Collision_1.s
index 055c8640..055c8640 100644
--- a/src/core/vu0Collision_1.s
+++ b/src/collision/vu0Collision_1.s
diff --git a/src/core/vu0Collision_2.s b/src/collision/vu0Collision_2.s
index 716c29ac..716c29ac 100644
--- a/src/core/vu0Collision_2.s
+++ b/src/collision/vu0Collision_2.s