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 <boost/foreach.hpp>
33 #include <osg/Geometry>
34 #include <osg/MatrixTransform>
37 #include <simgear/debug/logstream.hxx>
38 #include <simgear/scene/material/EffectGeode.hxx>
39 #include <simgear/scene/material/matlib.hxx>
40 #include <simgear/scene/material/mat.hxx>
41 #include <simgear/scene/util/OsgMath.hxx>
42 #include <simgear/structure/exception.hxx>
44 #include <Airports/simple.hxx>
45 #include <Airports/dynamics.hxx>
46 #include <Airports/runways.hxx>
48 #include <AIModel/AIAircraft.hxx>
49 #include <AIModel/performancedata.hxx>
50 #include <AIModel/AIFlightPlan.hxx>
52 #include <ATC/atc_mgr.hxx>
54 #include <Scenery/scenery.hxx>
56 #include "groundnetwork.hxx"
60 /***************************************************************************
62 **************************************************************************/
64 FGTaxiSegment::FGTaxiSegment(int aStart, int aEnd, bool isPushBack) :
70 isPushBackRoute(isPushBack),
78 bool FGTaxiSegment::bindToNodes(const IndexTaxiNodeMap& nodes)
80 IndexTaxiNodeMap::const_iterator it = nodes.find(startNode);
81 if (it == nodes.end()) {
87 it = nodes.find(endNode);
88 if (it == nodes.end()) {
94 start->addSegment(this);
96 SGGeodesy::inverse(start->geod(), end->geod(), heading, az2, length);
100 SGGeod FGTaxiSegment::getCenter() const
102 return SGGeodesy::direct(start->geod(), heading, length * 0.5);
105 void FGTaxiSegment::block(int id, time_t blockTime, time_t now)
107 BlockListIterator i = blockTimes.begin();
108 while (i != blockTimes.end()) {
109 if (i->getId() == id)
113 if (i == blockTimes.end()) {
114 blockTimes.push_back(Block(id, blockTime, now));
115 sort(blockTimes.begin(), blockTimes.end());
117 i->updateTimeStamps(blockTime, now);
121 // The segment has a block if any of the block times listed in the block list is
122 // smaller than the current time.
123 bool FGTaxiSegment::hasBlock(time_t now)
125 for (BlockListIterator i = blockTimes.begin(); i != blockTimes.end(); i++) {
126 if (i->getBlockTime() < now)
132 void FGTaxiSegment::unblock(time_t now)
134 if (blockTimes.size()) {
135 BlockListIterator i = blockTimes.begin();
136 if (i->getTimeStamp() < (now - 30)) {
144 /***************************************************************************
146 **************************************************************************/
147 bool FGTaxiRoute::next(int *nde)
149 //for (intVecIterator i = nodes.begin(); i != nodes.end(); i++)
150 // cerr << "FGTaxiRoute contains : " << *(i) << endl;
151 //cerr << "Offset from end: " << nodes.end() - currNode << endl;
152 //if (currNode != nodes.end())
153 // cerr << "true" << endl;
155 // cerr << "false" << endl;
156 //if (nodes.size() != (routes.size()) +1)
157 // cerr << "ALERT: Misconfigured TaxiRoute : " << nodes.size() << " " << routes.size() << endl;
159 if (currNode == nodes.end())
162 if (currNode != nodes.begin()) // make sure route corresponds to the end node
168 bool FGTaxiRoute::next(int *nde, int *rte)
170 //for (intVecIterator i = nodes.begin(); i != nodes.end(); i++)
171 // cerr << "FGTaxiRoute contains : " << *(i) << endl;
172 //cerr << "Offset from end: " << nodes.end() - currNode << endl;
173 //if (currNode != nodes.end())
174 // cerr << "true" << endl;
176 // cerr << "false" << endl;
177 if (nodes.size() != (routes.size()) + 1) {
178 SG_LOG(SG_GENERAL, SG_ALERT,
179 "ALERT: Misconfigured TaxiRoute : " << nodes.
180 size() << " " << routes.size());
181 throw sg_range_exception("misconfigured taxi route");
183 if (currNode == nodes.end())
186 //*rte = *(currRoute);
187 if (currNode != nodes.begin()) // Make sure route corresponds to the end node
192 // If currNode points to the first node, this means the aircraft is not on the taxi node
193 // yet. Make sure to return a unique identifyer in this situation though, because otherwise
194 // the speed adjust AI code may be unable to resolve whether two aircraft are on the same
195 // taxi route or not. the negative of the preceding route seems a logical choice, as it is
196 // unique for any starting location.
197 // Note that this is probably just a temporary fix until I get Parking / tower control working.
198 *rte = -1 * *(currRoute);
205 void FGTaxiRoute::rewind(int route)
211 if (!(next(&currPoint, &currRoute))) {
212 SG_LOG(SG_GENERAL, SG_ALERT,
213 "Error in rewinding TaxiRoute: current" << currRoute <<
216 } while (currRoute != route);
222 /***************************************************************************
224 **************************************************************************/
226 bool compare_trafficrecords(FGTrafficRecord a, FGTrafficRecord b)
228 return (a.getIntentions().size() < b.getIntentions().size());
231 FGGroundNetwork::FGGroundNetwork() :
240 currTraffic = activeTraffic.begin();
243 networkInitialized = false;
247 FGGroundNetwork::~FGGroundNetwork()
249 // JMT 2012-09-8 - disabling the groundnet-caching as part of enabling the
250 // navcache. The problem isn't the NavCache - it's that for the past few years,
251 // we have not being running destructors on FGPositioned classes, and hence,
252 // not running this code.
253 // When I fix FGPositioned lifetimes (unloading-at-runtime support), this
254 // will need to be re-visited so it can run safely during shutdown.
256 //cerr << "Running Groundnetwork Destructor " << endl;
257 bool saveData = false;
259 if (fgGetBool("/sim/ai/groundnet-cache")) {
260 SGPath cacheData(globals->get_fg_home());
261 cacheData.append("ai");
262 string airport = parent->getId();
264 if ((airport) != "") {
266 ::snprintf(buffer, 128, "%c/%c/%c/",
267 airport[0], airport[1], airport[2]);
268 cacheData.append(buffer);
269 if (!cacheData.exists()) {
270 cacheData.create_dir(0777);
272 cacheData.append(airport + "-groundnet-cache.txt");
273 cachefile.open(cacheData.str().c_str());
277 cachefile << "[GroundNetcachedata:ref:2011:09:04]" << endl;
278 for (FGTaxiNodeVectorIterator node = nodes.begin();
279 node != nodes.end(); node++) {
281 cachefile << (*node)->getIndex () << " "
282 << (*node)->getElevationM (parent->getElevation()*SG_FEET_TO_METER) << " "
288 pushBackNodes.clear();
289 for (FGTaxiSegmentVectorIterator seg = segments.begin();
290 seg != segments.end(); seg++) {
300 void FGGroundNetwork::saveElevationCache() {
301 //cerr << "Running Groundnetwork Destructor " << endl;
302 bool saveData = false;
304 if (fgGetBool("/sim/ai/groundnet-cache")) {
305 SGPath cacheData(globals->get_fg_home());
306 cacheData.append("ai");
307 string airport = parent->getId();
309 if ((airport) != "") {
311 ::snprintf(buffer, 128, "%c/%c/%c/",
312 airport[0], airport[1], airport[2]);
313 cacheData.append(buffer);
314 if (!cacheData.exists()) {
315 cacheData.create_dir(0777);
317 cacheData.append(airport + "-groundnet-cache.txt");
318 cachefile.open(cacheData.str().c_str());
322 cachefile << "[GroundNetcachedata:ref:2011:09:04]" << endl;
323 for (IndexTaxiNodeMap::iterator node = nodes.begin();
324 node != nodes.end(); node++) {
326 cachefile << node->second->getIndex () << " "
327 << node->second->getElevationM (parent->getElevation()*SG_FEET_TO_METER) << " "
336 void FGGroundNetwork::addSegment(FGTaxiSegment* seg)
338 segments.push_back(seg);
341 void FGGroundNetwork::addNode(FGTaxiNode* node)
344 IndexTaxiNodeMap::iterator it = nodes.find(node->getIndex());
345 if (it != nodes.end()) {
346 throw sg_range_exception();
349 nodes.insert(it, std::make_pair(node->getIndex(), node));
352 void FGGroundNetwork::addNodes(FGParkingVec * parkings)
354 BOOST_FOREACH(FGParking* parking, *parkings) {
359 void FGGroundNetwork::init()
361 if (networkInitialized) {
362 FGATCController::init();
363 //cerr << "FGground network already initialized" << endl;
370 // bind segments to nodes
371 BOOST_FOREACH(FGTaxiSegment* segment, segments) {
372 if (!segment->bindToNodes(nodes)) {
373 SG_LOG(SG_GENERAL, SG_ALERT, "unable to bind taxiway segment");
376 segment->setIndex(index++);
377 if (segment->isPushBack()) {
378 pushBackNodes.push_back(segment->getEnd());
382 // establish pairing of segments
383 BOOST_FOREACH(FGTaxiSegment* segment, segments) {
384 FGTaxiSegment* opp = segment->getEnd()->getArcTo(segment->getStart());
386 segment->setOpposite(opp);
390 if (fgGetBool("/sim/ai/groundnet-cache")) {
394 networkInitialized = true;
397 void FGGroundNetwork::parseCache()
399 SGPath cacheData(globals->get_fg_home());
400 cacheData.append("ai");
401 string airport = parent->getId();
403 if (airport.empty()) {
408 ::snprintf(buffer, 128, "%c/%c/%c/",
409 airport[0], airport[1], airport[2]);
410 cacheData.append(buffer);
411 if (!cacheData.exists()) {
412 cacheData.create_dir(0777);
416 cacheData.append(airport + "-groundnet-cache.txt");
417 if (cacheData.exists()) {
418 ifstream data(cacheData.c_str());
421 if (revisionStr != "[GroundNetcachedata:ref:2011:09:04]") {
422 SG_LOG(SG_GENERAL, SG_ALERT,"GroundNetwork Warning: discarding outdated cachefile " <<
423 cacheData.c_str() << " for Airport " << airport);
425 for (IndexTaxiNodeMap::iterator i = nodes.begin();
428 i->second->setElevation(parent->elevation() * SG_FEET_TO_METER);
429 data >> index >> elev;
432 if (index != i->second->getIndex()) {
433 SG_LOG(SG_GENERAL, SG_ALERT, "Index read from ground network cache at airport " << airport << " does not match index in the network itself");
435 i->second->setElevation(elev);
442 int FGGroundNetwork::findNearestNode(const SGGeod & aGeod)
444 double minDist = HUGE_VAL;
447 IndexTaxiNodeMap::iterator i;
448 for (i = nodes.begin(); i != nodes.end(); i++) {
449 double d = SGGeodesy::distanceM(aGeod, i->second->geod());
459 int FGGroundNetwork::findNearestNodeOnRunway(const SGGeod & aGeod, FGRunway* aRunway)
461 double minDist = HUGE_VAL;
464 IndexTaxiNodeMap::iterator i;
465 for (i = nodes.begin(); i != nodes.end(); i++) {
466 if (!i->second->getIsOnRunway()) {
469 // check point lies on the runway - i.e that course from aGeod to the
470 // runway end, matches the runway heading
472 double course = SGGeodesy::courseDeg(i->second->geod(), aRunway->end());
473 double headingDiff = course - aRunway->headingDeg();
474 SG_NORMALIZE_RANGE(headingDiff, -180.0, 180.0);
475 if (fabs(headingDiff) > 3.0) { // 3 degrees tolerance
480 double d = SGGeodesy::distanceM(aGeod, i->second->geod());
490 FGTaxiNode* FGGroundNetwork::findNode(unsigned int idx)
492 IndexTaxiNodeMap::iterator i = nodes.find(idx);
493 if (i == nodes.end()) {
500 FGTaxiSegment *FGGroundNetwork::findSegment(unsigned idx)
502 for (FGTaxiSegmentVectorIterator
503 itr = segments.begin();
504 itr != segments.end(); itr++)
506 if (itr->getIndex() == idx)
507 return itr->getAddress();
510 if ((idx > 0) && (idx <= segments.size()))
511 return segments[idx - 1];
513 //cerr << "Alert: trying to find invalid segment " << idx << endl;
519 FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end,
522 //implements Dijkstra's algorithm to find shortest distance route from start to end
523 //taken from http://en.wikipedia.org/wiki/Dijkstra's_algorithm
525 //double INFINITE = 100000000000.0;
526 // initialize scoring values
527 int nParkings = parent->getDynamics()->getNrOfParkings();
528 FGTaxiNodeVector unvisited;
531 // create vector from map values
532 IndexTaxiNodeMap::iterator i;
533 for (i = nodes.begin(); i != nodes.end(); i++) {
534 unvisited.push_back(i->second);
537 unvisited = pushBackNodes;
540 BOOST_FOREACH(FGTaxiNode* node, unvisited) {
541 node->setPathScore(HUGE_VAL); //infinity by all practical means
542 node->setPreviousNode(0); //
543 node->setPreviousSeg(0); //
546 FGTaxiNode *firstNode = findNode(start);
549 SG_LOG(SG_GENERAL, SG_ALERT,
550 "Error in ground network. Failed to find first waypoint: " << start
551 << " at " << ((parent) ? parent->getId() : "<unknown>"));
552 return FGTaxiRoute();
554 firstNode->setPathScore(0);
556 FGTaxiNode *lastNode = findNode(end);
559 SG_LOG(SG_GENERAL, SG_ALERT,
560 "Error in ground network. Failed to find last waypoint: " << end
561 << " at " << ((parent) ? parent->getId() : "<unknown>"));
562 return FGTaxiRoute();
565 while (!unvisited.empty()) {
566 FGTaxiNode *best = unvisited.front();
567 BOOST_FOREACH(FGTaxiNode* i, unvisited) {
568 if (i->getPathScore() < best->getPathScore()) {
573 // remove 'best' from the unvisited set
574 FGTaxiNodeVectorIterator newend =
575 remove(unvisited.begin(), unvisited.end(), best);
576 unvisited.erase(newend, unvisited.end());
578 if (best == lastNode) { // found route or best not connected
582 BOOST_FOREACH(FGTaxiSegment* seg, best->arcs()) {
583 if (!fullSearch && !seg->isPushBack()) {
584 continue; // inelligible!
587 FGTaxiNode *tgt = seg->getEnd();
588 double alt = best->getPathScore() + seg->getLength() +
589 seg->getPenalty(nParkings);
590 if (alt < tgt->getPathScore()) { // Relax (u,v)
591 tgt->setPathScore(alt);
592 tgt->setPreviousNode(best);
593 tgt->setPreviousSeg(seg);
595 } // of outgoing arcs/segments from current best node iteration
596 } // of unvisited nodes remaining
598 if (lastNode->getPathScore() == HUGE_VAL) {
599 // no valid route found
601 SG_LOG(SG_GENERAL, SG_ALERT,
602 "Failed to find route from waypoint " << start << " to "
603 << end << " at " << parent->getId());
607 //exit(1); //TODO exit more gracefully, no need to stall the whole sim with broken GN's
609 // assemble route from backtrace information
610 intVec nodes, routes;
611 FGTaxiNode *bt = lastNode;
612 while (bt->getPreviousNode() != 0) {
613 nodes.push_back(bt->getIndex());
614 routes.push_back(bt->getPreviousSegment()->getIndex());
615 bt = bt->getPreviousNode();
617 nodes.push_back(start);
618 reverse(nodes.begin(), nodes.end());
619 reverse(routes.begin(), routes.end());
621 return FGTaxiRoute(nodes, routes, lastNode->getPathScore(), 0);
625 int FGTaxiSegment::getPenalty(int nGates)
628 if (end->getIndex() < nGates) {
631 if (end->getIsOnRunway()) { // For now. In future versions, need to find out whether runway is active.
637 /* ATC Related Functions */
639 void FGGroundNetwork::announcePosition(int id,
640 FGAIFlightPlan * intendedRoute,
641 int currentPosition, double lat,
642 double lon, double heading,
643 double speed, double alt,
644 double radius, int leg,
645 FGAIAircraft * aircraft)
648 TrafficVectorIterator i = activeTraffic.begin();
649 // Search search if the current id alread has an entry
650 // This might be faster using a map instead of a vector, but let's start by taking a safe route
651 if (activeTraffic.size()) {
652 //while ((i->getId() != id) && i != activeTraffic.end()) {
653 while (i != activeTraffic.end()) {
654 if (i->getId() == id) {
660 // Add a new TrafficRecord if no one exsists for this aircraft.
661 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
665 rec.setPositionAndIntentions(currentPosition, intendedRoute);
666 rec.setPositionAndHeading(lat, lon, heading, speed, alt);
667 rec.setRadius(radius); // only need to do this when creating the record.
668 rec.setAircraft(aircraft);
670 activeTraffic.push_front(rec);
672 activeTraffic.push_back(rec);
676 i->setPositionAndIntentions(currentPosition, intendedRoute);
677 i->setPositionAndHeading(lat, lon, heading, speed, alt);
682 void FGGroundNetwork::signOff(int id)
684 TrafficVectorIterator i = activeTraffic.begin();
685 // Search search if the current id alread has an entry
686 // This might be faster using a map instead of a vector, but let's start by taking a safe route
687 if (activeTraffic.size()) {
688 //while ((i->getId() != id) && i != activeTraffic.end()) {
689 while (i != activeTraffic.end()) {
690 if (i->getId() == id) {
696 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
697 SG_LOG(SG_GENERAL, SG_ALERT,
698 "AI error: Aircraft without traffic record is signing off at " << SG_ORIGIN);
700 i = activeTraffic.erase(i);
704 * The ground network can deal with the following states:
705 * 0 = Normal; no action required
706 * 1 = "Acknowledge "Hold position
707 * 2 = "Acknowledge "Resume taxi".
708 * 3 = "Issue TaxiClearance"
709 * 4 = Acknowledge Taxi Clearance"
710 * 5 = Post acknowlegde taxiclearance: Start taxiing
712 * 7 = Acknowledge report runway
713 * 8 = Switch tower frequency
714 * 9 = Acknowledge switch tower frequency
715 *************************************************************************************************************************/
716 bool FGGroundNetwork::checkTransmissionState(int minState, int maxState, TrafficVectorIterator i, time_t now, AtcMsgId msgId,
719 int state = i->getState();
720 if ((state >= minState) && (state <= maxState) && available) {
721 if ((msgDir == ATC_AIR_TO_GROUND) && isUserAircraft(i->getAircraft())) {
722 //cerr << "Checking state " << state << " for " << i->getAircraft()->getCallSign() << endl;
723 static SGPropertyNode_ptr trans_num = globals->get_props()->getNode("/sim/atc/transmission-num", true);
724 int n = trans_num->getIntValue();
726 trans_num->setIntValue(-1);
728 //cerr << "Selected transmission message " << n << endl;
729 //FGATCManager *atc = (FGATCManager*) globals->get_subsystem("atc");
730 FGATCDialogNew::instance()->removeEntry(1);
732 //cerr << "creating message for " << i->getAircraft()->getCallSign() << endl;
733 transmit(&(*i), &(*parent->getDynamics()), msgId, msgDir, false);
737 transmit(&(*i), &(*parent->getDynamics()), msgId, msgDir, true);
739 lastTransmission = now;
746 void FGGroundNetwork::updateAircraftInformation(int id, double lat, double lon,
747 double heading, double speed, double alt,
750 time_t currentTime = time(NULL);
751 if (nextSave < currentTime) {
752 saveElevationCache();
753 nextSave = currentTime + 100 + rand() % 200;
755 // Check whether aircraft are on hold due to a preceding pushback. If so, make sure to
756 // Transmit air-to-ground "Ready to taxi request:
757 // Transmit ground to air approval / hold
758 // Transmit confirmation ...
759 // Probably use a status mechanism similar to the Engine start procedure in the startup controller.
762 TrafficVectorIterator i = activeTraffic.begin();
763 // Search search if the current id has an entry
764 // This might be faster using a map instead of a vector, but let's start by taking a safe route
765 TrafficVectorIterator current, closest;
766 if (activeTraffic.size()) {
767 //while ((i->getId() != id) && i != activeTraffic.end()) {
768 while (i != activeTraffic.end()) {
769 if (i->getId() == id) {
775 // update position of the current aircraft
776 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
777 SG_LOG(SG_GENERAL, SG_ALERT,
778 "AI error: updating aircraft without traffic record at " << SG_ORIGIN);
780 i->setPositionAndHeading(lat, lon, heading, speed, alt);
786 // Update every three secs, but add some randomness
787 // to prevent all IA objects doing this in synchrony
788 //if (getDt() < (3.0) + (rand() % 10))
792 current->clearResolveCircularWait();
793 current->setWaitsForId(0);
794 checkSpeedAdjustment(id, lat, lon, heading, speed, alt);
795 bool needsTaxiClearance = current->getAircraft()->getTaxiClearanceRequest();
796 if (!needsTaxiClearance) {
797 checkHoldPosition(id, lat, lon, heading, speed, alt);
798 //if (checkForCircularWaits(id)) {
799 // i->setResolveCircularWait();
802 current->setHoldPosition(true);
803 int state = current->getState();
804 time_t now = time(NULL) + fgGetLong("/sim/time/warp");
805 if ((now - lastTransmission) > 15) {
808 if (checkTransmissionState(0,2, current, now, MSG_REQUEST_TAXI_CLEARANCE, ATC_AIR_TO_GROUND)) {
809 current->setState(3);
811 if (checkTransmissionState(3,3, current, now, MSG_ISSUE_TAXI_CLEARANCE, ATC_GROUND_TO_AIR)) {
812 current->setState(4);
814 if (checkTransmissionState(4,4, current, now, MSG_ACKNOWLEDGE_TAXI_CLEARANCE, ATC_AIR_TO_GROUND)) {
815 current->setState(5);
817 if ((state == 5) && available) {
818 current->setState(0);
819 current->getAircraft()->setTaxiClearanceRequest(false);
820 current->setHoldPosition(false);
828 Scan for a speed adjustment change. Find the nearest aircraft that is in front
829 and adjust speed when we get too close. Only do this when current position and/or
830 intentions of the current aircraft match current taxiroute position of the proximate
831 aircraft. For traffic that is on other routes we need to issue a "HOLD Position"
832 instruction. See below for the hold position instruction.
834 Note that there currently still is one flaw in the logic that needs to be addressed.
835 There can be situations where one aircraft is in front of the current aircraft, on a separate
836 route, but really close after an intersection coming off the current route. This
837 aircraft is still close enough to block the current aircraft. This situation is currently
838 not addressed yet, but should be.
841 void FGGroundNetwork::checkSpeedAdjustment(int id, double lat,
842 double lon, double heading,
843 double speed, double alt)
846 TrafficVectorIterator current, closest, closestOnNetwork;
847 TrafficVectorIterator i = activeTraffic.begin();
848 bool otherReasonToSlowDown = false;
849 // bool previousInstruction;
850 if (activeTraffic.size()) {
851 //while ((i->getId() != id) && (i != activeTraffic.end()))
852 while (i != activeTraffic.end()) {
853 if (i->getId() == id) {
861 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
862 SG_LOG(SG_GENERAL, SG_ALERT,
863 "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkSpeedAdjustment at " << SG_ORIGIN);
868 // previousInstruction = current->getSpeedAdjustment();
869 double mindist = HUGE_VAL;
870 if (activeTraffic.size()) {
871 double course, dist, bearing, az2; // minbearing,
872 SGGeod curr(SGGeod::fromDegM(lon, lat, alt));
873 //TrafficVector iterator closest;
875 closestOnNetwork = current;
876 for (TrafficVectorIterator i = activeTraffic.begin();
877 i != activeTraffic.end(); i++) {
882 SGGeod other(SGGeod::fromDegM(i->getLongitude(),
885 SGGeodesy::inverse(curr, other, course, az2, dist);
886 bearing = fabs(heading - course);
888 bearing = 360 - bearing;
889 if ((dist < mindist) && (bearing < 60.0)) {
892 closestOnNetwork = i;
893 // minbearing = bearing;
897 //Check traffic at the tower controller
898 if (towerController->hasActiveTraffic()) {
899 for (TrafficVectorIterator i =
900 towerController->getActiveTraffic().begin();
901 i != towerController->getActiveTraffic().end(); i++) {
902 //cerr << "Comparing " << current->getId() << " and " << i->getId() << endl;
903 SGGeod other(SGGeod::fromDegM(i->getLongitude(),
906 SGGeodesy::inverse(curr, other, course, az2, dist);
907 bearing = fabs(heading - course);
909 bearing = 360 - bearing;
910 if ((dist < mindist) && (bearing < 60.0)) {
911 //cerr << "Current aircraft " << current->getAircraft()->getTrafficRef()->getCallSign()
912 // << " is closest to " << i->getAircraft()->getTrafficRef()->getCallSign()
913 // << ", which has status " << i->getAircraft()->isScheduledForTakeoff()
917 // minbearing = bearing;
918 otherReasonToSlowDown = true;
922 // Finally, check UserPosition
923 // Note, as of 2011-08-01, this should no longer be necessecary.
925 double userLatitude = fgGetDouble("/position/latitude-deg");
926 double userLongitude = fgGetDouble("/position/longitude-deg");
927 SGGeod user(SGGeod::fromDeg(userLongitude, userLatitude));
928 SGGeodesy::inverse(curr, user, course, az2, dist);
930 bearing = fabs(heading - course);
932 bearing = 360 - bearing;
933 if ((dist < mindist) && (bearing < 60.0)) {
936 minbearing = bearing;
937 otherReasonToSlowDown = true;
940 current->clearSpeedAdjustment();
941 bool needBraking = false;
942 if (current->checkPositionAndIntentions(*closest)
943 || otherReasonToSlowDown) {
944 double maxAllowableDistance =
945 (1.1 * current->getRadius()) +
946 (1.1 * closest->getRadius());
947 if (mindist < 2 * maxAllowableDistance) {
948 if (current->getId() == closest->getWaitsForId())
951 current->setWaitsForId(closest->getId());
952 if (closest->getId() != current->getId()) {
953 current->setSpeedAdjustment(closest->getSpeed() *
958 // closest->getAircraft()->getTakeOffStatus() &&
959 // (current->getAircraft()->getTrafficRef()->getDepartureAirport() == closest->getAircraft()->getTrafficRef()->getDepartureAirport()) &&
960 // (current->getAircraft()->GetFlightPlan()->getRunway() == closest->getAircraft()->GetFlightPlan()->getRunway())
962 // current->getAircraft()->scheduleForATCTowerDepartureControl(1);
964 current->setSpeedAdjustment(0); // This can only happen when the user aircraft is the one closest
966 if (mindist < maxAllowableDistance) {
967 //double newSpeed = (maxAllowableDistance-mindist);
968 //current->setSpeedAdjustment(newSpeed);
969 //if (mindist < 0.5* maxAllowableDistance)
971 current->setSpeedAdjustment(0);
976 if ((closest->getId() == closestOnNetwork->getId()) && (current->getPriority() < closest->getPriority()) && needBraking) {
977 swap(current, closest);
983 Check for "Hold position instruction".
984 The hold position should be issued under the following conditions:
985 1) For aircraft entering or crossing a runway with active traffic on it, or landing aircraft near it
986 2) For taxiing aircraft that use one taxiway in opposite directions
987 3) For crossing or merging taxiroutes.
990 void FGGroundNetwork::checkHoldPosition(int id, double lat,
991 double lon, double heading,
992 double speed, double alt)
994 TrafficVectorIterator current;
995 TrafficVectorIterator i = activeTraffic.begin();
996 if (activeTraffic.size()) {
997 //while ((i->getId() != id) && i != activeTraffic.end())
998 while (i != activeTraffic.end()) {
999 if (i->getId() == id) {
1007 time_t now = time(NULL) + fgGetLong("/sim/time/warp");
1008 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
1009 SG_LOG(SG_GENERAL, SG_ALERT,
1010 "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkHoldPosition at " << SG_ORIGIN);
1014 if (current->getAircraft()->getTakeOffStatus() == 1) {
1015 current->setHoldPosition(true);
1018 if (current->getAircraft()->getTakeOffStatus() == 2) {
1019 //cerr << current->getAircraft()->getCallSign() << ". Taxi in position and hold" << endl;
1020 current->setHoldPosition(false);
1021 current->clearSpeedAdjustment();
1024 bool origStatus = current->hasHoldPosition();
1025 current->setHoldPosition(false);
1026 //SGGeod curr(SGGeod::fromDegM(lon, lat, alt));
1027 int currentRoute = i->getCurrentPosition();
1029 if (i->getIntentions().size()) {
1030 nextRoute = (*(i->getIntentions().begin()));
1034 if (currentRoute > 0) {
1035 FGTaxiSegment *tx = findSegment(currentRoute);
1038 nx = findSegment(nextRoute);
1042 //if (tx->hasBlock(now) || nx->hasBlock(now) ) {
1043 // current->setHoldPosition(true);
1045 SGGeod start(SGGeod::fromDeg((i->getLongitude()), (i->getLatitude())));
1046 SGGeod end (nx->getStart()->geod());
1048 double distance = SGGeodesy::distanceM(start, end);
1049 if (nx->hasBlock(now) && (distance < i->getRadius() * 4)) {
1050 current->setHoldPosition(true);
1052 intVecIterator ivi = i->getIntentions().begin();
1053 while (ivi != i->getIntentions().end()) {
1055 distance += segments[(*ivi)-1]->getLength();
1056 if ((segments[(*ivi)-1]->hasBlock(now)) && (distance < i->getRadius() * 4)) {
1057 current->setHoldPosition(true);
1065 bool currStatus = current->hasHoldPosition();
1066 current->setHoldPosition(origStatus);
1067 // Either a Hold Position or a resume taxi transmission has been issued
1068 if ((now - lastTransmission) > 2) {
1071 if (current->getState() == 0) {
1072 if ((origStatus != currStatus) && available) {
1073 //cerr << "Issueing hold short instrudtion " << currStatus << " " << available << endl;
1074 if (currStatus == true) { // No has a hold short instruction
1075 transmit(&(*current), &(*parent->getDynamics()), MSG_HOLD_POSITION, ATC_GROUND_TO_AIR, true);
1076 //cerr << "Transmittin hold short instrudtion " << currStatus << " " << available << endl;
1077 current->setState(1);
1079 transmit(&(*current), &(*parent->getDynamics()), MSG_RESUME_TAXI, ATC_GROUND_TO_AIR, true);
1080 //cerr << "Transmittig resume instrudtion " << currStatus << " " << available << endl;
1081 current->setState(2);
1083 lastTransmission = now;
1085 // Don't act on the changed instruction until the transmission is confirmed
1086 // So set back to original status
1087 //cerr << "Current state " << current->getState() << endl;
1091 // 6 = Report runway
1092 // 7 = Acknowledge report runway
1093 // 8 = Switch tower frequency
1094 //9 = Acknowledge switch tower frequency
1096 //int state = current->getState();
1097 if (checkTransmissionState(1,1, current, now, MSG_ACKNOWLEDGE_HOLD_POSITION, ATC_AIR_TO_GROUND)) {
1098 current->setState(0);
1099 current->setHoldPosition(true);
1101 if (checkTransmissionState(2,2, current, now, MSG_ACKNOWLEDGE_RESUME_TAXI, ATC_AIR_TO_GROUND)) {
1102 current->setState(0);
1103 current->setHoldPosition(false);
1105 if (current->getAircraft()->getTakeOffStatus() && (current->getState() == 0)) {
1106 //cerr << "Scheduling " << current->getAircraft()->getCallSign() << " for hold short" << endl;
1107 current->setState(6);
1109 if (checkTransmissionState(6,6, current, now, MSG_REPORT_RUNWAY_HOLD_SHORT, ATC_AIR_TO_GROUND)) {
1111 if (checkTransmissionState(7,7, current, now, MSG_ACKNOWLEDGE_REPORT_RUNWAY_HOLD_SHORT, ATC_GROUND_TO_AIR)) {
1113 if (checkTransmissionState(8,8, current, now, MSG_SWITCH_TOWER_FREQUENCY, ATC_GROUND_TO_AIR)) {
1115 if (checkTransmissionState(9,9, current, now, MSG_ACKNOWLEDGE_SWITCH_TOWER_FREQUENCY, ATC_AIR_TO_GROUND)) {
1120 //current->setState(0);
1124 * Check whether situations occur where the current aircraft is waiting for itself
1125 * due to higher order interactions.
1126 * A 'circular' wait is a situation where a waits for b, b waits for c, and c waits
1127 * for a. Ideally each aircraft only waits for one other aircraft, so by tracing
1128 * through this list of waiting aircraft, we can check if we'd eventually end back
1129 * at the current aircraft.
1131 * Note that we should consider the situation where we are actually checking aircraft
1132 * d, which is waiting for aircraft a. d is not part of the loop, but is held back by
1133 * the looping aircraft. If we don't check for that, this function will get stuck into
1137 bool FGGroundNetwork::checkForCircularWaits(int id)
1139 //cerr << "Performing Wait check " << id << endl;
1141 TrafficVectorIterator current, other;
1142 TrafficVectorIterator i = activeTraffic.begin();
1143 int trafficSize = activeTraffic.size();
1145 while (i != activeTraffic.end()) {
1146 if (i->getId() == id) {
1154 if (i == activeTraffic.end() || (trafficSize == 0)) {
1155 SG_LOG(SG_GENERAL, SG_ALERT,
1156 "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits at " << SG_ORIGIN);
1160 target = current->getWaitsForId();
1161 //bool printed = false; // Note that this variable is for debugging purposes only.
1165 //cerr << "aircraft waits for user" << endl;
1170 while ((target > 0) && (target != id) && counter++ < trafficSize) {
1172 TrafficVectorIterator i = activeTraffic.begin();
1174 //while ((i->getId() != id) && i != activeTraffic.end())
1175 while (i != activeTraffic.end()) {
1176 if (i->getId() == target) {
1184 if (i == activeTraffic.end() || (trafficSize == 0)) {
1185 //cerr << "[Waiting for traffic at Runway: DONE] " << endl << endl;;
1186 // The target id is not found on the current network, which means it's at the tower
1187 //SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits");
1191 target = other->getWaitsForId();
1193 // actually this trap isn't as impossible as it first seemed:
1194 // the setWaitsForID(id) is set to current when the aircraft
1195 // is waiting for the user controlled aircraft.
1196 //if (current->getId() == other->getId()) {
1197 // cerr << "Caught the impossible trap" << endl;
1198 // cerr << "Current = " << current->getId() << endl;
1199 // cerr << "Other = " << other ->getId() << endl;
1200 // for (TrafficVectorIterator at = activeTraffic.begin();
1201 // at != activeTraffic.end();
1203 // cerr << "currently active aircraft : " << at->getCallSign() << " with Id " << at->getId() << " waits for " << at->getWaitsForId() << endl;
1206 if (current->getId() == other->getId())
1209 //cerr << current->getCallSign() << " (" << current->getId() << ") " << " -> " << other->getCallSign()
1210 // << " (" << other->getId() << "); " << endl;;
1220 // cerr << "[done] " << endl << endl;;
1222 SG_LOG(SG_GENERAL, SG_WARN,
1223 "Detected circular wait condition: Id = " << id <<
1224 "target = " << target);
1231 // Note that this function is probably obsolete...
1232 bool FGGroundNetwork::hasInstruction(int id)
1234 TrafficVectorIterator i = activeTraffic.begin();
1235 // Search search if the current id has an entry
1236 // This might be faster using a map instead of a vector, but let's start by taking a safe route
1237 if (activeTraffic.size()) {
1238 //while ((i->getId() != id) && i != activeTraffic.end()) {
1239 while (i != activeTraffic.end()) {
1240 if (i->getId() == id) {
1246 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
1247 SG_LOG(SG_GENERAL, SG_ALERT,
1248 "AI error: checking ATC instruction for aircraft without traffic record at " << SG_ORIGIN);
1250 return i->hasInstruction();
1255 FGATCInstruction FGGroundNetwork::getInstruction(int id)
1257 TrafficVectorIterator i = activeTraffic.begin();
1258 // Search search if the current id has an entry
1259 // This might be faster using a map instead of a vector, but let's start by taking a safe route
1260 if (activeTraffic.size()) {
1261 //while ((i->getId() != id) && i != activeTraffic.end()) {
1262 while (i != activeTraffic.end()) {
1263 if (i->getId() == id) {
1269 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
1270 SG_LOG(SG_GENERAL, SG_ALERT,
1271 "AI error: requesting ATC instruction for aircraft without traffic record at " << SG_ORIGIN);
1273 return i->getInstruction();
1275 return FGATCInstruction();
1278 // Note that this function is copied from simgear. for maintanance purposes, it's probabtl better to make a general function out of that.
1279 static void WorldCoordinate(osg::Matrix& obj_pos, double lat,
1280 double lon, double elev, double hdg, double slope)
1282 SGGeod geod = SGGeod::fromDegM(lon, lat, elev);
1283 obj_pos = makeZUpFrame(geod);
1284 // hdg is not a compass heading, but a counter-clockwise rotation
1285 // around the Z axis
1286 obj_pos.preMult(osg::Matrix::rotate(hdg * SGD_DEGREES_TO_RADIANS,
1288 obj_pos.preMult(osg::Matrix::rotate(slope * SGD_DEGREES_TO_RADIANS,
1295 void FGGroundNetwork::render(bool visible)
1298 SGMaterialLib *matlib = globals->get_matlib();
1301 globals->get_scenery()->get_scene_graph()->removeChild(group);
1302 //while (group->getNumChildren()) {
1303 // cerr << "Number of children: " << group->getNumChildren() << endl;
1304 //simgear::EffectGeode* geode = (simgear::EffectGeode*) group->getChild(0);
1305 //osg::MatrixTransform *obj_trans = (osg::MatrixTransform*) group->getChild(0);
1306 //geode->releaseGLObjects();
1307 //group->removeChild(geode);
1312 group = new osg::Group;
1313 FGScenery * local_scenery = globals->get_scenery();
1314 // double elevation_meters = 0.0;
1315 // double elevation_feet = 0.0;
1316 time_t now = time(NULL) + fgGetLong("/sim/time/warp");
1317 //for ( FGTaxiSegmentVectorIterator i = segments.begin(); i != segments.end(); i++) {
1319 for (TrafficVectorIterator i = activeTraffic.begin(); i != activeTraffic.end(); i++) {
1320 // Handle start point
1321 int pos = i->getCurrentPosition() - 1;
1324 SGGeod start(SGGeod::fromDeg((i->getLongitude()), (i->getLatitude())));
1325 SGGeod end (segments[pos]->getEnd()->geod());
1327 double length = SGGeodesy::distanceM(start, end);
1328 //heading = SGGeodesy::headingDeg(start->geod(), end->geod());
1330 double az2, heading; //, distanceM;
1331 SGGeodesy::inverse(start, end, heading, az2, length);
1332 double coveredDistance = length * 0.5;
1334 SGGeodesy::direct(start, heading, coveredDistance, center, az2);
1335 //cerr << "Active Aircraft : Centerpoint = (" << center.getLatitudeDeg() << ", " << center.getLongitudeDeg() << "). Heading = " << heading << endl;
1336 ///////////////////////////////////////////////////////////////////////////////
1337 // Make a helper function out of this
1338 osg::Matrix obj_pos;
1339 osg::MatrixTransform *obj_trans = new osg::MatrixTransform;
1340 obj_trans->setDataVariance(osg::Object::STATIC);
1341 // Experimental: Calculate slope here, based on length, and the individual elevations
1342 double elevationStart;
1343 if (isUserAircraft((i)->getAircraft())) {
1344 elevationStart = fgGetDouble("/position/ground-elev-m");
1346 elevationStart = ((i)->getAircraft()->_getAltitude());
1348 double elevationEnd = segments[pos]->getEnd()->getElevationM(parent->getElevation()*SG_FEET_TO_METER);
1349 //cerr << "Using elevation " << elevationEnd << endl;
1351 if ((elevationEnd == 0) || (elevationEnd = parent->getElevation())) {
1352 SGGeod center2 = end;
1353 center2.setElevationM(SG_MAX_ELEVATION_M);
1354 if (local_scenery->get_elevation_m( center2, elevationEnd, NULL )) {
1355 // elevation_feet = elevationEnd * SG_METER_TO_FEET + 0.5;
1356 //elevation_meters += 0.5;
1359 elevationEnd = parent->getElevation();
1361 segments[pos]->getEnd()->setElevation(elevationEnd);
1363 double elevationMean = (elevationStart + elevationEnd) / 2.0;
1364 double elevDiff = elevationEnd - elevationStart;
1366 double slope = atan2(elevDiff, length) * SGD_RADIANS_TO_DEGREES;
1368 //cerr << "1. Using mean elevation : " << elevationMean << " and " << slope << endl;
1370 WorldCoordinate( obj_pos, center.getLatitudeDeg(), center.getLongitudeDeg(), elevationMean+ 0.5, -(heading), slope );
1372 obj_trans->setMatrix( obj_pos );
1373 //osg::Vec3 center(0, 0, 0)
1375 float width = length /2.0;
1376 osg::Vec3 corner(-width, 0, 0.25f);
1377 osg::Vec3 widthVec(2*width + 1, 0, 0);
1378 osg::Vec3 heightVec(0, 1, 0);
1379 osg::Geometry* geometry;
1380 geometry = osg::createTexturedQuadGeometry(corner, widthVec, heightVec);
1381 simgear::EffectGeode* geode = new simgear::EffectGeode;
1382 geode->setName("test");
1383 geode->addDrawable(geometry);
1384 //osg::Node *custom_obj;
1386 if (segments[pos]->hasBlock(now)) {
1387 mat = matlib->find("UnidirectionalTaperRed");
1389 mat = matlib->find("UnidirectionalTaperGreen");
1392 geode->setEffect(mat->get_effect());
1393 obj_trans->addChild(geode);
1394 // wire as much of the scene graph together as we can
1395 //->addChild( obj_trans );
1396 group->addChild( obj_trans );
1397 /////////////////////////////////////////////////////////////////////
1399 //cerr << "BIG FAT WARNING: current position is here : " << pos << endl;
1401 for (intVecIterator j = (i)->getIntentions().begin(); j != (i)->getIntentions().end(); j++) {
1402 osg::Matrix obj_pos;
1405 osg::MatrixTransform *obj_trans = new osg::MatrixTransform;
1406 obj_trans->setDataVariance(osg::Object::STATIC);
1408 // Experimental: Calculate slope here, based on length, and the individual elevations
1409 double elevationStart = segments[k]->getStart()->getElevationM(parent->getElevation()*SG_FEET_TO_METER);
1410 double elevationEnd = segments[k]->getEnd ()->getElevationM(parent->getElevation()*SG_FEET_TO_METER);
1411 if ((elevationStart == 0) || (elevationStart == parent->getElevation())) {
1412 SGGeod center2 = segments[k]->getStart()->geod();
1413 center2.setElevationM(SG_MAX_ELEVATION_M);
1414 if (local_scenery->get_elevation_m( center2, elevationStart, NULL )) {
1415 // elevation_feet = elevationStart * SG_METER_TO_FEET + 0.5;
1416 //elevation_meters += 0.5;
1419 elevationStart = parent->getElevation();
1421 segments[k]->getStart()->setElevation(elevationStart);
1423 if ((elevationEnd == 0) || (elevationEnd == parent->getElevation())) {
1424 SGGeod center2 = segments[k]->getEnd()->geod();
1425 center2.setElevationM(SG_MAX_ELEVATION_M);
1426 if (local_scenery->get_elevation_m( center2, elevationEnd, NULL )) {
1427 // elevation_feet = elevationEnd * SG_METER_TO_FEET + 0.5;
1428 //elevation_meters += 0.5;
1431 elevationEnd = parent->getElevation();
1433 segments[k]->getEnd()->setElevation(elevationEnd);
1436 double elevationMean = (elevationStart + elevationEnd) / 2.0;
1437 double elevDiff = elevationEnd - elevationStart;
1438 double length = segments[k]->getLength();
1439 double slope = atan2(elevDiff, length) * SGD_RADIANS_TO_DEGREES;
1441 // cerr << "2. Using mean elevation : " << elevationMean << " and " << slope << endl;
1443 SGGeod segCenter = segments[k]->getCenter();
1444 WorldCoordinate( obj_pos, segCenter.getLatitudeDeg(), segCenter.getLongitudeDeg(), elevationMean+ 0.5, -(segments[k]->getHeading()), slope );
1446 obj_trans->setMatrix( obj_pos );
1447 //osg::Vec3 center(0, 0, 0)
1449 float width = segments[k]->getLength() /2.0;
1450 osg::Vec3 corner(-width, 0, 0.25f);
1451 osg::Vec3 widthVec(2*width + 1, 0, 0);
1452 osg::Vec3 heightVec(0, 1, 0);
1453 osg::Geometry* geometry;
1454 geometry = osg::createTexturedQuadGeometry(corner, widthVec, heightVec);
1455 simgear::EffectGeode* geode = new simgear::EffectGeode;
1456 geode->setName("test");
1457 geode->addDrawable(geometry);
1458 //osg::Node *custom_obj;
1460 if (segments[k]->hasBlock(now)) {
1461 mat = matlib->find("UnidirectionalTaperRed");
1463 mat = matlib->find("UnidirectionalTaperGreen");
1466 geode->setEffect(mat->get_effect());
1467 obj_trans->addChild(geode);
1468 // wire as much of the scene graph together as we can
1469 //->addChild( obj_trans );
1470 group->addChild( obj_trans );
1475 globals->get_scenery()->get_scene_graph()->addChild(group);
1479 string FGGroundNetwork::getName() {
1480 return string(parent->getId() + "-ground");
1483 void FGGroundNetwork::update(double dt)
1485 time_t now = time(NULL) + fgGetLong("/sim/time/warp");
1486 for (FGTaxiSegmentVectorIterator tsi = segments.begin(); tsi != segments.end(); tsi++) {
1487 (*tsi)->unblock(now);
1490 //sort(activeTraffic.begin(), activeTraffic.end(), compare_trafficrecords);
1491 // Handle traffic that is under ground control first; this way we'll prevent clutter at the gate areas.
1492 // Don't allow an aircraft to pushback when a taxiing aircraft is currently using part of the intended route.
1493 for (TrafficVectorIterator i = parent->getDynamics()->getStartupController()->getActiveTraffic().begin();
1494 i != parent->getDynamics()->getStartupController()->getActiveTraffic().end(); i++) {
1496 i->setPriority(priority++);
1497 // in meters per second;
1498 double vTaxi = (i->getAircraft()->getPerformance()->vTaxi() * SG_NM_TO_METER) / 3600;
1499 if (i->isActive(0)) {
1501 // Check for all active aircraft whether it's current pos segment is
1502 // an opposite of one of the departing aircraft's intentions
1503 for (TrafficVectorIterator j = activeTraffic.begin(); j != activeTraffic.end(); j++) {
1504 int pos = j->getCurrentPosition();
1506 FGTaxiSegment *seg = segments[pos-1]->opposite();
1508 int posReverse = seg->getIndex();
1509 for (intVecIterator k = i->getIntentions().begin(); k != i->getIntentions().end(); k++) {
1510 if ((*k) == posReverse) {
1512 segments[posReverse-1]->block(i->getId(), now, now);
1518 // if the current aircraft is still allowed to pushback, we can start reserving a route for if by blocking all the entry taxiways.
1519 if (i->pushBackAllowed()) {
1521 int pos = i->getCurrentPosition();
1523 FGTaxiSegment *seg = segments[pos-1];
1524 FGTaxiNode *node = seg->getEnd();
1525 length = seg->getLength();
1526 for (FGTaxiSegmentVectorIterator tsi = segments.begin(); tsi != segments.end(); tsi++) {
1527 if (((*tsi)->getEnd() == node) && ((*tsi) != seg)) {
1528 (*tsi)->block(i->getId(), now, now);
1532 for (intVecIterator j = i->getIntentions().begin(); j != i->getIntentions().end(); j++) {
1535 FGTaxiSegment *seg = segments[pos-1];
1536 FGTaxiNode *node = seg->getEnd();
1537 length += seg->getLength();
1538 time_t blockTime = now + (length / vTaxi);
1539 for (FGTaxiSegmentVectorIterator tsi = segments.begin(); tsi != segments.end(); tsi++) {
1540 if (((*tsi)->getEnd() == node) && ((*tsi) != seg)) {
1541 (*tsi)->block(i->getId(), blockTime-30, now);
1549 for (TrafficVectorIterator i = activeTraffic.begin(); i != activeTraffic.end(); i++) {
1551 double vTaxi = (i->getAircraft()->getPerformance()->vTaxi() * SG_NM_TO_METER) / 3600;
1552 i->setPriority(priority++);
1553 int pos = i->getCurrentPosition();
1555 length = segments[pos-1]->getLength();
1556 if (segments[pos-1]->hasBlock(now)) {
1557 //SG_LOG(SG_GENERAL, SG_ALERT, "Taxiway incursion for AI aircraft" << i->getAircraft()->getCallSign());
1562 for (ivi = i->getIntentions().begin(); ivi != i->getIntentions().end(); ivi++) {
1563 int segIndex = (*ivi);
1565 if (segments[segIndex-1]->hasBlock(now))
1569 //after this, ivi points just behind the last valid unblocked taxi segment.
1570 for (intVecIterator j = i->getIntentions().begin(); j != ivi; j++) {
1573 FGTaxiSegment *seg = segments[pos-1];
1574 FGTaxiNode *node = seg->getEnd();
1575 length += seg->getLength();
1576 for (FGTaxiSegmentVectorIterator tsi = segments.begin(); tsi != segments.end(); tsi++) {
1577 if (((*tsi)->getEnd() == node) && ((*tsi) != seg)) {
1578 time_t blockTime = now + (length / vTaxi);
1579 (*tsi)->block(i->getId(), blockTime - 30, now);