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