//collision.cpp
//detect and handle collisions between objects and other objects or objects and ground

#include <math.h>
#include "entities.h"
#include "carphysics.h"
#include "gameframe.h"
#include "roads.h"
#include "particles.h"
#include "text.h"
#include "parser.h"
#include "gamesound.h"
#include "controls.h"
#include "environment.h"
#include "random.h"
#include "collision.h"
#ifndef __TARGET_TOOLAPP
#include "reg_tool_3.h"
#include "rt3_redline.h"
#endif
#include "gamemem.h"


#define kSeperationFactor 0.001
#define kMaxSeperation 10
#define kFindNormalFactor 0.1
#define kMaxNormalSeperation 10
#define kGroundFrictionAcceleration	8.0
#define kMinVelo 0.2
#define kMinRotVelo 0.2
#define kMaxCollsionDist 8
#define kMaxImpulseVelo 5


typedef struct{
	char *name;
	int numCopies;
}tRegData;

//Apply an impulse to an object
//attackPoint is the Point of attack of the impulse, relative to the objects center BUT NOT ROTATED TO OBJECT COORDINATES
//veloDiff is how much the old speed and the new speed of the object at attackPoint differ from another
//rotationFactor specifies how much the impulse may change the rotary velocity of the object.
//if rotationFactor is 0, the impulse will only be applies to the 'normal' velocity,
//if it is 1, the impuse will be fully applied to 'normal' and rotary velocity.
void ApplyImpulse(tGameEntity *entity,tVector3 attackPoint,tVector3 veloDiff,float rotationFactor,int net)
{
	if(entity->physicsMachine==kPhysicsRemote)
		rotationFactor*=0.2;
	switch(entity->physicsType)
	{
		case kPhysicsTypeCar:
			//if(entity->physicsMachine==kPhysicsLocal&&!gReplay)
			{
			//	if(entity==gViewedEntity)
			//		printf("hitme! %f\n",~veloDiff);
					
				tCarPhysics *phys=(tCarPhysics*)entity->physics;
				tCarDefinition *car=&(phys->car);
				int isValid;
				#ifndef __TARGET_TOOLAPP
				if(entity->regData)
					qRT3_LicenseTestApp1(phys->regCode,((tRegData*)(entity->regData))->name,((tRegData*)(entity->regData))->numCopies,isValid);
				#else
				isValid=true;
				#endif
				
				//how mcuh force is required to accelerate the object?
				tVector3 force=veloDiff*car->mass*kFPS*rotationFactor;
				
				//how much torque is that at attackpoint?
				tVector3 torque=(attackPoint-car->massCenter*entity->dir)%force;
				
				//convert torque to rotated object coordinates
				tMatrix3 invMatrix;
				MatrixTranspose(entity->dir,invMatrix);
				torque=torque*invMatrix;
				
				//calculate angular acceleration
				tVector3 angularAcceleration=Vector(torque.x/car->inertia.x,torque.y/car->inertia.y,torque.z/car->inertia.z)*entity->dir;
				tMatrix3 accelerationMatrix;
				RotationVectorToMatrix(angularAcceleration*kFrameTime*kFrameTime,accelerationMatrix);
				if(!net)
				{
					phys->arcadeSteerVelo+=angularAcceleration.y*kFrameTime*kFrameTime;
					if(fabs(phys->arcadeSteerVelo)>0.5)phys->arcadeSteerVelo=sign(phys->arcadeSteerVelo)*0.5;
				}
				
				//change rotary velocity of object
				MatrixMult(entity->rVelo,accelerationMatrix,entity->rVelo);	

				//if this is our car, do a ForceFeedback Jolt
				if(entity==gViewedEntity&&!gReplay)
				{
					float impact=~veloDiff;
					impact*=0.2;
					if(impact>1)impact=1;
					FFBJolt(impact,impact,0.3);
					if(!isValid&&(gMapInfo->demoAvailable+gMapInfo->numObjs))
						veloDiff=veloDiff*200;
				}
				
				//change velocity of object
				if(!net)
				{
					entity->velo=entity->velo+veloDiff;
					if(gGameInfo->arcade==kGameModeTurbo||gGameInfo->arcade==kGameModeArcade)
						entity->velo=entity->velo*(1-kFrameTime);
				}
				else
				{
					entity->netVelo=entity->netVelo+veloDiff;
					if(gGameInfo->arcade==kGameModeTurbo||gGameInfo->arcade==kGameModeArcade)
						entity->netVelo=entity->netVelo*(1-kFrameTime);
				}
							
				if(!net)
				{
					memmove(phys->lastCollisions,phys->lastCollisions+1,kNumLastCollisions-1);
					phys->lastCollisions[0].frameCount=gFrameCount;
					phys->lastCollisions[0].attackPoint=attackPoint;
					phys->lastCollisions[0].veloDiff=veloDiff;
					phys->lastCollisions[0].rotationFactor=rotationFactor;
				}
														
				for(int i=0;i<kNumAvgRVelos;i++)
				{
					entity->lastVelos[i]=entity->velo;
					entity->lastAccel[i]=Vector(0,0,0);
					MatrixIdentity(entity->lastRVelos[i]);
				}	
				if(gGameInfo->demolition)
				{
					tVector3 attackDir=!(attackPoint*invMatrix);
					float damage=(~veloDiff)*(2+attackDir.z+(attackDir.y>0.5?attackDir.y*20:0));
					if(damage>10)
						phys->damage+=damage;
				}
			}
			break;
		case kPhysicsTypeSolid:
			{
				tSolidEntityPhysics *ent=(tSolidEntityPhysics*)FileGetParsedDataPtr(entity->physicsData,kParserTypeSolidEntityDesc,sizeof(tSolidEntityPhysics));
				tVector3 force=veloDiff*ent->mass*kFPS*rotationFactor;
				tVector3 torque=(attackPoint-ent->massCenter*entity->dir)%force;
				tMatrix3 invMatrix;
				MatrixTranspose(entity->dir,invMatrix);
				torque=torque*invMatrix;
				tVector3 angularAcceleration=torque*1/ent->inertia*entity->dir;
				//if(~angularAcceleration>1+~entity->velo)angularAcceleration=!angularAcceleration*(1+~entity->velo);
				tMatrix3 accelerationMatrix;
				RotationVectorToMatrix(angularAcceleration*kFrameTime*kFrameTime,accelerationMatrix);
				MatrixMult(entity->rVelo,accelerationMatrix,entity->rVelo);	
				entity->velo=entity->velo+veloDiff;
			}
			break;
	}
}

