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     init();
 464     TrafficVectorIterator i = activeTraffic.begin();
 465     // Search search if the current id alread has an entry
 466     // This might be faster using a map instead of a vector, but let's start by taking a safe route
 467     if (activeTraffic.size()) {
 468         //while ((i->getId() != id) && i != activeTraffic.end()) {
 469         while (i != activeTraffic.end()) {
 470             if (i->getId() == id) {
 471                 break;
 472             }
 473             i++;
 474         }
 475     }
 476     // Add a new TrafficRecord if no one exsists for this aircraft.
 477     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 478         FGTrafficRecord rec;
 479         rec.setId(id);
 480         rec.setLeg(leg);
 481         rec.setPositionAndIntentions(currentPosition, intendedRoute);
 482         rec.setPositionAndHeading(lat, lon, heading, speed, alt);
 483         rec.setRadius(radius);  // only need to do this when creating the record.
 484         rec.setAircraft(aircraft);
 485         activeTraffic.push_back(rec);
 486     } else {
 487         i->setPositionAndIntentions(currentPosition, intendedRoute);
 488         i->setPositionAndHeading(lat, lon, heading, speed, alt);
 489     }
 490 }
 491
 492 void FGGroundNetwork::signOff(int id)
 493 {
 494     TrafficVectorIterator i = activeTraffic.begin();
 495     // Search search if the current id alread has an entry
 496     // This might be faster using a map instead of a vector, but let's start by taking a safe route
 497     if (activeTraffic.size()) {
 498         //while ((i->getId() != id) && i != activeTraffic.end()) {
 499         while (i != activeTraffic.end()) {
 500             if (i->getId() == id) {
 501                 break;
 502             }
 503             i++;
 504         }
 505     }
 506     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 507         SG_LOG(SG_GENERAL, SG_ALERT,
 508                "AI error: Aircraft without traffic record is signing off");
 509     } else {
 510         i = activeTraffic.erase(i);
 511     }
 512 }
 513
 514 void FGGroundNetwork::updateAircraftInformation(int id, double lat, double lon,
 515                                                 double heading, double speed, double alt,
 516                                                 double dt)
 517 {
 518     // Check whether aircraft are on hold due to a preceding pushback. If so, make sure to
 519     // Transmit air-to-ground "Ready to taxi request:
 520     // Transmit ground to air approval / hold
 521     // Transmit confirmation ...
 522     // Probably use a status mechanism similar to the Engine start procedure in the startup controller.
 523
 524
 525     TrafficVectorIterator i = activeTraffic.begin();
 526     // Search search if the current id has an entry
 527     // This might be faster using a map instead of a vector, but let's start by taking a safe route
 528     TrafficVectorIterator current, closest;
 529     if (activeTraffic.size()) {
 530         //while ((i->getId() != id) && i != activeTraffic.end()) {
 531         while (i != activeTraffic.end()) {
 532             if (i->getId() == id) {
 533                 break;
 534             }
 535             i++;
 536         }
 537     }
 538     // update position of the current aircraft
 539     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 540         SG_LOG(SG_GENERAL, SG_ALERT,
 541                "AI error: updating aircraft without traffic record");
 542     } else {
 543         i->setPositionAndHeading(lat, lon, heading, speed, alt);
 544         current = i;
 545     }
 546
 547     setDt(getDt() + dt);
 548
 549     // Update every three secs, but add some randomness
 550     // to prevent all IA objects doing this in synchrony
 551     //if (getDt() < (3.0) + (rand() % 10))
 552     //  return;
 553     //else
 554     //  setDt(0);
 555     current->clearResolveCircularWait();
 556     current->setWaitsForId(0);
 557     checkSpeedAdjustment(id, lat, lon, heading, speed, alt);
 558     bool needsTaxiClearance = current->getAircraft()->getTaxiClearanceRequest();
 559     if (!needsTaxiClearance) {
 560         checkHoldPosition(id, lat, lon, heading, speed, alt);
 561         if (checkForCircularWaits(id)) {
 562             i->setResolveCircularWait();
 563         }
 564     } else {
 565         current->setHoldPosition(true);
 566         int state = current->getState();
 567         time_t now = time(NULL) + fgGetLong("/sim/time/warp");
 568         if ((now - lastTransmission) > 15) {
 569             available = true;
 570         }
 571         if ((state < 3) && available) {
 572              transmit(&(*current), MSG_REQUEST_TAXI_CLEARANCE, ATC_AIR_TO_GROUND);
 573              current->setState(3);
 574              lastTransmission = now;
 575              available = false;
 576         }
 577         if ((state == 3) && available) {
 578             transmit(&(*current), MSG_ISSUE_TAXI_CLEARANCE, ATC_GROUND_TO_AIR);
 579             current->setState(4);
 580             lastTransmission = now;
 581             available = false;
 582         }
 583         if ((state == 4) && available) {
 584             transmit(&(*current), MSG_ACKNOWLEDGE_TAXI_CLEARANCE, ATC_AIR_TO_GROUND);
 585             current->setState(5);
 586             lastTransmission = now;
 587             available = false;
 588         }
 589         if ((state == 5) && available) {
 590             current->setState(0);
 591             current->getAircraft()->setTaxiClearanceRequest(false);
 592             current->setHoldPosition(true);
 593             available = false;
 594         }
 595
 596     }
 597
 598 }
 599
 600 /**
 601    Scan for a speed adjustment change. Find the nearest aircraft that is in front
 602    and adjust speed when we get too close. Only do this when current position and/or
 603    intentions of the current aircraft match current taxiroute position of the proximate
 604    aircraft. For traffic that is on other routes we need to issue a "HOLD Position"
 605    instruction. See below for the hold position instruction.
 606
 607    Note that there currently still is one flaw in the logic that needs to be addressed.
 608    There can be situations where one aircraft is in front of the current aircraft, on a separate
 609    route, but really close after an intersection coming off the current route. This
 610    aircraft is still close enough to block the current aircraft. This situation is currently
 611    not addressed yet, but should be.
 612 */
 613
 614 void FGGroundNetwork::checkSpeedAdjustment(int id, double lat,
 615                                            double lon, double heading,
 616                                            double speed, double alt)
 617 {
 618
 619     TrafficVectorIterator current, closest;
 620     TrafficVectorIterator i = activeTraffic.begin();
 621     bool otherReasonToSlowDown = false;
 622     bool previousInstruction;
 623     if (activeTraffic.size()) {
 624         //while ((i->getId() != id) && (i != activeTraffic.end()))
 625         while (i != activeTraffic.end()) {
 626             if (i->getId() == id) {
 627                 break;
 628             }
 629             i++;
 630         }
 631     } else {
 632         return;
 633     }
 634     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 635         SG_LOG(SG_GENERAL, SG_ALERT,
 636                "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkSpeedAdjustment");
 637     }
 638     current = i;
 639     //closest = current;
 640
 641     previousInstruction = current->getSpeedAdjustment();
 642     double mindist = HUGE_VAL;
 643     if (activeTraffic.size()) {
 644         double course, dist, bearing, minbearing, az2;
 645         SGGeod curr(SGGeod::fromDegM(lon, lat, alt));
 646         //TrafficVector iterator closest;
 647         closest = current;
 648         for (TrafficVectorIterator i = activeTraffic.begin();
 649              i != activeTraffic.end(); i++) {
 650             if (i == current) {
 651                 continue;
 652             }
 653
 654             SGGeod other(SGGeod::fromDegM(i->getLongitude(),
 655                                           i->getLatitude(),
 656                                           i->getAltitude()));
 657             SGGeodesy::inverse(curr, other, course, az2, dist);
 658             bearing = fabs(heading - course);
 659             if (bearing > 180)
 660                 bearing = 360 - bearing;
 661             if ((dist < mindist) && (bearing < 60.0)) {
 662                 mindist = dist;
 663                 closest = i;
 664                 minbearing = bearing;
 665             }
 666         }
 667         //Check traffic at the tower controller
 668         if (towerController->hasActiveTraffic()) {
 669             for (TrafficVectorIterator i =
 670                  towerController->getActiveTraffic().begin();
 671                  i != towerController->getActiveTraffic().end(); i++) {
 672                 //cerr << "Comparing " << current->getId() << " and " << i->getId() << endl;
 673                 SGGeod other(SGGeod::fromDegM(i->getLongitude(),
 674                                               i->getLatitude(),
 675                                               i->getAltitude()));
 676                 SGGeodesy::inverse(curr, other, course, az2, dist);
 677                 bearing = fabs(heading - course);
 678                 if (bearing > 180)
 679                     bearing = 360 - bearing;
 680                 if ((dist < mindist) && (bearing < 60.0)) {
 681                     mindist = dist;
 682                     closest = i;
 683                     minbearing = bearing;
 684                     otherReasonToSlowDown = true;
 685                 }
 686             }
 687         }
 688         // Finally, check UserPosition
 689         double userLatitude = fgGetDouble("/position/latitude-deg");
 690         double userLongitude = fgGetDouble("/position/longitude-deg");
 691         SGGeod user(SGGeod::fromDeg(userLongitude, userLatitude));
 692         SGGeodesy::inverse(curr, user, course, az2, dist);
 693
 694         bearing = fabs(heading - course);
 695         if (bearing > 180)
 696             bearing = 360 - bearing;
 697         if ((dist < mindist) && (bearing < 60.0)) {
 698             mindist = dist;
 699             //closest = i;
 700             minbearing = bearing;
 701             otherReasonToSlowDown = true;
 702         }
 703
 704         current->clearSpeedAdjustment();
 705
 706         if (current->checkPositionAndIntentions(*closest)
 707             || otherReasonToSlowDown) {
 708             double maxAllowableDistance =
 709                 (1.1 * current->getRadius()) +
 710                 (1.1 * closest->getRadius());
 711             if (mindist < 2 * maxAllowableDistance) {
 712                 if (current->getId() == closest->getWaitsForId())
 713                     return;
 714                 else
 715                     current->setWaitsForId(closest->getId());
 716                 if (closest->getId() != current->getId())
 717                     current->setSpeedAdjustment(closest->getSpeed() *
 718                                                 (mindist / 100));
 719                 else
 720                     current->setSpeedAdjustment(0);     // This can only happen when the user aircraft is the one closest
 721                 if (mindist < maxAllowableDistance) {
 722                     //double newSpeed = (maxAllowableDistance-mindist);
 723                     //current->setSpeedAdjustment(newSpeed);
 724                     //if (mindist < 0.5* maxAllowableDistance)
 725                     //  {
 726                     current->setSpeedAdjustment(0);
 727                     //  }
 728                 }
 729             }
 730         }
 731     }
 732 }
 733
 734 /**
 735    Check for "Hold position instruction".
 736    The hold position should be issued under the following conditions:
 737    1) For aircraft entering or crossing a runway with active traffic on it, or landing aircraft near it
 738    2) For taxiing aircraft that use one taxiway in opposite directions
 739    3) For crossing or merging taxiroutes.
 740 */
 741
 742 void FGGroundNetwork::checkHoldPosition(int id, double lat,
 743                                         double lon, double heading,
 744                                         double speed, double alt)
 745 {
 746     TrafficVectorIterator current;
 747     TrafficVectorIterator i = activeTraffic.begin();
 748     if (activeTraffic.size()) {
 749         //while ((i->getId() != id) && i != activeTraffic.end())
 750         while (i != activeTraffic.end()) {
 751             if (i->getId() == id) {
 752                 break;
 753             }
 754             i++;
 755         }
 756     } else {
 757         return;
 758     }
 759     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 760         SG_LOG(SG_GENERAL, SG_ALERT,
 761                "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkHoldPosition");
 762     }
 763     current = i;
 764     bool origStatus = current->hasHoldPosition();
 765     current->setHoldPosition(false);
 766     SGGeod curr(SGGeod::fromDegM(lon, lat, alt));
 767
 768     for (i = activeTraffic.begin(); i != activeTraffic.end(); i++) {
 769         if (i->getId() != current->getId()) {
 770             int node = current->crosses(this, *i);
 771             if (node != -1) {
 772                 FGTaxiNode *taxiNode = findNode(node);
 773
 774                 // Determine whether it's save to continue or not.
 775                 // If we have a crossing route, there are two possibilities:
 776                 // 1) This is an interestion
 777                 // 2) This is oncoming two-way traffic, using the same taxiway.
 778                 //cerr << "Hold check 1 : " << id << " has common node " << node << endl;
 779
 780                 SGGeod other(SGGeod::
 781                              fromDegM(i->getLongitude(), i->getLatitude(),
 782                                       i->getAltitude()));
 783                 bool needsToWait;
 784                 bool opposing;
 785                 if (current->isOpposing(this, *i, node)) {
 786                     needsToWait = true;
 787                     opposing = true;
 788                     //cerr << "Hold check 2 : " << node << "  has opposing segment " << endl;
 789                     // issue a "Hold Position" as soon as we're close to the offending node
 790                     // For now, I'm doing this as long as the other aircraft doesn't
 791                     // have a hold instruction as soon as we're within a reasonable
 792                     // distance from the offending node.
 793                     // This may be a bit of a conservative estimate though, as it may
 794                     // be well possible that both aircraft can both continue to taxi
 795                     // without crashing into each other.
 796                 } else {
 797                     opposing = false;
 798                     if (SGGeodesy::distanceM(other, taxiNode->getGeod()) > 200) // 2.0*i->getRadius())
 799                     {
 800                         needsToWait = false;
 801                         //cerr << "Hold check 3 : " << id <<"  Other aircraft approaching node is still far away. (" << dist << " nm). Can safely continue "
 802                         //           << endl;
 803                     } else {
 804                         needsToWait = true;
 805                         //cerr << "Hold check 4: " << id << "  Would need to wait for other aircraft : distance = " << dist << " meters" << endl;
 806                     }
 807                 }
 808
 809                 double dist =
 810                     SGGeodesy::distanceM(curr, taxiNode->getGeod());
 811                 if (!(i->hasHoldPosition())) {
 812
 813                     if ((dist < 200) && //2.5*current->getRadius()) &&
 814                         (needsToWait) && (i->onRoute(this, *current)) &&
 815                         //((i->onRoute(this, *current)) || ((!(i->getSpeedAdjustment())))) &&
 816                         (!(current->getId() == i->getWaitsForId())))
 817                         //(!(i->getSpeedAdjustment()))) // &&
 818                         //(!(current->getSpeedAdjustment())))
 819
 820                     {
 821                         current->setHoldPosition(true);
 822                         current->setWaitsForId(i->getId());
 823                         //cerr << "Hold check 5: " << current->getCallSign() <<"  Setting Hold Position: distance to node ("  << node << ") "
 824                         //           << dist << " meters. Waiting for " << i->getCallSign();
 825                         //if (opposing)
 826                         //cerr <<" [opposing] " << endl;
 827                         //else
 828                         //        cerr << "[non-opposing] " << endl;
 829                         //if (i->hasSpeefAdjustment())
 830                         //        {
 831                         //          cerr << " (which in turn waits for ) " << i->
 832                     } else {
 833                         //cerr << "Hold check 6: " << id << "  No need to hold yet: Distance to node : " << dist << " nm"<< endl;
 834                     }
 835                 }
 836             }
 837         }
 838     }
 839     bool currStatus = current->hasHoldPosition();
 840
 841     // Either a Hold Position or a resume taxi transmission has been issued
 842     time_t now = time(NULL) + fgGetLong("/sim/time/warp");
 843     if ((now - lastTransmission) > 2) {
 844         available = true;
 845     }
 846     if ((origStatus != currStatus) && available) {
 847         //cerr << "Issueing hold short instrudtion " << currStatus << " " << available << endl;
 848         if (currStatus == true) { // No has a hold short instruction
 849            transmit(&(*current), MSG_HOLD_POSITION, ATC_GROUND_TO_AIR);
 850            //cerr << "Transmittin hold short instrudtion " << currStatus << " " << available << endl;
 851            current->setState(1);
 852         } else {
 853            transmit(&(*current), MSG_RESUME_TAXI, ATC_GROUND_TO_AIR);
 854            //cerr << "Transmittig resume instrudtion " << currStatus << " " << available << endl;
 855            current->setState(2);
 856         }
 857         lastTransmission = now;
 858         available = false;
 859         // Don't act on the changed instruction until the transmission is confirmed
 860         // So set back to original status
 861         current->setHoldPosition(origStatus);
 862         //cerr << "Current state " << current->getState() << endl;
 863     } else {
 864     }
 865     int state = current->getState();
 866     if ((state == 1) && (available)) {
 867         //cerr << "ACKNOWLEDGE HOLD" << endl;
 868         transmit(&(*current), MSG_ACKNOWLEDGE_HOLD_POSITION, ATC_AIR_TO_GROUND);
 869         current->setState(0);
 870         current->setHoldPosition(true);
 871         lastTransmission = now;
 872         available = false;
 873
 874     }
 875     if ((state == 2) && (available)) {
 876         //cerr << "ACKNOWLEDGE RESUME" << endl;
 877         transmit(&(*current), MSG_ACKNOWLEDGE_RESUME_TAXI, ATC_AIR_TO_GROUND);
 878         current->setState(0);
 879         current->setHoldPosition(false);
 880         lastTransmission = now;
 881         available = false;
 882     }
 883 }
 884
 885 /**
 886  * Check whether situations occur where the current aircraft is waiting for itself
 887  * due to higher order interactions.
 888  * A 'circular' wait is a situation where a waits for b, b waits for c, and c waits
 889  * for a. Ideally each aircraft only waits for one other aircraft, so by tracing
 890  * through this list of waiting aircraft, we can check if we'd eventually end back
 891  * at the current aircraft.
 892  *
 893  * Note that we should consider the situation where we are actually checking aircraft
 894  * d, which is waiting for aircraft a. d is not part of the loop, but is held back by
 895  * the looping aircraft. If we don't check for that, this function will get stuck into
 896  * endless loop.
 897  */
 898
 899 bool FGGroundNetwork::checkForCircularWaits(int id)
 900 {
 901     //cerr << "Performing Wait check " << id << endl;
 902     int target = 0;
 903     TrafficVectorIterator current, other;
 904     TrafficVectorIterator i = activeTraffic.begin();
 905     int trafficSize = activeTraffic.size();
 906     if (trafficSize) {
 907         while (i != activeTraffic.end()) {
 908             if (i->getId() == id) {
 909                 break;
 910             }
 911             i++;
 912         }
 913     } else {
 914         return false;
 915     }
 916     if (i == activeTraffic.end() || (trafficSize == 0)) {
 917         SG_LOG(SG_GENERAL, SG_ALERT,
 918                "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits");
 919     }
 920
 921     current = i;
 922     target = current->getWaitsForId();
 923     //bool printed = false; // Note that this variable is for debugging purposes only.
 924     int counter = 0;
 925
 926     if (id == target) {
 927         //cerr << "aircraft waits for user" << endl;
 928         return false;
 929     }
 930
 931
 932     while ((target > 0) && (target != id) && counter++ < trafficSize) {
 933         //printed = true;
 934         TrafficVectorIterator i = activeTraffic.begin();
 935         if (trafficSize) {
 936             //while ((i->getId() != id) && i != activeTraffic.end())
 937             while (i != activeTraffic.end()) {
 938                 if (i->getId() == target) {
 939                     break;
 940                 }
 941                 i++;
 942             }
 943         } else {
 944             return false;
 945         }
 946         if (i == activeTraffic.end() || (trafficSize == 0)) {
 947             //cerr << "[Waiting for traffic at Runway: DONE] " << endl << endl;;
 948             // The target id is not found on the current network, which means it's at the tower
 949             //SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits");
 950             return false;
 951         }
 952         other = i;
 953         target = other->getWaitsForId();
 954
 955         // actually this trap isn't as impossible as it first seemed:
 956         // the setWaitsForID(id) is set to current when the aircraft
 957         // is waiting for the user controlled aircraft.
 958         //if (current->getId() == other->getId()) {
 959         //    cerr << "Caught the impossible trap" << endl;
 960         //    cerr << "Current = " << current->getId() << endl;
 961         //    cerr << "Other   = " << other  ->getId() << endl;
 962         //    for (TrafficVectorIterator at = activeTraffic.begin();
 963         //          at != activeTraffic.end();
 964         //          at++) {
 965         //        cerr << "currently active aircraft : " << at->getCallSign() << " with Id " << at->getId() << " waits for " << at->getWaitsForId() << endl;
 966         //    }
 967         //    exit(1);
 968         if (current->getId() == other->getId())
 969             return false;
 970         //}
 971         //cerr << current->getCallSign() << " (" << current->getId()  << ") " << " -> " << other->getCallSign()
 972         //     << " (" << other->getId()  << "); " << endl;;
 973         //current = other;
 974     }
 975
 976
 977
 978
 979
 980
 981     //if (printed)
 982     //   cerr << "[done] " << endl << endl;;
 983     if (id == target) {
 984         SG_LOG(SG_GENERAL, SG_WARN,
 985                "Detected circular wait condition: Id = " << id <<
 986                "target = " << target);
 987         return true;
 988     } else {
 989         return false;
 990     }
 991 }
 992
 993 // Note that this function is probably obsolete...
 994 bool FGGroundNetwork::hasInstruction(int id)
 995 {
 996     TrafficVectorIterator i = activeTraffic.begin();
 997     // Search search if the current id has an entry
 998     // This might be faster using a map instead of a vector, but let's start by taking a safe route
 999     if (activeTraffic.size()) {
1000         //while ((i->getId() != id) && i != activeTraffic.end()) {
1001         while (i != activeTraffic.end()) {
1002             if (i->getId() == id) {
1003                 break;
1004             }
1005             i++;
1006         }
1007     }
1008     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
1009         SG_LOG(SG_GENERAL, SG_ALERT,
1010                "AI error: checking ATC instruction for aircraft without traffic record");
1011     } else {
1012         return i->hasInstruction();
1013     }
1014     return false;
1015 }
1016
1017 FGATCInstruction FGGroundNetwork::getInstruction(int id)
1018 {
1019     TrafficVectorIterator i = activeTraffic.begin();
1020     // Search search if the current id has an entry
1021     // This might be faster using a map instead of a vector, but let's start by taking a safe route
1022     if (activeTraffic.size()) {
1023         //while ((i->getId() != id) && i != activeTraffic.end()) {
1024         while (i != activeTraffic.end()) {
1025             if (i->getId() == id) {
1026                 break;
1027             }
1028             i++;
1029         }
1030     }
1031     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
1032         SG_LOG(SG_GENERAL, SG_ALERT,
1033                "AI error: requesting ATC instruction for aircraft without traffic record");
1034     } else {
1035         return i->getInstruction();
1036     }
1037     return FGATCInstruction();
1038 }