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