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);
176 void FGTaxiRoute::rewind(int route)
182 if (!(next(&currPoint, &currRoute))) {
183 SG_LOG(SG_GENERAL, SG_ALERT,
184 "Error in rewinding TaxiRoute: current" << currRoute <<
187 } while (currRoute != route);
193 /***************************************************************************
195 **************************************************************************/
196 bool compare_nodes(FGTaxiNode * a, FGTaxiNode * b)
201 bool compare_segments(FGTaxiSegment * a, FGTaxiSegment * b)
206 FGGroundNetwork::FGGroundNetwork()
214 currTraffic = activeTraffic.begin();
219 FGGroundNetwork::~FGGroundNetwork()
221 for (FGTaxiNodeVectorIterator node = nodes.begin();
222 node != nodes.end(); node++) {
226 pushBackNodes.clear();
227 for (FGTaxiSegmentVectorIterator seg = segments.begin();
228 seg != segments.end(); seg++) {
234 void FGGroundNetwork::addSegment(const FGTaxiSegment & seg)
236 segments.push_back(new FGTaxiSegment(seg));
239 void FGGroundNetwork::addNode(const FGTaxiNode & node)
241 nodes.push_back(new FGTaxiNode(node));
244 void FGGroundNetwork::addNodes(FGParkingVec * parkings)
247 FGParkingVecIterator i = parkings->begin();
248 while (i != parkings->end()) {
249 n.setIndex(i->getIndex());
250 n.setLatitude(i->getLatitude());
251 n.setLongitude(i->getLongitude());
252 n.setElevation(parent->getElevation());
253 nodes.push_back(new FGTaxiNode(n));
261 void FGGroundNetwork::init()
265 sort(nodes.begin(), nodes.end(), compare_nodes);
266 //sort(segments.begin(), segments.end(), compare_segments());
267 FGTaxiSegmentVectorIterator i = segments.begin();
268 while (i != segments.end()) {
269 (*i)->setStart(&nodes);
270 (*i)->setEnd(&nodes);
271 (*i)->setDimensions(parent->getElevation());
272 (*i)->setIndex(index);
273 if ((*i)->isPushBack()) {
274 pushBackNodes.push_back((*i)->getEnd());
276 //SG_LOG(SG_GENERAL, SG_BULK, "initializing segment " << (*i)->getIndex() << endl);
277 //SG_LOG(SG_GENERAL, SG_BULK, "Track distance = " << (*i)->getLength() << endl);
278 //SG_LOG(SG_GENERAL, SG_BULK, "Track runs from " << (*i)->getStart()->getIndex() << " to "
279 // << (*i)->getEnd()->getIndex() << endl);
284 i = segments.begin();
285 while (i != segments.end()) {
286 FGTaxiSegmentVectorIterator j = (*i)->getEnd()->getBeginRoute();
287 while (j != (*i)->getEnd()->getEndRoute()) {
288 if ((*j)->getEnd()->getIndex() == (*i)->getStart()->getIndex()) {
289 // int start1 = (*i)->getStart()->getIndex();
290 // int end1 = (*i)->getEnd() ->getIndex();
291 // int start2 = (*j)->getStart()->getIndex();
292 // int end2 = (*j)->getEnd()->getIndex();
293 // int oppIndex = (*j)->getIndex();
294 //cerr << "Opposite of " << (*i)->getIndex() << " (" << start1 << "," << end1 << ") "
295 // << "happens to be " << oppIndex << " (" << start2 << "," << end2 << ") " << endl;
296 (*i)->setOpposite(*j);
303 //FGTaxiNodeVectorIterator j = nodes.begin();
304 //while (j != nodes.end()) {
305 // if ((*j)->getHoldPointType() == 3) {
306 // pushBackNodes.push_back((*j));
310 //cerr << "Done initializing ground network" << endl;
314 int FGGroundNetwork::findNearestNode(const SGGeod & aGeod)
316 double minDist = HUGE_VAL;
319 for (FGTaxiNodeVectorIterator itr = nodes.begin(); itr != nodes.end();
321 double d = SGGeodesy::distanceM(aGeod, (*itr)->getGeod());
324 index = (*itr)->getIndex();
325 //cerr << "Minimum distance of " << minDist << " for index " << index << endl;
332 int FGGroundNetwork::findNearestNode(double lat, double lon)
334 return findNearestNode(SGGeod::fromDeg(lon, lat));
337 FGTaxiNode *FGGroundNetwork::findNode(unsigned idx)
339 for (FGTaxiNodeVectorIterator
341 itr != nodes.end(); itr++)
343 if (itr->getIndex() == idx)
344 return itr->getAddress();
347 if ((idx >= 0) && (idx < nodes.size()))
348 return nodes[idx]->getAddress();
353 FGTaxiSegment *FGGroundNetwork::findSegment(unsigned idx)
355 for (FGTaxiSegmentVectorIterator
356 itr = segments.begin();
357 itr != segments.end(); itr++)
359 if (itr->getIndex() == idx)
360 return itr->getAddress();
363 if ((idx > 0) && (idx <= segments.size()))
364 return segments[idx - 1]->getAddress();
366 //cerr << "Alert: trying to find invalid segment " << idx << endl;
372 FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end,
375 //implements Dijkstra's algorithm to find shortest distance route from start to end
376 //taken from http://en.wikipedia.org/wiki/Dijkstra's_algorithm
378 //double INFINITE = 100000000000.0;
379 // initialize scoring values
380 int nParkings = parent->getDynamics()->getNrOfParkings();
381 FGTaxiNodeVector *currNodesSet;
383 currNodesSet = &nodes;
385 currNodesSet = &pushBackNodes;
388 for (FGTaxiNodeVectorIterator
389 itr = currNodesSet->begin(); itr != currNodesSet->end(); itr++) {
390 (*itr)->setPathScore(HUGE_VAL); //infinity by all practical means
391 (*itr)->setPreviousNode(0); //
392 (*itr)->setPreviousSeg(0); //
395 FGTaxiNode *firstNode = findNode(start);
396 firstNode->setPathScore(0);
398 FGTaxiNode *lastNode = findNode(end);
400 FGTaxiNodeVector unvisited(*currNodesSet); // working copy
402 while (!unvisited.empty()) {
403 FGTaxiNode *best = *(unvisited.begin());
404 for (FGTaxiNodeVectorIterator
405 itr = unvisited.begin(); itr != unvisited.end(); itr++) {
406 if ((*itr)->getPathScore() < best->getPathScore())
410 FGTaxiNodeVectorIterator newend =
411 remove(unvisited.begin(), unvisited.end(), best);
412 unvisited.erase(newend, unvisited.end());
414 if (best == lastNode) { // found route or best not connected
417 for (FGTaxiSegmentVectorIterator
418 seg = best->getBeginRoute();
419 seg != best->getEndRoute(); seg++) {
420 if (fullSearch || (*seg)->isPushBack()) {
421 FGTaxiNode *tgt = (*seg)->getEnd();
423 best->getPathScore() + (*seg)->getLength() +
424 (*seg)->getPenalty(nParkings);
425 if (alt < tgt->getPathScore()) { // Relax (u,v)
426 tgt->setPathScore(alt);
427 tgt->setPreviousNode(best);
428 tgt->setPreviousSeg(*seg); //
431 // // cerr << "Skipping TaxiSegment " << (*seg)->getIndex() << endl;
437 if (lastNode->getPathScore() == HUGE_VAL) {
438 // no valid route found
440 SG_LOG(SG_GENERAL, SG_ALERT,
441 "Failed to find route from waypoint " << start << " to "
442 << end << " at " << parent->getId());
446 //exit(1); //TODO exit more gracefully, no need to stall the whole sim with broken GN's
448 // assemble route from backtrace information
449 intVec nodes, routes;
450 FGTaxiNode *bt = lastNode;
451 while (bt->getPreviousNode() != 0) {
452 nodes.push_back(bt->getIndex());
453 routes.push_back(bt->getPreviousSegment()->getIndex());
454 bt = bt->getPreviousNode();
456 nodes.push_back(start);
457 reverse(nodes.begin(), nodes.end());
458 reverse(routes.begin(), routes.end());
460 return FGTaxiRoute(nodes, routes, lastNode->getPathScore(), 0);
464 int FGTaxiSegment::getPenalty(int nGates)
467 if (end->getIndex() < nGates) {
470 if (end->getIsOnRunway()) { // For now. In future versions, need to find out whether runway is active.
476 /* ATC Related Functions */
478 void FGGroundNetwork::announcePosition(int id,
479 FGAIFlightPlan * intendedRoute,
480 int currentPosition, double lat,
481 double lon, double heading,
482 double speed, double alt,
483 double radius, int leg,
484 FGAIAircraft * aircraft)
487 TrafficVectorIterator i = activeTraffic.begin();
488 // Search search if the current id alread has an entry
489 // This might be faster using a map instead of a vector, but let's start by taking a safe route
490 if (activeTraffic.size()) {
491 //while ((i->getId() != id) && i != activeTraffic.end()) {
492 while (i != activeTraffic.end()) {
493 if (i->getId() == id) {
499 // Add a new TrafficRecord if no one exsists for this aircraft.
500 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
504 rec.setPositionAndIntentions(currentPosition, intendedRoute);
505 rec.setPositionAndHeading(lat, lon, heading, speed, alt);
506 rec.setRadius(radius); // only need to do this when creating the record.
507 rec.setAircraft(aircraft);
508 activeTraffic.push_back(rec);
510 i->setPositionAndIntentions(currentPosition, intendedRoute);
511 i->setPositionAndHeading(lat, lon, heading, speed, alt);
516 void FGGroundNetwork::signOff(int id)
518 TrafficVectorIterator i = activeTraffic.begin();
519 // Search search if the current id alread has an entry
520 // This might be faster using a map instead of a vector, but let's start by taking a safe route
521 if (activeTraffic.size()) {
522 //while ((i->getId() != id) && i != activeTraffic.end()) {
523 while (i != activeTraffic.end()) {
524 if (i->getId() == id) {
530 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
531 SG_LOG(SG_GENERAL, SG_ALERT,
532 "AI error: Aircraft without traffic record is signing off");
534 i = activeTraffic.erase(i);
538 bool FGGroundNetwork::checkTransmissionState(int minState, int maxState, TrafficVectorIterator i, time_t now, AtcMsgId msgId,
541 int state = i->getState();
542 if ((state >= minState) && (state <= maxState) && available) {
543 if ((msgDir == ATC_AIR_TO_GROUND) && isUserAircraft(i->getAircraft())) {
544 //cerr << "Checking state " << state << " for " << i->getAircraft()->getCallSign() << endl;
545 static SGPropertyNode_ptr trans_num = globals->get_props()->getNode("/sim/atc/transmission-num", true);
546 int n = trans_num->getIntValue();
548 trans_num->setIntValue(-1);
550 cerr << "Selected transmission message " << n << endl;
551 FGATCManager *atc = (FGATCManager*) globals->get_subsystem("atc");
552 atc->getATCDialog()->removeEntry(1);
554 //cerr << "creating message for " << i->getAircraft()->getCallSign() << endl;
555 transmit(&(*i), msgId, msgDir, false);
559 transmit(&(*i), msgId, msgDir, true);
561 lastTransmission = now;
568 void FGGroundNetwork::updateAircraftInformation(int id, double lat, double lon,
569 double heading, double speed, double alt,
572 // Check whether aircraft are on hold due to a preceding pushback. If so, make sure to
573 // Transmit air-to-ground "Ready to taxi request:
574 // Transmit ground to air approval / hold
575 // Transmit confirmation ...
576 // Probably use a status mechanism similar to the Engine start procedure in the startup controller.
579 TrafficVectorIterator i = activeTraffic.begin();
580 // Search search if the current id has an entry
581 // This might be faster using a map instead of a vector, but let's start by taking a safe route
582 TrafficVectorIterator current, closest;
583 if (activeTraffic.size()) {
584 //while ((i->getId() != id) && i != activeTraffic.end()) {
585 while (i != activeTraffic.end()) {
586 if (i->getId() == id) {
592 // update position of the current aircraft
593 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
594 SG_LOG(SG_GENERAL, SG_ALERT,
595 "AI error: updating aircraft without traffic record");
597 i->setPositionAndHeading(lat, lon, heading, speed, alt);
603 // Update every three secs, but add some randomness
604 // to prevent all IA objects doing this in synchrony
605 //if (getDt() < (3.0) + (rand() % 10))
609 current->clearResolveCircularWait();
610 current->setWaitsForId(0);
611 checkSpeedAdjustment(id, lat, lon, heading, speed, alt);
612 bool needsTaxiClearance = current->getAircraft()->getTaxiClearanceRequest();
613 if (!needsTaxiClearance) {
614 checkHoldPosition(id, lat, lon, heading, speed, alt);
615 if (checkForCircularWaits(id)) {
616 i->setResolveCircularWait();
619 current->setHoldPosition(true);
620 int state = current->getState();
621 time_t now = time(NULL) + fgGetLong("/sim/time/warp");
622 if ((now - lastTransmission) > 15) {
625 if (checkTransmissionState(0,2, current, now, MSG_REQUEST_TAXI_CLEARANCE, ATC_AIR_TO_GROUND)) {
626 current->setState(3);
628 if (checkTransmissionState(3,3, current, now, MSG_ISSUE_TAXI_CLEARANCE, ATC_GROUND_TO_AIR)) {
629 current->setState(4);
631 if (checkTransmissionState(4,4, current, now, MSG_ACKNOWLEDGE_TAXI_CLEARANCE, ATC_AIR_TO_GROUND)) {
632 current->setState(5);
634 if ((state == 5) && available) {
635 current->setState(0);
636 current->getAircraft()->setTaxiClearanceRequest(false);
637 current->setHoldPosition(true);
646 Scan for a speed adjustment change. Find the nearest aircraft that is in front
647 and adjust speed when we get too close. Only do this when current position and/or
648 intentions of the current aircraft match current taxiroute position of the proximate
649 aircraft. For traffic that is on other routes we need to issue a "HOLD Position"
650 instruction. See below for the hold position instruction.
652 Note that there currently still is one flaw in the logic that needs to be addressed.
653 There can be situations where one aircraft is in front of the current aircraft, on a separate
654 route, but really close after an intersection coming off the current route. This
655 aircraft is still close enough to block the current aircraft. This situation is currently
656 not addressed yet, but should be.
659 void FGGroundNetwork::checkSpeedAdjustment(int id, double lat,
660 double lon, double heading,
661 double speed, double alt)
664 TrafficVectorIterator current, closest;
665 TrafficVectorIterator i = activeTraffic.begin();
666 bool otherReasonToSlowDown = false;
667 bool previousInstruction;
668 if (activeTraffic.size()) {
669 //while ((i->getId() != id) && (i != activeTraffic.end()))
670 while (i != activeTraffic.end()) {
671 if (i->getId() == id) {
679 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
680 SG_LOG(SG_GENERAL, SG_ALERT,
681 "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkSpeedAdjustment");
686 previousInstruction = current->getSpeedAdjustment();
687 double mindist = HUGE_VAL;
688 if (activeTraffic.size()) {
689 double course, dist, bearing, minbearing, az2;
690 SGGeod curr(SGGeod::fromDegM(lon, lat, alt));
691 //TrafficVector iterator closest;
693 for (TrafficVectorIterator i = activeTraffic.begin();
694 i != activeTraffic.end(); i++) {
699 SGGeod other(SGGeod::fromDegM(i->getLongitude(),
702 SGGeodesy::inverse(curr, other, course, az2, dist);
703 bearing = fabs(heading - course);
705 bearing = 360 - bearing;
706 if ((dist < mindist) && (bearing < 60.0)) {
709 minbearing = bearing;
712 //Check traffic at the tower controller
713 if (towerController->hasActiveTraffic()) {
714 for (TrafficVectorIterator i =
715 towerController->getActiveTraffic().begin();
716 i != towerController->getActiveTraffic().end(); i++) {
717 //cerr << "Comparing " << current->getId() << " and " << i->getId() << endl;
718 SGGeod other(SGGeod::fromDegM(i->getLongitude(),
721 SGGeodesy::inverse(curr, other, course, az2, dist);
722 bearing = fabs(heading - course);
724 bearing = 360 - bearing;
725 if ((dist < mindist) && (bearing < 60.0)) {
728 minbearing = bearing;
729 otherReasonToSlowDown = true;
733 // Finally, check UserPosition
734 // Note, as of 2011-08-01, this should no longer be necessecary.
736 double userLatitude = fgGetDouble("/position/latitude-deg");
737 double userLongitude = fgGetDouble("/position/longitude-deg");
738 SGGeod user(SGGeod::fromDeg(userLongitude, userLatitude));
739 SGGeodesy::inverse(curr, user, course, az2, dist);
741 bearing = fabs(heading - course);
743 bearing = 360 - bearing;
744 if ((dist < mindist) && (bearing < 60.0)) {
747 minbearing = bearing;
748 otherReasonToSlowDown = true;
751 current->clearSpeedAdjustment();
753 if (current->checkPositionAndIntentions(*closest)
754 || otherReasonToSlowDown) {
755 double maxAllowableDistance =
756 (1.1 * current->getRadius()) +
757 (1.1 * closest->getRadius());
758 if (mindist < 2 * maxAllowableDistance) {
759 if (current->getId() == closest->getWaitsForId())
762 current->setWaitsForId(closest->getId());
763 if (closest->getId() != current->getId())
764 current->setSpeedAdjustment(closest->getSpeed() *
766 if (closest->getAircraft()->isScheduledForTakeoff())
767 current->getAircraft()->scheduleForATCTowerDepartureControl();
769 current->setSpeedAdjustment(0); // This can only happen when the user aircraft is the one closest
770 if (mindist < maxAllowableDistance) {
771 //double newSpeed = (maxAllowableDistance-mindist);
772 //current->setSpeedAdjustment(newSpeed);
773 //if (mindist < 0.5* maxAllowableDistance)
775 current->setSpeedAdjustment(0);
784 Check for "Hold position instruction".
785 The hold position should be issued under the following conditions:
786 1) For aircraft entering or crossing a runway with active traffic on it, or landing aircraft near it
787 2) For taxiing aircraft that use one taxiway in opposite directions
788 3) For crossing or merging taxiroutes.
791 void FGGroundNetwork::checkHoldPosition(int id, double lat,
792 double lon, double heading,
793 double speed, double alt)
795 TrafficVectorIterator current;
796 TrafficVectorIterator i = activeTraffic.begin();
797 if (activeTraffic.size()) {
798 //while ((i->getId() != id) && i != activeTraffic.end())
799 while (i != activeTraffic.end()) {
800 if (i->getId() == id) {
808 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
809 SG_LOG(SG_GENERAL, SG_ALERT,
810 "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkHoldPosition");
813 bool origStatus = current->hasHoldPosition();
814 current->setHoldPosition(false);
815 SGGeod curr(SGGeod::fromDegM(lon, lat, alt));
817 for (i = activeTraffic.begin(); i != activeTraffic.end(); i++) {
818 if (i->getId() != current->getId()) {
819 int node = current->crosses(this, *i);
821 FGTaxiNode *taxiNode = findNode(node);
823 // Determine whether it's save to continue or not.
824 // If we have a crossing route, there are two possibilities:
825 // 1) This is an interestion
826 // 2) This is oncoming two-way traffic, using the same taxiway.
827 //cerr << "Hold check 1 : " << id << " has common node " << node << endl;
829 SGGeod other(SGGeod::
830 fromDegM(i->getLongitude(), i->getLatitude(),
834 if (current->isOpposing(this, *i, node)) {
837 //cerr << "Hold check 2 : " << node << " has opposing segment " << endl;
838 // issue a "Hold Position" as soon as we're close to the offending node
839 // For now, I'm doing this as long as the other aircraft doesn't
840 // have a hold instruction as soon as we're within a reasonable
841 // distance from the offending node.
842 // This may be a bit of a conservative estimate though, as it may
843 // be well possible that both aircraft can both continue to taxi
844 // without crashing into each other.
847 if (SGGeodesy::distanceM(other, taxiNode->getGeod()) > 200) // 2.0*i->getRadius())
850 //cerr << "Hold check 3 : " << id <<" Other aircraft approaching node is still far away. (" << dist << " nm). Can safely continue "
854 //cerr << "Hold check 4: " << id << " Would need to wait for other aircraft : distance = " << dist << " meters" << endl;
859 SGGeodesy::distanceM(curr, taxiNode->getGeod());
860 if (!(i->hasHoldPosition())) {
862 if ((dist < 200) && //2.5*current->getRadius()) &&
863 (needsToWait) && (i->onRoute(this, *current)) &&
864 //((i->onRoute(this, *current)) || ((!(i->getSpeedAdjustment())))) &&
865 (!(current->getId() == i->getWaitsForId())))
866 //(!(i->getSpeedAdjustment()))) // &&
867 //(!(current->getSpeedAdjustment())))
870 current->setHoldPosition(true);
871 current->setWaitsForId(i->getId());
872 //cerr << "Hold check 5: " << current->getCallSign() <<" Setting Hold Position: distance to node (" << node << ") "
873 // << dist << " meters. Waiting for " << i->getCallSign();
875 //cerr <<" [opposing] " << endl;
877 // cerr << "[non-opposing] " << endl;
878 //if (i->hasSpeefAdjustment())
880 // cerr << " (which in turn waits for ) " << i->
882 //cerr << "Hold check 6: " << id << " No need to hold yet: Distance to node : " << dist << " nm"<< endl;
888 bool currStatus = current->hasHoldPosition();
890 // Either a Hold Position or a resume taxi transmission has been issued
891 time_t now = time(NULL) + fgGetLong("/sim/time/warp");
892 if ((now - lastTransmission) > 2) {
895 if ((origStatus != currStatus) && available) {
896 //cerr << "Issueing hold short instrudtion " << currStatus << " " << available << endl;
897 if (currStatus == true) { // No has a hold short instruction
898 transmit(&(*current), MSG_HOLD_POSITION, ATC_GROUND_TO_AIR, true);
899 //cerr << "Transmittin hold short instrudtion " << currStatus << " " << available << endl;
900 current->setState(1);
902 transmit(&(*current), MSG_RESUME_TAXI, ATC_GROUND_TO_AIR, true);
903 //cerr << "Transmittig resume instrudtion " << currStatus << " " << available << endl;
904 current->setState(2);
906 lastTransmission = now;
908 // Don't act on the changed instruction until the transmission is confirmed
909 // So set back to original status
910 current->setHoldPosition(origStatus);
911 //cerr << "Current state " << current->getState() << endl;
914 //int state = current->getState();
915 if (checkTransmissionState(1,1, current, now, MSG_ACKNOWLEDGE_HOLD_POSITION, ATC_AIR_TO_GROUND)) {
916 current->setState(0);
917 current->setHoldPosition(true);
919 if (checkTransmissionState(2,2, current, now, MSG_ACKNOWLEDGE_RESUME_TAXI, ATC_AIR_TO_GROUND)) {
920 current->setState(0);
921 current->setHoldPosition(false);
926 * Check whether situations occur where the current aircraft is waiting for itself
927 * due to higher order interactions.
928 * A 'circular' wait is a situation where a waits for b, b waits for c, and c waits
929 * for a. Ideally each aircraft only waits for one other aircraft, so by tracing
930 * through this list of waiting aircraft, we can check if we'd eventually end back
931 * at the current aircraft.
933 * Note that we should consider the situation where we are actually checking aircraft
934 * d, which is waiting for aircraft a. d is not part of the loop, but is held back by
935 * the looping aircraft. If we don't check for that, this function will get stuck into
939 bool FGGroundNetwork::checkForCircularWaits(int id)
941 //cerr << "Performing Wait check " << id << endl;
943 TrafficVectorIterator current, other;
944 TrafficVectorIterator i = activeTraffic.begin();
945 int trafficSize = activeTraffic.size();
947 while (i != activeTraffic.end()) {
948 if (i->getId() == id) {
956 if (i == activeTraffic.end() || (trafficSize == 0)) {
957 SG_LOG(SG_GENERAL, SG_ALERT,
958 "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits");
962 target = current->getWaitsForId();
963 //bool printed = false; // Note that this variable is for debugging purposes only.
967 //cerr << "aircraft waits for user" << endl;
972 while ((target > 0) && (target != id) && counter++ < trafficSize) {
974 TrafficVectorIterator i = activeTraffic.begin();
976 //while ((i->getId() != id) && i != activeTraffic.end())
977 while (i != activeTraffic.end()) {
978 if (i->getId() == target) {
986 if (i == activeTraffic.end() || (trafficSize == 0)) {
987 //cerr << "[Waiting for traffic at Runway: DONE] " << endl << endl;;
988 // The target id is not found on the current network, which means it's at the tower
989 //SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits");
993 target = other->getWaitsForId();
995 // actually this trap isn't as impossible as it first seemed:
996 // the setWaitsForID(id) is set to current when the aircraft
997 // is waiting for the user controlled aircraft.
998 //if (current->getId() == other->getId()) {
999 // cerr << "Caught the impossible trap" << endl;
1000 // cerr << "Current = " << current->getId() << endl;
1001 // cerr << "Other = " << other ->getId() << endl;
1002 // for (TrafficVectorIterator at = activeTraffic.begin();
1003 // at != activeTraffic.end();
1005 // cerr << "currently active aircraft : " << at->getCallSign() << " with Id " << at->getId() << " waits for " << at->getWaitsForId() << endl;
1008 if (current->getId() == other->getId())
1011 //cerr << current->getCallSign() << " (" << current->getId() << ") " << " -> " << other->getCallSign()
1012 // << " (" << other->getId() << "); " << endl;;
1022 // cerr << "[done] " << endl << endl;;
1024 SG_LOG(SG_GENERAL, SG_WARN,
1025 "Detected circular wait condition: Id = " << id <<
1026 "target = " << target);
1033 // Note that this function is probably obsolete...
1034 bool FGGroundNetwork::hasInstruction(int id)
1036 TrafficVectorIterator i = activeTraffic.begin();
1037 // Search search if the current id has an entry
1038 // This might be faster using a map instead of a vector, but let's start by taking a safe route
1039 if (activeTraffic.size()) {
1040 //while ((i->getId() != id) && i != activeTraffic.end()) {
1041 while (i != activeTraffic.end()) {
1042 if (i->getId() == id) {
1048 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
1049 SG_LOG(SG_GENERAL, SG_ALERT,
1050 "AI error: checking ATC instruction for aircraft without traffic record");
1052 return i->hasInstruction();
1057 FGATCInstruction FGGroundNetwork::getInstruction(int id)
1059 TrafficVectorIterator i = activeTraffic.begin();
1060 // Search search if the current id has an entry
1061 // This might be faster using a map instead of a vector, but let's start by taking a safe route
1062 if (activeTraffic.size()) {
1063 //while ((i->getId() != id) && i != activeTraffic.end()) {
1064 while (i != activeTraffic.end()) {
1065 if (i->getId() == id) {
1071 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
1072 SG_LOG(SG_GENERAL, SG_ALERT,
1073 "AI error: requesting ATC instruction for aircraft without traffic record");
1075 return i->getInstruction();
1077 return FGATCInstruction();
1080 // Note that this function is copied from simgear. for maintanance purposes, it's probabtl better to make a general function out of that.
1081 static void WorldCoordinate(osg::Matrix& obj_pos, double lat,
1082 double lon, double elev, double hdg)
1084 SGGeod geod = SGGeod::fromDegM(lon, lat, elev);
1085 obj_pos = geod.makeZUpFrame();
1086 // hdg is not a compass heading, but a counter-clockwise rotation
1087 // around the Z axis
1088 obj_pos.preMult(osg::Matrix::rotate(hdg * SGD_DEGREES_TO_RADIANS,
1095 void FGGroundNetwork::render(bool visible)
1098 SGMaterialLib *matlib = globals->get_matlib();
1101 globals->get_scenery()->get_scene_graph()->removeChild(group);
1102 //while (group->getNumChildren()) {
1103 // cerr << "Number of children: " << group->getNumChildren() << endl;
1104 simgear::EffectGeode* geode = (simgear::EffectGeode*) group->getChild(0);
1105 //osg::MatrixTransform *obj_trans = (osg::MatrixTransform*) group->getChild(0);
1106 //geode->releaseGLObjects();
1107 //group->removeChild(geode);
1112 group = new osg::Group;
1114 //for ( FGTaxiSegmentVectorIterator i = segments.begin(); i != segments.end(); i++) {
1116 for (TrafficVectorIterator i = activeTraffic.begin(); i != activeTraffic.end(); i++) {
1117 // Handle start point
1118 int pos = i->getCurrentPosition() - 1;
1121 SGGeod start(SGGeod::fromDeg((i->getLongitude()), (i->getLatitude())));
1122 SGGeod end (SGGeod::fromDeg(segments[pos]->getEnd()->getLongitude(), segments[pos]->getEnd()->getLatitude()));
1124 double length = SGGeodesy::distanceM(start, end);
1125 //heading = SGGeodesy::headingDeg(start->getGeod(), end->getGeod());
1127 double az2, heading; //, distanceM;
1128 SGGeodesy::inverse(start, end, heading, az2, length);
1129 double coveredDistance = length * 0.5;
1131 SGGeodesy::direct(start, heading, coveredDistance, center, az2);
1132 //cerr << "Active Aircraft : Centerpoint = (" << center.getLatitudeDeg() << ", " << center.getLongitudeDeg() << "). Heading = " << heading << endl;
1133 ///////////////////////////////////////////////////////////////////////////////
1134 // Make a helper function out of this
1135 osg::Matrix obj_pos;
1136 osg::MatrixTransform *obj_trans = new osg::MatrixTransform;
1137 obj_trans->setDataVariance(osg::Object::STATIC);
1139 WorldCoordinate( obj_pos, center.getLatitudeDeg(), center.getLongitudeDeg(), parent->elevation()+8+dx, -(heading) );
1141 obj_trans->setMatrix( obj_pos );
1142 //osg::Vec3 center(0, 0, 0)
1144 float width = length /2.0;
1145 osg::Vec3 corner(-width, 0, 0.25f);
1146 osg::Vec3 widthVec(2*width + 1, 0, 0);
1147 osg::Vec3 heightVec(0, 1, 0);
1148 osg::Geometry* geometry;
1149 geometry = osg::createTexturedQuadGeometry(corner, widthVec, heightVec);
1150 simgear::EffectGeode* geode = new simgear::EffectGeode;
1151 geode->setName("test");
1152 geode->addDrawable(geometry);
1153 //osg::Node *custom_obj;
1154 SGMaterial *mat = matlib->find("UnidirectionalTaper");
1156 geode->setEffect(mat->get_effect());
1157 obj_trans->addChild(geode);
1158 // wire as much of the scene graph together as we can
1159 //->addChild( obj_trans );
1160 group->addChild( obj_trans );
1161 /////////////////////////////////////////////////////////////////////
1163 //cerr << "BIG FAT WARNING: current position is here : " << pos << endl;
1165 for(intVecIterator j = (i)->getIntentions().begin(); j != (i)->getIntentions().end(); j++) {
1166 osg::Matrix obj_pos;
1169 osg::MatrixTransform *obj_trans = new osg::MatrixTransform;
1170 obj_trans->setDataVariance(osg::Object::STATIC);
1172 WorldCoordinate( obj_pos, segments[k]->getLatitude(), segments[k]->getLongitude(), parent->elevation()+8+dx, -(segments[k]->getHeading()) );
1174 obj_trans->setMatrix( obj_pos );
1175 //osg::Vec3 center(0, 0, 0)
1177 float width = segments[k]->getLength() /2.0;
1178 osg::Vec3 corner(-width, 0, 0.25f);
1179 osg::Vec3 widthVec(2*width + 1, 0, 0);
1180 osg::Vec3 heightVec(0, 1, 0);
1181 osg::Geometry* geometry;
1182 geometry = osg::createTexturedQuadGeometry(corner, widthVec, heightVec);
1183 simgear::EffectGeode* geode = new simgear::EffectGeode;
1184 geode->setName("test");
1185 geode->addDrawable(geometry);
1186 //osg::Node *custom_obj;
1187 SGMaterial *mat = matlib->find("UnidirectionalTaper");
1189 geode->setEffect(mat->get_effect());
1190 obj_trans->addChild(geode);
1191 // wire as much of the scene graph together as we can
1192 //->addChild( obj_trans );
1193 group->addChild( obj_trans );
1198 globals->get_scenery()->get_scene_graph()->addChild(group);
1202 string FGGroundNetwork::getName() {
1203 return string(parent->getId() + "-ground");