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 #include <fstream>
  30
  31
  32 #include <osg/Geode>
  33 #include <osg/Geometry>
  34 #include <osg/MatrixTransform>
  35 #include <osg/Shape>
  36
  37 #include <simgear/debug/logstream.hxx>
  38 #include <simgear/scene/material/EffectGeode.hxx>
  39 #include <simgear/scene/material/matlib.hxx>
  40 #include <simgear/scene/material/mat.hxx>
  41 #include <simgear/scene/util/OsgMath.hxx>
  42
  43 #include <Airports/simple.hxx>
  44 #include <Airports/dynamics.hxx>
  45
  46 #include <AIModel/AIAircraft.hxx>
  47 #include <AIModel/performancedata.hxx>
  48 #include <AIModel/AIFlightPlan.hxx>
  49
  50 #include <ATC/atc_mgr.hxx>
  51
  52 #include <Scenery/scenery.hxx>
  53
  54 #include "groundnetwork.hxx"
  55
  56 using std::string;
  57
  58 /***************************************************************************
  59  * FGTaxiSegment
  60  **************************************************************************/
  61
  62 void FGTaxiSegment::setStart(FGTaxiNodeVector * nodes)
  63 {
  64     FGTaxiNodeVectorIterator i = nodes->begin();
  65     while (i != nodes->end()) {
  66         //cerr << "Scanning start node index" << (*i)->getIndex() << endl;
  67         if ((*i)->getIndex() == startNode) {
  68             start = (*i)->getAddress();
  69             (*i)->addSegment(this);
  70             return;
  71         }
  72         i++;
  73     }
  74     SG_LOG(SG_GENERAL, SG_ALERT,
  75            "Could not find start node " << startNode << endl);
  76 }
  77
  78 void FGTaxiSegment::setEnd(FGTaxiNodeVector * nodes)
  79 {
  80     FGTaxiNodeVectorIterator i = nodes->begin();
  81     while (i != nodes->end()) {
  82         //cerr << "Scanning end node index" << (*i)->getIndex() << endl;
  83         if ((*i)->getIndex() == endNode) {
  84             end = (*i)->getAddress();
  85             return;
  86         }
  87         i++;
  88     }
  89     SG_LOG(SG_GENERAL, SG_ALERT,
  90            "Could not find end node " << endNode << endl);
  91 }
  92
  93
  94
  95 // There is probably a computationally cheaper way of
  96 // doing this.
  97 void FGTaxiSegment::setDimensions(double elevation)
  98 {
  99     length = SGGeodesy::distanceM(start->getGeod(), end->getGeod());
 100     //heading = SGGeodesy::headingDeg(start->getGeod(), end->getGeod());
 101
 102     double az2; //, distanceM;
 103     SGGeodesy::inverse(start->getGeod(), end->getGeod(), heading, az2, length);
 104     double coveredDistance = length * 0.5;
 105     SGGeodesy::direct(start->getGeod(), heading, coveredDistance, center, az2);
 106     //start->setElevation(elevation);
 107     //end->setElevation(elevation);
 108     //cerr << "Centerpoint = (" << center.getLatitudeDeg() << ", " << center.getLongitudeDeg() << "). Heading = " << heading << endl;
 109 }
 110
 111
 112 //void FGTaxiSegment::setCourseDiff(double crse)
 113 //{
 114 //    headingDiff = fabs(course - crse);
 115
 116 //    if (headingDiff > 180)
 117 //        headingDiff = fabs(headingDiff - 360);
 118 //}
 119
 120 void FGTaxiSegment::block(int id, time_t blockTime, time_t now)
 121 {
 122     BlockListIterator i = blockTimes.begin();
 123     while (i != blockTimes.end()) {
 124         if (i->getId() == id)
 125             break;
 126         i++;
 127     }
 128     if (i == blockTimes.end()) {
 129         blockTimes.push_back(Block(id, blockTime, now));
 130         sort(blockTimes.begin(), blockTimes.end());
 131     } else {
 132         i->updateTimeStamps(blockTime, now);
 133     }
 134 }
 135
 136 // The segment has a block if any of the block times listed in the block list is
 137 // smaller than the current time.
 138 bool FGTaxiSegment::hasBlock(time_t now)
 139 {
 140     for (BlockListIterator i = blockTimes.begin(); i != blockTimes.end(); i++) {
 141         if (i->getBlockTime() < now)
 142             return true;
 143     }
 144     return false;
 145 }
 146
 147 void FGTaxiSegment::unblock(time_t now)
 148 {
 149     if (blockTimes.size()) {
 150         BlockListIterator i = blockTimes.begin();
 151         if (i->getTimeStamp() < (now - 30)) {
 152             blockTimes.erase(i);
 153         }
 154     }
 155 }
 156
 157
 158
 159 /***************************************************************************
 160  * FGTaxiRoute
 161  **************************************************************************/
 162 bool FGTaxiRoute::next(int *nde)
 163 {
 164     //for (intVecIterator i = nodes.begin(); i != nodes.end(); i++)
 165     //  cerr << "FGTaxiRoute contains : " << *(i) << endl;
 166     //cerr << "Offset from end: " << nodes.end() - currNode << endl;
 167     //if (currNode != nodes.end())
 168     //  cerr << "true" << endl;
 169     //else
 170     //  cerr << "false" << endl;
 171     //if (nodes.size() != (routes.size()) +1)
 172     //  cerr << "ALERT: Misconfigured TaxiRoute : " << nodes.size() << " " << routes.size() << endl;
 173
 174     if (currNode == nodes.end())
 175         return false;
 176     *nde = *(currNode);
 177     if (currNode != nodes.begin())      // make sure route corresponds to the end node
 178         currRoute++;
 179     currNode++;
 180     return true;
 181 };
 182
 183 bool FGTaxiRoute::next(int *nde, int *rte)
 184 {
 185     //for (intVecIterator i = nodes.begin(); i != nodes.end(); i++)
 186     //  cerr << "FGTaxiRoute contains : " << *(i) << endl;
 187     //cerr << "Offset from end: " << nodes.end() - currNode << endl;
 188     //if (currNode != nodes.end())
 189     //  cerr << "true" << endl;
 190     //else
 191     //  cerr << "false" << endl;
 192     if (nodes.size() != (routes.size()) + 1) {
 193         SG_LOG(SG_GENERAL, SG_ALERT,
 194                "ALERT: Misconfigured TaxiRoute : " << nodes.
 195                size() << " " << routes.size());
 196         exit(1);
 197     }
 198     if (currNode == nodes.end())
 199         return false;
 200     *nde = *(currNode);
 201     //*rte = *(currRoute);
 202     if (currNode != nodes.begin())      // Make sure route corresponds to the end node
 203     {
 204         *rte = *(currRoute);
 205         currRoute++;
 206     } else {
 207         // If currNode points to the first node, this means the aircraft is not on the taxi node
 208         // yet. Make sure to return a unique identifyer in this situation though, because otherwise
 209         // the speed adjust AI code may be unable to resolve whether two aircraft are on the same
 210         // taxi route or not. the negative of the preceding route seems a logical choice, as it is
 211         // unique for any starting location.
 212         // Note that this is probably just a temporary fix until I get Parking / tower control working.
 213         *rte = -1 * *(currRoute);
 214     }
 215     currNode++;
 216     return true;
 217 };
 218
 219
 220 void FGTaxiRoute::rewind(int route)
 221 {
 222     int currPoint;
 223     int currRoute;
 224     first();
 225     do {
 226         if (!(next(&currPoint, &currRoute))) {
 227             SG_LOG(SG_GENERAL, SG_ALERT,
 228                    "Error in rewinding TaxiRoute: current" << currRoute <<
 229                    " goal " << route);
 230         }
 231     } while (currRoute != route);
 232 }
 233
 234
 235
 236
 237 /***************************************************************************
 238  * FGGroundNetwork()
 239  **************************************************************************/
 240 bool compare_nodes(FGTaxiNode * a, FGTaxiNode * b)
 241 {
 242     return (*a) < (*b);
 243 }
 244
 245 bool compare_segments(FGTaxiSegment * a, FGTaxiSegment * b)
 246 {
 247     return (*a) < (*b);
 248 }
 249
 250 bool compare_trafficrecords(FGTrafficRecord a, FGTrafficRecord b)
 251 {
 252     return (a.getIntentions().size() < b.getIntentions().size());
 253 }
 254
 255 FGGroundNetwork::FGGroundNetwork() :
 256   parent(NULL)
 257 {
 258     hasNetwork = false;
 259     foundRoute = false;
 260     totalDistance = 0;
 261     maxDistance = 0;
 262     //maxDepth    = 1000;
 263     count = 0;
 264     currTraffic = activeTraffic.begin();
 265     group = 0;
 266     version = 0;
 267     networkInitialized = false;
 268
 269 }
 270
 271 FGGroundNetwork::~FGGroundNetwork()
 272 {
 273 // JMT 2012-09-8 - disabling the groundnet-caching as part of enabling the
 274 // navcache. The problem isn't the NavCache - it's that for the past few years,
 275 // we have not being running destructors on FGPositioned classes, and hence,
 276 // not running this code.
 277 // When I fix FGPositioned lifetimes (unloading-at-runtime support), this
 278 // will need to be re-visited so it can run safely during shutdown.
 279 #if 0
 280     //cerr << "Running Groundnetwork Destructor " << endl;
 281     bool saveData = false;
 282     ofstream cachefile;
 283     if (fgGetBool("/sim/ai/groundnet-cache")) {
 284         SGPath cacheData(globals->get_fg_home());
 285         cacheData.append("ai");
 286         string airport = parent->getId();
 287
 288         if ((airport) != "") {
 289             char buffer[128];
 290             ::snprintf(buffer, 128, "%c/%c/%c/",
 291                        airport[0], airport[1], airport[2]);
 292             cacheData.append(buffer);
 293             if (!cacheData.exists()) {
 294                 cacheData.create_dir(0777);
 295             }
 296             cacheData.append(airport + "-groundnet-cache.txt");
 297             cachefile.open(cacheData.str().c_str());
 298             saveData = true;
 299         }
 300     }
 301     cachefile << "[GroundNetcachedata:ref:2011:09:04]" << endl;
 302     for (FGTaxiNodeVectorIterator node = nodes.begin();
 303             node != nodes.end(); node++) {
 304         if (saveData) {
 305             cachefile << (*node)->getIndex     () << " "
 306             << (*node)->getElevationM (parent->getElevation()*SG_FEET_TO_METER)   << " "
 307             << endl;
 308         }
 309         delete(*node);
 310     }
 311     nodes.clear();
 312     pushBackNodes.clear();
 313     for (FGTaxiSegmentVectorIterator seg = segments.begin();
 314             seg != segments.end(); seg++) {
 315         delete(*seg);
 316     }
 317     segments.clear();
 318     if (saveData) {
 319         cachefile.close();
 320     }
 321 #endif
 322 }
 323
 324 void FGGroundNetwork::saveElevationCache() {
 325     //cerr << "Running Groundnetwork Destructor " << endl;
 326     bool saveData = false;
 327     ofstream cachefile;
 328     if (fgGetBool("/sim/ai/groundnet-cache")) {
 329         SGPath cacheData(globals->get_fg_home());
 330         cacheData.append("ai");
 331         string airport = parent->getId();
 332
 333         if ((airport) != "") {
 334             char buffer[128];
 335             ::snprintf(buffer, 128, "%c/%c/%c/",
 336                        airport[0], airport[1], airport[2]);
 337             cacheData.append(buffer);
 338             if (!cacheData.exists()) {
 339                 cacheData.create_dir(0777);
 340             }
 341             cacheData.append(airport + "-groundnet-cache.txt");
 342             cachefile.open(cacheData.str().c_str());
 343             saveData = true;
 344         }
 345     }
 346     cachefile << "[GroundNetcachedata:ref:2011:09:04]" << endl;
 347     for (FGTaxiNodeVectorIterator node = nodes.begin();
 348             node != nodes.end(); node++) {
 349         if (saveData) {
 350             cachefile << (*node)->getIndex     () << " "
 351             << (*node)->getElevationM (parent->getElevation()*SG_FEET_TO_METER) << " "
 352             << endl;
 353         }
 354     }
 355     if (saveData) {
 356         cachefile.close();
 357     }
 358 }
 359
 360 void FGGroundNetwork::addSegment(const FGTaxiSegment & seg)
 361 {
 362     segments.push_back(new FGTaxiSegment(seg));
 363 }
 364
 365 void FGGroundNetwork::addNode(const FGTaxiNode & node)
 366 {
 367     nodes.push_back(new FGTaxiNode(node));
 368 }
 369
 370 void FGGroundNetwork::addNodes(FGParkingVec * parkings)
 371 {
 372     FGTaxiNode n;
 373     FGParkingVecIterator i = parkings->begin();
 374     while (i != parkings->end()) {
 375         n.setIndex(i->getIndex());
 376         n.setLatitude(i->getLatitude());
 377         n.setLongitude(i->getLongitude());
 378         n.setElevation(parent->getElevation()*SG_FEET_TO_METER);
 379         nodes.push_back(new FGTaxiNode(n));
 380
 381         i++;
 382     }
 383 }
 384
 385
 386
 387 void FGGroundNetwork::init()
 388 {
 389     if (networkInitialized) {
 390         FGATCController::init();
 391         //cerr << "FGground network already initialized" << endl;
 392         return;
 393     }
 394     hasNetwork = true;
 395     nextSave = 0;
 396     int index = 1;
 397     sort(nodes.begin(), nodes.end(), compare_nodes);
 398     //sort(segments.begin(), segments.end(), compare_segments());
 399     FGTaxiSegmentVectorIterator i = segments.begin();
 400     while (i != segments.end()) {
 401         (*i)->setStart(&nodes);
 402         (*i)->setEnd(&nodes);
 403         (*i)->setDimensions(parent->getElevation() * SG_FEET_TO_METER);
 404         (*i)->setIndex(index);
 405         if ((*i)->isPushBack()) {
 406             pushBackNodes.push_back((*i)->getEnd());
 407         }
 408         //SG_LOG(SG_GENERAL, SG_BULK,  "initializing segment " << (*i)->getIndex() << endl);
 409         //SG_LOG(SG_GENERAL, SG_BULK, "Track distance = "     << (*i)->getLength() << endl);
 410         //SG_LOG(SG_GENERAL, SG_BULK, "Track runs from "      << (*i)->getStart()->getIndex() << " to "
 411         //                                                    << (*i)->getEnd()->getIndex() << endl);
 412         i++;
 413         index++;
 414     }
 415
 416     i = segments.begin();
 417     while (i != segments.end()) {
 418         FGTaxiSegmentVectorIterator j = (*i)->getEnd()->getBeginRoute();
 419         while (j != (*i)->getEnd()->getEndRoute()) {
 420             if ((*j)->getEnd()->getIndex() == (*i)->getStart()->getIndex()) {
 421 //          int start1 = (*i)->getStart()->getIndex();
 422 //          int end1   = (*i)->getEnd()  ->getIndex();
 423 //          int start2 = (*j)->getStart()->getIndex();
 424 //          int end2   = (*j)->getEnd()->getIndex();
 425 //          int oppIndex = (*j)->getIndex();
 426                 //cerr << "Opposite of  " << (*i)->getIndex() << " (" << start1 << "," << end1 << ") "
 427                 //   << "happens to be " << oppIndex      << " (" << start2 << "," << end2 << ") " << endl;
 428                 (*i)->setOpposite(*j);
 429                 break;
 430             }
 431             j++;
 432         }
 433         i++;
 434     }
 435     //FGTaxiNodeVectorIterator j = nodes.begin();
 436     //while (j != nodes.end()) {
 437     //    if ((*j)->getHoldPointType() == 3) {
 438     //        pushBackNodes.push_back((*j));
 439     //    }
 440     //    j++;
 441     //}
 442     //cerr << "Done initializing ground network" << endl;
 443     //exit(1);
 444     if (fgGetBool("/sim/ai/groundnet-cache")) {
 445         SGPath cacheData(globals->get_fg_home());
 446         cacheData.append("ai");
 447         string airport = parent->getId();
 448
 449         if ((airport) != "") {
 450             char buffer[128];
 451             ::snprintf(buffer, 128, "%c/%c/%c/",
 452                        airport[0], airport[1], airport[2]);
 453             cacheData.append(buffer);
 454             if (!cacheData.exists()) {
 455                 cacheData.create_dir(0777);
 456             }
 457             int index;
 458             double elev;
 459             cacheData.append(airport + "-groundnet-cache.txt");
 460             if (cacheData.exists()) {
 461                 ifstream data(cacheData.c_str());
 462                 string revisionStr;
 463                 data >> revisionStr;
 464                 if (revisionStr != "[GroundNetcachedata:ref:2011:09:04]") {
 465                     SG_LOG(SG_GENERAL, SG_ALERT,"GroundNetwork Warning: discarding outdated cachefile " <<
 466                            cacheData.c_str() << " for Airport " << airport);
 467                 } else {
 468                     for (FGTaxiNodeVectorIterator i = nodes.begin();
 469                             i != nodes.end();
 470                             i++) {
 471                         (*i)->setElevation(parent->getElevation() * SG_FEET_TO_METER);
 472                         data >> index >> elev;
 473                         if (data.eof())
 474                             break;
 475                         if (index != (*i)->getIndex()) {
 476                             SG_LOG(SG_GENERAL, SG_ALERT, "Index read from ground network cache at airport " << airport << " does not match index in the network itself");
 477                         } else {
 478                             (*i)->setElevation(elev);
 479                         }
 480                     }
 481                 }
 482             }
 483         }
 484     }
 485     //cerr << "Finished initializing " << parent->getId() << " groundnetwork " << endl;
 486     networkInitialized = true;
 487 }
 488
 489 int FGGroundNetwork::findNearestNode(const SGGeod & aGeod)
 490 {
 491     double minDist = HUGE_VAL;
 492     int index = -1;
 493
 494     for (FGTaxiNodeVectorIterator itr = nodes.begin(); itr != nodes.end();
 495             itr++) {
 496         double d = SGGeodesy::distanceM(aGeod, (*itr)->getGeod());
 497         if (d < minDist) {
 498             minDist = d;
 499             index = (*itr)->getIndex();
 500             //cerr << "Minimum distance of " << minDist << " for index " << index << endl;
 501         }
 502     }
 503
 504     return index;
 505 }
 506
 507 int FGGroundNetwork::findNearestNodeOnRunway(const SGGeod & aGeod)
 508 {
 509     double minDist = HUGE_VAL;
 510     int index = -1;
 511
 512     for (FGTaxiNodeVectorIterator itr = nodes.begin(); itr != nodes.end();
 513             itr++) {
 514         if (!((*itr)->getIsOnRunway())) {
 515             continue;
 516         }
 517         double d = SGGeodesy::distanceM(aGeod, (*itr)->getGeod());
 518         if (d < minDist) {
 519             minDist = d;
 520             index = (*itr)->getIndex();
 521             //cerr << "Minimum distance of " << minDist << " for index " << index << endl;
 522         }
 523     }
 524
 525     return index;
 526 }
 527
 528
 529 int FGGroundNetwork::findNearestNode(double lat, double lon)
 530 {
 531     return findNearestNode(SGGeod::fromDeg(lon, lat));
 532 }
 533
 534 FGTaxiNode *FGGroundNetwork::findNode(unsigned idx)
 535 {                               /*
 536                                    for (FGTaxiNodeVectorIterator
 537                                    itr = nodes.begin();
 538                                    itr != nodes.end(); itr++)
 539                                    {
 540                                    if (itr->getIndex() == idx)
 541                                    return itr->getAddress();
 542                                    } */
 543
 544     if (idx < nodes.size())
 545         return nodes[idx]->getAddress();
 546     else
 547         return 0;
 548 }
 549
 550 FGTaxiSegment *FGGroundNetwork::findSegment(unsigned idx)
 551 {                               /*
 552                                    for (FGTaxiSegmentVectorIterator
 553                                    itr = segments.begin();
 554                                    itr != segments.end(); itr++)
 555                                    {
 556                                    if (itr->getIndex() == idx)
 557                                    return itr->getAddress();
 558                                    }
 559                                  */
 560     if ((idx > 0) && (idx <= segments.size()))
 561         return segments[idx - 1]->getAddress();
 562     else {
 563         //cerr << "Alert: trying to find invalid segment " << idx << endl;
 564         return 0;
 565     }
 566 }
 567
 568
 569 FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end,
 570         bool fullSearch)
 571 {
 572 //implements Dijkstra's algorithm to find shortest distance route from start to end
 573 //taken from http://en.wikipedia.org/wiki/Dijkstra's_algorithm
 574
 575     //double INFINITE = 100000000000.0;
 576     // initialize scoring values
 577     int nParkings = parent->getDynamics()->getNrOfParkings();
 578     FGTaxiNodeVector *currNodesSet;
 579     if (fullSearch) {
 580         currNodesSet = &nodes;
 581     } else {
 582         currNodesSet = &pushBackNodes;
 583     }
 584
 585     for (FGTaxiNodeVectorIterator
 586             itr = currNodesSet->begin(); itr != currNodesSet->end(); itr++) {
 587         (*itr)->setPathScore(HUGE_VAL); //infinity by all practical means
 588         (*itr)->setPreviousNode(0);     //
 589         (*itr)->setPreviousSeg(0);      //
 590     }
 591
 592     FGTaxiNode *firstNode = findNode(start);
 593     if (!firstNode)
 594     {
 595         SG_LOG(SG_GENERAL, SG_ALERT,
 596                "Error in ground network. Failed to find first waypoint: " << start
 597                << " at " << ((parent) ? parent->getId() : "<unknown>"));
 598         return FGTaxiRoute();
 599     }
 600     firstNode->setPathScore(0);
 601
 602     FGTaxiNode *lastNode = findNode(end);
 603     if (!lastNode)
 604     {
 605         SG_LOG(SG_GENERAL, SG_ALERT,
 606                "Error in ground network. Failed to find last waypoint: " << end
 607                << " at " << ((parent) ? parent->getId() : "<unknown>"));
 608         return FGTaxiRoute();
 609     }
 610
 611     FGTaxiNodeVector unvisited(*currNodesSet);  // working copy
 612
 613     while (!unvisited.empty()) {
 614         FGTaxiNode *best = *(unvisited.begin());
 615         for (FGTaxiNodeVectorIterator
 616                 itr = unvisited.begin(); itr != unvisited.end(); itr++) {
 617             if ((*itr)->getPathScore() < best->getPathScore())
 618                 best = (*itr);
 619         }
 620
 621         FGTaxiNodeVectorIterator newend =
 622             remove(unvisited.begin(), unvisited.end(), best);
 623         unvisited.erase(newend, unvisited.end());
 624
 625         if (best == lastNode) { // found route or best not connected
 626             break;
 627         } else {
 628             for (FGTaxiSegmentVectorIterator
 629                     seg = best->getBeginRoute();
 630                     seg != best->getEndRoute(); seg++) {
 631                 if (fullSearch || (*seg)->isPushBack()) {
 632                     FGTaxiNode *tgt = (*seg)->getEnd();
 633                     if (!tgt)
 634                     {
 635                         SG_LOG(SG_GENERAL, SG_ALERT,
 636                                "Error in ground network. Found empty segment "
 637                                << " at " << ((parent) ? parent->getId() : "<unknown>"));
 638                         return FGTaxiRoute();
 639                     }
 640                     double alt =
 641                         best->getPathScore() + (*seg)->getLength() +
 642                         (*seg)->getPenalty(nParkings);
 643                     if (alt < tgt->getPathScore()) {    // Relax (u,v)
 644                         tgt->setPathScore(alt);
 645                         tgt->setPreviousNode(best);
 646                         tgt->setPreviousSeg(*seg);      //
 647                     }
 648                 } else {
 649                     //   // cerr << "Skipping TaxiSegment " << (*seg)->getIndex() << endl;
 650                 }
 651             }
 652         }
 653     }
 654
 655     if (lastNode->getPathScore() == HUGE_VAL) {
 656         // no valid route found
 657         if (fullSearch) {
 658             SG_LOG(SG_GENERAL, SG_ALERT,
 659                    "Failed to find route from waypoint " << start << " to "
 660                    << end << " at " << parent->getId());
 661         }
 662         FGTaxiRoute empty;
 663         return empty;
 664         //exit(1); //TODO exit more gracefully, no need to stall the whole sim with broken GN's
 665     } else {
 666         // assemble route from backtrace information
 667         intVec nodes, routes;
 668         FGTaxiNode *bt = lastNode;
 669         while (bt->getPreviousNode() != 0) {
 670             nodes.push_back(bt->getIndex());
 671             routes.push_back(bt->getPreviousSegment()->getIndex());
 672             bt = bt->getPreviousNode();
 673         }
 674         nodes.push_back(start);
 675         reverse(nodes.begin(), nodes.end());
 676         reverse(routes.begin(), routes.end());
 677
 678         return FGTaxiRoute(nodes, routes, lastNode->getPathScore(), 0);
 679     }
 680 }
 681
 682 int FGTaxiSegment::getPenalty(int nGates)
 683 {
 684     int penalty = 0;
 685     if (end->getIndex() < nGates) {
 686         penalty += 10000;
 687     }
 688     if (end->getIsOnRunway()) { // For now. In future versions, need to find out whether runway is active.
 689         penalty += 1000;
 690     }
 691     return penalty;
 692 }
 693
 694 /* ATC Related Functions */
 695
 696 void FGGroundNetwork::announcePosition(int id,
 697                                        FGAIFlightPlan * intendedRoute,
 698                                        int currentPosition, double lat,
 699                                        double lon, double heading,
 700                                        double speed, double alt,
 701                                        double radius, int leg,
 702                                        FGAIAircraft * aircraft)
 703 {
 704     init();
 705     TrafficVectorIterator i = activeTraffic.begin();
 706     // Search search if the current id alread has an entry
 707     // This might be faster using a map instead of a vector, but let's start by taking a safe route
 708     if (activeTraffic.size()) {
 709         //while ((i->getId() != id) && i != activeTraffic.end()) {
 710         while (i != activeTraffic.end()) {
 711             if (i->getId() == id) {
 712                 break;
 713             }
 714             i++;
 715         }
 716     }
 717     // Add a new TrafficRecord if no one exsists for this aircraft.
 718     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 719         FGTrafficRecord rec;
 720         rec.setId(id);
 721         rec.setLeg(leg);
 722         rec.setPositionAndIntentions(currentPosition, intendedRoute);
 723         rec.setPositionAndHeading(lat, lon, heading, speed, alt);
 724         rec.setRadius(radius);  // only need to do this when creating the record.
 725         rec.setAircraft(aircraft);
 726         if (leg == 2) {
 727             activeTraffic.push_front(rec);
 728         } else {
 729             activeTraffic.push_back(rec);
 730         }
 731
 732     } else {
 733         i->setPositionAndIntentions(currentPosition, intendedRoute);
 734         i->setPositionAndHeading(lat, lon, heading, speed, alt);
 735     }
 736 }
 737
 738
 739 void FGGroundNetwork::signOff(int id)
 740 {
 741     TrafficVectorIterator i = activeTraffic.begin();
 742     // Search search if the current id alread has an entry
 743     // This might be faster using a map instead of a vector, but let's start by taking a safe route
 744     if (activeTraffic.size()) {
 745         //while ((i->getId() != id) && i != activeTraffic.end()) {
 746         while (i != activeTraffic.end()) {
 747             if (i->getId() == id) {
 748                 break;
 749             }
 750             i++;
 751         }
 752     }
 753     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 754         SG_LOG(SG_GENERAL, SG_ALERT,
 755                "AI error: Aircraft without traffic record is signing off at " << SG_ORIGIN);
 756     } else {
 757         i = activeTraffic.erase(i);
 758     }
 759 }
 760 /**
 761  * The ground network can deal with the following states:
 762  * 0 =  Normal; no action required
 763  * 1 = "Acknowledge "Hold position
 764  * 2 = "Acknowledge "Resume taxi".
 765  * 3 = "Issue TaxiClearance"
 766  * 4 = Acknowledge Taxi Clearance"
 767  * 5 = Post acknowlegde taxiclearance: Start taxiing
 768  * 6 = Report runway
 769  * 7 = Acknowledge report runway
 770  * 8 = Switch tower frequency
 771  * 9 = Acknowledge switch tower frequency
 772  *************************************************************************************************************************/
 773 bool FGGroundNetwork::checkTransmissionState(int minState, int maxState, TrafficVectorIterator i, time_t now, AtcMsgId msgId,
 774         AtcMsgDir msgDir)
 775 {
 776     int state = i->getState();
 777     if ((state >= minState) && (state <= maxState) && available) {
 778         if ((msgDir == ATC_AIR_TO_GROUND) && isUserAircraft(i->getAircraft())) {
 779             //cerr << "Checking state " << state << " for " << i->getAircraft()->getCallSign() << endl;
 780             static SGPropertyNode_ptr trans_num = globals->get_props()->getNode("/sim/atc/transmission-num", true);
 781             int n = trans_num->getIntValue();
 782             if (n == 0) {
 783                 trans_num->setIntValue(-1);
 784                 // PopupCallback(n);
 785                 //cerr << "Selected transmission message " << n << endl;
 786                 //FGATCManager *atc = (FGATCManager*) globals->get_subsystem("atc");
 787                 FGATCDialogNew::instance()->removeEntry(1);
 788             } else {
 789                 //cerr << "creating message for " << i->getAircraft()->getCallSign() << endl;
 790                 transmit(&(*i), &(*parent->getDynamics()), msgId, msgDir, false);
 791                 return false;
 792             }
 793         }
 794         transmit(&(*i), &(*parent->getDynamics()), msgId, msgDir, true);
 795         i->updateState();
 796         lastTransmission = now;
 797         available = false;
 798         return true;
 799     }
 800     return false;
 801 }
 802
 803 void FGGroundNetwork::updateAircraftInformation(int id, double lat, double lon,
 804         double heading, double speed, double alt,
 805         double dt)
 806 {
 807     time_t currentTime = time(NULL);
 808     if (nextSave < currentTime) {
 809         saveElevationCache();
 810         nextSave = currentTime + 100 + rand() % 200;
 811     }
 812     // Check whether aircraft are on hold due to a preceding pushback. If so, make sure to
 813     // Transmit air-to-ground "Ready to taxi request:
 814     // Transmit ground to air approval / hold
 815     // Transmit confirmation ...
 816     // Probably use a status mechanism similar to the Engine start procedure in the startup controller.
 817
 818
 819     TrafficVectorIterator i = activeTraffic.begin();
 820     // Search search if the current id has an entry
 821     // This might be faster using a map instead of a vector, but let's start by taking a safe route
 822     TrafficVectorIterator current, closest;
 823     if (activeTraffic.size()) {
 824         //while ((i->getId() != id) && i != activeTraffic.end()) {
 825         while (i != activeTraffic.end()) {
 826             if (i->getId() == id) {
 827                 break;
 828             }
 829             i++;
 830         }
 831     }
 832     // update position of the current aircraft
 833     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 834         SG_LOG(SG_GENERAL, SG_ALERT,
 835                "AI error: updating aircraft without traffic record at " << SG_ORIGIN);
 836     } else {
 837         i->setPositionAndHeading(lat, lon, heading, speed, alt);
 838         current = i;
 839     }
 840
 841     setDt(getDt() + dt);
 842
 843     // Update every three secs, but add some randomness
 844     // to prevent all IA objects doing this in synchrony
 845     //if (getDt() < (3.0) + (rand() % 10))
 846     //  return;
 847     //else
 848     //  setDt(0);
 849     current->clearResolveCircularWait();
 850     current->setWaitsForId(0);
 851     checkSpeedAdjustment(id, lat, lon, heading, speed, alt);
 852     bool needsTaxiClearance = current->getAircraft()->getTaxiClearanceRequest();
 853     if (!needsTaxiClearance) {
 854         checkHoldPosition(id, lat, lon, heading, speed, alt);
 855         //if (checkForCircularWaits(id)) {
 856         //    i->setResolveCircularWait();
 857         //}
 858     } else {
 859         current->setHoldPosition(true);
 860         int state = current->getState();
 861         time_t now = time(NULL) + fgGetLong("/sim/time/warp");
 862         if ((now - lastTransmission) > 15) {
 863             available = true;
 864         }
 865         if (checkTransmissionState(0,2, current, now, MSG_REQUEST_TAXI_CLEARANCE, ATC_AIR_TO_GROUND)) {
 866             current->setState(3);
 867         }
 868         if (checkTransmissionState(3,3, current, now, MSG_ISSUE_TAXI_CLEARANCE, ATC_GROUND_TO_AIR)) {
 869             current->setState(4);
 870         }
 871         if (checkTransmissionState(4,4, current, now, MSG_ACKNOWLEDGE_TAXI_CLEARANCE, ATC_AIR_TO_GROUND)) {
 872             current->setState(5);
 873         }
 874         if ((state == 5) && available) {
 875             current->setState(0);
 876             current->getAircraft()->setTaxiClearanceRequest(false);
 877             current->setHoldPosition(false);
 878             available = false;
 879         }
 880
 881     }
 882 }
 883
 884 /**
 885    Scan for a speed adjustment change. Find the nearest aircraft that is in front
 886    and adjust speed when we get too close. Only do this when current position and/or
 887    intentions of the current aircraft match current taxiroute position of the proximate
 888    aircraft. For traffic that is on other routes we need to issue a "HOLD Position"
 889    instruction. See below for the hold position instruction.
 890
 891    Note that there currently still is one flaw in the logic that needs to be addressed.
 892    There can be situations where one aircraft is in front of the current aircraft, on a separate
 893    route, but really close after an intersection coming off the current route. This
 894    aircraft is still close enough to block the current aircraft. This situation is currently
 895    not addressed yet, but should be.
 896 */
 897
 898 void FGGroundNetwork::checkSpeedAdjustment(int id, double lat,
 899         double lon, double heading,
 900         double speed, double alt)
 901 {
 902
 903     TrafficVectorIterator current, closest, closestOnNetwork;
 904     TrafficVectorIterator i = activeTraffic.begin();
 905     bool otherReasonToSlowDown = false;
 906 //    bool previousInstruction;
 907     if (activeTraffic.size()) {
 908         //while ((i->getId() != id) && (i != activeTraffic.end()))
 909         while (i != activeTraffic.end()) {
 910             if (i->getId() == id) {
 911                 break;
 912             }
 913             i++;
 914         }
 915     } else {
 916         return;
 917     }
 918     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 919         SG_LOG(SG_GENERAL, SG_ALERT,
 920                "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkSpeedAdjustment at " << SG_ORIGIN);
 921     }
 922     current = i;
 923     //closest = current;
 924
 925 //    previousInstruction = current->getSpeedAdjustment();
 926     double mindist = HUGE_VAL;
 927     if (activeTraffic.size()) {
 928         double course, dist, bearing, az2; // minbearing,
 929         SGGeod curr(SGGeod::fromDegM(lon, lat, alt));
 930         //TrafficVector iterator closest;
 931         closest = current;
 932         closestOnNetwork = current;
 933         for (TrafficVectorIterator i = activeTraffic.begin();
 934                 i != activeTraffic.end(); i++) {
 935             if (i == current) {
 936                 continue;
 937             }
 938
 939             SGGeod other(SGGeod::fromDegM(i->getLongitude(),
 940                                           i->getLatitude(),
 941                                           i->getAltitude()));
 942             SGGeodesy::inverse(curr, other, course, az2, dist);
 943             bearing = fabs(heading - course);
 944             if (bearing > 180)
 945                 bearing = 360 - bearing;
 946             if ((dist < mindist) && (bearing < 60.0)) {
 947                 mindist = dist;
 948                 closest = i;
 949                 closestOnNetwork = i;
 950 //                minbearing = bearing;
 951
 952             }
 953         }
 954         //Check traffic at the tower controller
 955         if (towerController->hasActiveTraffic()) {
 956             for (TrafficVectorIterator i =
 957                         towerController->getActiveTraffic().begin();
 958                     i != towerController->getActiveTraffic().end(); i++) {
 959                 //cerr << "Comparing " << current->getId() << " and " << i->getId() << endl;
 960                 SGGeod other(SGGeod::fromDegM(i->getLongitude(),
 961                                               i->getLatitude(),
 962                                               i->getAltitude()));
 963                 SGGeodesy::inverse(curr, other, course, az2, dist);
 964                 bearing = fabs(heading - course);
 965                 if (bearing > 180)
 966                     bearing = 360 - bearing;
 967                 if ((dist < mindist) && (bearing < 60.0)) {
 968                     //cerr << "Current aircraft " << current->getAircraft()->getTrafficRef()->getCallSign()
 969                     //     << " is closest to " << i->getAircraft()->getTrafficRef()->getCallSign()
 970                     //     << ", which has status " << i->getAircraft()->isScheduledForTakeoff()
 971                     //     << endl;
 972                     mindist = dist;
 973                     closest = i;
 974 //                    minbearing = bearing;
 975                     otherReasonToSlowDown = true;
 976                 }
 977             }
 978         }
 979         // Finally, check UserPosition
 980         // Note, as of 2011-08-01, this should no longer be necessecary.
 981         /*
 982         double userLatitude = fgGetDouble("/position/latitude-deg");
 983         double userLongitude = fgGetDouble("/position/longitude-deg");
 984         SGGeod user(SGGeod::fromDeg(userLongitude, userLatitude));
 985         SGGeodesy::inverse(curr, user, course, az2, dist);
 986
 987         bearing = fabs(heading - course);
 988         if (bearing > 180)
 989             bearing = 360 - bearing;
 990         if ((dist < mindist) && (bearing < 60.0)) {
 991             mindist = dist;
 992             //closest = i;
 993             minbearing = bearing;
 994             otherReasonToSlowDown = true;
 995         }
 996         */
 997         current->clearSpeedAdjustment();
 998         bool needBraking = false;
 999         if (current->checkPositionAndIntentions(*closest)
1000                 || otherReasonToSlowDown) {
1001             double maxAllowableDistance =
1002                 (1.1 * current->getRadius()) +
1003                 (1.1 * closest->getRadius());
1004             if (mindist < 2 * maxAllowableDistance) {
1005                 if (current->getId() == closest->getWaitsForId())
1006                     return;
1007                 else
1008                     current->setWaitsForId(closest->getId());
1009                 if (closest->getId() != current->getId()) {
1010                     current->setSpeedAdjustment(closest->getSpeed() *
1011                                                 (mindist / 100));
1012                     needBraking = true;
1013
1014 //                     if (
1015 //                         closest->getAircraft()->getTakeOffStatus() &&
1016 //                         (current->getAircraft()->getTrafficRef()->getDepartureAirport() ==  closest->getAircraft()->getTrafficRef()->getDepartureAirport()) &&
1017 //                         (current->getAircraft()->GetFlightPlan()->getRunway() == closest->getAircraft()->GetFlightPlan()->getRunway())
1018 //                     )
1019 //                         current->getAircraft()->scheduleForATCTowerDepartureControl(1);
1020                 } else {
1021                     current->setSpeedAdjustment(0);     // This can only happen when the user aircraft is the one closest
1022                 }
1023                 if (mindist < maxAllowableDistance) {
1024                     //double newSpeed = (maxAllowableDistance-mindist);
1025                     //current->setSpeedAdjustment(newSpeed);
1026                     //if (mindist < 0.5* maxAllowableDistance)
1027                     //  {
1028                     current->setSpeedAdjustment(0);
1029                     //  }
1030                 }
1031             }
1032         }
1033         if ((closest->getId() == closestOnNetwork->getId()) && (current->getPriority() < closest->getPriority()) && needBraking) {
1034             swap(current, closest);
1035         }
1036     }
1037 }
1038
1039 /**
1040    Check for "Hold position instruction".
1041    The hold position should be issued under the following conditions:
1042    1) For aircraft entering or crossing a runway with active traffic on it, or landing aircraft near it
1043    2) For taxiing aircraft that use one taxiway in opposite directions
1044    3) For crossing or merging taxiroutes.
1045 */
1046
1047 void FGGroundNetwork::checkHoldPosition(int id, double lat,
1048                                         double lon, double heading,
1049                                         double speed, double alt)
1050 {
1051     TrafficVectorIterator current;
1052     TrafficVectorIterator i = activeTraffic.begin();
1053     if (activeTraffic.size()) {
1054         //while ((i->getId() != id) && i != activeTraffic.end())
1055         while (i != activeTraffic.end()) {
1056             if (i->getId() == id) {
1057                 break;
1058             }
1059             i++;
1060         }
1061     } else {
1062         return;
1063     }
1064     time_t now = time(NULL) + fgGetLong("/sim/time/warp");
1065     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
1066         SG_LOG(SG_GENERAL, SG_ALERT,
1067                "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkHoldPosition at " << SG_ORIGIN);
1068     }
1069     current = i;
1070     //
1071     if (current->getAircraft()->getTakeOffStatus() == 1) {
1072         current->setHoldPosition(true);
1073         return;
1074     }
1075     if (current->getAircraft()->getTakeOffStatus() == 2) {
1076         //cerr << current->getAircraft()->getCallSign() << ". Taxi in position and hold" << endl;
1077         current->setHoldPosition(false);
1078         current->clearSpeedAdjustment();
1079         return;
1080     }
1081     bool origStatus = current->hasHoldPosition();
1082     current->setHoldPosition(false);
1083     SGGeod curr(SGGeod::fromDegM(lon, lat, alt));
1084     int currentRoute = i->getCurrentPosition();
1085     int nextRoute;
1086     if (i->getIntentions().size()) {
1087         nextRoute    = (*(i->getIntentions().begin()));
1088     } else {
1089         nextRoute = 0;
1090     }
1091     if (currentRoute > 0) {
1092         FGTaxiSegment *tx = findSegment(currentRoute);
1093         FGTaxiSegment *nx;
1094         if (nextRoute) {
1095             nx = findSegment(nextRoute);
1096         } else {
1097             nx = tx;
1098         }
1099         //if (tx->hasBlock(now) || nx->hasBlock(now) ) {
1100         //   current->setHoldPosition(true);
1101         //}
1102         SGGeod start(SGGeod::fromDeg((i->getLongitude()), (i->getLatitude())));
1103         SGGeod end  (SGGeod::fromDeg(nx->getStart()->getLongitude(), nx->getStart()->getLatitude()));
1104
1105         double distance = SGGeodesy::distanceM(start, end);
1106         if (nx->hasBlock(now) && (distance < i->getRadius() * 4)) {
1107             current->setHoldPosition(true);
1108         } else {
1109             intVecIterator ivi = i->getIntentions().begin();
1110             while (ivi != i->getIntentions().end()) {
1111                 if ((*ivi) > 0) {
1112                     distance += segments[(*ivi)-1]->getLength();
1113                     if ((segments[(*ivi)-1]->hasBlock(now)) && (distance < i->getRadius() * 4)) {
1114                         current->setHoldPosition(true);
1115                         break;
1116                     }
1117                 }
1118                 ivi++;
1119             }
1120         }
1121     }
1122     bool currStatus = current->hasHoldPosition();
1123     current->setHoldPosition(origStatus);
1124     // Either a Hold Position or a resume taxi transmission has been issued
1125     if ((now - lastTransmission) > 2) {
1126         available = true;
1127     }
1128     if (current->getState() == 0) {
1129         if ((origStatus != currStatus) && available) {
1130             //cerr << "Issueing hold short instrudtion " << currStatus << " " << available << endl;
1131             if (currStatus == true) { // No has a hold short instruction
1132                 transmit(&(*current), &(*parent->getDynamics()), MSG_HOLD_POSITION, ATC_GROUND_TO_AIR, true);
1133                 //cerr << "Transmittin hold short instrudtion " << currStatus << " " << available << endl;
1134                 current->setState(1);
1135             } else {
1136                 transmit(&(*current), &(*parent->getDynamics()), MSG_RESUME_TAXI, ATC_GROUND_TO_AIR, true);
1137                 //cerr << "Transmittig resume instrudtion " << currStatus << " " << available << endl;
1138                 current->setState(2);
1139             }
1140             lastTransmission = now;
1141             available = false;
1142             // Don't act on the changed instruction until the transmission is confirmed
1143             // So set back to original status
1144             //cerr << "Current state " << current->getState() << endl;
1145         }
1146
1147     }
1148     // 6 = Report runway
1149     // 7 = Acknowledge report runway
1150     // 8 = Switch tower frequency
1151     //9 = Acknowledge switch tower frequency
1152
1153     //int state = current->getState();
1154     if (checkTransmissionState(1,1, current, now, MSG_ACKNOWLEDGE_HOLD_POSITION, ATC_AIR_TO_GROUND)) {
1155         current->setState(0);
1156         current->setHoldPosition(true);
1157     }
1158     if (checkTransmissionState(2,2, current, now, MSG_ACKNOWLEDGE_RESUME_TAXI, ATC_AIR_TO_GROUND)) {
1159         current->setState(0);
1160         current->setHoldPosition(false);
1161     }
1162     if (current->getAircraft()->getTakeOffStatus() && (current->getState() == 0)) {
1163         //cerr << "Scheduling " << current->getAircraft()->getCallSign() << " for hold short" << endl;
1164         current->setState(6);
1165     }
1166     if (checkTransmissionState(6,6, current, now, MSG_REPORT_RUNWAY_HOLD_SHORT, ATC_AIR_TO_GROUND)) {
1167     }
1168     if (checkTransmissionState(7,7, current, now, MSG_ACKNOWLEDGE_REPORT_RUNWAY_HOLD_SHORT, ATC_GROUND_TO_AIR)) {
1169     }
1170     if (checkTransmissionState(8,8, current, now, MSG_SWITCH_TOWER_FREQUENCY, ATC_GROUND_TO_AIR)) {
1171     }
1172     if (checkTransmissionState(9,9, current, now, MSG_ACKNOWLEDGE_SWITCH_TOWER_FREQUENCY, ATC_AIR_TO_GROUND)) {
1173     }
1174
1175
1176
1177     //current->setState(0);
1178 }
1179
1180 /**
1181  * Check whether situations occur where the current aircraft is waiting for itself
1182  * due to higher order interactions.
1183  * A 'circular' wait is a situation where a waits for b, b waits for c, and c waits
1184  * for a. Ideally each aircraft only waits for one other aircraft, so by tracing
1185  * through this list of waiting aircraft, we can check if we'd eventually end back
1186  * at the current aircraft.
1187  *
1188  * Note that we should consider the situation where we are actually checking aircraft
1189  * d, which is waiting for aircraft a. d is not part of the loop, but is held back by
1190  * the looping aircraft. If we don't check for that, this function will get stuck into
1191  * endless loop.
1192  */
1193
1194 bool FGGroundNetwork::checkForCircularWaits(int id)
1195 {
1196     //cerr << "Performing Wait check " << id << endl;
1197     int target = 0;
1198     TrafficVectorIterator current, other;
1199     TrafficVectorIterator i = activeTraffic.begin();
1200     int trafficSize = activeTraffic.size();
1201     if (trafficSize) {
1202         while (i != activeTraffic.end()) {
1203             if (i->getId() == id) {
1204                 break;
1205             }
1206             i++;
1207         }
1208     } else {
1209         return false;
1210     }
1211     if (i == activeTraffic.end() || (trafficSize == 0)) {
1212         SG_LOG(SG_GENERAL, SG_ALERT,
1213                "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits at " << SG_ORIGIN);
1214     }
1215
1216     current = i;
1217     target = current->getWaitsForId();
1218     //bool printed = false; // Note that this variable is for debugging purposes only.
1219     int counter = 0;
1220
1221     if (id == target) {
1222         //cerr << "aircraft waits for user" << endl;
1223         return false;
1224     }
1225
1226
1227     while ((target > 0) && (target != id) && counter++ < trafficSize) {
1228         //printed = true;
1229         TrafficVectorIterator i = activeTraffic.begin();
1230         if (trafficSize) {
1231             //while ((i->getId() != id) && i != activeTraffic.end())
1232             while (i != activeTraffic.end()) {
1233                 if (i->getId() == target) {
1234                     break;
1235                 }
1236                 i++;
1237             }
1238         } else {
1239             return false;
1240         }
1241         if (i == activeTraffic.end() || (trafficSize == 0)) {
1242             //cerr << "[Waiting for traffic at Runway: DONE] " << endl << endl;;
1243             // The target id is not found on the current network, which means it's at the tower
1244             //SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits");
1245             return false;
1246         }
1247         other = i;
1248         target = other->getWaitsForId();
1249
1250         // actually this trap isn't as impossible as it first seemed:
1251         // the setWaitsForID(id) is set to current when the aircraft
1252         // is waiting for the user controlled aircraft.
1253         //if (current->getId() == other->getId()) {
1254         //    cerr << "Caught the impossible trap" << endl;
1255         //    cerr << "Current = " << current->getId() << endl;
1256         //    cerr << "Other   = " << other  ->getId() << endl;
1257         //    for (TrafficVectorIterator at = activeTraffic.begin();
1258         //          at != activeTraffic.end();
1259         //          at++) {
1260         //        cerr << "currently active aircraft : " << at->getCallSign() << " with Id " << at->getId() << " waits for " << at->getWaitsForId() << endl;
1261         //    }
1262         //    exit(1);
1263         if (current->getId() == other->getId())
1264             return false;
1265         //}
1266         //cerr << current->getCallSign() << " (" << current->getId()  << ") " << " -> " << other->getCallSign()
1267         //     << " (" << other->getId()  << "); " << endl;;
1268         //current = other;
1269     }
1270
1271
1272
1273
1274
1275
1276     //if (printed)
1277     //   cerr << "[done] " << endl << endl;;
1278     if (id == target) {
1279         SG_LOG(SG_GENERAL, SG_WARN,
1280                "Detected circular wait condition: Id = " << id <<
1281                "target = " << target);
1282         return true;
1283     } else {
1284         return false;
1285     }
1286 }
1287
1288 // Note that this function is probably obsolete...
1289 bool FGGroundNetwork::hasInstruction(int id)
1290 {
1291     TrafficVectorIterator i = activeTraffic.begin();
1292     // Search search if the current id has an entry
1293     // This might be faster using a map instead of a vector, but let's start by taking a safe route
1294     if (activeTraffic.size()) {
1295         //while ((i->getId() != id) && i != activeTraffic.end()) {
1296         while (i != activeTraffic.end()) {
1297             if (i->getId() == id) {
1298                 break;
1299             }
1300             i++;
1301         }
1302     }
1303     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
1304         SG_LOG(SG_GENERAL, SG_ALERT,
1305                "AI error: checking ATC instruction for aircraft without traffic record at " << SG_ORIGIN);
1306     } else {
1307         return i->hasInstruction();
1308     }
1309     return false;
1310 }
1311
1312 FGATCInstruction FGGroundNetwork::getInstruction(int id)
1313 {
1314     TrafficVectorIterator i = activeTraffic.begin();
1315     // Search search if the current id has an entry
1316     // This might be faster using a map instead of a vector, but let's start by taking a safe route
1317     if (activeTraffic.size()) {
1318         //while ((i->getId() != id) && i != activeTraffic.end()) {
1319         while (i != activeTraffic.end()) {
1320             if (i->getId() == id) {
1321                 break;
1322             }
1323             i++;
1324         }
1325     }
1326     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
1327         SG_LOG(SG_GENERAL, SG_ALERT,
1328                "AI error: requesting ATC instruction for aircraft without traffic record at " << SG_ORIGIN);
1329     } else {
1330         return i->getInstruction();
1331     }
1332     return FGATCInstruction();
1333 }
1334
1335 // Note that this function is copied from simgear. for maintanance purposes, it's probabtl better to make a general function out of that.
1336 static void WorldCoordinate(osg::Matrix& obj_pos, double lat,
1337                             double lon, double elev, double hdg, double slope)
1338 {
1339     SGGeod geod = SGGeod::fromDegM(lon, lat, elev);
1340     obj_pos = makeZUpFrame(geod);
1341     // hdg is not a compass heading, but a counter-clockwise rotation
1342     // around the Z axis
1343     obj_pos.preMult(osg::Matrix::rotate(hdg * SGD_DEGREES_TO_RADIANS,
1344                                         0.0, 0.0, 1.0));
1345     obj_pos.preMult(osg::Matrix::rotate(slope * SGD_DEGREES_TO_RADIANS,
1346                                         0.0, 1.0, 0.0));
1347 }
1348
1349
1350
1351
1352 void FGGroundNetwork::render(bool visible)
1353 {
1354
1355     SGMaterialLib *matlib = globals->get_matlib();
1356     if (group) {
1357         //int nr = ;
1358         globals->get_scenery()->get_scene_graph()->removeChild(group);
1359         //while (group->getNumChildren()) {
1360         //  cerr << "Number of children: " << group->getNumChildren() << endl;
1361         //simgear::EffectGeode* geode = (simgear::EffectGeode*) group->getChild(0);
1362         //osg::MatrixTransform *obj_trans = (osg::MatrixTransform*) group->getChild(0);
1363         //geode->releaseGLObjects();
1364         //group->removeChild(geode);
1365         //delete geode;
1366         group = 0;
1367     }
1368     if (visible) {
1369         group = new osg::Group;
1370         FGScenery * local_scenery = globals->get_scenery();
1371         // double elevation_meters = 0.0;
1372 //        double elevation_feet = 0.0;
1373         time_t now = time(NULL) + fgGetLong("/sim/time/warp");
1374         //for ( FGTaxiSegmentVectorIterator i = segments.begin(); i != segments.end(); i++) {
1375         //double dx = 0;
1376         for   (TrafficVectorIterator i = activeTraffic.begin(); i != activeTraffic.end(); i++) {
1377             // Handle start point
1378             int pos = i->getCurrentPosition() - 1;
1379             if (pos >= 0) {
1380
1381                 SGGeod start(SGGeod::fromDeg((i->getLongitude()), (i->getLatitude())));
1382                 SGGeod end  (SGGeod::fromDeg(segments[pos]->getEnd()->getLongitude(), segments[pos]->getEnd()->getLatitude()));
1383
1384                 double length = SGGeodesy::distanceM(start, end);
1385                 //heading = SGGeodesy::headingDeg(start->getGeod(), end->getGeod());
1386
1387                 double az2, heading; //, distanceM;
1388                 SGGeodesy::inverse(start, end, heading, az2, length);
1389                 double coveredDistance = length * 0.5;
1390                 SGGeod center;
1391                 SGGeodesy::direct(start, heading, coveredDistance, center, az2);
1392                 //cerr << "Active Aircraft : Centerpoint = (" << center.getLatitudeDeg() << ", " << center.getLongitudeDeg() << "). Heading = " << heading << endl;
1393                 ///////////////////////////////////////////////////////////////////////////////
1394                 // Make a helper function out of this
1395                 osg::Matrix obj_pos;
1396                 osg::MatrixTransform *obj_trans = new osg::MatrixTransform;
1397                 obj_trans->setDataVariance(osg::Object::STATIC);
1398                 // Experimental: Calculate slope here, based on length, and the individual elevations
1399                 double elevationStart;
1400                 if (isUserAircraft((i)->getAircraft())) {
1401                     elevationStart = fgGetDouble("/position/ground-elev-m");
1402                 } else {
1403                     elevationStart = ((i)->getAircraft()->_getAltitude());
1404                 }
1405                 double elevationEnd   = segments[pos]->getEnd()->getElevationM(parent->getElevation()*SG_FEET_TO_METER);
1406                 //cerr << "Using elevation " << elevationEnd << endl;
1407
1408                 if ((elevationEnd == 0) || (elevationEnd = parent->getElevation())) {
1409                     SGGeod center2 = end;
1410                     center2.setElevationM(SG_MAX_ELEVATION_M);
1411                     if (local_scenery->get_elevation_m( center2, elevationEnd, NULL )) {
1412 //                        elevation_feet = elevationEnd * SG_METER_TO_FEET + 0.5;
1413                         //elevation_meters += 0.5;
1414                     }
1415                     else {
1416                         elevationEnd = parent->getElevation();
1417                     }
1418                     segments[pos]->getEnd()->setElevation(elevationEnd);
1419                 }
1420                 double elevationMean  = (elevationStart + elevationEnd) / 2.0;
1421                 double elevDiff       = elevationEnd - elevationStart;
1422
1423                 double slope = atan2(elevDiff, length) * SGD_RADIANS_TO_DEGREES;
1424
1425                 //cerr << "1. Using mean elevation : " << elevationMean << " and " << slope << endl;
1426
1427                 WorldCoordinate( obj_pos, center.getLatitudeDeg(), center.getLongitudeDeg(), elevationMean+ 0.5, -(heading), slope );
1428
1429                 obj_trans->setMatrix( obj_pos );
1430                 //osg::Vec3 center(0, 0, 0)
1431
1432                 float width = length /2.0;
1433                 osg::Vec3 corner(-width, 0, 0.25f);
1434                 osg::Vec3 widthVec(2*width + 1, 0, 0);
1435                 osg::Vec3 heightVec(0, 1, 0);
1436                 osg::Geometry* geometry;
1437                 geometry = osg::createTexturedQuadGeometry(corner, widthVec, heightVec);
1438                 simgear::EffectGeode* geode = new simgear::EffectGeode;
1439                 geode->setName("test");
1440                 geode->addDrawable(geometry);
1441                 //osg::Node *custom_obj;
1442                 SGMaterial *mat;
1443                 if (segments[pos]->hasBlock(now)) {
1444                     mat = matlib->find("UnidirectionalTaperRed");
1445                 } else {
1446                     mat = matlib->find("UnidirectionalTaperGreen");
1447                 }
1448                 if (mat)
1449                     geode->setEffect(mat->get_effect());
1450                 obj_trans->addChild(geode);
1451                 // wire as much of the scene graph together as we can
1452                 //->addChild( obj_trans );
1453                 group->addChild( obj_trans );
1454                 /////////////////////////////////////////////////////////////////////
1455             } else {
1456                 //cerr << "BIG FAT WARNING: current position is here : " << pos << endl;
1457             }
1458             for (intVecIterator j = (i)->getIntentions().begin(); j != (i)->getIntentions().end(); j++) {
1459                 osg::Matrix obj_pos;
1460                 int k = (*j)-1;
1461                 if (k >= 0) {
1462                     osg::MatrixTransform *obj_trans = new osg::MatrixTransform;
1463                     obj_trans->setDataVariance(osg::Object::STATIC);
1464
1465                     // Experimental: Calculate slope here, based on length, and the individual elevations
1466                     double elevationStart = segments[k]->getStart()->getElevationM(parent->getElevation()*SG_FEET_TO_METER);
1467                     double elevationEnd   = segments[k]->getEnd  ()->getElevationM(parent->getElevation()*SG_FEET_TO_METER);
1468                     if ((elevationStart == 0)  || (elevationStart == parent->getElevation())) {
1469                         SGGeod center2 = segments[k]->getStart()->getGeod();
1470                         center2.setElevationM(SG_MAX_ELEVATION_M);
1471                         if (local_scenery->get_elevation_m( center2, elevationStart, NULL )) {
1472 //                            elevation_feet = elevationStart * SG_METER_TO_FEET + 0.5;
1473                             //elevation_meters += 0.5;
1474                         }
1475                         else {
1476                             elevationStart = parent->getElevation();
1477                         }
1478                         segments[k]->getStart()->setElevation(elevationStart);
1479                     }
1480                     if ((elevationEnd == 0) || (elevationEnd == parent->getElevation())) {
1481                         SGGeod center2 = segments[k]->getEnd()->getGeod();
1482                         center2.setElevationM(SG_MAX_ELEVATION_M);
1483                         if (local_scenery->get_elevation_m( center2, elevationEnd, NULL )) {
1484 //                            elevation_feet = elevationEnd * SG_METER_TO_FEET + 0.5;
1485                             //elevation_meters += 0.5;
1486                         }
1487                         else {
1488                             elevationEnd = parent->getElevation();
1489                         }
1490                         segments[k]->getEnd()->setElevation(elevationEnd);
1491                     }
1492
1493                     double elevationMean  = (elevationStart + elevationEnd) / 2.0;
1494                     double elevDiff       = elevationEnd - elevationStart;
1495                     double length         = segments[k]->getLength();
1496                     double slope = atan2(elevDiff, length) * SGD_RADIANS_TO_DEGREES;
1497
1498                     // cerr << "2. Using mean elevation : " << elevationMean << " and " << slope << endl;
1499
1500
1501                     WorldCoordinate( obj_pos, segments[k]->getLatitude(), segments[k]->getLongitude(), elevationMean+ 0.5, -(segments[k]->getHeading()), slope );
1502
1503                     obj_trans->setMatrix( obj_pos );
1504                     //osg::Vec3 center(0, 0, 0)
1505
1506                     float width = segments[k]->getLength() /2.0;
1507                     osg::Vec3 corner(-width, 0, 0.25f);
1508                     osg::Vec3 widthVec(2*width + 1, 0, 0);
1509                     osg::Vec3 heightVec(0, 1, 0);
1510                     osg::Geometry* geometry;
1511                     geometry = osg::createTexturedQuadGeometry(corner, widthVec, heightVec);
1512                     simgear::EffectGeode* geode = new simgear::EffectGeode;
1513                     geode->setName("test");
1514                     geode->addDrawable(geometry);
1515                     //osg::Node *custom_obj;
1516                     SGMaterial *mat;
1517                     if (segments[k]->hasBlock(now)) {
1518                         mat = matlib->find("UnidirectionalTaperRed");
1519                     } else {
1520                         mat = matlib->find("UnidirectionalTaperGreen");
1521                     }
1522                     if (mat)
1523                         geode->setEffect(mat->get_effect());
1524                     obj_trans->addChild(geode);
1525                     // wire as much of the scene graph together as we can
1526                     //->addChild( obj_trans );
1527                     group->addChild( obj_trans );
1528                 }
1529             }
1530             //dx += 0.1;
1531         }
1532         globals->get_scenery()->get_scene_graph()->addChild(group);
1533     }
1534 }
1535
1536 string FGGroundNetwork::getName() {
1537     return string(parent->getId() + "-ground");
1538 }
1539
1540 void FGGroundNetwork::update(double dt)
1541 {
1542     time_t now = time(NULL) + fgGetLong("/sim/time/warp");
1543     for (FGTaxiSegmentVectorIterator tsi = segments.begin(); tsi != segments.end(); tsi++) {
1544         (*tsi)->unblock(now);
1545     }
1546     int priority = 1;
1547     //sort(activeTraffic.begin(), activeTraffic.end(), compare_trafficrecords);
1548     // Handle traffic that is under ground control first; this way we'll prevent clutter at the gate areas.
1549     // Don't allow an aircraft to pushback when a taxiing aircraft is currently using part of the intended route.
1550     for   (TrafficVectorIterator i = parent->getDynamics()->getStartupController()->getActiveTraffic().begin();
1551             i != parent->getDynamics()->getStartupController()->getActiveTraffic().end(); i++) {
1552         i->allowPushBack();
1553         i->setPriority(priority++);
1554         // in meters per second;
1555         double vTaxi = (i->getAircraft()->getPerformance()->vTaxi() * SG_NM_TO_METER) / 3600;
1556         if (i->isActive(0)) {
1557
1558             // Check for all active aircraft whether it's current pos segment is
1559             // an opposite of one of the departing aircraft's intentions
1560             for (TrafficVectorIterator j = activeTraffic.begin(); j != activeTraffic.end(); j++) {
1561                 int pos = j->getCurrentPosition();
1562                 if (pos > 0) {
1563                     FGTaxiSegment *seg = segments[pos-1]->opposite();
1564                     if (seg) {
1565                         int posReverse = seg->getIndex();
1566                         for (intVecIterator k = i->getIntentions().begin(); k != i->getIntentions().end(); k++) {
1567                             if ((*k) == posReverse) {
1568                                 i->denyPushBack();
1569                                 segments[posReverse-1]->block(i->getId(), now, now);
1570                             }
1571                         }
1572                     }
1573                 }
1574             }
1575             // if the current aircraft is still allowed to pushback, we can start reserving a route for if by blocking all the entry taxiways.
1576             if (i->pushBackAllowed()) {
1577                 double length = 0;
1578                 int pos = i->getCurrentPosition();
1579                 if (pos > 0) {
1580                     FGTaxiSegment *seg = segments[pos-1];
1581                     FGTaxiNode *node = seg->getEnd();
1582                     length = seg->getLength();
1583                     for (FGTaxiSegmentVectorIterator tsi = segments.begin(); tsi != segments.end(); tsi++) {
1584                         if (((*tsi)->getEnd() == node) && ((*tsi) != seg)) {
1585                             (*tsi)->block(i->getId(), now, now);
1586                         }
1587                     }
1588                 }
1589                 for (intVecIterator j = i->getIntentions().begin(); j != i->getIntentions().end(); j++) {
1590                     int pos = (*j);
1591                     if (pos > 0) {
1592                         FGTaxiSegment *seg = segments[pos-1];
1593                         FGTaxiNode *node = seg->getEnd();
1594                         length += seg->getLength();
1595                         time_t blockTime = now + (length / vTaxi);
1596                         for (FGTaxiSegmentVectorIterator tsi = segments.begin(); tsi != segments.end(); tsi++) {
1597                             if (((*tsi)->getEnd() == node) && ((*tsi) != seg)) {
1598                                 (*tsi)->block(i->getId(), blockTime-30, now);
1599                             }
1600                         }
1601                     }
1602                 }
1603             }
1604         }
1605     }
1606     for   (TrafficVectorIterator i = activeTraffic.begin(); i != activeTraffic.end(); i++) {
1607         double length = 0;
1608         double vTaxi = (i->getAircraft()->getPerformance()->vTaxi() * SG_NM_TO_METER) / 3600;
1609         i->setPriority(priority++);
1610         int pos = i->getCurrentPosition();
1611         if (pos > 0) {
1612             length = segments[pos-1]->getLength();
1613             if (segments[pos-1]->hasBlock(now)) {
1614                 //SG_LOG(SG_GENERAL, SG_ALERT, "Taxiway incursion for AI aircraft" << i->getAircraft()->getCallSign());
1615             }
1616
1617         }
1618         intVecIterator ivi;
1619         for (ivi = i->getIntentions().begin(); ivi != i->getIntentions().end(); ivi++) {
1620             int segIndex = (*ivi);
1621             if (segIndex > 0) {
1622                 if (segments[segIndex-1]->hasBlock(now))
1623                     break;
1624             }
1625         }
1626         //after this, ivi points just behind the last valid unblocked taxi segment.
1627         for (intVecIterator j = i->getIntentions().begin(); j != ivi; j++) {
1628             int pos = (*j);
1629             if (pos > 0) {
1630                 FGTaxiSegment *seg = segments[pos-1];
1631                 FGTaxiNode *node = seg->getEnd();
1632                 length += seg->getLength();
1633                 for (FGTaxiSegmentVectorIterator tsi = segments.begin(); tsi != segments.end(); tsi++) {
1634                     if (((*tsi)->getEnd() == node) && ((*tsi) != seg)) {
1635                         time_t blockTime = now + (length / vTaxi);
1636                         (*tsi)->block(i->getId(), blockTime - 30, now);
1637                     }
1638                 }
1639             }
1640         }
1641     }
1642 }
1643