33 files changed, 1307 insertions, 1337 deletions

diff --git a/premake5.lua b/premake5.lua
index 7398eef0..a6c2b288 100644
--- a/premake5.lua
+++ b/premake5.lua
@@ -233,6 +233,7 @@ project "reVC"
 	files { addSrcFiles("src/audio") }
 	files { addSrcFiles("src/audio/eax") }
 	files { addSrcFiles("src/audio/oal") }
+	files { addSrcFiles("src/collision") }
 	files { addSrcFiles("src/control") }
 	files { addSrcFiles("src/core") }
 	files { addSrcFiles("src/entities") }
@@ -255,6 +256,7 @@ project "reVC"
 	includedirs { "src/audio" }
 	includedirs { "src/audio/eax" }
 	includedirs { "src/audio/oal" }
+	includedirs { "src/collision" }
 	includedirs { "src/control" }
 	includedirs { "src/core" }
 	includedirs { "src/entities" }
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..0df55925
--- /dev/null
+++ b/src/collision/ColBox.h
@@ -0,0 +1,22 @@
+#pragma once
+
+#include "SurfaceTable.h"
+
+struct CBox
+{
+	CVector min;
+	CVector max;
+	CVector GetSize(void) { return max - min; }
+	void Set(const CVector &min, const CVector &max) {  this->min = min; this->max = max; }
+};
+
+struct CColBox : public CBox
+{
+	uint8 surface;
+	uint8 piece;
+
+	void Set(const CVector &min, const CVector &max, uint8 surf, uint8 piece);
+	using CBox::Set;
+
+	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..ba33c134
--- /dev/null
+++ b/src/collision/ColModel.cpp
@@ -0,0 +1,201 @@
+#include "common.h"
+#include "ColModel.h"
+#include "Collision.h"
+#include "Game.h"
+#include "Pools.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 = LEVEL_GENERIC;	// generic col slot
+	ownsCollisionVolumes = true;
+}
+
+CColModel::~CColModel(void)
+{
+	RemoveCollisionVolumes();
+	RemoveTrianglePlanes();
+}
+
+void*
+CColModel::operator new(size_t)
+{
+	CColModel* node = CPools::GetColModelPool()->New();
+	assert(node);
+	return node;
+}
+
+void
+CColModel::operator delete(void *p, size_t)
+{
+	CPools::GetColModelPool()->Delete((CColModel*)p);
+}
+
+void
+CColModel::RemoveCollisionVolumes(void)
+{
+	if(ownsCollisionVolumes){
+		RwFree(spheres);
+		RwFree(lines);
+		RwFree(boxes);
+		RwFree(vertices);
+		RwFree(triangles);
+		CCollision::RemoveTrianglePlanes(this);
+	}
+	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/ColModel.h b/src/collision/ColModel.h
new file mode 100644
index 00000000..cd5ae651
--- /dev/null
+++ b/src/collision/ColModel.h
@@ -0,0 +1,39 @@
+#pragma once
+
+#include "templates.h"
+#include "ColBox.h"
+#include "ColSphere.h"
+#include "ColLine.h"
+#include "ColPoint.h"
+#include "ColTriangle.h"
+
+struct CColModel
+{
+	CSphere boundingSphere;
+	CBox boundingBox;
+	int16 numSpheres;
+	int16 numBoxes;
+	int16 numTriangles;
+	int8 numLines;
+	uint8 level;	// colstore slot but probably still named level
+	bool ownsCollisionVolumes;
+	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;
+
+	void *operator new(size_t);
+	void operator delete(void *p, size_t);
+	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..65f02860
--- /dev/null
+++ b/src/collision/ColSphere.cpp
@@ -0,0 +1,27 @@
+#include "common.h"
+#include "ColSphere.h"
+#include "General.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;
+}
+
+bool
+CColSphere::IntersectRay(CVector const& from, CVector const& dir, CVector &entry, CVector &exit)
+{
+	CVector distToCenter = from - center;
+	float distToTouchSqr = distToCenter.MagnitudeSqr() - sq(radius);
+	float root1, root2;
+
+	if (!CGeneral::SolveQuadratic(1.0f, DotProduct(distToCenter, dir) * 2.f, distToTouchSqr, root1, root2))
+		return false;
+
+	entry = from + dir * root1;
+	exit = from + dir * root2;
+	return true;
+}
\ No newline at end of file
diff --git a/src/collision/ColSphere.h b/src/collision/ColSphere.h
new file mode 100644
index 00000000..f86b282a
--- /dev/null
+++ b/src/collision/ColSphere.h
@@ -0,0 +1,21 @@
+#pragma once
+
+#include "SurfaceTable.h"
+
+struct CSphere
+{
+	// NB: this has to be compatible with a CVuVector
+	CVector center;
+	float radius;
+	void Set(float radius, const CVector &center) { this->center = center; this->radius = radius; }
+};
+
+struct CColSphere : public CSphere
+{
+	uint8 surface;
+	uint8 piece;
+
+	void Set(float radius, const CVector &center, uint8 surf, uint8 piece);
+	bool IntersectRay(CVector const &from, CVector const &dir, CVector &entry, CVector &exit);
+	using CSphere::Set;
+};
\ No newline at end of file
diff --git a/src/collision/ColTriangle.cpp b/src/collision/ColTriangle.cpp
new file mode 100644
index 00000000..843fb93f
--- /dev/null
+++ b/src/collision/ColTriangle.cpp
@@ -0,0 +1,32 @@
+#include "common.h"
+#include "ColTriangle.h"
+
+#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..a2580c58
--- /dev/null
+++ b/src/collision/ColTriangle.h
@@ -0,0 +1,77 @@
+#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(int a, int b, int c, uint8 surf)
+	{
+		this->a = a;
+		this->b = b;
+		this->c = c;
+		this->surface = surf;
+	}
+};
+
+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 GetNormalX() const { return normal.x; }
+	float GetNormalY() const { return normal.y; }
+	float GetNormalZ() const { return normal.z; }
+	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 91e7bba7..20778345 100644
--- a/src/core/Collision.cpp
+++ b/src/collision/Collision.cpp
@@ -26,303 +26,8 @@
 
 //--MIAMI: file done
 
-
-// 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)
@@ -365,17 +70,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;
 
@@ -2261,11 +1955,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
@@ -2298,7 +1993,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;
 	}
@@ -2870,295 +2566,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;
-}
-
-bool
-CColSphere::IntersectRay(CVector const& from, CVector const& dir, CVector &entry, CVector &exit)
-{
-	CVector distToCenter = from - center;
-	float distToTouchSqr = distToCenter.MagnitudeSqr() - sq(radius);
-	float root1, root2;
-
-	if (!CGeneral::SolveQuadratic(1.0f, DotProduct(distToCenter, dir) * 2.f, distToTouchSqr, root1, root2))
-		return false;
-
-	entry = from + dir * root1;
-	exit = from + dir * root2;
-	return true;
-}
-
-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
-
-CColPoint&
-CColPoint::operator=(const CColPoint& other)
-{
-	point = other.point;
-	normal = other.normal;
-	surfaceA = other.surfaceA;
-	pieceA = other.pieceA;
-	surfaceB = other.surfaceB;
-	pieceB = other.pieceB;
-	// doesn't copy depth
-	return *this;
-}
-
-CColModel::CColModel(void)
-{
-	numSpheres = 0;
-	spheres = nil;
-	numLines = 0;
-	lines = nil;
-	numBoxes = 0;
-	boxes = nil;
-	numTriangles = 0;
-	vertices = nil;
-	triangles = nil;
-	trianglePlanes = nil;
-	level = 0;	// generic col slot
-	ownsCollisionVolumes = true;
-}
-
-CColModel::~CColModel(void)
-{
-	RemoveCollisionVolumes();
-	RemoveTrianglePlanes();
-}
-
-void
-CColModel::RemoveCollisionVolumes(void)
-{
-	if(ownsCollisionVolumes){
-		RwFree(spheres);
-		RwFree(lines);
-		RwFree(boxes);
-		RwFree(vertices);
-		RwFree(triangles);
-		CCollision::RemoveTrianglePlanes(this);
-	}
-	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();
-}
-
-void*
-CColModel::operator new(size_t){
-	CColModel *node = CPools::GetColModelPool()->New();
-	assert(node);
-	return node;
-}
-
-void
-CColModel::operator delete(void *p, size_t){
-	CPools::GetColModelPool()->Delete((CColModel*)p);
-}
-
-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..57f5f86e
--- /dev/null
+++ b/src/collision/Collision.h
@@ -0,0 +1,68 @@
+#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;
+
+	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);
+	static void RemoveTrianglePlanes(CColModel *model);
+
+	// all these return true if there's a collision
+	static bool TestSphereSphere(const CSphere &s1, const CSphere &s2);
+	static bool TestSphereBox(const CSphere &sph, const CBox &box);
+	static bool TestLineBox(const CColLine &line, const CBox &box);
+	static bool TestVerticalLineBox(const CColLine &line, const CBox &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, bool ignoreShootThrough);
+
+	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, CStoredCollPoly *poly = nil);
+	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, bool ignoreShootThrough);
+	static bool ProcessVerticalLine(const CColLine &line, const CMatrix &matrix, CColModel &model, CColPoint &point, float &mindist, bool ignoreSeeThrough, bool ignoreShootThrough, 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 203d24d0..e7ba54f1 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;
diff --git a/src/core/TempColModels.h b/src/collision/TempColModels.h
index 0c936d6f..0c936d6f 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
diff --git a/src/control/CarCtrl.cpp b/src/control/CarCtrl.cpp
index 93e9aa44..2af218c4 100644
--- a/src/control/CarCtrl.cpp
+++ b/src/control/CarCtrl.cpp
@@ -2148,8 +2148,9 @@ bool CCarCtrl::PickNextNodeToFollowPath(CVehicle* pVehicle)
 			pVehicle->AutoPilot.m_vecDestinationCoors, pVehicle->AutoPilot.m_aPathFindNodesInfo,
 			&pVehicle->AutoPilot.m_nPathFindNodesCount, NUM_PATH_NODES_IN_AUTOPILOT,
 			pVehicle, nil, 999999.9f, -1);
-		if (pVehicle->AutoPilot.m_nPathFindNodesCount < 1)
+		if (pVehicle->AutoPilot.m_nPathFindNodesCount < 2)
 			return true;
+		pVehicle->AutoPilot.RemoveOnePathNode();
 	}
 	CPathNode* pNextPathNode = &ThePaths.m_pathNodes[pVehicle->AutoPilot.m_nNextRouteNode];
 	CCarPathLink* pCurLink = &ThePaths.m_carPathLinks[pVehicle->AutoPilot.m_nNextPathNodeInfo];
