#include "objects.h"

#include "vamosworld.h" //needed for texture based lighting
#include "globals.h"

OBJECTS::OBJECTS()
{
	error_log.open((settings.GetSettingsDir() + "/logs/objects.log").c_str());
//	UpdateSettings();
	
	sphere_reflection_loaded = false;
	
	//object_list = NULL;
	model_list = NULL;
}

OBJECTS::~OBJECTS()
{
	error_log.close();
	
	//deallocate objects and models
	DeleteAll();
}

void OBJECTS::DeleteAll()
{
	object_list_cached = false;
	object_list_skybox.clear();
	object_list_normal.clear();
	object_list_blend.clear();
	object_list_skybox_and_blend.clear();
	
	for (map <string, TEXTURE_HANDLE>::iterator i = texture_db.begin(); i != texture_db.end(); i++)
	{
		//glDeleteTextures(1, &(i->second));
		i->second.Unload();
	}
	
	if (sphere_reflection_loaded)
	{
		//glDeleteTextures(1, &sphere_reflection);
		sphere_reflection.Unload();
	}
	
	texture_db.clear();
	
	object_list.clear();
	
	//while (object_list != NULL)	delobject();
	
	while (model_list != NULL)
		delmodel();
}

/*void OBJECTS::delobject()
{
	if (object_list == NULL)
		return;
	
	//save old tree
	OBJECTNODE * old = object_list;
	object_list = object_list->next;
	delete old;
}*/

void OBJECTS::delmodel()
{
	if (model_list == NULL)
		return;
	
	//save old tree
	OBJECTMODEL * old = model_list;
	model_list = model_list->next;
	delete old;
}

void OBJECTS::UpdateSettings()
{
	settings.Get( "display.view_distance", lod_far );
	settings.Get( "display.width", display_x );
	settings.Get( "display.height", display_y );
	settings.Get( "display.FOV", field_of_view );
}

//returns true if object is drawn
bool OBJECTS::DrawObject(OBJECTNODE * object, float max_view_dist, bool envmap, bool lightmap, bool cubelighting)
{
	//int i;
	//float yoffset = 0.0f;
	float dx, dy, dz, rc;
	
	//range cull
	VERTEX objcenter;
	//cout << "Drawing " << object->model->name << ", " << &(object->model->jmodel.GetBBOX()) << ", " << &(object->model->jmodel.GetBBOX().GetCenter()) << ", " << &objcenter << endl;
	//cout << "Next: " << object->model->name << endl;
	//cout << "Drawing " << object->model->jmodel.GetModelPath() << ", " << &(object->model->jmodel.GetBBOX()) << ", " << &(object->model->jmodel.GetBBOX().GetCenter()) << ", " << &objcenter << endl;
	//objcenter.Set(object->model->jmodel.GetBBOX().GetCenter());
	float test123 = object->model->jmodel.GetBBOX().GetCenter().x;
	objcenter.x = test123;
	objcenter.x = object->model->jmodel.GetBBOX().GetCenter().x;
	objcenter.y = object->model->jmodel.GetBBOX().GetCenter().y;
	objcenter.z = object->model->jmodel.GetBBOX().GetCenter().z;
	float temp = objcenter.x;
	objcenter.x = objcenter.y;
	objcenter.y = objcenter.z;
	objcenter.z = temp;
	VERTEX objpos = object->pos + objcenter;
	dx=objpos.x+cam.position.x; dy=objpos.y+cam.position.y; dz=objpos.z+cam.position.z;
    rc=dx*dx+dy*dy+dz*dz;
	
	bool zerotransform = false;
	if (object->pos.x == 0 && object->pos.y == 0 && object->pos.z == 0)
		zerotransform = true;
	
	if (!object->model->skybox)
	{
		float temp_lod_far = max_view_dist + object->model->jmodel.GetRadius();
		if (rc > temp_lod_far*temp_lod_far) return false;
	
		//use different techniques based on range
		//frustum cull!
		float bound, rd;
		//if (rc > lod_far)
		{
			//bound = spread*2.0f;
			bound = object->model->jmodel.GetRadius();
			int i;
			for (i=0;i<6;i++) 
			{
				rd=cam.frustum[i][0]*objpos.x+
				   cam.frustum[i][1]*objpos.y+
				   cam.frustum[i][2]*objpos.z+
				   cam.frustum[i][3];
				if (rd<=-bound)
				{
					return false;
				}
			}
		}
	}
	
	if (object->model->skybox)
	{
	}
	else if (!zerotransform)
	{
		glPushMatrix();
		GLfloat transform_matrix[16];
		object->dir.GetMat(transform_matrix);
		
		glTranslatef(object->pos.x, object->pos.y, object->pos.z);
		glMultMatrixf(transform_matrix);
	}
	
	if (object->model != NULL)
	{
		object->model->jmodel.DrawStatic(envmap, cubelighting, lightmap);
	}
	else
		cout << "NULL model" << endl;
	
	if (object->model->skybox)
	{
	}
	else if (!zerotransform)
		glPopMatrix();
	
	return true;
}

void OBJECTS::Draw(bool cull) {Draw(cull, lod_far, field_of_view, (float)display_x / (float)display_y);}

