//carphysics.cpp
//simulation of all forces and tourques moving the cars around 

#include <math.h>

#include <AmbrosiaTools/stdtypes.h>

#include "vectors.h"
#include "entities.h"
#include "carphysics.h"
#include "gamemem.h"
#include "gameframe.h"
#include "roads.h"
#include "text.h"
#include "particles.h"
#include "tracks.h"
#include "collision.h"
#include "config.h"
#include "environment.h"
#include "gamesound.h"
#include "renderframe.h"
#include "networkphysics.h"

#define kMaxSmoke 120.0

#define kIndicatorTime 0.5
#define kMaxTwistAngle (PI/12)
#define kSpinRotations 10.0
#define kTrackPrecision 5 //each nTH frame a skid mark is generated

void InstallCarAddOns(tCarPhysics *phys)
{
	tCarDefinition *car=&(phys->car);
	for(int i=0;i<car->numAddOns;i++)
		//is this add-on installed?
		if((phys->addOns>>i)&1)
			if(!(phys->addOns&car->addOns[i].conflicts))
			{
				car->mass+=car->addOns[i].mass;
				car->massCenter=car->massCenter+car->addOns[i].massCenter;
				car->rearLift+=car->addOns[i].rearLift;
				car->frontLift+=car->addOns[i].frontLift;
				car->frontAirResistance+=car->addOns[i].frontAirResistance;	
				
				if(car->addOns[i].powerPercent>0)
				{
					car->power*=car->addOns[i].powerPercent;
					car->powerRPM*=car->addOns[i].powerPercent;
				}
				if(car->addOns[i].frontSwayBar>0)
					car->frontSwayBar=car->addOns[i].frontSwayBar;
				if(car->addOns[i].exhaustFire>0)
					car->exhaustFire=car->addOns[i].exhaustFire;
				if(car->addOns[i].rearSwayBar>0)
					car->rearSwayBar=car->addOns[i].rearSwayBar;
				if(car->addOns[i].damperStrength>0)
					car->damperStrength=car->addOns[i].damperStrength;
				if(car->addOns[i].engineInertia>0)
					car->engineInertia=car->addOns[i].engineInertia;
				if(car->addOns[i].topGearRatio>0)
					car->gearRatios[car->numGears-1]=car->addOns[i].topGearRatio;
				if(car->addOns[i].maxRPM>0)
					car->maxRPM=car->addOns[i].maxRPM;
				if(car->addOns[i].track>0)
				{
					car->wheels[0].pos.x-=car->addOns[i].track;
					car->wheels[1].pos.x+=car->addOns[i].track;
					car->wheels[2].pos.x-=car->addOns[i].track;
					car->wheels[3].pos.x+=car->addOns[i].track;
				}
				if(car->addOns[i].frontWheelWidth>0)
				{
					car->wheels[0].width=car->addOns[i].frontWheelWidth;
					car->wheels[1].width=car->addOns[i].frontWheelWidth;
				}
				if(car->addOns[i].rearWheelWidth>0)
				{
					car->wheels[2].width=car->addOns[i].rearWheelWidth;
					car->wheels[3].width=car->addOns[i].rearWheelWidth;
				}
				if(car->addOns[i].frontMaxSuspension>0)
				{
					car->wheels[0].maxSuspension=car->addOns[i].frontMaxSuspension;
					car->wheels[1].maxSuspension=car->addOns[i].frontMaxSuspension;
				}
				if(car->addOns[i].rearMaxSuspension>0)
				{
					car->wheels[2].maxSuspension=car->addOns[i].rearMaxSuspension;
					car->wheels[3].maxSuspension=car->addOns[i].rearMaxSuspension;
				}
				if(car->addOns[i].torquePercent>0)
					car->torque*=car->addOns[i].torquePercent;
				if(car->addOns[i].differentialLockCoefficient>0)
					car->differentialLockCoefficient=car->addOns[i].differentialLockCoefficient;
				if(car->addOns[i].finalDriveRatio>0)
					car->finalDriveRatio=car->addOns[i].finalDriveRatio;
				if(car->addOns[i].gearSwitchTime>0)
					car->gearSwitchTime=car->addOns[i].gearSwitchTime;
			}
}


float WheelGroundDistance(tGameEntity *carEntity,tVector3 wheelPos,tVector3 yDir,tWheelDefinition *wheel,tVector3 *groundNormal,float oldHeight,int *surfaceType,float *bump)
//returns the distance between the wheel at wheelPos and the ground.
//gives the normal of the ground under the wheel in groundNormal
//and the surface type of the ground in surfaceType
{
	tVector3 wheelGroundPoint=wheelPos-yDir*(wheel->radius+oldHeight);
	return GetGroundOffsetAndBump(wheelGroundPoint,&carEntity->lastRoadIndex,groundNormal,surfaceType,bump)+oldHeight;
}

void CalcWheelPositions(tGameEntity *carEntity,tCarDefinition *car,tCarPhysics *phys,tWheelCalcData *wheels)
//Calculates the positions and directions/angles of all wheels of the car passed in carEntity.
{
	for(int i=0;i<car->numWheels;i++)
	{
		wheels[i].pos=car->wheels[i].pos*carEntity->dir;
		wheels[i].velo=((wheels[i].pos*carEntity->rVelo)-wheels[i].pos)*kFPS+carEntity->velo;
		wheels[i].oldAngle=phys->wheels[i].angle;
		if(phys->wheels[i].rotation>=2*PI)
			phys->wheels[i].rotation-=2*PI;
		else if(phys->wheels[i].rotation<0)
			phys->wheels[i].rotation+=2*PI;
		phys->wheels[i].angle=car->wheels[i].maxAngle*phys->steering;//*(0.5+0.5*sqr(phys->steering));
	}
}

void CalcWheelGroundOffsets(tGameEntity *carEntity,tCarDefinition *car,tCarPhysics *phys,tWheelCalcData *wheels)
//Calculates the spring supression for all wheels of the car passed in carEntity.
{
	for(int i=0;i<car->numWheels;i++)
	{
		float dist=WheelGroundDistance(carEntity,wheels[i].pos+carEntity->pos,*MatrixGetYVector(carEntity->dir),&car->wheels[i],&wheels[i].groundNormal,phys->wheels[i].suspension,&phys->wheels[i].surfaceType,&(wheels[i].bump));
		if(wheels[i].onGround=dist<car->wheels[i].maxSuspension)
		{
			phys->wheels[i].suspension=dist;
			if(phys->wheels[i].suspension<0)
				phys->wheels[i].suspension=0;
		}
		else
			phys->wheels[i].suspension=car->wheels[i].maxSuspension;	
	}
}


void CalcSwayBars(tCarDefinition *car,tCarPhysics *phys,tWheelCalcData *wheels)
//Calculates the Forces generated by the cars Anti-Sway-Bars.
{
	float force;
	int arcade=(gGameInfo->arcade==kGameModeArcade||gGameInfo->arcade==kGameModeTurbo);

	force=(phys->wheels[1].suspension-phys->wheels[0].suspension)*(car->frontSwayBar+(arcade?car->mass*10:0));
	wheels[0].suspensionForce+=force;
	wheels[1].suspensionForce-=force;
	force=(phys->wheels[3].suspension-phys->wheels[2].suspension)*(car->rearSwayBar+(arcade?car->mass*10:0));
	wheels[2].suspensionForce+=force;
	wheels[3].suspensionForce-=force;
}