diff --git a/src/core/Collision.h b/src/core/Collision.h
deleted file mode 100644
index 93959a72..00000000
--- a/src/core/Collision.h
+++ /dev/null
@@ -1,257 +0,0 @@
-#pragma once
-
-#include "templates.h"
-#include "Game.h"	// for eLevelName
-#ifdef VU_COLLISION
-#include "VuVector.h"
-#endif
-
-// 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
-
-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
-};
-
-struct CSphere
-{
-	// NB: this has to be compatible with a CVuVector
-	CVector center;
-	float radius;
-	void Set(float radius, const CVector &center) { this->center = center; this->radius = radius; }
-};
-
-struct CBox
-{
-	CVector min;
-	CVector max;
-	CVector GetSize(void) { return max - min; }
-	void Set(const CVector &min, const CVector &max) {  this->min = min; this->max = max; }
-};
-
-struct CColSphere : public CSphere
-{
-	uint8 surface;
-	uint8 piece;
-
-	void Set(float radius, const CVector &center, uint8 surf, uint8 piece);
-	bool IntersectRay(CVector const &from, CVector const &dir, CVector &entry, CVector &exit);
-	using CSphere::Set;
-};
-
-struct CColBox : public CBox
-{
-	uint8 surface;
-	uint8 piece;
-
-	void Set(const CVector &min, const CVector &max, uint8 surf, uint8 piece);
-	using CBox::Set;
-};
-
-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);
-};
-
-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 GetNormalX() const { return normal.x; }
-	float GetNormalY() const { return normal.y; }
-	float GetNormalZ() const { return normal.z; }
-	float CalcPoint(const CVector &v) const { return DotProduct(normal, v) - dist; };
-#endif
-};
-
-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;
-
-	CColPoint& operator=(const CColPoint& other);
-};
-
-struct CStoredCollPoly
-{
-#ifdef VU_COLLISION
-	CVuVector verts[3];
-#else
-	CVector verts[3];
-#endif
-	bool valid;
-};
-
-struct CColModel
-{
-	CSphere boundingSphere;
-	CBox boundingBox;
-	int16 numSpheres;
-	int16 numBoxes;
-	int16 numTriangles;
-	int8 numLines;
-	uint8 level;	// colstore slot but probably still named level
-	bool ownsCollisionVolumes;
-	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;
-
-	void *operator new(size_t);
-	void operator delete(void *p, size_t);
-	CColModel& operator=(const CColModel& other);
-};
-
-class CCollision
-{
-public:
-	static eLevelName ms_collisionInMemory;
-	static CLinkList<CColModel*> ms_colModelCache;
-
-	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);
-	static void RemoveTrianglePlanes(CColModel *model);
-
-	// all these return true if there's a collision
-	static bool TestSphereSphere(const CSphere &s1, const CSphere &s2);
-	static bool TestSphereBox(const CSphere &sph, const CBox &box);
-	static bool TestLineBox(const CColLine &line, const CBox &box);
-	static bool TestVerticalLineBox(const CColLine &line, const CBox &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, bool ignoreShootThrough);
-
-	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, CStoredCollPoly *poly = nil);
-	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, bool ignoreShootThrough);
-	static bool ProcessVerticalLine(const CColLine &line, const CMatrix &matrix, CColModel &model, CColPoint &point, float &mindist, bool ignoreSeeThrough, bool ignoreShootThrough, 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/core/FileLoader.cpp b/src/core/FileLoader.cpp
index da678fd3..1c9b4036 100644
--- a/src/core/FileLoader.cpp
+++ b/src/core/FileLoader.cpp
@@ -345,7 +345,7 @@ CFileLoader::LoadCollisionModel(uint8 *buf, CColModel &model, char *modelname)
 	if(model.numTriangles > 0){
 		model.triangles = (CColTriangle*)RwMalloc(model.numTriangles*sizeof(CColTriangle));
 		for(i = 0; i < model.numTriangles; i++){
-			model.triangles[i].Set(model.vertices, *(int32*)buf, *(int32*)(buf+4), *(int32*)(buf+8), buf[12], buf[13]);
+			model.triangles[i].Set(*(int32*)buf, *(int32*)(buf+4), *(int32*)(buf+8), buf[12]);
 			buf += 16;
 		}
 	}else
diff --git a/src/core/Pad.cpp b/src/core/Pad.cpp
index b1e91f89..91e2d704 100644
--- a/src/core/Pad.cpp
+++ b/src/core/Pad.cpp
@@ -1833,7 +1833,7 @@ void CPad::Update(int16 pad)
 		{
 			if ( ShakeDur )
 			{
-				ShakeDur = Max(ShakeDur - CTimer::GetTimeStepInMilliseconds(), 0);
+				ShakeDur = Max(ShakeDur - (int32)CTimer::GetTimeStepInMilliseconds(), 0);
 
 				if ( ShakeDur == 0 )
 				{
diff --git a/src/entities/Physical.cpp b/src/entities/Physical.cpp
index 5483641f..acf9c702 100644
--- a/src/entities/Physical.cpp
+++ b/src/entities/Physical.cpp
@@ -633,7 +633,7 @@ CPhysical::ApplyCollision(CPhysical *B, CColPoint &colpoint, float &impulseA, fl
 						if(IsGlass(B->GetModelIndex()))
 							CGlass::WindowRespondsToSoftCollision(B, impulseA);
 						if(!A->bInfiniteMass)
-							A->ApplyMoveForce(colpoint.normal*(1.0f + A->m_fElasticity)*impulseA);
+							A->ApplyMoveForce(colpoint.GetNormal() * (1.0f + A->m_fElasticity) * impulseA);
 						return true;
 					}
 				}else if(!B->bInfiniteMass)
@@ -688,7 +688,7 @@ CPhysical::ApplyCollision(CPhysical *B, CColPoint &colpoint, float &impulseA, fl
 					}else{
 						if(IsGlass(B->GetModelIndex()))
 							CGlass::WindowRespondsToSoftCollision(B, impulseA);
-						CVector f = colpoint.normal * impulseA;
+						CVector f = colpoint.GetNormal() * impulseA;
 						if(A->IsVehicle() && colpoint.normal.z < 0.7f)
 							f.z *= 0.3f;
 						if(!A->bInfiniteMass){
@@ -1303,43 +1303,43 @@ CPhysical::ProcessShiftSectorList(CPtrList *lists)
 
 			mostColliding = 0;
 			for(j = 1; j < numCollisions; j++)
-				if(colpoints[j].depth > colpoints[mostColliding].depth)
+				if (colpoints[j].GetDepth() > colpoints[mostColliding].GetDepth())
 					mostColliding = j;
 
 			if(CWorld::bSecondShift)
 				for(j = 0; j < numCollisions; j++)
-					shift += colpoints[j].normal * colpoints[j].depth * 1.5f/numCollisions;
+					shift += colpoints[j].GetNormal() * colpoints[j].GetDepth() * 1.5f / numCollisions;
 			else
 				for(j = 0; j < numCollisions; j++)
-					shift += colpoints[j].normal * colpoints[j].depth * 1.2f/numCollisions;
+					shift += colpoints[j].GetNormal() * colpoints[j].GetDepth() * 1.2f / numCollisions;
 
 			if(A->IsVehicle() && B->IsVehicle()){
 				CVector dir = A->GetPosition() - B->GetPosition();
 				dir.Normalise();
 				if(dir.z < 0.0f && dir.z < A->GetForward().z && dir.z < A->GetRight().z)
 					dir.z = Min(0.0f, Min(A->GetForward().z, A->GetRight().z));
-				shift += dir * colpoints[mostColliding].depth * 0.5f;
+				shift += dir * colpoints[mostColliding].GetDepth() * 0.5f;
 			}else if(A->IsPed() && B->IsVehicle() && ((CVehicle*)B)->IsBoat()){
-				CVector dir = colpoints[mostColliding].normal;
+				CVector dir = colpoints[mostColliding].GetNormal();
 				float f = Min(Abs(dir.z), 0.9f);
 				dir.z = 0.0f;
 				dir.Normalise();
-				shift += dir * colpoints[mostColliding].depth / (1.0f - f);
+				shift += dir * colpoints[mostColliding].GetDepth() / (1.0f - f);
 				boat = B;
 			}else if(B->IsPed() && A->IsVehicle() && ((CVehicle*)A)->IsBoat()){
-				CVector dir = colpoints[mostColliding].normal * -1.0f;
+				CVector dir = colpoints[mostColliding].GetNormal() * -1.0f;
 				float f = Min(Abs(dir.z), 0.9f);
 				dir.z = 0.0f;
 				dir.Normalise();
-				B->GetMatrix().Translate(dir * colpoints[mostColliding].depth / (1.0f - f));
+				B->GetMatrix().Translate(dir * colpoints[mostColliding].GetDepth() / (1.0f - f));
 				// BUG? how can that ever happen? A is a Ped
 				if(B->IsVehicle())
 					B->ProcessEntityCollision(A, colpoints);
 			}else{
 				if(CWorld::bSecondShift)
-					shift += colpoints[mostColliding].normal * colpoints[mostColliding].depth * 0.4f;
+					shift += colpoints[mostColliding].GetNormal() * colpoints[mostColliding].GetDepth() * 0.4f;
 				else
-					shift += colpoints[mostColliding].normal * colpoints[mostColliding].depth * 0.2f;
+					shift += colpoints[mostColliding].GetNormal() * colpoints[mostColliding].GetDepth() * 0.2f;
 			}
 
 			doShift = true;
diff --git a/src/fakerw/fake.cpp b/src/fakerw/fake.cpp
index f37e5813..5d53605d 100644
--- a/src/fakerw/fake.cpp
+++ b/src/fakerw/fake.cpp
@@ -943,7 +943,9 @@ RwBool       RtCharsetDestroy(RtCharset * charSet) { charSet->destroy(); return
 RwInt32 _rwD3D8FindCorrectRasterFormat(RwRasterType type, RwInt32 flags)
 {
 #ifdef RW_GL3
-	return '3LGO';
+	if(flags & (rwRASTERFORMATPAL8 | rwRASTERFORMAT8888))
+		return 'NOPE';
+	return 'YUP';
 #endif
 	return flags & 0xF00;
 }
diff --git a/src/peds/Ped.cpp b/src/peds/Ped.cpp
index 59b56167..51b16663 100644
--- a/src/peds/Ped.cpp
+++ b/src/peds/Ped.cpp
@@ -140,9 +140,6 @@ bool CPed::bFannyMagnetCheat;
 bool CPed::bPedCheat3;
 CVector2D CPed::ms_vec2DFleePosition;
 
-CVector vecNextPathNode;
-bool vecNextPathNodeInitialized;
-
 void *CPed::operator new(size_t sz) { return CPools::GetPedPool()->New();  }
 void *CPed::operator new(size_t sz, int handle) { return CPools::GetPedPool()->New(handle); }
 void CPed::operator delete(void *p, size_t sz) { CPools::GetPedPool()->Delete((CPed*)p); }
@@ -287,8 +284,7 @@ CPed::CPed(uint32 pedType) : m_pedIK(this)
 	m_phoneId = -1;
 	m_lastAccident = 0;
 	m_fleeFrom = nil;
-	m_fleeFromPosX = 0;
-	m_fleeFromPosY = 0;
+	m_fleeFromPos = CVector2D(0.0f, 0.0f);
 	m_fleeTimer = 0;
 	m_threatEx = nil;
 	m_vecSeekPosEx = CVector(0.0f, 0.0f, 0.0f);
@@ -722,24 +718,11 @@ CPed::CanStrafeOrMouseControl(void)
 void
 CPed::AddWeaponModel(int id)
 {
-	RpAtomic *atm;
-
 	if (id != -1) {
-#ifdef PED_SKIN
-		if (IsClumpSkinned(GetClump())) {
-			if (m_pWeaponModel)
-				RemoveWeaponModel(-1);
+		if (m_pWeaponModel)
+			RemoveWeaponModel(-1);
 
-			m_pWeaponModel = (RpAtomic*)CModelInfo::GetModelInfo(id)->CreateInstance();
-		}
-		else
-#endif
-		{
-			atm = (RpAtomic*)CModelInfo::GetModelInfo(id)->CreateInstance();
-			RwFrameDestroy(RpAtomicGetFrame(atm));
-			RpAtomicSetFrame(atm, m_pFrames[PED_HANDR]->frame);
-			RpClumpAddAtomic(GetClump(), atm);
-		}
+		m_pWeaponModel = (RpAtomic*)CModelInfo::GetModelInfo(id)->CreateInstance();
 		CModelInfo::GetModelInfo(id)->AddRef();
 		m_wepModelID = id;
 
@@ -752,7 +735,6 @@ CPed::AddWeaponModel(int id)
 void
 CPed::AimGun(void)
 {
-	RwV3d pos;
 	CVector vector;
 
 	if (IsPlayer() && bIsDucking)
@@ -760,8 +742,7 @@ CPed::AimGun(void)
 
 	if (m_pSeekTarget) {
 		if (m_pSeekTarget->IsPed()) {
-			((CPed*)m_pSeekTarget)->m_pedIK.GetComponentPosition(pos, PED_MID);
-			vector = pos;
+			((CPed*)m_pSeekTarget)->m_pedIK.GetComponentPosition(vector, PED_MID);
 		} else {
 			vector = m_pSeekTarget->GetPosition();
 		}
@@ -1060,14 +1041,10 @@ CPed::FinishedReloadCB(CAnimBlendAssociation *reloadAssoc, void *arg)
 			CAnimManager::BlendAnimation(ped->GetClump(), weapon->m_AnimToPlay, GetPrimaryFireAnim(weapon), 8.0f);
 		fireAssoc->SetFinishCallback(FinishedAttackCB, ped);
 		fireAssoc->SetRun();
-		if (fireAssoc->currentTime != reloadAssoc->hierarchy->totalLength) {
-			if (fireAssoc->currentTime >= weapon->m_fAnimLoopStart)
-				return;
-
+		if (fireAssoc->currentTime == reloadAssoc->hierarchy->totalLength)
 			fireAssoc->SetCurrentTime(Max(weapon->m_fAnimLoopStart - 0.04f, 0.0f));
-		} else {
+		else if (fireAssoc->currentTime < weapon->m_fAnimLoopStart)
 			fireAssoc->SetCurrentTime(Max(weapon->m_fAnimLoopStart - 0.04f, 0.0f));
-		}
 	}
 }
 
@@ -1570,21 +1547,16 @@ void
 CPed::RemoveWeaponModel(int modelId)
 {
 	// modelId is not used!! This function just removes the current weapon.
-#ifdef PED_SKIN
-	if(IsClumpSkinned(GetClump())){
-		if(m_pWeaponModel){
-			if (modelId == -1
-				|| CVisibilityPlugins::GetAtomicModelInfo(m_pWeaponModel) == CModelInfo::GetModelInfo(modelId)) {
-				CVisibilityPlugins::GetAtomicModelInfo(m_pWeaponModel)->RemoveRef();
-				RwFrame* frm = RpAtomicGetFrame(m_pWeaponModel);
-				RpAtomicDestroy(m_pWeaponModel);
-				RwFrameDestroy(frm);
-				m_pWeaponModel = nil;
-			}
+	if(m_pWeaponModel){
+		if (modelId == -1
+			|| CVisibilityPlugins::GetAtomicModelInfo(m_pWeaponModel) == CModelInfo::GetModelInfo(modelId)) {
+			CVisibilityPlugins::GetAtomicModelInfo(m_pWeaponModel)->RemoveRef();
+			RwFrame* frm = RpAtomicGetFrame(m_pWeaponModel);
+			RpAtomicDestroy(m_pWeaponModel);
+			RwFrameDestroy(frm);
+			m_pWeaponModel = nil;
 		}
-	}else
-#endif
-		RwFrameForAllObjects(m_pFrames[PED_HANDR]->frame,RemoveAllModelCB,nil);
+	}
 
 	if (IsPlayer() && (modelId == -1 || modelId == MI_MINIGUN)) {
 		RpAtomic* &atm = ((CPlayerPed*)this)->m_pMinigunTopAtomic;
@@ -1637,8 +1609,8 @@ CPed::SelectGunIfArmed(void)
 		if (GetWeapon(i).m_nAmmoTotal > 0) {
 			eWeaponType weaponType = GetWeapon(i).m_eWeaponType;
 
-			// First condition checks for Pistol, Python and Shotguns
-			if ((weaponType >= WEAPONTYPE_COLT45 && weaponType < WEAPONTYPE_TEC9) ||
+			if (weaponType == WEAPONTYPE_COLT45 || weaponType == WEAPONTYPE_PYTHON || weaponType == WEAPONTYPE_SHOTGUN ||
+				weaponType == WEAPONTYPE_SPAS12_SHOTGUN || weaponType == WEAPONTYPE_STUBBY_SHOTGUN ||
 				weaponType == WEAPONTYPE_UZI || weaponType == WEAPONTYPE_M4 || weaponType == WEAPONTYPE_MP5 ||
 				weaponType == WEAPONTYPE_ROCKETLAUNCHER || weaponType == WEAPONTYPE_FLAMETHROWER || weaponType == WEAPONTYPE_SNIPERRIFLE) {
 				SetCurrentWeapon(i);
@@ -2689,8 +2661,8 @@ CPed::SortPeds(CPed **list, int min, int max)
 	float middleDist = middleDiff.Magnitude();
 
 	int left = max;
-	int right;
-	for(right = min; right <= left; ){
+	int right = min;
+	while(right <= left){
 		float rightDist, leftDist;
 		do {
 			rightDiff = GetPosition() - list[right]->GetPosition();
@@ -2718,33 +2690,7 @@ CPed::SortPeds(CPed **list, int min, int max)
 void
 CPed::BuildPedLists(void)
 {
-	if ((CTimer::GetFrameCounter() + m_randomSeed) % 16) {
-
-		for(int i = 0; i < ARRAY_SIZE(m_nearPeds); ) {
-			bool removePed = false;
-			if (m_nearPeds[i]) {
-				if (m_nearPeds[i]->IsPointerValid()) {
-					float distSqr = (GetPosition() - m_nearPeds[i]->GetPosition()).MagnitudeSqr2D();
-					if (distSqr > sq(nThreatReactionRangeMultiplier * 30.f)) {
-						removePed = true;
-					}
-				} else {
-					removePed = true;
-				}
-			}
-			if (removePed) {
-				// If we arrive here, the ped we're checking isn't "near", so we should remove it.
-				for (int j = i; j < ARRAY_SIZE(m_nearPeds) - 1; j++) {
-					m_nearPeds[j] = m_nearPeds[j + 1];
-					m_nearPeds[j + 1] = nil;
-				}
-				// Above loop won't work when it's 9, so we need to empty slot 9.
-				m_nearPeds[9] = nil;
-				m_numNearPeds--;
-			} else
-				i++;
-		}
-	} else {
+	if (((CTimer::GetFrameCounter() + m_randomSeed) % 16) == 0) {
 		CVector centre = CEntity::GetBoundCentre();
 		int deadsRegistered = 0;
 		CRect rect(centre.x - 20.f * nThreatReactionRangeMultiplier,
@@ -2789,6 +2735,31 @@ CPed::BuildPedLists(void)
 		for (int pedToClear = m_numNearPeds; pedToClear < ARRAY_SIZE(m_nearPeds); pedToClear++)
 			m_nearPeds[pedToClear] = nil;
 	}
+
+	for(int i = 0; i < ARRAY_SIZE(m_nearPeds); ) {
+		bool removePed = false;
+		if (m_nearPeds[i]) {
+			if (m_nearPeds[i]->IsPointerValid()) {
+				float distSqr = (GetPosition() - m_nearPeds[i]->GetPosition()).MagnitudeSqr2D();
+				if (distSqr > sq(nThreatReactionRangeMultiplier * 30.f)) {
+					removePed = true;
+				}
+			} else {
+				removePed = true;
+			}
+		}
+		if (removePed) {
+			// If we arrive here, the ped we're checking isn't "near", so we should remove it.
+			for (int j = i; j < ARRAY_SIZE(m_nearPeds) - 1; j++) {
+				m_nearPeds[j] = m_nearPeds[j + 1];
+				m_nearPeds[j + 1] = nil;
+			}
+			// Above loop won't work on last slot, so we need to empty it.
+			m_nearPeds[ARRAY_SIZE(m_nearPeds) - 1] = nil;
+			m_numNearPeds--;
+		} else
+			i++;
+	}
 }
 
 // --MIAMI: Done
@@ -2802,11 +2773,8 @@ CPed::SetPedStats(ePedStats pedStat)
 bool
 CPed::CanUseTorsoWhenLooking(void)
 {
-	if (m_nPedState != PED_DRIVING && m_nPedState != PED_DRAG_FROM_CAR && !bIsDucking) {
-		if (m_animGroup != ASSOCGRP_SEXYWOMAN && m_animGroup != ASSOCGRP_WOMAN)
-			return true;
-	}
-	return false;
+	return m_nPedState != PED_DRIVING && m_nPedState != PED_DRAG_FROM_CAR && !bIsDucking &&
+		m_animGroup != ASSOCGRP_SEXYWOMAN && m_animGroup != ASSOCGRP_WOMAN;
 }
 
 // --MIAMI: Done
@@ -3086,17 +3054,17 @@ CPed::CanPedJumpThis(CEntity *unused, CVector *damageNormal = nil)
 		if (damageNormal->z > 0.9f)
 			return false;
 
-		CColModel *ourCol = CModelInfo::GetModelInfo(m_modelIndex)->GetColModel();
+		CColModel *ourCol = GetColModel();
 		pos.z = ourCol->spheres->center.z - ourCol->spheres->radius * damageNormal->z + pos.z;
 		pos.z = pos.z + 0.05f;
 		float collPower = damageNormal->Magnitude2D();
-		if (damageNormal->z <= 0.5f) {
-			forwardOffset += collPower * ourCol->spheres->radius * forwardOffset;
-		} else {
+		if (damageNormal->z > 0.5f) {
 			CVector invDamageNormal(-damageNormal->x, -damageNormal->y, 0.0f);
 			invDamageNormal *= 1.0f / collPower;
 			CVector estimatedJumpDist = invDamageNormal + collPower * invDamageNormal * ourCol->spheres->radius;
 			forwardOffset = estimatedJumpDist * Min(2.0f / collPower, 4.0f);
+		} else {
+			forwardOffset += collPower * ourCol->spheres->radius * forwardOffset;
 		}
 	} else {
 		pos.z -= 0.15f;
@@ -3495,18 +3463,10 @@ CPed::SetObjective(eObjective newObj)
 			|| m_objective == OBJECTIVE_LEAVE_CAR_AND_DIE) && !IsPlayer() && !IsPedInControl()) {
 
 			bStartWanderPathOnFoot = true;
-			return;
-		}
-		// Unused code from assembly...
-		/*
-		else if(m_objective == OBJECTIVE_FLEE_CAR) {
-
 		} else {
-
+			m_objective = OBJECTIVE_NONE;
+			m_prevObjective = OBJECTIVE_NONE;
 		}
-		*/
-		m_objective = OBJECTIVE_NONE;
-		m_prevObjective = OBJECTIVE_NONE;
 	} else if (m_prevObjective != newObj || m_prevObjective == OBJECTIVE_NONE) {
 		SetObjectiveTimer(0);
 
@@ -3636,10 +3596,9 @@ CPed::InformMyGangOfAttack(CEntity *attacker)
 void
 CPed::QuitEnteringCar(void)
 {
-	CAnimBlendAssociation *animAssoc = m_pVehicleAnim;
 	CVehicle *veh = m_pMyVehicle;
-	if (animAssoc)
-		animAssoc->blendDelta = -1000.0f;
+	if (m_pVehicleAnim)
+		m_pVehicleAnim->blendDelta = -1000.0f;
 	
 	RestartNonPartialAnims();
 
@@ -3666,11 +3625,10 @@ CPed::QuitEnteringCar(void)
 	bUsesCollision = true;
 
 	if (DyingOrDead()) {
-		animAssoc = m_pVehicleAnim;
-		if (animAssoc) {
-			animAssoc->blendDelta = -4.0f;
-			animAssoc->flags |= ASSOC_DELETEFADEDOUT;
-			animAssoc->flags &= ~ASSOC_RUNNING;
+		if (m_pVehicleAnim) {
+			m_pVehicleAnim->blendDelta = -4.0f;
+			m_pVehicleAnim->flags |= ASSOC_DELETEFADEDOUT;
+			m_pVehicleAnim->flags &= ~ASSOC_RUNNING;
 		}
 	} else
 		SetIdle();
@@ -3762,21 +3720,16 @@ CPed::ReactToAttack(CEntity *attacker)
 bool
 CPed::TurnBody(void)
 {
-	float lookDir;
 	bool turnDone = true;
 
-	if (m_pLookTarget) {
-		const CVector &lookPos = m_pLookTarget->GetPosition();
-
-		lookDir = CGeneral::GetRadianAngleBetweenPoints(
-					lookPos.x,
-					lookPos.y,
+	if (m_pLookTarget)
+		m_fLookDirection = CGeneral::GetRadianAngleBetweenPoints(
+					m_pLookTarget->GetPosition().x,
+					m_pLookTarget->GetPosition().y,
 					GetPosition().x,
 					GetPosition().y);
-	} else
-		lookDir = m_fLookDirection;
 
-	float limitedLookDir = CGeneral::LimitRadianAngle(lookDir);
+	float limitedLookDir = CGeneral::LimitRadianAngle(m_fLookDirection);
 	float currentRot = m_fRotationCur;
 
 	if (currentRot - PI > limitedLookDir)
@@ -3814,6 +3767,7 @@ CPed::Chat(void)
 
 	if (partner->m_nPedState != PED_CHAT) {
 		ClearChat();
+		m_standardTimer = CTimer::GetTimeInMilliseconds() + 30000;
 		if (partner->m_pedInObjective) {
 			if (partner->m_objective == OBJECTIVE_KILL_CHAR_ON_FOOT ||
 				partner->m_objective == OBJECTIVE_FLEE_CHAR_ON_FOOT_TILL_SAFE)
@@ -3832,12 +3786,12 @@ CPed::Chat(void)
 		} else
 			Say(SOUND_PED_CHAT);
 
-	} else if (!RpAnimBlendClumpGetFirstAssociation(GetClump(), ASSOC_IDLE)) {
+	} else {
 
-		if (CGeneral::GetRandomNumber() < 20) {
+		if (CGeneral::GetRandomNumber() < 20 && !RpAnimBlendClumpGetFirstAssociation(GetClump(), ASSOC_IDLE)) {
 			CAnimManager::BlendAnimation(GetClump(), ASSOCGRP_STD, ANIM_XPRESS_SCRATCH, 4.0f);
 		}
-		if (!bIsTalking) {
+		if (!bIsTalking && !RpAnimBlendClumpGetFirstAssociation(GetClump(), ASSOC_IDLE)) {
 			CAnimBlendAssociation *chatAssoc = CAnimManager::BlendAnimation(GetClump(), ASSOCGRP_STD, ANIM_IDLE_CHAT, 4.0f);
 			float chatTime = CGeneral::GetRandomNumberInRange(0.0f, 3.0f);
 			chatAssoc->SetCurrentTime(chatTime);
@@ -4038,7 +3992,7 @@ CPed::CheckForPointBlankPeds(CPed *pedToVerify)
 		if (!pedToVerify || pedToVerify == nearPed) {
 
 			CVector diff = nearPed->GetPosition() - GetPosition();
-			if (diff.Magnitude() < pbDistance) {
+			if (diff.MagnitudeSqr() < SQR(pbDistance)) {
 
 				float neededAngle = CGeneral::GetRadianAngleBetweenPoints(
 					nearPed->GetPosition().x, nearPed->GetPosition().y,
@@ -4117,8 +4071,7 @@ CPed::ClearAll(void)
 	m_nMoveState = PEDMOVE_NONE;
 	m_pSeekTarget = nil;
 	m_vecSeekPos = CVector(0.0f, 0.0f, 0.0f);
-	m_fleeFromPosX = 0.0f;
-	m_fleeFromPosY = 0.0f;
+	m_fleeFromPos = CVector2D(0.0f, 0.0f);
 	m_fleeFrom = nil;
 	m_fleeTimer = 0;
 	m_threatEx = nil;
@@ -4138,7 +4091,6 @@ CPed::ClearAttack(void)
 	if (m_nPedState != PED_ATTACK || (bIsDucking && !IsPlayer()) || m_nWaitState == WAITSTATE_PLAYANIM_DUCK)
 		return;
 
-	// VC uses CCamera::Using1stPersonWeaponMode
 	if (FindPlayerPed() == this && TheCamera.Using1stPersonWeaponMode()) {
 		SetPointGunAt(nil);
 	} else if (bIsPointingGunAt) {
@@ -5092,7 +5044,7 @@ CPed::RestoreHeadingRate(void)
 void
 CPed::RestoreHeadingRateCB(CAnimBlendAssociation *assoc, void *arg)
 {
-	((CPed*)arg)->m_headingRate = ((CPed*)arg)->m_pedStats->m_headingChangeRate;
+	((CPed*)arg)->RestoreHeadingRate();
 }
 
 // --MIAMI: Done
@@ -5203,7 +5155,7 @@ CPed::SetPointGunAt(CEntity *to)
 
 	SetPedState(PED_AIM_GUN);
 	bIsPointingGunAt = true;
-	SetMoveState(PEDMOVE_NONE);
+	SetMoveState(PEDMOVE_STILL);
 
 	CAnimBlendAssociation *aimAssoc;
 
@@ -5705,12 +5657,7 @@ CPed::StartFightAttack(uint8 buttonPressure)
 		animAssoc = CAnimManager::BlendAnimation(GetClump(), m_curFightMove < FIGHTMOVE_MELEE1 ? ASSOCGRP_STD : weaponInfo->m_AnimToPlay,
 			tFightMoves[m_curFightMove].animId, 8.0f);
 
-		if (weaponInfo->m_AnimToPlay != ASSOCGRP_KNIFE || m_curFightMove < FIGHTMOVE_MELEE1) {
-			if (m_curFightMove == FIGHTMOVE_BACKKICK)
-				animAssoc->speed = 1.15f;
-			else
-				animAssoc->speed = 0.8f;
-		} else {
+		if (weaponInfo->m_AnimToPlay == ASSOCGRP_KNIFE && m_curFightMove >= FIGHTMOVE_MELEE1) {
 			switch (GetWeapon()->m_eWeaponType) {
 				case WEAPONTYPE_SCREWDRIVER:
 				case WEAPONTYPE_KNIFE:
@@ -5728,6 +5675,11 @@ CPed::StartFightAttack(uint8 buttonPressure)
 					animAssoc->speed = 0.9f;
 					break;
 			}
+		} else {
+			if (m_curFightMove == FIGHTMOVE_BACKKICK)
+				animAssoc->speed = 1.15f;
+			else
+				animAssoc->speed = 0.8f;
 		}
 		if (IsPlayer())
 			animAssoc->SetCurrentTime(0.08f);
@@ -5770,9 +5722,9 @@ CPed::LoadFightData(void)
 		line[linelen] = '\0';
 
 		// skip white space
-		for (lp = 0; line[lp] <= ' '; lp++);
+		for (lp = 0; line[lp] <= ' ' && line[lp] != '\0'; lp++);
 
-		if (lp >= linelen ||		// FIX: game uses == here, but this is safer if we have empty lines
+		if (line[lp] == '\0' ||
 			line[lp] == '#')
 			continue;
 
@@ -5838,11 +5790,12 @@ CPed::LoadFightData(void)
 int
 CPed::GetLocalDirection(const CVector2D &posOffset)
 {
-	float direction;
+	int direction;
+	float angle;
 
-	for (direction = posOffset.Heading() - m_fRotationCur + DEGTORAD(45.0f); direction < 0.0f; direction += TWOPI);
+	for (angle = posOffset.Heading() - m_fRotationCur + DEGTORAD(45.0f); angle < 0.0f; angle += TWOPI);
 
-	for (direction = (int)RADTODEG(direction) / 90; direction > 3; direction -= 4);
+	for (direction = RADTODEG(angle)/90.0f; direction > 3; direction -= 4);
 
 	// 0-forward, 1-left, 2-backward, 3-right.
 	return direction;
@@ -6530,8 +6483,7 @@ CPed::SetFlee(CVector2D const &from, int time)
 		SetStoredState();
 		SetPedState(PED_FLEE_POS);
 		SetMoveState(PEDMOVE_RUN);
-		m_fleeFromPosX = from.x;
-		m_fleeFromPosY = from.y;
+		m_fleeFromPos = from;
 	}
 
 	bUsePedNodeSeek = true;
@@ -7120,9 +7072,7 @@ CPed::CreateDeadPedMoney(void)
 	if (!CGame::nastyGame)
 		return;
 
-	int skin = GetModelIndex();
-
-	if ((skin >= MI_COP && skin <= MI_FIREMAN) || (CharCreatedBy == MISSION_CHAR && !bMoneyHasBeenGivenByScript) || bInVehicle)
+	if ((GetModelIndex() >= MI_COP && GetModelIndex() <= MI_FIREMAN) || (CharCreatedBy == MISSION_CHAR && !bMoneyHasBeenGivenByScript) || bInVehicle)
 		return;
 
 	int money = m_nPedMoney;
@@ -7204,6 +7154,7 @@ CPed::CreateDeadPedWeaponPickups(void)
 
 		int quantity = Min(weaponAmmo, AmmoForWeapon_OnStreet[weapon] / 2);
 		CreateDeadPedPickupCoors(&pickupPos.x, &pickupPos.y, &pickupPos.z);
+		pickupPos.z += 0.3f;
 		if (!CPickups::TryToMerge_WeaponType(pickupPos, weapon, PICKUP_ONCE_TIMEOUT, quantity, false)) {
 			CPickups::GenerateNewOne_WeaponType(pickupPos, weapon, PICKUP_ONCE_TIMEOUT, Min(weaponAmmo, quantity));
 		}
@@ -7696,28 +7647,15 @@ CPed::EnterCar(void)
 		// CVector posForDoor = GetPositionToOpenCarDoor(veh, m_vehEnterType);
 
 		if (veh->CanPedOpenLocks(this)) {
-			if (m_vehEnterType) {
-				CAnimBlendAssociation *enterAssoc = m_pVehicleAnim;
-				if (enterAssoc)
-					veh->ProcessOpenDoor(m_vehEnterType, enterAssoc->animId, enterAssoc->currentTime);
+			if (m_vehEnterType && m_pVehicleAnim) {
+				veh->ProcessOpenDoor(m_vehEnterType, m_pVehicleAnim->animId, m_pVehicleAnim->currentTime);
 			}
 		}
 		bIsInTheAir = false;
 		LineUpPedWithCar(LINE_UP_TO_CAR_START);
 		if (veh->IsBike()) {
 			CBike *bike = (CBike*)veh;
-			if (bike->GetStatus() != STATUS_ABANDONED || bike->bIsBeingPickedUp || !m_pVehicleAnim) {
-				if (m_nPedState == PED_CARJACK && m_pVehicleAnim) {
-					if (m_pVehicleAnim->currentTime > 0.4f && m_pVehicleAnim->currentTime - m_pVehicleAnim->timeStep <= 0.4f) {
-						int anim = m_pVehicleAnim->animId;
-						if (anim == ANIM_BIKE_KICK) {
-							DMAudio.PlayOneShot(m_audioEntityId, SOUND_187, 3.0f);
-						} else if (anim == ANIM_BIKE_ELBOW_R || anim == ANIM_BIKE_ELBOW_L) {
-							DMAudio.PlayOneShot(m_audioEntityId, SOUND_186, 3.0f);
-						}
-					}
-				}
-			} else {
+			if (bike->GetStatus() == STATUS_ABANDONED && !bike->bIsBeingPickedUp && m_pVehicleAnim) {
 				int anim = m_pVehicleAnim->animId;
 
 				// One is pickup and other one is pullup, not same :p
@@ -7725,6 +7663,15 @@ CPed::EnterCar(void)
 					bike->bIsBeingPickedUp = true;
 				else if ((anim == ANIM_BIKE_PULLUP_R || anim == ANIM_BIKE_PULLUP_L) && m_pVehicleAnim->currentTime > 0.4667f)
 					bike->bIsBeingPickedUp = true;
+			} else if (m_nPedState == PED_CARJACK && m_pVehicleAnim) {
+				if (m_pVehicleAnim->currentTime > 0.4f && m_pVehicleAnim->currentTime - m_pVehicleAnim->timeStep <= 0.4f) {
+					int anim = m_pVehicleAnim->animId;
+					if (anim == ANIM_BIKE_KICK) {
+						DMAudio.PlayOneShot(m_audioEntityId, SOUND_187, 3.0f);
+					} else if (anim == ANIM_BIKE_ELBOW_R || anim == ANIM_BIKE_ELBOW_L) {
+						DMAudio.PlayOneShot(m_audioEntityId, SOUND_186, 3.0f);
+					}
+				}
 			}
 		}
 	} else {
@@ -7894,16 +7841,26 @@ CPed::ExitCar(void)
 		else
 			LineUpPedWithCar(LINE_UP_TO_CAR_FALL);
 
-	} else if (exitAnim != ANIM_CAR_ROLLOUT_LHS && exitAnim != ANIM_CAR_ROLLOUT_RHS) {
+	} else if (exitAnim == ANIM_CAR_ROLLOUT_LHS || exitAnim == ANIM_CAR_ROLLOUT_RHS) {
+		if (animTime > 0.07f && m_pMyVehicle && m_pMyVehicle->IsCar()) {
+			if (exitAnim == ANIM_CAR_ROLLOUT_LHS) {
+				((CAutomobile*)m_pMyVehicle)->KnockPedOutCar(WEAPONTYPE_UNIDENTIFIED, CAR_DOOR_LF, this);
+			} else {
+				((CAutomobile*)m_pMyVehicle)->KnockPedOutCar(WEAPONTYPE_UNIDENTIFIED, CAR_DOOR_RF, this);
+			}
+		} else {
+			LineUpPedWithCar(LINE_UP_TO_CAR_FALL);
+		}
+	} else {
 		m_pMyVehicle->ProcessOpenDoor(m_vehEnterType, exitAnim, animTime);
 
 		if (m_pSeekTarget) {
 			// Car is upside down
 			if (m_pMyVehicle->GetUp().z > -0.8f) {
-				if (exitAnim != ANIM_CAR_CLOSE_RHS && exitAnim != ANIM_CAR_CLOSE_LHS && animTime <= 0.3f)
-					LineUpPedWithCar((m_pMyVehicle->GetModelIndex() == MI_DODO ? LINE_UP_TO_CAR_END : LINE_UP_TO_CAR_START));
-				else
+				if (exitAnim == ANIM_CAR_CLOSE_RHS || exitAnim == ANIM_CAR_CLOSE_LHS || animTime > 0.3f)
 					LineUpPedWithCar(LINE_UP_TO_CAR_END);
+				else
+					LineUpPedWithCar((m_pMyVehicle->GetModelIndex() == MI_DODO ? LINE_UP_TO_CAR_END : LINE_UP_TO_CAR_START));
 			}
 			else {
 				LineUpPedWithCar(LINE_UP_TO_CAR_END);
@@ -7914,7 +7871,7 @@ CPed::ExitCar(void)
 		if (m_nPedState == PED_EXIT_CAR) {
 			CPed* foundPed = nil;
 			for (int i = 0; i < m_numNearPeds; i++) {
-				if ((m_nearPeds[i]->GetPosition() - GetPosition()).MagnitudeSqr2D() < 0.04f) {
+				if ((m_nearPeds[i]->GetPosition() - GetPosition()).MagnitudeSqr2D() < SQR(0.2f)) {
 					foundPed = m_nearPeds[i];
 					break;
 				}
@@ -7923,12 +7880,6 @@ CPed::ExitCar(void)
 				if (animTime > 0.4f && foundPed->IsPedInControl())
 					foundPed->SetFall(1000, ANIM_KO_SKID_FRONT, 1);
 		}
-	} else if (animTime <= 0.07f || !m_pMyVehicle || !m_pMyVehicle->IsCar()) {
-		LineUpPedWithCar(LINE_UP_TO_CAR_FALL);
-	} else if (exitAnim == ANIM_CAR_ROLLOUT_LHS) {
-		((CAutomobile*)m_pMyVehicle)->KnockPedOutCar(WEAPONTYPE_UNIDENTIFIED, CAR_DOOR_LF, this);
-	} else {
-		((CAutomobile*)m_pMyVehicle)->KnockPedOutCar(WEAPONTYPE_UNIDENTIFIED, CAR_DOOR_RF, this);
 	}
 }
 
@@ -7954,21 +7905,19 @@ CPed::Fall(void)
 		if (!fallAssoc)
 			fallAssoc = RpAnimBlendClumpGetAssociation(GetClump(), ANIM_FALL_FRONT);
 
-		if (fallAssoc || !firstPartialAssoc || 0.8f * firstPartialAssoc->hierarchy->totalLength >= firstPartialAssoc->currentTime) {
-			if (fallAssoc) {
-				if (fallAssoc->blendAmount > 0.3f && fallAssoc->blendDelta >= 0.0f)	{
-					float time = fallAssoc->currentTime;
+		if (!fallAssoc && firstPartialAssoc && 0.8f * firstPartialAssoc->hierarchy->totalLength < firstPartialAssoc->currentTime) {
+			if (firstPartialAssoc->flags & ASSOC_FRONTAL) {
+				CAnimManager::BlendAnimation(GetClump(), ASSOCGRP_STD, ANIM_FALL_FRONT, 8.0f);
+			} else {
+				CAnimManager::BlendAnimation(GetClump(), ASSOCGRP_STD, ANIM_FALL_BACK, 8.0f);
+			}
+		} else if (fallAssoc && fallAssoc->blendAmount > 0.3f && fallAssoc->blendDelta >= 0.0f)	{
+			float time = fallAssoc->currentTime;
 
-					if (time > 0.667f && time - fallAssoc->timeStep <= 0.667f) {
-						fallAssoc->SetCurrentTime(0.0f);
-						fallAssoc->SetRun();
-					}
-				}
+			if (time > 0.667f && time - fallAssoc->timeStep <= 0.667f) {
+				fallAssoc->SetCurrentTime(0.0f);
+				fallAssoc->SetRun();
 			}
-		} else if (firstPartialAssoc->flags & ASSOC_FRONTAL) {
-			CAnimManager::BlendAnimation(GetClump(), ASSOCGRP_STD, ANIM_FALL_FRONT, 8.0f);
-		} else {
-			CAnimManager::BlendAnimation(GetClump(), ASSOCGRP_STD, ANIM_FALL_BACK, 8.0f);
 		}
 	} else if ((bKnockedUpIntoAir || bKnockedOffBike) && bIsStanding && !bWasStanding) {
 		fallAssoc = RpAnimBlendClumpGetAssociation(GetClump(), ANIM_FALL_BACK);
@@ -9088,9 +9037,7 @@ CPed::Seek(void)
 CVector*
 CPed::SeekFollowingPath(void)
 {
-	// unused
-	if (!vecNextPathNodeInitialized)
-		vecNextPathNodeInitialized = true;
+	static CVector vecNextPathNode;
 
 	if (m_nCurPathNodeId >= m_nNumPathNodes || m_nNumPathNodes == 0)
 		return nil;
@@ -9317,18 +9264,15 @@ CPed::FollowPath(void)
 CVector
 CPed::GetFormationPosition(void)
 {
-	CPed *referencePed = m_pedInObjective;
-
-	if (!referencePed)
+	if (!m_pedInObjective)
 		return GetPosition();
 
-	if (referencePed->m_nPedState == PED_DEAD) {
-		CPed *referencePedOfReference = referencePed->m_pedInObjective;
-		if (!referencePedOfReference) {
+	if (m_pedInObjective->m_nPedState == PED_DEAD) {
+		if (!m_pedInObjective->m_pedInObjective) {
 			m_pedInObjective = nil;
 			return GetPosition();
 		}
-		m_pedInObjective = referencePed = referencePedOfReference;
+		m_pedInObjective = m_pedInObjective->m_pedInObjective;
 	}
 
 	CVector formationOffset;
@@ -9388,8 +9332,7 @@ CPed::GetNearestDoor(CVehicle *veh, CVector &posToOpen)
 						&& m_pedStats->m_temper > 65
 						&& !m_vehEnterType || m_vehEnterType == CAR_WINDSCREEN)) {
 					m_vehEnterType = CAR_WINDSCREEN;
-					CVector windscreenPos = GetPositionToOpenCarDoor(veh, CAR_WINDSCREEN);
-					posToOpen = windscreenPos;
+					posToOpen = GetPositionToOpenCarDoor(veh, CAR_WINDSCREEN);
 					return;
 				}
 			}
@@ -9401,41 +9344,37 @@ CPed::GetNearestDoor(CVehicle *veh, CVector &posToOpen)
 	CVector lfPos = GetPositionToOpenCarDoor(veh, CAR_DOOR_LF);
 	CVector rfPos = GetPositionToOpenCarDoor(veh, CAR_DOOR_RF);
 
-	// Right front door is closer
-	if ((lfPos - GetPosition()).MagnitudeSqr2D() >= (rfPos - GetPosition()).MagnitudeSqr2D()) {
+	// Left front door is closer
+	if ((lfPos - GetPosition()).MagnitudeSqr2D() < (rfPos - GetPosition()).MagnitudeSqr2D()) {
+
+		if (veh->IsRoomForPedToLeaveCar(CAR_DOOR_LF, enterOffset)) {
+			m_vehEnterType = CAR_DOOR_LF;
+			posToOpen = lfPos;
+		} else if (veh->IsRoomForPedToLeaveCar(CAR_DOOR_RF, enterOffset)) {
+			m_vehEnterType = CAR_DOOR_RF;
+			posToOpen = rfPos;
+		}
+	} else {
 
 		if (veh->IsRoomForPedToLeaveCar(CAR_DOOR_RF, enterOffset)) {
 
 			CPed *rfPassenger = veh->pPassengers[0];
-			if (!rfPassenger || veh->IsBike()
-				|| rfPassenger->m_leader != this && !rfPassenger->bDontDragMeOutCar && (veh->VehicleCreatedBy != MISSION_VEHICLE || m_objective != OBJECTIVE_ENTER_CAR_AS_DRIVER)
-				|| veh->IsRoomForPedToLeaveCar(CAR_DOOR_LF, enterOffset) == 0) {
-
-				if ((veh->m_nGettingInFlags & CAR_DOOR_FLAG_RF) == 0
-					|| veh->IsRoomForPedToLeaveCar(CAR_DOOR_LF, enterOffset) == 0) {
-					m_vehEnterType = CAR_DOOR_RF;
-					posToOpen = rfPos;
-					return;
-				}
+			if (rfPassenger && !veh->IsBike()
+					&& (rfPassenger->m_leader == this || rfPassenger->bDontDragMeOutCar ||
+						veh->VehicleCreatedBy == MISSION_VEHICLE && m_objective == OBJECTIVE_ENTER_CAR_AS_DRIVER)
+					&& veh->IsRoomForPedToLeaveCar(CAR_DOOR_LF, enterOffset)
+				|| (veh->m_nGettingInFlags & CAR_DOOR_FLAG_RF) && veh->IsRoomForPedToLeaveCar(CAR_DOOR_LF, enterOffset)) {
+
+				m_vehEnterType = CAR_DOOR_LF;
+				posToOpen = lfPos;
+			} else {
+				m_vehEnterType = CAR_DOOR_RF;
+				posToOpen = rfPos;
 			}
-		} else {
-			if (!veh->IsRoomForPedToLeaveCar(CAR_DOOR_LF, enterOffset))
-				return;
+		} else if (veh->IsRoomForPedToLeaveCar(CAR_DOOR_LF, enterOffset)) {
+			m_vehEnterType = CAR_DOOR_LF;
+			posToOpen = lfPos;
 		}
-		m_vehEnterType = CAR_DOOR_LF;
-		posToOpen = lfPos;
-		return;
-	}
-
-	if (veh->IsRoomForPedToLeaveCar(CAR_DOOR_LF, enterOffset)) {
-		m_vehEnterType = CAR_DOOR_LF;
-		posToOpen = lfPos;
-		return;
-	}
-
-	if (veh->IsRoomForPedToLeaveCar(CAR_DOOR_RF, enterOffset)) {
-		m_vehEnterType = CAR_DOOR_RF;
-		posToOpen = rfPos;
 	}
 }
 
@@ -9579,11 +9518,11 @@ CPed::GoToNearestDoor(CVehicle *veh)
 bool
 CPed::HaveReachedNextPointOnRoute(float distToCountReached)
 {
-	if ((m_nextRoutePointPos - GetPosition()).Magnitude2D() >= distToCountReached)
-		return false;
-
-	m_routePointsPassed += m_routePointsBeingPassed;
-	return true;
+	if ((m_nextRoutePointPos - GetPosition()).Magnitude2D() < distToCountReached) {
+		m_routePointsPassed += m_routePointsBeingPassed;
+		return true;
+	}
+	return false;
 }
 
 // --MIAMI: Done
@@ -10714,7 +10653,7 @@ CPed::MoveHeadToLook(void)
 
 	if (m_pLookTarget) {
 		if (m_pLookTarget->IsPed()) {
-			((CPed*)m_pLookTarget)->m_pedIK.GetComponentPosition(*(RwV3d *)&lookPos, PED_MID);
+			((CPed*)m_pLookTarget)->m_pedIK.GetComponentPosition(lookPos, PED_MID);
 		} else {
 			lookPos = m_pLookTarget->GetPosition();
 		}
@@ -10735,34 +10674,28 @@ CPed::MoveHeadToLook(void)
 		if (m_lookTimer - CTimer::GetTimeInMilliseconds() >= 1000)
 			return;
 
-		bool notRocketLauncher = false;
-		bool notTwoHanded = false;
+		bool handFreeToMove = false;
 		AnimationId animToPlay = NUM_STD_ANIMS;
 
-		if (!GetWeapon()->IsType2Handed())
-			notTwoHanded = true;
+		if (!GetWeapon()->IsType2Handed() && GetWeapon()->m_eWeaponType != WEAPONTYPE_ROCKETLAUNCHER)
+			handFreeToMove = true;
 
-		if (notTwoHanded && GetWeapon()->m_eWeaponType != WEAPONTYPE_ROCKETLAUNCHER)
-			notRocketLauncher = true;
-
-		if (IsPlayer() && notRocketLauncher) {
+		if (IsPlayer() && handFreeToMove) {
 
 			if (m_pLookTarget->IsPed()) {
-
-				if (m_pedStats->m_temper >= 49 && ((CPed*)m_pLookTarget)->m_nPedType != PEDTYPE_COP) {
-
-					// FIX: Unreachable and meaningless condition
-#ifndef FIX_BUGS
-					if (m_pedStats->m_temper < 47)
+#ifdef FIX_BUGS
+				if (m_pedStats->m_temper > 49 || ((CPed*)m_pLookTarget)->m_nPedType == PEDTYPE_COP)
+#else
+				if (m_pedStats->m_temper < 49 || ((CPed*)m_pLookTarget)->m_nPedType == PEDTYPE_COP)
 #endif
-						animToPlay = ANIM_FIGHT_PPUNCH;
-				} else {
 					animToPlay = ANIM_FUCKU;
-				}
-			} else if (m_pedStats->m_temper > 49 || m_pLookTarget->GetModelIndex() == MI_POLICE) {
-				animToPlay = ANIM_FUCKU;
+				else if(m_pedStats->m_temper < 47)
+					animToPlay = ANIM_FIGHT_PPUNCH;
+			} else {
+				if (m_pedStats->m_temper > 49 || m_pLookTarget->GetModelIndex() == MI_POLICE)
+					animToPlay = ANIM_FUCKU;
 			}
-		} else if (notRocketLauncher && (CGeneral::GetRandomNumber() & 1)) {
+		} else if (handFreeToMove && (CGeneral::GetRandomNumber() & 1)) {
 			animToPlay = ANIM_FUCKU;
 		}
 
@@ -10775,19 +10708,11 @@ CPed::MoveHeadToLook(void)
 			}
 		}
 		bShakeFist = false;
-		return;
-	}
-
-	if (999999.0f == m_fLookDirection) {
+	} else if (m_fLookDirection == 999999.0f) {
 		ClearLookFlag();
-		return;
-	}
-
-	if (!m_pedIK.LookInDirection(m_fLookDirection, 0.0f)) {
-		if (!bKeepTryingToLook) {
+	} else if (!m_pedIK.LookInDirection(m_fLookDirection, 0.0f)) {
+		if (!bKeepTryingToLook)
 			ClearLookFlag();
-			return;
-		}
 	}
 }
 
@@ -12020,8 +11945,7 @@ CPed::ProcessControl(void)
 					break;
 				}
 				case PED_FLEE_POS:
-					ms_vec2DFleePosition.x = m_fleeFromPosX;
-					ms_vec2DFleePosition.y = m_fleeFromPosY;
+					ms_vec2DFleePosition = m_fleeFromPos;
 					Flee();
 					break;
 				case PED_FLEE_ENTITY:
@@ -12083,8 +12007,7 @@ CPed::ProcessControl(void)
 							if (m_fleeFrom) {
 								ms_vec2DFleePosition = m_fleeFrom->GetPosition();
 							} else {
-								ms_vec2DFleePosition.x = m_fleeFromPosX;
-								ms_vec2DFleePosition.y = m_fleeFromPosY;
+								ms_vec2DFleePosition = m_fleeFromPos;
 							}
 							Flee();
 						} else {
@@ -12333,7 +12256,7 @@ CPed::PedAnimDoorCloseCB(CAnimBlendAssociation *animAssoc, void *arg)
 			default: assert(0);
 		}
 
-		if (veh->Damage.GetDoorStatus(door) == DOOR_STATUS_SMASHED)
+		if (veh->Damage.GetDoorStatus(door) == DOOR_STATUS_SWINGING)
 			veh->Damage.SetDoorStatus(door, DOOR_STATUS_OK);
 
 		if (door == DOOR_FRONT_LEFT || ped->m_objective == OBJECTIVE_ENTER_CAR_AS_PASSENGER || veh->bIsBus || veh->m_nNumMaxPassengers == 0) {
@@ -12865,26 +12788,26 @@ CPed::PedAnimPullPedOutCB(CAnimBlendAssociation* animAssoc, void* arg)
 		}
 
 		if (ped->m_objective == OBJECTIVE_ENTER_CAR_AS_DRIVER) {
-			if (ped->m_vehEnterType != CAR_DOOR_LF && ped->m_vehEnterType != CAR_DOOR_LR) {
+			if (ped->m_vehEnterType == CAR_DOOR_LF || ped->m_vehEnterType == CAR_DOOR_LR) {
+				if (veh->IsBike())
+					ped->m_pVehicleAnim = CAnimManager::AddAnimation(ped->GetClump(), ((CBike*)veh)->m_bikeAnimType, ANIM_BIKE_JUMPON_R);
+				else if (isLow)
+					ped->m_pVehicleAnim = CAnimManager::AddAnimation(ped->GetClump(), ASSOCGRP_STD, ANIM_CAR_GETIN_LOW_LHS);
+				else
+					ped->m_pVehicleAnim = CAnimManager::AddAnimation(ped->GetClump(), ASSOCGRP_STD, ANIM_CAR_GETIN_LHS);
+			} else {
 				if (veh->IsBike())
 					ped->m_pVehicleAnim = CAnimManager::AddAnimation(ped->GetClump(), ((CBike*)veh)->m_bikeAnimType, ANIM_BIKE_JUMPON_L);
 				else if (isLow)
 					ped->m_pVehicleAnim = CAnimManager::AddAnimation(ped->GetClump(), ASSOCGRP_STD, ANIM_CAR_GETIN_LOW_RHS);
 				else
 					ped->m_pVehicleAnim = CAnimManager::AddAnimation(ped->GetClump(), ASSOCGRP_STD, ANIM_CAR_GETIN_RHS);
-
-			} else if (veh->IsBike()) {
-				ped->m_pVehicleAnim = CAnimManager::AddAnimation(ped->GetClump(), ((CBike*)veh)->m_bikeAnimType, ANIM_BIKE_JUMPON_R);
-			} else if (isLow) {
-				ped->m_pVehicleAnim = CAnimManager::AddAnimation(ped->GetClump(), ASSOCGRP_STD, ANIM_CAR_GETIN_LOW_LHS);
-			} else {
-				ped->m_pVehicleAnim = CAnimManager::AddAnimation(ped->GetClump(), ASSOCGRP_STD, ANIM_CAR_GETIN_LHS);
 			}
 			ped->m_pVehicleAnim->SetFinishCallback(PedAnimGetInCB, ped);
 		} else {
 			ped->QuitEnteringCar();
 		}
-	} else {
+	} else if(ped->m_nPedState != PED_DRIVING) {
 		ped->QuitEnteringCar();
 	}
 }
@@ -12947,9 +12870,7 @@ CPed::PedAnimStepOutCarCB(CAnimBlendAssociation* animAssoc, void* arg)
 		default:
 			break;
 	}
-	bool closeDoor = false;
-	if (!veh->IsDoorMissing(door))
-		closeDoor = true;
+	bool closeDoor = !veh->IsDoorMissing(door);
 
 	int padNo;
 	if (ped->IsPlayer()) {
@@ -12970,10 +12891,7 @@ CPed::PedAnimStepOutCarCB(CAnimBlendAssociation* animAssoc, void* arg)
 				break;
 		}
 		CPad* pad = CPad::GetPad(padNo);
-		bool engineIsIntact = false;
-		if (veh->IsCar() && ((CAutomobile*)veh)->Damage.GetEngineStatus() >= 225) {
-			engineIsIntact = true;
-		}
+		bool engineIsIntact = veh->IsCar() && ((CAutomobile*)veh)->Damage.GetEngineStatus() >= 225;
 		if (!pad->ArePlayerControlsDisabled() && veh->m_nDoorLock != CARLOCK_FORCE_SHUT_DOORS
 			&& (pad->GetTarget()
 				|| pad->NewState.LeftStickX
@@ -13036,10 +12954,8 @@ CPed::PedEvadeCB(CAnimBlendAssociation* animAssoc, void* arg)
 
 	if (!animAssoc) {
 		ped->ClearLookFlag();
-		if (ped->m_nPedState != PED_DIVE_AWAY && ped->m_nPedState != PED_STEP_AWAY)
-			return;
-
-		ped->RestorePreviousState();
+		if (ped->m_nPedState == PED_DIVE_AWAY || ped->m_nPedState == PED_STEP_AWAY)
+			ped->RestorePreviousState();
 
 	} else if (animAssoc->animId == ANIM_EV_DIVE) {
 		ped->bUpdateAnimHeading = true;
@@ -13053,10 +12969,8 @@ CPed::PedEvadeCB(CAnimBlendAssociation* animAssoc, void* arg)
 
 	} else if (animAssoc->flags & ASSOC_FADEOUTWHENDONE) {
 		ped->ClearLookFlag();
-		if (ped->m_nPedState != PED_DIVE_AWAY && ped->m_nPedState != PED_STEP_AWAY)
-			return;
-
-		ped->RestorePreviousState();
+		if (ped->m_nPedState == PED_DIVE_AWAY || ped->m_nPedState == PED_STEP_AWAY)
+			ped->RestorePreviousState();
 
 	} else if (ped->m_nPedState != PED_ARRESTED) {
 		animAssoc->flags |= ASSOC_DELETEFADEDOUT;
@@ -13100,9 +13014,9 @@ CPed::PedGetupCB(CAnimBlendAssociation* animAssoc, void* arg)
 			ped->SetMoveState(PEDMOVE_STILL);
 		else
 			ped->SetMoveState(PEDMOVE_RUN);
+		ped->SetMoveAnim();
 	}
 
-	ped->SetMoveAnim();
 	ped->bGetUpAnimStarted = false;
 }
 
@@ -13546,14 +13460,12 @@ CPed::RegisterThreatWithGangPeds(CEntity *attacker)
 		if (m_objective != OBJECTIVE_KILL_CHAR_ON_FOOT && m_objective != OBJECTIVE_KILL_CHAR_ANY_MEANS) {
 			if (attacker->IsPed()) {
 				attackerPed = (CPed*)attacker;
-			} else {
-				if (!attacker->IsVehicle())
-					return;
-
+			} else if (attacker->IsVehicle()) {
 				attackerPed = ((CVehicle*)attacker)->pDriver;
 				if (!attackerPed)
 					return;
-			}
+			} else
+				return;
 
 			if (attackerPed && (attackerPed->IsPlayer() || attackerPed->IsGangMember())) {
 				for (int i = 0; i < m_numNearPeds; ++i) {
@@ -14408,41 +14320,47 @@ CVector vecTestTemp(-1.0f, -1.0f, -1.0f);
 void
 CPed::Render(void)
 {
-	if (!bInVehicle || !m_pMyVehicle || m_nPedState == PED_EXIT_CAR || m_nPedState == PED_DRAG_FROM_CAR ||
-		bRenderPedInCar && (m_pMyVehicle->IsBike() || IsPlayer() ||
-		sq((m_pMyVehicle->IsBoat() ? 40.0f : 25.0f) * TheCamera.LODDistMultiplier) >= (TheCamera.GetPosition() - GetPosition()).MagnitudeSqr())) {
+	if (bInVehicle && m_pMyVehicle && m_nPedState != PED_EXIT_CAR && m_nPedState != PED_DRAG_FROM_CAR) {
+		if (!bRenderPedInCar)
+			return;
 
-		CEntity::Render();
+		if (!m_pMyVehicle->IsBike() && !IsPlayer()) {
+			float camDistSq = (TheCamera.GetPosition() - GetPosition()).MagnitudeSqr();
+			if (camDistSq > SQR((m_pMyVehicle->IsBoat() ? 40.0f : 25.0f) * TheCamera.LODDistMultiplier))
+				return;
+		}
+	}
 
-		if(m_pWeaponModel){
-			RpHAnimHierarchy *hier = GetAnimHierarchyFromSkinClump(GetClump());
-			int idx = RpHAnimIDGetIndex(hier, m_pFrames[PED_HANDR]->nodeID);
-			RwMatrix *mat = &RpHAnimHierarchyGetMatrixArray(hier)[idx];
-			RwFrame *frame = RpAtomicGetFrame(m_pWeaponModel);
-			*RwFrameGetMatrix(frame) = *mat;
-			RwFrameUpdateObjects(frame);
-			RpAtomicRender(m_pWeaponModel);
-			if (IsPlayer()) {
-				CPlayerPed *player = (CPlayerPed*)this;
-				if (player->m_pMinigunTopAtomic) {
-					frame = RpAtomicGetFrame(player->m_pMinigunTopAtomic);
-					*RwFrameGetMatrix(frame) = *mat;
+	CEntity::Render();
 
-					player->m_fGunSpinAngle = player->m_fGunSpinSpeed * CTimer::GetTimeStep() + player->m_fGunSpinAngle;
-					if (player->m_fGunSpinAngle > TWOPI)
-						player->m_fGunSpinAngle -= TWOPI;
+	if(m_pWeaponModel){
+		RpHAnimHierarchy *hier = GetAnimHierarchyFromSkinClump(GetClump());
+		int idx = RpHAnimIDGetIndex(hier, m_pFrames[PED_HANDR]->nodeID);
+		RwMatrix *mat = &RpHAnimHierarchyGetMatrixArray(hier)[idx];
+		RwFrame *frame = RpAtomicGetFrame(m_pWeaponModel);
+		*RwFrameGetMatrix(frame) = *mat;
+		RwFrameUpdateObjects(frame);
+		RpAtomicRender(m_pWeaponModel);
+		if (IsPlayer()) {
+			CPlayerPed *player = (CPlayerPed*)this;
+			if (player->m_pMinigunTopAtomic) {
+				frame = RpAtomicGetFrame(player->m_pMinigunTopAtomic);
+				*RwFrameGetMatrix(frame) = *mat;
 
-					CMatrix mgTopMat, localAdjMat;
-					mgTopMat.Attach(RwFrameGetMatrix(frame));
-					localAdjMat.SetRotateX(player->m_fGunSpinAngle);
-					localAdjMat.Rotate(DEGTORAD(-4.477f)* vecTestTemp.x, DEGTORAD(29.731f) * vecTestTemp.y, DEGTORAD(1.064f) * vecTestTemp.z);
-					localAdjMat.GetPosition() += CVector(0.829f, -0.001f, 0.226f);
-					mgTopMat = mgTopMat * localAdjMat;
-					mgTopMat.UpdateRW();
+				player->m_fGunSpinAngle = player->m_fGunSpinSpeed * CTimer::GetTimeStep() + player->m_fGunSpinAngle;
+				if (player->m_fGunSpinAngle > TWOPI)
+					player->m_fGunSpinAngle -= TWOPI;
 
-					RwFrameUpdateObjects(frame);
-					RpAtomicRender(player->m_pMinigunTopAtomic);
-				}
+				CMatrix mgTopMat, localAdjMat;
+				mgTopMat.Attach(RwFrameGetMatrix(frame));
+				localAdjMat.SetRotateX(player->m_fGunSpinAngle);
+				localAdjMat.Rotate(DEGTORAD(-4.477f)* vecTestTemp.x, DEGTORAD(29.731f) * vecTestTemp.y, DEGTORAD(1.064f) * vecTestTemp.z);
+				localAdjMat.GetPosition() += CVector(0.829f, -0.001f, 0.226f);
+				mgTopMat = mgTopMat * localAdjMat;
+				mgTopMat.UpdateRW();
+
+				RwFrameUpdateObjects(frame);
+				RpAtomicRender(player->m_pMinigunTopAtomic);
 			}
 		}
 	}
@@ -16701,7 +16619,7 @@ CPed::SetRadioStation(void)
 
 	if (GetModelIndex() != MI_PGA && GetModelIndex() != MI_PGB) {
 		if (m_pMyVehicle->m_nRadioStation != modelInfo->radio1 && m_pMyVehicle->m_nRadioStation != modelInfo->radio2) {
-			if (CGeneral::GetRandomNumber() < MYRAND_MAX / 2)
+			if (CGeneral::GetRandomTrueFalse())
 				m_pMyVehicle->m_nRadioStation = modelInfo->radio1;
 			else
 				m_pMyVehicle->m_nRadioStation = modelInfo->radio2;
@@ -17200,25 +17118,18 @@ CPed::SetFollowPath(CVector dest, float radius, eMoveState state, CEntity* walkA
 	bool weHaveTargetPed = targetEnt && targetEnt->IsPed();
 	bool useDestVec = !weHaveTargetPed;
 
-	CVector targetPos;
 	if (useDestVec)
-		targetPos = dest;
+		m_followPathDestPos = dest;
 	else
-		targetPos = targetEnt->GetPosition();
+		m_followPathDestPos = targetEnt->GetPosition();
 
-	m_followPathDestPos = targetPos;
 	if (targetEnt && m_nPedState == PED_SEEK_POS) {
 		m_followPathDestPos = m_vecSeekPos;
 	}
 
-	float newRadius = radius > 0.f ? radius : 20.f;
-	bool useGivenPedMove = true;
-
-	m_followPathAbortDist = newRadius;
-
-	if (state != PEDMOVE_RUN && state != PEDMOVE_WALK)
-		useGivenPedMove = false;
+	m_followPathAbortDist = radius > 0.f ? radius : 20.f;
 
+	bool useGivenPedMove = state == PEDMOVE_RUN || state == PEDMOVE_WALK;
 	if (useGivenPedMove)
 		m_followPathMoveState = state;
 	else
@@ -19468,18 +19379,10 @@ CPed::SetEnterCar_AllClear(CVehicle *car, uint32 doorNode, uint32 doorFlag)
 		car->AutoPilot.m_nCruiseSpeed = 0;
 
 	} else {
-		if (zDiff > 4.4f) {
-			if (m_vehEnterType == CAR_DOOR_RF || m_vehEnterType == CAR_DOOR_RR)
-				m_pVehicleAnim = CAnimManager::BlendAnimation(GetClump(), ASSOCGRP_STD, ANIM_CAR_ALIGNHI_RHS, 4.0f);
-			else
-				m_pVehicleAnim = CAnimManager::BlendAnimation(GetClump(), ASSOCGRP_STD, ANIM_CAR_ALIGNHI_LHS, 4.0f);
-
-		} else {
-			if (m_vehEnterType == CAR_DOOR_RF || m_vehEnterType == CAR_DOOR_RR)
-				m_pVehicleAnim = CAnimManager::BlendAnimation(GetClump(), ASSOCGRP_STD, ANIM_CAR_ALIGN_RHS, 4.0f);
-			else
-				m_pVehicleAnim = CAnimManager::BlendAnimation(GetClump(), ASSOCGRP_STD, ANIM_CAR_ALIGN_LHS, 4.0f);
-		}
+		if (m_vehEnterType == CAR_DOOR_LF || m_vehEnterType == CAR_DOOR_LR)
+			m_pVehicleAnim = CAnimManager::BlendAnimation(GetClump(), ASSOCGRP_STD, zDiff > 4.4f ? ANIM_CAR_ALIGNHI_RHS : ANIM_CAR_ALIGN_RHS, 4.0f);
+		else
+			m_pVehicleAnim = CAnimManager::BlendAnimation(GetClump(), ASSOCGRP_STD, zDiff > 4.4f ? ANIM_CAR_ALIGNHI_LHS : ANIM_CAR_ALIGN_LHS, 4.0f);
 		m_pVehicleAnim->SetFinishCallback(PedAnimAlignCB, this);
 	}
 }
@@ -19607,21 +19510,20 @@ CPed::WarpPedToNearEntityOffScreen(CEntity *warpTo)
 	CVector halfNormalizedDist = distVec / halfOfDist;
 
 	CVector appropriatePos = GetPosition();
-	CVector zCorrectedPos = appropriatePos;
-	int tryCount = Min(10, halfOfDist);
+	int tryCount = Min(10, (int)halfOfDist);
 	for (int i = 0; i < tryCount; ++i) {
 		appropriatePos += halfNormalizedDist;
+		CVector zCorrectedPos = appropriatePos;
 		CPedPlacement::FindZCoorForPed(&zCorrectedPos);
 
-		if (Abs(zCorrectedPos.z - warpToPos.z) >= 3.0f && Abs(zCorrectedPos.z - appropriatePos.z) >= 3.0f)
-			continue;
-
-		appropriatePos.z = zCorrectedPos.z;
-		if (!TheCamera.IsSphereVisible(appropriatePos, 0.6f, &TheCamera.GetCameraMatrix())
-			&& CWorld::GetIsLineOfSightClear(appropriatePos, warpToPos, true, true, false, true, false, false, false)
-			&& !CWorld::TestSphereAgainstWorld(appropriatePos, 0.6f, this, true, true, false, true, false, false)) {
-			teleported = true;
-			Teleport(appropriatePos);
+		if (Abs(zCorrectedPos.z - warpToPos.z) < 3.0f || Abs(zCorrectedPos.z - appropriatePos.z) < 3.0f) {
+			appropriatePos.z = zCorrectedPos.z;
+			if (!TheCamera.IsSphereVisible(appropriatePos, 0.6f, &TheCamera.GetCameraMatrix())
+				&& CWorld::GetIsLineOfSightClear(appropriatePos, warpToPos, true, true, false, true, false, false, false)
+				&& !CWorld::TestSphereAgainstWorld(appropriatePos, 0.6f, this, true, true, false, true, false, false)) {
+				teleported = true;
+				Teleport(appropriatePos);
+			}
 		}
 	}
 	m_leaveCarTimer = CTimer::GetTimeInMilliseconds() + 3000;
@@ -19642,21 +19544,20 @@ CPed::WarpPedToNearLeaderOffScreen(void)
 	CVector halfNormalizedDist = distVec / halfOfDist;
 
 	CVector appropriatePos = GetPosition();
-	CVector zCorrectedPos = appropriatePos;
-	int tryCount = Min(10, halfOfDist);
+	int tryCount = Min(10, (int)halfOfDist);
 	for (int i = 0; i < tryCount; ++i) {
 		appropriatePos += halfNormalizedDist;
+		CVector zCorrectedPos = appropriatePos;
 		CPedPlacement::FindZCoorForPed(&zCorrectedPos);
 
-		if (Abs(zCorrectedPos.z - warpToPos.z) >= 3.0f && Abs(zCorrectedPos.z - appropriatePos.z) >= 3.0f)
-			continue;
-
-		appropriatePos.z = zCorrectedPos.z;
-		if (!TheCamera.IsSphereVisible(appropriatePos, 0.6f, &TheCamera.GetCameraMatrix())
-			&& CWorld::GetIsLineOfSightClear(appropriatePos, warpToPos, true, true, false, true, false, false, false)
-			&& !CWorld::TestSphereAgainstWorld(appropriatePos, 0.6f, this, true, true, false, true, false, false)) {
-			teleported = true;
-			Teleport(appropriatePos);
+		if (Abs(zCorrectedPos.z - warpToPos.z) < 3.0f || Abs(zCorrectedPos.z - appropriatePos.z) < 3.0f) {
+			appropriatePos.z = zCorrectedPos.z;
+			if (!TheCamera.IsSphereVisible(appropriatePos, 0.6f)
+				&& CWorld::GetIsLineOfSightClear(appropriatePos, warpToPos, true, true, false, true, false, false, false)
+				&& !CWorld::TestSphereAgainstWorld(appropriatePos, 0.6f, this, true, true, false, true, false, false)) {
+				teleported = true;
+				Teleport(appropriatePos);
+			}
 		}
 	}
 	m_leaveCarTimer = CTimer::GetTimeInMilliseconds() + 3000;
@@ -19697,19 +19598,10 @@ CPed::SetCarJack_AllClear(CVehicle* car, uint32 doorNode, uint32 doorFlag)
 		float zDiff = Max(0.0f, carEnterPos.z - GetPosition().z);
 		bUsesCollision = false;
 
-		if (zDiff > 4.4f) {
-			if (m_vehEnterType == CAR_DOOR_RF || m_vehEnterType == CAR_DOOR_RR)
-				m_pVehicleAnim = CAnimManager::BlendAnimation(GetClump(), ASSOCGRP_STD, ANIM_CAR_ALIGNHI_RHS, 4.0f);
-			else
-				m_pVehicleAnim = CAnimManager::BlendAnimation(GetClump(), ASSOCGRP_STD, ANIM_CAR_ALIGNHI_LHS, 4.0f);
-
-		}
-		else {
-			if (m_vehEnterType == CAR_DOOR_RF || m_vehEnterType == CAR_DOOR_RR)
-				m_pVehicleAnim = CAnimManager::BlendAnimation(GetClump(), ASSOCGRP_STD, ANIM_CAR_ALIGN_RHS, 4.0f);
-			else
-				m_pVehicleAnim = CAnimManager::BlendAnimation(GetClump(), ASSOCGRP_STD, ANIM_CAR_ALIGN_LHS, 4.0f);
-		}
+		if (m_vehEnterType == CAR_DOOR_LF || m_vehEnterType == CAR_DOOR_LR)
+			m_pVehicleAnim = CAnimManager::BlendAnimation(GetClump(), ASSOCGRP_STD, zDiff > 4.4f ? ANIM_CAR_ALIGNHI_LHS : ANIM_CAR_ALIGN_LHS, 4.0f);
+		else
+			m_pVehicleAnim = CAnimManager::BlendAnimation(GetClump(), ASSOCGRP_STD, zDiff > 4.4f ? ANIM_CAR_ALIGNHI_RHS : ANIM_CAR_ALIGN_RHS, 4.0f);
 
 		m_pVehicleAnim->SetFinishCallback(PedAnimAlignCB, this);
 	}
@@ -19790,12 +19682,12 @@ CPed::SetCarJack(CVehicle* car)
 		pedInSeat = car->pDriver;
 
 	if (m_fHealth > 0.0f && (IsPlayer() || m_objective == OBJECTIVE_KILL_CHAR_ON_FOOT || m_objective == OBJECTIVE_KILL_CHAR_ANY_MEANS ||
-							 (car->VehicleCreatedBy != MISSION_VEHICLE && car->GetModelIndex() != MI_DODO)))
-			if (pedInSeat && !pedInSeat->IsPedDoingDriveByShooting() && pedInSeat->m_nPedState == PED_DRIVING)
-				if (m_nPedState != PED_CARJACK && !m_pVehicleAnim)
-					if ((car->IsDoorReady(door) || car->IsDoorFullyOpen(door)))
-						if (!car->bIsBeingCarJacked && !(doorFlag & car->m_nGettingInFlags) && !(doorFlag & car->m_nGettingOutFlags))
-							SetCarJack_AllClear(car, m_vehEnterType, doorFlag);
+					CharCreatedBy == MISSION_CHAR ||  (car->VehicleCreatedBy != MISSION_VEHICLE && car->GetModelIndex() != MI_DODO)))
+		if (pedInSeat && !pedInSeat->IsPedDoingDriveByShooting() && pedInSeat->m_nPedState == PED_DRIVING)
+			if (m_nPedState != PED_CARJACK && !m_pVehicleAnim)
+				if ((car->IsDoorReady(door) || car->IsDoorFullyOpen(door)))
+					if (!car->bIsBeingCarJacked && !(doorFlag & car->m_nGettingInFlags) && !(doorFlag & car->m_nGettingOutFlags))
+						SetCarJack_AllClear(car, m_vehEnterType, doorFlag);
 }
 
 // --MIAMI: Done
@@ -19816,9 +19708,9 @@ CPed::Solicit(void)
 			GetPosition().x, GetPosition().y);
 
 		if (m_fRotationDest < 0.0f) {
-			m_fRotationDest = m_fRotationDest + TWOPI;
+			m_fRotationDest += TWOPI;
 		} else if (m_fRotationDest > TWOPI) {
-			m_fRotationDest = m_fRotationDest - TWOPI;
+			m_fRotationDest -= TWOPI;
 		}
 
 		if ((GetPosition() - doorPos).MagnitudeSqr() <= 1.0f)
@@ -19863,39 +19755,39 @@ CPed::SetExitBoat(CVehicle *boat)
 		m_pCurSurface->RegisterReference((CEntity**)&m_pCurSurface);
 		m_pCurrentPhysSurface = boat;
 	} else {
-		if (boat->m_modelIndex != MI_SKIMMER || boat->bIsInWater) {
-			if (boat->m_modelIndex == MI_SKIMMER)
-				newPos.z += 2.0f;
-
-			m_vehEnterType = CAR_DOOR_RF;
-			PedSetOutCarCB(nil, this);
-			bIsStanding = true;
-			m_pCurSurface = boat;
-			m_pCurSurface->RegisterReference((CEntity**)&m_pCurSurface);
-			m_pCurrentPhysSurface = boat;
-			CColPoint foundCol;
-			CEntity *foundEnt = nil;
-			if (CWorld::ProcessVerticalLine(newPos, newPos.z - 1.4f, foundCol, foundEnt, false, true, false, false, false, false, nil))
-				newPos.z = FEET_OFFSET + foundCol.point.z;
-		} else {
-			m_vehEnterType = CAR_DOOR_RF;
-			PedSetOutCarCB(nil, this);
-			bIsStanding = true;
-			SetMoveState(PEDMOVE_STILL);
-			bTryingToReachDryLand = true;
-			float upMult = 1.04f + boatCol->boundingBox.min.z;
-			float rightMult = 0.6f * boatCol->boundingBox.max.x;
-			newPos = upMult * boat->GetUp() + rightMult * boat->GetRight() + boat->GetPosition();
-			SetPosition(newPos);
-			if (m_pMyVehicle) {
-				PositionPedOutOfCollision();
-			} else {
-				m_pMyVehicle = boat;
-				PositionPedOutOfCollision();
-				m_pMyVehicle = nil;
+		if (boat->m_modelIndex == MI_SKIMMER) {
+			if (!boat->bIsInWater) {
+				m_vehEnterType = CAR_DOOR_RF;
+				PedSetOutCarCB(nil, this);
+				bIsStanding = true;
+				SetMoveState(PEDMOVE_STILL);
+				bTryingToReachDryLand = true;
+				float upMult = 1.04f + boatCol->boundingBox.min.z;
+				float rightMult = 0.6f * boatCol->boundingBox.max.x;
+				newPos = upMult * boat->GetUp() + rightMult * boat->GetRight() + boat->GetPosition();
+				SetPosition(newPos);
+				if (m_pMyVehicle) {
+					PositionPedOutOfCollision();
+				} else {
+					m_pMyVehicle = boat;
+					PositionPedOutOfCollision();
+					m_pMyVehicle = nil;
+				}
+				return;
 			}
-			return;
+
+			newPos.z += 2.0f;
 		}
+		m_vehEnterType = CAR_DOOR_RF;
+		PedSetOutCarCB(nil, this);
+		bIsStanding = true;
+		m_pCurSurface = boat;
+		m_pCurSurface->RegisterReference((CEntity**)&m_pCurSurface);
+		m_pCurrentPhysSurface = boat;
+		CColPoint foundCol;
+		CEntity *foundEnt = nil;
+		if (CWorld::ProcessVerticalLine(newPos, newPos.z - 1.4f, foundCol, foundEnt, false, true, false, false, false, false, nil))
+			newPos.z = FEET_OFFSET + foundCol.point.z;
 	}
 	SetPosition(newPos);
 	SetMoveState(PEDMOVE_STILL);
@@ -20178,7 +20070,7 @@ CPed::AddInCarAnims(CVehicle* car, bool isDriver)
 bool
 CPed::CanBeDamagedByThisGangMember(CPed* who)
 {
-	return m_gangFlags & (1 << (uint8)(who->m_nPedType - PEDTYPE_GANG1));
+	return m_gangFlags & (1 << (who->m_nPedType - PEDTYPE_GANG1));
 }
 
 // --MIAMI: Done
@@ -20252,7 +20144,7 @@ CPed::ClearAnswerMobile(void)
 	if (m_nPedState == PED_ANSWER_MOBILE) {
 		m_nPedState = PED_IDLE;
 		RestorePreviousState();
-		m_pMyVehicle = nil;
+		m_pVehicleAnim = nil;
 	}
 }
 
@@ -20269,7 +20161,7 @@ CPed::Dress(void)
 	m_prevObjective = OBJECTIVE_NONE;
 	m_nWaitState = WAITSTATE_FALSE;
 	CWorld::Add(this);
-	m_headingRate = m_pedStats->m_headingChangeRate;
+	RestoreHeadingRate();
 }
 
 // --MIAMI: Done
@@ -20361,7 +20253,7 @@ CPed::DettachPedFromEntity(void)
 	m_attachedTo = nil;
 	if (m_nPedState == PED_DIE) {
 		m_pCollidingEntity = pVehicleAttachedTo;
-		ApplyMoveForce(pVehicleAttachedTo->GetForward() * -4.0f);
+		ApplyMoveForce(pVehicleAttachedTo->GetForward() * -4.0f + CVector(0.0f, 0.0f, 4.0f));
 		bIsStanding = false;
 	} else if (m_nPedState != PED_DEAD) {
 		RestorePreviousState();
@@ -20705,6 +20597,9 @@ CPed::DriveVehicle(void)
 void
 CPed::PositionAttachedPed()
 {
+	if(!m_attachedTo)
+		return;
+
 	CMatrix rotMatrix, targetMat;
 	targetMat = m_attachedTo->GetMatrix();
 	targetMat.GetPosition() += Multiply3x3(m_attachedTo->GetMatrix(), m_vecAttachOffset);
diff --git a/src/peds/Ped.h b/src/peds/Ped.h
index 00ce48f4..497994c2 100644
--- a/src/peds/Ped.h
+++ b/src/peds/Ped.h
@@ -451,9 +451,7 @@ public:
 	uint32 bVehExitWillBeInstant : 1;
 	uint32 bHasAlreadyBeenRecorded : 1;
 	uint32 bFallenDown : 1;
-#ifdef PED_SKIN
-	uint32 bDontAcceptIKLookAts : 1;	// TODO: find uses of this
-#endif
+	uint32 bDontAcceptIKLookAts : 1;
 	uint32 bReachedAttractorHeadingTarget : 1;
 	uint32 bTurnedAroundOnAttractor : 1;
 
@@ -513,10 +511,7 @@ public:
 	CEntity* m_pEventEntity;
 	float m_fAngleToEvent;
 	AnimBlendFrameData *m_pFrames[PED_NODE_MAX];
-#ifdef PED_SKIN
-	// stored inside the clump with non-skin ped
 	RpAtomic *m_pWeaponModel;
-#endif
 	AssocGroupId m_animGroup;
 	CAnimBlendAssociation *m_pVehicleAnim;
 	CVector2D m_vecAnimMoveDelta;
@@ -579,8 +574,7 @@ public:
 	CAccident *m_lastAccident;
 	uint32 m_nPedType;
 	CPedStats *m_pedStats;
-	float m_fleeFromPosX;
-	float m_fleeFromPosY;
+	CVector2D m_fleeFromPos;
 	CEntity *m_fleeFrom;
 	uint32 m_fleeTimer;
 	CEntity* m_threatEx; // TODO(Miami): What is this?
diff --git a/src/peds/PedChat.cpp b/src/peds/PedChat.cpp
index b7732274..71739902 100644
--- a/src/peds/PedChat.cpp
+++ b/src/peds/PedChat.cpp
@@ -75,28 +75,29 @@ CPed::ServiceTalkingWhenDead(void)
 void
 CPed::ServiceTalking(void)
 {
-	if (!bBodyPartJustCameOff || m_bodyPartBleeding != PED_HEAD) {
-		if (!CGame::germanGame && m_pFire)
-			m_queuedSound = SOUND_PED_BURNING;
-
-		if (m_queuedSound != SOUND_NO_SOUND) {
-			if (m_queuedSound == SOUND_PED_DEATH)
-				m_soundStart = CTimer::GetTimeInMilliseconds() - 1;
-
-			if (CTimer::GetTimeInMilliseconds() > m_soundStart) {
-				DMAudio.PlayOneShot(m_audioEntityId, m_queuedSound, 1.0f);
-				m_lastSoundStart = CTimer::GetTimeInMilliseconds();
-				m_soundStart =
-					CommentWaitTime[m_queuedSound - SOUND_PED_DEATH].m_nFixedDelayTime
-					+ CTimer::GetTimeInMilliseconds()
-					+ CGeneral::GetRandomNumberInRange(0, CommentWaitTime[m_queuedSound - SOUND_PED_DEATH].m_nOverrideFixedDelayTime);
-
-				if (m_queuedSound == SOUND_PED_PLAYER_BEFORESEX && IsPlayer())
-					m_soundStart += 2000;
-
-				m_lastQueuedSound = m_queuedSound;
-				m_queuedSound = SOUND_NO_SOUND;
-			}
+	if (bBodyPartJustCameOff && m_bodyPartBleeding == PED_HEAD)
+		return;
+
+	if (!CGame::germanGame && m_pFire)
+		m_queuedSound = SOUND_PED_BURNING;
+
+	if (m_queuedSound != SOUND_NO_SOUND) {
+		if (m_queuedSound == SOUND_PED_DEATH)
+			m_soundStart = CTimer::GetTimeInMilliseconds() - 1;
+
+		if (CTimer::GetTimeInMilliseconds() > m_soundStart) {
+			DMAudio.PlayOneShot(m_audioEntityId, m_queuedSound, 1.0f);
+			m_lastSoundStart = CTimer::GetTimeInMilliseconds();
+			m_soundStart =
+				CommentWaitTime[m_queuedSound - SOUND_PED_DEATH].m_nFixedDelayTime
+				+ CTimer::GetTimeInMilliseconds()
+				+ CGeneral::GetRandomNumberInRange(0, CommentWaitTime[m_queuedSound - SOUND_PED_DEATH].m_nOverrideFixedDelayTime);
+
+			if (m_queuedSound == SOUND_PED_PLAYER_BEFORESEX && IsPlayer())
+				m_soundStart += 2000;
+
+			m_lastQueuedSound = m_queuedSound;
+			m_queuedSound = SOUND_NO_SOUND;
 		}
 	}
 }
@@ -104,8 +105,6 @@ CPed::ServiceTalking(void)
 void
 CPed::Say(uint16 audio)
 {
-	uint16 audioToPlay = audio;
-
 	if (3.0f + TheCamera.GetPosition().z < GetPosition().z)
 		return;
 
@@ -138,19 +137,19 @@ CPed::Say(uint16 audio)
 		}
 	}
 
-	if (audioToPlay < m_queuedSound) {
-		if (audioToPlay != m_lastQueuedSound || audioToPlay == SOUND_PED_DEATH
+	if (audio < m_queuedSound) {
+		if (audio != m_lastQueuedSound || audio == SOUND_PED_DEATH
 
 		// See VC Ped Speech patch
 #ifdef FIX_BUGS
-			|| CommentWaitTime[audioToPlay - SOUND_PED_DEATH].m_nOverrideMaxRandomDelayTime
-				+ (uint32)CGeneral::GetRandomNumberInRange(0, CommentWaitTime[audioToPlay - SOUND_PED_DEATH].m_nMaxRandomDelayTime)
+			|| CommentWaitTime[audio - SOUND_PED_DEATH].m_nOverrideMaxRandomDelayTime
+				+ (uint32)CGeneral::GetRandomNumberInRange(0, CommentWaitTime[audio - SOUND_PED_DEATH].m_nMaxRandomDelayTime)
 #else
 			|| CommentWaitTime[m_queuedSound - SOUND_PED_DEATH].m_nOverrideMaxRandomDelayTime
 			+ (uint32)CGeneral::GetRandomNumberInRange(0, CommentWaitTime[m_queuedSound - SOUND_PED_DEATH].m_nMaxRandomDelayTime)
 #endif
 				+ m_lastSoundStart <= CTimer::GetTimeInMilliseconds()) {
-			m_queuedSound = audioToPlay;
+			m_queuedSound = audio;
 		}
 	}
 }
diff --git a/src/peds/PlayerPed.cpp b/src/peds/PlayerPed.cpp
index 24718b19..76599777 100644
--- a/src/peds/PlayerPed.cpp
+++ b/src/peds/PlayerPed.cpp
@@ -268,8 +268,7 @@ CPlayerPed::SetInitialState(void)
 	CTimer::SetTimeScale(1.0f);
 	m_pSeekTarget = nil;
 	m_vecSeekPos = CVector(0.0f, 0.0f, 0.0f);
-	m_fleeFromPosX = 0.0f;
-	m_fleeFromPosY = 0.0f;
+	m_fleeFromPos = CVector2D(0.0f, 0.0f);
 	m_fleeFrom = nil;
 	m_fleeTimer = 0;
 	m_objective = OBJECTIVE_NONE;