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