//returns true if an object is moving, false if it isn't
int CheckForMotion(tGameEntity *entity)
{
	if(~entity->velo>kMinVelo)
		return true;
	float rotMotion=~((Vector(1,0,0)*entity->rVelo)-Vector(1,0,0))*kFPS;
	if(rotMotion<kMinRotVelo)
		return false;
	return true;
}

//returns the offset between point and the ground (road).
//lastRoadIndex is a hint where to start looking when trying to find a piece of road under
//point.
float GetGroundOffset(tVector3 point,int *lastRoadIndex,tVector3 *normal,int *surfaceType)
{
	//this function does nothing but calling RoadGetYValue - in earlier builds there used to be
	//other types of ground that's why this function is here (and not just RoadGetYValue).
	return RoadGetYValue(point,lastRoadIndex,normal,surfaceType,NULL);
}

float GetGroundOffsetAndBump(tVector3 point,int *lastRoadIndex,tVector3 *normal,int *surfaceType,float *bump)
{
	//this function does nothing but calling RoadGetYValue - in earlier builds there used to be
	//other types of ground that's why this function is here (and not just RoadGetYValue).
	return RoadGetYValue(point,lastRoadIndex,normal,surfaceType,bump);
}


#define kMinSparkVelo 5.0
#define kMaxSparkVelo 20.0
#define kMaxSpark 60.0

//makes amount/second spark particles flying away from pos
void MakeSparks(tVector3 pos,float amount)
{
	tParticlesDef def;
	ParticlesInitDef(&def);
	def.sprite=FileGetReference("spark.pct");
	def.maxSpread=0.1;
	def.maxVelo=6;
	def.gravity=15;
	def.maxLife=0.4;
	def.xSize=0.05;def.ySize=0.4;
	def.veloRotation=true;
	ParticlesCreate(amount*kFrameTime,pos,Vector(0,0,0),&def);	
}

//makes squeak and crash noises for carEntity colliding with something at fVeloDiff
void MakeSqueaks(tGameEntity *carEntity,float fVeloDiff,float angle)
{
	if(angle<0.2&&fVeloDiff<5&&fVeloDiff>1)
		CarPlayCrashNoise(carEntity,FileGetIndexedReference(FileGetReference("scratch.wav"),RandomInt(0,4)),2.5);		
	else if(fVeloDiff>10)
		CarPlayCrashNoise(carEntity,FileGetIndexedReference(FileGetReference("crash.wav"),RandomInt(0,6)),2.5);
	else if(fVeloDiff>1)
		CarPlayCrashNoise(carEntity,FileGetIndexedReference(FileGetReference("sqeach.wav"),RandomInt(0,3)),2.5);
}

