




//
// Vehicle setup. Mostly for physics
//
function	SetupVehiclePhysics(vehicle ref as tVehicle, reset as integer, lookat as tPoint)
	a as tPoint
	i as integer
	s as tPoint
	n as tPoint

	vehicle.velocity.current = 0
	vehicle.delay.amount = 0.500
	vehicle.steering = STEERING_LOCAL
	vehicle.tilt.range = 5
	vehicle.tilt.spring = 0.3	
	vehicle.tilt.damp = 0.7		
	
	vehicle.velocity.linearUnit = 0.5 * SCALE_3D
	vehicle.velocity.angularUnit = 1.5 * SCALE_3D
	
	SetupVehicleLighting(vehicle)
	
	if reset = TRUE											// Full reset at start of game
		s = GetPathStart(vehicle)				
		Delete3DPhysicsBody(vehicle.hit)				
		SetObjectRotation(vehicle.hit, 0, 0, 0)		
		SetObjectPosition(vehicle.hit, s.x, s.y + vehicle.size.y * 0.7, s.z)
		SetObjectLookAt(vehicle.hit, lookat.x, s.y + vehicle.size.y * 0.7, lookat.z, 0)
	else
		s = GetFollowPos(vehicle.follow)
		n = GetFollowPos(vehicle.follow + 1)	
		Delete3DPhysicsBody(vehicle.hit)		
		SetObjectPosition(vehicle.hit, s.x, s.y + (vehicle.size.y * 0.7), s.z)
		SetObjectRotation(vehicle.hit, 0, 0, 0)		
		SetObjectLookAt(vehicle.hit, n.x, s.y, n.z, 0)		
	endif

	Create3DPhysicsDynamicBody(vehicle.hit)						// Set out hitbox to be a convex hull
	CreateVehicleShape(vehicle)
	SetObject3DPhysicsCanSleep(vehicle.hit, 0)	
	SetObject3DPhysicsDamping( vehicle.hit , 0.2, 0.9 ) 		// Initialize damping
	SetObject3DPhysicsMaxLinearVelocity( vehicle.hit, vehicle.velocity.max ) 
	SetObject3DPhysicsMass(vehicle.hit, GetObject3DPhysicsMass(vehicle.hit) * 2)	
	//SetObject3DPhysicsRestitution(vehicle.hit, .5)	
endfunction

function SetupVehicleLighting(vehicle ref as tVehicle)
	i as integer
	
	SetObjectCastShadow(vehicle.chassis, 1)
	
	for i=0 to vehicle.wheels.length
		SetObjectCastShadow(vehicle.wheels[i].id, 1)
	next
endfunction


function MoveVehicle(vehicle ref as tVehicle, env ref as tEnvironment, level ref as tLevel)
	select vehicle.state
		case VEHICLE_STATE_SPINOUT
			if vehicle.delay.count > timer()
				CheckNextNode(vehicle)
				SavePlayerStats(vehicle)
			else
				vehicle.state = VEHICLE_STATE_CONTROL
			endif
		endcase
		case VEHICLE_STATE_CONTROL
			DriveVehicle(vehicle, env, level)
		endcase
		case VEHICLE_STATE_FREE
			if vehicle.delay.count > timer()
				CheckNextNode(vehicle)
				SavePlayerStats(vehicle)
			else
				//SetObject3DPhysicsDamping( vehicle.hit , 0.2, 0.9 ) // Reset damping after crash
				vehicle.state = VEHICLE_STATE_CONTROL
			endif
		endcase
	endselect

endfunction

//
// This moves the vehicle
//
function DriveVehicle(vehicle ref as tVehicle, env ref as tEnvironment, level ref as tLevel)
	directionPoint	as tPoint
	i				as integer
	acceleration		as float
	d				as integer
	turn			as float
	linearPoint	 	as tPoint
	dot				as float
	vel				as float
	force			as float
	angularVelocity 	as float
	linearVelocity 	as float	
	drift			as tPoint
	drifting		as float

	if vehicle.monitor = TRUE
		//print("FPS="+str(ScreenFPS(),0)+" Tunnel="+str(vehicle.DotTunnel)+" Vel="+str(vehicle.velocity.current/SCALE_3D,2)+" Next="+str(vehicle.distNextFollow,0))
	endif
	
	if vehicle.flying > 200
		SetupVehiclePhysics(vehicle, FALSE, CreatePoint(0, 0, 0))
		vehicle.flying = 0
	endif
	
			
	linearPoint = GetVelocityPoint(vehicle)

