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
 396 FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end)
 397 {
 398 //implements Dijkstra's algorithm to find shortest distance route from start to end
 399 //taken from http://en.wikipedia.org/wiki/Dijkstra's_algorithm
 400
 401     //double INFINITE = 100000000000.0;
 402     // initialize scoring values
 403     for (FGTaxiNodeVectorIterator
 404          itr = nodes.begin();
 405          itr != nodes.end(); itr++) {
 406             (*itr)->pathscore = HUGE_VAL; //infinity by all practical means
 407             (*itr)->previousnode = 0; //
 408             (*itr)->previousseg = 0; //
 409          }
 410
 411     FGTaxiNode *firstNode = findNode(start);
 412     firstNode->pathscore = 0;
 413
 414     FGTaxiNode *lastNode  = findNode(end);
 415
 416     FGTaxiNodeVector unvisited(nodes); // working copy
 417
 418     while (!unvisited.empty()) {
 419         FGTaxiNode* best = *(unvisited.begin());
 420         for (FGTaxiNodeVectorIterator
 421              itr = unvisited.begin();
 422              itr != unvisited.end(); itr++) {
 423                  if ((*itr)->pathscore < best->pathscore)
 424                      best = (*itr);
 425         }
 426
 427         FGTaxiNodeVectorIterator newend = remove(unvisited.begin(), unvisited.end(), best);
 428         unvisited.erase(newend, unvisited.end());
 429
 430         if (best == lastNode) { // found route or best not connected
 431             break;
 432         } else {
 433             for (FGTaxiSegmentVectorIterator
 434                  seg = best->getBeginRoute();
 435                  seg != best->getEndRoute(); seg++) {
 436                 FGTaxiNode* tgt = (*seg)->getEnd();
 437                 double alt = best->pathscore + (*seg)->getLength();
 438                 if (alt < tgt->pathscore) {              // Relax (u,v)
 439                     tgt->pathscore = alt;
 440                     tgt->previousnode = best;
 441                     tgt->previousseg = *seg; //
 442                 }
 443             }
 444         }
 445     }
 446
 447     if (lastNode->pathscore == HUGE_VAL) {
 448         // no valid route found
 449         SG_LOG( SG_GENERAL, SG_ALERT, "Failed to find route from waypoint " << start << " to " << end << " at " <<
 450                 parent->getId());
 451         exit(1); //TODO exit more gracefully, no need to stall the whole sim with broken GN's
 452     } else {
 453         // assemble route from backtrace information
 454         intVec nodes, routes;
 455         FGTaxiNode* bt = lastNode;
 456         while (bt->previousnode != 0) {
 457             nodes.push_back(bt->getIndex());
 458             routes.push_back(bt->previousseg->getIndex());
 459             bt = bt->previousnode;
 460         }
 461         nodes.push_back(start);
 462         reverse(nodes.begin(), nodes.end());
 463         reverse(routes.begin(), routes.end());
 464
 465         return FGTaxiRoute(nodes, routes, lastNode->pathscore, 0);
 466     }
 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    current->clearResolveCircularWait();
 733    current->setWaitsForId(0);
 734    checkSpeedAdjustment(id, lat, lon, heading, speed, alt);
 735    checkHoldPosition   (id, lat, lon, heading, speed, alt);
 736    if (checkForCircularWaits(id)) {
 737        i->setResolveCircularWait();
 738    }
 739 }
 740
 741 /**
 742    Scan for a speed adjustment change. Find the nearest aircraft that is in front
 743    and adjust speed when we get too close. Only do this when current position and/or
 744    intentions of the current aircraft match current taxiroute position of the proximate
 745    aircraft. For traffic that is on other routes we need to issue a "HOLD Position"
 746    instruction. See below for the hold position instruction.
 747
 748    Note that there currently still is one flaw in the logic that needs to be addressed.
 749    can be situations where one aircraft is in front of the current aircraft, on a separate
 750    route, but really close after an intersection coming off the current route. This
 751    aircraft is still close enough to block the current aircraft. This situation is currently
 752    not addressed yet, but should be.
 753 */
 754
 755 void FGGroundNetwork::checkSpeedAdjustment(int id, double lat,
 756                                            double lon, double heading,
 757                                            double speed, double alt)
 758 {
 759
 760   TrafficVectorIterator current, closest;
 761   TrafficVectorIterator i = activeTraffic.begin();
 762   bool otherReasonToSlowDown = false;
 763   bool previousInstruction;
 764   if (activeTraffic.size())
 765     {
 766       //while ((i->getId() != id) && (i != activeTraffic.end()))
 767       while (i != activeTraffic.end()) {
 768         if (i->getId() == id) {
 769           break;
 770         }
 771         i++;
 772       }
 773     }
 774   else
 775     {
 776       return;
 777     }
 778   if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 779     SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkSpeedAdjustment");
 780   }
 781   current = i;
 782   //closest = current;
 783
 784   previousInstruction = current->getSpeedAdjustment();
 785   double mindist = HUGE;
 786   if (activeTraffic.size())
 787     {
 788       double course, dist, bearing, minbearing;
 789       SGWayPoint curr  (lon,
 790                         lat,
 791                         alt);
 792       //TrafficVector iterator closest;
 793       closest = current;
 794       for (TrafficVectorIterator i = activeTraffic.begin();
 795            i != activeTraffic.end(); i++)
 796         {
 797           if (i != current) {
 798             //SGWayPoint curr  (lon,
 799             //        lat,
 800             //        alt);
 801             SGWayPoint other    (i->getLongitude  (),
 802                                  i->getLatitude (),
 803                                  i->getAltitude  ());
 804             other.CourseAndDistance(curr, &course, &dist);
 805             bearing = fabs(heading-course);
 806             if (bearing > 180)
 807               bearing = 360-bearing;
 808             if ((dist < mindist) && (bearing < 60.0))
 809               {
 810                 mindist = dist;
 811                 closest = i;
 812                 minbearing = bearing;
 813               }
 814           }
 815         }
 816       //Check traffic at the tower controller
 817       if (towerController->hasActiveTraffic())
 818         {
 819           for (TrafficVectorIterator i = towerController->getActiveTraffic().begin();
 820                i != towerController->getActiveTraffic().end(); i++)
 821             {
 822               //cerr << "Comparing " << current->getId() << " and " << i->getId() << endl;
 823               //SGWayPoint curr  (lon,
 824               //                  lat,
 825               //                  alt);
 826               SGWayPoint other    (i->getLongitude  (),
 827                                    i->getLatitude (),
 828                                    i->getAltitude  ());
 829               other.CourseAndDistance(curr, &course, &dist);
 830               bearing = fabs(heading-course);
 831               if (bearing > 180)
 832                 bearing = 360-bearing;
 833               if ((dist < mindist) && (bearing < 60.0))
 834                 {
 835                   mindist = dist;
 836                   closest = i;
 837                   minbearing = bearing;
 838                   otherReasonToSlowDown = true;
 839                 }
 840             }
 841         }
 842       // Finally, check UserPosition
 843       double userLatitude  = fgGetDouble("/position/latitude-deg");
 844       double userLongitude = fgGetDouble("/position/longitude-deg");
 845       SGWayPoint user    (userLongitude,
 846                           userLatitude,
 847                           alt); // Alt is not really important here.
 848       user.CourseAndDistance(curr, &course, &dist);
 849       bearing = fabs(heading-course);
 850       if (bearing > 180)
 851         bearing = 360-bearing;
 852       if ((dist < mindist) && (bearing < 60.0))
 853         {
 854           mindist = dist;
 855           //closest = i;
 856           minbearing = bearing;
 857           otherReasonToSlowDown = true;
 858         }
 859
 860       //        if (closest == current) {
 861       //          //SG_LOG(SG_GENERAL, SG_ALERT, "AI error: closest and current match");
 862       //          //return;
 863       //        }
 864       //cerr << "Distance : " << dist << " bearing : " << bearing << " heading : " << heading
 865       //   << " course : " << course << endl;
 866       current->clearSpeedAdjustment();
 867
 868       if (current->checkPositionAndIntentions(*closest) || otherReasonToSlowDown)
 869         {
 870            double maxAllowableDistance = (1.1*current->getRadius()) + (1.1*closest->getRadius());
 871            if (mindist < 2*maxAllowableDistance)
 872              {
 873                if (current->getId() == closest->getWaitsForId())
 874                  return;
 875                else
 876                  current->setWaitsForId(closest->getId());
 877                if (closest != current)
 878                  current->setSpeedAdjustment(closest->getSpeed()* (mindist/100));
 879                else
 880                  current->setSpeedAdjustment(0); // This can only happen when the user aircraft is the one closest
 881                if (mindist < maxAllowableDistance)
 882                  {
 883                    //double newSpeed = (maxAllowableDistance-mindist);
 884                    //current->setSpeedAdjustment(newSpeed);
 885                    //if (mindist < 0.5* maxAllowableDistance)
 886                    //  {
 887                        current->setSpeedAdjustment(0);
 888                        //  }
 889                  }
 890              }
 891         }
 892     }
 893 }
 894
 895 /**
 896    Check for "Hold position instruction".
 897    The hold position should be issued under the following conditions:
 898    1) For aircraft entering or crossing a runway with active traffic on it, or landing aircraft near it
 899    2) For taxiing aircraft that use one taxiway in opposite directions
 900    3) For crossing or merging taxiroutes.
 901 */
 902
 903 void FGGroundNetwork::checkHoldPosition(int id, double lat,
 904                                         double lon, double heading,
 905                                         double speed, double alt)
 906 {
 907
 908   TrafficVectorIterator current;
 909   TrafficVectorIterator i = activeTraffic.begin();
 910   if (activeTraffic.size())
 911     {
 912       //while ((i->getId() != id) && i != activeTraffic.end())
 913       while (i != activeTraffic.end()) {
 914         if (i->getId() == id) {
 915           break;
 916         }
 917         i++;
 918       }
 919     }
 920   else
 921     {
 922       return ;
 923     }
 924   if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 925     SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkHoldPosition");
 926   }
 927   current = i;
 928   current->setHoldPosition(false);
 929   SGWayPoint curr  (lon,
 930                     lat,
 931                     alt);
 932   double course, dist, bearing, minbearing;
 933   for (i = activeTraffic.begin();
 934        i != activeTraffic.end(); i++)
 935     {
 936       if (i != current)
 937         {
 938           int node = current->crosses(this, *i);
 939           if (node != -1)
 940             {
 941               // Determine whether it's save to continue or not.
 942               // If we have a crossing route, there are two possibilities:
 943               // 1) This is an interestion
 944               // 2) This is oncoming two-way traffic, using the same taxiway.
 945               //cerr << "Hold check 1 : " << id << " has common node " << node << endl;
 946               SGWayPoint nodePos(findNode(node)->getLongitude  (),
 947                                  findNode(node)->getLatitude   (),
 948                                  alt);
 949
 950               SGWayPoint other    (i->getLongitude  (),
 951                                    i->getLatitude (),
 952                                    i->getAltitude  ());
 953               //other.CourseAndDistance(curr, &course, &dist);
 954               bool needsToWait;
 955               bool opposing;
 956               if (current->isOpposing(this, *i, node))
 957                 {
 958                   needsToWait = true;
 959                   opposing    = true;
 960                   //cerr << "Hold check 2 : " << node << "  has opposing segment " << endl;
 961                   // issue a "Hold Position" as soon as we're close to the offending node
 962                   // For now, I'm doing this as long as the other aircraft doesn't
 963                   // have a hold instruction as soon as we're within a reasonable
 964                   // distance from the offending node.
 965                   // This may be a bit of a conservative estimate though, as it may
 966                   // be well possible that both aircraft can both continue to taxi
 967                   // without crashing into each other.
 968                 }
 969               else
 970                 {
 971                   opposing = false;
 972                   other.CourseAndDistance(nodePos, &course, &dist);
 973                   if (dist > 200) // 2.0*i->getRadius())
 974                     {
 975                       needsToWait = false;
 976                       //cerr << "Hold check 3 : " << id <<"  Other aircraft approaching node is still far away. (" << dist << " nm). Can safely continue "
 977                       //           << endl;
 978                     }
 979                   else
 980                     {
 981                       needsToWait = true;
 982                       //cerr << "Hold check 4: " << id << "  Would need to wait for other aircraft : distance = " << dist << " meters" << endl;
 983                     }
 984                 }
 985               curr.CourseAndDistance(nodePos, &course, &dist);
 986               if (!(i->hasHoldPosition()))
 987                 {
 988
 989                   if ((dist < 200) && //2.5*current->getRadius()) &&
 990                       (needsToWait) &&
 991                       (i->onRoute(this, *current)) &&
 992                       //((i->onRoute(this, *current)) || ((!(i->getSpeedAdjustment())))) &&
 993                       (!(current->getId() == i->getWaitsForId())))
 994                       //(!(i->getSpeedAdjustment()))) // &&
 995                       //(!(current->getSpeedAdjustment())))
 996
 997                     {
 998                       current->setHoldPosition(true);
 999                       current->setWaitsForId(i->getId());
1000                       //cerr << "Hold check 5: " << current->getCallSign() <<"  Setting Hold Position: distance to node ("  << node << ") "
1001                       //           << dist << " meters. Waiting for " << i->getCallSign();
1002                       //if (opposing)
1003                       //cerr <<" [opposing] " << endl;
1004                       //else
1005                       //        cerr << "[non-opposing] " << endl;
1006                       //if (i->hasSpeefAdjustment())
1007                       //        {
1008                       //          cerr << " (which in turn waits for ) " << i->
1009                     }
1010                   else
1011                     {
1012                       //cerr << "Hold check 6: " << id << "  No need to hold yet: Distance to node : " << dist << " nm"<< endl;
1013                     }
1014                 }
1015             }
1016         }
1017     }
1018 }
1019
1020 /**
1021  * Check whether situations occur where the current aircraft is waiting for itself
1022  * due to higher order interactions.
1023  * A 'circular' wait is a situation where a waits for b, b waits for c, and c waits
1024  * for a. Ideally each aircraft only waits for one other aircraft, so by tracing
1025  * through this list of waiting aircraft, we can check if we'd eventually end back
1026  * at the current aircraft.
1027  *
1028  * Note that we should consider the situation where we are actually checking aircraft
1029  * d, which is waiting for aircraft a. d is not part of the loop, but is held back by
1030  * the looping aircraft. If we don't check for that, this function will get stuck into
1031  * endless loop.
1032  */
1033
1034 bool FGGroundNetwork::checkForCircularWaits(int id)
1035 {
1036    //cerr << "Performing Wait check " << id << endl;
1037    int target = 0;
1038    TrafficVectorIterator current, other;
1039    TrafficVectorIterator i = activeTraffic.begin();
1040    int trafficSize = activeTraffic.size();
1041    if (trafficSize)  {
1042         //while ((i->getId() != id) && i != activeTraffic.end())
1043         while (i != activeTraffic.end()) {
1044         if (i->getId() == id) {
1045             break;
1046         }
1047         i++;
1048     }
1049   }
1050   else {
1051       return false;
1052   }
1053   if (i == activeTraffic.end() || (trafficSize == 0)) {
1054     SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits");
1055   }
1056
1057    current = i;
1058    target = current->getWaitsForId();
1059    //bool printed = false; // Note that this variable is for debugging purposes only.
1060    int counter = 0;
1061    while ((target > 0) && (target != id) && counter++ < trafficSize) {
1062     //printed = true;
1063      TrafficVectorIterator i = activeTraffic.begin();
1064      if (trafficSize)  {
1065           //while ((i->getId() != id) && i != activeTraffic.end())
1066           while (i != activeTraffic.end()) {
1067           if (i->getId() == target) {
1068               break;
1069           }
1070           i++;
1071         }
1072       }
1073       else {
1074         return false;
1075     }
1076     if (i == activeTraffic.end() || (trafficSize == 0)) {
1077         //cerr << "[Waiting for traffic at Runway: DONE] " << endl << endl;;
1078       // The target id is not found on the current network, which means it's at the tower
1079       //SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits");
1080       return false;
1081     }
1082     other = i;
1083     target = other->getWaitsForId();
1084
1085     // actually this trap isn't as impossible as it first seemed:
1086     // the setWaitsForID(id) is set to current when the aircraft
1087     // is waiting for the user controlled aircraft.
1088     //if (current->getId() == other->getId()) {
1089     //    cerr << "Caught the impossible trap" << endl;
1090     //    cerr << "Current = " << current->getId() << endl;
1091     //    cerr << "Other   = " << other  ->getId() << endl;
1092     //    for (TrafficVectorIterator at = activeTraffic.begin();
1093     //          at != activeTraffic.end();
1094     //          at++) {
1095     //        cerr << "currently active aircraft : " << at->getCallSign() << " with Id " << at->getId() << " waits for " << at->getWaitsForId() << endl;
1096     //    }
1097     //    exit(1);
1098     if (current->getId() == other->getId())
1099         return false;
1100     //}
1101     //cerr << current->getCallSign() << " (" << current->getId()  << ") " << " -> " << other->getCallSign()
1102     //     << " (" << other->getId()  << "); " << endl;;
1103     //current = other;
1104    }
1105    //if (printed)
1106    //   cerr << "[done] " << endl << endl;;
1107    if (id == target) {
1108        SG_LOG(SG_GENERAL, SG_INFO, "Detected circular wait condition");
1109        return true;
1110    } else {
1111    return false;
1112    }
1113 }
1114
1115 // Note that this function is probably obsolete...
1116 bool FGGroundNetwork::hasInstruction(int id)
1117 {
1118     TrafficVectorIterator i = activeTraffic.begin();
1119    // Search search if the current id has an entry
1120    // This might be faster using a map instead of a vector, but let's start by taking a safe route
1121    if (activeTraffic.size())
1122      {
1123        //while ((i->getId() != id) && i != activeTraffic.end()) {
1124        while (i != activeTraffic.end()) {
1125          if (i->getId() == id) {
1126            break;
1127          }
1128          i++;
1129        }
1130      }
1131    if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
1132      SG_LOG(SG_GENERAL, SG_ALERT, "AI error: checking ATC instruction for aircraft without traffic record");
1133    } else {
1134      return i->hasInstruction();
1135    }
1136   return false;
1137 }
1138
1139 FGATCInstruction FGGroundNetwork::getInstruction(int id)
1140 {
1141   TrafficVectorIterator i = activeTraffic.begin();
1142   // Search search if the current id has an entry
1143    // This might be faster using a map instead of a vector, but let's start by taking a safe route
1144    if (activeTraffic.size()) {
1145      //while ((i->getId() != id) && i != activeTraffic.end()) {
1146      while (i != activeTraffic.end()) {
1147        if (i->getId() == id) {
1148          break;
1149        }
1150        i++;
1151      }
1152    }
1153    if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
1154      SG_LOG(SG_GENERAL, SG_ALERT, "AI error: requesting ATC instruction for aircraft without traffic record");
1155    } else {
1156      return i->getInstruction();
1157    }
1158   return FGATCInstruction();
1159 }
1160
1161