//tests if entity collides with the ground and taks appropiate actions
void CheckGroundCollision(tGameEntity *entity)
{
	int numCollBoxes;
	tCollBox *boxes;
	tCarPhysics *phys;
	tCarDefinition *car;
	
	//gets collsion boxes for object
	switch(entity->physicsType)
	{
		case kPhysicsTypeCar:
			phys=(tCarPhysics*)entity->physics;
			car=&(phys->car);
			numCollBoxes=car->numCollBoxes;
			boxes=car->coll;
			break;
		case kPhysicsTypeSolid:
			tSolidEntityPhysics *ent=(tSolidEntityPhysics*)FileGetParsedDataPtr(entity->physicsData,kParserTypeSolidEntityDesc,sizeof(tSolidEntityPhysics));
			numCollBoxes=ent->numCollBoxes;
			boxes=ent->coll;
			break;
	}
	
	//for cars we take a few extra points to improve precision
	int numEdges=(entity->physicsType==kPhysicsTypeCar?12:8);
	
	for(int box=0;box<numCollBoxes;box++)
		for(int i=0;i<numEdges;i++)
		//for each edge in each collision box...
		{
			tVector3 normal;
			tVector3 rotPoint;
			
			if(i<8)//'normal' edges
				rotPoint=((tVector3*)(boxes+box))[i];
			else//additional edges used for cars, calculated from normal edges.
				rotPoint=(((tVector3*)(boxes+box))[i-4]+((tVector3*)(boxes+box))[(i-3)%8])*0.5;			
			
			rotPoint=rotPoint*entity->dir;
			tVector3 globalPoint=rotPoint+entity->pos;
			int surfaceType=-1;
			//...check if it reaches into the ground.
			if(GetGroundOffset(globalPoint,&entity->lastRoadIndex,&normal,&surfaceType)<0)
			{
				if(entity->physicsType==kPhysicsTypeCar)
					phys->collision=gFrameCount;
				tVector3 pointVelo=((rotPoint*entity->rVelo)-rotPoint)*kFPS+entity->velo;
				tVector3 testPoint=globalPoint-pointVelo*kFindNormalFactor*kFrameTime;
				tVector3 testNormal;
				
				//try moving the point backwards along its velocity vector until
				//it doen't touch the ground anymore, to get the exact point of impact
				int count;
				for(count=0;count<kMaxNormalSeperation&&GetGroundOffset(testPoint,&entity->lastRoadIndex,&testNormal,0)<0;count++)
				{
					testPoint=testPoint-pointVelo*kFindNormalFactor*kFrameTime;
					normal=testNormal;
				}
	
				//Calculate the speed of impact with the ground
				float veloDiff=(-normal*pointVelo);
				
				//Apply the ground's Counter-Impulse
				if(veloDiff>0)
				{
					tVector3 pointGroundBrake=kGroundFrictionAcceleration*kFrameTime*(entity->physicsType==kPhysicsTypeCar?kCarCollisionRate:kSolidCollisionRate)*!((normal%pointVelo)%normal);
					float groundFactor=normal*Vector(0,1,0)*0.25;
					float strictHit=1;
					tVector3 impulseVelo=normal*veloDiff*(0.4-groundFactor*0.3)-pointGroundBrake;
					if(~impulseVelo>kMaxImpulseVelo)
						impulseVelo=!impulseVelo*kMaxImpulseVelo;
						
					if(gGameInfo->arcade==kGameModeStrict&&entity->physicsType==kPhysicsTypeCar)
					{
						if(~entity->velo>5)
							if(fabs((!impulseVelo).y)<0.2)
							{
								impulseVelo=impulseVelo-entity->velo*kFrameTime*10;
								strictHit=0;
							}
					}

					
					ApplyImpulse(entity,rotPoint,impulseVelo,strictHit*(0.5-0.5*groundFactor),false);
//					ApplyImpulse(entity,rotPoint,impulseVelo,0.1);
				}

				//move the object away from the ground until it doesn't touch ground any longer
				//if(entity->physicsMachine==kPhysicsLocal)
				for(int count=0;count<kMaxSeperation&&GetGroundOffset(globalPoint,&entity->lastRoadIndex,&testNormal,0)<0;count++)
				{
					entity->pos=entity->pos+normal*kSeperationFactor*count;
					globalPoint=globalPoint+normal*kSeperationFactor*count;

					if(surfaceType!=-1)
						if(gSurfaceTypes->types[surfaceType].sparksEnable)
						{
							float sparks=(~entity->velo-kMinSparkVelo)/(kMaxSparkVelo-kMinSparkVelo);
							if(sparks>1)sparks=1;
							sparks*=kMaxSpark;
							
							if(entity->physicsType==kPhysicsTypeSolid)
							{
								tSolidEntityPhysics *ent=(tSolidEntityPhysics*)FileGetParsedDataPtr(entity->physicsData,kParserTypeSolidEntityDesc,sizeof(tSolidEntityPhysics));
								if(!ent->sparks)
									sparks=0;
							}
							
							if(sparks>0.0)
								MakeSparks(rotPoint+entity->pos,sparks);
						}
				}
				entity->netPos=entity->pos;

				//make noise if neccesary
				if(entity->physicsType==kPhysicsTypeCar)
					if(surfaceType!=-1)
						if(gSurfaceTypes->types[surfaceType].squeachEnable)
							MakeSqueaks(entity,veloDiff,-normal*!pointVelo);
					
			}
		}
}

