#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);
void FGGroundNetwork::update(int id, double lat, double lon, double heading, double speed, double alt,
double dt) {
+ // Check whether aircraft are on hold due to a preceding pushback. If so, make sure to
+ // Transmit air-to-ground "Ready to taxi request:
+ // Transmit ground to air approval / hold
+ // Transmit confirmation ...
+ // Probably use a status mechanism similar to the Engine start procedure in the startup controller.
+
+
TrafficVectorIterator i = activeTraffic.begin();
// Search search if the current id has an entry
// This might be faster using a map instead of a vector, but let's start by taking a safe route
//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(userLatitude,userLongitude));
+ SGGeodesy::inverse(user, curr, course, az2, dist);
+
bearing = fabs(heading-course);
if (bearing > 180)
bearing = 360-bearing;
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()) &&
TrafficVectorIterator i = activeTraffic.begin();
int trafficSize = activeTraffic.size();
if (trafficSize) {
- //while ((i->getId() != id) && i != activeTraffic.end())
while (i != activeTraffic.end()) {
if (i->getId() == id) {
break;
}
else {
return false;
- }
+ }
if (i == activeTraffic.end() || (trafficSize == 0)) {
SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits");
}
target = current->getWaitsForId();
//bool printed = false; // Note that this variable is for debugging purposes only.
int counter = 0;
+
+ if (id == target) {
+ //cerr << "aircraft waits for user" << endl;
+ return false;
+ }
+
+
while ((target > 0) && (target != id) && counter++ < trafficSize) {
//printed = true;
TrafficVectorIterator i = activeTraffic.begin();
// << " (" << other->getId() << "); " << endl;;
//current = other;
}
+
+
+
+
+
+
//if (printed)
// cerr << "[done] " << endl << endl;;
if (id == target) {
- SG_LOG(SG_GENERAL, SG_INFO, "Detected circular wait condition");
+ SG_LOG(SG_GENERAL, SG_WARN, "Detected circular wait condition: Id = " << id << "target = " << target);
return true;
} else {
return false;