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   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_INFO,"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           (*i)->update();
 254           return true;
 255         }
 256
 257       // Departure time in the past and arrival time in the future.
 258       // This flight is in progress, so we need to calculate it's
 259       // approximate position and -if in range- create an AIAircraft
 260       // object for it.
 261       //if ((i->getDepartureTime() < now) && (i->getArrivalTime() > now))
 262
 263       // Part of this flight is in the future.
 264       if ((*i)->getArrivalTime() > now)
 265         {
 266           dep = (*i)->getDepartureAirport();
 267           arr = (*i)->getArrivalAirport  ();
 268           if (!(dep && arr))
 269             return false;
 270
 271           SGVec3d a = SGVec3d::fromGeoc(SGGeoc::fromDegM(dep->getLongitude(),
 272                                                 dep->getLatitude(), 1));
 273           SGVec3d b = SGVec3d::fromGeoc(SGGeoc::fromDegM(arr->getLongitude(),
 274                                                 arr->getLatitude(), 1));
 275           SGVec3d _cross = cross(b, a);
 276
 277           angle = sgACos(dot(a, b));
 278
 279           // Okay, at this point we have the angle between departure and
 280           // arrival airport, in degrees. From here we can interpolate the
 281           // position of the aircraft by calculating the ratio between
 282           // total time enroute and elapsed time enroute.
 283
 284           totalTimeEnroute     = (*i)->getArrivalTime() - (*i)->getDepartureTime();
 285           if (now > (*i)->getDepartureTime())
 286             {
 287               //err << "Lat = " << lat << ", lon = " << lon << endl;
 288               //cerr << "Time diff: " << now-i->getDepartureTime() << endl;
 289               elapsedTimeEnroute   = now - (*i)->getDepartureTime();
 290               remainingTimeEnroute = (*i)->getArrivalTime()   - now;
 291             }
 292           else
 293             {
 294               lat = dep->getLatitude();
 295               lon = dep->getLongitude();
 296               elapsedTimeEnroute = 0;
 297               remainingTimeEnroute = totalTimeEnroute;
 298             }
 299
 300           angle *= ( (double) elapsedTimeEnroute/ (double) totalTimeEnroute);
 301
 302
 303           //cout << "a = " << a[0] << " " << a[1] << " " << a[2]
 304           //     << "b = " << b[0] << " " << b[1] << " " << b[2] << endl;
 305           sgdMat4 matrix;
 306           sgdMakeRotMat4(matrix, angle, _cross.sg());
 307           SGVec3d newPos(0, 0, 0);
 308           for(int j = 0; j < 3; j++)
 309             {
 310               for (int k = 0; k<3; k++)
 311                 {
 312                   newPos[j] += matrix[j][k]*a[k];
 313                 }
 314             }
 315
 316           if (now > (*i)->getDepartureTime())
 317             {
 318               SGGeoc geoc = SGGeoc::fromCart(newPos);
 319               lat = geoc.getLatitudeDeg();
 320               lon = geoc.getLongitudeDeg();
 321             }
 322           else
 323             {
 324               lat = dep->getLatitude();
 325               lon = dep->getLongitude();
 326             }
 327
 328
 329           SGWayPoint current  (lon,
 330                                lat,
 331                                (*i)->getCruiseAlt(),
 332                                SGWayPoint::SPHERICAL);
 333           SGWayPoint user (   userLongitude,
 334                               userLatitude,
 335                               (*i)->getCruiseAlt(),
 336                               SGWayPoint::SPHERICAL);
 337           SGWayPoint dest (   arr->getLongitude(),
 338                               arr->getLatitude(),
 339                               (*i)->getCruiseAlt(),
 340                               SGWayPoint::SPHERICAL);
 341           // We really only need distance to user
 342           // and course to destination
 343           user.CourseAndDistance(current, &courseToUser, &distanceToUser);
 344           dest.CourseAndDistance(current, &courseToDest, &distanceToDest);
 345           speed =  (distanceToDest*SG_METER_TO_NM) /
 346             ((double) remainingTimeEnroute/3600.0);
 347
 348
 349           // If distance between user and simulated aircaft is less
 350           // then 500nm, create this flight. At jet speeds 500 nm is roughly
 351           // one hour flight time, so that would be a good approximate point
 352           // to start a more detailed simulation of this aircraft.
 353           //cerr << registration << " is currently enroute from "
 354           //   << dep->_id << " to " << arr->_id << "distance : "
 355           //   << distanceToUser*SG_METER_TO_NM << endl;
 356           if ((distanceToUser*SG_METER_TO_NM) < TRAFFICTOAIDIST)
 357             {
 358               string flightPlanName = dep->getId() + string("-") + arr->getId() +
 359                 string(".xml");
 360               //int alt;
 361               //if  ((i->getDepartureTime() < now))
 362               //{
 363               //          alt = i->getCruiseAlt() *100;
 364               //        }
 365               //else
 366               //{
 367               //          alt = dep->_elevation+19;
 368               //        }
 369
 370               // Only allow traffic to be created when the model path (or the AI version of mp) exists
 371               SGPath mp(globals->get_fg_root());
 372               SGPath mp_ai = mp;
 373
 374               mp.append(modelPath);
 375               mp_ai.append("AI");
 376               mp_ai.append(modelPath);
 377
 378               if (mp.exists() || mp_ai.exists())
 379               {
 380                   FGAIAircraft *aircraft = new FGAIAircraft(this);
 381                   aircraft->setPerformance(m_class); //"jet_transport";
 382                   aircraft->setCompany(airline); //i->getAirline();
 383                   aircraft->setAcType(acType); //i->getAcType();
 384                   aircraft->setPath(modelPath.c_str());
 385                   //aircraft->setFlightPlan(flightPlanName);
 386                   aircraft->setLatitude(lat);
 387                   aircraft->setLongitude(lon);
 388                   aircraft->setAltitude((*i)->getCruiseAlt()*100); // convert from FL to feet
 389                   aircraft->setSpeed(speed);
 390                   aircraft->setBank(0);
 391                   aircraft->SetFlightPlan(new FGAIFlightPlan(flightPlanName, courseToDest, deptime,
 392                                                              dep, arr,true, radius,
 393                                                              (*i)->getCruiseAlt()*100,
 394                                                              lat, lon, speed, flightType, acType,
 395                                                              airline));
 396                   aimgr->attach(aircraft);
 397
 398
 399                   AIManagerRef = aircraft->getID();
 400                   //cerr << "Class: " << m_class << ". acType: " << acType << ". Airline: " << airline << ". Speed = " << speed << ". From " << dep->getId() << " to " << arr->getId() << ". Time Fraction = " << (remainingTimeEnroute/(double) totalTimeEnroute) << endl;
 401                   //cerr << "Latitude : " << lat << ". Longitude : " << lon << endl;
 402                   //cerr << "Dep      : " << dep->getLatitude()<< ", "<< dep->getLongitude() << endl;
 403                   //cerr << "Arr      : " << arr->getLatitude()<< ", "<< arr->getLongitude() << endl;
 404                   //cerr << "Time remaining = " << (remainingTimeEnroute/3600.0) << endl;
 405                   //cerr << "Total time     = " << (totalTimeEnroute/3600.0) << endl;
 406                   //cerr << "Distance remaining = " << distanceToDest*SG_METER_TO_NM << endl;
 407                   }
 408               else
 409                 {
 410                   SG_LOG(SG_INPUT, SG_WARN, "TrafficManager: Could not load model " << mp.str());
 411                 }
 412             }
 413           return true;
 414     }
 415
 416       // Both departure and arrival time are in the future, so this
 417       // the aircraft is parked at the departure airport.
 418       // Currently this status is mostly ignored, but in future
 419       // versions, code should go here that -if within user range-
 420       // positions these aircraft at parking locations at the airport.
 421       if (((*i)->getDepartureTime() > now) && ((*i)->getArrivalTime() > now))
 422         {
 423           dep = (*i)->getDepartureAirport();
 424           return true;
 425         }
 426     }
 427   //cerr << "Traffic schedule got to beyond last clause" << endl;
 428     // EMH: prevent a warning, should this be 'true' instead?
 429     // DT: YES. Originally, this code couldn't be reached, but
 430     // when the "if(!(AIManagerManager))" clause is false we
 431     // fall through right to the end. This is a valid flow.
 432     // the actual value is pretty innocent, only it triggers
 433     // warning in TrafficManager::update().
 434     // (which was added as a sanity check for myself in the first place. :-)
 435     return true;
 436 }
 437
 438
 439 void FGAISchedule::next()
 440 {
 441   (*flights.begin())->update();
 442   sort(flights.begin(), flights.end(), compareScheduledFlights);
 443 }
 444
 445 double FGAISchedule::getSpeed()
 446 {
 447   double courseToDest;
 448   double distanceToDest;
 449   double speed, remainingTimeEnroute;
 450   FGAirport *dep, *arr;
 451
 452   FGScheduledFlightVecIterator i = flights.begin();
 453   dep = (*i)->getDepartureAirport();
 454   arr = (*i)->getArrivalAirport  ();
 455   if (!(dep && arr))
 456     return 0;
 457
 458   SGWayPoint dest (   dep->getLongitude(),
 459                       dep->getLatitude(),
 460                       (*i)->getCruiseAlt(),
 461                       SGWayPoint::SPHERICAL);
 462   SGWayPoint curr (    arr->getLongitude(),
 463                       arr->getLatitude(),
 464                        (*i)->getCruiseAlt(),
 465                        SGWayPoint::SPHERICAL);
 466   remainingTimeEnroute     = (*i)->getArrivalTime() - (*i)->getDepartureTime();
 467   dest.CourseAndDistance(curr, &courseToDest, &distanceToDest);
 468   speed =  (distanceToDest*SG_METER_TO_NM) /
 469     ((double) remainingTimeEnroute/3600.0);
 470   return speed;
 471 }
 472
 473 bool compareSchedules(FGAISchedule*a, FGAISchedule*b)
 474 {
 475   return (*a) < (*b);
 476 }
 477
 478
 479 // void FGAISchedule::setClosestDistanceToUser()
 480 // {
 481
 482
 483 //   double course;
 484 //   double dist;
 485
 486 //   Point3D temp;
 487 //   time_t
 488 //     totalTimeEnroute,
 489 //     elapsedTimeEnroute;
 490
 491 //   double userLatitude  = fgGetDouble("/position/latitude-deg");
 492 //   double userLongitude = fgGetDouble("/position/longitude-deg");
 493
 494 //   FGAirport *dep;
 495
 496 // #if defined( __CYGWIN__) || defined( __MINGW32__)
 497 //   #define HUGE HUGE_VAL
 498 // #endif
 499 //   distanceToUser = HUGE;
 500 //   FGScheduledFlightVecIterator i = flights.begin();
 501 //   while (i != flights.end())
 502 //     {
 503 //       dep = i->getDepartureAirport();
 504 //       //if (!(dep))
 505 //       //return HUGE;
 506
 507 //       SGWayPoint user (   userLongitude,
 508 //                        userLatitude,
 509 //                        i->getCruiseAlt());
 510 //       SGWayPoint current (dep->getLongitude(),
 511 //                        dep->getLatitude(),
 512 //                        0);
 513 //       user.CourseAndDistance(current, &course, &dist);
 514 //       if (dist < distanceToUser)
 515 //      {
 516 //        distanceToUser = dist;
 517 //        //cerr << "Found closest distance to user for " << registration << " to be " << distanceToUser << " at airport " << dep->getId() << endl;
 518 //      }
 519 //       i++;
 520 //     }
 521 //   //return distToUser;
 522 // }
 523