void OBJECTS::Draw(bool cull, float viewdist, float FOV, float hwratio, bool envmap, bool lightmap)
{
	glPushAttrib(GL_ALL_ATTRIB_BITS);	
	glPushMatrix();

	//setup global opengl flags
	
	glLightModeli(GL_LIGHT_MODEL_TWO_SIDE, 1);
		
	glColorMaterial (GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE);
	glEnable (GL_COLOR_MATERIAL);
	glColor3f (1.0, 1.0, 1.0);
	GLfloat specular [] = { 0.0, 0.0, 0.0, 0.0 };
	glMaterialfv (GL_FRONT_AND_BACK, GL_SPECULAR, specular);
	GLfloat shininess [] = { 0.0 };
	glMaterialfv (GL_FRONT_AND_BACK, GL_SHININESS, shininess);
	
	glEnable(GL_BLEND);
	//glDisable(GL_DEPTH_TEST);
	glAlphaFunc(GL_GREATER, 0.9f);
	glEnable(GL_ALPHA_TEST);
	glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
	glCullFace(GL_BACK);
	if (cull)
		glEnable(GL_CULL_FACE);
	else
		glDisable(GL_CULL_FACE);
	//glDepthMask(0);
	glColor4f(1,1,1,1);
	
	glRotated(-90, 1,0,0);
	
	bool cubelighting = (world.StaticCubeLighting() || world.DynamicCubeLighting()) && !envmap;
	
	unsigned int drawcount = 0;
	
	//skybox pass
	glDisable(GL_LIGHTING);
	glDisable(GL_FOG);
	glMatrixMode( GL_PROJECTION );
	glPushMatrix();
	glLoadIdentity( );
	gluPerspective( FOV, hwratio, 0.1f, 100000.0 );
	glMatrixMode( GL_MODELVIEW );
	glPushMatrix();
	GLdouble temp_matrix[16];
	cam.PutTransformInto(temp_matrix);
	glLoadMatrixd(temp_matrix);
	//glDepthMask(0);
	if (verticaltrackingskyboxes) glTranslatef(0,cam.GetPosition().y,0);
	glRotated(-90, 1,0,0);
	if (!object_list_cached)
	{
		//OBJECTNODE * curpos = object_list;
		//while (curpos != NULL)
		for (list <OBJECTNODE>::iterator curpos = object_list.begin(); curpos != object_list.end(); curpos++)
		{
			if (curpos->model->skybox)
			{
				if (curpos->model->blend)
					object_list_skybox_and_blend.push_back(&(*curpos));
				else
					object_list_skybox.push_back(&(*curpos));
			}
			else if (curpos->model->blend)
			{
				object_list_blend.push_back(&(*curpos));
			}
			else
			{
				object_list_normal.push_back(&(*curpos));
			}
		}
	}
	object_list_cached = true;
	//else
	{
		for (list <OBJECTNODE *>::iterator i = object_list_skybox.begin(); i != object_list_skybox.end(); i++)
		{
			if (DrawObject(*i, lod_far, envmap, false, cubelighting))
				drawcount++;
		}
	}
	{
		glDisable(GL_ALPHA_TEST);
		//glDepthMask(0);
		for (list <OBJECTNODE *>::iterator i = object_list_skybox_and_blend.begin(); i != object_list_skybox_and_blend.end(); i++)
		{
			if (DrawObject(*i, lod_far, envmap, false, cubelighting))
				drawcount++;
		}
		glEnable(GL_ALPHA_TEST);
		//glDepthMask(1);
	}
	glMatrixMode( GL_PROJECTION );
	glPopMatrix();
	glMatrixMode( GL_MODELVIEW );
	//glDepthMask(1);
	glPopMatrix();
	
	glClear(GL_DEPTH_BUFFER_BIT);
	
	if (cubelighting)
	{
		world.ActivateTextureBasedLighting(true);
		glDisable(GL_LIGHTING);
	}
	else
		glEnable(GL_LIGHTING);
	
	glEnable(GL_FOG);
	
	//normal pass
	/*if (!object_list_cached)
	{
		OBJECTNODE * curpos = object_list;
		while (curpos != NULL)
		{
			if (!curpos->model->skybox && !curpos->model->blend)
			{
				//DrawObject(curpos, viewdist, envmap, lightmap, cubelighting);
				object_list_normal.push_back(curpos);
			}
			curpos = curpos -> next;
		}
	}*/
	//else
	{
		for (list <OBJECTNODE *>::iterator i = object_list_normal.begin(); i != object_list_normal.end(); i++)
		{
			if (DrawObject(*i, viewdist, envmap, lightmap, cubelighting))
				drawcount++;
		}
	}
	
	if (cubelighting)
	{
		world.DeactivateTextureBasedLighting();
		
		//world.SetLighting(false);
	}
	
	//blend pass
	if (!(envmap && lightmap))
	{
		glDisable(GL_ALPHA_TEST);
		glDepthMask(0);
		glPolygonOffset(-10.0,-10.0);
		glEnable(GL_POLYGON_OFFSET_FILL);
		
		/*if (!object_list_cached)
		{
			OBJECTNODE * curpos = object_list;
			while (curpos != NULL)
			{
				if (curpos->model->blend)
				{
					//DrawObject(curpos, viewdist, envmap, lightmap, cubelighting);
					object_list_blend.push_back(curpos);
				}
				curpos = curpos -> next;
			}
		}*/
		//else
		{
			for (list <OBJECTNODE *>::iterator i = object_list_blend.begin(); i != object_list_blend.end(); i++)
			{
				if (DrawObject(*i, viewdist, envmap, lightmap, cubelighting))
					drawcount++;
			}
		}
		
		glEnable(GL_ALPHA_TEST);
		glDepthMask(1);
		glDisable(GL_POLYGON_OFFSET_FILL);
	}
	
	glPopMatrix();
	glPopAttrib();
	
	//reset gl flags
	glDisable(GL_BLEND);
	glColor4f(1.0f,1.0f,1.0f,1.0f);
	glEnable( GL_TEXTURE_2D );
	glEnable( GL_LIGHTING);
	glDisable(GL_ALPHA_TEST);
	
	//cout << "Drew " << drawcount << "/" << object_list.size() << " objects" << endl;
}

OBJECTNODE * OBJECTS::Add(VERTEX pos, float rotation, string modelname, string texname, bool mip, bool fullbright, bool skybox, bool drv, bool col, bool blend, JOEPACK * pack, float f1, float f2, float bl, float bm, float rr, float rd)
{
	/*OBJECTNODE * oldfirst = object_list;
	object_list = new OBJECTNODE;
	object_list->next = oldfirst;*/
	
	{
		OBJECTNODE newnode;
		object_list.push_back(newnode);
	}
	
	object_list.back().pos.x = pos.x;
	//object_list->pos.y = pos.y;
	object_list.back().pos.z = pos.z;
	
	object_list.back().driveable = drv;
	object_list.back().cancollide = col;
	
	object_list.back().friction1 = f1;
	object_list.back().friction2 = f2;
	
	object_list.back().bumplength = bl;
	object_list.back().bumpmag = bm;
	
	object_list.back().rolling_resistance_factor = rr;
	object_list.back().rolling_drag = rd;
	
	//if (pos.y == -1337)
	{
//		object_list->pos.y = terrain.GetHeight(object_list->pos.x, object_list->pos.z) + pos.y;
	}
	
	//apply rotation (no longer used)
	//object_list->dir.Rotate(rotation, 0, 1, 0);
	
	OBJECTMODEL * mplist = model_list;
	
	bool found = false;
	int count = 0;
	while (mplist != NULL && !found)
	{
		//cout << mplist->name << "," << modelname << endl;
		if (mplist->name == modelname)
		{
			found = true;
			object_list.back().model = mplist;
		}
		mplist = mplist -> next;
		count++;
	}
	
	if (!found)
	{
		object_list.back().model = AddModel(modelname, texname, mip, fullbright, skybox, blend, pack);
		object_list.back().texture = texname;
	}
	
	return &(object_list.back());
}

OBJECTNODE::OBJECTNODE()
{
	dir.LoadMultIdent();
}

OBJECTMODEL * OBJECTS::AddModel(string modelname, string texname, bool mip, bool fullbright, bool skybox, bool blend, JOEPACK * pack)
{
	OBJECTMODEL * oldfirst = model_list;
	model_list = new OBJECTMODEL;
	model_list->next = oldfirst;
	
	model_list->name = modelname;
	model_list->jmodel.Load(path + "/" + modelname, false, pack);
	
	model_list->fullbright = fullbright;
	model_list->blend = blend;
	model_list->skybox = skybox;
	
	//search for the texture in our texture db
	map <string, TEXTURE_HANDLE>::iterator tslot = texture_db.find(texname);
	if (tslot == texture_db.end())
	{
		TEXTURE_HANDLE newtexid;
		newtexid.Load(path + "/" + texname, mip);
		texture_db[texname] = newtexid;
		//cout << texname << ": " << newtexid << endl;
		model_list->jmodel.TextureID(&newtexid, 0);
	}
	else
	{
		model_list->jmodel.TextureID(&(tslot->second), 0);
	}
	
	//model_list->jmodel.ReflectionTextureID(sphere_reflection, 1);
	
	//cout << "Addmodel: " << modelname << endl;
	return model_list;
}

extern void LoadingScreen(string loadtext);

