src/Airports/groundnetwork.cxx

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