Original version as published in July 2012
Make code compile with GCC
Fix numerical problem that occurs when a switching point candidate is at the very end of a path segment.

The problem appears e. g. when passing in the following path:

waypoint << 1.0, 0.0, 0.0;
waypoint << 0.0, 1.0, 0.0;
waypoint << 0.0, 0.0, 1.0;

Thanks to Fredrik Solenberg for reporting the problem.