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