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 SG_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   sgdVec3 a, b, cross;
 177   sgdVec3 newPos;
 178   sgdMat4 matrix;
 179   double angle;
 180
 181   FGAIManager *aimgr;
 182   string airport;
 183
 184   double courseToUser,   courseToDest;
 185   double distanceToDest;
 186   double speed;
 187
 188   Point3D temp;
 189   time_t
 190     totalTimeEnroute,
 191     elapsedTimeEnroute,
 192     remainingTimeEnroute, deptime = 0;
 193   double
 194     userLatitude,
 195     userLongitude;
 196
 197   if (fgGetBool("/sim/traffic-manager/enabled") == false)
 198     return true;
 199
 200   aimgr = (FGAIManager *) globals-> get_subsystem("ai_model");
 201   // Before the flight status of this traffic entity is updated
 202   // for the first time, we need to roll back it's flight schedule so
 203   // so that all the flights are centered around this simulated week's time
 204   // table. This is to avoid the situation where the first scheduled flight is
 205   // in the future, causing the traffic manager to not generate traffic until
 206   // simulated time has caught up with the real world time at initialization.
 207   // This is to counter a more general initialization bug, caused by the fact
 208   // that warp is not yet set when the  schedule is initialized. This is
 209   // especially a problem when using a negative time offset.
 210   // i.e let's say we specify FlightGear to run with --time-offset=-24:00:00.
 211   // Then the schedule will initialize using today, but we will fly yesterday.
 212   // Thus, it would take a whole day of simulation before the traffic manager
 213   // finally kicks in.
 214   if (firstRun)
 215     {
 216       if (init() == false)
 217         AIManagerRef = BOGUS;
 218
 219       for (FGScheduledFlightVecIterator i = flights.begin();
 220            i != flights.end();
 221            i++)
 222         {
 223           (*i)->adjustTime(now);
 224         }
 225       if (fgGetBool("/sim/traffic-manager/instantaneous-action") == true)
 226         deptime = now + rand() % 300; // Wait up to 5 minutes until traffic starts moving to prevent too many aircraft
 227                                       // from cluttering the gate areas.
 228       firstRun = false;
 229     }
 230
 231   // Sort all the scheduled flights according to scheduled departure time.
 232   // Because this is done at every update, we only need to check the status
 233   // of the first listed flight.
 234   sort(flights.begin(), flights.end(), compareScheduledFlights);
 235   if (!deptime)
 236     deptime = (*flights.begin())->getDepartureTime();
 237   FGScheduledFlightVecIterator i = flights.begin();
 238   if (AIManagerRef)
 239     {
 240       // Check if this aircraft has been released.
 241       FGTrafficManager *tmgr = (FGTrafficManager *) globals->get_subsystem("Traffic Manager");
 242       if (tmgr->isReleased(AIManagerRef))
 243         AIManagerRef = 0;
 244     }
 245
 246   if (!AIManagerRef)
 247     {
 248       userLatitude  = fgGetDouble("/position/latitude-deg");
 249       userLongitude = fgGetDouble("/position/longitude-deg");
 250
 251       //cerr << "Estimated minimum distance to user: " << distanceToUser << endl;
 252       // This flight entry is entirely in the past, do we need to
 253       // push it forward in time to the next scheduled departure.
 254       if (((*i)->getDepartureTime() < now) && ((*i)->getArrivalTime() < now))
 255         {
 256           (*i)->update();
 257           return true;
 258         }
 259
 260       // Departure time in the past and arrival time in the future.
 261       // This flight is in progress, so we need to calculate it's
 262       // approximate position and -if in range- create an AIAircraft
 263       // object for it.
 264       //if ((i->getDepartureTime() < now) && (i->getArrivalTime() > now))
 265
 266
 267       // Part of this flight is in the future.
 268       if ((*i)->getArrivalTime() > now)
 269         {
 270           dep = (*i)->getDepartureAirport();
 271           arr = (*i)->getArrivalAirport  ();
 272           if (!(dep && arr))
 273             return false;
 274
 275           temp = sgPolarToCart3d(Point3D(dep->getLongitude() *
 276                                          SG_DEGREES_TO_RADIANS,
 277                                          dep->getLatitude()  *
 278                                          SG_DEGREES_TO_RADIANS,
 279                                          1.0));
 280           a[0] = temp.x();
 281           a[1] = temp.y();
 282           a[2] = temp.z();
 283
 284           temp = sgPolarToCart3d(Point3D(arr->getLongitude() *
 285                                          SG_DEGREES_TO_RADIANS,
 286                                          arr->getLatitude()  *
 287                                          SG_DEGREES_TO_RADIANS,
 288                                          1.0));
 289           b[0] = temp.x();
 290           b[1] = temp.y();
 291           b[2] = temp.z();
 292           sgdNormaliseVec3(a);
 293           sgdNormaliseVec3(b);
 294           sgdVectorProductVec3(cross,b,a);
 295
 296           angle = sgACos(sgdScalarProductVec3(a,b));
 297
 298           // Okay, at this point we have the angle between departure and
 299           // arrival airport, in degrees. From here we can interpolate the
 300           // position of the aircraft by calculating the ratio between
 301           // total time enroute and elapsed time enroute.
 302
 303           totalTimeEnroute     = (*i)->getArrivalTime() - (*i)->getDepartureTime();
 304           if (now > (*i)->getDepartureTime())
 305             {
 306               //err << "Lat = " << lat << ", lon = " << lon << endl;
 307               //cerr << "Time diff: " << now-i->getDepartureTime() << endl;
 308               elapsedTimeEnroute   = now - (*i)->getDepartureTime();
 309               remainingTimeEnroute = (*i)->getArrivalTime()   - now;
 310             }
 311           else
 312             {
 313               lat = dep->getLatitude();
 314               lon = dep->getLongitude();
 315               elapsedTimeEnroute = 0;
 316               remainingTimeEnroute = totalTimeEnroute;
 317             }
 318
 319           angle *= ( (double) elapsedTimeEnroute/ (double) totalTimeEnroute);
 320
 321
 322           //cout << "a = " << a[0] << " " << a[1] << " " << a[2]
 323           //     << "b = " << b[0] << " " << b[1] << " " << b[2] << endl;
 324           sgdMakeRotMat4(matrix, angle, cross);
 325           for(int j = 0; j < 3; j++)
 326             {
 327               newPos[j] =0.0;
 328               for (int k = 0; k<3; k++)
 329                 {
 330                   newPos[j] += matrix[j][k]*a[k];
 331                 }
 332             }
 333
 334           temp = sgCartToPolar3d(Point3D(newPos[0], newPos[1],newPos[2]));
 335           if (now > (*i)->getDepartureTime())
 336             {
 337               //cerr << "Lat = " << lat << ", lon = " << lon << endl;
 338               //cerr << "Time diff: " << now-i->getDepartureTime() << endl;
 339               lat = temp.lat() * SG_RADIANS_TO_DEGREES;
 340               lon = temp.lon() * SG_RADIANS_TO_DEGREES;
 341             }
 342           else
 343             {
 344               lat = dep->getLatitude();
 345               lon = dep->getLongitude();
 346             }
 347
 348
 349           SGWayPoint current  (lon,
 350                                lat,
 351                                (*i)->getCruiseAlt());
 352           SGWayPoint user (   userLongitude,
 353                               userLatitude,
 354                               (*i)->getCruiseAlt());
 355           SGWayPoint dest (   arr->getLongitude(),
 356                               arr->getLatitude(),
 357                               (*i)->getCruiseAlt());
 358           // We really only need distance to user
 359           // and course to destination
 360           user.CourseAndDistance(current, &courseToUser, &distanceToUser);
 361           dest.CourseAndDistance(current, &courseToDest, &distanceToDest);
 362           speed =  (distanceToDest*SG_METER_TO_NM) /
 363             ((double) remainingTimeEnroute/3600.0);
 364
 365
 366           // If distance between user and simulated aircaft is less
 367           // then 500nm, create this flight. At jet speeds 500 nm is roughly
 368           // one hour flight time, so that would be a good approximate point
 369           // to start a more detailed simulation of this aircraft.
 370           //cerr << registration << " is currently enroute from "
 371           //   << dep->_id << " to " << arr->_id << "distance : "
 372           //   << distanceToUser*SG_METER_TO_NM << endl;
 373           if ((distanceToUser*SG_METER_TO_NM) < TRAFFICTOAIDIST)
 374             {
 375               string flightPlanName = dep->getId() + string("-") + arr->getId() +
 376                 string(".xml");
 377               int alt;
 378               //if  ((i->getDepartureTime() < now))
 379               //{
 380               //          alt = i->getCruiseAlt() *100;
 381               //        }
 382               //else
 383               //{
 384               //          alt = dep->_elevation+19;
 385               //        }
 386
 387               // Only allow traffic to be created when the model path exists
 388               SGPath mp(globals->get_fg_root());
 389               mp.append(modelPath);
 390               if (mp.exists())
 391               {
 392                   FGAIAircraft *aircraft = new FGAIAircraft(this);
 393                   aircraft->setPerformance(m_class); //"jet_transport";
 394                   aircraft->setCompany(airline); //i->getAirline();
 395                   aircraft->setAcType(acType); //i->getAcType();
 396                   aircraft->setPath(modelPath.c_str());
 397                   //aircraft->setFlightPlan(flightPlanName);
 398                   aircraft->setLatitude(lat);
 399                   aircraft->setLongitude(lon);
 400                   aircraft->setAltitude((*i)->getCruiseAlt()*100); // convert from FL to feet
 401                   aircraft->setSpeed(speed);
 402                   aircraft->setBank(0);
 403                   aircraft->SetFlightPlan(new FGAIFlightPlan(flightPlanName, courseToDest, deptime,
 404                                                              dep, arr,true, radius,
 405                                                              (*i)->getCruiseAlt()*100,
 406                                                              lat, lon, speed, flightType, acType,
 407                                                              airline));
 408                   aimgr->attach(aircraft);
 409
 410
 411                   AIManagerRef = aircraft->getID();
 412                   //cerr << "Class: " << m_class << ". acType: " << acType << ". Airline: " << airline << ". Speed = " << speed << ". From " << dep->getId() << " to " << arr->getId() << ". Time Fraction = " << (remainingTimeEnroute/(double) totalTimeEnroute) << endl;
 413                   //cerr << "Latitude : " << lat << ". Longitude : " << lon << endl;
 414                   //cerr << "Dep      : " << dep->getLatitude()<< ", "<< dep->getLongitude() << endl;
 415                   //cerr << "Arr      : " << arr->getLatitude()<< ", "<< arr->getLongitude() << endl;
 416                   //cerr << "Time remaining = " << (remainingTimeEnroute/3600.0) << endl;
 417                   //cerr << "Total time     = " << (totalTimeEnroute/3600.0) << endl;
 418                   //cerr << "Distance remaining = " << distanceToDest*SG_METER_TO_NM << endl;
 419                   }
 420               else
 421                 {
 422                   SG_LOG(SG_INPUT, SG_WARN, "TrafficManager: Could not load model " << mp.str());
 423                 }
 424             }
 425           return true;
 426     }
 427
 428       // Both departure and arrival time are in the future, so this
 429       // the aircraft is parked at the departure airport.
 430       // Currently this status is mostly ignored, but in future
 431       // versions, code should go here that -if within user range-
 432       // positions these aircraft at parking locations at the airport.
 433       if (((*i)->getDepartureTime() > now) && ((*i)->getArrivalTime() > now))
 434         {
 435           dep = (*i)->getDepartureAirport();
 436           return true;
 437         }
 438     }
 439   //cerr << "Traffic schedule got to beyond last clause" << endl;
 440     // EMH: prevent a warning, should this be 'true' instead?
 441     // DT: YES. Originally, this code couldn't be reached, but
 442     // when the "if(!(AIManagerManager))" clause is false we
 443     // fall through right to the end. This is a valid flow.
 444     // the actual value is pretty innocent, only it triggers
 445     // warning in TrafficManager::update().
 446     // (which was added as a sanity check for myself in the first place. :-)
 447     return true;
 448 }
 449
 450
 451 void FGAISchedule::next()
 452 {
 453   (*flights.begin())->update();
 454   sort(flights.begin(), flights.end(), compareScheduledFlights);
 455 }
 456
 457 double FGAISchedule::getSpeed()
 458 {
 459   double courseToUser,   courseToDest;
 460   double distanceToUser, distanceToDest;
 461   double speed, remainingTimeEnroute;
 462   FGAirport *dep, *arr;
 463
 464   FGScheduledFlightVecIterator i = flights.begin();
 465   dep = (*i)->getDepartureAirport();
 466   arr = (*i)->getArrivalAirport  ();
 467   if (!(dep && arr))
 468     return 0;
 469
 470   SGWayPoint dest (   dep->getLongitude(),
 471                       dep->getLatitude(),
 472                       (*i)->getCruiseAlt());
 473   SGWayPoint curr (    arr->getLongitude(),
 474                       arr->getLatitude(),
 475                        (*i)->getCruiseAlt());
 476   remainingTimeEnroute     = (*i)->getArrivalTime() - (*i)->getDepartureTime();
 477   dest.CourseAndDistance(curr, &courseToDest, &distanceToDest);
 478   speed =  (distanceToDest*SG_METER_TO_NM) /
 479     ((double) remainingTimeEnroute/3600.0);
 480   return speed;
 481 }
 482
 483 bool compareSchedules(FGAISchedule*a, FGAISchedule*b)
 484 {
 485   return (*a) < (*b);
 486 }
 487
 488
 489 // void FGAISchedule::setClosestDistanceToUser()
 490 // {
 491
 492
 493 //   double course;
 494 //   double dist;
 495
 496 //   Point3D temp;
 497 //   time_t
 498 //     totalTimeEnroute,
 499 //     elapsedTimeEnroute;
 500
 501 //   double userLatitude  = fgGetDouble("/position/latitude-deg");
 502 //   double userLongitude = fgGetDouble("/position/longitude-deg");
 503
 504 //   FGAirport *dep;
 505
 506 // #if defined( __CYGWIN__) || defined( __MINGW32__)
 507 //   #define HUGE HUGE_VAL
 508 // #endif
 509 //   distanceToUser = HUGE;
 510 //   FGScheduledFlightVecIterator i = flights.begin();
 511 //   while (i != flights.end())
 512 //     {
 513 //       dep = i->getDepartureAirport();
 514 //       //if (!(dep))
 515 //       //return HUGE;
 516
 517 //       SGWayPoint user (   userLongitude,
 518 //                        userLatitude,
 519 //                        i->getCruiseAlt());
 520 //       SGWayPoint current (dep->getLongitude(),
 521 //                        dep->getLatitude(),
 522 //                        0);
 523 //       user.CourseAndDistance(current, &course, &dist);
 524 //       if (dist < distanceToUser)
 525 //      {
 526 //        distanceToUser = dist;
 527 //        //cerr << "Found closest distance to user for " << registration << " to be " << distanceToUser << " at airport " << dep->getId() << endl;
 528 //      }
 529 //       i++;
 530 //     }
 531 //   //return distToUser;
 532 // }
 533