/* vim: set noexpandtab shiftwidth=8 cino= fo+=awc:
 ***************************************************************************
 *            utility.cc
 *
 *  Sat Mar 26 08:52:54 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.
 */

#define GL_GLEXT_PROTOTYPES

#include "utility.h"
#include "textures.h"
#include <fstream>

#ifdef _WIN32
//#define GL_GLEXT_PROTOTYPES
#include <GL/glext.h>
#include <GL/glut.h>
//#include <GL/wglext.h>
//#include <GL/glprocs.h>
#include <windows.h>
#endif

#ifdef _WIN32
PFNGLMULTITEXCOORD2FARBPROC     pglMultiTexCoord2f     = NULL;
PFNGLMULTITEXCOORD4FARBPROC     pglMultiTexCoord4f     = NULL;
PFNGLCLIENTACTIVETEXTUREARBPROC	pglActiveTexture       = NULL;
PFNGLACTIVETEXTUREARBPROC       pglClientActiveTexture = NULL;
#endif

//includes needed to get folder contents
#ifndef _WIN32
#include <sys/types.h>
#include <dirent.h>
#endif

const float DEG2RAD = 3.14159 / 180.0;

void UTILITY::SelectTU(int TU)
{
	if (!initdone)
	{
	    initerror();
	    return;
	}

	GLenum tuenum;
	
	if (TU == 0)
		tuenum = GL_TEXTURE0_ARB;
	else if (TU == 1)
		tuenum = GL_TEXTURE1_ARB;
	else if (TU == 2)
		tuenum = GL_TEXTURE2_ARB;
	else if (TU == 3)
		tuenum = GL_TEXTURE3_ARB;
	else
		tuenum = GL_TEXTURE0_ARB;
	
	#ifdef _WIN32
	pglActiveTexture(tuenum);
	#else
	glActiveTextureARB(tuenum);
	#endif
}

void UTILITY::TexCoord2d2f(int TU, float u, float v)
{
	if (!initdone)
	{
	    initerror();
	    return;
	}

	GLenum tuenum;
	
	if (TU+1 > nb_multitexture)
		return;
	
	if (TU == 0)
		tuenum = GL_TEXTURE0_ARB;
	else if (TU == 1)
		tuenum = GL_TEXTURE1_ARB;
	else if (TU == 2)
		tuenum = GL_TEXTURE2_ARB;
	else if (TU == 3)
		tuenum = GL_TEXTURE3_ARB;
	else
		tuenum = GL_TEXTURE0_ARB;
	
	#ifdef _WIN32
	pglMultiTexCoord2f(tuenum, u, v);
	#else
	glMultiTexCoord2fARB(tuenum, u, v);
	#endif
}

GLuint UTILITY::TexLoad(string filename, int format, bool mipmap, bool cube, int &w, int &h, const bool supressederror, bool &err)
{
	return TexLoad(filename, format, mipmap, cube, w, h, supressederror, err, 0);
}

GLuint UTILITY::TexLoad(string filename, int format, bool mipmap, bool cube, int &w, int &h, const bool supressederror, bool &err, int attempt)
{
	string filepath;
	char buffer[1024];
	//filepath = DATA_DIR + "/tex/" + filename;

	switch (attempt)
	{
		case 0:
			filepath = filename;
			break;
		
		case 1:
			filepath = settings.GetFullDataPath(filename);
			break;
		
		case 2:
			//filepath = settings.GetFullDataPath("textures/" + texture_size + "/" + filename);
			filepath = settings.GetFullDataPath("textures/" + filename);
			break;
		
		default:
			filepath = filename;
			break;
	}

	//cout << "Trying to load texture " << filepath << endl;
	
	strcpy(buffer, filepath.c_str());
	
	GLuint new_handle = 0;
	
	//*** Load Texture ***
	SDL_Surface *TextureImage; // Create Storage Space For The Texture
	TextureImage = NULL;

	// Load The Bitmap, Check For Errors, If Bitmap's Not Found Quit
	if ((TextureImage=IMG_Load(buffer)))
	{
		new_handle = TexLoad(filename, TextureImage, format, mipmap, cube, w, h, supressederror, err); 
	}
	else
	{
		/*err = true;
		
	    //try once more
		//cout << "initially failed: " << buffer << endl;
		int try2 = -1;
		if (filepath.find(settings.GetDataDir() + "/textures") > filepath.length())
		{
			try2 = TexLoad(settings.GetFullDataPath("textures/" + settings.GetTexSize() + "/" + filepath), format, mipmap, w, h, supressederror, err);
		}
		//quit, bitmap not found
		if (err)
		{
			if (!supressederror) error_log << "Could not find texture: " << buffer << "\n";
			return 0;
		}
		else
		{
			err = false;
			return try2;
		}*/
		
		attempt++;
		
		if (attempt > 2)
		{
			err = true;
			if (!supressederror) error_log << "Could not find texture: " << filename << "\n";
			return 0;
		}
		else
		{
			return TexLoad(filename, format, mipmap, cube, w, h, supressederror, err, attempt);
		}
	}

	if (TextureImage) // If Texture Exists
	{
		// Free up any memory we may have used
		SDL_FreeSurface( TextureImage );
	}
	
	return new_handle;
}


GLuint UTILITY::TexLoad(string filename, SDL_Surface* surface, int format, bool mipmap, bool cube, int &w, int &h, const bool supressederror, bool &err)
{
	string texture_size;
	settings.Get( "display.texture_size", texture_size );

	GLuint new_handle = 0;
	
	//*** Load Texture ***
	SDL_Surface *TextureImage; // Create Storage Space for resized surface (if necessary)
	TextureImage = NULL;

	SDL_Surface * curtexture = surface;
	
	//scale texture down if necessary
	if (texture_size == "medium" || texture_size == "small")
	{
		int maxsize = 256;
		float minscale = 0.5;
		if (texture_size == "small")
		{
			maxsize = 128;
			minscale = 0.25;
		}
		float scalew = 1.0;
		float scaleh = 1.0;
		float scale = 1.0;
		
		if (surface->w > maxsize)
			scalew = (float)maxsize / surface->w;
		if (surface->h > maxsize)
			scaleh = (float)maxsize / surface->h;
		
		if (scalew < scaleh)
			scale = scalew;
		else
			scale = scaleh;
		
		if (scale < minscale) scale = minscale;
		
		if (scale < 1.0)
		{
			TextureImage = zoomSurface (surface, scale, scale, SMOOTHING_ON);
			curtexture = TextureImage;
		}
	}
	
	w = curtexture->w;
	h = curtexture->h;
	
	if (cube)
	{
		glGenTextures(1, &new_handle);
		glBindTexture(GL_TEXTURE_CUBE_MAP_ARB, new_handle);
		
		w = curtexture->w/3;
		h = curtexture->h/4;
		
		//cout << curtexture->w/3 << "," << curtexture->h/4 << endl;
		
		for (int i = 0; i < 6; i++)
		{
			//detect channels
			int format = GL_RGB;
			switch (curtexture->format->BytesPerPixel)
			{
				case 1:
					format = GL_LUMINANCE;
					break;
				case 2:
					format = GL_LUMINANCE_ALPHA;
					break;
				case 3:
					format = GL_RGB;
					break;
				case 4:
					format = GL_RGBA;
					break;
				default:
					//throw EXCEPTION(__FILE__, __LINE__, "Texture has unknown format: " + texture_info.GetName() + " (" + cubefile + ")");
					cerr << "Texture has unknown format: " + filename << endl;
					err = true;
					return 0;
					break;
			}
			
			/*if (format != GL_RGB)
			{
				//throw EXCEPTION(__FILE__, __LINE__, "Cube map texture format isn't GL_RGB (this causes problems for some reason): " + texture_info.GetName() + " (" + cubefile + ")");
				cerr << "Cube map texture isn't GL_RGB (but should be): " + filename + " has " << (int) curtexture->format->BytesPerPixel << " channels" << endl;
				err = true;
				return 0;
			}*/
			
			int offsetx = 0;
			int offsety = 0;
			
			GLenum targetparam;
			if (i == 0)
			{
				targetparam = GL_TEXTURE_CUBE_MAP_NEGATIVE_X_ARB;
				offsetx = 0;
				offsety = h;
			}
			else if (i == 1)
			{
				targetparam = GL_TEXTURE_CUBE_MAP_POSITIVE_X_ARB;
				offsetx = w*2;
				offsety = h;
			}
			else if (i == 2)
			{
				targetparam = GL_TEXTURE_CUBE_MAP_NEGATIVE_Y_ARB;
				offsetx = w;
				offsety = h*2;
			}
			else if (i == 3)
			{
				targetparam = GL_TEXTURE_CUBE_MAP_POSITIVE_Y_ARB;
				offsetx = w;
				offsety = 0;
			}
			else if (i == 4)
			{
				targetparam = GL_TEXTURE_CUBE_MAP_NEGATIVE_Z_ARB;
				offsetx = w;
				offsety = h*3;
			}
			else if (i == 5)
			{
				targetparam = GL_TEXTURE_CUBE_MAP_POSITIVE_Z_ARB;
				offsetx = w;
				offsety = h;
			}
			else
			{
				//throw EXCEPTION(__FILE__, __LINE__, "Iterated too far: " + texture_info.GetName() + " (" + cubefile + ")");
				cerr << __FILE__ << ", line " << __LINE__ << ": Iterater too far: " + filename << endl;
				err = true;
				return 0;
			}
			
			//cout << "Loading " << filename << " into id " << new_handle << endl;
			
			/*offsetx = 0;
			offsety = 0;*/
			
			unsigned char cubeface[w*h*curtexture->format->BytesPerPixel];
			
			if (i == 4) //special case for negative z
			{
				for (int yi = 0; yi < h; yi++)
				{
					for (int xi = 0; xi < w; xi++)
					{
						for (int ci = 0; ci < curtexture->format->BytesPerPixel; ci++)
						{
							int idx1 = ((h-yi-1)+offsety)*curtexture->w*curtexture->format->BytesPerPixel + (w-xi-1+offsetx)*curtexture->format->BytesPerPixel + ci;
							int idx2 = yi*w*curtexture->format->BytesPerPixel+xi*curtexture->format->BytesPerPixel+ci;
							cubeface[idx2] = ((unsigned char *)(curtexture->pixels))[idx1];
							//cout << idx1 << "," << idx2 << endl;
						}
					}
				}
			}
			else
			{
				for (int yi = 0; yi < h; yi++)
				{
					for (int xi = 0; xi < w; xi++)
					{
						for (int ci = 0; ci < curtexture->format->BytesPerPixel; ci++)
						{
							int idx1 = (yi+offsety)*curtexture->w*curtexture->format->BytesPerPixel+(xi+offsetx)*curtexture->format->BytesPerPixel+ci;
							int idx2 = yi*w*curtexture->format->BytesPerPixel+xi*curtexture->format->BytesPerPixel+ci;
							cubeface[idx2] = ((unsigned char *)(curtexture->pixels))[idx1];
							//cout << idx1 << "," << idx2 << endl;
						}
					}
				}
			}
			
			/*cubefiles[0] = game.pathmanager.GetTexturePathFromName(texture_info.GetName()+"-xp.png");
			cubefiles[1] = game.pathmanager.GetTexturePathFromName(texture_info.GetName()+"-xn.png");
			cubefiles[2] = game.pathmanager.GetTexturePathFromName(texture_info.GetName()+"-yn.png");
			cubefiles[3] = game.pathmanager.GetTexturePathFromName(texture_info.GetName()+"-yp.png");
			cubefiles[4] = game.pathmanager.GetTexturePathFromName(texture_info.GetName()+"-zn.png");
			cubefiles[5] = game.pathmanager.GetTexturePathFromName(texture_info.GetName()+"-zp.png");*/
			
			//glTexImage2D( targetparam, 0, format,texture_surface->w, texture_surface->h, 0, format, GL_UNSIGNED_BYTE, texture_surface->pixels );
			glTexImage2D( targetparam, 0, format, w, h, 0, format, GL_UNSIGNED_BYTE, cubeface );
			
			glTexParameteri(GL_TEXTURE_CUBE_MAP_ARB, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
			glTexParameteri(GL_TEXTURE_CUBE_MAP_ARB, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
			glTexParameteri(GL_TEXTURE_CUBE_MAP_ARB, GL_TEXTURE_WRAP_R, GL_CLAMP_TO_EDGE);
			glTexParameteri(GL_TEXTURE_CUBE_MAP_ARB,GL_TEXTURE_MAG_FILTER,GL_LINEAR);
			
			glTexParameteri(GL_TEXTURE_CUBE_MAP_ARB,GL_TEXTURE_MIN_FILTER,GL_LINEAR);
			
			glDisable(GL_TEXTURE_CUBE_MAP_ARB);
		}
	}
	else
	{
		switch (curtexture->format->BytesPerPixel)
		{
			case 1:
				format = GL_LUMINANCE;
				break;
			case 2:
				format = GL_LUMINANCE_ALPHA;
				break;
			case 3:
				format = GL_RGB;
				break;
			case 4:
				format = GL_RGBA;
				break;
			default:
				break;
		}
		
		glGenTextures(1, &new_handle);					// Create Texture
		
		// Create MipMapped Texture
		glBindTexture(GL_TEXTURE_2D, new_handle);
		glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT);
		glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT);
		glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MAG_FILTER,GL_LINEAR);
		
		if (mipmap)
		{
			glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MIN_FILTER,GL_LINEAR_MIPMAP_LINEAR);
			gluBuild2DMipmaps( GL_TEXTURE_2D, format, curtexture->w, curtexture->h, format, GL_UNSIGNED_BYTE, curtexture->pixels );
		}
		else
		{
			glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MIN_FILTER,GL_LINEAR);
			glTexImage2D( GL_TEXTURE_2D, 0, format, curtexture->w, curtexture->h, 0, format, GL_UNSIGNED_BYTE, curtexture->pixels );
		}
		
		//check for anisotropy
		int anisotropy = 0;
		settings.Get( "display.anisotropic", anisotropy );
		if (gfxcard.GetCapability(CARDINFOTYPE::ANISOTROPY) && anisotropy > 0)
		{
			//enable anisotropy based on level set in config file
			glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MAX_ANISOTROPY_EXT, (float)anisotropy);
		}
	}
	
	err = false;

	if (TextureImage) // If Texture Exists
	{
		// Free up any memory we may have used
		SDL_FreeSurface( TextureImage );
	}
	
	return new_handle;
}

