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(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
 140 }
 141
 142 bool FGAISchedule::init()
 143 {
 144   //tm targetTimeDate;
 145   //SGTime* currTimeDate = globals->get_time_params();
 146
 147   //tm *temp = currTimeDate->getGmt();
 148   //char buffer[512];
 149   //sgTimeFormatTime(&targetTimeDate, buffer);
 150   //cout << "Scheduled Time " << buffer << endl;
 151   //cout << "Time :" << time(NULL) << " SGTime : " << sgTimeGetGMT(temp) << endl;
 152   for (FGScheduledFlightVecIterator i = flights.begin();
 153        i != flights.end();
 154        i++)
 155     {
 156       //i->adjustTime(now);
 157       if (!(i->initializeAirports()))
 158         return false;
 159     }
 160   //sort(flights.begin(), flights.end());
 161   // Since time isn't initialized yet when this function is called,
 162   // Find the closest possible airport.
 163   // This should give a reasonable initialization order.
 164   //setClosestDistanceToUser();
 165   return true;
 166 }
 167
 168 bool FGAISchedule::update(time_t now)
 169 {
 170   FGAirport *dep;
 171   FGAirport *arr;
 172   sgdVec3 a, b, cross;
 173   sgdVec3 newPos;
 174   sgdMat4 matrix;
 175   double angle;
 176
 177   FGAIManager *aimgr;
 178   string airport;
 179
 180   double courseToUser,   courseToDest;
 181   double distanceToDest;
 182   double speed;
 183
 184   Point3D temp;
 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;
 223       firstRun = false;
 224     }
 225
 226   // Sort all the scheduled flights according to scheduled departure time.
 227   // Because this is done at every update, we only need to check the status
 228   // of the first listed flight.
 229   sort(flights.begin(), flights.end());
 230   if (!deptime)
 231     deptime = flights.begin()->getDepartureTime();
 232   FGScheduledFlightVecIterator i = flights.begin();
 233   if (AIManagerRef)
 234     {
 235       // Check if this aircraft has been released.
 236       FGTrafficManager *tmgr = (FGTrafficManager *) globals->get_subsystem("Traffic Manager");
 237       if (tmgr->isReleased(AIManagerRef))
 238         AIManagerRef = 0;
 239     }
 240
 241   if (!AIManagerRef)
 242     {
 243       userLatitude  = fgGetDouble("/position/latitude-deg");
 244       userLongitude = fgGetDouble("/position/longitude-deg");
 245
 246       //cerr << "Estimated minimum distance to user: " << distanceToUser << endl;
 247       // This flight entry is entirely in the past, do we need to
 248       // push it forward in time to the next scheduled departure.
 249       if ((i->getDepartureTime() < now) && (i->getArrivalTime() < now))
 250         {
 251           i->update();
 252           return true;
 253         }
 254
 255       // Departure time in the past and arrival time in the future.
 256       // This flight is in progress, so we need to calculate it's
 257       // approximate position and -if in range- create an AIAircraft
 258       // object for it.
 259       //if ((i->getDepartureTime() < now) && (i->getArrivalTime() > now))
 260
 261
 262       // Part of this flight is in the future.
 263       if (i->getArrivalTime() > now)
 264         {
 265           dep = i->getDepartureAirport();
 266           arr = i->getArrivalAirport  ();
 267           if (!(dep && arr))
 268             return false;
 269
 270           temp = sgPolarToCart3d(Point3D(dep->getLongitude() *
 271                                          SG_DEGREES_TO_RADIANS,
 272                                          dep->getLatitude()  *
 273                                          SG_DEGREES_TO_RADIANS,
 274                                          1.0));
 275           a[0] = temp.x();
 276           a[1] = temp.y();
 277           a[2] = temp.z();
 278
 279           temp = sgPolarToCart3d(Point3D(arr->getLongitude() *
 280                                          SG_DEGREES_TO_RADIANS,
 281                                          arr->getLatitude()  *
 282                                          SG_DEGREES_TO_RADIANS,
 283                                          1.0));
 284           b[0] = temp.x();
 285           b[1] = temp.y();
 286           b[2] = temp.z();
 287           sgdNormaliseVec3(a);
 288           sgdNormaliseVec3(b);
 289           sgdVectorProductVec3(cross,b,a);
 290
 291           angle = sgACos(sgdScalarProductVec3(a,b));
 292
 293           // Okay, at this point we have the angle between departure and
 294           // arrival airport, in degrees. From here we can interpolate the
 295           // position of the aircraft by calculating the ratio between
 296           // total time enroute and elapsed time enroute.
 297
 298           totalTimeEnroute     = i->getArrivalTime() - i->getDepartureTime();
 299           if (now > i->getDepartureTime())
 300             {
 301               //err << "Lat = " << lat << ", lon = " << lon << endl;
 302               //cerr << "Time diff: " << now-i->getDepartureTime() << endl;
 303               elapsedTimeEnroute   = now - i->getDepartureTime();
 304               remainingTimeEnroute = i->getArrivalTime()   - now;
 305             }
 306           else
 307             {
 308               lat = dep->getLatitude();
 309               lon = dep->getLongitude();
 310               elapsedTimeEnroute = 0;
 311               remainingTimeEnroute = totalTimeEnroute;
 312             }
 313
 314           angle *= ( (double) elapsedTimeEnroute/ (double) totalTimeEnroute);
 315
 316
 317           //cout << "a = " << a[0] << " " << a[1] << " " << a[2]
 318           //     << "b = " << b[0] << " " << b[1] << " " << b[2] << endl;
 319           sgdMakeRotMat4(matrix, angle, cross);
 320           for(int j = 0; j < 3; j++)
 321             {
 322               newPos[j] =0.0;
 323               for (int k = 0; k<3; k++)
 324                 {
 325                   newPos[j] += matrix[j][k]*a[k];
 326                 }
 327             }
 328
 329           temp = sgCartToPolar3d(Point3D(newPos[0], newPos[1],newPos[2]));
 330           if (now > i->getDepartureTime())
 331             {
 332               //cerr << "Lat = " << lat << ", lon = " << lon << endl;
 333               //cerr << "Time diff: " << now-i->getDepartureTime() << endl;
 334               lat = temp.lat() * SG_RADIANS_TO_DEGREES;
 335               lon = temp.lon() * SG_RADIANS_TO_DEGREES;
 336             }
 337           else
 338             {
 339               lat = dep->getLatitude();
 340               lon = dep->getLongitude();
 341             }
 342
 343
 344           SGWayPoint current  (lon,
 345                                lat,
 346                                i->getCruiseAlt());
 347           SGWayPoint user (   userLongitude,
 348                               userLatitude,
 349                               i->getCruiseAlt());
 350           SGWayPoint dest (   arr->getLongitude(),
 351                               arr->getLatitude(),
 352                               i->getCruiseAlt());
 353           // We really only need distance to user
 354           // and course to destination
 355           user.CourseAndDistance(current, &courseToUser, &distanceToUser);
 356           dest.CourseAndDistance(current, &courseToDest, &distanceToDest);
 357           speed =  (distanceToDest*SG_METER_TO_NM) /
 358             ((double) remainingTimeEnroute/3600.0);
 359
 360
 361           // If distance between user and simulated aircaft is less
 362           // then 500nm, create this flight. At jet speeds 500 nm is roughly
 363           // one hour flight time, so that would be a good approximate point
 364           // to start a more detailed simulation of this aircraft.
 365           //cerr << registration << " is currently enroute from "
 366           //   << dep->_id << " to " << arr->_id << "distance : "
 367           //   << distanceToUser*SG_METER_TO_NM << endl;
 368           if ((distanceToUser*SG_METER_TO_NM) < TRAFFICTOAIDIST)
 369             {
 370               string flightPlanName = dep->getId() + string("-") + arr->getId() +
 371                 string(".xml");
 372               int alt;
 373               //if  ((i->getDepartureTime() < now))
 374               //{
 375               //          alt = i->getCruiseAlt() *100;
 376               //        }
 377               //else
 378               //{
 379               //          alt = dep->_elevation+19;
 380               //        }
 381
 382               // Only allow traffic to be created when the model path exists
 383               SGPath mp(globals->get_fg_root());
 384               mp.append(modelPath);
 385               if (mp.exists())
 386               {
 387                   FGAIAircraft *aircraft = new FGAIAircraft(this);
 388                   aircraft->setPerformance(m_class); //"jet_transport";
 389                   aircraft->setCompany(airline); //i->getAirline();
 390                   aircraft->setAcType(acType); //i->getAcType();
 391                   aircraft->setPath(modelPath.c_str());
 392                   //aircraft->setFlightPlan(flightPlanName);
 393                   aircraft->setLatitude(lat);
 394                   aircraft->setLongitude(lon);
 395                   aircraft->setAltitude(i->getCruiseAlt()*100); // convert from FL to feet
 396                   aircraft->setSpeed(speed);
 397                   aircraft->setBank(0);
 398                   aircraft->SetFlightPlan(new FGAIFlightPlan(flightPlanName, courseToDest, deptime,
 399                                                              dep, arr,true, radius,
 400                                                              i->getCruiseAlt()*100,
 401                                                              lat, lon, speed, flightType, acType,
 402                                                              airline));
 403                   aimgr->attach(aircraft);
 404
 405
 406                   AIManagerRef = aircraft->getID();
 407                   //cerr << "Class: " << m_class << ". acType: " << acType << ". Airline: " << airline << ". Speed = " << speed << ". From " << dep->getId() << " to " << arr->getId() << ". Time Fraction = " << (remainingTimeEnroute/(double) totalTimeEnroute) << endl;
 408                   //cerr << "Latitude : " << lat << ". Longitude : " << lon << endl;
 409                   //cerr << "Dep      : " << dep->getLatitude()<< ", "<< dep->getLongitude() << endl;
 410                   //cerr << "Arr      : " << arr->getLatitude()<< ", "<< arr->getLongitude() << endl;
 411                   //cerr << "Time remaining = " << (remainingTimeEnroute/3600.0) << endl;
 412                   //cerr << "Total time     = " << (totalTimeEnroute/3600.0) << endl;
 413                   //cerr << "Distance remaining = " << distanceToDest*SG_METER_TO_NM << endl;
 414                   }
 415               else
 416                 {
 417                   SG_LOG(SG_INPUT, SG_WARN, "TrafficManager: Could not load model " << mp.str());
 418                 }
 419             }
 420           return true;
 421     }
 422
 423       // Both departure and arrival time are in the future, so this
 424       // the aircraft is parked at the departure airport.
 425       // Currently this status is mostly ignored, but in future
 426       // versions, code should go here that -if within user range-
 427       // positions these aircraft at parking locations at the airport.
 428   if ((i->getDepartureTime() > now) && (i->getArrivalTime() > now))
 429         {
 430           dep = i->getDepartureAirport();
 431           return true;
 432         }
 433     }
 434   //cerr << "Traffic schedule got to beyond last clause" << endl;
 435     // EMH: prevent a warning, should this be 'true' instead?
 436     // DT: YES. Originally, this code couldn't be reached, but
 437     // when the "if(!(AIManagerManager))" clause is false we
 438     // fall through right to the end. This is a valid flow.
 439     // the actual value is pretty innocent, only it triggers
 440     // warning in TrafficManager::update().
 441     // (which was added as a sanity check for myself in the first place. :-)
 442     return true;
 443 }
 444
 445
 446 void FGAISchedule::next()
 447 {
 448   flights.begin()->update();
 449   sort(flights.begin(), flights.end());
 450 }
 451
 452 double FGAISchedule::getSpeed()
 453 {
 454   double courseToUser,   courseToDest;
 455   double distanceToUser, distanceToDest;
 456   double speed, remainingTimeEnroute;
 457   FGAirport *dep, *arr;
 458
 459   FGScheduledFlightVecIterator i = flights.begin();
 460   dep = i->getDepartureAirport();
 461   arr = i->getArrivalAirport  ();
 462   if (!(dep && arr))
 463     return 0;
 464
 465   SGWayPoint dest (   dep->getLongitude(),
 466                       dep->getLatitude(),
 467                       i->getCruiseAlt());
 468   SGWayPoint curr (    arr->getLongitude(),
 469                       arr->getLatitude(),
 470                       i->getCruiseAlt());
 471   remainingTimeEnroute     = i->getArrivalTime() - i->getDepartureTime();
 472   dest.CourseAndDistance(curr, &courseToDest, &distanceToDest);
 473   speed =  (distanceToDest*SG_METER_TO_NM) /
 474     ((double) remainingTimeEnroute/3600.0);
 475   return speed;
 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 // }