
#include "global.h"
#include "overlays.h"
#include "bff_load.h"
#include "map_draw.h"
#include "maths.h"
#include "debris.h"
#include "timer.h"

#include "route.h"
#include "enemies.h"
#include <limits.h>
#include "main.h"

// How far a route is moved out from intersection points
// Must not be zero! 
#ifdef USE_BEVEL_ROUTES
	#define ROUTE_ADJUST_AMOUNT	10
#else
	#define ROUTE_ADJUST_AMOUNT	DOG_ROUTEPOINT_MARGIN
#endif

//#define ROUTE_DEBUG_PRINTFS
//#define ROUTE_DEBUG_KEEPLINES

#ifdef ROUTE_DEBUG_PRINTFS
	int route_debug_printfs = 0;
#endif

extern TextureType *enemy_markerspr;	// used for showing the path in debug mode

ROUTEFIND_STR *route_routes = NULL;
//ROUTEFIND_STR *route_bevel_routes = NULL;


#ifdef ROUTE_DEBUG_KEEPLINES
void routeShowRoute2(ROUTEBOX_VECTOR *route, int route_len, unsigned int colour, int which);
ROUTEBOX_VECTOR first_route[40];
int first_n_points = 0;
ROUTEBOX_VECTOR second_route[40];
int second_n_points = 0;
ROUTEBOX_VECTOR third_route[40];
int third_n_points = 0;
ROUTEBOX_VECTOR fourth_route[40];
int fourth_n_points = 0;
ROUTEBOX_VECTOR fifth_route[40];
int fifth_n_points = 0;
int routeCopyRoute(VECTOR *src, VECTOR *dest, ROUTEBOX_VECTOR *to, ROUTEBOX_VECTOR *from, int n_points)
{
	to[0].vx = src->vx;
	to[0].vz = src->vz;
	memcpy(&to[1],from, n_points * sizeof(ROUTEBOX_VECTOR));
	to[n_points+1].vx = dest->vx;
	to[n_points+1].vz = dest->vz;
	return n_points+1;
}
#endif



void routefindPreBFF()
{
	route_routes = NULL;
}
void routefindLinkBFF(ROUTEFIND_STR *routes)
{
	int i;

	route_routes = routes;


// Let's convert some integers to pointers...

	routes->boxes = ADD2POINTER(routes, (int)routes->boxes);

#ifdef USE_BEVEL_BOXES
	routes->bevel_boxes = ADD2POINTER(routes, (int)routes->bevel_boxes);
#endif
	utilPrintf("Router: n_boxes total = %d\n",routes->n_boxes);

	for(i = 0; i < routes->n_boxes; i++)
	{
//		utilPrintf("Original Vector offset = %d. n_vecs = %d\n",(int)routes->boxes[i].vecs,routes->boxes[i].n_vecs);
		routes->boxes[i].vecs = ADD2POINTER(routes, (int)routes->boxes[i].vecs);
#ifdef USE_BEVEL_BOXES
		routes->bevel_boxes[i].vecs = ADD2POINTER(routes, (int)routes->bevel_boxes[i].vecs);
#endif
	}




// ========= Generate the bevel routeboxes ===========
/*
	{
		int b;
		int v;
		int x;
		int dx1,dz1,dx2,dz2;
		ROUTEBOX_VECTOR *src1,*src2,*src3;
#define ROUTEBOX_BOUNDARY (DOG_ROUTEFIND_MARGIN + 100)
		ROUTEBOX *box;
		ROUTEBOX *dest;

		route_bevel_routes = MALLOC(sizeof(ROUTEFIND_STR));
		route_bevel_routes->n_boxes = route_routes->n_boxes;
		route_bevel_routes->boxes = MALLOC(route_bevel_routes->n_boxes * sizeof(ROUTEBOX));

		box = route_routes->boxes;
		for(b = 0; b < route_routes->n_boxes; b++,box++)
		{
			dest = &route_bevel_routes->boxes[b];
			dest->vecs = MALLOC(box->n_vecs * 2 * sizeof(ROUTEBOX_VECTOR));
			dest->minx = box->minx - ROUTEBOX_BOUNDARY;
			dest->minz = box->minz - ROUTEBOX_BOUNDARY;
			dest->maxx = box->maxx + ROUTEBOX_BOUNDARY;
			dest->maxz = box->maxz + ROUTEBOX_BOUNDARY;
			dest->n_vecs = 0;
			for(v = 0; v < box->n_vecs; v++)
			{
				src1 = &box->vecs[v];
				src2 = &box->vecs[(v+1) % box->n_vecs];
				src3 = &box->vecs[(v+2) % box->n_vecs];
//acute or obtuse decision time...
				// (z compt' of cross p)
				dx1 = src2->vx - src1->vx;
				dx2 = src3->vx - src2->vx;
				dz1 = src2->vz - src1->vz;
				dz2 = src3->vz - src2->vz;
				x = dx1 * dz2 - dx2 * dz1;

				if(x > 0)
				{
// acute: one point (the isect)
					routeCalculateCornerPoint(box, (v+1)%box->n_vecs, &dest->vecs[dest->n_vecs], ROUTEBOX_BOUNDARY);
					dest->n_vecs++;
				}
				else
				{
// obtuse: two points (bevel)
					
					dest->vecs[dest->n_vecs] = *src2;
					dest->vecs[dest->n_vecs+1] = *src2;

					routeAdjustIsectPoint(box, (v)%box->n_vecs, &dest->vecs[dest->n_vecs], ROUTEBOX_BOUNDARY);
					routeAdjustIsectPoint(box, (v+1)%box->n_vecs, &dest->vecs[dest->n_vecs+1], ROUTEBOX_BOUNDARY);
					dest->n_vecs+=2;
					
				}

			}
			DB("src %d, dest %d\n",box->n_vecs,dest->n_vecs);
		}

	}
*/
}

// This is only relevant as a debug tool
void routeDraw()
{
	int i;
	int p;
	int set;
	ROUTEBOX *box;
	VECTOR v[4];
	unsigned int col;
	unsigned int polcol;

	if(!route_routes)
		return;

	if(overlay_show_collision != DEBUG_OVERLAY_ENEMY_INFO && overlay_show_collision != DEBUG_OVERLAY_ROUTE_FIND)
		return;

#ifdef ROUTE_DEBUG_KEEPLINES
	routeShowRoute2(&first_route[0],first_n_points, 0xffffff,0);
	routeShowRoute2(&second_route[0],second_n_points, 0x0000ff,1);
//	routeShowRoute2(&third_route[0],third_n_points, 0x00ff00,2);
	routeShowRoute2(&fourth_route[0],fourth_n_points, 0x808080,2);
	routeShowRoute2(&fifth_route[0],fifth_n_points, 0xff00ff,3);
#endif

#ifdef ROUTE_DEBUG_PRINTFS
	if(padData.digital[0] & PAD_L2)
	{
		route_debug_printfs = 1;
	}
	else
	{
		route_debug_printfs = 0;
	}

#endif

#ifdef USE_BEVEL_ROUTES
	for(set = 0; set < 2; set++)
	{
		if(set == 0)
		{
			polcol = 0x000080;
			box = route_routes->bevel_boxes;
		}
		else
		{
			polcol = 0x008000;
			box = route_routes->boxes;
		}

#else
	box = route_routes->boxes;
	polcol = 0x000080;
#endif
	for(i = 0; i < route_routes->n_boxes; i++,box++)
	{
		for(p = 0; p < box->n_vecs; p++)
		{
//			ROUTEBOX_VECTOR temp;
//			temp = box->vecs[p];
//			routeKeepOutOfRouteboxes(&temp.vx,&temp.vz,DOG_ROUTEPOINT_MARGIN);
///			overlayShowSphere(temp.vx, -500, temp.vz, 40, 0x00ff00);




			v[0].vx = box->vecs[p].vx;
			v[0].vy = -100;
			v[0].vz = box->vecs[p].vz;

			if(p != box->n_vecs-1)
			{
				col = 0xffff8080;
				v[1].vx = box->vecs[p+1].vx;
				v[1].vy = -100;
				v[1].vz = box->vecs[p+1].vz;
			}
			else
			{
				col = 0xffff8010;
				v[1].vx = box->vecs[0].vx;
				v[1].vy = -100;
				v[1].vz = box->vecs[0].vz;
			}

			v[2].vx = v[1].vz - v[0].vz;
			v[2].vy = 0;
			v[2].vz = v[0].vx - v[1].vx;

			MakeUnit(&v[2]);

			v[2].vx = v[2].vx >> 3;
			v[2].vz = v[2].vz >> 3;


			v[3].vx = v[0].vx + v[2].vx;
			v[3].vy = -100;
			v[3].vz = v[0].vz + v[2].vz;

			v[2].vx = v[1].vx + v[2].vx;
			v[2].vy = -100;
			v[2].vz = v[1].vz + v[2].vz;




			overlayShowDebugLine(
				&v[0],
				&v[1],
				col,
				DEBUGPOLY_SUBDIV);	//+DEBUGPOLY_USEDEPTH);

			overlayShowDebugPolygon(&v[0], 4, polcol, DEBUGPOLY_SUBDIV | DEBUGPOLY_NOBORDER | DEBUGPOLY_DUBSIDED | DEBUGPOLY_USEDEPTH);

		}
	}
#ifdef USE_BEVEL_ROUTES
	}
#endif

}

// Okeydokey, this is the fun part...
// What we do is...
// Draw the line from src to dest
// Note if it hits anything.

// If it doesn't, return the straight line.

// If it does, find the shortest circumnavigation of the object to the line's exit point.

// (Always assuming, of course, that the line HAS an exit point. It won't have if the chicken goes somewhere that
// the dog can't go.)

