//Change waypoint if we have passed the waypoint we were heading towards
void TryChangeWaypoint()
{
//Should always be measured from the rear wheels because that's what is used when generating the path
//But in the final version we are moving the waypoints forward, so it depends on if we are reversing or not
//bool isReversing = wayPoints[currentWayPointIndex].isReversing;
bool isReversing = false;
Vector3 wp2 = GetWaypointPos(currentWayPointIndex, isReversing);
Vector3 wp1 = GetWaypointPos(currentWayPointIndex - 1, isReversing);
Vector3 frontAxle = isReversing ? carDataController.MirroredFrontWheelPos(transform) : carDataController.FrontWheelPos(transform);
//If we have reached the waypoint we are aiming for
if (CalculateProgress(frontAxle, wp1, wp2) > 1f)
{
currentWayPointIndex += 1;
//Clamp when we have reached the last waypoint so we start all over again
if (currentWayPointIndex > wayPoints.Count - 1 && isCircular)
{
currentWayPointIndex = 0;
}
//The path in not circular so we should stop following it
else if (currentWayPointIndex > wayPoints.Count - 1)
{
wayPoints = null;
//Stop the car when we have reached the end of the path
carScript.StopCar();
}
//Debug.Log(currentWayPointIndex);
}
}
