Fix bug in distance calculation for trajectory rollout

The parentheses for the square root in the distance calculation were wrong.

I suppose this is the reason why the "2 *" seemed necessary in the line below. However, I didn't test it.