void CalcWheelSupension(tGameEntity *carEntity,tCarDefinition *car,tCarPhysics *phys,tWheelCalcData *wheels)
//Calculates spring and damper forces
{
	//Calculate the distance of each wheel from car's center of gravity
	float totalInvMassDistance=0;
	float invMassDistance[kMaxWheels];
	for(int i=0;i<car->numWheels;i++)
	{
		invMassDistance[i]=1/~(car->massCenter-car->wheels[i].pos);
		totalInvMassDistance+=invMassDistance[i];
	}
	
	//factor used to calculate the spring forces
	float totalSpringForceFactor=car->mass*2*gEnvironment->gravity;
	
	tVector3 zDir=*MatrixGetZVector(carEntity->dir);
	float airRes=~carEntity->velo*0.5*fabs(zDir*carEntity->velo)*gEnvironment->airDensity;
	for(int i=0;i<car->numWheels;i++)
	{
		//from the distance to CG, determine the fraction of the cars mass this wheel has to carry.
		//not entierly correct, but should suffice for most cases
		float massFraction=invMassDistance[i]/totalInvMassDistance;
		float calcSuspension=(phys->wheels[i].suspension/car->wheels[i].maxSuspension)*2-1;
		calcSuspension=calcSuspension*calcSuspension*calcSuspension;
		calcSuspension=(calcSuspension*0.5+0.5)*car->wheels[i].maxSuspension;
		calcSuspension=phys->wheels[i].suspension;
		//calculate spring force, based on totalSpringForceFactor, massFraction, and the ratio of spring compression
		//wheels[i].suspensionForce=(car->wheels[i].maxSuspension-phys->wheels[i].suspension)*totalSpringForceFactor*massFraction/car->wheels[i].maxSuspension;
		wheels[i].suspensionForce=(car->wheels[i].maxSuspension-calcSuspension)*totalSpringForceFactor*massFraction/car->wheels[i].maxSuspension;

		//if(gGameInfo->arcade&&gFrameCount*kFrameTime<=1)wheels[i].suspensionForce*=gFrameCount*kFrameTime;
		
		//if the springs are fully compressed, we need to add another force, applied by the ground,
		//which keeps the wheel from falling into the ground
		if(!phys->wheels[i].suspension)
		{
			float aerodynamicForce=airRes*(i<2?car->frontLift:car->rearLift);

			float fGroundForce=-wheels[i].velo*wheels[i].groundNormal*kFPS*car->mass/car->numWheels-aerodynamicForce/2;
			if(fGroundForce>car->mass/car->numWheels*gEnvironment->gravity-aerodynamicForce/2)
				fGroundForce=car->mass/car->numWheels*gEnvironment->gravity-aerodynamicForce/2;
			if(fGroundForce>0)
				wheels[i].suspensionForce+=fGroundForce;
		}
		
		//the spring and ground force make up the normal force to the wheel, used to calculate tire traction
		wheels[i].normalForce=wheels[i].suspensionForce;

		//and now, calculate damper forces
		if(wheels[i].onGround){
			//wheels speed WRT the ground
			float wheelYVelo=wheels[i].velo*wheels[i].groundNormal;
			
			//friction caused by the suspension (constant, not dependant on the speed of wheel Y movement)
			float suspensionResistance=sign(wheelYVelo)*car->supsensionFriction*massFraction;
			
			//force caused by dampers (a linear function of wheel Y velocity)
			//dampers stronger in first second to avoid arcade mode shake bug.
			suspensionResistance+=wheelYVelo*car->damperStrength*massFraction*((gGameInfo->arcade&&gFrameCount*kFrameTime<=1)?5:1);
			
			//do not apply forces which are higher than necessary to damp all motion 
			if(fabs(suspensionResistance)>fabs(wheelYVelo*kFPS*car->mass/car->numWheels))
				suspensionResistance=wheelYVelo*kFPS*car->mass/car->numWheels;
				
			//to prevent unexpected behavior when loosing ground contact (ie. jumping with the car)
			if(suspensionResistance>2*car->mass)
				suspensionResistance=2*car->mass;
				
			//apply damper resistance
			wheels[i].suspensionForce-=suspensionResistance;
		}		
	}
	
	CalcSwayBars(car,phys,wheels);
	
	int wall=true;
	for(int i=0;i<car->numWheels;i++)
		if(wheels[i].groundNormal.y!=0)
			wall=false;
	if(wall)
		if(wheels[1].pos.y>wheels[0].pos.y)
		{
			wheels[1].suspensionForce+=100000;
			wheels[3].suspensionForce+=100000;		
		}
		else
		{
			wheels[0].suspensionForce+=100000;
			wheels[2].suspensionForce+=100000;		
		}
}


tVector3 PacejkaCalcWheelRoadForces(tGameEntity *carEntity,tCarDefinition *car,tCarPhysics *phys,tWheelCalcData *wheels,int i,float engineTorque,float frictionTorque);


float CalcTorque(float rpm,tCarDefinition *car)
//Calculate the maximal torque the car can currently produce, based on engine revs.
//this is a function based on the car's power and torque ratings, and does not
//necessaryly correspond to the actual car's torque curve, but it is usually close enough.
//the advantage of this is that we do not need to know a car's actual torque chart to simulate a car.
{
	float torqueResult;
	if(rpm<car->torqueRPM)
		torqueResult=car->torque*(-sqr(rpm/car->torqueRPM-1)+1);
	else {
		float maxPowerTorque=car->power/(car->powerRPM*2*PI/60);
		float aproxFactor=(car->torque-maxPowerTorque)/(2*car->torqueRPM*car->powerRPM-sqr(car->powerRPM)-sqr(car->torqueRPM));
		float torque=aproxFactor*sqr(rpm-car->torqueRPM)+car->torque;
		torqueResult=torque>0?torque:0;
	} 
	if(rpm>car->maxRPM)
	{
		torqueResult*=1-((rpm-car->maxRPM)*0.006);
		if(torqueResult<0)
			torqueResult=0;
	}
	if(rpm<0)
		torqueResult=0;
	return torqueResult;
}

#include "random.h"
void CalcWheelRoadForceNoClutch(tGameEntity *carEntity,tCarDefinition *car,tCarPhysics *phys,tWheelCalcData *wheels)
//Calculater the force the wheels apply onto road when the clutch is fully engaged
{
	//Calculate the drivetrain ratio
	float ratio=car->gearRatios[phys->gear+1]*car->finalDriveRatio;
	if(gGameInfo->arcade==kGameModeTurbo)
		ratio*=0.8;
	
	//Calculate the engine inertia at the final drive
	float inertia=car->engineInertia*sqr(ratio);
	
	//Calculate the frictional braking torque of the engine (in N*m)
	float engineFrictionTorque=25+phys->rpm*0.02;
	if(car->engineBaseFriction||car->engineRPMFriction)
		engineFrictionTorque=car->engineBaseFriction+car->idleRPM*car->engineRPMFriction;
	
	//Calculate the current engine torque (in N*m) 
	float engineTorque=(CalcTorque(phys->rpm,car)+engineFrictionTorque)*phys->throttle;
	
	if(phys->damage>kFatalDamage)
		engineTorque=0;
	if(phys->damage>kEngineDamage)
		engineTorque=RandomProb(0.6)?engineTorque:0;
	
	//Calculate average radius of powered wheels
	float wheelRadius=0;
	for(int i=0;i<car->numWheels;i++)
		wheelRadius+=car->wheels[i].radius*car->wheels[i].powered;

	//Calculate average angular velocity of powered wheels
	float averageAngularVelo=0;
	for(int i=0;i<car->numWheels;i++)
		averageAngularVelo+=car->wheels[i].powered*phys->wheels[i].angularVelo*(car->wheels[i].radius/wheelRadius);

	float rpm=0;
	for(int i=0;i<car->numWheels;i++)
	{
		//Calculate the inertia at the wheel
		wheels[i].inertia=car->wheels[i].inertia+car->wheels[i].powered*inertia;
		
		//Calculate the amount of braking applied to the wheel	
		float braked=phys->brake+phys->handbrake*car->wheels[i].handbraked;
		if(braked>1)braked=1;
		
		float glow=braked*fabs(phys->wheels[i].angularVelo)*0.02;
		if(glow>1)glow=1;
		phys->wheels[i].glow+=(glow-0.2)*0.3*kFrameTime;
		if(phys->wheels[i].glow>1)phys->wheels[i].glow=1;
		else if(phys->wheels[i].glow<0)phys->wheels[i].glow=0;

		//Calculate the torque applied by the brakes
		float brakeFriction=braked*car->wheels[i].braked+fabs(phys->wheels[i].angularVelo)*wheels[i].normalForce*gSurfaceTypes->types[phys->wheels[i].surfaceType].brakeFactor;
		
		//Calculate the torque applied by differential lock
		float lockingTorque=(averageAngularVelo-phys->wheels[i].angularVelo*(car->wheels[i].radius/wheelRadius))*car->wheels[i].powered*car->differentialLockCoefficient;

		//Calculate the total braking torque (brakes, friction) applied on the wheel
		float wheelFrictionTorque=engineFrictionTorque*car->wheels[i].powered*fabs(ratio)+car->wheels[i].friction+brakeFriction;
		
		//Calculate the engine torque applied on the wheels
		float wheelEngineTorque=engineTorque*car->wheels[i].powered*ratio+lockingTorque;
		
		if(wheels[i].onGround)
			//Calculate the force the wheel applies onto the road 
			wheels[i].roadForce=PacejkaCalcWheelRoadForces(carEntity,car,phys,wheels,i,wheelEngineTorque,wheelFrictionTorque);
		else{
			//wheel applies no force onto the road
			wheels[i].roadForce=Vector(0,0,0);
			phys->wheels[i].slipVelo=0;
		
			//Calculate wheel's new angular velocity
			float angularAcceleration=((wheelEngineTorque-wheelFrictionTorque*sign(phys->wheels[i].angularVelo))/wheels[i].inertia);
			if(-angularAcceleration*kFrameTime>phys->wheels[i].angularVelo)
				phys->wheels[i].angularVelo=0;
			else
				phys->wheels[i].angularVelo+=angularAcceleration*kFrameTime;
		}
		//rotate the wheel according to it's angular velocity
		phys->wheels[i].rotation+=phys->wheels[i].angularVelo*kFrameTime;
		
		//Calulate average wheels angular velocities.
		rpm+=phys->wheels[i].angularVelo*car->wheels[i].powered;
	}
	
	//Calculate new engine RPM
	phys->engineAngularVelo=rpm*ratio;
	phys->rpm=phys->engineAngularVelo*60.0/(2*PI);
}