void OBJECTS::LoadObjectsFromFolder(string objectpath)
{
	DeleteAll();
	collision.Clear();
	
	//sphere_reflection = textures.Load("weather/trackreflect.png", true);
	//sphere_reflection_loaded = true;
	
	JOEPACK pack;
	JOEPACK * packptr = NULL;
	
	if (pack.LoadPack(objectpath+"/objects.jpk"))
	{
		packptr = &pack;
		//cout << "Using object pack" << endl;
	}
	
	OBJECTNODE * added = NULL;
	
	ifstream o;
	
	o.open((objectpath+"/list.txt").c_str());
	
	path = objectpath;
	
	if (o)
	{
		string m;
		string t;
		string extra;
		bool mip;
		bool fb, sb;
		bool blend;
		VERTEX p;
		float r;
		bool c, d;
		float f1, f2;
		float bl, bm;
		float rr, rd;
		
		int count = 0;
		
		const int expectedparams = 14;
		int numparams = expectedparams;
		numparams = utility.iGetParam(o);
		
		if (numparams != expectedparams)
		{
			cerr << "Parameters per entry number incorrect, error in object list: " << objectpath+"/list.txt" << endl;
			cerr << "Expected " << expectedparams << " parameters, got " << numparams << endl;
			cerr << "Check the format.txt file in the VDrift-trackeditor/listedit/format.txt folder." << endl;
			return;
		}
		
		while (!o.eof())
		{
			if (count % 100 == 0)
			{
				//char buffer[256];
				//sprintf(buffer, "Loading...\nLoading scenery objects\n%i", count);
				//LoadingScreen(buffer);
				string msg = "Loading...\nLoading scenery objects";
				for (int sn = 0; sn < count/100; sn++)
					msg = msg + ".";
				LoadingScreen(msg);
			}
			
			m = utility.sGetParam(o);
			t = utility.sGetParam(o);
			mip = utility.bGetParam(o);
			fb = utility.bGetParam(o);
			sb = utility.bGetParam(o);
			//p.x = utility.fGetParam(o);
			blend = utility.bGetParam(o);
			//p.y = utility.fGetParam(o);
			//p.z = utility.fGetParam(o);
			bl = utility.fGetParam(o);
			bm = utility.fGetParam(o);
			//r = utility.fGetParam(o);
			p.zero();
			r = 0;
			d = utility.bGetParam(o);
			c = utility.bGetParam(o);
			f1 = utility.fGetParam(o);
			f2 = utility.fGetParam(o);
			rr = utility.fGetParam(o);
			rd = utility.fGetParam(o);
			
			for (int i = 0; i < numparams - expectedparams; i++)
			{
				extra = utility.sGetParam(o);
			}
			
			added = NULL;
			
			if (m != "" && m != utility.GetEOFString())
			{
				added = Add(p, r, m, t, mip, fb, sb, d, c, blend, packptr, f1, f2, bl, bm, rr, rd);
				count++;
				//cout << "added object " << m << "...";
				unsigned int i;
				for (i = 0; i < added->model->jmodel.GetFaces(); i++)
				{
					/*short vi[3];
					VERTEX tri[3];
					VERTEX norms[3];
					for (unsigned int v = 0; v < 3; v++)
					{
						vi[v] = added->model->jmodel.GetFace(i)[v];
						tri[v].Set(added->model->jmodel.GetVert(vi[v]));
						norms[v].Set(added->model->jmodel.GetNorm(added->model->jmodel.GetNormIdx(i)[v]));
					}
					VERTEX norm;
					for (unsigned int v = 0; v < 3; v++)
						norm = norm + norms[v];
					norm = norm.normalize();
					VERTEX tnorm = (tri[2] - tri[0]).cross(tri[1] - tri[0]);
					if (norm.dot(tnorm) > 0)
					{
						short tvi = vi[1];
						vi[1] = vi[2];
						vi[2] = tvi;
					}
					collision.AddColNode(added, vi);*/
					collision.AddColNode(added, added->model->jmodel.GetFace(i));
				}
				//cout << "done" << endl;
			}
		}
		
		o.close();

		collision.GenerateCollisionTree();
		
		GroupObjectListByTexture();
	}
	else
	{
		error_log << "Couldn't open Object List: " << objectpath << "/list.txt" << endl;
	}
	
	if (packptr != NULL)
		packptr->ClosePack();
}

bool OBJECTS::Collide(VERTEX origin, VERTEX direction, VERTEX &outtri, bool closest, VERTEX & normal, float seglen, OBJECTNODE * &colnode)
{
	return collision.CollideAABB(origin, direction, outtri, closest, normal, seglen, colnode);
}

bool OBJECTS::CollideD(VERTEXD origin, VERTEXD direction, VERTEXD &outtri, bool closest, VERTEXD & normal, double seglen)
{
	return collision.CollideAABB_double(origin, direction, outtri, closest, normal, seglen);
}

bool OBJECTS::CollideDriveable(VERTEX origin, VERTEX direction, VERTEX &outtri, bool closest, VERTEX & normal)
{
	return collision.CollideDriveable(origin, direction, outtri, closest, normal);
}

bool OBJECTS::CollideModel(VERTEX * modelverts, int numfaces, AABB bbox, VERTEX & outtri, bool closest, VERTEX & normal, float & depth)
{
	return collision.CollideModelAABB(modelverts, numfaces, bbox, outtri, closest, normal, depth);
}

OBJCOLNODE::OBJCOLNODE()
{
	object = NULL;
	int i;
	for (i = 0; i < 3; i++)
		vertexIndex[i] = 0;
}

bool OBJCOLNODE::operator==(const OBJCOLNODE & other)
{
	return (object == other.object && vertexIndex[0] == other.vertexIndex[0] &&
			vertexIndex[1] == other.vertexIndex[1] &&
			vertexIndex[2] == other.vertexIndex[2]);
}

bool OBJCOLNODE::EqualGeom(const OBJCOLNODE & other)
{
	return (vertexIndex[0] == other.vertexIndex[0] &&
			vertexIndex[1] == other.vertexIndex[1] &&
			vertexIndex[2] == other.vertexIndex[2]);
}

bool OBJCOLNODE::operator<(const OBJCOLNODE & other)
{
	bool output = false;
	
	int cmpresult = strcmp(object->model->name.c_str(),other.object->model->name.c_str());
	
	if (cmpresult < 0)
		output = true;
	else if (cmpresult == 0)
	{	
		if (vertexIndex[0] < other.vertexIndex[0])
			output = true;
		else if (vertexIndex[0] == other.vertexIndex[0])
		{
			if (vertexIndex[1] < other.vertexIndex[1])
				output = true;
			else if (vertexIndex[1] == other.vertexIndex[1])
			{
				if (vertexIndex[2] < other.vertexIndex[2])
					output = true;
			}
		}
	}
	
	return output;
}

OBJCOLNODE::OBJCOLNODE(const OBJCOLNODE & other)
{
	object = other.object;
	vertexIndex[0] = other.vertexIndex[0];
	vertexIndex[1] = other.vertexIndex[1];
	vertexIndex[2] = other.vertexIndex[2];
}

OBJCOLNODE& OBJCOLNODE::operator= (const OBJCOLNODE &other)
{
	object = other.object;
	vertexIndex[0] = other.vertexIndex[0];
	vertexIndex[1] = other.vertexIndex[1];
	vertexIndex[2] = other.vertexIndex[2];
	
	return *this;
}

void OBJCOLNODE::SortVerts()
{
	short omin, omax, omid;
	omin = omax = omid = 0;
	bool havemin = false;
	bool havemax = false;
	for (int i = 0; i < 3; i++)
	{
		if (!havemin)
		{
			omin = vertexIndex[i];
			havemin = true;
		}

		if (!havemax)
		{
			omax = vertexIndex[i];
			havemax = true;
		}
		
		if (vertexIndex[i] < omin)
			omin = vertexIndex[i];
		if (vertexIndex[i] > omax)
			omax = vertexIndex[i];
	}
	
	for (int i = 0; i < 3; i++)
	{
		if (vertexIndex[i] > omin && vertexIndex[i] < omax)
			omid = vertexIndex[i];
	}
	
	vertexIndex[0] = omin;
	vertexIndex[1] = omid;
	vertexIndex[2] = omax;
}

