diff --git a/src/AIModel/AIAircraft.cxx b/src/AIModel/AIAircraft.cxx index a9e22a723..4bc5cc3c0 100644 --- a/src/AIModel/AIAircraft.cxx +++ b/src/AIModel/AIAircraft.cxx @@ -644,16 +644,10 @@ bool FGAIAircraft::leadPointReached(FGAIFlightPlan::waypoint* curr) { bool FGAIAircraft::aiTrafficVisible() { - double userLatitude = fgGetDouble("/position/latitude-deg"); - double userLongitude = fgGetDouble("/position/longitude-deg"); - double course, distance; - - SGWayPoint current(pos.getLongitudeDeg(), pos.getLatitudeDeg(), 0); - SGWayPoint user (userLongitude, userLatitude, 0); - - user.CourseAndDistance(current, &course, &distance); - - return ((distance * SG_METER_TO_NM) <= TRAFFICTOAIDISTTODIE); + SGGeod userPos(SGGeod::fromDeg(fgGetDouble("/position/longitude-deg"), + fgGetDouble("/position/latitude-deg"))); + + return (SGGeodesy::distanceNm(userPos, pos) <= TRAFFICTOAIDISTTODIE); } diff --git a/src/AIModel/AIFlightPlanCreate.cxx b/src/AIModel/AIFlightPlanCreate.cxx index 59e011e6c..f30d44f70 100644 --- a/src/AIModel/AIFlightPlanCreate.cxx +++ b/src/AIModel/AIFlightPlanCreate.cxx @@ -286,7 +286,7 @@ void FGAIFlightPlan::createTakeoffTaxi(FGAIAircraft *ac, bool firstFlight, char buffer[10]; snprintf (buffer, 10, "%d", node); FGTaxiNode *tn = apt->getDynamics()->getGroundNetwork()->findNode(node); - waypoint* wpt = createOnGround(ac, buffer, tn->geod(), apt->getElevation(), ac->getPerformance()->vTaxi()); + waypoint* wpt = createOnGround(ac, buffer, tn->getGeod(), apt->getElevation(), ac->getPerformance()->vTaxi()); wpt->routeIndex = route; waypoints.push_back(wpt); } @@ -358,7 +358,7 @@ void FGAIFlightPlan::createLandingTaxi(FGAIAircraft *ac, FGAirport *apt, char buffer[10]; snprintf (buffer, 10, "%d", node); FGTaxiNode *tn = gn->findNode(node); - waypoint* wpt = createOnGround(ac, buffer, tn->geod(), apt->getElevation(), ac->getPerformance()->vTaxi()); + waypoint* wpt = createOnGround(ac, buffer, tn->getGeod(), apt->getElevation(), ac->getPerformance()->vTaxi()); wpt->routeIndex = route; waypoints.push_back(wpt); } diff --git a/src/AIModel/AIFlightPlanCreateCruise.cxx b/src/AIModel/AIFlightPlanCreateCruise.cxx index 8d9cc8db5..d7761b5d2 100755 --- a/src/AIModel/AIFlightPlanCreateCruise.cxx +++ b/src/AIModel/AIFlightPlanCreateCruise.cxx @@ -51,37 +51,30 @@ void FGAIFlightPlan::evaluateRoutePart(double deplat, intVec nodes; int tmpNode, prevNode; - - SGWayPoint first (deplon, - deplat, - 100); - SGWayPoint sec (arrlon, - arrlat, - 100); - double course, distance; - first.CourseAndDistance(sec, &course, &distance); - distance *= SG_METER_TO_NM; - - SGVec3d a = SGVec3d::fromGeoc(SGGeoc::fromDegM(deplon, deplat, 1)); - SGVec3d b = SGVec3d::fromGeoc(SGGeoc::fromDegM(arrlon, arrlat, 1)); + SGGeoc dep(SGGeoc::fromDegM(deplon, deplat, 100.0)); + SGGeoc arr(SGGeoc::fromDegM(arrlon, arrlat, 100.0)); + + SGVec3d a = SGVec3d::fromGeoc(dep); + SGVec3d b = SGVec3d::fromGeoc(arr); SGVec3d _cross = cross(b, a); double angle = sgACos(dot(a, b)); tmpNode = 0; for (double ang = 0.0; ang < angle; ang += 0.05) - { + { sgdVec3 newPos; sgdMat4 matrix; //cerr << "Angle = " << ang << endl; sgdMakeRotMat4(matrix, ang, _cross.sg()); for(int j = 0; j < 3; j++) - { - newPos[j] =0.0; - for (int k = 0; k<3; k++) - { - newPos[j] += matrix[j][k]*a[k]; - } - } + { + newPos[j] =0.0; + for (int k = 0; k<3; k++) + { + newPos[j] += matrix[j][k]*a[k]; + } + } + //cerr << "1"<< endl; SGGeoc geoc = SGGeoc::fromCart(SGVec3d(newPos[0], newPos[1], newPos[2])); @@ -91,17 +84,12 @@ void FGAIFlightPlan::evaluateRoutePart(double deplat, prevNode = tmpNode; tmpNode = globals->get_airwaynet()->findNearestNode(midlat, midlon); - double nodelat = globals->get_airwaynet()->findNode(tmpNode)->getLatitude (); - double nodelon = globals->get_airwaynet()->findNode(tmpNode)->getLongitude (); - SGWayPoint curr(midlat, - midlon, - 100); - SGWayPoint node(nodelat, - nodelon, - 100); - curr.CourseAndDistance(node, &course, &distance); - if ((distance < 25000) && (tmpNode != prevNode)) - nodes.push_back(tmpNode); + FGNode* node = globals->get_airwaynet()->findNode(tmpNode); + SGGeoc nodePos(SGGeoc::fromGeod(node->getPosition())); + + if ((tmpNode != prevNode) && (SGGeodesy::distanceM(geoc, nodePos) < 25000)) { + nodes.push_back(tmpNode); + } } intVecIterator i = nodes.begin(); diff --git a/src/Airports/gnnode.cxx b/src/Airports/gnnode.cxx index b7b21b040..c9883d812 100644 --- a/src/Airports/gnnode.cxx +++ b/src/Airports/gnnode.cxx @@ -44,6 +44,26 @@ bool sortByLength(FGTaxiSegment *a, FGTaxiSegment *b) { *************************************************************************/ +void FGTaxiNode::setLatitude (double val) +{ + geod.setLatitudeDeg(val); +} + +void FGTaxiNode::setLongitude(double val) +{ + geod.setLongitudeDeg(val); +} + +void FGTaxiNode::setLatitude (const string& val) +{ + geod.setLatitudeDeg(processPosition(val)); +} + +void FGTaxiNode::setLongitude(const string& val) +{ + geod.setLongitudeDeg(processPosition(val)); +} + void FGTaxiNode::sortEndSegments(bool byLength) { if (byLength) diff --git a/src/Airports/gnnode.hxx b/src/Airports/gnnode.hxx index d265ff5ad..3358ea51e 100644 --- a/src/Airports/gnnode.hxx +++ b/src/Airports/gnnode.hxx @@ -22,22 +22,17 @@ #include #include -using std::string; -using std::vector; - class FGTaxiSegment; -typedef vector FGTaxiSegmentVector; +typedef std::vector FGTaxiSegmentVector; typedef FGTaxiSegmentVector::iterator FGTaxiSegmentVectorIterator; bool sortByHeadingDiff(FGTaxiSegment *a, FGTaxiSegment *b); bool sortByLength (FGTaxiSegment *a, FGTaxiSegment *b); -double processPosition(const string& pos); class FGTaxiNode { private: - double lat; - double lon; + SGGeod geod; int index; bool isOnRunway; @@ -51,8 +46,6 @@ private: public: FGTaxiNode() : - lat (0.0), - lon (0.0), index(0), isOnRunway(false), holdType(0), @@ -63,8 +56,7 @@ public: }; FGTaxiNode(const FGTaxiNode &other) : - lat(other.lat), - lon(other.lon), + geod(other.geod), index(other.index), isOnRunway(other.isOnRunway), holdType(other.holdType), @@ -77,8 +69,7 @@ public: FGTaxiNode &operator =(const FGTaxiNode &other) { - lat = other.lat; - lon = other.lon; + geod = other.geod; index = other.index; isOnRunway = other.isOnRunway; holdType = other.holdType; @@ -90,10 +81,10 @@ FGTaxiNode &operator =(const FGTaxiNode &other) }; void setIndex(int idx) { index = idx; }; - void setLatitude (double val) { lat = val; }; - void setLongitude(double val) { lon = val; }; - void setLatitude (const string& val) { lat = processPosition(val); }; - void setLongitude(const string& val) { lon = processPosition(val); }; + void setLatitude (double val); + void setLongitude(double val); + void setLatitude (const std::string& val); + void setLongitude(const std::string& val); void addSegment(FGTaxiSegment *segment) { next.push_back(segment); }; void setHoldPointType(int val) { holdType = val; }; void setOnRunway(bool val) { isOnRunway = val; }; @@ -106,10 +97,10 @@ FGTaxiNode &operator =(const FGTaxiNode &other) FGTaxiSegment *getPreviousSegment() { return previousSeg; }; double getPathScore() { return pathScore; }; - double getLatitude() { return lat;}; - double getLongitude(){ return lon;}; + double getLatitude() { return geod.getLatitudeDeg();}; + double getLongitude(){ return geod.getLongitudeDeg();}; - SGGeod geod() const { return SGGeod::fromDeg(lon, lat); } + const SGGeod& getGeod() const { return geod; } int getIndex() { return index; }; int getHoldPointType() { return holdType; }; @@ -124,7 +115,7 @@ FGTaxiNode &operator =(const FGTaxiNode &other) }; -typedef vector FGTaxiNodeVector; +typedef std::vector FGTaxiNodeVector; typedef FGTaxiNodeVector::iterator FGTaxiNodeVectorIterator; #endif diff --git a/src/Airports/groundnetwork.cxx b/src/Airports/groundnetwork.cxx index 877357229..529fc5952 100644 --- a/src/Airports/groundnetwork.cxx +++ b/src/Airports/groundnetwork.cxx @@ -79,14 +79,7 @@ void FGTaxiSegment::setEnd(FGTaxiNodeVector *nodes) // doing this. void FGTaxiSegment::setTrackDistance() { - //double course; - SGWayPoint first (start->getLongitude(), - start->getLatitude(), - 0); - SGWayPoint second (end->getLongitude(), - end->getLatitude(), - 0); - first.CourseAndDistance(second, &course, &length); + length = SGGeodesy::distanceM(start->getGeod(), end->getGeod()); } @@ -301,35 +294,26 @@ void FGGroundNetwork::init() int FGGroundNetwork::findNearestNode(const SGGeod& aGeod) { - return findNearestNode(aGeod.getLatitudeDeg(), aGeod.getLongitudeDeg()); + double minDist = HUGE_VAL; + int index = -1; + + for (FGTaxiNodeVectorIterator itr = nodes.begin(); itr != nodes.end(); itr++) + { + double d = SGGeodesy::distanceM(aGeod, (*itr)->getGeod()); + if (d < minDist) + { + minDist = d; + index = (*itr)->getIndex(); + //cerr << "Minimum distance of " << minDist << " for index " << index << endl; + } + } + + return index; } int FGGroundNetwork::findNearestNode(double lat, double lon) { - double minDist = HUGE_VAL; - double dist; - int index; - SGWayPoint first (lon, - lat, - 0); - - for (FGTaxiNodeVectorIterator - itr = nodes.begin(); - itr != nodes.end(); itr++) - { - double course; - SGWayPoint second ((*itr)->getLongitude(), - (*itr)->getLatitude(), - 0); - first.CourseAndDistance(second, &course, &dist); - if (dist < minDist) - { - minDist = dist; - index = (*itr)->getIndex(); - //cerr << "Minimum distance of " << minDist << " for index " << index << endl; - } - } - return index; + return findNearestNode(SGGeod::fromDeg(lon, lat)); } FGTaxiNode *FGGroundNetwork::findNode(int idx) @@ -609,24 +593,21 @@ void FGGroundNetwork::checkSpeedAdjustment(int id, double lat, double mindist = HUGE_VAL; if (activeTraffic.size()) { - double course, dist, bearing, minbearing; - SGWayPoint curr (lon, - lat, - alt); + double course, dist, bearing, minbearing, az2; + SGGeod curr(SGGeod::fromDegM(lon, lat, alt)); //TrafficVector iterator closest; closest = current; for (TrafficVectorIterator i = activeTraffic.begin(); i != activeTraffic.end(); i++) { - if (i != current) { - //SGWayPoint curr (lon, - // lat, - // alt); - SGWayPoint other (i->getLongitude (), - i->getLatitude (), - i->getAltitude ()); - other.CourseAndDistance(curr, &course, &dist); - bearing = fabs(heading-course); + if (i == current) { + continue; + } + + SGGeod other(SGGeod::fromDegM(i->getLongitude(), + i->getLatitude(), i->getAltitude())); + SGGeodesy::inverse(other, curr, course, az2, dist); + bearing = fabs(heading-course); if (bearing > 180) bearing = 360-bearing; if ((dist < mindist) && (bearing < 60.0)) @@ -635,7 +616,6 @@ void FGGroundNetwork::checkSpeedAdjustment(int id, double lat, closest = i; minbearing = bearing; } - } } //Check traffic at the tower controller if (towerController->hasActiveTraffic()) @@ -644,16 +624,12 @@ void FGGroundNetwork::checkSpeedAdjustment(int id, double lat, i != towerController->getActiveTraffic().end(); i++) { //cerr << "Comparing " << current->getId() << " and " << i->getId() << endl; - //SGWayPoint curr (lon, - // lat, - // alt); - SGWayPoint other (i->getLongitude (), - i->getLatitude (), - i->getAltitude ()); - other.CourseAndDistance(curr, &course, &dist); - bearing = fabs(heading-course); + SGGeod other(SGGeod::fromDegM(i->getLongitude(), + i->getLatitude(), i->getAltitude())); + SGGeodesy::inverse(other, curr, course, az2, dist); + bearing = fabs(heading-course); if (bearing > 180) - bearing = 360-bearing; + bearing = 360-bearing; if ((dist < mindist) && (bearing < 60.0)) { mindist = dist; @@ -666,10 +642,9 @@ void FGGroundNetwork::checkSpeedAdjustment(int id, double lat, // Finally, check UserPosition double userLatitude = fgGetDouble("/position/latitude-deg"); double userLongitude = fgGetDouble("/position/longitude-deg"); - SGWayPoint user (userLongitude, - userLatitude, - alt); // Alt is not really important here. - user.CourseAndDistance(curr, &course, &dist); + SGGeod user(SGGeod::fromDeg(userLatitude,userLongitude)); + SGGeodesy::inverse(user, curr, course, az2, dist); + bearing = fabs(heading-course); if (bearing > 180) bearing = 360-bearing; @@ -750,10 +725,8 @@ void FGGroundNetwork::checkHoldPosition(int id, double lat, } current = i; current->setHoldPosition(false); - SGWayPoint curr (lon, - lat, - alt); - double course, dist, bearing, minbearing; + SGGeod curr(SGGeod::fromDegM(lon, lat, alt)); + for (i = activeTraffic.begin(); i != activeTraffic.end(); i++) { @@ -762,20 +735,16 @@ void FGGroundNetwork::checkHoldPosition(int id, double lat, int node = current->crosses(this, *i); if (node != -1) { + FGTaxiNode* taxiNode = findNode(node); + // Determine whether it's save to continue or not. // If we have a crossing route, there are two possibilities: // 1) This is an interestion // 2) This is oncoming two-way traffic, using the same taxiway. //cerr << "Hold check 1 : " << id << " has common node " << node << endl; - SGWayPoint nodePos(findNode(node)->getLongitude (), - findNode(node)->getLatitude (), - alt); - - SGWayPoint other (i->getLongitude (), - i->getLatitude (), - i->getAltitude ()); - //other.CourseAndDistance(curr, &course, &dist); - bool needsToWait; + + SGGeod other(SGGeod::fromDegM(i->getLongitude(), i->getLatitude(), i->getAltitude())); + bool needsToWait; bool opposing; if (current->isOpposing(this, *i, node)) { @@ -793,8 +762,7 @@ void FGGroundNetwork::checkHoldPosition(int id, double lat, else { opposing = false; - other.CourseAndDistance(nodePos, &course, &dist); - if (dist > 200) // 2.0*i->getRadius()) + if (SGGeodesy::distanceM(other, taxiNode->getGeod()) > 200) // 2.0*i->getRadius()) { needsToWait = false; //cerr << "Hold check 3 : " << id <<" Other aircraft approaching node is still far away. (" << dist << " nm). Can safely continue " @@ -806,8 +774,9 @@ void FGGroundNetwork::checkHoldPosition(int id, double lat, //cerr << "Hold check 4: " << id << " Would need to wait for other aircraft : distance = " << dist << " meters" << endl; } } - curr.CourseAndDistance(nodePos, &course, &dist); - if (!(i->hasHoldPosition())) + + double dist = SGGeodesy::distanceM(curr, taxiNode->getGeod()); + if (!(i->hasHoldPosition())) { if ((dist < 200) && //2.5*current->getRadius()) && diff --git a/src/Navaids/awynet.cxx b/src/Navaids/awynet.cxx index b2865b4e1..47b30ffd6 100755 --- a/src/Navaids/awynet.cxx +++ b/src/Navaids/awynet.cxx @@ -47,11 +47,18 @@ FGNode::FGNode() { } +FGNode::FGNode(double lt, double ln, int idx, std::string id) : + ident(id), + geod(SGGeod::fromDeg(ln, lt)), + index(idx) +{ +} + bool FGNode::matches(string id, double lt, double ln) { if ((ident == id) && - (fabs(lt - lat) < 1.0) && - (fabs(ln - lon) < 1.0)) + (fabs(lt - geod.getLatitudeDeg()) < 1.0) && + (fabs(ln - geod.getLongitudeDeg()) < 1.0)) return true; else return false; @@ -92,15 +99,7 @@ void FGAirway::setEnd(node_map *nodes) // doing this. void FGAirway::setTrackDistance() { - double course; - SGWayPoint first (start->getLongitude(), - start->getLatitude(), - 0); - SGWayPoint second (end->getLongitude(), - end->getLatitude(), - 0); - first.CourseAndDistance(second, &course, &length); - + length = SGGeodesy::distanceM(start->getPosition(), end->getPosition()); } /*************************************************************************** diff --git a/src/Navaids/awynet.hxx b/src/Navaids/awynet.hxx index 92ca9c368..81bc073a9 100755 --- a/src/Navaids/awynet.hxx +++ b/src/Navaids/awynet.hxx @@ -50,24 +50,21 @@ class FGNode { private: std::string ident; - double lat; - double lon; + SGGeod geod; 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) { lat = lt; lon = ln; index = idx; ident = id;}; + FGNode(double lt, double ln, int idx, std::string id); void setIndex(int idx) { index = idx;}; - void setLatitude (double val) { lat = val;}; - void setLongitude(double val) { lon = val;}; - //void setLatitude (const std::string& val) { lat = processPosition(val); }; - //void setLongitude(const std::string& val) { lon = processPosition(val); }; void addAirway(FGAirway *segment) { next.push_back(segment); }; - double getLatitude() { return lat;}; - double getLongitude(){ return lon;}; + double getLatitude() { return geod.getLatitudeDeg();}; + double getLongitude(){ return geod.getLongitudeDeg();}; + + const SGGeod& getPosition() {return geod;} int getIndex() { return index; }; std::string getIdent() { return ident; };