1 // groundnet.cxx - Implimentation of the FlightGear airport ground handling code
3 // Written by Durk Talsma, started June 2005.
5 // Copyright (C) 2004 Durk Talsma.
7 // This program is free software; you can redistribute it and/or
8 // modify it under the terms of the GNU General Public License as
9 // published by the Free Software Foundation; either version 2 of the
10 // License, or (at your option) any later version.
12 // This program is distributed in the hope that it will be useful, but
13 // WITHOUT ANY WARRANTY; without even the implied warranty of
14 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
15 // General Public License for more details.
17 // You should have received a copy of the GNU General Public License
18 // along with this program; if not, write to the Free Software
19 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
30 //#include <plib/sg.h>
31 //#include <plib/ul.h>
33 //#include <Environment/environment_mgr.hxx>
34 //#include <Environment/environment.hxx>
35 //#include <simgear/misc/sg_path.hxx>
36 //#include <simgear/props/props.hxx>
37 //#include <simgear/structure/subsystem_mgr.hxx>
38 #include <simgear/debug/logstream.hxx>
39 #include <simgear/route/waypoint.hxx>
40 //#include <Main/globals.hxx>
41 //#include <Main/fg_props.hxx>
42 //#include <Airports/runways.hxx>
44 #include <AIModel/AIFlightPlan.hxx>
48 #include "groundnetwork.hxx"
50 /***************************************************************************
52 **************************************************************************/
53 FGTaxiSegment::FGTaxiSegment()
55 oppositeDirection = 0;
59 void FGTaxiSegment::setStart(FGTaxiNodeVector *nodes)
61 FGTaxiNodeVectorIterator i = nodes->begin();
62 while (i != nodes->end())
64 if ((*i)->getIndex() == startNode)
66 start = (*i)->getAddress();
67 (*i)->addSegment(this);
74 void FGTaxiSegment::setEnd(FGTaxiNodeVector *nodes)
76 FGTaxiNodeVectorIterator i = nodes->begin();
77 while (i != nodes->end())
79 if ((*i)->getIndex() == endNode)
81 end = (*i)->getAddress();
90 // There is probably a computationally cheaper way of
92 void FGTaxiSegment::setTrackDistance()
95 SGWayPoint first (start->getLongitude(),
98 SGWayPoint second (end->getLongitude(),
101 first.CourseAndDistance(second, &course, &length);
105 void FGTaxiSegment::setCourseDiff(double crse)
107 headingDiff = fabs(course-crse);
109 if (headingDiff > 180)
110 headingDiff = fabs(headingDiff - 360);
114 /***************************************************************************
116 **************************************************************************/
117 bool FGTaxiRoute::next(int *nde)
119 //for (intVecIterator i = nodes.begin(); i != nodes.end(); i++)
120 // cerr << "FGTaxiRoute contains : " << *(i) << endl;
121 //cerr << "Offset from end: " << nodes.end() - currNode << endl;
122 //if (currNode != nodes.end())
123 // cerr << "true" << endl;
125 // cerr << "false" << endl;
126 //if (nodes.size() != (routes.size()) +1)
127 // cerr << "ALERT: Misconfigured TaxiRoute : " << nodes.size() << " " << routes.size() << endl;
129 if (currNode == nodes.end())
132 if (currNode != nodes.begin()) // make sure route corresponds to the end node
138 bool FGTaxiRoute::next(int *nde, int *rte)
140 //for (intVecIterator i = nodes.begin(); i != nodes.end(); i++)
141 // cerr << "FGTaxiRoute contains : " << *(i) << endl;
142 //cerr << "Offset from end: " << nodes.end() - currNode << endl;
143 //if (currNode != nodes.end())
144 // cerr << "true" << endl;
146 // cerr << "false" << endl;
147 if (nodes.size() != (routes.size()) +1) {
148 SG_LOG(SG_GENERAL, SG_ALERT, "ALERT: Misconfigured TaxiRoute : " << nodes.size() << " " << routes.size());
151 if (currNode == nodes.end())
154 //*rte = *(currRoute);
155 if (currNode != nodes.begin()) // Make sure route corresponds to the end node
162 // If currNode points to the first node, this means the aircraft is not on the taxi node
163 // yet. Make sure to return a unique identifyer in this situation though, because otherwise
164 // the speed adjust AI code may be unable to resolve whether two aircraft are on the same
165 // taxi route or not. the negative of the preceding route seems a logical choice, as it is
166 // unique for any starting location.
167 // Note that this is probably just a temporary fix until I get Parking / tower control working.
168 *rte = -1 * *(currRoute);
174 void FGTaxiRoute::rewind(int route)
180 if (!(next(&currPoint, &currRoute))) {
181 SG_LOG(SG_GENERAL,SG_ALERT, "Error in rewinding TaxiRoute: current" << currRoute
182 << " goal " << route);
184 } while (currRoute != route);
190 /***************************************************************************
192 **************************************************************************/
193 bool compare_nodes(FGTaxiNode *a, FGTaxiNode *b) {
197 bool compare_segments(FGTaxiSegment *a, FGTaxiSegment *b) {
201 FGGroundNetwork::FGGroundNetwork()
209 currTraffic = activeTraffic.begin();
213 FGGroundNetwork::~FGGroundNetwork()
215 for (FGTaxiNodeVectorIterator node = nodes.begin();
222 for (FGTaxiSegmentVectorIterator seg = segments.begin();
223 seg != segments.end();
231 void FGGroundNetwork::addSegment(const FGTaxiSegment &seg)
233 segments.push_back(new FGTaxiSegment(seg));
236 void FGGroundNetwork::addNode(const FGTaxiNode &node)
238 nodes.push_back(new FGTaxiNode(node));
241 void FGGroundNetwork::addNodes(FGParkingVec *parkings)
244 FGParkingVecIterator i = parkings->begin();
245 while (i != parkings->end())
247 n.setIndex(i->getIndex());
248 n.setLatitude(i->getLatitude());
249 n.setLongitude(i->getLongitude());
250 nodes.push_back(new FGTaxiNode(n));
258 void FGGroundNetwork::init()
262 sort(nodes.begin(), nodes.end(), compare_nodes);
263 //sort(segments.begin(), segments.end(), compare_segments());
264 FGTaxiSegmentVectorIterator i = segments.begin();
265 while(i != segments.end()) {
266 //cerr << "initializing node " << i->getIndex() << endl;
267 (*i)->setStart(&nodes);
268 (*i)->setEnd (&nodes);
269 (*i)->setTrackDistance();
270 (*i)->setIndex(index);
271 //cerr << "Track distance = " << i->getLength() << endl;
272 //cerr << "Track ends at" << i->getEnd()->getIndex() << endl;
277 i = segments.begin();
278 while(i != segments.end()) {
279 FGTaxiSegmentVectorIterator j = (*i)->getEnd()->getBeginRoute();
280 while (j != (*i)->getEnd()->getEndRoute())
282 if ((*j)->getEnd()->getIndex() == (*i)->getStart()->getIndex())
284 int start1 = (*i)->getStart()->getIndex();
285 int end1 = (*i)->getEnd() ->getIndex();
286 int start2 = (*j)->getStart()->getIndex();
287 int end2 = (*j)->getEnd()->getIndex();
288 int oppIndex = (*j)->getIndex();
289 //cerr << "Opposite of " << (*i)->getIndex() << " (" << start1 << "," << end1 << ") "
290 // << "happens to be " << oppIndex << " (" << start2 << "," << end2 << ") " << endl;
291 (*i)->setOpposite(*j);
298 //cerr << "Done initializing ground network" << endl;
304 int FGGroundNetwork::findNearestNode(double lat, double lon)
306 double minDist = HUGE_VAL;
309 SGWayPoint first (lon,
313 for (FGTaxiNodeVectorIterator
315 itr != nodes.end(); itr++)
318 SGWayPoint second ((*itr)->getLongitude(),
319 (*itr)->getLatitude(),
321 first.CourseAndDistance(second, &course, &dist);
325 index = (*itr)->getIndex();
326 //cerr << "Minimum distance of " << minDist << " for index " << index << endl;
332 FGTaxiNode *FGGroundNetwork::findNode(int idx)
334 for (FGTaxiNodeVectorIterator
336 itr != nodes.end(); itr++)
338 if (itr->getIndex() == idx)
339 return itr->getAddress();
342 if ((idx >= 0) && (idx < nodes.size()))
343 return nodes[idx]->getAddress();
348 FGTaxiSegment *FGGroundNetwork::findSegment(int idx)
350 for (FGTaxiSegmentVectorIterator
351 itr = segments.begin();
352 itr != segments.end(); itr++)
354 if (itr->getIndex() == idx)
355 return itr->getAddress();
358 if ((idx > 0) && (idx <= segments.size()))
359 return segments[idx-1]->getAddress();
362 //cerr << "Alert: trying to find invalid segment " << idx << endl;
368 FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end)
370 //implements Dijkstra's algorithm to find shortest distance route from start to end
371 //taken from http://en.wikipedia.org/wiki/Dijkstra's_algorithm
373 //double INFINITE = 100000000000.0;
374 // initialize scoring values
375 for (FGTaxiNodeVectorIterator
377 itr != nodes.end(); itr++) {
378 (*itr)->pathscore = HUGE_VAL; //infinity by all practical means
379 (*itr)->previousnode = 0; //
380 (*itr)->previousseg = 0; //
383 FGTaxiNode *firstNode = findNode(start);
384 firstNode->pathscore = 0;
386 FGTaxiNode *lastNode = findNode(end);
388 FGTaxiNodeVector unvisited(nodes); // working copy
390 while (!unvisited.empty()) {
391 FGTaxiNode* best = *(unvisited.begin());
392 for (FGTaxiNodeVectorIterator
393 itr = unvisited.begin();
394 itr != unvisited.end(); itr++) {
395 if ((*itr)->pathscore < best->pathscore)
399 FGTaxiNodeVectorIterator newend = remove(unvisited.begin(), unvisited.end(), best);
400 unvisited.erase(newend, unvisited.end());
402 if (best == lastNode) { // found route or best not connected
405 for (FGTaxiSegmentVectorIterator
406 seg = best->getBeginRoute();
407 seg != best->getEndRoute(); seg++) {
408 FGTaxiNode* tgt = (*seg)->getEnd();
409 double alt = best->pathscore + (*seg)->getLength();
410 if (alt < tgt->pathscore) { // Relax (u,v)
411 tgt->pathscore = alt;
412 tgt->previousnode = best;
413 tgt->previousseg = *seg; //
419 if (lastNode->pathscore == HUGE_VAL) {
420 // no valid route found
421 SG_LOG( SG_GENERAL, SG_ALERT, "Failed to find route from waypoint " << start << " to " << end << " at " <<
423 exit(1); //TODO exit more gracefully, no need to stall the whole sim with broken GN's
425 // assemble route from backtrace information
426 intVec nodes, routes;
427 FGTaxiNode* bt = lastNode;
428 while (bt->previousnode != 0) {
429 nodes.push_back(bt->getIndex());
430 routes.push_back(bt->previousseg->getIndex());
431 bt = bt->previousnode;
433 nodes.push_back(start);
434 reverse(nodes.begin(), nodes.end());
435 reverse(routes.begin(), routes.end());
437 return FGTaxiRoute(nodes, routes, lastNode->pathscore, 0);
442 // void FGGroundNetwork::trace(FGTaxiNode *currNode, int end, int depth, double distance)
444 // // Just check some preconditions of the trace algorithm
445 // if (nodesStack.size() != routesStack.size())
447 // SG_LOG(SG_GENERAL, SG_ALERT, "size of nodesStack and routesStack is not equal. NodesStack :"
448 // << nodesStack.size() << ". RoutesStack : " << routesStack.size());
450 // nodesStack.push_back(currNode->getIndex());
451 // totalDistance += distance;
452 // //cerr << "Starting trace " << currNode->getIndex() << " " << "total distance: " << totalDistance << endl;
453 // // << currNode->getIndex() << endl;
455 // // If the current route matches the required end point we found a valid route
456 // // So we can add this to the routing table
457 // if (currNode->getIndex() == end)
460 // //cerr << "Found route : " << totalDistance << "" << " " << *(nodesStack.end()-1) << " Depth = " << depth << endl;
461 // routes.push_back(FGTaxiRoute(nodesStack,routesStack,totalDistance, depth));
462 // if (nodesStack.empty() || routesStack.empty())
464 // printRoutingError(string("while finishing route"));
466 // nodesStack.pop_back();
467 // routesStack.pop_back();
468 // if (!(foundRoute)) {
469 // maxDistance = totalDistance;
472 // if (totalDistance < maxDistance)
473 // maxDistance = totalDistance;
474 // foundRoute = true;
475 // totalDistance -= distance;
480 // // search if the currentNode has been encountered before
481 // // if so, we should step back one level, because it is
482 // // rather rediculous to proceed further from here.
483 // // if the current node has not been encountered before,
484 // // i should point to nodesStack.end()-1; and we can continue
485 // // if i is not nodesStack.end, the previous node was found,
486 // // and we should return.
487 // // This only works at trace levels of 1 or higher though
489 // intVecIterator i = nodesStack.begin();
490 // while ((*i) != currNode->getIndex()) {
491 // //cerr << "Route so far : " << (*i) << endl;
494 // if (i != nodesStack.end()-1) {
495 // if (nodesStack.empty() || routesStack.empty())
497 // printRoutingError(string("while returning from an already encountered node"));
499 // nodesStack.pop_back();
500 // routesStack.pop_back();
501 // totalDistance -= distance;
504 // if (depth >= maxDepth) {
506 // if (!(count % 100000)) {
507 // maxDepth--; // Gradually decrease maxdepth, to prevent "eternal searches"
508 // //cerr << "Reducing maxdepth to " << maxDepth << endl;
510 // nodesStack.pop_back();
511 // routesStack.pop_back();
512 // totalDistance -= distance;
515 // // If the total distance from start to the current waypoint
516 // // is longer than that of a route we can also stop this trace
517 // // and go back one level.
518 // if ((totalDistance > maxDistance) && foundRoute)
521 // //cerr << "Stopping rediculously long trace: " << totalDistance << endl;
522 // if (nodesStack.empty() || routesStack.empty())
524 // printRoutingError(string("while returning from finding a rediculously long route"));
526 // nodesStack.pop_back();
527 // routesStack.pop_back();
528 // totalDistance -= distance;
533 //cerr << "2" << endl;
534 if (currNode->getBeginRoute() != currNode->getEndRoute())
536 double course, length;
537 //cerr << "3" << endl;
538 // calculate distance and heading "as the crow flies" between starn and end points"
539 SGWayPoint first(currNode->getLongitude(),
540 currNode->getLatitude(),
542 //SGWayPoint second (lastNode->getLongitude(),
543 // lastNode->getLatitude(),
546 first.CourseAndDistance(destination, &course, &length);
547 //for (FGTaxiSegmentVectorIterator
548 // itr = segments.begin();
549 // itr != segments.end(); itr++)
551 // (*itr)->setCourseDiff(course);
553 //FGTaxiNodeVectorIterator nde = nodes.begin();
554 //while (nde != nodes.end()) {
555 //(*nde)->sortEndSegments();
558 for (FGTaxiSegmentVectorIterator
559 i = currNode->getBeginRoute();
560 i != currNode->getEndRoute();
563 (*i)->setCourseDiff(course);
565 currNode->sortEndSegments(foundRoute);
566 for (FGTaxiSegmentVectorIterator
567 i = currNode->getBeginRoute();
568 i != currNode->getEndRoute();
571 //cerr << (*i)->getLength() << endl;
572 //cerr << (*i)->getIndex() << endl;
573 int idx = (*i)->getIndex();
574 routesStack.push_back((*i)->getIndex());
575 trace((*i)->getEnd(), end, depth+1, (*i)->getLength());
577 // // cerr << currNode -> getIndex() << " ";
578 // route.push_back(currNode->getIndex());
585 //SG_LOG( SG_GENERAL, SG_DEBUG, "4" );
587 if (nodesStack.empty())
589 printRoutingError(string("while finishing trace"));
591 nodesStack.pop_back();
592 // Make sure not to dump the level-zero routesStack entry, because that was never created.
595 routesStack.pop_back();
596 //cerr << "leaving trace " << routesStack.size() << endl;
598 totalDistance -= distance;
602 void FGGroundNetwork::printRoutingError(string mess)
604 SG_LOG(SG_GENERAL, SG_ALERT, "Error in ground network trace algorithm " << mess);
605 if (nodesStack.empty())
607 SG_LOG(SG_GENERAL, SG_ALERT, " nodesStack is empty. Dumping routesStack");
608 for (intVecIterator i = routesStack.begin() ; i != routesStack.end(); i++)
609 SG_LOG(SG_GENERAL, SG_ALERT, "Route " << (*i));
611 if (routesStack.empty())
613 SG_LOG(SG_GENERAL, SG_ALERT, " routesStack is empty. Dumping nodesStack");
614 for (intVecIterator i = nodesStack.begin() ; i != nodesStack.end(); i++)
615 SG_LOG(SG_GENERAL, SG_ALERT, "Node " << (*i));
621 void FGGroundNetwork::announcePosition(int id, FGAIFlightPlan *intendedRoute, int currentPosition,
622 double lat, double lon, double heading,
623 double speed, double alt, double radius, int leg,
626 TrafficVectorIterator i = activeTraffic.begin();
627 // Search search if the current id alread has an entry
628 // This might be faster using a map instead of a vector, but let's start by taking a safe route
629 if (activeTraffic.size()) {
630 //while ((i->getId() != id) && i != activeTraffic.end()) {
631 while (i != activeTraffic.end()) {
632 if (i->getId() == id) {
638 // Add a new TrafficRecord if no one exsists for this aircraft.
639 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
642 rec.setPositionAndIntentions(currentPosition, intendedRoute);
643 rec.setPositionAndHeading(lat, lon, heading, speed, alt);
644 rec.setRadius(radius); // only need to do this when creating the record.
645 rec.setCallSign(callsign);
646 activeTraffic.push_back(rec);
648 i->setPositionAndIntentions(currentPosition, intendedRoute);
649 i->setPositionAndHeading(lat, lon, heading, speed, alt);
653 void FGGroundNetwork::signOff(int id) {
654 TrafficVectorIterator i = activeTraffic.begin();
655 // Search search if the current id alread has an entry
656 // This might be faster using a map instead of a vector, but let's start by taking a safe route
657 if (activeTraffic.size()) {
658 //while ((i->getId() != id) && i != activeTraffic.end()) {
659 while (i != activeTraffic.end()) {
660 if (i->getId() == id) {
666 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
667 SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Aircraft without traffic record is signing off");
669 i = activeTraffic.erase(i);
673 void FGGroundNetwork::update(int id, double lat, double lon, double heading, double speed, double alt,
675 TrafficVectorIterator i = activeTraffic.begin();
676 // Search search if the current id has an entry
677 // This might be faster using a map instead of a vector, but let's start by taking a safe route
678 TrafficVectorIterator current, closest;
679 if (activeTraffic.size()) {
680 //while ((i->getId() != id) && i != activeTraffic.end()) {
681 while (i != activeTraffic.end()) {
682 if (i->getId() == id) {
688 // update position of the current aircraft
689 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
690 SG_LOG(SG_GENERAL, SG_ALERT, "AI error: updating aircraft without traffic record");
692 i->setPositionAndHeading(lat, lon, heading, speed, alt);
698 // Update every three secs, but add some randomness
699 // to prevent all IA objects doing this in synchrony
700 //if (getDt() < (3.0) + (rand() % 10))
704 current->clearResolveCircularWait();
705 current->setWaitsForId(0);
706 checkSpeedAdjustment(id, lat, lon, heading, speed, alt);
707 checkHoldPosition (id, lat, lon, heading, speed, alt);
708 if (checkForCircularWaits(id)) {
709 i->setResolveCircularWait();
714 Scan for a speed adjustment change. Find the nearest aircraft that is in front
715 and adjust speed when we get too close. Only do this when current position and/or
716 intentions of the current aircraft match current taxiroute position of the proximate
717 aircraft. For traffic that is on other routes we need to issue a "HOLD Position"
718 instruction. See below for the hold position instruction.
720 Note that there currently still is one flaw in the logic that needs to be addressed.
721 can be situations where one aircraft is in front of the current aircraft, on a separate
722 route, but really close after an intersection coming off the current route. This
723 aircraft is still close enough to block the current aircraft. This situation is currently
724 not addressed yet, but should be.
727 void FGGroundNetwork::checkSpeedAdjustment(int id, double lat,
728 double lon, double heading,
729 double speed, double alt)
732 TrafficVectorIterator current, closest;
733 TrafficVectorIterator i = activeTraffic.begin();
734 bool otherReasonToSlowDown = false;
735 bool previousInstruction;
736 if (activeTraffic.size())
738 //while ((i->getId() != id) && (i != activeTraffic.end()))
739 while (i != activeTraffic.end()) {
740 if (i->getId() == id) {
750 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
751 SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkSpeedAdjustment");
756 previousInstruction = current->getSpeedAdjustment();
757 double mindist = HUGE;
758 if (activeTraffic.size())
760 double course, dist, bearing, minbearing;
761 SGWayPoint curr (lon,
764 //TrafficVector iterator closest;
766 for (TrafficVectorIterator i = activeTraffic.begin();
767 i != activeTraffic.end(); i++)
770 //SGWayPoint curr (lon,
773 SGWayPoint other (i->getLongitude (),
776 other.CourseAndDistance(curr, &course, &dist);
777 bearing = fabs(heading-course);
779 bearing = 360-bearing;
780 if ((dist < mindist) && (bearing < 60.0))
784 minbearing = bearing;
788 //Check traffic at the tower controller
789 if (towerController->hasActiveTraffic())
791 for (TrafficVectorIterator i = towerController->getActiveTraffic().begin();
792 i != towerController->getActiveTraffic().end(); i++)
794 //cerr << "Comparing " << current->getId() << " and " << i->getId() << endl;
795 //SGWayPoint curr (lon,
798 SGWayPoint other (i->getLongitude (),
801 other.CourseAndDistance(curr, &course, &dist);
802 bearing = fabs(heading-course);
804 bearing = 360-bearing;
805 if ((dist < mindist) && (bearing < 60.0))
809 minbearing = bearing;
810 otherReasonToSlowDown = true;
814 // Finally, check UserPosition
815 double userLatitude = fgGetDouble("/position/latitude-deg");
816 double userLongitude = fgGetDouble("/position/longitude-deg");
817 SGWayPoint user (userLongitude,
819 alt); // Alt is not really important here.
820 user.CourseAndDistance(curr, &course, &dist);
821 bearing = fabs(heading-course);
823 bearing = 360-bearing;
824 if ((dist < mindist) && (bearing < 60.0))
828 minbearing = bearing;
829 otherReasonToSlowDown = true;
832 // if (closest == current) {
833 // //SG_LOG(SG_GENERAL, SG_ALERT, "AI error: closest and current match");
836 //cerr << "Distance : " << dist << " bearing : " << bearing << " heading : " << heading
837 // << " course : " << course << endl;
838 current->clearSpeedAdjustment();
840 if (current->checkPositionAndIntentions(*closest) || otherReasonToSlowDown)
842 double maxAllowableDistance = (1.1*current->getRadius()) + (1.1*closest->getRadius());
843 if (mindist < 2*maxAllowableDistance)
845 if (current->getId() == closest->getWaitsForId())
848 current->setWaitsForId(closest->getId());
849 if (closest != current)
850 current->setSpeedAdjustment(closest->getSpeed()* (mindist/100));
852 current->setSpeedAdjustment(0); // This can only happen when the user aircraft is the one closest
853 if (mindist < maxAllowableDistance)
855 //double newSpeed = (maxAllowableDistance-mindist);
856 //current->setSpeedAdjustment(newSpeed);
857 //if (mindist < 0.5* maxAllowableDistance)
859 current->setSpeedAdjustment(0);
868 Check for "Hold position instruction".
869 The hold position should be issued under the following conditions:
870 1) For aircraft entering or crossing a runway with active traffic on it, or landing aircraft near it
871 2) For taxiing aircraft that use one taxiway in opposite directions
872 3) For crossing or merging taxiroutes.
875 void FGGroundNetwork::checkHoldPosition(int id, double lat,
876 double lon, double heading,
877 double speed, double alt)
880 TrafficVectorIterator current;
881 TrafficVectorIterator i = activeTraffic.begin();
882 if (activeTraffic.size())
884 //while ((i->getId() != id) && i != activeTraffic.end())
885 while (i != activeTraffic.end()) {
886 if (i->getId() == id) {
896 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
897 SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkHoldPosition");
900 current->setHoldPosition(false);
901 SGWayPoint curr (lon,
904 double course, dist, bearing, minbearing;
905 for (i = activeTraffic.begin();
906 i != activeTraffic.end(); i++)
910 int node = current->crosses(this, *i);
913 // Determine whether it's save to continue or not.
914 // If we have a crossing route, there are two possibilities:
915 // 1) This is an interestion
916 // 2) This is oncoming two-way traffic, using the same taxiway.
917 //cerr << "Hold check 1 : " << id << " has common node " << node << endl;
918 SGWayPoint nodePos(findNode(node)->getLongitude (),
919 findNode(node)->getLatitude (),
922 SGWayPoint other (i->getLongitude (),
925 //other.CourseAndDistance(curr, &course, &dist);
928 if (current->isOpposing(this, *i, node))
932 //cerr << "Hold check 2 : " << node << " has opposing segment " << endl;
933 // issue a "Hold Position" as soon as we're close to the offending node
934 // For now, I'm doing this as long as the other aircraft doesn't
935 // have a hold instruction as soon as we're within a reasonable
936 // distance from the offending node.
937 // This may be a bit of a conservative estimate though, as it may
938 // be well possible that both aircraft can both continue to taxi
939 // without crashing into each other.
944 other.CourseAndDistance(nodePos, &course, &dist);
945 if (dist > 200) // 2.0*i->getRadius())
948 //cerr << "Hold check 3 : " << id <<" Other aircraft approaching node is still far away. (" << dist << " nm). Can safely continue "
954 //cerr << "Hold check 4: " << id << " Would need to wait for other aircraft : distance = " << dist << " meters" << endl;
957 curr.CourseAndDistance(nodePos, &course, &dist);
958 if (!(i->hasHoldPosition()))
961 if ((dist < 200) && //2.5*current->getRadius()) &&
963 (i->onRoute(this, *current)) &&
964 //((i->onRoute(this, *current)) || ((!(i->getSpeedAdjustment())))) &&
965 (!(current->getId() == i->getWaitsForId())))
966 //(!(i->getSpeedAdjustment()))) // &&
967 //(!(current->getSpeedAdjustment())))
970 current->setHoldPosition(true);
971 current->setWaitsForId(i->getId());
972 //cerr << "Hold check 5: " << current->getCallSign() <<" Setting Hold Position: distance to node (" << node << ") "
973 // << dist << " meters. Waiting for " << i->getCallSign();
975 //cerr <<" [opposing] " << endl;
977 // cerr << "[non-opposing] " << endl;
978 //if (i->hasSpeefAdjustment())
980 // cerr << " (which in turn waits for ) " << i->
984 //cerr << "Hold check 6: " << id << " No need to hold yet: Distance to node : " << dist << " nm"<< endl;
993 * Check whether situations occur where the current aircraft is waiting for itself
994 * due to higher order interactions.
995 * A 'circular' wait is a situation where a waits for b, b waits for c, and c waits
996 * for a. Ideally each aircraft only waits for one other aircraft, so by tracing
997 * through this list of waiting aircraft, we can check if we'd eventually end back
998 * at the current aircraft.
1000 * Note that we should consider the situation where we are actually checking aircraft
1001 * d, which is waiting for aircraft a. d is not part of the loop, but is held back by
1002 * the looping aircraft. If we don't check for that, this function will get stuck into
1006 bool FGGroundNetwork::checkForCircularWaits(int id)
1008 //cerr << "Performing Wait check " << id << endl;
1010 TrafficVectorIterator current, other;
1011 TrafficVectorIterator i = activeTraffic.begin();
1012 int trafficSize = activeTraffic.size();
1014 while (i != activeTraffic.end()) {
1015 if (i->getId() == id) {
1024 if (i == activeTraffic.end() || (trafficSize == 0)) {
1025 SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits");
1029 target = current->getWaitsForId();
1030 //bool printed = false; // Note that this variable is for debugging purposes only.
1034 //cerr << "aircraft waits for user" << endl;
1039 while ((target > 0) && (target != id) && counter++ < trafficSize) {
1041 TrafficVectorIterator i = activeTraffic.begin();
1043 //while ((i->getId() != id) && i != activeTraffic.end())
1044 while (i != activeTraffic.end()) {
1045 if (i->getId() == target) {
1054 if (i == activeTraffic.end() || (trafficSize == 0)) {
1055 //cerr << "[Waiting for traffic at Runway: DONE] " << endl << endl;;
1056 // The target id is not found on the current network, which means it's at the tower
1057 //SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits");
1061 target = other->getWaitsForId();
1063 // actually this trap isn't as impossible as it first seemed:
1064 // the setWaitsForID(id) is set to current when the aircraft
1065 // is waiting for the user controlled aircraft.
1066 //if (current->getId() == other->getId()) {
1067 // cerr << "Caught the impossible trap" << endl;
1068 // cerr << "Current = " << current->getId() << endl;
1069 // cerr << "Other = " << other ->getId() << endl;
1070 // for (TrafficVectorIterator at = activeTraffic.begin();
1071 // at != activeTraffic.end();
1073 // cerr << "currently active aircraft : " << at->getCallSign() << " with Id " << at->getId() << " waits for " << at->getWaitsForId() << endl;
1076 if (current->getId() == other->getId())
1079 //cerr << current->getCallSign() << " (" << current->getId() << ") " << " -> " << other->getCallSign()
1080 // << " (" << other->getId() << "); " << endl;;
1090 // cerr << "[done] " << endl << endl;;
1092 SG_LOG(SG_GENERAL, SG_ALERT, "Detected circular wait condition");
1093 cerr << "Id = " << id << endl;
1094 cerr << "target = " << target << endl;
1101 // Note that this function is probably obsolete...
1102 bool FGGroundNetwork::hasInstruction(int id)
1104 TrafficVectorIterator i = activeTraffic.begin();
1105 // Search search if the current id has an entry
1106 // This might be faster using a map instead of a vector, but let's start by taking a safe route
1107 if (activeTraffic.size())
1109 //while ((i->getId() != id) && i != activeTraffic.end()) {
1110 while (i != activeTraffic.end()) {
1111 if (i->getId() == id) {
1117 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
1118 SG_LOG(SG_GENERAL, SG_ALERT, "AI error: checking ATC instruction for aircraft without traffic record");
1120 return i->hasInstruction();
1125 FGATCInstruction FGGroundNetwork::getInstruction(int id)
1127 TrafficVectorIterator i = activeTraffic.begin();
1128 // Search search if the current id has an entry
1129 // This might be faster using a map instead of a vector, but let's start by taking a safe route
1130 if (activeTraffic.size()) {
1131 //while ((i->getId() != id) && i != activeTraffic.end()) {
1132 while (i != activeTraffic.end()) {
1133 if (i->getId() == id) {
1139 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
1140 SG_LOG(SG_GENERAL, SG_ALERT, "AI error: requesting ATC instruction for aircraft without traffic record");
1142 return i->getInstruction();
1144 return FGATCInstruction();