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