//Calculates how much throttle has to be given to maintain the car's idle RPM
float CalcIdleThrottle(tCarDefinition *car)
{
	float engineFrictionTorque=25+car->idleRPM*0.02;
	if(car->engineBaseFriction||car->engineRPMFriction)
		engineFrictionTorque=car->engineBaseFriction+car->idleRPM*car->engineRPMFriction;
	float engineTorque=CalcTorque(car->idleRPM,car)+engineFrictionTorque;
	return engineFrictionTorque/engineTorque;
}

float CalcEngine(tCarDefinition *car,tCarPhysics *phys)
{
	if(phys->idleThrottle==0)
		phys->idleThrottle=CalcIdleThrottle(car);
	float rawEngineTorque=CalcTorque(phys->rpm,car);
	float engineFrictionTorque=25+phys->rpm*0.02;
	if(car->engineBaseFriction||car->engineRPMFriction)
		engineFrictionTorque=car->engineBaseFriction+phys->rpm*car->engineRPMFriction;
	float engineTorque=(rawEngineTorque+fabs(engineFrictionTorque))*phys->throttle;
	if(phys->rpm>5*car->maxRPM)//safety against engine physics getting out of hand.
	{
		phys->rpm=0;
		phys->gear=0;
	}
	if(phys->damage>kFatalDamage)
		engineTorque=0;
	if(phys->damage>kEngineDamage)
		engineTorque=RandomProb(0.6)?engineTorque:0;

	float engineAngularAcceleration=(engineTorque-engineFrictionTorque)/car->engineInertia;
	phys->rpm=phys->engineAngularVelo*(60.0/(2*PI));
	if(phys->rpm<car->idleRPM)
	{
		phys->rpm=car->idleRPM;
		phys->clutch=0;
	}
	phys->engineAngularVelo+=engineAngularAcceleration*kFrameTime;
	return engineAngularAcceleration;
}

float CalcWheelTorques(tGameEntity *carEntity,tCarDefinition *car,tCarPhysics *phys,tWheelCalcData *wheels,float ratio)
{
	float oldDrivetrainAngularVelo=phys->drivetrainAngularVelo;
	phys->drivetrainAngularVelo=0;

	//Calculate average radius of powered wheels
	float wheelRadius=0;
	for(int i=0;i<car->numWheels;i++)
		wheelRadius+=car->wheels[i].radius*car->wheels[i].powered;

	//Calculate average angular velocity of powered wheels
	float averageAngularVelo=0;
	for(int i=0;i<car->numWheels;i++)
		averageAngularVelo+=car->wheels[i].powered*phys->wheels[i].angularVelo*(car->wheels[i].radius/wheelRadius);

	for(int i=0;i<car->numWheels;i++)
	{
		wheels[i].inertia=car->wheels[i].inertia;

		//Calculate the amount of braking applied to the wheel	
		float braked=phys->brake+phys->handbrake*car->wheels[i].handbraked;
		if(braked>1)braked=1;
		if(gFrameCount*kFrameTime<kStartGameDelaySeconds)
			braked=1;
		
		float glow=braked*fabs(phys->wheels[i].angularVelo)*0.02;
		if(glow>1)glow=1;
		phys->wheels[i].glow+=(glow-0.2)*0.3*kFrameTime;
		if(phys->wheels[i].glow>1)phys->wheels[i].glow=1;
		else if(phys->wheels[i].glow<0)phys->wheels[i].glow=0;

		//Calculate the torque applied by the brakes
		float brakeFriction=braked*car->wheels[i].braked+fabs(phys->wheels[i].angularVelo)*wheels[i].normalForce*gSurfaceTypes->types[phys->wheels[i].surfaceType].brakeFactor;
				
		//Calculate the torque applied by differential lock
		float lockingTorque=(averageAngularVelo-phys->wheels[i].angularVelo*(car->wheels[i].radius/wheelRadius))*car->wheels[i].powered*car->differentialLockCoefficient;
		
		//Calculate the total braking torque (brakes, friction) applied on the wheel
		float wheelFrictionTorque=car->wheels[i].friction+brakeFriction;

		float wheelEngineTorque=phys->clutchTorque*car->wheels[i].powered*ratio+lockingTorque;
		if(wheels[i].onGround)
			wheels[i].roadForce=PacejkaCalcWheelRoadForces(carEntity,car,phys,wheels,i,wheelEngineTorque,wheelFrictionTorque);
		else{
			float angularAcceleration=((wheelEngineTorque-wheelFrictionTorque*sign(phys->wheels[i].angularVelo))/wheels[i].inertia);
			if(-angularAcceleration*kFrameTime>phys->wheels[i].angularVelo)
				phys->wheels[i].angularVelo=0;
			else
				phys->wheels[i].angularVelo+=angularAcceleration*kFrameTime;
			wheels[i].roadForce=Vector(0,0,0);
			phys->wheels[i].slipVelo=0;
		}
		phys->wheels[i].rotation+=phys->wheels[i].angularVelo*kFrameTime;
		phys->drivetrainAngularVelo+=phys->wheels[i].angularVelo*car->wheels[i].powered;
	}
	phys->drivetrainAngularVelo*=ratio;
	return (phys->drivetrainAngularVelo-oldDrivetrainAngularVelo)/kFrameTime;
}

void CalcClutchTorqueTransfer(tCarDefinition *car,tCarPhysics *phys,float ratio,float engineAngularAcceleration,float drivetrainAngularAcceleration)
{
	if(ratio==0||phys->clutch==0)
	{
		phys->clutchTorque=0;
		return;
	}
	float angularAcceleration=(phys->engineAngularVelo+engineAngularAcceleration*kFrameTime-phys->drivetrainAngularVelo+drivetrainAngularAcceleration*kFrameTime)/kFrameTime;
	float i1=car->engineInertia;
	float i2=0;
	for(int i=0;i<car->numWheels;i++)
		if(car->wheels[i].powered)
			i2+=car->wheels[i].inertia/sqr(ratio);
	float inertia=(i1*i2)/(i1+i2);
	float optimalTorque=inertia*angularAcceleration;
	
	float maxTorque=car->maxClutchTorqueTransfer*phys->clutch*2;
	phys->clutchTorque=optimalTorque<maxTorque?optimalTorque:maxTorque;
	phys->engineAngularVelo-=phys->clutchTorque/i1*kFrameTime;
}

void CalcWheelRoadForceClutch(tGameEntity *carEntity,tCarDefinition *car,tCarPhysics *phys,tWheelCalcData *wheels)
{
	float ratio=car->gearRatios[phys->gear+1]*car->finalDriveRatio;
	if(gGameInfo->arcade==kGameModeTurbo)
		ratio*=0.8;
	float engineAngularAcceleration=CalcEngine(car,phys);
	float drivetrainAngularAcceleration=CalcWheelTorques(carEntity,car,phys,wheels,ratio);
	CalcClutchTorqueTransfer(car,phys,ratio,engineAngularAcceleration,drivetrainAngularAcceleration);
}

