src/Airports/groundnetwork.cxx

   1 // groundnet.cxx - Implimentation of the FlightGear airport ground handling code
   2 //
   3 // Written by Durk Talsma, started June 2005.
   4 //
   5 // Copyright (C) 2004 Durk Talsma.
   6 //
   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.
  11 //
  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.
  16 //
  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.
  20 //
  21 // $Id$
  22
  23 #ifdef HAVE_CONFIG_H
  24 #  include <config.h>
  25 #endif
  26
  27 #include <math.h>
  28 #include <algorithm>
  29
  30 #include <simgear/debug/logstream.hxx>
  31 #include <simgear/route/waypoint.hxx>
  32
  33 #include <Airports/dynamics.hxx>
  34
  35 #include <AIModel/AIAircraft.hxx>
  36 #include <AIModel/AIFlightPlan.hxx>
  37
  38 #include "groundnetwork.hxx"
  39
  40 /***************************************************************************
  41  * FGTaxiSegment
  42  **************************************************************************/
  43
  44 void FGTaxiSegment::setStart(FGTaxiNodeVector * nodes)
  45 {
  46     FGTaxiNodeVectorIterator i = nodes->begin();
  47     while (i != nodes->end()) {
  48         //cerr << "Scanning start node index" << (*i)->getIndex() << endl;
  49         if ((*i)->getIndex() == startNode) {
  50             start = (*i)->getAddress();
  51             (*i)->addSegment(this);
  52             return;
  53         }
  54         i++;
  55     }
  56     SG_LOG(SG_GENERAL, SG_ALERT,
  57            "Could not find start node " << startNode << endl);
  58 }
  59
  60 void FGTaxiSegment::setEnd(FGTaxiNodeVector * nodes)
  61 {
  62     FGTaxiNodeVectorIterator i = nodes->begin();
  63     while (i != nodes->end()) {
  64         //cerr << "Scanning end node index" << (*i)->getIndex() << endl;
  65         if ((*i)->getIndex() == endNode) {
  66             end = (*i)->getAddress();
  67             return;
  68         }
  69         i++;
  70     }
  71     SG_LOG(SG_GENERAL, SG_ALERT,
  72            "Could not find end node " << endNode << endl);
  73 }
  74
  75
  76
  77 // There is probably a computationally cheaper way of
  78 // doing this.
  79 void FGTaxiSegment::setTrackDistance()
  80 {
  81     length = SGGeodesy::distanceM(start->getGeod(), end->getGeod());
  82 }
  83
  84
  85 void FGTaxiSegment::setCourseDiff(double crse)
  86 {
  87     headingDiff = fabs(course - crse);
  88
  89     if (headingDiff > 180)
  90         headingDiff = fabs(headingDiff - 360);
  91 }
  92
  93
  94 /***************************************************************************
  95  * FGTaxiRoute
  96  **************************************************************************/
  97 bool FGTaxiRoute::next(int *nde)
  98 {
  99     //for (intVecIterator i = nodes.begin(); i != nodes.end(); i++)
 100     //  cerr << "FGTaxiRoute contains : " << *(i) << endl;
 101     //cerr << "Offset from end: " << nodes.end() - currNode << endl;
 102     //if (currNode != nodes.end())
 103     //  cerr << "true" << endl;
 104     //else
 105     //  cerr << "false" << endl;
 106     //if (nodes.size() != (routes.size()) +1)
 107     //  cerr << "ALERT: Misconfigured TaxiRoute : " << nodes.size() << " " << routes.size() << endl;
 108
 109     if (currNode == nodes.end())
 110         return false;
 111     *nde = *(currNode);
 112     if (currNode != nodes.begin())      // make sure route corresponds to the end node
 113         currRoute++;
 114     currNode++;
 115     return true;
 116 };
 117
 118 bool FGTaxiRoute::next(int *nde, int *rte)
 119 {
 120     //for (intVecIterator i = nodes.begin(); i != nodes.end(); i++)
 121     //  cerr << "FGTaxiRoute contains : " << *(i) << endl;
 122     //cerr << "Offset from end: " << nodes.end() - currNode << endl;
 123     //if (currNode != nodes.end())
 124     //  cerr << "true" << endl;
 125     //else
 126     //  cerr << "false" << endl;
 127     if (nodes.size() != (routes.size()) + 1) {
 128         SG_LOG(SG_GENERAL, SG_ALERT,
 129                "ALERT: Misconfigured TaxiRoute : " << nodes.
 130                size() << " " << routes.size());
 131         exit(1);
 132     }
 133     if (currNode == nodes.end())
 134         return false;
 135     *nde = *(currNode);
 136     //*rte = *(currRoute);
 137     if (currNode != nodes.begin())      // Make sure route corresponds to the end node
 138     {
 139         *rte = *(currRoute);
 140         currRoute++;
 141     } else {
 142         // If currNode points to the first node, this means the aircraft is not on the taxi node
 143         // yet. Make sure to return a unique identifyer in this situation though, because otherwise
 144         // the speed adjust AI code may be unable to resolve whether two aircraft are on the same
 145         // taxi route or not. the negative of the preceding route seems a logical choice, as it is
 146         // unique for any starting location.
 147         // Note that this is probably just a temporary fix until I get Parking / tower control working.
 148         *rte = -1 * *(currRoute);
 149     }
 150     currNode++;
 151     return true;
 152 };
 153
 154 void FGTaxiRoute::rewind(int route)
 155 {
 156     int currPoint;
 157     int currRoute;
 158     first();
 159     do {
 160         if (!(next(&currPoint, &currRoute))) {
 161             SG_LOG(SG_GENERAL, SG_ALERT,
 162                    "Error in rewinding TaxiRoute: current" << currRoute <<
 163                    " goal " << route);
 164         }
 165     } while (currRoute != route);
 166 }
 167
 168
 169
 170
 171 /***************************************************************************
 172  * FGGroundNetwork()
 173  **************************************************************************/
 174 bool compare_nodes(FGTaxiNode * a, FGTaxiNode * b)
 175 {
 176     return (*a) < (*b);
 177 }
 178
 179 bool compare_segments(FGTaxiSegment * a, FGTaxiSegment * b)
 180 {
 181     return (*a) < (*b);
 182 }
 183
 184 FGGroundNetwork::FGGroundNetwork()
 185 {
 186     hasNetwork = false;
 187     foundRoute = false;
 188     totalDistance = 0;
 189     maxDistance = 0;
 190     //maxDepth    = 1000;
 191     count = 0;
 192     currTraffic = activeTraffic.begin();
 193
 194 }
 195
 196 FGGroundNetwork::~FGGroundNetwork()
 197 {
 198     for (FGTaxiNodeVectorIterator node = nodes.begin();
 199          node != nodes.end(); node++) {
 200         delete(*node);
 201     }
 202     nodes.clear();
 203     pushBackNodes.clear();
 204     for (FGTaxiSegmentVectorIterator seg = segments.begin();
 205          seg != segments.end(); seg++) {
 206         delete(*seg);
 207     }
 208     segments.clear();
 209 }
 210
 211 void FGGroundNetwork::addSegment(const FGTaxiSegment & seg)
 212 {
 213     segments.push_back(new FGTaxiSegment(seg));
 214 }
 215
 216 void FGGroundNetwork::addNode(const FGTaxiNode & node)
 217 {
 218     nodes.push_back(new FGTaxiNode(node));
 219 }
 220
 221 void FGGroundNetwork::addNodes(FGParkingVec * parkings)
 222 {
 223     FGTaxiNode n;
 224     FGParkingVecIterator i = parkings->begin();
 225     while (i != parkings->end()) {
 226         n.setIndex(i->getIndex());
 227         n.setLatitude(i->getLatitude());
 228         n.setLongitude(i->getLongitude());
 229         nodes.push_back(new FGTaxiNode(n));
 230
 231         i++;
 232     }
 233 }
 234
 235
 236
 237 void FGGroundNetwork::init()
 238 {
 239     hasNetwork = true;
 240     int index = 1;
 241     sort(nodes.begin(), nodes.end(), compare_nodes);
 242     //sort(segments.begin(), segments.end(), compare_segments());
 243     FGTaxiSegmentVectorIterator i = segments.begin();
 244     while (i != segments.end()) {
 245         (*i)->setStart(&nodes);
 246         (*i)->setEnd(&nodes);
 247         (*i)->setTrackDistance();
 248         (*i)->setIndex(index);
 249         if ((*i)->isPushBack()) {
 250             pushBackNodes.push_back((*i)->getEnd());
 251         }
 252         //SG_LOG(SG_GENERAL, SG_BULK,  "initializing segment " << (*i)->getIndex() << endl);
 253         //SG_LOG(SG_GENERAL, SG_BULK, "Track distance = "     << (*i)->getLength() << endl);
 254         //SG_LOG(SG_GENERAL, SG_BULK, "Track runs from "      << (*i)->getStart()->getIndex() << " to "
 255         //                                                    << (*i)->getEnd()->getIndex() << endl);
 256         i++;
 257         index++;
 258     }
 259
 260     i = segments.begin();
 261     while (i != segments.end()) {
 262         FGTaxiSegmentVectorIterator j = (*i)->getEnd()->getBeginRoute();
 263         while (j != (*i)->getEnd()->getEndRoute()) {
 264             if ((*j)->getEnd()->getIndex() == (*i)->getStart()->getIndex()) {
 265 //          int start1 = (*i)->getStart()->getIndex();
 266 //          int end1   = (*i)->getEnd()  ->getIndex();
 267 //          int start2 = (*j)->getStart()->getIndex();
 268 //          int end2   = (*j)->getEnd()->getIndex();
 269 //          int oppIndex = (*j)->getIndex();
 270                 //cerr << "Opposite of  " << (*i)->getIndex() << " (" << start1 << "," << end1 << ") "
 271                 //   << "happens to be " << oppIndex      << " (" << start2 << "," << end2 << ") " << endl;
 272                 (*i)->setOpposite(*j);
 273                 break;
 274             }
 275             j++;
 276         }
 277         i++;
 278     }
 279     //FGTaxiNodeVectorIterator j = nodes.begin();
 280     //while (j != nodes.end()) {
 281     //    if ((*j)->getHoldPointType() == 3) {
 282     //        pushBackNodes.push_back((*j));
 283     //    }
 284     //    j++;
 285     //}
 286     //cerr << "Done initializing ground network" << endl;
 287     //exit(1);
 288 }
 289
 290 int FGGroundNetwork::findNearestNode(const SGGeod & aGeod)
 291 {
 292     double minDist = HUGE_VAL;
 293     int index = -1;
 294
 295     for (FGTaxiNodeVectorIterator itr = nodes.begin(); itr != nodes.end();
 296          itr++) {
 297         double d = SGGeodesy::distanceM(aGeod, (*itr)->getGeod());
 298         if (d < minDist) {
 299             minDist = d;
 300             index = (*itr)->getIndex();
 301             //cerr << "Minimum distance of " << minDist << " for index " << index << endl;
 302         }
 303     }
 304
 305     return index;
 306 }
 307
 308 int FGGroundNetwork::findNearestNode(double lat, double lon)
 309 {
 310     return findNearestNode(SGGeod::fromDeg(lon, lat));
 311 }
 312
 313 FGTaxiNode *FGGroundNetwork::findNode(unsigned idx)
 314 {                               /*
 315                                    for (FGTaxiNodeVectorIterator
 316                                    itr = nodes.begin();
 317                                    itr != nodes.end(); itr++)
 318                                    {
 319                                    if (itr->getIndex() == idx)
 320                                    return itr->getAddress();
 321                                    } */
 322
 323     if ((idx >= 0) && (idx < nodes.size()))
 324         return nodes[idx]->getAddress();
 325     else
 326         return 0;
 327 }
 328
 329 FGTaxiSegment *FGGroundNetwork::findSegment(unsigned idx)
 330 {                               /*
 331                                    for (FGTaxiSegmentVectorIterator
 332                                    itr = segments.begin();
 333                                    itr != segments.end(); itr++)
 334                                    {
 335                                    if (itr->getIndex() == idx)
 336                                    return itr->getAddress();
 337                                    }
 338                                  */
 339     if ((idx > 0) && (idx <= segments.size()))
 340         return segments[idx - 1]->getAddress();
 341     else {
 342         //cerr << "Alert: trying to find invalid segment " << idx << endl;
 343         return 0;
 344     }
 345 }
 346
 347
 348 FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end,
 349                                                bool fullSearch)
 350 {
 351 //implements Dijkstra's algorithm to find shortest distance route from start to end
 352 //taken from http://en.wikipedia.org/wiki/Dijkstra's_algorithm
 353
 354     //double INFINITE = 100000000000.0;
 355     // initialize scoring values
 356     int nParkings = parent->getDynamics()->getNrOfParkings();
 357     FGTaxiNodeVector *currNodesSet;
 358     if (fullSearch) {
 359         currNodesSet = &nodes;
 360     } else {
 361         currNodesSet = &pushBackNodes;
 362     }
 363
 364     for (FGTaxiNodeVectorIterator
 365          itr = currNodesSet->begin(); itr != currNodesSet->end(); itr++) {
 366         (*itr)->setPathScore(HUGE_VAL); //infinity by all practical means
 367         (*itr)->setPreviousNode(0);     //
 368         (*itr)->setPreviousSeg(0);      //
 369     }
 370
 371     FGTaxiNode *firstNode = findNode(start);
 372     firstNode->setPathScore(0);
 373
 374     FGTaxiNode *lastNode = findNode(end);
 375
 376     FGTaxiNodeVector unvisited(*currNodesSet);  // working copy
 377
 378     while (!unvisited.empty()) {
 379         FGTaxiNode *best = *(unvisited.begin());
 380         for (FGTaxiNodeVectorIterator
 381              itr = unvisited.begin(); itr != unvisited.end(); itr++) {
 382             if ((*itr)->getPathScore() < best->getPathScore())
 383                 best = (*itr);
 384         }
 385
 386         FGTaxiNodeVectorIterator newend =
 387             remove(unvisited.begin(), unvisited.end(), best);
 388         unvisited.erase(newend, unvisited.end());
 389
 390         if (best == lastNode) { // found route or best not connected
 391             break;
 392         } else {
 393             for (FGTaxiSegmentVectorIterator
 394                  seg = best->getBeginRoute();
 395                  seg != best->getEndRoute(); seg++) {
 396                 if (fullSearch || (*seg)->isPushBack()) {
 397                     FGTaxiNode *tgt = (*seg)->getEnd();
 398                     double alt =
 399                         best->getPathScore() + (*seg)->getLength() +
 400                         (*seg)->getPenalty(nParkings);
 401                     if (alt < tgt->getPathScore()) {    // Relax (u,v)
 402                         tgt->setPathScore(alt);
 403                         tgt->setPreviousNode(best);
 404                         tgt->setPreviousSeg(*seg);      //
 405                     }
 406                 } else {
 407                     //   // cerr << "Skipping TaxiSegment " << (*seg)->getIndex() << endl;
 408                 }
 409             }
 410         }
 411     }
 412
 413     if (lastNode->getPathScore() == HUGE_VAL) {
 414         // no valid route found
 415         if (fullSearch) {
 416             SG_LOG(SG_GENERAL, SG_ALERT,
 417                    "Failed to find route from waypoint " << start << " to "
 418                    << end << " at " << parent->getId());
 419         }
 420         FGTaxiRoute empty;
 421         return empty;
 422         //exit(1); //TODO exit more gracefully, no need to stall the whole sim with broken GN's
 423     } else {
 424         // assemble route from backtrace information
 425         intVec nodes, routes;
 426         FGTaxiNode *bt = lastNode;
 427         while (bt->getPreviousNode() != 0) {
 428             nodes.push_back(bt->getIndex());
 429             routes.push_back(bt->getPreviousSegment()->getIndex());
 430             bt = bt->getPreviousNode();
 431         }
 432         nodes.push_back(start);
 433         reverse(nodes.begin(), nodes.end());
 434         reverse(routes.begin(), routes.end());
 435
 436         return FGTaxiRoute(nodes, routes, lastNode->getPathScore(), 0);
 437     }
 438 }
 439
 440 int FGTaxiSegment::getPenalty(int nGates)
 441 {
 442     int penalty = 0;
 443     if (end->getIndex() < nGates) {
 444         penalty += 10000;
 445     }
 446     if (end->getIsOnRunway()) { // For now. In future versions, need to find out whether runway is active.
 447         penalty += 1000;
 448     }
 449     return penalty;
 450 }
 451
 452 /* ATC Related Functions */
 453
 454 void FGGroundNetwork::announcePosition(int id,
 455                                        FGAIFlightPlan * intendedRoute,
 456                                        int currentPosition, double lat,
 457                                        double lon, double heading,
 458                                        double speed, double alt,
 459                                        double radius, int leg,
 460                                        FGAIAircraft * aircraft)
 461 {
 462     TrafficVectorIterator i = activeTraffic.begin();
 463     // Search search if the current id alread has an entry
 464     // This might be faster using a map instead of a vector, but let's start by taking a safe route
 465     if (activeTraffic.size()) {
 466         //while ((i->getId() != id) && i != activeTraffic.end()) {
 467         while (i != activeTraffic.end()) {
 468             if (i->getId() == id) {
 469                 break;
 470             }
 471             i++;
 472         }
 473     }
 474     // Add a new TrafficRecord if no one exsists for this aircraft.
 475     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 476         FGTrafficRecord rec;
 477         rec.setId(id);
 478         rec.setLeg(leg);
 479         rec.setPositionAndIntentions(currentPosition, intendedRoute);
 480         rec.setPositionAndHeading(lat, lon, heading, speed, alt);
 481         rec.setRadius(radius);  // only need to do this when creating the record.
 482         rec.setAircraft(aircraft);
 483         activeTraffic.push_back(rec);
 484     } else {
 485         i->setPositionAndIntentions(currentPosition, intendedRoute);
 486         i->setPositionAndHeading(lat, lon, heading, speed, alt);
 487     }
 488 }
 489
 490 void FGGroundNetwork::signOff(int id)
 491 {
 492     TrafficVectorIterator i = activeTraffic.begin();
 493     // Search search if the current id alread has an entry
 494     // This might be faster using a map instead of a vector, but let's start by taking a safe route
 495     if (activeTraffic.size()) {
 496         //while ((i->getId() != id) && i != activeTraffic.end()) {
 497         while (i != activeTraffic.end()) {
 498             if (i->getId() == id) {
 499                 break;
 500             }
 501             i++;
 502         }
 503     }
 504     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 505         SG_LOG(SG_GENERAL, SG_ALERT,
 506                "AI error: Aircraft without traffic record is signing off");
 507     } else {
 508         i = activeTraffic.erase(i);
 509     }
 510 }
 511
 512 void FGGroundNetwork::update(int id, double lat, double lon,
 513                              double heading, double speed, double alt,
 514                              double dt)
 515 {
 516     // Check whether aircraft are on hold due to a preceding pushback. If so, make sure to
 517     // Transmit air-to-ground "Ready to taxi request:
 518     // Transmit ground to air approval / hold
 519     // Transmit confirmation ...
 520     // Probably use a status mechanism similar to the Engine start procedure in the startup controller.
 521
 522
 523     TrafficVectorIterator i = activeTraffic.begin();
 524     // Search search if the current id has an entry
 525     // This might be faster using a map instead of a vector, but let's start by taking a safe route
 526     TrafficVectorIterator current, closest;
 527     if (activeTraffic.size()) {
 528         //while ((i->getId() != id) && i != activeTraffic.end()) {
 529         while (i != activeTraffic.end()) {
 530             if (i->getId() == id) {
 531                 break;
 532             }
 533             i++;
 534         }
 535     }
 536     // update position of the current aircraft
 537     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 538         SG_LOG(SG_GENERAL, SG_ALERT,
 539                "AI error: updating aircraft without traffic record");
 540     } else {
 541         i->setPositionAndHeading(lat, lon, heading, speed, alt);
 542         current = i;
 543     }
 544
 545     setDt(getDt() + dt);
 546
 547     // Update every three secs, but add some randomness
 548     // to prevent all IA objects doing this in synchrony
 549     //if (getDt() < (3.0) + (rand() % 10))
 550     //  return;
 551     //else
 552     //  setDt(0);
 553     current->clearResolveCircularWait();
 554     current->setWaitsForId(0);
 555     checkSpeedAdjustment(id, lat, lon, heading, speed, alt);
 556     bool needsTaxiClearance = current->getAircraft()->getTaxiClearanceRequest();
 557     if (!needsTaxiClearance) {
 558         checkHoldPosition(id, lat, lon, heading, speed, alt);
 559         if (checkForCircularWaits(id)) {
 560             i->setResolveCircularWait();
 561         }
 562     } else {
 563         current->setHoldPosition(true);
 564         int state = current->getState();
 565         time_t now = time(NULL) + fgGetLong("/sim/time/warp");
 566         if ((now - lastTransmission) > 15) {
 567             available = true;
 568         }
 569         if ((state < 3) && available) {
 570              transmit(&(*current), MSG_REQUEST_TAXI_CLEARANCE, ATC_AIR_TO_GROUND);
 571              current->setState(3);
 572              lastTransmission = now;
 573              available = false;
 574         }
 575         if ((state == 3) && available) {
 576             transmit(&(*current), MSG_ISSUE_TAXI_CLEARANCE, ATC_GROUND_TO_AIR);
 577             current->setState(4);
 578             lastTransmission = now;
 579             available = false;
 580         }
 581         if ((state == 4) && available) {
 582             transmit(&(*current), MSG_ACKNOWLEDGE_TAXI_CLEARANCE, ATC_AIR_TO_GROUND);
 583             current->setState(5);
 584             lastTransmission = now;
 585             available = false;
 586         }
 587         if ((state == 5) && available) {
 588             current->setState(0);
 589             current->getAircraft()->setTaxiClearanceRequest(false);
 590             current->setHoldPosition(true);
 591             available = false;
 592         }
 593
 594     }
 595
 596 }
 597
 598 /**
 599    Scan for a speed adjustment change. Find the nearest aircraft that is in front
 600    and adjust speed when we get too close. Only do this when current position and/or
 601    intentions of the current aircraft match current taxiroute position of the proximate
 602    aircraft. For traffic that is on other routes we need to issue a "HOLD Position"
 603    instruction. See below for the hold position instruction.
 604
 605    Note that there currently still is one flaw in the logic that needs to be addressed.
 606    There can be situations where one aircraft is in front of the current aircraft, on a separate
 607    route, but really close after an intersection coming off the current route. This
 608    aircraft is still close enough to block the current aircraft. This situation is currently
 609    not addressed yet, but should be.
 610 */
 611
 612 void FGGroundNetwork::checkSpeedAdjustment(int id, double lat,
 613                                            double lon, double heading,
 614                                            double speed, double alt)
 615 {
 616
 617     TrafficVectorIterator current, closest;
 618     TrafficVectorIterator i = activeTraffic.begin();
 619     bool otherReasonToSlowDown = false;
 620     bool previousInstruction;
 621     if (activeTraffic.size()) {
 622         //while ((i->getId() != id) && (i != activeTraffic.end()))
 623         while (i != activeTraffic.end()) {
 624             if (i->getId() == id) {
 625                 break;
 626             }
 627             i++;
 628         }
 629     } else {
 630         return;
 631     }
 632     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 633         SG_LOG(SG_GENERAL, SG_ALERT,
 634                "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkSpeedAdjustment");
 635     }
 636     current = i;
 637     //closest = current;
 638
 639     previousInstruction = current->getSpeedAdjustment();
 640     double mindist = HUGE_VAL;
 641     if (activeTraffic.size()) {
 642         double course, dist, bearing, minbearing, az2;
 643         SGGeod curr(SGGeod::fromDegM(lon, lat, alt));
 644         //TrafficVector iterator closest;
 645         closest = current;
 646         for (TrafficVectorIterator i = activeTraffic.begin();
 647              i != activeTraffic.end(); i++) {
 648             if (i == current) {
 649                 continue;
 650             }
 651
 652             SGGeod other(SGGeod::fromDegM(i->getLongitude(),
 653                                           i->getLatitude(),
 654                                           i->getAltitude()));
 655             SGGeodesy::inverse(curr, other, course, az2, dist);
 656             bearing = fabs(heading - course);
 657             if (bearing > 180)
 658                 bearing = 360 - bearing;
 659             if ((dist < mindist) && (bearing < 60.0)) {
 660                 mindist = dist;
 661                 closest = i;
 662                 minbearing = bearing;
 663             }
 664         }
 665         //Check traffic at the tower controller
 666         if (towerController->hasActiveTraffic()) {
 667             for (TrafficVectorIterator i =
 668                  towerController->getActiveTraffic().begin();
 669                  i != towerController->getActiveTraffic().end(); i++) {
 670                 //cerr << "Comparing " << current->getId() << " and " << i->getId() << endl;
 671                 SGGeod other(SGGeod::fromDegM(i->getLongitude(),
 672                                               i->getLatitude(),
 673                                               i->getAltitude()));
 674                 SGGeodesy::inverse(curr, other, course, az2, dist);
 675                 bearing = fabs(heading - course);
 676                 if (bearing > 180)
 677                     bearing = 360 - bearing;
 678                 if ((dist < mindist) && (bearing < 60.0)) {
 679                     mindist = dist;
 680                     closest = i;
 681                     minbearing = bearing;
 682                     otherReasonToSlowDown = true;
 683                 }
 684             }
 685         }
 686         // Finally, check UserPosition
 687         double userLatitude = fgGetDouble("/position/latitude-deg");
 688         double userLongitude = fgGetDouble("/position/longitude-deg");
 689         SGGeod user(SGGeod::fromDeg(userLongitude, userLatitude));
 690         SGGeodesy::inverse(curr, user, course, az2, dist);
 691
 692         bearing = fabs(heading - course);
 693         if (bearing > 180)
 694             bearing = 360 - bearing;
 695         if ((dist < mindist) && (bearing < 60.0)) {
 696             mindist = dist;
 697             //closest = i;
 698             minbearing = bearing;
 699             otherReasonToSlowDown = true;
 700         }
 701
 702         current->clearSpeedAdjustment();
 703
 704         if (current->checkPositionAndIntentions(*closest)
 705             || otherReasonToSlowDown) {
 706             double maxAllowableDistance =
 707                 (1.1 * current->getRadius()) +
 708                 (1.1 * closest->getRadius());
 709             if (mindist < 2 * maxAllowableDistance) {
 710                 if (current->getId() == closest->getWaitsForId())
 711                     return;
 712                 else
 713                     current->setWaitsForId(closest->getId());
 714                 if (closest->getId() != current->getId())
 715                     current->setSpeedAdjustment(closest->getSpeed() *
 716                                                 (mindist / 100));
 717                 else
 718                     current->setSpeedAdjustment(0);     // This can only happen when the user aircraft is the one closest
 719                 if (mindist < maxAllowableDistance) {
 720                     //double newSpeed = (maxAllowableDistance-mindist);
 721                     //current->setSpeedAdjustment(newSpeed);
 722                     //if (mindist < 0.5* maxAllowableDistance)
 723                     //  {
 724                     current->setSpeedAdjustment(0);
 725                     //  }
 726                 }
 727             }
 728         }
 729     }
 730 }
 731
 732 /**
 733    Check for "Hold position instruction".
 734    The hold position should be issued under the following conditions:
 735    1) For aircraft entering or crossing a runway with active traffic on it, or landing aircraft near it
 736    2) For taxiing aircraft that use one taxiway in opposite directions
 737    3) For crossing or merging taxiroutes.
 738 */
 739
 740 void FGGroundNetwork::checkHoldPosition(int id, double lat,
 741                                         double lon, double heading,
 742                                         double speed, double alt)
 743 {
 744     TrafficVectorIterator current;
 745     TrafficVectorIterator i = activeTraffic.begin();
 746     if (activeTraffic.size()) {
 747         //while ((i->getId() != id) && i != activeTraffic.end())
 748         while (i != activeTraffic.end()) {
 749             if (i->getId() == id) {
 750                 break;
 751             }
 752             i++;
 753         }
 754     } else {
 755         return;
 756     }
 757     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 758         SG_LOG(SG_GENERAL, SG_ALERT,
 759                "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkHoldPosition");
 760     }
 761     current = i;
 762     bool origStatus = current->hasHoldPosition();
 763     current->setHoldPosition(false);
 764     SGGeod curr(SGGeod::fromDegM(lon, lat, alt));
 765
 766     for (i = activeTraffic.begin(); i != activeTraffic.end(); i++) {
 767         if (i->getId() != current->getId()) {
 768             int node = current->crosses(this, *i);
 769             if (node != -1) {
 770                 FGTaxiNode *taxiNode = findNode(node);
 771
 772                 // Determine whether it's save to continue or not.
 773                 // If we have a crossing route, there are two possibilities:
 774                 // 1) This is an interestion
 775                 // 2) This is oncoming two-way traffic, using the same taxiway.
 776                 //cerr << "Hold check 1 : " << id << " has common node " << node << endl;
 777
 778                 SGGeod other(SGGeod::
 779                              fromDegM(i->getLongitude(), i->getLatitude(),
 780                                       i->getAltitude()));
 781                 bool needsToWait;
 782                 bool opposing;
 783                 if (current->isOpposing(this, *i, node)) {
 784                     needsToWait = true;
 785                     opposing = true;
 786                     //cerr << "Hold check 2 : " << node << "  has opposing segment " << endl;
 787                     // issue a "Hold Position" as soon as we're close to the offending node
 788                     // For now, I'm doing this as long as the other aircraft doesn't
 789                     // have a hold instruction as soon as we're within a reasonable
 790                     // distance from the offending node.
 791                     // This may be a bit of a conservative estimate though, as it may
 792                     // be well possible that both aircraft can both continue to taxi
 793                     // without crashing into each other.
 794                 } else {
 795                     opposing = false;
 796                     if (SGGeodesy::distanceM(other, taxiNode->getGeod()) > 200) // 2.0*i->getRadius())
 797                     {
 798                         needsToWait = false;
 799                         //cerr << "Hold check 3 : " << id <<"  Other aircraft approaching node is still far away. (" << dist << " nm). Can safely continue "
 800                         //           << endl;
 801                     } else {
 802                         needsToWait = true;
 803                         //cerr << "Hold check 4: " << id << "  Would need to wait for other aircraft : distance = " << dist << " meters" << endl;
 804                     }
 805                 }
 806
 807                 double dist =
 808                     SGGeodesy::distanceM(curr, taxiNode->getGeod());
 809                 if (!(i->hasHoldPosition())) {
 810
 811                     if ((dist < 200) && //2.5*current->getRadius()) &&
 812                         (needsToWait) && (i->onRoute(this, *current)) &&
 813                         //((i->onRoute(this, *current)) || ((!(i->getSpeedAdjustment())))) &&
 814                         (!(current->getId() == i->getWaitsForId())))
 815                         //(!(i->getSpeedAdjustment()))) // &&
 816                         //(!(current->getSpeedAdjustment())))
 817
 818                     {
 819                         current->setHoldPosition(true);
 820                         current->setWaitsForId(i->getId());
 821                         //cerr << "Hold check 5: " << current->getCallSign() <<"  Setting Hold Position: distance to node ("  << node << ") "
 822                         //           << dist << " meters. Waiting for " << i->getCallSign();
 823                         //if (opposing)
 824                         //cerr <<" [opposing] " << endl;
 825                         //else
 826                         //        cerr << "[non-opposing] " << endl;
 827                         //if (i->hasSpeefAdjustment())
 828                         //        {
 829                         //          cerr << " (which in turn waits for ) " << i->
 830                     } else {
 831                         //cerr << "Hold check 6: " << id << "  No need to hold yet: Distance to node : " << dist << " nm"<< endl;
 832                     }
 833                 }
 834             }
 835         }
 836     }
 837     bool currStatus = current->hasHoldPosition();
 838
 839     // Either a Hold Position or a resume taxi transmission has been issued
 840     time_t now = time(NULL) + fgGetLong("/sim/time/warp");
 841     if ((now - lastTransmission) > 2) {
 842         available = true;
 843     }
 844     if ((origStatus != currStatus) && available) {
 845         //cerr << "Issueing hold short instrudtion " << currStatus << " " << available << endl;
 846         if (currStatus == true) { // No has a hold short instruction
 847            transmit(&(*current), MSG_HOLD_POSITION, ATC_GROUND_TO_AIR);
 848            //cerr << "Transmittin hold short instrudtion " << currStatus << " " << available << endl;
 849            current->setState(1);
 850         } else {
 851            transmit(&(*current), MSG_RESUME_TAXI, ATC_GROUND_TO_AIR);
 852            //cerr << "Transmittig resume instrudtion " << currStatus << " " << available << endl;
 853            current->setState(2);
 854         }
 855         lastTransmission = now;
 856         available = false;
 857         // Don't act on the changed instruction until the transmission is confirmed
 858         // So set back to original status
 859         current->setHoldPosition(origStatus);
 860         //cerr << "Current state " << current->getState() << endl;
 861     } else {
 862     }
 863     int state = current->getState();
 864     if ((state == 1) && (available)) {
 865         //cerr << "ACKNOWLEDGE HOLD" << endl;
 866         transmit(&(*current), MSG_ACKNOWLEDGE_HOLD_POSITION, ATC_AIR_TO_GROUND);
 867         current->setState(0);
 868         current->setHoldPosition(true);
 869         lastTransmission = now;
 870         available = false;
 871
 872     }
 873     if ((state == 2) && (available)) {
 874         //cerr << "ACKNOWLEDGE RESUME" << endl;
 875         transmit(&(*current), MSG_ACKNOWLEDGE_RESUME_TAXI, ATC_AIR_TO_GROUND);
 876         current->setState(0);
 877         current->setHoldPosition(false);
 878         lastTransmission = now;
 879         available = false;
 880     }
 881 }
 882
 883 /**
 884  * Check whether situations occur where the current aircraft is waiting for itself
 885  * due to higher order interactions.
 886  * A 'circular' wait is a situation where a waits for b, b waits for c, and c waits
 887  * for a. Ideally each aircraft only waits for one other aircraft, so by tracing
 888  * through this list of waiting aircraft, we can check if we'd eventually end back
 889  * at the current aircraft.
 890  *
 891  * Note that we should consider the situation where we are actually checking aircraft
 892  * d, which is waiting for aircraft a. d is not part of the loop, but is held back by
 893  * the looping aircraft. If we don't check for that, this function will get stuck into
 894  * endless loop.
 895  */
 896
 897 bool FGGroundNetwork::checkForCircularWaits(int id)
 898 {
 899     //cerr << "Performing Wait check " << id << endl;
 900     int target = 0;
 901     TrafficVectorIterator current, other;
 902     TrafficVectorIterator i = activeTraffic.begin();
 903     int trafficSize = activeTraffic.size();
 904     if (trafficSize) {
 905         while (i != activeTraffic.end()) {
 906             if (i->getId() == id) {
 907                 break;
 908             }
 909             i++;
 910         }
 911     } else {
 912         return false;
 913     }
 914     if (i == activeTraffic.end() || (trafficSize == 0)) {
 915         SG_LOG(SG_GENERAL, SG_ALERT,
 916                "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits");
 917     }
 918
 919     current = i;
 920     target = current->getWaitsForId();
 921     //bool printed = false; // Note that this variable is for debugging purposes only.
 922     int counter = 0;
 923
 924     if (id == target) {
 925         //cerr << "aircraft waits for user" << endl;
 926         return false;
 927     }
 928
 929
 930     while ((target > 0) && (target != id) && counter++ < trafficSize) {
 931         //printed = true;
 932         TrafficVectorIterator i = activeTraffic.begin();
 933         if (trafficSize) {
 934             //while ((i->getId() != id) && i != activeTraffic.end())
 935             while (i != activeTraffic.end()) {
 936                 if (i->getId() == target) {
 937                     break;
 938                 }
 939                 i++;
 940             }
 941         } else {
 942             return false;
 943         }
 944         if (i == activeTraffic.end() || (trafficSize == 0)) {
 945             //cerr << "[Waiting for traffic at Runway: DONE] " << endl << endl;;
 946             // The target id is not found on the current network, which means it's at the tower
 947             //SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits");
 948             return false;
 949         }
 950         other = i;
 951         target = other->getWaitsForId();
 952
 953         // actually this trap isn't as impossible as it first seemed:
 954         // the setWaitsForID(id) is set to current when the aircraft
 955         // is waiting for the user controlled aircraft.
 956         //if (current->getId() == other->getId()) {
 957         //    cerr << "Caught the impossible trap" << endl;
 958         //    cerr << "Current = " << current->getId() << endl;
 959         //    cerr << "Other   = " << other  ->getId() << endl;
 960         //    for (TrafficVectorIterator at = activeTraffic.begin();
 961         //          at != activeTraffic.end();
 962         //          at++) {
 963         //        cerr << "currently active aircraft : " << at->getCallSign() << " with Id " << at->getId() << " waits for " << at->getWaitsForId() << endl;
 964         //    }
 965         //    exit(1);
 966         if (current->getId() == other->getId())
 967             return false;
 968         //}
 969         //cerr << current->getCallSign() << " (" << current->getId()  << ") " << " -> " << other->getCallSign()
 970         //     << " (" << other->getId()  << "); " << endl;;
 971         //current = other;
 972     }
 973
 974
 975
 976
 977
 978
 979     //if (printed)
 980     //   cerr << "[done] " << endl << endl;;
 981     if (id == target) {
 982         SG_LOG(SG_GENERAL, SG_WARN,
 983                "Detected circular wait condition: Id = " << id <<
 984                "target = " << target);
 985         return true;
 986     } else {
 987         return false;
 988     }
 989 }
 990
 991 // Note that this function is probably obsolete...
 992 bool FGGroundNetwork::hasInstruction(int id)
 993 {
 994     TrafficVectorIterator i = activeTraffic.begin();
 995     // Search search if the current id has an entry
 996     // This might be faster using a map instead of a vector, but let's start by taking a safe route
 997     if (activeTraffic.size()) {
 998         //while ((i->getId() != id) && i != activeTraffic.end()) {
 999         while (i != activeTraffic.end()) {
1000             if (i->getId() == id) {
1001                 break;
1002             }
1003             i++;
1004         }
1005     }
1006     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
1007         SG_LOG(SG_GENERAL, SG_ALERT,
1008                "AI error: checking ATC instruction for aircraft without traffic record");
1009     } else {
1010         return i->hasInstruction();
1011     }
1012     return false;
1013 }
1014
1015 FGATCInstruction FGGroundNetwork::getInstruction(int id)
1016 {
1017     TrafficVectorIterator i = activeTraffic.begin();
1018     // Search search if the current id has an entry
1019     // This might be faster using a map instead of a vector, but let's start by taking a safe route
1020     if (activeTraffic.size()) {
1021         //while ((i->getId() != id) && i != activeTraffic.end()) {
1022         while (i != activeTraffic.end()) {
1023             if (i->getId() == id) {
1024                 break;
1025             }
1026             i++;
1027         }
1028     }
1029     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
1030         SG_LOG(SG_GENERAL, SG_ALERT,
1031                "AI error: requesting ATC instruction for aircraft without traffic record");
1032     } else {
1033         return i->getInstruction();
1034     }
1035     return FGATCInstruction();
1036 }