// At this stage, we have a minimum of...
// Vector from src to box intersect
// Vector from intersect to a corner
// Vector from the corner to either another corner, or to the exit point.

// We could return at this stage.
// However, what we should do is try for a shortcuts across the first two & second two
// (corner-exit point can't be clipped, but we could try to shortcut over a few points...
// (corner-next corner can be clipped if it's a concave box)
// The way to DO this is actually by routefinding between 'em (!)
// ...Alternatively, scan for *totally unblocked* shortcuts as we follow the long path. (a tad non-optimal, but prolly less maths)

// Note. Maths errors require our route to be kept outside the box by an error margin
// in any case. Tsk.

void routeDebugMarker(ROUTEBOX_VECTOR *src, int r, unsigned int c)
{
	overlayShowSphere(src->vx, -500, src->vz, r, c);
}


// return t = 0...4096 along src...dest, or -1 for no isect.
// Be wary of huge numbers incoming. Note that the lines CAN be big.
int routeLineIsectFind(VECTOR *src, VECTOR *dest, ROUTEBOX_VECTOR *v1, ROUTEBOX_VECTOR *v2)
{
//	int x1,z1,
	signed int x2,z2;
	signed int dx1,dz1;
	signed int dx2,dz2;
	signed int num;
	signed int s;
	signed int top,bot;
	signed int r;

// Let's do a quick ballpark boundscheck to start with, eh?
// Moved to caller
/*
	if(dest->vx < src->vx)
	{
		if(v1->vx > src->vx && v2->vx > src->vx) return -1;
	}
	else
	{
		if(v1->vx < src->vx && v2->vx < src->vx) return -1;
	}
	if(dest->vz < src->vz)
	{
		if(v1->vz > src->vz && v2->vz > src->vz) return -1;
	}
	else
	{
		if(v1->vz < src->vz && v2->vz < src->vz) return -1;
	}
*/

// line1:	x = t(destx-srcx)
//			z = t(destz-srcz)
// line2:	x = (v1x-srcx) + t2(v2x-v1x)
//			z = (v1z-srcz) + t2(v2z-v1z)

//		t = ((v1x-srcx)(v2z-v1z) - (v1z-srcz)(v2x-v1x)) / ((destx-srcx)(v2z-v1z) - (destz-srcz)(v2x-v1x))

//		t2 = (v1z-srcz)(destx-srcx) - (v1x-srcx)(destz-srcz) / ((v2x-v1x)(destz-srcz) - (v2z-v1z)(destx-srcx))



// Relocate to src-relative...
//	x1 = srcx;
//	z1 = srcz;
	x2 = v1->vx - src->vx;
	z2 = v1->vz - src->vz;
	dx1 = dest->vx - src->vx;
	dz1 = dest->vz - src->vz;
	dx2 = v2->vx - v1->vx;
	dz2 = v2->vz - v1->vz;

// We want ((number * number * 2) *4096) to not overflow (after which, we divide it by (number * number * 2) again)
// If we leave the 4096-scaling for a mo, then there's a 32768 limit, rather than a 512 one.
	num = ABS(x2) | ABS(z2) | ABS(dx1) | ABS(dz1) | ABS(dx2) | ABS(dz2);
	s = 0;
	while(num > 32768)
	{
		num = num>>2;
		s+=2;
	}

	x2 = x2>>s;
	z2 = z2>>s;
	dx1 = dx1>>s;
	dz1 = dz1>>s;
	dx2 = dx2>>s;
	dz2 = dz2>>s;


// Note. The isect's only valid if both t and t2 are in the range 0....1

// Calculate "t2"
	top = z2 * dx1 - x2 * dz1;
	bot = dx2 * dz1 - dz2 * dx1;

	if(ABS(top) > ABS(bot))
		return -1;
	if(!bot)
		return -1;
	if((top>0 && bot < 0) || (bot>0 && top < 0))
		return -1;






// Calculate "t"
	top = x2 * dz2 - z2 * dx2;
	bot = dx1 * dz2 - dz1 * dx2 ;

// We're not interest in the result if it's outside 0...1
	if(ABS(top) > ABS(bot))
		return -1;
// Zero-length test vector, or parallel.
	if(!bot)
		return -1;

	if((top>0 && bot < 0) || (bot>0 && top < 0))
		return -1;





// return the fraction. Phew.
	while( ABS(top) > 0x7ffff)
	{
		top = top>>1;
		bot = bot>>1;
	}

	r = (top * 4096) / bot;
	return r;

}

// "t" should be 0...4096
void routeVectorFraction(VECTOR *src, VECTOR *dest, int t, ROUTEBOX_VECTOR *out)
{
	VECTOR d;
	int num;
	int s;
	int bot;
	d.vx = dest->vx - src->vx;
	d.vz = dest->vz - src->vz;

#ifdef ROUTE_DEBUG_PRINTFS
	{
	if(ABS(d.vx) >= 0x7ffff)
		DB("dx = &%8x, adx = &%8x\n",d.vx,(int)ABS(d.vx));
	if(ABS(d.vz) >= 0x7ffff)
		DB("dz = &%8x, adz = &%8x\n",d.vz,(int)ABS(d.vz));
	}
#endif

	ASSERT(ABS(d.vx) < 0x7ffff);
	ASSERT(ABS(d.vz) < 0x7ffff);

// Actually, we shouldn't have any vectors going over 0x7ffff, surely?
// Well, on the boss chases, yes possibly. But they don't involve full-map-length routefinding.
// Or, if they do, all we need to do is plug this code back in... Hence the asserts above
/*
	num = ABS(d.vx) | ABS(d.vz);
	s = 0;
	while(num > 0x7ffff)
	{
		num = num>>2;
		s+=2;
	}

	d.vx = d.vx >> s;
	d.vz = d.vz >> s;
	bot = 4096 >> s;

	out->vx = src->vx + (d.vx * t) / bot;
	out->vz = src->vz + (d.vz * t) / bot;
*/
	out->vx = src->vx + (d.vx * t) / 4096;
	out->vz = src->vz + (d.vz * t) / 4096;
}

typedef struct ROUTE_BOXISECT_STR
{
	ROUTEBOX *box;
	int n;
	int plo;
	ROUTEBOX_VECTOR isectlo;
	int tlo;
	int phi;
	ROUTEBOX_VECTOR isecthi;
	int thi;
}ROUTE_BOXISECT_STR;

// The normal should be in the BFF compiler...
void routeCalculateCornerPoint(ROUTEBOX *box, int p, ROUTEBOX_VECTOR *isect, int r)
{
	ROUTEBOX_VECTOR v[2];
	int p1,p2;
	VECTOR temp;
	VECTOR temp2;

	isect->vx = box->vecs[p].vx;
	isect->vz = box->vecs[p].vz;
	routeKeepOutOfRouteboxes(&isect->vx, &isect->vz, r);
	return;




	p1 = (p+1) % box->n_vecs;
	p2 = (p + box->n_vecs-1) % box->n_vecs;

	v[0].vx = box->vecs[p1].vx - box->vecs[p].vx;
	v[0].vz = box->vecs[p1].vz - box->vecs[p].vz;
	
//	v[1].vx = box->vecs[p2].vx - box->vecs[p].vx;
//	v[1].vz = box->vecs[p2].vz - box->vecs[p].vz;
	v[1].vx = box->vecs[p].vx - box->vecs[p2].vx;
	v[1].vz = box->vecs[p].vz - box->vecs[p2].vz;


	temp.vx = -v[0].vz;
	temp.vz = v[0].vx;
	temp.vy = 0;
	MakeUnit(&temp);
	temp2.vx = -v[1].vz;
	temp2.vz = v[1].vx;
	temp2.vy = 0;
	MakeUnit(&temp2);

	isect->vx = box->vecs[p].vx + (((temp.vx+temp2.vx) * r) >> 12);
	isect->vz = box->vecs[p].vz + (((temp.vz+temp2.vz) * r) >> 12);

/*	
	temp.vx = v[0].vx + v[1].vx;
	temp.vz = v[0].vz + v[1].vz;
	temp.vy = 0;
	MakeUnit(&temp);
	isect->vx = box->vecs[p].vx - ((temp.vx * r) >> 12);
	isect->vz = box->vecs[p].vz - ((temp.vz * r) >> 12);
*/
}

void routeAdjustIsectPoint(ROUTEBOX *box, int p, ROUTEBOX_VECTOR *isect, int r)
{
	int p1;
	ROUTEBOX_VECTOR v;
	VECTOR temp;

// Actually no, we want to project back along the incoming line, rather than directly out from the routebox

	routeKeepOutOfRouteboxes(&isect->vx, &isect->vz, r);
	return;	// No adjustment any more - It's done with the "keep-out-of..." code


	p1 = (p+1) % box->n_vecs;
	v.vx = box->vecs[p1].vx - box->vecs[p].vx;
	v.vz = box->vecs[p1].vz - box->vecs[p].vz;

	temp.vx = -v.vz;
	temp.vz = v.vx;
	temp.vy = 0;
	MakeUnit(&temp);

	isect->vx += ((temp.vx * r) >> 12);
	isect->vz += ((temp.vz * r) >> 12);

}





