/***************************************************************************
 *            bezier.cc
 *
 *  Sat Nov 12 10:20:06 2005
 *  Copyright  2005  Joe Venzon
 *  joe@venzon.net
 ****************************************************************************/

/*
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU Library General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
 */
 
#include "bezier.h"

BEZIER::BEZIER()
{
	int x,y;
	
	for (x = 0; x < 4; x++)
	{
		for (y = 0; y < 4; y++)
		{
			points[x][y].zero();
		}
	}

	next_patch = NULL;
	turn = 0;
	dist_from_start = 0.0;
	length = 0.0;
}

BEZIER::~BEZIER()
{
	
}

void BEZIER::SetFromCorners(VERTEX fl, VERTEX fr, VERTEX bl, VERTEX br)
{
	VERTEX temp;
	
	center = fl + fr + bl + br;
	center.Scale(0.25);
	radius = 0;
	if ((fl - center).len() > radius)
		radius = (fl - center).len();
	if ((fr - center).len() > radius)
		radius = (fr - center).len();
	if ((bl - center).len() > radius)
		radius = (bl - center).len();
	if ((br - center).len() > radius)
		radius = (br - center).len();
	
	//assign corners
	points[0][0] = fl;
	points[0][3] = fr;
	points[3][3] = br;
	points[3][0] = bl;
	
	//calculate intermediate front and back points
	
	temp = fr - fl;
	points[0][1] = fl + temp.normalize().ScaleR(temp.len()/3.0);
	points[0][2] = fl + temp.normalize().ScaleR(2.0*temp.len()/3.0);
	
	temp = br - bl;
	points[3][1] = bl + temp.normalize().ScaleR(temp.len()/3.0);
	points[3][2] = bl + temp.normalize().ScaleR(2.0*temp.len()/3.0);
	
	
	//calculate intermediate left and right points	
	int i;
	for (i = 0; i < 4; i++)
	{
		temp = points[3][i] - points[0][i];
		points[1][i] = points[0][i] + temp.normalize().ScaleR(temp.len()/3.0);
		points[2][i] = points[0][i] + temp.normalize().ScaleR(2.0*temp.len()/3.0);
	}
}

void BEZIER::Visualize(bool wireframe, bool fill, VERTEX color)
{
	if (!fill && !wireframe)
		return;
	
	glPushAttrib(GL_ALL_ATTRIB_BITS);
	
	glDisable(GL_LIGHTING);
	glDisable(GL_TEXTURE_2D);
	
	if (fill)
	{
		glColor4f(1,1,1,1);
		glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
		//glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
		glDisable(GL_DEPTH_TEST);
		glEnable(GL_BLEND);
		glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA);
		float trans = 0.25;
		glColor4f(1,1,1,trans);
		DrawSurf(SURFDRAW_VIS, trans);
		glEnable(GL_DEPTH_TEST);
	}
	
	if (wireframe)
	{
		glDisable(GL_DEPTH_TEST);
		glLineWidth(1.0);
		glEnable(GL_BLEND);
		glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA);
		//glColor4f(.5,.6,.5,0.1);
		glColor4f(color.x, color.y, color.z,0.1);
		DrawControlPoints();
		
		glEnable(GL_DEPTH_TEST);
		glDisable(GL_BLEND);
		glLineWidth(2.0);
		glColor4f(color.x, color.y, color.z,1);
		DrawControlPoints();
	}
	
	glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
	glPopAttrib();
}

void BEZIER::DrawSurf(int div, float trans)
{
	int x, y;
	
	float px, py, pxo, pyo, col;
	
	//VERTEX temp[4];
	VERTEX normal;
	
	if (div < 1)
		div = 1;
	
	glBegin(GL_TRIANGLES);
	
	for (x = 0; x < div; x++)
	{
		for (y = 0; y < div; y++)
		{
			//if (x < div - 1 && y < div - 1)
			{
				px = (float) x / (float) div;
				py = (float) y / (float) div;
				pxo = (float) (x+1) / (float) div;
				pyo = (float) (y+1) / (float) div;
				
				normal = (SurfCoord(pxo, pyo) - SurfCoord(px, py)).cross(SurfCoord(pxo, py) - SurfCoord(px, py));
				normal = normal.normalize();
				col = normal.y;
				col = col * col;
				glColor4f(col,col,col, trans);
				
				glVertex3fv(SurfCoord(pxo, py).v3());
				glVertex3fv(SurfCoord(px, py).v3());
				glVertex3fv(SurfCoord(px, pyo).v3());
				
				normal = (SurfCoord(px, pyo) - SurfCoord(px, py)).cross(SurfCoord(pxo, pyo) - SurfCoord(px, py));
				normal = normal.normalize();
				col = normal.y;
				col = col * col;
				glColor4f(col,col,col, trans);
				
				glVertex3fv(SurfCoord(px, pyo).v3());
				glVertex3fv(SurfCoord(pxo, pyo).v3());
				glVertex3fv(SurfCoord(pxo, py).v3());
			}
		}
	}

	glEnd();
}