void OBJECTCOLLISION::AddColNode(OBJECTNODE * newobject, short * newvi)
{
	OBJCOLNODE temp;
	temp.object = newobject;
	int i;
	for (i = 0; i < 3; i++)
		temp.vertexIndex[i] = newvi[i];
	
	//temp.SortVerts();
	
	if (COLLIDE_AND_DRIVE_TOGETHER)
	{
		if (newobject->cancollide || newobject->driveable)
		{
			colnodes.push_back(temp);
			drvnodes.push_back(temp);
		}
	}
	else
	{
		if (newobject->cancollide)
		{
			colnodes.push_back(temp);
		}
		
		if (newobject->driveable)
		{
			drvnodes.push_back(temp);
		}
	}
}

bool OBJECTCOLLISION::CollideDriveable(VERTEX origin, VERTEX direction, VERTEX &outtri, bool closest, VERTEX & normal)
{
	list <OBJCOLNODE>::iterator i1 = drvnodes.begin();
	
	bool hadcollision = false;
	VERTEX curtri[3], tvert;
	int retval;
	float t,u,v;
	
	origin.z = -origin.z;
	
	int count = 0;
	
	while (i1 != drvnodes.end())
	{
		//collide = curnode->model->jmodel.Collide(origin - curnode->pos, direction, tvert, closest);
		count++;
		
		curtri[0].Set(i1->object->model->jmodel.GetVert(i1->vertexIndex[0]));
		curtri[1].Set(i1->object->model->jmodel.GetVert(i1->vertexIndex[1]));
		curtri[2].Set(i1->object->model->jmodel.GetVert(i1->vertexIndex[2]));
		
		//curtri = curtri + ((VERTEX) (i1->object)->pos);
		int n;
		for (n = 0; n < 3; n++)
			curtri[n] = curtri[n] + i1->object->pos;
			
		retval = INTERSECT_FUNCTION(origin.v3(), direction.v3(),
			curtri[0].v3(), curtri[1].v3(), curtri[2].v3(), 
			&t, &u, &v);
		
		if (retval)
		{
			if (t < 0)
			{
				//no collision
				retval = 0;
			}
			else
			{
				//collision
				tvert = curtri[0].ScaleR(1-u-v) + curtri[1].ScaleR(u) + curtri[2].ScaleR(v);
				
				//calculate normal
				curtri[0].z = -curtri[0].z;
				curtri[1].z = -curtri[1].z;
				curtri[2].z = -curtri[2].z;
				normal = (curtri[2] - curtri[0]).cross(curtri[1] - curtri[0]);
				normal = normal.normalize();
				
				if (normal.dot(direction) > 0)
					normal.Scale(-1);
				
				if (!closest)
				{
					outtri = tvert;
					
					//implicit caching
					OBJCOLNODE temp;
					temp = *i1;
					
					//cout << colnodes.size() << endl;
					//cout << count << endl;
					
					drvnodes.erase(i1);
					drvnodes.insert(drvnodes.begin(), temp);
					
					return true;
				}
			}
		}

		//collide = curnode->model->jmodel.Collide(curnode->pos - origin, direction, tvert, closest);
		//tvert = ;
		if (retval && closest)
		{
			if ((hadcollision && (origin - tvert).len() < (origin - outtri).len()) || !hadcollision)
			{
				//outtri = tvert + curnode->pos;
				outtri = tvert;
			}
			
			hadcollision = true;
		}
		
		i1++;
	}
	
	if (closest && hadcollision)
	{
		return true;
	}
	
	//cout << "nocol: " << count << endl;
	
	return false;
}

OBJCOLBRANCH::OBJCOLBRANCH()
{
	left = NULL;
	right = NULL;
	leaves.clear();
}

void OBJCOLBRANCH::DeleteChildren()
{
	leaves.clear();
	
	if (left != NULL)
	{
		left->DeleteChildren();
		delete left;
		left = NULL;
	}
	
	if (right != NULL)
	{
		right->DeleteChildren();
		delete right;
		right = NULL;
	}
}

void OBJECTCOLLISION::GenerateCollisionTree()
{
	bool verbose = false;
	
	if (verbose)
		cout << "Generating collision tree..." << endl;
	
	coltree.DeleteChildren();
	
	if (verbose)
		cout << "deleted old tree" << endl;
	
	if (verbose)
		cout << "Sorting collision nodes...";
	//colnodes.sort();
	if (verbose)
		cout << "done" << endl;
	
	coltree.left = NULL;
	coltree.right = NULL;
	
	coltree.leaves.clear();
	
	//while (curnode != NULL)
	int count = 0;
	
	for (list <OBJCOLNODE>::iterator i = colnodes.begin(); i != colnodes.end(); i++)
	{
		bool dup = false;
		
		/*if (i != colnodes.begin())
		{
			list <OBJCOLNODE>::iterator prev = i;
			prev--;
			if (*prev == *i)
				dup = true;
		}*/
		
		if (!dup)
		{
			coltree.leaves.push_back(&(*i));
			count++;
		}
	}
	
	if (verbose)
		cout << "copied leaves: " << count << "/" << colnodes.size() << endl;
	
	GenerateBranches(&coltree);
	
	if (verbose)
		cout << "done" << endl;
}

