From bc7ac3493eb061c24eff6050f1d86a103cb6fb56 Mon Sep 17 00:00:00 2001 From: jmt Date: Wed, 7 Oct 2009 08:34:53 +0000 Subject: [PATCH] Refactor airway code to use SGGeod in external APIs. First of various changes to make airways more useful in GPS/FMS modules. --- src/AIModel/AIFlightPlanCreateCruise.cxx | 10 ++--- src/Navaids/awynet.cxx | 49 +++++++++++------------- src/Navaids/awynet.hxx | 17 ++++---- 3 files changed, 33 insertions(+), 43 deletions(-) diff --git a/src/AIModel/AIFlightPlanCreateCruise.cxx b/src/AIModel/AIFlightPlanCreateCruise.cxx index 4f44ab1e2..d20a2134b 100755 --- a/src/AIModel/AIFlightPlanCreateCruise.cxx +++ b/src/AIModel/AIFlightPlanCreateCruise.cxx @@ -76,18 +76,14 @@ void FGAIFlightPlan::evaluateRoutePart(double deplat, } //cerr << "1"<< endl; - SGGeoc geoc = SGGeoc::fromCart(SGVec3d(newPos[0], newPos[1], newPos[2])); - - double midlat = geoc.getLatitudeDeg(); - double midlon = geoc.getLongitudeDeg(); + SGGeod geod = SGGeod::fromCart(SGVec3d(newPos[0], newPos[1], newPos[2])); prevNode = tmpNode; - tmpNode = globals->get_airwaynet()->findNearestNode(midlat, midlon); + tmpNode = globals->get_airwaynet()->findNearestNode(geod); FGNode* node = globals->get_airwaynet()->findNode(tmpNode); - SGGeoc nodePos(SGGeoc::fromGeod(node->getPosition())); - if ((tmpNode != prevNode) && (SGGeodesy::distanceM(geoc, nodePos) < 25000)) { + if ((tmpNode != prevNode) && (SGGeodesy::distanceM(geod, node->getPosition()) < 25000)) { nodes.push_back(tmpNode); } } diff --git a/src/Navaids/awynet.cxx b/src/Navaids/awynet.cxx index 32e08ac03..ab8ec1177 100755 --- a/src/Navaids/awynet.cxx +++ b/src/Navaids/awynet.cxx @@ -32,6 +32,7 @@ #include #include #include +#include #include "awynet.hxx" @@ -47,18 +48,19 @@ FGNode::FGNode() { } -FGNode::FGNode(double lt, double ln, int idx, std::string id) : +FGNode::FGNode(const SGGeod& aPos, int idx, std::string id) : ident(id), - geod(SGGeod::fromDeg(ln, lt)), + geod(aPos), index(idx) { + cart = SGVec3d::fromGeod(geod); } -bool FGNode::matches(string id, double lt, double ln) +bool FGNode::matches(std::string id, const SGGeod& aPos) { if ((ident == id) && - (fabs(lt - geod.getLatitudeDeg()) < 1.0) && - (fabs(ln - geod.getLongitudeDeg()) < 1.0)) + (fabs(aPos.getLatitudeDeg() - geod.getLatitudeDeg()) < 1.0) && + (fabs(aPos.getLongitudeDeg() - geod.getLongitudeDeg()) < 1.0)) return true; else return false; @@ -195,9 +197,9 @@ void FGAirwayNetwork::init() } -void FGAirwayNetwork::load(SGPath path) +void FGAirwayNetwork::load(const SGPath& path) { - string identStart, identEnd, token, name; + std::string identStart, identEnd, token, name; double latStart, lonStart, latEnd, lonEnd; int type, base, top; int airwayIndex = 0; @@ -272,7 +274,8 @@ void FGAirwayNetwork::load(SGPath path) node_map_iterator itr = nodesMap.find(string(buffer)); if (itr == nodesMap.end()) { startIndex = nodes.size(); - n = new FGNode(latStart, lonStart, startIndex, identStart); + SGGeod startPos(SGGeod::fromDeg(lonStart, latStart)); + n = new FGNode(startPos, startIndex, identStart); nodesMap[string(buffer)] = n; nodes.push_back(n); //cout << "Adding node: " << identStart << endl; @@ -287,7 +290,8 @@ void FGAirwayNetwork::load(SGPath path) itr = nodesMap.find(string(buffer)); if (itr == nodesMap.end()) { endIndex = nodes.size(); - n = new FGNode(latEnd, lonEnd, endIndex, identEnd); + SGGeod endPos(SGGeod::fromDeg(lonEnd, latEnd)); + n = new FGNode(endPos, endIndex, identEnd); nodesMap[string(buffer)] = n; nodes.push_back(n); //cout << "Adding node: " << identEnd << endl; @@ -310,32 +314,23 @@ void FGAirwayNetwork::load(SGPath path) } } - int FGAirwayNetwork::findNearestNode(double lat, double lon) +int FGAirwayNetwork::findNearestNode(const SGGeod& aPos) { double minDist = HUGE_VAL; - double distsqrt, lat2, lon2; int index; + SGVec3d cart = SGVec3d::fromGeod(aPos); + //cerr << "Lat " << lat << " lon " << lon << endl; for (FGNodeVectorIterator itr = nodes.begin(); itr != nodes.end(); itr++) { - lat2 = (*itr)->getLatitude(); - lon2 = (*itr)->getLongitude(); - // Note: This equation should adjust for decreasing distance per longitude - // with increasing lat. - distsqrt = - (lat-lat2)*(lat-lat2) + - (lon-lon2)*(lon-lon2); - - if (distsqrt < minDist) - { - minDist = distsqrt; - //cerr << "Test" << endl; - index = (*itr)->getIndex(); - //cerr << "Minimum distance of " << minDist << " for index " << index << endl; - //cerr << (*itr)->getLatitude() << " " << (*itr)->getLongitude() << endl; - } + double d2 = distSqr(cart, (*itr)->getCart()); + if (d2 < minDist) + { + minDist = d2; + index = (*itr)->getIndex(); + } //cerr << (*itr)->getIndex() << endl; } //cerr << " returning " << index << endl; diff --git a/src/Navaids/awynet.hxx b/src/Navaids/awynet.hxx index 81bc073a9..9ac508204 100755 --- a/src/Navaids/awynet.hxx +++ b/src/Navaids/awynet.hxx @@ -30,13 +30,14 @@ #include #include -#include #include //#include "parking.hxx" class FGAirway; // forward reference +class SGPath; +class SGGeod; typedef std::vector FGAirwayVector; typedef std::vector FGAirwayPointerVector; @@ -51,28 +52,26 @@ class FGNode private: std::string ident; SGGeod geod; + SGVec3d cart; // cached cartesian position int index; FGAirwayPointerVector next; // a vector to all the segments leaving from this node public: FGNode(); - FGNode(double lt, double ln, int idx, std::string id); + FGNode(const SGGeod& aPos, int idx, std::string id); void setIndex(int idx) { index = idx;}; void addAirway(FGAirway *segment) { next.push_back(segment); }; - double getLatitude() { return geod.getLatitudeDeg();}; - double getLongitude(){ return geod.getLongitudeDeg();}; - const SGGeod& getPosition() {return geod;} - + const SGVec3d& getCart() { return cart; } int getIndex() { return index; }; std::string getIdent() { return ident; }; FGNode *getAddress() { return this;}; FGAirwayPointerVectorIterator getBeginRoute() { return next.begin(); }; FGAirwayPointerVectorIterator getEndRoute() { return next.end(); }; - bool matches(std::string ident, double lat, double lon); + bool matches(std::string ident, const SGGeod& aPos); }; typedef std::vector FGNodeVector; @@ -191,12 +190,12 @@ public: void init(); bool exists() { return hasNetwork; }; - int findNearestNode(double lat, double lon); + int findNearestNode(const SGGeod& aPos); FGNode *findNode(int idx); FGAirRoute findShortestRoute(int start, int end); void trace(FGNode *, int, int, double dist); - void load(SGPath path); + void load(const SGPath& path); }; #endif -- 2.39.5