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               AIManagerRef = aimgr->createAircraft( &entity, this);
 371               //cerr << "Class: " << m_class << ". acType: " << acType << ". Airline: " << airline << ". Speed = " << speed << ". From " << dep->getId() << " to " << arr->getId() << ". Time Fraction = " << (remainingTimeEnroute/(double) totalTimeEnroute) << endl;
 372               //cerr << "Latitude : " << lat << ". Longitude : " << lon << endl;
 373               //cerr << "Dep      : " << dep->getLatitude()<< ", "<< dep->getLongitude() << endl;
 374               //cerr << "Arr      : " << arr->getLatitude()<< ", "<< arr->getLongitude() << endl;
 375               //cerr << "Time remaining = " << (remainingTimeEnroute/3600.0) << endl;
 376               //cerr << "Total time     = " << (totalTimeEnroute/3600.0) << endl;
 377               //cerr << "Distance remaining = " << distanceToDest*SG_METER_TO_NM << endl;
 378
 379             }
 380           return true;
 381     }
 382
 383       // Both departure and arrival time are in the future, so this
 384       // the aircraft is parked at the departure airport.
 385       // Currently this status is mostly ignored, but in future
 386       // versions, code should go here that -if within user range-
 387       // positions these aircraft at parking locations at the airport.
 388   if ((i->getDepartureTime() > now) && (i->getArrivalTime() > now))
 389         {
 390           dep = i->getDepartureAirport();
 391           return true;
 392         }
 393     }
 394
 395     // EMH: prevent a warning, should this be 'true' instead?
 396     return false;
 397 }
 398
 399
 400 void FGAISchedule::next()
 401 {
 402   flights.begin()->update();
 403   sort(flights.begin(), flights.end());
 404 }
 405
 406 double FGAISchedule::getSpeed()
 407 {
 408   double courseToUser,   courseToDest;
 409   double distanceToUser, distanceToDest;
 410   double speed, remainingTimeEnroute;
 411   FGAirport *dep, *arr;
 412
 413   FGScheduledFlightVecIterator i = flights.begin();
 414   dep = i->getDepartureAirport();
 415   arr = i->getArrivalAirport  ();
 416   if (!(dep && arr))
 417     return 0;
 418
 419   SGWayPoint dest (   dep->getLongitude(),
 420                       dep->getLatitude(),
 421                       i->getCruiseAlt());
 422   SGWayPoint curr (    arr->getLongitude(),
 423                       arr->getLatitude(),
 424                       i->getCruiseAlt());
 425   remainingTimeEnroute     = i->getArrivalTime() - i->getDepartureTime();
 426   dest.CourseAndDistance(curr, &courseToDest, &distanceToDest);
 427   speed =  (distanceToDest*SG_METER_TO_NM) /
 428     ((double) remainingTimeEnroute/3600.0);
 429   return speed;
 430 }
 431
 432
 433 void FGAISchedule::setClosestDistanceToUser()
 434 {
 435
 436
 437   double course;
 438   double dist;
 439
 440   Point3D temp;
 441   time_t
 442     totalTimeEnroute,
 443     elapsedTimeEnroute;
 444
 445   double userLatitude  = fgGetDouble("/position/latitude-deg");
 446   double userLongitude = fgGetDouble("/position/longitude-deg");
 447
 448   FGAirport *dep;
 449
 450 #ifdef __CYGWIN__
 451   #define HUGE HUGE_VAL
 452 #endif
 453   distanceToUser = HUGE;
 454   FGScheduledFlightVecIterator i = flights.begin();
 455   while (i != flights.end())
 456     {
 457       dep = i->getDepartureAirport();
 458       //if (!(dep))
 459       //return HUGE;
 460
 461       SGWayPoint user (   userLongitude,
 462                           userLatitude,
 463                           i->getCruiseAlt());
 464       SGWayPoint current (dep->getLongitude(),
 465                           dep->getLatitude(),
 466                           0);
 467       user.CourseAndDistance(current, &course, &dist);
 468       if (dist < distanceToUser)
 469         {
 470           distanceToUser = dist;
 471           //cerr << "Found closest distance to user for " << registration << " to be " << distanceToUser << " at airport " << dep->getId() << endl;
 472         }
 473       i++;
 474     }
 475   //return distToUser;
 476 }