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