private double PostProcessSpeed(SmoothedPath path)
{
const double a_lat_max = 1.5;
const double lat_target_decel = 1;
double maxSpeed = 20;
// return an astronomical speed if there are no curvatures
if (path.Count < 3)
return maxSpeed;
// calculate the curvature at each point
// we want the lateral acceleration to be no more than a_lat_max, so calculate the speed we would need to be going now achieve that deceleration
double dist = 0;
for (int i = 1; i < path.Count-1; i++) {
dist += path[i-1].DistanceTo(path[i]);
// lateral acceleration = v^2*curvature
double curvature = path.GetCurvature(i);
double desiredSpeed = Math.Sqrt(a_lat_max/Math.Abs(curvature));
double desiredMaxSpeed = Math.Sqrt(desiredSpeed*desiredSpeed + 2*lat_target_decel*dist);
if (desiredMaxSpeed < maxSpeed) {
maxSpeed = desiredMaxSpeed;
}
}
return maxSpeed;
}
