Display AI traffic route in map.
[fg:flightgear.git] / src / AIModel / AIFlightPlan.cxx
1 // // FGAIFlightPlan - class for loading and storing  AI flight plans
2 // Written by David Culp, started May 2004
3 // - davidculp2@comcast.net
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 #ifdef HAVE_CONFIG_H
20 #  include <config.h>
21 #endif
22
23 #include <iostream>
24
25 #include <simgear/misc/sg_path.hxx>
26 #include <simgear/debug/logstream.hxx>
27 #include <simgear/math/sg_geodesy.hxx>
28 #include <simgear/structure/exception.hxx>
29 #include <simgear/constants.h>
30 #include <simgear/props/props.hxx>
31 #include <simgear/props/props_io.hxx>
32
33 #include <Main/globals.hxx>
34 #include <Main/fg_props.hxx>
35 #include <Main/fg_init.hxx>
36 #include <Airports/airport.hxx>
37 #include <Airports/dynamics.hxx>
38 #include <Airports/runways.hxx>
39 #include <Airports/groundnetwork.hxx>
40
41 #include <Environment/environment_mgr.hxx>
42 #include <Environment/environment.hxx>
43
44 #include "AIFlightPlan.hxx"
45 #include "AIAircraft.hxx"
46
47 using std::cerr;
48 using std::string;
49
50 FGAIWaypoint::FGAIWaypoint() {
51   speed       = 0;
52   crossat     = 0;
53   finished    = 0;
54   gear_down   = 0;
55   flaps_down  = 0;
56   on_ground   = 0;
57   routeIndex  = 0;
58   time_sec    = 0;
59   trackLength = 0;
60 }
61
62 bool FGAIWaypoint::contains(const string& target) {
63     size_t found = name.find(target);
64     if (found == string::npos)
65         return false;
66     else
67         return true;
68 }
69
70 double FGAIWaypoint::getLatitude()
71 {
72   return pos.getLatitudeDeg();
73 }
74
75 double FGAIWaypoint::getLongitude()
76 {
77   return pos.getLongitudeDeg();
78 }
79
80 double FGAIWaypoint::getAltitude()
81 {
82   return pos.getElevationFt();
83 }
84
85 void FGAIWaypoint::setLatitude(double lat)
86 {
87   pos.setLatitudeDeg(lat);
88 }
89
90 void FGAIWaypoint::setLongitude(double lon)
91 {
92   pos.setLongitudeDeg(lon);
93 }
94
95 void FGAIWaypoint::setAltitude(double alt)
96 {
97   pos.setElevationFt(alt);
98 }
99
100 FGAIFlightPlan::FGAIFlightPlan() :
101     sid(NULL),
102     repeat(false),
103     distance_to_go(0),
104     lead_distance(0),
105     leadInAngle(0),
106     start_time(0),
107     arrivalTime(0),
108     leg(0),
109     lastNodeVisited(0),
110     isValid(true)
111 {
112     wpt_iterator    = waypoints.begin();
113 }
114
115 FGAIFlightPlan::FGAIFlightPlan(const string& filename) :
116     sid(NULL),
117     repeat(false),
118     distance_to_go(0),
119     lead_distance(0),
120     leadInAngle(0),
121     start_time(0),
122     arrivalTime(0),
123     leg(10),
124     lastNodeVisited(0),
125     isValid(parseProperties(filename))
126 {
127 }
128
129
130 // This is a modified version of the constructor,
131 // Which not only reads the waypoints from a 
132 // Flight plan file, but also adds the current
133 // Position computed by the traffic manager, as well
134 // as setting speeds and altitude computed by the
135 // traffic manager. 
136 FGAIFlightPlan::FGAIFlightPlan(FGAIAircraft *ac,
137                                const std::string& p,
138                                double course,
139                                time_t start,
140                                FGAirport *dep,
141                                FGAirport *arr,
142                                bool firstLeg,
143                                double radius,
144                                double alt,
145                                double lat,
146                                double lon,
147                                double speed,
148                                const string& fltType,
149                                const string& acType,
150                                const string& airline) :
151     sid(NULL),
152     repeat(false),
153     distance_to_go(0),
154     lead_distance(0),
155     leadInAngle(0),
156     start_time(start),
157     arrivalTime(0),
158     leg(10),
159     lastNodeVisited(0),
160     isValid(false),
161     departure(dep),
162     arrival(arr)
163 {
164   if (parseProperties(p)) {
165     isValid = true;
166   } else {
167     createWaypoints(ac, course, start, dep, arr, firstLeg, radius,
168                     alt, lat, lon, speed, fltType, acType, airline);
169   }
170 }
171
172 FGAIFlightPlan::~FGAIFlightPlan()
173 {
174   deleteWaypoints();
175   //delete taxiRoute;
176 }
177
178 void FGAIFlightPlan::createWaypoints(FGAIAircraft *ac,
179                                      double course,
180                                      time_t start,
181                                      FGAirport *dep,
182                                      FGAirport *arr,
183                                      bool firstLeg,
184                                      double radius,
185                                      double alt,
186                                      double lat,
187                                      double lon,
188                                      double speed,
189                                      const string& fltType,
190                                      const string& acType,
191                                      const string& airline)
192 {
193   time_t now = time(NULL) + fgGetLong("/sim/time/warp");
194   time_t timeDiff = now-start;
195   leg = 1;
196   
197   if ((timeDiff > 60) && (timeDiff < 1500))
198     leg = 2;
199   //else if ((timeDiff >= 1200) && (timeDiff < 1500)) {
200         //leg = 3;
201   //ac->setTakeOffStatus(2);
202   //}
203   else if ((timeDiff >= 1500) && (timeDiff < 2000))
204     leg = 4;
205   else if (timeDiff >= 2000)
206     leg = 5;
207   /*
208    if (timeDiff >= 2000)
209    leg = 5;
210    */
211   SG_LOG(SG_AI, SG_INFO, "Route from " << dep->getId() << " to " << arr->getId() << ". Set leg to : " << leg << " " << ac->getTrafficRef()->getCallSign());
212   wpt_iterator = waypoints.begin();
213   bool dist = 0;
214   isValid = create(ac, dep, arr, leg, alt, speed, lat, lon,
215                    firstLeg, radius, fltType, acType, airline, dist);
216   wpt_iterator = waypoints.begin();
217 }
218
219 bool FGAIFlightPlan::parseProperties(const std::string& filename)
220 {
221   SGPath path( globals->get_fg_root() );
222   path.append( "/AI/FlightPlans/" + filename );
223   if (!path.exists()) {
224     return false;
225   }
226   
227   SGPropertyNode root;
228   try {
229     readProperties(path.str(), &root);
230   } catch (const sg_exception &e) {
231     SG_LOG(SG_AI, SG_ALERT, "Error reading AI flight plan: " << path.str()
232            << "message:" << e.getFormattedMessage());
233     return false;
234   }
235   
236   SGPropertyNode * node = root.getNode("flightplan");
237   for (int i = 0; i < node->nChildren(); i++) {
238     FGAIWaypoint* wpt = new FGAIWaypoint;
239     SGPropertyNode * wpt_node = node->getChild(i);
240     wpt->setName       (wpt_node->getStringValue("name", "END"     ));
241     wpt->setLatitude   (wpt_node->getDoubleValue("lat", 0          ));
242     wpt->setLongitude  (wpt_node->getDoubleValue("lon", 0          ));
243     wpt->setAltitude   (wpt_node->getDoubleValue("alt", 0          ));
244     wpt->setSpeed      (wpt_node->getDoubleValue("ktas", 0         ));
245     wpt->setCrossat    (wpt_node->getDoubleValue("crossat", -10000 ));
246     wpt->setGear_down  (wpt_node->getBoolValue("gear-down", false  ));
247     wpt->setFlaps_down (wpt_node->getBoolValue("flaps-down", false ));
248     wpt->setOn_ground  (wpt_node->getBoolValue("on-ground", false  ));
249     wpt->setTime_sec   (wpt_node->getDoubleValue("time-sec", 0     ));
250     wpt->setTime       (wpt_node->getStringValue("time", ""        ));
251     wpt->setFinished   ((wpt->getName() == "END"));
252     pushBackWaypoint( wpt );
253   }
254   
255   wpt_iterator = waypoints.begin();
256   return true;
257 }
258
259 FGAIWaypoint* const FGAIFlightPlan::getPreviousWaypoint( void ) const
260 {
261   if (wpt_iterator == waypoints.begin()) {
262     return 0;
263   } else {
264     wpt_vector_iterator prev = wpt_iterator;
265     return *(--prev);
266   }
267 }
268
269 FGAIWaypoint* const FGAIFlightPlan::getCurrentWaypoint( void ) const
270 {
271   if (wpt_iterator == waypoints.end())
272       return 0;
273   return *wpt_iterator;
274 }
275
276 FGAIWaypoint* const FGAIFlightPlan::getNextWaypoint( void ) const
277 {
278   wpt_vector_iterator i = waypoints.end();
279   i--;  // end() points to one element after the last one. 
280   if (wpt_iterator == i) {
281     return 0;
282   } else {
283     wpt_vector_iterator next = wpt_iterator;
284     return *(++next);
285   }
286 }
287
288 void FGAIFlightPlan::IncrementWaypoint(bool eraseWaypoints )
289 {
290     if (eraseWaypoints)
291     {
292         if (wpt_iterator == waypoints.begin())
293             wpt_iterator++;
294         else
295         if (!waypoints.empty())
296         {
297             delete *(waypoints.begin());
298             waypoints.erase(waypoints.begin());
299             wpt_iterator = waypoints.begin();
300             wpt_iterator++;
301         }
302     }
303     else
304         wpt_iterator++;
305 }
306
307 void FGAIFlightPlan::DecrementWaypoint(bool eraseWaypoints )
308 {
309     if (eraseWaypoints)
310     {
311         if (wpt_iterator == waypoints.end())
312             wpt_iterator--;
313         else
314         if (!waypoints.empty())
315         {
316             delete *(waypoints.end()-1);
317             waypoints.erase(waypoints.end()-1);
318             wpt_iterator = waypoints.end();
319             wpt_iterator--;
320         }
321     }
322     else
323         wpt_iterator--;
324 }
325
326 void FGAIFlightPlan::eraseLastWaypoint()
327 {
328     delete (waypoints.back());
329     waypoints.pop_back();;
330     wpt_iterator = waypoints.begin();
331     wpt_iterator++;
332 }
333
334
335
336
337 // gives distance in feet from a position to a waypoint
338 double FGAIFlightPlan::getDistanceToGo(double lat, double lon, FGAIWaypoint* wp) const{
339   return SGGeodesy::distanceM(SGGeod::fromDeg(lon, lat), wp->getPos());
340 }
341
342 // sets distance in feet from a lead point to the current waypoint
343 void FGAIFlightPlan::setLeadDistance(double speed, double bearing, 
344                                      FGAIWaypoint* current, FGAIWaypoint* next){
345   double turn_radius;
346   // Handle Ground steering
347   // At a turn rate of 30 degrees per second, it takes 12 seconds to do a full 360 degree turn
348   // So, to get an estimate of the turn radius, calculate the cicumference of the circle
349   // we travel on. Get the turn radius by dividing by PI (*2).
350   if (speed < 0.5) {
351         lead_distance = 0.5;
352         return;
353   }
354   if (speed < 25) {
355        turn_radius = ((360/30)*fabs(speed)) / (2*M_PI);
356   } else 
357       turn_radius = 0.1911 * speed * speed; // an estimate for 25 degrees bank
358
359   double inbound = bearing;
360   double outbound = getBearing(current, next);
361   leadInAngle = fabs(inbound - outbound);
362   if (leadInAngle > 180.0) 
363     leadInAngle = 360.0 - leadInAngle;
364   //if (leadInAngle < 30.0) // To prevent lead_dist from getting so small it is skipped 
365   //  leadInAngle = 30.0;
366   
367   //lead_distance = turn_radius * sin(leadInAngle * SG_DEGREES_TO_RADIANS); 
368   lead_distance = turn_radius * tan((leadInAngle * SG_DEGREES_TO_RADIANS)/2);
369   /*
370   if ((lead_distance > (3*turn_radius)) && (current->on_ground == false)) {
371       // cerr << "Warning: Lead-in distance is large. Inbound = " << inbound
372       //      << ". Outbound = " << outbound << ". Lead in angle = " << leadInAngle  << ". Turn radius = " << turn_radius << endl;
373        lead_distance = 3 * turn_radius;
374        return;
375   }
376   if ((leadInAngle > 90) && (current->on_ground == true)) {
377       lead_distance = turn_radius * tan((90 * SG_DEGREES_TO_RADIANS)/2);
378       return;
379   }*/
380 }
381
382 void FGAIFlightPlan::setLeadDistance(double distance_ft){
383   lead_distance = distance_ft;
384 }
385
386
387 double FGAIFlightPlan::getBearing(FGAIWaypoint* first, FGAIWaypoint* second) const
388 {
389   return SGGeodesy::courseDeg(first->getPos(), second->getPos());
390 }
391
392 double FGAIFlightPlan::getBearing(const SGGeod& aPos, FGAIWaypoint* wp) const
393 {
394   return SGGeodesy::courseDeg(aPos, wp->getPos());
395 }
396
397 void FGAIFlightPlan::deleteWaypoints()
398 {
399   for (wpt_vector_iterator i = waypoints.begin(); i != waypoints.end();i++)
400     delete (*i);
401   waypoints.clear();
402   wpt_iterator = waypoints.begin();
403 }
404
405 // Delete all waypoints except the last, 
406 // which we will recycle as the first waypoint in the next leg;
407 void FGAIFlightPlan::resetWaypoints()
408 {
409   if (waypoints.begin() == waypoints.end())
410     return;
411   else
412     {
413       FGAIWaypoint *wpt = new FGAIWaypoint;
414       wpt_vector_iterator i = waypoints.end();
415       i--;
416       wpt->setName        ( (*i)->getName()       );
417       wpt->setPos         ( (*i)->getPos()        );
418       wpt->setCrossat     ( (*i)->getCrossat()    );
419       wpt->setGear_down   ( (*i)->getGear_down()  );
420       wpt->setFlaps_down  ( (*i)->getFlaps_down() );
421       wpt->setFinished    ( false                 );
422       wpt->setOn_ground   ( (*i)->getOn_ground()  );
423       //cerr << "Recycling waypoint " << wpt->name << endl;
424       deleteWaypoints();
425       pushBackWaypoint(wpt);
426     }
427 }
428
429 void FGAIFlightPlan::pushBackWaypoint(FGAIWaypoint *wpt)
430 {
431   // std::vector::push_back invalidates waypoints
432   //  so we should restore wpt_iterator after push_back
433   //  (or it could be an index in the vector)
434   size_t pos = wpt_iterator - waypoints.begin();
435   waypoints.push_back(wpt);
436   wpt_iterator = waypoints.begin() + pos;
437 }
438
439 // Start flightplan over from the beginning
440 void FGAIFlightPlan::restart()
441 {
442   wpt_iterator = waypoints.begin();
443 }
444
445 int FGAIFlightPlan::getRouteIndex(int i) {
446   if ((i > 0) && (i < (int)waypoints.size())) {
447     return waypoints[i]->getRouteIndex();
448   }
449   else
450     return 0;
451 }
452
453 double FGAIFlightPlan::checkTrackLength(const string& wptName) const {
454     // skip the first two waypoints: first one is behind, second one is partially done;
455     double trackDistance = 0;
456     wpt_vector_iterator wptvec = waypoints.begin();
457     wptvec++;
458     wptvec++;
459     while ((wptvec != waypoints.end()) && (!((*wptvec)->contains(wptName)))) {
460            trackDistance += (*wptvec)->getTrackLength();
461            wptvec++;
462     }
463     if (wptvec == waypoints.end()) {
464         trackDistance = 0; // name not found
465     }
466     return trackDistance;
467 }
468
469 void FGAIFlightPlan::shortenToFirst(unsigned int number, string name)
470 {
471     while (waypoints.size() > number + 3) {
472         eraseLastWaypoint();
473     }
474     (waypoints.back())->setName((waypoints.back())->getName() + name);
475 }
476
477 void FGAIFlightPlan::setGate(const ParkingAssignment& pka)
478 {
479   gate = pka;
480 }
481
482 FGParking* FGAIFlightPlan::getParkingGate()
483 {
484   return gate.parking();
485 }
486
487 FGAirportRef FGAIFlightPlan::departureAirport() const
488 {
489     return departure;
490 }
491
492 FGAirportRef FGAIFlightPlan::arrivalAirport() const
493 {
494     return arrival;
495 }