void OBJECTCOLLISION::GenerateBranches(OBJCOLBRANCH * branch)
{
	bool verbose = false;
	
	list <OBJCOLNODE *>::iterator i1 = branch->leaves.begin();
	
	//build total aabb and find average vert center
	VERTEX avgcenter;
	float maxv[3];
	float minv[3];
	bool havevals[6];
	int n = 0;
	for (n = 0; n < 6; n++)
		havevals[n] = false;
	//maxvals.Set(-1e10,-1e10,-1e10);
	//minvals.Set(1e10,1e10,1e10);
	
	int numleaves = 0;
	
	for (i1 = branch->leaves.begin(); i1 != branch->leaves.end(); i1++)
		numleaves++;
	
	i1 = branch->leaves.begin();
	while (i1 != branch->leaves.end())
	{
		VERTEX p[3];
		p[0].Set((*i1)->object->model->jmodel.GetVert((*i1)->vertexIndex[0]));
		p[1].Set((*i1)->object->model->jmodel.GetVert((*i1)->vertexIndex[1]));
		p[2].Set((*i1)->object->model->jmodel.GetVert((*i1)->vertexIndex[2]));
		
		int i, c;
		for (c = 0; c < 3; c++)
		{
			float * v = p[c].v3();
			for (i = 0; i < 3; i++)
			{
				if (v[i] > maxv[i] || !havevals[i])
				{
					maxv[i] = v[i];
					havevals[i] = true;
				}
				if (v[i] < minv[i] || !havevals[i+3])
				{
					minv[i] = v[i];
					havevals[i+3] = true;
				}
			}
		}
		
		VERTEX pcenter = p[0] + p[1] + p[2];
		pcenter.Scale(0.333333);
		avgcenter = avgcenter + pcenter.ScaleR(1.0/numleaves);
		
		i1++;
	}
	
	VERTEX minvals, maxvals;
	minvals.Set(minv);
	maxvals.Set(maxv);
	branch->bbox.SetFromCorners(minvals, maxvals);
	if (verbose)
	{
		cout << "Bounding box:" << endl;
		branch->bbox.GetPos().DebugPrint();
		branch->bbox.GetSize().DebugPrint();
		branch->bbox.GetCenter().DebugPrint();
	}
	
	//find axis of maximum change
	VERTEX axismask;
	axismask.Set(1,0,0);
	if (maxvals.x - minvals.x > maxvals.y - minvals.y && maxvals.x - minvals.x > maxvals.z - minvals.z)
	{
		axismask.Set(1,0,0);
	}
	else if (maxvals.y - minvals.y > maxvals.x - minvals.x && maxvals.y - minvals.y > maxvals.z - minvals.z)
	{
		axismask.Set(0,1,0);
	}
	else if (maxvals.z - minvals.z > maxvals.y - minvals.y && maxvals.z - minvals.z > maxvals.x - minvals.x)
	{
		axismask.Set(0,0,1);
	}
	
	int ll = 0;
	int lr = 0;
	
	//only propagate leaves if we're not a leaf
	if (numleaves > TRIANGLES_PER_BBOX)
	{
		//create children
		branch->left = new OBJCOLBRANCH;
		branch->right = new OBJCOLBRANCH;
		
		int distributor = 0; //hack to stop infinite recursion
		
		//throw leaves into children
		for (i1 = branch->leaves.begin(); i1 != branch->leaves.end(); i1++)
		{
			VERTEX p[3];
			p[0].Set((*i1)->object->model->jmodel.GetVert((*i1)->vertexIndex[0]));
			p[1].Set((*i1)->object->model->jmodel.GetVert((*i1)->vertexIndex[1]));
			p[2].Set((*i1)->object->model->jmodel.GetVert((*i1)->vertexIndex[2]));
			
			VERTEX pcenter = p[0] + p[1] + p[2];
			pcenter.Scale(0.333333);
			
			if (pcenter.dot(axismask) - avgcenter.dot(axismask) > 0.0)
			{
				branch->right->leaves.push_back(*i1);
				distributor = 1;
			}
			else if (pcenter.dot(axismask) - avgcenter.dot(axismask) < 0.0)
			{
				branch->left->leaves.push_back(*i1);
				distributor = 0;
			}
			else
			{
				//leaf is right at average, distribute evenly
				if (distributor % 2 == 0)
					branch->right->leaves.push_back(*i1);
				else
					branch->left->leaves.push_back(*i1);
				
				distributor++;
			}
		}
		
		//clear out leaves
		branch->leaves.clear();
		
		//count leaves of children
		for (i1 = branch->left->leaves.begin(); i1 != branch->left->leaves.end(); i1++)
			ll++;
		for (i1 = branch->right->leaves.begin(); i1 != branch->right->leaves.end(); i1++)
			lr++;
		
		if (verbose) cout << "Parent Leaves: " << numleaves << " L: " << ll << " R: " << lr << endl;
		
		if (ll == 0 || lr == 0)
		{
			if (lr != 0)
			{
				for (i1 = branch->right->leaves.begin(); i1 != branch->right->leaves.end(); i1++)
					branch->leaves.push_back(*i1);
				branch->right->leaves.clear();
			}
			
			if (ll != 0)
			{
				for (i1 = branch->left->leaves.begin(); i1 != branch->left->leaves.end(); i1++)
					branch->leaves.push_back(*i1);
				branch->left->leaves.clear();
			}
			
			delete branch->left;
			branch->left = NULL;
			
			delete branch->right;
			branch->right = NULL;
		}
		else
		{
			GenerateBranches(branch->left);
			GenerateBranches(branch->right);
		}
		
		/*if (ll == 0)
		{
			delete branch->left;
			branch->left = NULL;
		}
		else if GenerateBranches(branch->left);
		
		if (lr == 0)
		{
			delete branch->right;
			branch->right = NULL;
		}
		else GenerateBranches(branch->right);*/
	}
	else
	{
		branch->right = NULL;
		branch->left = NULL;
		
		if (verbose) cout << "(Leaf)" << endl;
	}
}

void OBJECTCOLLISION::Clear()
{
	coltree.DeleteChildren();
	colnodes.clear();
	drvnodes.clear();
}

OBJECTCOLLISION::~OBJECTCOLLISION()
{
	Clear();
}

bool OBJECTCOLLISION::CollideModelAABB(VERTEX * modelverts, int numfaces, AABB modelbbox, VERTEX & outtri, bool closest, VERTEX & normal, float & depth)
{
	normal.zero();
	
	int testcount = 0;
	
	bool col = CollideBranchModel(modelverts, numfaces, modelbbox, outtri, closest, &coltree, normal, depth, testcount);
	
	//cout << "Leaf tests: " << testcount << endl;
	
	normal = normal.normalize();
	
	return col;
}

bool OBJECTCOLLISION::CollideAABB(VERTEX origin, VERTEX direction, VERTEX &outtri, bool closest, VERTEX & normal, float seglen, OBJECTNODE * &colnode)
{
	bool col = false;
	
	/*if (lastcolpatch != NULL)
	{
		col = lastcolpatch->BEZIER_COLLIDE_FUNCTION(origin, direction, outtri);
		if (col && !closest)
		{
			colpatch = lastcolpatch;

			return true;
		}
	}*/
	
	origin.z = -origin.z;
	int count = 0;
	col = CollideBranch(origin, direction, outtri, closest, &coltree, normal, seglen, count, colnode);
	//cout << count << endl;
	/*if (col)
		lastcolpatch = colpatch;*/
	
	return col;
}

bool OBJECTCOLLISION::CollideAABB_double(VERTEXD origin, VERTEXD direction, VERTEXD &outtri, bool closest, VERTEXD & normal, double seglen)
{
	bool col = false;
	
	origin.z = -origin.z;
	direction.z = -direction.z;
	
	col = CollideBranch_double(origin, direction, outtri, closest, &coltree, normal, seglen);
	
	return col;
}

