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