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