/*
	if vehicle.name = "player"
		if CheckTunneling(vehicle, env, level) = TRUE
			SavePlayerStats(vehicle)
			exitfunction
		endif
	endif
		*/
			
	if CheckCollision(vehicle, env, level) = TRUE
		SavePlayerStats(vehicle)
		exitfunction
	endif

	if vehicle.velocity.boost > 0
		if timer() > vehicle.velocity.time
			vehicle.velocity.max = vehicle.velocity.boost
			SetObject3DPhysicsMaxLinearVelocity( vehicle.hit, vehicle.velocity.max ) 			
			vehicle.velocity.boost = 0
		endif
	endif

	
	if vehicle.npc = TRUE
		turn = GetTurnFromPath(vehicle)
	else
		turn = GetTurnFromKeyboard(vehicle)		
	endif

	vehicle.wheelAngle = vehicle.wheelAngle  * 0.8						// Dampen the wheel angle
	
	for i=0 to vehicle.wheels.length										// Check the wheels
		inc d, CheckWheels(vehicle, i, env, level)
	next
																		// If we have less than 3 wheels on the ground then we're in the air
																		// so turn everything off until we are in contact with the ground again	
																		// Get the linear velocity
	linearVelocity = DistancePoint(linearPoint)
																		// Get the angular velocity
	angularVelocity = DistancePoint(CreatePoint(GetObject3DPhysicsAngularVelocityX(vehicle.hit),  GetObject3DPhysicsAngularVelocityY(vehicle.hit), GetObject3DPhysicsAngularVelocityZ(vehicle.hit)))
	
	if d < 3 															
		if GetObject3DPhysicsLinearVelocityY(vehicle.hit) < -(25 * SCALE_3D)	// Falling velocity too high, throttle it
			SetObject3DPhysicsLinearVelocity(vehicle.hit, linearPoint.x, linearPoint.y, linearPoint.z, linearVelocity * .9)		
		endif

		inc vehicle.flying
		
		SavePlayerStats(vehicle)
				
		exitfunction
	endif
	
	if vehicle.flying > 0
		vehicle.landed = TRUE
	else			
		vehicle.landed = FALSE
	endif
	
	vehicle.flying = 0

	if vehicle.npc = TRUE
		acceleration = GetAccFromPath(vehicle)
	else
		acceleration = GetAccFromKeyboard(vehicle, env)											// Acceleration
	endif
	
	directionPoint = GetVehicleDirectionGround(vehicle, FALSE)								// Get the vector of the vehicle
	
	SetVectorPoint(vehicle.vectU, VectorNormalize(linearPoint))
	SetVectorPoint(vehicle.vectV, VectorNormalize(directionPoint))
	dot = GetVector3Dot(vehicle.vectU, vehicle.VectV)
	
	vehicle.velocity.current = linearVelocity
	
	if dot < 0
		vehicle.velocity.current = vehicle.velocity.current * -1
	endif
	
	if vehicle.velocity.current < 0 
		if abs( vehicle.velocity.current + acceleration ) < ( vehicle.velocity.max * 0.5)		// Reverse maximum is
			inc vehicle.velocity.current,  acceleration
		else
			vehicle.velocity.current = ( vehicle.velocity.max * -0.5)
		endif
	else
		if abs( vehicle.velocity.current + acceleration ) < vehicle.velocity.max
			inc vehicle.velocity.current,  acceleration
		else
			vehicle.velocity.current = vehicle.velocity.max * (vehicle.velocity.current / abs(vehicle.velocity.current))
		endif
	endif

	if acceleration <> 0 
		if dot < 0 or vehicle.drift.on = FALSE
			drift = directionPoint					// Going backwards, no drift
		else
			//drift = DividePoint(AddPoints(directionPoint, linearPoint), vehicle.drift.amount)			// Drift
			drift = Lerp(linearPoint, directionPoint, vehicle.drift.amount)			// Drift
		endif
		
		SetObject3DPhysicsLinearVelocity( vehicle.hit, drift.x, drift.y, drift.z, vehicle.velocity.current)				
	else
		if dot <> 0 
			vehicle.velocity.current = DistancePoint(linearPoint) * (dot / abs(dot))	
			//drift = DividePoint(AddPoints(directionPoint, linearPoint), vehicle.drift.amount)			// Drift			
			drift = Lerp(linearPoint, directionPoint, vehicle.drift.amount * 1.5)			// Drift			
			SetObject3DPhysicsLinearVelocity( vehicle.hit, drift.x, drift.y, drift.z, vehicle.velocity.current)		
		endif
	endif
	
	inc vehicle.wheelAngle, turn * 3
	SetObjectRotation(vehicle.wheels[FRONT_RIGHT].pivot, 0, vehicle.wheelAngle, 0)
	SetObjectRotation(vehicle.wheels[FRONT_LEFT].pivot, 0, vehicle.wheelAngle, 0)		

	// Apply the turning force (Dampening is set in SetupVehicle())
	
	if turn <> 0 and abs(vehicle.velocity.current) > (1 * SCALE_3d)
		if dot < 0
			turn = -turn
		endif

		if abs(GetObjectAngleZ(vehicle.detail)) < 5 
			SetObjectRotation(vehicle.detail, 0, 0, GetObjectAngleZ(vehicle.detail) + -turn * 0.2)
		endif
		
		if dot <> 0
			linearPoint.x = 0 : linearPoint.y = turn : linearPoint.z = 0
			SetObject3DPhysicsAngularVelocity( vehicle.hit, linearPoint.x, linearPoint.y, linearPoint.z, vehicle.velocity.angularUnit )
		endif			
	else
		SetObjectRotation(vehicle.detail, 0, 0, 0)
		SetObject3DPhysicsAngularVelocity( vehicle.hit, 0, 1, 0, 0)
	endif

	/*
	//
	// Simulate inertia on the chassis (Rock and roll)

	force = GetObject3DPhysicsAngularVelocityY(vehicle.hit)
	
	if force <> 0
		force = ( force / abs(force) ) 
	endif
	
	inc vehicle.tilt.force, force

	//vehicle.tilt.force = clamp(vehicle.tilt.force, -3, 3) 						// apply force to the tilt angle, scale it to give the right amount	

	tilt as float = 0	// body is upright
	tilt = GetObjectAngleZ(vehicle.chassis)

	if force = 0
		vehicle.tilt.force = vehicle.tilt.force - (tilt * vehicle.tilt.spring) // the more the car tilts, the harder springs push back towards tilt zero
	endif
	
	vehicle.tilt.force = vehicle.tilt.force * vehicle.tilt.damp  // damping the vehicle.tilt.force so it doesn't bounce forever, try 0.9 to start				

	tilt = tilt + vehicle.tilt.force 
	
	tilt = clamp(tilt, -vehicle.tilt.range, vehicle.tilt.range) // apply force to the tilt angle, scale it to give the right amount	
	
	SetObjectRotation(vehicle.chassis, GetObjectAngleX(vehicle.chassis), GetObjectAngleY(vehicle.chassis), tilt)
	*/
	SavePlayerStats(vehicle)
	