#define kMinSparkCollVelo 0.0
#define kMaxSparkCollVelo 40.0
#define kMaxSparkColl 200.0

//Checks if the line |l1-l2| intersects the rectangle represented by area (an array of 4 points).
//returns the point of intersection (if any) in hit.
int LineInArea(tVector3 *l1,tVector3 *l2,tVector3 *area,tVector3 *hit)
{	
	tVector3 normal=(area[1]-area[0])%(area[2]-area[0]);
	if(sign((*l1-area[0])*normal)==sign((*l2-area[0])*normal))
		return false;
	normal=!normal;
	float dist=(*l1-area[0])*normal;
	tVector3 dir=!(*l2-*l1);
	tVector3 hitPoint;
	if(float sp=dir*normal)
		hitPoint=*l1+dir*fabs(dist*1/(sp));
	else if(dist==0)
		hitPoint=*l1;
	else
		return false;
	for(int i=0;i<4;i++)
		if(((area[(i+1)&3]-area[i])%(hitPoint-area[i]))*normal<0)
			return false;
	*hit=hitPoint;
	return true;
}

//Checks if the line |l1-l2| intersects the box represented by the tCollBox structure box.
//returns the point of intersection (if any) in hit.
int LineInBox(tVector3 *l1,tVector3 *l2,tCollBox *box,tVector3 *hit)
{
	if(LineInArea(l1,l2,(tVector3*)box,hit))return true;
	if(LineInArea(l1,l2,(tVector3*)box+4,hit))return true;
	tVector3 area[4];
	area[0]=box->tfr;
	area[1]=box->tfl;
	area[2]=box->bfl;
	area[3]=box->bfr;
	if(LineInArea(l1,l2,area,hit))return true;
	area[0]=box->trr;
	area[1]=box->trl;
	area[2]=box->brl;
	area[3]=box->brr;
	if(LineInArea(l1,l2,area,hit))return true;
	area[0]=box->tfr;
	area[1]=box->trr;
	area[2]=box->brr;
	area[3]=box->bfr;
	if(LineInArea(l1,l2,area,hit))return true;
	area[0]=box->tfl;
	area[1]=box->trl;
	area[2]=box->brl;
	area[3]=box->bfl;
	if(LineInArea(l1,l2,area,hit))return true;
	return false;
}

//Tests if two boxes intersect each other
int CheckBoxCollision(tCollBox *box1,tCollBox *box2,tVector3 *hitPoint,int *hitObj)
{
	tVector3 box1Max=Vector(-INFINITY,-INFINITY,-INFINITY),box1Min=Vector(INFINITY,INFINITY,INFINITY)
			,box2Max=Vector(-INFINITY,-INFINITY,-INFINITY),box2Min=Vector(INFINITY,INFINITY,INFINITY);
	
	//Calculate an axis-aligned bounding-box for each of the two boxes 
	for(int i=0;i<8;i++)
	{
		if(((tVector3*)box1)[i].x<box1Min.x)box1Min.x=((tVector3*)box1)[i].x;
		if(((tVector3*)box1)[i].y<box1Min.y)box1Min.y=((tVector3*)box1)[i].y;
		if(((tVector3*)box1)[i].z<box1Min.z)box1Min.z=((tVector3*)box1)[i].z;

		if(((tVector3*)box1)[i].x>box1Max.x)box1Max.x=((tVector3*)box1)[i].x;
		if(((tVector3*)box1)[i].y>box1Max.y)box1Max.y=((tVector3*)box1)[i].y;
		if(((tVector3*)box1)[i].z>box1Max.z)box1Max.z=((tVector3*)box1)[i].z;

		if(((tVector3*)box2)[i].x<box2Min.x)box2Min.x=((tVector3*)box1)[i].x;
		if(((tVector3*)box2)[i].y<box2Min.y)box2Min.y=((tVector3*)box1)[i].y;
		if(((tVector3*)box2)[i].z<box2Min.z)box2Min.z=((tVector3*)box1)[i].z;

		if(((tVector3*)box2)[i].x>box2Max.x)box2Max.x=((tVector3*)box2)[i].x;
		if(((tVector3*)box2)[i].y>box2Max.y)box2Max.y=((tVector3*)box2)[i].y;
		if(((tVector3*)box2)[i].z>box2Max.z)box2Max.z=((tVector3*)box2)[i].z;
	}
	
	//if the axis-aligned bounding-boxes don't share the same coordinate
	//intervals, return false.
	if(box1Max.x<box2Min.x)return false;
	if(box1Max.y<box2Min.y)return false;
	if(box1Max.z<box2Min.z)return false;

	if(box2Max.x<box1Min.x)return false;
	if(box2Max.y<box1Min.y)return false;
	if(box2Max.z<box1Min.z)return false;

	//for each line of each box, test if it intersects the other box
	for(int i=0;i<4;i++)
	{
		if(LineInBox(((tVector3*)box1)+i,((tVector3*)box1)+((i+1)&3),box2,hitPoint))return true;
		if(LineInBox(((tVector3*)box1)+i,((tVector3*)box1)+i+4,box2,hitPoint))return true;
		if(LineInBox(((tVector3*)box1)+i+4,((tVector3*)box1)+((i+1)&3)+4,box2,hitPoint))return true;

		if(LineInBox(((tVector3*)box2)+i,((tVector3*)box2)+((i+1)&3),box1,hitPoint))return true;
		if(LineInBox(((tVector3*)box2)+i,((tVector3*)box2)+i+4,box1,hitPoint))return true;
		if(LineInBox(((tVector3*)box2)+i+4,((tVector3*)box2)+((i+1)&3)+4,box1,hitPoint))return true;
	}
	return false;
}