void CalcWheelRoadForce(tGameEntity *carEntity,tCarDefinition *car,tCarPhysics *phys,tWheelCalcData *wheels)
{
	if(phys->clutch==1.0&&phys->gear!=0)
		CalcWheelRoadForceNoClutch(carEntity,car,phys,wheels);
	else
		CalcWheelRoadForceClutch(carEntity,car,phys,wheels);	
}

void CalcTotalWheelForce(tGameEntity *carEntity,tCarDefinition *car,tCarPhysics *phys,tWheelCalcData *wheels,tVector3 *totalWheelForce,tVector3 *totalWheelTorque)
{
	*totalWheelForce=Vector(0,0,0);
	*totalWheelTorque=Vector(0,0,0);
	for(int i=0;i<car->numWheels;i++){
		tVector3 wheelForce=wheels[i].roadForce+wheels[i].suspensionForce*wheels[i].groundNormal;
		tVector3 wheelGroundPoint=wheels[i].pos-*MatrixGetYVector(carEntity->dir)*(car->wheels[i].radius+phys->wheels[i].suspension);
		tVector3 wheelCarTorque=(wheelGroundPoint-car->massCenter*carEntity->dir)%wheelForce;
		*totalWheelTorque=*totalWheelTorque+wheelCarTorque;	
		*totalWheelForce=*totalWheelForce+wheelForce;
	}
	if(phys->rpm<car->jerkRPM)
		phys->rpm=car->jerkRPM;
}

#define kMaxDraftDistance 60.0
#define kMaxDraftAngle (PI/15.0)
#define kMaxDraftVeloAngle (PI/10.0)

float CalcDraftFactor(tGameEntity *car1)
{
	float draftFactor=0;
	for(int i=0;i<gGameInfo->numPlayers;i++)
		if(gCarEntities[i]!=car1)
		{
			tGameEntity *car2=gCarEntities[i];
			tVector3 v=car2->pos-car1->pos;
			float dir,veloDir,dist;
			
			dir=1-acos((!car1->velo)*!v)/kMaxDraftAngle;
			if(isnan(dir))dir=1;
			if(dir<0)dir=0;

			veloDir=1-acos((!car1->velo)*(!car2->velo))/kMaxDraftVeloAngle;
			if(isnan(veloDir))veloDir=1;
			if(veloDir<0)veloDir=0;

			dist=1-(~v)/kMaxDraftDistance;
			if(dist<0)dist=0;

			draftFactor+=dir*veloDir*dist;
		}
	if(draftFactor>1)
		draftFactor=1;
	return draftFactor;
}

//#define __NO_AERODYNAMICS
void CalcAerodynamics(tGameEntity *carEntity,tCarDefinition *car,tVector3 *aerodynamicForce,tVector3 *aerodynamicTorque)
{
#ifndef __NO_AERODYNAMICS
	tVector3 xDir=*MatrixGetXVector(carEntity->dir);
	tVector3 yDir=*MatrixGetYVector(carEntity->dir);
	tVector3 zDir=*MatrixGetZVector(carEntity->dir);
	float fVelo=~carEntity->velo;
	float draftFactor=CalcDraftFactor(carEntity);
	
	float airRes=fVelo*0.5*fabs(zDir*carEntity->velo)*gEnvironment->airDensity;
	tVector3 frontSpoilPoint=Vector(0,car->massCenter.y,car->wheels[0].pos.z)*carEntity->dir;
	tVector3 rearSpoilPoint=Vector(0,car->massCenter.y,car->wheels[2].pos.z)*carEntity->dir;
	*aerodynamicForce=yDir*airRes*(car->frontLift+car->rearLift);
	*aerodynamicTorque=(frontSpoilPoint-car->massCenter*carEntity->dir)%(yDir*airRes*(car->frontLift))+(rearSpoilPoint-car->massCenter*carEntity->dir)%(yDir*airRes*(car->rearLift));	
	
	tVector3 dragCenter=Vector(0,car->massCenter.y,0);
	float dragFactor=fabs(zDir*carEntity->velo*car->frontAirResistance)+fabs(yDir*carEntity->velo*car->topAirResistance)+fabs(xDir*carEntity->velo*car->sideAirResistance); 
	*aerodynamicForce=*aerodynamicForce-carEntity->velo*0.5*dragFactor*gEnvironment->airDensity*(1-0.9*draftFactor);
	*aerodynamicTorque=*aerodynamicTorque+((dragCenter-car->massCenter)*carEntity->dir)%*aerodynamicForce;
#else
	*aerodynamicForce=Vector(0,0,0);
	*aerodynamicTorque=Vector(0,0,0);
#endif
}

void CarLights(tCarPhysics *phys)
{
	phys->lightFlags&=~(kLightFlagRevLight+kLightFlagBrakeLight);
	if((int)(gFrameCount*kFrameTime/kIndicatorTime)&1)
		phys->lightFlags&=~(kLightFlagLIndLight+kLightFlagRIndLight);
	if(phys->brake>0)
		phys->lightFlags|=kLightFlagBrakeLight;
	if(phys->gear==-1)
		phys->lightFlags|=kLightFlagRevLight;
	if(gEnvironment->spotLightEnable&&gFrameCount==0)
		phys->lightFlags|=kLightFlagDriveLight;
}

