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
  63      if (wpt->name == "END") wpt->finished = true;
  64      else wpt->finished = false;
  65
  66      waypoints.push_back( wpt );
  67    }
  68
  69   wpt_iterator = waypoints.begin();
  70   //cout << waypoints.size() << " waypoints read." << endl;
  71 }
  72
  73
  74 FGAIFlightPlan::~FGAIFlightPlan()
  75 {
  76   waypoints.clear();
  77 }
  78
  79
  80 FGAIFlightPlan::waypoint*
  81 FGAIFlightPlan::getPreviousWaypoint( void )
  82 {
  83   if (wpt_iterator == waypoints.begin()) {
  84     return 0;
  85   } else {
  86     wpt_vector_iterator prev = wpt_iterator;
  87     return *(--prev);
  88   }
  89 }
  90
  91 FGAIFlightPlan::waypoint*
  92 FGAIFlightPlan::getCurrentWaypoint( void )
  93 {
  94   return *wpt_iterator;
  95 }
  96
  97 FGAIFlightPlan::waypoint*
  98 FGAIFlightPlan::getNextWaypoint( void )
  99 {
 100   if (wpt_iterator == waypoints.end()) {
 101     return 0;
 102   } else {
 103     wpt_vector_iterator next = wpt_iterator;
 104     return *(++next);
 105   }
 106 }
 107
 108 void FGAIFlightPlan::IncrementWaypoint( void )
 109 {
 110   wpt_iterator++;
 111 }
 112
 113 // gives distance in feet from a position to a waypoint
 114 double FGAIFlightPlan::getDistanceToGo(double lat, double lon, waypoint* wp){
 115    // get size of a degree at the present latitude
 116    // this won't work over large distances
 117    double ft_per_deg_lat = 366468.96 - 3717.12 * cos(lat / SG_RADIANS_TO_DEGREES);
 118    double ft_per_deg_lon = 365228.16 * cos(lat / SG_RADIANS_TO_DEGREES);
 119    double lat_diff_ft = fabs(wp->latitude - lat) * ft_per_deg_lat;
 120    double lon_diff_ft = fabs(wp->longitude - lon) * ft_per_deg_lon;
 121    return sqrt((lat_diff_ft * lat_diff_ft) + (lon_diff_ft * lon_diff_ft));
 122 }
 123
 124 // sets distance in feet from a lead point to the current waypoint
 125 void FGAIFlightPlan::setLeadDistance(double speed, double bearing,
 126                                      waypoint* current, waypoint* next){
 127   double turn_radius = 0.1911 * speed * speed; // an estimate for 25 degrees bank
 128   double inbound = bearing;
 129   double outbound = getBearing(current, next);
 130   double diff = fabs(inbound - outbound);
 131   if (diff > 180.0) diff = 360.0 - diff;
 132   lead_distance = turn_radius * sin(diff * SG_DEGREES_TO_RADIANS);
 133 }
 134
 135 void FGAIFlightPlan::setLeadDistance(double distance_ft){
 136   lead_distance = distance_ft;
 137 }
 138
 139
 140 double FGAIFlightPlan::getBearing(waypoint* first, waypoint* second){
 141   return getBearing(first->latitude, first->longitude, second);
 142 }
 143
 144
 145 double FGAIFlightPlan::getBearing(double lat, double lon, waypoint* wp){
 146   double latd = lat;
 147   double lond = lon;
 148   double latt = wp->latitude;
 149   double lont = wp->longitude;
 150   double ft_per_deg_lat = 366468.96 - 3717.12 * cos(lat/SG_RADIANS_TO_DEGREES);
 151   double ft_per_deg_lon = 365228.16 * cos(lat/SG_RADIANS_TO_DEGREES);
 152
 153   if (lond < 0.0) lond+=360.0;
 154   if (lont < 0.0) lont+=360.0;
 155   latd+=90.0;
 156   latt+=90.0;
 157
 158   double lat_diff = (latt - latd) * ft_per_deg_lat;
 159   double lon_diff = (lont - lond) * ft_per_deg_lon;
 160   double angle = atan(fabs(lat_diff / lon_diff)) * SG_RADIANS_TO_DEGREES;
 161
 162   bool southerly = true;
 163   if (latt > latd) southerly = false;
 164   bool easterly = false;
 165   if (lont > lond) easterly = true;
 166   if (southerly && easterly) return 90.0 + angle;
 167   if (!southerly && easterly) return 90.0 - angle;
 168   if (southerly && !easterly) return 270.0 - angle;
 169   if (!southerly && !easterly) return 270.0 + angle;
 170
 171   // Omit a compiler warning.
 172   return 0;
 173 }
 174