void BEZIER::DrawControlPoints()
{
	glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);

	//glColor4f(1,0,0,1);
	
	/*int x, y;
	glPointSize(5.0);
	glBegin(GL_POINTS);
	
	for (x = 0; x < 4; x++)
	{
		for (y = 0; y < 4; y++)
		{
			glVertex3fv(points[x][y].v3());
		}
	}

	glEnd();*/
	
	//glColor4f(0,1,0,1);
	
	glBegin(GL_QUADS);
	
	glVertex3fv(points[0][0].v3());
	glVertex3fv(points[3][0].v3());
	glVertex3fv(points[3][3].v3());
	glVertex3fv(points[0][3].v3());

	glEnd();
	
	glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
}

void BEZIER::Attach(BEZIER & other, bool reverse)
{
	int x;
	
	//move the other patch to the location of this patch
	/*for (x = 0; x < 4; x++)
		points[0][x] = other.points[3][x];*/

	if (!reverse)
	{
		//move the other patch to the location of this patch and force its
		// intermediate points into a nice grid layout
		other.SetFromCorners(other.points[0][0], other.points[0][3], points[0][0], points[0][3]);
		
		VERTEX slope;
		
		for (x = 0; x < 4; x++)
		{
			//slope points in the forward direction
			slope = (other.points[0][x] - points[3][x]).normalize();
			
			float otherlen = (other.points[0][x] - other.points[3][x]).len();
			float mylen = (points[0][x] - points[3][x]).len();
			
			float meanlen = (otherlen + mylen)/2.0;
			float leglen = meanlen / 3.0;
			
			other.points[2][x] = other.points[3][x] + slope.ScaleR(leglen);
			points[1][x] = points[0][x] + slope.ScaleR(-leglen);
		}
	}
	
	//store the pointer to next patch
	next_patch = &other;

	//calculate the track radius at the connection of this patch and next patch
	VERTEX a = SurfCoord(0.5,0.0);
	VERTEX b = SurfCoord(0.5,1.0);
	VERTEX c = next_patch->SurfCoord(0.5,1.0);
	
	if (reverse)
	{
		a = SurfCoord(0.5,1.0);
		b = SurfCoord(0.5,0.0);
		c = next_patch->SurfCoord(0.5,0.0);
		
		//Reverse();
	}
	
	VERTEX d1 = a - b;
	VERTEX d2 = c - b;
	float diff = d2.len() - d1.len();
	float angle = acos( d1.dot(d2) / d1.len() / d2.len());
	float alpha = (M_PI * diff + 2.0 * d1.len() * angle) / (d1.len() + d2.len()) / 2.0;
	if (fabs(alpha - M_PI/2.0) < 0.001) track_radius = 10000.0;
	else track_radius = d1.len() / 2.0 / cos(alpha);

	//determine it's a left or right turn at the connection
	VERTEX d = d1.cross(d2);
	if (fabs(d.x) < 0.1 && fabs(d.y) < 0.1 && fabs(d.z) < 0.1)
	{
		turn = 0; //straight ahead
	}
	else if (d.y > 0.0) turn = -1; //left turn ahead
	else turn = 1; //right turn ahead

	//calculate distance from start of the road
	if (next_patch->next_patch == NULL || reverse) next_patch->dist_from_start = dist_from_start + d1.len();
	length = d1.len();
}

void BEZIER::Reverse()
{
	VERTEX oldpoints[4][4];
	
	for (int n = 0; n < 4; n++)
		for (int i = 0; i < 4; i++)
			oldpoints[n][i] = points[n][i];
		
	for (int n = 0; n < 4; n++)
		for (int i = 0; i < 4; i++)
			points[n][i] = oldpoints[3-n][3-i];
}

VERTEX BEZIER::Bernstein(float u, VERTEX *p)
{
	VERTEX	a, b, c, d, r;

	a = p[0].ScaleR(pow(u,3));
	b = p[1].ScaleR(3*pow(u,2)*(1-u));
	c = p[2].ScaleR(3*u*pow((1-u),2));
	d = p[3].ScaleR(pow((1-u),3));

	//r = pointAdd(pointAdd(a, b), pointAdd(c, d));
	r = a+b+c+d;

	return r;
}

VERTEX BEZIER::SurfCoord(float px, float py)
{
	VERTEX temp[4];
	VERTEX temp2[4];
	int i, j;

	/*if (px == 0.0 && py == 0.0)
		return points[3][3];
	if (px == 1.0 && py == 1.0)
		return points[0][0];
	if (px == 1.0 && py == 0.0)
		return points[0][3];
	if (px == 0.0 && py == 1.0)
		return points[3][0];*/
	
	//get splines along x axis
	for (j = 0; j < 4; j++)
	{
		for (i = 0; i < 4; i++)
			temp2[i] = points[j][i];
		temp[j] = Bernstein(px, temp2);
	}
	
	return Bernstein(py, temp);
}