endfunction




function SavePlayerStats(vehicle ref as tVehicle)
														// Save some stats
    vehicle.velocity.last = CreatePoint(GetObject3DPhysicsLinearVelocityX(vehicle.hit),  GetObject3DPhysicsLinearVelocityY(vehicle.hit), GetObject3DPhysicsLinearVelocityZ(vehicle.hit))
    vehicle.lastPos = CreatePoint(GetObjectX(vehicle.hit), GetObjectY(vehicle.hit), GetObjectZ(vehicle.hit))	
endfunction

//
// Get input values
//
function GetTurnFromKeyboard(vehicle as tVehicle)
	turn as integer
	
	if GetRawJoystickExists(1)
		if GetRawJoystickButtonState(1, JOYSTICK_DPAD_RIGHT)
			turn = 1
		endif
				
		if GetRawJoystickButtonState(1, JOYSTICK_DPAD_LEFT)
			turn = -1
		endif	
	endif

	if GetRawKeyState(KEY_RIGHT) 
		turn = 1
	endif	
	
	if GetRawKeyState(KEY_LEFT) 
		turn = -1
	endif	

	if GetRawKeyState(KEY_CONTROL) or GetRawKeyState(KEY_SHIFT)
		exitfunction turn
	endif
	
	if GetRawKeyState(asc("D")) 
		turn = 1
	endif	
	
	if GetRawKeyState(asc("A")) 
		turn = -1
	endif	
	