//Create Tracks and Particle Effects for the car
void CarCreateEffects(tGameEntity *carEntity,tCarDefinition *car,tCarPhysics *phys,tWheelCalcData *wheels)
{
	float totalSlip=0;
	
	for(int i=0;i<car->numWheels;i++)
		{
			tSurfaceType *surf=gSurfaceTypes->types+phys->wheels[i].surfaceType;
			
			totalSlip+=fabs(phys->wheels[i].slipVelo)/car->numWheels;
			
			//does this surface smoke?
			if(wheels[i].onGround)
				if(surf->smokeEnable&&!((gCameraMode==kCameraCockpit||gCameraMode==kCameraCockpitCarHidden)&&carEntity==gViewedEntity))
				{
					//is the wheel slipping enough to smoke?
					float smoke=(fabs(phys->wheels[i].slipVelo)-surf->minSmokeSlideVelo)/(surf->maxSmokeSlideVelo-surf->minSmokeSlideVelo);
					if(smoke>0)
					{
						//create particles
						tParticlesDef def;
						ParticlesInitDef(&def);
						
						if(smoke>1)smoke=1;
						smoke*=kMaxSmoke;
						def.sprite=surf->smokeTexture;
						def.maxSpread=surf->smokeSpread;
						def.maxVelo=surf->smokeMaxVelo;
						def.gravity=surf->smokeGravity;
						def.maxLife=surf->smokeMaxLife;
						def.brightness=0.5+(gEnvironment->ambient.x+gEnvironment->ambient.y+gEnvironment->ambient.z)*0.166;
						def.screenTexture=surf->smokeStickEnable?surf->smokeTexture:-1;
						def.xSize=surf->smokeSize;def.ySize=surf->smokeSize;
						tVector3 baseVelo=Vector(0,0.03,-0.1*phys->wheels[i].slip)*carEntity->dir*surf->smokeSpeed+carEntity->velo*0.3;
						ParticlesCreate(smoke*kFrameTime,wheels[i].pos+carEntity->pos,baseVelo,&def);
					}
				}

			//does this surface leave tracks?
			if(surf->trackEnable&&!((gCameraMode==kCameraCockpit||gCameraMode==kCameraCockpitCarHidden)&&carEntity==gViewedEntity))
				//we only generate tracks every kTrackPrecision-th frame for performance reasons
				if(!(gFrameCount%kTrackPrecision))
				{
					//Calculate track intensity
					float tracks=(fabs(phys->wheels[i].slipVelo)-surf->minTrackSlideVelo)/(surf->maxTrackSlideVelo-surf->minTrackSlideVelo);
					if(tracks>0&&wheels[i].onGround)
					{
						//create tracks.
						if(tracks>1)tracks=1;
						tVector3 pos1=wheels[i].pos+carEntity->pos-wheels[i].groundNormal*(phys->wheels[i].suspension+car->wheels[i].radius);
						pos1.y-=wheels[i].bump;
						tVector3 pos2=pos1+wheels[i].velo*kFrameTime;
						phys->wheels[i].lastTrack=TracksAdd(pos1,pos2,wheels[i].groundNormal,car->wheels[i].width,gSurfaceTypes->types[phys->wheels[i].surfaceType].trackTexture,tracks,phys->wheels[i].lastTrack);
					}
					else phys->wheels[i].lastTrack=0;
				}
		}
	
	//make the car dirtier
	phys->dirt+=totalSlip*kFrameTime*0.1;	
	if(phys->dirt>100)
		phys->dirt=100;
	//exhaust fire
	if(car->exhaustFire+(phys->damage>kEngineDamage?100000:0))
	{
		float exhaustFire=(((phys->oldThrottle-phys->throttle)*kFPS*phys->rpm)-(car->exhaustFire+(phys->damage>kEngineDamage?100000:0)))/((car->exhaustFire+(phys->damage>kEngineDamage?100000:0)));
		if(exhaustFire>0)
		{
			if(exhaustFire>1)exhaustFire=1;
			CarPlayCrashNoise(carEntity,FileGetReference("exhaust.wav"),exhaustFire);
			if(exhaustFire>0.5)
			{
				tParticlesDef def;
				ParticlesInitDef(&def);

				def.sprite=FileGetReference("exhaustfire.pct");
				def.maxSpread=0.1;
				def.maxVelo=0.2;
				def.gravity=-0.2;
				def.maxLife=0.3;
				def.screenTexture=-1;
				def.xSize=0.6;def.ySize=0.6;
				def.grow=true;
				tVector3 baseVelo=Vector(0,0,0);
				//two exhaust pipes. Both may be at the same space to make it look like one.
				ParticlesCreate(exhaustFire*200*kFrameTime,car->exhaust1Pos*carEntity->dir+carEntity->pos,baseVelo,&def);
				ParticlesCreate(exhaustFire*200*kFrameTime,car->exhaust2Pos*carEntity->dir+carEntity->pos,baseVelo,&def);
			}
		}
	}	
	phys->oldThrottle=phys->throttle;
	
	//exhaust smoke.
	float v=~carEntity->velo;
	if(v<10&&phys->damage<kFatalDamage)
	{
		tParticlesDef def;
		ParticlesInitDef(&def);
		
		float smoke=0.2+0.8*phys->throttle;
		def.sprite=FileGetReference("exhaustsmoke.pct");
		def.maxSpread=0.1;
		def.maxVelo=0.2;
		def.gravity=-0.8;
		def.maxLife=2;
		def.a=1-(v*0.1);
		def.screenTexture=-1;
		def.xSize=0.4;def.ySize=0.4;
		def.grow=true;
		tVector3 baseVelo=-*MatrixGetZVector(carEntity->dir)*smoke*2;
		//two exhaust pipes. Both may be at the same space to make it look like one.
		ParticlesCreate(smoke*40*kFrameTime,car->exhaust1Pos*carEntity->dir+carEntity->pos,baseVelo,&def);
		ParticlesCreate(smoke*40*kFrameTime,car->exhaust2Pos*carEntity->dir+carEntity->pos,baseVelo,&def);
	}
	
	if(phys->damage>kEngineDamage)
	{
		tParticlesDef def;
		ParticlesInitDef(&def);
		
		float smoke=0.2+0.8*phys->throttle;
		def.sprite=FileGetReference("exhaustsmoke.pct");
		def.maxSpread=0.2;
		def.maxVelo=0.3;
		def.gravity=-0.7;
		def.maxLife=2;
		def.a=1;
		def.screenTexture=-1;
		def.xSize=1.5;def.ySize=1.5;
		def.grow=true;
		tVector3 baseVelo=Vector(0,1,0);
		//two exhaust pipes. Both may be at the same space to make it look like one.
		ParticlesCreate(smoke*60*kFrameTime,Vector(0,1,2.3)*carEntity->dir+carEntity->pos,baseVelo,&def);
	}
}

void CarPhysicsEntitySimulation(tGameEntity *carEntity)
{
	tCarPhysics *phys=(tCarPhysics*)carEntity->physics;
	tCarDefinition *car=&(phys->car);
	tWheelCalcData wheels[kMaxWheels];
		
	//Wheels position and velocity
	CalcWheelPositions(carEntity,car,phys,wheels);
	
	//wheels position relative to the ground
	CalcWheelGroundOffsets(carEntity,car,phys,wheels);

	//wheels springs & suspension
	CalcWheelSupension(carEntity,car,phys,wheels);
		
	if(carEntity->physicsMachine==kPhysicsRemote||gReplay)
	{
		for(int i=0;i<kMaxWheels;i++)
			wheels[i].roadForce=Vector(0,0,0);
		//Sum of wheel forces
		tVector3 totalWheelForce,totalWheelTorque;
		CalcTotalWheelForce(carEntity,car,phys,wheels,&totalWheelForce,&totalWheelTorque);

		//Gravity
		tVector3 gravityForce=Vector(0,-car->mass*gEnvironment->gravity,0);

		//Apply Forces
		tVector3 totalForce=totalWheelForce+gravityForce;
		tVector3 acceleration=totalForce*(1/car->mass);
		carEntity->velo=carEntity->velo+acceleration*kFrameTime;
	}
	else
	{	
		//Force the wheel projects onto the road
		CalcWheelRoadForce(carEntity,car,phys,wheels);
			
		//Sum of wheel forces
		tVector3 totalWheelForce,totalWheelTorque;
		CalcTotalWheelForce(carEntity,car,phys,wheels,&totalWheelForce,&totalWheelTorque);

		//Aerodynamics
		tVector3 aerodynamicForce,aerodynamicTorque;
		CalcAerodynamics(carEntity,car,&aerodynamicForce,&aerodynamicTorque);
		
		//Gravity
		tVector3 gravityForce=Vector(0,-car->mass*gEnvironment->gravity,0);
			
		//Apply Forces
		tVector3 totalForce=totalWheelForce+aerodynamicForce+gravityForce;
		tVector3 acceleration=totalForce*(1/car->mass);
		carEntity->accel=acceleration;
		carEntity->velo=carEntity->velo+acceleration*kFrameTime;

		//Apply Torques
		tVector3 totalTorque=totalWheelTorque+aerodynamicTorque;

		tMatrix3 invMatrix;
		MatrixTranspose(carEntity->dir,invMatrix);
		totalTorque=totalTorque*invMatrix;
		float zTorq=1;
		if(gFrameCount<kFPS*5)
			zTorq=gFrameCount/(kFPS*5);
		tVector3 angularAcceleration=Vector(totalTorque.x/car->inertia.x,totalTorque.y/car->inertia.y,zTorq*totalTorque.z/car->inertia.z)*carEntity->dir;
		tMatrix3 accelerationMatrix;
		RotationVectorToMatrix(angularAcceleration*kFrameTime*kFrameTime,accelerationMatrix);
		MatrixMult(carEntity->rVelo,accelerationMatrix,carEntity->rVelo);	
		
		//if(carEntity->physicsMachine==kPhysicsLocal&&!gReplay)
		//{
		//Lights
		CarLights(phys);
		
		int wheelOnGround=0;
		for(int i=0;i<car->numWheels;i++)
		{
			if(wheels[i].onGround)
				wheelOnGround++;
			phys->wheels[i].onGround=wheels[i].onGround;
		}
//		if(wheelOnGround>2)//no y network Accel when on ground to dampen spring motion on bumpy surfaces
//			carEntity->accel.y=0;
		
		//Crash Recovery
		if(((wheelOnGround<car->numWheels&&wheelOnGround<4)||phys->collision>gFrameCount-10)&&phys->damage<kFatalDamage)
		{
			if(gFrameCount-phys->crashTime>kFPS*5)
				if(sqr(carEntity->velo)<sqr(2)||(phys->collision>gFrameCount-10&&sqr(carEntity->velo)<sqr(15)))
				{
					RoadCenterCar(carEntity);	
					phys->crashTime=gFrameCount;
					phys->stuckTime=gFrameCount;
				}
		}
		else
			phys->crashTime=gFrameCount;
		
		carEntity->lastActivity=gFrameCount;

		int wheelsOnGround=0;
		for(int i=0;i<car->numWheels;i++)
			if(wheels[i].onGround)
				wheelsOnGround++;
		phys->onGround=wheelsOnGround>=2;

	}
	
	//Smoke and Tracks!
	CarCreateEffects(carEntity,car,phys,wheels);
}