// Find the intersections between a vector and a single routebox
int routeIsectFind(VECTOR *src, VECTOR *dest, ROUTEBOX *box, ROUTE_BOXISECT_STR *isect)
{
	int p;
	int t;
	ROUTEBOX_VECTOR *rv;
	ROUTEBOX_VECTOR *rv2;
//	int mint, maxt;

// Ideally, this quick reject needs to come before the call.
	if(src->vx < box->minx && dest->vx < box->minx) return 0;
	if(src->vx > box->maxx && dest->vx > box->maxx) return 0;
	if(src->vz < box->minz && dest->vz < box->minz) return 0;
	if(src->vz > box->maxz && dest->vz > box->maxz) return 0;

//	mint = maxt = -1;
	isect->n = 0;
	isect->tlo = -1;
	isect->thi = -1;
	rv = box->vecs;
	for(p = 0; p < box->n_vecs; p++,rv++)
	{
		if(p == box->n_vecs-1)
		{
			rv2 = &box->vecs[0];
		}
		else
		{
			rv2 = rv + 1;
		}


// Quick boundcheck elimination of the line-line isect
		if(dest->vx < src->vx)
		{
			if(rv->vx > src->vx && rv2->vx > src->vx) continue;
		}
		else
		{
			if(rv->vx < src->vx && rv2->vx < src->vx) continue;
		}
		if(dest->vz < src->vz)
		{
			if(rv->vz > src->vz && rv2->vz > src->vz) continue;
		}
		else
		{
			if(rv->vz < src->vz && rv2->vz < src->vz) continue;
		}


		t = routeLineIsectFind(src,dest,rv,rv2);
		if(t != -1)
		{
			VECTOR temp;
			isect->n++;

			if(isect->tlo == -1 || t < isect->tlo)
			{
				isect->plo = p;
// Delay the vector calculation, so it's only done on the first box to be hit
//				routeVectorFraction(src, dest, t, &isect->isectlo);
//				routeDebugMarker(&isect->isectlo);
				isect->tlo = t;
			}
			if(isect->thi == -1 || t > isect->thi)
			{
				isect->phi = p;
				isect->thi = t;
//				routeVectorFraction(src, dest, t, &isect->isecthi);
//				routeDebugMarker(&isect->isecthi);
			}
		}
	}

// If "n" is an odd number, then either "src" or "dest" is inside a box.

// If "n" is an even number, then we need to add the isects to the path

	return isect->n;
}

// Find the nearest point on a routebox's outline to a target which is inside the routebox
int routeFindNearestRoutePoint(VECTOR *src, ROUTEBOX *box, VECTOR *out)
{
	int i;
	VECTOR p1,p2;
	VECTOR isect;

	int best_dist = 0x7fffffff;
	int d;
	int best_point = 0;

//	utilPrintf("Inside Box Check\n");
	for(i = 0; i < box->n_vecs; i++)
	{
		p1.vx = box->vecs[i].vx;
		p1.vy = 0;
		p1.vz = box->vecs[i].vz;
		if(i == box->n_vecs-1)
		{
			p2.vx = box->vecs[0].vx;
			p2.vy = 0;
			p2.vz = box->vecs[0].vz;
		}
		else
		{
			p2.vx = box->vecs[i+1].vx;
			p2.vy = 0;
			p2.vz = box->vecs[i+1].vz;
		}

		d = mathsDistancePointToLine2D(&p1,&p2,src,&isect);
		if(d < best_dist)
		{
			*out = isect;
			best_dist = d;
			best_point = i;
		}
	}

//	utilPrintf("Nearest point on route = %d %d %d\n",out->vx /8, -out->vy /8, out->vz / 8);
	return best_point;
}


// Scan through the routeboxes, finding the first one that a vector intersects with
int routeFindIsects(VECTOR *src, VECTOR *dest, ROUTEBOX_VECTOR *route, int max_len)
{
	int bn;
	ROUTEBOX *box;
	int isects;
	ROUTE_BOXISECT_STR isect[2];
	ROUTE_BOXISECT_STR *close_box;

	int current;
	int closest;

	int distance_back;
	int distance_fore;
	int dx,dz;
	int p0,p1;
	int i;

	if(overlay_show_collision == DEBUG_OVERLAY_ENEMY_INFO)
	{
		overlayShowDebugLine(
			src,
			dest,
			0x00ffff,
			DEBUGPOLY_SUBDIV);	//+DEBUGPOLY_USEDEPTH);
	}

	current = 0;
	closest = 1;
	isect[closest].tlo = -1;

#ifdef USE_BEVEL_ROUTES
	box = route_routes->bevel_boxes;
#else
	box = route_routes->boxes;
#endif
	for(bn = 0; bn < route_routes->n_boxes; bn++,box++)
	{
		isects = routeIsectFind(src,dest,box, &isect[current]);

// isects = 0,1,2. Possibly more if there're any concave boxes.
// Either way, we need to scan round the box once we've got the entry & exit points

		if(isects)
		{
			if(isect[closest].tlo == -1 || isect[current].tlo < isect[closest].tlo)
			{
				closest = current;
				current = 1-closest;
				isect[closest].box = box;
			}
		}
	}


	if(isect[closest].tlo == -1)
	{
#ifdef ROUTE_DEBUG_PRINTFS
		if(route_debug_printfs)
			DB("routeFindIsects: None. Route[0] = target\n");
#endif
		route[0].vx = dest->vx;
		route[0].vz = dest->vz;
		return 1;
	}



// Now then. We need to figure the shortest direction round the obstacle...

	close_box = &isect[closest];
	box = close_box->box;


// Show us the intersections with the nearest routebox
	routeVectorFraction(src, dest, isect[closest].tlo, &isect[closest].isectlo);
#ifdef ROUTE_DEBUG_PRINTFS
	if(route_debug_printfs)
		DB("routeFindIsects: Adjusting TLo Isect. Line Isect at %d %d\n",isect[closest].isectlo.vx>>3,isect[closest].isectlo.vz>>3);
#endif
	routeAdjustIsectPoint(box, isect[closest].plo, &isect[closest].isectlo, ROUTE_ADJUST_AMOUNT);
#ifdef ROUTE_DEBUG_PRINTFS
	if(route_debug_printfs)
		DB("                 Result = %d %d\n",isect[closest].isectlo.vx>>3,isect[closest].isectlo.vz>>3);
#endif

//	routeDebugMarker(&isect[closest].isectlo);


// What to do if we have an odd number of isects...
// (ie, one of the points is inside the box)
// Answer, we want the "exit point" to be the nearest point on the box to the dest point.
// This, of course, assumes that it *is* the dest point that's in the box.

// Oh rats. It doesn't work unless we actually ALTER THE DEST to be this new "how close can we get to the dest?" point
// (for shortcutting purposes)

	if(isect[closest].n & 1)
	{
		VECTOR vnear;
		int near_p;

		near_p = routeFindNearestRoutePoint(dest, box, &vnear);

		if(overlay_show_collision == DEBUG_OVERLAY_ENEMY_INFO)
		{
			overlayShowSphere(vnear.vx,-500, vnear.vz, 40, 0xff00ff);
		}

		close_box->isecthi.vx = vnear.vx;
		close_box->isecthi.vz = vnear.vz;
//		close_box->phi = (near_p + box->n_vecs - 1) % box->n_vecs;
		close_box->phi = near_p;
		routeAdjustIsectPoint(box, close_box->phi, &close_box->isecthi, ROUTE_ADJUST_AMOUNT);
#ifdef ROUTE_DEBUG_PRINTFS
		if(route_debug_printfs)
		{
			DB("   routeFindIsects. Single Isect. NearPoint source & adjusted: %d,%d -> %d,%d\n",
			vnear.vx>>3, vnear.vz>>3,
			close_box->isecthi.vx>>3, close_box->isecthi.vz>>3);
		}
#endif


// Adjust the route's target point.
		dest->vx = close_box->isecthi.vx;
		dest->vz = close_box->isecthi.vz;

		if(overlay_show_collision == DEBUG_OVERLAY_ENEMY_INFO)
		{
			overlayShowSphere(close_box->isecthi.vx,-500, close_box->isecthi.vz, 200, 0xff00ff);
		}
// Important point here. Under these circumstances, the low & high isects may well be on the same line

// Let's do this special case, then...
//		if(isect[closest].n == 1)
		if(close_box->phi == close_box->plo)
		{
#ifdef ROUTE_DEBUG_PRINTFS
			if(route_debug_printfs)
				DB("  routeFindIsects.Single Isect. ClosePoint on same line as Isect. Twopoints\n");
#endif
			isect[closest].n = 2;
			route[0] = close_box->isectlo;
			route[1] = close_box->isecthi;
			return 2;
		}
// Otherwise, we now have an entry point and a (closest point == destination), rather than an entry and an exit
// And so, we drop through to the "work out the way round" code.

	}
	else
	{
#ifdef ROUTE_DEBUG_PRINTFS
		if(route_debug_printfs)
			DB("  routeFindIsects. Two Isects. Adjusting THi Isect\n");
#endif
// Normal exit-point calculation
		routeVectorFraction(src, dest, close_box->thi, &close_box->isecthi);
		routeAdjustIsectPoint(box, close_box->phi, &close_box->isecthi, ROUTE_ADJUST_AMOUNT);
	//	routeDebugMarker(&isect[closest].isecthi);
	}


	if(close_box->plo == close_box->phi)
	{
#ifdef ROUTE_DEBUG_PRINTFS
		if(route_debug_printfs)
			DB("  routeFindIsects. Two Isects. Both on same line. Reducing to IsectLO only. One point\n");
#endif
// Entry & exit points are on the same line.
// (Which can, but won't always, happen if the target is inside a box)
		route[0] = close_box->isectlo;
		return 1;
	}




// Scan Back...

// Distance from Entry to start of that line segment
	p0 = close_box->plo;
	dx = close_box->isectlo.vx - box->vecs[p0].vx;
	dz = close_box->isectlo.vz - box->vecs[p0].vz;
	distance_back = Magnitude2D(dx,dz);
	for(;;)
	{
		p1 = p0-1;
		if(p1 < 0)
			p1 = box->n_vecs-1;

		if(p1 != close_box->phi)
		{
// Lengths of previous line segments
			dx = box->vecs[p1].vx - box->vecs[p0].vx;
			dz = box->vecs[p1].vz - box->vecs[p0].vz;
			distance_back += Magnitude2D(dx,dz);
			p0 = p1;
		}
		else
		{
// distance_back from start of "next" line segment to the exit point
			dx = close_box->isecthi.vx - box->vecs[p0].vx;
			dz = close_box->isecthi.vz - box->vecs[p0].vz;
			distance_back += Magnitude2D(dx,dz);
			break;
		}
	}

// Special case for identical entry & exit points
// (Chicken is inside this routebox being the most likely cause)
	if(close_box->plo == close_box->phi)
	{
//		dx = close_box->isecthi.vx - close_box->isectlo.vx;
//		dz = close_box->isecthi.vz - close_box->isectlo.vz;

		i = 0;
		route[i] = close_box->isectlo;	// In all probability, this point will be optimised out of the route.
		i++;
		route[i] = close_box->isecthi;
		i++;
		return i;
	}

// Scan Forward...

// Distance from Entry to start of the next line segment
	p0 = close_box->plo + 1;
	if(p0 >= box->n_vecs)
		p0 = 0;
	dx = close_box->isectlo.vx - box->vecs[p0].vx;
	dz = close_box->isectlo.vz - box->vecs[p0].vz;
	distance_fore = Magnitude2D(dx,dz);
	for(;;)
	{

// Lengths of next line segments
		if(p0 != close_box->phi)
		{
			p1 = p0 + 1;
			if(p1 >= box->n_vecs)
				p1 = 0;
			dx = box->vecs[p1].vx - box->vecs[p0].vx;
			dz = box->vecs[p1].vz - box->vecs[p0].vz;
			distance_fore += Magnitude2D(dx,dz);
			p0 = p1;
		}
		else
		{
// Distance from start of this segment to the exit point
			dx = close_box->isecthi.vx - box->vecs[p0].vx;
			dz = close_box->isecthi.vz - box->vecs[p0].vz;
			distance_fore += Magnitude2D(dx,dz);
			break;
		}
	}

	i = 0;
	route[i] = close_box->isectlo;
	i++;

	if(distance_fore <= distance_back)
	{
		p0 = close_box->plo;

		for(;p0 != close_box->phi;)
		{
			p0++;
			if(p0 >= box->n_vecs)
				p0 = 0;
//			route[i] = box->vecs[p0];

			routeCalculateCornerPoint(box, p0, &route[i], ROUTE_ADJUST_AMOUNT);
#ifdef ROUTE_DEBUG_PRINTFS
		if(route_debug_printfs)
			DB("  routeFindIsects. Forward Corner Calc = %d,%d\n", route[i].vx>>3, route[i].vz>>3);
#endif

			i++;
//			routeDebugMarker(&box->vecs[p0]);

			if(i >= max_len)
				return i;
		}
	}
	else
	{
		p0 = close_box->plo;

		for(;p0 != close_box->phi;)
		{
//			routeDebugMarker(&box->vecs[p0]);
//			route[i] = box->vecs[p0];
			routeCalculateCornerPoint(box, p0, &route[i], ROUTE_ADJUST_AMOUNT);
#ifdef ROUTE_DEBUG_PRINTFS
		if(route_debug_printfs)
			DB("  routeFindIsects. Backward Corner Calc %d,%d\n", route[i].vx>>3, route[i].vz>>3);
#endif

			i++;

			p0--;
			if(p0 < 0 )
				p0 = box->n_vecs-1;

			if(i >= max_len)
				return i;

		}
	}
	route[i] = close_box->isecthi;
	i++;
	return i;
}


