src/Traffic/Schedule.cxx

   1 /******************************************************************************
   2  * Schedule.cxx
   3  * Written by Durk Talsma, started May 5, 2004.
   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  *****************************************************************************/
  23 #include <stdlib.h>
  24 #include <time.h>
  25 #include <iostream>
  26 #include <fstream>
  27
  28
  29 #include <string>
  30 #include <vector>
  31 #include <algorithm>
  32
  33 #include <plib/sg.h>
  34
  35 #include <simgear/compiler.h>
  36 #include <simgear/math/polar3d.hxx>
  37 #include <simgear/math/sg_geodesy.hxx>
  38 #include <simgear/props/props.hxx>
  39 #include <simgear/route/waypoint.hxx>
  40 #include <simgear/structure/subsystem_mgr.hxx>
  41 #include <simgear/xml/easyxml.hxx>
  42
  43 #include <AIModel/AIFlightPlan.hxx>
  44 #include <AIModel/AIManager.hxx>
  45 #include <AIModel/AIAircraft.hxx>
  46 #include <Airports/simple.hxx>
  47 #include <Main/fg_init.hxx>   // That's pretty ugly, but I need fgFindAirportID
  48
  49
  50 #include "SchedFlight.hxx"
  51 #include "TrafficMgr.hxx"
  52
  53 SG_USING_STD( sort );
  54
  55 /******************************************************************************
  56  * the FGAISchedule class contains data members and code to maintain a
  57  * schedule of Flights for an articically controlled aircraft.
  58  *****************************************************************************/
  59 FGAISchedule::FGAISchedule()
  60 {
  61   firstRun     = true;
  62   AIManagerRef = 0;
  63
  64   heavy = false;
  65   lat = 0;
  66   lon = 0;
  67   radius = 0;
  68   groundOffset = 0;
  69   distanceToUser = 0;
  70 }
  71
  72 FGAISchedule::FGAISchedule(string    mdl,
  73                            string    liv,
  74                            string    reg,
  75                            bool      hvy,
  76                            string act,
  77                            string arln,
  78                            string mclass,
  79                            string fltpe,
  80                            double rad,
  81                            double grnd,
  82                            FGScheduledFlightVec flt)
  83 {
  84   modelPath    = mdl;
  85   livery       = liv;
  86   registration = reg;
  87   acType       = act;
  88   airline      = arln;
  89   m_class      = mclass;
  90   flightType   = fltpe;
  91   lat = 0;
  92   lon = 0;
  93   radius       = rad;
  94   groundOffset = grnd;
  95   distanceToUser = 0;
  96   heavy = hvy;
  97   for (FGScheduledFlightVecIterator i = flt.begin();
  98        i != flt.end();
  99        i++)
 100     flights.push_back(FGScheduledFlight((*i)));
 101   AIManagerRef = 0;
 102   firstRun = true;
 103 }
 104
 105 FGAISchedule::FGAISchedule(const FGAISchedule &other)
 106 {
 107   modelPath    = other.modelPath;
 108   livery       = other.livery;
 109   registration = other.registration;
 110   heavy        = other.heavy;
 111   flights      = other.flights;
 112   lat          = other.lat;
 113   lon          = other.lon;
 114   AIManagerRef = other.AIManagerRef;
 115   acType       = other.acType;
 116   airline      = other.airline;
 117   m_class      = other.m_class;
 118   firstRun     = other.firstRun;
 119   radius       = other.radius;
 120   groundOffset = other.groundOffset;
 121   flightType   = other.flightType;
 122   distanceToUser = other.distanceToUser;
 123 }
 124
 125
 126 FGAISchedule::~FGAISchedule()
 127 {
 128
 129 }
 130
 131 bool FGAISchedule::init()
 132 {
 133   //tm targetTimeDate;
 134   //SGTime* currTimeDate = globals->get_time_params();
 135
 136   //tm *temp = currTimeDate->getGmt();
 137   //char buffer[512];
 138   //sgTimeFormatTime(&targetTimeDate, buffer);
 139   //cout << "Scheduled Time " << buffer << endl;
 140   //cout << "Time :" << time(NULL) << " SGTime : " << sgTimeGetGMT(temp) << endl;
 141   for (FGScheduledFlightVecIterator i = flights.begin();
 142        i != flights.end();
 143        i++)
 144     {
 145       //i->adjustTime(now);
 146       if (!(i->initializeAirports()))
 147         return false;
 148     }
 149   //sort(flights.begin(), flights.end());
 150   // Since time isn't initialized yet when this function is called,
 151   // Find the closest possible airport.
 152   // This should give a reasonable initialization order.
 153   setClosestDistanceToUser();
 154   return true;
 155 }
 156
 157 bool FGAISchedule::update(time_t now)
 158 {
 159   FGAirport *dep;
 160   FGAirport *arr;
 161   sgdVec3 a, b, cross;
 162   sgdVec3 newPos;
 163   sgdMat4 matrix;
 164   double angle;
 165
 166   FGAIManager *aimgr;
 167   string airport;
 168
 169   double courseToUser,   courseToDest;
 170   double distanceToDest;
 171   double speed;
 172
 173   Point3D temp;
 174   time_t
 175     totalTimeEnroute,
 176     elapsedTimeEnroute,
 177     remainingTimeEnroute;
 178   double
 179     userLatitude,
 180     userLongitude;
 181
 182   if (fgGetBool("/sim/traffic-manager/enabled") == false)
 183     return true;
 184
 185   aimgr = (FGAIManager *) globals-> get_subsystem("ai_model");
 186   // Before the flight status of this traffic entity is updated
 187   // for the first time, we need to roll back it's flight schedule so
 188   // so that all the flights are centered around this simulated week's time
 189   // table. This is to avoid the situation where the first scheduled flight is
 190   // in the future, causing the traffic manager to not generate traffic until
 191   // simulated time has caught up with the real world time at initialization.
 192   // This is to counter a more general initialization bug, caused by the fact
 193   // that warp is not yet set when the  schedule is initialized. This is
 194   // especially a problem when using a negative time offset.
 195   // i.e let's say we specify FlightGear to run with --time-offset=-24:00:00.
 196   // Then the schedule will initialize using today, but we will fly yesterday.
 197   // Thus, it would take a whole day of simulation before the traffic manager
 198   // finally kicks in.
 199   if (firstRun)
 200     {
 201       for (FGScheduledFlightVecIterator i = flights.begin();
 202            i != flights.end();
 203            i++)
 204         {
 205           i->adjustTime(now);
 206         }
 207       firstRun = false;
 208     }
 209
 210   // Sort all the scheduled flights according to scheduled departure time.
 211   // Because this is done at every update, we only need to check the status
 212   // of the first listed flight.
 213   sort(flights.begin(), flights.end());
 214   FGScheduledFlightVecIterator i = flights.begin();
 215   if (AIManagerRef)
 216     {
 217       // Check if this aircraft has been released.
 218       FGTrafficManager *tmgr = (FGTrafficManager *) globals->get_subsystem("Traffic Manager");
 219       if (tmgr->isReleased(AIManagerRef))
 220         AIManagerRef = 0;
 221     }
 222
 223   if (!AIManagerRef)
 224     {
 225       userLatitude  = fgGetDouble("/position/latitude-deg");
 226       userLongitude = fgGetDouble("/position/longitude-deg");
 227
 228       //cerr << "Estimated minimum distance to user: " << distanceToUser << endl;
 229       // This flight entry is entirely in the past, do we need to
 230       // push it forward in time to the next scheduled departure.
 231       if ((i->getDepartureTime() < now) && (i->getArrivalTime() < now))
 232         {
 233           i->update();
 234           return true;
 235         }
 236
 237       // Departure time in the past and arrival time in the future.
 238       // This flight is in progress, so we need to calculate it's
 239       // approximate position and -if in range- create an AIAircraft
 240       // object for it.
 241       //if ((i->getDepartureTime() < now) && (i->getArrivalTime() > now))
 242
 243
 244       // Part of this flight is in the future.
 245       if (i->getArrivalTime() > now)
 246         {
 247           dep = i->getDepartureAirport();
 248           arr = i->getArrivalAirport  ();
 249           if (!(dep && arr))
 250             return false;
 251
 252           temp = sgPolarToCart3d(Point3D(dep->getLongitude() *
 253                                          SG_DEGREES_TO_RADIANS,
 254                                          dep->getLatitude()  *
 255                                          SG_DEGREES_TO_RADIANS,
 256                                          1.0));
 257           a[0] = temp.x();
 258           a[1] = temp.y();
 259           a[2] = temp.z();
 260
 261           temp = sgPolarToCart3d(Point3D(arr->getLongitude() *
 262                                          SG_DEGREES_TO_RADIANS,
 263                                          arr->getLatitude()  *
 264                                          SG_DEGREES_TO_RADIANS,
 265                                          1.0));
 266           b[0] = temp.x();
 267           b[1] = temp.y();
 268           b[2] = temp.z();
 269           sgdNormaliseVec3(a);
 270           sgdNormaliseVec3(b);
 271           sgdVectorProductVec3(cross,b,a);
 272
 273           angle = sgACos(sgdScalarProductVec3(a,b));
 274
 275           // Okay, at this point we have the angle between departure and
 276           // arrival airport, in degrees. From here we can interpolate the
 277           // position of the aircraft by calculating the ratio between
 278           // total time enroute and elapsed time enroute.
 279
 280           totalTimeEnroute     = i->getArrivalTime() - i->getDepartureTime();
 281           if (now > i->getDepartureTime())
 282             {
 283               //err << "Lat = " << lat << ", lon = " << lon << endl;
 284               //cerr << "Time diff: " << now-i->getDepartureTime() << endl;
 285               elapsedTimeEnroute   = now - i->getDepartureTime();
 286               remainingTimeEnroute = i->getArrivalTime()   - now;
 287             }
 288           else
 289             {
 290               lat = dep->getLatitude();
 291               lon = dep->getLongitude();
 292               elapsedTimeEnroute = 0;
 293               remainingTimeEnroute = totalTimeEnroute;
 294             }
 295
 296           angle *= ( (double) elapsedTimeEnroute/ (double) totalTimeEnroute);
 297
 298
 299           //cout << "a = " << a[0] << " " << a[1] << " " << a[2]
 300           //     << "b = " << b[0] << " " << b[1] << " " << b[2] << endl;
 301           sgdMakeRotMat4(matrix, angle, cross);
 302           for(int j = 0; j < 3; j++)
 303             {
 304               newPos[j] =0.0;
 305               for (int k = 0; k<3; k++)
 306                 {
 307                   newPos[j] += matrix[j][k]*a[k];
 308                 }
 309             }
 310
 311           temp = sgCartToPolar3d(Point3D(newPos[0], newPos[1],newPos[2]));
 312           if (now > i->getDepartureTime())
 313             {
 314               //cerr << "Lat = " << lat << ", lon = " << lon << endl;
 315               //cerr << "Time diff: " << now-i->getDepartureTime() << endl;
 316               lat = temp.lat() * SG_RADIANS_TO_DEGREES;
 317               lon = temp.lon() * SG_RADIANS_TO_DEGREES;
 318             }
 319           else
 320             {
 321               lat = dep->getLatitude();
 322               lon = dep->getLongitude();
 323             }
 324
 325
 326           SGWayPoint current  (lon,
 327                                lat,
 328                                i->getCruiseAlt());
 329           SGWayPoint user (   userLongitude,
 330                               userLatitude,
 331                               i->getCruiseAlt());
 332           SGWayPoint dest (   arr->getLongitude(),
 333                               arr->getLatitude(),
 334                               i->getCruiseAlt());
 335           // We really only need distance to user
 336           // and course to destination
 337           user.CourseAndDistance(current, &courseToUser, &distanceToUser);
 338           dest.CourseAndDistance(current, &courseToDest, &distanceToDest);
 339           speed =  (distanceToDest*SG_METER_TO_NM) /
 340             ((double) remainingTimeEnroute/3600.0);
 341
 342
 343           // If distance between user and simulated aircaft is less
 344           // then 500nm, create this flight. At jet speeds 500 nm is roughly
 345           // one hour flight time, so that would be a good approximate point
 346           // to start a more detailed simulation of this aircraft.
 347           //cerr << registration << " is currently enroute from "
 348           //   << dep->_id << " to " << arr->_id << "distance : "
 349           //   << distanceToUser*SG_METER_TO_NM << endl;
 350           if ((distanceToUser*SG_METER_TO_NM) < TRAFFICTOAIDIST)
 351             {
 352               string flightPlanName = dep->getId() + string("-") + arr->getId() +
 353                 string(".xml");
 354               int alt;
 355               //if  ((i->getDepartureTime() < now))
 356               //{
 357               //          alt = i->getCruiseAlt() *100;
 358               //        }
 359               //else
 360               //{
 361               //          alt = dep->_elevation+19;
 362               //        }
 363
 364               // Fixme: A non-existent model path results in an
 365               // abort, due to an unhandled exeption, in fg main loop.
 366               FGAIAircraft *aircraft = new FGAIAircraft(this);
 367               aircraft->setPerformance(m_class); //"jet_transport";
 368               aircraft->setCompany(airline); //i->getAirline();
 369               aircraft->setAcType(acType); //i->getAcType();
 370               aircraft->setPath(modelPath.c_str());
 371               aircraft->setFlightPlan(flightPlanName);
 372               aircraft->setLatitude(lat);
 373               aircraft->setLongitude(lon);
 374               aircraft->setAltitude(i->getCruiseAlt()*100); // convert from FL to feet
 375               aircraft->setSpeed(speed);
 376               aircraft->setBank(0);
 377               aircraft->SetFlightPlan(new FGAIFlightPlan(modelPath, courseToDest, i->getDepartureTime(), dep,
 378                                                          arr,true, radius, i->getCruiseAlt()*100, lat, lon, speed, flightType, acType, airline));
 379               aimgr->attach(aircraft);
 380
 381
 382               AIManagerRef = aircraft->getID();
 383               //cerr << "Class: " << m_class << ". acType: " << acType << ". Airline: " << airline << ". Speed = " << speed << ". From " << dep->getId() << " to " << arr->getId() << ". Time Fraction = " << (remainingTimeEnroute/(double) totalTimeEnroute) << endl;
 384               //cerr << "Latitude : " << lat << ". Longitude : " << lon << endl;
 385               //cerr << "Dep      : " << dep->getLatitude()<< ", "<< dep->getLongitude() << endl;
 386               //cerr << "Arr      : " << arr->getLatitude()<< ", "<< arr->getLongitude() << endl;
 387               //cerr << "Time remaining = " << (remainingTimeEnroute/3600.0) << endl;
 388               //cerr << "Total time     = " << (totalTimeEnroute/3600.0) << endl;
 389               //cerr << "Distance remaining = " << distanceToDest*SG_METER_TO_NM << endl;
 390
 391             }
 392           return true;
 393     }
 394
 395       // Both departure and arrival time are in the future, so this
 396       // the aircraft is parked at the departure airport.
 397       // Currently this status is mostly ignored, but in future
 398       // versions, code should go here that -if within user range-
 399       // positions these aircraft at parking locations at the airport.
 400   if ((i->getDepartureTime() > now) && (i->getArrivalTime() > now))
 401         {
 402           dep = i->getDepartureAirport();
 403           return true;
 404         }
 405     }
 406   //cerr << "Traffic schedule got to beyond last clause" << endl;
 407     // EMH: prevent a warning, should this be 'true' instead?
 408     // DT: YES. Originally, this code couldn't be reached, but
 409     // when the "if(!(AIManagerManager))" clause is false we
 410     // fall through right to the end. This is a valid flow.
 411     // the actual value is pretty innocent, only it triggers
 412     // warning in TrafficManager::update().
 413     // (which was added as a sanity check for myself in the first place. :-)
 414     return true;
 415 }
 416
 417
 418 void FGAISchedule::next()
 419 {
 420   flights.begin()->update();
 421   sort(flights.begin(), flights.end());
 422 }
 423
 424 double FGAISchedule::getSpeed()
 425 {
 426   double courseToUser,   courseToDest;
 427   double distanceToUser, distanceToDest;
 428   double speed, remainingTimeEnroute;
 429   FGAirport *dep, *arr;
 430
 431   FGScheduledFlightVecIterator i = flights.begin();
 432   dep = i->getDepartureAirport();
 433   arr = i->getArrivalAirport  ();
 434   if (!(dep && arr))
 435     return 0;
 436
 437   SGWayPoint dest (   dep->getLongitude(),
 438                       dep->getLatitude(),
 439                       i->getCruiseAlt());
 440   SGWayPoint curr (    arr->getLongitude(),
 441                       arr->getLatitude(),
 442                       i->getCruiseAlt());
 443   remainingTimeEnroute     = i->getArrivalTime() - i->getDepartureTime();
 444   dest.CourseAndDistance(curr, &courseToDest, &distanceToDest);
 445   speed =  (distanceToDest*SG_METER_TO_NM) /
 446     ((double) remainingTimeEnroute/3600.0);
 447   return speed;
 448 }
 449
 450
 451 void FGAISchedule::setClosestDistanceToUser()
 452 {
 453
 454
 455   double course;
 456   double dist;
 457
 458   Point3D temp;
 459   time_t
 460     totalTimeEnroute,
 461     elapsedTimeEnroute;
 462
 463   double userLatitude  = fgGetDouble("/position/latitude-deg");
 464   double userLongitude = fgGetDouble("/position/longitude-deg");
 465
 466   FGAirport *dep;
 467
 468 #if defined( __CYGWIN__) || defined( __MINGW32__)
 469   #define HUGE HUGE_VAL
 470 #endif
 471   distanceToUser = HUGE;
 472   FGScheduledFlightVecIterator i = flights.begin();
 473   while (i != flights.end())
 474     {
 475       dep = i->getDepartureAirport();
 476       //if (!(dep))
 477       //return HUGE;
 478
 479       SGWayPoint user (   userLongitude,
 480                           userLatitude,
 481                           i->getCruiseAlt());
 482       SGWayPoint current (dep->getLongitude(),
 483                           dep->getLatitude(),
 484                           0);
 485       user.CourseAndDistance(current, &course, &dist);
 486       if (dist < distanceToUser)
 487         {
 488           distanceToUser = dist;
 489           //cerr << "Found closest distance to user for " << registration << " to be " << distanceToUser << " at airport " << dep->getId() << endl;
 490         }
 491       i++;
 492     }
 493   //return distToUser;
 494 }