#define kMaxCornerAccel 15
#define kMaxEngineAccel 6
#define kMaxBrakeAccel 8
#define kEngineFactor 0.8

void CarPhysicsEntitySimple(tGameEntity *carEntity)
{
	tCarPhysics *phys=(tCarPhysics*)carEntity->physics;

	tVector3 velo=carEntity->velo;

	tVector3 rVelo=MatrixToEulerAngles(carEntity->rVelo);
	rVelo.y=0;
	EulerAnglesToMatrix(rVelo,carEntity->rVelo);
	
	CarPhysicsEntitySimulation(carEntity);

	carEntity->velo.x=(5*velo.x+carEntity->velo.x)/6.0;
	carEntity->velo.z=(5*velo.z+carEntity->velo.z)/6.0;

	tCarDefinition *car=&(phys->car);
	
	float groundFriction=0;
	for(int i=0;i<car->numWheels;i++)
		groundFriction+=gSurfaceTypes->types[phys->wheels[i].surfaceType].grip/(float)car->numWheels;
	
	//Calculate the drivetrain ratio
	float ratio=car->gearRatios[phys->gear+1]*car->finalDriveRatio;
	
	//Calculate the engine inertia at the final drive
	float inertia=car->engineInertia*sqr(ratio);
	
	//Calculate the frictional braking torque of the engine (in N*m)
	float engineFrictionTorque=25+phys->rpm*0.02;
	if(car->engineBaseFriction||car->engineRPMFriction)
		engineFrictionTorque=car->engineBaseFriction+phys->rpm*car->engineRPMFriction;
	
	//Calculate the current engine torque (in N*m) 
	float engineTorque=(CalcTorque(phys->rpm,car)+engineFrictionTorque)*phys->throttle-engineFrictionTorque;
	
	if(engineTorque<0&&carEntity->velo**MatrixGetZVector(carEntity->dir)*ratio<0)
		engineTorque*=-1;

	tVector3 flatDir=!Vector(MatrixGetZVector(carEntity->dir)->x,0,MatrixGetZVector(carEntity->dir)->z);
	tVector3 flatVelo=Vector(velo.x,0,velo.z);

	float fEngineForce=(car->aiSpeedIndex!=0?car->aiSpeedIndex:1)*kEngineFactor*engineTorque*ratio/car->wheels[2].radius;
	if(fEngineForce>car->mass*kMaxEngineAccel*groundFriction)
		fEngineForce=car->mass*kMaxEngineAccel*groundFriction;
	tVector3 engineForce=flatDir*fEngineForce;
	
	float arcadeDraftBoost=CalcDraftFactor(carEntity)*(~carEntity->velo)/70.0;
	
	if(phys->arcadeDraftBoost<arcadeDraftBoost)
		phys->arcadeDraftBoost+=2*kFrameTime;
	if(phys->arcadeDraftBoost>arcadeDraftBoost)
		phys->arcadeDraftBoost-=2*kFrameTime;
	
	if(phys->arcadeDraftBoost>1)phys->arcadeDraftBoost=1;
	if(phys->arcadeDraftBoost<0)phys->arcadeDraftBoost=0;
	
	if(carEntity->controlType==kControlTypeAIInput)
		phys->arcadeDraftBoost=0;
		
	//engineForce=engineForce+!engineForce*14*car->mass*phys->arcadeDraftBoost;

	tVector3 airRes=-carEntity->velo*~carEntity->velo*0.5*car->frontAirResistance*gEnvironment->airDensity;
	
	float surfaceBrake=0;
	for(int i=0;i<car->numWheels;i++)
		surfaceBrake+=gSurfaceTypes->types[phys->wheels[i].surfaceType].brakeFactor*fabs(phys->wheels[i].angularVelo)/(float)car->numWheels;
	float brake=phys->arcadeBrake+phys->handbrake*0.5;
	if(brake>1)brake=1;
	brake+=surfaceBrake;
	tVector3 brakeForce=-!flatVelo*brake*car->mass*kMaxBrakeAccel;
	if(sqr(brakeForce/car->mass)*kFrameTime>sqr(flatVelo))
		brakeForce=-!flatVelo*car->mass*kFrameTime;
		
	
	tVector3 totalForce;
	if(phys->onGround)
		totalForce=engineForce+airRes+brakeForce;
	else
		totalForce=airRes;
	tVector3 totalAccel=totalForce/car->mass;
	if(gGameInfo->arcade==kGameModeTurbo)
		totalAccel=totalAccel*2.3;
	
	carEntity->velo=carEntity->velo+(totalAccel*kFrameTime);

	tMatrix3 m;
	MatrixIdentity(m);
	MatrixRotY(m,phys->steering*0.1);
	
	flatDir=flatDir*~flatVelo;
	flatDir=flatDir*m;
	float rev=sign(flatDir*flatVelo);
	flatDir=flatDir*rev;
	float diff=~(flatVelo-flatDir);
	float cornerAccel=kFrameTime*kMaxCornerAccel*groundFriction;
	if(gGameInfo->arcade==kGameModeTurbo)
		cornerAccel*=1.9;
	if(cornerAccel>diff)cornerAccel=diff;
	
	tVector3 vCornerAccel=-!(flatVelo-flatDir)*cornerAccel;
	carEntity->velo=carEntity->velo+vCornerAccel;
	carEntity->accel=totalAccel+vCornerAccel*kFPS;

	float handBrakeFactor=1+phys->handbrake*2;
	if(rev<0)
		handBrakeFactor=1;
	float veloSteer=((15/(~velo+1))+1)*handBrakeFactor;
	float maxRotSteer=10*kFrameTime*handBrakeFactor;
	float acardeSteerVeloInput=rev*phys->steering*~velo*kFrameTime*0.1*handBrakeFactor;
	if(!phys->onGround)
		acardeSteerVeloInput=0;
	if(acardeSteerVeloInput>phys->arcadeSteerVelo){
		phys->arcadeSteerVelo+=0.015*veloSteer*kFrameTime;
		if(acardeSteerVeloInput<phys->arcadeSteerVelo)
			phys->arcadeSteerVelo=acardeSteerVeloInput;
	}
	else{
		phys->arcadeSteerVelo-=0.015*veloSteer*kFrameTime;
		if(acardeSteerVeloInput>phys->arcadeSteerVelo)
			phys->arcadeSteerVelo=acardeSteerVeloInput;
	}
	if(fabs(phys->arcadeSteerVelo)>maxRotSteer)
		phys->arcadeSteerVelo=sign(phys->arcadeSteerVelo)*maxRotSteer;
	MatrixRotY(carEntity->rVelo,phys->arcadeSteerVelo);
}