int routeKillClosePoints(VECTOR *src, ROUTEBOX_VECTOR *route, int n_route_points, int margin)
{
	int d;
	VECTOR temp;
	int i;
	ROUTEBOX *box;

// Trim down any points that are too close together
	temp.vx = src->vx;
	temp.vy = 0;
	temp.vz = src->vz;
	for(i = 0; i < n_route_points; i++)
	{
		d = Magnitude2D(route[i].vx - temp.vx, route[i].vz - temp.vz);
		if ( d < margin)
		{
			if(i != n_route_points-1)
			{
//				DB("  routeKillClosePoints. Removing point %i for closeness\n",i);
				FMEMCPY(&route[i+1],&route[i], (n_route_points - i) * sizeof(ROUTEBOX_VECTOR));
			}
			n_route_points--;
			i--;
		}
		else
		{
			temp.vx = route[i].vx;
			temp.vz = route[i].vz;
		}
	}
	return n_route_points;
}

// Stage 2: See if we can shortcut
// from the start to any of the points on the route.
// from the end to any of the points on the route.

int routeTryShortcuts(VECTOR *src, VECTOR *dest, ROUTEBOX_VECTOR *route, int n_route_points)
{
	int i;
	int bn;
	ROUTEBOX *box;
	VECTOR temp;
	int isects;
	ROUTE_BOXISECT_STR isect;


// Try to snip out the first part of the route
	for(i = n_route_points-1; i >= 1; i--)
	{
		temp.vx = route[i].vx;
		temp.vy = 0;
		temp.vz = route[i].vz;

#ifdef USE_BEVEL_ROUTES
		box = route_routes->bevel_boxes;
#else
		box = route_routes->boxes;
#endif
		isects = 0;
		for(bn = 0; bn < route_routes->n_boxes; bn++,box++)
		{
			isects = routeIsectFind(src,&temp,box, &isect);
			if(isects)
				break;
		}
		if(!isects)
		{
// We have a direct shortcut!
//			utilPrintf("Snipping Entry points. Shortcut to point %d\n",i);

#ifdef ROUTE_DEBUG_PRINTFS
		if(route_debug_printfs)
			DB("  routeTryShortcuts. Starting shortcut to %i\n",i);
#endif

			FMEMCPY(&route[0],&route[i], (n_route_points - i) * sizeof(ROUTEBOX_VECTOR));
			n_route_points = n_route_points - i;

			bn = 65535;
			i = 0;
		}
	}

// Try to snip out the second part of the route

	for(i = 0; i < n_route_points-1; i++)
	{
		temp.vx = route[i].vx;
		temp.vy = 0;
		temp.vz = route[i].vz;

#ifdef USE_BEVEL_ROUTES
		box = route_routes->bevel_boxes;
#else
		box = route_routes->boxes;
#endif
		isects = 0;
		for(bn = 0; bn < route_routes->n_boxes; bn++,box++)
		{
			isects = routeIsectFind(&temp,dest, box, &isect);
			if(isects)
				break;
		}
		if(!isects)
		{
//			utilPrintf("Snipping Exit points. Shortcut from point %d\n",i);
#ifdef ROUTE_DEBUG_PRINTFS
		if(route_debug_printfs)
			DB("  routeTryShortcuts. Ending shortcut to %i\n",i);
#endif
			route[i+1].vx = dest->vx;
			route[i+1].vz = dest->vz;
			n_route_points = i+2;

			bn = 65535;
			i = 65535;
		}
	}
	return n_route_points;
}




