src/AIModel/AIFlightPlanCreateCruise.cxx

   1 /******************************************************************************
   2  * AIFlightPlanCreateCruise.cxx
   3  * Written by Durk Talsma, started February, 2006.
   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., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301, USA.
  18  *
  19  *
  20  **************************************************************************/
  21
  22 #ifdef HAVE_CONFIG_H
  23 #  include <config.h>
  24 #endif
  25
  26 #include <fstream>
  27 #include <iostream>
  28
  29 #include <Airports/airport.hxx>
  30 #include <Airports/runways.hxx>
  31 #include <Airports/dynamics.hxx>
  32
  33 #include <Environment/environment_mgr.hxx>
  34 #include <Environment/environment.hxx>
  35
  36 #include <Traffic/Schedule.hxx>
  37
  38 #include "AIFlightPlan.hxx"
  39 #include "AIAircraft.hxx"
  40 #include "performancedata.hxx"
  41
  42 using std::iostream;
  43 using std::string;
  44
  45 /*
  46 void FGAIFlightPlan::evaluateRoutePart(double deplat,
  47                                        double deplon,
  48                                        double arrlat,
  49                                        double arrlon)
  50 {
  51   // First do a prescan of all the waypoints that are within a reasonable distance of the
  52   // ideal route.
  53   intVec nodes;
  54   int tmpNode, prevNode;
  55
  56   SGGeoc dep(SGGeoc::fromDegM(deplon, deplat, 100.0));
  57   SGGeoc arr(SGGeoc::fromDegM(arrlon, arrlat, 100.0));
  58
  59   SGVec3d a = SGVec3d::fromGeoc(dep);
  60   SGVec3d nb = normalize(SGVec3d::fromGeoc(arr));
  61   SGVec3d na = normalize(a);
  62
  63   SGVec3d _cross = cross(nb, na);
  64
  65   double angle = acos(dot(na, nb));
  66   const double angleStep = 0.05 * SG_DEGREES_TO_RADIANS;
  67   tmpNode = 0;
  68   for (double ang = 0.0; ang < angle; ang += angleStep)
  69   {
  70       SGQuatd q = SGQuatd::fromAngleAxis(ang, _cross);
  71       SGGeod geod = SGGeod::fromCart(q.transform(a));
  72
  73       prevNode = tmpNode;
  74       tmpNode = globals->get_airwaynet()->findNearestNode(geod);
  75
  76       FGNode* node = globals->get_airwaynet()->findNode(tmpNode);
  77
  78       if ((tmpNode != prevNode) && (SGGeodesy::distanceM(geod, node->getPosition()) < 25000)) {
  79         nodes.push_back(tmpNode);
  80       }
  81     }
  82
  83     intVecIterator i = nodes.begin();
  84     intVecIterator j = nodes.end();
  85     while (i != nodes.end())
  86     {
  87                 j = nodes.end();
  88                 while (j != i)
  89                 {
  90                         j--;
  91                         FGAirRoute routePart = globals->get_airwaynet()->findShortestRoute(*i, *j);
  92                         if (!(routePart.empty()))
  93                         {
  94                                 airRoute.add(routePart);
  95                                 i = j;
  96                                 break;
  97                         }
  98                 }
  99                 i++;
 100         }
 101 }
 102
 103 */
 104 /*
 105 void FGAIFlightPlan::createCruise(bool firstFlight, FGAirport *dep,
 106                                   FGAirport *arr, double latitude,
 107                                   double longitude, double speed, double alt)
 108 {
 109   bool useInitialWayPoint = true;
 110   bool useCurrentWayPoint = false;
 111   double heading;
 112   double lat, lon, az;
 113    double lat2, lon2, az2;
 114    double azimuth;
 115   waypoint *wpt;
 116   waypoint *init_waypoint;
 117     intVec ids;
 118     char buffer[32];
 119     SGPath routefile = globals->get_fg_root();
 120     init_waypoint = new waypoint;
 121     init_waypoint->name      = "Initial waypoint";
 122     init_waypoint->latitude  = latitude;
 123     init_waypoint->longitude = longitude;;
 124     //wpt->altitude  = apt->getElevation(); // should maybe be tn->elev too
 125     init_waypoint->altitude  = alt;
 126     init_waypoint->speed     = 450; //speed;
 127     init_waypoint->crossat   = -10000;
 128     init_waypoint->gear_down = false;
 129     init_waypoint->flaps_down= false;
 130     init_waypoint->finished  = false;
 131     init_waypoint->on_ground = false;
 132     pushBackWaypoint(init_waypoint);
 133     routefile.append("Data/AI/FlightPlans");
 134     snprintf(buffer, 32, "%s-%s.txt",
 135              dep->getId().c_str(),
 136              arr->getId().c_str());
 137     routefile.append(buffer);
 138     cerr << "trying to read " << routefile.c_str()<< endl;
 139     //exit(1);
 140     if (routefile.exists())
 141       {
 142          sg_gzifstream in( routefile.str() );
 143         do {
 144           in >> route;
 145         } while (!(in.eof()));
 146       }
 147     else {
 148     //int runwayId = apt->getDynamics()->getGroundNetwork()->findNearestNode(lat2, lon2);
 149     //int startId = globals->get_airwaynet()->findNearestNode(dep->getLatitude(), dep->getLongitude());
 150     //int endId   = globals->get_airwaynet()->findNearestNode(arr->getLatitude(), arr->getLongitude());
 151     //FGAirRoute route;
 152     evaluateRoutePart(dep->getLatitude(), dep->getLongitude(),
 153                       arr->getLatitude(), arr->getLongitude());
 154     //exit(1);
 155     }
 156     route.first();
 157     int node;
 158     if (route.empty()) {
 159       // if no route could be found, create a direct gps route...
 160       cerr << "still no route found from " << dep->getName() << " to << " << arr->getName() <<endl;
 161
 162       //exit(1);
 163     } else {
 164       while(route.next(&node))
 165         {
 166           FGNode *fn = globals->get_airwaynet()->findNode(node);
 167           //cerr << "Checking status of each waypoint: " << fn->getIdent();
 168
 169           SGWayPoint first(init_waypoint->longitude,
 170                            init_waypoint->latitude,
 171                            alt);
 172           SGWayPoint curr (fn->getLongitude(),
 173                            fn->getLatitude(),
 174                            alt);
 175           SGWayPoint arr  (arr->getLongitude(),
 176                            arr->getLatitude(),
 177                            alt);
 178
 179           double crse, crsDiff;
 180           double dist;
 181           first.CourseAndDistance(arr,   &course, &distance);
 182           first.CourseAndDistance(curr, &crse, &dist);
 183
 184           dist *= SG_METER_TO_NM;
 185
 186           // We're only interested in the absolute value of crsDiff
 187           // wich should fall in the 0-180 deg range.
 188           crsDiff = fabs(crse-course);
 189           if (crsDiff > 180)
 190             crsDiff = 360-crsDiff;
 191           // These are the three conditions that we consider including
 192           // in our flight plan:
 193           // 1) current waypoint is less then 100 miles away OR
 194           // 2) curren waypoint is ahead of us, at any distance
 195           //cerr << " Distance : " << dist << " : Course diff " << crsDiff
 196           //     << " crs to dest : " << course
 197           //     << " crs to wpt  : " << crse;
 198           if ((dist > 20.0) && (crsDiff > 90.0))
 199             {
 200               //useWpt = false;
 201               // Once we start including waypoints, we have to continue, even though
 202               // one of the following way point would suffice.
 203               // so once is the useWpt flag is set to true, we cannot reset it to false.
 204               //cerr << " discarding " << endl;
 205               //   << ": Course difference = " << crsDiff
 206               //  << "Course = " << course
 207               // << "crse   = " << crse << endl;
 208             }
 209           else {
 210             //i = ids.end()-1;
 211             //cerr << " accepting " << endl;
 212
 213             //ids.pop_back();
 214             wpt = new waypoint;
 215             wpt->name      = "Airway"; // fixme: should be the name of the waypoint
 216             wpt->latitude  = fn->getLatitude();
 217             wpt->longitude = fn->getLongitude();
 218             //wpt->altitude  = apt->getElevation(); // should maybe be tn->elev too
 219             wpt->altitude  = alt;
 220             wpt->speed     = 450; //speed;
 221             wpt->crossat   = -10000;
 222             wpt->gear_down = false;
 223             wpt->flaps_down= false;
 224             wpt->finished  = false;
 225             wpt->on_ground = false;
 226             pushBackWaypoint(wpt);
 227           }
 228
 229           if (!(routefile.exists()))
 230             {
 231               route.first();
 232               fstream outf( routefile.c_str(), fstream::out );
 233               while (route.next(&node))
 234                 outf << node << endl;
 235             }
 236         }
 237     }
 238     arr->getDynamics()->getActiveRunway("com", 2, activeRunway);
 239     if (!(globals->get_runways()->search(arr->getId(),
 240                                          activeRunway,
 241                                          &rwy)))
 242   {
 243     cout << "Failed to find runway for " << arr->getId() << endl;
 244     // Hmm, how do we handle a potential error like this?
 245     exit(1);
 246   }
 247     //string test;
 248     //arr->getActiveRunway(string("com"), 1, test);
 249     //exit(1);
 250
 251     //cerr << "Altitude = " << alt << endl;
 252     //cerr << "Done" << endl;
 253     //if (arr->getId() == "EHAM")
 254     //  {
 255     //    cerr << "Creating cruise to EHAM " << latitude << " " << longitude << endl;
 256     //  }
 257     heading = rwy._heading;
 258     azimuth = heading + 180.0;
 259     while ( azimuth >= 360.0 ) { azimuth -= 360.0; }
 260
 261
 262     // Note: This places us at the location of the active
 263     // runway during initial cruise. This needs to be
 264     // fixed later.
 265     geo_direct_wgs_84 ( 0, rwy._lat, rwy._lon, azimuth,
 266                         110000,
 267                         &lat2, &lon2, &az2 );
 268     wpt = new waypoint;
 269     wpt->name      = "BOD"; //wpt_node->getStringValue("name", "END");
 270     wpt->latitude  = lat2;
 271     wpt->longitude = lon2;
 272     wpt->altitude  = alt;
 273     wpt->speed     = speed;
 274     wpt->crossat   = alt;
 275     wpt->gear_down = false;
 276     wpt->flaps_down= false;
 277     wpt->finished  = false;
 278     wpt->on_ground = false;
 279     pushBackWaypoint(wpt);
 280 }
 281 */
 282
 283
 284 /*******************************************************************
 285  * CreateCruise
 286  * initialize the Aircraft at the parking location
 287  *
 288  * Note that this is the original version that does not
 289  * do any dynamic route computation.
 290  ******************************************************************/
 291 bool FGAIFlightPlan::createCruise(FGAIAircraft *ac, bool firstFlight, FGAirport *dep,
 292                                   FGAirport *arr, double latitude,
 293                                   double longitude, double speed,
 294                                   double alt, const string& fltType)
 295 {
 296   double vCruise = ac->getPerformance()->vCruise();
 297   FGAIWaypoint *wpt;
 298   wpt = createInAir(ac, "Cruise", SGGeod::fromDeg(longitude, latitude), alt, vCruise);
 299   pushBackWaypoint(wpt);
 300
 301   const string& rwyClass = getRunwayClassFromTrafficType(fltType);
 302   double heading = ac->getTrafficRef()->getCourse();
 303   arr->getDynamics()->getActiveRunway(rwyClass, 2, activeRunway, heading);
 304   FGRunway* rwy = arr->getRunwayByIdent(activeRunway);
 305   assert( rwy != NULL );
 306   // begin descent 110km out
 307   SGGeod beginDescentPoint     = rwy->pointOnCenterline(0);
 308   SGGeod secondaryDescentPoint = rwy->pointOnCenterline(-10000);
 309
 310   wpt = createInAir(ac, "BOD", beginDescentPoint,  alt, vCruise);
 311   pushBackWaypoint(wpt);
 312   wpt = createInAir(ac, "BOD2", secondaryDescentPoint, alt, vCruise);
 313   pushBackWaypoint(wpt);
 314   return true;
 315 }