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