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