X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FAIModel%2FAIFlightPlan.cxx;h=60952d799621889b7ced9a87f5578853c5b9ec5b;hb=388a0a5b6af2b91f2ed36488177813d693ed374e;hp=289d850dd374ffa35f857c5d91b9c3c37b2d1640;hpb=22cd6dfb2a56f6198277076a3c00dc90c8a5c2b8;p=flightgear.git

diff --git a/src/AIModel/AIFlightPlan.cxx b/src/AIModel/AIFlightPlan.cxx
index 289d850dd..60952d799 100644
--- a/src/AIModel/AIFlightPlan.cxx
+++ b/src/AIModel/AIFlightPlan.cxx
@@ -14,36 +14,63 @@
 //
 // You should have received a copy of the GNU General Public License
 // along with this program; if not, write to the Free Software
-// Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
 
+#ifdef HAVE_CONFIG_H
+#  include <config.h>
+#endif
+
+#include <iostream>
 
-#include "AIFlightPlan.hxx"
 #include <simgear/misc/sg_path.hxx>
 #include <simgear/debug/logstream.hxx>
+#include <simgear/route/waypoint.hxx>
+#include <simgear/math/sg_geodesy.hxx>
 #include <simgear/structure/exception.hxx>
 #include <simgear/constants.h>
-#ifdef __BORLANDC__
-#  define exception c_exception
-#endif
 #include <simgear/props/props.hxx>
+#include <simgear/props/props_io.hxx>
+
 #include <Main/globals.hxx>
 #include <Main/fg_props.hxx>
+#include <Main/fg_init.hxx>
+#include <Airports/simple.hxx>
+#include <Airports/runways.hxx>
+#include <Airports/groundnetwork.hxx>
+
+#include <Environment/environment_mgr.hxx>
+#include <Environment/environment.hxx>
 
+#include "AIFlightPlan.hxx"
+#include "AIAircraft.hxx"
 
-FGAIFlightPlan::FGAIFlightPlan(string filename)
+using std::cerr;
+
+FGAIFlightPlan::FGAIFlightPlan() 
+{
+   sid = 0;
+}
+
+FGAIFlightPlan::FGAIFlightPlan(const string& filename)
 {
   int i;
+  sid = 0;
+  start_time = 0;
+  leg = 10;
+  gateId = 0;
+  taxiRoute = 0;
   SGPath path( globals->get_fg_root() );
-  path.append( ("/Data/AI/FlightPlans/" + filename).c_str() );
+  path.append( ("/AI/FlightPlans/" + filename).c_str() );
   SGPropertyNode root;
+  repeat = false;
 
   try {
       readProperties(path.str(), &root);
-  } catch (const sg_exception &e) {
+  } catch (const sg_exception &) {
       SG_LOG(SG_GENERAL, SG_ALERT,
-       "Error reading AI flight plan: ");
-       cout << path.str() << endl;
-      return;
+       "Error reading AI flight plan: " << path.str());
+       // cout << path.str() << endl;
+     return;
   }
 
   SGPropertyNode * node = root.getNode("flightplan");
@@ -60,6 +87,8 @@ FGAIFlightPlan::FGAIFlightPlan(string filename)
      wpt->gear_down = wpt_node->getBoolValue("gear-down", false);
      wpt->flaps_down= wpt_node->getBoolValue("flaps-down", false);
      wpt->on_ground = wpt_node->getBoolValue("on-ground", false);
+     wpt->time_sec   = wpt_node->getDoubleValue("time-sec", 0);
+     wpt->time       = wpt_node->getStringValue("time", "");
 
      if (wpt->name == "END") wpt->finished = true;
      else wpt->finished = false;
@@ -72,14 +101,216 @@ FGAIFlightPlan::FGAIFlightPlan(string filename)
 }
 
 