GLuint UTILITY::TexLoad(string texfile, bool mipmap)
{
	return TexLoad(texfile, GL_RGBA, mipmap);
}

GLuint UTILITY::TexLoad(string filename, int format, bool mipmap)
{
	int dw, dh, new_handle;
	bool err = false;
	
	new_handle = TexLoad(filename, format, mipmap, false, dw, dh, false, err);
	
	return new_handle;
}

UTILITY::UTILITY()
{
	error_log.open((settings.GetSettingsDir() + "/logs/utility.log").c_str());
	initdone = false;
}

extern bool verbose_output;
UTILITY::~UTILITY()
{
	if (verbose_output)
		cout << "utility deinit" << endl;
	
	error_log.close();
}

void UTILITY::Init()
{
	//cout << "utility init test" << endl;

#ifdef _WIN32
	pglMultiTexCoord4f = (PFNGLMULTITEXCOORD4FARBPROC) wglGetProcAddress("glMultiTexCoord4fARB");
	pglMultiTexCoord2f = (PFNGLMULTITEXCOORD2FARBPROC) wglGetProcAddress("glMultiTexCoord2fARB");
	pglActiveTexture = (PFNGLCLIENTACTIVETEXTUREARBPROC) wglGetProcAddress("glActiveTextureARB");
	pglClientActiveTexture = (PFNGLACTIVETEXTUREARBPROC) wglGetProcAddress("glClientActiveTextureARB");
	
	if (pglMultiTexCoord4f == NULL)
		error_log << "main, WIN32: wglGetProcAddress(\"glMultiTexCoord4fARB\") failed." << endl;
	if (pglMultiTexCoord2f == NULL)
		error_log << "main, WIN32: wglGetProcAddress(\"glMultiTexCoord2fARB\") failed." << endl;
	if (pglActiveTexture == NULL)
		error_log << "main, WIN32: wglGetProcAddress(\"glActiveTextureARB\") failed." << endl;
	if (pglClientActiveTexture == NULL)
		error_log << "main, WIN32: wglGetProcAddress(\"glClientActiveTextureARB\") failed." << endl;
		
		//cout << "utility init test" << endl;
#endif


	glGetIntegerv( GL_MAX_TEXTURE_UNITS_ARB,&nb_multitexture );
	
	//nb_multitexture = 2;
	
	cout << "Multitexture units (4 are required for all effects): " << nb_multitexture << endl;
	if (nb_multitexture < 2)
		cout << "You have less than the recommended number of texture units." << endl << "Some effects will not be rendered." << endl << "Upgrade your graphics card!" << endl;
	else if (nb_multitexture <= 3)
		cout << "You have less than the recommended number of texture units." << endl << "Some textures will lack detail." << endl << "Upgrade your graphics card!" << endl;
	else
		cout << "Your GPU meets the texture unit requirements." << endl;
	
	cout << endl << "Note to user:  All error messages will be put in " + settings.GetSettingsDir() + "/logs/." << endl;

	initdone = true;
}

string UTILITY::sGetLine(ifstream &ffrom)
{
	string trashstr;
	char trashchar[1024];

	//ffrom >> trashstr;
	ffrom.getline(trashchar,1024,'\n');

	while ((trashchar[0] == '#' || strlen(trashchar) <= 1 ) && !ffrom.eof())
	{
		ffrom.getline(trashchar, 1024, '\n');
		//ffrom >> trashstr;
		//ffrom.getline(trashchar,1024,'\n');
	}
	trashstr = trashchar;

	if (ffrom.eof() && trashstr.length() == 0)
		return ENDOFFILESTRING;
	else
		return trashstr;
}

string UTILITY::sGetParam(ifstream &ffrom)
{
	string trashstr;
	char trashchar[1024];

	ffrom >> trashstr;

	while (trashstr.c_str()[0] == '#' && !ffrom.eof() && trashstr != "")
	{
		ffrom.getline(trashchar, 1024, '\n');
		ffrom >> trashstr;
	}
	
	if (ffrom.eof() && trashstr.c_str()[0] == '#')
		return ENDOFFILESTRING;
	else
		return trashstr;
}

int UTILITY::iGetParam(ifstream &ffrom)
{
	string trashstr;
	char trashchar[1024];

	ffrom >> trashstr;

	while (trashstr.c_str()[0] == '#' && !ffrom.eof())
	{
		ffrom.getline(trashchar, 1024, '\n');
		ffrom >> trashstr;
	}

	return atoi(trashstr.c_str());
}

float UTILITY::fGetParam(ifstream &ffrom)
{
	string trashstr;
	char trashchar[1024];

	ffrom >> trashstr;

	while (trashstr.c_str()[0] == '#' && !ffrom.eof())
	{
		ffrom.getline(trashchar, 1024, '\n');
		ffrom >> trashstr;
	}

	return atof(trashstr.c_str());
}

void UTILITY::initerror()
{
	//error_log << "Utility library not yet initialized" << endl;
}

void UTILITY::Tex2D(int TU, bool enable)
{
	if (!initdone)
	{
	    initerror();
	    return;
	}

	SelectTU(TU);
	if (enable)
	{
		glEnable(GL_TEXTURE_2D);
		//glEnable(GL_TEXTURE_CUBE_MAP_ARB);
	}
	else
	{
		glDisable(GL_TEXTURE_2D);
		glDisable(GL_TEXTURE_CUBE_MAP_ARB);
	}
}

