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