/// <summary>
/// Calculate the car's new heading
/// </summary>
/// <param name="theta">The car's heading (= rotation) [rad]</param>
/// <param name="beta">Steering angle [rad]</param>
/// <returns>The car's new heading</returns>
public static float CalculateNewHeading(float theta, float beta)
{
//Change heading
theta = theta + beta;
//Clamp heading - is sometimes causing infinite loop so dont use the old version?
theta = HelpStuff.WrapAngleInRadians(theta);
//Clamp heading
//if (theta > 2f * Mathf.PI)
//{
// theta = theta - 2f * Mathf.PI;
//}
//if (theta < 0f)
//{
// theta = 2f * Mathf.PI + theta;
//}
//Debug.Log(theta + " " + theta2);
return(theta);
}
/// <summary>
/// Calculate the heading of a trailer attached to a drag vehicle
/// </summary>
/// <param name="thetaOld">The trailers old heading [rad]</param>
/// <param name="thetaOldDragVehicle">The drag vehicles old heading [rad]</param>
/// <param name="D">Drive distance [m]. Should be negative if we reverse</param>
/// <param name="d">Distance between trailer attachment point and trailer rear axle [m]</param>
/// <returns>The trailer's new heading [rad]</returns>
public static float CalculateNewTrailerHeading(float thetaOld, float thetaOldDragVehicle, float D, float d)
{
//According to some D is velocity of the drag vehicle but is not really working
//From "Planning algorithms" which is different from http://planning.cs.uiuc.edu/node661.html#77556
//where (thetaOldDragVehicle - thetaOld) is the opposite which gives a mirrored result
//float theta = thetaOld + (D / d) * Mathf.Sin(thetaOldDragVehicle - thetaOld);
float theta = thetaOld + ((D / d) * Mathf.Sin(thetaOldDragVehicle - thetaOld));
//Clamp heading - is sometimes causing infinite loop so dont use?
theta = HelpStuff.WrapAngleInRadians(theta);
//Clamp heading
//if (theta > 2f * Mathf.PI)
//{
// theta = theta - 2f * Mathf.PI;
//}
//if (theta < 0f)
//{
// theta = 2f * Mathf.PI + theta;
//}
return(theta);
}