bool UTILITY::bGetParam(ifstream &ffrom)
{
	string trashstr;
	char trashchar[1024];

	ffrom >> trashstr;

	while (trashstr.c_str()[0] == '#' && !ffrom.eof())
	{
		ffrom.getline(trashchar, 1024, '\n');
		ffrom >> trashstr;
	}
	
	if (trashstr == "true" || trashstr == "1" || trashstr == "on")
		return true;
	else
		return false;
}

int UTILITY::numTUs()
{
	if (!initdone)
	{
	    initerror();
	    return 0;
	}
	
	return nb_multitexture;
}

void UTILITY::Draw2D(float x1, float y1, float x2, float y2, const TEXTURE_HANDLE * const texid)
{
	Draw2D(x1, y1, x2, y2, texid, 0.0);
}

void UTILITY::Draw2D(float x1, float y1, float x2, float y2, const TEXTURE_HANDLE * const texid, float rotation)
{
	Draw2D(x1, y1, x2, y2, texid, rotation, 0);
}

void UTILITY::Draw2D(float x1, float y1, float x2, float y2, const TEXTURE_HANDLE * const texid, float rotation, int texsize)
{
	Draw2D(x1, y1, x2, y2, texid, rotation, texsize, 1.0);
}

void UTILITY::Draw2D(float x1, float y1, float x2, float y2, const TEXTURE_HANDLE * const texid, float rotation, int texsize, float opacity)
{
	int sx, sy;
	
	QUATERNION rot;
	rot.Rotate(rotation, 0,0,1);
	
	glPushAttrib(GL_ALL_ATTRIB_BITS);
	Tex2D(0, true);
	
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP);

	sx = 1600;
	sy = 1200;
	
	int x = (int) (x1*(float)sx);
	int y = (int) ((1.0-y1)*(float)sy);
	
	int w = (int) ((x2-x1)*(float)sx);
	int h = (int) ((y2-y1)*(float)sy);

	// Select our texture
	//glBindTexture( GL_TEXTURE_2D, texid );
	texid->Activate();

	// Disable depth testing 
	glDisable( GL_DEPTH_TEST );
	//and lighting...
	glDisable( GL_LIGHTING);
	glBlendFunc( GL_ONE_MINUS_SRC_ALPHA, GL_ONE_MINUS_SRC_COLOR);
	glBlendFunc( GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
	glEnable( GL_BLEND );

	// Select The Projection Matrix
	glMatrixMode( GL_PROJECTION );
	// Store The Projection Matrix
	glPushMatrix( );

	// Reset The Projection Matrix 
	glLoadIdentity( );
	// Set Up An Ortho Screen 
	glOrtho( 0, sx, 0, sy, -1, 1 );

	// Select The Modelview Matrix
	glMatrixMode( GL_MODELVIEW );
	// Stor the Modelview Matrix
	glPushMatrix( );
	// Reset The Modelview Matrix
	glLoadIdentity( );

	// Position The Text (0,0 - Bottom Left)
	glTranslated( x, y, 0 );

	/*// Choose The Font Set (0 or 1)
	glListBase( base - 32 + ( 128 * set ) );

	// Write The Text To The Screen
	glCallLists( strlen( string ), GL_BYTE, string );*/
	
	//glColor3f(1,1,1);
	glColor4f(1,1,1,opacity);
	
	/*for (unsigned int i = 0; i < strlen(string); i++)
	{
		if (string[i] == 32)
			glTranslated( 10, 0, 0 );
		else if (string[i] == '\n')
		{
			num_crs++;
			glLoadIdentity( );
			glTranslated(x,y-20*num_crs,0);
		}
		else
		{
			int pos = base - 32 + (128*set) + string[i];
			if (pos < 0)
				pos = 0;
			glCallList(pos);
		}
	}*/
	
	VERTEX v[4];
	v[0].Set(0,0,0);
	v[1].Set(w,0,0);
	v[2].Set(w,-h,0);
	v[3].Set(0,-h,0);
	
	int i;
	
	for (i = 0; i < 4; i++)
	{
		v[i].x -= w/2.0;
		v[i].y += h/2.0;
	}
	
	for (i = 0; i < 4; i++)
		v[i] = rot.RotateVec(v[i]);
	
	for (i = 0; i < 4; i++)
	{
		v[i].x += w/2.0;
		v[i].y -= h/2.0;
	}

	float tmin, tmax;
	tmin = 0;
	tmax = 1;
	if (texsize > 0)
	{
		tmin = 1.0/(float)texsize;
		tmax = ((float)texsize-1.0)/(float)texsize;
	}

	//draw box
	glBegin( GL_QUADS );
	        /* Texture Coord (Bottom Left) */
	        glTexCoord2f( tmin, tmin);
		/* Vertex Coord (Bottom Left) */
		glVertex2f( v[0].x, v[0].y );

		/* Texture Coord (Bottom Right) */
		glTexCoord2f( tmax, tmin);
		/* Vertex Coord (Bottom Right) */
		glVertex2f( v[1].x, v[1].y );

		/* Texture Coord (Top Right) */
		glTexCoord2f( tmax, tmax);
		/* Vertex Coord (Top Right) */
		glVertex2f( v[2].x, v[2].y );

		/* Texture Coord (Top Left) */
		glTexCoord2f( tmin, tmax);
		/* Vertex Coord (Top Left) */
		glVertex2f( v[3].x, v[3].y );
	glEnd( );

	// Select The Projection Matrix
	glMatrixMode( GL_PROJECTION );
	// Restore The Old Projection Matrix
	glPopMatrix( );
	
	// Select the Modelview Matrix
	glMatrixMode( GL_MODELVIEW );
	// Restore the Old Projection Matrix
	glPopMatrix( );

	// Re-enable Depth Testing
	glEnable( GL_DEPTH_TEST );
	//and lighting...
	glEnable( GL_LIGHTING);
	glDisable(GL_BLEND);
	
	glPopAttrib();
}

void UTILITY::DrawEllipse( float center_x, float center_y, float radius_x, float radius_y, float color_r, float color_g, float color_b, float opacity )
{
	int sx = 1600;
	int sy = 1200;
	int x = (int)( center_x * (float)sx );
        int y = (int)( ( 1.0 - center_y ) * (float)sy );

	glLoadIdentity();
	glTranslated( x, y, 0 );
	glColor4f( color_r, color_g, color_b, opacity );

	glBegin( GL_LINE_LOOP );

	for( int i = 0; i < 360; i++ )
	{
		//convert degrees into radians
		float rad = i * DEG2RAD;
		glVertex2f( cos( rad ) * radius_x, sin( rad ) * radius_y );
	}

	glEnd();
}

float UTILITY::GetValue(SDL_Surface * surf, int channel, float x, float y, bool interpolate)
{
	return GetValue(surf, channel, x, y, interpolate, false);
}

float UTILITY::GetValue(SDL_Surface * surf, int channel, float x, float y, bool interpolate, bool wrap)
{
	//check for NANs
	if (!(x < 0 || x >= 0) || !(y < 0 || y >= 0))
		return 0;
	
	if (channel >= surf->format->BytesPerPixel || channel < 0)
	{
		error_log << "Asked for channel " << channel << ", image only has " << surf->format->BytesPerPixel << endl;
		channel = surf->format->BytesPerPixel - 1;
	}
	
	{
		if (x < 0)
			x = 0;
		if (x > 1)
			x = 1;
		if (y < 0)
			y = 0;
		if (y > 1)
			y = 1;
		int ix, iy;
		ix = (int)(x * surf->w);
		iy = (int)(y * surf->h);
		float dx, dy;
		dx = x - ix;
		dy = y - iy;
		
		unsigned char * pix;
		pix = (unsigned char *) surf->pixels;
		
		if (ix >= surf->w)
		{
			if (wrap)
				ix = ix % surf->w;
			else
				ix = surf->w - 1;
		}
		if (iy >= surf->h)
		{
			if (wrap)
				iy = iy % surf->h;
			else
				iy = surf->h - 1;
		}
		
		if (!interpolate)
		{
			return pix[(ix+surf->pitch*iy)*surf->format->BytesPerPixel+channel] / 255.0f;
		}
		else
		{
			float ul, ur, ll, lr;
			
			int rx, ry;
			
			rx = ix + 1;
			if (rx >= surf->w)
			{
				if (wrap)
					rx = rx % surf->w;
				else
					rx = ix;
			}
			
			ry = iy + 1;
			if (ry >= surf->h)
			{
				if (wrap)
					ry = ry % surf->h;
				else
					ry = iy;
			}
			
			rx = ix;
			ry = iy;
			
			ul = pix[(ix+surf->pitch*iy)*surf->format->BytesPerPixel+channel] / 255.0f;
			ur = pix[(rx+surf->pitch*iy)*surf->format->BytesPerPixel+channel] / 255.0f;
			ll = pix[(ix+surf->pitch*ry)*surf->format->BytesPerPixel+channel] / 255.0f;
			lr = pix[(rx+surf->pitch*ry)*surf->format->BytesPerPixel+channel] / 255.0f;
			
			float u, l;
			
			u = ul*(1.0-dx)+ur*dx;
			l = ll*(1.0-dx)+lr*dx;
			
			return u*(1.0-dy)+l*dy;
		}
	}
}

bool UTILITY::FileExists(string filename)
{
	ifstream test;
	test.open(filename.c_str());
	if (test)
	{
		test.close();
		return true;
	}
	else
		return false;
}

