Path interpolation and max speed calculation