Fixed waypoint bug

Fixed a bug where waypoints were being skipped incorrectly. It occurred
due to a floating point comparison which did not take account of any
acceptable epsilon error. 

Note that waypoints are likely to be calculated differently now, so
object positions will be too, and overall system behaviour and output
will be affected.