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"
57 /***************************************************************************
59 **************************************************************************/
61 void FGTaxiSegment::setStart(FGTaxiNodeVector * nodes)
63 FGTaxiNodeVectorIterator i = nodes->begin();
64 while (i != nodes->end()) {
65 //cerr << "Scanning start node index" << (*i)->getIndex() << endl;
66 if ((*i)->getIndex() == startNode) {
67 start = (*i)->getAddress();
68 (*i)->addSegment(this);
73 SG_LOG(SG_GENERAL, SG_ALERT,
74 "Could not find start node " << startNode << endl);
77 void FGTaxiSegment::setEnd(FGTaxiNodeVector * nodes)
79 FGTaxiNodeVectorIterator i = nodes->begin();
80 while (i != nodes->end()) {
81 //cerr << "Scanning end node index" << (*i)->getIndex() << endl;
82 if ((*i)->getIndex() == endNode) {
83 end = (*i)->getAddress();
88 SG_LOG(SG_GENERAL, SG_ALERT,
89 "Could not find end node " << endNode << endl);
94 // There is probably a computationally cheaper way of
96 void FGTaxiSegment::setDimensions(double elevation)
98 length = SGGeodesy::distanceM(start->getGeod(), end->getGeod());
99 //heading = SGGeodesy::headingDeg(start->getGeod(), end->getGeod());
101 double az2; //, distanceM;
102 SGGeodesy::inverse(start->getGeod(), end->getGeod(), heading, az2, length);
103 double coveredDistance = length * 0.5;
104 SGGeodesy::direct(start->getGeod(), heading, coveredDistance, center, az2);
105 //start->setElevation(elevation);
106 //end->setElevation(elevation);
107 //cerr << "Centerpoint = (" << center.getLatitudeDeg() << ", " << center.getLongitudeDeg() << "). Heading = " << heading << endl;
111 //void FGTaxiSegment::setCourseDiff(double crse)
113 // headingDiff = fabs(course - crse);
115 // if (headingDiff > 180)
116 // headingDiff = fabs(headingDiff - 360);
119 void FGTaxiSegment::block(int id, time_t blockTime, time_t now)
121 BlockListIterator i = blockTimes.begin();
122 while (i != blockTimes.end()) {
123 if (i->getId() == id)
127 if (i == blockTimes.end()) {
128 blockTimes.push_back(Block(id, blockTime, now));
129 sort(blockTimes.begin(), blockTimes.end());
131 i->updateTimeStamps(blockTime, now);
135 // The segment has a block if any of the block times listed in the block list is
136 // smaller than the current time.
137 bool FGTaxiSegment::hasBlock(time_t now)
139 for (BlockListIterator i = blockTimes.begin(); i != blockTimes.end(); i++) {
140 if (i->getBlockTime() < now)
146 void FGTaxiSegment::unblock(time_t now)
148 if (blockTimes.size()) {
149 BlockListIterator i = blockTimes.begin();
150 if (i->getTimeStamp() < (now - 30)) {
158 /***************************************************************************
160 **************************************************************************/
161 bool FGTaxiRoute::next(int *nde)
163 //for (intVecIterator i = nodes.begin(); i != nodes.end(); i++)
164 // cerr << "FGTaxiRoute contains : " << *(i) << endl;
165 //cerr << "Offset from end: " << nodes.end() - currNode << endl;
166 //if (currNode != nodes.end())
167 // cerr << "true" << endl;
169 // cerr << "false" << endl;
170 //if (nodes.size() != (routes.size()) +1)
171 // cerr << "ALERT: Misconfigured TaxiRoute : " << nodes.size() << " " << routes.size() << endl;
173 if (currNode == nodes.end())
176 if (currNode != nodes.begin()) // make sure route corresponds to the end node
182 bool FGTaxiRoute::next(int *nde, int *rte)
184 //for (intVecIterator i = nodes.begin(); i != nodes.end(); i++)
185 // cerr << "FGTaxiRoute contains : " << *(i) << endl;
186 //cerr << "Offset from end: " << nodes.end() - currNode << endl;
187 //if (currNode != nodes.end())
188 // cerr << "true" << endl;
190 // cerr << "false" << endl;
191 if (nodes.size() != (routes.size()) + 1) {
192 SG_LOG(SG_GENERAL, SG_ALERT,
193 "ALERT: Misconfigured TaxiRoute : " << nodes.
194 size() << " " << routes.size());
197 if (currNode == nodes.end())
200 //*rte = *(currRoute);
201 if (currNode != nodes.begin()) // Make sure route corresponds to the end node
206 // If currNode points to the first node, this means the aircraft is not on the taxi node
207 // yet. Make sure to return a unique identifyer in this situation though, because otherwise
208 // the speed adjust AI code may be unable to resolve whether two aircraft are on the same
209 // taxi route or not. the negative of the preceding route seems a logical choice, as it is
210 // unique for any starting location.
211 // Note that this is probably just a temporary fix until I get Parking / tower control working.
212 *rte = -1 * *(currRoute);
219 void FGTaxiRoute::rewind(int route)
225 if (!(next(&currPoint, &currRoute))) {
226 SG_LOG(SG_GENERAL, SG_ALERT,
227 "Error in rewinding TaxiRoute: current" << currRoute <<
230 } while (currRoute != route);
236 /***************************************************************************
238 **************************************************************************/
239 bool compare_nodes(FGTaxiNode * a, FGTaxiNode * b)
244 bool compare_segments(FGTaxiSegment * a, FGTaxiSegment * b)
249 bool compare_trafficrecords(FGTrafficRecord a, FGTrafficRecord b)
251 return (a.getIntentions().size() < b.getIntentions().size());
254 FGGroundNetwork::FGGroundNetwork() :
263 currTraffic = activeTraffic.begin();
266 networkInitialized = false;
270 FGGroundNetwork::~FGGroundNetwork()
272 // JMT 2012-09-8 - disabling the groundnet-caching as part of enabling the
273 // navcache. The problem isn't the NavCache - it's that for the past few years,
274 // we have not being running destructors on FGPositioned classes, and hence,
275 // not running this code.
276 // When I fix FGPositioned lifetimes (unloading-at-runtime support), this
277 // will need to be re-visited so it can run safely during shutdown.
279 //cerr << "Running Groundnetwork Destructor " << endl;
280 bool saveData = false;
282 if (fgGetBool("/sim/ai/groundnet-cache")) {
283 SGPath cacheData(globals->get_fg_home());
284 cacheData.append("ai");
285 string airport = parent->getId();
287 if ((airport) != "") {
289 ::snprintf(buffer, 128, "%c/%c/%c/",
290 airport[0], airport[1], airport[2]);
291 cacheData.append(buffer);
292 if (!cacheData.exists()) {
293 cacheData.create_dir(0777);
295 cacheData.append(airport + "-groundnet-cache.txt");
296 cachefile.open(cacheData.str().c_str());
300 cachefile << "[GroundNetcachedata:ref:2011:09:04]" << endl;
301 for (FGTaxiNodeVectorIterator node = nodes.begin();
302 node != nodes.end(); node++) {
304 cachefile << (*node)->getIndex () << " "
305 << (*node)->getElevationM (parent->getElevation()*SG_FEET_TO_METER) << " "
311 pushBackNodes.clear();
312 for (FGTaxiSegmentVectorIterator seg = segments.begin();
313 seg != segments.end(); seg++) {
323 void FGGroundNetwork::saveElevationCache() {
324 //cerr << "Running Groundnetwork Destructor " << endl;
325 bool saveData = false;
327 if (fgGetBool("/sim/ai/groundnet-cache")) {
328 SGPath cacheData(globals->get_fg_home());
329 cacheData.append("ai");
330 string airport = parent->getId();
332 if ((airport) != "") {
334 ::snprintf(buffer, 128, "%c/%c/%c/",
335 airport[0], airport[1], airport[2]);
336 cacheData.append(buffer);
337 if (!cacheData.exists()) {
338 cacheData.create_dir(0777);
340 cacheData.append(airport + "-groundnet-cache.txt");
341 cachefile.open(cacheData.str().c_str());
345 cachefile << "[GroundNetcachedata:ref:2011:09:04]" << endl;
346 for (FGTaxiNodeVectorIterator node = nodes.begin();
347 node != nodes.end(); node++) {
349 cachefile << (*node)->getIndex () << " "
350 << (*node)->getElevationM (parent->getElevation()*SG_FEET_TO_METER) << " "
359 void FGGroundNetwork::addSegment(const FGTaxiSegment & seg)
361 segments.push_back(new FGTaxiSegment(seg));
364 void FGGroundNetwork::addNode(const FGTaxiNode & node)
366 nodes.push_back(new FGTaxiNode(node));
369 void FGGroundNetwork::addNodes(FGParkingVec * parkings)
372 FGParkingVecIterator i = parkings->begin();
373 while (i != parkings->end()) {
374 n.setIndex(i->getIndex());
375 n.setLatitude(i->getLatitude());
376 n.setLongitude(i->getLongitude());
377 n.setElevation(parent->getElevation()*SG_FEET_TO_METER);
378 nodes.push_back(new FGTaxiNode(n));
386 void FGGroundNetwork::init()
388 if (networkInitialized) {
389 FGATCController::init();
390 //cerr << "FGground network already initialized" << endl;
396 sort(nodes.begin(), nodes.end(), compare_nodes);
397 //sort(segments.begin(), segments.end(), compare_segments());
398 FGTaxiSegmentVectorIterator i = segments.begin();
399 while (i != segments.end()) {
400 (*i)->setStart(&nodes);
401 (*i)->setEnd(&nodes);
402 (*i)->setDimensions(parent->getElevation() * SG_FEET_TO_METER);
403 (*i)->setIndex(index);
404 if ((*i)->isPushBack()) {
405 pushBackNodes.push_back((*i)->getEnd());
407 //SG_LOG(SG_GENERAL, SG_BULK, "initializing segment " << (*i)->getIndex() << endl);
408 //SG_LOG(SG_GENERAL, SG_BULK, "Track distance = " << (*i)->getLength() << endl);
409 //SG_LOG(SG_GENERAL, SG_BULK, "Track runs from " << (*i)->getStart()->getIndex() << " to "
410 // << (*i)->getEnd()->getIndex() << endl);
415 i = segments.begin();
416 while (i != segments.end()) {
417 FGTaxiSegmentVectorIterator j = (*i)->getEnd()->getBeginRoute();
418 while (j != (*i)->getEnd()->getEndRoute()) {
419 if ((*j)->getEnd()->getIndex() == (*i)->getStart()->getIndex()) {
420 // int start1 = (*i)->getStart()->getIndex();
421 // int end1 = (*i)->getEnd() ->getIndex();
422 // int start2 = (*j)->getStart()->getIndex();
423 // int end2 = (*j)->getEnd()->getIndex();
424 // int oppIndex = (*j)->getIndex();
425 //cerr << "Opposite of " << (*i)->getIndex() << " (" << start1 << "," << end1 << ") "
426 // << "happens to be " << oppIndex << " (" << start2 << "," << end2 << ") " << endl;
427 (*i)->setOpposite(*j);
434 //FGTaxiNodeVectorIterator j = nodes.begin();
435 //while (j != nodes.end()) {
436 // if ((*j)->getHoldPointType() == 3) {
437 // pushBackNodes.push_back((*j));
441 //cerr << "Done initializing ground network" << endl;
443 if (fgGetBool("/sim/ai/groundnet-cache")) {
444 SGPath cacheData(globals->get_fg_home());
445 cacheData.append("ai");
446 string airport = parent->getId();
448 if ((airport) != "") {
450 ::snprintf(buffer, 128, "%c/%c/%c/",
451 airport[0], airport[1], airport[2]);
452 cacheData.append(buffer);
453 if (!cacheData.exists()) {
454 cacheData.create_dir(0777);
458 cacheData.append(airport + "-groundnet-cache.txt");
459 if (cacheData.exists()) {
460 ifstream data(cacheData.c_str());
463 if (revisionStr != "[GroundNetcachedata:ref:2011:09:04]") {
464 SG_LOG(SG_GENERAL, SG_ALERT,"GroundNetwork Warning: discarding outdated cachefile " <<
465 cacheData.c_str() << " for Airport " << airport);
467 for (FGTaxiNodeVectorIterator i = nodes.begin();
470 (*i)->setElevation(parent->getElevation() * SG_FEET_TO_METER);
471 data >> index >> elev;
474 if (index != (*i)->getIndex()) {
475 SG_LOG(SG_GENERAL, SG_ALERT, "Index read from ground network cache at airport " << airport << " does not match index in the network itself");
477 (*i)->setElevation(elev);
484 //cerr << "Finished initializing " << parent->getId() << " groundnetwork " << endl;
485 networkInitialized = true;
488 int FGGroundNetwork::findNearestNode(const SGGeod & aGeod)
490 double minDist = HUGE_VAL;
493 for (FGTaxiNodeVectorIterator itr = nodes.begin(); itr != nodes.end();
495 double d = SGGeodesy::distanceM(aGeod, (*itr)->getGeod());
498 index = (*itr)->getIndex();
499 //cerr << "Minimum distance of " << minDist << " for index " << index << endl;
506 int FGGroundNetwork::findNearestNodeOnRunway(const SGGeod & aGeod)
508 double minDist = HUGE_VAL;
511 for (FGTaxiNodeVectorIterator itr = nodes.begin(); itr != nodes.end();
513 if (!((*itr)->getIsOnRunway())) {
516 double d = SGGeodesy::distanceM(aGeod, (*itr)->getGeod());
519 index = (*itr)->getIndex();
520 //cerr << "Minimum distance of " << minDist << " for index " << index << endl;
528 int FGGroundNetwork::findNearestNode(double lat, double lon)
530 return findNearestNode(SGGeod::fromDeg(lon, lat));
533 FGTaxiNode *FGGroundNetwork::findNode(unsigned idx)
535 for (FGTaxiNodeVectorIterator
537 itr != nodes.end(); itr++)
539 if (itr->getIndex() == idx)
540 return itr->getAddress();
543 if (idx < nodes.size())
544 return nodes[idx]->getAddress();
549 FGTaxiSegment *FGGroundNetwork::findSegment(unsigned idx)
551 for (FGTaxiSegmentVectorIterator
552 itr = segments.begin();
553 itr != segments.end(); itr++)
555 if (itr->getIndex() == idx)
556 return itr->getAddress();
559 if ((idx > 0) && (idx <= segments.size()))
560 return segments[idx - 1]->getAddress();
562 //cerr << "Alert: trying to find invalid segment " << idx << endl;
568 FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end,
571 //implements Dijkstra's algorithm to find shortest distance route from start to end
572 //taken from http://en.wikipedia.org/wiki/Dijkstra's_algorithm
574 //double INFINITE = 100000000000.0;
575 // initialize scoring values
576 int nParkings = parent->getDynamics()->getNrOfParkings();
577 FGTaxiNodeVector *currNodesSet;
579 currNodesSet = &nodes;
581 currNodesSet = &pushBackNodes;
584 for (FGTaxiNodeVectorIterator
585 itr = currNodesSet->begin(); itr != currNodesSet->end(); itr++) {
586 (*itr)->setPathScore(HUGE_VAL); //infinity by all practical means
587 (*itr)->setPreviousNode(0); //
588 (*itr)->setPreviousSeg(0); //
591 FGTaxiNode *firstNode = findNode(start);
594 SG_LOG(SG_GENERAL, SG_ALERT,
595 "Error in ground network. Failed to find first waypoint: " << start
596 << " at " << ((parent) ? parent->getId() : "<unknown>"));
597 return FGTaxiRoute();
599 firstNode->setPathScore(0);
601 FGTaxiNode *lastNode = findNode(end);
604 SG_LOG(SG_GENERAL, SG_ALERT,
605 "Error in ground network. Failed to find last waypoint: " << end
606 << " at " << ((parent) ? parent->getId() : "<unknown>"));
607 return FGTaxiRoute();
610 FGTaxiNodeVector unvisited(*currNodesSet); // working copy
612 while (!unvisited.empty()) {
613 FGTaxiNode *best = *(unvisited.begin());
614 for (FGTaxiNodeVectorIterator
615 itr = unvisited.begin(); itr != unvisited.end(); itr++) {
616 if ((*itr)->getPathScore() < best->getPathScore())
620 FGTaxiNodeVectorIterator newend =
621 remove(unvisited.begin(), unvisited.end(), best);
622 unvisited.erase(newend, unvisited.end());
624 if (best == lastNode) { // found route or best not connected
627 for (FGTaxiSegmentVectorIterator
628 seg = best->getBeginRoute();
629 seg != best->getEndRoute(); seg++) {
630 if (fullSearch || (*seg)->isPushBack()) {
631 FGTaxiNode *tgt = (*seg)->getEnd();
634 SG_LOG(SG_GENERAL, SG_ALERT,
635 "Error in ground network. Found empty segment "
636 << " at " << ((parent) ? parent->getId() : "<unknown>"));
637 return FGTaxiRoute();
640 best->getPathScore() + (*seg)->getLength() +
641 (*seg)->getPenalty(nParkings);
642 if (alt < tgt->getPathScore()) { // Relax (u,v)
643 tgt->setPathScore(alt);
644 tgt->setPreviousNode(best);
645 tgt->setPreviousSeg(*seg); //
648 // // cerr << "Skipping TaxiSegment " << (*seg)->getIndex() << endl;
654 if (lastNode->getPathScore() == HUGE_VAL) {
655 // no valid route found
657 SG_LOG(SG_GENERAL, SG_ALERT,
658 "Failed to find route from waypoint " << start << " to "
659 << end << " at " << parent->getId());
663 //exit(1); //TODO exit more gracefully, no need to stall the whole sim with broken GN's
665 // assemble route from backtrace information
666 intVec nodes, routes;
667 FGTaxiNode *bt = lastNode;
668 while (bt->getPreviousNode() != 0) {
669 nodes.push_back(bt->getIndex());
670 routes.push_back(bt->getPreviousSegment()->getIndex());
671 bt = bt->getPreviousNode();
673 nodes.push_back(start);
674 reverse(nodes.begin(), nodes.end());
675 reverse(routes.begin(), routes.end());
677 return FGTaxiRoute(nodes, routes, lastNode->getPathScore(), 0);
681 int FGTaxiSegment::getPenalty(int nGates)
684 if (end->getIndex() < nGates) {
687 if (end->getIsOnRunway()) { // For now. In future versions, need to find out whether runway is active.
693 /* ATC Related Functions */
695 void FGGroundNetwork::announcePosition(int id,
696 FGAIFlightPlan * intendedRoute,
697 int currentPosition, double lat,
698 double lon, double heading,
699 double speed, double alt,
700 double radius, int leg,
701 FGAIAircraft * aircraft)
704 TrafficVectorIterator i = activeTraffic.begin();
705 // Search search if the current id alread has an entry
706 // This might be faster using a map instead of a vector, but let's start by taking a safe route
707 if (activeTraffic.size()) {
708 //while ((i->getId() != id) && i != activeTraffic.end()) {
709 while (i != activeTraffic.end()) {
710 if (i->getId() == id) {
716 // Add a new TrafficRecord if no one exsists for this aircraft.
717 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
721 rec.setPositionAndIntentions(currentPosition, intendedRoute);
722 rec.setPositionAndHeading(lat, lon, heading, speed, alt);
723 rec.setRadius(radius); // only need to do this when creating the record.
724 rec.setAircraft(aircraft);
726 activeTraffic.push_front(rec);
728 activeTraffic.push_back(rec);
732 i->setPositionAndIntentions(currentPosition, intendedRoute);
733 i->setPositionAndHeading(lat, lon, heading, speed, alt);
738 void FGGroundNetwork::signOff(int id)
740 TrafficVectorIterator i = activeTraffic.begin();
741 // Search search if the current id alread has an entry
742 // This might be faster using a map instead of a vector, but let's start by taking a safe route
743 if (activeTraffic.size()) {
744 //while ((i->getId() != id) && i != activeTraffic.end()) {
745 while (i != activeTraffic.end()) {
746 if (i->getId() == id) {
752 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
753 SG_LOG(SG_GENERAL, SG_ALERT,
754 "AI error: Aircraft without traffic record is signing off at " << SG_ORIGIN);
756 i = activeTraffic.erase(i);
760 * The ground network can deal with the following states:
761 * 0 = Normal; no action required
762 * 1 = "Acknowledge "Hold position
763 * 2 = "Acknowledge "Resume taxi".
764 * 3 = "Issue TaxiClearance"
765 * 4 = Acknowledge Taxi Clearance"
766 * 5 = Post acknowlegde taxiclearance: Start taxiing
768 * 7 = Acknowledge report runway
769 * 8 = Switch tower frequency
770 * 9 = Acknowledge switch tower frequency
771 *************************************************************************************************************************/
772 bool FGGroundNetwork::checkTransmissionState(int minState, int maxState, TrafficVectorIterator i, time_t now, AtcMsgId msgId,
775 int state = i->getState();
776 if ((state >= minState) && (state <= maxState) && available) {
777 if ((msgDir == ATC_AIR_TO_GROUND) && isUserAircraft(i->getAircraft())) {
778 //cerr << "Checking state " << state << " for " << i->getAircraft()->getCallSign() << endl;
779 static SGPropertyNode_ptr trans_num = globals->get_props()->getNode("/sim/atc/transmission-num", true);
780 int n = trans_num->getIntValue();
782 trans_num->setIntValue(-1);
784 //cerr << "Selected transmission message " << n << endl;
785 //FGATCManager *atc = (FGATCManager*) globals->get_subsystem("atc");
786 FGATCDialogNew::instance()->removeEntry(1);
788 //cerr << "creating message for " << i->getAircraft()->getCallSign() << endl;
789 transmit(&(*i), &(*parent->getDynamics()), msgId, msgDir, false);
793 transmit(&(*i), &(*parent->getDynamics()), msgId, msgDir, true);
795 lastTransmission = now;
802 void FGGroundNetwork::updateAircraftInformation(int id, double lat, double lon,
803 double heading, double speed, double alt,
806 time_t currentTime = time(NULL);
807 if (nextSave < currentTime) {
808 saveElevationCache();
809 nextSave = currentTime + 100 + rand() % 200;
811 // Check whether aircraft are on hold due to a preceding pushback. If so, make sure to
812 // Transmit air-to-ground "Ready to taxi request:
813 // Transmit ground to air approval / hold
814 // Transmit confirmation ...
815 // Probably use a status mechanism similar to the Engine start procedure in the startup controller.
818 TrafficVectorIterator i = activeTraffic.begin();
819 // Search search if the current id has an entry
820 // This might be faster using a map instead of a vector, but let's start by taking a safe route
821 TrafficVectorIterator current, closest;
822 if (activeTraffic.size()) {
823 //while ((i->getId() != id) && i != activeTraffic.end()) {
824 while (i != activeTraffic.end()) {
825 if (i->getId() == id) {
831 // update position of the current aircraft
832 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
833 SG_LOG(SG_GENERAL, SG_ALERT,
834 "AI error: updating aircraft without traffic record at " << SG_ORIGIN);
836 i->setPositionAndHeading(lat, lon, heading, speed, alt);
842 // Update every three secs, but add some randomness
843 // to prevent all IA objects doing this in synchrony
844 //if (getDt() < (3.0) + (rand() % 10))
848 current->clearResolveCircularWait();
849 current->setWaitsForId(0);
850 checkSpeedAdjustment(id, lat, lon, heading, speed, alt);
851 bool needsTaxiClearance = current->getAircraft()->getTaxiClearanceRequest();
852 if (!needsTaxiClearance) {
853 checkHoldPosition(id, lat, lon, heading, speed, alt);
854 //if (checkForCircularWaits(id)) {
855 // i->setResolveCircularWait();
858 current->setHoldPosition(true);
859 int state = current->getState();
860 time_t now = time(NULL) + fgGetLong("/sim/time/warp");
861 if ((now - lastTransmission) > 15) {
864 if (checkTransmissionState(0,2, current, now, MSG_REQUEST_TAXI_CLEARANCE, ATC_AIR_TO_GROUND)) {
865 current->setState(3);
867 if (checkTransmissionState(3,3, current, now, MSG_ISSUE_TAXI_CLEARANCE, ATC_GROUND_TO_AIR)) {
868 current->setState(4);
870 if (checkTransmissionState(4,4, current, now, MSG_ACKNOWLEDGE_TAXI_CLEARANCE, ATC_AIR_TO_GROUND)) {
871 current->setState(5);
873 if ((state == 5) && available) {
874 current->setState(0);
875 current->getAircraft()->setTaxiClearanceRequest(false);
876 current->setHoldPosition(false);
884 Scan for a speed adjustment change. Find the nearest aircraft that is in front
885 and adjust speed when we get too close. Only do this when current position and/or
886 intentions of the current aircraft match current taxiroute position of the proximate
887 aircraft. For traffic that is on other routes we need to issue a "HOLD Position"
888 instruction. See below for the hold position instruction.
890 Note that there currently still is one flaw in the logic that needs to be addressed.
891 There can be situations where one aircraft is in front of the current aircraft, on a separate
892 route, but really close after an intersection coming off the current route. This
893 aircraft is still close enough to block the current aircraft. This situation is currently
894 not addressed yet, but should be.
897 void FGGroundNetwork::checkSpeedAdjustment(int id, double lat,
898 double lon, double heading,
899 double speed, double alt)
902 TrafficVectorIterator current, closest, closestOnNetwork;
903 TrafficVectorIterator i = activeTraffic.begin();
904 bool otherReasonToSlowDown = false;
905 // bool previousInstruction;
906 if (activeTraffic.size()) {
907 //while ((i->getId() != id) && (i != activeTraffic.end()))
908 while (i != activeTraffic.end()) {
909 if (i->getId() == id) {
917 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
918 SG_LOG(SG_GENERAL, SG_ALERT,
919 "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkSpeedAdjustment at " << SG_ORIGIN);
924 // previousInstruction = current->getSpeedAdjustment();
925 double mindist = HUGE_VAL;
926 if (activeTraffic.size()) {
927 double course, dist, bearing, az2; // minbearing,
928 SGGeod curr(SGGeod::fromDegM(lon, lat, alt));
929 //TrafficVector iterator closest;
931 closestOnNetwork = current;
932 for (TrafficVectorIterator i = activeTraffic.begin();
933 i != activeTraffic.end(); i++) {
938 SGGeod other(SGGeod::fromDegM(i->getLongitude(),
941 SGGeodesy::inverse(curr, other, course, az2, dist);
942 bearing = fabs(heading - course);
944 bearing = 360 - bearing;
945 if ((dist < mindist) && (bearing < 60.0)) {
948 closestOnNetwork = i;
949 // minbearing = bearing;
953 //Check traffic at the tower controller
954 if (towerController->hasActiveTraffic()) {
955 for (TrafficVectorIterator i =
956 towerController->getActiveTraffic().begin();
957 i != towerController->getActiveTraffic().end(); i++) {
958 //cerr << "Comparing " << current->getId() << " and " << i->getId() << endl;
959 SGGeod other(SGGeod::fromDegM(i->getLongitude(),
962 SGGeodesy::inverse(curr, other, course, az2, dist);
963 bearing = fabs(heading - course);
965 bearing = 360 - bearing;
966 if ((dist < mindist) && (bearing < 60.0)) {
967 //cerr << "Current aircraft " << current->getAircraft()->getTrafficRef()->getCallSign()
968 // << " is closest to " << i->getAircraft()->getTrafficRef()->getCallSign()
969 // << ", which has status " << i->getAircraft()->isScheduledForTakeoff()
973 // minbearing = bearing;
974 otherReasonToSlowDown = true;
978 // Finally, check UserPosition
979 // Note, as of 2011-08-01, this should no longer be necessecary.
981 double userLatitude = fgGetDouble("/position/latitude-deg");
982 double userLongitude = fgGetDouble("/position/longitude-deg");
983 SGGeod user(SGGeod::fromDeg(userLongitude, userLatitude));
984 SGGeodesy::inverse(curr, user, course, az2, dist);
986 bearing = fabs(heading - course);
988 bearing = 360 - bearing;
989 if ((dist < mindist) && (bearing < 60.0)) {
992 minbearing = bearing;
993 otherReasonToSlowDown = true;
996 current->clearSpeedAdjustment();
997 bool needBraking = false;
998 if (current->checkPositionAndIntentions(*closest)
999 || otherReasonToSlowDown) {
1000 double maxAllowableDistance =
1001 (1.1 * current->getRadius()) +
1002 (1.1 * closest->getRadius());
1003 if (mindist < 2 * maxAllowableDistance) {
1004 if (current->getId() == closest->getWaitsForId())
1007 current->setWaitsForId(closest->getId());
1008 if (closest->getId() != current->getId()) {
1009 current->setSpeedAdjustment(closest->getSpeed() *
1014 // closest->getAircraft()->getTakeOffStatus() &&
1015 // (current->getAircraft()->getTrafficRef()->getDepartureAirport() == closest->getAircraft()->getTrafficRef()->getDepartureAirport()) &&
1016 // (current->getAircraft()->GetFlightPlan()->getRunway() == closest->getAircraft()->GetFlightPlan()->getRunway())
1018 // current->getAircraft()->scheduleForATCTowerDepartureControl(1);
1020 current->setSpeedAdjustment(0); // This can only happen when the user aircraft is the one closest
1022 if (mindist < maxAllowableDistance) {
1023 //double newSpeed = (maxAllowableDistance-mindist);
1024 //current->setSpeedAdjustment(newSpeed);
1025 //if (mindist < 0.5* maxAllowableDistance)
1027 current->setSpeedAdjustment(0);
1032 if ((closest->getId() == closestOnNetwork->getId()) && (current->getPriority() < closest->getPriority()) && needBraking) {
1033 swap(current, closest);
1039 Check for "Hold position instruction".
1040 The hold position should be issued under the following conditions:
1041 1) For aircraft entering or crossing a runway with active traffic on it, or landing aircraft near it
1042 2) For taxiing aircraft that use one taxiway in opposite directions
1043 3) For crossing or merging taxiroutes.
1046 void FGGroundNetwork::checkHoldPosition(int id, double lat,
1047 double lon, double heading,
1048 double speed, double alt)
1050 TrafficVectorIterator current;
1051 TrafficVectorIterator i = activeTraffic.begin();
1052 if (activeTraffic.size()) {
1053 //while ((i->getId() != id) && i != activeTraffic.end())
1054 while (i != activeTraffic.end()) {
1055 if (i->getId() == id) {
1063 time_t now = time(NULL) + fgGetLong("/sim/time/warp");
1064 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
1065 SG_LOG(SG_GENERAL, SG_ALERT,
1066 "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkHoldPosition at " << SG_ORIGIN);
1070 if (current->getAircraft()->getTakeOffStatus() == 1) {
1071 current->setHoldPosition(true);
1074 if (current->getAircraft()->getTakeOffStatus() == 2) {
1075 //cerr << current->getAircraft()->getCallSign() << ". Taxi in position and hold" << endl;
1076 current->setHoldPosition(false);
1077 current->clearSpeedAdjustment();
1080 bool origStatus = current->hasHoldPosition();
1081 current->setHoldPosition(false);
1082 SGGeod curr(SGGeod::fromDegM(lon, lat, alt));
1083 int currentRoute = i->getCurrentPosition();
1085 if (i->getIntentions().size()) {
1086 nextRoute = (*(i->getIntentions().begin()));
1090 if (currentRoute > 0) {
1091 FGTaxiSegment *tx = findSegment(currentRoute);
1094 nx = findSegment(nextRoute);
1098 //if (tx->hasBlock(now) || nx->hasBlock(now) ) {
1099 // current->setHoldPosition(true);
1101 SGGeod start(SGGeod::fromDeg((i->getLongitude()), (i->getLatitude())));
1102 SGGeod end (SGGeod::fromDeg(nx->getStart()->getLongitude(), nx->getStart()->getLatitude()));
1104 double distance = SGGeodesy::distanceM(start, end);
1105 if (nx->hasBlock(now) && (distance < i->getRadius() * 4)) {
1106 current->setHoldPosition(true);
1108 intVecIterator ivi = i->getIntentions().begin();
1109 while (ivi != i->getIntentions().end()) {
1111 distance += segments[(*ivi)-1]->getLength();
1112 if ((segments[(*ivi)-1]->hasBlock(now)) && (distance < i->getRadius() * 4)) {
1113 current->setHoldPosition(true);
1121 bool currStatus = current->hasHoldPosition();
1122 current->setHoldPosition(origStatus);
1123 // Either a Hold Position or a resume taxi transmission has been issued
1124 if ((now - lastTransmission) > 2) {
1127 if (current->getState() == 0) {
1128 if ((origStatus != currStatus) && available) {
1129 //cerr << "Issueing hold short instrudtion " << currStatus << " " << available << endl;
1130 if (currStatus == true) { // No has a hold short instruction
1131 transmit(&(*current), &(*parent->getDynamics()), MSG_HOLD_POSITION, ATC_GROUND_TO_AIR, true);
1132 //cerr << "Transmittin hold short instrudtion " << currStatus << " " << available << endl;
1133 current->setState(1);
1135 transmit(&(*current), &(*parent->getDynamics()), MSG_RESUME_TAXI, ATC_GROUND_TO_AIR, true);
1136 //cerr << "Transmittig resume instrudtion " << currStatus << " " << available << endl;
1137 current->setState(2);
1139 lastTransmission = now;
1141 // Don't act on the changed instruction until the transmission is confirmed
1142 // So set back to original status
1143 //cerr << "Current state " << current->getState() << endl;
1147 // 6 = Report runway
1148 // 7 = Acknowledge report runway
1149 // 8 = Switch tower frequency
1150 //9 = Acknowledge switch tower frequency
1152 //int state = current->getState();
1153 if (checkTransmissionState(1,1, current, now, MSG_ACKNOWLEDGE_HOLD_POSITION, ATC_AIR_TO_GROUND)) {
1154 current->setState(0);
1155 current->setHoldPosition(true);
1157 if (checkTransmissionState(2,2, current, now, MSG_ACKNOWLEDGE_RESUME_TAXI, ATC_AIR_TO_GROUND)) {
1158 current->setState(0);
1159 current->setHoldPosition(false);
1161 if (current->getAircraft()->getTakeOffStatus() && (current->getState() == 0)) {
1162 //cerr << "Scheduling " << current->getAircraft()->getCallSign() << " for hold short" << endl;
1163 current->setState(6);
1165 if (checkTransmissionState(6,6, current, now, MSG_REPORT_RUNWAY_HOLD_SHORT, ATC_AIR_TO_GROUND)) {
1167 if (checkTransmissionState(7,7, current, now, MSG_ACKNOWLEDGE_REPORT_RUNWAY_HOLD_SHORT, ATC_GROUND_TO_AIR)) {
1169 if (checkTransmissionState(8,8, current, now, MSG_SWITCH_TOWER_FREQUENCY, ATC_GROUND_TO_AIR)) {
1171 if (checkTransmissionState(9,9, current, now, MSG_ACKNOWLEDGE_SWITCH_TOWER_FREQUENCY, ATC_AIR_TO_GROUND)) {
1176 //current->setState(0);
1180 * Check whether situations occur where the current aircraft is waiting for itself
1181 * due to higher order interactions.
1182 * A 'circular' wait is a situation where a waits for b, b waits for c, and c waits
1183 * for a. Ideally each aircraft only waits for one other aircraft, so by tracing
1184 * through this list of waiting aircraft, we can check if we'd eventually end back
1185 * at the current aircraft.
1187 * Note that we should consider the situation where we are actually checking aircraft
1188 * d, which is waiting for aircraft a. d is not part of the loop, but is held back by
1189 * the looping aircraft. If we don't check for that, this function will get stuck into
1193 bool FGGroundNetwork::checkForCircularWaits(int id)
1195 //cerr << "Performing Wait check " << id << endl;
1197 TrafficVectorIterator current, other;
1198 TrafficVectorIterator i = activeTraffic.begin();
1199 int trafficSize = activeTraffic.size();
1201 while (i != activeTraffic.end()) {
1202 if (i->getId() == id) {
1210 if (i == activeTraffic.end() || (trafficSize == 0)) {
1211 SG_LOG(SG_GENERAL, SG_ALERT,
1212 "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits at " << SG_ORIGIN);
1216 target = current->getWaitsForId();
1217 //bool printed = false; // Note that this variable is for debugging purposes only.
1221 //cerr << "aircraft waits for user" << endl;
1226 while ((target > 0) && (target != id) && counter++ < trafficSize) {
1228 TrafficVectorIterator i = activeTraffic.begin();
1230 //while ((i->getId() != id) && i != activeTraffic.end())
1231 while (i != activeTraffic.end()) {
1232 if (i->getId() == target) {
1240 if (i == activeTraffic.end() || (trafficSize == 0)) {
1241 //cerr << "[Waiting for traffic at Runway: DONE] " << endl << endl;;
1242 // The target id is not found on the current network, which means it's at the tower
1243 //SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits");
1247 target = other->getWaitsForId();
1249 // actually this trap isn't as impossible as it first seemed:
1250 // the setWaitsForID(id) is set to current when the aircraft
1251 // is waiting for the user controlled aircraft.
1252 //if (current->getId() == other->getId()) {
1253 // cerr << "Caught the impossible trap" << endl;
1254 // cerr << "Current = " << current->getId() << endl;
1255 // cerr << "Other = " << other ->getId() << endl;
1256 // for (TrafficVectorIterator at = activeTraffic.begin();
1257 // at != activeTraffic.end();
1259 // cerr << "currently active aircraft : " << at->getCallSign() << " with Id " << at->getId() << " waits for " << at->getWaitsForId() << endl;
1262 if (current->getId() == other->getId())
1265 //cerr << current->getCallSign() << " (" << current->getId() << ") " << " -> " << other->getCallSign()
1266 // << " (" << other->getId() << "); " << endl;;
1276 // cerr << "[done] " << endl << endl;;
1278 SG_LOG(SG_GENERAL, SG_WARN,
1279 "Detected circular wait condition: Id = " << id <<
1280 "target = " << target);
1287 // Note that this function is probably obsolete...
1288 bool FGGroundNetwork::hasInstruction(int id)
1290 TrafficVectorIterator i = activeTraffic.begin();
1291 // Search search if the current id has an entry
1292 // This might be faster using a map instead of a vector, but let's start by taking a safe route
1293 if (activeTraffic.size()) {
1294 //while ((i->getId() != id) && i != activeTraffic.end()) {
1295 while (i != activeTraffic.end()) {
1296 if (i->getId() == id) {
1302 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
1303 SG_LOG(SG_GENERAL, SG_ALERT,
1304 "AI error: checking ATC instruction for aircraft without traffic record at " << SG_ORIGIN);
1306 return i->hasInstruction();
1311 FGATCInstruction FGGroundNetwork::getInstruction(int id)
1313 TrafficVectorIterator i = activeTraffic.begin();
1314 // Search search if the current id has an entry
1315 // This might be faster using a map instead of a vector, but let's start by taking a safe route
1316 if (activeTraffic.size()) {
1317 //while ((i->getId() != id) && i != activeTraffic.end()) {
1318 while (i != activeTraffic.end()) {
1319 if (i->getId() == id) {
1325 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
1326 SG_LOG(SG_GENERAL, SG_ALERT,
1327 "AI error: requesting ATC instruction for aircraft without traffic record at " << SG_ORIGIN);
1329 return i->getInstruction();
1331 return FGATCInstruction();
1334 // Note that this function is copied from simgear. for maintanance purposes, it's probabtl better to make a general function out of that.
1335 static void WorldCoordinate(osg::Matrix& obj_pos, double lat,
1336 double lon, double elev, double hdg, double slope)
1338 SGGeod geod = SGGeod::fromDegM(lon, lat, elev);
1339 obj_pos = makeZUpFrame(geod);
1340 // hdg is not a compass heading, but a counter-clockwise rotation
1341 // around the Z axis
1342 obj_pos.preMult(osg::Matrix::rotate(hdg * SGD_DEGREES_TO_RADIANS,
1344 obj_pos.preMult(osg::Matrix::rotate(slope * SGD_DEGREES_TO_RADIANS,
1351 void FGGroundNetwork::render(bool visible)
1354 SGMaterialLib *matlib = globals->get_matlib();
1357 globals->get_scenery()->get_scene_graph()->removeChild(group);
1358 //while (group->getNumChildren()) {
1359 // cerr << "Number of children: " << group->getNumChildren() << endl;
1360 //simgear::EffectGeode* geode = (simgear::EffectGeode*) group->getChild(0);
1361 //osg::MatrixTransform *obj_trans = (osg::MatrixTransform*) group->getChild(0);
1362 //geode->releaseGLObjects();
1363 //group->removeChild(geode);
1368 group = new osg::Group;
1369 FGScenery * local_scenery = globals->get_scenery();
1370 // double elevation_meters = 0.0;
1371 // double elevation_feet = 0.0;
1372 time_t now = time(NULL) + fgGetLong("/sim/time/warp");
1373 //for ( FGTaxiSegmentVectorIterator i = segments.begin(); i != segments.end(); i++) {
1375 for (TrafficVectorIterator i = activeTraffic.begin(); i != activeTraffic.end(); i++) {
1376 // Handle start point
1377 int pos = i->getCurrentPosition() - 1;
1380 SGGeod start(SGGeod::fromDeg((i->getLongitude()), (i->getLatitude())));
1381 SGGeod end (SGGeod::fromDeg(segments[pos]->getEnd()->getLongitude(), segments[pos]->getEnd()->getLatitude()));
1383 double length = SGGeodesy::distanceM(start, end);
1384 //heading = SGGeodesy::headingDeg(start->getGeod(), end->getGeod());
1386 double az2, heading; //, distanceM;
1387 SGGeodesy::inverse(start, end, heading, az2, length);
1388 double coveredDistance = length * 0.5;
1390 SGGeodesy::direct(start, heading, coveredDistance, center, az2);
1391 //cerr << "Active Aircraft : Centerpoint = (" << center.getLatitudeDeg() << ", " << center.getLongitudeDeg() << "). Heading = " << heading << endl;
1392 ///////////////////////////////////////////////////////////////////////////////
1393 // Make a helper function out of this
1394 osg::Matrix obj_pos;
1395 osg::MatrixTransform *obj_trans = new osg::MatrixTransform;
1396 obj_trans->setDataVariance(osg::Object::STATIC);
1397 // Experimental: Calculate slope here, based on length, and the individual elevations
1398 double elevationStart;
1399 if (isUserAircraft((i)->getAircraft())) {
1400 elevationStart = fgGetDouble("/position/ground-elev-m");
1402 elevationStart = ((i)->getAircraft()->_getAltitude());
1404 double elevationEnd = segments[pos]->getEnd()->getElevationM(parent->getElevation()*SG_FEET_TO_METER);
1405 //cerr << "Using elevation " << elevationEnd << endl;
1407 if ((elevationEnd == 0) || (elevationEnd = parent->getElevation())) {
1408 SGGeod center2 = end;
1409 center2.setElevationM(SG_MAX_ELEVATION_M);
1410 if (local_scenery->get_elevation_m( center2, elevationEnd, NULL )) {
1411 // elevation_feet = elevationEnd * SG_METER_TO_FEET + 0.5;
1412 //elevation_meters += 0.5;
1415 elevationEnd = parent->getElevation();
1417 segments[pos]->getEnd()->setElevation(elevationEnd);
1419 double elevationMean = (elevationStart + elevationEnd) / 2.0;
1420 double elevDiff = elevationEnd - elevationStart;
1422 double slope = atan2(elevDiff, length) * SGD_RADIANS_TO_DEGREES;
1424 //cerr << "1. Using mean elevation : " << elevationMean << " and " << slope << endl;
1426 WorldCoordinate( obj_pos, center.getLatitudeDeg(), center.getLongitudeDeg(), elevationMean+ 0.5, -(heading), slope );
1428 obj_trans->setMatrix( obj_pos );
1429 //osg::Vec3 center(0, 0, 0)
1431 float width = length /2.0;
1432 osg::Vec3 corner(-width, 0, 0.25f);
1433 osg::Vec3 widthVec(2*width + 1, 0, 0);
1434 osg::Vec3 heightVec(0, 1, 0);
1435 osg::Geometry* geometry;
1436 geometry = osg::createTexturedQuadGeometry(corner, widthVec, heightVec);
1437 simgear::EffectGeode* geode = new simgear::EffectGeode;
1438 geode->setName("test");
1439 geode->addDrawable(geometry);
1440 //osg::Node *custom_obj;
1442 if (segments[pos]->hasBlock(now)) {
1443 mat = matlib->find("UnidirectionalTaperRed");
1445 mat = matlib->find("UnidirectionalTaperGreen");
1448 geode->setEffect(mat->get_effect());
1449 obj_trans->addChild(geode);
1450 // wire as much of the scene graph together as we can
1451 //->addChild( obj_trans );
1452 group->addChild( obj_trans );
1453 /////////////////////////////////////////////////////////////////////
1455 //cerr << "BIG FAT WARNING: current position is here : " << pos << endl;
1457 for (intVecIterator j = (i)->getIntentions().begin(); j != (i)->getIntentions().end(); j++) {
1458 osg::Matrix obj_pos;
1461 osg::MatrixTransform *obj_trans = new osg::MatrixTransform;
1462 obj_trans->setDataVariance(osg::Object::STATIC);
1464 // Experimental: Calculate slope here, based on length, and the individual elevations
1465 double elevationStart = segments[k]->getStart()->getElevationM(parent->getElevation()*SG_FEET_TO_METER);
1466 double elevationEnd = segments[k]->getEnd ()->getElevationM(parent->getElevation()*SG_FEET_TO_METER);
1467 if ((elevationStart == 0) || (elevationStart == parent->getElevation())) {
1468 SGGeod center2 = segments[k]->getStart()->getGeod();
1469 center2.setElevationM(SG_MAX_ELEVATION_M);
1470 if (local_scenery->get_elevation_m( center2, elevationStart, NULL )) {
1471 // elevation_feet = elevationStart * SG_METER_TO_FEET + 0.5;
1472 //elevation_meters += 0.5;
1475 elevationStart = parent->getElevation();
1477 segments[k]->getStart()->setElevation(elevationStart);
1479 if ((elevationEnd == 0) || (elevationEnd == parent->getElevation())) {
1480 SGGeod center2 = segments[k]->getEnd()->getGeod();
1481 center2.setElevationM(SG_MAX_ELEVATION_M);
1482 if (local_scenery->get_elevation_m( center2, elevationEnd, NULL )) {
1483 // elevation_feet = elevationEnd * SG_METER_TO_FEET + 0.5;
1484 //elevation_meters += 0.5;
1487 elevationEnd = parent->getElevation();
1489 segments[k]->getEnd()->setElevation(elevationEnd);
1492 double elevationMean = (elevationStart + elevationEnd) / 2.0;
1493 double elevDiff = elevationEnd - elevationStart;
1494 double length = segments[k]->getLength();
1495 double slope = atan2(elevDiff, length) * SGD_RADIANS_TO_DEGREES;
1497 // cerr << "2. Using mean elevation : " << elevationMean << " and " << slope << endl;
1500 WorldCoordinate( obj_pos, segments[k]->getLatitude(), segments[k]->getLongitude(), elevationMean+ 0.5, -(segments[k]->getHeading()), slope );
1502 obj_trans->setMatrix( obj_pos );
1503 //osg::Vec3 center(0, 0, 0)
1505 float width = segments[k]->getLength() /2.0;
1506 osg::Vec3 corner(-width, 0, 0.25f);
1507 osg::Vec3 widthVec(2*width + 1, 0, 0);
1508 osg::Vec3 heightVec(0, 1, 0);
1509 osg::Geometry* geometry;
1510 geometry = osg::createTexturedQuadGeometry(corner, widthVec, heightVec);
1511 simgear::EffectGeode* geode = new simgear::EffectGeode;
1512 geode->setName("test");
1513 geode->addDrawable(geometry);
1514 //osg::Node *custom_obj;
1516 if (segments[k]->hasBlock(now)) {
1517 mat = matlib->find("UnidirectionalTaperRed");
1519 mat = matlib->find("UnidirectionalTaperGreen");
1522 geode->setEffect(mat->get_effect());
1523 obj_trans->addChild(geode);
1524 // wire as much of the scene graph together as we can
1525 //->addChild( obj_trans );
1526 group->addChild( obj_trans );
1531 globals->get_scenery()->get_scene_graph()->addChild(group);
1535 string FGGroundNetwork::getName() {
1536 return string(parent->getId() + "-ground");
1539 void FGGroundNetwork::update(double dt)
1541 time_t now = time(NULL) + fgGetLong("/sim/time/warp");
1542 for (FGTaxiSegmentVectorIterator tsi = segments.begin(); tsi != segments.end(); tsi++) {
1543 (*tsi)->unblock(now);
1546 //sort(activeTraffic.begin(), activeTraffic.end(), compare_trafficrecords);
1547 // Handle traffic that is under ground control first; this way we'll prevent clutter at the gate areas.
1548 // Don't allow an aircraft to pushback when a taxiing aircraft is currently using part of the intended route.
1549 for (TrafficVectorIterator i = parent->getDynamics()->getStartupController()->getActiveTraffic().begin();
1550 i != parent->getDynamics()->getStartupController()->getActiveTraffic().end(); i++) {
1552 i->setPriority(priority++);
1553 // in meters per second;
1554 double vTaxi = (i->getAircraft()->getPerformance()->vTaxi() * SG_NM_TO_METER) / 3600;
1555 if (i->isActive(0)) {
1557 // Check for all active aircraft whether it's current pos segment is
1558 // an opposite of one of the departing aircraft's intentions
1559 for (TrafficVectorIterator j = activeTraffic.begin(); j != activeTraffic.end(); j++) {
1560 int pos = j->getCurrentPosition();
1562 FGTaxiSegment *seg = segments[pos-1]->opposite();
1564 int posReverse = seg->getIndex();
1565 for (intVecIterator k = i->getIntentions().begin(); k != i->getIntentions().end(); k++) {
1566 if ((*k) == posReverse) {
1568 segments[posReverse-1]->block(i->getId(), now, now);
1574 // if the current aircraft is still allowed to pushback, we can start reserving a route for if by blocking all the entry taxiways.
1575 if (i->pushBackAllowed()) {
1577 int pos = i->getCurrentPosition();
1579 FGTaxiSegment *seg = segments[pos-1];
1580 FGTaxiNode *node = seg->getEnd();
1581 length = seg->getLength();
1582 for (FGTaxiSegmentVectorIterator tsi = segments.begin(); tsi != segments.end(); tsi++) {
1583 if (((*tsi)->getEnd() == node) && ((*tsi) != seg)) {
1584 (*tsi)->block(i->getId(), now, now);
1588 for (intVecIterator j = i->getIntentions().begin(); j != i->getIntentions().end(); j++) {
1591 FGTaxiSegment *seg = segments[pos-1];
1592 FGTaxiNode *node = seg->getEnd();
1593 length += seg->getLength();
1594 time_t blockTime = now + (length / vTaxi);
1595 for (FGTaxiSegmentVectorIterator tsi = segments.begin(); tsi != segments.end(); tsi++) {
1596 if (((*tsi)->getEnd() == node) && ((*tsi) != seg)) {
1597 (*tsi)->block(i->getId(), blockTime-30, now);
1605 for (TrafficVectorIterator i = activeTraffic.begin(); i != activeTraffic.end(); i++) {
1607 double vTaxi = (i->getAircraft()->getPerformance()->vTaxi() * SG_NM_TO_METER) / 3600;
1608 i->setPriority(priority++);
1609 int pos = i->getCurrentPosition();
1611 length = segments[pos-1]->getLength();
1612 if (segments[pos-1]->hasBlock(now)) {
1613 //SG_LOG(SG_GENERAL, SG_ALERT, "Taxiway incursion for AI aircraft" << i->getAircraft()->getCallSign());
1618 for (ivi = i->getIntentions().begin(); ivi != i->getIntentions().end(); ivi++) {
1619 int segIndex = (*ivi);
1621 if (segments[segIndex-1]->hasBlock(now))
1625 //after this, ivi points just behind the last valid unblocked taxi segment.
1626 for (intVecIterator j = i->getIntentions().begin(); j != ivi; j++) {
1629 FGTaxiSegment *seg = segments[pos-1];
1630 FGTaxiNode *node = seg->getEnd();
1631 length += seg->getLength();
1632 for (FGTaxiSegmentVectorIterator tsi = segments.begin(); tsi != segments.end(); tsi++) {
1633 if (((*tsi)->getEnd() == node) && ((*tsi) != seg)) {
1634 time_t blockTime = now + (length / vTaxi);
1635 (*tsi)->block(i->getId(), blockTime - 30, now);