VERTEX BEZIER::SurfNorm(float px, float py)
{
	VERTEX temp[4];
	VERTEX temp2[4];
	VERTEX tempx[4];
	int i, j;

	//get splines along x axis
	for (j = 0; j < 4; j++)
	{
		for (i = 0; i < 4; i++)
			temp2[i] = points[j][i];
		temp[j] = Bernstein(px, temp2);
	}
	
	//get splines along y axis
	for (j = 0; j < 4; j++)
	{
		for (i = 0; i < 4; i++)
			temp2[i] = points[i][j];
		tempx[j] = Bernstein(py, temp2);
	}
	
	//return BernsteinTangent(py, temp);
	return BernsteinTangent(px, tempx).cross(BernsteinTangent(py, temp)).normalize().InvertR();
}

VERTEX BEZIER::BernsteinTangent(float u, VERTEX *p)
{
	VERTEX	a, b, c, r;

	a = (p[1]-p[0]).ScaleR(3*pow(u,2));
	b = (p[2]-p[1]).ScaleR(3*2*u*(1-u));
	c = (p[3]-p[2]).ScaleR(3*pow((1-u),2));

	//r = pointAdd(pointAdd(a, b), pointAdd(c, d));
	r = a+b+c;

	return r;
}

void BEZIER::GetTri(int div, int num, VERTEX outtri[3])
{
	int firsttri = num % 2;
	
	int x = (num/2) % div;
	int y = (num/2) / div;

	if (firsttri == 0)
	{		
		float px = (float) x / (float) div;
		float py = (float) y / (float) div;
		float pxo = (float) (x+1) / (float) div;
		float pyo = (float) (y+1) / (float) div;
	
		/*glVertex3fv(SurfCoord(px, py).v3());
		glVertex3fv(SurfCoord(pxo, py).v3());
		glVertex3fv(SurfCoord(pxo, pyo).v3());*/
		
		outtri[0] = SurfCoord(px, py);
		outtri[1] = SurfCoord(pxo, py);
		outtri[2] = SurfCoord(px, pyo);
	}
	else
	{
		float px = (float) x / (float) div;
		float py = (float) y / (float) div;
		float pxo = (float) (x+1) / (float) div;
		float pyo = (float) (y+1) / (float) div;
		
		/*glVertex3fv(SurfCoord(px, py).v3());
		glVertex3fv(SurfCoord(pxo, pyo).v3());
		glVertex3fv(SurfCoord(px, pyo).v3());*/
		
		outtri[0] = SurfCoord(pxo, pyo);
		outtri[1] = SurfCoord(px, pyo);
		outtri[2] = SurfCoord(pxo, py);
	}
}

bool BEZIER::CollideSubDiv(VERTEX origin, VERTEX direction, VERTEX &outtri)
{
	int i;
	
	VERTEX curtri[3];
	
	int retval = 0;
	
	float t, u, v;
	
	int x, y;
	
	for (i = 0; i < NumTris(COLLISION_DIVS) && !retval; i++)
	{	
		GetTri(COLLISION_DIVS, i, curtri);
		
		retval = INTERSECT_FUNCTION(origin.v3(), direction.v3(),
			curtri[0].v3(), curtri[1].v3(), curtri[2].v3(), 
			&t, &u, &v);
		
		if (retval)
		{
			if (i % 2 == 0)
			{
				u = 1.0 - u;
				v = 1.0 - v;
			}
			
			u = 1.0 - u;
			v = 1.0 - v;
			
			x = (i/2) % COLLISION_DIVS;
			y = (i/2) / COLLISION_DIVS;
			u += (float) x;
			v += (float) y;
			u = u / (float) COLLISION_DIVS;
			v = v / (float) COLLISION_DIVS;
			
			//u = 1.0 - u;
			//v = 1.0 - v;
			
			//cout << u << "," << v << endl;
			outtri = SurfCoord(u, v);
			//outtri = curtri[0].ScaleR(1-u-v) + curtri[1].ScaleR(u) + curtri[2].ScaleR(v);
			return true;
		}
	}
	
	outtri = origin;
	return false;
}