endfunction turn


function GetTurnFromPath(vehicle ref as tVehicle)
	turn	as integer
	m 		as tMove
	a 		as tAction	
	
	a = vehicle.action
	
	m = a.move[a.current]
	
	select m.turn
		case FOLLOW_PATH
			turn = FollowPath(vehicle)
			a = vehicle.action
		endcase
		case default
			turn = m.turn
		endcase
	endselect
	
	if m.time <> FOREVER
		if (Timer() - a.start) > m.time
			if a.current < a.move.length
				inc a.current
			else
				a = GetAction(a.nextAction, vehicle)
			endif
			if a.start <> FOREVER
				a.start = Timer()	
			endif						
		endif
	endif
	
	vehicle.action = a
	
	if GetRawKeyState(asc(right(vehicle.name, 1)))
		if GetRawKeyState(KEY_LEFT)
			turn = -1
		endif
		if GetRawKeyState(KEY_RIGHT)
			turn = 1
		endif
	endif
		

endfunction turn

function CheckNextNode(vehicle ref as tVehicle)
	currentNode	as tPoint
	prevNode	as tPoint
	nextNode	as tPoint	
	vehiclePos	as tPoint
	forward		as tPoint
	n1			as tPoint
	n2			as tPoint
	shift		as float
	r			as integer
	node		as tNode
	distance	as float
	d1			as float
	d2			as float
	
	r = FALSE

	forward = GetObjectPosition(vehicle.dotID)		
	vehiclePos = GetObjectPosition(vehicle.hit)
	currentNode = GetPathCurrentPoint(vehicle)				
	
	node = GetPathNode(vehicle, vehicle.path.current)
	currentNode = node.slots[vehicle.slot]	
	node = GetPathNode(vehicle, vehicle.path.current - 1)
	prevNode = node.slots[vehicle.slot]
	node = GetPathNode(vehicle, vehicle.path.current + 1)
	nextNode = node.slots[vehicle.slot]
		
	SetVectorPoint(vehicle.vector1, VectorNormalize(SubtractPoints(vehiclePos, forward)))
	SetVectorPoint(vehicle.vector2, VectorNormalize(SubtractPoints(vehiclePos, currentNode)))	
	vehicle.dotMove = GetVector3Dot(vehicle.vector1, vehicle.vector2)
								
	if left(vehicle.action.name,1) = "3"
		if vehicle.dotMove > 0.8
			vehicle.action = GetAction(ACTION_GO, vehicle)	
		endif		
	endif
	
	if vehicle.npc = TRUE
		if vehicle.dotMove > 0.95 or vehicle.velocity.current < vehicle.velocity.max * 0.2
			vehicle.drift.on = TRUE
		endif
	endif
	
	vehicle.distCurrentNode = DistanceXZ(vehiclePos, currentNode)	
	
	if vehicle.dotMove < 0	
		d1 = PointToLine(vehiclePos, prevNode, currentNode, n1)						
		d2 = PointToLine(vehiclePos, currentNode, nextNode, n2)			
		if d2 < d1 and vehicle.distCurrentNode  < 2000
			r = TRUE
			GetNextNode(vehicle)			
		endif
	endif

endfunction r