void CarPhysicsEntityMagic(tGameEntity *carEntity)
{
	tCarPhysics *phys=(tCarPhysics*)carEntity->physics;

	MatrixIdentity(carEntity->rVelo);

	tVector3 x[4],a[4];
	x[0]=Vector(1.2,0,2);
	x[1]=Vector(-1.2,0,2);
	x[2]=Vector(1.2,0,-2);
	x[3]=Vector(-1.2,0,-2);
	tVector3 howerAccel=Vector(0,0,0);
	tVector3 howerRotAccel=Vector(0,0,0);
	for(int i=0;i<4;i++)
	{
		x[i]=x[i]*carEntity->dir;
		tVector3 normal;
		float offs=GetGroundOffset(x[i]+carEntity->pos,&carEntity->lastRoadIndex,&normal,NULL);
		if(offs<0.2)offs=0.2;
		a[i]=normal*(1.25/offs)*(1+0.5*phys->steering*(i&1?1:-1))*(1+0.1*phys->brake*(i&2?1:-1))*(1-0.1*phys->throttle*(i&2?1:-1));
		howerAccel=howerAccel+a[i];
		howerRotAccel=howerRotAccel+x[i]%a[i];
	}
	howerAccel=howerAccel*(1+sin(gFrameCount*kFrameTime*2)*0.12);

	tMatrix3 invMatrix;
	MatrixTranspose(carEntity->dir,invMatrix);
	howerRotAccel=Vector(howerRotAccel.x*5,0,howerRotAccel.z*30);
	tMatrix3 accelerationMatrix;
	RotationVectorToMatrix(howerRotAccel*kFrameTime*kFrameTime,accelerationMatrix);
	MatrixMult(carEntity->rVelo,accelerationMatrix,carEntity->rVelo);	
	
	tVector3 velo=carEntity->velo;
	tVector3 gravityAccel=Vector(0,-gEnvironment->gravity,0);
	tVector3 engineAccel=phys->throttle**MatrixGetZVector(carEntity->dir)*(20);
	tVector3 brakeAccel=phys->brake*!(*MatrixGetZVector(carEntity->dir)+((sqr(carEntity->velo)>0.001)?carEntity->velo*0.1:Vector(0,0,0)))*-(22);
	if(gFrameCount*kFrameTime<=kStartGameDelaySeconds)
	{
		engineAccel=Vector(0,0,0);
		brakeAccel=Vector(0,0,0);
	}
	if(phys->lapCount>gGameInfo->numLaps&&gGameInfo->numLaps!=-1)
		brakeAccel=phys->brake*(((sqr(carEntity->velo)>0.001)?!carEntity->velo:Vector(0,0,0)))*-(22);
	brakeAccel.y=0;
	tVector3 dampAccel=Vector(0,0,0);
	tVector3 airResAccel=Vector(0,0,0);
	if(sqr(carEntity->velo)>0.001)
	{
		dampAccel=-!carEntity->velo*5;
		airResAccel=-!carEntity->velo*sqr(carEntity->velo)*0.003;
		if(sqr(dampAccel*kFrameTime)>sqr(carEntity->velo))
			dampAccel=-carEntity->velo*kFPS;
		dampAccel.x*=0.1;
		dampAccel.z*=0.1;		
	}
	tVector3 totalAccel=gravityAccel+howerAccel+engineAccel+brakeAccel+dampAccel+airResAccel;

	carEntity->velo=carEntity->velo+totalAccel*kFrameTime;
	carEntity->lastActivity=gFrameCount;

	tVector3 flatDir=!Vector(MatrixGetZVector(carEntity->dir)->x,0,MatrixGetZVector(carEntity->dir)->z);
	tVector3 flatVelo=Vector(velo.x,0,velo.z);
	
	tMatrix3 m;
	MatrixIdentity(m);
	MatrixRotY(m,phys->steering*0.1);

	flatDir=flatDir*~flatVelo;
	flatDir=flatDir*m;
	float rev=sign(flatDir*flatVelo);
	flatDir=flatDir*rev;
	float diff=~(flatVelo-flatDir);

	float cornerAccel=kFrameTime*kMaxCornerAccel;
	if(gGameInfo->arcade==kGameModeTurbo)
		cornerAccel*=1.9;
	if(cornerAccel>diff)cornerAccel=diff;
	
	tVector3 vCornerAccel=-!(flatVelo-flatDir)*cornerAccel;
	carEntity->velo=carEntity->velo+vCornerAccel;
	carEntity->accel=totalAccel+vCornerAccel*kFPS;
	
	float acardeSteerVeloInput=phys->steering*kFrameTime*(3+~velo*0.04);
	if(acardeSteerVeloInput>phys->arcadeSteerVelo){
		phys->arcadeSteerVelo+=0.15*kFrameTime;
		if(acardeSteerVeloInput<phys->arcadeSteerVelo)
			phys->arcadeSteerVelo=acardeSteerVeloInput;
	}
	else{
		phys->arcadeSteerVelo-=0.15*kFrameTime;
		if(acardeSteerVeloInput>phys->arcadeSteerVelo)
			phys->arcadeSteerVelo=acardeSteerVeloInput;
	}	
	if(gFrameCount*kFrameTime>=kStartGameDelaySeconds)
		MatrixRotY(carEntity->rVelo,phys->arcadeSteerVelo);
	
	if(gEnvironment->spotLightEnable&&gFrameCount==0)
		phys->lightFlags|=kLightFlagDriveLight;

	if(phys->collision>gFrameCount-10)
	{
		if(gFrameCount-phys->crashTime>kFPS*5)
			if(sqr(carEntity->velo)<sqr(2))
			{
				RoadCenterCar(carEntity);	
				phys->crashTime=gFrameCount;
				phys->stuckTime=gFrameCount;
			}
	}else
		phys->crashTime=gFrameCount;
	
	*MatrixGetYVector(carEntity->dir)=!(*MatrixGetYVector(carEntity->dir)+(Vector(0,1,0)-*MatrixGetYVector(carEntity->dir))*kFrameTime*0.5);
	*MatrixGetXVector(carEntity->dir)=!(*MatrixGetYVector(carEntity->dir)%*MatrixGetZVector(carEntity->dir));
	*MatrixGetZVector(carEntity->dir)=!(*MatrixGetXVector(carEntity->dir)%*MatrixGetYVector(carEntity->dir));
}



void CarPhysicsEntity(tGameEntity *carEntity)
{		
	tCarPhysics *phys=(tCarPhysics*)carEntity->physics;
	tCarDefinition *car=&(phys->car);
	if(car->magic)	
		CarPhysicsEntityMagic(carEntity);
	else if((gGameInfo->arcade==kGameModeTurbo||gGameInfo->arcade==kGameModeArcade||carEntity->controlType==kControlTypeAIInput)&&carEntity->physicsMachine==kPhysicsLocal)
		CarPhysicsEntitySimple(carEntity);
	else
		CarPhysicsEntitySimulation(carEntity);
}
#define kPredictTime 0.15
//Move the car around

void MatrixReAdjust2(tMatrix3 m)
{
	*(tVector3*)m[1]=!*(tVector3*)m[1];
	*(tVector3*)m[0]=!(*(tVector3*)m[1]%*(tVector3*)m[2]);
	*(tVector3*)m[2]=!(*(tVector3*)m[0]%*(tVector3*)m[1]);
}

void FullyResetCar(tGameEntity *carEntity)
{
	tCarPhysics *phys=(tCarPhysics*)carEntity->physics;
	carEntity->pos=gMapInfo->startPos+Vector(0,10,0);
	carEntity->oldPos=carEntity->pos;
	carEntity->netPos=carEntity->pos;
	carEntity->velo=Vector(0,0,0);
	carEntity->netVelo=Vector(0,0,0);
	carEntity->collVelo=Vector(0,0,0);
	carEntity->accel=Vector(0,0,0);
	MatrixIdentity(carEntity->dir);
	MatrixIdentity(carEntity->rVelo);
	MatrixIdentity(carEntity->oldDir);
	MatrixIdentity(carEntity->netDir);
	MatrixIdentity(carEntity->netRVelo);
	for(int i=0;i<kNumAvgRVelos;i++)
	{
		MatrixIdentity(carEntity->lastRVelos[i]);
		carEntity->lastVelos[i]=Vector(0,0,0);
		carEntity->lastAccel[i]=Vector(0,0,0);
	}
	carEntity->remoteCameraPos=Vector(0,0,0);
	carEntity->zDist=0;
	phys->maxSpeed=0;
	phys->averageSpeed=0;
	phys->accel100=0;
	phys->accel200=0;
	phys->accelQuarter=0;
	phys->accelKM=0;
	phys->odo;
	phys->position=0;
	phys->lead=0;
	phys->rpm=0;
	phys->engineAngularVelo=0;
	phys->drivetrainAngularVelo=0;
	phys->clutchTorque=0;
	for(int i=0;i<kMaxWheels;i++)
	{
		phys->wheels[i].angle=0;
		phys->wheels[i].suspension=0;
		phys->wheels[i].slipVelo=0;
		phys->wheels[i].slip=0;
		phys->wheels[i].slipAngle=0;
		phys->wheels[i].rotation=0;
		phys->wheels[i].angularVelo=0;
		phys->wheels[i].glow=0;
	}
}


