1 files changed, 1100 insertions, 18 deletions

diff --git a/src/core/Collision.cpp b/src/core/Collision.cpp
index 99be816f..6522ff1c 100644
--- a/src/core/Collision.cpp
+++ b/src/core/Collision.cpp
@@ -1,5 +1,6 @@
 #include "common.h"
 
+#include "VuVector.h"
 #include "main.h"
 #include "Lists.h"
 #include "Game.h"
@@ -21,6 +22,346 @@
 #include "Lines.h"
 #include "Collision.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
+}
+
+inline int
+GetVUresult(void)
+{
+#ifdef GTA_PS2
+	int ret;
+	__asm__ volatile (
+		"cfc2.i	%0,vi01\n"	// .i important! wait for VU0 to finish
+		: "=r" (ret)
+	);
+	return ret;
+#else
+	return vi01;
+#endif
+}
+
+inline int
+GetVUresult(CVuVector &point, CVuVector &normal, float &dist)
+{
+#ifdef GTA_PS2
+	int ret;
+	__asm__ volatile (
+		"cfc2.i	%0,vi01\n"	// .i important! wait for VU0 to finish
+		"sqc2	vf01,(%1)\n"
+		"sqc2	vf02,(%2)\n"
+		"qmfc2	$12,vf03\n"
+		"sw	$12,(%3)\n"
+		: "=r" (ret)
+		: "r" (&point), "r" (&normal), "r" (&dist)
+		: "$12"
+	);
+	return ret;
+#else
+	point = vf01;
+	normal = vf02;
+	dist = vf03.x;
+	return vi01;
+#endif
+}
+
+#endif
+
+
 enum Direction
 {
 	DIR_X_POS,
@@ -397,6 +738,20 @@ CCollision::TestVerticalLineBox(const CColLine &line, const CColBox &box)
 bool
 CCollision::TestLineTriangle(const CColLine &line, const CompressedVector *verts, const CColTriangle &tri, const CColTrianglePlane &plane)
 {
+#ifdef VU_COLLISION
+	// not used in favour of optimized loops
+	VuTriangle vutri;
+	verts[tri.a].Unpack(vutri.v0);
+	verts[tri.b].Unpack(vutri.v1);
+	verts[tri.c].Unpack(vutri.v2);
+	plane.Unpack(vutri.plane);
+
+	LineToTriangleCollisionCompressed(*(CVuVector*)&line.p0, *(CVuVector*)&line.p1, vutri);
+
+	if(GetVUresult())
+		return true;
+	return false;
+#else
 	float t;
 	CVector normal;
 	plane.GetNormal(normal);
@@ -470,6 +825,7 @@ CCollision::TestLineTriangle(const CColLine &line, const CompressedVector *verts
 	if(CrossProduct2D(vec3-vec1, vect-vec1) > 0.0f) return false;
 	if(CrossProduct2D(vec3-vec2, vect-vec2) < 0.0f) return false;
 	return true;
+#endif
 }
 
 // Test if line segment intersects with sphere.
@@ -507,6 +863,20 @@ bool
 CCollision::TestSphereTriangle(const CColSphere &sphere,
 	const CompressedVector *verts, const CColTriangle &tri, const CColTrianglePlane &plane)
 {
+#ifdef VU_COLLISION
+	// not used in favour of optimized loops
+	VuTriangle vutri;
+	verts[tri.a].Unpack(vutri.v0);
+	verts[tri.b].Unpack(vutri.v1);
+	verts[tri.c].Unpack(vutri.v2);
+	plane.Unpack(vutri.plane);
+
+	SphereToTriangleCollisionCompressed(*(CVuVector*)&sphere, vutri);
+
+	if(GetVUresult())
+		return true;
+	return false;
+#else
 	// If sphere and plane don't intersect, no collision
 	float planedist = plane.CalcPoint(sphere.center);
 	if(Abs(planedist) > sphere.radius)
@@ -520,7 +890,9 @@ CCollision::TestSphereTriangle(const CColSphere &sphere,
 	CVector vec2 = vb - va;
 	float len = vec2.Magnitude();
 	vec2 = vec2 * (1.0f/len);
-	CVector vec1 = CrossProduct(vec2, plane.normal);
+	CVector normal;
+	plane.GetNormal(normal);
+	CVector vec1 = CrossProduct(vec2, normal);
 
 	// We know A has local coordinate [0,0] and B has [0,len].
 	// Now calculate coordinates on triangle for these two vectors:
@@ -563,11 +935,78 @@ CCollision::TestSphereTriangle(const CColSphere &sphere,
 	}
 
 	return dist < sphere.radius;
+#endif
 }
 
 bool
 CCollision::TestLineOfSight(const CColLine &line, const CMatrix &matrix, CColModel &model, bool ignoreSeeThrough)
 {
+#ifdef VU_COLLISION
+	CMatrix matTransform;
+	int i;
+
+	// transform line to model space
+	Invert(matrix, matTransform);
+	CVuVector newline[2];
+	TransformPoints(newline, 2, matTransform, (RwV3d*)&line.p0, sizeof(CColLine)/2);
+
+	// If we don't intersect with the bounding box, no chance on the rest
+	if(!TestLineBox(*(CColLine*)newline, model.boundingBox))
+		return false;
+
+	for(i = 0; i < model.numSpheres; i++){
+		if(ignoreSeeThrough && IsSeeThrough(model.spheres[i].surface)) continue;
+		if(TestLineSphere(*(CColLine*)newline, model.spheres[i]))
+			return true;
+	}
+
+	for(i = 0; i < model.numBoxes; i++){
+		if(ignoreSeeThrough && IsSeeThrough(model.boxes[i].surface)) continue;
+		if(TestLineBox(*(CColLine*)newline, model.boxes[i]))
+			return true;
+	}
+
+	CalculateTrianglePlanes(&model);
+	int lastTest = -1;
+	VuTriangle vutri;
+	for(i = 0; i < model.numTriangles; i++){
+		if(ignoreSeeThrough && IsSeeThrough(model.triangles[i].surface)) continue;
+
+		CColTriangle *tri = &model.triangles[i];
+		model.vertices[tri->a].Unpack(vutri.v0);
+		model.vertices[tri->b].Unpack(vutri.v1);
+		model.vertices[tri->c].Unpack(vutri.v2);
+		model.trianglePlanes[i].Unpack(vutri.plane);
+
+		LineToTriangleCollisionCompressed(newline[0], newline[1], vutri);
+		lastTest = i;
+		break;
+	}
+#ifdef FIX_BUGS
+	// no need to check first again
+	i++;
+#endif
+	for(; i < model.numTriangles; i++){
+		if(ignoreSeeThrough && IsSeeThrough(model.triangles[i].surface)) continue;
+
+		CColTriangle *tri = &model.triangles[i];
+		model.vertices[tri->a].Unpack(vutri.v0);
+		model.vertices[tri->b].Unpack(vutri.v1);
+		model.vertices[tri->c].Unpack(vutri.v2);
+		model.trianglePlanes[i].Unpack(vutri.plane);
+
+		if(GetVUresult())
+			return true;
+
+		LineToTriangleCollisionCompressed(newline[0], newline[1], vutri);
+		lastTest = i;
+
+	}
+	if(lastTest != -1 && GetVUresult())
+		return true;
+
+	return false;
+#else
 	static CMatrix matTransform;
 	int i;
 
@@ -599,6 +1038,7 @@ CCollision::TestLineOfSight(const CColLine &line, const CMatrix &matrix, CColMod
 	}
 
 	return false;
+#endif
 }
 
 
@@ -614,20 +1054,24 @@ CCollision::ProcessSphereSphere(const CColSphere &s1, const CColSphere &s2, CCol
 {
 	CVector dist = s1.center - s2.center;
 	float d = dist.Magnitude() - s2.radius;	// distance from s1's center to s2
-	float dc = d < 0.0f ? 0.0f : d;		// clamp to zero, i.e. if s1's center is inside s2
+	float depth = s1.radius - d;	// sphere overlap
+	if(d < 0.0f) d = 0.0f;		// clamp to zero, i.e. if s1's center is inside s2
 	// no collision if sphere is not close enough
-	if(mindistsq <= dc*dc || s1.radius <= dc)
-		return false;
-	dist.Normalise();
-	point.point = s1.center - dist*dc;
-	point.normal = dist;
-	point.surfaceA = s1.surface;
-	point.pieceA = s1.piece;
-	point.surfaceB = s2.surface;
-	point.pieceB = s2.piece;
-	point.depth = s1.radius - d;	// sphere overlap
-	mindistsq = dc*dc;		// collision radius
-	return true;
+	if(d*d < mindistsq && d < s1.radius){
+		dist.Normalise();
+		point.point = s1.center - dist*d;
+		point.normal = dist;
+#ifndef VU_COLLISION
+		point.surfaceA = s1.surface;
+		point.pieceA = s1.piece;
+		point.surfaceB = s2.surface;
+		point.pieceB = s2.piece;
+#endif
+		point.depth = depth;	
+		mindistsq = d*d;		// collision radius
+		return true;
+	}
+	return false;
 }
 
 bool
@@ -667,10 +1111,12 @@ CCollision::ProcessSphereBox(const CColSphere &sph, const CColBox &box, CColPoin
 		if(lensq < mindistsq){
 			point.normal = dist * (1.0f/Sqrt(lensq));
 			point.point = sph.center - point.normal;
+#ifndef VU_COLLISION
 			point.surfaceA = sph.surface;
 			point.pieceA = sph.piece;
 			point.surfaceB = box.surface;
 			point.pieceB = box.piece;
+#endif
 
 			// find absolute distance to the closer side in each dimension
 			float dx = dist.x > 0.0f ?
@@ -710,10 +1156,12 @@ CCollision::ProcessSphereBox(const CColSphere &sph, const CColBox &box, CColPoin
 			float len = Sqrt(lensq);
 			point.point = p;
 			point.normal = dist * (1.0f/len);
+#ifndef VU_COLLISION
 			point.surfaceA = sph.surface;
 			point.pieceA = sph.piece;
 			point.surfaceB = box.surface;
 			point.pieceB = box.piece;
+#endif
 			point.depth = sph.radius - len;
 			mindistsq = lensq;
 			return true;
@@ -828,10 +1276,12 @@ CCollision::ProcessLineBox(const CColLine &line, const CColBox &box, CColPoint &
 
 	point.point = p;
 	point.normal = normal;
+#ifndef VU_COLLISION
 	point.surfaceA = 0;
 	point.pieceA = 0;
 	point.surfaceB = box.surface;
 	point.pieceB = box.piece;
+#endif
 	mindist = mint;
 
 	return true;
@@ -861,10 +1311,12 @@ CCollision::ProcessLineSphere(const CColLine &line, const CColSphere &sphere, CC
 	point.point = line.p0 + v01*t;
 	point.normal = point.point - sphere.center;
 	point.normal.Normalise();
+#ifndef VU_COLLISION
 	point.surfaceA = 0;
 	point.pieceA = 0;
 	point.surfaceB = sphere.surface;
 	point.pieceB = sphere.piece;
+#endif
 	mindist = t;
 	return true;
 }
@@ -874,6 +1326,17 @@ CCollision::ProcessVerticalLineTriangle(const CColLine &line,
 	const CompressedVector *verts, const CColTriangle &tri, const CColTrianglePlane &plane,
 	CColPoint &point, float &mindist, CStoredCollPoly *poly)
 {
+#ifdef VU_COLLISION
+	// not used in favour of optimized loops
+	bool res = ProcessLineTriangle(line, verts, tri, plane, point, mindist);
+	if(res && poly){
+		poly->verts[0] = verts[tri.a].Get();
+		poly->verts[1] = verts[tri.b].Get();
+		poly->verts[2] = verts[tri.c].Get();
+		poly->valid = true;
+	}
+	return res;
+#else
 	float t;
 	CVector normal;
 
@@ -960,11 +1423,40 @@ CCollision::ProcessVerticalLineTriangle(const CColLine &line,
 	}
 	mindist = t;
 	return true;
+#endif
 }
 
 bool
 CCollision::IsStoredPolyStillValidVerticalLine(const CVector &pos, float z, CColPoint &point, CStoredCollPoly *poly)
 {
+#ifdef VU_COLLISION
+	if(!poly->valid)
+		return false;
+
+	CVuVector p0 = pos;
+	CVuVector p1 = pos;
+	p1.z = z;
+
+	CVector v01 = poly->verts[1] - poly->verts[0];
+	CVector v02 = poly->verts[2] - poly->verts[0];
+	CVuVector plane = CrossProduct(v02, v01);
+	plane.Normalise();
+	plane.w = DotProduct(plane, poly->verts[0]);
+
+	LineToTriangleCollision(p0, p1, poly->verts[0], poly->verts[1], poly->verts[2], plane);
+
+	CVuVector pnt;
+	float dist;
+	if(!GetVUresult(pnt, plane, dist))
+#ifdef FIX_BUGS
+		// perhaps not needed but be safe
+		return poly->valid = false;
+#else
+		return false;
+#endif
+	point.point = pnt;
+	return true;
+#else
 	float t;
 
 	if(!poly->valid)
@@ -987,7 +1479,9 @@ CCollision::IsStoredPolyStillValidVerticalLine(const CVector &pos, float z, CCol
 		return poly->valid = false;
 
 	// intersection parameter on line
-	t = -plane.CalcPoint(p0) / DotProduct(p1 - p0, plane.normal);
+	CVector normal;
+	plane.GetNormal(normal);
+	t = -plane.CalcPoint(p0) / DotProduct(p1 - p0, normal);
 	// find point of intersection
 	CVector p = p0 + (p1-p0)*t;
 
@@ -1037,13 +1531,36 @@ CCollision::IsStoredPolyStillValidVerticalLine(const CVector &pos, float z, CCol
 	if(CrossProduct2D(vec3-vec2, vect-vec2) < 0.0f) return poly->valid = false;
 	point.point = p;
 	return poly->valid = true;
+#endif
 }
 
 bool
-CCollision::ProcessLineTriangle(const CColLine &line ,
+CCollision::ProcessLineTriangle(const CColLine &line,
 	const CompressedVector *verts, const CColTriangle &tri, const CColTrianglePlane &plane,
 	CColPoint &point, float &mindist)
 {
+#ifdef VU_COLLISION
+	// not used in favour of optimized loops
+	VuTriangle vutri;
+	verts[tri.a].Unpack(vutri.v0);
+	verts[tri.b].Unpack(vutri.v1);
+	verts[tri.c].Unpack(vutri.v2);
+	plane.Unpack(vutri.plane);
+
+	LineToTriangleCollisionCompressed(*(CVuVector*)&line.p0, *(CVuVector*)&line.p1, vutri);
+
+	CVuVector pnt, normal;
+	float dist;
+	if(GetVUresult(pnt, normal, dist)){
+		if(dist < mindist){
+			point.point = pnt;
+			point.normal = normal;
+			mindist = dist;
+			return true;
+		}
+	}
+	return false;
+#else
 	float t;
 	CVector normal;
 	plane.GetNormal(normal);
@@ -1117,6 +1634,7 @@ CCollision::ProcessLineTriangle(const CColLine &line ,
 	point.pieceB = 0;
 	mindist = t;
 	return true;
+#endif
 }
 
 bool
@@ -1124,6 +1642,30 @@ CCollision::ProcessSphereTriangle(const CColSphere &sphere,
 	const CompressedVector *verts, const CColTriangle &tri, const CColTrianglePlane &plane,
 	CColPoint &point, float &mindistsq)
 {
+#ifdef VU_COLLISION
+	// not used in favour of optimized loops
+	VuTriangle vutri;
+	verts[tri.a].Unpack(vutri.v0);
+	verts[tri.b].Unpack(vutri.v1);
+	verts[tri.c].Unpack(vutri.v2);
+	plane.Unpack(vutri.plane);
+
+	SphereToTriangleCollisionCompressed(*(CVuVector*)&sphere, vutri);
+
+	CVuVector pnt, normal;
+	float dist;
+	if(GetVUresult(pnt, normal, dist) && dist*dist < mindistsq){
+		float depth = sphere.radius - dist;
+		if(depth > point.depth){
+			point.point = pnt;
+			point.normal = normal;
+			point.depth = depth;
+			mindistsq = dist*dist;
+			return true;
+		}
+	}
+	return false;
+#else
 	// If sphere and plane don't intersect, no collision
 	float planedist = plane.CalcPoint(sphere.center);
 	float distsq = planedist*planedist;
@@ -1191,13 +1733,16 @@ CCollision::ProcessSphereTriangle(const CColSphere &sphere,
 	point.point = p;
 	point.normal = sphere.center - p;
 	point.normal.Normalise();
+#ifndef VU_COLLISION
 	point.surfaceA = sphere.surface;
 	point.pieceA = sphere.piece;
 	point.surfaceB = tri.surface;
 	point.pieceB = 0;
+#endif
 	point.depth = sphere.radius - dist;
 	mindistsq = dist*dist;
 	return true;
+#endif
 }
 
 bool
@@ -1205,6 +1750,94 @@ CCollision::ProcessLineOfSight(const CColLine &line,
 	const CMatrix &matrix, CColModel &model,
 	CColPoint &point, float &mindist, bool ignoreSeeThrough)
 {
+#ifdef VU_COLLISION
+	CMatrix matTransform;
+	int i;
+
+	// transform line to model space
+	Invert(matrix, matTransform);
+	CVuVector newline[2];
+	TransformPoints(newline, 2, matTransform, (RwV3d*)&line.p0, sizeof(CColLine)/2);
+
+	if(mindist < 1.0f)
+		newline[1] = newline[0] + (newline[1] - newline[0])*mindist;
+
+	// If we don't intersect with the bounding box, no chance on the rest
+	if(!TestLineBox(*(CColLine*)newline, model.boundingBox))
+		return false;
+
+	float coldist = 1.0f;
+	for(i = 0; i < model.numSpheres; i++){
+		if(ignoreSeeThrough && IsSeeThrough(model.spheres[i].surface)) continue;
+		if(ProcessLineSphere(*(CColLine*)newline, model.spheres[i], point, coldist))
+			point.Set(0, 0, model.spheres[i].surface, model.spheres[i].piece);
+	}
+
+	for(i = 0; i < model.numBoxes; i++){
+		if(ignoreSeeThrough && IsSeeThrough(model.boxes[i].surface)) continue;
+		if(ProcessLineBox(*(CColLine*)newline, model.boxes[i], point, coldist))
+			point.Set(0, 0, model.boxes[i].surface, model.boxes[i].piece);
+	}
+
+	CalculateTrianglePlanes(&model);
+	VuTriangle vutri;
+	CColTriangle *lasttri = nil;
+	for(i = 0; i < model.numTriangles; i++){
+		if(ignoreSeeThrough && IsSeeThrough(model.triangles[i].surface)) continue;
+
+		CColTriangle *tri = &model.triangles[i];
+		model.vertices[tri->a].Unpack(vutri.v0);
+		model.vertices[tri->b].Unpack(vutri.v1);
+		model.vertices[tri->c].Unpack(vutri.v2);
+		model.trianglePlanes[i].Unpack(vutri.plane);
+
+		LineToTriangleCollisionCompressed(newline[0], newline[1], vutri);
+		lasttri = tri;
+		break;
+	}
+#ifdef FIX_BUGS
+	// no need to check first again
+	i++;
+#endif
+	CVuVector pnt, normal;
+	float dist;
+	for(; i < model.numTriangles; i++){
+		if(ignoreSeeThrough && IsSeeThrough(model.triangles[i].surface)) continue;
+
+		CColTriangle *tri = &model.triangles[i];
+		model.vertices[tri->a].Unpack(vutri.v0);
+		model.vertices[tri->b].Unpack(vutri.v1);
+		model.vertices[tri->c].Unpack(vutri.v2);
+		model.trianglePlanes[i].Unpack(vutri.plane);
+
+		if(GetVUresult(pnt, normal, dist))
+			if(dist < coldist){
+				point.point = pnt;
+				point.normal = normal;
+				point.Set(0, 0, lasttri->surface, 0);
+				coldist = dist;
+			}
+
+		LineToTriangleCollisionCompressed(newline[0], newline[1], vutri);
+		lasttri = tri;
+	}
+	if(lasttri && GetVUresult(pnt, normal, dist))
+		if(dist < coldist){
+			point.point = pnt;
+			point.normal = normal;
+			point.Set(0, 0, lasttri->surface, 0);
+			coldist = dist;
+		}
+
+
+	if(coldist < 1.0f){
+		point.point = matrix * point.point;
+		point.normal = Multiply3x3(matrix, point.normal);
+		mindist *= coldist;
+		return true;
+	}
+	return false;
+#else
 	static CMatrix matTransform;
 	int i;
 
@@ -1240,6 +1873,7 @@ CCollision::ProcessLineOfSight(const CColLine &line,
 		return true;
 	}
 	return false;
+#endif
 }
 
 bool
@@ -1247,6 +1881,125 @@ CCollision::ProcessVerticalLine(const CColLine &line,
 	const CMatrix &matrix, CColModel &model,
 	CColPoint &point, float &mindist, bool ignoreSeeThrough, CStoredCollPoly *poly)
 {
+#ifdef VU_COLLISION
+	static CStoredCollPoly TempStoredPoly;
+	CMatrix matTransform;
+	int i;
+
+	// transform line to model space
+	Invert(matrix, matTransform);
+	CVuVector newline[2];
+	TransformPoints(newline, 2, matTransform, (RwV3d*)&line.p0, sizeof(CColLine)/2);
+
+	if(mindist < 1.0f)
+		newline[1] = newline[0] + (newline[1] - newline[0])*mindist;
+
+	if(!TestLineBox(*(CColLine*)newline, model.boundingBox))
+		return false;
+
+	float coldist = 1.0f;
+	for(i = 0; i < model.numSpheres; i++){
+		if(ignoreSeeThrough && IsSeeThrough(model.spheres[i].surface)) continue;
+		if(ProcessLineSphere(*(CColLine*)newline, model.spheres[i], point, coldist))
+			point.Set(0, 0, model.spheres[i].surface, model.spheres[i].piece);
+	}
+
+	for(i = 0; i < model.numBoxes; i++){
+		if(ignoreSeeThrough && IsSeeThrough(model.boxes[i].surface)) continue;
+		if(ProcessLineBox(*(CColLine*)newline, model.boxes[i], point, coldist))
+			point.Set(0, 0, model.boxes[i].surface, model.boxes[i].piece);
+	}
+
+	CalculateTrianglePlanes(&model);
+	TempStoredPoly.valid = false;
+	if(model.numTriangles){
+		bool registeredCol;
+		CColTriangle *lasttri = nil;
+		VuTriangle vutri;
+		for(i = 0; i < model.numTriangles; i++){
+			if(ignoreSeeThrough && IsSeeThrough(model.triangles[i].surface)) continue;
+
+			CColTriangle *tri = &model.triangles[i];
+			model.vertices[tri->a].Unpack(vutri.v0);
+			model.vertices[tri->b].Unpack(vutri.v1);
+			model.vertices[tri->c].Unpack(vutri.v2);
+			model.trianglePlanes[i].Unpack(vutri.plane);
+
+			LineToTriangleCollisionCompressed(newline[0], newline[1], vutri);
+			lasttri = tri;
+			break;
+		}
+#ifdef FIX_BUGS
+		// no need to check first again
+		i++;
+#endif
+		CVuVector pnt, normal;
+		float dist;
+		for(; i < model.numTriangles; i++){
+			if(ignoreSeeThrough && IsSeeThrough(model.triangles[i].surface)) continue;
+
+			CColTriangle *tri = &model.triangles[i];
+			model.vertices[tri->a].Unpack(vutri.v0);
+			model.vertices[tri->b].Unpack(vutri.v1);
+			model.vertices[tri->c].Unpack(vutri.v2);
+			model.trianglePlanes[i].Unpack(vutri.plane);
+
+			if(GetVUresult(pnt, normal, dist)){
+				if(dist < coldist){
+					point.point = pnt;
+					point.normal = normal;
+					point.Set(0, 0, lasttri->surface, 0);
+					coldist = dist;
+					registeredCol = true;
+				}else
+					registeredCol = false;
+			}else
+				registeredCol = false;
+
+			if(registeredCol){
+				TempStoredPoly.verts[0] = model.vertices[lasttri->a].Get();
+				TempStoredPoly.verts[1] = model.vertices[lasttri->b].Get();
+				TempStoredPoly.verts[2] = model.vertices[lasttri->c].Get();
+				TempStoredPoly.valid = true;
+			}
+
+			LineToTriangleCollisionCompressed(newline[0], newline[1], vutri);
+			lasttri = tri;
+		}
+		if(lasttri && GetVUresult(pnt, normal, dist)){
+			if(dist < coldist){
+				point.point = pnt;
+				point.normal = normal;
+				point.Set(0, 0, lasttri->surface, 0);
+				coldist = dist;
+				registeredCol = true;
+			}else
+				registeredCol = false;
+		}else
+			registeredCol = false;
+
+		if(registeredCol){
+			TempStoredPoly.verts[0] = model.vertices[lasttri->a].Get();
+			TempStoredPoly.verts[1] = model.vertices[lasttri->b].Get();
+			TempStoredPoly.verts[2] = model.vertices[lasttri->c].Get();
+			TempStoredPoly.valid = true;
+		}
+	}
+
+	if(coldist < 1.0f){
+		point.point = matrix * point.point;
+		point.normal = Multiply3x3(matrix, point.normal);
+		if(TempStoredPoly.valid && poly){
+			*poly = TempStoredPoly;
+			poly->verts[0] = matrix * CVector(poly->verts[0]);
+			poly->verts[1] = matrix * CVector(poly->verts[1]);
+			poly->verts[2] = matrix * CVector(poly->verts[2]);
+		}
+		mindist *= coldist;
+		return true;
+	}
+	return false;
+#else
 	static CStoredCollPoly TempStoredPoly;
 	int i;
 
@@ -1290,6 +2043,7 @@ CCollision::ProcessVerticalLine(const CColLine &line,
 		return true;
 	}
 	return false;
+#endif
 }
 
 enum {
@@ -1299,6 +2053,15 @@ enum {
 	MAXNUMTRIS = 600
 };
 
+#ifdef VU_COLLISION
+#ifdef GTA_PS2
+#define SPR(off) ((uint8*)(0x70000000 + (off)))
+#else
+static uint8 fakeSPR[16*1024];
+#define SPR(off) ((uint8*)(fakeSPR + (off)))
+#endif
+#endif
+
 // This checks model A's spheres and lines against model B's spheres, boxes and triangles.
 // Returns the number of A's spheres that collide.
 // Returned ColPoints are in world space.
@@ -1308,6 +2071,308 @@ CCollision::ProcessColModels(const CMatrix &matrixA, CColModel &modelA,
 	const CMatrix &matrixB, CColModel &modelB,
 	CColPoint *spherepoints, CColPoint *linepoints, float *linedists)
 {
+#ifdef VU_COLLISION
+	CVuVector *aSpheresA = (CVuVector*)SPR(0x0000);
+	CVuVector *aSpheresB = (CVuVector*)SPR(0x0800);
+	CVuVector *aLinesA = (CVuVector*)SPR(0x1000);
+	int32 *aSphereIndicesA = (int32*)SPR(0x1200);
+	int32 *aSphereIndicesB = (int32*)SPR(0x1400);
+	int32 *aBoxIndicesB = (int32*)SPR(0x1600);
+	int32 *aTriangleIndicesB = (int32*)SPR(0x1680);
+	bool *aCollided = (bool*)SPR(0x1FE0);
+	CMatrix &matAB = *(CMatrix*)SPR(0x1FF0);
+	CMatrix &matBA = *(CMatrix*)SPR(0x2040);
+	int i, j, k;
+
+	// From model A space to model B space
+	Invert(matrixB, matAB);
+	matAB *= matrixA;
+
+	CVuVector bsphereAB;	// bounding sphere of A in B space
+	TransformPoint(bsphereAB, matAB, *(RwV3d*)modelA.boundingSphere.center);	// inlined
+	bsphereAB.w = modelA.boundingSphere.radius;
+	if(!TestSphereBox(*(CColSphere*)&bsphereAB, modelB.boundingBox))
+		return 0;
+
+	// transform modelA's spheres and lines to B space
+	TransformPoints(aSpheresA, modelA.numSpheres, matAB, (RwV3d*)&modelA.spheres->center, sizeof(CColSphere));
+	for(i = 0; i < modelA.numSpheres; i++)
+		aSpheresA[i].w = modelA.spheres[i].radius;
+	TransformPoints(aLinesA, modelA.numLines*2, matAB, (RwV3d*)&modelA.lines->p0, sizeof(CColLine)/2);
+
+	// Test them against model B's bounding volumes
+	int numSpheresA = 0;
+	for(i = 0; i < modelA.numSpheres; i++)
+		if(TestSphereBox(*(CColSphere*)&aSpheresA[i], modelB.boundingBox))
+			aSphereIndicesA[numSpheresA++] = i;
+	// No collision
+	if(numSpheresA == 0 && modelA.numLines == 0)
+		return 0;
+
+
+	// B to A space
+	Invert(matrixA, matBA);
+	matBA *= matrixB;
+
+	// transform modelB's spheres to A space
+	TransformPoints(aSpheresB, modelB.numSpheres, matBA, (RwV3d*)&modelB.spheres->center, sizeof(CColSphere));
+	for(i = 0; i < modelB.numSpheres; i++)
+		aSpheresB[i].w = modelB.spheres[i].radius;
+
+	// Check model B against A's bounding volumes
+	int numSpheresB = 0;
+	int numBoxesB = 0;
+	int numTrianglesB = 0;
+	for(i = 0; i < modelB.numSpheres; i++)
+		if(TestSphereBox(*(CColSphere*)&aSpheresB[i], modelA.boundingBox))
+			aSphereIndicesB[numSpheresB++] = i;
+	for(i = 0; i < modelB.numBoxes; i++)
+		if(TestSphereBox(*(CColSphere*)&bsphereAB, modelB.boxes[i]))
+			aBoxIndicesB[numBoxesB++] = i;
+	CalculateTrianglePlanes(&modelB);
+	if(modelB.numTriangles){
+		VuTriangle vutri;
+		// process the first triangle
+		CColTriangle *tri = &modelB.triangles[0];
+		modelB.vertices[tri->a].Unpack(vutri.v0);
+		modelB.vertices[tri->b].Unpack(vutri.v1);
+		modelB.vertices[tri->c].Unpack(vutri.v2);
+		modelB.trianglePlanes[0].Unpack(vutri.plane);
+
+		SphereToTriangleCollisionCompressed(bsphereAB, vutri);
+
+		for(i = 1; i < modelB.numTriangles; i++){
+			// set up the next triangle while VU0 is running
+			tri = &modelB.triangles[i];
+			modelB.vertices[tri->a].Unpack(vutri.v0);
+			modelB.vertices[tri->b].Unpack(vutri.v1);
+			modelB.vertices[tri->c].Unpack(vutri.v2);
+			modelB.trianglePlanes[i].Unpack(vutri.plane);
+
+			// check previous result
+			if(GetVUresult())
+				aTriangleIndicesB[numTrianglesB++] = i-1;
+
+			// kick off this one
+			SphereToTriangleCollisionCompressed(bsphereAB, vutri);
+		}
+
+		// check last result
+		if(GetVUresult())
+			aTriangleIndicesB[numTrianglesB++] = i-1;
+	}
+	// No collision
+	if(numSpheresB == 0 && numBoxesB == 0 && numTrianglesB == 0)
+		return 0;
+
+	// We now have the collision volumes in A and B that are worth processing.
+
+	// Process A's spheres against B's collision volumes
+	int numCollisions = 0;
+	spherepoints[numCollisions].depth = -1.0f;
+	for(i = 0; i < numSpheresA; i++){
+		float coldist = 1.0e24f;
+		bool hasCollided = false;
+		CColSphere *sphA = &modelA.spheres[aSphereIndicesA[i]];
+		CVuVector *vusphA = &aSpheresA[aSphereIndicesA[i]];
+
+		for(j = 0; j < numSpheresB; j++)
+			// This actually looks like something was inlined here
+			if(ProcessSphereSphere(*(CColSphere*)vusphA, modelB.spheres[aSphereIndicesB[j]],
+					spherepoints[numCollisions], coldist)){
+				spherepoints[numCollisions].Set(
+					sphA->surface, sphA->piece,
+					modelB.spheres[aSphereIndicesB[j]].surface, modelB.spheres[aSphereIndicesB[j]].piece);
+				hasCollided = true;
+			}
+		for(j = 0; j < numBoxesB; j++)
+			if(ProcessSphereBox(*(CColSphere*)vusphA, modelB.boxes[aBoxIndicesB[j]],
+					spherepoints[numCollisions], coldist)){
+				spherepoints[numCollisions].Set(
+					sphA->surface, sphA->piece,
+					modelB.boxes[aBoxIndicesB[j]].surface, modelB.boxes[aBoxIndicesB[j]].piece);
+				hasCollided = true;
+			}
+		if(numTrianglesB){
+			CVuVector point, normal;
+			float depth;
+			bool registeredCol;
+			CColTriangle *lasttri;
+
+			VuTriangle vutri;
+			// process the first triangle
+			k = aTriangleIndicesB[0];
+			CColTriangle *tri = &modelB.triangles[k];
+			modelB.vertices[tri->a].Unpack(vutri.v0);
+			modelB.vertices[tri->b].Unpack(vutri.v1);
+			modelB.vertices[tri->c].Unpack(vutri.v2);
+			modelB.trianglePlanes[k].Unpack(vutri.plane);
+
+			SphereToTriangleCollisionCompressed(*vusphA, vutri);
+			lasttri = tri;
+
+			for(j = 1; j < numTrianglesB; j++){
+				k = aTriangleIndicesB[j];
+				// set up the next triangle while VU0 is running
+				tri = &modelB.triangles[k];
+				modelB.vertices[tri->a].Unpack(vutri.v0);
+				modelB.vertices[tri->b].Unpack(vutri.v1);
+				modelB.vertices[tri->c].Unpack(vutri.v2);
+				modelB.trianglePlanes[k].Unpack(vutri.plane);
+
+				// check previous result
+				// TODO: this looks inlined but spherepoints[numCollisions] does not...
+				if(GetVUresult(point, normal, depth)){
+					depth = sphA->radius - depth;
+					if(depth > spherepoints[numCollisions].depth){
+						spherepoints[numCollisions].point = point;
+						spherepoints[numCollisions].normal = normal;
+						spherepoints[numCollisions].Set(depth,
+							sphA->surface, sphA->piece, lasttri->surface, 0);
+						registeredCol = true;
+					}else
+						registeredCol = false;
+				}else
+					registeredCol = false;
+
+				if(registeredCol)
+					hasCollided = true;
+
+				// kick off this one
+				SphereToTriangleCollisionCompressed(*vusphA, vutri);
+				lasttri = tri;
+			}
+
+			// check last result
+			// TODO: this looks inlined but spherepoints[numCollisions] does not...
+			if(GetVUresult(point, normal, depth)){
+				depth = sphA->radius - depth;
+				if(depth > spherepoints[numCollisions].depth){
+					spherepoints[numCollisions].point = point;
+					spherepoints[numCollisions].normal = normal;
+					spherepoints[numCollisions].Set(depth,
+						sphA->surface, sphA->piece, lasttri->surface, 0);
+					registeredCol = true;
+				}else
+					registeredCol = false;
+			}else
+				registeredCol = false;
+
+			if(registeredCol)
+				hasCollided = true;
+		}
+
+		if(hasCollided){
+			numCollisions++;
+			if(numCollisions == MAX_COLLISION_POINTS)
+				break;
+			spherepoints[numCollisions].depth = -1.0f;
+		}
+	}
+	for(i = 0; i < numCollisions; i++){
+		// TODO: both VU0 macros
+		spherepoints[i].point = matrixB * spherepoints[i].point;
+		spherepoints[i].normal = Multiply3x3(matrixB, spherepoints[i].normal);
+	}
+
+	// And the same thing for the lines in A
+	for(i = 0; i < modelA.numLines; i++){
+		aCollided[i] = false;
+		CVuVector *lineA = &aLinesA[i*2];
+
+		for(j = 0; j < numSpheresB; j++)
+			if(ProcessLineSphere(*(CColLine*)lineA, modelB.spheres[aSphereIndicesB[j]],
+					linepoints[i], linedists[i])){
+				linepoints[i].Set(0, 0,
+#ifdef FIX_BUGS
+					modelB.spheres[aSphereIndicesB[j]].surface, modelB.spheres[aSphereIndicesB[j]].piece);
+#else
+					modelB.spheres[j].surface, modelB.spheres[j].piece);
+#endif
+				aCollided[i] = true;
+			}
+		for(j = 0; j < numBoxesB; j++)
+			if(ProcessLineBox(*(CColLine*)lineA, modelB.boxes[aBoxIndicesB[j]],
+					linepoints[i], linedists[i])){
+				linepoints[i].Set(0, 0,
+					modelB.boxes[aBoxIndicesB[j]].surface, modelB.boxes[aBoxIndicesB[j]].piece);
+				aCollided[i] = true;
+			}
+		if(numTrianglesB){
+			CVuVector point, normal;
+			float dist;
+			bool registeredCol;
+			CColTriangle *lasttri;
+
+			VuTriangle vutri;
+			// process the first triangle
+			k = aTriangleIndicesB[0];
+			CColTriangle *tri = &modelB.triangles[k];
+			modelB.vertices[tri->a].Unpack(vutri.v0);
+			modelB.vertices[tri->b].Unpack(vutri.v1);
+			modelB.vertices[tri->c].Unpack(vutri.v2);
+			modelB.trianglePlanes[k].Unpack(vutri.plane);
+
+			LineToTriangleCollisionCompressed(lineA[0], lineA[1], vutri);
+			lasttri = tri;
+
+			for(j = 1; j < numTrianglesB; j++){
+				k = aTriangleIndicesB[j];
+				// set up the next triangle while VU0 is running
+				CColTriangle *tri = &modelB.triangles[k];
+				modelB.vertices[tri->a].Unpack(vutri.v0);
+				modelB.vertices[tri->b].Unpack(vutri.v1);
+				modelB.vertices[tri->c].Unpack(vutri.v2);
+				modelB.trianglePlanes[k].Unpack(vutri.plane);
+
+				// check previous result
+				// TODO: this again somewhat looks inlined
+				if(GetVUresult(point, normal, dist)){
+					if(dist < linedists[i]){
+						linepoints[i].point = point;
+						linepoints[i].normal = normal;
+						linedists[i] = dist;
+						linepoints[i].Set(0, 0, lasttri->surface, 0);
+						registeredCol = true;
+					}else
+						registeredCol = false;
+				}else
+					registeredCol = false;
+
+				if(registeredCol)
+					aCollided[i] = true;
+
+				// kick of this one
+				LineToTriangleCollisionCompressed(lineA[0], lineA[1], vutri);
+				lasttri = tri;
+			}
+
+			// check last result
+			if(GetVUresult(point, normal, dist)){
+				if(dist < linedists[i]){
+					linepoints[i].point = point;
+					linepoints[i].normal = normal;
+					linedists[i] = dist;
+					linepoints[i].Set(0, 0, lasttri->surface, 0);
+					registeredCol = true;
+				}else
+					registeredCol = false;
+			}else
+				registeredCol = false;
+
+			if(registeredCol)
+				aCollided[i] = true;
+		}
+
+		if(aCollided[i]){
+			// TODO: both VU0 macros
+			linepoints[i].point = matrixB * linepoints[i].point;
+			linepoints[i].normal = Multiply3x3(matrixB, linepoints[i].normal);
+		}
+	}
+
+	return numCollisions;	// sphere collisions
+#else
 	static int aSphereIndicesA[MAXNUMSPHERES];
 	static int aLineIndicesA[MAXNUMLINES];
 	static int aSphereIndicesB[MAXNUMSPHERES];
@@ -1324,14 +2389,16 @@ CCollision::ProcessColModels(const CMatrix &matrixA, CColModel &modelA,
 	assert(modelA.numLines <= MAXNUMLINES);
 
 	// From model A space to model B space
-	matAB = Invert(matrixB, matAB) * matrixA;
+	Invert(matrixB, matAB);
+	matAB *= matrixA;
 
 	CColSphere bsphereAB;	// bounding sphere of A in B space
 	bsphereAB.Set(modelA.boundingSphere.radius, matAB * modelA.boundingSphere.center);
 	if(!TestSphereBox(bsphereAB, modelB.boundingBox))
 		return 0;
 	// B to A space
-	matBA = Invert(matrixA, matBA) * matrixB;
+	matBA = Invert(matrixA, matBA);
+	matBA *= matrixB;
 
 	// transform modelA's spheres and lines to B space
 	for(i = 0; i < modelA.numSpheres; i++){
@@ -1444,6 +2511,7 @@ CCollision::ProcessColModels(const CMatrix &matrixA, CColModel &modelA,
 		}
 
 	return numCollisions;	// sphere collisions
+#endif
 }
 
 
@@ -1987,6 +3055,19 @@ CColTriangle::Set(const CompressedVector *, int a, int b, int c, uint8 surf, uin
 	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)
 {
@@ -2002,6 +3083,7 @@ CColTrianglePlane::Set(const CVector &va, const CVector &vb, const CVector &vc)
 	else
 		dir = normal.z < 0.0f ? DIR_Z_NEG : DIR_Z_POS;
 }
+#endif
 
 CColModel::CColModel(void)
 {