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.
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>
43 #include <Airports/simple.hxx>
44 #include <Airports/dynamics.hxx>
46 #include <AIModel/AIAircraft.hxx>
47 #include <AIModel/performancedata.hxx>
48 #include <AIModel/AIFlightPlan.hxx>
50 #include <ATC/atc_mgr.hxx>
52 #include <Scenery/scenery.hxx>
54 #include "groundnetwork.hxx"
58 /***************************************************************************
60 **************************************************************************/
62 void FGTaxiSegment::setStart(FGTaxiNodeVector * nodes)
64 FGTaxiNodeVectorIterator i = nodes->begin();
65 while (i != nodes->end()) {
66 //cerr << "Scanning start node index" << (*i)->getIndex() << endl;
67 if ((*i)->getIndex() == startNode) {
68 start = (*i)->getAddress();
69 (*i)->addSegment(this);
74 SG_LOG(SG_GENERAL, SG_ALERT,
75 "Could not find start node " << startNode << endl);
78 void FGTaxiSegment::setEnd(FGTaxiNodeVector * nodes)
80 FGTaxiNodeVectorIterator i = nodes->begin();
81 while (i != nodes->end()) {
82 //cerr << "Scanning end node index" << (*i)->getIndex() << endl;
83 if ((*i)->getIndex() == endNode) {
84 end = (*i)->getAddress();
89 SG_LOG(SG_GENERAL, SG_ALERT,
90 "Could not find end node " << endNode << endl);
95 // There is probably a computationally cheaper way of
97 void FGTaxiSegment::setDimensions(double elevation)
99 length = SGGeodesy::distanceM(start->getGeod(), end->getGeod());
100 //heading = SGGeodesy::headingDeg(start->getGeod(), end->getGeod());
102 double az2; //, distanceM;
103 SGGeodesy::inverse(start->getGeod(), end->getGeod(), heading, az2, length);
104 double coveredDistance = length * 0.5;
105 SGGeodesy::direct(start->getGeod(), heading, coveredDistance, center, az2);
106 //start->setElevation(elevation);
107 //end->setElevation(elevation);
108 //cerr << "Centerpoint = (" << center.getLatitudeDeg() << ", " << center.getLongitudeDeg() << "). Heading = " << heading << endl;
112 //void FGTaxiSegment::setCourseDiff(double crse)
114 // headingDiff = fabs(course - crse);
116 // if (headingDiff > 180)
117 // headingDiff = fabs(headingDiff - 360);
120 void FGTaxiSegment::block(int id, time_t blockTime, time_t now)
122 BlockListIterator i = blockTimes.begin();
123 while (i != blockTimes.end()) {
124 if (i->getId() == id)
128 if (i == blockTimes.end()) {
129 blockTimes.push_back(Block(id, blockTime, now));
130 sort(blockTimes.begin(), blockTimes.end());
132 i->updateTimeStamps(blockTime, now);
136 // The segment has a block if any of the block times listed in the block list is
137 // smaller than the current time.
138 bool FGTaxiSegment::hasBlock(time_t now)
140 for (BlockListIterator i = blockTimes.begin(); i != blockTimes.end(); i++) {
141 if (i->getBlockTime() < now)
147 void FGTaxiSegment::unblock(time_t now)
149 if (blockTimes.size()) {
150 BlockListIterator i = blockTimes.begin();
151 if (i->getTimeStamp() < (now - 30)) {
159 /***************************************************************************
161 **************************************************************************/
162 bool FGTaxiRoute::next(int *nde)
164 //for (intVecIterator i = nodes.begin(); i != nodes.end(); i++)
165 // cerr << "FGTaxiRoute contains : " << *(i) << endl;
166 //cerr << "Offset from end: " << nodes.end() - currNode << endl;
167 //if (currNode != nodes.end())
168 // cerr << "true" << endl;
170 // cerr << "false" << endl;
171 //if (nodes.size() != (routes.size()) +1)
172 // cerr << "ALERT: Misconfigured TaxiRoute : " << nodes.size() << " " << routes.size() << endl;
174 if (currNode == nodes.end())
177 if (currNode != nodes.begin()) // make sure route corresponds to the end node
183 bool FGTaxiRoute::next(int *nde, int *rte)
185 //for (intVecIterator i = nodes.begin(); i != nodes.end(); i++)
186 // cerr << "FGTaxiRoute contains : " << *(i) << endl;
187 //cerr << "Offset from end: " << nodes.end() - currNode << endl;
188 //if (currNode != nodes.end())
189 // cerr << "true" << endl;
191 // cerr << "false" << endl;
192 if (nodes.size() != (routes.size()) + 1) {
193 SG_LOG(SG_GENERAL, SG_ALERT,
194 "ALERT: Misconfigured TaxiRoute : " << nodes.
195 size() << " " << routes.size());
198 if (currNode == nodes.end())
201 //*rte = *(currRoute);
202 if (currNode != nodes.begin()) // Make sure route corresponds to the end node
207 // If currNode points to the first node, this means the aircraft is not on the taxi node
208 // yet. Make sure to return a unique identifyer in this situation though, because otherwise
209 // the speed adjust AI code may be unable to resolve whether two aircraft are on the same
210 // taxi route or not. the negative of the preceding route seems a logical choice, as it is
211 // unique for any starting location.
212 // Note that this is probably just a temporary fix until I get Parking / tower control working.
213 *rte = -1 * *(currRoute);
220 void FGTaxiRoute::rewind(int route)
226 if (!(next(&currPoint, &currRoute))) {
227 SG_LOG(SG_GENERAL, SG_ALERT,
228 "Error in rewinding TaxiRoute: current" << currRoute <<
231 } while (currRoute != route);
237 /***************************************************************************
239 **************************************************************************/
240 bool compare_nodes(FGTaxiNode * a, FGTaxiNode * b)
245 bool compare_segments(FGTaxiSegment * a, FGTaxiSegment * b)
250 bool compare_trafficrecords(FGTrafficRecord a, FGTrafficRecord b)
252 return (a.getIntentions().size() < b.getIntentions().size());
255 FGGroundNetwork::FGGroundNetwork() :
264 currTraffic = activeTraffic.begin();
267 networkInitialized = false;
271 FGGroundNetwork::~FGGroundNetwork()
273 // JMT 2012-09-8 - disabling the groundnet-caching as part of enabling the
274 // navcache. The problem isn't the NavCache - it's that for the past few years,
275 // we have not being running destructors on FGPositioned classes, and hence,
276 // not running this code.
277 // When I fix FGPositioned lifetimes (unloading-at-runtime support), this
278 // will need to be re-visited so it can run safely during shutdown.
280 //cerr << "Running Groundnetwork Destructor " << endl;
281 bool saveData = false;
283 if (fgGetBool("/sim/ai/groundnet-cache")) {
284 SGPath cacheData(globals->get_fg_home());
285 cacheData.append("ai");
286 string airport = parent->getId();
288 if ((airport) != "") {
290 ::snprintf(buffer, 128, "%c/%c/%c/",
291 airport[0], airport[1], airport[2]);
292 cacheData.append(buffer);
293 if (!cacheData.exists()) {
294 cacheData.create_dir(0777);
296 cacheData.append(airport + "-groundnet-cache.txt");
297 cachefile.open(cacheData.str().c_str());
301 cachefile << "[GroundNetcachedata:ref:2011:09:04]" << endl;
302 for (FGTaxiNodeVectorIterator node = nodes.begin();
303 node != nodes.end(); node++) {
305 cachefile << (*node)->getIndex () << " "
306 << (*node)->getElevationM (parent->getElevation()*SG_FEET_TO_METER) << " "
312 pushBackNodes.clear();
313 for (FGTaxiSegmentVectorIterator seg = segments.begin();
314 seg != segments.end(); seg++) {
324 void FGGroundNetwork::saveElevationCache() {
325 //cerr << "Running Groundnetwork Destructor " << endl;
326 bool saveData = false;
328 if (fgGetBool("/sim/ai/groundnet-cache")) {
329 SGPath cacheData(globals->get_fg_home());
330 cacheData.append("ai");
331 string airport = parent->getId();
333 if ((airport) != "") {
335 ::snprintf(buffer, 128, "%c/%c/%c/",
336 airport[0], airport[1], airport[2]);
337 cacheData.append(buffer);
338 if (!cacheData.exists()) {
339 cacheData.create_dir(0777);
341 cacheData.append(airport + "-groundnet-cache.txt");
342 cachefile.open(cacheData.str().c_str());
346 cachefile << "[GroundNetcachedata:ref:2011:09:04]" << endl;
347 for (FGTaxiNodeVectorIterator node = nodes.begin();
348 node != nodes.end(); node++) {
350 cachefile << (*node)->getIndex () << " "
351 << (*node)->getElevationM (parent->getElevation()*SG_FEET_TO_METER) << " "
360 void FGGroundNetwork::addSegment(const FGTaxiSegment & seg)
362 segments.push_back(new FGTaxiSegment(seg));
365 void FGGroundNetwork::addNode(const FGTaxiNode & node)
367 nodes.push_back(new FGTaxiNode(node));
370 void FGGroundNetwork::addNodes(FGParkingVec * parkings)
373 FGParkingVecIterator i = parkings->begin();
374 while (i != parkings->end()) {
375 n.setIndex(i->getIndex());
376 n.setLatitude(i->getLatitude());
377 n.setLongitude(i->getLongitude());
378 n.setElevation(parent->getElevation()*SG_FEET_TO_METER);
379 nodes.push_back(new FGTaxiNode(n));
387 void FGGroundNetwork::init()
389 if (networkInitialized) {
390 FGATCController::init();
391 //cerr << "FGground network already initialized" << endl;
397 sort(nodes.begin(), nodes.end(), compare_nodes);
398 //sort(segments.begin(), segments.end(), compare_segments());
399 FGTaxiSegmentVectorIterator i = segments.begin();
400 while (i != segments.end()) {
401 (*i)->setStart(&nodes);
402 (*i)->setEnd(&nodes);
403 (*i)->setDimensions(parent->getElevation() * SG_FEET_TO_METER);
404 (*i)->setIndex(index);
405 if ((*i)->isPushBack()) {
406 pushBackNodes.push_back((*i)->getEnd());
408 //SG_LOG(SG_GENERAL, SG_BULK, "initializing segment " << (*i)->getIndex() << endl);
409 //SG_LOG(SG_GENERAL, SG_BULK, "Track distance = " << (*i)->getLength() << endl);
410 //SG_LOG(SG_GENERAL, SG_BULK, "Track runs from " << (*i)->getStart()->getIndex() << " to "
411 // << (*i)->getEnd()->getIndex() << endl);
416 i = segments.begin();
417 while (i != segments.end()) {
418 FGTaxiSegmentVectorIterator j = (*i)->getEnd()->getBeginRoute();
419 while (j != (*i)->getEnd()->getEndRoute()) {
420 if ((*j)->getEnd()->getIndex() == (*i)->getStart()->getIndex()) {
421 // int start1 = (*i)->getStart()->getIndex();
422 // int end1 = (*i)->getEnd() ->getIndex();
423 // int start2 = (*j)->getStart()->getIndex();
424 // int end2 = (*j)->getEnd()->getIndex();
425 // int oppIndex = (*j)->getIndex();
426 //cerr << "Opposite of " << (*i)->getIndex() << " (" << start1 << "," << end1 << ") "
427 // << "happens to be " << oppIndex << " (" << start2 << "," << end2 << ") " << endl;
428 (*i)->setOpposite(*j);
435 //FGTaxiNodeVectorIterator j = nodes.begin();
436 //while (j != nodes.end()) {
437 // if ((*j)->getHoldPointType() == 3) {
438 // pushBackNodes.push_back((*j));
442 //cerr << "Done initializing ground network" << endl;
444 if (fgGetBool("/sim/ai/groundnet-cache")) {
445 SGPath cacheData(globals->get_fg_home());
446 cacheData.append("ai");
447 string airport = parent->getId();
449 if ((airport) != "") {
451 ::snprintf(buffer, 128, "%c/%c/%c/",
452 airport[0], airport[1], airport[2]);
453 cacheData.append(buffer);
454 if (!cacheData.exists()) {
455 cacheData.create_dir(0777);
459 cacheData.append(airport + "-groundnet-cache.txt");
460 if (cacheData.exists()) {
461 ifstream data(cacheData.c_str());
464 if (revisionStr != "[GroundNetcachedata:ref:2011:09:04]") {
465 SG_LOG(SG_GENERAL, SG_ALERT,"GroundNetwork Warning: discarding outdated cachefile " <<
466 cacheData.c_str() << " for Airport " << airport);
468 for (FGTaxiNodeVectorIterator i = nodes.begin();
471 (*i)->setElevation(parent->getElevation() * SG_FEET_TO_METER);
472 data >> index >> elev;
475 if (index != (*i)->getIndex()) {
476 SG_LOG(SG_GENERAL, SG_ALERT, "Index read from ground network cache at airport " << airport << " does not match index in the network itself");
478 (*i)->setElevation(elev);
485 //cerr << "Finished initializing " << parent->getId() << " groundnetwork " << endl;
486 networkInitialized = true;
489 int FGGroundNetwork::findNearestNode(const SGGeod & aGeod)
491 double minDist = HUGE_VAL;
494 for (FGTaxiNodeVectorIterator itr = nodes.begin(); itr != nodes.end();
496 double d = SGGeodesy::distanceM(aGeod, (*itr)->getGeod());
499 index = (*itr)->getIndex();
500 //cerr << "Minimum distance of " << minDist << " for index " << index << endl;
507 int FGGroundNetwork::findNearestNodeOnRunway(const SGGeod & aGeod)
509 double minDist = HUGE_VAL;
512 for (FGTaxiNodeVectorIterator itr = nodes.begin(); itr != nodes.end();
514 if (!((*itr)->getIsOnRunway())) {
517 double d = SGGeodesy::distanceM(aGeod, (*itr)->getGeod());
520 index = (*itr)->getIndex();
521 //cerr << "Minimum distance of " << minDist << " for index " << index << endl;
529 int FGGroundNetwork::findNearestNode(double lat, double lon)
531 return findNearestNode(SGGeod::fromDeg(lon, lat));
534 FGTaxiNode *FGGroundNetwork::findNode(unsigned idx)
536 for (FGTaxiNodeVectorIterator
538 itr != nodes.end(); itr++)
540 if (itr->getIndex() == idx)
541 return itr->getAddress();
544 if (idx < nodes.size())
545 return nodes[idx]->getAddress();
550 FGTaxiSegment *FGGroundNetwork::findSegment(unsigned idx)
552 for (FGTaxiSegmentVectorIterator
553 itr = segments.begin();
554 itr != segments.end(); itr++)
556 if (itr->getIndex() == idx)
557 return itr->getAddress();
560 if ((idx > 0) && (idx <= segments.size()))
561 return segments[idx - 1]->getAddress();
563 //cerr << "Alert: trying to find invalid segment " << idx << endl;
569 FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end,
572 //implements Dijkstra's algorithm to find shortest distance route from start to end
573 //taken from http://en.wikipedia.org/wiki/Dijkstra's_algorithm
575 //double INFINITE = 100000000000.0;
576 // initialize scoring values
577 int nParkings = parent->getDynamics()->getNrOfParkings();
578 FGTaxiNodeVector *currNodesSet;
580 currNodesSet = &nodes;
582 currNodesSet = &pushBackNodes;
585 for (FGTaxiNodeVectorIterator
586 itr = currNodesSet->begin(); itr != currNodesSet->end(); itr++) {
587 (*itr)->setPathScore(HUGE_VAL); //infinity by all practical means
588 (*itr)->setPreviousNode(0); //
589 (*itr)->setPreviousSeg(0); //
592 FGTaxiNode *firstNode = findNode(start);
595 SG_LOG(SG_GENERAL, SG_ALERT,
596 "Error in ground network. Failed to find first waypoint: " << start
597 << " at " << ((parent) ? parent->getId() : "<unknown>"));
598 return FGTaxiRoute();
600 firstNode->setPathScore(0);
602 FGTaxiNode *lastNode = findNode(end);
605 SG_LOG(SG_GENERAL, SG_ALERT,
606 "Error in ground network. Failed to find last waypoint: " << end
607 << " at " << ((parent) ? parent->getId() : "<unknown>"));
608 return FGTaxiRoute();
611 FGTaxiNodeVector unvisited(*currNodesSet); // working copy
613 while (!unvisited.empty()) {
614 FGTaxiNode *best = *(unvisited.begin());
615 for (FGTaxiNodeVectorIterator
616 itr = unvisited.begin(); itr != unvisited.end(); itr++) {
617 if ((*itr)->getPathScore() < best->getPathScore())
621 FGTaxiNodeVectorIterator newend =
622 remove(unvisited.begin(), unvisited.end(), best);
623 unvisited.erase(newend, unvisited.end());
625 if (best == lastNode) { // found route or best not connected
628 for (FGTaxiSegmentVectorIterator
629 seg = best->getBeginRoute();
630 seg != best->getEndRoute(); seg++) {
631 if (fullSearch || (*seg)->isPushBack()) {
632 FGTaxiNode *tgt = (*seg)->getEnd();
635 SG_LOG(SG_GENERAL, SG_ALERT,
636 "Error in ground network. Found empty segment "
637 << " at " << ((parent) ? parent->getId() : "<unknown>"));
638 return FGTaxiRoute();
641 best->getPathScore() + (*seg)->getLength() +
642 (*seg)->getPenalty(nParkings);
643 if (alt < tgt->getPathScore()) { // Relax (u,v)
644 tgt->setPathScore(alt);
645 tgt->setPreviousNode(best);
646 tgt->setPreviousSeg(*seg); //
649 // // cerr << "Skipping TaxiSegment " << (*seg)->getIndex() << endl;
655 if (lastNode->getPathScore() == HUGE_VAL) {
656 // no valid route found
658 SG_LOG(SG_GENERAL, SG_ALERT,
659 "Failed to find route from waypoint " << start << " to "
660 << end << " at " << parent->getId());
664 //exit(1); //TODO exit more gracefully, no need to stall the whole sim with broken GN's
666 // assemble route from backtrace information
667 intVec nodes, routes;
668 FGTaxiNode *bt = lastNode;
669 while (bt->getPreviousNode() != 0) {
670 nodes.push_back(bt->getIndex());
671 routes.push_back(bt->getPreviousSegment()->getIndex());
672 bt = bt->getPreviousNode();
674 nodes.push_back(start);
675 reverse(nodes.begin(), nodes.end());
676 reverse(routes.begin(), routes.end());
678 return FGTaxiRoute(nodes, routes, lastNode->getPathScore(), 0);
682 int FGTaxiSegment::getPenalty(int nGates)
685 if (end->getIndex() < nGates) {
688 if (end->getIsOnRunway()) { // For now. In future versions, need to find out whether runway is active.
694 /* ATC Related Functions */
696 void FGGroundNetwork::announcePosition(int id,
697 FGAIFlightPlan * intendedRoute,
698 int currentPosition, double lat,
699 double lon, double heading,
700 double speed, double alt,
701 double radius, int leg,
702 FGAIAircraft * aircraft)
705 TrafficVectorIterator i = activeTraffic.begin();
706 // Search search if the current id alread has an entry
707 // This might be faster using a map instead of a vector, but let's start by taking a safe route
708 if (activeTraffic.size()) {
709 //while ((i->getId() != id) && i != activeTraffic.end()) {
710 while (i != activeTraffic.end()) {
711 if (i->getId() == id) {
717 // Add a new TrafficRecord if no one exsists for this aircraft.
718 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
722 rec.setPositionAndIntentions(currentPosition, intendedRoute);
723 rec.setPositionAndHeading(lat, lon, heading, speed, alt);
724 rec.setRadius(radius); // only need to do this when creating the record.
725 rec.setAircraft(aircraft);
727 activeTraffic.push_front(rec);
729 activeTraffic.push_back(rec);
733 i->setPositionAndIntentions(currentPosition, intendedRoute);
734 i->setPositionAndHeading(lat, lon, heading, speed, alt);
739 void FGGroundNetwork::signOff(int id)
741 TrafficVectorIterator i = activeTraffic.begin();
742 // Search search if the current id alread has an entry
743 // This might be faster using a map instead of a vector, but let's start by taking a safe route
744 if (activeTraffic.size()) {
745 //while ((i->getId() != id) && i != activeTraffic.end()) {
746 while (i != activeTraffic.end()) {
747 if (i->getId() == id) {
753 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
754 SG_LOG(SG_GENERAL, SG_ALERT,
755 "AI error: Aircraft without traffic record is signing off at " << SG_ORIGIN);
757 i = activeTraffic.erase(i);
761 * The ground network can deal with the following states:
762 * 0 = Normal; no action required
763 * 1 = "Acknowledge "Hold position
764 * 2 = "Acknowledge "Resume taxi".
765 * 3 = "Issue TaxiClearance"
766 * 4 = Acknowledge Taxi Clearance"
767 * 5 = Post acknowlegde taxiclearance: Start taxiing
769 * 7 = Acknowledge report runway
770 * 8 = Switch tower frequency
771 * 9 = Acknowledge switch tower frequency
772 *************************************************************************************************************************/
773 bool FGGroundNetwork::checkTransmissionState(int minState, int maxState, TrafficVectorIterator i, time_t now, AtcMsgId msgId,
776 int state = i->getState();
777 if ((state >= minState) && (state <= maxState) && available) {
778 if ((msgDir == ATC_AIR_TO_GROUND) && isUserAircraft(i->getAircraft())) {
779 //cerr << "Checking state " << state << " for " << i->getAircraft()->getCallSign() << endl;
780 static SGPropertyNode_ptr trans_num = globals->get_props()->getNode("/sim/atc/transmission-num", true);
781 int n = trans_num->getIntValue();
783 trans_num->setIntValue(-1);
785 //cerr << "Selected transmission message " << n << endl;
786 //FGATCManager *atc = (FGATCManager*) globals->get_subsystem("atc");
787 FGATCDialogNew::instance()->removeEntry(1);
789 //cerr << "creating message for " << i->getAircraft()->getCallSign() << endl;
790 transmit(&(*i), &(*parent->getDynamics()), msgId, msgDir, false);
794 transmit(&(*i), &(*parent->getDynamics()), msgId, msgDir, true);
796 lastTransmission = now;
803 void FGGroundNetwork::updateAircraftInformation(int id, double lat, double lon,
804 double heading, double speed, double alt,
807 time_t currentTime = time(NULL);
808 if (nextSave < currentTime) {
809 saveElevationCache();
810 nextSave = currentTime + 100 + rand() % 200;
812 // Check whether aircraft are on hold due to a preceding pushback. If so, make sure to
813 // Transmit air-to-ground "Ready to taxi request:
814 // Transmit ground to air approval / hold
815 // Transmit confirmation ...
816 // Probably use a status mechanism similar to the Engine start procedure in the startup controller.
819 TrafficVectorIterator i = activeTraffic.begin();
820 // Search search if the current id has an entry
821 // This might be faster using a map instead of a vector, but let's start by taking a safe route
822 TrafficVectorIterator current, closest;
823 if (activeTraffic.size()) {
824 //while ((i->getId() != id) && i != activeTraffic.end()) {
825 while (i != activeTraffic.end()) {
826 if (i->getId() == id) {
832 // update position of the current aircraft
833 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
834 SG_LOG(SG_GENERAL, SG_ALERT,
835 "AI error: updating aircraft without traffic record at " << SG_ORIGIN);
837 i->setPositionAndHeading(lat, lon, heading, speed, alt);
843 // Update every three secs, but add some randomness
844 // to prevent all IA objects doing this in synchrony
845 //if (getDt() < (3.0) + (rand() % 10))
849 current->clearResolveCircularWait();
850 current->setWaitsForId(0);
851 checkSpeedAdjustment(id, lat, lon, heading, speed, alt);
852 bool needsTaxiClearance = current->getAircraft()->getTaxiClearanceRequest();
853 if (!needsTaxiClearance) {
854 checkHoldPosition(id, lat, lon, heading, speed, alt);
855 //if (checkForCircularWaits(id)) {
856 // i->setResolveCircularWait();
859 current->setHoldPosition(true);
860 int state = current->getState();
861 time_t now = time(NULL) + fgGetLong("/sim/time/warp");
862 if ((now - lastTransmission) > 15) {
865 if (checkTransmissionState(0,2, current, now, MSG_REQUEST_TAXI_CLEARANCE, ATC_AIR_TO_GROUND)) {
866 current->setState(3);
868 if (checkTransmissionState(3,3, current, now, MSG_ISSUE_TAXI_CLEARANCE, ATC_GROUND_TO_AIR)) {
869 current->setState(4);
871 if (checkTransmissionState(4,4, current, now, MSG_ACKNOWLEDGE_TAXI_CLEARANCE, ATC_AIR_TO_GROUND)) {
872 current->setState(5);
874 if ((state == 5) && available) {
875 current->setState(0);
876 current->getAircraft()->setTaxiClearanceRequest(false);
877 current->setHoldPosition(false);
885 Scan for a speed adjustment change. Find the nearest aircraft that is in front
886 and adjust speed when we get too close. Only do this when current position and/or
887 intentions of the current aircraft match current taxiroute position of the proximate
888 aircraft. For traffic that is on other routes we need to issue a "HOLD Position"
889 instruction. See below for the hold position instruction.
891 Note that there currently still is one flaw in the logic that needs to be addressed.
892 There can be situations where one aircraft is in front of the current aircraft, on a separate
893 route, but really close after an intersection coming off the current route. This
894 aircraft is still close enough to block the current aircraft. This situation is currently
895 not addressed yet, but should be.
898 void FGGroundNetwork::checkSpeedAdjustment(int id, double lat,
899 double lon, double heading,
900 double speed, double alt)
903 TrafficVectorIterator current, closest, closestOnNetwork;
904 TrafficVectorIterator i = activeTraffic.begin();
905 bool otherReasonToSlowDown = false;
906 // bool previousInstruction;
907 if (activeTraffic.size()) {
908 //while ((i->getId() != id) && (i != activeTraffic.end()))
909 while (i != activeTraffic.end()) {
910 if (i->getId() == id) {
918 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
919 SG_LOG(SG_GENERAL, SG_ALERT,
920 "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkSpeedAdjustment at " << SG_ORIGIN);
925 // previousInstruction = current->getSpeedAdjustment();
926 double mindist = HUGE_VAL;
927 if (activeTraffic.size()) {
928 double course, dist, bearing, az2; // minbearing,
929 SGGeod curr(SGGeod::fromDegM(lon, lat, alt));
930 //TrafficVector iterator closest;
932 closestOnNetwork = current;
933 for (TrafficVectorIterator i = activeTraffic.begin();
934 i != activeTraffic.end(); i++) {
939 SGGeod other(SGGeod::fromDegM(i->getLongitude(),
942 SGGeodesy::inverse(curr, other, course, az2, dist);
943 bearing = fabs(heading - course);
945 bearing = 360 - bearing;
946 if ((dist < mindist) && (bearing < 60.0)) {
949 closestOnNetwork = i;
950 // minbearing = bearing;
954 //Check traffic at the tower controller
955 if (towerController->hasActiveTraffic()) {
956 for (TrafficVectorIterator i =
957 towerController->getActiveTraffic().begin();
958 i != towerController->getActiveTraffic().end(); i++) {
959 //cerr << "Comparing " << current->getId() << " and " << i->getId() << endl;
960 SGGeod other(SGGeod::fromDegM(i->getLongitude(),
963 SGGeodesy::inverse(curr, other, course, az2, dist);
964 bearing = fabs(heading - course);
966 bearing = 360 - bearing;
967 if ((dist < mindist) && (bearing < 60.0)) {
968 //cerr << "Current aircraft " << current->getAircraft()->getTrafficRef()->getCallSign()
969 // << " is closest to " << i->getAircraft()->getTrafficRef()->getCallSign()
970 // << ", which has status " << i->getAircraft()->isScheduledForTakeoff()
974 // minbearing = bearing;
975 otherReasonToSlowDown = true;
979 // Finally, check UserPosition
980 // Note, as of 2011-08-01, this should no longer be necessecary.
982 double userLatitude = fgGetDouble("/position/latitude-deg");
983 double userLongitude = fgGetDouble("/position/longitude-deg");
984 SGGeod user(SGGeod::fromDeg(userLongitude, userLatitude));
985 SGGeodesy::inverse(curr, user, course, az2, dist);
987 bearing = fabs(heading - course);
989 bearing = 360 - bearing;
990 if ((dist < mindist) && (bearing < 60.0)) {
993 minbearing = bearing;
994 otherReasonToSlowDown = true;
997 current->clearSpeedAdjustment();
998 bool needBraking = false;
999 if (current->checkPositionAndIntentions(*closest)
1000 || otherReasonToSlowDown) {
1001 double maxAllowableDistance =
1002 (1.1 * current->getRadius()) +
1003 (1.1 * closest->getRadius());
1004 if (mindist < 2 * maxAllowableDistance) {
1005 if (current->getId() == closest->getWaitsForId())
1008 current->setWaitsForId(closest->getId());
1009 if (closest->getId() != current->getId()) {
1010 current->setSpeedAdjustment(closest->getSpeed() *
1015 // closest->getAircraft()->getTakeOffStatus() &&
1016 // (current->getAircraft()->getTrafficRef()->getDepartureAirport() == closest->getAircraft()->getTrafficRef()->getDepartureAirport()) &&
1017 // (current->getAircraft()->GetFlightPlan()->getRunway() == closest->getAircraft()->GetFlightPlan()->getRunway())
1019 // current->getAircraft()->scheduleForATCTowerDepartureControl(1);
1021 current->setSpeedAdjustment(0); // This can only happen when the user aircraft is the one closest
1023 if (mindist < maxAllowableDistance) {
1024 //double newSpeed = (maxAllowableDistance-mindist);
1025 //current->setSpeedAdjustment(newSpeed);
1026 //if (mindist < 0.5* maxAllowableDistance)
1028 current->setSpeedAdjustment(0);
1033 if ((closest->getId() == closestOnNetwork->getId()) && (current->getPriority() < closest->getPriority()) && needBraking) {
1034 swap(current, closest);
1040 Check for "Hold position instruction".
1041 The hold position should be issued under the following conditions:
1042 1) For aircraft entering or crossing a runway with active traffic on it, or landing aircraft near it
1043 2) For taxiing aircraft that use one taxiway in opposite directions
1044 3) For crossing or merging taxiroutes.
1047 void FGGroundNetwork::checkHoldPosition(int id, double lat,
1048 double lon, double heading,
1049 double speed, double alt)
1051 TrafficVectorIterator current;
1052 TrafficVectorIterator i = activeTraffic.begin();
1053 if (activeTraffic.size()) {
1054 //while ((i->getId() != id) && i != activeTraffic.end())
1055 while (i != activeTraffic.end()) {
1056 if (i->getId() == id) {
1064 time_t now = time(NULL) + fgGetLong("/sim/time/warp");
1065 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
1066 SG_LOG(SG_GENERAL, SG_ALERT,
1067 "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkHoldPosition at " << SG_ORIGIN);
1071 if (current->getAircraft()->getTakeOffStatus() == 1) {
1072 current->setHoldPosition(true);
1075 if (current->getAircraft()->getTakeOffStatus() == 2) {
1076 //cerr << current->getAircraft()->getCallSign() << ". Taxi in position and hold" << endl;
1077 current->setHoldPosition(false);
1078 current->clearSpeedAdjustment();
1081 bool origStatus = current->hasHoldPosition();
1082 current->setHoldPosition(false);
1083 SGGeod curr(SGGeod::fromDegM(lon, lat, alt));
1084 int currentRoute = i->getCurrentPosition();
1086 if (i->getIntentions().size()) {
1087 nextRoute = (*(i->getIntentions().begin()));
1091 if (currentRoute > 0) {
1092 FGTaxiSegment *tx = findSegment(currentRoute);
1095 nx = findSegment(nextRoute);
1099 //if (tx->hasBlock(now) || nx->hasBlock(now) ) {
1100 // current->setHoldPosition(true);
1102 SGGeod start(SGGeod::fromDeg((i->getLongitude()), (i->getLatitude())));
1103 SGGeod end (SGGeod::fromDeg(nx->getStart()->getLongitude(), nx->getStart()->getLatitude()));
1105 double distance = SGGeodesy::distanceM(start, end);
1106 if (nx->hasBlock(now) && (distance < i->getRadius() * 4)) {
1107 current->setHoldPosition(true);
1109 intVecIterator ivi = i->getIntentions().begin();
1110 while (ivi != i->getIntentions().end()) {
1112 distance += segments[(*ivi)-1]->getLength();
1113 if ((segments[(*ivi)-1]->hasBlock(now)) && (distance < i->getRadius() * 4)) {
1114 current->setHoldPosition(true);
1122 bool currStatus = current->hasHoldPosition();
1123 current->setHoldPosition(origStatus);
1124 // Either a Hold Position or a resume taxi transmission has been issued
1125 if ((now - lastTransmission) > 2) {
1128 if (current->getState() == 0) {
1129 if ((origStatus != currStatus) && available) {
1130 //cerr << "Issueing hold short instrudtion " << currStatus << " " << available << endl;
1131 if (currStatus == true) { // No has a hold short instruction
1132 transmit(&(*current), &(*parent->getDynamics()), MSG_HOLD_POSITION, ATC_GROUND_TO_AIR, true);
1133 //cerr << "Transmittin hold short instrudtion " << currStatus << " " << available << endl;
1134 current->setState(1);
1136 transmit(&(*current), &(*parent->getDynamics()), MSG_RESUME_TAXI, ATC_GROUND_TO_AIR, true);
1137 //cerr << "Transmittig resume instrudtion " << currStatus << " " << available << endl;
1138 current->setState(2);
1140 lastTransmission = now;
1142 // Don't act on the changed instruction until the transmission is confirmed
1143 // So set back to original status
1144 //cerr << "Current state " << current->getState() << endl;
1148 // 6 = Report runway
1149 // 7 = Acknowledge report runway
1150 // 8 = Switch tower frequency
1151 //9 = Acknowledge switch tower frequency
1153 //int state = current->getState();
1154 if (checkTransmissionState(1,1, current, now, MSG_ACKNOWLEDGE_HOLD_POSITION, ATC_AIR_TO_GROUND)) {
1155 current->setState(0);
1156 current->setHoldPosition(true);
1158 if (checkTransmissionState(2,2, current, now, MSG_ACKNOWLEDGE_RESUME_TAXI, ATC_AIR_TO_GROUND)) {
1159 current->setState(0);
1160 current->setHoldPosition(false);
1162 if (current->getAircraft()->getTakeOffStatus() && (current->getState() == 0)) {
1163 //cerr << "Scheduling " << current->getAircraft()->getCallSign() << " for hold short" << endl;
1164 current->setState(6);
1166 if (checkTransmissionState(6,6, current, now, MSG_REPORT_RUNWAY_HOLD_SHORT, ATC_AIR_TO_GROUND)) {
1168 if (checkTransmissionState(7,7, current, now, MSG_ACKNOWLEDGE_REPORT_RUNWAY_HOLD_SHORT, ATC_GROUND_TO_AIR)) {
1170 if (checkTransmissionState(8,8, current, now, MSG_SWITCH_TOWER_FREQUENCY, ATC_GROUND_TO_AIR)) {
1172 if (checkTransmissionState(9,9, current, now, MSG_ACKNOWLEDGE_SWITCH_TOWER_FREQUENCY, ATC_AIR_TO_GROUND)) {
1177 //current->setState(0);
1181 * Check whether situations occur where the current aircraft is waiting for itself
1182 * due to higher order interactions.
1183 * A 'circular' wait is a situation where a waits for b, b waits for c, and c waits
1184 * for a. Ideally each aircraft only waits for one other aircraft, so by tracing
1185 * through this list of waiting aircraft, we can check if we'd eventually end back
1186 * at the current aircraft.
1188 * Note that we should consider the situation where we are actually checking aircraft
1189 * d, which is waiting for aircraft a. d is not part of the loop, but is held back by
1190 * the looping aircraft. If we don't check for that, this function will get stuck into
1194 bool FGGroundNetwork::checkForCircularWaits(int id)
1196 //cerr << "Performing Wait check " << id << endl;
1198 TrafficVectorIterator current, other;
1199 TrafficVectorIterator i = activeTraffic.begin();
1200 int trafficSize = activeTraffic.size();
1202 while (i != activeTraffic.end()) {
1203 if (i->getId() == id) {
1211 if (i == activeTraffic.end() || (trafficSize == 0)) {
1212 SG_LOG(SG_GENERAL, SG_ALERT,
1213 "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits at " << SG_ORIGIN);
1217 target = current->getWaitsForId();
1218 //bool printed = false; // Note that this variable is for debugging purposes only.
1222 //cerr << "aircraft waits for user" << endl;
1227 while ((target > 0) && (target != id) && counter++ < trafficSize) {
1229 TrafficVectorIterator i = activeTraffic.begin();
1231 //while ((i->getId() != id) && i != activeTraffic.end())
1232 while (i != activeTraffic.end()) {
1233 if (i->getId() == target) {
1241 if (i == activeTraffic.end() || (trafficSize == 0)) {
1242 //cerr << "[Waiting for traffic at Runway: DONE] " << endl << endl;;
1243 // The target id is not found on the current network, which means it's at the tower
1244 //SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits");
1248 target = other->getWaitsForId();
1250 // actually this trap isn't as impossible as it first seemed:
1251 // the setWaitsForID(id) is set to current when the aircraft
1252 // is waiting for the user controlled aircraft.
1253 //if (current->getId() == other->getId()) {
1254 // cerr << "Caught the impossible trap" << endl;
1255 // cerr << "Current = " << current->getId() << endl;
1256 // cerr << "Other = " << other ->getId() << endl;
1257 // for (TrafficVectorIterator at = activeTraffic.begin();
1258 // at != activeTraffic.end();
1260 // cerr << "currently active aircraft : " << at->getCallSign() << " with Id " << at->getId() << " waits for " << at->getWaitsForId() << endl;
1263 if (current->getId() == other->getId())
1266 //cerr << current->getCallSign() << " (" << current->getId() << ") " << " -> " << other->getCallSign()
1267 // << " (" << other->getId() << "); " << endl;;
1277 // cerr << "[done] " << endl << endl;;
1279 SG_LOG(SG_GENERAL, SG_WARN,
1280 "Detected circular wait condition: Id = " << id <<
1281 "target = " << target);
1288 // Note that this function is probably obsolete...
1289 bool FGGroundNetwork::hasInstruction(int id)
1291 TrafficVectorIterator i = activeTraffic.begin();
1292 // Search search if the current id has an entry
1293 // This might be faster using a map instead of a vector, but let's start by taking a safe route
1294 if (activeTraffic.size()) {
1295 //while ((i->getId() != id) && i != activeTraffic.end()) {
1296 while (i != activeTraffic.end()) {
1297 if (i->getId() == id) {
1303 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
1304 SG_LOG(SG_GENERAL, SG_ALERT,
1305 "AI error: checking ATC instruction for aircraft without traffic record at " << SG_ORIGIN);
1307 return i->hasInstruction();
1312 FGATCInstruction FGGroundNetwork::getInstruction(int id)
1314 TrafficVectorIterator i = activeTraffic.begin();
1315 // Search search if the current id has an entry
1316 // This might be faster using a map instead of a vector, but let's start by taking a safe route
1317 if (activeTraffic.size()) {
1318 //while ((i->getId() != id) && i != activeTraffic.end()) {
1319 while (i != activeTraffic.end()) {
1320 if (i->getId() == id) {
1326 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
1327 SG_LOG(SG_GENERAL, SG_ALERT,
1328 "AI error: requesting ATC instruction for aircraft without traffic record at " << SG_ORIGIN);
1330 return i->getInstruction();
1332 return FGATCInstruction();
1335 // Note that this function is copied from simgear. for maintanance purposes, it's probabtl better to make a general function out of that.
1336 static void WorldCoordinate(osg::Matrix& obj_pos, double lat,
1337 double lon, double elev, double hdg, double slope)
1339 SGGeod geod = SGGeod::fromDegM(lon, lat, elev);
1340 obj_pos = makeZUpFrame(geod);
1341 // hdg is not a compass heading, but a counter-clockwise rotation
1342 // around the Z axis
1343 obj_pos.preMult(osg::Matrix::rotate(hdg * SGD_DEGREES_TO_RADIANS,
1345 obj_pos.preMult(osg::Matrix::rotate(slope * SGD_DEGREES_TO_RADIANS,
1352 void FGGroundNetwork::render(bool visible)
1355 SGMaterialLib *matlib = globals->get_matlib();
1358 globals->get_scenery()->get_scene_graph()->removeChild(group);
1359 //while (group->getNumChildren()) {
1360 // cerr << "Number of children: " << group->getNumChildren() << endl;
1361 //simgear::EffectGeode* geode = (simgear::EffectGeode*) group->getChild(0);
1362 //osg::MatrixTransform *obj_trans = (osg::MatrixTransform*) group->getChild(0);
1363 //geode->releaseGLObjects();
1364 //group->removeChild(geode);
1369 group = new osg::Group;
1370 FGScenery * local_scenery = globals->get_scenery();
1371 // double elevation_meters = 0.0;
1372 // double elevation_feet = 0.0;
1373 time_t now = time(NULL) + fgGetLong("/sim/time/warp");
1374 //for ( FGTaxiSegmentVectorIterator i = segments.begin(); i != segments.end(); i++) {
1376 for (TrafficVectorIterator i = activeTraffic.begin(); i != activeTraffic.end(); i++) {
1377 // Handle start point
1378 int pos = i->getCurrentPosition() - 1;
1381 SGGeod start(SGGeod::fromDeg((i->getLongitude()), (i->getLatitude())));
1382 SGGeod end (SGGeod::fromDeg(segments[pos]->getEnd()->getLongitude(), segments[pos]->getEnd()->getLatitude()));
1384 double length = SGGeodesy::distanceM(start, end);
1385 //heading = SGGeodesy::headingDeg(start->getGeod(), end->getGeod());
1387 double az2, heading; //, distanceM;
1388 SGGeodesy::inverse(start, end, heading, az2, length);
1389 double coveredDistance = length * 0.5;
1391 SGGeodesy::direct(start, heading, coveredDistance, center, az2);
1392 //cerr << "Active Aircraft : Centerpoint = (" << center.getLatitudeDeg() << ", " << center.getLongitudeDeg() << "). Heading = " << heading << endl;
1393 ///////////////////////////////////////////////////////////////////////////////
1394 // Make a helper function out of this
1395 osg::Matrix obj_pos;
1396 osg::MatrixTransform *obj_trans = new osg::MatrixTransform;
1397 obj_trans->setDataVariance(osg::Object::STATIC);
1398 // Experimental: Calculate slope here, based on length, and the individual elevations
1399 double elevationStart;
1400 if (isUserAircraft((i)->getAircraft())) {
1401 elevationStart = fgGetDouble("/position/ground-elev-m");
1403 elevationStart = ((i)->getAircraft()->_getAltitude());
1405 double elevationEnd = segments[pos]->getEnd()->getElevationM(parent->getElevation()*SG_FEET_TO_METER);
1406 //cerr << "Using elevation " << elevationEnd << endl;
1408 if ((elevationEnd == 0) || (elevationEnd = parent->getElevation())) {
1409 SGGeod center2 = end;
1410 center2.setElevationM(SG_MAX_ELEVATION_M);
1411 if (local_scenery->get_elevation_m( center2, elevationEnd, NULL )) {
1412 // elevation_feet = elevationEnd * SG_METER_TO_FEET + 0.5;
1413 //elevation_meters += 0.5;
1416 elevationEnd = parent->getElevation();
1418 segments[pos]->getEnd()->setElevation(elevationEnd);
1420 double elevationMean = (elevationStart + elevationEnd) / 2.0;
1421 double elevDiff = elevationEnd - elevationStart;
1423 double slope = atan2(elevDiff, length) * SGD_RADIANS_TO_DEGREES;
1425 //cerr << "1. Using mean elevation : " << elevationMean << " and " << slope << endl;
1427 WorldCoordinate( obj_pos, center.getLatitudeDeg(), center.getLongitudeDeg(), elevationMean+ 0.5, -(heading), slope );
1429 obj_trans->setMatrix( obj_pos );
1430 //osg::Vec3 center(0, 0, 0)
1432 float width = length /2.0;
1433 osg::Vec3 corner(-width, 0, 0.25f);
1434 osg::Vec3 widthVec(2*width + 1, 0, 0);
1435 osg::Vec3 heightVec(0, 1, 0);
1436 osg::Geometry* geometry;
1437 geometry = osg::createTexturedQuadGeometry(corner, widthVec, heightVec);
1438 simgear::EffectGeode* geode = new simgear::EffectGeode;
1439 geode->setName("test");
1440 geode->addDrawable(geometry);
1441 //osg::Node *custom_obj;
1443 if (segments[pos]->hasBlock(now)) {
1444 mat = matlib->find("UnidirectionalTaperRed");
1446 mat = matlib->find("UnidirectionalTaperGreen");
1449 geode->setEffect(mat->get_effect());
1450 obj_trans->addChild(geode);
1451 // wire as much of the scene graph together as we can
1452 //->addChild( obj_trans );
1453 group->addChild( obj_trans );
1454 /////////////////////////////////////////////////////////////////////
1456 //cerr << "BIG FAT WARNING: current position is here : " << pos << endl;
1458 for (intVecIterator j = (i)->getIntentions().begin(); j != (i)->getIntentions().end(); j++) {
1459 osg::Matrix obj_pos;
1462 osg::MatrixTransform *obj_trans = new osg::MatrixTransform;
1463 obj_trans->setDataVariance(osg::Object::STATIC);
1465 // Experimental: Calculate slope here, based on length, and the individual elevations
1466 double elevationStart = segments[k]->getStart()->getElevationM(parent->getElevation()*SG_FEET_TO_METER);
1467 double elevationEnd = segments[k]->getEnd ()->getElevationM(parent->getElevation()*SG_FEET_TO_METER);
1468 if ((elevationStart == 0) || (elevationStart == parent->getElevation())) {
1469 SGGeod center2 = segments[k]->getStart()->getGeod();
1470 center2.setElevationM(SG_MAX_ELEVATION_M);
1471 if (local_scenery->get_elevation_m( center2, elevationStart, NULL )) {
1472 // elevation_feet = elevationStart * SG_METER_TO_FEET + 0.5;
1473 //elevation_meters += 0.5;
1476 elevationStart = parent->getElevation();
1478 segments[k]->getStart()->setElevation(elevationStart);
1480 if ((elevationEnd == 0) || (elevationEnd == parent->getElevation())) {
1481 SGGeod center2 = segments[k]->getEnd()->getGeod();
1482 center2.setElevationM(SG_MAX_ELEVATION_M);
1483 if (local_scenery->get_elevation_m( center2, elevationEnd, NULL )) {
1484 // elevation_feet = elevationEnd * SG_METER_TO_FEET + 0.5;
1485 //elevation_meters += 0.5;
1488 elevationEnd = parent->getElevation();
1490 segments[k]->getEnd()->setElevation(elevationEnd);
1493 double elevationMean = (elevationStart + elevationEnd) / 2.0;
1494 double elevDiff = elevationEnd - elevationStart;
1495 double length = segments[k]->getLength();
1496 double slope = atan2(elevDiff, length) * SGD_RADIANS_TO_DEGREES;
1498 // cerr << "2. Using mean elevation : " << elevationMean << " and " << slope << endl;
1501 WorldCoordinate( obj_pos, segments[k]->getLatitude(), segments[k]->getLongitude(), elevationMean+ 0.5, -(segments[k]->getHeading()), slope );
1503 obj_trans->setMatrix( obj_pos );
1504 //osg::Vec3 center(0, 0, 0)
1506 float width = segments[k]->getLength() /2.0;
1507 osg::Vec3 corner(-width, 0, 0.25f);
1508 osg::Vec3 widthVec(2*width + 1, 0, 0);
1509 osg::Vec3 heightVec(0, 1, 0);
1510 osg::Geometry* geometry;
1511 geometry = osg::createTexturedQuadGeometry(corner, widthVec, heightVec);
1512 simgear::EffectGeode* geode = new simgear::EffectGeode;
1513 geode->setName("test");
1514 geode->addDrawable(geometry);
1515 //osg::Node *custom_obj;
1517 if (segments[k]->hasBlock(now)) {
1518 mat = matlib->find("UnidirectionalTaperRed");
1520 mat = matlib->find("UnidirectionalTaperGreen");
1523 geode->setEffect(mat->get_effect());
1524 obj_trans->addChild(geode);
1525 // wire as much of the scene graph together as we can
1526 //->addChild( obj_trans );
1527 group->addChild( obj_trans );
1532 globals->get_scenery()->get_scene_graph()->addChild(group);
1536 string FGGroundNetwork::getName() {
1537 return string(parent->getId() + "-ground");
1540 void FGGroundNetwork::update(double dt)
1542 time_t now = time(NULL) + fgGetLong("/sim/time/warp");
1543 for (FGTaxiSegmentVectorIterator tsi = segments.begin(); tsi != segments.end(); tsi++) {
1544 (*tsi)->unblock(now);
1547 //sort(activeTraffic.begin(), activeTraffic.end(), compare_trafficrecords);
1548 // Handle traffic that is under ground control first; this way we'll prevent clutter at the gate areas.
1549 // Don't allow an aircraft to pushback when a taxiing aircraft is currently using part of the intended route.
1550 for (TrafficVectorIterator i = parent->getDynamics()->getStartupController()->getActiveTraffic().begin();
1551 i != parent->getDynamics()->getStartupController()->getActiveTraffic().end(); i++) {
1553 i->setPriority(priority++);
1554 // in meters per second;
1555 double vTaxi = (i->getAircraft()->getPerformance()->vTaxi() * SG_NM_TO_METER) / 3600;
1556 if (i->isActive(0)) {
1558 // Check for all active aircraft whether it's current pos segment is
1559 // an opposite of one of the departing aircraft's intentions
1560 for (TrafficVectorIterator j = activeTraffic.begin(); j != activeTraffic.end(); j++) {
1561 int pos = j->getCurrentPosition();
1563 FGTaxiSegment *seg = segments[pos-1]->opposite();
1565 int posReverse = seg->getIndex();
1566 for (intVecIterator k = i->getIntentions().begin(); k != i->getIntentions().end(); k++) {
1567 if ((*k) == posReverse) {
1569 segments[posReverse-1]->block(i->getId(), now, now);
1575 // if the current aircraft is still allowed to pushback, we can start reserving a route for if by blocking all the entry taxiways.
1576 if (i->pushBackAllowed()) {
1578 int pos = i->getCurrentPosition();
1580 FGTaxiSegment *seg = segments[pos-1];
1581 FGTaxiNode *node = seg->getEnd();
1582 length = seg->getLength();
1583 for (FGTaxiSegmentVectorIterator tsi = segments.begin(); tsi != segments.end(); tsi++) {
1584 if (((*tsi)->getEnd() == node) && ((*tsi) != seg)) {
1585 (*tsi)->block(i->getId(), now, now);
1589 for (intVecIterator j = i->getIntentions().begin(); j != i->getIntentions().end(); j++) {
1592 FGTaxiSegment *seg = segments[pos-1];
1593 FGTaxiNode *node = seg->getEnd();
1594 length += seg->getLength();
1595 time_t blockTime = now + (length / vTaxi);
1596 for (FGTaxiSegmentVectorIterator tsi = segments.begin(); tsi != segments.end(); tsi++) {
1597 if (((*tsi)->getEnd() == node) && ((*tsi) != seg)) {
1598 (*tsi)->block(i->getId(), blockTime-30, now);
1606 for (TrafficVectorIterator i = activeTraffic.begin(); i != activeTraffic.end(); i++) {
1608 double vTaxi = (i->getAircraft()->getPerformance()->vTaxi() * SG_NM_TO_METER) / 3600;
1609 i->setPriority(priority++);
1610 int pos = i->getCurrentPosition();
1612 length = segments[pos-1]->getLength();
1613 if (segments[pos-1]->hasBlock(now)) {
1614 //SG_LOG(SG_GENERAL, SG_ALERT, "Taxiway incursion for AI aircraft" << i->getAircraft()->getCallSign());
1619 for (ivi = i->getIntentions().begin(); ivi != i->getIntentions().end(); ivi++) {
1620 int segIndex = (*ivi);
1622 if (segments[segIndex-1]->hasBlock(now))
1626 //after this, ivi points just behind the last valid unblocked taxi segment.
1627 for (intVecIterator j = i->getIntentions().begin(); j != ivi; j++) {
1630 FGTaxiSegment *seg = segments[pos-1];
1631 FGTaxiNode *node = seg->getEnd();
1632 length += seg->getLength();
1633 for (FGTaxiSegmentVectorIterator tsi = segments.begin(); tsi != segments.end(); tsi++) {
1634 if (((*tsi)->getEnd() == node) && ((*tsi) != seg)) {
1635 time_t blockTime = now + (length / vTaxi);
1636 (*tsi)->block(i->getId(), blockTime - 30, now);