+// This is a modified version of the constructor,
+// Which not only reads the waypoints from a 
+// Flight plan file, but also adds the current
+// Position computed by the traffic manager, as well
+// as setting speeds and altitude computed by the
+// traffic manager. 
+FGAIFlightPlan::FGAIFlightPlan(FGAIAircraft *ac,
+                               const std::string& p,
+			       double course,
+			       time_t start,
+			       FGAirport *dep,
+			       FGAirport *arr,
+			       bool firstLeg,
+			       double radius,
+                               double alt,
+                               double lat,
+                               double lon,
+                               double speed,
+			       const string& fltType,
+			       const string& acType,
+			       const string& airline)
+{
+  sid = 0;
+  repeat = false;
+  leg = 10;
+  gateId=0;
+  taxiRoute = 0;
+  start_time = start;
+  bool useInitialWayPoint = true;
+  bool useCurrentWayPoint = false;
+  SGPath path( globals->get_fg_root() );
+  path.append( "/AI/FlightPlans" );
+  path.append( p );
+  
+  SGPropertyNode root;
+  
+  // This is a bit of a hack:
+  // Normally the value of course will be used to evaluate whether
+  // or not a waypoint will be used for midair initialization of 
+  // an AI aircraft. However, if a course value of 999 will be passed
+  // when an update request is received, which will by definition always be
+  // on the ground and should include all waypoints.
+  if (course == 999) 
+    {
+      useInitialWayPoint = false;
+      useCurrentWayPoint = true;
+    }
+
+  if (path.exists()) 
+    {
+      try 
+	{
+	  readProperties(path.str(), &root);
+	  
+	  SGPropertyNode * node = root.getNode("flightplan");
+	  
+	  //waypoints.push_back( init_waypoint );
+	  for (int i = 0; i < node->nChildren(); i++) { 
+	    //cout << "Reading waypoint " << i << endl;
+	    waypoint* wpt = new waypoint;
+	    SGPropertyNode * wpt_node = node->getChild(i);
+	    wpt->name      = wpt_node->getStringValue("name", "END");
+	    wpt->latitude  = wpt_node->getDoubleValue("lat", 0);
+	    wpt->longitude = wpt_node->getDoubleValue("lon", 0);
+	    wpt->altitude  = wpt_node->getDoubleValue("alt", 0);
+	    wpt->speed     = wpt_node->getDoubleValue("ktas", 0);
+	    //wpt->speed     = speed;
+	    wpt->crossat   = wpt_node->getDoubleValue("crossat", -10000);
+	    wpt->gear_down = wpt_node->getBoolValue("gear-down", false);
+	    wpt->flaps_down= wpt_node->getBoolValue("flaps-down", false);
+	    
+	    if (wpt->name == "END") wpt->finished = true;
+	    else wpt->finished = false;
+	    waypoints.push_back(wpt);
+	  } // of node loop
+	} catch (const sg_exception &e) {
+      SG_LOG(SG_GENERAL, SG_WARN, "Error reading AI flight plan: " << 
+        e.getMessage() << " from " << e.getOrigin());
+    }
+  } else {
+      // cout << path.str() << endl;
+      // cout << "Trying to create this plan dynamically" << endl;
+      // cout << "Route from " << dep->id << " to " << arr->id << endl;
+      time_t now = time(NULL) + fgGetLong("/sim/time/warp");
+      time_t timeDiff = now-start; 
+      leg = 1;
+      /*
+      if ((timeDiff > 300) && (timeDiff < 1200))
+	leg = 2;
+      else if ((timeDiff >= 1200) && (timeDiff < 1500))
+	leg = 3;
+      else if ((timeDiff >= 1500) && (timeDiff < 2000))
+	leg = 4;
+      else if (timeDiff >= 2000)
+	leg = 5;
+      */
+      if (timeDiff >= 2000)
+          leg = 5;
+
+      SG_LOG(SG_GENERAL, SG_INFO, "Route from " << dep->getId() << " to " << arr->getId() << ". Set leg to : " << leg << " " << ac->getTrafficRef()->getCallSign());
+      wpt_iterator = waypoints.begin();
+      create(ac, dep,arr, leg, alt, speed, lat, lon,
+	     firstLeg, radius, fltType, acType, airline);
+      wpt_iterator = waypoints.begin();
+      //cerr << "after create: " << (*wpt_iterator)->name << endl;
+      //leg++;
+      // Now that we have dynamically created a flight plan,
+      // we need to add some code that pops any waypoints already past.
+      //return;
+    }
+  /*
+    waypoint* init_waypoint   = new waypoint;
+    init_waypoint->name       = string("initial position");
+    init_waypoint->latitude   = entity->latitude;
+    init_waypoint->longitude  = entity->longitude;
+    init_waypoint->altitude   = entity->altitude;
+    init_waypoint->speed      = entity->speed;
+    init_waypoint->crossat    = - 10000;
+    init_waypoint->gear_down  = false;
+    init_waypoint->flaps_down = false;
+    init_waypoint->finished   = false;
+    
+    wpt_vector_iterator i = waypoints.begin();
+    while (i != waypoints.end())
+    {
+      //cerr << "Checking status of each waypoint: " << (*i)->name << endl;
+       SGWayPoint first(init_waypoint->longitude, 
+		       init_waypoint->latitude, 
+		       init_waypoint->altitude);
+      SGWayPoint curr ((*i)->longitude, 
+		       (*i)->latitude, 
+		       (*i)->altitude);
+      double crse, crsDiff;
+      double dist;
+      curr.CourseAndDistance(first, &crse, &dist);
+      
+      dist *= SG_METER_TO_NM;
+      
+      // We're only interested in the absolute value of crsDiff
+      // wich should fall in the 0-180 deg range.
+      crsDiff = fabs(crse-course);
+      if (crsDiff > 180)
+	crsDiff = 360-crsDiff;
+      // These are the three conditions that we consider including
+      // in our flight plan:
+      // 1) current waypoint is less then 100 miles away OR
+      // 2) curren waypoint is ahead of us, at any distance
+     
+      if ((dist > 20.0) && (crsDiff > 90.0) && ((*i)->name != string ("EOF")))
+	{
+	  //useWpt = false;
+	  // Once we start including waypoints, we have to continue, even though
+	  // one of the following way point would suffice. 
+	  // so once is the useWpt flag is set to true, we cannot reset it to false.
+	  //cerr << "Discarding waypoint: " << (*i)->name 
+	  //   << ": Course difference = " << crsDiff
+	  //  << "Course = " << course
+	  // << "crse   = " << crse << endl;
+	}
+      else
+	useCurrentWayPoint = true;
+      
+      if (useCurrentWayPoint)
+	{
+	  if ((dist > 100.0) && (useInitialWayPoint))
+	    {
+	      //waypoints.push_back(init_waypoint);;
+	      waypoints.insert(i, init_waypoint);
+	      //cerr << "Using waypoint : " << init_waypoint->name <<  endl;
+	    }
+	  //if (useInitialWayPoint)
+	  // {
+	  //    (*i)->speed = dist; // A hack
+	  //  }
+	  //waypoints.push_back( wpt );
+	  //cerr << "Using waypoint : " << (*i)->name 
+	  //  << ": course diff : " << crsDiff 
+	  //   << "Course = " << course
+	  //   << "crse   = " << crse << endl
+	  //    << "distance      : " << dist << endl;
+	  useInitialWayPoint = false;
+	  i++;
+	}
+      else 
+	{
+	  //delete wpt;
+	  delete *(i);
+	  i = waypoints.erase(i);
+	  }
+	  
+	}
+  */
+  //for (i = waypoints.begin(); i != waypoints.end(); i++)
+  //  cerr << "Using waypoint : " << (*i)->name << endl;
+  //wpt_iterator = waypoints.begin();
+  //cout << waypoints.size() << " waypoints read." << endl;
+}
+
+
+
+
 FGAIFlightPlan::~FGAIFlightPlan()
 {
-  waypoints.clear();
+  deleteWaypoints();
+  delete taxiRoute;
 }
 
 
