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