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