bool BEZIER::CollideNewton(VERTEX origin, VERTEX direction, VERTEX &outtri)
{
	int i;
	
	VERTEX curtri[3];
	VERTEX coltri[3];
	
	int retval = 0;
	bool collided = false;
	
	float su, sv, tu, tv;
	su = sv = tu = tv = 0;
	
	float t, u, v;
	
	int x, y;
	int j;
	
	for (i = 0; i < NumTris(1) && !retval; i++)
	{	
		GetTri(COLLISION_DIVS, i, curtri);
		
		retval = INTERSECT_FUNCTION(origin.v3(), direction.v3(),
			curtri[0].v3(), curtri[1].v3(), curtri[2].v3(), 
			&t, &u, &v);
		
		if (retval)
		{
			for (j = 0; j < 3; j++)
				coltri[j] = curtri[j];
			tu = u;
			tv = v;
			
			if (i % 2 == 0)
			{
				u = 1.0 - u;
				v = 1.0 - v;
			}
			
			u = 1.0 - u;
			v = 1.0 - v;
			
			x = (i/2) % COLLISION_DIVS;
			y = (i/2) / COLLISION_DIVS;
			u += (float) x;
			v += (float) y;
			u = u / (float) COLLISION_DIVS;
			v = v / (float) COLLISION_DIVS;
			
			//u = 1.0 - u;
			//v = 1.0 - v;
			
			//cout << u << "," << v << endl;
			outtri = SurfCoord(u, v);
			su = u;
			sv = v;
			//return true;
			collided = true;
		}
	}
	
	if (collided)
	{
		bool loop = true;
		bool verbose = false;
		VERTEX normal;
		VERTEX approx;
		VERTEX actual;
		VERTEX guess = outtri;
		VERTEX correct;
		MATRIX3 proj;
		
		float error, lasterror;
		
		approx = curtri[0].ScaleR(1-tu-tv) + curtri[1].ScaleR(tu) + curtri[2].ScaleR(tv);
		
		float spanu = (points[0][3] - points[0][0]).len();
		float spanv = (points[3][0] - points[0][0]).len();
		
		//error = (approx - outtri).len();
		//error is the distance from direction vector to actual position
		VERTEX dirnorm = direction.normalize();
		VERTEX errorvec;
		
		errorvec = (guess - origin) - dirnorm.ScaleR((guess - origin).dot(dirnorm));
		error = errorvec.len();
		lasterror = error;
		
		if (verbose)
			cout << error << endl;
		
		float uit = (error / spanu)/2.0;
		float vit = (error / spanv)/2.0;
		float nu, nv;
		
		float biasv, biasu;
		biasu = biasv = 1.0;
		
		float erroru, errorv;
		
		nu = su + uit;
		nv = sv + vit;
		if (nu > 1.0)
			nu = 1.0;
		if (nv > 1.0)
			nv = 1.0;
		guess = SurfCoord(nu,sv);
		errorvec = (guess - origin) - dirnorm.ScaleR((guess - origin).dot(dirnorm));
		error = errorvec.len();
		erroru = error - lasterror;
		if (error > lasterror)
			biasu = -1.0;
		if (verbose) cout << error << endl;
		guess = SurfCoord(su,nv);
		errorvec = (guess - origin) - dirnorm.ScaleR((guess - origin).dot(dirnorm));
		error = errorvec.len();
		errorv = error - lasterror;
		if (error > lasterror)
			biasv = -1.0;
		if (verbose) cout << error << endl;
			
		biasu *= erroru/(erroru+errorv);
		biasv *= errorv/(erroru+errorv);
		
		VERTEX lastguess = guess;
		
		nu = su;
		nv = sv;
		
		float ou, ov;
		ou = nu;
		ov = nv;
		
		VERTEX tempguess;
		
		int n;
		for (n = 0; (n < 20) && loop; n++)
		{
			lastguess = guess;
			ou = nu;
			ov = nv;
			
			nu += biasu * (error / spanu)/1.0;
			nv += biasv * (error / spanv)/1.0;
			
			tempguess = SurfCoord(nu, ov);
			if (((tempguess - origin) - dirnorm.ScaleR((tempguess - origin).dot(dirnorm))).len() > lasterror)
			{
				biasu = 0.0;
				nu = ou;
			}
			tempguess = SurfCoord(ou, nv);
			if (((tempguess - origin) - dirnorm.ScaleR((tempguess - origin).dot(dirnorm))).len() > lasterror)
			{
				biasv = 0.0;
				nv = ov;
			}
			
			guess = SurfCoord(nu, nv);
			
			lasterror = error;
			errorvec = (guess - origin) - dirnorm.ScaleR((guess - origin).dot(dirnorm));
			error = errorvec.len();
			if (verbose) cout << n << ": " << error << endl;
			
			if (lasterror <= error || biasu == 0.0 && biasv == 0.0)
				loop = false;
		}
		
		/*int n;
		for (n = 0; (n < 1) && loop; n++)
		{
			cout << n << ": " << error << endl;
			
			actual = SurfCoord(su, sv);
			normal = SurfNorm(su, sv);
			proj.ProjectionMatrix(normal.normalize());
			correct = approx - actual;
			guess = proj.Multiply(correct);
			
			lasterror = error;
			errorvec = dirnorm.ScaleR((outtri - origin).dot(dirnorm));
			error = errorvec.len();
		}*/
		
		/*approx.DebugPrint();
		actual.DebugPrint();
		normal.DebugPrint();
		//proj.DebugPrint();
		guess.DebugPrint();
		guess = guess + actual;
		guess.DebugPrint();*/
		//cout << n << ": " << (guess - outtri).len() << endl;
		if (verbose) cout << endl;
		
		outtri = lastguess;	
		
		return true;
	}
	
	outtri = origin;
	return false;
}

