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