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 int FGGroundNetwork::findNearestNode(const SGGeod& aGeod)
 316 {
 317   return findNearestNode(aGeod.getLatitudeDeg(), aGeod.getLongitudeDeg());
 318 }
 319
 320 int FGGroundNetwork::findNearestNode(double lat, double lon)
 321 {
 322   double minDist = HUGE_VAL;
 323   double dist;
 324   int index;
 325   SGWayPoint first  (lon,
 326                      lat,
 327                      0);
 328
 329   for (FGTaxiNodeVectorIterator
 330          itr = nodes.begin();
 331        itr != nodes.end(); itr++)
 332     {
 333       double course;
 334       SGWayPoint second ((*itr)->getLongitude(),
 335                          (*itr)->getLatitude(),
 336                          0);
 337       first.CourseAndDistance(second, &course, &dist);
 338       if (dist < minDist)
 339         {
 340           minDist = dist;
 341           index = (*itr)->getIndex();
 342           //cerr << "Minimum distance of " << minDist << " for index " << index << endl;
 343         }
 344     }
 345   return index;
 346 }
 347
 348 FGTaxiNode *FGGroundNetwork::findNode(int idx)
 349 { /*
 350     for (FGTaxiNodeVectorIterator
 351     itr = nodes.begin();
 352     itr != nodes.end(); itr++)
 353     {
 354     if (itr->getIndex() == idx)
 355     return itr->getAddress();
 356     }*/
 357
 358   if ((idx >= 0) && (idx < nodes.size()))
 359     return nodes[idx]->getAddress();
 360   else
 361     return 0;
 362 }
 363
 364 FGTaxiSegment *FGGroundNetwork::findSegment(int idx)
 365 {/*
 366   for (FGTaxiSegmentVectorIterator
 367          itr = segments.begin();
 368        itr != segments.end(); itr++)
 369     {
 370       if (itr->getIndex() == idx)
 371         return itr->getAddress();
 372     }
 373  */
 374   if ((idx > 0) && (idx <= segments.size()))
 375     return segments[idx-1]->getAddress();
 376   else
 377     {
 378       //cerr << "Alert: trying to find invalid segment " << idx << endl;
 379       return 0;
 380     }
 381 }
 382
 383
 384 FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end, bool fullSearch)
 385 {
 386 //implements Dijkstra's algorithm to find shortest distance route from start to end
 387 //taken from http://en.wikipedia.org/wiki/Dijkstra's_algorithm
 388
 389     //double INFINITE = 100000000000.0;
 390     // initialize scoring values
 391     int nParkings = parent->getDynamics()->getNrOfParkings();
 392     FGTaxiNodeVector *currNodesSet;
 393     if (fullSearch) {
 394          currNodesSet = &nodes;
 395     } else {
 396          currNodesSet = &pushBackNodes;
 397     }
 398
 399     for (FGTaxiNodeVectorIterator
 400          itr = currNodesSet->begin();
 401          itr != currNodesSet->end(); itr++) {
 402             (*itr)->setPathScore(HUGE_VAL); //infinity by all practical means
 403             (*itr)->setPreviousNode(0); //
 404             (*itr)->setPreviousSeg (0); //
 405          }
 406
 407     FGTaxiNode *firstNode = findNode(start);
 408     firstNode->setPathScore(0);
 409
 410     FGTaxiNode *lastNode  = findNode(end);
 411
 412     FGTaxiNodeVector unvisited(*currNodesSet); // working copy
 413
 414     while (!unvisited.empty()) {
 415         FGTaxiNode* best = *(unvisited.begin());
 416         for (FGTaxiNodeVectorIterator
 417              itr = unvisited.begin();
 418              itr != unvisited.end(); itr++) {
 419                  if ((*itr)->getPathScore() < best->getPathScore())
 420                      best = (*itr);
 421         }
 422
 423         FGTaxiNodeVectorIterator newend = remove(unvisited.begin(), unvisited.end(), best);
 424         unvisited.erase(newend, unvisited.end());
 425
 426         if (best == lastNode) { // found route or best not connected
 427             break;
 428         } else {
 429             for (FGTaxiSegmentVectorIterator
 430                  seg = best->getBeginRoute();
 431                  seg != best->getEndRoute(); seg++) {
 432                 if (fullSearch || (*seg)->isPushBack()) {
 433                     FGTaxiNode* tgt = (*seg)->getEnd();
 434                     double alt = best->getPathScore() + (*seg)->getLength() + (*seg)->getPenalty(nParkings);
 435                     if (alt < tgt->getPathScore()) {              // Relax (u,v)
 436                         tgt->setPathScore(alt);
 437                         tgt->setPreviousNode(best);
 438                         tgt->setPreviousSeg(*seg); //
 439                    }
 440                 } else {
 441                 //   // cerr << "Skipping TaxiSegment " << (*seg)->getIndex() << endl;
 442                 }
 443             }
 444         }
 445     }
 446
 447     if (lastNode->getPathScore() == HUGE_VAL) {
 448         // no valid route found
 449         if (fullSearch) {
 450             SG_LOG( SG_GENERAL, SG_ALERT, "Failed to find route from waypoint " << start << " to " << end << " at " <<
 451                     parent->getId());
 452         }
 453         FGTaxiRoute empty;
 454         return empty;
 455         //exit(1); //TODO exit more gracefully, no need to stall the whole sim with broken GN's
 456     } else {
 457         // assemble route from backtrace information
 458         intVec nodes, routes;
 459         FGTaxiNode* bt = lastNode;
 460         while (bt->getPreviousNode() != 0) {
 461             nodes.push_back(bt->getIndex());
 462             routes.push_back(bt->getPreviousSegment()->getIndex());
 463             bt = bt->getPreviousNode();
 464         }
 465         nodes.push_back(start);
 466         reverse(nodes.begin(), nodes.end());
 467         reverse(routes.begin(), routes.end());
 468
 469         return FGTaxiRoute(nodes, routes, lastNode->getPathScore(), 0);
 470     }
 471 }
 472
 473 int FGTaxiSegment::getPenalty(int nGates) {
 474      int penalty = 0;
 475      if (end->getIndex() < nGates) {
 476          penalty += 10000;
 477      }
 478      if (end->getIsOnRunway()) { // For now. In future versions, need to find out whether runway is active.
 479          penalty += 1000;
 480      }
 481      return penalty;
 482 }
 483
 484 /* ATC Related Functions */
 485
 486 void FGGroundNetwork::announcePosition(int id, FGAIFlightPlan *intendedRoute, int currentPosition,
 487                                        double lat, double lon, double heading,
 488                                        double speed, double alt, double radius, int leg,
 489                                        FGAIAircraft *aircraft)
 490 {
 491    TrafficVectorIterator i = activeTraffic.begin();
 492    // Search search if the current id alread has an entry
 493    // This might be faster using a map instead of a vector, but let's start by taking a safe route
 494    if (activeTraffic.size()) {
 495      //while ((i->getId() != id) && i != activeTraffic.end()) {
 496      while (i != activeTraffic.end()) {
 497        if (i->getId() == id) {
 498          break;
 499        }
 500        i++;
 501      }
 502    }
 503    // Add a new TrafficRecord if no one exsists for this aircraft.
 504    if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 505      FGTrafficRecord rec;
 506      rec.setId(id);
 507      rec.setPositionAndIntentions(currentPosition, intendedRoute);
 508      rec.setPositionAndHeading(lat, lon, heading, speed, alt);
 509      rec.setRadius(radius); // only need to do this when creating the record.
 510      rec.setAircraft(aircraft);
 511      activeTraffic.push_back(rec);
 512    } else {
 513      i->setPositionAndIntentions(currentPosition, intendedRoute);
 514      i->setPositionAndHeading(lat, lon, heading, speed, alt);
 515    }
 516 }
 517
 518 void FGGroundNetwork::signOff(int id) {
 519   TrafficVectorIterator i = activeTraffic.begin();
 520    // Search search if the current id alread has an entry
 521    // This might be faster using a map instead of a vector, but let's start by taking a safe route
 522    if (activeTraffic.size()) {
 523      //while ((i->getId() != id) && i != activeTraffic.end()) {
 524      while (i != activeTraffic.end()) {
 525        if (i->getId() == id) {
 526          break;
 527        }
 528        i++;
 529      }
 530    }
 531    if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 532      SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Aircraft without traffic record is signing off");
 533    } else {
 534        i = activeTraffic.erase(i);
 535    }
 536 }
 537
 538 void FGGroundNetwork::update(int id, double lat, double lon, double heading, double speed, double alt,
 539                              double dt) {
 540    TrafficVectorIterator i = activeTraffic.begin();
 541    // Search search if the current id has an entry
 542    // This might be faster using a map instead of a vector, but let's start by taking a safe route
 543    TrafficVectorIterator current, closest;
 544    if (activeTraffic.size()) {
 545      //while ((i->getId() != id) && i != activeTraffic.end()) {
 546      while (i != activeTraffic.end()) {
 547        if (i->getId() == id) {
 548          break;
 549        }
 550        i++;
 551      }
 552    }
 553    // update position of the current aircraft
 554    if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 555      SG_LOG(SG_GENERAL, SG_ALERT, "AI error: updating aircraft without traffic record");
 556    } else {
 557      i->setPositionAndHeading(lat, lon, heading, speed, alt);
 558      current = i;
 559    }
 560
 561    setDt(getDt() + dt);
 562
 563    // Update every three secs, but add some randomness
 564    // to prevent all IA objects doing this in synchrony
 565    //if (getDt() < (3.0) + (rand() % 10))
 566    //  return;
 567    //else
 568    //  setDt(0);
 569    current->clearResolveCircularWait();
 570    current->setWaitsForId(0);
 571    checkSpeedAdjustment(id, lat, lon, heading, speed, alt);
 572    checkHoldPosition   (id, lat, lon, heading, speed, alt);
 573    if (checkForCircularWaits(id)) {
 574        i->setResolveCircularWait();
 575    }
 576 }
 577
 578 /**
 579    Scan for a speed adjustment change. Find the nearest aircraft that is in front
 580    and adjust speed when we get too close. Only do this when current position and/or
 581    intentions of the current aircraft match current taxiroute position of the proximate
 582    aircraft. For traffic that is on other routes we need to issue a "HOLD Position"
 583    instruction. See below for the hold position instruction.
 584
 585    Note that there currently still is one flaw in the logic that needs to be addressed.
 586    can be situations where one aircraft is in front of the current aircraft, on a separate
 587    route, but really close after an intersection coming off the current route. This
 588    aircraft is still close enough to block the current aircraft. This situation is currently
 589    not addressed yet, but should be.
 590 */
 591
 592 void FGGroundNetwork::checkSpeedAdjustment(int id, double lat,
 593                                            double lon, double heading,
 594                                            double speed, double alt)
 595 {
 596
 597   TrafficVectorIterator current, closest;
 598   TrafficVectorIterator i = activeTraffic.begin();
 599   bool otherReasonToSlowDown = false;
 600   bool previousInstruction;
 601   if (activeTraffic.size())
 602     {
 603       //while ((i->getId() != id) && (i != activeTraffic.end()))
 604       while (i != activeTraffic.end()) {
 605         if (i->getId() == id) {
 606           break;
 607         }
 608         i++;
 609       }
 610     }
 611   else
 612     {
 613       return;
 614     }
 615   if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 616     SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkSpeedAdjustment");
 617   }
 618   current = i;
 619   //closest = current;
 620
 621   previousInstruction = current->getSpeedAdjustment();
 622   double mindist = HUGE_VAL;
 623   if (activeTraffic.size())
 624     {
 625       double course, dist, bearing, minbearing;
 626       SGWayPoint curr  (lon,
 627                         lat,
 628                         alt);
 629       //TrafficVector iterator closest;
 630       closest = current;
 631       for (TrafficVectorIterator i = activeTraffic.begin();
 632            i != activeTraffic.end(); i++)
 633         {
 634           if (i != current) {
 635             //SGWayPoint curr  (lon,
 636             //        lat,
 637             //        alt);
 638             SGWayPoint other    (i->getLongitude  (),
 639                                  i->getLatitude (),
 640                                  i->getAltitude  ());
 641             other.CourseAndDistance(curr, &course, &dist);
 642             bearing = fabs(heading-course);
 643             if (bearing > 180)
 644               bearing = 360-bearing;
 645             if ((dist < mindist) && (bearing < 60.0))
 646               {
 647                 mindist = dist;
 648                 closest = i;
 649                 minbearing = bearing;
 650               }
 651           }
 652         }
 653       //Check traffic at the tower controller
 654       if (towerController->hasActiveTraffic())
 655         {
 656           for (TrafficVectorIterator i = towerController->getActiveTraffic().begin();
 657                i != towerController->getActiveTraffic().end(); i++)
 658             {
 659               //cerr << "Comparing " << current->getId() << " and " << i->getId() << endl;
 660               //SGWayPoint curr  (lon,
 661               //                  lat,
 662               //                  alt);
 663               SGWayPoint other    (i->getLongitude  (),
 664                                    i->getLatitude (),
 665                                    i->getAltitude  ());
 666               other.CourseAndDistance(curr, &course, &dist);
 667               bearing = fabs(heading-course);
 668               if (bearing > 180)
 669                 bearing = 360-bearing;
 670               if ((dist < mindist) && (bearing < 60.0))
 671                 {
 672                   mindist = dist;
 673                   closest = i;
 674                   minbearing = bearing;
 675                   otherReasonToSlowDown = true;
 676                 }
 677             }
 678         }
 679       // Finally, check UserPosition
 680       double userLatitude  = fgGetDouble("/position/latitude-deg");
 681       double userLongitude = fgGetDouble("/position/longitude-deg");
 682       SGWayPoint user    (userLongitude,
 683                           userLatitude,
 684                           alt); // Alt is not really important here.
 685       user.CourseAndDistance(curr, &course, &dist);
 686       bearing = fabs(heading-course);
 687       if (bearing > 180)
 688         bearing = 360-bearing;
 689       if ((dist < mindist) && (bearing < 60.0))
 690         {
 691           mindist = dist;
 692           //closest = i;
 693           minbearing = bearing;
 694           otherReasonToSlowDown = true;
 695         }
 696
 697       //        if (closest == current) {
 698       //          //SG_LOG(SG_GENERAL, SG_ALERT, "AI error: closest and current match");
 699       //          //return;
 700       //        }
 701       //cerr << "Distance : " << dist << " bearing : " << bearing << " heading : " << heading
 702       //   << " course : " << course << endl;
 703       current->clearSpeedAdjustment();
 704
 705       if (current->checkPositionAndIntentions(*closest) || otherReasonToSlowDown)
 706         {
 707            double maxAllowableDistance = (1.1*current->getRadius()) + (1.1*closest->getRadius());
 708            if (mindist < 2*maxAllowableDistance)
 709              {
 710                if (current->getId() == closest->getWaitsForId())
 711                  return;
 712                else
 713                  current->setWaitsForId(closest->getId());
 714                if (closest->getId() != current->getId())
 715                  current->setSpeedAdjustment(closest->getSpeed()* (mindist/100));
 716                else
 717                  current->setSpeedAdjustment(0); // This can only happen when the user aircraft is the one closest
 718                if (mindist < maxAllowableDistance)
 719                  {
 720                    //double newSpeed = (maxAllowableDistance-mindist);
 721                    //current->setSpeedAdjustment(newSpeed);
 722                    //if (mindist < 0.5* maxAllowableDistance)
 723                    //  {
 724                        current->setSpeedAdjustment(0);
 725                        //  }
 726                  }
 727              }
 728         }
 729     }
 730 }
 731
 732 /**
 733    Check for "Hold position instruction".
 734    The hold position should be issued under the following conditions:
 735    1) For aircraft entering or crossing a runway with active traffic on it, or landing aircraft near it
 736    2) For taxiing aircraft that use one taxiway in opposite directions
 737    3) For crossing or merging taxiroutes.
 738 */
 739
 740 void FGGroundNetwork::checkHoldPosition(int id, double lat,
 741                                         double lon, double heading,
 742                                         double speed, double alt)
 743 {
 744
 745   TrafficVectorIterator current;
 746   TrafficVectorIterator i = activeTraffic.begin();
 747   if (activeTraffic.size())
 748     {
 749       //while ((i->getId() != id) && i != activeTraffic.end())
 750       while (i != activeTraffic.end()) {
 751         if (i->getId() == id) {
 752           break;
 753         }
 754         i++;
 755       }
 756     }
 757   else
 758     {
 759       return ;
 760     }
 761   if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 762     SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkHoldPosition");
 763   }
 764   current = i;
 765   current->setHoldPosition(false);
 766   SGWayPoint curr  (lon,
 767                     lat,
 768                     alt);
 769   double course, dist, bearing, minbearing;
 770   for (i = activeTraffic.begin();
 771        i != activeTraffic.end(); i++)
 772     {
 773       if (i->getId() != current->getId())
 774         {
 775           int node = current->crosses(this, *i);
 776           if (node != -1)
 777             {
 778               // Determine whether it's save to continue or not.
 779               // If we have a crossing route, there are two possibilities:
 780               // 1) This is an interestion
 781               // 2) This is oncoming two-way traffic, using the same taxiway.
 782               //cerr << "Hold check 1 : " << id << " has common node " << node << endl;
 783               SGWayPoint nodePos(findNode(node)->getLongitude  (),
 784                                  findNode(node)->getLatitude   (),
 785                                  alt);
 786
 787               SGWayPoint other    (i->getLongitude  (),
 788                                    i->getLatitude (),
 789                                    i->getAltitude  ());
 790               //other.CourseAndDistance(curr, &course, &dist);
 791               bool needsToWait;
 792               bool opposing;
 793               if (current->isOpposing(this, *i, node))
 794                 {
 795                   needsToWait = true;
 796                   opposing    = true;
 797                   //cerr << "Hold check 2 : " << node << "  has opposing segment " << endl;
 798                   // issue a "Hold Position" as soon as we're close to the offending node
 799                   // For now, I'm doing this as long as the other aircraft doesn't
 800                   // have a hold instruction as soon as we're within a reasonable
 801                   // distance from the offending node.
 802                   // This may be a bit of a conservative estimate though, as it may
 803                   // be well possible that both aircraft can both continue to taxi
 804                   // without crashing into each other.
 805                 }
 806               else
 807                 {
 808                   opposing = false;
 809                   other.CourseAndDistance(nodePos, &course, &dist);
 810                   if (dist > 200) // 2.0*i->getRadius())
 811                     {
 812                       needsToWait = false;
 813                       //cerr << "Hold check 3 : " << id <<"  Other aircraft approaching node is still far away. (" << dist << " nm). Can safely continue "
 814                       //           << endl;
 815                     }
 816                   else
 817                     {
 818                       needsToWait = true;
 819                       //cerr << "Hold check 4: " << id << "  Would need to wait for other aircraft : distance = " << dist << " meters" << endl;
 820                     }
 821                 }
 822               curr.CourseAndDistance(nodePos, &course, &dist);
 823               if (!(i->hasHoldPosition()))
 824                 {
 825
 826                   if ((dist < 200) && //2.5*current->getRadius()) &&
 827                       (needsToWait) &&
 828                       (i->onRoute(this, *current)) &&
 829                       //((i->onRoute(this, *current)) || ((!(i->getSpeedAdjustment())))) &&
 830                       (!(current->getId() == i->getWaitsForId())))
 831                       //(!(i->getSpeedAdjustment()))) // &&
 832                       //(!(current->getSpeedAdjustment())))
 833
 834                     {
 835                       current->setHoldPosition(true);
 836                       current->setWaitsForId(i->getId());
 837                       //cerr << "Hold check 5: " << current->getCallSign() <<"  Setting Hold Position: distance to node ("  << node << ") "
 838                       //           << dist << " meters. Waiting for " << i->getCallSign();
 839                       //if (opposing)
 840                       //cerr <<" [opposing] " << endl;
 841                       //else
 842                       //        cerr << "[non-opposing] " << endl;
 843                       //if (i->hasSpeefAdjustment())
 844                       //        {
 845                       //          cerr << " (which in turn waits for ) " << i->
 846                     }
 847                   else
 848                     {
 849                       //cerr << "Hold check 6: " << id << "  No need to hold yet: Distance to node : " << dist << " nm"<< endl;
 850                     }
 851                 }
 852             }
 853         }
 854     }
 855 }
 856
 857 /**
 858  * Check whether situations occur where the current aircraft is waiting for itself
 859  * due to higher order interactions.
 860  * A 'circular' wait is a situation where a waits for b, b waits for c, and c waits
 861  * for a. Ideally each aircraft only waits for one other aircraft, so by tracing
 862  * through this list of waiting aircraft, we can check if we'd eventually end back
 863  * at the current aircraft.
 864  *
 865  * Note that we should consider the situation where we are actually checking aircraft
 866  * d, which is waiting for aircraft a. d is not part of the loop, but is held back by
 867  * the looping aircraft. If we don't check for that, this function will get stuck into
 868  * endless loop.
 869  */
 870
 871 bool FGGroundNetwork::checkForCircularWaits(int id)
 872 {
 873    //cerr << "Performing Wait check " << id << endl;
 874    int target = 0;
 875    TrafficVectorIterator current, other;
 876    TrafficVectorIterator i = activeTraffic.begin();
 877    int trafficSize = activeTraffic.size();
 878    if (trafficSize)  {
 879         while (i != activeTraffic.end()) {
 880         if (i->getId() == id) {
 881             break;
 882         }
 883         i++;
 884     }
 885   }
 886   else {
 887       return false;
 888   }
 889   if (i == activeTraffic.end() || (trafficSize == 0)) {
 890     SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits");
 891   }
 892
 893    current = i;
 894    target = current->getWaitsForId();
 895    //bool printed = false; // Note that this variable is for debugging purposes only.
 896    int counter = 0;
 897
 898    if (id == target) {
 899        //cerr << "aircraft waits for user" << endl;
 900        return false;
 901    }
 902
 903
 904    while ((target > 0) && (target != id) && counter++ < trafficSize) {
 905     //printed = true;
 906      TrafficVectorIterator i = activeTraffic.begin();
 907      if (trafficSize)  {
 908           //while ((i->getId() != id) && i != activeTraffic.end())
 909           while (i != activeTraffic.end()) {
 910           if (i->getId() == target) {
 911               break;
 912           }
 913           i++;
 914         }
 915       }
 916       else {
 917         return false;
 918     }
 919     if (i == activeTraffic.end() || (trafficSize == 0)) {
 920         //cerr << "[Waiting for traffic at Runway: DONE] " << endl << endl;;
 921       // The target id is not found on the current network, which means it's at the tower
 922       //SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits");
 923       return false;
 924     }
 925     other = i;
 926     target = other->getWaitsForId();
 927
 928     // actually this trap isn't as impossible as it first seemed:
 929     // the setWaitsForID(id) is set to current when the aircraft
 930     // is waiting for the user controlled aircraft.
 931     //if (current->getId() == other->getId()) {
 932     //    cerr << "Caught the impossible trap" << endl;
 933     //    cerr << "Current = " << current->getId() << endl;
 934     //    cerr << "Other   = " << other  ->getId() << endl;
 935     //    for (TrafficVectorIterator at = activeTraffic.begin();
 936     //          at != activeTraffic.end();
 937     //          at++) {
 938     //        cerr << "currently active aircraft : " << at->getCallSign() << " with Id " << at->getId() << " waits for " << at->getWaitsForId() << endl;
 939     //    }
 940     //    exit(1);
 941     if (current->getId() == other->getId())
 942         return false;
 943     //}
 944     //cerr << current->getCallSign() << " (" << current->getId()  << ") " << " -> " << other->getCallSign()
 945     //     << " (" << other->getId()  << "); " << endl;;
 946     //current = other;
 947    }
 948
 949
 950
 951
 952
 953
 954    //if (printed)
 955    //   cerr << "[done] " << endl << endl;;
 956    if (id == target) {
 957        SG_LOG(SG_GENERAL, SG_WARN, "Detected circular wait condition: Id = " << id << "target = " << target);
 958        return true;
 959    } else {
 960    return false;
 961    }
 962 }
 963
 964 // Note that this function is probably obsolete...
 965 bool FGGroundNetwork::hasInstruction(int id)
 966 {
 967     TrafficVectorIterator i = activeTraffic.begin();
 968    // Search search if the current id has an entry
 969    // This might be faster using a map instead of a vector, but let's start by taking a safe route
 970    if (activeTraffic.size())
 971      {
 972        //while ((i->getId() != id) && i != activeTraffic.end()) {
 973        while (i != activeTraffic.end()) {
 974          if (i->getId() == id) {
 975            break;
 976          }
 977          i++;
 978        }
 979      }
 980    if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 981      SG_LOG(SG_GENERAL, SG_ALERT, "AI error: checking ATC instruction for aircraft without traffic record");
 982    } else {
 983      return i->hasInstruction();
 984    }
 985   return false;
 986 }
 987
 988 FGATCInstruction FGGroundNetwork::getInstruction(int id)
 989 {
 990   TrafficVectorIterator i = activeTraffic.begin();
 991   // Search search if the current id has an entry
 992    // This might be faster using a map instead of a vector, but let's start by taking a safe route
 993    if (activeTraffic.size()) {
 994      //while ((i->getId() != id) && i != activeTraffic.end()) {
 995      while (i != activeTraffic.end()) {
 996        if (i->getId() == id) {
 997          break;
 998        }
 999        i++;
1000      }
1001    }
1002    if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
1003      SG_LOG(SG_GENERAL, SG_ALERT, "AI error: requesting ATC instruction for aircraft without traffic record");
1004    } else {
1005      return i->getInstruction();
1006    }
1007   return FGATCInstruction();
1008 }
1009
1010