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