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                            FGScheduledFlightVec flt)
  69 {
  70   modelPath    = mdl;
  71   livery       = liv;
  72   registration = reg;
  73   heavy = hvy;
  74   for (FGScheduledFlightVecIterator i = flt.begin();
  75        i != flt.end();
  76        i++)
  77     flights.push_back(FGScheduledFlight((*i)));
  78   AIManagerRef = 0;
  79   firstRun = true;
  80 }
  81
  82 FGAISchedule::FGAISchedule(const FGAISchedule &other)
  83 {
  84   modelPath    = other.modelPath;
  85   livery       = other.livery;
  86   registration = other.registration;
  87   heavy        = other.heavy;
  88   flights      = other.flights;
  89   lat          = other.lat;
  90   lon          = other.lon;
  91   AIManagerRef = other.AIManagerRef;
  92   firstRun     = other.firstRun;
  93 }
  94
  95 FGAISchedule::~FGAISchedule()
  96 {
  97
  98 }
  99
 100 void FGAISchedule::update(time_t now)
 101 {
 102   FGAirport *dep;
 103   FGAirport *arr;
 104   sgdVec3 a, b, cross;
 105   sgdVec3 newPos;
 106   sgdMat4 matrix;
 107   double angle;
 108
 109   FGAIManager *aimgr;
 110   string airport;
 111
 112   double courseToUser,   courseToDest;
 113   double distanceToUser, distanceToDest;
 114   double speed;
 115
 116   Point3D temp;
 117   time_t
 118     totalTimeEnroute,
 119     elapsedTimeEnroute,
 120     remainingTimeEnroute;
 121   double
 122     userLatitude,
 123     userLongitude;
 124
 125   if (fgGetBool("/sim/traffic-manager/enabled") == false)
 126     return;
 127
 128   aimgr = (FGAIManager *) globals-> get_subsystem("ai_model");
 129   // Before the flight status of this traffic entity is updated
 130   // for the first time, we need to roll back it's flight schedule so
 131   // so that all the flights are centered around this simulated week's time
 132   // table. This is to avoid the situation where the first scheduled flight is
 133   // in the future, causing the traffic manager to not generate traffic until
 134   // simulated time has caught up with the real world time at initialization.
 135   // This is to counter a more general initialization bug, caused by the fact
 136   // that warp is not yet set when the  schedule is initialized. This is
 137   // especially a problem when using a negative time offset.
 138   // i.e let's say we specify FlightGear to run with --time-offset=-24:00:00.
 139   // Then the schedule will initialize using today, but we will fly yesterday.
 140   // Thus, it would take a whole day of simulation before the traffic manager
 141   // finally kicks in.
 142   if (firstRun)
 143     {
 144       for (FGScheduledFlightVecIterator i = flights.begin();
 145            i != flights.end();
 146            i++)
 147         {
 148           i->adjustTime(now);
 149         }
 150       firstRun = false;
 151     }
 152
 153   // Sort all the scheduled flights according to scheduled departure time.
 154   // Because this is done at every update, we only need to check the status
 155   // of the first listed flight.
 156   sort(flights.begin(), flights.end());
 157   FGScheduledFlightVecIterator i = flights.begin();
 158   if (AIManagerRef)
 159     {
 160       // Check if this aircraft has been released.
 161       FGTrafficManager *tmgr = (FGTrafficManager *) globals->get_subsystem("Traffic Manager");
 162       if (tmgr->isReleased(AIManagerRef))
 163         AIManagerRef = 0;
 164     }
 165
 166   if (!AIManagerRef)
 167     {
 168       userLatitude  = fgGetDouble("/position/latitude-deg");
 169       userLongitude = fgGetDouble("/position/longitude-deg");
 170
 171
 172       // This flight entry is entirely in the past, do we need to
 173       // push it forward in time to the next scheduled departure.
 174       if ((i->getDepartureTime() < now) && (i->getArrivalTime() < now))
 175         {
 176           i->update();
 177           return;
 178         }
 179
 180       // Departure time in the past and arrival time in the future.
 181       // This flight is in progress, so we need to calculate it's
 182       // approximate position and -if in range- create an AIAircraft
 183       // object for it.
 184       //if ((i->getDepartureTime() < now) && (i->getArrivalTime() > now))
 185
 186
 187       // Part of this flight is in the future.
 188       if (i->getArrivalTime() > now)
 189         {
 190           dep = i->getDepartureAirport();
 191           arr = i->getArrivalAirport  ();
 192
 193           temp = sgPolarToCart3d(Point3D(dep->_longitude *
 194                                          SG_DEGREES_TO_RADIANS,
 195                                          dep->_latitude  *
 196                                          SG_DEGREES_TO_RADIANS,
 197                                          1.0));
 198           a[0] = temp.x();
 199           a[1] = temp.y();
 200           a[2] = temp.z();
 201
 202           temp = sgPolarToCart3d(Point3D(arr->_longitude *
 203                                          SG_DEGREES_TO_RADIANS,
 204                                          arr->_latitude  *
 205                                          SG_DEGREES_TO_RADIANS,
 206                                          1.0));
 207           b[0] = temp.x();
 208           b[1] = temp.y();
 209           b[2] = temp.z();
 210           sgdNormaliseVec3(a);
 211           sgdNormaliseVec3(b);
 212           sgdVectorProductVec3(cross,b,a);
 213
 214           angle = sgACos(sgdScalarProductVec3(a,b));
 215
 216           // Okay, at this point we have the angle between departure and
 217           // arrival airport, in degrees. From here we can interpolate the
 218           // position of the aircraft by calculating the ratio between
 219           // total time enroute and elapsed time enroute.
 220
 221           totalTimeEnroute     = i->getArrivalTime() - i->getDepartureTime();
 222           elapsedTimeEnroute   = now - i->getDepartureTime();
 223           remainingTimeEnroute = i->getArrivalTime()   - now;
 224
 225           angle *= ( (double) elapsedTimeEnroute/ (double) totalTimeEnroute);
 226
 227
 228           //cout << "a = " << a[0] << " " << a[1] << " " << a[2]
 229           //     << "b = " << b[0] << " " << b[1] << " " << b[2] << endl;
 230           sgdMakeRotMat4(matrix, angle, cross);
 231           for(int j = 0; j < 3; j++)
 232             {
 233               newPos[j] =0.0;
 234               for (int k = 0; k<3; k++)
 235                 {
 236                   newPos[j] += matrix[j][k]*a[k];
 237                 }
 238             }
 239
 240           temp = sgCartToPolar3d(Point3D(newPos[0], newPos[1],newPos[2]));
 241
 242           if (now > i->getDepartureTime())
 243             {
 244               //cerr << "Lat = " << lat << ", lon = " << lon << endl;
 245               //cerr << "Time diff: " << now-i->getDepartureTime() << endl;
 246               lat = temp.lat() * SG_RADIANS_TO_DEGREES;
 247               lon = temp.lon() * SG_RADIANS_TO_DEGREES;
 248               //err << "Lat = " << lat << ", lon = " << lon << endl;
 249               //cerr << "Time diff: " << now-i->getDepartureTime() << endl;
 250             }
 251           else
 252             {
 253               lat = dep->_latitude;
 254               lon = dep->_longitude;
 255             }
 256
 257           SGWayPoint current  (lon,
 258                                lat,
 259                                i->getCruiseAlt());
 260           SGWayPoint user (   userLongitude,
 261                               userLatitude,
 262                               i->getCruiseAlt());
 263           SGWayPoint dest (   arr->_longitude,
 264                               arr->_latitude,
 265                               i->getCruiseAlt());
 266           // We really only need distance to user
 267           // and course to destination
 268           user.CourseAndDistance(current, &courseToUser, &distanceToUser);
 269           dest.CourseAndDistance(current, &courseToDest, &distanceToDest);
 270           speed =  (distanceToDest*SG_METER_TO_NM) /
 271             ((double) remainingTimeEnroute/3600.0);
 272
 273
 274           // If distance between user and simulated aircaft is less
 275           // then 500nm, create this flight. At jet speeds 500 nm is roughly
 276           // one hour flight time, so that would be a good approximate point
 277           // to start a more detailed simulation of this aircraft.
 278           //cerr << registration << " is currently enroute from "
 279           //   << dep->_id << " to " << arr->_id << "distance : "
 280           //   << distanceToUser*SG_METER_TO_NM << endl;
 281           if ((distanceToUser*SG_METER_TO_NM) < 500.0)
 282             {
 283               string flightPlanName = dep->_id + string("-") + arr->_id +
 284                 string(".xml");
 285               int alt;
 286               //if  ((i->getDepartureTime() < now))
 287               //{
 288               //          alt = i->getCruiseAlt() *100;
 289               //        }
 290               //else
 291               //{
 292               //          alt = dep->_elevation+19;
 293               //        }
 294
 295               FGAIModelEntity entity;
 296
 297               entity.m_class = "jet_transport";
 298               entity.path = modelPath.c_str();
 299               entity.flightplan = flightPlanName.c_str();
 300               entity.latitude = lat;
 301               entity.longitude = lon;
 302               entity.altitude = i->getCruiseAlt() *100; // convert from FL to feet
 303               entity.speed = 450;
 304               entity.fp = new FGAIFlightPlan(&entity, courseToDest, i->getDepartureTime(), dep, arr);
 305
 306               // Fixme: A non-existent model path results in an
 307               // abort, due to an unhandled exeption, in fg main loop.
 308               AIManagerRef = aimgr->createAircraft( &entity, this);
 309               //cerr << "Created: " << AIManagerRef << endl;
 310             }
 311           return;
 312         }
 313
 314       // Both departure and arrival time are in the future, so this
 315       // the aircraft is parked at the departure airport.
 316       // Currently this status is mostly ignored, but in future
 317       // versions, code should go here that -if within user range-
 318       // positions these aircraft at parking locations at the airport.
 319       if ((i->getDepartureTime() > now) && (i->getArrivalTime() > now))
 320         {
 321           dep = i->getDepartureAirport();
 322           return;
 323         }
 324     }
 325 }
 326
 327
 328 void FGAISchedule::next()
 329 {
 330   flights.begin()->update();
 331   sort(flights.begin(), flights.end());
 332 }
 333