1 // route.cxx - classes supporting waypoints and route structures
3 // Written by James Turner, started 2009.
5 // Copyright (C) 2009 Curtis L. Olson
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.
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.
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.
32 #include <boost/algorithm/string/case_conv.hpp>
33 #include <boost/algorithm/string.hpp>
36 #include <simgear/structure/exception.hxx>
37 #include <simgear/xml/easyxml.hxx>
38 #include <simgear/misc/sg_path.hxx>
41 #include <Navaids/procedure.hxx>
42 #include <Navaids/waypoint.hxx>
43 #include <Airports/simple.hxx>
50 namespace flightgear {
52 Waypt::Waypt(Route* aOwner) :
55 _altRestrict(RESTRICT_NONE),
56 _speedRestrict(RESTRICT_NONE),
62 std::string Waypt::ident() const
67 bool Waypt::flag(WayptFlag aFlag) const
69 return ((_flags & aFlag) != 0);
72 void Waypt::setFlag(WayptFlag aFlag, bool aV)
74 _flags = (_flags & ~aFlag);
75 if (aV) _flags |= aFlag;
78 bool Waypt::matches(Waypt* aOther) const
81 if (ident() != aOther->ident()) { // cheap check first
85 return matches(aOther->position());
89 bool Waypt::matches(const SGGeod& aPos) const
91 double d = SGGeodesy::distanceM(position(), aPos);
92 return (d < 100.0); // 100 metres seems plenty
95 void Waypt::setAltitude(double aAlt, RouteRestriction aRestrict)
98 _altRestrict = aRestrict;
101 void Waypt::setSpeed(double aSpeed, RouteRestriction aRestrict)
104 _speedRestrict = aRestrict;
107 std::pair<double, double>
108 Waypt::courseAndDistanceFrom(const SGGeod& aPos) const
110 if (flag(WPT_DYNAMIC)) {
111 return std::make_pair(0.0, 0.0);
114 double course, az2, distance;
115 SGGeodesy::inverse(aPos, position(), course, az2, distance);
116 return std::make_pair(course, distance);
119 ///////////////////////////////////////////////////////////////////////////
122 static RouteRestriction restrictionFromString(const char* aStr)
124 std::string l = boost::to_lower_copy(std::string(aStr));
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;
131 if (l.empty()) return RESTRICT_NONE;
132 throw sg_io_exception("unknown restriction specification:" + l,
133 "Route restrictFromString");
136 static const char* restrictionToString(RouteRestriction 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";
144 throw sg_exception("invalid route restriction",
145 "Route restrictToString");
149 Waypt* Waypt::createInstance(Route* aOwner, const std::string& aTypeName)
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);
172 if (!r || (r->type() != aTypeName)) {
173 throw sg_exception("broken factory method for type:" + aTypeName,
174 "Waypt::createInstance");
180 WayptRef Waypt::createFromProperties(Route* aOwner, SGPropertyNode_ptr aProp)
182 if (!aProp->hasChild("type")) {
183 throw sg_io_exception("bad props node, no type provided",
184 "Waypt::createFromProperties");
187 WayptRef nd(createInstance(aOwner, aProp->getStringValue("type")));
188 nd->initFromProperties(aProp);
192 void Waypt::saveAsNode(SGPropertyNode* n) const
194 n->setStringValue("type", type());
195 writeToProperties(n);
198 void Waypt::initFromProperties(SGPropertyNode_ptr aProp)
200 if (aProp->hasChild("generated")) {
201 setFlag(WPT_GENERATED, aProp->getBoolValue("generated"));
204 if (aProp->hasChild("overflight")) {
205 setFlag(WPT_OVERFLIGHT, aProp->getBoolValue("overflight"));
208 if (aProp->hasChild("arrival")) {
209 setFlag(WPT_ARRIVAL, aProp->getBoolValue("arrival"));
212 if (aProp->hasChild("departure")) {
213 setFlag(WPT_DEPARTURE, aProp->getBoolValue("departure"));
216 if (aProp->hasChild("miss")) {
217 setFlag(WPT_MISS, aProp->getBoolValue("miss"));
220 if (aProp->hasChild("alt-restrict")) {
221 _altRestrict = restrictionFromString(aProp->getStringValue("alt-restrict"));
222 _altitudeFt = aProp->getDoubleValue("altitude-ft");
225 if (aProp->hasChild("speed-restrict")) {
226 _speedRestrict = restrictionFromString(aProp->getStringValue("speed-restrict"));
227 _speedKts = aProp->getDoubleValue("speed-kts");
233 void Waypt::writeToProperties(SGPropertyNode_ptr aProp) const
235 if (flag(WPT_OVERFLIGHT)) {
236 aProp->setBoolValue("overflight", true);
239 if (flag(WPT_DEPARTURE)) {
240 aProp->setBoolValue("departure", true);
243 if (flag(WPT_ARRIVAL)) {
244 aProp->setBoolValue("arrival", true);
247 if (flag(WPT_MISS)) {
248 aProp->setBoolValue("miss", true);
251 if (flag(WPT_GENERATED)) {
252 aProp->setBoolValue("generated", true);
255 if (_altRestrict != RESTRICT_NONE) {
256 aProp->setStringValue("alt-restrict", restrictionToString(_altRestrict));
257 aProp->setDoubleValue("altitude-ft", _altitudeFt);
260 if (_speedRestrict != RESTRICT_NONE) {
261 aProp->setStringValue("speed-restrict", restrictionToString(_speedRestrict));
262 aProp->setDoubleValue("speed-kts", _speedKts);
266 void Route::dumpRouteToFile(const WayptVec& aRoute, const std::string& aName)
268 SGPath p = "/Users/jmt/Desktop/" + aName + ".kml";
270 f.open(p.str().c_str(), fstream::out | fstream::app);
272 SG_LOG(SG_GENERAL, SG_WARN, "unable to open:" << p.str());
277 f << "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n"
278 "<kml xmlns=\"http://www.opengis.net/kml/2.2\">\n"
281 dumpRouteToLineString(aName, aRoute, f);
289 void Route::dumpRouteToLineString(const std::string& aIdent,
290 const WayptVec& aRoute, std::ostream& aStream)
293 aStream << "<Placemark>\n";
294 aStream << "<name>" << aIdent << "</name>\n";
295 aStream << "<LineString>\n";
296 aStream << "<tessellate>1</tessellate>\n";
297 aStream << "<coordinates>\n";
300 for (unsigned int i=0; i<aRoute.size(); ++i) {
301 SGGeod pos = aRoute[i]->position();
302 aStream << pos.getLongitudeDeg() << "," << pos.getLatitudeDeg() << " " << endl;
306 aStream << "</coordinates>\n"
308 "</Placemark>\n" << endl;
311 ///////////////////////////////////////////////////////////////////////////
313 class NavdataVisitor : public XMLVisitor {
315 NavdataVisitor(FGAirport* aApt, const SGPath& aPath);
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);
328 Waypt* buildWaypoint();
329 void processRunways(ArrivalDeparture* aProc, const XMLAttributes &atts);
331 void finishApproach();
337 string _text; ///< last element text value
343 WayptVec _waypoints; ///< waypoint list for current approach/sid/star
344 WayptVec _transWaypts; ///< waypoint list for current transition
348 string _ident; // id of segment under construction
350 double _longitude, _latitude, _altitude, _speed;
351 RouteRestriction _altRestrict;
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
358 double _course, _radial, _dmeDistance;
361 void Route::loadAirportProcedures(const SGPath& aPath, FGAirport* aApt)
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());
376 NavdataVisitor::NavdataVisitor(FGAirport* aApt, const SGPath& aPath):
385 void NavdataVisitor::startXML()
389 void NavdataVisitor::endXML()
393 void NavdataVisitor::startElement(const char* name, const XMLAttributes &atts)
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());
402 } else if (tag == "Sid") {
403 string ident(atts.getValue("Name"));
404 _sid = new SID(ident);
406 processRunways(_sid, atts);
407 } else if (tag == "Star") {
408 string ident(atts.getValue("Name"));
409 _star = new STAR(ident);
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"))
419 // reset waypoint data
421 _altRestrict = RESTRICT_NONE;
423 } else if (tag == "Approach") {
424 _ident = atts.getValue("Name");
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();
440 void NavdataVisitor::processRunways(ArrivalDeparture* aProc, const XMLAttributes &atts)
443 if (atts.hasAttribute("Runways")) {
444 v = atts.getValue("Runways");
448 for (unsigned int r=0; r<_airport->numRunways(); ++r) {
449 aProc->addRunway(_airport->getRunwayByIndex(r));
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);
462 void NavdataVisitor::endElement(const char* name)
465 if ((tag == "Sid_Waypoint") ||
466 (tag == "App_Waypoint") ||
467 (tag == "Star_Waypoint"))
469 _waypoints.push_back(buildWaypoint());
470 } else if ((tag == "AppTr_Waypoint") ||
471 (tag == "SidTr_Waypoint") ||
472 (tag == "RwyTr_Waypoint") ||
473 (tag == "StarTr_Waypoint"))
475 _transWaypts.push_back(buildWaypoint());
476 } else if (tag == "Sid_Transition") {
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") {
484 Transition* t = new Transition(_transIdent, _star, _transWaypts);
485 _star->addTransition(t);
486 } else if (tag == "App_Transition") {
488 Transition* t = new Transition(_transIdent, _approach, _transWaypts);
489 _approach->addTransition(t);
490 } else if (tag == "RunwayTransition") {
491 ArrivalDeparture* ad;
493 // SID waypoints are stored backwards, to share code with STARs
494 std::reverse(_transWaypts.begin(), _transWaypts.end());
500 Transition* t = new Transition(_transIdent, ad, _transWaypts);
501 FGRunwayRef rwy = _airport->getRunwayByIdent(_transIdent);
502 ad->addRunwayTransition(rwy, t);
503 } else if (tag == "Approach") {
505 } else if (tag == "Sid") {
507 } else if (tag == "Star") {
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") {
515 } else if (tag == "Type") {
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") {
523 _altRestrict = RESTRICT_AT;
524 } else if (_text == "above") {
525 _altRestrict = RESTRICT_ABOVE;
526 } else if (_text == "below") {
527 _altRestrict = RESTRICT_BELOW;
529 throw sg_format_exception("Unrecognized altitude restriction", _text);
531 } else if (tag == "Hld_Rad_or_Inbd") {
532 if (_text == "Inbd") {
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());
554 Waypt* NavdataVisitor::buildWaypoint()
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);
569 if (_holdRighthanded) {
576 h->setHoldDistance(_holdTD);
578 h->setHoldTime(_holdTD * 60.0);
581 if (_holdRadial >= 0.0) {
582 h->setHoldRadial(_holdRadial);
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);
595 SG_LOG(SG_GENERAL, SG_ALERT, "implement waypoint type:" << _wayptType);
596 throw sg_format_exception("Unrecognized waypt type", _wayptType);
600 if ((_altitude > 0.0) && (_altRestrict != RESTRICT_NONE)) {
601 wp->setAltitude(_altitude,_altRestrict);
605 wp->setSpeed(_speed, RESTRICT_AT); // or _BELOW?
611 void NavdataVisitor::finishApproach()
613 WayptVec::iterator it;
616 // find the runway node
617 for (it = _waypoints.begin(); it != _waypoints.end(); ++it) {
618 FGPositionedRef navid = (*it)->source();
623 if (navid->type() == FGPositioned::RUNWAY) {
624 rwy = (FGRunway*) navid.get();
630 throw sg_format_exception("Malformed approach, no runway waypt", _ident);
633 WayptVec primary(_waypoints.begin(), it);
634 // erase all points up to and including the runway, to leave only the
636 _waypoints.erase(_waypoints.begin(), ++it);
638 _approach->setRunway(rwy);
639 _approach->setPrimaryAndMissed(primary, _waypoints);
640 _airport->addApproach(_approach);
644 void NavdataVisitor::finishSid()
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);
654 void NavdataVisitor::finishStar()
656 _star->setCommon(_waypoints);
657 _airport->addSTAR(_star);
661 void NavdataVisitor::data (const char * s, int len)
663 _text += string(s, len);
667 void NavdataVisitor::pi (const char * target, const char * data) {
668 //cout << "Processing instruction " << target << ' ' << data << endl;
671 void NavdataVisitor::warning (const char * message, int line, int column) {
672 SG_LOG(SG_IO, SG_WARN, "Warning: " << message << " (" << line << ',' << column << ')');
675 void NavdataVisitor::error (const char * message, int line, int column) {
676 SG_LOG(SG_IO, SG_ALERT, "Error: " << message << " (" << line << ',' << column << ')');
679 } // of namespace flightgear