bool OBJECTCOLLISION::CollideBranch(VERTEX origin, VERTEX direction, VERTEX &outtri, bool closest, OBJCOLBRANCH * branch, VERTEX & normal, float seglen, int & testcount, OBJECTNODE * &colnode)
{
	if (branch == NULL)
		return false;
	
	bool verbose = false;
	bool collideverbose = false;
	
	if (verbose)
	{
		if (branch->left != NULL && branch->right != NULL)
		{
			cout << "Parent" << endl;
		}
		else
			cout << "Leaf" << endl;
	}
	
	//what to do if we're a leaf
	if (branch->left == NULL && branch->right == NULL)
	{
		testcount++;
		
		bool hadcollision = false;
		VERTEX curtri[3], tvert;
		VERTEX tnorm;
		float t,u,v;
		
		int retval = 0;
		
		list <OBJCOLNODE *>::iterator i1 = branch->leaves.begin();
		
		for (i1 = branch->leaves.begin(); i1 != branch->leaves.end(); i1++)
		{
			curtri[0].Set((*i1)->object->model->jmodel.GetVert((*i1)->vertexIndex[0]));
			curtri[1].Set((*i1)->object->model->jmodel.GetVert((*i1)->vertexIndex[1]));
			curtri[2].Set((*i1)->object->model->jmodel.GetVert((*i1)->vertexIndex[2]));
			
			//curtri = curtri + ((VERTEX) (i1->object)->pos);
			int n;
			for (n = 0; n < 3; n++)
				curtri[n] = curtri[n] + (*i1)->object->pos;
				
			retval = INTERSECT_FUNCTION(origin.v3(), direction.v3(),
				curtri[0].v3(), curtri[1].v3(), curtri[2].v3(), 
				&t, &u, &v);
			
			if (retval)
			{
				if (t < 0)
				{
					//no collision
					retval = 0;
				}
				else
				{
					//collision
					tvert = curtri[0].ScaleR(1-u-v) + curtri[1].ScaleR(u) + curtri[2].ScaleR(v);
					tvert.z = -tvert.z;
					
					//calculate normal
					curtri[0].z = -curtri[0].z;
					curtri[1].z = -curtri[1].z;
					curtri[2].z = -curtri[2].z;
					normal = (curtri[2] - curtri[0]).cross(curtri[1] - curtri[0]);
					normal = normal.normalize();
					
					if (normal.dot(direction) > 0)
						normal.Scale(-1);
					
					if (collideverbose)
					{
						cout << "TUV: " << t << "," << u << "," << v << endl;
						direction.DebugPrint();
						normal.DebugPrint();
						tvert.DebugPrint();
						cout << endl;
					}
					
					if (!closest)
					{
						outtri = tvert;
						
						colnode = (*i1)->object;
						
						return true;
					}
				}
			}
	
			//collide = curnode->model->jmodel.Collide(curnode->pos - origin, direction, tvert, closest);
			//tvert = ;
			if (retval && closest)
			{
				if ((hadcollision && (origin - tvert).len() < (origin - outtri).len()) || !hadcollision)
				{
					//outtri = tvert + curnode->pos;
					outtri = tvert;
					colnode = (*i1)->object;
				}
				
				hadcollision = true;
			}
		}
		
		if (closest && hadcollision)
		{
			return true;
		}
		
		return false;
	}
	
	if (verbose)
	{
		origin.DebugPrint();
		branch->bbox.DebugPrint();
	}
	
	//check bounding box
	if (!branch->bbox.IntersectRay(origin, direction))
		return false;
	
	/*if (!branch->bbox.IntersectSegment(origin, direction, seglen))
		return false;*/
	
	if (verbose)
		cout << "BBOX collision" << endl;
	
	bool rcol = false;
	bool lcol = false;

	VERTEX r_outtri, l_outtri;
	OBJECTNODE * r_colnode = NULL;
	OBJECTNODE * l_colnode = NULL;
	
	if (right != NULL)
		rcol = CollideBranch(origin, direction, r_outtri, closest, branch->right, normal, seglen, testcount, r_colnode);
		
	//collision on the right branch
	/*if (rcol && !closest)
	{
		outtri = r_outtri;
		colpatch = r_colpatch;
		return rcol;
	}*/
	
	if (left != NULL)
		lcol = CollideBranch(origin, direction, l_outtri, closest, branch->left, normal, seglen, testcount, l_colnode);
	
	//collision on the left branch only
	if (lcol && !rcol)
	{
		outtri = l_outtri;
		colnode = l_colnode;
		return lcol;
	}
	
	//collision on the right branch only
	if (rcol && !lcol)
	{
		outtri = r_outtri;
		colnode = r_colnode;
		return rcol;
	}
	
	
	//collision on both branches
	if (lcol && rcol)
	{
		if ((origin - r_outtri).len() < (origin - l_outtri).len())
		{
			outtri = r_outtri;
			colnode = r_colnode;
			return rcol;
		}
		else
		{
			outtri = l_outtri;
			colnode = l_colnode;
			return lcol;
		}
	}
	
	return false;
}

bool OBJECTCOLLISION::CollideBranch_double(VERTEXD origin, VERTEXD direction, VERTEXD &outtri, bool closest, OBJCOLBRANCH * branch, VERTEXD & normal, double seglen)
{
	if (branch == NULL)
		return false;
	
	bool verbose = false;
	
	if (verbose)
	{
		if (branch->left != NULL && branch->right != NULL)
		{
			cout << "Parent" << endl;
		}
		else
			cout << "Leaf" << endl;
	}
	
	//what to do if we're a leaf
	if (branch->left == NULL && branch->right == NULL)
	{
		bool hadcollision = false;
		VERTEXD curtri[3], tvert;
		VERTEXD tnorm;
		double t,u,v;
		
		int retval = 0;
		
		list <OBJCOLNODE *>::iterator i1 = branch->leaves.begin();
		
		for (i1 = branch->leaves.begin(); i1 != branch->leaves.end(); i1++)
		{
			curtri[0].Set(((*i1)->object->model->jmodel.GetVert((*i1)->vertexIndex[0])));
			curtri[1].Set(((*i1)->object->model->jmodel.GetVert((*i1)->vertexIndex[1])));
			curtri[2].Set(((*i1)->object->model->jmodel.GetVert((*i1)->vertexIndex[2])));
			
			//curtri = curtri + ((VERTEX) (i1->object)->pos);
			int n;
			for (n = 0; n < 3; n++)
				curtri[n] = curtri[n] + (*i1)->object->pos;
				
			retval = utility.IntersectTriangleD(origin.v3(), direction.v3(),
				curtri[0].v3(), curtri[1].v3(), curtri[2].v3(), 
				&t, &u, &v);
			
			if (retval)
			{
				if (t < 0)
				{
					//no collision
					retval = 0;
				}
				else
				{
					//collision...?
					tvert = curtri[0].ScaleR(1-u-v) + curtri[1].ScaleR(u) + curtri[2].ScaleR(v);
					
					//if ((tvert - origin).len() > seglen || (tvert-origin).dot(direction) <= 0)
					double dist = (tvert-origin).dot(direction);
					if (dist <= 0 || dist > seglen)
					{
						/*cout << "Discarded: " << endl;
						tvert.DebugPrint();
						origin.DebugPrint();
						direction.DebugPrint();
						cout << (tvert-origin).dot(direction) << endl << endl;*/
						
						retval = 0;
					}
					else
					{
						/*cout << "Not discarded: " << endl;
						tvert.DebugPrint();
						origin.DebugPrint();
						direction.DebugPrint();
						cout << (tvert-origin).dot(direction) << endl << endl;*/
						
						//calculate normal
						curtri[0].z = -curtri[0].z;
						curtri[1].z = -curtri[1].z;
						curtri[2].z = -curtri[2].z;
						tnorm = (curtri[2] - curtri[0]).cross(curtri[1] - curtri[0]);
						tnorm = tnorm.normalize();
						VERTEXD tdir;
						tdir.Set(direction.x, direction.y, -direction.z);
						if (tnorm.dot(tdir) > 0)
							tnorm.Scale(-1);
						
						/*direction.DebugPrint();
						normal.DebugPrint();
						cout << endl;*/
						
						if (!closest)
						{
							tvert.z = -tvert.z;
							outtri = tvert;
							normal = tnorm;
							return true;
						}
					}
				}
			}
	
			//collide = curnode->model->jmodel.Collide(curnode->pos - origin, direction, tvert, closest);
			//tvert = ;
			if (retval && closest)
			{
				if ((hadcollision && (origin - tvert).len() < (origin - outtri).len()) || !hadcollision)
				{
					//outtri = tvert + curnode->pos;
					tvert.z = -tvert.z;
					outtri = tvert;
					normal = tnorm;
				}
				
				hadcollision = true;
			}
		}
		
		if (closest && hadcollision)
		{
			return true;
		}
		
		return false;
	}
	
	if (verbose)
	{
		origin.DebugPrint();
		branch->bbox.DebugPrint();
	}
	
	//check bounding box
	/*if (!branch->bbox.IntersectRay(origin, direction))
		return false;*/
	
	VERTEX originf, directionf;
	float seglenf;
	seglenf = seglen;
	originf.Set(origin.x, origin.y, origin.z);
	directionf.Set(direction.x, direction.y, direction.z);
	
	if (!branch->bbox.IntersectSegment(originf, directionf, seglenf))
		return false;
	
	if (verbose)
		cout << "BBOX collision" << endl;
	
	bool rcol = false;
	bool lcol = false;

	VERTEXD r_outtri, l_outtri;
	
	if (right != NULL)
		rcol = CollideBranch_double(origin, direction, r_outtri, closest, branch->right, normal, seglen);
		
	//collision on the right branch
	/*if (rcol && !closest)
	{
		outtri = r_outtri;
		colpatch = r_colpatch;
		return rcol;
	}*/
	
	if (left != NULL)
		lcol = CollideBranch_double(origin, direction, l_outtri, closest, branch->left, normal, seglen);
	
	//collision on the left branch only
	if (lcol && !rcol)
	{
		outtri = l_outtri;
		return lcol;
	}
	
	//collision on the right branch only
	if (rcol && !lcol)
	{
		outtri = r_outtri;
		return rcol;
	}
	
	//collision on both branches
	if (lcol && rcol)
	{
		VERTEXD originz;
		originz.Set(origin.x, origin.y, -origin.z);
		
		if ((originz - r_outtri).len() < (originz - l_outtri).len())
		{
			outtri = r_outtri;
			return rcol;
		}
		else
		{
			outtri = l_outtri;
			return lcol;
		}
	}
	
	return false;
}

