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