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