-
-// Calculate perpendicular distance from the current route segment
-// This routine assumes all points are laying on a flat plane and
-// ignores the altitude (or Z) dimension. For best results, use with
-// CARTESIAN way points.
-double SGRoute::distance_off_route( double x, double y ) const {
- if ( current_wp > 0 ) {
- int n0 = current_wp - 1;
- int n1 = current_wp;
- sgdVec3 p, p0, p1, d;
- sgdSetVec3( p, x, y, 0.0 );
- sgdSetVec3( p0,
- route[n0].get_target_lon(), route[n0].get_target_lat(),
- 0.0 );
- sgdSetVec3( p1,
- route[n1].get_target_lon(), route[n1].get_target_lat(),
- 0.0 );
- sgdSubVec3( d, p0, p1 );
-
- return sqrt( sgdClosestPointToLineDistSquared( p, p0, d ) );
-
- } else {
- // We are tracking the first waypoint so there is no route
- // segment. If you add the current location as the first
- // waypoint and the actual waypoint as the second, then we
- // will have a route segment and calculate distance from it.
-
- return 0;
- }
-}
-