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