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.
32 #include <osg/Geometry>
33 #include <osg/MatrixTransform>
36 #include <simgear/debug/logstream.hxx>
37 #include <simgear/route/waypoint.hxx>
38 #include <simgear/scene/material/EffectGeode.hxx>
39 #include <simgear/scene/material/matlib.hxx>
40 #include <simgear/scene/material/mat.hxx>
42 #include <Airports/simple.hxx>
43 #include <Airports/dynamics.hxx>
45 #include <AIModel/AIAircraft.hxx>
46 #include <AIModel/performancedata.hxx>
47 #include <AIModel/AIFlightPlan.hxx>
49 #include <ATC/atc_mgr.hxx>
51 #include <Scenery/scenery.hxx>
53 #include "groundnetwork.hxx"
56 /***************************************************************************
58 **************************************************************************/
60 void FGTaxiSegment::setStart(FGTaxiNodeVector * nodes)
62 FGTaxiNodeVectorIterator i = nodes->begin();
63 while (i != nodes->end()) {
64 //cerr << "Scanning start node index" << (*i)->getIndex() << endl;
65 if ((*i)->getIndex() == startNode) {
66 start = (*i)->getAddress();
67 (*i)->addSegment(this);
72 SG_LOG(SG_GENERAL, SG_ALERT,
73 "Could not find start node " << startNode << endl);
76 void FGTaxiSegment::setEnd(FGTaxiNodeVector * nodes)
78 FGTaxiNodeVectorIterator i = nodes->begin();
79 while (i != nodes->end()) {
80 //cerr << "Scanning end node index" << (*i)->getIndex() << endl;
81 if ((*i)->getIndex() == endNode) {
82 end = (*i)->getAddress();
87 SG_LOG(SG_GENERAL, SG_ALERT,
88 "Could not find end node " << endNode << endl);
93 // There is probably a computationally cheaper way of
95 void FGTaxiSegment::setDimensions(double elevation)
97 length = SGGeodesy::distanceM(start->getGeod(), end->getGeod());
98 //heading = SGGeodesy::headingDeg(start->getGeod(), end->getGeod());
100 double az2; //, distanceM;
101 SGGeodesy::inverse(start->getGeod(), end->getGeod(), heading, az2, length);
102 double coveredDistance = length * 0.5;
103 SGGeodesy::direct(start->getGeod(), heading, coveredDistance, center, az2);
104 //start->setElevation(elevation);
105 //end->setElevation(elevation);
106 //cerr << "Centerpoint = (" << center.getLatitudeDeg() << ", " << center.getLongitudeDeg() << "). Heading = " << heading << endl;
110 //void FGTaxiSegment::setCourseDiff(double crse)
112 // headingDiff = fabs(course - crse);
114 // if (headingDiff > 180)
115 // headingDiff = fabs(headingDiff - 360);
119 /***************************************************************************
121 **************************************************************************/
122 bool FGTaxiRoute::next(int *nde)
124 //for (intVecIterator i = nodes.begin(); i != nodes.end(); i++)
125 // cerr << "FGTaxiRoute contains : " << *(i) << endl;
126 //cerr << "Offset from end: " << nodes.end() - currNode << endl;
127 //if (currNode != nodes.end())
128 // cerr << "true" << endl;
130 // cerr << "false" << endl;
131 //if (nodes.size() != (routes.size()) +1)
132 // cerr << "ALERT: Misconfigured TaxiRoute : " << nodes.size() << " " << routes.size() << endl;
134 if (currNode == nodes.end())
137 if (currNode != nodes.begin()) // make sure route corresponds to the end node
143 bool FGTaxiRoute::next(int *nde, int *rte)
145 //for (intVecIterator i = nodes.begin(); i != nodes.end(); i++)
146 // cerr << "FGTaxiRoute contains : " << *(i) << endl;
147 //cerr << "Offset from end: " << nodes.end() - currNode << endl;
148 //if (currNode != nodes.end())
149 // cerr << "true" << endl;
151 // cerr << "false" << endl;
152 if (nodes.size() != (routes.size()) + 1) {
153 SG_LOG(SG_GENERAL, SG_ALERT,
154 "ALERT: Misconfigured TaxiRoute : " << nodes.
155 size() << " " << routes.size());
158 if (currNode == nodes.end())
161 //*rte = *(currRoute);
162 if (currNode != nodes.begin()) // Make sure route corresponds to the end node
167 // If currNode points to the first node, this means the aircraft is not on the taxi node
168 // yet. Make sure to return a unique identifyer in this situation though, because otherwise
169 // the speed adjust AI code may be unable to resolve whether two aircraft are on the same
170 // taxi route or not. the negative of the preceding route seems a logical choice, as it is
171 // unique for any starting location.
172 // Note that this is probably just a temporary fix until I get Parking / tower control working.
173 *rte = -1 * *(currRoute);
180 void FGTaxiRoute::rewind(int route)
186 if (!(next(&currPoint, &currRoute))) {
187 SG_LOG(SG_GENERAL, SG_ALERT,
188 "Error in rewinding TaxiRoute: current" << currRoute <<
191 } while (currRoute != route);
197 /***************************************************************************
199 **************************************************************************/
200 bool compare_nodes(FGTaxiNode * a, FGTaxiNode * b)
205 bool compare_segments(FGTaxiSegment * a, FGTaxiSegment * b)
210 bool compare_trafficrecords(FGTrafficRecord a, FGTrafficRecord b)
212 return (a.getIntentions().size() < b.getIntentions().size());
215 FGGroundNetwork::FGGroundNetwork()
223 currTraffic = activeTraffic.begin();
226 networkInitialized = false;
230 FGGroundNetwork::~FGGroundNetwork()
232 //cerr << "Running Groundnetwork Destructor " << endl;
233 bool saveData = false;
235 if (fgGetBool("/sim/ai/groundnet-cache")) {
236 SGPath cacheData(fgGetString("/sim/fg-home"));
237 cacheData.append("ai");
238 string airport = parent->getId();
240 if ((airport) != "") {
242 ::snprintf(buffer, 128, "%c/%c/%c/",
243 airport[0], airport[1], airport[2]);
244 cacheData.append(buffer);
245 if (!cacheData.exists()) {
246 cacheData.create_dir(0777);
248 cacheData.append(airport + "-groundnet-cache.txt");
249 cachefile.open(cacheData.str().c_str());
253 cachefile << "[GroundNetcachedata:ref:2011:09:04]" << endl;
254 for (FGTaxiNodeVectorIterator node = nodes.begin();
255 node != nodes.end(); node++) {
257 cachefile << (*node)->getIndex () << " "
258 << (*node)->getElevationM (parent->getElevation()*SG_FEET_TO_METER) << " "
264 pushBackNodes.clear();
265 for (FGTaxiSegmentVectorIterator seg = segments.begin();
266 seg != segments.end(); seg++) {
275 void FGGroundNetwork::saveElevationCache() {
276 //cerr << "Running Groundnetwork Destructor " << endl;
277 bool saveData = false;
279 if (fgGetBool("/sim/ai/groundnet-cache")) {
280 SGPath cacheData(fgGetString("/sim/fg-home"));
281 cacheData.append("ai");
282 string airport = parent->getId();
284 if ((airport) != "") {
286 ::snprintf(buffer, 128, "%c/%c/%c/",
287 airport[0], airport[1], airport[2]);
288 cacheData.append(buffer);
289 if (!cacheData.exists()) {
290 cacheData.create_dir(0777);
292 cacheData.append(airport + "-groundnet-cache.txt");
293 cachefile.open(cacheData.str().c_str());
297 cachefile << "[GroundNetcachedata:ref:2011:09:04]" << endl;
298 for (FGTaxiNodeVectorIterator node = nodes.begin();
299 node != nodes.end(); node++) {
301 cachefile << (*node)->getIndex () << " "
302 << (*node)->getElevationM (parent->getElevation()*SG_FEET_TO_METER) << " "
311 void FGGroundNetwork::addSegment(const FGTaxiSegment & seg)
313 segments.push_back(new FGTaxiSegment(seg));
316 void FGGroundNetwork::addNode(const FGTaxiNode & node)
318 nodes.push_back(new FGTaxiNode(node));
321 void FGGroundNetwork::addNodes(FGParkingVec * parkings)
324 FGParkingVecIterator i = parkings->begin();
325 while (i != parkings->end()) {
326 n.setIndex(i->getIndex());
327 n.setLatitude(i->getLatitude());
328 n.setLongitude(i->getLongitude());
329 n.setElevation(parent->getElevation()*SG_FEET_TO_METER);
330 nodes.push_back(new FGTaxiNode(n));
338 void FGGroundNetwork::init()
340 if (networkInitialized) {
341 FGATCController::init();
342 //cerr << "FGground network already initialized" << endl;
348 sort(nodes.begin(), nodes.end(), compare_nodes);
349 //sort(segments.begin(), segments.end(), compare_segments());
350 FGTaxiSegmentVectorIterator i = segments.begin();
351 while (i != segments.end()) {
352 (*i)->setStart(&nodes);
353 (*i)->setEnd(&nodes);
354 (*i)->setDimensions(parent->getElevation() * SG_FEET_TO_METER);
355 (*i)->setIndex(index);
356 if ((*i)->isPushBack()) {
357 pushBackNodes.push_back((*i)->getEnd());
359 //SG_LOG(SG_GENERAL, SG_BULK, "initializing segment " << (*i)->getIndex() << endl);
360 //SG_LOG(SG_GENERAL, SG_BULK, "Track distance = " << (*i)->getLength() << endl);
361 //SG_LOG(SG_GENERAL, SG_BULK, "Track runs from " << (*i)->getStart()->getIndex() << " to "
362 // << (*i)->getEnd()->getIndex() << endl);
367 i = segments.begin();
368 while (i != segments.end()) {
369 FGTaxiSegmentVectorIterator j = (*i)->getEnd()->getBeginRoute();
370 while (j != (*i)->getEnd()->getEndRoute()) {
371 if ((*j)->getEnd()->getIndex() == (*i)->getStart()->getIndex()) {
372 // int start1 = (*i)->getStart()->getIndex();
373 // int end1 = (*i)->getEnd() ->getIndex();
374 // int start2 = (*j)->getStart()->getIndex();
375 // int end2 = (*j)->getEnd()->getIndex();
376 // int oppIndex = (*j)->getIndex();
377 //cerr << "Opposite of " << (*i)->getIndex() << " (" << start1 << "," << end1 << ") "
378 // << "happens to be " << oppIndex << " (" << start2 << "," << end2 << ") " << endl;
379 (*i)->setOpposite(*j);
386 //FGTaxiNodeVectorIterator j = nodes.begin();
387 //while (j != nodes.end()) {
388 // if ((*j)->getHoldPointType() == 3) {
389 // pushBackNodes.push_back((*j));
393 //cerr << "Done initializing ground network" << endl;
395 if (fgGetBool("/sim/ai/groundnet-cache")) {
396 SGPath cacheData(fgGetString("/sim/fg-home"));
397 cacheData.append("ai");
398 string airport = parent->getId();
400 if ((airport) != "") {
402 ::snprintf(buffer, 128, "%c/%c/%c/",
403 airport[0], airport[1], airport[2]);
404 cacheData.append(buffer);
405 if (!cacheData.exists()) {
406 cacheData.create_dir(0777);
410 cacheData.append(airport + "-groundnet-cache.txt");
411 if (cacheData.exists()) {
412 ifstream data(cacheData.c_str());
415 if (revisionStr != "[GroundNetcachedata:ref:2011:09:04]") {
416 SG_LOG(SG_GENERAL, SG_ALERT,"GroundNetwork Warning: discarding outdated cachefile " <<
417 cacheData.c_str() << " for Airport " << airport);
419 for (FGTaxiNodeVectorIterator i = nodes.begin();
422 (*i)->setElevation(parent->getElevation() * SG_FEET_TO_METER);
423 data >> index >> elev;
426 if (index != (*i)->getIndex()) {
427 SG_LOG(SG_GENERAL, SG_ALERT, "Index read from ground network cache at airport " << airport << " does not match index in the network itself");
429 (*i)->setElevation(elev);
436 //cerr << "Finished initializing " << parent->getId() << " groundnetwork " << endl;
437 networkInitialized = true;
440 int FGGroundNetwork::findNearestNode(const SGGeod & aGeod)
442 double minDist = HUGE_VAL;
445 for (FGTaxiNodeVectorIterator itr = nodes.begin(); itr != nodes.end();
447 double d = SGGeodesy::distanceM(aGeod, (*itr)->getGeod());
450 index = (*itr)->getIndex();
451 //cerr << "Minimum distance of " << minDist << " for index " << index << endl;
458 int FGGroundNetwork::findNearestNodeOnRunway(const SGGeod & aGeod)
460 double minDist = HUGE_VAL;
463 for (FGTaxiNodeVectorIterator itr = nodes.begin(); itr != nodes.end();
465 if (!((*itr)->getIsOnRunway())) {
468 double d = SGGeodesy::distanceM(aGeod, (*itr)->getGeod());
471 index = (*itr)->getIndex();
472 //cerr << "Minimum distance of " << minDist << " for index " << index << endl;
480 int FGGroundNetwork::findNearestNode(double lat, double lon)
482 return findNearestNode(SGGeod::fromDeg(lon, lat));
485 FGTaxiNode *FGGroundNetwork::findNode(unsigned idx)
487 for (FGTaxiNodeVectorIterator
489 itr != nodes.end(); itr++)
491 if (itr->getIndex() == idx)
492 return itr->getAddress();
495 if ((idx >= 0) && (idx < nodes.size()))
496 return nodes[idx]->getAddress();
501 FGTaxiSegment *FGGroundNetwork::findSegment(unsigned idx)
503 for (FGTaxiSegmentVectorIterator
504 itr = segments.begin();
505 itr != segments.end(); itr++)
507 if (itr->getIndex() == idx)
508 return itr->getAddress();
511 if ((idx > 0) && (idx <= segments.size()))
512 return segments[idx - 1]->getAddress();
514 //cerr << "Alert: trying to find invalid segment " << idx << endl;
520 FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end,
523 //implements Dijkstra's algorithm to find shortest distance route from start to end
524 //taken from http://en.wikipedia.org/wiki/Dijkstra's_algorithm
526 //double INFINITE = 100000000000.0;
527 // initialize scoring values
528 int nParkings = parent->getDynamics()->getNrOfParkings();
529 FGTaxiNodeVector *currNodesSet;
531 currNodesSet = &nodes;
533 currNodesSet = &pushBackNodes;
536 for (FGTaxiNodeVectorIterator
537 itr = currNodesSet->begin(); itr != currNodesSet->end(); itr++) {
538 (*itr)->setPathScore(HUGE_VAL); //infinity by all practical means
539 (*itr)->setPreviousNode(0); //
540 (*itr)->setPreviousSeg(0); //
543 FGTaxiNode *firstNode = findNode(start);
544 firstNode->setPathScore(0);
546 FGTaxiNode *lastNode = findNode(end);
548 FGTaxiNodeVector unvisited(*currNodesSet); // working copy
550 while (!unvisited.empty()) {
551 FGTaxiNode *best = *(unvisited.begin());
552 for (FGTaxiNodeVectorIterator
553 itr = unvisited.begin(); itr != unvisited.end(); itr++) {
554 if ((*itr)->getPathScore() < best->getPathScore())
558 FGTaxiNodeVectorIterator newend =
559 remove(unvisited.begin(), unvisited.end(), best);
560 unvisited.erase(newend, unvisited.end());
562 if (best == lastNode) { // found route or best not connected
565 for (FGTaxiSegmentVectorIterator
566 seg = best->getBeginRoute();
567 seg != best->getEndRoute(); seg++) {
568 if (fullSearch || (*seg)->isPushBack()) {
569 FGTaxiNode *tgt = (*seg)->getEnd();
571 best->getPathScore() + (*seg)->getLength() +
572 (*seg)->getPenalty(nParkings);
573 if (alt < tgt->getPathScore()) { // Relax (u,v)
574 tgt->setPathScore(alt);
575 tgt->setPreviousNode(best);
576 tgt->setPreviousSeg(*seg); //
579 // // cerr << "Skipping TaxiSegment " << (*seg)->getIndex() << endl;
585 if (lastNode->getPathScore() == HUGE_VAL) {
586 // no valid route found
588 SG_LOG(SG_GENERAL, SG_ALERT,
589 "Failed to find route from waypoint " << start << " to "
590 << end << " at " << parent->getId());
594 //exit(1); //TODO exit more gracefully, no need to stall the whole sim with broken GN's
596 // assemble route from backtrace information
597 intVec nodes, routes;
598 FGTaxiNode *bt = lastNode;
599 while (bt->getPreviousNode() != 0) {
600 nodes.push_back(bt->getIndex());
601 routes.push_back(bt->getPreviousSegment()->getIndex());
602 bt = bt->getPreviousNode();
604 nodes.push_back(start);
605 reverse(nodes.begin(), nodes.end());
606 reverse(routes.begin(), routes.end());
608 return FGTaxiRoute(nodes, routes, lastNode->getPathScore(), 0);
612 int FGTaxiSegment::getPenalty(int nGates)
615 if (end->getIndex() < nGates) {
618 if (end->getIsOnRunway()) { // For now. In future versions, need to find out whether runway is active.
624 /* ATC Related Functions */
626 void FGGroundNetwork::announcePosition(int id,
627 FGAIFlightPlan * intendedRoute,
628 int currentPosition, double lat,
629 double lon, double heading,
630 double speed, double alt,
631 double radius, int leg,
632 FGAIAircraft * aircraft)
635 TrafficVectorIterator i = activeTraffic.begin();
636 // Search search if the current id alread has an entry
637 // This might be faster using a map instead of a vector, but let's start by taking a safe route
638 if (activeTraffic.size()) {
639 //while ((i->getId() != id) && i != activeTraffic.end()) {
640 while (i != activeTraffic.end()) {
641 if (i->getId() == id) {
647 // Add a new TrafficRecord if no one exsists for this aircraft.
648 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
652 rec.setPositionAndIntentions(currentPosition, intendedRoute);
653 rec.setPositionAndHeading(lat, lon, heading, speed, alt);
654 rec.setRadius(radius); // only need to do this when creating the record.
655 rec.setAircraft(aircraft);
657 activeTraffic.push_front(rec);
659 activeTraffic.push_back(rec);
663 i->setPositionAndIntentions(currentPosition, intendedRoute);
664 i->setPositionAndHeading(lat, lon, heading, speed, alt);
669 void FGGroundNetwork::signOff(int id)
671 TrafficVectorIterator i = activeTraffic.begin();
672 // Search search if the current id alread has an entry
673 // This might be faster using a map instead of a vector, but let's start by taking a safe route
674 if (activeTraffic.size()) {
675 //while ((i->getId() != id) && i != activeTraffic.end()) {
676 while (i != activeTraffic.end()) {
677 if (i->getId() == id) {
683 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
684 SG_LOG(SG_GENERAL, SG_ALERT,
685 "AI error: Aircraft without traffic record is signing off at " << SG_ORIGIN);
687 i = activeTraffic.erase(i);
691 * The ground network can deal with the following states:
692 * 0 = Normal; no action required
693 * 1 = "Acknowledge "Hold position
694 * 2 = "Acknowledge "Resume taxi".
695 * 3 = "Issue TaxiClearance"
696 * 4 = Acknowledge Taxi Clearance"
697 * 5 = Post acknowlegde taxiclearance: Start taxiing
699 * 7 = Acknowledge report runway
700 * 8 = Switch tower frequency
701 * 9 = Acknowledge switch tower frequency
702 *************************************************************************************************************************/
703 bool FGGroundNetwork::checkTransmissionState(int minState, int maxState, TrafficVectorIterator i, time_t now, AtcMsgId msgId,
706 int state = i->getState();
707 if ((state >= minState) && (state <= maxState) && available) {
708 if ((msgDir == ATC_AIR_TO_GROUND) && isUserAircraft(i->getAircraft())) {
709 //cerr << "Checking state " << state << " for " << i->getAircraft()->getCallSign() << endl;
710 static SGPropertyNode_ptr trans_num = globals->get_props()->getNode("/sim/atc/transmission-num", true);
711 int n = trans_num->getIntValue();
713 trans_num->setIntValue(-1);
715 //cerr << "Selected transmission message " << n << endl;
716 FGATCManager *atc = (FGATCManager*) globals->get_subsystem("atc");
717 FGATCDialogNew::instance()->removeEntry(1);
719 //cerr << "creating message for " << i->getAircraft()->getCallSign() << endl;
720 transmit(&(*i), msgId, msgDir, false);
724 transmit(&(*i), msgId, msgDir, true);
726 lastTransmission = now;
733 void FGGroundNetwork::updateAircraftInformation(int id, double lat, double lon,
734 double heading, double speed, double alt,
737 time_t currentTime = time(NULL);
738 if (nextSave < currentTime) {
739 saveElevationCache();
740 nextSave = currentTime + 100 + rand() % 200;
742 // Check whether aircraft are on hold due to a preceding pushback. If so, make sure to
743 // Transmit air-to-ground "Ready to taxi request:
744 // Transmit ground to air approval / hold
745 // Transmit confirmation ...
746 // Probably use a status mechanism similar to the Engine start procedure in the startup controller.
749 TrafficVectorIterator i = activeTraffic.begin();
750 // Search search if the current id has an entry
751 // This might be faster using a map instead of a vector, but let's start by taking a safe route
752 TrafficVectorIterator current, closest;
753 if (activeTraffic.size()) {
754 //while ((i->getId() != id) && i != activeTraffic.end()) {
755 while (i != activeTraffic.end()) {
756 if (i->getId() == id) {
762 // update position of the current aircraft
763 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
764 SG_LOG(SG_GENERAL, SG_ALERT,
765 "AI error: updating aircraft without traffic record at " << SG_ORIGIN);
767 i->setPositionAndHeading(lat, lon, heading, speed, alt);
773 // Update every three secs, but add some randomness
774 // to prevent all IA objects doing this in synchrony
775 //if (getDt() < (3.0) + (rand() % 10))
779 current->clearResolveCircularWait();
780 current->setWaitsForId(0);
781 checkSpeedAdjustment(id, lat, lon, heading, speed, alt);
782 bool needsTaxiClearance = current->getAircraft()->getTaxiClearanceRequest();
783 if (!needsTaxiClearance) {
784 checkHoldPosition(id, lat, lon, heading, speed, alt);
785 //if (checkForCircularWaits(id)) {
786 // i->setResolveCircularWait();
789 current->setHoldPosition(true);
790 int state = current->getState();
791 time_t now = time(NULL) + fgGetLong("/sim/time/warp");
792 if ((now - lastTransmission) > 15) {
795 if (checkTransmissionState(0,2, current, now, MSG_REQUEST_TAXI_CLEARANCE, ATC_AIR_TO_GROUND)) {
796 current->setState(3);
798 if (checkTransmissionState(3,3, current, now, MSG_ISSUE_TAXI_CLEARANCE, ATC_GROUND_TO_AIR)) {
799 current->setState(4);
801 if (checkTransmissionState(4,4, current, now, MSG_ACKNOWLEDGE_TAXI_CLEARANCE, ATC_AIR_TO_GROUND)) {
802 current->setState(5);
804 if ((state == 5) && available) {
805 current->setState(0);
806 current->getAircraft()->setTaxiClearanceRequest(false);
807 current->setHoldPosition(false);
815 Scan for a speed adjustment change. Find the nearest aircraft that is in front
816 and adjust speed when we get too close. Only do this when current position and/or
817 intentions of the current aircraft match current taxiroute position of the proximate
818 aircraft. For traffic that is on other routes we need to issue a "HOLD Position"
819 instruction. See below for the hold position instruction.
821 Note that there currently still is one flaw in the logic that needs to be addressed.
822 There can be situations where one aircraft is in front of the current aircraft, on a separate
823 route, but really close after an intersection coming off the current route. This
824 aircraft is still close enough to block the current aircraft. This situation is currently
825 not addressed yet, but should be.
828 void FGGroundNetwork::checkSpeedAdjustment(int id, double lat,
829 double lon, double heading,
830 double speed, double alt)
833 TrafficVectorIterator current, closest, closestOnNetwork;
834 TrafficVectorIterator i = activeTraffic.begin();
835 bool otherReasonToSlowDown = false;
836 bool previousInstruction;
837 if (activeTraffic.size()) {
838 //while ((i->getId() != id) && (i != activeTraffic.end()))
839 while (i != activeTraffic.end()) {
840 if (i->getId() == id) {
848 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
849 SG_LOG(SG_GENERAL, SG_ALERT,
850 "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkSpeedAdjustment at " << SG_ORIGIN);
855 previousInstruction = current->getSpeedAdjustment();
856 double mindist = HUGE_VAL;
857 if (activeTraffic.size()) {
858 double course, dist, bearing, minbearing, az2;
859 SGGeod curr(SGGeod::fromDegM(lon, lat, alt));
860 //TrafficVector iterator closest;
862 closestOnNetwork = current;
863 for (TrafficVectorIterator i = activeTraffic.begin();
864 i != activeTraffic.end(); i++) {
869 SGGeod other(SGGeod::fromDegM(i->getLongitude(),
872 SGGeodesy::inverse(curr, other, course, az2, dist);
873 bearing = fabs(heading - course);
875 bearing = 360 - bearing;
876 if ((dist < mindist) && (bearing < 60.0)) {
879 closestOnNetwork = i;
880 minbearing = bearing;
884 //Check traffic at the tower controller
885 if (towerController->hasActiveTraffic()) {
886 for (TrafficVectorIterator i =
887 towerController->getActiveTraffic().begin();
888 i != towerController->getActiveTraffic().end(); i++) {
889 //cerr << "Comparing " << current->getId() << " and " << i->getId() << endl;
890 SGGeod other(SGGeod::fromDegM(i->getLongitude(),
893 SGGeodesy::inverse(curr, other, course, az2, dist);
894 bearing = fabs(heading - course);
896 bearing = 360 - bearing;
897 if ((dist < mindist) && (bearing < 60.0)) {
898 //cerr << "Current aircraft " << current->getAircraft()->getTrafficRef()->getCallSign()
899 // << " is closest to " << i->getAircraft()->getTrafficRef()->getCallSign()
900 // << ", which has status " << i->getAircraft()->isScheduledForTakeoff()
904 minbearing = bearing;
905 otherReasonToSlowDown = true;
909 // Finally, check UserPosition
910 // Note, as of 2011-08-01, this should no longer be necessecary.
912 double userLatitude = fgGetDouble("/position/latitude-deg");
913 double userLongitude = fgGetDouble("/position/longitude-deg");
914 SGGeod user(SGGeod::fromDeg(userLongitude, userLatitude));
915 SGGeodesy::inverse(curr, user, course, az2, dist);
917 bearing = fabs(heading - course);
919 bearing = 360 - bearing;
920 if ((dist < mindist) && (bearing < 60.0)) {
923 minbearing = bearing;
924 otherReasonToSlowDown = true;
927 current->clearSpeedAdjustment();
928 bool needBraking = false;
929 if (current->checkPositionAndIntentions(*closest)
930 || otherReasonToSlowDown) {
931 double maxAllowableDistance =
932 (1.1 * current->getRadius()) +
933 (1.1 * closest->getRadius());
934 if (mindist < 2 * maxAllowableDistance) {
935 if (current->getId() == closest->getWaitsForId())
938 current->setWaitsForId(closest->getId());
939 if (closest->getId() != current->getId()) {
940 current->setSpeedAdjustment(closest->getSpeed() *
945 // closest->getAircraft()->getTakeOffStatus() &&
946 // (current->getAircraft()->getTrafficRef()->getDepartureAirport() == closest->getAircraft()->getTrafficRef()->getDepartureAirport()) &&
947 // (current->getAircraft()->GetFlightPlan()->getRunway() == closest->getAircraft()->GetFlightPlan()->getRunway())
949 // current->getAircraft()->scheduleForATCTowerDepartureControl(1);
951 current->setSpeedAdjustment(0); // This can only happen when the user aircraft is the one closest
953 if (mindist < maxAllowableDistance) {
954 //double newSpeed = (maxAllowableDistance-mindist);
955 //current->setSpeedAdjustment(newSpeed);
956 //if (mindist < 0.5* maxAllowableDistance)
958 current->setSpeedAdjustment(0);
963 if ((closest->getId() == closestOnNetwork->getId()) && (current->getPriority() < closest->getPriority()) && needBraking) {
964 swap(current, closest);
970 Check for "Hold position instruction".
971 The hold position should be issued under the following conditions:
972 1) For aircraft entering or crossing a runway with active traffic on it, or landing aircraft near it
973 2) For taxiing aircraft that use one taxiway in opposite directions
974 3) For crossing or merging taxiroutes.
977 void FGGroundNetwork::checkHoldPosition(int id, double lat,
978 double lon, double heading,
979 double speed, double alt)
981 TrafficVectorIterator current;
982 TrafficVectorIterator i = activeTraffic.begin();
983 if (activeTraffic.size()) {
984 //while ((i->getId() != id) && i != activeTraffic.end())
985 while (i != activeTraffic.end()) {
986 if (i->getId() == id) {
994 time_t now = time(NULL) + fgGetLong("/sim/time/warp");
995 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
996 SG_LOG(SG_GENERAL, SG_ALERT,
997 "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkHoldPosition at " << SG_ORIGIN);
1001 if (current->getAircraft()->getTakeOffStatus() == 1) {
1002 current->setHoldPosition(true);
1005 if (current->getAircraft()->getTakeOffStatus() == 2) {
1006 //cerr << current->getAircraft()->getCallSign() << ". Taxi in position and hold" << endl;
1007 current->setHoldPosition(false);
1008 current->clearSpeedAdjustment();
1011 bool origStatus = current->hasHoldPosition();
1012 current->setHoldPosition(false);
1013 SGGeod curr(SGGeod::fromDegM(lon, lat, alt));
1014 int currentRoute = i->getCurrentPosition();
1016 if (i->getIntentions().size()) {
1017 nextRoute = (*(i->getIntentions().begin()));
1021 if (currentRoute > 0) {
1022 FGTaxiSegment *tx = findSegment(currentRoute);
1025 nx = findSegment(nextRoute);
1029 if (tx->hasBlock(now) || nx->hasBlock(now) ) {
1030 current->setHoldPosition(true);
1035 /* for (i = activeTraffic.begin(); i != activeTraffic.end(); i++) {
1036 if (i->getId() != current->getId()) {
1037 int node = current->crosses(this, *i);
1039 FGTaxiNode *taxiNode = findNode(node);
1041 // Determine whether it's save to continue or not.
1042 // If we have a crossing route, there are two possibilities:
1043 // 1) This is an interestion
1044 // 2) This is oncoming two-way traffic, using the same taxiway.
1045 //cerr << "Hold check 1 : " << id << " has common node " << node << endl;
1047 SGGeod other(SGGeod::
1048 fromDegM(i->getLongitude(), i->getLatitude(),
1052 if (current->isOpposing(this, *i, node)) {
1055 //cerr << "Hold check 2 : " << node << " has opposing segment " << endl;
1056 // issue a "Hold Position" as soon as we're close to the offending node
1057 // For now, I'm doing this as long as the other aircraft doesn't
1058 // have a hold instruction as soon as we're within a reasonable
1059 // distance from the offending node.
1060 // This may be a bit of a conservative estimate though, as it may
1061 // be well possible that both aircraft can both continue to taxi
1062 // without crashing into each other.
1065 if (SGGeodesy::distanceM(other, taxiNode->getGeod()) > 200) // 2.0*i->getRadius())
1067 needsToWait = false;
1068 //cerr << "Hold check 3 : " << id <<" Other aircraft approaching node is still far away. (" << dist << " nm). Can safely continue "
1072 //cerr << "Hold check 4: " << id << " Would need to wait for other aircraft : distance = " << dist << " meters" << endl;
1077 SGGeodesy::distanceM(curr, taxiNode->getGeod());
1078 if (!(i->hasHoldPosition())) {
1080 if ((dist < 200) && //2.5*current->getRadius()) &&
1081 (needsToWait) && (i->onRoute(this, *current)) &&
1082 //((i->onRoute(this, *current)) || ((!(i->getSpeedAdjustment())))) &&
1083 (!(current->getId() == i->getWaitsForId())))
1084 //(!(i->getSpeedAdjustment()))) // &&
1085 //(!(current->getSpeedAdjustment())))
1088 if (!(isUserAircraft(i->getAircraft()))) { // test code. Don't wait for the user, let the user wait for you.
1089 current->setHoldPosition(true);
1090 current->setWaitsForId(i->getId());
1092 //cerr << "Hold check 5: " << current->getCallSign() <<" Setting Hold Position: distance to node (" << node << ") "
1093 // << dist << " meters. Waiting for " << i->getCallSign();
1095 //cerr <<" [opposing] " << endl;
1097 // cerr << "[non-opposing] " << endl;
1098 //if (i->hasSpeefAdjustment())
1100 // cerr << " (which in turn waits for ) " << i->
1102 //cerr << "Hold check 6: " << id << " No need to hold yet: Distance to node : " << dist << " nm"<< endl;
1108 bool currStatus = current->hasHoldPosition();
1109 current->setHoldPosition(origStatus);
1110 // Either a Hold Position or a resume taxi transmission has been issued
1111 if ((now - lastTransmission) > 2) {
1114 if (current->getState() == 0) {
1115 if ((origStatus != currStatus) && available) {
1116 //cerr << "Issueing hold short instrudtion " << currStatus << " " << available << endl;
1117 if (currStatus == true) { // No has a hold short instruction
1118 transmit(&(*current), MSG_HOLD_POSITION, ATC_GROUND_TO_AIR, true);
1119 //cerr << "Transmittin hold short instrudtion " << currStatus << " " << available << endl;
1120 current->setState(1);
1122 transmit(&(*current), MSG_RESUME_TAXI, ATC_GROUND_TO_AIR, true);
1123 //cerr << "Transmittig resume instrudtion " << currStatus << " " << available << endl;
1124 current->setState(2);
1126 lastTransmission = now;
1128 // Don't act on the changed instruction until the transmission is confirmed
1129 // So set back to original status
1130 //cerr << "Current state " << current->getState() << endl;
1134 // 6 = Report runway
1135 // 7 = Acknowledge report runway
1136 // 8 = Switch tower frequency
1137 //9 = Acknowledge switch tower frequency
1139 //int state = current->getState();
1140 if (checkTransmissionState(1,1, current, now, MSG_ACKNOWLEDGE_HOLD_POSITION, ATC_AIR_TO_GROUND)) {
1141 current->setState(0);
1142 current->setHoldPosition(true);
1144 if (checkTransmissionState(2,2, current, now, MSG_ACKNOWLEDGE_RESUME_TAXI, ATC_AIR_TO_GROUND)) {
1145 current->setState(0);
1146 current->setHoldPosition(false);
1148 if (current->getAircraft()->getTakeOffStatus() && (current->getState() == 0)) {
1149 //cerr << "Scheduling " << current->getAircraft()->getCallSign() << " for hold short" << endl;
1150 current->setState(6);
1152 if (checkTransmissionState(6,6, current, now, MSG_REPORT_RUNWAY_HOLD_SHORT, ATC_AIR_TO_GROUND)) {
1154 if (checkTransmissionState(7,7, current, now, MSG_ACKNOWLEDGE_REPORT_RUNWAY_HOLD_SHORT, ATC_GROUND_TO_AIR)) {
1156 if (checkTransmissionState(8,8, current, now, MSG_SWITCH_TOWER_FREQUENCY, ATC_GROUND_TO_AIR)) {
1158 if (checkTransmissionState(9,9, current, now, MSG_ACKNOWLEDGE_SWITCH_TOWER_FREQUENCY, ATC_AIR_TO_GROUND)) {
1163 //current->setState(0);
1167 * Check whether situations occur where the current aircraft is waiting for itself
1168 * due to higher order interactions.
1169 * A 'circular' wait is a situation where a waits for b, b waits for c, and c waits
1170 * for a. Ideally each aircraft only waits for one other aircraft, so by tracing
1171 * through this list of waiting aircraft, we can check if we'd eventually end back
1172 * at the current aircraft.
1174 * Note that we should consider the situation where we are actually checking aircraft
1175 * d, which is waiting for aircraft a. d is not part of the loop, but is held back by
1176 * the looping aircraft. If we don't check for that, this function will get stuck into
1180 bool FGGroundNetwork::checkForCircularWaits(int id)
1182 //cerr << "Performing Wait check " << id << endl;
1184 TrafficVectorIterator current, other;
1185 TrafficVectorIterator i = activeTraffic.begin();
1186 int trafficSize = activeTraffic.size();
1188 while (i != activeTraffic.end()) {
1189 if (i->getId() == id) {
1197 if (i == activeTraffic.end() || (trafficSize == 0)) {
1198 SG_LOG(SG_GENERAL, SG_ALERT,
1199 "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits at " << SG_ORIGIN);
1203 target = current->getWaitsForId();
1204 //bool printed = false; // Note that this variable is for debugging purposes only.
1208 //cerr << "aircraft waits for user" << endl;
1213 while ((target > 0) && (target != id) && counter++ < trafficSize) {
1215 TrafficVectorIterator i = activeTraffic.begin();
1217 //while ((i->getId() != id) && i != activeTraffic.end())
1218 while (i != activeTraffic.end()) {
1219 if (i->getId() == target) {
1227 if (i == activeTraffic.end() || (trafficSize == 0)) {
1228 //cerr << "[Waiting for traffic at Runway: DONE] " << endl << endl;;
1229 // The target id is not found on the current network, which means it's at the tower
1230 //SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits");
1234 target = other->getWaitsForId();
1236 // actually this trap isn't as impossible as it first seemed:
1237 // the setWaitsForID(id) is set to current when the aircraft
1238 // is waiting for the user controlled aircraft.
1239 //if (current->getId() == other->getId()) {
1240 // cerr << "Caught the impossible trap" << endl;
1241 // cerr << "Current = " << current->getId() << endl;
1242 // cerr << "Other = " << other ->getId() << endl;
1243 // for (TrafficVectorIterator at = activeTraffic.begin();
1244 // at != activeTraffic.end();
1246 // cerr << "currently active aircraft : " << at->getCallSign() << " with Id " << at->getId() << " waits for " << at->getWaitsForId() << endl;
1249 if (current->getId() == other->getId())
1252 //cerr << current->getCallSign() << " (" << current->getId() << ") " << " -> " << other->getCallSign()
1253 // << " (" << other->getId() << "); " << endl;;
1263 // cerr << "[done] " << endl << endl;;
1265 SG_LOG(SG_GENERAL, SG_WARN,
1266 "Detected circular wait condition: Id = " << id <<
1267 "target = " << target);
1274 // Note that this function is probably obsolete...
1275 bool FGGroundNetwork::hasInstruction(int id)
1277 TrafficVectorIterator i = activeTraffic.begin();
1278 // Search search if the current id has an entry
1279 // This might be faster using a map instead of a vector, but let's start by taking a safe route
1280 if (activeTraffic.size()) {
1281 //while ((i->getId() != id) && i != activeTraffic.end()) {
1282 while (i != activeTraffic.end()) {
1283 if (i->getId() == id) {
1289 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
1290 SG_LOG(SG_GENERAL, SG_ALERT,
1291 "AI error: checking ATC instruction for aircraft without traffic record at " << SG_ORIGIN);
1293 return i->hasInstruction();
1298 FGATCInstruction FGGroundNetwork::getInstruction(int id)
1300 TrafficVectorIterator i = activeTraffic.begin();
1301 // Search search if the current id has an entry
1302 // This might be faster using a map instead of a vector, but let's start by taking a safe route
1303 if (activeTraffic.size()) {
1304 //while ((i->getId() != id) && i != activeTraffic.end()) {
1305 while (i != activeTraffic.end()) {
1306 if (i->getId() == id) {
1312 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
1313 SG_LOG(SG_GENERAL, SG_ALERT,
1314 "AI error: requesting ATC instruction for aircraft without traffic record at " << SG_ORIGIN);
1316 return i->getInstruction();
1318 return FGATCInstruction();
1321 // Note that this function is copied from simgear. for maintanance purposes, it's probabtl better to make a general function out of that.
1322 static void WorldCoordinate(osg::Matrix& obj_pos, double lat,
1323 double lon, double elev, double hdg, double slope)
1325 SGGeod geod = SGGeod::fromDegM(lon, lat, elev);
1326 obj_pos = geod.makeZUpFrame();
1327 // hdg is not a compass heading, but a counter-clockwise rotation
1328 // around the Z axis
1329 obj_pos.preMult(osg::Matrix::rotate(hdg * SGD_DEGREES_TO_RADIANS,
1331 obj_pos.preMult(osg::Matrix::rotate(slope * SGD_DEGREES_TO_RADIANS,
1338 void FGGroundNetwork::render(bool visible)
1341 SGMaterialLib *matlib = globals->get_matlib();
1344 globals->get_scenery()->get_scene_graph()->removeChild(group);
1345 //while (group->getNumChildren()) {
1346 // cerr << "Number of children: " << group->getNumChildren() << endl;
1347 //simgear::EffectGeode* geode = (simgear::EffectGeode*) group->getChild(0);
1348 //osg::MatrixTransform *obj_trans = (osg::MatrixTransform*) group->getChild(0);
1349 //geode->releaseGLObjects();
1350 //group->removeChild(geode);
1355 group = new osg::Group;
1356 FGScenery * local_scenery = globals->get_scenery();
1357 double elevation_meters = 0.0;
1358 double elevation_feet = 0.0;
1359 time_t now = time(NULL) + fgGetLong("/sim/time/warp");
1360 //for ( FGTaxiSegmentVectorIterator i = segments.begin(); i != segments.end(); i++) {
1362 for (TrafficVectorIterator i = activeTraffic.begin(); i != activeTraffic.end(); i++) {
1363 // Handle start point
1364 int pos = i->getCurrentPosition() - 1;
1367 SGGeod start(SGGeod::fromDeg((i->getLongitude()), (i->getLatitude())));
1368 SGGeod end (SGGeod::fromDeg(segments[pos]->getEnd()->getLongitude(), segments[pos]->getEnd()->getLatitude()));
1370 double length = SGGeodesy::distanceM(start, end);
1371 //heading = SGGeodesy::headingDeg(start->getGeod(), end->getGeod());
1373 double az2, heading; //, distanceM;
1374 SGGeodesy::inverse(start, end, heading, az2, length);
1375 double coveredDistance = length * 0.5;
1377 SGGeodesy::direct(start, heading, coveredDistance, center, az2);
1378 //cerr << "Active Aircraft : Centerpoint = (" << center.getLatitudeDeg() << ", " << center.getLongitudeDeg() << "). Heading = " << heading << endl;
1379 ///////////////////////////////////////////////////////////////////////////////
1380 // Make a helper function out of this
1381 osg::Matrix obj_pos;
1382 osg::MatrixTransform *obj_trans = new osg::MatrixTransform;
1383 obj_trans->setDataVariance(osg::Object::STATIC);
1384 // Experimental: Calculate slope here, based on length, and the individual elevations
1385 double elevationStart;
1386 if (isUserAircraft((i)->getAircraft())) {
1387 elevationStart = fgGetDouble("/position/ground-elev-m");
1389 elevationStart = ((i)->getAircraft()->_getAltitude());
1391 double elevationEnd = segments[pos]->getEnd()->getElevationM(parent->getElevation()*SG_FEET_TO_METER);
1392 //cerr << "Using elevation " << elevationEnd << endl;
1394 if ((elevationEnd == 0) || (elevationEnd = parent->getElevation())) {
1395 SGGeod center2 = end;
1396 center2.setElevationM(SG_MAX_ELEVATION_M);
1397 if (local_scenery->get_elevation_m( center2, elevationEnd, NULL )) {
1398 elevation_feet = elevationEnd * SG_METER_TO_FEET + 0.5;
1399 //elevation_meters += 0.5;
1402 elevationEnd = parent->getElevation();
1404 segments[pos]->getEnd()->setElevation(elevationEnd);
1406 double elevationMean = (elevationStart + elevationEnd) / 2.0;
1407 double elevDiff = elevationEnd - elevationStart;
1409 double slope = atan2(elevDiff, length) * SGD_RADIANS_TO_DEGREES;
1411 //cerr << "1. Using mean elevation : " << elevationMean << " and " << slope << endl;
1413 WorldCoordinate( obj_pos, center.getLatitudeDeg(), center.getLongitudeDeg(), elevationMean+ 0.5, -(heading), slope );
1415 obj_trans->setMatrix( obj_pos );
1416 //osg::Vec3 center(0, 0, 0)
1418 float width = length /2.0;
1419 osg::Vec3 corner(-width, 0, 0.25f);
1420 osg::Vec3 widthVec(2*width + 1, 0, 0);
1421 osg::Vec3 heightVec(0, 1, 0);
1422 osg::Geometry* geometry;
1423 geometry = osg::createTexturedQuadGeometry(corner, widthVec, heightVec);
1424 simgear::EffectGeode* geode = new simgear::EffectGeode;
1425 geode->setName("test");
1426 geode->addDrawable(geometry);
1427 //osg::Node *custom_obj;
1429 if (segments[pos]->hasBlock(now)) {
1430 mat = matlib->find("UnidirectionalTaperRed");
1432 mat = matlib->find("UnidirectionalTaperGreen");
1435 geode->setEffect(mat->get_effect());
1436 obj_trans->addChild(geode);
1437 // wire as much of the scene graph together as we can
1438 //->addChild( obj_trans );
1439 group->addChild( obj_trans );
1440 /////////////////////////////////////////////////////////////////////
1442 //cerr << "BIG FAT WARNING: current position is here : " << pos << endl;
1444 for (intVecIterator j = (i)->getIntentions().begin(); j != (i)->getIntentions().end(); j++) {
1445 osg::Matrix obj_pos;
1448 osg::MatrixTransform *obj_trans = new osg::MatrixTransform;
1449 obj_trans->setDataVariance(osg::Object::STATIC);
1451 // Experimental: Calculate slope here, based on length, and the individual elevations
1452 double elevationStart = segments[k]->getStart()->getElevationM(parent->getElevation()*SG_FEET_TO_METER);
1453 double elevationEnd = segments[k]->getEnd ()->getElevationM(parent->getElevation()*SG_FEET_TO_METER);
1454 if ((elevationStart == 0) || (elevationStart == parent->getElevation())) {
1455 SGGeod center2 = segments[k]->getStart()->getGeod();
1456 center2.setElevationM(SG_MAX_ELEVATION_M);
1457 if (local_scenery->get_elevation_m( center2, elevationStart, NULL )) {
1458 elevation_feet = elevationStart * SG_METER_TO_FEET + 0.5;
1459 //elevation_meters += 0.5;
1462 elevationStart = parent->getElevation();
1464 segments[k]->getStart()->setElevation(elevationStart);
1466 if ((elevationEnd == 0) || (elevationEnd == parent->getElevation())) {
1467 SGGeod center2 = segments[k]->getEnd()->getGeod();
1468 center2.setElevationM(SG_MAX_ELEVATION_M);
1469 if (local_scenery->get_elevation_m( center2, elevationEnd, NULL )) {
1470 elevation_feet = elevationEnd * SG_METER_TO_FEET + 0.5;
1471 //elevation_meters += 0.5;
1474 elevationEnd = parent->getElevation();
1476 segments[k]->getEnd()->setElevation(elevationEnd);
1479 double elevationMean = (elevationStart + elevationEnd) / 2.0;
1480 double elevDiff = elevationEnd - elevationStart;
1481 double length = segments[k]->getLength();
1482 double slope = atan2(elevDiff, length) * SGD_RADIANS_TO_DEGREES;
1484 // cerr << "2. Using mean elevation : " << elevationMean << " and " << slope << endl;
1487 WorldCoordinate( obj_pos, segments[k]->getLatitude(), segments[k]->getLongitude(), elevationMean+ 0.5, -(segments[k]->getHeading()), slope );
1489 obj_trans->setMatrix( obj_pos );
1490 //osg::Vec3 center(0, 0, 0)
1492 float width = segments[k]->getLength() /2.0;
1493 osg::Vec3 corner(-width, 0, 0.25f);
1494 osg::Vec3 widthVec(2*width + 1, 0, 0);
1495 osg::Vec3 heightVec(0, 1, 0);
1496 osg::Geometry* geometry;
1497 geometry = osg::createTexturedQuadGeometry(corner, widthVec, heightVec);
1498 simgear::EffectGeode* geode = new simgear::EffectGeode;
1499 geode->setName("test");
1500 geode->addDrawable(geometry);
1501 //osg::Node *custom_obj;
1503 if (segments[k]->hasBlock(now)) {
1504 mat = matlib->find("UnidirectionalTaperRed");
1506 mat = matlib->find("UnidirectionalTaperGreen");
1509 geode->setEffect(mat->get_effect());
1510 obj_trans->addChild(geode);
1511 // wire as much of the scene graph together as we can
1512 //->addChild( obj_trans );
1513 group->addChild( obj_trans );
1518 globals->get_scenery()->get_scene_graph()->addChild(group);
1522 string FGGroundNetwork::getName() {
1523 return string(parent->getId() + "-ground");
1526 void FGGroundNetwork::update(double dt)
1528 time_t now = time(NULL) + fgGetLong("/sim/time/warp");
1529 for (FGTaxiSegmentVectorIterator tsi = segments.begin(); tsi != segments.end(); tsi++) {
1530 (*tsi)->unblock(now);
1533 //sort(activeTraffic.begin(), activeTraffic.end(), compare_trafficrecords);
1534 // Handle traffic that is under ground control first; this way we'll prevent clutter at the gate areas.
1535 // Don't allow an aircraft to pushback when a taxiing aircraft is currently using part of the intended route.
1536 for (TrafficVectorIterator i = parent->getDynamics()->getStartupController()->getActiveTraffic().begin();
1537 i != parent->getDynamics()->getStartupController()->getActiveTraffic().end(); i++) {
1539 i->setPriority(priority++);
1540 // in meters per second;
1541 double vTaxi = (i->getAircraft()->getPerformance()->vTaxi() * SG_NM_TO_METER) / 3600;
1542 if (i->isActive(60)) {
1544 // Check for all active aircraft whether it's current pos segment is
1545 // an opposite of one of the departing aircraft's intentions
1546 for (TrafficVectorIterator j = activeTraffic.begin(); j != activeTraffic.end(); j++) {
1547 int pos = j->getCurrentPosition();
1549 FGTaxiSegment *seg = segments[pos-1]->opposite();
1551 int posReverse = seg->getIndex();
1552 for (intVecIterator k = i->getIntentions().begin(); k != i->getIntentions().end(); k++) {
1553 if ((*k) == posReverse) {
1555 segments[posReverse-1]->block(now);
1561 // if the current aircraft is still allowed to pushback, we can start reserving a route for if by blocking all the entry taxiways.
1562 if (i->pushBackAllowed()) {
1564 int pos = i->getCurrentPosition();
1566 FGTaxiSegment *seg = segments[pos-1];
1567 FGTaxiNode *node = seg->getEnd();
1568 length = seg->getLength();
1569 for (FGTaxiSegmentVectorIterator tsi = segments.begin(); tsi != segments.end(); tsi++) {
1570 if (((*tsi)->getEnd() == node) && ((*tsi) != seg)) {
1575 for (intVecIterator j = i->getIntentions().begin(); j != i->getIntentions().end(); j++) {
1578 FGTaxiSegment *seg = segments[pos-1];
1579 FGTaxiNode *node = seg->getEnd();
1580 length += seg->getLength();
1581 time_t blockTime = now + (length / vTaxi);
1582 for (FGTaxiSegmentVectorIterator tsi = segments.begin(); tsi != segments.end(); tsi++) {
1583 if (((*tsi)->getEnd() == node) && ((*tsi) != seg)) {
1584 (*tsi)->block(blockTime-30);
1592 for (TrafficVectorIterator i = activeTraffic.begin(); i != activeTraffic.end(); i++) {
1594 double vTaxi = (i->getAircraft()->getPerformance()->vTaxi() * SG_NM_TO_METER) / 3600;
1595 i->setPriority(priority++);
1596 int pos = i->getCurrentPosition();
1598 length = segments[pos-1]->getLength();
1599 if (segments[pos-1]->hasBlock(now)) {
1600 //SG_LOG(SG_GENERAL, SG_ALERT, "Taxiway incursion for AI aircraft" << i->getAircraft()->getCallSign());
1605 for (ivi = i->getIntentions().begin(); ivi != i->getIntentions().end(); ivi++) {
1606 int segIndex = (*ivi);
1608 if (segments[segIndex-1]->hasBlock(now))
1612 //after this, ivi points just behind the last valid unblocked taxi segment.
1613 for (intVecIterator j = i->getIntentions().begin(); j != ivi; j++) {
1616 FGTaxiSegment *seg = segments[pos-1];
1617 FGTaxiNode *node = seg->getEnd();
1618 length += seg->getLength();
1619 for (FGTaxiSegmentVectorIterator tsi = segments.begin(); tsi != segments.end(); tsi++) {
1620 if (((*tsi)->getEnd() == node) && ((*tsi) != seg)) {
1621 time_t blockTime = now + (length / vTaxi);
1622 (*tsi)->block(blockTime - 30);