int UTILITY::IntersectTriangleD(double orig[3], double dir[3],
                   double vert0[3], double vert1[3], double vert2[3],
                   double *t, double *u, double *v)
{
   double edge1[3], edge2[3], tvec[3], pvec[3], qvec[3];
   double det,inv_det;

   /* find vectors for two edges sharing vert0 */
   SUB(edge1, vert1, vert0);
   SUB(edge2, vert2, vert0);

   /* begin calculating determinant - also used to calculate U parameter */
   CROSS(pvec, dir, edge2);

   /* if determinant is near zero, ray lies in plane of triangle */
   det = DOT(edge1, pvec);

#ifdef TEST_CULL           /* define TEST_CULL if culling is desired */
   if (det < EPSILON)
      return 0;

   /* calculate distance from vert0 to ray origin */
   SUB(tvec, orig, vert0);

   /* calculate U parameter and test bounds */
   *u = DOT(tvec, pvec);
   if (*u < 0.0 || *u > det)
      return 0;

   /* prepare to test V parameter */
   CROSS(qvec, tvec, edge1);

    /* calculate V parameter and test bounds */
   *v = DOT(dir, qvec);
   if (*v < 0.0 || *u + *v > det)
      return 0;

   /* calculate t, scale parameters, ray intersects triangle */
   *t = DOT(edge2, qvec);
   inv_det = 1.0 / det;
   *t *= inv_det;
   *u *= inv_det;
   *v *= inv_det;
#else                    /* the non-culling branch */
   if (det > -EPSILON && det < EPSILON)
     return 0;
   inv_det = 1.0 / det;

   /* calculate distance from vert0 to ray origin */
   SUB(tvec, orig, vert0);

   /* calculate U parameter and test bounds */
   *u = DOT(tvec, pvec) * inv_det;
   if (*u < 0.0 || *u > 1.0)
     return 0;

   /* prepare to test V parameter */
   CROSS(qvec, tvec, edge1);

   /* calculate V parameter and test bounds */
   *v = DOT(dir, qvec) * inv_det;
   if (*v < 0.0 || *u + *v > 1.0)
     return 0;

   /* calculate t, ray intersects triangle */
   *t = DOT(edge2, qvec) * inv_det;
#endif
   return 1;
}

int UTILITY::IntersectTriangleF(float orig[3], float dir[3],
                   float vert0[3], float vert1[3], float vert2[3],
                   float *t, float *u, float *v)
{
   float edge1[3], edge2[3], tvec[3], pvec[3], qvec[3];
   float det,inv_det;

   /* find vectors for two edges sharing vert0 */
   SUB(edge1, vert1, vert0);
   SUB(edge2, vert2, vert0);

   /* begin calculating determinant - also used to calculate U parameter */
   CROSS(pvec, dir, edge2);

   /* if determinant is near zero, ray lies in plane of triangle */
   det = DOT(edge1, pvec);

#ifdef TEST_CULL           /* define TEST_CULL if culling is desired */
   if (det < EPSILON)
      return 0;

   /* calculate distance from vert0 to ray origin */
   SUB(tvec, orig, vert0);
   
   /* calculate U parameter and test bounds */
   *u = DOT(tvec, pvec);
   if (*u < 0.0 || *u > det)
      return 0;

   /* prepare to test V parameter */
   CROSS(qvec, tvec, edge1);

    /* calculate V parameter and test bounds */
   *v = DOT(dir, qvec);
   if (*v < 0.0 || *u + *v > det)
      return 0;

   /* calculate t, scale parameters, ray intersects triangle */
   *t = DOT(edge2, qvec);
   inv_det = 1.0 / det;
   *t *= inv_det;
   *u *= inv_det;
   *v *= inv_det;
#else                    /* the non-culling branch */
   if (det > -EPSILON && det < EPSILON)
     return 0;
   inv_det = 1.0 / det;

   /* calculate distance from vert0 to ray origin */
   SUB(tvec, orig, vert0);

   /* calculate U parameter and test bounds */
   *u = DOT(tvec, pvec) * inv_det;
   if (*u < 0.0 || *u > 1.0)
     return 0;

   /* prepare to test V parameter */
   CROSS(qvec, tvec, edge1);

   /* calculate V parameter and test bounds */
   *v = DOT(dir, qvec) * inv_det;
   if (*v < 0.0 || *u + *v > 1.0)
     return 0;

   /* calculate t, ray intersects triangle */
   *t = DOT(edge2, qvec) * inv_det;
#endif
   if (isNaN(*u) || isNaN(*v) || isNaN(*t))
	   return 0;
   return 1;
}

void UTILITY::DrawButton(float x1, float y1, float x2, float y2, float sidewidth, TEXTURE_HANDLE * texid, float opacity)
{
	int sx, sy;
	
	/*QUATERNION rot;
	rot.Rotate(rotation, 0,0,1);*/
	
	glPushAttrib(GL_ALL_ATTRIB_BITS);
	
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP);
	
	sx = 1600;
	sy = 1200;
	
	int x = (int) (x1*(float)sx);
	int y = (int) ((1.0-y1)*(float)sy);
	
	int w = (int) ((x2-x1)*(float)sx);
	int h = (int) ((y2-y1)*(float)sy);
	
	int sw = (int) (sidewidth * (float) sx);

	// Select our texture
	//glBindTexture( GL_TEXTURE_2D, texid );
	texid->Activate();

	// Disable depth testing 
	glDisable( GL_DEPTH_TEST );
	//and lighting...
	glDisable( GL_LIGHTING);
	glBlendFunc( GL_ONE_MINUS_SRC_ALPHA, GL_ONE_MINUS_SRC_COLOR);
	glBlendFunc( GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
	glEnable( GL_BLEND );

	// Select The Projection Matrix
	glMatrixMode( GL_PROJECTION );
	// Store The Projection Matrix
	glPushMatrix( );

	// Reset The Projection Matrix 
	glLoadIdentity( );
	// Set Up An Ortho Screen 
	glOrtho( 0, sx, 0, sy, -1, 1 );

	// Select The Modelview Matrix
	glMatrixMode( GL_MODELVIEW );
	glPushMatrix( );
	glLoadIdentity( );
	glTranslated( x, y, 0 );
	glColor4f(1,1,1,opacity);
	
	VERTEX v[4];
	v[0].Set(0,0,0);
	v[1].Set(sw,0,0);
	v[2].Set(sw,-h,0);
	v[3].Set(0,-h,0);
	
	glTranslated(-sw, 0, 0);
	
	//draw left side
	glBegin( GL_QUADS );
	        /* Texture Coord (Bottom Left) */
	        glTexCoord2f( 0, 0);
			/* Vertex Coord (Bottom Left) */
			glVertex2f( v[0].x, v[0].y );
	
			/* Texture Coord (Bottom Right) */
			glTexCoord2f( 0.5, 0);
			/* Vertex Coord (Bottom Right) */
			glVertex2f( v[1].x, v[1].y );
	
			/* Texture Coord (Top Right) */
			glTexCoord2f( 0.5, 1);
			/* Vertex Coord (Top Right) */
			glVertex2f( v[2].x, v[2].y );
	
			/* Texture Coord (Top Left) */
			glTexCoord2f( 0, 1);
			/* Vertex Coord (Top Left) */
			glVertex2f( v[3].x, v[3].y );
	      glEnd( );

	glTranslated(sw, 0, 0);
	
	v[0].Set(0,0,0);
	v[1].Set(w,0,0);
	v[2].Set(w,-h,0);
	v[3].Set(0,-h,0);
	
	//draw middle
	glBegin( GL_QUADS );
	        /* Texture Coord (Bottom Left) */
	        glTexCoord2f( 0.5, 0);
			/* Vertex Coord (Bottom Left) */
			glVertex2f( v[0].x, v[0].y );
	
			/* Texture Coord (Bottom Right) */
			glTexCoord2f( 0.5, 0);
			/* Vertex Coord (Bottom Right) */
			glVertex2f( v[1].x, v[1].y );
	
			/* Texture Coord (Top Right) */
			glTexCoord2f( 0.5, 1);
			/* Vertex Coord (Top Right) */
			glVertex2f( v[2].x, v[2].y );
	
			/* Texture Coord (Top Left) */
			glTexCoord2f( 0.5, 1);
			/* Vertex Coord (Top Left) */
			glVertex2f( v[3].x, v[3].y );
	      glEnd( );
	
	
	v[0].Set(0,0,0);
	v[1].Set(sw,0,0);
	v[2].Set(sw,-h,0);
	v[3].Set(0,-h,0);
	
	glTranslated(w, 0, 0);
	
	//draw right side
	glBegin( GL_QUADS );
	        /* Texture Coord (Bottom Left) */
	        glTexCoord2f( 0.5, 0);
			/* Vertex Coord (Bottom Left) */
			glVertex2f( v[0].x, v[0].y );
	
			/* Texture Coord (Bottom Right) */
			glTexCoord2f( 1, 0);
			/* Vertex Coord (Bottom Right) */
			glVertex2f( v[1].x, v[1].y );
	
			/* Texture Coord (Top Right) */
			glTexCoord2f( 1, 1);
			/* Vertex Coord (Top Right) */
			glVertex2f( v[2].x, v[2].y );
	
			/* Texture Coord (Top Left) */
			glTexCoord2f( 0.5, 1);
			/* Vertex Coord (Top Left) */
			glVertex2f( v[3].x, v[3].y );
	      glEnd( );

	// Select the Modelview Matrix
	glMatrixMode( GL_MODELVIEW );
	// Restore the Old Projection Matrix
	glPopMatrix( );

	// Select The Projection Matrix
	glMatrixMode( GL_PROJECTION );
	// Restore The Old Projection Matrix
	glPopMatrix( );

	// Re-enable Depth Testing
	glEnable( GL_DEPTH_TEST );
	//and lighting...
	glEnable( GL_LIGHTING);
	glDisable(GL_BLEND);
	
	glPopAttrib();
}