bool OBJECTCOLLISION::CollideBranchModel(VERTEX * modelverts, int numfaces, AABB & modelbbox, VERTEX & outtri, bool closest, OBJCOLBRANCH * branch, VERTEX & normal, float & depth, int & testcount)
{
	if (branch == NULL)
		return false;
	
	bool verbose = false;
	
	if (verbose)
	{
		if (branch->left != NULL && branch->right != NULL)
		{
			cout << "Parent" << endl;
		}
		else
			cout << "Leaf" << endl;
	}
	
	//what to do if we're a leaf
	if (branch->left == NULL && branch->right == NULL)
	{
		testcount++;
		
		//cout << "Leaf " << endl;
		VERTEX cartri[3];
		VERTEX objecttri[3];
		
		list <OBJCOLNODE *>::iterator i1 = branch->leaves.begin();
		
		for (i1 = branch->leaves.begin(); i1 != branch->leaves.end(); i1++)
		{
			objecttri[0].Set(((*i1)->object->model->jmodel.GetVert((*i1)->vertexIndex[0])));
			objecttri[1].Set(((*i1)->object->model->jmodel.GetVert((*i1)->vertexIndex[1])));
			objecttri[2].Set(((*i1)->object->model->jmodel.GetVert((*i1)->vertexIndex[2])));
			int n;
			for (n = 0; n < 3; n++)
				objecttri[n] = objecttri[n] + (*i1)->object->pos;
		
			int f, v;
			
			for (f = 0; f < numfaces; f++)
			{
				for (v = 0; v < 3; v++)
					cartri[v] = modelverts[f*3+v];
				
				int which = 0;
				VERTEX colpt, coldest, colorig;
				int retval = 0;
				float colpt1[3], colpt2[3];
				int coplanar;
				float destdepth = 0.0;
				float origdepth = 0.0;
				
				bool newisect = true;
				
				//retval = utility.BruteForceTriangleIntersectionF(cartri, objecttri, colpt, coldest, colorig, which);
				if (newisect)
					retval = utility.MollerTriTriIsectWithLine(cartri[0].v3(), cartri[1].v3(), cartri[2].v3(),
						objecttri[0].v3(), objecttri[1].v3(), objecttri[2].v3(), &coplanar, colpt1, colpt2);
				else
					//retval = utility.BruteForceTriangleIntersectionF2(cartri, objecttri, colpt, coldest, colorig, which, destdepth, origdepth);
					retval = utility.BruteForceTriangleIntersectionF(cartri, objecttri, colpt, coldest, colorig, which);
				
				if (newisect)
				{
					colorig.Set(colpt1);
					coldest.Set(colpt2);
					colpt = (coldest+colorig).ScaleR(0.5);
				}
				
				//if (f == 0)
				if (0)
				{
					for (n = 0; n < 3; n++)
						objecttri[n].DebugPrint();
					
					cout << "---" << endl;
					
					for (n = 0; n < 3; n++)
						cartri[n].DebugPrint();
					
					cout << endl;
				}
				
				if (retval)
				{
					//cout << " col " << endl;
					
					VERTEX tcolpt;
					tcolpt.Set(colpt.x, colpt.y, -colpt.z);
					VERTEX tcoldest;
					tcoldest.Set(coldest.x, coldest.y, -coldest.z);
					VERTEX tcolorig;
					tcolorig.Set(colorig.x, colorig.y, -colorig.z);
					
					VERTEX tobjecttri[3], tcartri[3];
					VERTEX tnormd, tnorm;
					//cout << "object tri: " << endl;
					for (n = 0; n < 3; n++)
					{
						tcartri[n].Set(cartri[n].x, cartri[n].y, -cartri[n].z);
						tobjecttri[n].Set(objecttri[n].x, objecttri[n].y, -objecttri[n].z);
						//tobjecttri[n].DebugPrint();
					}
					
					tnorm = (tobjecttri[2] - tobjecttri[0]).cross(tobjecttri[1] - tobjecttri[0]);
					tnorm = tnorm.normalize();
					
					if (which == 0)
					{
						tnormd = (tcartri[2] - tcartri[0]).cross(tcartri[1] - tcartri[0]);
						tnormd = tnormd.normalize();
					}
					else
						tnormd = tnorm;
					
					tnormd = tnorm;
					
					//hack to try to fix normals that are pointing the wrong way
					VERTEX carcenter = modelbbox.GetCenter();
					carcenter.z = -carcenter.z;
					VERTEX tdir;
					tdir = tcolpt - carcenter;
					if (tnorm.dot(tdir) > 0)
						tnorm.Scale(-1);
					if (tnormd.dot(tdir) > 0)
						tnormd.Scale(-1);
					
				float candidatedepth = destdepth;
					
					//adjust the collision info to make sure the depth is
					// into the surface
					if ((tcoldest-tcolorig).dot(tnormd) > 0)
					{
						VERTEX tvert;
						tvert = tcoldest;
						tcoldest = tcolorig;
						tcolorig = tvert;
						candidatedepth = origdepth;						
					}
					
					//find the depth
					float tdepth = (tcolpt - tcoldest).dot(tnormd);
					
					if (newisect)
						tdepth = (tcolpt - tcoldest).len();
					//else
						//tdepth = candidatedepth;
					
					//cout << tdepth << endl;
					
					//if (!closest)
					{
						outtri = tcolpt;
						normal = tnorm;
						normal = tnormd;
						depth = tdepth;

						if (verbose)
						{
							tcolorig.DebugPrint();
							tcoldest.DebugPrint();
							outtri.DebugPrint();
							tnormd.DebugPrint();
							normal.DebugPrint();
							//carcenter.DebugPrint();
							cout << depth << endl << endl;
						}
						
						return true;
					}
				}
			}
		}
		
		return false;
		
		/*bool hadcollision = false;
		VERTEXD curtri[3], tvert;
		VERTEXD tnorm;
		double t,u,v;
		
		int retval = 0;
		
		list <OBJCOLNODE *>::iterator i1 = branch->leaves.begin();
		
		for (i1 = branch->leaves.begin(); i1 != branch->leaves.end(); i1++)
		{
			curtri[0].Set(((*i1)->object->model->jmodel.GetVert((*i1)->vertexIndex[0])));
			curtri[1].Set(((*i1)->object->model->jmodel.GetVert((*i1)->vertexIndex[1])));
			curtri[2].Set(((*i1)->object->model->jmodel.GetVert((*i1)->vertexIndex[2])));
			
			//curtri = curtri + ((VERTEX) (i1->object)->pos);
			int n;
			for (n = 0; n < 3; n++)
				curtri[n] = curtri[n] + (*i1)->object->pos;
				
			retval = utility.IntersectTriangleD(origin.v3(), direction.v3(),
				curtri[0].v3(), curtri[1].v3(), curtri[2].v3(), 
				&t, &u, &v);
			
			if (retval)
			{
				if (t < 0)
				{
					//no collision
					retval = 0;
				}
				else
				{
					//collision...?
					tvert = curtri[0].ScaleR(1-u-v) + curtri[1].ScaleR(u) + curtri[2].ScaleR(v);
					
					//if ((tvert - origin).len() > seglen || (tvert-origin).dot(direction) <= 0)
					double dist = (tvert-origin).dot(direction);
					if (dist <= 0 || dist > seglen)
					{
						
						retval = 0;
					}
					else
					{
						
						//calculate normal
						curtri[0].z = -curtri[0].z;
						curtri[1].z = -curtri[1].z;
						curtri[2].z = -curtri[2].z;
						tnorm = (curtri[2] - curtri[0]).cross(curtri[1] - curtri[0]);
						tnorm = tnorm.normalize();
						VERTEXD tdir;
						tdir.Set(direction.x, direction.y, -direction.z);
						if (tnorm.dot(tdir) > 0)
							tnorm.Scale(-1);
						
						if (!closest)
						{
							tvert.z = -tvert.z;
							outtri = tvert;
							normal = tnorm;
							return true;
						}
					}
				}
			}
	
			//collide = curnode->model->jmodel.Collide(curnode->pos - origin, direction, tvert, closest);
			//tvert = ;
			if (retval && closest)
			{
				if ((hadcollision && (origin - tvert).len() < (origin - outtri).len()) || !hadcollision)
				{
					//outtri = tvert + curnode->pos;
					tvert.z = -tvert.z;
					outtri = tvert;
					normal = tnorm;
				}
				
				hadcollision = true;
			}
		}
		
		if (closest && hadcollision)
		{
			return true;
		}
		
		return false;*/
	}
	
	if (verbose)
	{
		branch->bbox.DebugPrint();
		modelbbox.DebugPrint();
		cout << endl;
	}
	
	//check bounding box
	/*if (!branch->bbox.IntersectRay(origin, direction))
		return false;*/
		
	if (!branch->bbox.IntersectAABB(modelbbox))
		return false;
	
	if (verbose)
		cout << "BBOX collision" << endl;
	
	//modelbbox.DebugPrint();
	
	bool rcol = false;
	bool lcol = false;

	VERTEX r_outtri, l_outtri;
	VERTEX r_normal, l_normal;
	float r_depth, l_depth;
	
	if (right != NULL)
		rcol = CollideBranchModel(modelverts, numfaces, modelbbox, r_outtri, closest, branch->right, r_normal, r_depth, testcount);
		
	//collision on the right branch
	/*if (rcol && !closest)
	{
		outtri = r_outtri;
		colpatch = r_colpatch;
		return rcol;
	}*/
	
	if (left != NULL)
		lcol = CollideBranchModel(modelverts, numfaces, modelbbox, l_outtri, closest, branch->left, l_normal, l_depth, testcount);
	
	//collision on the left branch only
	if (lcol && !rcol)
	{
		outtri = l_outtri;
		normal = normal + l_normal;
		depth = l_depth;
		return lcol;
	}
	
	//collision on the right branch only
	if (rcol && !lcol)
	{
		outtri = r_outtri;
		normal = normal + r_normal;
		depth = r_depth;
		return rcol;
	}
	
	//collision on both branches
	if (lcol && rcol)
	{	
		//if ((modelbbox.GetCenter() - r_outtri).len() < (modelbbox.GetCenter() - l_outtri).len())
		//if (r_depth > l_depth)
		if (r_normal.y*r_normal.y < l_normal.y*l_normal.y)
		//if (1)
		{
			outtri = r_outtri;
			normal = r_normal;
			//normal = normal + r_normal + l_normal;
			depth = r_depth;
			return rcol;
		}
		else
		{
			outtri = l_outtri;
			normal = l_normal;
			//normal = normal + r_normal + l_normal;
			depth = l_depth;
			return lcol;
		}
		
		/*outtri = (r_outtri + l_outtri).ScaleR(0.5);
		normal = (r_normal + l_normal).normalize();
		depth = (l_depth + r_depth)*0.5;*/
	}
	
	return false;
}