function FollowPath(vehicle ref as tVehicle)
	turn		as integer
	distance 	as float
	absDistance as float
	vehiclePos	as tPoint
	currentNode	as tPoint
	forward		as tPoint
	node		as tNode
	new			as integer
	thresh		as integer
	
	CheckNextNode(vehicle)
		
	vehiclePos = GetObjectPosition(vehicle.hit) 
	node = GetPathNode(vehicle, vehicle.path.current)
	currentNode = node.slots[vehicle.slot]
		
	distance = DistanceXZ(vehiclePos, currentNode)	
	
	if DEBUG
		Draw3dLine(vehiclePos, currentNode, vehicle.colour)
	endif
	
	node = GetPathNode(vehicle, GetPathCurrentIndex(vehicle))
	thresh = ( vehicle.velocity.max - ((5 - vehicle.slot) * SCALE_3D * 5.5))
	
	if vehicle.velocity.boost > 0
		thresh = thresh * 0.8
	endif
	
	if distance < thresh
		if vehicle.action.name = "go" and vehicle.velocity.current > vehicle.velocity.max * .5
			vehicle.drift.on = FALSE
			exitfunction node.turn
		endif
	endif
	
	forward = GetObjectPosition(vehicle.forward) 
	distance = PointSide(vehiclePos, currentNode, forward)
	
	
	
	SetVectorPoint(vehicle.vector1, VectorNormalize(SubtractPoints(vehicle.lastPos, vehiclePos)))
	SetVectorPoint(vehicle.vector2, VectorNormalize(SubtractPoints(vehiclePos, forward)))
	vehicle.dotNext = GetVector3Dot(vehicle.vector1, vehicle.vector2)
	
	if vehicle.dotNext <> 0
		distance = distance * (vehicle.dotNext/abs(vehicle.dotNext))
	endif
	
	absDistance = abs(distance)
	
	if absDistance > 60
		turn = distance / absDistance
	endif
	
						
endfunction turn

function GetAccFromKeyboard(vehicle ref as tVehicle, env ref as tEnvironment)
	v as integer
	r as integer
	
	if vehicle.action.name <> "stop"
		if GetRawJoystickExists(1)
			if GetRawJoystickButtonState(1, JOYSTICK_A)
				v = -vehicle.velocity.linearUnit
			endif
					
			if GetRawJoystickButtonState(1, JOYSTICK_Y)
				v = vehicle.velocity.linearUnit
			endif	
		endif
		if GetRawKeyState(KEY_DOWN) or GetJoystickButton(JOYSTICK_A)
			v = -vehicle.velocity.linearUnit
		endif
				
		if GetRawKeyState(KEY_UP) or GetJoystickButton(JOYSTICK_A)
			v = vehicle.velocity.linearUnit
		endif	
		if GetRawKeyState(KEY_CONTROL) = 0 and GetRawKeyState(KEY_SHIFT) = 0
			if GetRawKeyState(asc("S"))
				v = -vehicle.velocity.linearUnit
			endif
					
			if GetRawKeyState(asc("W")) 
				v = vehicle.velocity.linearUnit
			endif	
		endif
	endif

	CheckNextNode(vehicle)
	
			
endfunction v

	
function GetAccFromPath(vehicle ref as tVehicle)
	v	as integer
	m	as tMove
	a	as tAction
	t 	as float
	vel	as float
		
	a = vehicle.action

	m = a.move[a.current]
	
	if a.speed > 0
		if vehicle.velocity.save = 0
			vehicle.velocity.save = vehicle.velocity.max
			vehicle.velocity.max = vehicle.velocity.max * a.speed	
		endif
	else
		if vehicle.velocity.save > 0
			vehicle.velocity.max = vehicle.velocity.save
			vehicle.velocity.save = 0
		endif
	endif
	
	v = vehicle.velocity.linearUnit * m.acc

	if m.time <> FOREVER
		t = Timer() - a.start
		if t > m.time
			if a.current < a.move.length
				inc a.current
			else
				a = GetAction(a.nextAction, vehicle)
			endif
			if a.start <> FOREVER
				a.start = Timer()	
			endif			
		endif
	endif
	
	vehicle.action = a
	
	
	if vehicle.drift.on = FALSE
		v = 0
	endif
	
endfunction v

