AI Modifications: 1) Added a patch that takes ground speed into consideration

using Curt's new speed adjustment code. 2) Separated the function
FlightPlanCreateCruise() into a new source file in preparation of a more
elaborate airway following scheme.

src/AIModel/AIAircraft.cxx

@@ -193,7 +193,6 @@ void FGAIAircraft::Run(double dt) {
         // from control properties.  These default to the initial
         // settings in the config file, but can be changed "on the
         // fly".
-
         string lat_mode = props->getStringValue("controls/flight/lateral-mode");
         if ( lat_mode == "roll" ) {
             double angle
@@ -233,18 +232,29 @@ void FGAIAircraft::Run(double dt) {
     } else {
         speed_diff = groundTargetSpeed - speed;
     }
+
     if (speed_diff > 0.0) {
         speed += performance->accel * dt;
-        if ( speed > tgt_speed ) speed = tgt_speed; // don't overshoot
+	if (no_roll) { // apply overshoot correction
+	  if ( speed > groundTargetSpeed ) speed = groundTargetSpeed;
+	}else {
+	  if ( speed > tgt_speed ) speed = tgt_speed;
+	}
     } else if (speed_diff < 0.0) {
         if (no_roll) {
             // on ground (aircraft can't roll)
+	    // deceleration performance is better due to wheel brakes.
             speed -= performance->decel * dt * 3;
         } else {
             speed -= performance->decel * dt;
         }
-        if ( speed < tgt_speed ) speed = tgt_speed; // don't overshoot
+	if (no_roll) { // apply overshoot correction
+        if (speed < groundTargetSpeed ) speed = groundTargetSpeed;
+	} else { 
+	  if ( speed < tgt_speed ) speed = tgt_speed;
 	}
+    }
+    
     
     // convert speed to degrees per second
     speed_north_deg_sec = cos( hdg * SGD_DEGREES_TO_RADIANS )

src/AIModel/AIFlightPlan.cxx

@@ -110,6 +110,7 @@ FGAIFlightPlan::FGAIFlightPlan(const std::string& p,
 			       const string& acType,
 			       const string& airline)
 {
+  repeat = false;
   leg = 10;
   gateId=0;
   start_time = start;
@@ -118,6 +119,7 @@ FGAIFlightPlan::FGAIFlightPlan(const std::string& p,
   SGPath path( globals->get_fg_root() );
   path.append( "/AI/FlightPlans" );
   path.append( p );
+  
   SGPropertyNode root;
   
   // This is a bit of a hack:

src/AIModel/AIFlightPlan.hxx

@@ -14,7 +14,7 @@
 //
 // 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., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+// Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 
 #ifndef _FG_AIFLIGHTPLAN_HXX
 #define _FG_AIFLIGHTPLAN_HXX
@@ -25,6 +25,7 @@
 
 #include <Airports/simple.hxx>
 #include <Airports/runways.hxx>
+#include <Navaids/awynet.hxx>
 
 #include "AIBase.hxx"
 
@@ -93,6 +94,8 @@ public:
   bool getRepeat(void) const { return repeat; };
   void restart(void);
   
+  int getNrOfWayPoints() { return waypoints.end() - waypoints.begin(); };
+
 private:
   FGRunway rwy;
   typedef vector <waypoint*> wpt_vector_type;
@@ -109,6 +112,16 @@ private:
   int leg;
   int gateId;
   string activeRunway;
+  FGAirRoute route;
+
+
+  Point3D temp;
+  sgdVec3 a, b, cross;
+  sgdVec3 newPos;
+  sgdMat4 matrix;
+  double angle;
+  double midlat, midlon;
+  double course, distance;  
 
   void createPushBack(bool, FGAirport*, double, double, double, const string&, const string&, const string&);
   void createTaxi(bool, int, FGAirport *, double, double, double, const string&, const string&, const string&);
@@ -120,6 +133,9 @@ private:
   void createParking(FGAirport *, double radius);
   void deleteWaypoints(); 
   void resetWaypoints();
+
+  //void createCruiseFallback(bool, FGAirport*, FGAirport*, double, double, double, double);
+ void evaluateRoutePart(double deplat, double deplon, double arrlat, double arrlon);
 };    
 
 

src/AIModel/AIFlightPlanCreate.cxx

@@ -636,6 +636,7 @@ void FGAIFlightPlan::createClimb(bool firstFlight, FGAirport *apt, double speed,
   heading = rwy._heading;
   double azimuth = heading + 180.0;
   while ( azimuth >= 360.0 ) { azimuth -= 360.0; }
+  cerr << "Creating climb at : " << rwy._id << " " << rwy._rwy_no << endl;
   geo_direct_wgs_84 ( 0, rwy._lat, rwy._lon, heading, 
 		      10*SG_NM_TO_METER,
 		      &lat2, &lon2, &az2 );
@@ -671,69 +672,69 @@ void FGAIFlightPlan::createClimb(bool firstFlight, FGAirport *apt, double speed,
 }
 
 
-/*******************************************************************
+// /*******************************************************************
- * CreateCruise
+//  * CreateCruise
- * initialize the Aircraft at the parking location
+//  * initialize the Aircraft at the parking location
- ******************************************************************/
+//  ******************************************************************/
-void FGAIFlightPlan::createCruise(bool firstFlight, FGAirport *dep, 
+// void FGAIFlightPlan::createCruise(bool firstFlight, FGAirport *dep, 
-				  FGAirport *arr, double latitude, 
+// 				  FGAirport *arr, double latitude, 
-				  double longitude, double speed, 
+// 				  double longitude, double speed, 
-				  double alt)
+// 				  double alt)
-{
+// {
-  double wind_speed;
+//   double wind_speed;
-  double wind_heading;
+//   double wind_heading;
-  double heading;
+//   double heading;
-  double lat, lon, az;
+//   double lat, lon, az;
-  double lat2, lon2, az2;
+//   double lat2, lon2, az2;
-  double azimuth;
+//   double azimuth;
-  waypoint *wpt;
+//   waypoint *wpt;
 
-  wpt = new waypoint;
+//   wpt = new waypoint;
-  wpt->name      = "Cruise"; //wpt_node->getStringValue("name", "END");
+//   wpt->name      = "Cruise"; //wpt_node->getStringValue("name", "END");
-  wpt->latitude  = latitude;
+//   wpt->latitude  = latitude;
-  wpt->longitude = longitude;
+//   wpt->longitude = longitude;
-  wpt->altitude  = alt;
+//   wpt->altitude  = alt;
-  wpt->speed     = speed; 
+//   wpt->speed     = speed; 
-  wpt->crossat   = -10000;
+//   wpt->crossat   = -10000;
-  wpt->gear_down = false;
+//   wpt->gear_down = false;
-  wpt->flaps_down= false;
+//   wpt->flaps_down= false;
-  wpt->finished  = false;
+//   wpt->finished  = false;
-  wpt->on_ground = false;
+//   wpt->on_ground = false;
-  waypoints.push_back(wpt); 
+//   waypoints.push_back(wpt); 
   
  
-  // should be changed dynamically to allow "gen" and "mil"
+//   // should be changed dynamically to allow "gen" and "mil"
-  arr->getDynamics()->getActiveRunway("com", 2, activeRunway);
+//   arr->getDynamics()->getActiveRunway("com", 2, activeRunway);
-  if (!(globals->get_runways()->search(arr->getId(), 
+//   if (!(globals->get_runways()->search(arr->getId(), 
-				       activeRunway, 
+// 				       activeRunway, 
-				       &rwy)))
+// 				       &rwy)))
-    {
+//     {
-      SG_LOG(SG_INPUT, SG_ALERT, "Failed to find runway " << 
+//       SG_LOG(SG_INPUT, SG_ALERT, "Failed to find runway " << 
-	     activeRunway << 
+// 	     activeRunway << 
-	     " at airport     " << arr->getId());
+// 	     " at airport     " << arr->getId());
-      exit(1);
+//       exit(1);
-    }
+//     }
-  heading = rwy._heading;
+//   heading = rwy._heading;
-  azimuth = heading + 180.0;
+//   azimuth = heading + 180.0;
-  while ( azimuth >= 360.0 ) { azimuth -= 360.0; }
+//   while ( azimuth >= 360.0 ) { azimuth -= 360.0; }
   
   
-  geo_direct_wgs_84 ( 0, rwy._lat, rwy._lon, azimuth, 
+//   geo_direct_wgs_84 ( 0, rwy._lat, rwy._lon, azimuth, 
-		      110000,
+// 		      110000,
-		      &lat2, &lon2, &az2 );
+// 		      &lat2, &lon2, &az2 );
-  wpt = new waypoint;
+//   wpt = new waypoint;
-  wpt->name      = "BOD";
+//   wpt->name      = "BOD";
-  wpt->latitude  = lat2;
+//   wpt->latitude  = lat2;
-  wpt->longitude = lon2;
+//   wpt->longitude = lon2;
-  wpt->altitude  = alt;
+//   wpt->altitude  = alt;
-  wpt->speed     = speed; 
+//   wpt->speed     = speed; 
-  wpt->crossat   = alt;
+//   wpt->crossat   = alt;
-  wpt->gear_down = false;
+//   wpt->gear_down = false;
-  wpt->flaps_down= false;
+//   wpt->flaps_down= false;
-  wpt->finished  = false;
+//   wpt->finished  = false;
-  wpt->on_ground = false;
+//   wpt->on_ground = false;
-  waypoints.push_back(wpt); 
+//   waypoints.push_back(wpt); 
-}
+// }
 
 /*******************************************************************
  * CreateDecent

src/AIModel/AIFlightPlanCreateCruise.cxx

@@ -0,0 +1,377 @@
+/******************************************************************************
+ * AIFlightPlanCreateCruise.cxx
+ * Written by Durk Talsma, started February, 2006.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of the
+ * License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * 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.
+ *
+ *
+ **************************************************************************/
+#include <fstream>
+#include <iostream>
+#include "AIFlightPlan.hxx"
+#include <simgear/math/polar3d.hxx>
+#include <simgear/math/sg_geodesy.hxx>
+#include <simgear/route/waypoint.hxx>
+
+#include <Navaids/awynet.hxx>
+#include <Airports/runways.hxx>
+
+#include <Environment/environment_mgr.hxx>
+#include <Environment/environment.hxx>
+
+SG_USING_STD(iostream);
+
+void FGAIFlightPlan::evaluateRoutePart(double deplat,
+				       double deplon,
+				       double arrlat,
+				       double arrlon)
+{
+  // First do a prescan of all the waypoints that are within a reasonable distance of the
+  // ideal route.
+  intVec nodes;
+  int tmpNode, prevNode;
+
+
+  SGWayPoint first (deplon,
+		    deplat,
+		    100);
+  SGWayPoint sec (arrlon,
+		  arrlat,
+		  100);
+  first.CourseAndDistance(sec, &course, &distance);
+  distance *= SG_METER_TO_NM;
+  temp = sgPolarToCart3d(Point3D(deplon *
+				 SG_DEGREES_TO_RADIANS,
+				 deplat  *
+				 SG_DEGREES_TO_RADIANS,
+				 1.0));
+  a[0] = temp.x();
+  a[1] = temp.y();
+  a[2] = temp.z();
+
+  temp = sgPolarToCart3d(Point3D(arrlon *
+   				 SG_DEGREES_TO_RADIANS,
+   				 arrlat  *
+   				 SG_DEGREES_TO_RADIANS,
+   				 1.0));
+  b[0] = temp.x();
+  b[1] = temp.y();
+  b[2] = temp.z();
+  sgdNormaliseVec3(a);
+  sgdNormaliseVec3(b);
+  sgdVectorProductVec3(cross,b,a);
+
+  angle = sgACos(sgdScalarProductVec3(a,b));
+  tmpNode = 0;
+  for (double ang = 0.0; ang < angle; ang += 0.05)
+    {
+      //cerr << "Angle = " << ang << endl;
+      sgdMakeRotMat4(matrix, ang, cross);
+      for(int j = 0; j < 3; j++)
+	{
+	  newPos[j] =0.0;
+	  for (int k = 0; k<3; k++)
+	    {
+	      newPos[j] += matrix[j][k]*a[k];
+	    }
+	}
+      //cerr << "1"<< endl;
+      temp = sgCartToPolar3d(Point3D(newPos[0], newPos[1],newPos[2]));
+      midlat = temp.lat() * SG_RADIANS_TO_DEGREES;
+      midlon = temp.lon() * SG_RADIANS_TO_DEGREES;
+
+      prevNode = tmpNode;
+      tmpNode = globals->get_airwaynet()->findNearestNode(midlat, midlon);
+
+      double nodelat = globals->get_airwaynet()->findNode(tmpNode)->getLatitude  ();
+      double nodelon = globals->get_airwaynet()->findNode(tmpNode)->getLongitude ();
+      SGWayPoint curr(midlat,
+      	              midlon,
+      	              100);
+      SGWayPoint node(nodelat,
+      	              nodelon,
+      	              100);
+      curr.CourseAndDistance(node, &course, &distance);
+      if ((distance < 25000) && (tmpNode != prevNode))
+      	nodes.push_back(tmpNode);
+    }
+
+    intVecIterator i = nodes.begin();
+    intVecIterator j = nodes.end();
+    while (i != nodes.end())
+    {
+		j = nodes.end();
+		while (j != i)
+		{
+			j--;
+			FGAirRoute routePart = globals->get_airwaynet()->findShortestRoute(*i, *j);
+			if (!(routePart.empty()))
+			{
+				route.add(routePart);
+				i = j;
+				break;
+			}
+		}
+		i++;
+	}
+}
+
+
+/*
+void FGAIFlightPlan::createCruise(bool firstFlight, FGAirport *dep,
+				  FGAirport *arr, double latitude,
+				  double longitude, double speed, double alt)
+{
+  bool useInitialWayPoint = true;
+  bool useCurrentWayPoint = false;
+  double heading;
+  double lat, lon, az;
+   double lat2, lon2, az2;
+   double azimuth;
+  waypoint *wpt;
+  waypoint *init_waypoint;
+    intVec ids;
+    char buffer[32];
+    SGPath routefile = globals->get_fg_root();
+    init_waypoint = new waypoint;
+    init_waypoint->name      = "Initial waypoint";
+    init_waypoint->latitude  = latitude;
+    init_waypoint->longitude = longitude;;
+    //wpt->altitude  = apt->getElevation(); // should maybe be tn->elev too
+    init_waypoint->altitude  = alt;
+    init_waypoint->speed     = 450; //speed;
+    init_waypoint->crossat   = -10000;
+    init_waypoint->gear_down = false;
+    init_waypoint->flaps_down= false;
+    init_waypoint->finished  = false;
+    init_waypoint->on_ground = false;
+    waypoints.push_back(init_waypoint);
+    routefile.append("Data/AI/FlightPlans");
+    snprintf(buffer, 32, "%s-%s.txt",
+	     dep->getId().c_str(),
+	     arr->getId().c_str());
+    routefile.append(buffer);
+    cerr << "trying to read " << routefile.c_str()<< endl;
+    //exit(1);
+    if (routefile.exists())
+      {
+	 sg_gzifstream in( routefile.str() );
+	do {
+	  in >> route;
+	} while (!(in.eof()));
+      }
+    else {
+    //int runwayId = apt->getDynamics()->getGroundNetwork()->findNearestNode(lat2, lon2);
+    //int startId = globals->get_airwaynet()->findNearestNode(dep->getLatitude(), dep->getLongitude());
+    //int endId   = globals->get_airwaynet()->findNearestNode(arr->getLatitude(), arr->getLongitude());
+    //FGAirRoute route;
+    evaluateRoutePart(dep->getLatitude(), dep->getLongitude(),
+		      arr->getLatitude(), arr->getLongitude());
+    //exit(1);
+    }
+    route.first();
+    int node;
+    if (route.empty()) {
+      // if no route could be found, create a direct gps route...
+      cerr << "still no route found from " << dep->getName() << " to << " << arr->getName() <<endl;
+
+      //exit(1);
+    } else {
+      while(route.next(&node))
+	{
+	  FGNode *fn = globals->get_airwaynet()->findNode(node);
+	  //cerr << "Checking status of each waypoint: " << fn->getIdent();
+
+	  SGWayPoint first(init_waypoint->longitude,
+			   init_waypoint->latitude,
+			   alt);
+	  SGWayPoint curr (fn->getLongitude(),
+			   fn->getLatitude(),
+			   alt);
+	  SGWayPoint arr  (arr->getLongitude(),
+			   arr->getLatitude(),
+			   alt);
+	  
+	  double crse, crsDiff;
+	  double dist;
+	  first.CourseAndDistance(arr,   &course, &distance);
+	  first.CourseAndDistance(curr, &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
+	  //cerr << " Distance : " << dist << " : Course diff " << crsDiff 
+	  //     << " crs to dest : " << course
+	  //     << " crs to wpt  : " << crse;
+	  if ((dist > 20.0) && (crsDiff > 90.0))
+	    {
+	      //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 " << endl;
+	      //   << ": Course difference = " << crsDiff
+	      //  << "Course = " << course
+	      // << "crse   = " << crse << endl;
+	    }
+	  else {
+	    //i = ids.end()-1;
+	    //cerr << " accepting " << endl;
+
+	    //ids.pop_back();
+	    wpt = new waypoint;
+	    wpt->name      = "Airway"; // fixme: should be the name of the waypoint
+	    wpt->latitude  = fn->getLatitude();
+	    wpt->longitude = fn->getLongitude();
+	    //wpt->altitude  = apt->getElevation(); // should maybe be tn->elev too
+	    wpt->altitude  = alt;
+	    wpt->speed     = 450; //speed;
+	    wpt->crossat   = -10000;
+	    wpt->gear_down = false;
+	    wpt->flaps_down= false;
+	    wpt->finished  = false;
+	    wpt->on_ground = false;
+	    waypoints.push_back(wpt);
+	  }
+
+	  if (!(routefile.exists()))
+	    {
+	      route.first();
+	      fstream outf( routefile.c_str(), fstream::out );
+	      while (route.next(&node))
+		outf << node << endl;
+	    }
+	}
+    }
+    arr->getDynamics()->getActiveRunway("com", 2, activeRunway);
+    if (!(globals->get_runways()->search(arr->getId(),
+					 activeRunway,
+					 &rwy)))
+  {
+    cout << "Failed to find runway for " << arr->getId() << endl;
+    // Hmm, how do we handle a potential error like this?
+    exit(1);
+  }
+    //string test;
+    //arr->getActiveRunway(string("com"), 1, test);
+    //exit(1);
+
+    //cerr << "Altitude = " << alt << endl;
+    //cerr << "Done" << endl;
+    //if (arr->getId() == "EHAM")
+    //  {
+    //    cerr << "Creating cruise to EHAM " << latitude << " " << longitude << endl;
+    //  }
+    heading = rwy._heading;
+    azimuth = heading + 180.0;
+    while ( azimuth >= 360.0 ) { azimuth -= 360.0; }
+
+
+    // Note: This places us at the location of the active
+    // runway during initial cruise. This needs to be
+    // fixed later.
+    geo_direct_wgs_84 ( 0, rwy._lat, rwy._lon, azimuth,
+			110000,
+			&lat2, &lon2, &az2 );
+    wpt = new waypoint;
+    wpt->name      = "BOD"; //wpt_node->getStringValue("name", "END");
+    wpt->latitude  = lat2;
+    wpt->longitude = lon2;
+    wpt->altitude  = alt;
+    wpt->speed     = speed;
+    wpt->crossat   = alt;
+    wpt->gear_down = false;
+    wpt->flaps_down= false;
+    wpt->finished  = false;
+    wpt->on_ground = false;
+    waypoints.push_back(wpt);
+}
+*/
+
+
+/*******************************************************************
+ * CreateCruise
+ * initialize the Aircraft at the parking location
+ *
+ * Note that this is the original version that does not 
+ * do any dynamic route computation.
+ ******************************************************************/
+void FGAIFlightPlan::createCruise(bool firstFlight, FGAirport *dep, 
+				  FGAirport *arr, double latitude, 
+				  double longitude, double speed, 
+				  double alt)
+{
+  double wind_speed;
+  double wind_heading;
+  double heading;
+  double lat, lon, az;
+  double lat2, lon2, az2;
+  double azimuth;
+  waypoint *wpt;
+
+  wpt = new waypoint;
+  wpt->name      = "Cruise"; //wpt_node->getStringValue("name", "END");
+  wpt->latitude  = latitude;
+  wpt->longitude = longitude;
+  wpt->altitude  = alt;
+  wpt->speed     = speed; 
+  wpt->crossat   = -10000;
+  wpt->gear_down = false;
+  wpt->flaps_down= false;
+  wpt->finished  = false;
+  wpt->on_ground = false;
+  waypoints.push_back(wpt); 
+  
+ 
+  // should be changed dynamically to allow "gen" and "mil"
+  arr->getDynamics()->getActiveRunway("com", 2, activeRunway);
+  if (!(globals->get_runways()->search(arr->getId(), 
+				       activeRunway, 
+				       &rwy)))
+    {
+      SG_LOG(SG_INPUT, SG_ALERT, "Failed to find runway " << 
+	     activeRunway << 
+	     " at airport     " << arr->getId());
+      exit(1);
+    }
+  heading = rwy._heading;
+  azimuth = heading + 180.0;
+  while ( azimuth >= 360.0 ) { azimuth -= 360.0; }
+  
+  
+  geo_direct_wgs_84 ( 0, rwy._lat, rwy._lon, azimuth, 
+		      110000,
+		      &lat2, &lon2, &az2 );
+  wpt = new waypoint;
+  wpt->name      = "BOD";
+  wpt->latitude  = lat2;
+  wpt->longitude = lon2;
+  wpt->altitude  = alt;
+  wpt->speed     = speed; 
+  wpt->crossat   = alt;
+  wpt->gear_down = false;
+  wpt->flaps_down= false;
+  wpt->finished  = false;
+  wpt->on_ground = false;
+  waypoints.push_back(wpt); 
+}

src/AIModel/Makefile.am

@@ -10,7 +10,8 @@ libAIModel_a_SOURCES = \
 	AIBallistic.hxx AIBallistic.cxx \
 	AIStorm.hxx AIStorm.cxx \
 	AIThermal.hxx AIThermal.cxx \
-	AIFlightPlan.hxx AIFlightPlan.cxx AIFlightPlanCreate.cxx \
+	AIFlightPlan.hxx AIFlightPlan.cxx \
+	AIFlightPlanCreate.cxx AIFlightPlanCreateCruise.cxx \
         AICarrier.hxx AICarrier.cxx \
         AIStatic.hxx AIStatic.cxx