simgear/route/route.cxx

   1 // route.cxx -- Class to manage a list of waypoints (route)
   2 //
   3 // Written by Curtis Olson, started October 2000.
   4 //
   5 // Copyright (C) 2000  Curtis L. Olson  - curt@hfrl.umn.edu
   6 //
   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.
  11 //
  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.
  16 //
  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., 675 Mass Ave, Cambridge, MA 02139, USA.
  20 //
  21 // $Id$
  22
  23
  24 #include <plib/sg.h>
  25
  26 #include <simgear/math/vector.hxx>
  27
  28 #include "route.hxx"
  29
  30
  31 // constructor
  32 SGRoute::SGRoute() {
  33     route.clear();
  34 }
  35
  36
  37 // destructor
  38 SGRoute::~SGRoute() {
  39 }
  40
  41
  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;
  49         int n1 = current_wp;
  50         sgdVec3 p, p0, p1, d;
  51         sgdSetVec3( p, x, y, 0.0 );
  52         sgdSetVec3( p0,
  53                     route[n0].get_target_lon(), route[n0].get_target_lat(),
  54                     0.0 );
  55         sgdSetVec3( p1,
  56                     route[n1].get_target_lon(), route[n1].get_target_lat(),
  57                     0.0 );
  58         sgdSubVec3( d, p0, p1 );
  59
  60         return sqrt( sgdClosestPointToLineDistSquared( p, p0, d ) );
  61
  62     } else {
  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.
  67
  68         return 0;
  69     }
  70 }