//
// This checks for collision with walls
// If the collision is dramatic it turns everything off for vehicle.delay.amount seconds
// If the collision is just grazing the wall then ignore it
//
function	CheckCollision(vehicle ref as tVehicle, env ref as tEnvironment, level as tLevel)
	o	as integer
	b	as float
	a	as float
	c	as tPoint
	r 	as integer = FALSE
	v	as tVehicle
	vel as tPoint
	
	vehicle.dotHit = 0
	vehicle.crashed = FALSE
	
	vel = GetVelocityPoint(vehicle)

	if vehicle.delay.count < timer() 
		if GetObject3DPhysicsFirstContact( vehicle.hit ) 
			vehicle.collision = FALSE
			repeat 
				o = GetObject3DPhysicsContactObjectB()
				if o = level.walls.id
					vehicle.drift.on = TRUE					
					if r = FALSE
						a = DistancePoint(vel)
						b = DistancePoint(vehicle.velocity.last)
						if a / b < 0.7												// Change in velocity is dramatic so we are not just grazing a wall.					
							vehicle.state = VEHICLE_STATE_FREE
							r = TRUE						
							vehicle.velocity.current=0
							vehicle.delay.count = timer() + vehicle.delay.amount		//  Turn everything off
							if vehicle.npc = TRUE
								SetVehicle3PointTurn(vehicle, env)
							endif
							//SetObjectColor(vehicle.chassis, 0xff, 0, 0, 0xff)
						endif
					endif
					vehicle.crashed = TRUE
				elseif o <> level.ground.id
					if o = PLAYER_ID
						vehicle.crashed = TRUE
					elseif o > PLAYER_ID and o < PLAYER_ID + 100
						vehicle.crashed = TRUE
						SetVehiclePause(vehicle, mod(o,PLAYER_ID))
					elseif o > BANANAS and o < BANANAS + 999
						print("Banana "+vehicle.name)
					elseif o > GRENADES and o < GRENADES + 999
						print("Grenade "+vehicle.name)
					elseif o = level.grass.id 
						SetupVehiclePhysics(vehicle, FALSE, CreatePoint(0, 0, 0))							
						// print("Grass "+vehicle.name)
					elseif o = level.bridge.id
						: // print(vehicle.name+" Collided with bridge")								
					else
						SetObjectColor(o, 0xff, 0, 0, 0xff)
						Message("Unknown ["+vehicle.name+"] ("+LookupObject(o)+")")
					endif
				endif				
			until GetObject3DPhysicsNextContact() = FALSE
		endif
	endif
endfunction r

function LookupObject(o as integer)
	i as integer
	name as string
	name = str(o)
	for i=0 to gLevel.objs.length
		if o = gLevel.objs[i].id
			name = gLevel.objs[i].name
			exit
		endif
	next
endfunction name

function CheckTunneling(vehicle ref as tVehicle, env ref as tEnvironment, level ref as tLevel)
	f		as tPoint
	r		as tPoint
	l		as tPoint	
	rc1		as integer
	rc2		as integer
	v1		as tPoint
	v2		as tPoint
	v		as tPoint
	
	if vehicle.velocity.current < ( vehicle.velocity.max * 0.8 )
		exitfunction FALSE
	endif	
	
	v1 = GetVelocityPoint(vehicle)	
	v2 = GetVehicleDirectionGround(vehicle, TRUE)	
	
	SetVectorPoint(vehicle.vector1, VectorNormalize(v1))
	SetVectorPoint(vehicle.vector2, VectorNormalize(v2))	
	vehicle.dotTunnel = GetVector3Dot(vehicle.vector1, vehicle.vector2)
	
	if abs(vehicle.dotTunnel) > 0.9
		exitfunction FALSE
	endif
	
	f = GetObjectPos(vehicle.center)
	r = GetObjectPos(vehicle.rightSensor)	
	l = GetObjectPos(vehicle.leftSensor)		
		
	if ObjectRayCast(level.walls.id, f.x, f.y, f.z, r.x, r.y, r.z) = 0 
		if ObjectRayCast(level.walls.id, f.x, f.y, f.z, l.x, l.y, l.z) = 0 		
			exitfunction FALSE
		endif
	endif
		
	//PlayCrash(vehicle, env.bump)	
	v = v1					
	vehicle.velocity.current=0
	vehicle.delay.count = timer() + vehicle.delay.amount				
	SetObjectColor(vehicle.chassis, 0, 0xff, 0, 0xff)
	vehicle.state = VEHICLE_STATE_FREE
	SetObject3DPhysicsLinearVelocity(vehicle.hit, v.x, v.y, v.z, 30 * SCALE_3D)
endfunction TRUE


function GetVelocityPoint(vehicle ref as tVehicle)
	p as tPoint
	
	p.x = GetObject3DPhysicsLinearVelocityX(vehicle.hit)
	p.y = GetObject3DPhysicsLinearVelocityY(vehicle.hit)	
	p.z = GetObject3DPhysicsLinearVelocityZ(vehicle.hit)	
endfunction p