void OBJECTS::GroupObjectListByTexture()
{
	object_list.sort();
	/*map <string, list <OBJECTNODE *> > grouper;
	
	//do the group
	OBJECTNODE * curpos = object_list;
	while (curpos != NULL)
	{
		grouper[curpos->texture].push_back(curpos);
		curpos = curpos -> next;
	}
	
	//re-order object list
	OBJECTNODE * * ptrtoptr = &object_list;
	for (map <string, list <OBJECTNODE *> >::iterator g = grouper.begin(); g != grouper.end(); g++)
	{
		for (list <OBJECTNODE *>::iterator i = g->second.begin(); i != g->second.end(); i++)
		{
			*ptrtoptr = *i;
			ptrtoptr = &((*i)->next);
		}
	}
	
	*ptrtoptr = NULL;*/
}

void OBJECTS::GetTrisInBBox(AABB bbox, list <OBJECTTRI> & trilist)
{
	collision.GetTrisInBBox(bbox, trilist);
}

void OBJECTCOLLISION::GetTrisInBBox(AABB bbox, list <OBJECTTRI> & trilist)
{
	CollideBranchGetTrisInBBox(bbox, trilist, &coltree);
}

bool OBJECTCOLLISION::CollideBranchGetTrisInBBox(AABB getbbox, list <OBJECTTRI> & trilist, OBJCOLBRANCH * branch)
{
	if (branch == NULL)
		return false;
	
	bool verbose = false;
	
	if (verbose)
	{
		if (branch->left != NULL && branch->right != NULL)
		{
			cout << "Parent" << endl;
		}
		else
			cout << "Leaf" << endl;
	}
	
	//what to do if we're a leaf
	if (branch->left == NULL && branch->right == NULL)
	{
		list <OBJCOLNODE *>::iterator i1 = branch->leaves.begin();
		
		for (i1 = branch->leaves.begin(); i1 != branch->leaves.end(); i1++)
		{
			OBJECTTRI tri;
			tri.v1.Set(((*i1)->object->model->jmodel.GetVert((*i1)->vertexIndex[0])));
			tri.v2.Set(((*i1)->object->model->jmodel.GetVert((*i1)->vertexIndex[1])));
			tri.v3.Set(((*i1)->object->model->jmodel.GetVert((*i1)->vertexIndex[2])));
			trilist.push_back(tri);
		}

		//return false;
		return true;
	}
	
	if (verbose)
	{
		branch->bbox.DebugPrint();
		getbbox.DebugPrint();
		cout << endl;
	}
		
	if (!branch->bbox.IntersectAABB(getbbox))
		return false;
	
	if (verbose)
		cout << "BBOX collision" << endl;
	
	//modelbbox.DebugPrint();
	
	bool rcol = false;
	bool lcol = false;
	
	if (right != NULL)
		rcol = CollideBranchGetTrisInBBox(getbbox, trilist, branch->right);
	
	if (left != NULL)
		lcol = CollideBranchGetTrisInBBox(getbbox, trilist, branch->left);
	
	//collision on the left branch only
	if (lcol && !rcol)
	{
		return lcol;
	}
	
	//collision on the right branch only
	if (rcol && !lcol)
	{
		return rcol;
	}
	
	//collision on both branches
	if (lcol && rcol)
	{	
		return true;
	}
	
	return false;
}
