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/AIFlightPlan.hxx>
48 #include <ATC/atc_mgr.hxx>
50 #include <Scenery/scenery.hxx>
52 #include "groundnetwork.hxx"
54 /***************************************************************************
56 **************************************************************************/
58 void FGTaxiSegment::setStart(FGTaxiNodeVector * nodes)
60 FGTaxiNodeVectorIterator i = nodes->begin();
61 while (i != nodes->end()) {
62 //cerr << "Scanning start node index" << (*i)->getIndex() << endl;
63 if ((*i)->getIndex() == startNode) {
64 start = (*i)->getAddress();
65 (*i)->addSegment(this);
70 SG_LOG(SG_GENERAL, SG_ALERT,
71 "Could not find start node " << startNode << endl);
74 void FGTaxiSegment::setEnd(FGTaxiNodeVector * nodes)
76 FGTaxiNodeVectorIterator i = nodes->begin();
77 while (i != nodes->end()) {
78 //cerr << "Scanning end node index" << (*i)->getIndex() << endl;
79 if ((*i)->getIndex() == endNode) {
80 end = (*i)->getAddress();
85 SG_LOG(SG_GENERAL, SG_ALERT,
86 "Could not find end node " << endNode << endl);
91 // There is probably a computationally cheaper way of
93 void FGTaxiSegment::setDimensions(double elevation)
95 length = SGGeodesy::distanceM(start->getGeod(), end->getGeod());
96 //heading = SGGeodesy::headingDeg(start->getGeod(), end->getGeod());
98 double az2; //, distanceM;
99 SGGeodesy::inverse(start->getGeod(), end->getGeod(), heading, az2, length);
100 double coveredDistance = length * 0.5;
101 SGGeodesy::direct(start->getGeod(), heading, coveredDistance, center, az2);
102 //start->setElevation(elevation);
103 //end->setElevation(elevation);
104 //cerr << "Centerpoint = (" << center.getLatitudeDeg() << ", " << center.getLongitudeDeg() << "). Heading = " << heading << endl;
108 //void FGTaxiSegment::setCourseDiff(double crse)
110 // headingDiff = fabs(course - crse);
112 // if (headingDiff > 180)
113 // headingDiff = fabs(headingDiff - 360);
117 /***************************************************************************
119 **************************************************************************/
120 bool FGTaxiRoute::next(int *nde)
122 //for (intVecIterator i = nodes.begin(); i != nodes.end(); i++)
123 // cerr << "FGTaxiRoute contains : " << *(i) << endl;
124 //cerr << "Offset from end: " << nodes.end() - currNode << endl;
125 //if (currNode != nodes.end())
126 // cerr << "true" << endl;
128 // cerr << "false" << endl;
129 //if (nodes.size() != (routes.size()) +1)
130 // cerr << "ALERT: Misconfigured TaxiRoute : " << nodes.size() << " " << routes.size() << endl;
132 if (currNode == nodes.end())
135 if (currNode != nodes.begin()) // make sure route corresponds to the end node
141 bool FGTaxiRoute::next(int *nde, int *rte)
143 //for (intVecIterator i = nodes.begin(); i != nodes.end(); i++)
144 // cerr << "FGTaxiRoute contains : " << *(i) << endl;
145 //cerr << "Offset from end: " << nodes.end() - currNode << endl;
146 //if (currNode != nodes.end())
147 // cerr << "true" << endl;
149 // cerr << "false" << endl;
150 if (nodes.size() != (routes.size()) + 1) {
151 SG_LOG(SG_GENERAL, SG_ALERT,
152 "ALERT: Misconfigured TaxiRoute : " << nodes.
153 size() << " " << routes.size());
156 if (currNode == nodes.end())
159 //*rte = *(currRoute);
160 if (currNode != nodes.begin()) // Make sure route corresponds to the end node
165 // If currNode points to the first node, this means the aircraft is not on the taxi node
166 // yet. Make sure to return a unique identifyer in this situation though, because otherwise
167 // the speed adjust AI code may be unable to resolve whether two aircraft are on the same
168 // taxi route or not. the negative of the preceding route seems a logical choice, as it is
169 // unique for any starting location.
170 // Note that this is probably just a temporary fix until I get Parking / tower control working.
171 *rte = -1 * *(currRoute);
178 void FGTaxiRoute::rewind(int route)
184 if (!(next(&currPoint, &currRoute))) {
185 SG_LOG(SG_GENERAL, SG_ALERT,
186 "Error in rewinding TaxiRoute: current" << currRoute <<
189 } while (currRoute != route);
195 /***************************************************************************
197 **************************************************************************/
198 bool compare_nodes(FGTaxiNode * a, FGTaxiNode * b)
203 bool compare_segments(FGTaxiSegment * a, FGTaxiSegment * b)
208 FGGroundNetwork::FGGroundNetwork()
216 currTraffic = activeTraffic.begin();
218 networkInitialized = false;
222 FGGroundNetwork::~FGGroundNetwork()
224 //cerr << "Running Groundnetwork Destructor " << endl;
225 bool saveData = false;
227 if (fgGetBool("/sim/ai/groundnet-cache")) {
228 SGPath cacheData(fgGetString("/sim/fg-home"));
229 cacheData.append("ai");
230 string airport = parent->getId();
232 if ((airport) != "") {
234 ::snprintf(buffer, 128, "%c/%c/%c/",
235 airport[0], airport[1], airport[2]);
236 cacheData.append(buffer);
237 if (!cacheData.exists()) {
238 cacheData.create_dir(0777);
240 cacheData.append(airport + "-groundnet-cache.txt");
241 cachefile.open(cacheData.str().c_str());
245 cachefile << "[GroundNetcachedata:ref:2011:09:04]" << endl;
246 for (FGTaxiNodeVectorIterator node = nodes.begin();
247 node != nodes.end(); node++) {
249 cachefile << (*node)->getIndex () << " "
250 << (*node)->getElevation () << " "
256 pushBackNodes.clear();
257 for (FGTaxiSegmentVectorIterator seg = segments.begin();
258 seg != segments.end(); seg++) {
267 void FGGroundNetwork::saveElevationCache() {
268 //cerr << "Running Groundnetwork Destructor " << endl;
269 bool saveData = false;
271 if (fgGetBool("/sim/ai/groundnet-cache")) {
272 SGPath cacheData(fgGetString("/sim/fg-home"));
273 cacheData.append("ai");
274 string airport = parent->getId();
276 if ((airport) != "") {
278 ::snprintf(buffer, 128, "%c/%c/%c/",
279 airport[0], airport[1], airport[2]);
280 cacheData.append(buffer);
281 if (!cacheData.exists()) {
282 cacheData.create_dir(0777);
284 cacheData.append(airport + "-groundnet-cache.txt");
285 cachefile.open(cacheData.str().c_str());
289 cachefile << "[GroundNetcachedata:ref:2011:09:04]" << endl;
290 for (FGTaxiNodeVectorIterator node = nodes.begin();
291 node != nodes.end(); node++) {
293 cachefile << (*node)->getIndex () << " "
294 << (*node)->getElevation () << " "
303 void FGGroundNetwork::addSegment(const FGTaxiSegment & seg)
305 segments.push_back(new FGTaxiSegment(seg));
308 void FGGroundNetwork::addNode(const FGTaxiNode & node)
310 nodes.push_back(new FGTaxiNode(node));
313 void FGGroundNetwork::addNodes(FGParkingVec * parkings)
316 FGParkingVecIterator i = parkings->begin();
317 while (i != parkings->end()) {
318 n.setIndex(i->getIndex());
319 n.setLatitude(i->getLatitude());
320 n.setLongitude(i->getLongitude());
321 n.setElevation(parent->getElevation());
322 nodes.push_back(new FGTaxiNode(n));
330 void FGGroundNetwork::init()
332 if (networkInitialized) {
333 FGATCController::init();
334 //cerr << "FGground network already initialized" << endl;
340 sort(nodes.begin(), nodes.end(), compare_nodes);
341 //sort(segments.begin(), segments.end(), compare_segments());
342 FGTaxiSegmentVectorIterator i = segments.begin();
343 while (i != segments.end()) {
344 (*i)->setStart(&nodes);
345 (*i)->setEnd(&nodes);
346 (*i)->setDimensions(parent->getElevation() * SG_FEET_TO_METER);
347 (*i)->setIndex(index);
348 if ((*i)->isPushBack()) {
349 pushBackNodes.push_back((*i)->getEnd());
351 //SG_LOG(SG_GENERAL, SG_BULK, "initializing segment " << (*i)->getIndex() << endl);
352 //SG_LOG(SG_GENERAL, SG_BULK, "Track distance = " << (*i)->getLength() << endl);
353 //SG_LOG(SG_GENERAL, SG_BULK, "Track runs from " << (*i)->getStart()->getIndex() << " to "
354 // << (*i)->getEnd()->getIndex() << endl);
359 i = segments.begin();
360 while (i != segments.end()) {
361 FGTaxiSegmentVectorIterator j = (*i)->getEnd()->getBeginRoute();
362 while (j != (*i)->getEnd()->getEndRoute()) {
363 if ((*j)->getEnd()->getIndex() == (*i)->getStart()->getIndex()) {
364 // int start1 = (*i)->getStart()->getIndex();
365 // int end1 = (*i)->getEnd() ->getIndex();
366 // int start2 = (*j)->getStart()->getIndex();
367 // int end2 = (*j)->getEnd()->getIndex();
368 // int oppIndex = (*j)->getIndex();
369 //cerr << "Opposite of " << (*i)->getIndex() << " (" << start1 << "," << end1 << ") "
370 // << "happens to be " << oppIndex << " (" << start2 << "," << end2 << ") " << endl;
371 (*i)->setOpposite(*j);
378 //FGTaxiNodeVectorIterator j = nodes.begin();
379 //while (j != nodes.end()) {
380 // if ((*j)->getHoldPointType() == 3) {
381 // pushBackNodes.push_back((*j));
385 //cerr << "Done initializing ground network" << endl;
387 if (fgGetBool("/sim/ai/groundnet-cache")) {
388 SGPath cacheData(fgGetString("/sim/fg-home"));
389 cacheData.append("ai");
390 string airport = parent->getId();
392 if ((airport) != "") {
394 ::snprintf(buffer, 128, "%c/%c/%c/",
395 airport[0], airport[1], airport[2]);
396 cacheData.append(buffer);
397 if (!cacheData.exists()) {
398 cacheData.create_dir(0777);
402 cacheData.append(airport + "-groundnet-cache.txt");
403 if (cacheData.exists()) {
404 ifstream data(cacheData.c_str());
407 if (revisionStr != "[GroundNetcachedata:ref:2011:09:04]") {
408 SG_LOG(SG_GENERAL, SG_ALERT,"GroundNetwork Warning: discarding outdated cachefile " <<
409 cacheData.c_str() << " for Airport " << airport);
411 for (FGTaxiNodeVectorIterator i = nodes.begin();
414 (*i)->setElevation(parent->getElevation() * SG_FEET_TO_METER);
415 data >> index >> elev;
418 if (index != (*i)->getIndex()) {
419 SG_LOG(SG_GENERAL, SG_ALERT, "Index read from ground network cache at airport " << airport << " does not match index in the network itself");
421 (*i)->setElevation(elev);
428 //cerr << "Finished initializing " << parent->getId() << " groundnetwork " << endl;
429 networkInitialized = true;
432 int FGGroundNetwork::findNearestNode(const SGGeod & aGeod)
434 double minDist = HUGE_VAL;
437 for (FGTaxiNodeVectorIterator itr = nodes.begin(); itr != nodes.end();
439 double d = SGGeodesy::distanceM(aGeod, (*itr)->getGeod());
442 index = (*itr)->getIndex();
443 //cerr << "Minimum distance of " << minDist << " for index " << index << endl;
450 int FGGroundNetwork::findNearestNode(double lat, double lon)
452 return findNearestNode(SGGeod::fromDeg(lon, lat));
455 FGTaxiNode *FGGroundNetwork::findNode(unsigned idx)
457 for (FGTaxiNodeVectorIterator
459 itr != nodes.end(); itr++)
461 if (itr->getIndex() == idx)
462 return itr->getAddress();
465 if ((idx >= 0) && (idx < nodes.size()))
466 return nodes[idx]->getAddress();
471 FGTaxiSegment *FGGroundNetwork::findSegment(unsigned idx)
473 for (FGTaxiSegmentVectorIterator
474 itr = segments.begin();
475 itr != segments.end(); itr++)
477 if (itr->getIndex() == idx)
478 return itr->getAddress();
481 if ((idx > 0) && (idx <= segments.size()))
482 return segments[idx - 1]->getAddress();
484 //cerr << "Alert: trying to find invalid segment " << idx << endl;
490 FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end,
493 //implements Dijkstra's algorithm to find shortest distance route from start to end
494 //taken from http://en.wikipedia.org/wiki/Dijkstra's_algorithm
496 //double INFINITE = 100000000000.0;
497 // initialize scoring values
498 int nParkings = parent->getDynamics()->getNrOfParkings();
499 FGTaxiNodeVector *currNodesSet;
501 currNodesSet = &nodes;
503 currNodesSet = &pushBackNodes;
506 for (FGTaxiNodeVectorIterator
507 itr = currNodesSet->begin(); itr != currNodesSet->end(); itr++) {
508 (*itr)->setPathScore(HUGE_VAL); //infinity by all practical means
509 (*itr)->setPreviousNode(0); //
510 (*itr)->setPreviousSeg(0); //
513 FGTaxiNode *firstNode = findNode(start);
514 firstNode->setPathScore(0);
516 FGTaxiNode *lastNode = findNode(end);
518 FGTaxiNodeVector unvisited(*currNodesSet); // working copy
520 while (!unvisited.empty()) {
521 FGTaxiNode *best = *(unvisited.begin());
522 for (FGTaxiNodeVectorIterator
523 itr = unvisited.begin(); itr != unvisited.end(); itr++) {
524 if ((*itr)->getPathScore() < best->getPathScore())
528 FGTaxiNodeVectorIterator newend =
529 remove(unvisited.begin(), unvisited.end(), best);
530 unvisited.erase(newend, unvisited.end());
532 if (best == lastNode) { // found route or best not connected
535 for (FGTaxiSegmentVectorIterator
536 seg = best->getBeginRoute();
537 seg != best->getEndRoute(); seg++) {
538 if (fullSearch || (*seg)->isPushBack()) {
539 FGTaxiNode *tgt = (*seg)->getEnd();
541 best->getPathScore() + (*seg)->getLength() +
542 (*seg)->getPenalty(nParkings);
543 if (alt < tgt->getPathScore()) { // Relax (u,v)
544 tgt->setPathScore(alt);
545 tgt->setPreviousNode(best);
546 tgt->setPreviousSeg(*seg); //
549 // // cerr << "Skipping TaxiSegment " << (*seg)->getIndex() << endl;
555 if (lastNode->getPathScore() == HUGE_VAL) {
556 // no valid route found
558 SG_LOG(SG_GENERAL, SG_ALERT,
559 "Failed to find route from waypoint " << start << " to "
560 << end << " at " << parent->getId());
564 //exit(1); //TODO exit more gracefully, no need to stall the whole sim with broken GN's
566 // assemble route from backtrace information
567 intVec nodes, routes;
568 FGTaxiNode *bt = lastNode;
569 while (bt->getPreviousNode() != 0) {
570 nodes.push_back(bt->getIndex());
571 routes.push_back(bt->getPreviousSegment()->getIndex());
572 bt = bt->getPreviousNode();
574 nodes.push_back(start);
575 reverse(nodes.begin(), nodes.end());
576 reverse(routes.begin(), routes.end());
578 return FGTaxiRoute(nodes, routes, lastNode->getPathScore(), 0);
582 int FGTaxiSegment::getPenalty(int nGates)
585 if (end->getIndex() < nGates) {
588 if (end->getIsOnRunway()) { // For now. In future versions, need to find out whether runway is active.
594 /* ATC Related Functions */
596 void FGGroundNetwork::announcePosition(int id,
597 FGAIFlightPlan * intendedRoute,
598 int currentPosition, double lat,
599 double lon, double heading,
600 double speed, double alt,
601 double radius, int leg,
602 FGAIAircraft * aircraft)
605 TrafficVectorIterator i = activeTraffic.begin();
606 // Search search if the current id alread has an entry
607 // This might be faster using a map instead of a vector, but let's start by taking a safe route
608 if (activeTraffic.size()) {
609 //while ((i->getId() != id) && i != activeTraffic.end()) {
610 while (i != activeTraffic.end()) {
611 if (i->getId() == id) {
617 // Add a new TrafficRecord if no one exsists for this aircraft.
618 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
622 rec.setPositionAndIntentions(currentPosition, intendedRoute);
623 rec.setPositionAndHeading(lat, lon, heading, speed, alt);
624 rec.setRadius(radius); // only need to do this when creating the record.
625 rec.setAircraft(aircraft);
626 activeTraffic.push_back(rec);
628 i->setPositionAndIntentions(currentPosition, intendedRoute);
629 i->setPositionAndHeading(lat, lon, heading, speed, alt);
634 void FGGroundNetwork::signOff(int id)
636 TrafficVectorIterator i = activeTraffic.begin();
637 // Search search if the current id alread has an entry
638 // This might be faster using a map instead of a vector, but let's start by taking a safe route
639 if (activeTraffic.size()) {
640 //while ((i->getId() != id) && i != activeTraffic.end()) {
641 while (i != activeTraffic.end()) {
642 if (i->getId() == id) {
648 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
649 SG_LOG(SG_GENERAL, SG_ALERT,
650 "AI error: Aircraft without traffic record is signing off");
652 i = activeTraffic.erase(i);
656 * The ground network can deal with the following states:
657 * 0 = Normal; no action required
658 * 1 = "Acknowledge "Hold position
659 * 2 = "Acknowledge "Resume taxi".
660 * 3 = "Issue TaxiClearance"
661 * 4 = Acknowledge Taxi Clearance"
662 * 5 = Post acknowlegde taxiclearance: Start taxiing
664 * 7 = Acknowledge report runway
665 * 8 = Switch tower frequency
666 * 9 = Acknowledge switch tower frequency
667 *************************************************************************************************************************/
668 bool FGGroundNetwork::checkTransmissionState(int minState, int maxState, TrafficVectorIterator i, time_t now, AtcMsgId msgId,
671 int state = i->getState();
672 if ((state >= minState) && (state <= maxState) && available) {
673 if ((msgDir == ATC_AIR_TO_GROUND) && isUserAircraft(i->getAircraft())) {
674 //cerr << "Checking state " << state << " for " << i->getAircraft()->getCallSign() << endl;
675 static SGPropertyNode_ptr trans_num = globals->get_props()->getNode("/sim/atc/transmission-num", true);
676 int n = trans_num->getIntValue();
678 trans_num->setIntValue(-1);
680 //cerr << "Selected transmission message " << n << endl;
681 FGATCManager *atc = (FGATCManager*) globals->get_subsystem("atc");
682 FGATCDialogNew::instance()->removeEntry(1);
684 //cerr << "creating message for " << i->getAircraft()->getCallSign() << endl;
685 transmit(&(*i), msgId, msgDir, false);
689 transmit(&(*i), msgId, msgDir, true);
691 lastTransmission = now;
698 void FGGroundNetwork::updateAircraftInformation(int id, double lat, double lon,
699 double heading, double speed, double alt,
702 time_t currentTime = time(NULL);
703 if (nextSave < currentTime) {
704 saveElevationCache();
705 nextSave = currentTime + 100 + rand() % 200;
707 // Check whether aircraft are on hold due to a preceding pushback. If so, make sure to
708 // Transmit air-to-ground "Ready to taxi request:
709 // Transmit ground to air approval / hold
710 // Transmit confirmation ...
711 // Probably use a status mechanism similar to the Engine start procedure in the startup controller.
714 TrafficVectorIterator i = activeTraffic.begin();
715 // Search search if the current id has an entry
716 // This might be faster using a map instead of a vector, but let's start by taking a safe route
717 TrafficVectorIterator current, closest;
718 if (activeTraffic.size()) {
719 //while ((i->getId() != id) && i != activeTraffic.end()) {
720 while (i != activeTraffic.end()) {
721 if (i->getId() == id) {
727 // update position of the current aircraft
728 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
729 SG_LOG(SG_GENERAL, SG_ALERT,
730 "AI error: updating aircraft without traffic record");
732 i->setPositionAndHeading(lat, lon, heading, speed, alt);
738 // Update every three secs, but add some randomness
739 // to prevent all IA objects doing this in synchrony
740 //if (getDt() < (3.0) + (rand() % 10))
744 current->clearResolveCircularWait();
745 current->setWaitsForId(0);
746 checkSpeedAdjustment(id, lat, lon, heading, speed, alt);
747 bool needsTaxiClearance = current->getAircraft()->getTaxiClearanceRequest();
748 if (!needsTaxiClearance) {
749 checkHoldPosition(id, lat, lon, heading, speed, alt);
750 if (checkForCircularWaits(id)) {
751 i->setResolveCircularWait();
754 current->setHoldPosition(true);
755 int state = current->getState();
756 time_t now = time(NULL) + fgGetLong("/sim/time/warp");
757 if ((now - lastTransmission) > 15) {
760 if (checkTransmissionState(0,2, current, now, MSG_REQUEST_TAXI_CLEARANCE, ATC_AIR_TO_GROUND)) {
761 current->setState(3);
763 if (checkTransmissionState(3,3, current, now, MSG_ISSUE_TAXI_CLEARANCE, ATC_GROUND_TO_AIR)) {
764 current->setState(4);
766 if (checkTransmissionState(4,4, current, now, MSG_ACKNOWLEDGE_TAXI_CLEARANCE, ATC_AIR_TO_GROUND)) {
767 current->setState(5);
769 if ((state == 5) && available) {
770 current->setState(0);
771 current->getAircraft()->setTaxiClearanceRequest(false);
772 current->setHoldPosition(false);
780 Scan for a speed adjustment change. Find the nearest aircraft that is in front
781 and adjust speed when we get too close. Only do this when current position and/or
782 intentions of the current aircraft match current taxiroute position of the proximate
783 aircraft. For traffic that is on other routes we need to issue a "HOLD Position"
784 instruction. See below for the hold position instruction.
786 Note that there currently still is one flaw in the logic that needs to be addressed.
787 There can be situations where one aircraft is in front of the current aircraft, on a separate
788 route, but really close after an intersection coming off the current route. This
789 aircraft is still close enough to block the current aircraft. This situation is currently
790 not addressed yet, but should be.
793 void FGGroundNetwork::checkSpeedAdjustment(int id, double lat,
794 double lon, double heading,
795 double speed, double alt)
798 TrafficVectorIterator current, closest;
799 TrafficVectorIterator i = activeTraffic.begin();
800 bool otherReasonToSlowDown = false;
801 bool previousInstruction;
802 if (activeTraffic.size()) {
803 //while ((i->getId() != id) && (i != activeTraffic.end()))
804 while (i != activeTraffic.end()) {
805 if (i->getId() == id) {
813 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
814 SG_LOG(SG_GENERAL, SG_ALERT,
815 "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkSpeedAdjustment");
820 previousInstruction = current->getSpeedAdjustment();
821 double mindist = HUGE_VAL;
822 if (activeTraffic.size()) {
823 double course, dist, bearing, minbearing, az2;
824 SGGeod curr(SGGeod::fromDegM(lon, lat, alt));
825 //TrafficVector iterator closest;
827 for (TrafficVectorIterator i = activeTraffic.begin();
828 i != activeTraffic.end(); i++) {
833 SGGeod other(SGGeod::fromDegM(i->getLongitude(),
836 SGGeodesy::inverse(curr, other, course, az2, dist);
837 bearing = fabs(heading - course);
839 bearing = 360 - bearing;
840 if ((dist < mindist) && (bearing < 60.0)) {
843 minbearing = bearing;
846 //Check traffic at the tower controller
847 if (towerController->hasActiveTraffic()) {
848 for (TrafficVectorIterator i =
849 towerController->getActiveTraffic().begin();
850 i != towerController->getActiveTraffic().end(); i++) {
851 //cerr << "Comparing " << current->getId() << " and " << i->getId() << endl;
852 SGGeod other(SGGeod::fromDegM(i->getLongitude(),
855 SGGeodesy::inverse(curr, other, course, az2, dist);
856 bearing = fabs(heading - course);
858 bearing = 360 - bearing;
859 if ((dist < mindist) && (bearing < 60.0)) {
860 //cerr << "Current aircraft " << current->getAircraft()->getTrafficRef()->getCallSign()
861 // << " is closest to " << i->getAircraft()->getTrafficRef()->getCallSign()
862 // << ", which has status " << i->getAircraft()->isScheduledForTakeoff()
866 minbearing = bearing;
867 otherReasonToSlowDown = true;
871 // Finally, check UserPosition
872 // Note, as of 2011-08-01, this should no longer be necessecary.
874 double userLatitude = fgGetDouble("/position/latitude-deg");
875 double userLongitude = fgGetDouble("/position/longitude-deg");
876 SGGeod user(SGGeod::fromDeg(userLongitude, userLatitude));
877 SGGeodesy::inverse(curr, user, course, az2, dist);
879 bearing = fabs(heading - course);
881 bearing = 360 - bearing;
882 if ((dist < mindist) && (bearing < 60.0)) {
885 minbearing = bearing;
886 otherReasonToSlowDown = true;
889 current->clearSpeedAdjustment();
891 if (current->checkPositionAndIntentions(*closest)
892 || otherReasonToSlowDown) {
893 double maxAllowableDistance =
894 (1.1 * current->getRadius()) +
895 (1.1 * closest->getRadius());
896 if (mindist < 2 * maxAllowableDistance) {
897 if (current->getId() == closest->getWaitsForId())
900 current->setWaitsForId(closest->getId());
901 if (closest->getId() != current->getId()) {
902 current->setSpeedAdjustment(closest->getSpeed() *
905 closest->getAircraft()->getTakeOffStatus() &&
906 (current->getAircraft()->getTrafficRef()->getDepartureAirport() == closest->getAircraft()->getTrafficRef()->getDepartureAirport()) &&
907 (current->getAircraft()->GetFlightPlan()->getRunway() == closest->getAircraft()->GetFlightPlan()->getRunway())
909 current->getAircraft()->scheduleForATCTowerDepartureControl(1);
911 current->setSpeedAdjustment(0); // This can only happen when the user aircraft is the one closest
913 if (mindist < maxAllowableDistance) {
914 //double newSpeed = (maxAllowableDistance-mindist);
915 //current->setSpeedAdjustment(newSpeed);
916 //if (mindist < 0.5* maxAllowableDistance)
918 current->setSpeedAdjustment(0);
927 Check for "Hold position instruction".
928 The hold position should be issued under the following conditions:
929 1) For aircraft entering or crossing a runway with active traffic on it, or landing aircraft near it
930 2) For taxiing aircraft that use one taxiway in opposite directions
931 3) For crossing or merging taxiroutes.
934 void FGGroundNetwork::checkHoldPosition(int id, double lat,
935 double lon, double heading,
936 double speed, double alt)
938 TrafficVectorIterator current;
939 TrafficVectorIterator i = activeTraffic.begin();
940 if (activeTraffic.size()) {
941 //while ((i->getId() != id) && i != activeTraffic.end())
942 while (i != activeTraffic.end()) {
943 if (i->getId() == id) {
951 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
952 SG_LOG(SG_GENERAL, SG_ALERT,
953 "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkHoldPosition");
956 bool origStatus = current->hasHoldPosition();
957 current->setHoldPosition(false);
958 SGGeod curr(SGGeod::fromDegM(lon, lat, alt));
960 for (i = activeTraffic.begin(); i != activeTraffic.end(); i++) {
961 if (i->getId() != current->getId()) {
962 int node = current->crosses(this, *i);
964 FGTaxiNode *taxiNode = findNode(node);
966 // Determine whether it's save to continue or not.
967 // If we have a crossing route, there are two possibilities:
968 // 1) This is an interestion
969 // 2) This is oncoming two-way traffic, using the same taxiway.
970 //cerr << "Hold check 1 : " << id << " has common node " << node << endl;
972 SGGeod other(SGGeod::
973 fromDegM(i->getLongitude(), i->getLatitude(),
977 if (current->isOpposing(this, *i, node)) {
980 //cerr << "Hold check 2 : " << node << " has opposing segment " << endl;
981 // issue a "Hold Position" as soon as we're close to the offending node
982 // For now, I'm doing this as long as the other aircraft doesn't
983 // have a hold instruction as soon as we're within a reasonable
984 // distance from the offending node.
985 // This may be a bit of a conservative estimate though, as it may
986 // be well possible that both aircraft can both continue to taxi
987 // without crashing into each other.
990 if (SGGeodesy::distanceM(other, taxiNode->getGeod()) > 200) // 2.0*i->getRadius())
993 //cerr << "Hold check 3 : " << id <<" Other aircraft approaching node is still far away. (" << dist << " nm). Can safely continue "
997 //cerr << "Hold check 4: " << id << " Would need to wait for other aircraft : distance = " << dist << " meters" << endl;
1002 SGGeodesy::distanceM(curr, taxiNode->getGeod());
1003 if (!(i->hasHoldPosition())) {
1005 if ((dist < 200) && //2.5*current->getRadius()) &&
1006 (needsToWait) && (i->onRoute(this, *current)) &&
1007 //((i->onRoute(this, *current)) || ((!(i->getSpeedAdjustment())))) &&
1008 (!(current->getId() == i->getWaitsForId())))
1009 //(!(i->getSpeedAdjustment()))) // &&
1010 //(!(current->getSpeedAdjustment())))
1013 if (!(isUserAircraft(i->getAircraft()))) { // test code. Don't wait for the user, let the user wait for you.
1014 current->setHoldPosition(true);
1015 current->setWaitsForId(i->getId());
1017 //cerr << "Hold check 5: " << current->getCallSign() <<" Setting Hold Position: distance to node (" << node << ") "
1018 // << dist << " meters. Waiting for " << i->getCallSign();
1020 //cerr <<" [opposing] " << endl;
1022 // cerr << "[non-opposing] " << endl;
1023 //if (i->hasSpeefAdjustment())
1025 // cerr << " (which in turn waits for ) " << i->
1027 //cerr << "Hold check 6: " << id << " No need to hold yet: Distance to node : " << dist << " nm"<< endl;
1033 bool currStatus = current->hasHoldPosition();
1034 current->setHoldPosition(origStatus);
1035 // Either a Hold Position or a resume taxi transmission has been issued
1036 time_t now = time(NULL) + fgGetLong("/sim/time/warp");
1037 if ((now - lastTransmission) > 2) {
1040 if (current->getState() == 0) {
1041 if ((origStatus != currStatus) && available) {
1042 //cerr << "Issueing hold short instrudtion " << currStatus << " " << available << endl;
1043 if (currStatus == true) { // No has a hold short instruction
1044 transmit(&(*current), MSG_HOLD_POSITION, ATC_GROUND_TO_AIR, true);
1045 //cerr << "Transmittin hold short instrudtion " << currStatus << " " << available << endl;
1046 current->setState(1);
1048 transmit(&(*current), MSG_RESUME_TAXI, ATC_GROUND_TO_AIR, true);
1049 //cerr << "Transmittig resume instrudtion " << currStatus << " " << available << endl;
1050 current->setState(2);
1052 lastTransmission = now;
1054 // Don't act on the changed instruction until the transmission is confirmed
1055 // So set back to original status
1056 //cerr << "Current state " << current->getState() << endl;
1060 // 6 = Report runway
1061 // 7 = Acknowledge report runway
1062 // 8 = Switch tower frequency
1063 //9 = Acknowledge switch tower frequency
1065 //int state = current->getState();
1066 if (checkTransmissionState(1,1, current, now, MSG_ACKNOWLEDGE_HOLD_POSITION, ATC_AIR_TO_GROUND)) {
1067 current->setState(0);
1068 current->setHoldPosition(true);
1070 if (checkTransmissionState(2,2, current, now, MSG_ACKNOWLEDGE_RESUME_TAXI, ATC_AIR_TO_GROUND)) {
1071 current->setState(0);
1072 current->setHoldPosition(false);
1074 if (current->getAircraft()->getTakeOffStatus() && (current->getState() == 0)) {
1075 //cerr << "Scheduling " << current->getAircraft()->getCallSign() << " for hold short" << endl;
1076 current->setState(6);
1078 if (checkTransmissionState(6,6, current, now, MSG_REPORT_RUNWAY_HOLD_SHORT, ATC_AIR_TO_GROUND)) {
1080 if (checkTransmissionState(7,7, current, now, MSG_ACKNOWLEDGE_REPORT_RUNWAY_HOLD_SHORT, ATC_GROUND_TO_AIR)) {
1082 if (checkTransmissionState(8,8, current, now, MSG_SWITCH_TOWER_FREQUENCY, ATC_GROUND_TO_AIR)) {
1084 if (checkTransmissionState(9,9, current, now, MSG_ACKNOWLEDGE_SWITCH_TOWER_FREQUENCY, ATC_AIR_TO_GROUND)) {
1089 //current->setState(0);
1093 * Check whether situations occur where the current aircraft is waiting for itself
1094 * due to higher order interactions.
1095 * A 'circular' wait is a situation where a waits for b, b waits for c, and c waits
1096 * for a. Ideally each aircraft only waits for one other aircraft, so by tracing
1097 * through this list of waiting aircraft, we can check if we'd eventually end back
1098 * at the current aircraft.
1100 * Note that we should consider the situation where we are actually checking aircraft
1101 * d, which is waiting for aircraft a. d is not part of the loop, but is held back by
1102 * the looping aircraft. If we don't check for that, this function will get stuck into
1106 bool FGGroundNetwork::checkForCircularWaits(int id)
1108 //cerr << "Performing Wait check " << id << endl;
1110 TrafficVectorIterator current, other;
1111 TrafficVectorIterator i = activeTraffic.begin();
1112 int trafficSize = activeTraffic.size();
1114 while (i != activeTraffic.end()) {
1115 if (i->getId() == id) {
1123 if (i == activeTraffic.end() || (trafficSize == 0)) {
1124 SG_LOG(SG_GENERAL, SG_ALERT,
1125 "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits");
1129 target = current->getWaitsForId();
1130 //bool printed = false; // Note that this variable is for debugging purposes only.
1134 //cerr << "aircraft waits for user" << endl;
1139 while ((target > 0) && (target != id) && counter++ < trafficSize) {
1141 TrafficVectorIterator i = activeTraffic.begin();
1143 //while ((i->getId() != id) && i != activeTraffic.end())
1144 while (i != activeTraffic.end()) {
1145 if (i->getId() == target) {
1153 if (i == activeTraffic.end() || (trafficSize == 0)) {
1154 //cerr << "[Waiting for traffic at Runway: DONE] " << endl << endl;;
1155 // The target id is not found on the current network, which means it's at the tower
1156 //SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits");
1160 target = other->getWaitsForId();
1162 // actually this trap isn't as impossible as it first seemed:
1163 // the setWaitsForID(id) is set to current when the aircraft
1164 // is waiting for the user controlled aircraft.
1165 //if (current->getId() == other->getId()) {
1166 // cerr << "Caught the impossible trap" << endl;
1167 // cerr << "Current = " << current->getId() << endl;
1168 // cerr << "Other = " << other ->getId() << endl;
1169 // for (TrafficVectorIterator at = activeTraffic.begin();
1170 // at != activeTraffic.end();
1172 // cerr << "currently active aircraft : " << at->getCallSign() << " with Id " << at->getId() << " waits for " << at->getWaitsForId() << endl;
1175 if (current->getId() == other->getId())
1178 //cerr << current->getCallSign() << " (" << current->getId() << ") " << " -> " << other->getCallSign()
1179 // << " (" << other->getId() << "); " << endl;;
1189 // cerr << "[done] " << endl << endl;;
1191 SG_LOG(SG_GENERAL, SG_WARN,
1192 "Detected circular wait condition: Id = " << id <<
1193 "target = " << target);
1200 // Note that this function is probably obsolete...
1201 bool FGGroundNetwork::hasInstruction(int id)
1203 TrafficVectorIterator i = activeTraffic.begin();
1204 // Search search if the current id has an entry
1205 // This might be faster using a map instead of a vector, but let's start by taking a safe route
1206 if (activeTraffic.size()) {
1207 //while ((i->getId() != id) && i != activeTraffic.end()) {
1208 while (i != activeTraffic.end()) {
1209 if (i->getId() == id) {
1215 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
1216 SG_LOG(SG_GENERAL, SG_ALERT,
1217 "AI error: checking ATC instruction for aircraft without traffic record");
1219 return i->hasInstruction();
1224 FGATCInstruction FGGroundNetwork::getInstruction(int id)
1226 TrafficVectorIterator i = activeTraffic.begin();
1227 // Search search if the current id has an entry
1228 // This might be faster using a map instead of a vector, but let's start by taking a safe route
1229 if (activeTraffic.size()) {
1230 //while ((i->getId() != id) && i != activeTraffic.end()) {
1231 while (i != activeTraffic.end()) {
1232 if (i->getId() == id) {
1238 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
1239 SG_LOG(SG_GENERAL, SG_ALERT,
1240 "AI error: requesting ATC instruction for aircraft without traffic record");
1242 return i->getInstruction();
1244 return FGATCInstruction();
1247 // Note that this function is copied from simgear. for maintanance purposes, it's probabtl better to make a general function out of that.
1248 static void WorldCoordinate(osg::Matrix& obj_pos, double lat,
1249 double lon, double elev, double hdg, double slope)
1251 SGGeod geod = SGGeod::fromDegM(lon, lat, elev);
1252 obj_pos = geod.makeZUpFrame();
1253 // hdg is not a compass heading, but a counter-clockwise rotation
1254 // around the Z axis
1255 obj_pos.preMult(osg::Matrix::rotate(hdg * SGD_DEGREES_TO_RADIANS,
1257 obj_pos.preMult(osg::Matrix::rotate(slope * SGD_DEGREES_TO_RADIANS,
1264 void FGGroundNetwork::render(bool visible)
1267 SGMaterialLib *matlib = globals->get_matlib();
1270 globals->get_scenery()->get_scene_graph()->removeChild(group);
1271 //while (group->getNumChildren()) {
1272 // cerr << "Number of children: " << group->getNumChildren() << endl;
1273 //simgear::EffectGeode* geode = (simgear::EffectGeode*) group->getChild(0);
1274 //osg::MatrixTransform *obj_trans = (osg::MatrixTransform*) group->getChild(0);
1275 //geode->releaseGLObjects();
1276 //group->removeChild(geode);
1281 group = new osg::Group;
1282 FGScenery * local_scenery = globals->get_scenery();
1283 double elevation_meters = 0.0;
1284 double elevation_feet = 0.0;
1285 //for ( FGTaxiSegmentVectorIterator i = segments.begin(); i != segments.end(); i++) {
1287 for (TrafficVectorIterator i = activeTraffic.begin(); i != activeTraffic.end(); i++) {
1288 // Handle start point
1289 int pos = i->getCurrentPosition() - 1;
1292 SGGeod start(SGGeod::fromDeg((i->getLongitude()), (i->getLatitude())));
1293 SGGeod end (SGGeod::fromDeg(segments[pos]->getEnd()->getLongitude(), segments[pos]->getEnd()->getLatitude()));
1295 double length = SGGeodesy::distanceM(start, end);
1296 //heading = SGGeodesy::headingDeg(start->getGeod(), end->getGeod());
1298 double az2, heading; //, distanceM;
1299 SGGeodesy::inverse(start, end, heading, az2, length);
1300 double coveredDistance = length * 0.5;
1302 SGGeodesy::direct(start, heading, coveredDistance, center, az2);
1303 //cerr << "Active Aircraft : Centerpoint = (" << center.getLatitudeDeg() << ", " << center.getLongitudeDeg() << "). Heading = " << heading << endl;
1304 ///////////////////////////////////////////////////////////////////////////////
1305 // Make a helper function out of this
1306 osg::Matrix obj_pos;
1307 osg::MatrixTransform *obj_trans = new osg::MatrixTransform;
1308 obj_trans->setDataVariance(osg::Object::STATIC);
1309 // Experimental: Calculate slope here, based on length, and the individual elevations
1310 double elevationStart;
1311 if (isUserAircraft((i)->getAircraft())) {
1312 elevationStart = fgGetDouble("/position/ground-elev-m");
1314 elevationStart = ((i)->getAircraft()->_getAltitude());
1316 double elevationEnd = segments[pos]->getEnd()->getElevation();
1317 //cerr << "Using elevation " << elevationEnd << endl;
1319 if ((elevationEnd == 0) || (elevationEnd = parent->getElevation())) {
1320 SGGeod center2 = end;
1321 center2.setElevationM(SG_MAX_ELEVATION_M);
1322 if (local_scenery->get_elevation_m( center2, elevationEnd, NULL )) {
1323 elevation_feet = elevationEnd * SG_METER_TO_FEET + 0.5;
1324 //elevation_meters += 0.5;
1327 elevationEnd = parent->getElevation();
1329 segments[pos]->getEnd()->setElevation(elevationEnd);
1331 double elevationMean = (elevationStart + elevationEnd) / 2.0;
1332 double elevDiff = elevationEnd - elevationStart;
1334 double slope = atan2(elevDiff, length) * SGD_RADIANS_TO_DEGREES;
1336 //cerr << "1. Using mean elevation : " << elevationMean << " and " << slope << endl;
1338 WorldCoordinate( obj_pos, center.getLatitudeDeg(), center.getLongitudeDeg(), elevationMean+ 0.5, -(heading), slope );
1340 obj_trans->setMatrix( obj_pos );
1341 //osg::Vec3 center(0, 0, 0)
1343 float width = length /2.0;
1344 osg::Vec3 corner(-width, 0, 0.25f);
1345 osg::Vec3 widthVec(2*width + 1, 0, 0);
1346 osg::Vec3 heightVec(0, 1, 0);
1347 osg::Geometry* geometry;
1348 geometry = osg::createTexturedQuadGeometry(corner, widthVec, heightVec);
1349 simgear::EffectGeode* geode = new simgear::EffectGeode;
1350 geode->setName("test");
1351 geode->addDrawable(geometry);
1352 //osg::Node *custom_obj;
1353 SGMaterial *mat = matlib->find("UnidirectionalTaper");
1355 geode->setEffect(mat->get_effect());
1356 obj_trans->addChild(geode);
1357 // wire as much of the scene graph together as we can
1358 //->addChild( obj_trans );
1359 group->addChild( obj_trans );
1360 /////////////////////////////////////////////////////////////////////
1362 //cerr << "BIG FAT WARNING: current position is here : " << pos << endl;
1364 for(intVecIterator j = (i)->getIntentions().begin(); j != (i)->getIntentions().end(); j++) {
1365 osg::Matrix obj_pos;
1368 osg::MatrixTransform *obj_trans = new osg::MatrixTransform;
1369 obj_trans->setDataVariance(osg::Object::STATIC);
1371 // Experimental: Calculate slope here, based on length, and the individual elevations
1372 double elevationStart = segments[k]->getStart()->getElevation();
1373 double elevationEnd = segments[k]->getEnd ()->getElevation();
1374 if ((elevationStart == 0) || (elevationStart == parent->getElevation())) {
1375 SGGeod center2 = segments[k]->getStart()->getGeod();
1376 center2.setElevationM(SG_MAX_ELEVATION_M);
1377 if (local_scenery->get_elevation_m( center2, elevationStart, NULL )) {
1378 elevation_feet = elevationStart * SG_METER_TO_FEET + 0.5;
1379 //elevation_meters += 0.5;
1382 elevationStart = parent->getElevation();
1384 segments[k]->getStart()->setElevation(elevationStart);
1386 if ((elevationEnd == 0) || (elevationEnd == parent->getElevation())) {
1387 SGGeod center2 = segments[k]->getEnd()->getGeod();
1388 center2.setElevationM(SG_MAX_ELEVATION_M);
1389 if (local_scenery->get_elevation_m( center2, elevationEnd, NULL )) {
1390 elevation_feet = elevationEnd * SG_METER_TO_FEET + 0.5;
1391 //elevation_meters += 0.5;
1394 elevationEnd = parent->getElevation();
1396 segments[k]->getEnd()->setElevation(elevationEnd);
1399 double elevationMean = (elevationStart + elevationEnd) / 2.0;
1400 double elevDiff = elevationEnd - elevationStart;
1401 double length = segments[k]->getLength();
1402 double slope = atan2(elevDiff, length) * SGD_RADIANS_TO_DEGREES;
1404 // cerr << "2. Using mean elevation : " << elevationMean << " and " << slope << endl;
1407 WorldCoordinate( obj_pos, segments[k]->getLatitude(), segments[k]->getLongitude(), elevationMean+ 0.5, -(segments[k]->getHeading()), slope );
1409 obj_trans->setMatrix( obj_pos );
1410 //osg::Vec3 center(0, 0, 0)
1412 float width = segments[k]->getLength() /2.0;
1413 osg::Vec3 corner(-width, 0, 0.25f);
1414 osg::Vec3 widthVec(2*width + 1, 0, 0);
1415 osg::Vec3 heightVec(0, 1, 0);
1416 osg::Geometry* geometry;
1417 geometry = osg::createTexturedQuadGeometry(corner, widthVec, heightVec);
1418 simgear::EffectGeode* geode = new simgear::EffectGeode;
1419 geode->setName("test");
1420 geode->addDrawable(geometry);
1421 //osg::Node *custom_obj;
1422 SGMaterial *mat = matlib->find("UnidirectionalTaper");
1424 geode->setEffect(mat->get_effect());
1425 obj_trans->addChild(geode);
1426 // wire as much of the scene graph together as we can
1427 //->addChild( obj_trans );
1428 group->addChild( obj_trans );
1433 globals->get_scenery()->get_scene_graph()->addChild(group);
1437 string FGGroundNetwork::getName() {
1438 return string(parent->getId() + "-ground");