function SetVehicle3PointTurn(vehicle ref as tVehicle, env ref as tEnvironment)
	dot as float
	o	as integer
	v1	as tPoint
	v2  as tPoint
	vehiclePos as tPoint
	forward as tPoint
	collision as tPoint
	currentNode as tPoint
	nextNode as tPoint
	nearest as tPoint
	
	
	if vehicle.action.name = "stop" or vehicle.action.name = "slow"
		exitfunction
	endif
			
	collision = CreatePoint(GetObject3DPhysicsContactX()*SCALE_3D, GetObject3DPhysicsContactY()*SCALE_3D,GetObject3DPhysicsContactZ()*SCALE_3D)
	vehiclePos = GetObjectPosition(vehicle.center)
	forward = GetObjectPosition(vehicle.dotID)
	vehiclePos.y = 0 : collision.y = 0 : forward.y = 0
	
	v1 = VectorNormalize(SubtractPoints(vehiclePos, collision))
	v2 = VectorNormalize(SubtractPoints(vehiclePos, forward))
	SetVectorPoint(vehicle.vector1, v1)
	SetVectorPoint(vehicle.vector2, v2)	
	vehicle.dotHit = GetVector3Dot(vehicle.vector1, vehicle.vector2)

	if left(vehicle.action.name,1) <> "3"		
		CheckNextNode(vehicle)
	endif
	
	if vehicle.dotHit > 0 
		vehicle.action = GetAction(ACTION_3BACK, vehicle)
	else
		vehicle.action = GetAction(ACTION_3FORWARD, vehicle)				
	endif		
	
	vehicle.action.start = Timer()	
	vehicle.collision = TRUE		
			
endfunction

function SetVehiclePause(vehicle ref as tVehicle, no as integer)
	node	as tPoint
	v		as tVehicle
	v2		as tvehicle
	d1		as float
	d2		as float
	
	v2 = gVehicles[no]
	
	if vehicle.action.name = "stop" or vehicle.action.name = "slow"
		exitfunction
	endif

	node = GetPathCurrentPoint(vehicle)
	
	d1 = DistanceXZ(GetObjectPosition(vehicle.hit), node)
	d2 = DistanceXZ(GetObjectPosition(v2.hit), node)	
	
	if d1 > d2
		if vehicle.action.name <> "stop"		
			vehicle.action = GetAction(ACTION_PAUSE, vehicle)
			vehicle.action.start = Timer()	
		endif
	endif

endfunction

//
// This manages the wheels
// 1. It determines if the wheel is in contact with the ground
// 2. It manages the suspension (Moving the wheel up and down)
//
function	CheckWheels(vehicle ref as tVehicle, i as integer, env ref as tEnvironment, level ref as tLevel)
	l 		as line
	h		as tPoint
	d		as float
	m		as tPoint
	c		as integer
	w   	as tWheel
	v		as float
	y		as float
	ease	as float = 0.3
	p		as tPoint	
	id		as integer
	
	w = vehicle.wheels[i]
		
	//
	// Manage the rolling of the wheel
	//
	if vehicle.velocity.current > 0
		d = 1
	else
		d = -1
	endif
	
	v = Distance3D(CreatePoint(GetObjectX(vehicle.hit), GetObjectY(vehicle.hit), GetObjectZ(vehicle.hit)), vehicle.lastPos)  * d	
	RotateObjectLocalX(w.id, (v / w.circumfrence) * 360)
				
	//
	// Determine if the wheel is touching the ground
	// and manage the suspension
	//
	SetObjectColor(w.sensor.t, 0xff, 0, 0, 0xff)	
	
	p = w.pos	
	l.f = GetObjectPosition(w.sensor.f)
	l.t = GetObjectPosition(w.sensor.t)		

	c = ObjectRayCast(level.ground.id, l.f.x, l.f.y, l.f.z, l.t.x, l.t.y, l.t.z )

	w.hit.contact = FALSE
	
	if c <> 0			// Sensor has tHit the ground
		id = GetObjectRayCastHitID( 0 )
		if id <> level.ground.id
			Message("Collision not with ground")
		endif
		w.hit.pos.x = GetObjectRayCastX(0) : w.hit.pos.y = GetObjectRayCastY(0) :w.hit.pos.z = GetObjectRayCastZ(0)
		d = GetObjectRayCastDistance( 0 ) 
		SetObjectPosition(w.id, 0, -d, 0)		
		SetObjectPosition(w.hit.id, 0, GetObjectY(w.sensor.f) - d, 0)		
		if d > w.radius  // Not touching the ground
			d = 0
		else
			w.hit.contact = TRUE
			SetObjectColor(w.sensor.t, 0, 0xff, 0, 0xff)
			d = 1
		endif
	else				// Not touching the ground
		d = 0
		l.f = GetObjectLocalPos(w.id)
		l.t.y = -(w.radius * vehicle.suspension)
		SetObjectPosition(w.id, 0, l.f.y + (l.t.y - l.f.y) * ease, 0)
	endif
	
	vehicle.wheels[i] = w