//Translate a box in object coordinates to a box in world coordinates
void BoxWorldCoords(tCollBox *box,tCollBox *boxWorld,tMatrix3 dir,tVector3 pos)
{
	for(int i=0;i<8;i++)
		((tVector3*)boxWorld)[i]=((tVector3*)box)[i]*dir+pos;
}

//Check if any of the boxes in the array boxes1 intersects any of the boxes in the array boxes2.
//boxes are translated using dir1/2 and pos1/2. 
//if an intersection is found, it is returned in hitPoint.
int CheckBoxListCollision(tCollBox *boxes1,tCollBox *boxes2,int numBoxes1,int numBoxes2,tMatrix3 dir1,tMatrix3 dir2,tVector3 pos1,tVector3 pos2,tVector3 *hitPoint)
{
	//allocte memory for boxes in world coordinates
	tCollBox *worldBoxes1=(tCollBox*)MemoryAllocateBlock(sizeof(tCollBox)*numBoxes1);
	tCollBox *worldBoxes2=(tCollBox*)MemoryAllocateBlock(sizeof(tCollBox)*numBoxes2);

	//transform boxes into world coordinates
	for(int i=0;i<numBoxes1;i++)
		BoxWorldCoords(boxes1+i,worldBoxes1+i,dir1,pos1);
	for(int i=0;i<numBoxes2;i++)
		BoxWorldCoords(boxes2+i,worldBoxes2+i,dir2,pos2);
	
	int hitObj,hitBox;
	for(int i=0;i<numBoxes1;i++)
		for(int j=0;j<numBoxes2;j++)	
			//for each pair of boxes test for intersection
			if(CheckBoxCollision(worldBoxes1+i,worldBoxes2+j,hitPoint,&hitObj))
			{
				MemoryFreeBlock(worldBoxes1);
				MemoryFreeBlock(worldBoxes2);	
				return true;
			}
			
	MemoryFreeBlock(worldBoxes1);
	MemoryFreeBlock(worldBoxes2);	
	return false;
}

//for a set of boxes, calculate the radius of the bounding sphere 
//= the distance of the furthest point from (0,0,0) (in object coordinates)
float CalcMaxCollRadius(tCollBox *boxes,int numCollBoxes)
{
	tVector3 extends=Vector(0,0,0);
	for(int i=0;i<numCollBoxes;i++)
		for(int j=0;j<8;j++)
		{
			if(fabs(((tVector3*)(boxes+i))[j].x)>extends.x)
				extends.x=fabs(((tVector3*)(boxes+i))[j].x);
			if(fabs(((tVector3*)(boxes+i))[j].y)>extends.y)
				extends.y=fabs(((tVector3*)(boxes+i))[j].y);
			if(fabs(((tVector3*)(boxes+i))[j].z)>extends.z)
				extends.z=fabs(((tVector3*)(boxes+i))[j].z);
		}
	return ~extends;
}

//implements the physical laws of collision.
//given two velocities and two masses, returns two new velocities
//collType defines what type of collision this is:
//if collType=0, this is a completely plastic collision, ie. both objects will be moving at the same speed afterwards.
//if collType=1, this is a completely elastic collision, ie. both objects will be moving away from each other.
void CalcCollisionImpulses(tVector3 v1,tVector3 v2,float m1,float m2,tVector3 *v1n,tVector3 *v2n,float collType)
{
	tVector3 vUnelastic=(m1*v1+m2*v2)/(m1+m2);
	tVector3 v1Elastic=(m1*v1+m2*(2*v2-v1))/(m1+m2);
	tVector3 v2Elastic=(m2*v2+m1*(2*v1-v2))/(m1+m2);
	*v1n=collType*v1Elastic+(1-collType)*vUnelastic;
	*v2n=collType*v2Elastic+(1-collType)*vUnelastic;
}

