Fix bug in distance calculation for trajectory rollout
Use hypot() instead of sqrt(x*x, y*y)