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