bool UTILITY::IntersectQuadrilateralF(VERTEX orig, VERTEX dir,
					VERTEX v_00, VERTEX v_10, VERTEX v_11, VERTEX v_01,
					float &t, float &u, float &v)
{	
	// Reject rays that are parallel to Q, and rays that intersect the plane
	// of Q either on the left of the line V00V01 or below the line V00V10.
	VERTEX E_01 = v_10 - v_00;
	VERTEX E_03 = v_01 - v_00;
	VERTEX P = dir.cross(E_03);
	float det = E_01.dot(P);
	
	if (std::abs(det) < EPSILON) return false;
	
	VERTEX T = orig - v_00;
	float alpha = T.dot(P) / det;
	
	if (alpha < 0.0) return false;
	
	VERTEX Q = T.cross(E_01);
	float beta = dir.dot(Q) / det;
	
	if (beta < 0.0) return false;
	
	if (alpha + beta > 1.0)
	{
		// Reject rays that that intersect the plane of Q either on
		// the right of the line V11V10 or above the line V11V00.
		VERTEX E_23 = v_01 - v_11;
		VERTEX E_21 = v_10 - v_11;
		VERTEX P_prime = dir.cross(E_21);
		float det_prime = E_23.dot(P_prime);
		
		if (std::abs(det_prime) < EPSILON) return false;
			
		VERTEX T_prime = orig - v_11;
		float alpha_prime = T_prime.dot(P_prime) / det_prime;
		
		if (alpha_prime < 0.0) return false;
			
		VERTEX Q_prime = T_prime.cross(E_23);
		float beta_prime = dir.dot(Q_prime) / det_prime;
		
		if (beta_prime < 0.0) return false;
	}
	
	// Compute the ray parameter of the intersection point, and
	// reject the ray if it does not hit Q.
	t = E_03.dot(Q) / det;
	
	if (t < 0.0) return false;
	
	// Compute the barycentric coordinates of the fourth vertex.
	// These do not depend on the ray, and can be precomputed
	// and stored with the quadrilateral.
	float alpha_11, beta_11;
	VERTEX E_02 = v_11 - v_00;
	VERTEX n = E_01.cross(E_03);
	
	if ((std::abs(n.x) >= std::abs(n.y))
		&& (std::abs(n.x) >= std::abs(n.z)))
	{
		alpha_11 = ((E_02.y * E_03.z) - (E_02.z * E_03.y)) / n.x;
		beta_11 = ((E_01.y * E_02.z) - (E_01.z  * E_02.y)) / n.x;
	}
	else if ((std::abs(n.y) >= std::abs(n.x))
		&& (std::abs(n.y) >= std::abs(n.z)))
	{
		alpha_11 = ((E_02.z * E_03.x) - (E_02.x * E_03.z)) / n.y;
		beta_11 = ((E_01.z * E_02.x) - (E_01.x  * E_02.z)) / n.y;
	}
	else
	{
		alpha_11 = ((E_02.x * E_03.y) - (E_02.y * E_03.x)) / n.z;
		beta_11 = ((E_01.x * E_02.y) - (E_01.y  * E_02.x)) / n.z;
	}
	
	// Compute the bilinear coordinates of the intersection point.
	if (std::abs(alpha_11 - (1.0)) < EPSILON)
	{
		// Q is a trapezium.
		u = alpha;
		if (std::abs(beta_11 - (1.0)) < EPSILON) v = beta; // Q is a parallelogram.
		else v = beta / ((u * (beta_11 - (1.0))) + (1.0)); // Q is a trapezium.
	}
	else if (std::abs(beta_11 - (1.0)) < EPSILON)
	{
		// Q is a trapezium.
		v = beta;
		if ( ((v * (alpha_11 - (1.0))) + (1.0)) == 0 )
		{
			return false;
		}
		u = alpha / ((v * (alpha_11 - (1.0))) + (1.0));
	}
	else
	{
		float A = (1.0) - beta_11;
		float B = (alpha * (beta_11 - (1.0)))
		  - (beta * (alpha_11 - (1.0))) - (1.0);
		float C = alpha;
		float D = (B * B) - ((4.0) * A * C);
		if (D < 0) return false;
		float Q = (-0.5) * (B + ((B < (0.0) ? (-1.0) : (1.0))
		  * std::sqrt(D)));
		u = Q / A;
		if ((u < (0.0)) || (u > (1.0))) u = C / Q;
		v = beta / ((u * (beta_11 - (1.0))) + (1.0));
	}
	
	return true;
}

bool UTILITY::IntersectQuadrilateralD(VERTEXD orig, VERTEXD dir,
					VERTEXD v_00, VERTEXD v_10, VERTEXD v_11, VERTEXD v_01,
					double &t, double &u, double &v)
{	
	// Reject rays that are parallel to Q, and rays that intersect the plane
	// of Q either on the left of the line V00V01 or below the line V00V10.
	VERTEXD E_01 = v_10 - v_00;
	VERTEXD E_03 = v_01 - v_00;
	VERTEXD P = dir.cross(E_03);
	double det = E_01.dot(P);
	
	if (std::abs(det) < EPSILON) return false;
	
	VERTEXD T = orig - v_00;
	double alpha = T.dot(P) / det;
	
	if (alpha < 0.0) return false;
	
	VERTEXD Q = T.cross(E_01);
	double beta = dir.dot(Q) / det;
	
	if (beta < 0.0) return false;
	
	if (alpha + beta > 1.0)
	{
		// Reject rays that that intersect the plane of Q either on
		// the right of the line V11V10 or above the line V11V00.
		VERTEXD E_23 = v_01 - v_11;
		VERTEXD E_21 = v_10 - v_11;
		VERTEXD P_prime = dir.cross(E_21);
		double det_prime = E_23.dot(P_prime);
		
		if (std::abs(det_prime) < EPSILON) return false;
			
		VERTEXD T_prime = orig - v_11;
		double alpha_prime = T_prime.dot(P_prime) / det_prime;
		
		if (alpha_prime < 0.0) return false;
			
		VERTEXD Q_prime = T_prime.cross(E_23);
		double beta_prime = dir.dot(Q_prime) / det_prime;
		
		if (beta_prime < 0.0) return false;
	}
	
	// Compute the ray parameter of the intersection point, and
	// reject the ray if it does not hit Q.
	t = E_03.dot(Q) / det;
	
	if (t < 0.0) return false;
	
	// Compute the barycentric coordinates of the fourth vertex.
	// These do not depend on the ray, and can be precomputed
	// and stored with the quadrilateral.
	double alpha_11, beta_11;
	VERTEXD E_02 = v_11 - v_00;
	VERTEXD n = E_01.cross(E_03);
	
	if ((std::abs(n.x) >= std::abs(n.y))
		&& (std::abs(n.x) >= std::abs(n.z)))
	{
		alpha_11 = ((E_02.y * E_03.z) - (E_02.z * E_03.y)) / n.x;
		beta_11 = ((E_01.y * E_02.z) - (E_01.z  * E_02.y)) / n.x;
	}
	else if ((std::abs(n.y) >= std::abs(n.x))
		&& (std::abs(n.y) >= std::abs(n.z)))
	{
		alpha_11 = ((E_02.z * E_03.x) - (E_02.x * E_03.z)) / n.y;
		beta_11 = ((E_01.z * E_02.x) - (E_01.x  * E_02.z)) / n.y;
	}
	else
	{
		alpha_11 = ((E_02.x * E_03.y) - (E_02.y * E_03.x)) / n.z;
		beta_11 = ((E_01.x * E_02.y) - (E_01.y  * E_02.x)) / n.z;
	}
	
	// Compute the bilinear coordinates of the intersection point.
	if (std::abs(alpha_11 - (1.0)) < EPSILON)
	{
		// Q is a trapezium.
		u = alpha;
		if (std::abs(beta_11 - (1.0)) < EPSILON) v = beta; // Q is a parallelogram.
		else v = beta / ((u * (beta_11 - (1.0))) + (1.0)); // Q is a trapezium.
	}
	else if (std::abs(beta_11 - (1.0)) < EPSILON)
	{
		// Q is a trapezium.
		v = beta;
		u = alpha / ((v * (alpha_11 - (1.0))) + (1.0));
	}
	else
	{
		double A = (1.0) - beta_11;
		double B = (alpha * (beta_11 - (1.0)))
		  - (beta * (alpha_11 - (1.0))) - (1.0);
		double C = alpha;
		double D = (B * B) - ((4.0) * A * C);
		if (D < 0) return false;
		double Q = (-0.5) * (B + ((B < (0.0) ? (-1.0) : (1.0))
		  * std::sqrt(D)));
		u = Q / A;
		if ((u < (0.0)) || (u > (1.0))) u = C / Q;
		v = beta / ((u * (beta_11 - (1.0))) + (1.0));
	}
	
	return true;
}

/* some macros */
#define CROSS(dest,v1,v2)                      \
              dest[0]=v1[1]*v2[2]-v1[2]*v2[1]; \
              dest[1]=v1[2]*v2[0]-v1[0]*v2[2]; \
              dest[2]=v1[0]*v2[1]-v1[1]*v2[0];

#define DOT(v1,v2) (v1[0]*v2[0]+v1[1]*v2[1]+v1[2]*v2[2])

#define SUB(dest,v1,v2) dest[0]=v1[0]-v2[0]; dest[1]=v1[1]-v2[1]; dest[2]=v1[2]-v2[2]; 

#define ADD(dest,v1,v2) dest[0]=v1[0]+v2[0]; dest[1]=v1[1]+v2[1]; dest[2]=v1[2]+v2[2]; 

#define MULT(dest,v,factor) dest[0]=factor*v[0]; dest[1]=factor*v[1]; dest[2]=factor*v[2];

#define SET(dest,src) dest[0]=src[0]; dest[1]=src[1]; dest[2]=src[2]; 

/* sort so that a<=b */
#define SORT(a,b)       \
             if(a>b)    \
             {          \
               float c; \
               c=a;     \
               a=b;     \
               b=c;     \
             }

/* sort so that a<=b */
#define SORT2(a,b,smallest)       \
             if(a>b)       \
             {             \
               float c;    \
               c=a;        \
               a=b;        \
               b=c;        \
               smallest=1; \
             }             \
             else smallest=0;
			 
