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 **************************************************************************/
54 void FGTaxiSegment::setStart(FGTaxiNodeVector *nodes)
56 FGTaxiNodeVectorIterator i = nodes->begin();
57 while (i != nodes->end())
59 //cerr << "Scanning start node index" << (*i)->getIndex() << endl;
60 if ((*i)->getIndex() == startNode)
62 start = (*i)->getAddress();
63 (*i)->addSegment(this);
68 SG_LOG(SG_GENERAL, SG_ALERT, "Could not find start node " << startNode << endl);
71 void FGTaxiSegment::setEnd(FGTaxiNodeVector *nodes)
73 FGTaxiNodeVectorIterator i = nodes->begin();
74 while (i != nodes->end())
76 //cerr << "Scanning end node index" << (*i)->getIndex() << endl;
77 if ((*i)->getIndex() == endNode)
79 end = (*i)->getAddress();
84 SG_LOG(SG_GENERAL, SG_ALERT, "Could not find end node " << endNode << endl);
89 // There is probably a computationally cheaper way of
91 void FGTaxiSegment::setTrackDistance()
94 SGWayPoint first (start->getLongitude(),
97 SGWayPoint second (end->getLongitude(),
100 first.CourseAndDistance(second, &course, &length);
104 void FGTaxiSegment::setCourseDiff(double crse)
106 headingDiff = fabs(course-crse);
108 if (headingDiff > 180)
109 headingDiff = fabs(headingDiff - 360);
113 /***************************************************************************
115 **************************************************************************/
116 bool FGTaxiRoute::next(int *nde)
118 //for (intVecIterator i = nodes.begin(); i != nodes.end(); i++)
119 // cerr << "FGTaxiRoute contains : " << *(i) << endl;
120 //cerr << "Offset from end: " << nodes.end() - currNode << endl;
121 //if (currNode != nodes.end())
122 // cerr << "true" << endl;
124 // cerr << "false" << endl;
125 //if (nodes.size() != (routes.size()) +1)
126 // cerr << "ALERT: Misconfigured TaxiRoute : " << nodes.size() << " " << routes.size() << endl;
128 if (currNode == nodes.end())
131 if (currNode != nodes.begin()) // make sure route corresponds to the end node
137 bool FGTaxiRoute::next(int *nde, int *rte)
139 //for (intVecIterator i = nodes.begin(); i != nodes.end(); i++)
140 // cerr << "FGTaxiRoute contains : " << *(i) << endl;
141 //cerr << "Offset from end: " << nodes.end() - currNode << endl;
142 //if (currNode != nodes.end())
143 // cerr << "true" << endl;
145 // cerr << "false" << endl;
146 if (nodes.size() != (routes.size()) +1) {
147 SG_LOG(SG_GENERAL, SG_ALERT, "ALERT: Misconfigured TaxiRoute : " << nodes.size() << " " << routes.size());
150 if (currNode == nodes.end())
153 //*rte = *(currRoute);
154 if (currNode != nodes.begin()) // Make sure route corresponds to the end node
161 // If currNode points to the first node, this means the aircraft is not on the taxi node
162 // yet. Make sure to return a unique identifyer in this situation though, because otherwise
163 // the speed adjust AI code may be unable to resolve whether two aircraft are on the same
164 // taxi route or not. the negative of the preceding route seems a logical choice, as it is
165 // unique for any starting location.
166 // Note that this is probably just a temporary fix until I get Parking / tower control working.
167 *rte = -1 * *(currRoute);
173 void FGTaxiRoute::rewind(int route)
179 if (!(next(&currPoint, &currRoute))) {
180 SG_LOG(SG_GENERAL,SG_ALERT, "Error in rewinding TaxiRoute: current" << currRoute
181 << " goal " << route);
183 } while (currRoute != route);
189 /***************************************************************************
191 **************************************************************************/
192 bool compare_nodes(FGTaxiNode *a, FGTaxiNode *b) {
196 bool compare_segments(FGTaxiSegment *a, FGTaxiSegment *b) {
200 FGGroundNetwork::FGGroundNetwork()
208 currTraffic = activeTraffic.begin();
212 FGGroundNetwork::~FGGroundNetwork()
214 for (FGTaxiNodeVectorIterator node = nodes.begin();
221 pushBackNodes.clear();
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 (*i)->setStart(&nodes);
267 (*i)->setEnd (&nodes);
268 (*i)->setTrackDistance();
269 (*i)->setIndex(index);
270 if ((*i)->isPushBack()) {
271 pushBackNodes.push_back((*i)->getEnd());
273 //SG_LOG(SG_GENERAL, SG_BULK, "initializing segment " << (*i)->getIndex() << endl);
274 //SG_LOG(SG_GENERAL, SG_BULK, "Track distance = " << (*i)->getLength() << endl);
275 //SG_LOG(SG_GENERAL, SG_BULK, "Track runs from " << (*i)->getStart()->getIndex() << " to "
276 // << (*i)->getEnd()->getIndex() << endl);
281 i = segments.begin();
282 while(i != segments.end()) {
283 FGTaxiSegmentVectorIterator j = (*i)->getEnd()->getBeginRoute();
284 while (j != (*i)->getEnd()->getEndRoute())
286 if ((*j)->getEnd()->getIndex() == (*i)->getStart()->getIndex())
288 int start1 = (*i)->getStart()->getIndex();
289 int end1 = (*i)->getEnd() ->getIndex();
290 int start2 = (*j)->getStart()->getIndex();
291 int end2 = (*j)->getEnd()->getIndex();
292 int oppIndex = (*j)->getIndex();
293 //cerr << "Opposite of " << (*i)->getIndex() << " (" << start1 << "," << end1 << ") "
294 // << "happens to be " << oppIndex << " (" << start2 << "," << end2 << ") " << endl;
295 (*i)->setOpposite(*j);
302 //FGTaxiNodeVectorIterator j = nodes.begin();
303 //while (j != nodes.end()) {
304 // if ((*j)->getHoldPointType() == 3) {
305 // pushBackNodes.push_back((*j));
309 //cerr << "Done initializing ground network" << endl;
315 int FGGroundNetwork::findNearestNode(double lat, double lon)
317 double minDist = HUGE_VAL;
320 SGWayPoint first (lon,
324 for (FGTaxiNodeVectorIterator
326 itr != nodes.end(); itr++)
329 SGWayPoint second ((*itr)->getLongitude(),
330 (*itr)->getLatitude(),
332 first.CourseAndDistance(second, &course, &dist);
336 index = (*itr)->getIndex();
337 //cerr << "Minimum distance of " << minDist << " for index " << index << endl;
343 FGTaxiNode *FGGroundNetwork::findNode(int idx)
345 for (FGTaxiNodeVectorIterator
347 itr != nodes.end(); itr++)
349 if (itr->getIndex() == idx)
350 return itr->getAddress();
353 if ((idx >= 0) && (idx < nodes.size()))
354 return nodes[idx]->getAddress();
359 FGTaxiSegment *FGGroundNetwork::findSegment(int idx)
361 for (FGTaxiSegmentVectorIterator
362 itr = segments.begin();
363 itr != segments.end(); itr++)
365 if (itr->getIndex() == idx)
366 return itr->getAddress();
369 if ((idx > 0) && (idx <= segments.size()))
370 return segments[idx-1]->getAddress();
373 //cerr << "Alert: trying to find invalid segment " << idx << endl;
379 FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end, bool fullSearch)
381 //implements Dijkstra's algorithm to find shortest distance route from start to end
382 //taken from http://en.wikipedia.org/wiki/Dijkstra's_algorithm
384 //double INFINITE = 100000000000.0;
385 // initialize scoring values
386 int nParkings = parent->getDynamics()->getNrOfParkings();
387 FGTaxiNodeVector *currNodesSet;
389 currNodesSet = &nodes;
391 currNodesSet = &pushBackNodes;
394 for (FGTaxiNodeVectorIterator
395 itr = currNodesSet->begin();
396 itr != currNodesSet->end(); itr++) {
397 (*itr)->setPathScore(HUGE_VAL); //infinity by all practical means
398 (*itr)->setPreviousNode(0); //
399 (*itr)->setPreviousSeg (0); //
402 FGTaxiNode *firstNode = findNode(start);
403 firstNode->setPathScore(0);
405 FGTaxiNode *lastNode = findNode(end);
407 FGTaxiNodeVector unvisited(*currNodesSet); // working copy
409 while (!unvisited.empty()) {
410 FGTaxiNode* best = *(unvisited.begin());
411 for (FGTaxiNodeVectorIterator
412 itr = unvisited.begin();
413 itr != unvisited.end(); itr++) {
414 if ((*itr)->getPathScore() < best->getPathScore())
418 FGTaxiNodeVectorIterator newend = remove(unvisited.begin(), unvisited.end(), best);
419 unvisited.erase(newend, unvisited.end());
421 if (best == lastNode) { // found route or best not connected
424 for (FGTaxiSegmentVectorIterator
425 seg = best->getBeginRoute();
426 seg != best->getEndRoute(); seg++) {
427 if (fullSearch || (*seg)->isPushBack()) {
428 FGTaxiNode* tgt = (*seg)->getEnd();
429 double alt = best->getPathScore() + (*seg)->getLength() + (*seg)->getPenalty(nParkings);
430 if (alt < tgt->getPathScore()) { // Relax (u,v)
431 tgt->setPathScore(alt);
432 tgt->setPreviousNode(best);
433 tgt->setPreviousSeg(*seg); //
436 // // cerr << "Skipping TaxiSegment " << (*seg)->getIndex() << endl;
442 if (lastNode->getPathScore() == HUGE_VAL) {
443 // no valid route found
445 SG_LOG( SG_GENERAL, SG_ALERT, "Failed to find route from waypoint " << start << " to " << end << " at " <<
450 //exit(1); //TODO exit more gracefully, no need to stall the whole sim with broken GN's
452 // assemble route from backtrace information
453 intVec nodes, routes;
454 FGTaxiNode* bt = lastNode;
455 while (bt->getPreviousNode() != 0) {
456 nodes.push_back(bt->getIndex());
457 routes.push_back(bt->getPreviousSegment()->getIndex());
458 bt = bt->getPreviousNode();
460 nodes.push_back(start);
461 reverse(nodes.begin(), nodes.end());
462 reverse(routes.begin(), routes.end());
464 return FGTaxiRoute(nodes, routes, lastNode->getPathScore(), 0);
468 int FGTaxiSegment::getPenalty(int nGates) {
470 if (end->getIndex() < nGates) {
473 if (end->getIsOnRunway()) { // For now. In future versions, need to find out whether runway is active.
479 // void FGGroundNetwork::trace(FGTaxiNode *currNode, int end, int depth, double distance)
481 // // Just check some preconditions of the trace algorithm
482 // if (nodesStack.size() != routesStack.size())
484 // SG_LOG(SG_GENERAL, SG_ALERT, "size of nodesStack and routesStack is not equal. NodesStack :"
485 // << nodesStack.size() << ". RoutesStack : " << routesStack.size());
487 // nodesStack.push_back(currNode->getIndex());
488 // totalDistance += distance;
489 // //cerr << "Starting trace " << currNode->getIndex() << " " << "total distance: " << totalDistance << endl;
490 // // << currNode->getIndex() << endl;
492 // // If the current route matches the required end point we found a valid route
493 // // So we can add this to the routing table
494 // if (currNode->getIndex() == end)
497 // //cerr << "Found route : " << totalDistance << "" << " " << *(nodesStack.end()-1) << " Depth = " << depth << endl;
498 // routes.push_back(FGTaxiRoute(nodesStack,routesStack,totalDistance, depth));
499 // if (nodesStack.empty() || routesStack.empty())
501 // printRoutingError(string("while finishing route"));
503 // nodesStack.pop_back();
504 // routesStack.pop_back();
505 // if (!(foundRoute)) {
506 // maxDistance = totalDistance;
509 // if (totalDistance < maxDistance)
510 // maxDistance = totalDistance;
511 // foundRoute = true;
512 // totalDistance -= distance;
517 // // search if the currentNode has been encountered before
518 // // if so, we should step back one level, because it is
519 // // rather rediculous to proceed further from here.
520 // // if the current node has not been encountered before,
521 // // i should point to nodesStack.end()-1; and we can continue
522 // // if i is not nodesStack.end, the previous node was found,
523 // // and we should return.
524 // // This only works at trace levels of 1 or higher though
526 // intVecIterator i = nodesStack.begin();
527 // while ((*i) != currNode->getIndex()) {
528 // //cerr << "Route so far : " << (*i) << endl;
531 // if (i != nodesStack.end()-1) {
532 // if (nodesStack.empty() || routesStack.empty())
534 // printRoutingError(string("while returning from an already encountered node"));
536 // nodesStack.pop_back();
537 // routesStack.pop_back();
538 // totalDistance -= distance;
541 // if (depth >= maxDepth) {
543 // if (!(count % 100000)) {
544 // maxDepth--; // Gradually decrease maxdepth, to prevent "eternal searches"
545 // //cerr << "Reducing maxdepth to " << maxDepth << endl;
547 // nodesStack.pop_back();
548 // routesStack.pop_back();
549 // totalDistance -= distance;
552 // // If the total distance from start to the current waypoint
553 // // is longer than that of a route we can also stop this trace
554 // // and go back one level.
555 // if ((totalDistance > maxDistance) && foundRoute)
558 // //cerr << "Stopping rediculously long trace: " << totalDistance << endl;
559 // if (nodesStack.empty() || routesStack.empty())
561 // printRoutingError(string("while returning from finding a rediculously long route"));
563 // nodesStack.pop_back();
564 // routesStack.pop_back();
565 // totalDistance -= distance;
570 //cerr << "2" << endl;
571 if (currNode->getBeginRoute() != currNode->getEndRoute())
573 double course, length;
574 //cerr << "3" << endl;
575 // calculate distance and heading "as the crow flies" between starn and end points"
576 SGWayPoint first(currNode->getLongitude(),
577 currNode->getLatitude(),
579 //SGWayPoint second (lastNode->getLongitude(),
580 // lastNode->getLatitude(),
583 first.CourseAndDistance(destination, &course, &length);
584 //for (FGTaxiSegmentVectorIterator
585 // itr = segments.begin();
586 // itr != segments.end(); itr++)
588 // (*itr)->setCourseDiff(course);
590 //FGTaxiNodeVectorIterator nde = nodes.begin();
591 //while (nde != nodes.end()) {
592 //(*nde)->sortEndSegments();
595 for (FGTaxiSegmentVectorIterator
596 i = currNode->getBeginRoute();
597 i != currNode->getEndRoute();
600 (*i)->setCourseDiff(course);
602 currNode->sortEndSegments(foundRoute);
603 for (FGTaxiSegmentVectorIterator
604 i = currNode->getBeginRoute();
605 i != currNode->getEndRoute();
608 //cerr << (*i)->getLength() << endl;
609 //cerr << (*i)->getIndex() << endl;
610 int idx = (*i)->getIndex();
611 routesStack.push_back((*i)->getIndex());
612 trace((*i)->getEnd(), end, depth+1, (*i)->getLength());
614 // // cerr << currNode -> getIndex() << " ";
615 // route.push_back(currNode->getIndex());
622 //SG_LOG( SG_GENERAL, SG_DEBUG, "4" );
624 if (nodesStack.empty())
626 printRoutingError(string("while finishing trace"));
628 nodesStack.pop_back();
629 // Make sure not to dump the level-zero routesStack entry, because that was never created.
632 routesStack.pop_back();
633 //cerr << "leaving trace " << routesStack.size() << endl;
635 totalDistance -= distance;
639 void FGGroundNetwork::printRoutingError(string mess)
641 SG_LOG(SG_GENERAL, SG_ALERT, "Error in ground network trace algorithm " << mess);
642 if (nodesStack.empty())
644 SG_LOG(SG_GENERAL, SG_ALERT, " nodesStack is empty. Dumping routesStack");
645 for (intVecIterator i = routesStack.begin() ; i != routesStack.end(); i++)
646 SG_LOG(SG_GENERAL, SG_ALERT, "Route " << (*i));
648 if (routesStack.empty())
650 SG_LOG(SG_GENERAL, SG_ALERT, " routesStack is empty. Dumping nodesStack");
651 for (intVecIterator i = nodesStack.begin() ; i != nodesStack.end(); i++)
652 SG_LOG(SG_GENERAL, SG_ALERT, "Node " << (*i));
658 void FGGroundNetwork::announcePosition(int id, FGAIFlightPlan *intendedRoute, int currentPosition,
659 double lat, double lon, double heading,
660 double speed, double alt, double radius, int leg,
663 TrafficVectorIterator i = activeTraffic.begin();
664 // Search search if the current id alread has an entry
665 // This might be faster using a map instead of a vector, but let's start by taking a safe route
666 if (activeTraffic.size()) {
667 //while ((i->getId() != id) && i != activeTraffic.end()) {
668 while (i != activeTraffic.end()) {
669 if (i->getId() == id) {
675 // Add a new TrafficRecord if no one exsists for this aircraft.
676 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
679 rec.setPositionAndIntentions(currentPosition, intendedRoute);
680 rec.setPositionAndHeading(lat, lon, heading, speed, alt);
681 rec.setRadius(radius); // only need to do this when creating the record.
682 rec.setCallSign(callsign);
683 activeTraffic.push_back(rec);
685 i->setPositionAndIntentions(currentPosition, intendedRoute);
686 i->setPositionAndHeading(lat, lon, heading, speed, alt);
690 void FGGroundNetwork::signOff(int id) {
691 TrafficVectorIterator i = activeTraffic.begin();
692 // Search search if the current id alread has an entry
693 // This might be faster using a map instead of a vector, but let's start by taking a safe route
694 if (activeTraffic.size()) {
695 //while ((i->getId() != id) && i != activeTraffic.end()) {
696 while (i != activeTraffic.end()) {
697 if (i->getId() == id) {
703 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
704 SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Aircraft without traffic record is signing off");
706 i = activeTraffic.erase(i);
710 void FGGroundNetwork::update(int id, double lat, double lon, double heading, double speed, double alt,
712 TrafficVectorIterator i = activeTraffic.begin();
713 // Search search if the current id has an entry
714 // This might be faster using a map instead of a vector, but let's start by taking a safe route
715 TrafficVectorIterator current, closest;
716 if (activeTraffic.size()) {
717 //while ((i->getId() != id) && i != activeTraffic.end()) {
718 while (i != activeTraffic.end()) {
719 if (i->getId() == id) {
725 // update position of the current aircraft
726 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
727 SG_LOG(SG_GENERAL, SG_ALERT, "AI error: updating aircraft without traffic record");
729 i->setPositionAndHeading(lat, lon, heading, speed, alt);
735 // Update every three secs, but add some randomness
736 // to prevent all IA objects doing this in synchrony
737 //if (getDt() < (3.0) + (rand() % 10))
741 current->clearResolveCircularWait();
742 current->setWaitsForId(0);
743 checkSpeedAdjustment(id, lat, lon, heading, speed, alt);
744 checkHoldPosition (id, lat, lon, heading, speed, alt);
745 if (checkForCircularWaits(id)) {
746 i->setResolveCircularWait();
751 Scan for a speed adjustment change. Find the nearest aircraft that is in front
752 and adjust speed when we get too close. Only do this when current position and/or
753 intentions of the current aircraft match current taxiroute position of the proximate
754 aircraft. For traffic that is on other routes we need to issue a "HOLD Position"
755 instruction. See below for the hold position instruction.
757 Note that there currently still is one flaw in the logic that needs to be addressed.
758 can be situations where one aircraft is in front of the current aircraft, on a separate
759 route, but really close after an intersection coming off the current route. This
760 aircraft is still close enough to block the current aircraft. This situation is currently
761 not addressed yet, but should be.
764 void FGGroundNetwork::checkSpeedAdjustment(int id, double lat,
765 double lon, double heading,
766 double speed, double alt)
769 TrafficVectorIterator current, closest;
770 TrafficVectorIterator i = activeTraffic.begin();
771 bool otherReasonToSlowDown = false;
772 bool previousInstruction;
773 if (activeTraffic.size())
775 //while ((i->getId() != id) && (i != activeTraffic.end()))
776 while (i != activeTraffic.end()) {
777 if (i->getId() == id) {
787 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
788 SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkSpeedAdjustment");
793 previousInstruction = current->getSpeedAdjustment();
794 double mindist = HUGE;
795 if (activeTraffic.size())
797 double course, dist, bearing, minbearing;
798 SGWayPoint curr (lon,
801 //TrafficVector iterator closest;
803 for (TrafficVectorIterator i = activeTraffic.begin();
804 i != activeTraffic.end(); i++)
807 //SGWayPoint curr (lon,
810 SGWayPoint other (i->getLongitude (),
813 other.CourseAndDistance(curr, &course, &dist);
814 bearing = fabs(heading-course);
816 bearing = 360-bearing;
817 if ((dist < mindist) && (bearing < 60.0))
821 minbearing = bearing;
825 //Check traffic at the tower controller
826 if (towerController->hasActiveTraffic())
828 for (TrafficVectorIterator i = towerController->getActiveTraffic().begin();
829 i != towerController->getActiveTraffic().end(); i++)
831 //cerr << "Comparing " << current->getId() << " and " << i->getId() << endl;
832 //SGWayPoint curr (lon,
835 SGWayPoint other (i->getLongitude (),
838 other.CourseAndDistance(curr, &course, &dist);
839 bearing = fabs(heading-course);
841 bearing = 360-bearing;
842 if ((dist < mindist) && (bearing < 60.0))
846 minbearing = bearing;
847 otherReasonToSlowDown = true;
851 // Finally, check UserPosition
852 double userLatitude = fgGetDouble("/position/latitude-deg");
853 double userLongitude = fgGetDouble("/position/longitude-deg");
854 SGWayPoint user (userLongitude,
856 alt); // Alt is not really important here.
857 user.CourseAndDistance(curr, &course, &dist);
858 bearing = fabs(heading-course);
860 bearing = 360-bearing;
861 if ((dist < mindist) && (bearing < 60.0))
865 minbearing = bearing;
866 otherReasonToSlowDown = true;
869 // if (closest == current) {
870 // //SG_LOG(SG_GENERAL, SG_ALERT, "AI error: closest and current match");
873 //cerr << "Distance : " << dist << " bearing : " << bearing << " heading : " << heading
874 // << " course : " << course << endl;
875 current->clearSpeedAdjustment();
877 if (current->checkPositionAndIntentions(*closest) || otherReasonToSlowDown)
879 double maxAllowableDistance = (1.1*current->getRadius()) + (1.1*closest->getRadius());
880 if (mindist < 2*maxAllowableDistance)
882 if (current->getId() == closest->getWaitsForId())
885 current->setWaitsForId(closest->getId());
886 if (closest->getId() != current->getId())
887 current->setSpeedAdjustment(closest->getSpeed()* (mindist/100));
889 current->setSpeedAdjustment(0); // This can only happen when the user aircraft is the one closest
890 if (mindist < maxAllowableDistance)
892 //double newSpeed = (maxAllowableDistance-mindist);
893 //current->setSpeedAdjustment(newSpeed);
894 //if (mindist < 0.5* maxAllowableDistance)
896 current->setSpeedAdjustment(0);
905 Check for "Hold position instruction".
906 The hold position should be issued under the following conditions:
907 1) For aircraft entering or crossing a runway with active traffic on it, or landing aircraft near it
908 2) For taxiing aircraft that use one taxiway in opposite directions
909 3) For crossing or merging taxiroutes.
912 void FGGroundNetwork::checkHoldPosition(int id, double lat,
913 double lon, double heading,
914 double speed, double alt)
917 TrafficVectorIterator current;
918 TrafficVectorIterator i = activeTraffic.begin();
919 if (activeTraffic.size())
921 //while ((i->getId() != id) && i != activeTraffic.end())
922 while (i != activeTraffic.end()) {
923 if (i->getId() == id) {
933 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
934 SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkHoldPosition");
937 current->setHoldPosition(false);
938 SGWayPoint curr (lon,
941 double course, dist, bearing, minbearing;
942 for (i = activeTraffic.begin();
943 i != activeTraffic.end(); i++)
945 if (i->getId() != current->getId())
947 int node = current->crosses(this, *i);
950 // Determine whether it's save to continue or not.
951 // If we have a crossing route, there are two possibilities:
952 // 1) This is an interestion
953 // 2) This is oncoming two-way traffic, using the same taxiway.
954 //cerr << "Hold check 1 : " << id << " has common node " << node << endl;
955 SGWayPoint nodePos(findNode(node)->getLongitude (),
956 findNode(node)->getLatitude (),
959 SGWayPoint other (i->getLongitude (),
962 //other.CourseAndDistance(curr, &course, &dist);
965 if (current->isOpposing(this, *i, node))
969 //cerr << "Hold check 2 : " << node << " has opposing segment " << endl;
970 // issue a "Hold Position" as soon as we're close to the offending node
971 // For now, I'm doing this as long as the other aircraft doesn't
972 // have a hold instruction as soon as we're within a reasonable
973 // distance from the offending node.
974 // This may be a bit of a conservative estimate though, as it may
975 // be well possible that both aircraft can both continue to taxi
976 // without crashing into each other.
981 other.CourseAndDistance(nodePos, &course, &dist);
982 if (dist > 200) // 2.0*i->getRadius())
985 //cerr << "Hold check 3 : " << id <<" Other aircraft approaching node is still far away. (" << dist << " nm). Can safely continue "
991 //cerr << "Hold check 4: " << id << " Would need to wait for other aircraft : distance = " << dist << " meters" << endl;
994 curr.CourseAndDistance(nodePos, &course, &dist);
995 if (!(i->hasHoldPosition()))
998 if ((dist < 200) && //2.5*current->getRadius()) &&
1000 (i->onRoute(this, *current)) &&
1001 //((i->onRoute(this, *current)) || ((!(i->getSpeedAdjustment())))) &&
1002 (!(current->getId() == i->getWaitsForId())))
1003 //(!(i->getSpeedAdjustment()))) // &&
1004 //(!(current->getSpeedAdjustment())))
1007 current->setHoldPosition(true);
1008 current->setWaitsForId(i->getId());
1009 //cerr << "Hold check 5: " << current->getCallSign() <<" Setting Hold Position: distance to node (" << node << ") "
1010 // << dist << " meters. Waiting for " << i->getCallSign();
1012 //cerr <<" [opposing] " << endl;
1014 // cerr << "[non-opposing] " << endl;
1015 //if (i->hasSpeefAdjustment())
1017 // cerr << " (which in turn waits for ) " << i->
1021 //cerr << "Hold check 6: " << id << " No need to hold yet: Distance to node : " << dist << " nm"<< endl;
1030 * Check whether situations occur where the current aircraft is waiting for itself
1031 * due to higher order interactions.
1032 * A 'circular' wait is a situation where a waits for b, b waits for c, and c waits
1033 * for a. Ideally each aircraft only waits for one other aircraft, so by tracing
1034 * through this list of waiting aircraft, we can check if we'd eventually end back
1035 * at the current aircraft.
1037 * Note that we should consider the situation where we are actually checking aircraft
1038 * d, which is waiting for aircraft a. d is not part of the loop, but is held back by
1039 * the looping aircraft. If we don't check for that, this function will get stuck into
1043 bool FGGroundNetwork::checkForCircularWaits(int id)
1045 //cerr << "Performing Wait check " << id << endl;
1047 TrafficVectorIterator current, other;
1048 TrafficVectorIterator i = activeTraffic.begin();
1049 int trafficSize = activeTraffic.size();
1051 while (i != activeTraffic.end()) {
1052 if (i->getId() == id) {
1061 if (i == activeTraffic.end() || (trafficSize == 0)) {
1062 SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits");
1066 target = current->getWaitsForId();
1067 //bool printed = false; // Note that this variable is for debugging purposes only.
1071 //cerr << "aircraft waits for user" << endl;
1076 while ((target > 0) && (target != id) && counter++ < trafficSize) {
1078 TrafficVectorIterator i = activeTraffic.begin();
1080 //while ((i->getId() != id) && i != activeTraffic.end())
1081 while (i != activeTraffic.end()) {
1082 if (i->getId() == target) {
1091 if (i == activeTraffic.end() || (trafficSize == 0)) {
1092 //cerr << "[Waiting for traffic at Runway: DONE] " << endl << endl;;
1093 // The target id is not found on the current network, which means it's at the tower
1094 //SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits");
1098 target = other->getWaitsForId();
1100 // actually this trap isn't as impossible as it first seemed:
1101 // the setWaitsForID(id) is set to current when the aircraft
1102 // is waiting for the user controlled aircraft.
1103 //if (current->getId() == other->getId()) {
1104 // cerr << "Caught the impossible trap" << endl;
1105 // cerr << "Current = " << current->getId() << endl;
1106 // cerr << "Other = " << other ->getId() << endl;
1107 // for (TrafficVectorIterator at = activeTraffic.begin();
1108 // at != activeTraffic.end();
1110 // cerr << "currently active aircraft : " << at->getCallSign() << " with Id " << at->getId() << " waits for " << at->getWaitsForId() << endl;
1113 if (current->getId() == other->getId())
1116 //cerr << current->getCallSign() << " (" << current->getId() << ") " << " -> " << other->getCallSign()
1117 // << " (" << other->getId() << "); " << endl;;
1127 // cerr << "[done] " << endl << endl;;
1129 SG_LOG(SG_GENERAL, SG_WARN, "Detected circular wait condition: Id = " << id << "target = " << target);
1136 // Note that this function is probably obsolete...
1137 bool FGGroundNetwork::hasInstruction(int id)
1139 TrafficVectorIterator i = activeTraffic.begin();
1140 // Search search if the current id has an entry
1141 // This might be faster using a map instead of a vector, but let's start by taking a safe route
1142 if (activeTraffic.size())
1144 //while ((i->getId() != id) && i != activeTraffic.end()) {
1145 while (i != activeTraffic.end()) {
1146 if (i->getId() == id) {
1152 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
1153 SG_LOG(SG_GENERAL, SG_ALERT, "AI error: checking ATC instruction for aircraft without traffic record");
1155 return i->hasInstruction();
1160 FGATCInstruction FGGroundNetwork::getInstruction(int id)
1162 TrafficVectorIterator i = activeTraffic.begin();
1163 // Search search if the current id has an entry
1164 // This might be faster using a map instead of a vector, but let's start by taking a safe route
1165 if (activeTraffic.size()) {
1166 //while ((i->getId() != id) && i != activeTraffic.end()) {
1167 while (i != activeTraffic.end()) {
1168 if (i->getId() == id) {
1174 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
1175 SG_LOG(SG_GENERAL, SG_ALERT, "AI error: requesting ATC instruction for aircraft without traffic record");
1177 return i->getInstruction();
1179 return FGATCInstruction();