#include <math.h>
#include <algorithm>
-//#include <plib/sg.h>
-//#include <plib/ul.h>
-
-//#include <Environment/environment_mgr.hxx>
-//#include <Environment/environment.hxx>
-//#include <simgear/misc/sg_path.hxx>
-//#include <simgear/props/props.hxx>
-//#include <simgear/structure/subsystem_mgr.hxx>
#include <simgear/debug/logstream.hxx>
#include <simgear/route/waypoint.hxx>
-//#include <Main/globals.hxx>
-//#include <Main/fg_props.hxx>
-//#include <Airports/runways.hxx>
-#include <AIModel/AIFlightPlan.hxx>
+#include <Airports/dynamics.hxx>
-//#include STL_STRING
+#include <AIModel/AIFlightPlan.hxx>
#include "groundnetwork.hxx"
-SG_USING_STD(sort);
-
-/*****************************************************************************
- * Helper function for parsing position string
- ****************************************************************************/
-double processPosition(const string &pos)
-{
- string prefix;
- string subs;
- string degree;
- string decimal;
- int sign = 1;
- double value;
- subs = pos;
- prefix= subs.substr(0,1);
- if (prefix == string("S") || (prefix == string("W")))
- sign = -1;
- subs = subs.substr(1, subs.length());
- degree = subs.substr(0, subs.find(" ",0));
- decimal = subs.substr(subs.find(" ",0), subs.length());
-
-
- //cerr << sign << " "<< degree << " " << decimal << endl;
- value = sign * (atof(degree.c_str()) + atof(decimal.c_str())/60.0);
- //cerr << value <<endl;
- //exit(1);
- return value;
-}
-
-/**************************************************************************
- * FGTaxiNode
- *************************************************************************/
-FGTaxiNode::FGTaxiNode()
-{
-}
-
-void FGTaxiNode::sortEndSegments(bool byLength)
-{
- if (byLength)
- sort(next.begin(), next.end(), sortByLength);
- else
- sort(next.begin(), next.end(), sortByHeadingDiff);
-}
-
-
-bool compare_nodes(FGTaxiNode *a, FGTaxiNode *b) {
-return (*a) < (*b);
-}
-
/***************************************************************************
* FGTaxiSegment
**************************************************************************/
-FGTaxiSegment::FGTaxiSegment()
-{
- oppositeDirection = 0;
- isActive = true;
-}
void FGTaxiSegment::setStart(FGTaxiNodeVector *nodes)
{
FGTaxiNodeVectorIterator i = nodes->begin();
while (i != nodes->end())
{
+ //cerr << "Scanning start node index" << (*i)->getIndex() << endl;
if ((*i)->getIndex() == startNode)
{
start = (*i)->getAddress();
}
i++;
}
+ SG_LOG(SG_GENERAL, SG_ALERT, "Could not find start node " << startNode << endl);
}
void FGTaxiSegment::setEnd(FGTaxiNodeVector *nodes)
FGTaxiNodeVectorIterator i = nodes->begin();
while (i != nodes->end())
{
+ //cerr << "Scanning end node index" << (*i)->getIndex() << endl;
if ((*i)->getIndex() == endNode)
{
end = (*i)->getAddress();
}
i++;
}
+ SG_LOG(SG_GENERAL, SG_ALERT, "Could not find end node " << endNode << endl);
}
// 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());
}
headingDiff = fabs(headingDiff - 360);
}
-bool compare_segments(FGTaxiSegment *a, FGTaxiSegment *b) {
-return (*a) < (*b);
-}
-
-bool sortByHeadingDiff(FGTaxiSegment *a, FGTaxiSegment *b) {
- return a->hasSmallerHeadingDiff(*b);
-}
-bool sortByLength(FGTaxiSegment *a, FGTaxiSegment *b) {
- return a->getLength() > b->getLength();
-}
/***************************************************************************
* FGTaxiRoute
**************************************************************************/
/***************************************************************************
* FGGroundNetwork()
**************************************************************************/
+bool compare_nodes(FGTaxiNode *a, FGTaxiNode *b) {
+return (*a) < (*b);
+}
+
+bool compare_segments(FGTaxiSegment *a, FGTaxiSegment *b) {
+return (*a) < (*b);
+}
FGGroundNetwork::FGGroundNetwork()
{
delete (*node);
}
nodes.clear();
+ pushBackNodes.clear();
for (FGTaxiSegmentVectorIterator seg = segments.begin();
seg != segments.end();
seg++)
//sort(segments.begin(), segments.end(), compare_segments());
FGTaxiSegmentVectorIterator i = segments.begin();
while(i != segments.end()) {
- //cerr << "initializing node " << i->getIndex() << endl;
(*i)->setStart(&nodes);
(*i)->setEnd (&nodes);
(*i)->setTrackDistance();
(*i)->setIndex(index);
- //cerr << "Track distance = " << i->getLength() << endl;
- //cerr << "Track ends at" << i->getEnd()->getIndex() << endl;
+ if ((*i)->isPushBack()) {
+ pushBackNodes.push_back((*i)->getEnd());
+ }
+ //SG_LOG(SG_GENERAL, SG_BULK, "initializing segment " << (*i)->getIndex() << endl);
+ //SG_LOG(SG_GENERAL, SG_BULK, "Track distance = " << (*i)->getLength() << endl);
+ //SG_LOG(SG_GENERAL, SG_BULK, "Track runs from " << (*i)->getStart()->getIndex() << " to "
+ // << (*i)->getEnd()->getIndex() << endl);
i++;
index++;
}
{
if ((*j)->getEnd()->getIndex() == (*i)->getStart()->getIndex())
{
- int start1 = (*i)->getStart()->getIndex();
- int end1 = (*i)->getEnd() ->getIndex();
- int start2 = (*j)->getStart()->getIndex();
- int end2 = (*j)->getEnd()->getIndex();
- int oppIndex = (*j)->getIndex();
+// int start1 = (*i)->getStart()->getIndex();
+// int end1 = (*i)->getEnd() ->getIndex();
+// int start2 = (*j)->getStart()->getIndex();
+// int end2 = (*j)->getEnd()->getIndex();
+// int oppIndex = (*j)->getIndex();
//cerr << "Opposite of " << (*i)->getIndex() << " (" << start1 << "," << end1 << ") "
// << "happens to be " << oppIndex << " (" << start2 << "," << end2 << ") " << endl;
(*i)->setOpposite(*j);
}
i++;
}
+ //FGTaxiNodeVectorIterator j = nodes.begin();
+ //while (j != nodes.end()) {
+ // if ((*j)->getHoldPointType() == 3) {
+ // pushBackNodes.push_back((*j));
+ // }
+ // j++;
+ //}
//cerr << "Done initializing ground network" << endl;
//exit(1);
}
-
-
-int FGGroundNetwork::findNearestNode(double lat, double lon)
+int FGGroundNetwork::findNearestNode(const SGGeod& aGeod)
{
double minDist = HUGE_VAL;
- double dist;
- int index;
- SGWayPoint first (lon,
- lat,
- 0);
+ int index = -1;
- for (FGTaxiNodeVectorIterator
- itr = nodes.begin();
- itr != nodes.end(); itr++)
+ for (FGTaxiNodeVectorIterator itr = nodes.begin(); itr != nodes.end(); itr++)
+ {
+ double d = SGGeodesy::distanceM(aGeod, (*itr)->getGeod());
+ if (d < minDist)
{
- 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;
- }
+ minDist = d;
+ index = (*itr)->getIndex();
+ //cerr << "Minimum distance of " << minDist << " for index " << index << endl;
}
+ }
+
return index;
}
-FGTaxiNode *FGGroundNetwork::findNode(int idx)
+int FGGroundNetwork::findNearestNode(double lat, double lon)
+{
+ return findNearestNode(SGGeod::fromDeg(lon, lat));
+}
+
+FGTaxiNode *FGGroundNetwork::findNode(unsigned idx)
{ /*
for (FGTaxiNodeVectorIterator
itr = nodes.begin();
return 0;
}
-FGTaxiSegment *FGGroundNetwork::findSegment(int idx)
+FGTaxiSegment *FGGroundNetwork::findSegment(unsigned idx)
{/*
for (FGTaxiSegmentVectorIterator
itr = segments.begin();
}
-FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end)
+FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end, bool fullSearch)
{
//implements Dijkstra's algorithm to find shortest distance route from start to end
//taken from http://en.wikipedia.org/wiki/Dijkstra's_algorithm
//double INFINITE = 100000000000.0;
// initialize scoring values
+ int nParkings = parent->getDynamics()->getNrOfParkings();
+ FGTaxiNodeVector *currNodesSet;
+ if (fullSearch) {
+ currNodesSet = &nodes;
+ } else {
+ currNodesSet = &pushBackNodes;
+ }
+
for (FGTaxiNodeVectorIterator
- itr = nodes.begin();
- itr != nodes.end(); itr++) {
- (*itr)->pathscore = HUGE_VAL; //infinity by all practical means
- (*itr)->previousnode = 0; //
- (*itr)->previousseg = 0; //
+ itr = currNodesSet->begin();
+ itr != currNodesSet->end(); itr++) {
+ (*itr)->setPathScore(HUGE_VAL); //infinity by all practical means
+ (*itr)->setPreviousNode(0); //
+ (*itr)->setPreviousSeg (0); //
}
FGTaxiNode *firstNode = findNode(start);
- firstNode->pathscore = 0;
+ firstNode->setPathScore(0);
FGTaxiNode *lastNode = findNode(end);
- FGTaxiNodeVector unvisited(nodes); // working copy
+ FGTaxiNodeVector unvisited(*currNodesSet); // working copy
while (!unvisited.empty()) {
FGTaxiNode* best = *(unvisited.begin());
for (FGTaxiNodeVectorIterator
itr = unvisited.begin();
itr != unvisited.end(); itr++) {
- if ((*itr)->pathscore < best->pathscore)
+ if ((*itr)->getPathScore() < best->getPathScore())
best = (*itr);
}
for (FGTaxiSegmentVectorIterator
seg = best->getBeginRoute();
seg != best->getEndRoute(); seg++) {
- FGTaxiNode* tgt = (*seg)->getEnd();
- double alt = best->pathscore + (*seg)->getLength();
- if (alt < tgt->pathscore) { // Relax (u,v)
- tgt->pathscore = alt;
- tgt->previousnode = best;
- tgt->previousseg = *seg; //
+ if (fullSearch || (*seg)->isPushBack()) {
+ FGTaxiNode* tgt = (*seg)->getEnd();
+ double alt = best->getPathScore() + (*seg)->getLength() + (*seg)->getPenalty(nParkings);
+ if (alt < tgt->getPathScore()) { // Relax (u,v)
+ tgt->setPathScore(alt);
+ tgt->setPreviousNode(best);
+ tgt->setPreviousSeg(*seg); //
+ }
+ } else {
+ // // cerr << "Skipping TaxiSegment " << (*seg)->getIndex() << endl;
}
}
}
}
- if (lastNode->pathscore == HUGE_VAL) {
+ if (lastNode->getPathScore() == HUGE_VAL) {
// no valid route found
- SG_LOG( SG_GENERAL, SG_ALERT, "Failed to find route from waypoint " << start << " to " << end << " at " <<
- parent->getId());
- exit(1); //TODO exit more gracefully, no need to stall the whole sim with broken GN's
+ if (fullSearch) {
+ SG_LOG( SG_GENERAL, SG_ALERT, "Failed to find route from waypoint " << start << " to " << end << " at " <<
+ parent->getId());
+ }
+ FGTaxiRoute empty;
+ return empty;
+ //exit(1); //TODO exit more gracefully, no need to stall the whole sim with broken GN's
} else {
// assemble route from backtrace information
intVec nodes, routes;
FGTaxiNode* bt = lastNode;
- while (bt->previousnode != 0) {
+ while (bt->getPreviousNode() != 0) {
nodes.push_back(bt->getIndex());
- routes.push_back(bt->previousseg->getIndex());
- bt = bt->previousnode;
+ routes.push_back(bt->getPreviousSegment()->getIndex());
+ bt = bt->getPreviousNode();
}
nodes.push_back(start);
reverse(nodes.begin(), nodes.end());
reverse(routes.begin(), routes.end());
- return FGTaxiRoute(nodes, routes, lastNode->pathscore, 0);
+ return FGTaxiRoute(nodes, routes, lastNode->getPathScore(), 0);
}
}
-
-// void FGGroundNetwork::trace(FGTaxiNode *currNode, int end, int depth, double distance)
-// {
-// // Just check some preconditions of the trace algorithm
-// if (nodesStack.size() != routesStack.size())
-// {
-// SG_LOG(SG_GENERAL, SG_ALERT, "size of nodesStack and routesStack is not equal. NodesStack :"
-// << nodesStack.size() << ". RoutesStack : " << routesStack.size());
-// }
-// nodesStack.push_back(currNode->getIndex());
-// totalDistance += distance;
-// //cerr << "Starting trace " << currNode->getIndex() << " " << "total distance: " << totalDistance << endl;
-// // << currNode->getIndex() << endl;
-//
-// // If the current route matches the required end point we found a valid route
-// // So we can add this to the routing table
-// if (currNode->getIndex() == end)
-// {
-// maxDepth = depth;
-// //cerr << "Found route : " << totalDistance << "" << " " << *(nodesStack.end()-1) << " Depth = " << depth << endl;
-// routes.push_back(FGTaxiRoute(nodesStack,routesStack,totalDistance, depth));
-// if (nodesStack.empty() || routesStack.empty())
-// {
-// printRoutingError(string("while finishing route"));
-// }
-// nodesStack.pop_back();
-// routesStack.pop_back();
-// if (!(foundRoute)) {
-// maxDistance = totalDistance;
-// }
-// else
-// if (totalDistance < maxDistance)
-// maxDistance = totalDistance;
-// foundRoute = true;
-// totalDistance -= distance;
-// return;
-// }
-//
-//
-// // search if the currentNode has been encountered before
-// // if so, we should step back one level, because it is
-// // rather rediculous to proceed further from here.
-// // if the current node has not been encountered before,
-// // i should point to nodesStack.end()-1; and we can continue
-// // if i is not nodesStack.end, the previous node was found,
-// // and we should return.
-// // This only works at trace levels of 1 or higher though
-// if (depth > 0) {
-// intVecIterator i = nodesStack.begin();
-// while ((*i) != currNode->getIndex()) {
-// //cerr << "Route so far : " << (*i) << endl;
-// i++;
-// }
-// if (i != nodesStack.end()-1) {
-// if (nodesStack.empty() || routesStack.empty())
-// {
-// printRoutingError(string("while returning from an already encountered node"));
-// }
-// nodesStack.pop_back();
-// routesStack.pop_back();
-// totalDistance -= distance;
-// return;
-// }
-// if (depth >= maxDepth) {
-// count++;
-// if (!(count % 100000)) {
-// maxDepth--; // Gradually decrease maxdepth, to prevent "eternal searches"
-// //cerr << "Reducing maxdepth to " << maxDepth << endl;
-// }
-// nodesStack.pop_back();
-// routesStack.pop_back();
-// totalDistance -= distance;
-// return;
-// }
-// // If the total distance from start to the current waypoint
-// // is longer than that of a route we can also stop this trace
-// // and go back one level.
-// if ((totalDistance > maxDistance) && foundRoute)
-// //if (foundRoute)
-// {
-// //cerr << "Stopping rediculously long trace: " << totalDistance << endl;
-// if (nodesStack.empty() || routesStack.empty())
-// {
-// printRoutingError(string("while returning from finding a rediculously long route"));
-// }
-// nodesStack.pop_back();
-// routesStack.pop_back();
-// totalDistance -= distance;
-// return;
-// }
-// }
-/*
- //cerr << "2" << endl;
- if (currNode->getBeginRoute() != currNode->getEndRoute())
- {
- double course, length;
- //cerr << "3" << endl;
- // calculate distance and heading "as the crow flies" between starn and end points"
- SGWayPoint first(currNode->getLongitude(),
- currNode->getLatitude(),
- 0);
- //SGWayPoint second (lastNode->getLongitude(),
- // lastNode->getLatitude(),
- // 0);
-
- first.CourseAndDistance(destination, &course, &length);
- //for (FGTaxiSegmentVectorIterator
- // itr = segments.begin();
- // itr != segments.end(); itr++)
- // {
- // (*itr)->setCourseDiff(course);
- // }
- //FGTaxiNodeVectorIterator nde = nodes.begin();
- //while (nde != nodes.end()) {
- //(*nde)->sortEndSegments();
- //nde++;
-
- for (FGTaxiSegmentVectorIterator
- i = currNode->getBeginRoute();
- i != currNode->getEndRoute();
- i++)
- {
- (*i)->setCourseDiff(course);
- }
- currNode->sortEndSegments(foundRoute);
- for (FGTaxiSegmentVectorIterator
- i = currNode->getBeginRoute();
- i != currNode->getEndRoute();
- i++)
- {
- //cerr << (*i)->getLength() << endl;
- //cerr << (*i)->getIndex() << endl;
- int idx = (*i)->getIndex();
- routesStack.push_back((*i)->getIndex());
- trace((*i)->getEnd(), end, depth+1, (*i)->getLength());
- // {
- // // cerr << currNode -> getIndex() << " ";
- // route.push_back(currNode->getIndex());
- // return true;
- // }
- }
- }
- else
- {
- //SG_LOG( SG_GENERAL, SG_DEBUG, "4" );
- }
- if (nodesStack.empty())
- {
- printRoutingError(string("while finishing trace"));
- }
- nodesStack.pop_back();
- // Make sure not to dump the level-zero routesStack entry, because that was never created.
- if (depth)
- {
- routesStack.pop_back();
- //cerr << "leaving trace " << routesStack.size() << endl;
- }
- totalDistance -= distance;
- return;
-}*/
-
-void FGGroundNetwork::printRoutingError(string mess)
-{
- SG_LOG(SG_GENERAL, SG_ALERT, "Error in ground network trace algorithm " << mess);
- if (nodesStack.empty())
- {
- SG_LOG(SG_GENERAL, SG_ALERT, " nodesStack is empty. Dumping routesStack");
- for (intVecIterator i = routesStack.begin() ; i != routesStack.end(); i++)
- SG_LOG(SG_GENERAL, SG_ALERT, "Route " << (*i));
- }
- if (routesStack.empty())
- {
- SG_LOG(SG_GENERAL, SG_ALERT, " routesStack is empty. Dumping nodesStack");
- for (intVecIterator i = nodesStack.begin() ; i != nodesStack.end(); i++)
- SG_LOG(SG_GENERAL, SG_ALERT, "Node " << (*i));
- }
- //exit(1);
+int FGTaxiSegment::getPenalty(int nGates) {
+ int penalty = 0;
+ if (end->getIndex() < nGates) {
+ penalty += 10000;
+ }
+ if (end->getIsOnRunway()) { // For now. In future versions, need to find out whether runway is active.
+ penalty += 1000;
+ }
+ return penalty;
}
+/* ATC Related Functions */
void FGGroundNetwork::announcePosition(int id, FGAIFlightPlan *intendedRoute, int currentPosition,
double lat, double lon, double heading,
double speed, double alt, double radius, int leg,
- string callsign)
+ FGAIAircraft *aircraft)
{
TrafficVectorIterator i = activeTraffic.begin();
// Search search if the current id alread has an entry
rec.setPositionAndIntentions(currentPosition, intendedRoute);
rec.setPositionAndHeading(lat, lon, heading, speed, alt);
rec.setRadius(radius); // only need to do this when creating the record.
- rec.setCallSign(callsign);
+ rec.setAircraft(aircraft);
activeTraffic.push_back(rec);
} else {
i->setPositionAndIntentions(currentPosition, intendedRoute);
instruction. See below for the hold position instruction.
Note that there currently still is one flaw in the logic that needs to be addressed.
- can be situations where one aircraft is in front of the current aircraft, on a separate
+ There can be situations where one aircraft is in front of the current aircraft, on a separate
route, but really close after an intersection coming off the current route. This
aircraft is still close enough to block the current aircraft. This situation is currently
not addressed yet, but should be.
//closest = current;
previousInstruction = current->getSpeedAdjustment();
- double mindist = HUGE;
+ 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(curr, other, course, az2, dist);
+ bearing = fabs(heading-course);
if (bearing > 180)
bearing = 360-bearing;
if ((dist < mindist) && (bearing < 60.0))
closest = i;
minbearing = bearing;
}
- }
}
//Check traffic at the tower controller
if (towerController->hasActiveTraffic())
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(curr, other, course, az2, dist);
+ bearing = fabs(heading-course);
if (bearing > 180)
- bearing = 360-bearing;
+ bearing = 360-bearing;
if ((dist < mindist) && (bearing < 60.0))
{
mindist = dist;
// 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(userLongitude,userLatitude));
+ SGGeodesy::inverse(curr, user, course, az2, dist);
+
bearing = fabs(heading-course);
if (bearing > 180)
- bearing = 360-bearing;
+ bearing = 360-bearing;
if ((dist < mindist) && (bearing < 60.0))
{
mindist = dist;
minbearing = bearing;
otherReasonToSlowDown = true;
}
-
- // if (closest == current) {
- // //SG_LOG(SG_GENERAL, SG_ALERT, "AI error: closest and current match");
- // //return;
- // }
- //cerr << "Distance : " << dist << " bearing : " << bearing << " heading : " << heading
- // << " course : " << course << endl;
+
current->clearSpeedAdjustment();
if (current->checkPositionAndIntentions(*closest) || otherReasonToSlowDown)
return;
else
current->setWaitsForId(closest->getId());
- if (closest != current)
+ if (closest->getId() != current->getId())
current->setSpeedAdjustment(closest->getSpeed()* (mindist/100));
else
current->setSpeedAdjustment(0); // This can only happen when the user aircraft is the one closest
}
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++)
{
- if (i != current)
+ if (i->getId() != current->getId())
{
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))
{
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 "
//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()) &&
//if (printed)
// cerr << "[done] " << endl << endl;;
if (id == target) {
- SG_LOG(SG_GENERAL, SG_ALERT, "Detected circular wait condition");
- cerr << "Id = " << id << endl;
- cerr << "target = " << target << endl;
+ SG_LOG(SG_GENERAL, SG_WARN, "Detected circular wait condition: Id = " << id << "target = " << target);
return true;
} else {
return false;
projects / flightgear.git / blobdiff