2 // groundnet.cxx - Implimentation of the FlightGear airport ground handling code
4 // Written by Durk Talsma, started June 2005.
6 // Copyright (C) 2004 Durk Talsma.
8 // This program is free software; you can redistribute it and/or
9 // modify it under the terms of the GNU General Public License as
10 // published by the Free Software Foundation; either version 2 of the
11 // License, or (at your option) any later version.
13 // This program is distributed in the hope that it will be useful, but
14 // WITHOUT ANY WARRANTY; without even the implied warranty of
15 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
16 // General Public License for more details.
18 // You should have received a copy of the GNU General Public License
19 // along with this program; if not, write to the Free Software
20 // 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/AIFlightPlan.hxx>
49 #include <ATC/atc_mgr.hxx>
51 #include <Scenery/scenery.hxx>
53 #include "groundnetwork.hxx"
55 /***************************************************************************
57 **************************************************************************/
59 void FGTaxiSegment::setStart(FGTaxiNodeVector * nodes)
61 FGTaxiNodeVectorIterator i = nodes->begin();
62 while (i != nodes->end()) {
63 //cerr << "Scanning start node index" << (*i)->getIndex() << endl;
64 if ((*i)->getIndex() == startNode) {
65 start = (*i)->getAddress();
66 (*i)->addSegment(this);
71 SG_LOG(SG_GENERAL, SG_ALERT,
72 "Could not find start node " << startNode << endl);
75 void FGTaxiSegment::setEnd(FGTaxiNodeVector * nodes)
77 FGTaxiNodeVectorIterator i = nodes->begin();
78 while (i != nodes->end()) {
79 //cerr << "Scanning end node index" << (*i)->getIndex() << endl;
80 if ((*i)->getIndex() == endNode) {
81 end = (*i)->getAddress();
86 SG_LOG(SG_GENERAL, SG_ALERT,
87 "Could not find end node " << endNode << endl);
92 // There is probably a computationally cheaper way of
94 void FGTaxiSegment::setDimensions(double elevation)
96 length = SGGeodesy::distanceM(start->getGeod(), end->getGeod());
97 //heading = SGGeodesy::headingDeg(start->getGeod(), end->getGeod());
99 double az2; //, distanceM;
100 SGGeodesy::inverse(start->getGeod(), end->getGeod(), heading, az2, length);
101 double coveredDistance = length * 0.5;
102 SGGeodesy::direct(start->getGeod(), heading, coveredDistance, center, az2);
103 //cerr << "Centerpoint = (" << center.getLatitudeDeg() << ", " << center.getLongitudeDeg() << "). Heading = " << heading << endl;
107 //void FGTaxiSegment::setCourseDiff(double crse)
109 // headingDiff = fabs(course - crse);
111 // if (headingDiff > 180)
112 // headingDiff = fabs(headingDiff - 360);
116 /***************************************************************************
118 **************************************************************************/
119 bool FGTaxiRoute::next(int *nde)
121 //for (intVecIterator i = nodes.begin(); i != nodes.end(); i++)
122 // cerr << "FGTaxiRoute contains : " << *(i) << endl;
123 //cerr << "Offset from end: " << nodes.end() - currNode << endl;
124 //if (currNode != nodes.end())
125 // cerr << "true" << endl;
127 // cerr << "false" << endl;
128 //if (nodes.size() != (routes.size()) +1)
129 // cerr << "ALERT: Misconfigured TaxiRoute : " << nodes.size() << " " << routes.size() << endl;
131 if (currNode == nodes.end())
134 if (currNode != nodes.begin()) // make sure route corresponds to the end node
140 bool FGTaxiRoute::next(int *nde, int *rte)
142 //for (intVecIterator i = nodes.begin(); i != nodes.end(); i++)
143 // cerr << "FGTaxiRoute contains : " << *(i) << endl;
144 //cerr << "Offset from end: " << nodes.end() - currNode << endl;
145 //if (currNode != nodes.end())
146 // cerr << "true" << endl;
148 // cerr << "false" << endl;
149 if (nodes.size() != (routes.size()) + 1) {
150 SG_LOG(SG_GENERAL, SG_ALERT,
151 "ALERT: Misconfigured TaxiRoute : " << nodes.
152 size() << " " << routes.size());
155 if (currNode == nodes.end())
158 //*rte = *(currRoute);
159 if (currNode != nodes.begin()) // Make sure route corresponds to the end node
164 // If currNode points to the first node, this means the aircraft is not on the taxi node
165 // yet. Make sure to return a unique identifyer in this situation though, because otherwise
166 // the speed adjust AI code may be unable to resolve whether two aircraft are on the same
167 // taxi route or not. the negative of the preceding route seems a logical choice, as it is
168 // unique for any starting location.
169 // Note that this is probably just a temporary fix until I get Parking / tower control working.
170 *rte = -1 * *(currRoute);
177 void FGTaxiRoute::rewind(int route)
183 if (!(next(&currPoint, &currRoute))) {
184 SG_LOG(SG_GENERAL, SG_ALERT,
185 "Error in rewinding TaxiRoute: current" << currRoute <<
188 } while (currRoute != route);
194 /***************************************************************************
196 **************************************************************************/
197 bool compare_nodes(FGTaxiNode * a, FGTaxiNode * b)
202 bool compare_segments(FGTaxiSegment * a, FGTaxiSegment * b)
207 FGGroundNetwork::FGGroundNetwork()
215 currTraffic = activeTraffic.begin();
220 FGGroundNetwork::~FGGroundNetwork()
222 for (FGTaxiNodeVectorIterator node = nodes.begin();
223 node != nodes.end(); node++) {
227 pushBackNodes.clear();
228 for (FGTaxiSegmentVectorIterator seg = segments.begin();
229 seg != segments.end(); seg++) {
235 void FGGroundNetwork::addSegment(const FGTaxiSegment & seg)
237 segments.push_back(new FGTaxiSegment(seg));
240 void FGGroundNetwork::addNode(const FGTaxiNode & node)
242 nodes.push_back(new FGTaxiNode(node));
245 void FGGroundNetwork::addNodes(FGParkingVec * parkings)
248 FGParkingVecIterator i = parkings->begin();
249 while (i != parkings->end()) {
250 n.setIndex(i->getIndex());
251 n.setLatitude(i->getLatitude());
252 n.setLongitude(i->getLongitude());
253 n.setElevation(parent->getElevation());
254 nodes.push_back(new FGTaxiNode(n));
262 void FGGroundNetwork::init()
266 sort(nodes.begin(), nodes.end(), compare_nodes);
267 //sort(segments.begin(), segments.end(), compare_segments());
268 FGTaxiSegmentVectorIterator i = segments.begin();
269 while (i != segments.end()) {
270 (*i)->setStart(&nodes);
271 (*i)->setEnd(&nodes);
272 (*i)->setDimensions(parent->getElevation());
273 (*i)->setIndex(index);
274 if ((*i)->isPushBack()) {
275 pushBackNodes.push_back((*i)->getEnd());
277 //SG_LOG(SG_GENERAL, SG_BULK, "initializing segment " << (*i)->getIndex() << endl);
278 //SG_LOG(SG_GENERAL, SG_BULK, "Track distance = " << (*i)->getLength() << endl);
279 //SG_LOG(SG_GENERAL, SG_BULK, "Track runs from " << (*i)->getStart()->getIndex() << " to "
280 // << (*i)->getEnd()->getIndex() << endl);
285 i = segments.begin();
286 while (i != segments.end()) {
287 FGTaxiSegmentVectorIterator j = (*i)->getEnd()->getBeginRoute();
288 while (j != (*i)->getEnd()->getEndRoute()) {
289 if ((*j)->getEnd()->getIndex() == (*i)->getStart()->getIndex()) {
290 // int start1 = (*i)->getStart()->getIndex();
291 // int end1 = (*i)->getEnd() ->getIndex();
292 // int start2 = (*j)->getStart()->getIndex();
293 // int end2 = (*j)->getEnd()->getIndex();
294 // int oppIndex = (*j)->getIndex();
295 //cerr << "Opposite of " << (*i)->getIndex() << " (" << start1 << "," << end1 << ") "
296 // << "happens to be " << oppIndex << " (" << start2 << "," << end2 << ") " << endl;
297 (*i)->setOpposite(*j);
304 //FGTaxiNodeVectorIterator j = nodes.begin();
305 //while (j != nodes.end()) {
306 // if ((*j)->getHoldPointType() == 3) {
307 // pushBackNodes.push_back((*j));
311 //cerr << "Done initializing ground network" << endl;
315 int FGGroundNetwork::findNearestNode(const SGGeod & aGeod)
317 double minDist = HUGE_VAL;
320 for (FGTaxiNodeVectorIterator itr = nodes.begin(); itr != nodes.end();
322 double d = SGGeodesy::distanceM(aGeod, (*itr)->getGeod());
325 index = (*itr)->getIndex();
326 //cerr << "Minimum distance of " << minDist << " for index " << index << endl;
333 int FGGroundNetwork::findNearestNode(double lat, double lon)
335 return findNearestNode(SGGeod::fromDeg(lon, lat));
338 FGTaxiNode *FGGroundNetwork::findNode(unsigned idx)
340 for (FGTaxiNodeVectorIterator
342 itr != nodes.end(); itr++)
344 if (itr->getIndex() == idx)
345 return itr->getAddress();
348 if ((idx >= 0) && (idx < nodes.size()))
349 return nodes[idx]->getAddress();
354 FGTaxiSegment *FGGroundNetwork::findSegment(unsigned idx)
356 for (FGTaxiSegmentVectorIterator
357 itr = segments.begin();
358 itr != segments.end(); itr++)
360 if (itr->getIndex() == idx)
361 return itr->getAddress();
364 if ((idx > 0) && (idx <= segments.size()))
365 return segments[idx - 1]->getAddress();
367 //cerr << "Alert: trying to find invalid segment " << idx << endl;
373 FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end,
376 //implements Dijkstra's algorithm to find shortest distance route from start to end
377 //taken from http://en.wikipedia.org/wiki/Dijkstra's_algorithm
379 //double INFINITE = 100000000000.0;
380 // initialize scoring values
381 int nParkings = parent->getDynamics()->getNrOfParkings();
382 FGTaxiNodeVector *currNodesSet;
384 currNodesSet = &nodes;
386 currNodesSet = &pushBackNodes;
389 for (FGTaxiNodeVectorIterator
390 itr = currNodesSet->begin(); itr != currNodesSet->end(); itr++) {
391 (*itr)->setPathScore(HUGE_VAL); //infinity by all practical means
392 (*itr)->setPreviousNode(0); //
393 (*itr)->setPreviousSeg(0); //
396 FGTaxiNode *firstNode = findNode(start);
397 firstNode->setPathScore(0);
399 FGTaxiNode *lastNode = findNode(end);
401 FGTaxiNodeVector unvisited(*currNodesSet); // working copy
403 while (!unvisited.empty()) {
404 FGTaxiNode *best = *(unvisited.begin());
405 for (FGTaxiNodeVectorIterator
406 itr = unvisited.begin(); itr != unvisited.end(); itr++) {
407 if ((*itr)->getPathScore() < best->getPathScore())
411 FGTaxiNodeVectorIterator newend =
412 remove(unvisited.begin(), unvisited.end(), best);
413 unvisited.erase(newend, unvisited.end());
415 if (best == lastNode) { // found route or best not connected
418 for (FGTaxiSegmentVectorIterator
419 seg = best->getBeginRoute();
420 seg != best->getEndRoute(); seg++) {
421 if (fullSearch || (*seg)->isPushBack()) {
422 FGTaxiNode *tgt = (*seg)->getEnd();
424 best->getPathScore() + (*seg)->getLength() +
425 (*seg)->getPenalty(nParkings);
426 if (alt < tgt->getPathScore()) { // Relax (u,v)
427 tgt->setPathScore(alt);
428 tgt->setPreviousNode(best);
429 tgt->setPreviousSeg(*seg); //
432 // // cerr << "Skipping TaxiSegment " << (*seg)->getIndex() << endl;
438 if (lastNode->getPathScore() == HUGE_VAL) {
439 // no valid route found
441 SG_LOG(SG_GENERAL, SG_ALERT,
442 "Failed to find route from waypoint " << start << " to "
443 << end << " at " << parent->getId());
447 //exit(1); //TODO exit more gracefully, no need to stall the whole sim with broken GN's
449 // assemble route from backtrace information
450 intVec nodes, routes;
451 FGTaxiNode *bt = lastNode;
452 while (bt->getPreviousNode() != 0) {
453 nodes.push_back(bt->getIndex());
454 routes.push_back(bt->getPreviousSegment()->getIndex());
455 bt = bt->getPreviousNode();
457 nodes.push_back(start);
458 reverse(nodes.begin(), nodes.end());
459 reverse(routes.begin(), routes.end());
461 return FGTaxiRoute(nodes, routes, lastNode->getPathScore(), 0);
465 int FGTaxiSegment::getPenalty(int nGates)
468 if (end->getIndex() < nGates) {
471 if (end->getIsOnRunway()) { // For now. In future versions, need to find out whether runway is active.
477 /* ATC Related Functions */
479 void FGGroundNetwork::announcePosition(int id,
480 FGAIFlightPlan * intendedRoute,
481 int currentPosition, double lat,
482 double lon, double heading,
483 double speed, double alt,
484 double radius, int leg,
485 FGAIAircraft * aircraft)
488 TrafficVectorIterator i = activeTraffic.begin();
489 // Search search if the current id alread has an entry
490 // This might be faster using a map instead of a vector, but let's start by taking a safe route
491 if (activeTraffic.size()) {
492 //while ((i->getId() != id) && i != activeTraffic.end()) {
493 while (i != activeTraffic.end()) {
494 if (i->getId() == id) {
500 // Add a new TrafficRecord if no one exsists for this aircraft.
501 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
505 rec.setPositionAndIntentions(currentPosition, intendedRoute);
506 rec.setPositionAndHeading(lat, lon, heading, speed, alt);
507 rec.setRadius(radius); // only need to do this when creating the record.
508 rec.setAircraft(aircraft);
509 activeTraffic.push_back(rec);
511 i->setPositionAndIntentions(currentPosition, intendedRoute);
512 i->setPositionAndHeading(lat, lon, heading, speed, alt);
517 void FGGroundNetwork::signOff(int id)
519 TrafficVectorIterator i = activeTraffic.begin();
520 // Search search if the current id alread has an entry
521 // This might be faster using a map instead of a vector, but let's start by taking a safe route
522 if (activeTraffic.size()) {
523 //while ((i->getId() != id) && i != activeTraffic.end()) {
524 while (i != activeTraffic.end()) {
525 if (i->getId() == id) {
531 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
532 SG_LOG(SG_GENERAL, SG_ALERT,
533 "AI error: Aircraft without traffic record is signing off");
535 i = activeTraffic.erase(i);
539 * The ground network can deal with the following states:
540 * 0 = Normal; no action required
541 * 1 = "Acknowledge "Hold position
542 * 2 = "Acknowledge "Resume taxi".
543 * 3 = "Issue TaxiClearance"
544 * 4 = Acknowledge Taxi Clearance"
545 * 5 = Post acknowlegde taxiclearance: Start taxiing
547 * 7 = Acknowledge report runway
548 * 8 = Switch tower frequency
549 * 9 = Acknowledge switch tower frequency
550 *************************************************************************************************************************/
551 bool FGGroundNetwork::checkTransmissionState(int minState, int maxState, TrafficVectorIterator i, time_t now, AtcMsgId msgId,
554 int state = i->getState();
555 if ((state >= minState) && (state <= maxState) && available) {
556 if ((msgDir == ATC_AIR_TO_GROUND) && isUserAircraft(i->getAircraft())) {
557 //cerr << "Checking state " << state << " for " << i->getAircraft()->getCallSign() << endl;
558 static SGPropertyNode_ptr trans_num = globals->get_props()->getNode("/sim/atc/transmission-num", true);
559 int n = trans_num->getIntValue();
561 trans_num->setIntValue(-1);
563 //cerr << "Selected transmission message " << n << endl;
564 FGATCManager *atc = (FGATCManager*) globals->get_subsystem("atc");
565 atc->getATCDialog()->removeEntry(1);
567 //cerr << "creating message for " << i->getAircraft()->getCallSign() << endl;
568 transmit(&(*i), msgId, msgDir, false);
572 transmit(&(*i), msgId, msgDir, true);
574 lastTransmission = now;
581 void FGGroundNetwork::updateAircraftInformation(int id, double lat, double lon,
582 double heading, double speed, double alt,
585 // Check whether aircraft are on hold due to a preceding pushback. If so, make sure to
586 // Transmit air-to-ground "Ready to taxi request:
587 // Transmit ground to air approval / hold
588 // Transmit confirmation ...
589 // Probably use a status mechanism similar to the Engine start procedure in the startup controller.
592 TrafficVectorIterator i = activeTraffic.begin();
593 // Search search if the current id has an entry
594 // This might be faster using a map instead of a vector, but let's start by taking a safe route
595 TrafficVectorIterator current, closest;
596 if (activeTraffic.size()) {
597 //while ((i->getId() != id) && i != activeTraffic.end()) {
598 while (i != activeTraffic.end()) {
599 if (i->getId() == id) {
605 // update position of the current aircraft
606 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
607 SG_LOG(SG_GENERAL, SG_ALERT,
608 "AI error: updating aircraft without traffic record");
610 i->setPositionAndHeading(lat, lon, heading, speed, alt);
616 // Update every three secs, but add some randomness
617 // to prevent all IA objects doing this in synchrony
618 //if (getDt() < (3.0) + (rand() % 10))
622 current->clearResolveCircularWait();
623 current->setWaitsForId(0);
624 checkSpeedAdjustment(id, lat, lon, heading, speed, alt);
625 bool needsTaxiClearance = current->getAircraft()->getTaxiClearanceRequest();
626 if (!needsTaxiClearance) {
627 checkHoldPosition(id, lat, lon, heading, speed, alt);
628 if (checkForCircularWaits(id)) {
629 i->setResolveCircularWait();
632 current->setHoldPosition(true);
633 int state = current->getState();
634 time_t now = time(NULL) + fgGetLong("/sim/time/warp");
635 if ((now - lastTransmission) > 15) {
638 if (checkTransmissionState(0,2, current, now, MSG_REQUEST_TAXI_CLEARANCE, ATC_AIR_TO_GROUND)) {
639 current->setState(3);
641 if (checkTransmissionState(3,3, current, now, MSG_ISSUE_TAXI_CLEARANCE, ATC_GROUND_TO_AIR)) {
642 current->setState(4);
644 if (checkTransmissionState(4,4, current, now, MSG_ACKNOWLEDGE_TAXI_CLEARANCE, ATC_AIR_TO_GROUND)) {
645 current->setState(5);
647 if ((state == 5) && available) {
648 current->setState(0);
649 current->getAircraft()->setTaxiClearanceRequest(false);
650 current->setHoldPosition(false);
658 Scan for a speed adjustment change. Find the nearest aircraft that is in front
659 and adjust speed when we get too close. Only do this when current position and/or
660 intentions of the current aircraft match current taxiroute position of the proximate
661 aircraft. For traffic that is on other routes we need to issue a "HOLD Position"
662 instruction. See below for the hold position instruction.
664 Note that there currently still is one flaw in the logic that needs to be addressed.
665 There can be situations where one aircraft is in front of the current aircraft, on a separate
666 route, but really close after an intersection coming off the current route. This
667 aircraft is still close enough to block the current aircraft. This situation is currently
668 not addressed yet, but should be.
671 void FGGroundNetwork::checkSpeedAdjustment(int id, double lat,
672 double lon, double heading,
673 double speed, double alt)
676 TrafficVectorIterator current, closest;
677 TrafficVectorIterator i = activeTraffic.begin();
678 bool otherReasonToSlowDown = false;
679 bool previousInstruction;
680 if (activeTraffic.size()) {
681 //while ((i->getId() != id) && (i != activeTraffic.end()))
682 while (i != activeTraffic.end()) {
683 if (i->getId() == id) {
691 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
692 SG_LOG(SG_GENERAL, SG_ALERT,
693 "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkSpeedAdjustment");
698 previousInstruction = current->getSpeedAdjustment();
699 double mindist = HUGE_VAL;
700 if (activeTraffic.size()) {
701 double course, dist, bearing, minbearing, az2;
702 SGGeod curr(SGGeod::fromDegM(lon, lat, alt));
703 //TrafficVector iterator closest;
705 for (TrafficVectorIterator i = activeTraffic.begin();
706 i != activeTraffic.end(); i++) {
711 SGGeod other(SGGeod::fromDegM(i->getLongitude(),
714 SGGeodesy::inverse(curr, other, course, az2, dist);
715 bearing = fabs(heading - course);
717 bearing = 360 - bearing;
718 if ((dist < mindist) && (bearing < 60.0)) {
721 minbearing = bearing;
724 //Check traffic at the tower controller
725 if (towerController->hasActiveTraffic()) {
726 for (TrafficVectorIterator i =
727 towerController->getActiveTraffic().begin();
728 i != towerController->getActiveTraffic().end(); i++) {
729 //cerr << "Comparing " << current->getId() << " and " << i->getId() << endl;
730 SGGeod other(SGGeod::fromDegM(i->getLongitude(),
733 SGGeodesy::inverse(curr, other, course, az2, dist);
734 bearing = fabs(heading - course);
736 bearing = 360 - bearing;
737 if ((dist < mindist) && (bearing < 60.0)) {
738 //cerr << "Current aircraft " << current->getAircraft()->getTrafficRef()->getCallSign()
739 // << " is closest to " << i->getAircraft()->getTrafficRef()->getCallSign()
740 // << ", which has status " << i->getAircraft()->isScheduledForTakeoff()
744 minbearing = bearing;
745 otherReasonToSlowDown = true;
749 // Finally, check UserPosition
750 // Note, as of 2011-08-01, this should no longer be necessecary.
752 double userLatitude = fgGetDouble("/position/latitude-deg");
753 double userLongitude = fgGetDouble("/position/longitude-deg");
754 SGGeod user(SGGeod::fromDeg(userLongitude, userLatitude));
755 SGGeodesy::inverse(curr, user, course, az2, dist);
757 bearing = fabs(heading - course);
759 bearing = 360 - bearing;
760 if ((dist < mindist) && (bearing < 60.0)) {
763 minbearing = bearing;
764 otherReasonToSlowDown = true;
767 current->clearSpeedAdjustment();
769 if (current->checkPositionAndIntentions(*closest)
770 || otherReasonToSlowDown) {
771 double maxAllowableDistance =
772 (1.1 * current->getRadius()) +
773 (1.1 * closest->getRadius());
774 if (mindist < 2 * maxAllowableDistance) {
775 if (current->getId() == closest->getWaitsForId())
778 current->setWaitsForId(closest->getId());
779 if (closest->getId() != current->getId()) {
780 current->setSpeedAdjustment(closest->getSpeed() *
783 closest->getAircraft()->getTakeOffStatus() &&
784 (current->getAircraft()->getTrafficRef()->getDepartureAirport() == closest->getAircraft()->getTrafficRef()->getDepartureAirport()) &&
785 (current->getAircraft()->GetFlightPlan()->getRunway() == closest->getAircraft()->GetFlightPlan()->getRunway())
787 current->getAircraft()->scheduleForATCTowerDepartureControl(1);
789 current->setSpeedAdjustment(0); // This can only happen when the user aircraft is the one closest
791 if (mindist < maxAllowableDistance) {
792 //double newSpeed = (maxAllowableDistance-mindist);
793 //current->setSpeedAdjustment(newSpeed);
794 //if (mindist < 0.5* maxAllowableDistance)
796 current->setSpeedAdjustment(0);
805 Check for "Hold position instruction".
806 The hold position should be issued under the following conditions:
807 1) For aircraft entering or crossing a runway with active traffic on it, or landing aircraft near it
808 2) For taxiing aircraft that use one taxiway in opposite directions
809 3) For crossing or merging taxiroutes.
812 void FGGroundNetwork::checkHoldPosition(int id, double lat,
813 double lon, double heading,
814 double speed, double alt)
816 TrafficVectorIterator current;
817 TrafficVectorIterator i = activeTraffic.begin();
818 if (activeTraffic.size()) {
819 //while ((i->getId() != id) && i != activeTraffic.end())
820 while (i != activeTraffic.end()) {
821 if (i->getId() == id) {
829 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
830 SG_LOG(SG_GENERAL, SG_ALERT,
831 "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkHoldPosition");
834 bool origStatus = current->hasHoldPosition();
835 current->setHoldPosition(false);
836 SGGeod curr(SGGeod::fromDegM(lon, lat, alt));
838 for (i = activeTraffic.begin(); i != activeTraffic.end(); i++) {
839 if (i->getId() != current->getId()) {
840 int node = current->crosses(this, *i);
842 FGTaxiNode *taxiNode = findNode(node);
844 // Determine whether it's save to continue or not.
845 // If we have a crossing route, there are two possibilities:
846 // 1) This is an interestion
847 // 2) This is oncoming two-way traffic, using the same taxiway.
848 //cerr << "Hold check 1 : " << id << " has common node " << node << endl;
850 SGGeod other(SGGeod::
851 fromDegM(i->getLongitude(), i->getLatitude(),
855 if (current->isOpposing(this, *i, node)) {
858 //cerr << "Hold check 2 : " << node << " has opposing segment " << endl;
859 // issue a "Hold Position" as soon as we're close to the offending node
860 // For now, I'm doing this as long as the other aircraft doesn't
861 // have a hold instruction as soon as we're within a reasonable
862 // distance from the offending node.
863 // This may be a bit of a conservative estimate though, as it may
864 // be well possible that both aircraft can both continue to taxi
865 // without crashing into each other.
868 if (SGGeodesy::distanceM(other, taxiNode->getGeod()) > 200) // 2.0*i->getRadius())
871 //cerr << "Hold check 3 : " << id <<" Other aircraft approaching node is still far away. (" << dist << " nm). Can safely continue "
875 //cerr << "Hold check 4: " << id << " Would need to wait for other aircraft : distance = " << dist << " meters" << endl;
880 SGGeodesy::distanceM(curr, taxiNode->getGeod());
881 if (!(i->hasHoldPosition())) {
883 if ((dist < 200) && //2.5*current->getRadius()) &&
884 (needsToWait) && (i->onRoute(this, *current)) &&
885 //((i->onRoute(this, *current)) || ((!(i->getSpeedAdjustment())))) &&
886 (!(current->getId() == i->getWaitsForId())))
887 //(!(i->getSpeedAdjustment()))) // &&
888 //(!(current->getSpeedAdjustment())))
891 if (!(isUserAircraft(i->getAircraft()))) { // test code. Don't wait for the user, let the user wait for you.
892 current->setHoldPosition(true);
893 current->setWaitsForId(i->getId());
895 //cerr << "Hold check 5: " << current->getCallSign() <<" Setting Hold Position: distance to node (" << node << ") "
896 // << dist << " meters. Waiting for " << i->getCallSign();
898 //cerr <<" [opposing] " << endl;
900 // cerr << "[non-opposing] " << endl;
901 //if (i->hasSpeefAdjustment())
903 // cerr << " (which in turn waits for ) " << i->
905 //cerr << "Hold check 6: " << id << " No need to hold yet: Distance to node : " << dist << " nm"<< endl;
911 bool currStatus = current->hasHoldPosition();
912 current->setHoldPosition(origStatus);
913 // Either a Hold Position or a resume taxi transmission has been issued
914 time_t now = time(NULL) + fgGetLong("/sim/time/warp");
915 if ((now - lastTransmission) > 2) {
918 if (current->getState() == 0) {
919 if ((origStatus != currStatus) && available) {
920 //cerr << "Issueing hold short instrudtion " << currStatus << " " << available << endl;
921 if (currStatus == true) { // No has a hold short instruction
922 transmit(&(*current), MSG_HOLD_POSITION, ATC_GROUND_TO_AIR, true);
923 //cerr << "Transmittin hold short instrudtion " << currStatus << " " << available << endl;
924 current->setState(1);
926 transmit(&(*current), MSG_RESUME_TAXI, ATC_GROUND_TO_AIR, true);
927 //cerr << "Transmittig resume instrudtion " << currStatus << " " << available << endl;
928 current->setState(2);
930 lastTransmission = now;
932 // Don't act on the changed instruction until the transmission is confirmed
933 // So set back to original status
934 //cerr << "Current state " << current->getState() << endl;
939 // 7 = Acknowledge report runway
940 // 8 = Switch tower frequency
941 //9 = Acknowledge switch tower frequency
943 //int state = current->getState();
944 if (checkTransmissionState(1,1, current, now, MSG_ACKNOWLEDGE_HOLD_POSITION, ATC_AIR_TO_GROUND)) {
945 current->setState(0);
946 current->setHoldPosition(true);
948 if (checkTransmissionState(2,2, current, now, MSG_ACKNOWLEDGE_RESUME_TAXI, ATC_AIR_TO_GROUND)) {
949 current->setState(0);
950 current->setHoldPosition(false);
952 if (current->getAircraft()->getTakeOffStatus() && (current->getState() == 0)) {
953 //cerr << "Scheduling " << current->getAircraft()->getCallSign() << " for hold short" << endl;
954 current->setState(6);
956 if (checkTransmissionState(6,6, current, now, MSG_REPORT_RUNWAY_HOLD_SHORT, ATC_AIR_TO_GROUND)) {
958 if (checkTransmissionState(7,7, current, now, MSG_ACKNOWLEDGE_REPORT_RUNWAY_HOLD_SHORT, ATC_GROUND_TO_AIR)) {
960 if (checkTransmissionState(8,8, current, now, MSG_SWITCH_TOWER_FREQUENCY, ATC_GROUND_TO_AIR)) {
962 if (checkTransmissionState(9,9, current, now, MSG_ACKNOWLEDGE_SWITCH_TOWER_FREQUENCY, ATC_AIR_TO_GROUND)) {
967 //current->setState(0);
971 * Check whether situations occur where the current aircraft is waiting for itself
972 * due to higher order interactions.
973 * A 'circular' wait is a situation where a waits for b, b waits for c, and c waits
974 * for a. Ideally each aircraft only waits for one other aircraft, so by tracing
975 * through this list of waiting aircraft, we can check if we'd eventually end back
976 * at the current aircraft.
978 * Note that we should consider the situation where we are actually checking aircraft
979 * d, which is waiting for aircraft a. d is not part of the loop, but is held back by
980 * the looping aircraft. If we don't check for that, this function will get stuck into
984 bool FGGroundNetwork::checkForCircularWaits(int id)
986 //cerr << "Performing Wait check " << id << endl;
988 TrafficVectorIterator current, other;
989 TrafficVectorIterator i = activeTraffic.begin();
990 int trafficSize = activeTraffic.size();
992 while (i != activeTraffic.end()) {
993 if (i->getId() == id) {
1001 if (i == activeTraffic.end() || (trafficSize == 0)) {
1002 SG_LOG(SG_GENERAL, SG_ALERT,
1003 "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits");
1007 target = current->getWaitsForId();
1008 //bool printed = false; // Note that this variable is for debugging purposes only.
1012 //cerr << "aircraft waits for user" << endl;
1017 while ((target > 0) && (target != id) && counter++ < trafficSize) {
1019 TrafficVectorIterator i = activeTraffic.begin();
1021 //while ((i->getId() != id) && i != activeTraffic.end())
1022 while (i != activeTraffic.end()) {
1023 if (i->getId() == target) {
1031 if (i == activeTraffic.end() || (trafficSize == 0)) {
1032 //cerr << "[Waiting for traffic at Runway: DONE] " << endl << endl;;
1033 // The target id is not found on the current network, which means it's at the tower
1034 //SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits");
1038 target = other->getWaitsForId();
1040 // actually this trap isn't as impossible as it first seemed:
1041 // the setWaitsForID(id) is set to current when the aircraft
1042 // is waiting for the user controlled aircraft.
1043 //if (current->getId() == other->getId()) {
1044 // cerr << "Caught the impossible trap" << endl;
1045 // cerr << "Current = " << current->getId() << endl;
1046 // cerr << "Other = " << other ->getId() << endl;
1047 // for (TrafficVectorIterator at = activeTraffic.begin();
1048 // at != activeTraffic.end();
1050 // cerr << "currently active aircraft : " << at->getCallSign() << " with Id " << at->getId() << " waits for " << at->getWaitsForId() << endl;
1053 if (current->getId() == other->getId())
1056 //cerr << current->getCallSign() << " (" << current->getId() << ") " << " -> " << other->getCallSign()
1057 // << " (" << other->getId() << "); " << endl;;
1067 // cerr << "[done] " << endl << endl;;
1069 SG_LOG(SG_GENERAL, SG_WARN,
1070 "Detected circular wait condition: Id = " << id <<
1071 "target = " << target);
1078 // Note that this function is probably obsolete...
1079 bool FGGroundNetwork::hasInstruction(int id)
1081 TrafficVectorIterator i = activeTraffic.begin();
1082 // Search search if the current id has an entry
1083 // This might be faster using a map instead of a vector, but let's start by taking a safe route
1084 if (activeTraffic.size()) {
1085 //while ((i->getId() != id) && i != activeTraffic.end()) {
1086 while (i != activeTraffic.end()) {
1087 if (i->getId() == id) {
1093 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
1094 SG_LOG(SG_GENERAL, SG_ALERT,
1095 "AI error: checking ATC instruction for aircraft without traffic record");
1097 return i->hasInstruction();
1102 FGATCInstruction FGGroundNetwork::getInstruction(int id)
1104 TrafficVectorIterator i = activeTraffic.begin();
1105 // Search search if the current id has an entry
1106 // This might be faster using a map instead of a vector, but let's start by taking a safe route
1107 if (activeTraffic.size()) {
1108 //while ((i->getId() != id) && i != activeTraffic.end()) {
1109 while (i != activeTraffic.end()) {
1110 if (i->getId() == id) {
1116 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
1117 SG_LOG(SG_GENERAL, SG_ALERT,
1118 "AI error: requesting ATC instruction for aircraft without traffic record");
1120 return i->getInstruction();
1122 return FGATCInstruction();
1125 // Note that this function is copied from simgear. for maintanance purposes, it's probabtl better to make a general function out of that.
1126 static void WorldCoordinate(osg::Matrix& obj_pos, double lat,
1127 double lon, double elev, double hdg)
1129 SGGeod geod = SGGeod::fromDegM(lon, lat, elev);
1130 obj_pos = geod.makeZUpFrame();
1131 // hdg is not a compass heading, but a counter-clockwise rotation
1132 // around the Z axis
1133 obj_pos.preMult(osg::Matrix::rotate(hdg * SGD_DEGREES_TO_RADIANS,
1140 void FGGroundNetwork::render(bool visible)
1143 SGMaterialLib *matlib = globals->get_matlib();
1146 globals->get_scenery()->get_scene_graph()->removeChild(group);
1147 //while (group->getNumChildren()) {
1148 // cerr << "Number of children: " << group->getNumChildren() << endl;
1149 simgear::EffectGeode* geode = (simgear::EffectGeode*) group->getChild(0);
1150 //osg::MatrixTransform *obj_trans = (osg::MatrixTransform*) group->getChild(0);
1151 //geode->releaseGLObjects();
1152 //group->removeChild(geode);
1157 group = new osg::Group;
1159 //for ( FGTaxiSegmentVectorIterator i = segments.begin(); i != segments.end(); i++) {
1161 for (TrafficVectorIterator i = activeTraffic.begin(); i != activeTraffic.end(); i++) {
1162 // Handle start point
1163 int pos = i->getCurrentPosition() - 1;
1166 SGGeod start(SGGeod::fromDeg((i->getLongitude()), (i->getLatitude())));
1167 SGGeod end (SGGeod::fromDeg(segments[pos]->getEnd()->getLongitude(), segments[pos]->getEnd()->getLatitude()));
1169 double length = SGGeodesy::distanceM(start, end);
1170 //heading = SGGeodesy::headingDeg(start->getGeod(), end->getGeod());
1172 double az2, heading; //, distanceM;
1173 SGGeodesy::inverse(start, end, heading, az2, length);
1174 double coveredDistance = length * 0.5;
1176 SGGeodesy::direct(start, heading, coveredDistance, center, az2);
1177 //cerr << "Active Aircraft : Centerpoint = (" << center.getLatitudeDeg() << ", " << center.getLongitudeDeg() << "). Heading = " << heading << endl;
1178 ///////////////////////////////////////////////////////////////////////////////
1179 // Make a helper function out of this
1180 osg::Matrix obj_pos;
1181 osg::MatrixTransform *obj_trans = new osg::MatrixTransform;
1182 obj_trans->setDataVariance(osg::Object::STATIC);
1184 WorldCoordinate( obj_pos, center.getLatitudeDeg(), center.getLongitudeDeg(), parent->elevation()+8+dx, -(heading) );
1186 obj_trans->setMatrix( obj_pos );
1187 //osg::Vec3 center(0, 0, 0)
1189 float width = length /2.0;
1190 osg::Vec3 corner(-width, 0, 0.25f);
1191 osg::Vec3 widthVec(2*width + 1, 0, 0);
1192 osg::Vec3 heightVec(0, 1, 0);
1193 osg::Geometry* geometry;
1194 geometry = osg::createTexturedQuadGeometry(corner, widthVec, heightVec);
1195 simgear::EffectGeode* geode = new simgear::EffectGeode;
1196 geode->setName("test");
1197 geode->addDrawable(geometry);
1198 //osg::Node *custom_obj;
1199 SGMaterial *mat = matlib->find("UnidirectionalTaper");
1201 geode->setEffect(mat->get_effect());
1202 obj_trans->addChild(geode);
1203 // wire as much of the scene graph together as we can
1204 //->addChild( obj_trans );
1205 group->addChild( obj_trans );
1206 /////////////////////////////////////////////////////////////////////
1208 //cerr << "BIG FAT WARNING: current position is here : " << pos << endl;
1210 for(intVecIterator j = (i)->getIntentions().begin(); j != (i)->getIntentions().end(); j++) {
1211 osg::Matrix obj_pos;
1214 osg::MatrixTransform *obj_trans = new osg::MatrixTransform;
1215 obj_trans->setDataVariance(osg::Object::STATIC);
1217 WorldCoordinate( obj_pos, segments[k]->getLatitude(), segments[k]->getLongitude(), parent->elevation()+8+dx, -(segments[k]->getHeading()) );
1219 obj_trans->setMatrix( obj_pos );
1220 //osg::Vec3 center(0, 0, 0)
1222 float width = segments[k]->getLength() /2.0;
1223 osg::Vec3 corner(-width, 0, 0.25f);
1224 osg::Vec3 widthVec(2*width + 1, 0, 0);
1225 osg::Vec3 heightVec(0, 1, 0);
1226 osg::Geometry* geometry;
1227 geometry = osg::createTexturedQuadGeometry(corner, widthVec, heightVec);
1228 simgear::EffectGeode* geode = new simgear::EffectGeode;
1229 geode->setName("test");
1230 geode->addDrawable(geometry);
1231 //osg::Node *custom_obj;
1232 SGMaterial *mat = matlib->find("UnidirectionalTaper");
1234 geode->setEffect(mat->get_effect());
1235 obj_trans->addChild(geode);
1236 // wire as much of the scene graph together as we can
1237 //->addChild( obj_trans );
1238 group->addChild( obj_trans );
1243 globals->get_scenery()->get_scene_graph()->addChild(group);
1247 string FGGroundNetwork::getName() {
1248 return string(parent->getId() + "-ground");