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