// Prevent the LINES of a route from coming within (whatever) of the routeboxes.
// (We'll just do the first (src->sv[0]) vector. Life's simpler that way)
int routeKeepLinesOutOfRouteboxes(VECTOR *src,ROUTEBOX_VECTOR *sv,int n_route_points,int max_len, int margin)
{
	VECTOR a,b,c,out,dirn;
	int sp;
	int p;
	int d;
	ROUTEBOX *box;
	ROUTEBOX_VECTOR *rv;
	ROUTEBOX_VECTOR xtrapoint;
	int bn;
	int closest_d;
	VECTOR closest_out;
	ROUTEBOX_VECTOR *closest_rv;
	ROUTEBOX *closest_box;
	int counter;

	a.vy = b.vy = c.vy = 0;



//	routeDebugMarker(&sv[0], 80, 0xffff00);

	counter = 0;
	closest_d = 0;
	while(counter < 10 && closest_d != margin)
	{
		closest_d = margin;
		counter++;

#ifdef USE_BEVEL_ROUTES
		box = route_routes->bevel_boxes;
#else
		box = route_routes->boxes;
#endif
		a.vx = src->vx;
		a.vz = src->vz;
		b.vx = sv[0].vx;
		b.vz = sv[0].vz;

		for(bn = 0; bn < route_routes->n_boxes; bn++,box++)
		{
			rv = box->vecs;
			for(p = 0; p < box->n_vecs; p++,rv++)
			{
				if(sv[0].vx < rv->vx-closest_d && src->vx < rv->vx-closest_d) continue;
				if(sv[0].vx > rv->vx+closest_d && src->vx > rv->vx+closest_d) continue;
				if(sv[0].vz < rv->vz-closest_d && src->vz < rv->vz-closest_d) continue;
				if(sv[0].vz > rv->vz+closest_d && src->vz > rv->vz+closest_d) continue;

				c.vx = rv->vx;
				c.vz = rv->vz;

				d = mathsDistancePointToLine2D(&a, &b, &c, &out);
				if(d < closest_d)
				{
					closest_d = d;
					closest_out = out;
					closest_rv = rv;
					closest_box = box;
				}
			}
		}


		if(closest_d != margin)
		{
	// "out" is a point on the line. We wanna project out from there by "margin" (plus a bit)

	// okay, we want to stuff that point into our route

			// If the duff point is at either end of the line, there ain't much we can do about it.
			if(    (closest_out.vx != sv[0].vx || closest_out.vz != sv[0].vz)
				&& (closest_out.vx != src->vx || closest_out.vz != src->vz)
				)
			{

	// It is technically possible (ie, Jon did it) for the line to actually touch the routebox point
	// (And not to be classed as an intersection by the routefinding code itself)
	//			if(closest_out.vx == closest_rv->vx && closest_out.vz == closest_rv->vz)

	// Note that due to maths errors which can be introduced by the down-shifts in the previous parts of the algorithm,
	// points CAN be inside the routeboxes to quite a degree. Worst case, I've had long line, which should have
	// been marked as intersecting the routebox twice, but which wasn't. Mag was 73 here, and the normal push sent it the wrong way.
	// Corner push was necessary.

	//			DB("Mag = %d\n",Magnitude2D(closest_out.vx - closest_rv->vx, closest_out.vz - closest_rv->vz));
				if(Magnitude2D(closest_out.vx - closest_rv->vx, closest_out.vz - closest_rv->vz) < 100)
	//			if(closest_out.vx == closest_rv->vx && closest_out.vz == closest_rv->vz)
				{
	// Don't always use this, because it can project backwards
					xtrapoint.vx = closest_rv->vx;
					xtrapoint.vz = closest_rv->vz;
					routeKeepOutOfRouteboxes(&xtrapoint.vx, &xtrapoint.vz, margin+DOG_ROUTELINE_SCHMITT);

	//	DB("KLOOR on corner\n");

				}
				else
				{
					dirn.vx = closest_out.vx - closest_rv->vx;
					dirn.vy = 0;
					dirn.vz = closest_out.vz - closest_rv->vz;
					MakeUnit(&dirn);

				// Project the points out by further than the margin dictates
				// (So that when we get there, our next route doesn't too close to (margin) of the point
					xtrapoint.vx = closest_rv->vx + ((dirn.vx * (margin+DOG_ROUTELINE_SCHMITT)) >> 12);
					xtrapoint.vz = closest_rv->vz + ((dirn.vz * (margin+DOG_ROUTELINE_SCHMITT)) >> 12);


	//			overlayShowSphere(closest_rv->vx, -500, closest_rv->vz, 40, 0x00ff00);

	//	DB("KLOOR on point\n");
				}



				if(    (xtrapoint.vx != sv[0].vx || xtrapoint.vz != sv[0].vz)
					&& (xtrapoint.vx != src->vx || xtrapoint.vz != src->vz)
					)
				{
					sp = n_route_points;
					if(sp >= max_len)
					{
						sp--;
					}
					else
					{
						n_route_points++;
					}
					for(; sp > 0; sp--)
					{
						sv[sp] = sv[sp-1];
					}

					sv[0].vx = xtrapoint.vx;
					sv[0].vz = xtrapoint.vz;

		#ifdef ROUTE_DEBUG_PRINTFS
					if(route_debug_printfs)
					{
						DB("  routeKeepLinesOutOfRouteboxes. Route too close to boxpoint (%d %d) at linepoint(%d %d): Inserting extra first point %d %d\n",
						closest_rv->vx>>3,closest_rv->vz>>3,
						closest_out.vx,closest_out.vz,
						sv[0].vx>>3,sv[0].vz>>3);

						DB("    Route is now (%d %d) %d %d, %d %d\n",src->vx, src->vz, sv[0].vx,sv[0].vz, sv[1].vx,sv[1].vz);
					}
		#endif
				}
				else
				{
		#ifdef ROUTE_DEBUG_PRINTFS
					if(route_debug_printfs)
					{
						DB("  routeKeepLinesOutOfRouteboxes. Route appears to be too close, but adjusted point is the same as an existing one\n");
					}
		#endif
					closest_d = margin;
				}

			}
		}
	}


//	DB("counter = %d\n",counter);
	return n_route_points;
}




// The longest possible route contains...
// 1) From start to a box...
// 2) N_Boxes from box to box
// 3) Total number of sides of each box, per box.
// Blimey.
// But that IS an absolute max, which we can use as a side
// Hang on, what about shortcuts?
// A shortcut can only add points which were weren't hit in the original path.


// tbd - we actually need to keep track of the max route length
int routeFindRoute(VECTOR *src, VECTOR *dest_in, ROUTEBOX_VECTOR *route, int max_len)
{
//	ROUTEBOX_VECTOR route[20];
	int n_route_points;
	int i;
	VECTOR temp[2];
	int base_point = 0;
	VECTOR localdest;

	localdest = *dest_in;

	if(!route_routes)
		return 0;


	i = Magnitude2D(dest_in->vx - src->vx,dest_in->vz - src->vz);
	if ( i < ((CLOSE_ENOUGH_TO_IGNORE_DIFFERENCE)+10))
	{
		overlayDebugMessage("Silly Short Route",60);
		DB("silly short route\n");
		return 0;
	}


	TIMER_START(TIMER_ROUTE);
//	DB("find\n");
/*
	overlayShowDebugLine(
		src,
		dest,
		0x00ffff,
		DEBUGPOLY_SUBDIV);	//+DEBUGPOLY_USEDEPTH);
*/
#ifdef ROUTE_DEBUG_PRINTFS
		if(route_debug_printfs)
			DB("=== routeFindRoute ===\n");
#endif

	n_route_points = routeFindIsects(src,&localdest,&route[0],max_len);

// If the target point is outside the routeboxes...
// Force it to be an achievable point outside the routeboxes
	if(!(n_route_points & 1))
	{
		int t1,t2;
		t1 = localdest.vx;
		t2 = localdest.vz;
		routeKeepOutOfRouteboxes(&t1, &t2, DOG_ROUTEPOINT_MARGIN);
		localdest.vx = t1;
		localdest.vz = t2;
#ifdef ROUTE_DEBUG_PRINTFS
		if(route_debug_printfs)
			DB("Dest adjusted to %d %d\n",localdest.vx>>3,localdest.vz>>3);
#endif
	}

#ifdef ROUTE_DEBUG_KEEPLINES
	first_n_points = routeCopyRoute(src,dest_in,&first_route[0],route,n_route_points);
#endif

// Okay. We now have a route that navigates us round the first obstacle.

// See if we can take shortcut from the start to any of the points, and from the end to any of the points.

	if(overlay_show_collision == DEBUG_OVERLAY_ENEMY_INFO)
	{
		for(i = 0; i < n_route_points; i++)
		{
			routeDebugMarker(&route[i], 80, 0xffff00);
		}
	}

#ifdef ROUTE_DEBUG_PRINTFS
		{
			int i;
			int old;
		if(route_debug_printfs)
		{
			DB("=Route before shortcuts\n");
			DB("  Src %d,%d\n",src->vx>>3,src->vz>>3);
			for(i = 0; i < n_route_points; i++)
			{
				DB("  point %d = %d,%d\n",i,route[i].vx>>3, route[i].vz>>3);
			}
			DB("  Dest %d %d\n",dest_in->vx>>3,dest_in->vz>>3);
		}
			old = n_route_points;

#endif
	n_route_points = routeTryShortcuts(src,&localdest,&route[0],n_route_points);

#ifdef ROUTE_DEBUG_KEEPLINES
	second_n_points = routeCopyRoute(src,&localdest,&second_route[0],route,n_route_points);
#endif


#ifdef ROUTE_DEBUG_PRINTFS
			if(old != n_route_points)
			{
		if(route_debug_printfs)
		{
				DB("=Route before shortcuts\n");
				DB("  Src %d,%d\n",src->vx>>3,src->vz>>3);
				for(i = 0; i < n_route_points; i++)
				{
					DB("  point %d = %d,%d\n",i,route[i].vx>>3, route[i].vz>>3);
				}
				DB("  Dest %d %d\n",dest_in->vx>>3,dest_in->vz>>3);
		}
			}
		}

#endif



// We need to know what angle we're going to turn at the first corner.
// But we may only have one point at this point, or two points of which the second isn't in a "real" position.
// So if we ain't got enough for a good angle decision, then routefind  from the first point in our path


	if(
//		n_route_points < 3
//		&& 
		route[n_route_points-1].vx != localdest.vx 
		&& route[n_route_points-1].vz != localdest.vz
		)
	{
		int temp_n_points;
		ROUTEBOX_VECTOR *temp_route = &route[1];
		temp[0].vx = route[0].vx;
		temp[0].vy = src->vy;
		temp[0].vz = route[0].vz;

#ifdef ROUTE_DEBUG_PRINTFS
		if(route_debug_printfs)
			DB("=== Route not targetted: Establish secondary route ===\n");
#endif

		temp_n_points = routeFindIsects(&temp[0],&localdest,&temp_route[0],max_len-1);

		// RFW... Just making sure... (coz otherwise we get target points miles away from where we want to be. Which buggers the garden dog no end)
		if(
			   temp_route[temp_n_points-1].vx != localdest.vx	// Note slight +1/-1 wierdness, coz this is a route starting at route point 1
			&& temp_route[temp_n_points-1].vz != localdest.vz
			&& temp_n_points < max_len-1
		)
		{
			temp_route[temp_n_points].vx = localdest.vx;
			temp_route[temp_n_points].vz = localdest.vz;
			temp_n_points++;
		}




#ifdef ROUTE_DEBUG_PRINTFS
		if(route_debug_printfs)
		{
			int i;
			DB("=Route before shortcuts, untargetted\n");
			DB("Src %d,%d\n",src->vx>>3,src->vz>>3);
			for(i = 0; i < temp_n_points + 1; i++)	// Again, note the extra one...
			{
				DB("point %d = %d,%d\n",i,route[i].vx>>3, route[i].vz>>3);
			}
			DB("  Dest %d %d\n",dest_in->vx>>3,dest_in->vz>>3);

		}
#endif



		temp_n_points = routeTryShortcuts(&temp[0],&localdest,&temp_route[0],temp_n_points);	// Shortcuts from point 1, remember

		n_route_points = temp_n_points+1;	// re-introduce the original. Phew. now we're back to normal

#ifdef ROUTE_DEBUG_PRINTFS
		if(route_debug_printfs)
		{
			int i;
			DB("=Route after shortcuts, untargetted\n");
			DB("  Src %d,%d\n",src->vx>>3,src->vz>>3);
			for(i = 0; i < n_route_points; i++)
			{
				DB("  point %d = %d,%d\n",i,route[i].vx>>3, route[i].vz>>3);
			}
			DB("  Dest %d %d\n",dest_in->vx>>3,dest_in->vz>>3);
		}
#endif

	}

#ifdef ROUTE_DEBUG_KEEPLINES
	third_n_points = routeCopyRoute(src,&localdest,&third_route[0],route,n_route_points);
#endif


	{
		int i;
// Make sure that the route itself is kept outside the routeboxes (!)
// as a "stop the dog from trying to run through things" measure
		for(i = 0; i < n_route_points; i++)
		{
			routeKeepOutOfRouteboxes(&route[i].vx, &route[i].vz, DOG_ROUTEPOINT_MARGIN);
		}
	}

#ifdef ROUTE_DEBUG_KEEPLINES
	fourth_n_points = routeCopyRoute(src,&localdest,&fourth_route[0],route, n_route_points);
#endif


// Also, we need to ensure that the LINES don't come within (margin) of the routebox's POINTS. Oh bugger.
// (Just do the first one, eh?)

	n_route_points = routeKeepLinesOutOfRouteboxes(src,&route[0],n_route_points,max_len,DOG_ROUTELINE_MARGIN);

// Very small "margin", note. Rqd, because the new "keep out" function tends to put points on top of each other at inside corners
	n_route_points = routeKillClosePoints(src, &route[0], n_route_points, 10);


#ifdef ROUTE_DEBUG_KEEPLINES
	fifth_n_points = routeCopyRoute(src,&localdest,&fifth_route[0],route, n_route_points);
#endif



#ifdef ROUTE_DEBUG_PRINTFS
	if(route_debug_printfs)
	{
		int i;
		DB("=== Routefind Done ===\n");

		DB("=Final Route (Bar point adjust)\n");
		DB("  Src %d,%d\n",src->vx>>3,src->vz>>3);
		for(i = 0; i < n_route_points; i++)
		{
			DB("  point %d = %d,%d\n",i,route[i].vx>>3, route[i].vz>>3);
		}
		DB("  Dest %d %d\n",dest_in->vx>>3,dest_in->vz>>3);
	}
#endif


	if(overlay_show_collision == DEBUG_OVERLAY_ENEMY_INFO || overlay_show_collision == DEBUG_OVERLAY_ROUTE_FIND)
	{

		temp[1].vx = src->vx;
		temp[1].vy = -500;
		temp[1].vz = src->vz;


		for(i = 0; i < n_route_points; i++)
		{
			temp[0] = temp[1];
			temp[1].vx = route[i].vx;
			temp[1].vy = -500;
			temp[1].vz = route[i].vz;

			overlayShowDebugLine(
				&temp[0],
				&temp[1],
				0xffffff,
				DEBUGPOLY_SUBDIV+DEBUGPOLY_USEDEPTH);

			routeDebugMarker(&route[i], 100, 0x00ffff);
		}
	}

	TIMER_STOP(TIMER_ROUTE);
	return n_route_points;

}