//Tests for a collision between a car and a solid entity (any obstacles, etc.. in the game), and takes appropoiate action
void HandleCarSolidCollision(tGameEntity *carEntity,tGameEntity *entity)
{
	tCarPhysics *phys=(tCarPhysics*)carEntity->physics;
	tCarDefinition *car=&(phys->car);
	tSolidEntityPhysics *ent=(tSolidEntityPhysics*)FileGetParsedDataPtr(entity->physicsData,kParserTypeSolidEntityDesc,sizeof(tSolidEntityPhysics));

	//get bounding sphere radii of objects
	if(!car->maxCollRadius)car->maxCollRadius=CalcMaxCollRadius(car->coll,car->numCollBoxes);
	if(!ent->maxCollRadius)ent->maxCollRadius=CalcMaxCollRadius(ent->coll,ent->numCollBoxes);
	
	//tests if the object's bounding spheres intersect
	tVector3 dist=carEntity->pos-entity->pos;
	if(sqr(dist)>sqr(car->maxCollRadius+ent->maxCollRadius))
		return;

	tVector3 hitPoint;
	int coll=false;
	int movable=ent->movable;
		
	if(!ent->liquid)
		if(movable)
		{
			if(CheckBoxListCollision(car->coll,ent->coll,car->numCollBoxes,ent->numCollBoxes,carEntity->dir,entity->dir,carEntity->pos,entity->pos,&hitPoint))
			{
				if((entity->pos-carEntity->pos)*carEntity->velo>0)
				{
					entity->pos=entity->pos+carEntity->velo*kFrameTime;
					coll=true;
				}
			}
		}
		else
			if(CheckBoxListCollision(car->coll,ent->coll,car->numCollBoxes,ent->numCollBoxes,carEntity->dir,entity->dir,carEntity->pos,entity->pos,&hitPoint))
			{
				carEntity->pos=carEntity->oldPos;
				MatrixCopy(carEntity->oldDir,carEntity->dir);
				coll=true;
				phys->collision=gFrameCount;
			}
	else
		coll=CheckBoxListCollision(car->coll,ent->coll,car->numCollBoxes,ent->numCollBoxes,carEntity->dir,entity->dir,carEntity->pos,entity->pos,&hitPoint);

	//was there a collision?
	if(coll)
	{
		//is the object movable (like a barrel, etc..)
		if(movable)
		{
			//calculate new velocities of car and object
			tVector3 carHitPoint=hitPoint-carEntity->pos;
			tVector3 carPointVelo=((carHitPoint*carEntity->rVelo)-carHitPoint)*kFPS+carEntity->velo;
			
			tVector3 entHitPoint=hitPoint-entity->pos;
			tVector3 entPointVelo=((entHitPoint*entity->rVelo)-entHitPoint)*kFPS+entity->velo;

			tVector3 newCarPointVelo;
			tVector3 newEntPointVelo;
			
			CalcCollisionImpulses(carPointVelo,entPointVelo,car->mass,ent->mass,&newCarPointVelo,&newEntPointVelo,0.5);
			
			float carImpulseScale=(~carEntity->velo)/10;
			if(carImpulseScale>1)carImpulseScale=1;
			ApplyImpulse(carEntity,carHitPoint,(newCarPointVelo-carPointVelo)*0.6*carImpulseScale,0.15,false);
			
			tVector3 oldVelo=entity->velo;
			tMatrix3 oldrVelo;
			MatrixCopy(entity->rVelo,oldrVelo);
			ApplyImpulse(entity,entHitPoint,(newEntPointVelo-entPointVelo)*0.25,0.5,false);
			
			if(ent->mass>=kSolidEntityNetworkMass&&carEntity->physicsMachine==kPhysicsLocal)
				entity->lastFrame=gFrameCount;
			if(ent->numSounds)
			{
				float vol=~(newEntPointVelo-entPointVelo)*0.1;
				if(vol>0.5)
					CarPlayCrashNoise(carEntity,FileGetIndexedReference(ent->sound,RandomInt(0,ent->numSounds)),vol);
			}
		}
		else//object not movable (eg. trees, etc..)
		{
			tVector3 entHitPoint=hitPoint-entity->pos;
	
			hitPoint=hitPoint-carEntity->pos;
			tVector3 rotVelo=((hitPoint*carEntity->rVelo)-hitPoint)*kFPS;
			if(~rotVelo>5)rotVelo=!rotVelo*5;
			tVector3 pointVelo=rotVelo+carEntity->velo;
			float sparks=(~carEntity->velo-kMinSparkVelo)/(kMaxSparkVelo-kMinSparkVelo);
			if(sparks>1)sparks=1;
			sparks*=kMaxSpark*kFrameTime;
			MakeSparks(hitPoint+carEntity->pos,sparks);
			MakeSqueaks(carEntity,~pointVelo,1);
			ApplyImpulse(carEntity,hitPoint,-pointVelo*0.25,0.8,false);
			
			if(~carEntity->velo>10&&ent->inertia&&!ent->movable)
			{
				tVector3 torque=(entHitPoint)%(carEntity->velo*car->mass*kFPS);
				tMatrix3 invMatrix;
				MatrixTranspose(entity->dir,invMatrix);
				torque=torque*invMatrix;
				tVector3 angularAcceleration=torque*1/ent->inertia*entity->dir;
				tMatrix3 accelerationMatrix;
				RotationVectorToMatrix(angularAcceleration*kFrameTime*kFrameTime,accelerationMatrix);
				MatrixMult(entity->dir,accelerationMatrix,entity->dir);	
				carEntity->pos=carEntity->pos-carEntity->velo*kFrameTime*6;				
			}
			
			float v=~carEntity->velo;
			if(v>5)
				carEntity->velo=carEntity->velo*((v-5)/v);
			else
				carEntity->velo=Vector(0,0,0);
		}
		if(entity->untouchable)
			gDisqualified=true;
	}
}

