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