src/AIModel/AIFlightPlan.cxx

   1 // FGAIFlightPlan - class for loading and storing  AI flight plans
   2 // Written by David Culp, started May 2004
   3 // - davidculp2@comcast.net
   4 //
   5 // This program is free software; you can redistribute it and/or
   6 // modify it under the terms of the GNU General Public License as
   7 // published by the Free Software Foundation; either version 2 of the
   8 // License, or (at your option) any later version.
   9 //
  10 // This program is distributed in the hope that it will be useful, but
  11 // WITHOUT ANY WARRANTY; without even the implied warranty of
  12 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  13 // General Public License for more details.
  14 //
  15 // You should have received a copy of the GNU General Public License
  16 // along with this program; if not, write to the Free Software
  17 // Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  18
  19
  20 #include "AIFlightPlan.hxx"
  21 #include <simgear/misc/sg_path.hxx>
  22 #include <simgear/debug/logstream.hxx>
  23 #include <simgear/structure/exception.hxx>
  24 #include <simgear/constants.h>
  25 #ifdef __BORLANDC__
  26 #  define exception c_exception
  27 #endif
  28 #include <simgear/props/props.hxx>
  29 #include <Main/globals.hxx>
  30 #include <Main/fg_props.hxx>
  31
  32
  33 FGAIFlightPlan::FGAIFlightPlan(string filename)
  34 {
  35   int i;
  36   SGPath path( globals->get_fg_root() );
  37   path.append( ("/Data/AI/FlightPlans/" + filename).c_str() );
  38   SGPropertyNode root;
  39
  40   try {
  41       readProperties(path.str(), &root);
  42   } catch (const sg_exception &e) {
  43       SG_LOG(SG_GENERAL, SG_ALERT,
  44        "Error reading AI flight plan: ");
  45        cout << path.str() << endl;
  46       return;
  47   }
  48
  49   SGPropertyNode * node = root.getNode("flightplan");
  50   for (i = 0; i < node->nChildren(); i++) {
  51      //cout << "Reading waypoint " << i << endl;
  52      waypoint* wpt = new waypoint;
  53      SGPropertyNode * wpt_node = node->getChild(i);
  54      wpt->name      = wpt_node->getStringValue("name", "END");
  55      wpt->latitude  = wpt_node->getDoubleValue("lat", 0);
  56      wpt->longitude = wpt_node->getDoubleValue("lon", 0);
  57      wpt->altitude  = wpt_node->getDoubleValue("alt", 0);
  58      wpt->speed     = wpt_node->getDoubleValue("ktas", 0);
  59      wpt->crossat   = wpt_node->getDoubleValue("crossat", -10000);
  60      wpt->gear_down = wpt_node->getBoolValue("gear-down", false);
  61      wpt->flaps_down= wpt_node->getBoolValue("flaps-down", false);
  62      wpt->on_ground = wpt_node->getBoolValue("on-ground", false);
  63
  64      if (wpt->name == "END") wpt->finished = true;
  65      else wpt->finished = false;
  66
  67      waypoints.push_back( wpt );
  68    }
  69
  70   wpt_iterator = waypoints.begin();
  71   //cout << waypoints.size() << " waypoints read." << endl;
  72 }
  73
  74
  75 FGAIFlightPlan::~FGAIFlightPlan()
  76 {
  77   waypoints.clear();
  78 }
  79
  80
  81 FGAIFlightPlan::waypoint*
  82 FGAIFlightPlan::getPreviousWaypoint( void )
  83 {
  84   if (wpt_iterator == waypoints.begin()) {
  85     return 0;
  86   } else {
  87     wpt_vector_iterator prev = wpt_iterator;
  88     return *(--prev);
  89   }
  90 }
  91
  92 FGAIFlightPlan::waypoint*
  93 FGAIFlightPlan::getCurrentWaypoint( void )
  94 {
  95   return *wpt_iterator;
  96 }
  97
  98 FGAIFlightPlan::waypoint*
  99 FGAIFlightPlan::getNextWaypoint( void )
 100 {
 101   if (wpt_iterator == waypoints.end()) {
 102     return 0;
 103   } else {
 104     wpt_vector_iterator next = wpt_iterator;
 105     return *(++next);
 106   }
 107 }
 108
 109 void FGAIFlightPlan::IncrementWaypoint( void )
 110 {
 111   wpt_iterator++;
 112 }
 113
 114 // gives distance in feet from a position to a waypoint
 115 double FGAIFlightPlan::getDistanceToGo(double lat, double lon, waypoint* wp){
 116    // get size of a degree at the present latitude
 117    // this won't work over large distances
 118    double ft_per_deg_lat = 366468.96 - 3717.12 * cos(lat / SG_RADIANS_TO_DEGREES);
 119    double ft_per_deg_lon = 365228.16 * cos(lat / SG_RADIANS_TO_DEGREES);
 120    double lat_diff_ft = fabs(wp->latitude - lat) * ft_per_deg_lat;
 121    double lon_diff_ft = fabs(wp->longitude - lon) * ft_per_deg_lon;
 122    return sqrt((lat_diff_ft * lat_diff_ft) + (lon_diff_ft * lon_diff_ft));
 123 }
 124
 125 // sets distance in feet from a lead point to the current waypoint
 126 void FGAIFlightPlan::setLeadDistance(double speed, double bearing,
 127                                      waypoint* current, waypoint* next){
 128   double turn_radius = 0.1911 * speed * speed; // an estimate for 25 degrees bank
 129   double inbound = bearing;
 130   double outbound = getBearing(current, next);
 131   double diff = fabs(inbound - outbound);
 132   if (diff > 180.0) diff = 360.0 - diff;
 133   lead_distance = turn_radius * sin(diff * SG_DEGREES_TO_RADIANS);
 134 }
 135
 136 void FGAIFlightPlan::setLeadDistance(double distance_ft){
 137   lead_distance = distance_ft;
 138 }
 139
 140
 141 double FGAIFlightPlan::getBearing(waypoint* first, waypoint* second){
 142   return getBearing(first->latitude, first->longitude, second);
 143 }
 144
 145
 146 double FGAIFlightPlan::getBearing(double lat, double lon, waypoint* wp){
 147   double latd = lat;
 148   double lond = lon;
 149   double latt = wp->latitude;
 150   double lont = wp->longitude;
 151   double ft_per_deg_lat = 366468.96 - 3717.12 * cos(lat/SG_RADIANS_TO_DEGREES);
 152   double ft_per_deg_lon = 365228.16 * cos(lat/SG_RADIANS_TO_DEGREES);
 153
 154   if (lond < 0.0) lond+=360.0;
 155   if (lont < 0.0) lont+=360.0;
 156   latd+=90.0;
 157   latt+=90.0;
 158
 159   double lat_diff = (latt - latd) * ft_per_deg_lat;
 160   double lon_diff = (lont - lond) * ft_per_deg_lon;
 161   double angle = atan(fabs(lat_diff / lon_diff)) * SG_RADIANS_TO_DEGREES;
 162
 163   bool southerly = true;
 164   if (latt > latd) southerly = false;
 165   bool easterly = false;
 166   if (lont > lond) easterly = true;
 167   if (southerly && easterly) return 90.0 + angle;
 168   if (!southerly && easterly) return 90.0 - angle;
 169   if (southerly && !easterly) return 270.0 - angle;
 170   if (!southerly && !easterly) return 270.0 + angle;
 171
 172   // Omit a compiler warning.
 173   return 0;
 174 }
 175