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   SG_LOG (SG_GENERAL, SG_INFO,"Processing registration " << registration << " with callsign " << (*i)->getCallSign());
 239   if (AIManagerRef)
 240     {
 241       // Check if this aircraft has been released.
 242       FGTrafficManager *tmgr = (FGTrafficManager *) globals->get_subsystem("Traffic Manager");
 243       if (tmgr->isReleased(AIManagerRef))
 244         AIManagerRef = 0;
 245     }
 246
 247   if (!AIManagerRef)
 248     {
 249       userLatitude  = fgGetDouble("/position/latitude-deg");
 250       userLongitude = fgGetDouble("/position/longitude-deg");
 251
 252       //cerr << "Estimated minimum distance to user: " << distanceToUser << endl;
 253       // This flight entry is entirely in the past, do we need to
 254       // push it forward in time to the next scheduled departure.
 255       if (((*i)->getDepartureTime() < now) && ((*i)->getArrivalTime() < now))
 256         {
 257           (*i)->update();
 258           return true;
 259         }
 260
 261       // Departure time in the past and arrival time in the future.
 262       // This flight is in progress, so we need to calculate it's
 263       // approximate position and -if in range- create an AIAircraft
 264       // object for it.
 265       //if ((i->getDepartureTime() < now) && (i->getArrivalTime() > now))
 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::SPHERICAL);
 353           SGWayPoint user (   userLongitude,
 354                               userLatitude,
 355                               (*i)->getCruiseAlt(),
 356                               SGWayPoint::SPHERICAL);
 357           SGWayPoint dest (   arr->getLongitude(),
 358                               arr->getLatitude(),
 359                               (*i)->getCruiseAlt(),
 360                               SGWayPoint::SPHERICAL);
 361           // We really only need distance to user
 362           // and course to destination
 363           user.CourseAndDistance(current, &courseToUser, &distanceToUser);
 364           dest.CourseAndDistance(current, &courseToDest, &distanceToDest);
 365           speed =  (distanceToDest*SG_METER_TO_NM) /
 366             ((double) remainingTimeEnroute/3600.0);
 367
 368
 369           // If distance between user and simulated aircaft is less
 370           // then 500nm, create this flight. At jet speeds 500 nm is roughly
 371           // one hour flight time, so that would be a good approximate point
 372           // to start a more detailed simulation of this aircraft.
 373           //cerr << registration << " is currently enroute from "
 374           //   << dep->_id << " to " << arr->_id << "distance : "
 375           //   << distanceToUser*SG_METER_TO_NM << endl;
 376           if ((distanceToUser*SG_METER_TO_NM) < TRAFFICTOAIDIST)
 377             {
 378               string flightPlanName = dep->getId() + string("-") + arr->getId() +
 379                 string(".xml");
 380               //int alt;
 381               //if  ((i->getDepartureTime() < now))
 382               //{
 383               //          alt = i->getCruiseAlt() *100;
 384               //        }
 385               //else
 386               //{
 387               //          alt = dep->_elevation+19;
 388               //        }
 389
 390               // Only allow traffic to be created when the model path (or the AI version of mp) exists
 391               SGPath mp(globals->get_fg_root());
 392               SGPath mp_ai = mp;
 393
 394               mp.append(modelPath);
 395               mp_ai.append("AI");
 396               mp_ai.append(modelPath);
 397
 398               if (mp.exists() || mp_ai.exists())
 399               {
 400                   FGAIAircraft *aircraft = new FGAIAircraft(this);
 401                   aircraft->setPerformance(m_class); //"jet_transport";
 402                   aircraft->setCompany(airline); //i->getAirline();
 403                   aircraft->setAcType(acType); //i->getAcType();
 404                   aircraft->setPath(modelPath.c_str());
 405                   //aircraft->setFlightPlan(flightPlanName);
 406                   aircraft->setLatitude(lat);
 407                   aircraft->setLongitude(lon);
 408                   aircraft->setAltitude((*i)->getCruiseAlt()*100); // convert from FL to feet
 409                   aircraft->setSpeed(speed);
 410                   aircraft->setBank(0);
 411                   aircraft->SetFlightPlan(new FGAIFlightPlan(flightPlanName, courseToDest, deptime,
 412                                                              dep, arr,true, radius,
 413                                                              (*i)->getCruiseAlt()*100,
 414                                                              lat, lon, speed, flightType, acType,
 415                                                              airline));
 416                   aimgr->attach(aircraft);
 417
 418
 419                   AIManagerRef = aircraft->getID();
 420                   //cerr << "Class: " << m_class << ". acType: " << acType << ". Airline: " << airline << ". Speed = " << speed << ". From " << dep->getId() << " to " << arr->getId() << ". Time Fraction = " << (remainingTimeEnroute/(double) totalTimeEnroute) << endl;
 421                   //cerr << "Latitude : " << lat << ". Longitude : " << lon << endl;
 422                   //cerr << "Dep      : " << dep->getLatitude()<< ", "<< dep->getLongitude() << endl;
 423                   //cerr << "Arr      : " << arr->getLatitude()<< ", "<< arr->getLongitude() << endl;
 424                   //cerr << "Time remaining = " << (remainingTimeEnroute/3600.0) << endl;
 425                   //cerr << "Total time     = " << (totalTimeEnroute/3600.0) << endl;
 426                   //cerr << "Distance remaining = " << distanceToDest*SG_METER_TO_NM << endl;
 427                   }
 428               else
 429                 {
 430                   SG_LOG(SG_INPUT, SG_WARN, "TrafficManager: Could not load model " << mp.str());
 431                 }
 432             }
 433           return true;
 434     }
 435
 436       // Both departure and arrival time are in the future, so this
 437       // the aircraft is parked at the departure airport.
 438       // Currently this status is mostly ignored, but in future
 439       // versions, code should go here that -if within user range-
 440       // positions these aircraft at parking locations at the airport.
 441       if (((*i)->getDepartureTime() > now) && ((*i)->getArrivalTime() > now))
 442         {
 443           dep = (*i)->getDepartureAirport();
 444           return true;
 445         }
 446     }
 447   //cerr << "Traffic schedule got to beyond last clause" << endl;
 448     // EMH: prevent a warning, should this be 'true' instead?
 449     // DT: YES. Originally, this code couldn't be reached, but
 450     // when the "if(!(AIManagerManager))" clause is false we
 451     // fall through right to the end. This is a valid flow.
 452     // the actual value is pretty innocent, only it triggers
 453     // warning in TrafficManager::update().
 454     // (which was added as a sanity check for myself in the first place. :-)
 455     return true;
 456 }
 457
 458
 459 void FGAISchedule::next()
 460 {
 461   (*flights.begin())->update();
 462   sort(flights.begin(), flights.end(), compareScheduledFlights);
 463 }
 464
 465 double FGAISchedule::getSpeed()
 466 {
 467   double courseToDest;
 468   double distanceToDest;
 469   double speed, remainingTimeEnroute;
 470   FGAirport *dep, *arr;
 471
 472   FGScheduledFlightVecIterator i = flights.begin();
 473   dep = (*i)->getDepartureAirport();
 474   arr = (*i)->getArrivalAirport  ();
 475   if (!(dep && arr))
 476     return 0;
 477
 478   SGWayPoint dest (   dep->getLongitude(),
 479                       dep->getLatitude(),
 480                       (*i)->getCruiseAlt(),
 481                       SGWayPoint::SPHERICAL);
 482   SGWayPoint curr (    arr->getLongitude(),
 483                       arr->getLatitude(),
 484                        (*i)->getCruiseAlt(),
 485                        SGWayPoint::SPHERICAL);
 486   remainingTimeEnroute     = (*i)->getArrivalTime() - (*i)->getDepartureTime();
 487   dest.CourseAndDistance(curr, &courseToDest, &distanceToDest);
 488   speed =  (distanceToDest*SG_METER_TO_NM) /
 489     ((double) remainingTimeEnroute/3600.0);
 490   return speed;
 491 }
 492
 493 bool compareSchedules(FGAISchedule*a, FGAISchedule*b)
 494 {
 495   return (*a) < (*b);
 496 }
 497
 498
 499 // void FGAISchedule::setClosestDistanceToUser()
 500 // {
 501
 502
 503 //   double course;
 504 //   double dist;
 505
 506 //   Point3D temp;
 507 //   time_t
 508 //     totalTimeEnroute,
 509 //     elapsedTimeEnroute;
 510
 511 //   double userLatitude  = fgGetDouble("/position/latitude-deg");
 512 //   double userLongitude = fgGetDouble("/position/longitude-deg");
 513
 514 //   FGAirport *dep;
 515
 516 // #if defined( __CYGWIN__) || defined( __MINGW32__)
 517 //   #define HUGE HUGE_VAL
 518 // #endif
 519 //   distanceToUser = HUGE;
 520 //   FGScheduledFlightVecIterator i = flights.begin();
 521 //   while (i != flights.end())
 522 //     {
 523 //       dep = i->getDepartureAirport();
 524 //       //if (!(dep))
 525 //       //return HUGE;
 526
 527 //       SGWayPoint user (   userLongitude,
 528 //                        userLatitude,
 529 //                        i->getCruiseAlt());
 530 //       SGWayPoint current (dep->getLongitude(),
 531 //                        dep->getLatitude(),
 532 //                        0);
 533 //       user.CourseAndDistance(current, &course, &dist);
 534 //       if (dist < distanceToUser)
 535 //      {
 536 //        distanceToUser = dist;
 537 //        //cerr << "Found closest distance to user for " << registration << " to be " << distanceToUser << " at airport " << dep->getId() << endl;
 538 //      }
 539 //       i++;
 540 //     }
 541 //   //return distToUser;
 542 // }
 543