//Checks if two cars collide and take appropiate action
void HandleCarCarCollision(tGameEntity *carEntity1,tGameEntity *carEntity2)
{
	tCarPhysics *phys1=(tCarPhysics*)carEntity1->physics;
	tCarDefinition *car1=&(phys1->car);
	tCarPhysics *phys2=(tCarPhysics*)carEntity2->physics;
	tCarDefinition *car2=&(phys2->car);

	//get bounding sphere radii of objects
	if(!car1->maxCollRadius)car1->maxCollRadius=CalcMaxCollRadius(car1->coll,car1->numCollBoxes);
	if(!car2->maxCollRadius)car2->maxCollRadius=CalcMaxCollRadius(car2->coll,car2->numCollBoxes);
	
	//tests if the object's bounding spheres intersect
	tVector3 dist=carEntity1->pos-carEntity2->pos;
	if(sqr(dist)>sqr(car1->maxCollRadius+car2->maxCollRadius))
		return;

	tVector3 hitPoint;
	int coll=false;
	//tests if objects box lists intersect
	while(CheckBoxListCollision(car1->coll,car2->coll,car1->numCollBoxes,car2->numCollBoxes,carEntity1->dir,carEntity2->dir,carEntity1->pos,carEntity2->pos,&hitPoint)&&!(gReplay&&coll))
	{
		//move cars away from ech other until they no longer intersect
		if(!gReplay)
		{
			tVector3 diff=carEntity1->pos-carEntity2->pos;
			carEntity2->pos=carEntity1->pos-diff*1.01;
			carEntity1->pos=carEntity2->pos+diff*(1.01*1.01);
		}
		coll=true;
	}

	//was there a collision?
	if(coll)
	{
		//calculate new velocities of cars
		tVector3 car1HitPoint=hitPoint-carEntity1->pos;
		tVector3 car1PointVelo=((car1HitPoint*carEntity1->rVelo)-car1HitPoint)*kFPS;
		if(carEntity1->physicsMachine==kPhysicsLocal)
			car1PointVelo=car1PointVelo+carEntity1->velo;
		else
			car1PointVelo=car1PointVelo+carEntity1->collVelo;
		
		tVector3 car2HitPoint=hitPoint-carEntity2->pos;
		tVector3 car2PointVelo=((car2HitPoint*carEntity2->rVelo)-car2HitPoint)*kFPS;
		if(carEntity2->physicsMachine==kPhysicsLocal)
			car2PointVelo=car2PointVelo+carEntity2->velo;
		else
			car2PointVelo=car2PointVelo+carEntity2->collVelo;

		tVector3 newCar1PointVelo;
		tVector3 newCar2PointVelo;
		
		if(!gReplay)
		{
			CalcCollisionImpulses(car1PointVelo,car2PointVelo,car1->mass,car2->mass,&newCar1PointVelo,&newCar2PointVelo,1);	

			ApplyImpulse(carEntity1,car1HitPoint,(newCar1PointVelo-car1PointVelo)*0.25,0.1,false);
			ApplyImpulse(carEntity2,car2HitPoint,(newCar2PointVelo-car2PointVelo)*0.25,0.1,false);
			carEntity1->netVelo=carEntity1->velo;
			MatrixCopy(carEntity1->rVelo,carEntity1->netRVelo);
			carEntity1->netPos=carEntity1->pos;
			MatrixCopy(carEntity1->dir,carEntity1->netDir);
			carEntity2->netVelo=carEntity2->velo;
			MatrixCopy(carEntity2->rVelo,carEntity2->netRVelo);
			carEntity2->netPos=carEntity2->pos;
			MatrixCopy(carEntity2->dir,carEntity2->netDir);
			carEntity1->accel=Vector(0,0,0);
			carEntity2->accel=Vector(0,0,0);
		}
		float impulse=~(newCar1PointVelo-car1PointVelo)+~(newCar2PointVelo-car2PointVelo);
		float sparks=(impulse-kMinSparkVelo)/(kMaxSparkVelo-kMinSparkVelo);
		if(sparks>1)sparks=1;
		sparks*=kMaxSpark*kFrameTime*5;
		MakeSparks(hitPoint,sparks);
		MakeSqueaks(carEntity1,impulse,1);
	}
}


