/*
Modelling of Car motion using Xpresso/Coffee
Steve Baines April 2003
E: steve@astrofish.com
W: www.astrofish.com

Version 1.0

This allows a car to be controlled with speed and steering controls.
Wheel rotation and exact wheel pointing angles are not modelled, and the car is 
assumed not to lose traction at any point. To simplify the maths, this version 
assumes that the rear wheels also steer.

For any given steering angle, the car is considered to be instantaneously travelling
in a circle with notional wheels at the midpoint of the axles tangential to the circle.
For a car with non-steering back wheels, the 'best fit' circle needs to be calculated.

Note that the accuracy of the position and rotation of the car is not dependent
on the time-step. The only thing that the timestep affects is that the steering
is assumed to be held constant for the duration of a timestep.

(The length of the wheel base can be adjusted by changing the Z seperation of the
'middle' wheels. Keep them symetric about zero though).

*/


main()
{

if(frame == 0) {
  position = startPos;
  heading = 0.0;
}

// Get speed into sensible range.
speedCtrl = 20.0 * speedCtrl;
steerCtrl = -steerCtrl;

// We calculate the maths based on imaginary centre wheels
// at front and back.

// Rear centre wheel is at angle '-steerCtrl' degrees.
// Front centre wheel is at angle 'steerCtrl' degrees.
// Unless it is sliding (which we don't model!) 
// the Car must be moving on a circle such that both wheels are
// tangential to the circle. 

//   Note that this is only possible when both front and rear wheels
//   steer. If the rear wheels are fixed then although the car is driving
//   in a circle, the wheels can't be simultaneously tangential, so the
//   tyres are constantly being 'twisted'.
//   To make the maths easier for this demo, I made the back wheels steer...

// Distance that we will cover this frame
var dist = speedCtrl*deltaT;

// If we are steering straight then the turning radius is infinite.
// We handle this as a special case.
if(abs(steerCtrl)<0.01) {
  headingNew = heading; // No change to heading
  positionNew = position; // Move in a straight line
  positionNew.x -= dist*sin(heading);
  positionNew.z += dist*cos(heading);
	// Hide the turning circle
  circlePos = positionNew;
  return;
}

// 'wheelBase' is the distance between the front and rear 'centre' wheels.
// Calculate the radius of the current turning circle.
// Note that this can be negative. This effectively just says
// which way around the circle we are going.
var radius = wheelBase / (2.0 * sin(steerCtrl));
circleRad = abs(radius);

// What fraction of the circle do we move this frame
// (distance / circumference)
var fracCirc = dist / (2.0*PI*radius);
// Hence, what angle have we rotated through (in radians)
var deltaHeading = fracCirc * (2.0*PI);
// Note that this simplifies to: deltaHeading = dist/radius
// I've left them seperate to make is clearer where this comes from.

// Set new heading
headingNew = heading+deltaHeading;

// Make sure we stay in a sensible range of angles
while(headingNew > 2*PI) {
  headingNew -= 2*PI;
}
while(headingNew < 0) {
  headingNew += 2*PI;
}

// Calculate distance from centre of car to centre of turning circle.
// (for shallow steering angles, this is almost the same as 'radius'
var r2 = wheelBase / (2.0 * tan(steerCtrl)); 

// Calculate centre of turning circle
circlePos = position;
circlePos.x -= r2*cos(heading);
circlePos.z -= r2*sin(heading);

// Because we are on a circle, we can calculate our new position from the new bearing
// that we have just calculated.
positionNew = position;
positionNew.x = circlePos.x + r2*cos(headingNew);
positionNew.z = circlePos.z + r2*sin(headingNew);

// This simplifies slightly to:
// positionNew.y = position.y;
// positionNew.x = position + r2*( cos(headingNew)-cos(heading) );
// positionNew.z = position + r2*( sin(headingNew)-sin(heading) );
// (Should probably replace this with a polynomial expansion for numerical accuracy)
}