#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/AIAircraft.hxx>
+#include <AIModel/AIFlightPlan.hxx>
#include "groundnetwork.hxx"
-SG_USING_STD(sort);
-
-/**************************************************************************
- * FGTaxiNode
- *************************************************************************/
-FGTaxiNode::FGTaxiNode()
-{
-}
-
/***************************************************************************
* FGTaxiSegment
**************************************************************************/
-FGTaxiSegment::FGTaxiSegment()
-{
-}
-void FGTaxiSegment::setStart(FGTaxiNodeVector *nodes)
+void FGTaxiSegment::setStart(FGTaxiNodeVector * nodes)
{
- FGTaxiNodeVectorIterator i = nodes->begin();
- while (i != nodes->end())
- {
- if (i->getIndex() == startNode)
- {
- start = i->getAddress();
- i->addSegment(this);
- return;
- }
- i++;
+ FGTaxiNodeVectorIterator i = nodes->begin();
+ while (i != nodes->end()) {
+ //cerr << "Scanning start node index" << (*i)->getIndex() << endl;
+ if ((*i)->getIndex() == startNode) {
+ start = (*i)->getAddress();
+ (*i)->addSegment(this);
+ return;
+ }
+ i++;
}
+ SG_LOG(SG_GENERAL, SG_ALERT,
+ "Could not find start node " << startNode << endl);
}
-void FGTaxiSegment::setEnd(FGTaxiNodeVector *nodes)
+void FGTaxiSegment::setEnd(FGTaxiNodeVector * nodes)
{
- FGTaxiNodeVectorIterator i = nodes->begin();
- while (i != nodes->end())
- {
- if (i->getIndex() == endNode)
- {
- end = i->getAddress();
- return;
- }
- i++;
+ FGTaxiNodeVectorIterator i = nodes->begin();
+ while (i != nodes->end()) {
+ //cerr << "Scanning end node index" << (*i)->getIndex() << endl;
+ if ((*i)->getIndex() == endNode) {
+ end = (*i)->getAddress();
+ return;
+ }
+ i++;
}
+ SG_LOG(SG_GENERAL, SG_ALERT,
+ "Could not find end node " << endNode << endl);
}
+
+
// There is probably a computationally cheaper way of
// 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());
+}
+
+
+void FGTaxiSegment::setCourseDiff(double crse)
+{
+ headingDiff = fabs(course - crse);
+
+ if (headingDiff > 180)
+ headingDiff = fabs(headingDiff - 360);
}
+
+
/***************************************************************************
* FGTaxiRoute
**************************************************************************/
-bool FGTaxiRoute::next(int *nde)
-{
- //for (intVecIterator i = nodes.begin(); i != nodes.end(); i++)
- // cerr << "FGTaxiRoute contains : " << *(i) << endl;
- //cerr << "Offset from end: " << nodes.end() - currNode << endl;
- //if (currNode != nodes.end())
- // cerr << "true" << endl;
- //else
- // cerr << "false" << endl;
- //if (nodes.size() != (routes.size()) +1)
- // cerr << "ALERT: Misconfigured TaxiRoute : " << nodes.size() << " " << routes.size() << endl;
-
- if (currNode == nodes.end())
- return false;
- *nde = *(currNode);
- if (currNode != nodes.begin()) // make sure route corresponds to the end node
- currRoute++;
- currNode++;
- return true;
+bool FGTaxiRoute::next(int *nde)
+{
+ //for (intVecIterator i = nodes.begin(); i != nodes.end(); i++)
+ // cerr << "FGTaxiRoute contains : " << *(i) << endl;
+ //cerr << "Offset from end: " << nodes.end() - currNode << endl;
+ //if (currNode != nodes.end())
+ // cerr << "true" << endl;
+ //else
+ // cerr << "false" << endl;
+ //if (nodes.size() != (routes.size()) +1)
+ // cerr << "ALERT: Misconfigured TaxiRoute : " << nodes.size() << " " << routes.size() << endl;
+
+ if (currNode == nodes.end())
+ return false;
+ *nde = *(currNode);
+ if (currNode != nodes.begin()) // make sure route corresponds to the end node
+ currRoute++;
+ currNode++;
+ return true;
};
-bool FGTaxiRoute::next(int *nde, int *rte)
-{
- //for (intVecIterator i = nodes.begin(); i != nodes.end(); i++)
- // cerr << "FGTaxiRoute contains : " << *(i) << endl;
- //cerr << "Offset from end: " << nodes.end() - currNode << endl;
- //if (currNode != nodes.end())
- // cerr << "true" << endl;
- //else
- // cerr << "false" << endl;
- if (nodes.size() != (routes.size()) +1) {
- SG_LOG(SG_GENERAL, SG_ALERT, "ALERT: Misconfigured TaxiRoute : " << nodes.size() << " " << routes.size());
- exit(1);
- }
- if (currNode == nodes.end())
- return false;
- *nde = *(currNode);
- //*rte = *(currRoute);
- if (currNode != nodes.begin()) // Make sure route corresponds to the end node
- {
- *rte = *(currRoute);
- currRoute++;
+bool FGTaxiRoute::next(int *nde, int *rte)
+{
+ //for (intVecIterator i = nodes.begin(); i != nodes.end(); i++)
+ // cerr << "FGTaxiRoute contains : " << *(i) << endl;
+ //cerr << "Offset from end: " << nodes.end() - currNode << endl;
+ //if (currNode != nodes.end())
+ // cerr << "true" << endl;
+ //else
+ // cerr << "false" << endl;
+ if (nodes.size() != (routes.size()) + 1) {
+ SG_LOG(SG_GENERAL, SG_ALERT,
+ "ALERT: Misconfigured TaxiRoute : " << nodes.
+ size() << " " << routes.size());
+ exit(1);
}
- else
+ if (currNode == nodes.end())
+ return false;
+ *nde = *(currNode);
+ //*rte = *(currRoute);
+ if (currNode != nodes.begin()) // Make sure route corresponds to the end node
{
- // If currNode points to the first node, this means the aircraft is not on the taxi node
- // yet. Make sure to return a unique identifyer in this situation though, because otherwise
- // the speed adjust AI code may be unable to resolve whether two aircraft are on the same
- // taxi route or not. the negative of the preceding route seems a logical choice, as it is
- // unique for any starting location.
- // Note that this is probably just a temporary fix until I get Parking / tower control working.
- *rte = -1 * *(currRoute);
- }
- currNode++;
- return true;
+ *rte = *(currRoute);
+ currRoute++;
+ } else {
+ // If currNode points to the first node, this means the aircraft is not on the taxi node
+ // yet. Make sure to return a unique identifyer in this situation though, because otherwise
+ // the speed adjust AI code may be unable to resolve whether two aircraft are on the same
+ // taxi route or not. the negative of the preceding route seems a logical choice, as it is
+ // unique for any starting location.
+ // Note that this is probably just a temporary fix until I get Parking / tower control working.
+ *rte = -1 * *(currRoute);
+ }
+ currNode++;
+ return true;
};
void FGTaxiRoute::rewind(int route)
{
- int currPoint;
- int currRoute;
- first();
- do {
- if (!(next(&currPoint, &currRoute))) {
- SG_LOG(SG_GENERAL,SG_ALERT, "Error in rewinding TaxiRoute: current" << currRoute
- << " goal " << route);
- }
- } while (currRoute != route);
+ int currPoint;
+ int currRoute;
+ first();
+ do {
+ if (!(next(&currPoint, &currRoute))) {
+ SG_LOG(SG_GENERAL, SG_ALERT,
+ "Error in rewinding TaxiRoute: current" << currRoute <<
+ " goal " << route);
+ }
+ } while (currRoute != route);
}
+
+
+
/***************************************************************************
- * FGTrafficRecord
+ * FGGroundNetwork()
**************************************************************************/
-void FGTrafficRecord::setPositionAndIntentions(int pos, FGAIFlightPlan *route)
+bool compare_nodes(FGTaxiNode * a, FGTaxiNode * b)
{
-
- currentPos = pos;
- if (intentions.size()) {
- if (*intentions.begin() != pos) {
- SG_LOG(SG_GENERAL, SG_ALERT, "Error in FGTrafficRecord::setPositionAndIntentions");
- cerr << "Pos : " << pos << " Curr " << *(intentions.begin()) << endl;
- for (intVecIterator i = intentions.begin(); i != intentions.end() ; i++) {
- cerr << (*i) << " ";
- }
- cerr << endl;
- }
- intentions.erase(intentions.begin());
- } else {
- //int legNr, routeNr;
- //FGAIFlightPlan::waypoint* const wpt= route->getCurrentWaypoint();
- int size = route->getNrOfWayPoints();
- cerr << "Setting pos" << pos << " ";
- cerr << "setting intentions ";
- for (int i = 0; i < size; i++) {
- int val = route->getRouteIndex(i);
-
- if ((val) && (val != pos))
- {
- intentions.push_back(val);
- cerr << val<< " ";
- }
- }
- cerr << endl;
- //while (route->next(&legNr, &routeNr)) {
- //intentions.push_back(routeNr);
- //}
- //route->rewind(currentPos);
- }
- //exit(1);
+ return (*a) < (*b);
}
-bool FGTrafficRecord::checkPositionAndIntentions(FGTrafficRecord &other)
+bool compare_segments(FGTaxiSegment * a, FGTaxiSegment * b)
{
- bool result = false;
- //cerr << "Start check 1" << endl;
- if (currentPos == other.currentPos)
- {
- //cerr << "Check Position and intentions: current matches" << endl;
- result = true;
- }
- // else if (other.intentions.size())
-// {
-// cerr << "Start check 2" << endl;
-// intVecIterator i = other.intentions.begin();
-// while (!((i == other.intentions.end()) || ((*i) == currentPos)))
-// i++;
-// if (i != other.intentions.end()) {
-// cerr << "Check Position and intentions: current matches other.intentions" << endl;
-// result = true;
-// }
- else if (intentions.size()) {
- //cerr << "Start check 3" << endl;
- intVecIterator i = intentions.begin();
- while (!((i == intentions.end()) || ((*i) == other.currentPos)))
- i++;
- if (i != intentions.end()) {
- //cerr << "Check Position and intentions: .other.current matches" << endl;
- result = true;
- }
- }
- //cerr << "Done !!" << endl;
- return result;
+ return (*a) < (*b);
}
-void FGTrafficRecord::setPositionAndHeading(double lat, double lon, double hdg,
- double spd, double alt)
+FGGroundNetwork::FGGroundNetwork()
{
- latitude = lat;
- longitude = lon;
- heading = hdg;
- speed = spd;
- altitude = alt;
-}
+ hasNetwork = false;
+ foundRoute = false;
+ totalDistance = 0;
+ maxDistance = 0;
+ //maxDepth = 1000;
+ count = 0;
+ currTraffic = activeTraffic.begin();
-/***************************************************************************
- * FGGroundNetwork()
- **************************************************************************/
+}
-FGGroundNetwork::FGGroundNetwork()
+FGGroundNetwork::~FGGroundNetwork()
{
- hasNetwork = false;
- foundRoute = false;
- totalDistance = 0;
- maxDistance = 0;
- currTraffic = activeTraffic.begin();
+ for (FGTaxiNodeVectorIterator node = nodes.begin();
+ node != nodes.end(); node++) {
+ delete(*node);
+ }
+ nodes.clear();
+ pushBackNodes.clear();
+ for (FGTaxiSegmentVectorIterator seg = segments.begin();
+ seg != segments.end(); seg++) {
+ delete(*seg);
+ }
+ segments.clear();
}
-void FGGroundNetwork::addSegment(const FGTaxiSegment &seg)
+void FGGroundNetwork::addSegment(const FGTaxiSegment & seg)
{
- segments.push_back(seg);
+ segments.push_back(new FGTaxiSegment(seg));
}
-void FGGroundNetwork::addNode(const FGTaxiNode &node)
+void FGGroundNetwork::addNode(const FGTaxiNode & node)
{
- nodes.push_back(node);
+ nodes.push_back(new FGTaxiNode(node));
}
-void FGGroundNetwork::addNodes(FGParkingVec *parkings)
+void FGGroundNetwork::addNodes(FGParkingVec * parkings)
{
- FGTaxiNode n;
- FGParkingVecIterator i = parkings->begin();
- while (i != parkings->end())
- {
- n.setIndex(i->getIndex());
- n.setLatitude(i->getLatitude());
- n.setLongitude(i->getLongitude());
- nodes.push_back(n);
+ FGTaxiNode n;
+ FGParkingVecIterator i = parkings->begin();
+ while (i != parkings->end()) {
+ n.setIndex(i->getIndex());
+ n.setLatitude(i->getLatitude());
+ n.setLongitude(i->getLongitude());
+ nodes.push_back(new FGTaxiNode(n));
- i++;
+ i++;
}
}
void FGGroundNetwork::init()
{
- hasNetwork = true;
- int index = 1;
- 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;
- i++;
- }
- //exit(1);
-}
+ hasNetwork = true;
+ int index = 1;
+ sort(nodes.begin(), nodes.end(), compare_nodes);
+ //sort(segments.begin(), segments.end(), compare_segments());
+ FGTaxiSegmentVectorIterator i = segments.begin();
+ while (i != segments.end()) {
+ (*i)->setStart(&nodes);
+ (*i)->setEnd(&nodes);
+ (*i)->setTrackDistance();
+ (*i)->setIndex(index);
+ 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++;
+ }
-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;
+ i = segments.begin();
+ while (i != segments.end()) {
+ FGTaxiSegmentVectorIterator j = (*i)->getEnd()->getBeginRoute();
+ while (j != (*i)->getEnd()->getEndRoute()) {
+ 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();
+ //cerr << "Opposite of " << (*i)->getIndex() << " (" << start1 << "," << end1 << ") "
+ // << "happens to be " << oppIndex << " (" << start2 << "," << end2 << ") " << endl;
+ (*i)->setOpposite(*j);
+ break;
+ }
+ 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);
}
-FGTaxiNode *FGGroundNetwork::findNode(int idx)
+int FGGroundNetwork::findNearestNode(const SGGeod & aGeod)
{
- for (FGTaxiNodeVectorIterator
- itr = nodes.begin();
- itr != nodes.end(); itr++)
- {
- if (itr->getIndex() == idx)
- return itr->getAddress();
+ 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 0;
+
+ return index;
}
-FGTaxiSegment *FGGroundNetwork::findSegment(int idx)
+int FGGroundNetwork::findNearestNode(double lat, double lon)
{
- for (FGTaxiSegmentVectorIterator
- itr = segments.begin();
- itr != segments.end(); itr++)
- {
- if (itr->getIndex() == idx)
- return itr->getAddress();
- }
- return 0;
+ return findNearestNode(SGGeod::fromDeg(lon, lat));
}
-FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end)
-{
- foundRoute = false;
- totalDistance = 0;
- FGTaxiNode *firstNode = findNode(start);
- FGTaxiNode *lastNode = findNode(end);
- //prevNode = prevPrevNode = -1;
- //prevNode = start;
- routes.clear();
- nodesStack.clear();
- routesStack.clear();
-
- trace(firstNode, end, 0, 0);
- FGTaxiRoute empty;
-
- if (!foundRoute)
- {
- SG_LOG( SG_GENERAL, SG_INFO, "Failed to find route from waypoint " << start << " to " << end );
- exit(1);
- }
- sort(routes.begin(), routes.end());
- //for (intVecIterator i = route.begin(); i != route.end(); i++)
- // {
- // rte->push_back(*i);
- // }
-
- if (routes.begin() != routes.end())
- return *(routes.begin());
- else
- return empty;
+FGTaxiNode *FGGroundNetwork::findNode(unsigned idx)
+{ /*
+ for (FGTaxiNodeVectorIterator
+ itr = nodes.begin();
+ itr != nodes.end(); itr++)
+ {
+ if (itr->getIndex() == idx)
+ return itr->getAddress();
+ } */
+
+ if ((idx >= 0) && (idx < nodes.size()))
+ return nodes[idx]->getAddress();
+ else
+ return 0;
+}
+
+FGTaxiSegment *FGGroundNetwork::findSegment(unsigned idx)
+{ /*
+ for (FGTaxiSegmentVectorIterator
+ itr = segments.begin();
+ itr != segments.end(); itr++)
+ {
+ if (itr->getIndex() == idx)
+ return itr->getAddress();
+ }
+ */
+ if ((idx > 0) && (idx <= segments.size()))
+ return segments[idx - 1]->getAddress();
+ else {
+ //cerr << "Alert: trying to find invalid segment " << idx << endl;
+ return 0;
+ }
}
-void FGGroundNetwork::trace(FGTaxiNode *currNode, int end, int depth, double distance)
+FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end,
+ bool fullSearch)
{
- // 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());
+//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;
}
- nodesStack.push_back(currNode->getIndex());
- totalDistance += distance;
- //cerr << "Starting trace " << depth << " 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)
- {
- //cerr << "Found route : " << totalDistance << "" << " " << *(nodesStack.end()-1) << endl;
- routes.push_back(FGTaxiRoute(nodesStack,routesStack,totalDistance));
- 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 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)
- {
- //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())
- {
- //cerr << "3" << endl;
- for (FGTaxiSegmentPointerVectorIterator
- 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" );
+ for (FGTaxiNodeVectorIterator
+ itr = currNodesSet->begin(); itr != currNodesSet->end(); itr++) {
+ (*itr)->setPathScore(HUGE_VAL); //infinity by all practical means
+ (*itr)->setPreviousNode(0); //
+ (*itr)->setPreviousSeg(0); //
}
- if (nodesStack.empty())
- {
- printRoutingError(string("while finishing trace"));
+
+ FGTaxiNode *firstNode = findNode(start);
+ firstNode->setPathScore(0);
+
+ FGTaxiNode *lastNode = findNode(end);
+
+ FGTaxiNodeVector unvisited(*currNodesSet); // working copy
+
+ while (!unvisited.empty()) {
+ FGTaxiNode *best = *(unvisited.begin());
+ for (FGTaxiNodeVectorIterator
+ itr = unvisited.begin(); itr != unvisited.end(); itr++) {
+ if ((*itr)->getPathScore() < best->getPathScore())
+ best = (*itr);
+ }
+
+ FGTaxiNodeVectorIterator newend =
+ remove(unvisited.begin(), unvisited.end(), best);
+ unvisited.erase(newend, unvisited.end());
+
+ if (best == lastNode) { // found route or best not connected
+ break;
+ } else {
+ for (FGTaxiSegmentVectorIterator
+ seg = best->getBeginRoute();
+ seg != best->getEndRoute(); 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;
+ }
+ }
+ }
}
- 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;
+
+ if (lastNode->getPathScore() == HUGE_VAL) {
+ // no valid route found
+ 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->getPreviousNode() != 0) {
+ nodes.push_back(bt->getIndex());
+ 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->getPathScore(), 0);
}
- totalDistance -= distance;
- return;
}
-void FGGroundNetwork::printRoutingError(string mess)
+int FGTaxiSegment::getPenalty(int nGates)
{
- 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));
+ int penalty = 0;
+ if (end->getIndex() < nGates) {
+ penalty += 10000;
}
- 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));
+ if (end->getIsOnRunway()) { // For now. In future versions, need to find out whether runway is active.
+ penalty += 1000;
}
- //exit(1);
+ 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)
+void FGGroundNetwork::announcePosition(int id,
+ FGAIFlightPlan * intendedRoute,
+ int currentPosition, double lat,
+ double lon, double heading,
+ double speed, double alt,
+ double radius, int leg,
+ FGAIAircraft * aircraft)
{
- TrafficVectorIterator i = activeTraffic.begin();
- // Search search if the current id alread has an entry
- // This might be faster using a map instead of a vector, but let's start by taking a safe route
- if (activeTraffic.size()) {
- while ((i->getId() != id) && i != activeTraffic.end()) {
- i++;
- }
- }
- // Add a new TrafficRecord if no one exsists for this aircraft.
- if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
- FGTrafficRecord rec;
- rec.setId(id);
- rec.setPositionAndIntentions(currentPosition, intendedRoute);
- rec.setPositionAndHeading(lat, lon, heading, speed, alt);
- rec.setRadius(radius); // only need to do this when creating the record.
- activeTraffic.push_back(rec);
- } else {
- i->setPositionAndIntentions(currentPosition, intendedRoute);
- i->setPositionAndHeading(lat, lon, heading, speed, alt);
- }
+ TrafficVectorIterator i = activeTraffic.begin();
+ // Search search if the current id alread has an entry
+ // This might be faster using a map instead of a vector, but let's start by taking a safe route
+ if (activeTraffic.size()) {
+ //while ((i->getId() != id) && i != activeTraffic.end()) {
+ while (i != activeTraffic.end()) {
+ if (i->getId() == id) {
+ break;
+ }
+ i++;
+ }
+ }
+ // Add a new TrafficRecord if no one exsists for this aircraft.
+ if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
+ FGTrafficRecord rec;
+ rec.setId(id);
+ rec.setLeg(leg);
+ rec.setPositionAndIntentions(currentPosition, intendedRoute);
+ rec.setPositionAndHeading(lat, lon, heading, speed, alt);
+ rec.setRadius(radius); // only need to do this when creating the record.
+ rec.setAircraft(aircraft);
+ activeTraffic.push_back(rec);
+ } else {
+ i->setPositionAndIntentions(currentPosition, intendedRoute);
+ i->setPositionAndHeading(lat, lon, heading, speed, alt);
+ }
}
-void FGGroundNetwork::signOff(int id) {
- TrafficVectorIterator i = activeTraffic.begin();
- // Search search if the current id alread has an entry
- // This might be faster using a map instead of a vector, but let's start by taking a safe route
- if (activeTraffic.size()) {
- while ((i->getId() != id) && i != activeTraffic.end()) {
- i++;
- }
- }
- if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
- SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Aircraft without traffic record is signing off");
- } else {
- i = activeTraffic.erase(i);
- }
+void FGGroundNetwork::signOff(int id)
+{
+ TrafficVectorIterator i = activeTraffic.begin();
+ // Search search if the current id alread has an entry
+ // This might be faster using a map instead of a vector, but let's start by taking a safe route
+ if (activeTraffic.size()) {
+ //while ((i->getId() != id) && i != activeTraffic.end()) {
+ while (i != activeTraffic.end()) {
+ if (i->getId() == id) {
+ break;
+ }
+ i++;
+ }
+ }
+ if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
+ SG_LOG(SG_GENERAL, SG_ALERT,
+ "AI error: Aircraft without traffic record is signing off");
+ } else {
+ i = activeTraffic.erase(i);
+ }
}
-void FGGroundNetwork::update(int id, double lat, double lon, double heading, double speed, double alt) {
- 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
- TrafficVectorIterator current, closest;
- if (activeTraffic.size()) {
- while ((i->getId() != id) && i != activeTraffic.end()) {
- i++;
- }
- }
- // update position of the current aircraft
- if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
- SG_LOG(SG_GENERAL, SG_ALERT, "AI error: updating aircraft without traffic record");
- } else {
- i->setPositionAndHeading(lat, lon, heading, speed, alt);
- current = i;
- }
-
- // Scan for a speed adjustment change. Find the nearest aircraft that is in front
- // and adjust speed when we get too close. Only do this when current position and/or
- // intentions of the current aircraft match current taxiroute position of the proximate
- // aircraft.
- double mindist = HUGE;
- if (activeTraffic.size())
- {
- double course, dist, bearing, minbearing;
-
- //TrafficVector iterator closest;
- 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 (bearing > 180)
- bearing = 360-bearing;
- if ((dist < mindist) && (bearing < 60.0))
- {
- mindist = dist;
- closest = i;
- minbearing = bearing;
- }
- }
- }
- //cerr << "Distance : " << dist << " bearing : " << bearing << " heading : " << heading
- // << " course : " << course << endl;
- current->clearSpeedAdjustment();
- // Only clear the heading adjustment at positive speeds, otherwise the waypoint following
- // code wreaks havoc
- if (speed > 0.2)
- current->clearHeadingAdjustment();
- // All clear
- if (mindist > 100)
- {
- //current->clearSpeedAdjustment();
- //current->clearHeadingAdjustment();
- }
- else
- {
- if (current->getId() == closest->getWaitsForId())
- return;
- else
- current->setWaitsForId(closest->getId());
-
- // Getting close: Slow down to a bit less than the other aircraft
- double maxAllowableDistance = (1.1*current->getRadius()) + (1.1*closest->getRadius());
- if (mindist > maxAllowableDistance)
- {
- if (current->checkPositionAndIntentions(*closest))
- {
- // Adjust speed, but don't let it drop to below 1 knots
- //if (fabs(speed) > 1)
- if (!(current->hasHeadingAdjustment()))
- {
- current->setSpeedAdjustment(closest->getSpeed()* (mindist/100));
- //cerr << "Adjusting speed to " << closest->getSpeed() * (mindist / 100) << " "
- // << "Bearing = " << minbearing << " Distance = " << mindist
- // << " Latitude = " <<lat << " longitude = " << lon << endl;
- //<< " Latitude = " <<closest->getLatitude()
- //<< " longitude = " << closest->getLongitude()
- // << endl;
- }
- else
- {
- double newSpeed = (maxAllowableDistance-mindist);
- current->setSpeedAdjustment(newSpeed);
- }
- }
- }
- else
- {
- if (!(current->hasHeadingAdjustment()))
- {
- double newSpeed;
- if (mindist > 10) {
- newSpeed = 0.01;
- current->setSpeedAdjustment(newSpeed);
- } else {
- newSpeed = -1 * (maxAllowableDistance-mindist);
- current->setSpeedAdjustment(newSpeed);
- current->setHeadingAdjustment(heading);
- // if (mindist < 5) {
- // double bank_sense = 0;
- // current->setSpeedAdjustment(-0.1);
- // // Do a heading adjustment
- // double diff = fabs(heading - bearing);
- // if (diff > 180)
- // diff = fabs(diff - 360);
-
- // double sum = heading + diff;
- // if (sum > 360.0)
- // sum -= 360.0;
- // if (fabs(sum - bearing) < 1.0) {
- // bank_sense = -1.0; // turn left for evasive action
- // } else {
- // bank_sense = 1.0; // turn right for evasive action
- // }
- // double newHeading = heading + bank_sense;
- // if (newHeading < 0) newHeading += 360;
- // if (newHeading > 360) newHeading -= 360;
- // current->setHeadingAdjustment(newHeading);
- // //cerr << "Yikes: TOOOO close. backing up and turning to heading " << newHeading
- // // << endl;
- // cerr << "Troubleshooting: " << current->getId() << " Closest : " << closest->getId()
- // << endl;
- // }
- }
- }
- }
- }
+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
+ TrafficVectorIterator current, closest;
+ if (activeTraffic.size()) {
+ //while ((i->getId() != id) && i != activeTraffic.end()) {
+ while (i != activeTraffic.end()) {
+ if (i->getId() == id) {
+ break;
+ }
+ i++;
+ }
+ }
+ // update position of the current aircraft
+ if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
+ SG_LOG(SG_GENERAL, SG_ALERT,
+ "AI error: updating aircraft without traffic record");
+ } else {
+ i->setPositionAndHeading(lat, lon, heading, speed, alt);
+ current = i;
+ }
+
+ setDt(getDt() + dt);
+
+ // Update every three secs, but add some randomness
+ // to prevent all IA objects doing this in synchrony
+ //if (getDt() < (3.0) + (rand() % 10))
+ // return;
+ //else
+ // setDt(0);
+ current->clearResolveCircularWait();
+ current->setWaitsForId(0);
+ checkSpeedAdjustment(id, lat, lon, heading, speed, alt);
+ checkHoldPosition(id, lat, lon, heading, speed, alt);
+ if (checkForCircularWaits(id)) {
+ i->setResolveCircularWait();
+ }
+ bool needsTaxiClearance = current->getAircraft()->getTaxiClearanceRequest();
+ int state = current->getState();
+ time_t now = time(NULL) + fgGetLong("/sim/time/warp");
+ if ((now - lastTransmission) > 15) {
+ available = true;
+ }
+ if (needsTaxiClearance && available) {
+ transmit(&(*current), MSG_REQUEST_TAXI_CLEARANCE, ATC_AIR_TO_GROUND);
+ current->getAircraft()->setTaxiClearanceRequest(false);
+ current->setState(3);
+ lastTransmission = now;
+ available = false;
+ }
+ if ((state == 3) && available) {
+ transmit(&(*current), MSG_ISSUE_TAXI_CLEARANCE, ATC_GROUND_TO_AIR);
+ current->setState(4);
+ lastTransmission = now;
+ available = false;
+ }
+ if ((state == 4) && available) {
+ transmit(&(*current), MSG_ACKNOWLEDGE_TAXI_CLEARANCE, ATC_AIR_TO_GROUND);
+ current->setState(0);
+ lastTransmission = now;
+ available = false;
}
}
-bool FGGroundNetwork::hasInstruction(int id)
+/**
+ Scan for a speed adjustment change. Find the nearest aircraft that is in front
+ and adjust speed when we get too close. Only do this when current position and/or
+ intentions of the current aircraft match current taxiroute position of the proximate
+ aircraft. For traffic that is on other routes we need to issue a "HOLD Position"
+ instruction. See below for the hold position instruction.
+
+ Note that there currently still is one flaw in the logic that needs to be addressed.
+ 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.
+*/
+
+void FGGroundNetwork::checkSpeedAdjustment(int id, double lat,
+ double lon, double heading,
+ double speed, double alt)
{
- 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
- if (activeTraffic.size()) {
- while ((i->getId() != id) && i != activeTraffic.end()) {
- i++;
- }
- }
- if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
- SG_LOG(SG_GENERAL, SG_ALERT, "AI error: checking ATC instruction for aircraft without traffic record");
- } else {
- return i->hasInstruction();
- }
- return false;
+
+ TrafficVectorIterator current, closest;
+ TrafficVectorIterator i = activeTraffic.begin();
+ bool otherReasonToSlowDown = false;
+ bool previousInstruction;
+ if (activeTraffic.size()) {
+ //while ((i->getId() != id) && (i != activeTraffic.end()))
+ while (i != activeTraffic.end()) {
+ if (i->getId() == id) {
+ break;
+ }
+ i++;
+ }
+ } else {
+ return;
+ }
+ if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
+ SG_LOG(SG_GENERAL, SG_ALERT,
+ "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkSpeedAdjustment");
+ }
+ current = i;
+ //closest = current;
+
+ previousInstruction = current->getSpeedAdjustment();
+ double mindist = HUGE_VAL;
+ if (activeTraffic.size()) {
+ 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) {
+ 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)) {
+ mindist = dist;
+ closest = i;
+ minbearing = bearing;
+ }
+ }
+ //Check traffic at the tower controller
+ if (towerController->hasActiveTraffic()) {
+ for (TrafficVectorIterator i =
+ towerController->getActiveTraffic().begin();
+ i != towerController->getActiveTraffic().end(); i++) {
+ //cerr << "Comparing " << current->getId() << " and " << i->getId() << endl;
+ 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)) {
+ mindist = dist;
+ closest = i;
+ minbearing = bearing;
+ otherReasonToSlowDown = true;
+ }
+ }
+ }
+ // Finally, check UserPosition
+ double userLatitude = fgGetDouble("/position/latitude-deg");
+ double userLongitude = fgGetDouble("/position/longitude-deg");
+ SGGeod user(SGGeod::fromDeg(userLongitude, userLatitude));
+ SGGeodesy::inverse(curr, user, course, az2, dist);
+
+ bearing = fabs(heading - course);
+ if (bearing > 180)
+ bearing = 360 - bearing;
+ if ((dist < mindist) && (bearing < 60.0)) {
+ mindist = dist;
+ //closest = i;
+ minbearing = bearing;
+ otherReasonToSlowDown = true;
+ }
+
+ current->clearSpeedAdjustment();
+
+ if (current->checkPositionAndIntentions(*closest)
+ || otherReasonToSlowDown) {
+ double maxAllowableDistance =
+ (1.1 * current->getRadius()) +
+ (1.1 * closest->getRadius());
+ if (mindist < 2 * maxAllowableDistance) {
+ if (current->getId() == closest->getWaitsForId())
+ return;
+ else
+ current->setWaitsForId(closest->getId());
+ 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
+ if (mindist < maxAllowableDistance) {
+ //double newSpeed = (maxAllowableDistance-mindist);
+ //current->setSpeedAdjustment(newSpeed);
+ //if (mindist < 0.5* maxAllowableDistance)
+ // {
+ current->setSpeedAdjustment(0);
+ // }
+ }
+ }
+ }
+ }
}
-FGATCInstruction FGGroundNetwork::getInstruction(int id)
+/**
+ Check for "Hold position instruction".
+ The hold position should be issued under the following conditions:
+ 1) For aircraft entering or crossing a runway with active traffic on it, or landing aircraft near it
+ 2) For taxiing aircraft that use one taxiway in opposite directions
+ 3) For crossing or merging taxiroutes.
+*/
+
+void FGGroundNetwork::checkHoldPosition(int id, double lat,
+ double lon, double heading,
+ double speed, double alt)
{
- 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
- if (activeTraffic.size()) {
- while ((i->getId() != id) && i != activeTraffic.end()) {
- i++;
- }
- }
- if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
- SG_LOG(SG_GENERAL, SG_ALERT, "AI error: requesting ATC instruction for aircraft without traffic record");
- } else {
- return i->getInstruction();
- }
- return FGATCInstruction();
-}
+ TrafficVectorIterator current;
+ TrafficVectorIterator i = activeTraffic.begin();
+ if (activeTraffic.size()) {
+ //while ((i->getId() != id) && i != activeTraffic.end())
+ while (i != activeTraffic.end()) {
+ if (i->getId() == id) {
+ break;
+ }
+ i++;
+ }
+ } else {
+ return;
+ }
+ if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
+ SG_LOG(SG_GENERAL, SG_ALERT,
+ "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkHoldPosition");
+ }
+ current = i;
+ bool origStatus = current->hasHoldPosition();
+ current->setHoldPosition(false);
+ SGGeod curr(SGGeod::fromDegM(lon, lat, alt));
+ for (i = activeTraffic.begin(); i != activeTraffic.end(); i++) {
+ 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;
+ SGGeod other(SGGeod::
+ fromDegM(i->getLongitude(), i->getLatitude(),
+ i->getAltitude()));
+ bool needsToWait;
+ bool opposing;
+ if (current->isOpposing(this, *i, node)) {
+ needsToWait = true;
+ opposing = true;
+ //cerr << "Hold check 2 : " << node << " has opposing segment " << endl;
+ // issue a "Hold Position" as soon as we're close to the offending node
+ // For now, I'm doing this as long as the other aircraft doesn't
+ // have a hold instruction as soon as we're within a reasonable
+ // distance from the offending node.
+ // This may be a bit of a conservative estimate though, as it may
+ // be well possible that both aircraft can both continue to taxi
+ // without crashing into each other.
+ } else {
+ opposing = false;
+ 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 "
+ // << endl;
+ } else {
+ needsToWait = true;
+ //cerr << "Hold check 4: " << id << " Would need to wait for other aircraft : distance = " << dist << " meters" << endl;
+ }
+ }
-/***************************************************************************
- * FGATCInstruction
+ double dist =
+ SGGeodesy::distanceM(curr, taxiNode->getGeod());
+ if (!(i->hasHoldPosition())) {
+
+ if ((dist < 200) && //2.5*current->getRadius()) &&
+ (needsToWait) && (i->onRoute(this, *current)) &&
+ //((i->onRoute(this, *current)) || ((!(i->getSpeedAdjustment())))) &&
+ (!(current->getId() == i->getWaitsForId())))
+ //(!(i->getSpeedAdjustment()))) // &&
+ //(!(current->getSpeedAdjustment())))
+
+ {
+ current->setHoldPosition(true);
+ current->setWaitsForId(i->getId());
+ //cerr << "Hold check 5: " << current->getCallSign() <<" Setting Hold Position: distance to node (" << node << ") "
+ // << dist << " meters. Waiting for " << i->getCallSign();
+ //if (opposing)
+ //cerr <<" [opposing] " << endl;
+ //else
+ // cerr << "[non-opposing] " << endl;
+ //if (i->hasSpeefAdjustment())
+ // {
+ // cerr << " (which in turn waits for ) " << i->
+ } else {
+ //cerr << "Hold check 6: " << id << " No need to hold yet: Distance to node : " << dist << " nm"<< endl;
+ }
+ }
+ }
+ }
+ }
+ bool currStatus = current->hasHoldPosition();
+
+ // Either a Hold Position or a resume taxi transmission has been issued
+ time_t now = time(NULL) + fgGetLong("/sim/time/warp");
+ if ((now - lastTransmission) > 2) {
+ available = true;
+ }
+ if ((origStatus != currStatus) && available) {
+ //cerr << "Issueing hold short instrudtion " << currStatus << " " << available << endl;
+ if (currStatus == true) { // No has a hold short instruction
+ transmit(&(*current), MSG_HOLD_POSITION, ATC_GROUND_TO_AIR);
+ //cerr << "Transmittin hold short instrudtion " << currStatus << " " << available << endl;
+ current->setState(1);
+ } else {
+ transmit(&(*current), MSG_RESUME_TAXI, ATC_GROUND_TO_AIR);
+ //cerr << "Transmittig resume instrudtion " << currStatus << " " << available << endl;
+ current->setState(2);
+ }
+ lastTransmission = now;
+ available = false;
+ // Don't act on the changed instruction until the transmission is confirmed
+ // So set back to original status
+ current->setHoldPosition(origStatus);
+ //cerr << "Current state " << current->getState() << endl;
+ } else {
+ }
+ int state = current->getState();
+ if ((state == 1) && (available)) {
+ //cerr << "ACKNOWLEDGE HOLD" << endl;
+ transmit(&(*current), MSG_ACKNOWLEDGE_HOLD_POSITION, ATC_AIR_TO_GROUND);
+ current->setState(0);
+ current->setHoldPosition(true);
+ lastTransmission = now;
+ available = false;
+
+ }
+ if ((state == 2) && (available)) {
+ //cerr << "ACKNOWLEDGE RESUME" << endl;
+ transmit(&(*current), MSG_ACKNOWLEDGE_RESUME_TAXI, ATC_AIR_TO_GROUND);
+ current->setState(0);
+ current->setHoldPosition(false);
+ lastTransmission = now;
+ available = false;
+ }
+}
+
+/**
+ * Check whether situations occur where the current aircraft is waiting for itself
+ * due to higher order interactions.
+ * A 'circular' wait is a situation where a waits for b, b waits for c, and c waits
+ * for a. Ideally each aircraft only waits for one other aircraft, so by tracing
+ * through this list of waiting aircraft, we can check if we'd eventually end back
+ * at the current aircraft.
*
- * This class is really out of place here, and should be combined with
- * FGATC controller and go into it's own file / directory
- * I'm leaving it for now though, because I'm testing this stuff quite
- * heavily.
- **************************************************************************/
-FGATCInstruction::FGATCInstruction()
+ * Note that we should consider the situation where we are actually checking aircraft
+ * d, which is waiting for aircraft a. d is not part of the loop, but is held back by
+ * the looping aircraft. If we don't check for that, this function will get stuck into
+ * endless loop.
+ */
+
+bool FGGroundNetwork::checkForCircularWaits(int id)
{
- holdPattern = false;
- holdPosition = false;
- changeSpeed = false;
- changeHeading = false;
- changeAltitude = false;
-
- double speed = 0;
- double heading = 0;
- double alt = 0;
+ //cerr << "Performing Wait check " << id << endl;
+ int target = 0;
+ TrafficVectorIterator current, other;
+ TrafficVectorIterator i = activeTraffic.begin();
+ int trafficSize = activeTraffic.size();
+ if (trafficSize) {
+ while (i != activeTraffic.end()) {
+ if (i->getId() == id) {
+ break;
+ }
+ i++;
+ }
+ } 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");
+ }
+
+ current = i;
+ 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();
+ if (trafficSize) {
+ //while ((i->getId() != id) && i != activeTraffic.end())
+ while (i != activeTraffic.end()) {
+ if (i->getId() == target) {
+ break;
+ }
+ i++;
+ }
+ } else {
+ return false;
+ }
+ if (i == activeTraffic.end() || (trafficSize == 0)) {
+ //cerr << "[Waiting for traffic at Runway: DONE] " << endl << endl;;
+ // The target id is not found on the current network, which means it's at the tower
+ //SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits");
+ return false;
+ }
+ other = i;
+ target = other->getWaitsForId();
+
+ // actually this trap isn't as impossible as it first seemed:
+ // the setWaitsForID(id) is set to current when the aircraft
+ // is waiting for the user controlled aircraft.
+ //if (current->getId() == other->getId()) {
+ // cerr << "Caught the impossible trap" << endl;
+ // cerr << "Current = " << current->getId() << endl;
+ // cerr << "Other = " << other ->getId() << endl;
+ // for (TrafficVectorIterator at = activeTraffic.begin();
+ // at != activeTraffic.end();
+ // at++) {
+ // cerr << "currently active aircraft : " << at->getCallSign() << " with Id " << at->getId() << " waits for " << at->getWaitsForId() << endl;
+ // }
+ // exit(1);
+ if (current->getId() == other->getId())
+ return false;
+ //}
+ //cerr << current->getCallSign() << " (" << current->getId() << ") " << " -> " << other->getCallSign()
+ // << " (" << other->getId() << "); " << endl;;
+ //current = other;
+ }
+
+
+
+
+
+
+ //if (printed)
+ // cerr << "[done] " << endl << endl;;
+ if (id == target) {
+ SG_LOG(SG_GENERAL, SG_WARN,
+ "Detected circular wait condition: Id = " << id <<
+ "target = " << target);
+ return true;
+ } else {
+ return false;
+ }
}
-bool FGATCInstruction::hasInstruction()
+// Note that this function is probably obsolete...
+bool FGGroundNetwork::hasInstruction(int id)
{
- return (holdPattern || holdPosition || changeSpeed || changeHeading || changeAltitude);
+ 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
+ if (activeTraffic.size()) {
+ //while ((i->getId() != id) && i != activeTraffic.end()) {
+ while (i != activeTraffic.end()) {
+ if (i->getId() == id) {
+ break;
+ }
+ i++;
+ }
+ }
+ if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
+ SG_LOG(SG_GENERAL, SG_ALERT,
+ "AI error: checking ATC instruction for aircraft without traffic record");
+ } else {
+ return i->hasInstruction();
+ }
+ return false;
}
-
+FGATCInstruction FGGroundNetwork::getInstruction(int id)
+{
+ 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
+ if (activeTraffic.size()) {
+ //while ((i->getId() != id) && i != activeTraffic.end()) {
+ while (i != activeTraffic.end()) {
+ if (i->getId() == id) {
+ break;
+ }
+ i++;
+ }
+ }
+ if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
+ SG_LOG(SG_GENERAL, SG_ALERT,
+ "AI error: requesting ATC instruction for aircraft without traffic record");
+ } else {
+ return i->getInstruction();
+ }
+ return FGATCInstruction();
+}
projects / flightgear.git / blobdiff