BEZIER & BEZIER::CopyFrom(const BEZIER &other)
{
	int x, y;
	
	for (x = 0; x < 4; x++)
	{
		for (y = 0; y < 4; y++)
		{
			points[x][y] = other.points[x][y];
		}
	}
	
	center = other.center;
	radius = other.radius;
	length = other.length;
	dist_from_start = other.dist_from_start;
	next_patch = other.next_patch;
	track_radius = other.track_radius;
	turn = other.turn;

	return *this;
}

bool BEZIER::ReadFrom(ifstream &openfile)
{
	int x, y;
	
	if (!openfile)
		return false;
	
	for (x = 0; x < 4; x++)
	{
		for (y = 0; y < 4; y++)
		{
			openfile >> points[x][y].x;
			openfile >> points[x][y].y;
			openfile >> points[x][y].z;
		}
	}
	
	return true;
}

bool BEZIER::WriteTo(ofstream &openfile)
{
	int x, y;
	
	for (x = 0; x < 4; x++)
	{
		for (y = 0; y < 4; y++)
		{
			openfile << points[x][y].x << " ";
			openfile << points[x][y].y << " ";
			openfile << points[x][y].z << endl;
		}
	}
	
	return true;
}

bool BEZIER::CollideSingleQuad(VERTEX origin, VERTEX direction, VERTEX &outtri)
{
	bool col = false;
	
	float t, u, v;
	
	col = INTERSECT_QUAD_FUNCTION(origin, direction,
		points[0][0], points[0][3], points[3][3], points[3][0],
		t, u, v);
	
	if (col)
	{
		/*if (i % 2 == 0)
		{
			u = 1.0 - u;
			v = 1.0 - v;
		}
		
		u = 1.0 - u;
		v = 1.0 - v;
		
		x = (i/2) % COLLISION_DIVS;
		y = (i/2) / COLLISION_DIVS;
		u += (float) x;
		v += (float) y;
		u = u / (float) COLLISION_DIVS;
		v = v / (float) COLLISION_DIVS;*/
		
		u = 1.0 - u;
		v = 1.0 - v;
		
		//cout << u << "," << v << endl;
		outtri = SurfCoord(u, v);
		//outtri = curtri[0].ScaleR(1-u-v) + curtri[1].ScaleR(u) + curtri[2].ScaleR(v);
		return true;
	}
	
	outtri = origin;
	return false;
}

bool BEZIER::CollideQuadNewton(VERTEX origin, VERTEX direction, VERTEX &outtri)
{
	bool col = false;
	
	float t, u, v;
	
	col = INTERSECT_QUAD_FUNCTION(origin, direction,
		points[0][0], points[0][3], points[3][3], points[3][0],
		t, u, v);
	
	if (col)
	{
		/*if (i % 2 == 0)
		{
			u = 1.0 - u;
			v = 1.0 - v;
		}
		
		u = 1.0 - u;
		v = 1.0 - v;
		
		x = (i/2) % COLLISION_DIVS;
		y = (i/2) / COLLISION_DIVS;
		u += (float) x;
		v += (float) y;
		u = u / (float) COLLISION_DIVS;
		v = v / (float) COLLISION_DIVS;*/
		
		u = 1.0 - u;
		v = 1.0 - v;
		
		//cout << u << "," << v << endl;
		outtri = SurfCoord(u, v);
		float su = u;
		float sv = v;
		//outtri = curtri[0].ScaleR(1-u-v) + curtri[1].ScaleR(u) + curtri[2].ScaleR(v);
		
		bool loop = true;
		bool verbose = false;
		VERTEX normal;
		VERTEX approx;
		VERTEX actual;
		VERTEX guess = outtri;
		VERTEX correct;
		MATRIX3 proj;
		
		float error, lasterror;
		
		float spanu = (points[0][3] - points[0][0]).len();
		float spanv = (points[3][0] - points[0][0]).len();
		
		//error = (approx - outtri).len();
		//error is the distance from direction vector to actual position
		VERTEX dirnorm = direction.normalize();
		VERTEX errorvec;
		
		errorvec = (guess - origin) - dirnorm.ScaleR((guess - origin).dot(dirnorm));
		error = errorvec.len();
		lasterror = error;
		
		if (verbose)
			cout << error << endl;
		
		float uit = (error / spanu)/2.0;
		float vit = (error / spanv)/2.0;
		float nu, nv;
		
		float biasv, biasu;
		biasu = biasv = 1.0;
		
		float erroru, errorv;
		
		nu = su + uit;
		nv = sv + vit;
		if (nu > 1.0)
			nu = 1.0;
		if (nv > 1.0)
			nv = 1.0;
		guess = SurfCoord(nu,sv);
		errorvec = (guess - origin) - dirnorm.ScaleR((guess - origin).dot(dirnorm));
		error = errorvec.len();
		erroru = error - lasterror;
		if (error > lasterror)
			biasu = -1.0;
		if (verbose) cout << error << endl;
		guess = SurfCoord(su,nv);
		errorvec = (guess - origin) - dirnorm.ScaleR((guess - origin).dot(dirnorm));
		error = errorvec.len();
		errorv = error - lasterror;
		if (error > lasterror)
			biasv = -1.0;
		if (verbose) cout << error << endl;
			
		biasu *= erroru/(erroru+errorv);
		biasv *= errorv/(erroru+errorv);
		
		VERTEX lastguess = guess;
		
		nu = su;
		nv = sv;
		
		float ou, ov;
		ou = nu;
		ov = nv;
		
		VERTEX tempguess;
		
		int n;
		for (n = 0; (n < 5) && loop; n++)
		{
			lastguess = guess;
			ou = nu;
			ov = nv;
			
			nu += biasu * (error / spanu)/1.0;
			nv += biasv * (error / spanv)/1.0;
			
			tempguess = SurfCoord(nu, ov);
			if (((tempguess - origin) - dirnorm.ScaleR((tempguess - origin).dot(dirnorm))).len() > lasterror)
			{
				biasu = 0.0;
				nu = ou;
			}
			tempguess = SurfCoord(ou, nv);
			if (((tempguess - origin) - dirnorm.ScaleR((tempguess - origin).dot(dirnorm))).len() > lasterror)
			{
				biasv = 0.0;
				nv = ov;
			}
			
			guess = SurfCoord(nu, nv);
			
			lasterror = error;
			errorvec = (guess - origin) - dirnorm.ScaleR((guess - origin).dot(dirnorm));
			error = errorvec.len();
			if (verbose) cout << n << ": " << error << endl;
			
			if (lasterror <= error || biasu == 0.0 && biasv == 0.0)
				loop = false;
		}
		
		/*int n;
		for (n = 0; (n < 1) && loop; n++)
		{
			cout << n << ": " << error << endl;
			
			actual = SurfCoord(su, sv);
			normal = SurfNorm(su, sv);
			proj.ProjectionMatrix(normal.normalize());
			correct = approx - actual;
			guess = proj.Multiply(correct);
			
			lasterror = error;
			errorvec = dirnorm.ScaleR((outtri - origin).dot(dirnorm));
			error = errorvec.len();
		}*/
		
		/*approx.DebugPrint();
		actual.DebugPrint();
		normal.DebugPrint();
		//proj.DebugPrint();
		guess.DebugPrint();
		guess = guess + actual;
		guess.DebugPrint();*/
		//cout << n << ": " << (guess - outtri).len() << endl;
		if (verbose) cout << endl;
		
		outtri = lastguess;	
		
		return true;
	}
	
	outtri = origin;
	return false;	
}

