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       userLatitude  = fgGetDouble("/position/latitude-deg");
 161       userLongitude = fgGetDouble("/position/longitude-deg");
 162
 163
 164       // This flight entry is entirely in the past, do we need to
 165       // push it forward in time to the next scheduled departure.
 166       if ((i->getDepartureTime() < now) && (i->getArrivalTime() < now))
 167         {
 168           i->update();
 169           return;
 170         }
 171
 172       // Departure time in the past and arrival time in the future.
 173       // This flight is in progress, so we need to calculate it's
 174       // approximate position and -if in range- create an AIAircraft
 175       // object for it.
 176       if ((i->getDepartureTime() < now) && (i->getArrivalTime() > now))
 177         {
 178           dep = i->getDepartureAirport();
 179           arr = i->getArrivalAirport  ();
 180
 181           temp = sgPolarToCart3d(Point3D(dep->longitude *
 182                                          SG_DEGREES_TO_RADIANS,
 183                                          dep->latitude  *
 184                                          SG_DEGREES_TO_RADIANS,
 185                                          1.0));
 186           a[0] = temp.x();
 187           a[1] = temp.y();
 188           a[2] = temp.z();
 189
 190           temp = sgPolarToCart3d(Point3D(arr->longitude *
 191                                          SG_DEGREES_TO_RADIANS,
 192                                          arr->latitude  *
 193                                          SG_DEGREES_TO_RADIANS,
 194                                          1.0));
 195           b[0] = temp.x();
 196           b[1] = temp.y();
 197           b[2] = temp.z();
 198           sgdNormaliseVec3(a);
 199           sgdNormaliseVec3(b);
 200           sgdVectorProductVec3(cross,b,a);
 201
 202           angle = sgACos(sgdScalarProductVec3(a,b));
 203
 204           // Okay, at this point we have the angle between departure and
 205           // arrival airport, in degrees. From here we can interpolate the
 206           // position of the aircraft by calculating the ratio between
 207           // total time enroute and elapsed time enroute.
 208           totalTimeEnroute     = i->getArrivalTime() - i->getDepartureTime();
 209           elapsedTimeEnroute   = now - i->getDepartureTime();
 210           remainingTimeEnroute = i->getArrivalTime()   - now;
 211
 212           angle *= ( (double) elapsedTimeEnroute/ (double) totalTimeEnroute);
 213
 214
 215           //cout << "a = " << a[0] << " " << a[1] << " " << a[2]
 216           //     << "b = " << b[0] << " " << b[1] << " " << b[2] << endl;
 217           sgdMakeRotMat4(matrix, angle, cross);
 218           for(int j = 0; j < 3; j++)
 219             {
 220               newPos[j] =0.0;
 221               for (int k = 0; k<3; k++)
 222                 {
 223                   newPos[j] += matrix[j][k]*a[k];
 224                 }
 225             }
 226
 227           temp = sgCartToPolar3d(Point3D(newPos[0], newPos[1],newPos[2]));
 228
 229           lat = temp.lat() * SG_RADIANS_TO_DEGREES;
 230           lon = temp.lon() * SG_RADIANS_TO_DEGREES;
 231
 232           SGWayPoint current  (lon,
 233                                lat,
 234                                i->getCruiseAlt());
 235           SGWayPoint user (   userLongitude,
 236                               userLatitude,
 237                               i->getCruiseAlt());
 238           SGWayPoint dest (   arr->longitude,
 239                               arr->latitude,
 240                               i->getCruiseAlt());
 241           // We really only need distance to user
 242           // and course to destination
 243           current.CourseAndDistance(user, &courseToUser, &distanceToUser);
 244           current.CourseAndDistance(dest, &courseToDest, &distanceToDest);
 245           speed =  (distanceToDest*SG_METER_TO_NM) /
 246             ((double) remainingTimeEnroute/3600.0);
 247
 248
 249           // If distance between user and simulated aircaft is less
 250           // then 500nm, create this flight. At jet speeds 500 nm is roughly
 251           // one hour flight time, so that would be a good approximate point
 252           // to start a more detailed simulation of this aircraft.
 253           //cerr << registration << " is currently enroute from "
 254           //   << dep->id << " to " << arr->id << "distance : "
 255           //   << distanceToUser*SG_METER_TO_NM << endl;
 256           if ((distanceToUser*SG_METER_TO_NM) < 500.0)
 257             {
 258               string flightPlanName = dep->id + string("-") + arr->id +
 259                 string(".xml");
 260
 261               FGAIModelEntity entity;
 262
 263               entity.m_class = "jet_transport";
 264               entity.path = modelPath.c_str();
 265               entity.flightplan = flightPlanName.c_str();
 266               entity.latitude = lat;
 267               entity.longitude = lon;
 268               entity.altitude = i->getCruiseAlt() * 100; // convert from FL to feet
 269               entity.speed = 450;
 270               entity.fp = new FGAIFlightPlan(&entity, courseToDest, dep, arr);
 271
 272               // Fixme: A non-existent model path results in an
 273               // abort, due to an unhandled exeption, in fg main loop.
 274               AIManagerRef = aimgr->createAircraft( &entity );
 275               //cerr << "Created: " << AIManagerRef << endl;
 276             }
 277           return;
 278         }
 279
 280       // Both departure and arrival time are in the future, so this
 281       // the aircraft is parked at the departure airport.
 282       // Currently this status is mostly ignored, but in furture
 283       // versions, code should go here that -if within user range-
 284       // positions these aircraft at parking locations at the airport.
 285       if ((i->getDepartureTime() > now) && (i->getArrivalTime() > now))
 286         {
 287           dep = i->getDepartureAirport();
 288           return;
 289         }
 290     }
 291 }