-FGAIFlightPlan::waypoint*
-FGAIFlightPlan::getPreviousWaypoint( void )
+FGAIFlightPlan::waypoint* const
+FGAIFlightPlan::getPreviousWaypoint( void ) const
 {
   if (wpt_iterator == waypoints.begin()) {
     return 0;
@@ -89,16 +320,18 @@ FGAIFlightPlan::getPreviousWaypoint( void )
   }
 }
 
-FGAIFlightPlan::waypoint*
-FGAIFlightPlan::getCurrentWaypoint( void )
+FGAIFlightPlan::waypoint* const
+FGAIFlightPlan::getCurrentWaypoint( void ) const
 {
   return *wpt_iterator;
 }
 
-FGAIFlightPlan::waypoint*
-FGAIFlightPlan::getNextWaypoint( void )
+FGAIFlightPlan::waypoint* const
+FGAIFlightPlan::getNextWaypoint( void ) const
 {
-  if (wpt_iterator == waypoints.end()) {
+  wpt_vector_iterator i = waypoints.end();
+  i--;  // end() points to one element after the last one. 
+  if (wpt_iterator == i) {
     return 0;
   } else {
     wpt_vector_iterator next = wpt_iterator;
@@ -106,31 +339,88 @@ FGAIFlightPlan::getNextWaypoint( void )
   }
 }
 
-void FGAIFlightPlan::IncrementWaypoint( void )
+void FGAIFlightPlan::IncrementWaypoint(bool eraseWaypoints )
 {
-  wpt_iterator++;
+  if (eraseWaypoints)
+    {
+      if (wpt_iterator == waypoints.begin())
+	wpt_iterator++;
+      else
+	{
+	  delete *(waypoints.begin());
+	  waypoints.erase(waypoints.begin());
+	  wpt_iterator = waypoints.begin();
+	  wpt_iterator++;
+	}
+    }
+  else
+    wpt_iterator++;
+
 }
 
+void FGAIFlightPlan::DecrementWaypoint(bool eraseWaypoints )
+{
+    if (eraseWaypoints)
+    {
+        if (wpt_iterator == waypoints.end())
+            wpt_iterator--;
+        else
+        {
+            delete *(waypoints.end());
+            waypoints.erase(waypoints.end());
+            wpt_iterator = waypoints.end();
+            wpt_iterator--;
+        }
+    }
+    else
+        wpt_iterator--;
+
+}
+
+
 // gives distance in feet from a position to a waypoint
-double FGAIFlightPlan::getDistanceToGo(double lat, double lon, waypoint* wp){
-   // get size of a degree at the present latitude
-   // this won't work over large distances
-   double ft_per_deg_lat = 366468.96 - 3717.12 * cos(lat / SG_RADIANS_TO_DEGREES);
-   double ft_per_deg_lon = 365228.16 * cos(lat / SG_RADIANS_TO_DEGREES);
-   double lat_diff_ft = fabs(wp->latitude - lat) * ft_per_deg_lat;
-   double lon_diff_ft = fabs(wp->longitude - lon) * ft_per_deg_lon;
-   return sqrt((lat_diff_ft * lat_diff_ft) + (lon_diff_ft * lon_diff_ft));
+double FGAIFlightPlan::getDistanceToGo(double lat, double lon, waypoint* wp) const{
+  return SGGeodesy::distanceM(SGGeod::fromDeg(lon, lat), 
+      SGGeod::fromDeg(wp->longitude, wp->latitude));
 }
 
 // sets distance in feet from a lead point to the current waypoint
 void FGAIFlightPlan::setLeadDistance(double speed, double bearing, 
                                      waypoint* current, waypoint* next){
-  double turn_radius = 0.1911 * speed * speed; // an estimate for 25 degrees bank
+  double turn_radius;
+  // Handle Ground steering
+  // At a turn rate of 30 degrees per second, it takes 12 seconds to do a full 360 degree turn
+  // So, to get an estimate of the turn radius, calculate the cicumference of the circle
+  // we travel on. Get the turn radius by dividing by PI (*2).
+  if (speed < 0.5) {
+        lead_distance = 0.5;
+        return;
+  }
+  if (speed < 25) {
+       turn_radius = ((360/30)*fabs(speed)) / (2*M_PI);
+  } else 
+      turn_radius = 0.1911 * speed * speed; // an estimate for 25 degrees bank
+
   double inbound = bearing;
   double outbound = getBearing(current, next);
-  double diff = fabs(inbound - outbound);
-  if (diff > 180.0) diff = 360.0 - diff;
-  lead_distance = turn_radius * sin(diff * SG_DEGREES_TO_RADIANS); 
+  leadInAngle = fabs(inbound - outbound);
+  if (leadInAngle > 180.0) 
+    leadInAngle = 360.0 - leadInAngle;
+  //if (leadInAngle < 30.0) // To prevent lead_dist from getting so small it is skipped 
+  //  leadInAngle = 30.0;
+  
+  //lead_distance = turn_radius * sin(leadInAngle * SG_DEGREES_TO_RADIANS); 
+  lead_distance = turn_radius * tan((leadInAngle * SG_DEGREES_TO_RADIANS)/2);
+  if ((lead_distance > (3*turn_radius)) && (current->on_ground == false)) {
+      // cerr << "Warning: Lead-in distance is large. Inbound = " << inbound
+      //      << ". Outbound = " << outbound << ". Lead in angle = " << leadInAngle  << ". Turn radius = " << turn_radius << endl;
+       lead_distance = 3 * turn_radius;
+       return;
+  }
+  if ((leadInAngle > 90) && (current->on_ground == true)) {
+      lead_distance = turn_radius * tan((90 * SG_DEGREES_TO_RADIANS)/2);
+      return;
+  }
 }
 
 void FGAIFlightPlan::setLeadDistance(double distance_ft){
@@ -138,38 +428,68 @@ void FGAIFlightPlan::setLeadDistance(double distance_ft){
 }
 
 
-double FGAIFlightPlan::getBearing(waypoint* first, waypoint* second){
+double FGAIFlightPlan::getBearing(waypoint* first, waypoint* second) const{
   return getBearing(first->latitude, first->longitude, second);
 }
 
 
-double FGAIFlightPlan::getBearing(double lat, double lon, waypoint* wp){
-  double latd = lat;
-  double lond = lon;
-  double latt = wp->latitude;
-  double lont = wp->longitude;
-  double ft_per_deg_lat = 366468.96 - 3717.12 * cos(lat/SG_RADIANS_TO_DEGREES);
-  double ft_per_deg_lon = 365228.16 * cos(lat/SG_RADIANS_TO_DEGREES);
+double FGAIFlightPlan::getBearing(double lat, double lon, waypoint* wp) const{
+  return SGGeodesy::courseDeg(SGGeod::fromDeg(lon, lat), 
+      SGGeod::fromDeg(wp->longitude, wp->latitude));
+}
 
-  if (lond < 0.0) lond+=360.0;
-  if (lont < 0.0) lont+=360.0;
-  latd+=90.0;
-  latt+=90.0;
+void FGAIFlightPlan::deleteWaypoints()
+{
+  for (wpt_vector_iterator i = waypoints.begin(); i != waypoints.end();i++)
+    delete (*i);
+  waypoints.clear();
+}
+
+// Delete all waypoints except the last, 
+// which we will recycle as the first waypoint in the next leg;
+void FGAIFlightPlan::resetWaypoints()
+{
+  if (waypoints.begin() == waypoints.end())
+    return;
+  else
+    {
+      waypoint *wpt = new waypoint;
+      wpt_vector_iterator i = waypoints.end();
+      i--;
+      wpt->name      = (*i)->name;
+      wpt->latitude  = (*i)->latitude;
+      wpt->longitude =  (*i)->longitude;
+      wpt->altitude  =  (*i)->altitude;
+      wpt->speed     =  (*i)->speed;
+      wpt->crossat   =  (*i)->crossat;
+      wpt->gear_down =  (*i)->gear_down;
+      wpt->flaps_down=  (*i)->flaps_down;
+      wpt->finished  = false;
+      wpt->on_ground =  (*i)->on_ground;
+      //cerr << "Recycling waypoint " << wpt->name << endl;
+      deleteWaypoints();
+      waypoints.push_back(wpt);
+    }
+}
 
-  double lat_diff = (latt - latd) * ft_per_deg_lat;
-  double lon_diff = (lont - lond) * ft_per_deg_lon;
-  double angle = atan(fabs(lat_diff / lon_diff)) * SG_RADIANS_TO_DEGREES;
+// Start flightplan over from the beginning
+void FGAIFlightPlan::restart()
+{
+  wpt_iterator = waypoints.begin();
+}
 
-  bool southerly = true;
-  if (latt > latd) southerly = false;
-  bool easterly = false;
-  if (lont > lond) easterly = true;
-  if (southerly && easterly) return 90.0 + angle;
-  if (!southerly && easterly) return 90.0 - angle;
-  if (southerly && !easterly) return 270.0 - angle;
-  if (!southerly && !easterly) return 270.0 + angle; 
 
-  // Omit a compiler warning.
-  return 0;
+void FGAIFlightPlan::deleteTaxiRoute() 
+{
+  delete taxiRoute;
+  taxiRoute = 0;
 }
 
+
+int FGAIFlightPlan::getRouteIndex(int i) {
+  if ((i > 0) && (i < (int)waypoints.size())) {
+    return waypoints[i]->routeIndex;
+  }
+  else
+    return 0;
+}