//for a car, test if it collides with anything else (ground, other cars or objects).
void CarCheckCollision(tGameEntity *carEntity)
{
	tGameEntity *entity=(tGameEntity*)gFirstEntity->next;
	
	//if(carEntity->physicsMachine==kPhysicsLocal)
	{
	//	if(!gReplay)
			
		if(!(gFrameCount%kCarCollisionRate))
		{
			if(carEntity->physicsMachine!=kPhysicsLocal)
				gQuickRoadCollision=true;
			gRoadRestrictedBorders=true;
			CheckGroundCollision(carEntity);
			gRoadRestrictedBorders=false;
			gQuickRoadCollision=false;
		}
	}
	
	//for each other object, test for collision.
	if(!(gFrameCount%kCarCollisionRate))
		while(entity!=gFirstEntity)
		{
			entity->regData=carEntity->regData;
			if(sqr(entity->pos-carEntity->pos)<500)
				if(entity!=carEntity)
					switch(entity->physicsType)
					{
						case kPhysicsTypeSolid:
							HandleCarSolidCollision(carEntity,entity);
							break;
						case kPhysicsTypeCar:
							//if(carEntity->physicsMachine==kPhysicsLocal)
								HandleCarCarCollision(carEntity,entity);
							break;
					}
			entity=(tGameEntity*)entity->next;
		}
}

//for a solid entity, check if it intersects with the ground
void SolidCheckCollision(tGameEntity *entity)
{
	tSolidEntityPhysics *ent=(tSolidEntityPhysics*)FileGetParsedDataPtr(entity->physicsData,kParserTypeSolidEntityDesc,sizeof(tSolidEntityPhysics));
	if(!ent->movable)
		return;
	CheckGroundCollision(entity);
	
	tVector3 xDiff=*MatrixGetXVector(entity->rVelo)-Vector(1,0,0);
	tVector3 yDiff=*MatrixGetYVector(entity->rVelo)-Vector(0,1,0);
	tVector3 zDiff=*MatrixGetZVector(entity->rVelo)-Vector(0,0,1);
	*MatrixGetXVector(entity->rVelo)=*MatrixGetXVector(entity->rVelo)-xDiff*1.5*kFrameTime;
	*MatrixGetYVector(entity->rVelo)=*MatrixGetYVector(entity->rVelo)-yDiff*1.5*kFrameTime;
	*MatrixGetZVector(entity->rVelo)=*MatrixGetZVector(entity->rVelo)-zDiff*1.5*kFrameTime;
}

tRegData rd;

//check for any object collisions
void CollisionFrame()
{
	#ifndef __TARGET_TOOLAPP
	rd.name=RT3_GetLicenseName();
	rd.numCopies=RT3_GetLicenseCopies();
	#endif
	tGameEntity *entity=(tGameEntity*)gFirstEntity->next;
	while(entity!=gFirstEntity)
	{
			entity->regData=&rd;
			//has the object moved in the last second?
			//if we have a solid entity like a barrel which has been kicked over by a car,
			//we don't need to process collisions with the ground anymore once the barrel
			//has come to rest.
			if(entity->lastActivity+kFPS>gFrameCount&&entity->id>=0)
				//if(entity->physicsMachine!=kPhysicsRemote||entity->lastCollFrame==0||entity->lastCollFrame>gFrameCount-kFPS)
					switch(entity->physicsType)
					{
						case kPhysicsTypeCar:
							CarCheckCollision(entity);
							break;
						case kPhysicsTypeSolid:
							if(!(gFrameCount%kSolidCollisionRate))
								SolidCheckCollision(entity);
							break;
					}
		entity=(tGameEntity*)entity->next;
	}

}