endfunction d	

function	GetVehicleDirectionGround(vehicle as tVehicle, flag as integer)
	v	as tPoint
	f	as tPoint
	t	as tPoint
	
	//
	// Select right or left side wheels contact to calculate direction
	//
	if ( vehicle.wheels[FRONT_RIGHT].hit.contact = TRUE and vehicle.wheels[REAR_RIGHT].hit.contact = TRUE ) or flag = TRUE
		f = vehicle.wheels[REAR_RIGHT].hit.pos
		t = vehicle.wheels[FRONT_RIGHT].hit.pos		
	elseif vehicle.wheels[FRONT_LEFT].hit.contact = TRUE and vehicle.wheels[REAR_LEFT].hit.contact = TRUE 
		f = vehicle.wheels[REAR_LEFT].hit.pos
		t = vehicle.wheels[FRONT_LEFT].hit.pos		
	else
		Message("Catastrophic error : Wheels aren't touching")
	endif
	
	v.x = t.x - f.x : v.y = t.y - f.y : v.z = t.z - f.z 
	
endfunction v




function DeleteVehicle(vehicle ref as tVehicle)
	i 	as integer
	w 	as tWheel
	new	as tVehicle
	
	Delete3DPhysicsBody(vehicle.hit)
	
	for i=0 to vehicle.wheels.length
		w = vehicle.wheels[i]
		w.sensor.f = DestroyObject(w.sensor.f)
		w.sensor.t = DestroyObject(w.sensor.t)		
		w.shape = DestroyObject(w.shape)				
		w.hit.id = DestroyObject(w.hit.id)						
		w.id = DestroyObject(w.id)							
		w.pivot = DestroyObject(w.pivot)		
		
		Delete3DPhysicsRay(w.ray)	: w.ray = 0
		DeleteVector3(w.fromVect)	: w.fromVect = 0
		DeleteVector3(w.toVect)		: w.toVect = 0
	next
	
	vehicle.wheels.length = -1

	DeleteVector3(vehicle.vector1) : vehicle.vector1 = 0
	DeleteVector3(vehicle.vector2) : vehicle.vector2 = 0
	
	DeleteVector3(vehicle.vectU) : vehicle.vectU = 0
	DeleteVector3(vehicle.vectV) : vehicle.vectV = 0
	
	vehicle.detail = DestroyObject(vehicle.detail)
	vehicle.dotID = DestroyObject(vehicle.dotID)
	vehicle.forward = DestroyObject(vehicle.forward)
	vehicle.reverse = DestroyObject(vehicle.reverse)
	vehicle.rightSensor = DestroyObject(vehicle.rightSensor)
	vehicle.leftSensor = DestroyObject(vehicle.leftSensor)
	vehicle.chassis = DestroyObject(vehicle.chassis)	
	vehicle.hit = DestroyObject(vehicle.hit)
	vehicle.center = DestroyObject(vehicle.center)
	
	vehicle.camera1st.dolly = DestroyObject(vehicle.camera1st.dolly)
	vehicle.camera1st.id = DestroyObject(vehicle.camera1st.id)
	vehicle.camera3rd.dolly = DestroyObject(vehicle.camera3rd.dolly)
	vehicle.camera3rd.id = DestroyObject(vehicle.camera3rd.id)
	
	if GetSpriteExists(vehicle.spot)
		DeleteSprite(vehicle.spot)
	endif

	if GetTextExists(vehicle.tag)
		DeleteText(vehicle.tag)
	endif
	
	if GetImageExists(vehicle.image)
		DeleteImage(vehicle.image)
	endif
	
	StopSoundInstance(vehicle.engine)
	StopSoundInstance(vehicle.land)
	StopSoundInstance(vehicle.bump)

	vehicle = new
	
endfunction
