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