bool BEZIER::CollideSubDivQuad(VERTEX origin, VERTEX direction, VERTEX &outtri)
{
	bool col = false;
	
	const bool verbose = false;
	
	const bool projhack = true;
	
	if (projhack)
		origin.y = 1;
	
	float t, u, v;
	
	float su = 0;
	float sv = 0;
	
	float umin = 0;
	float umax = 1;
	float vmin = 0;
	float vmax = 1;
	
	/*VERTEX ul = points[0][0];
	VERTEX ur = points[0][3];
	VERTEX br = points[3][3];
	VERTEX bl = points[3][0];*/
	
	VERTEX ul = points[3][3];
	VERTEX ur = points[3][0];
	VERTEX br = points[0][0];
	VERTEX bl = points[0][3];
	
	int subdivnum = 0;

	int i;
	for (i = 0; i < COLLISION_QUAD_DIVS; i++)
	{	
		//speedup for i == 0
		if (i != 0)
		{
			ul = SurfCoord(umin, vmin);
			ur = SurfCoord(umax, vmin);
			br = SurfCoord(umax, vmax);
			bl = SurfCoord(umin, vmax);
		}
		
		if (projhack)
		{
			ul.y = ur.y = br.y = bl.y = 0;
		}
		
		//u = v = 0.0;

		col = INTERSECT_QUAD_FUNCTION(origin, direction,
			ul, ur, br, bl,
			t, u, v);
		
		if (col)
		{
			//correct UV
			//u = 1.0 - u;
			//v = 1.0 - v;
			
			//expand quad UV to surface UV
			su = u * (umax - umin) + umin;
			sv = v * (vmax - vmin) + vmin;
			
			//calculate error
			if (verbose)
			{
				VERTEX guess = SurfCoord(su, sv);
				VERTEX dirnorm = direction.normalize();
				VERTEX errorvec;
				
				errorvec = (guess - origin) - dirnorm.ScaleR((guess - origin).dot(dirnorm));
				
				cout << i << ": " << errorvec.len() << endl;
			}
			
			//evaluate which quadrant was hit
			if (v <= 0.5)
			{
				//top side
				vmax = 0.5*(vmax-vmin)+vmin;
				subdivnum = 0;
			}
			else
			{
				//bottom side
				vmin = 0.5*(vmax-vmin)+vmin;
				subdivnum = 2;
			}
			
			if (u <= 0.5)
			{
				//left side
				umax = 0.5*(umax-umin) + umin;
			}
			else
			{
				//right side
				umin = 0.5*(umax-umin) + umin;
				subdivnum++;
			}
			
			//outtri = SurfCoord(u, v);
		}
		else
		{
			if (verbose)
				cout << i << ": nocol " << origin.x << endl;
			
			if ((i == 0) && QUAD_DIV_FAST_DISCARD)
			//if (QUAD_DIV_FAST_DISCARD)
			{
				outtri = origin;
				return false;
			}
			else
			{
				//need to try again with a different subdiv
				
				//undo previous changes to uv rect
				float umin0 = umin;
				float umax0 = umax;
				float vmin0 = vmin;
				float vmax0 = vmax;
				
				if (subdivnum < 2)
					vmax0 = (vmax-vmin)*2.0 + vmin;
				else
					vmin0 = vmax - (vmax-vmin)*2.0;
				
				if (subdivnum % 2 == 0)
					umax0 = (umax-umin)*2.0 + umin;
				else
					umin0 = umax - (umax-umin)*2.0;
				
				if (verbose)
				{
					cout << endl << "Guessed wrong on " << subdivnum << endl;
					cout << umin << ", " << umax << " : " << vmin << ", " << vmax << endl;
					cout << umin0 << ", " << umax0 << " : " << vmin0 << ", " << vmax0 << endl;
				}
				
				if (verbose)
				{
					cout << "!" << subdivnum << endl;
					ul.DebugPrint();
					ur.DebugPrint();
					br.DebugPrint();
					bl.DebugPrint();
					cout << endl;
				}
				
				//try other rects
				int s;
				bool scol = false;
				int curnum = 0;
				for (s = 1; s < 4 && !scol; s++)
				{
					curnum = (4 + subdivnum - s) % 4;
					
					vmin = vmin0;
					vmax = vmax0;
					umin = umin0;
					umax = umin0;
					
					if (curnum < 2)
						vmax = 0.5*(vmax0-vmin0)+vmin0;
					else
						vmin = 0.5*(vmax0-vmin0)+vmin0;
					
					if (curnum % 2 == 0)
						umax = 0.5*(umax0-umin0) + umin0;
					else
						umin = 0.5*(umax0-umin0) + umin0;
					
					if (verbose)
					{
						cout << curnum << endl;
						cout << umin0 << ", " << umax0 << " : " << vmin0 << ", " << vmax0 << endl;
						cout << umin << ", " << umax << " : " << vmin << ", " << vmax << endl;
					}

					ul = SurfCoord(umin, vmin);
					ur = SurfCoord(umax, vmin);
					br = SurfCoord(umax, vmax);
					bl = SurfCoord(umin, vmax);
					
					if (projhack)
					{
						ul.y = ur.y = br.y = bl.y = 0;
					}
					
					if (verbose)
					{
						ul.DebugPrint();
						ur.DebugPrint();
						br.DebugPrint();
						bl.DebugPrint();
						cout << endl;
					}
			
					scol = INTERSECT_QUAD_FUNCTION(origin, direction,
						ul, ur, br, bl,
						t, u, v);
					
					if (scol)
					{
						//rejoice!
						su = u * (umax - umin) + umin;
						sv = v * (vmax - vmin) + vmin;
						
						//evaluate which quadrant was hit
						if (v <= 0.5)
						{
							//top side
							vmax = 0.5*(vmax-vmin)+vmin;
							subdivnum = 0;
						}
						else
						{
							//bottom side
							vmin = 0.5*(vmax-vmin)+vmin;
							subdivnum = 2;
						}
						
						if (u <= 0.5)
						{
							//left side
							umax = 0.5*(umax-umin) + umin;
						}
						else
						{
							//right side
							umin = 0.5*(umax-umin) + umin;
							subdivnum++;
						}
					}
				}
				
				if (verbose)
				{
					if (!scol)
					{
						cout << "Never found poper quad. " << origin.x << endl;
						//origin.DebugPrint();
						//direction.DebugPrint();
						//cout << endl;
					}
					//else
						//cout << "Found proper quad after " << s-1 << " trie(s). " << origin.x << endl;
				}
				
				if (!scol)
				{
					//cry
					
					//outtri = origin;
					//return false;
				}
			}
			
			/*if (i != 0)
			{
				loop = false;
				u = oldu;
				v = oldv;
			}*/
		}
	}
	
	if (verbose)
		cout << endl;
	
	/*col = INTERSECT_QUAD_FUNCTION(origin, direction,
		points[0][0], points[0][3], points[3][3], points[3][0],
		t, u, v);
	
	if (col)
	{	
		u = 1.0 - u;
		v = 1.0 - v;
		
		//cout << u << "," << v << endl;
		outtri = SurfCoord(u, v);
		return true;
	}*/
	
	if (col || QUAD_DIV_FAST_DISCARD)
	//if (col)
	{
		if (!col)
			cout << "blip " << su << "," << sv << ": " << u << "," << v << endl;
		outtri = SurfCoord(su, sv);
		return true;
	}
	else
	{
		outtri = origin;
		return false;
	}	
}