#define ISECT(VV0,VV1,VV2,D0,D1,D2,isect0,isect1) \
              isect0=VV0+(VV1-VV0)*D0/(D0-D1);    \
              isect1=VV0+(VV2-VV0)*D0/(D0-D2);


#define COMPUTE_INTERVALS(VV0,VV1,VV2,D0,D1,D2,D0D1,D0D2,isect0,isect1) \
  if(D0D1>0.0f)                                         \
  {                                                     \
    /* here we know that D0D2<=0.0 */                   \
    /* that is D0, D1 are on the same side, D2 on the other or on the plane */ \
    ISECT(VV2,VV0,VV1,D2,D0,D1,isect0,isect1);          \
  }                                                     \
  else if(D0D2>0.0f)                                    \
  {                                                     \
    /* here we know that d0d1<=0.0 */                   \
    ISECT(VV1,VV0,VV2,D1,D0,D2,isect0,isect1);          \
  }                                                     \
  else if(D1*D2>0.0f || D0!=0.0f)                       \
  {                                                     \
    /* here we know that d0d1<=0.0 or that D0!=0.0 */   \
    ISECT(VV0,VV1,VV2,D0,D1,D2,isect0,isect1);          \
  }                                                     \
  else if(D1!=0.0f)                                     \
  {                                                     \
    ISECT(VV1,VV0,VV2,D1,D0,D2,isect0,isect1);          \
  }                                                     \
  else if(D2!=0.0f)                                     \
  {                                                     \
    ISECT(VV2,VV0,VV1,D2,D0,D1,isect0,isect1);          \
  }                                                     \
  else                                                  \
  {                                                     \
    /* triangles are coplanar */                        \
    return coplanar_tri_tri(N1,V0,V1,V2,U0,U1,U2);      \
  }

/* this edge to edge test is based on Franlin Antonio's gem:
   "Faster Line Segment Intersection", in Graphics Gems III,
   pp. 199-202 */ 
#define EDGE_EDGE_TEST(V0,U0,U1)                      \
  Bx=U0[i0]-U1[i0];                                   \
  By=U0[i1]-U1[i1];                                   \
  Cx=V0[i0]-U0[i0];                                   \
  Cy=V0[i1]-U0[i1];                                   \
  f=Ay*Bx-Ax*By;                                      \
  d=By*Cx-Bx*Cy;                                      \
  if((f>0 && d>=0 && d<=f) || (f<0 && d<=0 && d>=f))  \
  {                                                   \
    e=Ax*Cy-Ay*Cx;                                    \
    if(f>0)                                           \
    {                                                 \
      if(e>=0 && e<=f) return 1;                      \
    }                                                 \
    else                                              \
    {                                                 \
      if(e<=0 && e>=f) return 1;                      \
    }                                                 \
  }                                

#define EDGE_AGAINST_TRI_EDGES(V0,V1,U0,U1,U2) \
{                                              \
  float Ax,Ay,Bx,By,Cx,Cy,e,d,f;               \
  Ax=V1[i0]-V0[i0];                            \
  Ay=V1[i1]-V0[i1];                            \
  /* test edge U0,U1 against V0,V1 */          \
  EDGE_EDGE_TEST(V0,U0,U1);                    \
  /* test edge U1,U2 against V0,V1 */          \
  EDGE_EDGE_TEST(V0,U1,U2);                    \
  /* test edge U2,U1 against V0,V1 */          \
  EDGE_EDGE_TEST(V0,U2,U0);                    \
}

#define POINT_IN_TRI(V0,U0,U1,U2)           \
{                                           \
  float a,b,c,d0,d1,d2;                     \
  /* is T1 completly inside T2? */          \
  /* check if V0 is inside tri(U0,U1,U2) */ \
  a=U1[i1]-U0[i1];                          \
  b=-(U1[i0]-U0[i0]);                       \
  c=-a*U0[i0]-b*U0[i1];                     \
  d0=a*V0[i0]+b*V0[i1]+c;                   \
                                            \
  a=U2[i1]-U1[i1];                          \
  b=-(U2[i0]-U1[i0]);                       \
  c=-a*U1[i0]-b*U1[i1];                     \
  d1=a*V0[i0]+b*V0[i1]+c;                   \
                                            \
  a=U0[i1]-U2[i1];                          \
  b=-(U0[i0]-U2[i0]);                       \
  c=-a*U2[i0]-b*U2[i1];                     \
  d2=a*V0[i0]+b*V0[i1]+c;                   \
  if(d0*d1>0.0)                             \
  {                                         \
    if(d0*d2>0.0) return 1;                 \
  }                                         \
}
  
inline void isect2(float VTX0[3],float VTX1[3],float VTX2[3],float VV0,float VV1,float VV2,
            float D0,float D1,float D2,float *isect0,float *isect1,float isectpoint0[3],float isectpoint1[3]) 
{
  float tmp=D0/(D0-D1);          
  float diff[3];
  *isect0=VV0+(VV1-VV0)*tmp;         
  SUB(diff,VTX1,VTX0);              
  MULT(diff,diff,tmp);               
  ADD(isectpoint0,diff,VTX0);        
  tmp=D0/(D0-D2);                    
  *isect1=VV0+(VV2-VV0)*tmp;          
  SUB(diff,VTX2,VTX0);                   
  MULT(diff,diff,tmp);                 
  ADD(isectpoint1,VTX0,diff);          
}

int coplanar_tri_tri(float N[3],float V0[3],float V1[3],float V2[3],
                     float U0[3],float U1[3],float U2[3])
{
   float A[3];
   short i0,i1;
   /* first project onto an axis-aligned plane, that maximizes the area */
   /* of the triangles, compute indices: i0,i1. */
   A[0]=fabs(N[0]);
   A[1]=fabs(N[1]);
   A[2]=fabs(N[2]);
   if(A[0]>A[1])
   {
      if(A[0]>A[2])  
      {
          i0=1;      /* A[0] is greatest */
          i1=2;
      }
      else
      {
          i0=0;      /* A[2] is greatest */
          i1=1;
      }
   }
   else   /* A[0]<=A[1] */
   {
      if(A[2]>A[1])
      {
          i0=0;      /* A[2] is greatest */
          i1=1;                                           
      }
      else
      {
          i0=0;      /* A[1] is greatest */
          i1=2;
      }
    }               
                
    /* test all edges of triangle 1 against the edges of triangle 2 */
    EDGE_AGAINST_TRI_EDGES(V0,V1,U0,U1,U2);
    EDGE_AGAINST_TRI_EDGES(V1,V2,U0,U1,U2);
    EDGE_AGAINST_TRI_EDGES(V2,V0,U0,U1,U2);
                
    /* finally, test if tri1 is totally contained in tri2 or vice versa */
    POINT_IN_TRI(V0,U0,U1,U2);
    POINT_IN_TRI(U0,V0,V1,V2);

    return 0;
}

inline int compute_intervals_isectline(float VERT0[3],float VERT1[3],float VERT2[3],
                                       float VV0,float VV1,float VV2,float D0,float D1,float D2,
                                       float D0D1,float D0D2,float *isect0,float *isect1,
                                       float isectpoint0[3],float isectpoint1[3])
{
  if(D0D1>0.0f)                                        
  {                                                    
    /* here we know that D0D2<=0.0 */                  
    /* that is D0, D1 are on the same side, D2 on the other or on the plane */
    isect2(VERT2,VERT0,VERT1,VV2,VV0,VV1,D2,D0,D1,isect0,isect1,isectpoint0,isectpoint1);
  } 
  else if(D0D2>0.0f)                                   
    {                                                   
    /* here we know that d0d1<=0.0 */             
    isect2(VERT1,VERT0,VERT2,VV1,VV0,VV2,D1,D0,D2,isect0,isect1,isectpoint0,isectpoint1);
  }                                                  
  else if(D1*D2>0.0f || D0!=0.0f)   
  {                                   
    /* here we know that d0d1<=0.0 or that D0!=0.0 */
    isect2(VERT0,VERT1,VERT2,VV0,VV1,VV2,D0,D1,D2,isect0,isect1,isectpoint0,isectpoint1);   
  }                                                  
  else if(D1!=0.0f)                                  
  {                                               
    isect2(VERT1,VERT0,VERT2,VV1,VV0,VV2,D1,D0,D2,isect0,isect1,isectpoint0,isectpoint1); 
  }                                         
  else if(D2!=0.0f)                                  
  {                                                   
    isect2(VERT2,VERT0,VERT1,VV2,VV0,VV1,D2,D0,D1,isect0,isect1,isectpoint0,isectpoint1);     
  }                                                 
  else                                               
  {                                                   
    /* triangles are coplanar */    
    return 1;
  }
  return 0;
}

