]> git.mxchange.org Git - flightgear.git/blob - src/Navaids/route.cxx
Win32 fix
[flightgear.git] / src / Navaids / route.cxx
1 // route.cxx - classes supporting waypoints and route structures
2
3 // Written by James Turner, started 2009.
4 //
5 // Copyright (C) 2009  Curtis L. Olson
6 //
7 // This program is free software; you can redistribute it and/or
8 // modify it under the terms of the GNU General Public License as
9 // published by the Free Software Foundation; either version 2 of the
10 // License, or (at your option) any later version.
11 //
12 // This program is distributed in the hope that it will be useful, but
13 // WITHOUT ANY WARRANTY; without even the implied warranty of
14 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
15 // General Public License for more details.
16 //
17 // You should have received a copy of the GNU General Public License
18 // along with this program; if not, write to the Free Software
19 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
20
21 #ifdef HAVE_CONFIG_H
22 # include "config.h"
23 #endif
24
25 #include "route.hxx"
26
27 // std
28 #include <map>
29 #include <fstream>
30
31 // Boost
32 #include <boost/algorithm/string/case_conv.hpp>
33 #include <boost/algorithm/string.hpp>
34
35 // SimGear
36 #include <simgear/structure/exception.hxx>
37 #include <simgear/xml/easyxml.hxx>
38 #include <simgear/misc/sg_path.hxx>
39
40 // FlightGear
41 #include <Navaids/procedure.hxx>
42 #include <Navaids/waypoint.hxx>
43 #include <Airports/simple.hxx>
44
45 using std::string;
46 using std::vector;
47 using std::endl;
48 using std::fstream;
49
50 namespace flightgear {
51
52 Waypt::Waypt(Route* aOwner) :
53   _altitudeFt(0.0),
54   _speedKts(0.0),
55   _altRestrict(RESTRICT_NONE),
56   _speedRestrict(RESTRICT_NONE),
57   _owner(aOwner),
58   _flags(0)
59 {
60 }
61
62 std::string Waypt::ident() const
63 {
64   return "";
65 }
66   
67 bool Waypt::flag(WayptFlag aFlag) const
68 {
69   return ((_flags & aFlag) != 0);
70 }
71         
72 void Waypt::setFlag(WayptFlag aFlag, bool aV)
73 {
74   _flags = (_flags & ~aFlag);
75   if (aV) _flags |= aFlag;
76 }
77
78 bool Waypt::matches(Waypt* aOther) const
79 {
80   assert(aOther);
81   if (ident() != aOther->ident()) { // cheap check first
82     return false;
83   }
84   
85   return matches(aOther->position());
86 }
87
88
89 bool Waypt::matches(const SGGeod& aPos) const
90 {
91   double d = SGGeodesy::distanceM(position(), aPos);
92   return (d < 100.0); // 100 metres seems plenty
93 }
94
95 void Waypt::setAltitude(double aAlt, RouteRestriction aRestrict)
96 {
97   _altitudeFt = aAlt;
98   _altRestrict = aRestrict;
99 }
100
101 void Waypt::setSpeed(double aSpeed, RouteRestriction aRestrict)
102 {
103   _speedKts = aSpeed;
104   _speedRestrict = aRestrict;
105 }
106
107 std::pair<double, double>
108 Waypt::courseAndDistanceFrom(const SGGeod& aPos) const
109 {
110   if (flag(WPT_DYNAMIC)) {
111     return std::make_pair(0.0, 0.0);
112   }
113   
114   double course, az2, distance;
115   SGGeodesy::inverse(aPos, position(), course, az2, distance);
116   return std::make_pair(course, distance);
117 }
118   
119 ///////////////////////////////////////////////////////////////////////////
120 // persistence
121
122 static RouteRestriction restrictionFromString(const char* aStr)
123 {
124   std::string l = boost::to_lower_copy(std::string(aStr));
125
126   if (l == "at") return RESTRICT_AT;
127   if (l == "above") return RESTRICT_ABOVE;
128   if (l == "below") return RESTRICT_BELOW;
129   if (l == "none") return RESTRICT_NONE;
130   
131   if (l.empty()) return RESTRICT_NONE;
132   throw sg_io_exception("unknown restriction specification:" + l, 
133     "Route restrictFromString");
134 }
135
136 static const char* restrictionToString(RouteRestriction aRestrict)
137 {
138   switch (aRestrict) {
139   case RESTRICT_AT: return "at";
140   case RESTRICT_BELOW: return "below";
141   case RESTRICT_ABOVE: return "above";
142   case RESTRICT_NONE: return "none";
143   default:
144     throw sg_exception("invalid route restriction",
145       "Route restrictToString");
146   }
147 }
148
149 Waypt* Waypt::createInstance(Route* aOwner, const std::string& aTypeName)
150 {
151   Waypt* r = NULL;
152   if (aTypeName == "basic") {
153     r = new BasicWaypt(aOwner);
154   } else if (aTypeName == "navaid") {
155     r = new NavaidWaypoint(aOwner);
156   } else if (aTypeName == "offset-navaid") {
157     r = new OffsetNavaidWaypoint(aOwner);
158   } else if (aTypeName == "hold") {
159     r = new Hold(aOwner);
160   } else if (aTypeName == "runway") {
161     r = new RunwayWaypt(aOwner);
162   } else if (aTypeName == "hdgToAlt") {
163     r = new HeadingToAltitude(aOwner);
164   } else if (aTypeName == "dmeIntercept") {
165     r = new DMEIntercept(aOwner);
166   } else if (aTypeName == "radialIntercept") {
167     r = new RadialIntercept(aOwner);
168   } else if (aTypeName == "vectors") {
169     r = new ATCVectors(aOwner);
170   } 
171
172   if (!r || (r->type() != aTypeName)) {
173     throw sg_exception("broken factory method for type:" + aTypeName,
174       "Waypt::createInstance");
175   }
176   
177   return r;
178 }
179
180 WayptRef Waypt::createFromProperties(Route* aOwner, SGPropertyNode_ptr aProp)
181 {
182   if (!aProp->hasChild("type")) {
183     throw sg_io_exception("bad props node, no type provided", 
184       "Waypt::createFromProperties");
185   }
186   
187   WayptRef nd(createInstance(aOwner, aProp->getStringValue("type")));
188   nd->initFromProperties(aProp);
189   return nd;
190 }
191   
192 void Waypt::saveAsNode(SGPropertyNode* n) const
193 {
194   n->setStringValue("type", type());
195   writeToProperties(n);
196 }
197
198 void Waypt::initFromProperties(SGPropertyNode_ptr aProp)
199 {
200   if (aProp->hasChild("generated")) {
201     setFlag(WPT_GENERATED, aProp->getBoolValue("generated")); 
202   }
203   
204   if (aProp->hasChild("overflight")) {
205     setFlag(WPT_OVERFLIGHT, aProp->getBoolValue("overflight")); 
206   }
207   
208   if (aProp->hasChild("arrival")) {
209     setFlag(WPT_ARRIVAL, aProp->getBoolValue("arrival")); 
210   }
211   
212   if (aProp->hasChild("departure")) {
213     setFlag(WPT_DEPARTURE, aProp->getBoolValue("departure")); 
214   }
215   
216   if (aProp->hasChild("miss")) {
217     setFlag(WPT_MISS, aProp->getBoolValue("miss")); 
218   }
219   
220   if (aProp->hasChild("alt-restrict")) {
221     _altRestrict = restrictionFromString(aProp->getStringValue("alt-restrict"));
222     _altitudeFt = aProp->getDoubleValue("altitude-ft");
223   }
224   
225   if (aProp->hasChild("speed-restrict")) {
226     _speedRestrict = restrictionFromString(aProp->getStringValue("speed-restrict"));
227     _speedKts = aProp->getDoubleValue("speed-kts");
228   }
229   
230   
231 }
232
233 void Waypt::writeToProperties(SGPropertyNode_ptr aProp) const
234 {
235   if (flag(WPT_OVERFLIGHT)) {
236     aProp->setBoolValue("overflight", true);
237   }
238
239   if (flag(WPT_DEPARTURE)) {
240     aProp->setBoolValue("departure", true);
241   }
242   
243   if (flag(WPT_ARRIVAL)) {
244     aProp->setBoolValue("arrival", true);
245   }
246   
247   if (flag(WPT_MISS)) {
248     aProp->setBoolValue("miss", true);
249   }
250   
251   if (flag(WPT_GENERATED)) {
252     aProp->setBoolValue("generated", true);
253   }
254   
255   if (_altRestrict != RESTRICT_NONE) {
256     aProp->setStringValue("alt-restrict", restrictionToString(_altRestrict));
257     aProp->setDoubleValue("altitude-ft", _altitudeFt);
258   }
259   
260   if (_speedRestrict != RESTRICT_NONE) {
261     aProp->setStringValue("speed-restrict", restrictionToString(_speedRestrict));
262     aProp->setDoubleValue("speed-kts", _speedKts);
263   }
264 }
265
266 void Route::dumpRouteToFile(const WayptVec& aRoute, const std::string& aName)
267 {
268   SGPath p = "/Users/jmt/Desktop/" + aName + ".kml";
269   std::fstream f;
270   f.open(p.str().c_str(), fstream::out | fstream::app);
271   if (!f.is_open()) {
272     SG_LOG(SG_GENERAL, SG_WARN, "unable to open:" << p.str());
273     return;
274   }
275   
276 // pre-amble
277   f << "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n"
278       "<kml xmlns=\"http://www.opengis.net/kml/2.2\">\n"
279     "<Document>\n";
280
281   dumpRouteToLineString(aName, aRoute, f);
282   
283 // post-amble
284   f << "</Document>\n" 
285     "</kml>" << endl;
286   f.close();
287 }
288
289 void Route::dumpRouteToLineString(const std::string& aIdent,
290   const WayptVec& aRoute, std::ostream& aStream)
291 {
292   // preamble
293   aStream << "<Placemark>\n";
294   aStream << "<name>" << aIdent << "</name>\n";
295   aStream << "<LineString>\n";
296   aStream << "<tessellate>1</tessellate>\n";
297   aStream << "<coordinates>\n";
298   
299   // waypoints
300   for (unsigned int i=0; i<aRoute.size(); ++i) {
301     SGGeod pos = aRoute[i]->position();
302     aStream << pos.getLongitudeDeg() << "," << pos.getLatitudeDeg() << " " << endl;
303   }
304   
305   // postable
306   aStream << "</coordinates>\n"
307     "</LineString>\n"
308     "</Placemark>\n" << endl;
309 }
310
311 ///////////////////////////////////////////////////////////////////////////
312
313 class NavdataVisitor : public XMLVisitor {
314 public:
315   NavdataVisitor(FGAirport* aApt, const SGPath& aPath);
316
317 protected:
318   virtual void startXML (); 
319   virtual void endXML   ();
320   virtual void startElement (const char * name, const XMLAttributes &atts);
321   virtual void endElement (const char * name);
322   virtual void data (const char * s, int len);
323   virtual void pi (const char * target, const char * data);
324   virtual void warning (const char * message, int line, int column);
325   virtual void error (const char * message, int line, int column);
326
327 private:
328   Waypt* buildWaypoint();
329   void processRunways(ArrivalDeparture* aProc, const XMLAttributes &atts);
330  
331   void finishApproach();
332   void finishSid();
333   void finishStar();
334   
335   FGAirport* _airport;
336   SGPath _path;
337   string _text; ///< last element text value
338   
339   SID* _sid;
340   STAR* _star;
341   Approach* _approach;
342
343   WayptVec _waypoints; ///< waypoint list for current approach/sid/star
344   WayptVec _transWaypts; ///< waypoint list for current transition
345   
346   string _wayptName;
347   string _wayptType;
348   string _ident; // id of segment under construction
349   string _transIdent;
350   double _longitude, _latitude, _altitude, _speed;
351   RouteRestriction _altRestrict;
352   
353   double _holdRadial; // inbound hold radial, or -1 if radial is 'inbound'
354   double _holdTD; ///< hold time (seconds) or distance (nm), based on flag below
355   bool _holdRighthanded;
356   bool _holdDistance; // true, TD is distance in nm; false, TD is time in seconds
357   
358   double _course, _radial, _dmeDistance;
359 };
360
361 void Route::loadAirportProcedures(const SGPath& aPath, FGAirport* aApt)
362 {
363   assert(aApt);
364   try {
365     NavdataVisitor visitor(aApt, aPath);
366     readXML(aPath.str(), visitor);
367   } catch (sg_io_exception& ex) {
368     SG_LOG(SG_GENERAL, SG_WARN, "failured parsing procedures: " << aPath.str() <<
369       "\n\t" << ex.getMessage() << "\n\tat:" << ex.getLocation().asString());
370   } catch (sg_exception& ex) {
371     SG_LOG(SG_GENERAL, SG_WARN, "failured parsing procedures: " << aPath.str() <<
372       "\n\t" << ex.getMessage());
373   }
374 }
375
376 NavdataVisitor::NavdataVisitor(FGAirport* aApt, const SGPath& aPath):
377   _airport(aApt),
378   _path(aPath),
379   _sid(NULL),
380   _star(NULL),
381   _approach(NULL)
382 {
383 }
384
385 void NavdataVisitor::startXML()
386 {
387 }
388
389 void NavdataVisitor::endXML()
390 {
391 }
392
393 void NavdataVisitor::startElement(const char* name, const XMLAttributes &atts)
394 {
395   _text.clear();
396   string tag(name);
397   if (tag == "Airport") {
398     string icao(atts.getValue("ICAOcode"));
399     if (_airport->ident() != icao) {
400       throw sg_format_exception("Airport and ICAO mismatch", icao, _path.str());
401     }
402   } else if (tag == "Sid") {
403     string ident(atts.getValue("Name"));
404     _sid = new SID(ident);
405     _waypoints.clear();
406     processRunways(_sid, atts);
407   } else if (tag == "Star") {
408     string ident(atts.getValue("Name"));
409     _star = new STAR(ident);
410     _waypoints.clear();
411     processRunways(_star, atts);
412   } else if ((tag == "Sid_Waypoint") ||
413       (tag == "App_Waypoint") ||
414       (tag == "Star_Waypoint") ||
415       (tag == "AppTr_Waypoint") ||
416       (tag == "SidTr_Waypoint") ||
417       (tag == "RwyTr_Waypoint"))
418   {
419     // reset waypoint data
420     _speed = 0.0;
421     _altRestrict = RESTRICT_NONE;
422     _altitude = 0.0;
423   } else if (tag == "Approach") {
424     _ident = atts.getValue("Name");
425     _waypoints.clear();
426     _approach = new Approach(_ident);
427   } else if ((tag == "Sid_Transition") || 
428              (tag == "App_Transition") ||
429              (tag == "Star_Transition")) {
430     _transIdent = atts.getValue("Name");
431     _transWaypts.clear();
432   } else if (tag == "RunwayTransition") {
433     _transIdent = atts.getValue("Runway");
434     _transWaypts.clear();
435   } else {
436     
437   }
438 }
439
440 void NavdataVisitor::processRunways(ArrivalDeparture* aProc, const XMLAttributes &atts)
441 {
442   string v("All");
443   if (atts.hasAttribute("Runways")) {
444     v = atts.getValue("Runways");
445   }
446   
447   if (v == "All") {
448     for (unsigned int r=0; r<_airport->numRunways(); ++r) {
449       aProc->addRunway(_airport->getRunwayByIndex(r));
450     }
451     return;
452   }
453   
454   vector<string> rwys;
455   boost::split(rwys, v, boost::is_any_of(" ,"));
456   for (unsigned int r=0; r<rwys.size(); ++r) {
457     FGRunway* rwy = _airport->getRunwayByIdent(rwys[r]);
458     aProc->addRunway(rwy);
459   }
460 }
461
462 void NavdataVisitor::endElement(const char* name)
463 {
464   string tag(name);
465   if ((tag == "Sid_Waypoint") ||
466       (tag == "App_Waypoint") ||
467       (tag == "Star_Waypoint"))
468   {
469     _waypoints.push_back(buildWaypoint());
470   } else if ((tag == "AppTr_Waypoint") || 
471              (tag == "SidTr_Waypoint") ||
472              (tag == "RwyTr_Waypoint") ||
473              (tag == "StarTr_Waypoint")) 
474   {
475     _transWaypts.push_back(buildWaypoint());
476   } else if (tag == "Sid_Transition") {
477     assert(_sid);
478     // SID waypoints are stored backwards, to share code with STARs
479     std::reverse(_transWaypts.begin(), _transWaypts.end());
480     Transition* t = new Transition(_transIdent, _sid, _transWaypts);
481     _sid->addTransition(t);
482   } else if (tag == "Star_Transition") {
483     assert(_star);
484     Transition* t = new Transition(_transIdent, _star, _transWaypts);
485     _star->addTransition(t);
486   } else if (tag == "App_Transition") {
487     assert(_approach);
488     Transition* t = new Transition(_transIdent, _approach, _transWaypts);
489     _approach->addTransition(t);
490   } else if (tag == "RunwayTransition") {
491     ArrivalDeparture* ad;
492     if (_sid) {
493       // SID waypoints are stored backwards, to share code with STARs
494       std::reverse(_transWaypts.begin(), _transWaypts.end());
495       ad = _sid;
496     } else {
497       ad = _star;
498     }
499     
500     Transition* t = new Transition(_transIdent, ad, _transWaypts);
501     FGRunwayRef rwy = _airport->getRunwayByIdent(_transIdent);
502     ad->addRunwayTransition(rwy, t);
503   } else if (tag == "Approach") {
504     finishApproach();
505   } else if (tag == "Sid") {
506     finishSid();
507   } else if (tag == "Star") {
508     finishStar();  
509   } else if (tag == "Longitude") {
510     _longitude = atof(_text.c_str());
511   } else if (tag == "Latitude") {
512     _latitude = atof(_text.c_str());
513   } else if (tag == "Name") {
514     _wayptName = _text;
515   } else if (tag == "Type") {
516     _wayptType = _text;
517   } else if (tag == "Speed") {
518     _speed = atoi(_text.c_str());
519   } else if (tag == "Altitude") {
520     _altitude = atof(_text.c_str());
521   } else if (tag == "AltitudeRestriction") {
522     if (_text == "at") {
523       _altRestrict = RESTRICT_AT;
524     } else if (_text == "above") {
525       _altRestrict = RESTRICT_ABOVE;
526     } else if (_text == "below") {
527       _altRestrict = RESTRICT_BELOW;
528     } else {
529       throw sg_format_exception("Unrecognized altitude restriction", _text);
530     }
531   } else if (tag == "Hld_Rad_or_Inbd") {
532     if (_text == "Inbd") {
533       _holdRadial = -1.0;
534     }
535   } else if (tag == "Hld_Time_or_Dist") {
536     _holdDistance = (_text == "Dist");
537   } else if (tag == "Hld_Rad_value") {
538     _holdRadial = atof(_text.c_str());
539   } else if (tag == "Hld_Turn") {
540     _holdRighthanded = (_text == "Right");
541   } else if (tag == "Hld_td_value") {
542     _holdTD = atof(_text.c_str());
543   } else if (tag == "Hdg_Crs_value") {
544     _course = atof(_text.c_str());
545   } else if (tag == "DMEtoIntercept") {
546     _dmeDistance = atof(_text.c_str());
547   } else if (tag == "RadialtoIntercept") {
548     _radial = atof(_text.c_str());
549   } else {
550     
551   }
552 }
553
554 Waypt* NavdataVisitor::buildWaypoint()
555 {
556   Waypt* wp = NULL;
557   if (_wayptType == "Normal") {
558     // new LatLonWaypoint
559     SGGeod pos(SGGeod::fromDeg(_longitude, _latitude));
560     wp = new BasicWaypt(pos, _wayptName, NULL);
561   } else if (_wayptType == "Runway") {
562     string ident = _wayptName.substr(2);
563     FGRunwayRef rwy = _airport->getRunwayByIdent(ident);
564     wp = new RunwayWaypt(rwy, NULL);
565   } else if (_wayptType == "Hold") {
566     SGGeod pos(SGGeod::fromDeg(_longitude, _latitude));
567     Hold* h = new Hold(pos, _wayptName, NULL);
568     wp = h;
569     if (_holdRighthanded) {
570       h->setRightHanded();
571     } else {
572       h->setLeftHanded();
573     }
574     
575     if (_holdDistance) {
576       h->setHoldDistance(_holdTD);
577     } else {
578       h->setHoldTime(_holdTD * 60.0);
579     }
580     
581     if (_holdRadial >= 0.0) {
582       h->setHoldRadial(_holdRadial);
583     }
584   } else if (_wayptType == "Vectors") {
585     wp = new ATCVectors(NULL, _airport);
586   } else if ((_wayptType == "Intc") || (_wayptType == "VorRadialIntc")) {
587     SGGeod pos(SGGeod::fromDeg(_longitude, _latitude));
588     wp = new RadialIntercept(NULL, _wayptName, pos, _course, _radial);
589   } else if (_wayptType == "DmeIntc") {
590     SGGeod pos(SGGeod::fromDeg(_longitude, _latitude));
591     wp = new DMEIntercept(NULL, _wayptName, pos, _course, _dmeDistance);
592   } else if (_wayptType == "ConstHdgtoAlt") {
593     wp = new HeadingToAltitude(NULL, _wayptName, _course);
594   } else {
595     SG_LOG(SG_GENERAL, SG_ALERT, "implement waypoint type:" << _wayptType);
596     throw sg_format_exception("Unrecognized waypt type", _wayptType);
597   }
598   
599   assert(wp);
600   if ((_altitude > 0.0) && (_altRestrict != RESTRICT_NONE)) {
601     wp->setAltitude(_altitude,_altRestrict);
602   }
603   
604   if (_speed > 0.0) {
605     wp->setSpeed(_speed, RESTRICT_AT); // or _BELOW?
606   }
607   
608   return wp;
609 }
610
611 void NavdataVisitor::finishApproach()
612 {
613   WayptVec::iterator it;
614   FGRunwayRef rwy;
615   
616 // find the runway node
617   for (it = _waypoints.begin(); it != _waypoints.end(); ++it) {
618     FGPositionedRef navid = (*it)->source();
619     if (!navid) {
620       continue;
621     }
622     
623     if (navid->type() == FGPositioned::RUNWAY) {
624       rwy = (FGRunway*) navid.get();
625       break;
626     }
627   }
628   
629   if (!rwy) {
630     throw sg_format_exception("Malformed approach, no runway waypt", _ident);
631   }
632   
633   WayptVec primary(_waypoints.begin(), it);
634   // erase all points up to and including the runway, to leave only the
635   // missed segments
636   _waypoints.erase(_waypoints.begin(), ++it);
637   
638   _approach->setRunway(rwy);
639   _approach->setPrimaryAndMissed(primary, _waypoints);
640   _airport->addApproach(_approach);
641   _approach = NULL;
642 }
643
644 void NavdataVisitor::finishSid()
645 {
646   // reverse order, because that's how we deal with commonality between
647   // STARs and SIDs. SID::route undoes  this
648   std::reverse(_waypoints.begin(), _waypoints.end());
649   _sid->setCommon(_waypoints);
650   _airport->addSID(_sid);
651   _sid = NULL;
652 }
653
654 void NavdataVisitor::finishStar()
655 {
656   _star->setCommon(_waypoints);
657   _airport->addSTAR(_star);
658   _star = NULL;
659 }
660
661 void NavdataVisitor::data (const char * s, int len)
662 {
663   _text += string(s, len);
664 }
665
666
667 void NavdataVisitor::pi (const char * target, const char * data) {
668   //cout << "Processing instruction " << target << ' ' << data << endl;
669 }
670
671 void NavdataVisitor::warning (const char * message, int line, int column) {
672   SG_LOG(SG_IO, SG_WARN, "Warning: " << message << " (" << line << ',' << column << ')');
673 }
674
675 void NavdataVisitor::error (const char * message, int line, int column) {
676   SG_LOG(SG_IO, SG_ALERT, "Error: " << message << " (" << line << ',' << column << ')');
677 }
678
679 } // of namespace flightgear