void routeShowRoute(VECTOR *src, ROUTEBOX_VECTOR *route, int route_len)
{
	VECTOR temp[2];
	int i;

	temp[1].vx = src->vx;
	temp[1].vy = -500;
	temp[1].vz = src->vz;


	for(i = 0; i < route_len; i++)
	{
		temp[0] = temp[1];
		temp[1].vx = route[i].vx;
		temp[1].vy = -500;
		temp[1].vz = route[i].vz;

		overlayShowDebugLine(
			&temp[0],
			&temp[1],
			0xff00ff,
			DEBUGPOLY_SUBDIV+DEBUGPOLY_USEDEPTH);

//		routeDebugMarker(&route[i], 200, 0x00ffff);
	}
}

#ifdef ROUTE_DEBUG_KEEPLINES
void routeShowRoute2(ROUTEBOX_VECTOR *route, int route_len, unsigned int colour, int which)
{
	VECTOR temp[2];
	int i;

	if(route_len < 2)
		return;

	temp[1].vx = route[0].vx;
	temp[1].vy = -500;
	temp[1].vz = route[0].vz;


	if((((loop_ticker2 >> 5) & 3) == which) || which == -1)
	{
	for(i = 1; i < route_len; i++)
	{
		temp[0] = temp[1];
		temp[1].vx = route[i].vx;
		temp[1].vy = -500;
		temp[1].vz = route[i].vz;

		overlayShowDebugLine(
			&temp[0],
			&temp[1],
			colour,
			DEBUGPOLY_SUBDIV+DEBUGPOLY_USEDEPTH);

//			routeDebugMarker(&route[i], 200, colour);
			routeDebugMarker(&route[i], 50, colour);
		
	}
	}
}
#endif

// This is for returning to your old path, after giving up.
// Returns point number at start of the nearest line-segment
int routeFindNearestPoint(VECTOR *src, ENEMYPOINTPOINTER path, int n_points, VECTOR *out)
{
	int i;
	VECTOR *p1,*p2;
	VECTOR isect;


	int best_dist = 0x7fffffff;
	int d;
	int best_point = 0;

	if(n_points < 2)	// one point. Go to it. eg - summoned persuit dogs
	{
		*out = path[0].v;
		return 0;
	}

//	utilPrintf("----------\n");
//	utilPrintf("point = %d %d %d\n",src->vx/8,-src->vy/8,src->vz/8);
	for(i = 0; i < n_points; i++)
	{
		p1 = &path[i].v;
		if(i == n_points-1)
			p2 = &path[0].v;
		else
			p2 = &path[i+1].v;

		d = mathsDistancePointToLine2D(p1,p2,src,&isect);
		if(d < best_dist)
		{
			*out = isect;
			best_dist = d;
			best_point = i;
		}
	}

//	utilPrintf("Nearest point on route = %d %d %d\n",out->vx /8, -out->vy /8, out->vz / 8);
	return best_point;
}

/*
// If the enemy's velocity puts it inside a routebox, then it starts misbehaving. So push it out...
// (note - this shouldn't use the bevel boxes, but the real ones)
int routeKeepOutOfRouteboxes(int *vx, int *vz, int margin)
{
	ROUTEBOX *box;
	int bn;
	int p;
	int d;
	VECTOR out;
	VECTOR dirn;
	int rval;
	VECTOR src;

	VECTOR v1,v2;

	int reset = 1;
	int counter = 0;

//#ifdef USE_BEVEL_ROUTES
//	margin = 4;
//#endif

	if(!route_routes)
		return 0;

	TIMER_START(TIMER_ROUTE);

	rval = 0;
//#ifdef USE_BEVEL_ROUTES
//	box = route_routes->bevel_boxes;
//#else
//#endif

//	while(reset == 1 && counter < 10)	// Nasty little loop to push points out of tricky corners
	{
		counter++;
		reset = 0;
		box = route_routes->boxes;
		for(bn = 0; bn < route_routes->n_boxes; bn++,box++)
		{
			// If we can do a quick reject, let's do so.

			if(*vx < box->minx -margin) continue;
			if(*vx > box->maxx +margin) continue;
			if(*vz < box->minz -margin) continue;
			if(*vz > box->maxz +margin) continue;

			// okay, do the sums

			v1.vx = box->vecs[box->n_vecs-1].vx;
			v1.vy = 0;
			v1.vz = box->vecs[box->n_vecs-1].vz;
			for(p = 0; p < box->n_vecs; p++)
			{

				v2 = v1;
				v1.vx = box->vecs[p].vx;
				v1.vz = box->vecs[p].vz;	// y's still zero


				// Avoid calculating distance from (for eg) every line in the level border
				if(*vx < v1.vx - margin && *vx < v2.vx-margin) continue;
				if(*vz < v1.vz - margin && *vz < v2.vz-margin) continue;
				if(*vx > v1.vx + margin && *vx > v2.vx+margin) continue;
				if(*vz > v1.vz + margin && *vz > v2.vz+margin) continue;

				src.vx = *vx;
				src.vz = *vz;
				d = mathsDistancePointToLine2D(&v1, &v2, &src, &out);
				if(d < margin)
				{
	// "out" is a point on the line. We wanna project out from there by "margin"
	// (means we'll get pointed ends. drat)
	// Alternatively, src->out will be a vector that's rounded at the ends, but using it will project us INTO
	// the box is we manage to get over half-way through.


					if(margin == DOG_ROUTEPOINT_MARGIN)
					{
	// Method using right-angle to line
	// (used for route points)
	// Copes with being on or the wrong side of the line, but overestimates corners rather badly
						dirn.vx = v2.vx - v1.vx;
						dirn.vy = 0;
						dirn.vz = v2.vz - v1.vz;
						MakeUnit(&dirn);

						*vx = out.vx + ((dirn.vz * margin) >> 12);
						*vz = out.vz - ((dirn.vx * margin) >> 12);
					}
					else
					{
	// Method using "out" point
	// (used for enemy position)
	// Rounded corners, but suffers probs if inpoint is on or behind the line)
						dirn.vx = *vx - out.vx;
						dirn.vy = 0;
						dirn.vz = *vz - out.vz;

						MakeUnit(&dirn);

						*vx = out.vx + ((dirn.vx * margin) >> 12);
						*vz = out.vz + ((dirn.vz * margin) >> 12);
					}
	#ifdef ROUTE_DEBUG_PRINTFS
					// Print routefind ones. Not dog-keep-out ones
					{
						if(route_debug_printfs)
							DB("  routeKeepOutOfRouteboxes. Margin %d, Point adjustment. Line %d, Point %d %d. Out = %d %d\n", margin, p, src.vx>>3, src.vz>>3, (*vx) >> 3, (*vz) >> 3);
					}
	#endif

					rval = 1;

					reset = 1;
				}
			}
		}
	}
	TIMER_STOP(TIMER_ROUTE);
	return rval;
}
*/