int UTILITY::MollerTriTriIsectWithLine(float * V0, float * V1, float * V2,
					float * U0, float * U1, float * U2, int * coplanar,
					float * isectpt1, float * isectpt2)
{
  float E1[3],E2[3];
  float N1[3],N2[3],d1,d2;
  float du0,du1,du2,dv0,dv1,dv2;
  float D[3];
  float isect1[2], isect2[2];
  float isectpointA1[3],isectpointA2[3];
  float isectpointB1[3],isectpointB2[3];
  float du0du1,du0du2,dv0dv1,dv0dv2;
  short index;
  float vp0,vp1,vp2;
  float up0,up1,up2;
  float b,c,max;
//  float tmp,diff[3];
  int smallest1,smallest2;
	
	for (int i = 0; i < 2; i++)
	{
		isect1[i] = 0;
		isect2[i] = 0;
	}
	
	for (int i = 0; i < 3; i++)
	{
		isectpointA1[i] = 0;
		isectpointB1[i] = 0;
		isectpointA2[i] = 0;
		isectpointB2[i] = 0;
	}
  
  /* compute plane equation of triangle(V0,V1,V2) */
  SUB(E1,V1,V0);
  SUB(E2,V2,V0);
  CROSS(N1,E1,E2);
  d1=-DOT(N1,V0);
  /* plane equation 1: N1.X+d1=0 */

  /* put U0,U1,U2 into plane equation 1 to compute signed distances to the plane*/
  du0=DOT(N1,U0)+d1;
  du1=DOT(N1,U1)+d1;
  du2=DOT(N1,U2)+d1;

  /* coplanarity robustness check */
#if USE_EPSILON_TEST==TRUE
  if(fabs(du0)<EPSILON) du0=0.0;
  if(fabs(du1)<EPSILON) du1=0.0;
  if(fabs(du2)<EPSILON) du2=0.0;
#endif
  du0du1=du0*du1;
  du0du2=du0*du2;

  if(du0du1>0.0f && du0du2>0.0f) /* same sign on all of them + not equal 0 ? */
    return 0;                    /* no intersection occurs */

  /* compute plane of triangle (U0,U1,U2) */
  SUB(E1,U1,U0);
  SUB(E2,U2,U0);
  CROSS(N2,E1,E2);
  d2=-DOT(N2,U0);
  /* plane equation 2: N2.X+d2=0 */

  /* put V0,V1,V2 into plane equation 2 */
  dv0=DOT(N2,V0)+d2;
  dv1=DOT(N2,V1)+d2;
  dv2=DOT(N2,V2)+d2;

#if USE_EPSILON_TEST==TRUE
  if(fabs(dv0)<EPSILON) dv0=0.0;
  if(fabs(dv1)<EPSILON) dv1=0.0;
  if(fabs(dv2)<EPSILON) dv2=0.0;
#endif

  dv0dv1=dv0*dv1;
  dv0dv2=dv0*dv2;
        
  if(dv0dv1>0.0f && dv0dv2>0.0f) /* same sign on all of them + not equal 0 ? */
    return 0;                    /* no intersection occurs */

  /* compute direction of intersection line */
  CROSS(D,N1,N2);

  /* compute and index to the largest component of D */
  max=fabs(D[0]);
  index=0;
  b=fabs(D[1]);
  c=fabs(D[2]);
  if(b>max) max=b,index=1;
  if(c>max) max=c,index=2;

  /* this is the simplified projection onto L*/
  vp0=V0[index];
  vp1=V1[index];
  vp2=V2[index];
  
  up0=U0[index];
  up1=U1[index];
  up2=U2[index];

  /* compute interval for triangle 1 */
  *coplanar=compute_intervals_isectline(V0,V1,V2,vp0,vp1,vp2,dv0,dv1,dv2,
                                       dv0dv1,dv0dv2,&isect1[0],&isect1[1],isectpointA1,isectpointA2);
  if(*coplanar) return coplanar_tri_tri(N1,V0,V1,V2,U0,U1,U2);     


  /* compute interval for triangle 2 */
  compute_intervals_isectline(U0,U1,U2,up0,up1,up2,du0,du1,du2,
                              du0du1,du0du2,&isect2[0],&isect2[1],isectpointB1,isectpointB2);

  SORT2(isect1[0],isect1[1],smallest1);
  SORT2(isect2[0],isect2[1],smallest2);

  if(isect1[1]<isect2[0] || isect2[1]<isect1[0]) return 0;

  /* at this point, we know that the triangles intersect */

  if(isect2[0]<isect1[0])
  {
    if(smallest1==0) { SET(isectpt1,isectpointA1); }
    else { SET(isectpt1,isectpointA2); }

    if(isect2[1]<isect1[1])
    {
      if(smallest2==0) { SET(isectpt2,isectpointB2); }
      else { SET(isectpt2,isectpointB1); }
    }
    else
    {
      if(smallest1==0) { SET(isectpt2,isectpointA2); }
      else { SET(isectpt2,isectpointA1); }
    }
  }
  else
  {
    if(smallest2==0) { SET(isectpt1,isectpointB1); }
    else { SET(isectpt1,isectpointB2); }

    if(isect2[1]>isect1[1])
    {
      if(smallest1==0) { SET(isectpt2,isectpointA2); }
      else { SET(isectpt2,isectpointA1); }      
    }
    else
    {
      if(smallest2==0) { SET(isectpt2,isectpointB2); }
      else { SET(isectpt2,isectpointB1); } 
    }
  }
  return 1;
}

#define NEWCOMPUTE_INTERVALS(VV0,VV1,VV2,D0,D1,D2,D0D1,D0D2,A,B,C,X0,X1) \
{ \
        if(D0D1>0.0f) \
        { \
                /* here we know that D0D2<=0.0 */ \
            /* that is D0, D1 are on the same side, D2 on the other or on the plane */ \
                A=VV2; B=(VV0-VV2)*D2; C=(VV1-VV2)*D2; X0=D2-D0; X1=D2-D1; \
        } \
        else if(D0D2>0.0f)\
        { \
                /* here we know that d0d1<=0.0 */ \
            A=VV1; B=(VV0-VV1)*D1; C=(VV2-VV1)*D1; X0=D1-D0; X1=D1-D2; \
        } \
        else if(D1*D2>0.0f || D0!=0.0f) \
        { \
                /* here we know that d0d1<=0.0 or that D0!=0.0 */ \
                A=VV0; B=(VV1-VV0)*D0; C=(VV2-VV0)*D0; X0=D0-D1; X1=D0-D2; \
        } \
        else if(D1!=0.0f) \
        { \
                A=VV1; B=(VV0-VV1)*D1; C=(VV2-VV1)*D1; X0=D1-D0; X1=D1-D2; \
        } \
        else if(D2!=0.0f) \
        { \
                A=VV2; B=(VV0-VV2)*D2; C=(VV1-VV2)*D2; X0=D2-D0; X1=D2-D1; \
        } \
        else \
        { \
                /* triangles are coplanar */ \
                return coplanar_tri_tri(N1,V0,V1,V2,U0,U1,U2); \
        } \
}

#define FABS(x) ((float)fabs(x))

int NoDivTriTriIsect(float V0[3],float V1[3],float V2[3],
                     float U0[3],float U1[3],float U2[3])
{
  float E1[3],E2[3];
  float N1[3],N2[3],d1,d2;
  float du0,du1,du2,dv0,dv1,dv2;
  float D[3];
  float isect1[2], isect2[2];
  float du0du1,du0du2,dv0dv1,dv0dv2;
  short index;
  float vp0,vp1,vp2;
  float up0,up1,up2;
  float bb,cc,max;
  float a,b,c,x0,x1;
  float d,e,f,y0,y1;
  float xx,yy,xxyy,tmp;

  /* compute plane equation of triangle(V0,V1,V2) */
  SUB(E1,V1,V0);
  SUB(E2,V2,V0);
  CROSS(N1,E1,E2);
  d1=-DOT(N1,V0);
  /* plane equation 1: N1.X+d1=0 */

  /* put U0,U1,U2 into plane equation 1 to compute signed distances to the plane*/
  du0=DOT(N1,U0)+d1;
  du1=DOT(N1,U1)+d1;
  du2=DOT(N1,U2)+d1;

  /* coplanarity robustness check */
#if USE_EPSILON_TEST==TRUE
  if(FABS(du0)<EPSILON) du0=0.0;
  if(FABS(du1)<EPSILON) du1=0.0;
  if(FABS(du2)<EPSILON) du2=0.0;
#endif
  du0du1=du0*du1;
  du0du2=du0*du2;

  if(du0du1>0.0f && du0du2>0.0f) /* same sign on all of them + not equal 0 ? */
    return 0;                    /* no intersection occurs */

  /* compute plane of triangle (U0,U1,U2) */
  SUB(E1,U1,U0);
  SUB(E2,U2,U0);
  CROSS(N2,E1,E2);
  d2=-DOT(N2,U0);
  /* plane equation 2: N2.X+d2=0 */

  /* put V0,V1,V2 into plane equation 2 */
  dv0=DOT(N2,V0)+d2;
  dv1=DOT(N2,V1)+d2;
  dv2=DOT(N2,V2)+d2;

#if USE_EPSILON_TEST==TRUE
  if(FABS(dv0)<EPSILON) dv0=0.0;
  if(FABS(dv1)<EPSILON) dv1=0.0;
  if(FABS(dv2)<EPSILON) dv2=0.0;
#endif

  dv0dv1=dv0*dv1;
  dv0dv2=dv0*dv2;

  if(dv0dv1>0.0f && dv0dv2>0.0f) /* same sign on all of them + not equal 0 ? */
    return 0;                    /* no intersection occurs */

  /* compute direction of intersection line */
  CROSS(D,N1,N2);

  /* compute and index to the largest component of D */
  max=(float)FABS(D[0]);
  index=0;
  bb=(float)FABS(D[1]);
  cc=(float)FABS(D[2]);
  if(bb>max) max=bb,index=1;
  if(cc>max) max=cc,index=2;

  /* this is the simplified projection onto L*/
  vp0=V0[index];
  vp1=V1[index];
  vp2=V2[index];

  up0=U0[index];
  up1=U1[index];
  up2=U2[index];

  /* compute interval for triangle 1 */
  NEWCOMPUTE_INTERVALS(vp0,vp1,vp2,dv0,dv1,dv2,dv0dv1,dv0dv2,a,b,c,x0,x1);

  /* compute interval for triangle 2 */
  NEWCOMPUTE_INTERVALS(up0,up1,up2,du0,du1,du2,du0du1,du0du2,d,e,f,y0,y1);

  xx=x0*x1;
  yy=y0*y1;
  xxyy=xx*yy;

  tmp=a*xxyy;
  isect1[0]=tmp+b*x1*yy;
  isect1[1]=tmp+c*x0*yy;

  tmp=d*xxyy;
  isect2[0]=tmp+e*xx*y1;
  isect2[1]=tmp+f*xx*y0;

  SORT(isect1[0],isect1[1]);
  SORT(isect2[0],isect2[1]);

  if(isect1[1]<isect2[0] || isect2[1]<isect1[0]) return 0;
  return 1;
}