void CarMotionEntity(tGameEntity *carEntity)
{
	tCarPhysics *phys=(tCarPhysics*)carEntity->physics;
	tCarDefinition *car=&(phys->car);

	for(int i=kNumAvgRVelos-2;i>=0;i--)
	{
		MatrixCopy(carEntity->lastRVelos[i],carEntity->lastRVelos[i+1]);
		carEntity->lastVelos[i+1]=carEntity->lastVelos[i];
		carEntity->lastAccel[i+1]=carEntity->lastAccel[i];
	}
	MatrixCopy(carEntity->rVelo,carEntity->lastRVelos[0]);
	carEntity->lastVelos[0]=carEntity->velo;
	carEntity->lastAccel[0]=carEntity->accel;


	carEntity->lastActivity=gFrameCount;
	//move the car
	
	if(carEntity->physicsMachine==kPhysicsRemote)
	{
		if(!gReplay)
		{
			float smoothness=0;
			for(int i=0;i<kNumLastCollisions;i++)
				if((gFrameCount-phys->lastCollisions[i].frameCount)*kFrameTime<0.5)
					smoothness+=(8.0f/kNumLastCollisions);
			if(smoothness>1)
				smoothness=1;
			float predictTime=kPredictTime*(1+smoothness*1.5);
			carEntity->netVelo=carEntity->netVelo+carEntity->accel*kFrameTime;
			carEntity->netPos=carEntity->netPos+carEntity->netVelo*kFrameTime;
			carEntity->velo=carEntity->velo+((carEntity->netPos+carEntity->netVelo*predictTime+carEntity->accel*predictTime*predictTime*0.5)-(carEntity->pos+carEntity->velo*predictTime))/predictTime;
			MatrixMult(carEntity->netDir,carEntity->rVelo,carEntity->netDir);
			tVector3 netx=*MatrixGetXVector(carEntity->netDir);
			tVector3 nety=*MatrixGetYVector(carEntity->netDir);
			tVector3 netz=*MatrixGetZVector(carEntity->netDir);
			tVector3 curx=*MatrixGetXVector(carEntity->dir);
			tVector3 curz=*MatrixGetZVector(carEntity->dir);

			*MatrixGetXVector(carEntity->dir)=curx+(netx-curx)*0.3;
			*MatrixGetYVector(carEntity->dir)=nety;
			*MatrixGetZVector(carEntity->dir)=curz+(netz-curz)*0.3;
			MatrixReAdjust2(carEntity->dir);
			
			if(sqr(carEntity->pos-carEntity->netPos)>sqr(5+smoothness*10))
			{
				carEntity->pos=carEntity->netPos;
				carEntity->velo=carEntity->netVelo;
				MatrixCopy(carEntity->netDir,carEntity->dir);
			}
		}
		for(int i=0;i<car->numWheels;i++)
			phys->wheels[i].rotation+=phys->wheels[i].angularVelo*kFrameTime;
	}
	if(gReplay)
	{
		carEntity->velo=carEntity->velo+carEntity->accel*kFrameTime;
		MatrixMult(carEntity->netDir,carEntity->rVelo,carEntity->netDir);
		tVector3 netx=*MatrixGetXVector(carEntity->netDir);
		tVector3 nety=*MatrixGetYVector(carEntity->netDir);
		tVector3 netz=*MatrixGetZVector(carEntity->netDir);
		tVector3 curx=*MatrixGetXVector(carEntity->dir);
		tVector3 curz=*MatrixGetZVector(carEntity->dir);

		*MatrixGetXVector(carEntity->dir)=curx+(netx-curx)*0.3;
		*MatrixGetYVector(carEntity->dir)=nety;
		*MatrixGetZVector(carEntity->dir)=curz+(netz-curz)*0.3;
		MatrixReAdjust2(carEntity->dir);
	}
	carEntity->pos=carEntity->pos+carEntity->velo*kFrameTime;

	
	//rotate the car around it's center of gravity
	tVector3 massCenter=car->massCenter*carEntity->dir;
	tVector3 massMotion=((massCenter*carEntity->rVelo)-massCenter);
	MatrixMult(carEntity->dir,carEntity->rVelo,carEntity->dir);
	carEntity->pos=carEntity->pos-massMotion;	
	
	//update road position index
	float oldPosition=phys->position;
	phys->position=RoadGetPosition(carEntity->pos,carEntity->lastRoadIndex,NULL);
	
	if(carEntity==gViewedEntity)
		for(int i=0;i<gNumCornerSigns;i++)
			if(gGameInfo->reverse){
				if(phys->position<gCornerSigns[i].pos&&oldPosition>gCornerSigns[i].pos)
				{
					gAccelSignDisplayIntensity=1.5;
					gAccelSignDisplayCorner=-gCornerSigns[i].accel;
				}
			}else{
				if(phys->position>gCornerSigns[i].pos&&oldPosition<gCornerSigns[i].pos)
				{
					gAccelSignDisplayIntensity=1.5;
					gAccelSignDisplayCorner=gCornerSigns[i].accel;
				}
			}
		

	//have we crossed the road position lap line?
	if(gGameInfo->reverse)
	{
		if(oldPosition>phys->position+50)
			phys->lap--;
		else if(oldPosition<phys->position-50)
			phys->lap++;
	}
	else			
	{
		if(oldPosition>phys->position+50)
			phys->lap++;
		else if(oldPosition<phys->position-50)
			phys->lap--;
	}

	//test if we are stuck (used for ai recovery)
	int stuck=gFrameCount*kFrameTime>5&&sqr(carEntity->velo)<sqr(8)&&phys->lapCount<=gGameInfo->numLaps;
	if(stuck)
		phys->stuckTime++;
	else
		phys->stuckTime=0;

	//are we moving backwards? (used to display "wrong direction" message).
	if((gGameInfo->reverse?oldPosition<=phys->position:oldPosition>=phys->position)&&sqr(carEntity->velo)>sqr(5))
		phys->wrongDriectionFrames++;
	else
		phys->wrongDriectionFrames-=10;
	if(phys->wrongDriectionFrames<0)
		phys->wrongDriectionFrames=0;
	else if(phys->wrongDriectionFrames>kFPS*5)
	{
		phys->wrongDriectionFrames=kFPS*5;
		if(gGameInfo->network&kGameInfoNetworkCantGoBackwards)
		{
			if(carEntity==gViewedEntity)
				TextPrintfToBufferFormatedFading(Vector(kFadingMessageXPos,kFadingMessageYPos),kFadingMessageSize,kTextAlignMiddle,0.9,1.8,"Wrong Way!");
			RoadCenterCar(carEntity);
		}
	}

	float velo=~carEntity->velo;
	//Check if we have beaten our speed record for this game
	if(velo>phys->maxSpeed)
		phys->maxSpeed=velo;

	//Update our average speed
	if(phys->lapCount&&(phys->lapCount<=gGameInfo->numLaps||gGameInfo->numLaps==-1)&&!gReplay)
	{
		phys->averageSpeed=((phys->averageSpeed*(phys->averageSpeedFrames))+velo)/(phys->averageSpeedFrames+1);
		phys->averageSpeedFrames++;
	}
	if(velo>=100/3.6&&phys->accel100==0)
		phys->accel100=gFrameCount*kFrameTime-kStartGameDelaySeconds;
	if(velo>=200/3.6&&phys->accel200==0)
		phys->accel200=gFrameCount*kFrameTime-kStartGameDelaySeconds;
	phys->odo+=velo*kFrameTime;
	if(phys->odo>=402.32454&&phys->accelQuarter==0)
		phys->accelQuarter=gFrameCount*kFrameTime-kStartGameDelaySeconds;
	if(phys->odo>=1000&&phys->accelKM==0)
		phys->accelKM=gFrameCount*kFrameTime-kStartGameDelaySeconds;
	/*if(Button()&&phys->accel200!=0&&!gReplay)
	{
		printf("0-100km/h: %.1f seconds\n0-200km/h: %.1f seconds\n100-200km/h: %.1f seconds\nQuarter Mile: %.1f seconds\n1km: %.1f seconds\nTop Speed:%.1f km/h\n",phys->accel100,phys->accel200,phys->accel200-phys->accel100,phys->accelQuarter,phys->accelKM,phys->maxSpeed*3.6);
		phys->accel200=0;
	}*/
	if(isnan(carEntity->pos.x)||isnan(carEntity->pos.y)||isnan(carEntity->pos.z))
		FullyResetCar(carEntity);
}