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