int UTILITY::BruteForceTriangleIntersectionF(VERTEX * tri1, VERTEX * tri2, 
					VERTEX & colpt, VERTEX & coldest, VERTEX & colorig, int & whichtri)
{
	//could do a simple test here to speed things up a bit:
	// find the distance from tri2 to tri1's plane, return false if all
	// distances are positive or negative, do the same for tri1 on tri2's plane
	
	int col = NoDivTriTriIsect(tri1[0].v3(), tri1[1].v3(), tri1[2].v3(), 
		tri2[0].v3(), tri2[1].v3(), tri2[2].v3());
	
	if (!col)
		return col;
	
	col = 0;
	
	//collide all tri1 edges to tri2 and vice versa
	VERTEX * edgetri = tri1;
	VERTEX * coltri = tri2;
	int i = 0;
	VERTEX segment;
	VERTEX dir;
	int i0;
	float t, u, v;
	for (i = 0; i < 6; i++)
	{
		if (i == 3)
		{
			edgetri = tri2;
			coltri = tri1;
		}
		
		i0 = i % 3;
		
		segment = edgetri[(i0+1)%3]-edgetri[i0];
		dir = segment.normalize();
	
		col = IntersectTriangleF(edgetri[i0].v3(), dir.v3(),
                   coltri[0].v3(), coltri[1].v3(), coltri[2].v3(),
                   &t, &u, &v);
		
		if (t > segment.len() || t < 0)
			col = 0;
		
		if (col)
		{
			whichtri = 1 - i / 3;
			colorig = edgetri[i0];
			colpt = edgetri[i0] + dir.ScaleR(t);
			//colseg = edgetri[(i0+1)%3] - colpt;
			coldest = edgetri[(i0+1)%3];
			return col;
		}
	}
	
	return col;
}

VERTEX NormalFromTri(VERTEX * tri)
{
	VERTEX norm;
	norm = (tri[2] - tri[0]).cross(tri[1] - tri[0]);
	norm = norm.normalize();
	return norm;
}

//float PointToTriDistance(VERTEX * tri, VERTEX norm, float d, VERTEX point)
float PointToTriDistance(VERTEX norm, float d, VERTEX point)
{
	//float d = -norm.x*tri[0].x-norm.y*tri[0].y-norm.z*tri[0].z;
	
	//point = point - tri[0];
	
	float num, denom;
	num = norm.x*point.x+norm.y*point.y+norm.z*point.z+d;
	//denom = sqrt(norm.x*norm.x+norm.y*norm.y+norm.z*norm.z);
	denom = 1.0f;
	
	return num / denom;
}

int UTILITY::BruteForceTriangleIntersectionF2(VERTEX * tri1, VERTEX * tri2, 
					VERTEX & colpt, VERTEX & coldest, VERTEX & colorig, int & whichtri,
					float & destdepth, float & origdepth)
{
	//could do a simple test here to speed things up a bit:
	// find the distance from tri2 to tri1's plane, return false if all
	// distances are positive or negative, do the same for tri1 on tri2's plane
	
	int col = NoDivTriTriIsect(tri1[0].v3(), tri1[1].v3(), tri1[2].v3(), 
		tri2[0].v3(), tri2[1].v3(), tri2[2].v3());
	
	if (!col)
		return col;
	
	VERTEX norm1 = NormalFromTri(tri1);
	VERTEX norm2 = NormalFromTri(tri2);
	
	//load up all point to triangle distances
	
	float d1 = -norm1.x*tri1[0].x-norm1.y*tri1[0].y-norm1.z*tri1[0].z;
	float d2 = -norm2.x*tri2[0].x-norm2.y*tri2[0].y-norm2.z*tri2[0].z;
	
	/*float tri1to2[3];
	float tri2to1[3];
	
	for (int i = 0; i < 3; i++)
		tri1to2[3] = PointToTriDistance(tri2, norm2, d2, tri1[i]);
	
	for (int i = 0; i < 3; i++)
		tri2to1[3] = PointToTriDistance(tri1, norm1, d1, tri2[i]);
	
	whichtri = 1;
	
	//find which group of depths crosses zero
	if ((tri1to2[0] < 0 && tri1to2[1] < 0 && tri1to2[2] < 0) ||
		(tri1to2[0] > 0 && tri1to2[1] > 0 && tri1to2[2] > 0))
	{
		if ((tri2to1[0] < 0 && tri2to1[1] < 0 && tri2to1[2] < 0) ||
			(tri2to1[0] > 0 && tri2to1[1] > 0 && tri2to1[2] > 0))
				return col;
		else
		{
			whichtri = 0;
		}
	}
	else
	{
		whichtri = 1;
	}
	
	//use whatever depths cross zero
	VERTEX * depths = tri1to2;
	if (whichtri == 0)
		depths = tri2to1;
	
	//find the extreme values that cross zero
	int idxmin = 0;
	int idxmax = 0;
	float min = 0;
	float max = 0;
	for (int i = 0; i < 3; i++)
	{
		if (depths[i] < min)
		{
			min = depths[i];
			idxmin = i;
		}
		if (depths[i] > max)
		{
			max = depths[i];
			idxmax = i;
		}
	}
	
	if (whichtri == 0)
	{
		//colpt = 
	}
	if (whichtri == 1)
	{
		//colpt = tri1[idxmin]
		//colorig.Set(-d2/norm2.x,-d2/norm2.y,-d2/norm2.z);
		colorig = tri1[idxmin];
		coldest = tri1[idxmax];
	}
	
	return col;*/
	
	col = 0;
	
	//collide all tri1 edges to tri2 and vice versa
	VERTEX * edgetri = tri1;
	VERTEX * coltri = tri2;
	int i = 0;
	VERTEX segment;
	VERTEX dir;
	int i0;
	float t, u, v;
	for (i = 0; i < 6; i++)
	{
		if (i == 3)
		{
			edgetri = tri2;
			coltri = tri1;
		}
		
		i0 = i % 3;
		
		segment = edgetri[(i0+1)%3]-edgetri[i0];
		dir = segment.normalize();
	
		col = IntersectTriangleF(edgetri[i0].v3(), dir.v3(),
                   coltri[0].v3(), coltri[1].v3(), coltri[2].v3(),
                   &t, &u, &v);
		
		if (t > segment.len() || t < 0)
			col = 0;
		
		if (col)
		{
			whichtri = 1 - i / 3;
			float d = d2;
			VERTEX n = norm2;
			if (whichtri == 0)
			{
				d = d1;
				n = norm1;
			}
			colorig = edgetri[i0];
			colpt = edgetri[i0] + dir.ScaleR(t);
			//colseg = edgetri[(i0+1)%3] - colpt;
			coldest = edgetri[(i0+1)%3];
			
			destdepth = PointToTriDistance(n, d, coldest);
			origdepth = PointToTriDistance(n, d, colorig);
			
			if (destdepth < 0)
				destdepth = -destdepth;
			if (origdepth < 0)
				origdepth = -origdepth;
			
			return col;
		}
	}
	
	return col;
}

bool UTILITY::FileCopy( const string & srcfile, const string & dstfile ) const
{
	ifstream src(srcfile.c_str(), ios_base::binary);
	ofstream dst(dstfile.c_str(), ios_base::binary);
	
	if (!src || !dst)
		return false;
	
	char c;
	while (src.get(c))
		dst.put(c);
	
	return true;
}

string UTILITY::GetPathFromFilename(string filename)
{
	unsigned int pos = filename.find_last_of("/\\");
	if (pos < filename.length())
		return filename.substr(0,pos);
	else
		return "";
}

bool UTILITY::isNaN(float val)
{
	return (!(val <= 0 || val > 0));
}

bool UTILITY::DirExists (string folderpath) {
	#ifndef _WIN32
	return UTILITY::FileExists (folderpath);
	#else
	// WARNING: works ony if dir is not empty
	HANDLE          hList;
	TCHAR           szDir[MAX_PATH+1];
	WIN32_FIND_DATA FileData;
	sprintf(szDir, "%s\\*", folderpath.c_str ());
	// Get the first file
	hList = FindFirstFile(szDir, &FileData);
	if (hList == INVALID_HANDLE_VALUE)
	{
		return false;
	} else {
		return true;
	}
	#endif
}

bool UTILITY::GetFolderIndex(string folderpath, list <string> & outputfolderlist)
{
	//------Folder listing code for POSIX
	#ifndef _WIN32
	DIR *dp;
	struct dirent *ep;
	dp = opendir (folderpath.c_str());
	if (dp != NULL)
	{
		while ( ( ep = readdir( dp ) ) )
		{
			//puts (ep->d_name);
			string newname = ep->d_name;
			if (newname != "." && newname != "..")
			{
				outputfolderlist.push_back(newname);
			}
		}
		(void) closedir (dp);
	}
	else
		return false;
	#else 	//------End POSIX-specific folder listing code ---- Start WIN32 Specific code
	HANDLE          hList;
	TCHAR           szDir[MAX_PATH+1];
	WIN32_FIND_DATA FileData;

	// Get the proper directory path
	sprintf(szDir, "%s\\*", folderpath.c_str ());

	// Get the first file
	hList = FindFirstFile(szDir, &FileData);
	if (hList == INVALID_HANDLE_VALUE)
	{ 
		printf("No files found\n\n");
	}
	else
	{
		// Traverse through the directory structure
		while (FindNextFile(hList, &FileData))
		{
			// Check the object is a directory or not
			if (FileData.dwFileAttributes & FILE_ATTRIBUTE_HIDDEN)
			{} else
			{
			   	   if ((strcmp(FileData.cFileName, ".") != 0) &&
				   	  (strcmp(FileData.cFileName, "..") != 0))
					{
						outputfolderlist.push_back (FileData.cFileName);
					}
			}
		}
	}

	FindClose(hList);
	#endif //------End WIN32 specific folder listing code
	
	outputfolderlist.sort();
	return true;
}

double UTILITY::dabs(double val)
{
	if (val < 0)
		return -val;
	else
		return val;
}
