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