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 (or the AI version of mp) exists
 388               SGPath mp(globals->get_fg_root());
 389               SGPath mp_ai = mp;
 390
 391               mp.append(modelPath);
 392               mp_ai.append("AI");
 393               mp_ai.append(modelPath);
 394
 395               if (mp.exists() || mp_ai.exists())
 396               {
 397                   FGAIAircraft *aircraft = new FGAIAircraft(this);
 398                   aircraft->setPerformance(m_class); //"jet_transport";
 399                   aircraft->setCompany(airline); //i->getAirline();
 400                   aircraft->setAcType(acType); //i->getAcType();
 401                   aircraft->setPath(modelPath.c_str());
 402                   //aircraft->setFlightPlan(flightPlanName);
 403                   aircraft->setLatitude(lat);
 404                   aircraft->setLongitude(lon);
 405                   aircraft->setAltitude((*i)->getCruiseAlt()*100); // convert from FL to feet
 406                   aircraft->setSpeed(speed);
 407                   aircraft->setBank(0);
 408                   aircraft->SetFlightPlan(new FGAIFlightPlan(flightPlanName, courseToDest, deptime,
 409                                                              dep, arr,true, radius,
 410                                                              (*i)->getCruiseAlt()*100,
 411                                                              lat, lon, speed, flightType, acType,
 412                                                              airline));
 413                   aimgr->attach(aircraft);
 414
 415
 416                   AIManagerRef = aircraft->getID();
 417                   //cerr << "Class: " << m_class << ". acType: " << acType << ". Airline: " << airline << ". Speed = " << speed << ". From " << dep->getId() << " to " << arr->getId() << ". Time Fraction = " << (remainingTimeEnroute/(double) totalTimeEnroute) << endl;
 418                   //cerr << "Latitude : " << lat << ". Longitude : " << lon << endl;
 419                   //cerr << "Dep      : " << dep->getLatitude()<< ", "<< dep->getLongitude() << endl;
 420                   //cerr << "Arr      : " << arr->getLatitude()<< ", "<< arr->getLongitude() << endl;
 421                   //cerr << "Time remaining = " << (remainingTimeEnroute/3600.0) << endl;
 422                   //cerr << "Total time     = " << (totalTimeEnroute/3600.0) << endl;
 423                   //cerr << "Distance remaining = " << distanceToDest*SG_METER_TO_NM << endl;
 424                   }
 425               else
 426                 {
 427                   SG_LOG(SG_INPUT, SG_WARN, "TrafficManager: Could not load model " << mp.str());
 428                 }
 429             }
 430           return true;
 431     }
 432
 433       // Both departure and arrival time are in the future, so this
 434       // the aircraft is parked at the departure airport.
 435       // Currently this status is mostly ignored, but in future
 436       // versions, code should go here that -if within user range-
 437       // positions these aircraft at parking locations at the airport.
 438       if (((*i)->getDepartureTime() > now) && ((*i)->getArrivalTime() > now))
 439         {
 440           dep = (*i)->getDepartureAirport();
 441           return true;
 442         }
 443     }
 444   //cerr << "Traffic schedule got to beyond last clause" << endl;
 445     // EMH: prevent a warning, should this be 'true' instead?
 446     // DT: YES. Originally, this code couldn't be reached, but
 447     // when the "if(!(AIManagerManager))" clause is false we
 448     // fall through right to the end. This is a valid flow.
 449     // the actual value is pretty innocent, only it triggers
 450     // warning in TrafficManager::update().
 451     // (which was added as a sanity check for myself in the first place. :-)
 452     return true;
 453 }
 454
 455
 456 void FGAISchedule::next()
 457 {
 458   (*flights.begin())->update();
 459   sort(flights.begin(), flights.end(), compareScheduledFlights);
 460 }
 461
 462 double FGAISchedule::getSpeed()
 463 {
 464   double courseToDest;
 465   double distanceToDest;
 466   double speed, remainingTimeEnroute;
 467   FGAirport *dep, *arr;
 468
 469   FGScheduledFlightVecIterator i = flights.begin();
 470   dep = (*i)->getDepartureAirport();
 471   arr = (*i)->getArrivalAirport  ();
 472   if (!(dep && arr))
 473     return 0;
 474
 475   SGWayPoint dest (   dep->getLongitude(),
 476                       dep->getLatitude(),
 477                       (*i)->getCruiseAlt());
 478   SGWayPoint curr (    arr->getLongitude(),
 479                       arr->getLatitude(),
 480                        (*i)->getCruiseAlt());
 481   remainingTimeEnroute     = (*i)->getArrivalTime() - (*i)->getDepartureTime();
 482   dest.CourseAndDistance(curr, &courseToDest, &distanceToDest);
 483   speed =  (distanceToDest*SG_METER_TO_NM) /
 484     ((double) remainingTimeEnroute/3600.0);
 485   return speed;
 486 }
 487
 488 bool compareSchedules(FGAISchedule*a, FGAISchedule*b)
 489 {
 490   return (*a) < (*b);
 491 }
 492
 493
 494 // void FGAISchedule::setClosestDistanceToUser()
 495 // {
 496
 497
 498 //   double course;
 499 //   double dist;
 500
 501 //   Point3D temp;
 502 //   time_t
 503 //     totalTimeEnroute,
 504 //     elapsedTimeEnroute;
 505
 506 //   double userLatitude  = fgGetDouble("/position/latitude-deg");
 507 //   double userLongitude = fgGetDouble("/position/longitude-deg");
 508
 509 //   FGAirport *dep;
 510
 511 // #if defined( __CYGWIN__) || defined( __MINGW32__)
 512 //   #define HUGE HUGE_VAL
 513 // #endif
 514 //   distanceToUser = HUGE;
 515 //   FGScheduledFlightVecIterator i = flights.begin();
 516 //   while (i != flights.end())
 517 //     {
 518 //       dep = i->getDepartureAirport();
 519 //       //if (!(dep))
 520 //       //return HUGE;
 521
 522 //       SGWayPoint user (   userLongitude,
 523 //                        userLatitude,
 524 //                        i->getCruiseAlt());
 525 //       SGWayPoint current (dep->getLongitude(),
 526 //                        dep->getLatitude(),
 527 //                        0);
 528 //       user.CourseAndDistance(current, &course, &dist);
 529 //       if (dist < distanceToUser)
 530 //      {
 531 //        distanceToUser = dist;
 532 //        //cerr << "Found closest distance to user for " << registration << " to be " << distanceToUser << " at airport " << dep->getId() << endl;
 533 //      }
 534 //       i++;
 535 //     }
 536 //   //return distToUser;
 537 // }
 538