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