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