long routeDotProduct(ROUTEBOX_VECTOR *v1, ROUTEBOX_VECTOR *v2)
{
	ROUTEBOX_VECTOR u1,u2;
	long num;
	long result;
	int mf;
	int s=0;

#ifdef USE_DIVTABLE
	int dt;
#endif

	num=0;

	num|=(ABS(v1->vx));
	num|=(ABS(v2->vx));
	num|=(ABS(v1->vz));
	num|=(ABS(v2->vz));

	while(num > 26754) // anything squared *3 bigger than this will overflow a long
	{
		num = num>>2;
		s+=2;
	}

	if(s)
	{
		mf=1<<s;

#ifdef USE_DIVTABLE
		if(mf>=MAX_DIVS) mf=MAX_DIVS;
		
		dt=(int)pDivTable[mf];

		u1.vx=(v1->vx)*dt;
		u2.vx=(v2->vx)*dt;
		u1.vz=(v1->vz)*dt;
		u2.vz=(v2->vz)*dt;
#else
		u1.vx=(v1->vx)/mf;
		u2.vx=(v2->vx)/mf;
		u1.vz=(v1->vz)/mf;
		u2.vz=(v2->vz)/mf;
#endif

		if(s<6)
			result=((u1.vx)*(u2.vx) + (u1.vz)*(u2.vz))/(1<<(12-(2*s)));
		else
			if(s==6)
				result=(u1.vx)*(u2.vx) + (u1.vz)*(u2.vz);
			else
				result=((u1.vx)*(u2.vx) + (u1.vz)*(u2.vz))<<((2*s)-12);
	}
	else
	{
		result=((v1->vx)*(v2->vx) + (v1->vz)*(v2->vz))/4096;
	}

	return(result);
}


int routeMakeUnit(ROUTEBOX_VECTOR *vect)
{
	int tb,s,d;
	long n;
	long m = Magnitude2D(vect->vx,vect->vz);

	if(m != 0)
	{
		// find top bit
		n=ABS(vect->vx)|ABS(vect->vz);

		for(tb=31; !(n&0x80000000); tb--)  	n=n<<1;

		if(tb>18)
		{
			s=30-tb;
			d=tb-18;
			vect->vx= (vect->vx<<s)/(m>>d);
			vect->vz= (vect->vz<<s)/(m>>d);
		}
		else
		{
			vect->vx= (vect->vx<<FRACTIONAL_BITS)/m;
			vect->vz= (vect->vz<<FRACTIONAL_BITS)/m;
		}
	}

	return m;
}

int routeDistancePointToLine(ROUTEBOX_VECTOR *p1, ROUTEBOX_VECTOR *p2, int x, int z, ROUTEBOX_VECTOR *out)
{
	ROUTEBOX_VECTOR dirn;
	ROUTEBOX_VECTOR dp;
	int mag;
	int dot;
	dirn.vx = p2->vx - p1->vx;
	dirn.vz = p2->vz - p1->vz;

	dp.vx = x - p1->vx;
	dp.vz = z - p1->vz;

//	utilPrintf("dp = %d %d %d, dirn = %d %d %d",dp.vx/8,-dp.vy/8,dp.vz/8, dirn.vx/8,-dirn.vy/8,dirn.vz/8);

	mag = routeMakeUnit(&dirn);

	if(mag == 0)
	{
		if(out)
		{
			*out = *p1;
//			utilPrintf("p2l: p1 = %d %d %d, p2 = %d %d %d. Zero \n",p1->vx/8,-p1->vy/8,p1->vz/8,p2->vx/8,-p2->vy/8,p2->vz/8);
		}

		return(Magnitude2D(dp.vx,dp.vz));
	}

	dot = routeDotProduct(&dp,&dirn);

//	utilPrintf("unitdirn = %d %d %d, magdirn = %d, dot = %d\n",dirn.vx, -dirn.vx, dirn.vz, mag/8,dot/8);

//	dot = dot / 4096;
//	utilPrintf("dot = %d, mag = %d\n",dot/8,mag/8);

	if(dot < 0)
	{
		if(out)
		{
			*out = *p1;
//			utilPrintf("p2l: p1 = %d %d %d, p2 = %d %d %d. End1 \n",p1->vx/8,-p1->vy/8,p1->vz/8,p2->vx/8,-p2->vy/8,p2->vz/8);
		}
		return Magnitude2D(dp.vx,dp.vz);
	}
	else if(dot > mag)
	{
		if(out)
		{
			*out = *p2;
//			utilPrintf("p2l: p1 = %d %d %d, p2 = %d %d %d. End2 \n",p1->vx/8,-p1->vy/8,p1->vz/8,p2->vx/8,-p2->vy/8,p2->vz/8);
		}
		return (Magnitude2D(x - p2->vx,z - p2->vz));
	}
	else
	{
		int s = 12;
// Dirn is a unit vec
// dot is a number from 0 up to the magnitude of the (p1->p2) vector.
		while(dot > 0x7ffff)
		{
			dot = dot >> 2;
			s -= 2;
		}
//		utilPrintf("dot = %d, s= %d\n",dot,s);
		dp.vx = p1->vx + ((dot * dirn.vx) >> s);
		dp.vz = p1->vz + ((dot * dirn.vz) >> s);

		if(out)
		{
			*out = dp;
//			utilPrintf("p2l: p1 = %d %d %d, p2 = %d %d %d. Mid \n",p1->vx/8,-p1->vy/8,p1->vz/8,p2->vx/8,-p2->vy/8,p2->vz/8);
//			utilPrintf("    out = %d %d %d\n",out->vx/8,-out->vy/8,out->vz/8);
		}
		return (Magnitude2D(x - dp.vx,z - dp.vz));
	}
}


// The important thing's the sign
int routeAcuteCheck(ROUTEBOX_VECTOR *v1, ROUTEBOX_VECTOR *v2, ROUTEBOX_VECTOR *v3)
{
	int dx1,dz1,dx2,dz2;
	long result;
	int mf;
#ifdef USE_DIVTABLE
	int dt;
#endif
	int s=0;
	ULONG num;


	dx1 = v2->vx - v1->vx;
	dz1 = v2->vz - v1->vz;
	dx2 = v3->vx - v2->vx;
	dz2 = v3->vz - v2->vz;


	num =(ABS(dx1));
	num|=(ABS(dx2));
	num|=(ABS(dz1));
	num|=(ABS(dz2));

	while(num > 26754) // anything squared *3 bigger than this will overflow a long
	{

		num = num>>2;
		s+=2;
	}

	if(s)
	{
		mf=1<<s;

#ifdef USE_DIVTABLE
		if(mf>=MAX_DIVS) mf=MAX_DIVS;
		
		dt=(int)pDivTable[mf];

		dx1=(dx1)*dt;
		dx2=(dx2)*dt;
		dz1=(dz1)*dt;
		dz2=(dz2)*dt;
#else
		dx1=(dx1)/mf;
		dx2=(dx2)/mf;
		dz1=(dz1)/mf;
		dz2=(dz2)/mf;
#endif
	}

	result = dx1 * dz2 - dx2 * dz1;
	return(result);
}




// Push vx,vz "margin" away from "out"
void routeOutsideCornerPush(VECTOR *out, int *vx, int *vz, int margin)
{
	VECTOR dirn;
	dirn.vx = *vx - out->vx;
	dirn.vy = 0;
	dirn.vz = *vz - out->vz;
	MakeUnit(&dirn);
	*vx = out->vx + ((dirn.vx * margin) >> 12);
	*vz = out->vz + ((dirn.vz * margin) >> 12);
}

// Called when a point is ON the corner, and needs pushing out with some directional info
// Give it some outside push, & then outsideCornerPush it.
void routeOutsideMidpoint(ROUTEBOX_VECTOR *v1,ROUTEBOX_VECTOR *v2,ROUTEBOX_VECTOR *v3, int *vx, int *vz)
{
	ROUTEBOX_VECTOR d1,d2;
	d1.vx = v1->vx - v2->vx;
	d1.vz = v1->vz - v2->vz;
	d2.vx = v3->vx - v2->vx;
	d2.vz = v3->vz - v2->vz;

	routeMakeUnit(&d1);
	routeMakeUnit(&d2);
	*vx -= (d1.vx + d2.vx)/4;
	*vz -= (d1.vz + d2.vz)/4;
}

void routeEdgePush(VECTOR *out, int *vx, int *vz, ROUTEBOX_VECTOR *v1, ROUTEBOX_VECTOR *v2, int margin)
{
	VECTOR dirn;
	dirn.vx = v1->vx - v2->vx;
	dirn.vy = 0;
	dirn.vz = v1->vz - v2->vz;
	MakeUnit(&dirn);
	*vx = out->vx + ((dirn.vz * margin) >> 12);
	*vz = out->vz - ((dirn.vx * margin) >> 12);
}

