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