bool BEZIER::CollideSubDivQuadSimple(VERTEX origin, VERTEX direction, VERTEX &outtri)
{
	VERTEX normal;
	return CollideSubDivQuadSimpleNorm(origin, direction, outtri, normal);
}

bool BEZIER::CollideSubDivQuadSimpleNorm(VERTEX origin, VERTEX direction, VERTEX &outtri, VERTEX & normal)
{
	bool col = false;
	
	const bool verbose = false;
	//const bool verbose = true;
	
	const bool projhack = false;
	
	if (projhack)
		origin.y = 1;
	
	float t, u, v;
	
	float su = 0;
	float sv = 0;
	
	float umin = 0;
	float umax = 1;
	float vmin = 0;
	float vmax = 1;
	
	//const float fuzziness = 0.13;
	
	/*VERTEX ul = points[0][0];
	VERTEX ur = points[0][3];
	VERTEX br = points[3][3];
	VERTEX bl = points[3][0];*/
	
	VERTEX ul = points[3][3];
	VERTEX ur = points[3][0];
	VERTEX br = points[0][0];
	VERTEX bl = points[0][3];
	
	//int subdivnum = 0;

	bool loop = true;
	
	float areacut = 0.5;

	int i;
	for (i = 0; i < COLLISION_QUAD_DIVS && loop; i++)
	{
		float tu[2];
		float tv[2];
		
		//speedup for i == 0
		//if (i != 0)
		{
			tu[0] = umin;
			if (tu[0] < 0)
				tu[0] = 0;
			tu[1] = umax;
			if (tu[1] > 1)
				tu[1] = 1;
			
			tv[0] = vmin;
			if (tv[0] < 0)
				tv[0] = 0;
			tv[1] = vmax;
			
			if (tv[1] > 1)
				tv[1] = 1;
			
			ul = SurfCoord(tu[0], tv[0]);
			ur = SurfCoord(tu[1], tv[0]);
			br = SurfCoord(tu[1], tv[1]);
			bl = SurfCoord(tu[0], tv[1]);
			
			/*umin = u[0];
			umax = v[1];
			vmin = v[0];
			vmax = v[1];*/
			
			/*ul = SurfCoord(umin, vmin);
			ur = SurfCoord(umax, vmin);
			br = SurfCoord(umax, vmax);
			bl = SurfCoord(umin, vmax);*/
		}
		
		if (projhack)
		{
			ul.y = ur.y = br.y = bl.y = 0;
		}
		
		//u = v = 0.0;

		col = INTERSECT_QUAD_FUNCTION(origin, direction,
			ul, ur, br, bl,
			t, u, v);
		
		if (col)
		{
			//expand quad UV to surface UV
			//su = u * (umax - umin) + umin;
			//sv = v * (vmax - vmin) + vmin;
			
			su = u * (tu[1] - tu[0]) + tu[0];
			sv = v * (tv[1] - tv[0]) + tv[0];
			
			//place max and min according to area hit
			vmax = sv + (0.5*areacut)*(vmax-vmin);
			vmin = sv - (0.5*areacut)*(vmax-vmin);
			umax = su + (0.5*areacut)*(umax-umin);
			umin = su - (0.5*areacut)*(umax-umin);
			
			/*//evaluate which quadrant was hit
			if (v <= 0.5)
			{
				//top side
				vmax = (0.5+fuzziness)*(vmax-vmin)+vmin;
				//vmax = (0.5+fuzziness)*(vmax-vmin)+vmin-fuzziness*0.5;
				//vmin -= fuzziness*0.5;
				subdivnum = 0;
			}
			else
			{
				//bottom side
				vmin = (0.5-fuzziness)*(vmax-vmin)+vmin;
				subdivnum = 2;
			}
			
			if (u <= 0.5)
			{
				//left side
				umax = (0.5+fuzziness)*(umax-umin) + umin;
			}
			else
			{
				//right side
				umin = (0.5-fuzziness)*(umax-umin) + umin;
				subdivnum++;
			}*/
		}
		else
		{
			if ((i == 0) && QUAD_DIV_FAST_DISCARD)
			//if (QUAD_DIV_FAST_DISCARD)
			{
				outtri = origin;
				return false;
			}
			else
			{
				if (verbose)
				{
					cout << "<" << i << ": nocol " << su << "," << sv << ">" << endl;
					cout << "<" << umin << "," << umax << ";" << vmin << "," << vmax << ">" << endl;
					
					ul.DebugPrint();
					ur.DebugPrint();
					bl.DebugPrint();
					br.DebugPrint();
					
					cout << endl;
				}
				
				loop = false;
			}
			
			/*if (i != 0)
			{
				loop = false;
				u = oldu;
				v = oldv;
			}*/
		}
	}
	
	if (col)// || QUAD_DIV_FAST_DISCARD)
	//if (col)
	{
		/*if (!col)
			cout << "blip " << su << "," << sv << ": " << u << "," << v << endl;*/
		outtri = SurfCoord(su, sv);
		normal = SurfNorm(su, sv);
		//if (verbose)
			//cout << "<" << i << ">" << endl;
		return true;
	}
	else
	{
		outtri = origin;
		return false;
	}
}
