1 // route.cxx -- Class to manage a list of waypoints (route)
3 // Written by Curtis Olson, started October 2000.
5 // Copyright (C) 2000 Curtis L. Olson - curt@hfrl.umn.edu
7 // This program is free software; you can redistribute it and/or
8 // modify it under the terms of the GNU General Public License as
9 // published by the Free Software Foundation; either version 2 of the
10 // License, or (at your option) any later version.
12 // This program is distributed in the hope that it will be useful, but
13 // WITHOUT ANY WARRANTY; without even the implied warranty of
14 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
15 // General Public License for more details.
17 // You should have received a copy of the GNU General Public License
18 // along with this program; if not, write to the Free Software
19 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
26 #include <simgear/math/vector.hxx>
42 // Calculate perpendicular distance from the current route segment
43 // This routine assumes all points are laying on a flat plane and
44 // ignores the altitude (or Z) dimension. For best results, use with
45 // CARTESIAN way points.
46 double SGRoute::distance_off_route( double x, double y ) const {
47 if ( current_wp > 0 ) {
48 int n0 = current_wp - 1;
51 sgdSetVec3( p, x, y, 0.0 );
53 route[n0].get_target_lon(), route[n0].get_target_lat(),
56 route[n1].get_target_lon(), route[n1].get_target_lat(),
58 sgdSubVec3( d, p0, p1 );
60 return sqrt( sgdClosestPointToLineDistSquared( p, p0, d ) );
63 // We are tracking the first waypoint so there is no route
64 // segment. If you add the current location as the first
65 // waypoint and the actual waypoint as the second, then we
66 // will have a route segment and calculate distance from it.