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