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/route/waypoint.hxx>
39 #include <simgear/scene/material/EffectGeode.hxx>
40 #include <simgear/scene/material/matlib.hxx>
41 #include <simgear/scene/material/mat.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()
262 currTraffic = activeTraffic.begin();
265 networkInitialized = false;
269 FGGroundNetwork::~FGGroundNetwork()
271 //cerr << "Running Groundnetwork Destructor " << endl;
272 bool saveData = false;
274 if (fgGetBool("/sim/ai/groundnet-cache")) {
275 SGPath cacheData(fgGetString("/sim/fg-home"));
276 cacheData.append("ai");
277 string airport = parent->getId();
279 if ((airport) != "") {
281 ::snprintf(buffer, 128, "%c/%c/%c/",
282 airport[0], airport[1], airport[2]);
283 cacheData.append(buffer);
284 if (!cacheData.exists()) {
285 cacheData.create_dir(0777);
287 cacheData.append(airport + "-groundnet-cache.txt");
288 cachefile.open(cacheData.str().c_str());
292 cachefile << "[GroundNetcachedata:ref:2011:09:04]" << endl;
293 for (FGTaxiNodeVectorIterator node = nodes.begin();
294 node != nodes.end(); node++) {
296 cachefile << (*node)->getIndex () << " "
297 << (*node)->getElevationM (parent->getElevation()*SG_FEET_TO_METER) << " "
303 pushBackNodes.clear();
304 for (FGTaxiSegmentVectorIterator seg = segments.begin();
305 seg != segments.end(); seg++) {
314 void FGGroundNetwork::saveElevationCache() {
315 //cerr << "Running Groundnetwork Destructor " << endl;
316 bool saveData = false;
318 if (fgGetBool("/sim/ai/groundnet-cache")) {
319 SGPath cacheData(fgGetString("/sim/fg-home"));
320 cacheData.append("ai");
321 string airport = parent->getId();
323 if ((airport) != "") {
325 ::snprintf(buffer, 128, "%c/%c/%c/",
326 airport[0], airport[1], airport[2]);
327 cacheData.append(buffer);
328 if (!cacheData.exists()) {
329 cacheData.create_dir(0777);
331 cacheData.append(airport + "-groundnet-cache.txt");
332 cachefile.open(cacheData.str().c_str());
336 cachefile << "[GroundNetcachedata:ref:2011:09:04]" << endl;
337 for (FGTaxiNodeVectorIterator node = nodes.begin();
338 node != nodes.end(); node++) {
340 cachefile << (*node)->getIndex () << " "
341 << (*node)->getElevationM (parent->getElevation()*SG_FEET_TO_METER) << " "
350 void FGGroundNetwork::addSegment(const FGTaxiSegment & seg)
352 segments.push_back(new FGTaxiSegment(seg));
355 void FGGroundNetwork::addNode(const FGTaxiNode & node)
357 nodes.push_back(new FGTaxiNode(node));
360 void FGGroundNetwork::addNodes(FGParkingVec * parkings)
363 FGParkingVecIterator i = parkings->begin();
364 while (i != parkings->end()) {
365 n.setIndex(i->getIndex());
366 n.setLatitude(i->getLatitude());
367 n.setLongitude(i->getLongitude());
368 n.setElevation(parent->getElevation()*SG_FEET_TO_METER);
369 nodes.push_back(new FGTaxiNode(n));
377 void FGGroundNetwork::init()
379 if (networkInitialized) {
380 FGATCController::init();
381 //cerr << "FGground network already initialized" << endl;
387 sort(nodes.begin(), nodes.end(), compare_nodes);
388 //sort(segments.begin(), segments.end(), compare_segments());
389 FGTaxiSegmentVectorIterator i = segments.begin();
390 while (i != segments.end()) {
391 (*i)->setStart(&nodes);
392 (*i)->setEnd(&nodes);
393 (*i)->setDimensions(parent->getElevation() * SG_FEET_TO_METER);
394 (*i)->setIndex(index);
395 if ((*i)->isPushBack()) {
396 pushBackNodes.push_back((*i)->getEnd());
398 //SG_LOG(SG_GENERAL, SG_BULK, "initializing segment " << (*i)->getIndex() << endl);
399 //SG_LOG(SG_GENERAL, SG_BULK, "Track distance = " << (*i)->getLength() << endl);
400 //SG_LOG(SG_GENERAL, SG_BULK, "Track runs from " << (*i)->getStart()->getIndex() << " to "
401 // << (*i)->getEnd()->getIndex() << endl);
406 i = segments.begin();
407 while (i != segments.end()) {
408 FGTaxiSegmentVectorIterator j = (*i)->getEnd()->getBeginRoute();
409 while (j != (*i)->getEnd()->getEndRoute()) {
410 if ((*j)->getEnd()->getIndex() == (*i)->getStart()->getIndex()) {
411 // int start1 = (*i)->getStart()->getIndex();
412 // int end1 = (*i)->getEnd() ->getIndex();
413 // int start2 = (*j)->getStart()->getIndex();
414 // int end2 = (*j)->getEnd()->getIndex();
415 // int oppIndex = (*j)->getIndex();
416 //cerr << "Opposite of " << (*i)->getIndex() << " (" << start1 << "," << end1 << ") "
417 // << "happens to be " << oppIndex << " (" << start2 << "," << end2 << ") " << endl;
418 (*i)->setOpposite(*j);
425 //FGTaxiNodeVectorIterator j = nodes.begin();
426 //while (j != nodes.end()) {
427 // if ((*j)->getHoldPointType() == 3) {
428 // pushBackNodes.push_back((*j));
432 //cerr << "Done initializing ground network" << endl;
434 if (fgGetBool("/sim/ai/groundnet-cache")) {
435 SGPath cacheData(fgGetString("/sim/fg-home"));
436 cacheData.append("ai");
437 string airport = parent->getId();
439 if ((airport) != "") {
441 ::snprintf(buffer, 128, "%c/%c/%c/",
442 airport[0], airport[1], airport[2]);
443 cacheData.append(buffer);
444 if (!cacheData.exists()) {
445 cacheData.create_dir(0777);
449 cacheData.append(airport + "-groundnet-cache.txt");
450 if (cacheData.exists()) {
451 ifstream data(cacheData.c_str());
454 if (revisionStr != "[GroundNetcachedata:ref:2011:09:04]") {
455 SG_LOG(SG_GENERAL, SG_ALERT,"GroundNetwork Warning: discarding outdated cachefile " <<
456 cacheData.c_str() << " for Airport " << airport);
458 for (FGTaxiNodeVectorIterator i = nodes.begin();
461 (*i)->setElevation(parent->getElevation() * SG_FEET_TO_METER);
462 data >> index >> elev;
465 if (index != (*i)->getIndex()) {
466 SG_LOG(SG_GENERAL, SG_ALERT, "Index read from ground network cache at airport " << airport << " does not match index in the network itself");
468 (*i)->setElevation(elev);
475 //cerr << "Finished initializing " << parent->getId() << " groundnetwork " << endl;
476 networkInitialized = true;
479 int FGGroundNetwork::findNearestNode(const SGGeod & aGeod)
481 double minDist = HUGE_VAL;
484 for (FGTaxiNodeVectorIterator itr = nodes.begin(); itr != nodes.end();
486 double d = SGGeodesy::distanceM(aGeod, (*itr)->getGeod());
489 index = (*itr)->getIndex();
490 //cerr << "Minimum distance of " << minDist << " for index " << index << endl;
497 int FGGroundNetwork::findNearestNodeOnRunway(const SGGeod & aGeod)
499 double minDist = HUGE_VAL;
502 for (FGTaxiNodeVectorIterator itr = nodes.begin(); itr != nodes.end();
504 if (!((*itr)->getIsOnRunway())) {
507 double d = SGGeodesy::distanceM(aGeod, (*itr)->getGeod());
510 index = (*itr)->getIndex();
511 //cerr << "Minimum distance of " << minDist << " for index " << index << endl;
519 int FGGroundNetwork::findNearestNode(double lat, double lon)
521 return findNearestNode(SGGeod::fromDeg(lon, lat));
524 FGTaxiNode *FGGroundNetwork::findNode(unsigned idx)
526 for (FGTaxiNodeVectorIterator
528 itr != nodes.end(); itr++)
530 if (itr->getIndex() == idx)
531 return itr->getAddress();
534 if ((idx >= 0) && (idx < nodes.size()))
535 return nodes[idx]->getAddress();
540 FGTaxiSegment *FGGroundNetwork::findSegment(unsigned idx)
542 for (FGTaxiSegmentVectorIterator
543 itr = segments.begin();
544 itr != segments.end(); itr++)
546 if (itr->getIndex() == idx)
547 return itr->getAddress();
550 if ((idx > 0) && (idx <= segments.size()))
551 return segments[idx - 1]->getAddress();
553 //cerr << "Alert: trying to find invalid segment " << idx << endl;
559 FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end,
562 //implements Dijkstra's algorithm to find shortest distance route from start to end
563 //taken from http://en.wikipedia.org/wiki/Dijkstra's_algorithm
565 //double INFINITE = 100000000000.0;
566 // initialize scoring values
567 int nParkings = parent->getDynamics()->getNrOfParkings();
568 FGTaxiNodeVector *currNodesSet;
570 currNodesSet = &nodes;
572 currNodesSet = &pushBackNodes;
575 for (FGTaxiNodeVectorIterator
576 itr = currNodesSet->begin(); itr != currNodesSet->end(); itr++) {
577 (*itr)->setPathScore(HUGE_VAL); //infinity by all practical means
578 (*itr)->setPreviousNode(0); //
579 (*itr)->setPreviousSeg(0); //
582 FGTaxiNode *firstNode = findNode(start);
585 SG_LOG(SG_GENERAL, SG_ALERT,
586 "Error in ground network. Failed to find first waypoint: " << start
587 << " at " << ((parent) ? parent->getId() : "<unknown>"));
588 return FGTaxiRoute();
590 firstNode->setPathScore(0);
592 FGTaxiNode *lastNode = findNode(end);
595 SG_LOG(SG_GENERAL, SG_ALERT,
596 "Error in ground network. Failed to find last waypoint: " << end
597 << " at " << ((parent) ? parent->getId() : "<unknown>"));
598 return FGTaxiRoute();
601 FGTaxiNodeVector unvisited(*currNodesSet); // working copy
603 while (!unvisited.empty()) {
604 FGTaxiNode *best = *(unvisited.begin());
605 for (FGTaxiNodeVectorIterator
606 itr = unvisited.begin(); itr != unvisited.end(); itr++) {
607 if ((*itr)->getPathScore() < best->getPathScore())
611 FGTaxiNodeVectorIterator newend =
612 remove(unvisited.begin(), unvisited.end(), best);
613 unvisited.erase(newend, unvisited.end());
615 if (best == lastNode) { // found route or best not connected
618 for (FGTaxiSegmentVectorIterator
619 seg = best->getBeginRoute();
620 seg != best->getEndRoute(); seg++) {
621 if (fullSearch || (*seg)->isPushBack()) {
622 FGTaxiNode *tgt = (*seg)->getEnd();
625 SG_LOG(SG_GENERAL, SG_ALERT,
626 "Error in ground network. Found empty segment "
627 << " at " << ((parent) ? parent->getId() : "<unknown>"));
628 return FGTaxiRoute();
631 best->getPathScore() + (*seg)->getLength() +
632 (*seg)->getPenalty(nParkings);
633 if (alt < tgt->getPathScore()) { // Relax (u,v)
634 tgt->setPathScore(alt);
635 tgt->setPreviousNode(best);
636 tgt->setPreviousSeg(*seg); //
639 // // cerr << "Skipping TaxiSegment " << (*seg)->getIndex() << endl;
645 if (lastNode->getPathScore() == HUGE_VAL) {
646 // no valid route found
648 SG_LOG(SG_GENERAL, SG_ALERT,
649 "Failed to find route from waypoint " << start << " to "
650 << end << " at " << parent->getId());
654 //exit(1); //TODO exit more gracefully, no need to stall the whole sim with broken GN's
656 // assemble route from backtrace information
657 intVec nodes, routes;
658 FGTaxiNode *bt = lastNode;
659 while (bt->getPreviousNode() != 0) {
660 nodes.push_back(bt->getIndex());
661 routes.push_back(bt->getPreviousSegment()->getIndex());
662 bt = bt->getPreviousNode();
664 nodes.push_back(start);
665 reverse(nodes.begin(), nodes.end());
666 reverse(routes.begin(), routes.end());
668 return FGTaxiRoute(nodes, routes, lastNode->getPathScore(), 0);
672 int FGTaxiSegment::getPenalty(int nGates)
675 if (end->getIndex() < nGates) {
678 if (end->getIsOnRunway()) { // For now. In future versions, need to find out whether runway is active.
684 /* ATC Related Functions */
686 void FGGroundNetwork::announcePosition(int id,
687 FGAIFlightPlan * intendedRoute,
688 int currentPosition, double lat,
689 double lon, double heading,
690 double speed, double alt,
691 double radius, int leg,
692 FGAIAircraft * aircraft)
695 TrafficVectorIterator i = activeTraffic.begin();
696 // Search search if the current id alread has an entry
697 // This might be faster using a map instead of a vector, but let's start by taking a safe route
698 if (activeTraffic.size()) {
699 //while ((i->getId() != id) && i != activeTraffic.end()) {
700 while (i != activeTraffic.end()) {
701 if (i->getId() == id) {
707 // Add a new TrafficRecord if no one exsists for this aircraft.
708 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
712 rec.setPositionAndIntentions(currentPosition, intendedRoute);
713 rec.setPositionAndHeading(lat, lon, heading, speed, alt);
714 rec.setRadius(radius); // only need to do this when creating the record.
715 rec.setAircraft(aircraft);
717 activeTraffic.push_front(rec);
719 activeTraffic.push_back(rec);
723 i->setPositionAndIntentions(currentPosition, intendedRoute);
724 i->setPositionAndHeading(lat, lon, heading, speed, alt);
729 void FGGroundNetwork::signOff(int id)
731 TrafficVectorIterator i = activeTraffic.begin();
732 // Search search if the current id alread has an entry
733 // This might be faster using a map instead of a vector, but let's start by taking a safe route
734 if (activeTraffic.size()) {
735 //while ((i->getId() != id) && i != activeTraffic.end()) {
736 while (i != activeTraffic.end()) {
737 if (i->getId() == id) {
743 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
744 SG_LOG(SG_GENERAL, SG_ALERT,
745 "AI error: Aircraft without traffic record is signing off at " << SG_ORIGIN);
747 i = activeTraffic.erase(i);
751 * The ground network can deal with the following states:
752 * 0 = Normal; no action required
753 * 1 = "Acknowledge "Hold position
754 * 2 = "Acknowledge "Resume taxi".
755 * 3 = "Issue TaxiClearance"
756 * 4 = Acknowledge Taxi Clearance"
757 * 5 = Post acknowlegde taxiclearance: Start taxiing
759 * 7 = Acknowledge report runway
760 * 8 = Switch tower frequency
761 * 9 = Acknowledge switch tower frequency
762 *************************************************************************************************************************/
763 bool FGGroundNetwork::checkTransmissionState(int minState, int maxState, TrafficVectorIterator i, time_t now, AtcMsgId msgId,
766 int state = i->getState();
767 if ((state >= minState) && (state <= maxState) && available) {
768 if ((msgDir == ATC_AIR_TO_GROUND) && isUserAircraft(i->getAircraft())) {
769 //cerr << "Checking state " << state << " for " << i->getAircraft()->getCallSign() << endl;
770 static SGPropertyNode_ptr trans_num = globals->get_props()->getNode("/sim/atc/transmission-num", true);
771 int n = trans_num->getIntValue();
773 trans_num->setIntValue(-1);
775 //cerr << "Selected transmission message " << n << endl;
776 //FGATCManager *atc = (FGATCManager*) globals->get_subsystem("atc");
777 FGATCDialogNew::instance()->removeEntry(1);
779 //cerr << "creating message for " << i->getAircraft()->getCallSign() << endl;
780 transmit(&(*i), &(*parent->getDynamics()), msgId, msgDir, false);
784 transmit(&(*i), &(*parent->getDynamics()), msgId, msgDir, true);
786 lastTransmission = now;
793 void FGGroundNetwork::updateAircraftInformation(int id, double lat, double lon,
794 double heading, double speed, double alt,
797 time_t currentTime = time(NULL);
798 if (nextSave < currentTime) {
799 saveElevationCache();
800 nextSave = currentTime + 100 + rand() % 200;
802 // Check whether aircraft are on hold due to a preceding pushback. If so, make sure to
803 // Transmit air-to-ground "Ready to taxi request:
804 // Transmit ground to air approval / hold
805 // Transmit confirmation ...
806 // Probably use a status mechanism similar to the Engine start procedure in the startup controller.
809 TrafficVectorIterator i = activeTraffic.begin();
810 // Search search if the current id has an entry
811 // This might be faster using a map instead of a vector, but let's start by taking a safe route
812 TrafficVectorIterator current, closest;
813 if (activeTraffic.size()) {
814 //while ((i->getId() != id) && i != activeTraffic.end()) {
815 while (i != activeTraffic.end()) {
816 if (i->getId() == id) {
822 // update position of the current aircraft
823 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
824 SG_LOG(SG_GENERAL, SG_ALERT,
825 "AI error: updating aircraft without traffic record at " << SG_ORIGIN);
827 i->setPositionAndHeading(lat, lon, heading, speed, alt);
833 // Update every three secs, but add some randomness
834 // to prevent all IA objects doing this in synchrony
835 //if (getDt() < (3.0) + (rand() % 10))
839 current->clearResolveCircularWait();
840 current->setWaitsForId(0);
841 checkSpeedAdjustment(id, lat, lon, heading, speed, alt);
842 bool needsTaxiClearance = current->getAircraft()->getTaxiClearanceRequest();
843 if (!needsTaxiClearance) {
844 checkHoldPosition(id, lat, lon, heading, speed, alt);
845 //if (checkForCircularWaits(id)) {
846 // i->setResolveCircularWait();
849 current->setHoldPosition(true);
850 int state = current->getState();
851 time_t now = time(NULL) + fgGetLong("/sim/time/warp");
852 if ((now - lastTransmission) > 15) {
855 if (checkTransmissionState(0,2, current, now, MSG_REQUEST_TAXI_CLEARANCE, ATC_AIR_TO_GROUND)) {
856 current->setState(3);
858 if (checkTransmissionState(3,3, current, now, MSG_ISSUE_TAXI_CLEARANCE, ATC_GROUND_TO_AIR)) {
859 current->setState(4);
861 if (checkTransmissionState(4,4, current, now, MSG_ACKNOWLEDGE_TAXI_CLEARANCE, ATC_AIR_TO_GROUND)) {
862 current->setState(5);
864 if ((state == 5) && available) {
865 current->setState(0);
866 current->getAircraft()->setTaxiClearanceRequest(false);
867 current->setHoldPosition(false);
875 Scan for a speed adjustment change. Find the nearest aircraft that is in front
876 and adjust speed when we get too close. Only do this when current position and/or
877 intentions of the current aircraft match current taxiroute position of the proximate
878 aircraft. For traffic that is on other routes we need to issue a "HOLD Position"
879 instruction. See below for the hold position instruction.
881 Note that there currently still is one flaw in the logic that needs to be addressed.
882 There can be situations where one aircraft is in front of the current aircraft, on a separate
883 route, but really close after an intersection coming off the current route. This
884 aircraft is still close enough to block the current aircraft. This situation is currently
885 not addressed yet, but should be.
888 void FGGroundNetwork::checkSpeedAdjustment(int id, double lat,
889 double lon, double heading,
890 double speed, double alt)
893 TrafficVectorIterator current, closest, closestOnNetwork;
894 TrafficVectorIterator i = activeTraffic.begin();
895 bool otherReasonToSlowDown = false;
896 bool previousInstruction;
897 if (activeTraffic.size()) {
898 //while ((i->getId() != id) && (i != activeTraffic.end()))
899 while (i != activeTraffic.end()) {
900 if (i->getId() == id) {
908 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
909 SG_LOG(SG_GENERAL, SG_ALERT,
910 "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkSpeedAdjustment at " << SG_ORIGIN);
915 previousInstruction = current->getSpeedAdjustment();
916 double mindist = HUGE_VAL;
917 if (activeTraffic.size()) {
918 double course, dist, bearing, minbearing, az2;
919 SGGeod curr(SGGeod::fromDegM(lon, lat, alt));
920 //TrafficVector iterator closest;
922 closestOnNetwork = current;
923 for (TrafficVectorIterator i = activeTraffic.begin();
924 i != activeTraffic.end(); i++) {
929 SGGeod other(SGGeod::fromDegM(i->getLongitude(),
932 SGGeodesy::inverse(curr, other, course, az2, dist);
933 bearing = fabs(heading - course);
935 bearing = 360 - bearing;
936 if ((dist < mindist) && (bearing < 60.0)) {
939 closestOnNetwork = i;
940 minbearing = bearing;
944 //Check traffic at the tower controller
945 if (towerController->hasActiveTraffic()) {
946 for (TrafficVectorIterator i =
947 towerController->getActiveTraffic().begin();
948 i != towerController->getActiveTraffic().end(); i++) {
949 //cerr << "Comparing " << current->getId() << " and " << i->getId() << endl;
950 SGGeod other(SGGeod::fromDegM(i->getLongitude(),
953 SGGeodesy::inverse(curr, other, course, az2, dist);
954 bearing = fabs(heading - course);
956 bearing = 360 - bearing;
957 if ((dist < mindist) && (bearing < 60.0)) {
958 //cerr << "Current aircraft " << current->getAircraft()->getTrafficRef()->getCallSign()
959 // << " is closest to " << i->getAircraft()->getTrafficRef()->getCallSign()
960 // << ", which has status " << i->getAircraft()->isScheduledForTakeoff()
964 minbearing = bearing;
965 otherReasonToSlowDown = true;
969 // Finally, check UserPosition
970 // Note, as of 2011-08-01, this should no longer be necessecary.
972 double userLatitude = fgGetDouble("/position/latitude-deg");
973 double userLongitude = fgGetDouble("/position/longitude-deg");
974 SGGeod user(SGGeod::fromDeg(userLongitude, userLatitude));
975 SGGeodesy::inverse(curr, user, course, az2, dist);
977 bearing = fabs(heading - course);
979 bearing = 360 - bearing;
980 if ((dist < mindist) && (bearing < 60.0)) {
983 minbearing = bearing;
984 otherReasonToSlowDown = true;
987 current->clearSpeedAdjustment();
988 bool needBraking = false;
989 if (current->checkPositionAndIntentions(*closest)
990 || otherReasonToSlowDown) {
991 double maxAllowableDistance =
992 (1.1 * current->getRadius()) +
993 (1.1 * closest->getRadius());
994 if (mindist < 2 * maxAllowableDistance) {
995 if (current->getId() == closest->getWaitsForId())
998 current->setWaitsForId(closest->getId());
999 if (closest->getId() != current->getId()) {
1000 current->setSpeedAdjustment(closest->getSpeed() *
1005 // closest->getAircraft()->getTakeOffStatus() &&
1006 // (current->getAircraft()->getTrafficRef()->getDepartureAirport() == closest->getAircraft()->getTrafficRef()->getDepartureAirport()) &&
1007 // (current->getAircraft()->GetFlightPlan()->getRunway() == closest->getAircraft()->GetFlightPlan()->getRunway())
1009 // current->getAircraft()->scheduleForATCTowerDepartureControl(1);
1011 current->setSpeedAdjustment(0); // This can only happen when the user aircraft is the one closest
1013 if (mindist < maxAllowableDistance) {
1014 //double newSpeed = (maxAllowableDistance-mindist);
1015 //current->setSpeedAdjustment(newSpeed);
1016 //if (mindist < 0.5* maxAllowableDistance)
1018 current->setSpeedAdjustment(0);
1023 if ((closest->getId() == closestOnNetwork->getId()) && (current->getPriority() < closest->getPriority()) && needBraking) {
1024 swap(current, closest);
1030 Check for "Hold position instruction".
1031 The hold position should be issued under the following conditions:
1032 1) For aircraft entering or crossing a runway with active traffic on it, or landing aircraft near it
1033 2) For taxiing aircraft that use one taxiway in opposite directions
1034 3) For crossing or merging taxiroutes.
1037 void FGGroundNetwork::checkHoldPosition(int id, double lat,
1038 double lon, double heading,
1039 double speed, double alt)
1041 TrafficVectorIterator current;
1042 TrafficVectorIterator i = activeTraffic.begin();
1043 if (activeTraffic.size()) {
1044 //while ((i->getId() != id) && i != activeTraffic.end())
1045 while (i != activeTraffic.end()) {
1046 if (i->getId() == id) {
1054 time_t now = time(NULL) + fgGetLong("/sim/time/warp");
1055 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
1056 SG_LOG(SG_GENERAL, SG_ALERT,
1057 "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkHoldPosition at " << SG_ORIGIN);
1061 if (current->getAircraft()->getTakeOffStatus() == 1) {
1062 current->setHoldPosition(true);
1065 if (current->getAircraft()->getTakeOffStatus() == 2) {
1066 //cerr << current->getAircraft()->getCallSign() << ". Taxi in position and hold" << endl;
1067 current->setHoldPosition(false);
1068 current->clearSpeedAdjustment();
1071 bool origStatus = current->hasHoldPosition();
1072 current->setHoldPosition(false);
1073 SGGeod curr(SGGeod::fromDegM(lon, lat, alt));
1074 int currentRoute = i->getCurrentPosition();
1076 if (i->getIntentions().size()) {
1077 nextRoute = (*(i->getIntentions().begin()));
1081 if (currentRoute > 0) {
1082 FGTaxiSegment *tx = findSegment(currentRoute);
1085 nx = findSegment(nextRoute);
1089 //if (tx->hasBlock(now) || nx->hasBlock(now) ) {
1090 // current->setHoldPosition(true);
1092 SGGeod start(SGGeod::fromDeg((i->getLongitude()), (i->getLatitude())));
1093 SGGeod end (SGGeod::fromDeg(nx->getStart()->getLongitude(), nx->getStart()->getLatitude()));
1095 double distance = SGGeodesy::distanceM(start, end);
1096 if (nx->hasBlock(now) && (distance < i->getRadius() * 4)) {
1097 current->setHoldPosition(true);
1099 intVecIterator ivi = i->getIntentions().begin();
1100 while (ivi != i->getIntentions().end()) {
1102 distance += segments[(*ivi)-1]->getLength();
1103 if ((segments[(*ivi)-1]->hasBlock(now)) && (distance < i->getRadius() * 4)) {
1104 current->setHoldPosition(true);
1112 bool currStatus = current->hasHoldPosition();
1113 current->setHoldPosition(origStatus);
1114 // Either a Hold Position or a resume taxi transmission has been issued
1115 if ((now - lastTransmission) > 2) {
1118 if (current->getState() == 0) {
1119 if ((origStatus != currStatus) && available) {
1120 //cerr << "Issueing hold short instrudtion " << currStatus << " " << available << endl;
1121 if (currStatus == true) { // No has a hold short instruction
1122 transmit(&(*current), &(*parent->getDynamics()), MSG_HOLD_POSITION, ATC_GROUND_TO_AIR, true);
1123 //cerr << "Transmittin hold short instrudtion " << currStatus << " " << available << endl;
1124 current->setState(1);
1126 transmit(&(*current), &(*parent->getDynamics()), MSG_RESUME_TAXI, ATC_GROUND_TO_AIR, true);
1127 //cerr << "Transmittig resume instrudtion " << currStatus << " " << available << endl;
1128 current->setState(2);
1130 lastTransmission = now;
1132 // Don't act on the changed instruction until the transmission is confirmed
1133 // So set back to original status
1134 //cerr << "Current state " << current->getState() << endl;
1138 // 6 = Report runway
1139 // 7 = Acknowledge report runway
1140 // 8 = Switch tower frequency
1141 //9 = Acknowledge switch tower frequency
1143 //int state = current->getState();
1144 if (checkTransmissionState(1,1, current, now, MSG_ACKNOWLEDGE_HOLD_POSITION, ATC_AIR_TO_GROUND)) {
1145 current->setState(0);
1146 current->setHoldPosition(true);
1148 if (checkTransmissionState(2,2, current, now, MSG_ACKNOWLEDGE_RESUME_TAXI, ATC_AIR_TO_GROUND)) {
1149 current->setState(0);
1150 current->setHoldPosition(false);
1152 if (current->getAircraft()->getTakeOffStatus() && (current->getState() == 0)) {
1153 //cerr << "Scheduling " << current->getAircraft()->getCallSign() << " for hold short" << endl;
1154 current->setState(6);
1156 if (checkTransmissionState(6,6, current, now, MSG_REPORT_RUNWAY_HOLD_SHORT, ATC_AIR_TO_GROUND)) {
1158 if (checkTransmissionState(7,7, current, now, MSG_ACKNOWLEDGE_REPORT_RUNWAY_HOLD_SHORT, ATC_GROUND_TO_AIR)) {
1160 if (checkTransmissionState(8,8, current, now, MSG_SWITCH_TOWER_FREQUENCY, ATC_GROUND_TO_AIR)) {
1162 if (checkTransmissionState(9,9, current, now, MSG_ACKNOWLEDGE_SWITCH_TOWER_FREQUENCY, ATC_AIR_TO_GROUND)) {
1167 //current->setState(0);
1171 * Check whether situations occur where the current aircraft is waiting for itself
1172 * due to higher order interactions.
1173 * A 'circular' wait is a situation where a waits for b, b waits for c, and c waits
1174 * for a. Ideally each aircraft only waits for one other aircraft, so by tracing
1175 * through this list of waiting aircraft, we can check if we'd eventually end back
1176 * at the current aircraft.
1178 * Note that we should consider the situation where we are actually checking aircraft
1179 * d, which is waiting for aircraft a. d is not part of the loop, but is held back by
1180 * the looping aircraft. If we don't check for that, this function will get stuck into
1184 bool FGGroundNetwork::checkForCircularWaits(int id)
1186 //cerr << "Performing Wait check " << id << endl;
1188 TrafficVectorIterator current, other;
1189 TrafficVectorIterator i = activeTraffic.begin();
1190 int trafficSize = activeTraffic.size();
1192 while (i != activeTraffic.end()) {
1193 if (i->getId() == id) {
1201 if (i == activeTraffic.end() || (trafficSize == 0)) {
1202 SG_LOG(SG_GENERAL, SG_ALERT,
1203 "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits at " << SG_ORIGIN);
1207 target = current->getWaitsForId();
1208 //bool printed = false; // Note that this variable is for debugging purposes only.
1212 //cerr << "aircraft waits for user" << endl;
1217 while ((target > 0) && (target != id) && counter++ < trafficSize) {
1219 TrafficVectorIterator i = activeTraffic.begin();
1221 //while ((i->getId() != id) && i != activeTraffic.end())
1222 while (i != activeTraffic.end()) {
1223 if (i->getId() == target) {
1231 if (i == activeTraffic.end() || (trafficSize == 0)) {
1232 //cerr << "[Waiting for traffic at Runway: DONE] " << endl << endl;;
1233 // The target id is not found on the current network, which means it's at the tower
1234 //SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits");
1238 target = other->getWaitsForId();
1240 // actually this trap isn't as impossible as it first seemed:
1241 // the setWaitsForID(id) is set to current when the aircraft
1242 // is waiting for the user controlled aircraft.
1243 //if (current->getId() == other->getId()) {
1244 // cerr << "Caught the impossible trap" << endl;
1245 // cerr << "Current = " << current->getId() << endl;
1246 // cerr << "Other = " << other ->getId() << endl;
1247 // for (TrafficVectorIterator at = activeTraffic.begin();
1248 // at != activeTraffic.end();
1250 // cerr << "currently active aircraft : " << at->getCallSign() << " with Id " << at->getId() << " waits for " << at->getWaitsForId() << endl;
1253 if (current->getId() == other->getId())
1256 //cerr << current->getCallSign() << " (" << current->getId() << ") " << " -> " << other->getCallSign()
1257 // << " (" << other->getId() << "); " << endl;;
1267 // cerr << "[done] " << endl << endl;;
1269 SG_LOG(SG_GENERAL, SG_WARN,
1270 "Detected circular wait condition: Id = " << id <<
1271 "target = " << target);
1278 // Note that this function is probably obsolete...
1279 bool FGGroundNetwork::hasInstruction(int id)
1281 TrafficVectorIterator i = activeTraffic.begin();
1282 // Search search if the current id has an entry
1283 // This might be faster using a map instead of a vector, but let's start by taking a safe route
1284 if (activeTraffic.size()) {
1285 //while ((i->getId() != id) && i != activeTraffic.end()) {
1286 while (i != activeTraffic.end()) {
1287 if (i->getId() == id) {
1293 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
1294 SG_LOG(SG_GENERAL, SG_ALERT,
1295 "AI error: checking ATC instruction for aircraft without traffic record at " << SG_ORIGIN);
1297 return i->hasInstruction();
1302 FGATCInstruction FGGroundNetwork::getInstruction(int id)
1304 TrafficVectorIterator i = activeTraffic.begin();
1305 // Search search if the current id has an entry
1306 // This might be faster using a map instead of a vector, but let's start by taking a safe route
1307 if (activeTraffic.size()) {
1308 //while ((i->getId() != id) && i != activeTraffic.end()) {
1309 while (i != activeTraffic.end()) {
1310 if (i->getId() == id) {
1316 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
1317 SG_LOG(SG_GENERAL, SG_ALERT,
1318 "AI error: requesting ATC instruction for aircraft without traffic record at " << SG_ORIGIN);
1320 return i->getInstruction();
1322 return FGATCInstruction();
1325 // Note that this function is copied from simgear. for maintanance purposes, it's probabtl better to make a general function out of that.
1326 static void WorldCoordinate(osg::Matrix& obj_pos, double lat,
1327 double lon, double elev, double hdg, double slope)
1329 SGGeod geod = SGGeod::fromDegM(lon, lat, elev);
1330 obj_pos = geod.makeZUpFrame();
1331 // hdg is not a compass heading, but a counter-clockwise rotation
1332 // around the Z axis
1333 obj_pos.preMult(osg::Matrix::rotate(hdg * SGD_DEGREES_TO_RADIANS,
1335 obj_pos.preMult(osg::Matrix::rotate(slope * SGD_DEGREES_TO_RADIANS,
1342 void FGGroundNetwork::render(bool visible)
1345 SGMaterialLib *matlib = globals->get_matlib();
1348 globals->get_scenery()->get_scene_graph()->removeChild(group);
1349 //while (group->getNumChildren()) {
1350 // cerr << "Number of children: " << group->getNumChildren() << endl;
1351 //simgear::EffectGeode* geode = (simgear::EffectGeode*) group->getChild(0);
1352 //osg::MatrixTransform *obj_trans = (osg::MatrixTransform*) group->getChild(0);
1353 //geode->releaseGLObjects();
1354 //group->removeChild(geode);
1359 group = new osg::Group;
1360 FGScenery * local_scenery = globals->get_scenery();
1361 // double elevation_meters = 0.0;
1362 double elevation_feet = 0.0;
1363 time_t now = time(NULL) + fgGetLong("/sim/time/warp");
1364 //for ( FGTaxiSegmentVectorIterator i = segments.begin(); i != segments.end(); i++) {
1366 for (TrafficVectorIterator i = activeTraffic.begin(); i != activeTraffic.end(); i++) {
1367 // Handle start point
1368 int pos = i->getCurrentPosition() - 1;
1371 SGGeod start(SGGeod::fromDeg((i->getLongitude()), (i->getLatitude())));
1372 SGGeod end (SGGeod::fromDeg(segments[pos]->getEnd()->getLongitude(), segments[pos]->getEnd()->getLatitude()));
1374 double length = SGGeodesy::distanceM(start, end);
1375 //heading = SGGeodesy::headingDeg(start->getGeod(), end->getGeod());
1377 double az2, heading; //, distanceM;
1378 SGGeodesy::inverse(start, end, heading, az2, length);
1379 double coveredDistance = length * 0.5;
1381 SGGeodesy::direct(start, heading, coveredDistance, center, az2);
1382 //cerr << "Active Aircraft : Centerpoint = (" << center.getLatitudeDeg() << ", " << center.getLongitudeDeg() << "). Heading = " << heading << endl;
1383 ///////////////////////////////////////////////////////////////////////////////
1384 // Make a helper function out of this
1385 osg::Matrix obj_pos;
1386 osg::MatrixTransform *obj_trans = new osg::MatrixTransform;
1387 obj_trans->setDataVariance(osg::Object::STATIC);
1388 // Experimental: Calculate slope here, based on length, and the individual elevations
1389 double elevationStart;
1390 if (isUserAircraft((i)->getAircraft())) {
1391 elevationStart = fgGetDouble("/position/ground-elev-m");
1393 elevationStart = ((i)->getAircraft()->_getAltitude());
1395 double elevationEnd = segments[pos]->getEnd()->getElevationM(parent->getElevation()*SG_FEET_TO_METER);
1396 //cerr << "Using elevation " << elevationEnd << endl;
1398 if ((elevationEnd == 0) || (elevationEnd = parent->getElevation())) {
1399 SGGeod center2 = end;
1400 center2.setElevationM(SG_MAX_ELEVATION_M);
1401 if (local_scenery->get_elevation_m( center2, elevationEnd, NULL )) {
1402 elevation_feet = elevationEnd * SG_METER_TO_FEET + 0.5;
1403 //elevation_meters += 0.5;
1406 elevationEnd = parent->getElevation();
1408 segments[pos]->getEnd()->setElevation(elevationEnd);
1410 double elevationMean = (elevationStart + elevationEnd) / 2.0;
1411 double elevDiff = elevationEnd - elevationStart;
1413 double slope = atan2(elevDiff, length) * SGD_RADIANS_TO_DEGREES;
1415 //cerr << "1. Using mean elevation : " << elevationMean << " and " << slope << endl;
1417 WorldCoordinate( obj_pos, center.getLatitudeDeg(), center.getLongitudeDeg(), elevationMean+ 0.5, -(heading), slope );
1419 obj_trans->setMatrix( obj_pos );
1420 //osg::Vec3 center(0, 0, 0)
1422 float width = length /2.0;
1423 osg::Vec3 corner(-width, 0, 0.25f);
1424 osg::Vec3 widthVec(2*width + 1, 0, 0);
1425 osg::Vec3 heightVec(0, 1, 0);
1426 osg::Geometry* geometry;
1427 geometry = osg::createTexturedQuadGeometry(corner, widthVec, heightVec);
1428 simgear::EffectGeode* geode = new simgear::EffectGeode;
1429 geode->setName("test");
1430 geode->addDrawable(geometry);
1431 //osg::Node *custom_obj;
1433 if (segments[pos]->hasBlock(now)) {
1434 mat = matlib->find("UnidirectionalTaperRed");
1436 mat = matlib->find("UnidirectionalTaperGreen");
1439 geode->setEffect(mat->get_effect());
1440 obj_trans->addChild(geode);
1441 // wire as much of the scene graph together as we can
1442 //->addChild( obj_trans );
1443 group->addChild( obj_trans );
1444 /////////////////////////////////////////////////////////////////////
1446 //cerr << "BIG FAT WARNING: current position is here : " << pos << endl;
1448 for (intVecIterator j = (i)->getIntentions().begin(); j != (i)->getIntentions().end(); j++) {
1449 osg::Matrix obj_pos;
1452 osg::MatrixTransform *obj_trans = new osg::MatrixTransform;
1453 obj_trans->setDataVariance(osg::Object::STATIC);
1455 // Experimental: Calculate slope here, based on length, and the individual elevations
1456 double elevationStart = segments[k]->getStart()->getElevationM(parent->getElevation()*SG_FEET_TO_METER);
1457 double elevationEnd = segments[k]->getEnd ()->getElevationM(parent->getElevation()*SG_FEET_TO_METER);
1458 if ((elevationStart == 0) || (elevationStart == parent->getElevation())) {
1459 SGGeod center2 = segments[k]->getStart()->getGeod();
1460 center2.setElevationM(SG_MAX_ELEVATION_M);
1461 if (local_scenery->get_elevation_m( center2, elevationStart, NULL )) {
1462 elevation_feet = elevationStart * SG_METER_TO_FEET + 0.5;
1463 //elevation_meters += 0.5;
1466 elevationStart = parent->getElevation();
1468 segments[k]->getStart()->setElevation(elevationStart);
1470 if ((elevationEnd == 0) || (elevationEnd == parent->getElevation())) {
1471 SGGeod center2 = segments[k]->getEnd()->getGeod();
1472 center2.setElevationM(SG_MAX_ELEVATION_M);
1473 if (local_scenery->get_elevation_m( center2, elevationEnd, NULL )) {
1474 elevation_feet = elevationEnd * SG_METER_TO_FEET + 0.5;
1475 //elevation_meters += 0.5;
1478 elevationEnd = parent->getElevation();
1480 segments[k]->getEnd()->setElevation(elevationEnd);
1483 double elevationMean = (elevationStart + elevationEnd) / 2.0;
1484 double elevDiff = elevationEnd - elevationStart;
1485 double length = segments[k]->getLength();
1486 double slope = atan2(elevDiff, length) * SGD_RADIANS_TO_DEGREES;
1488 // cerr << "2. Using mean elevation : " << elevationMean << " and " << slope << endl;
1491 WorldCoordinate( obj_pos, segments[k]->getLatitude(), segments[k]->getLongitude(), elevationMean+ 0.5, -(segments[k]->getHeading()), slope );
1493 obj_trans->setMatrix( obj_pos );
1494 //osg::Vec3 center(0, 0, 0)
1496 float width = segments[k]->getLength() /2.0;
1497 osg::Vec3 corner(-width, 0, 0.25f);
1498 osg::Vec3 widthVec(2*width + 1, 0, 0);
1499 osg::Vec3 heightVec(0, 1, 0);
1500 osg::Geometry* geometry;
1501 geometry = osg::createTexturedQuadGeometry(corner, widthVec, heightVec);
1502 simgear::EffectGeode* geode = new simgear::EffectGeode;
1503 geode->setName("test");
1504 geode->addDrawable(geometry);
1505 //osg::Node *custom_obj;
1507 if (segments[k]->hasBlock(now)) {
1508 mat = matlib->find("UnidirectionalTaperRed");
1510 mat = matlib->find("UnidirectionalTaperGreen");
1513 geode->setEffect(mat->get_effect());
1514 obj_trans->addChild(geode);
1515 // wire as much of the scene graph together as we can
1516 //->addChild( obj_trans );
1517 group->addChild( obj_trans );
1522 globals->get_scenery()->get_scene_graph()->addChild(group);
1526 string FGGroundNetwork::getName() {
1527 return string(parent->getId() + "-ground");
1530 void FGGroundNetwork::update(double dt)
1532 time_t now = time(NULL) + fgGetLong("/sim/time/warp");
1533 for (FGTaxiSegmentVectorIterator tsi = segments.begin(); tsi != segments.end(); tsi++) {
1534 (*tsi)->unblock(now);
1537 //sort(activeTraffic.begin(), activeTraffic.end(), compare_trafficrecords);
1538 // Handle traffic that is under ground control first; this way we'll prevent clutter at the gate areas.
1539 // Don't allow an aircraft to pushback when a taxiing aircraft is currently using part of the intended route.
1540 for (TrafficVectorIterator i = parent->getDynamics()->getStartupController()->getActiveTraffic().begin();
1541 i != parent->getDynamics()->getStartupController()->getActiveTraffic().end(); i++) {
1543 i->setPriority(priority++);
1544 // in meters per second;
1545 double vTaxi = (i->getAircraft()->getPerformance()->vTaxi() * SG_NM_TO_METER) / 3600;
1546 if (i->isActive(0)) {
1548 // Check for all active aircraft whether it's current pos segment is
1549 // an opposite of one of the departing aircraft's intentions
1550 for (TrafficVectorIterator j = activeTraffic.begin(); j != activeTraffic.end(); j++) {
1551 int pos = j->getCurrentPosition();
1553 FGTaxiSegment *seg = segments[pos-1]->opposite();
1555 int posReverse = seg->getIndex();
1556 for (intVecIterator k = i->getIntentions().begin(); k != i->getIntentions().end(); k++) {
1557 if ((*k) == posReverse) {
1559 segments[posReverse-1]->block(i->getId(), now, now);
1565 // if the current aircraft is still allowed to pushback, we can start reserving a route for if by blocking all the entry taxiways.
1566 if (i->pushBackAllowed()) {
1568 int pos = i->getCurrentPosition();
1570 FGTaxiSegment *seg = segments[pos-1];
1571 FGTaxiNode *node = seg->getEnd();
1572 length = seg->getLength();
1573 for (FGTaxiSegmentVectorIterator tsi = segments.begin(); tsi != segments.end(); tsi++) {
1574 if (((*tsi)->getEnd() == node) && ((*tsi) != seg)) {
1575 (*tsi)->block(i->getId(), now, now);
1579 for (intVecIterator j = i->getIntentions().begin(); j != i->getIntentions().end(); j++) {
1582 FGTaxiSegment *seg = segments[pos-1];
1583 FGTaxiNode *node = seg->getEnd();
1584 length += seg->getLength();
1585 time_t blockTime = now + (length / vTaxi);
1586 for (FGTaxiSegmentVectorIterator tsi = segments.begin(); tsi != segments.end(); tsi++) {
1587 if (((*tsi)->getEnd() == node) && ((*tsi) != seg)) {
1588 (*tsi)->block(i->getId(), blockTime-30, now);
1596 for (TrafficVectorIterator i = activeTraffic.begin(); i != activeTraffic.end(); i++) {
1598 double vTaxi = (i->getAircraft()->getPerformance()->vTaxi() * SG_NM_TO_METER) / 3600;
1599 i->setPriority(priority++);
1600 int pos = i->getCurrentPosition();
1602 length = segments[pos-1]->getLength();
1603 if (segments[pos-1]->hasBlock(now)) {
1604 //SG_LOG(SG_GENERAL, SG_ALERT, "Taxiway incursion for AI aircraft" << i->getAircraft()->getCallSign());
1609 for (ivi = i->getIntentions().begin(); ivi != i->getIntentions().end(); ivi++) {
1610 int segIndex = (*ivi);
1612 if (segments[segIndex-1]->hasBlock(now))
1616 //after this, ivi points just behind the last valid unblocked taxi segment.
1617 for (intVecIterator j = i->getIntentions().begin(); j != ivi; j++) {
1620 FGTaxiSegment *seg = segments[pos-1];
1621 FGTaxiNode *node = seg->getEnd();
1622 length += seg->getLength();
1623 for (FGTaxiSegmentVectorIterator tsi = segments.begin(); tsi != segments.end(); tsi++) {
1624 if (((*tsi)->getEnd() == node) && ((*tsi) != seg)) {
1625 time_t blockTime = now + (length / vTaxi);
1626 (*tsi)->block(i->getId(), blockTime - 30, now);