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.
72 /** Update the length of the leg ending at waypoint index */
73 void SGRoute::update_distance(int index)
75 SGWayPoint& curr = route[ index ];
81 const SGWayPoint& prev = route[ index - 1 ];
82 curr.CourseAndDistance( prev, &course, &dist );
85 curr.set_distance( dist );
89 * Add waypoint (default), or insert waypoint at position n.
90 * @param wp a waypoint
92 void SGRoute::add_waypoint( const SGWayPoint &wp, int n ) {
93 int size = route.size();
94 if ( n < 0 || n >= size ) {
96 route.push_back( wp );
98 route.insert( route.begin() + n, 1, wp );
99 // update distance of next leg if not at end of route
100 update_distance( n + 1 );
102 update_distance( n );
105 /** Delete waypoint with index n (last one if n < 0) */
106 void SGRoute::delete_waypoint( int n ) {
107 int size = route.size();
110 if ( n < 0 || n >= size )
113 route.erase( route.begin() + n );
114 // update distance of next leg if not at end of route
116 update_distance( n );