// vx/z has already been pushed out from the v1/v2 line
// However, it now needs to be pushed out, along "dirn" till it's away from v1/v2 as well
// v1,v2 = "the other line". v3,v4 = main line
void routeInsideCornerPush(ROUTEBOX_VECTOR *rb_out, int *vx, int *vz, ROUTEBOX_VECTOR *v1,ROUTEBOX_VECTOR *v2, ROUTEBOX_VECTOR *v3,ROUTEBOX_VECTOR *v4, int margin)
{
	ROUTEBOX_VECTOR target;
	ROUTEBOX_VECTOR dirn;
	long dot;

//	overlayShowSphere(*vx, -500, *vz, 100, 0x004000);

	dirn.vx = v1->vx - v2->vx;
	dirn.vz = v1->vz - v2->vz;
	routeMakeUnit(&dirn);	// Direction of the other line

	target.vx = rb_out->vx + ((dirn.vz * margin) >> 12);
	target.vz = rb_out->vz - ((dirn.vx * margin) >> 12);

//		overlayShowSphere(rb_out->vx, -500, rb_out->vz, 100, 0x400040);
//		overlayShowSphere(target.vx, -500, target.vz, 100, 0x800080);

	dirn.vx = target.vx - *vx;	// vector from mainlines pushed point to said point pushed out from other line
	dirn.vz = target.vz - *vz;

// We want the component of that in the (v3-v4) direction
	target.vx = v4->vx - v3->vx;
	target.vz = v4->vz - v3->vz;
	routeMakeUnit(&target);		// Unit vector in v3->v4 direction

	dot = routeDotProduct(&dirn,&target);	// comp't
//	*vx += dirn.vx * target.vx / 4096;
//	*vz += dirn.vz * target.vz / 4096;
	*vx += dot * target.vx / 4096;	// added in
	*vz += dot * target.vz / 4096;
}

int routeKeepOutOfRouteboxes(int *vx, int *vz, int margin)
{
	ROUTEBOX *box;
	int bn;
	int p;
	int d;
	VECTOR out;
	VECTOR dirn;
	int rval;
	VECTOR src;
	ROUTEBOX_VECTOR rb_out;

	VECTOR v1,v2;

	int closest_d = INT_MAX;
	VECTOR closest_out;
	int closest_bn = -1;
	int closest_pn = -1;


	int counter = 0;

//#ifdef USE_BEVEL_ROUTES
//	margin = 4;
//#endif

	if(!route_routes)
		return 0;

	TIMER_START(TIMER_ROUTE);

	rval = 0;
//#ifdef USE_BEVEL_ROUTES
//	box = route_routes->bevel_boxes;
//#else
//#endif

//	while(closest_d == INT_MAX && counter < 10)	// Nasty little loop to push points out of tricky corners
	{
		counter++;
		closest_d = INT_MAX;

		box = route_routes->boxes;
		for(bn = 0; bn < route_routes->n_boxes; bn++,box++)
		{
			// If we can do a quick reject, let's do so.

			if(*vx < box->minx -margin) continue;
			if(*vx > box->maxx +margin) continue;
			if(*vz < box->minz -margin) continue;
			if(*vz > box->maxz +margin) continue;

			// okay, do the sums

			v1.vx = box->vecs[box->n_vecs-1].vx;
			v1.vy = 0;
			v1.vz = box->vecs[box->n_vecs-1].vz;
			for(p = 0; p < box->n_vecs; p++)
			{

				v2 = v1;
				v1.vx = box->vecs[p].vx;
				v1.vz = box->vecs[p].vz;	// y's still zero


				// Avoid calculating distance from (for eg) every line in the level border
				if(*vx < v1.vx - margin && *vx < v2.vx-margin) continue;
				if(*vz < v1.vz - margin && *vz < v2.vz-margin) continue;
				if(*vx > v1.vx + margin && *vx > v2.vx+margin) continue;
				if(*vz > v1.vz + margin && *vz > v2.vz+margin) continue;

				src.vx = *vx;
				src.vz = *vz;
				d = mathsDistancePointToLine2D(&v1, &v2, &src, &out);
				if(d < margin)
				{
					if(d < closest_d)
					{
						closest_d = d;
						closest_out = out;
						closest_bn = bn;
						closest_pn = p;
					}
				}
			}
		}



		if(closest_d != INT_MAX)
		{
			ROUTEBOX_VECTOR *v[4];

			box = &route_routes->boxes[closest_bn];

			closest_pn-=2;
			if(closest_pn < 0) closest_pn += box->n_vecs;
			v[0] = &box->vecs[closest_pn];
			closest_pn++;
			if(closest_pn >=box->n_vecs) closest_pn = 0;
			v[1] = &box->vecs[closest_pn];
			closest_pn++;
			if(closest_pn >=box->n_vecs) closest_pn = 0;
			v[2] = &box->vecs[closest_pn];
			closest_pn++;
			if(closest_pn >=box->n_vecs) closest_pn = 0;
			v[3] = &box->vecs[closest_pn];

// check v (0,1,2) for clockwise to establish if the first corner's convex or concave
			if(routeAcuteCheck(v[0],v[1],v[2]) > 0)
			{
				if(routeAcuteCheck(v[1],v[2],v[3]) > 0)
				{
					int vx2,vz2;
//					DB("I, I\n");
					vx2 = *vx;
					vz2 = *vz;

// Note - It's assumed you can't hit both ends of an I-I line
// (When in practice, it might be possible)
					routeEdgePush(&closest_out,&vx2,&vz2,v[1],v[2],margin);
					if(routeDistancePointToLine(v[0], v[1], vx2, vz2, &rb_out) < margin)
					{
//						DB("I, I Critical 1\n");
						*vx = vx2;
						*vz = vz2;
						routeInsideCornerPush(&rb_out,vx,vz,v[0],v[1], v[1],v[2], margin);
					}
					else if(routeDistancePointToLine(v[2], v[3], vx2, vz2, &rb_out) < margin)
					{
//						DB("I, I Critical 2\n");
						*vx = vx2;
						*vz = vz2;
						routeInsideCornerPush(&rb_out,vx,vz,v[2],v[3], v[2],v[1], margin);
					}
					else
					{
//						DB("I, I Safe\n");
						routeEdgePush(&closest_out,vx,vz,v[1],v[2],margin);
					}
				}
				else
				{
					int vx2,vz2;
					vx2 = *vx;
					vz2 = *vz;

					routeEdgePush(&closest_out,&vx2,&vz2,v[1],v[2],margin);

					if(routeDistancePointToLine(v[0], v[1], vx2, vz2, &rb_out) < margin)
					{
//						DB("I, O Critical\n");
						*vx = vx2;
						*vz = vz2;
						routeInsideCornerPush(&rb_out,vx,vz,v[0],v[1], v[1],v[2], margin);
					}
					else
					{
//						DB("I, O Safe\n");
						if(
						   (closest_out.vx == v[2]->vx && closest_out.vz == v[2]->vz)
						   )
						{
							if(*vx == v[2]->vx && *vz == v[2]->vz)
								routeOutsideMidpoint(v[1],v[2],v[3],vx,vz);
							routeOutsideCornerPush(&closest_out,vx,vz,margin);
						}
						else
						{
							routeEdgePush(&closest_out,vx,vz,v[1],v[2],margin);
						}
					}
				}
			}
			else
			{
				if(routeAcuteCheck(v[1],v[2],v[3]) > 0)
				{
					int vx2,vz2;
					vx2 = *vx;
					vz2 = *vz;

					routeEdgePush(&closest_out,&vx2,&vz2,v[1],v[2],margin);

					if(routeDistancePointToLine(v[2], v[3], vx2, vz2, &rb_out) < margin)
					{
//						DB("O, I Critical\n");
						*vx = vx2;
						*vz = vz2;
						routeInsideCornerPush(&rb_out,vx,vz,v[2],v[3], v[2],v[1], margin);
					}
					else
					{
//						DB("O, I Safe\n");
						if(
						   (closest_out.vx == v[1]->vx && closest_out.vz == v[1]->vz)
						   )
						{
							if(*vx == v[1]->vx && *vz == v[1]->vz)
								routeOutsideMidpoint(v[0],v[1],v[2],vx,vz);
							routeOutsideCornerPush(&closest_out,vx,vz,margin);
						}
						else
						{
							routeEdgePush(&closest_out,vx,vz,v[1],v[2],margin);
						}
					}
				}
				else
				{
//					DB("O, O\n");
					// Two outside corners (The easy one)
					// If the nearest point was an end, then we're guaranteed to be on the outside of the corner, so a straight "push" is okay.
					// Otherwise, push us out from the line
					if(
						   (closest_out.vx == v[1]->vx && closest_out.vz == v[1]->vz)
						)
					{
						// push off of outside-corner
						if(*vx == v[1]->vx && *vz == v[1]->vz)
							routeOutsideMidpoint(v[0],v[1],v[2],vx,vz);
						routeOutsideCornerPush(&closest_out,vx,vz,margin);
					}

					else if(
						    (closest_out.vx == v[2]->vx && closest_out.vz == v[2]->vz)
						)
					{
						// push off of outside-corner
						if(*vx == v[2]->vx && *vz == v[2]->vz)
							routeOutsideMidpoint(v[1],v[2],v[3],vx,vz);
						routeOutsideCornerPush(&closest_out,vx,vz,margin);
					}

					else
					{
						routeEdgePush(&closest_out,vx,vz,v[1],v[2],margin);

//						// Inside or on edge: Push away from the edge
//						MakeUnit(&dirn);
//						*vx = closest_out.vx + ((dirn.vz * margin) >> 12);
//						*vz = closest_out.vz - ((dirn.vx * margin) >> 12);
					}
				}
			}







//			overlayShowSphere(*vx, -500, *vz, 100, 0x008000);
			rval = 1;

		}
	}
	TIMER_STOP(TIMER_ROUTE);
	return rval;
}




