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         closestOnNetwork = current;
 857         for (TrafficVectorIterator i = activeTraffic.begin();
 858                 i != activeTraffic.end(); i++) {
 859             if (i == current) {
 860                 continue;
 861             }
 862
 863             SGGeod other(SGGeod::fromDegM(i->getLongitude(),
 864                                           i->getLatitude(),
 865                                           i->getAltitude()));
 866             SGGeodesy::inverse(curr, other, course, az2, dist);
 867             bearing = fabs(heading - course);
 868             if (bearing > 180)
 869                 bearing = 360 - bearing;
 870             if ((dist < mindist) && (bearing < 60.0)) {
 871                 mindist = dist;
 872                 closest = i;
 873                 closestOnNetwork = i;
 874                 minbearing = bearing;
 875
 876             }
 877         }
 878         //Check traffic at the tower controller
 879         if (towerController->hasActiveTraffic()) {
 880             for (TrafficVectorIterator i =
 881                         towerController->getActiveTraffic().begin();
 882                     i != towerController->getActiveTraffic().end(); i++) {
 883                 //cerr << "Comparing " << current->getId() << " and " << i->getId() << endl;
 884                 SGGeod other(SGGeod::fromDegM(i->getLongitude(),
 885                                               i->getLatitude(),
 886                                               i->getAltitude()));
 887                 SGGeodesy::inverse(curr, other, course, az2, dist);
 888                 bearing = fabs(heading - course);
 889                 if (bearing > 180)
 890                     bearing = 360 - bearing;
 891                 if ((dist < mindist) && (bearing < 60.0)) {
 892                     //cerr << "Current aircraft " << current->getAircraft()->getTrafficRef()->getCallSign()
 893                     //     << " is closest to " << i->getAircraft()->getTrafficRef()->getCallSign()
 894                     //     << ", which has status " << i->getAircraft()->isScheduledForTakeoff()
 895                     //     << endl;
 896                     mindist = dist;
 897                     closest = i;
 898                     minbearing = bearing;
 899                     otherReasonToSlowDown = true;
 900                 }
 901             }
 902         }
 903         // Finally, check UserPosition
 904         // Note, as of 2011-08-01, this should no longer be necessecary.
 905         /*
 906         double userLatitude = fgGetDouble("/position/latitude-deg");
 907         double userLongitude = fgGetDouble("/position/longitude-deg");
 908         SGGeod user(SGGeod::fromDeg(userLongitude, userLatitude));
 909         SGGeodesy::inverse(curr, user, course, az2, dist);
 910
 911         bearing = fabs(heading - course);
 912         if (bearing > 180)
 913             bearing = 360 - bearing;
 914         if ((dist < mindist) && (bearing < 60.0)) {
 915             mindist = dist;
 916             //closest = i;
 917             minbearing = bearing;
 918             otherReasonToSlowDown = true;
 919         }
 920         */
 921         current->clearSpeedAdjustment();
 922         bool needBraking = false;
 923         if (current->checkPositionAndIntentions(*closest)
 924                 || otherReasonToSlowDown) {
 925             double maxAllowableDistance =
 926                 (1.1 * current->getRadius()) +
 927                 (1.1 * closest->getRadius());
 928             if (mindist < 2 * maxAllowableDistance) {
 929                 if (current->getId() == closest->getWaitsForId())
 930                     return;
 931                 else
 932                     current->setWaitsForId(closest->getId());
 933                 if (closest->getId() != current->getId()) {
 934                     current->setSpeedAdjustment(closest->getSpeed() *
 935                                                 (mindist / 100));
 936                     needBraking = true;
 937
 938 //                     if (
 939 //                         closest->getAircraft()->getTakeOffStatus() &&
 940 //                         (current->getAircraft()->getTrafficRef()->getDepartureAirport() ==  closest->getAircraft()->getTrafficRef()->getDepartureAirport()) &&
 941 //                         (current->getAircraft()->GetFlightPlan()->getRunway() == closest->getAircraft()->GetFlightPlan()->getRunway())
 942 //                     )
 943 //                         current->getAircraft()->scheduleForATCTowerDepartureControl(1);
 944                 } else {
 945                     current->setSpeedAdjustment(0);     // This can only happen when the user aircraft is the one closest
 946                 }
 947                 if (mindist < maxAllowableDistance) {
 948                     //double newSpeed = (maxAllowableDistance-mindist);
 949                     //current->setSpeedAdjustment(newSpeed);
 950                     //if (mindist < 0.5* maxAllowableDistance)
 951                     //  {
 952                     current->setSpeedAdjustment(0);
 953                     //  }
 954                 }
 955             }
 956         }
 957         if ((closest->getId() == closestOnNetwork->getId()) && (current->getPriority() < closest->getPriority()) && needBraking) {
 958             swap(current, closest);
 959         }
 960     }
 961 }
 962
 963 /**
 964    Check for "Hold position instruction".
 965    The hold position should be issued under the following conditions:
 966    1) For aircraft entering or crossing a runway with active traffic on it, or landing aircraft near it
 967    2) For taxiing aircraft that use one taxiway in opposite directions
 968    3) For crossing or merging taxiroutes.
 969 */
 970
 971 void FGGroundNetwork::checkHoldPosition(int id, double lat,
 972                                         double lon, double heading,
 973                                         double speed, double alt)
 974 {
 975     TrafficVectorIterator current;
 976     TrafficVectorIterator i = activeTraffic.begin();
 977     if (activeTraffic.size()) {
 978         //while ((i->getId() != id) && i != activeTraffic.end())
 979         while (i != activeTraffic.end()) {
 980             if (i->getId() == id) {
 981                 break;
 982             }
 983             i++;
 984         }
 985     } else {
 986         return;
 987     }
 988     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 989         SG_LOG(SG_GENERAL, SG_ALERT,
 990                "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkHoldPosition at " << SG_ORIGIN);
 991     }
 992     current = i;
 993     //
 994     if (current->getAircraft()->getTakeOffStatus() == 1) {
 995         current->setHoldPosition(true);
 996         return;
 997     }
 998     if (current->getAircraft()->getTakeOffStatus() == 2) {
 999         current->setHoldPosition(false);
1000         return;
1001     }
1002     bool origStatus = current->hasHoldPosition();
1003     current->setHoldPosition(false);
1004     SGGeod curr(SGGeod::fromDegM(lon, lat, alt));
1005     int currentRoute = i->getCurrentPosition();
1006     int nextRoute;
1007     if (i->getIntentions().size()) {
1008         nextRoute    = (*(i->getIntentions().begin()));
1009     } else {
1010         nextRoute = 0;
1011     }
1012     if (currentRoute > 0) {
1013         FGTaxiSegment *tx = findSegment(currentRoute);
1014         FGTaxiSegment *nx;
1015         if (nextRoute) {
1016             nx = findSegment(nextRoute);
1017         } else {
1018             nx = tx;
1019         }
1020         if (tx->hasBlock() || nx->hasBlock() ) {
1021             current->setHoldPosition(true);
1022         }
1023     }
1024
1025
1026     /*    for (i = activeTraffic.begin(); i != activeTraffic.end(); i++) {
1027             if (i->getId() != current->getId()) {
1028                 int node = current->crosses(this, *i);
1029                 if (node != -1) {
1030                     FGTaxiNode *taxiNode = findNode(node);
1031
1032                     // Determine whether it's save to continue or not.
1033                     // If we have a crossing route, there are two possibilities:
1034                     // 1) This is an interestion
1035                     // 2) This is oncoming two-way traffic, using the same taxiway.
1036                     //cerr << "Hold check 1 : " << id << " has common node " << node << endl;
1037
1038                     SGGeod other(SGGeod::
1039                                  fromDegM(i->getLongitude(), i->getLatitude(),
1040                                           i->getAltitude()));
1041                     bool needsToWait;
1042                     bool opposing;
1043                     if (current->isOpposing(this, *i, node)) {
1044                         needsToWait = true;
1045                         opposing = true;
1046                         //cerr << "Hold check 2 : " << node << "  has opposing segment " << endl;
1047                         // issue a "Hold Position" as soon as we're close to the offending node
1048                         // For now, I'm doing this as long as the other aircraft doesn't
1049                         // have a hold instruction as soon as we're within a reasonable
1050                         // distance from the offending node.
1051                         // This may be a bit of a conservative estimate though, as it may
1052                         // be well possible that both aircraft can both continue to taxi
1053                         // without crashing into each other.
1054                     } else {
1055                         opposing = false;
1056                         if (SGGeodesy::distanceM(other, taxiNode->getGeod()) > 200) // 2.0*i->getRadius())
1057                         {
1058                             needsToWait = false;
1059                             //cerr << "Hold check 3 : " << id <<"  Other aircraft approaching node is still far away. (" << dist << " nm). Can safely continue "
1060                             //           << endl;
1061                         } else {
1062                             needsToWait = true;
1063                             //cerr << "Hold check 4: " << id << "  Would need to wait for other aircraft : distance = " << dist << " meters" << endl;
1064                         }
1065                     }
1066
1067                     double dist =
1068                         SGGeodesy::distanceM(curr, taxiNode->getGeod());
1069                     if (!(i->hasHoldPosition())) {
1070
1071                         if ((dist < 200) && //2.5*current->getRadius()) &&
1072                             (needsToWait) && (i->onRoute(this, *current)) &&
1073                             //((i->onRoute(this, *current)) || ((!(i->getSpeedAdjustment())))) &&
1074                             (!(current->getId() == i->getWaitsForId())))
1075                             //(!(i->getSpeedAdjustment()))) // &&
1076                             //(!(current->getSpeedAdjustment())))
1077
1078                         {
1079                             if (!(isUserAircraft(i->getAircraft()))) { // test code. Don't wait for the user, let the user wait for you.
1080                                 current->setHoldPosition(true);
1081                                 current->setWaitsForId(i->getId());
1082                             }
1083                             //cerr << "Hold check 5: " << current->getCallSign() <<"  Setting Hold Position: distance to node ("  << node << ") "
1084                             //           << dist << " meters. Waiting for " << i->getCallSign();
1085                             //if (opposing)
1086                             //cerr <<" [opposing] " << endl;
1087                             //else
1088                             //        cerr << "[non-opposing] " << endl;
1089                             //if (i->hasSpeefAdjustment())
1090                             //        {
1091                             //          cerr << " (which in turn waits for ) " << i->
1092                         } else {
1093                             //cerr << "Hold check 6: " << id << "  No need to hold yet: Distance to node : " << dist << " nm"<< endl;
1094                         }
1095                     }
1096                 }
1097             }
1098         } */
1099     bool currStatus = current->hasHoldPosition();
1100     current->setHoldPosition(origStatus);
1101     // Either a Hold Position or a resume taxi transmission has been issued
1102     time_t now = time(NULL) + fgGetLong("/sim/time/warp");
1103     if ((now - lastTransmission) > 2) {
1104         available = true;
1105     }
1106     if (current->getState() == 0) {
1107         if ((origStatus != currStatus) && available) {
1108             //cerr << "Issueing hold short instrudtion " << currStatus << " " << available << endl;
1109             if (currStatus == true) { // No has a hold short instruction
1110                 transmit(&(*current), MSG_HOLD_POSITION, ATC_GROUND_TO_AIR, true);
1111                 //cerr << "Transmittin hold short instrudtion " << currStatus << " " << available << endl;
1112                 current->setState(1);
1113             } else {
1114                 transmit(&(*current), MSG_RESUME_TAXI, ATC_GROUND_TO_AIR, true);
1115                 //cerr << "Transmittig resume instrudtion " << currStatus << " " << available << endl;
1116                 current->setState(2);
1117             }
1118             lastTransmission = now;
1119             available = false;
1120             // Don't act on the changed instruction until the transmission is confirmed
1121             // So set back to original status
1122             //cerr << "Current state " << current->getState() << endl;
1123         }
1124
1125     }
1126     // 6 = Report runway
1127     // 7 = Acknowledge report runway
1128     // 8 = Switch tower frequency
1129     //9 = Acknowledge switch tower frequency
1130
1131     //int state = current->getState();
1132     if (checkTransmissionState(1,1, current, now, MSG_ACKNOWLEDGE_HOLD_POSITION, ATC_AIR_TO_GROUND)) {
1133         current->setState(0);
1134         current->setHoldPosition(true);
1135     }
1136     if (checkTransmissionState(2,2, current, now, MSG_ACKNOWLEDGE_RESUME_TAXI, ATC_AIR_TO_GROUND)) {
1137         current->setState(0);
1138         current->setHoldPosition(false);
1139     }
1140     if (current->getAircraft()->getTakeOffStatus() && (current->getState() == 0)) {
1141         //cerr << "Scheduling " << current->getAircraft()->getCallSign() << " for hold short" << endl;
1142         current->setState(6);
1143     }
1144     if (checkTransmissionState(6,6, current, now, MSG_REPORT_RUNWAY_HOLD_SHORT, ATC_AIR_TO_GROUND)) {
1145     }
1146     if (checkTransmissionState(7,7, current, now, MSG_ACKNOWLEDGE_REPORT_RUNWAY_HOLD_SHORT, ATC_GROUND_TO_AIR)) {
1147     }
1148     if (checkTransmissionState(8,8, current, now, MSG_SWITCH_TOWER_FREQUENCY, ATC_GROUND_TO_AIR)) {
1149     }
1150     if (checkTransmissionState(9,9, current, now, MSG_ACKNOWLEDGE_SWITCH_TOWER_FREQUENCY, ATC_AIR_TO_GROUND)) {
1151     }
1152
1153
1154
1155     //current->setState(0);
1156 }
1157
1158 /**
1159  * Check whether situations occur where the current aircraft is waiting for itself
1160  * due to higher order interactions.
1161  * A 'circular' wait is a situation where a waits for b, b waits for c, and c waits
1162  * for a. Ideally each aircraft only waits for one other aircraft, so by tracing
1163  * through this list of waiting aircraft, we can check if we'd eventually end back
1164  * at the current aircraft.
1165  *
1166  * Note that we should consider the situation where we are actually checking aircraft
1167  * d, which is waiting for aircraft a. d is not part of the loop, but is held back by
1168  * the looping aircraft. If we don't check for that, this function will get stuck into
1169  * endless loop.
1170  */
1171
1172 bool FGGroundNetwork::checkForCircularWaits(int id)
1173 {
1174     //cerr << "Performing Wait check " << id << endl;
1175     int target = 0;
1176     TrafficVectorIterator current, other;
1177     TrafficVectorIterator i = activeTraffic.begin();
1178     int trafficSize = activeTraffic.size();
1179     if (trafficSize) {
1180         while (i != activeTraffic.end()) {
1181             if (i->getId() == id) {
1182                 break;
1183             }
1184             i++;
1185         }
1186     } else {
1187         return false;
1188     }
1189     if (i == activeTraffic.end() || (trafficSize == 0)) {
1190         SG_LOG(SG_GENERAL, SG_ALERT,
1191                "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits at " << SG_ORIGIN);
1192     }
1193
1194     current = i;
1195     target = current->getWaitsForId();
1196     //bool printed = false; // Note that this variable is for debugging purposes only.
1197     int counter = 0;
1198
1199     if (id == target) {
1200         //cerr << "aircraft waits for user" << endl;
1201         return false;
1202     }
1203
1204
1205     while ((target > 0) && (target != id) && counter++ < trafficSize) {
1206         //printed = true;
1207         TrafficVectorIterator i = activeTraffic.begin();
1208         if (trafficSize) {
1209             //while ((i->getId() != id) && i != activeTraffic.end())
1210             while (i != activeTraffic.end()) {
1211                 if (i->getId() == target) {
1212                     break;
1213                 }
1214                 i++;
1215             }
1216         } else {
1217             return false;
1218         }
1219         if (i == activeTraffic.end() || (trafficSize == 0)) {
1220             //cerr << "[Waiting for traffic at Runway: DONE] " << endl << endl;;
1221             // The target id is not found on the current network, which means it's at the tower
1222             //SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits");
1223             return false;
1224         }
1225         other = i;
1226         target = other->getWaitsForId();
1227
1228         // actually this trap isn't as impossible as it first seemed:
1229         // the setWaitsForID(id) is set to current when the aircraft
1230         // is waiting for the user controlled aircraft.
1231         //if (current->getId() == other->getId()) {
1232         //    cerr << "Caught the impossible trap" << endl;
1233         //    cerr << "Current = " << current->getId() << endl;
1234         //    cerr << "Other   = " << other  ->getId() << endl;
1235         //    for (TrafficVectorIterator at = activeTraffic.begin();
1236         //          at != activeTraffic.end();
1237         //          at++) {
1238         //        cerr << "currently active aircraft : " << at->getCallSign() << " with Id " << at->getId() << " waits for " << at->getWaitsForId() << endl;
1239         //    }
1240         //    exit(1);
1241         if (current->getId() == other->getId())
1242             return false;
1243         //}
1244         //cerr << current->getCallSign() << " (" << current->getId()  << ") " << " -> " << other->getCallSign()
1245         //     << " (" << other->getId()  << "); " << endl;;
1246         //current = other;
1247     }
1248
1249
1250
1251
1252
1253
1254     //if (printed)
1255     //   cerr << "[done] " << endl << endl;;
1256     if (id == target) {
1257         SG_LOG(SG_GENERAL, SG_WARN,
1258                "Detected circular wait condition: Id = " << id <<
1259                "target = " << target);
1260         return true;
1261     } else {
1262         return false;
1263     }
1264 }
1265
1266 // Note that this function is probably obsolete...
1267 bool FGGroundNetwork::hasInstruction(int id)
1268 {
1269     TrafficVectorIterator i = activeTraffic.begin();
1270     // Search search if the current id has an entry
1271     // This might be faster using a map instead of a vector, but let's start by taking a safe route
1272     if (activeTraffic.size()) {
1273         //while ((i->getId() != id) && i != activeTraffic.end()) {
1274         while (i != activeTraffic.end()) {
1275             if (i->getId() == id) {
1276                 break;
1277             }
1278             i++;
1279         }
1280     }
1281     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
1282         SG_LOG(SG_GENERAL, SG_ALERT,
1283                "AI error: checking ATC instruction for aircraft without traffic record at " << SG_ORIGIN);
1284     } else {
1285         return i->hasInstruction();
1286     }
1287     return false;
1288 }
1289
1290 FGATCInstruction FGGroundNetwork::getInstruction(int id)
1291 {
1292     TrafficVectorIterator i = activeTraffic.begin();
1293     // Search search if the current id has an entry
1294     // This might be faster using a map instead of a vector, but let's start by taking a safe route
1295     if (activeTraffic.size()) {
1296         //while ((i->getId() != id) && i != activeTraffic.end()) {
1297         while (i != activeTraffic.end()) {
1298             if (i->getId() == id) {
1299                 break;
1300             }
1301             i++;
1302         }
1303     }
1304     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
1305         SG_LOG(SG_GENERAL, SG_ALERT,
1306                "AI error: requesting ATC instruction for aircraft without traffic record at " << SG_ORIGIN);
1307     } else {
1308         return i->getInstruction();
1309     }
1310     return FGATCInstruction();
1311 }
1312
1313 // Note that this function is copied from simgear. for maintanance purposes, it's probabtl better to make a general function out of that.
1314 static void WorldCoordinate(osg::Matrix& obj_pos, double lat,
1315                             double lon, double elev, double hdg, double slope)
1316 {
1317     SGGeod geod = SGGeod::fromDegM(lon, lat, elev);
1318     obj_pos = geod.makeZUpFrame();
1319     // hdg is not a compass heading, but a counter-clockwise rotation
1320     // around the Z axis
1321     obj_pos.preMult(osg::Matrix::rotate(hdg * SGD_DEGREES_TO_RADIANS,
1322                                         0.0, 0.0, 1.0));
1323     obj_pos.preMult(osg::Matrix::rotate(slope * SGD_DEGREES_TO_RADIANS,
1324                                         0.0, 1.0, 0.0));
1325 }
1326
1327
1328
1329
1330 void FGGroundNetwork::render(bool visible)
1331 {
1332
1333     SGMaterialLib *matlib = globals->get_matlib();
1334     if (group) {
1335         //int nr = ;
1336         globals->get_scenery()->get_scene_graph()->removeChild(group);
1337         //while (group->getNumChildren()) {
1338         //  cerr << "Number of children: " << group->getNumChildren() << endl;
1339         //simgear::EffectGeode* geode = (simgear::EffectGeode*) group->getChild(0);
1340         //osg::MatrixTransform *obj_trans = (osg::MatrixTransform*) group->getChild(0);
1341         //geode->releaseGLObjects();
1342         //group->removeChild(geode);
1343         //delete geode;
1344         group = 0;
1345     }
1346     if (visible) {
1347         group = new osg::Group;
1348         FGScenery * local_scenery = globals->get_scenery();
1349         double elevation_meters = 0.0;
1350         double elevation_feet = 0.0;
1351         //for ( FGTaxiSegmentVectorIterator i = segments.begin(); i != segments.end(); i++) {
1352         double dx = 0;
1353         for   (TrafficVectorIterator i = activeTraffic.begin(); i != activeTraffic.end(); i++) {
1354             // Handle start point
1355             int pos = i->getCurrentPosition() - 1;
1356             if (pos >= 0) {
1357
1358                 SGGeod start(SGGeod::fromDeg((i->getLongitude()), (i->getLatitude())));
1359                 SGGeod end  (SGGeod::fromDeg(segments[pos]->getEnd()->getLongitude(), segments[pos]->getEnd()->getLatitude()));
1360
1361                 double length = SGGeodesy::distanceM(start, end);
1362                 //heading = SGGeodesy::headingDeg(start->getGeod(), end->getGeod());
1363
1364                 double az2, heading; //, distanceM;
1365                 SGGeodesy::inverse(start, end, heading, az2, length);
1366                 double coveredDistance = length * 0.5;
1367                 SGGeod center;
1368                 SGGeodesy::direct(start, heading, coveredDistance, center, az2);
1369                 //cerr << "Active Aircraft : Centerpoint = (" << center.getLatitudeDeg() << ", " << center.getLongitudeDeg() << "). Heading = " << heading << endl;
1370                 ///////////////////////////////////////////////////////////////////////////////
1371                 // Make a helper function out of this
1372                 osg::Matrix obj_pos;
1373                 osg::MatrixTransform *obj_trans = new osg::MatrixTransform;
1374                 obj_trans->setDataVariance(osg::Object::STATIC);
1375                 // Experimental: Calculate slope here, based on length, and the individual elevations
1376                 double elevationStart;
1377                 if (isUserAircraft((i)->getAircraft())) {
1378                     elevationStart = fgGetDouble("/position/ground-elev-m");
1379                 } else {
1380                     elevationStart = ((i)->getAircraft()->_getAltitude());
1381                 }
1382                 double elevationEnd   = segments[pos]->getEnd()->getElevationM(parent->getElevation()*SG_FEET_TO_METER);
1383                 //cerr << "Using elevation " << elevationEnd << endl;
1384
1385                 if ((elevationEnd == 0) || (elevationEnd = parent->getElevation())) {
1386                     SGGeod center2 = end;
1387                     center2.setElevationM(SG_MAX_ELEVATION_M);
1388                     if (local_scenery->get_elevation_m( center2, elevationEnd, NULL )) {
1389                         elevation_feet = elevationEnd * SG_METER_TO_FEET + 0.5;
1390                         //elevation_meters += 0.5;
1391                     }
1392                     else {
1393                         elevationEnd = parent->getElevation();
1394                     }
1395                     segments[pos]->getEnd()->setElevation(elevationEnd);
1396                 }
1397                 double elevationMean  = (elevationStart + elevationEnd) / 2.0;
1398                 double elevDiff       = elevationEnd - elevationStart;
1399
1400                 double slope = atan2(elevDiff, length) * SGD_RADIANS_TO_DEGREES;
1401
1402                 //cerr << "1. Using mean elevation : " << elevationMean << " and " << slope << endl;
1403
1404                 WorldCoordinate( obj_pos, center.getLatitudeDeg(), center.getLongitudeDeg(), elevationMean+ 0.5, -(heading), slope );
1405
1406                 obj_trans->setMatrix( obj_pos );
1407                 //osg::Vec3 center(0, 0, 0)
1408
1409                 float width = length /2.0;
1410                 osg::Vec3 corner(-width, 0, 0.25f);
1411                 osg::Vec3 widthVec(2*width + 1, 0, 0);
1412                 osg::Vec3 heightVec(0, 1, 0);
1413                 osg::Geometry* geometry;
1414                 geometry = osg::createTexturedQuadGeometry(corner, widthVec, heightVec);
1415                 simgear::EffectGeode* geode = new simgear::EffectGeode;
1416                 geode->setName("test");
1417                 geode->addDrawable(geometry);
1418                 //osg::Node *custom_obj;
1419                 SGMaterial *mat;
1420                 if (segments[pos]->hasBlock()) {
1421                     mat = matlib->find("UnidirectionalTaperRed");
1422                 } else {
1423                     mat = matlib->find("UnidirectionalTaperGreen");
1424                 }
1425                 if (mat)
1426                     geode->setEffect(mat->get_effect());
1427                 obj_trans->addChild(geode);
1428                 // wire as much of the scene graph together as we can
1429                 //->addChild( obj_trans );
1430                 group->addChild( obj_trans );
1431                 /////////////////////////////////////////////////////////////////////
1432             } else {
1433                 //cerr << "BIG FAT WARNING: current position is here : " << pos << endl;
1434             }
1435             for (intVecIterator j = (i)->getIntentions().begin(); j != (i)->getIntentions().end(); j++) {
1436                 osg::Matrix obj_pos;
1437                 int k = (*j)-1;
1438                 if (k >= 0) {
1439                     osg::MatrixTransform *obj_trans = new osg::MatrixTransform;
1440                     obj_trans->setDataVariance(osg::Object::STATIC);
1441
1442                     // Experimental: Calculate slope here, based on length, and the individual elevations
1443                     double elevationStart = segments[k]->getStart()->getElevationM(parent->getElevation()*SG_FEET_TO_METER);
1444                     double elevationEnd   = segments[k]->getEnd  ()->getElevationM(parent->getElevation()*SG_FEET_TO_METER);
1445                     if ((elevationStart == 0)  || (elevationStart == parent->getElevation())) {
1446                         SGGeod center2 = segments[k]->getStart()->getGeod();
1447                         center2.setElevationM(SG_MAX_ELEVATION_M);
1448                         if (local_scenery->get_elevation_m( center2, elevationStart, NULL )) {
1449                             elevation_feet = elevationStart * SG_METER_TO_FEET + 0.5;
1450                             //elevation_meters += 0.5;
1451                         }
1452                         else {
1453                             elevationStart = parent->getElevation();
1454                         }
1455                         segments[k]->getStart()->setElevation(elevationStart);
1456                     }
1457                     if ((elevationEnd == 0) || (elevationEnd == parent->getElevation())) {
1458                         SGGeod center2 = segments[k]->getEnd()->getGeod();
1459                         center2.setElevationM(SG_MAX_ELEVATION_M);
1460                         if (local_scenery->get_elevation_m( center2, elevationEnd, NULL )) {
1461                             elevation_feet = elevationEnd * SG_METER_TO_FEET + 0.5;
1462                             //elevation_meters += 0.5;
1463                         }
1464                         else {
1465                             elevationEnd = parent->getElevation();
1466                         }
1467                         segments[k]->getEnd()->setElevation(elevationEnd);
1468                     }
1469
1470                     double elevationMean  = (elevationStart + elevationEnd) / 2.0;
1471                     double elevDiff       = elevationEnd - elevationStart;
1472                     double length         = segments[k]->getLength();
1473                     double slope = atan2(elevDiff, length) * SGD_RADIANS_TO_DEGREES;
1474
1475                     // cerr << "2. Using mean elevation : " << elevationMean << " and " << slope << endl;
1476
1477
1478                     WorldCoordinate( obj_pos, segments[k]->getLatitude(), segments[k]->getLongitude(), elevationMean+ 0.5, -(segments[k]->getHeading()), slope );
1479
1480                     obj_trans->setMatrix( obj_pos );
1481                     //osg::Vec3 center(0, 0, 0)
1482
1483                     float width = segments[k]->getLength() /2.0;
1484                     osg::Vec3 corner(-width, 0, 0.25f);
1485                     osg::Vec3 widthVec(2*width + 1, 0, 0);
1486                     osg::Vec3 heightVec(0, 1, 0);
1487                     osg::Geometry* geometry;
1488                     geometry = osg::createTexturedQuadGeometry(corner, widthVec, heightVec);
1489                     simgear::EffectGeode* geode = new simgear::EffectGeode;
1490                     geode->setName("test");
1491                     geode->addDrawable(geometry);
1492                     //osg::Node *custom_obj;
1493                     SGMaterial *mat;
1494                     if (segments[k]->hasBlock()) {
1495                         mat = matlib->find("UnidirectionalTaperRed");
1496                     } else {
1497                         mat = matlib->find("UnidirectionalTaperGreen");
1498                     }
1499                     if (mat)
1500                         geode->setEffect(mat->get_effect());
1501                     obj_trans->addChild(geode);
1502                     // wire as much of the scene graph together as we can
1503                     //->addChild( obj_trans );
1504                     group->addChild( obj_trans );
1505                 }
1506             }
1507             //dx += 0.1;
1508         }
1509         globals->get_scenery()->get_scene_graph()->addChild(group);
1510     }
1511 }
1512
1513 string FGGroundNetwork::getName() {
1514     return string(parent->getId() + "-ground");
1515 }
1516
1517 void FGGroundNetwork::update(double dt)
1518 {
1519     for (FGTaxiSegmentVectorIterator tsi = segments.begin(); tsi != segments.end(); tsi++) {
1520         (*tsi)->unblock();
1521     }
1522     int priority = 1;
1523     //sort(activeTraffic.begin(), activeTraffic.end(), compare_trafficrecords);
1524     // Handle traffic that is under ground control first; this way we'll prevent clutter at the gate areas.
1525     // Don't allow an aircraft to pushback when a taxiing aircraft is currently using part of the intended route.
1526     for   (TrafficVectorIterator i = parent->getDynamics()->getStartupController()->getActiveTraffic().begin();
1527             i != parent->getDynamics()->getStartupController()->getActiveTraffic().end(); i++) {
1528         i->allowPushBack();
1529         i->setPriority(priority++);
1530         if (i->isActive(60)) {
1531
1532             // Check for all active aircraft whether it's current pos segment is
1533             // an opposite of one of the departing aircraft's intentions
1534             for (TrafficVectorIterator j = activeTraffic.begin(); j != activeTraffic.end(); j++) {
1535                 int pos = j->getCurrentPosition();
1536                 if (pos > 0) {
1537                     FGTaxiSegment *seg = segments[pos-1]->opposite();
1538                     if (seg) {
1539                         int posReverse = seg->getIndex();
1540                         for (intVecIterator k = i->getIntentions().begin(); k != i->getIntentions().end(); k++) {
1541                             if ((*k) == posReverse) {
1542                                 i->denyPushBack();
1543                                 segments[posReverse-1]->block();
1544                             }
1545                         }
1546                     }
1547                 }
1548             }
1549             // if the current aircraft is still allowed to pushback, we can start reserving a route for if by blocking all the entry taxiways.
1550             if (i->pushBackAllowed()) {
1551                 int pos = i->getCurrentPosition();
1552                 if (pos > 0) {
1553                     FGTaxiSegment *seg = segments[pos-1];
1554                     FGTaxiNode *node = seg->getEnd();
1555                     for (FGTaxiSegmentVectorIterator tsi = segments.begin(); tsi != segments.end(); tsi++) {
1556                         if (((*tsi)->getEnd() == node) && ((*tsi) != seg)) {
1557                             (*tsi)->block();
1558                         }
1559                     }
1560                 }
1561                 for (intVecIterator j = i->getIntentions().begin(); j != i->getIntentions().end(); j++) {
1562                     int pos = (*j);
1563                     if (pos > 0) {
1564                         FGTaxiSegment *seg = segments[pos-1];
1565                         FGTaxiNode *node = seg->getEnd();
1566                         for (FGTaxiSegmentVectorIterator tsi = segments.begin(); tsi != segments.end(); tsi++) {
1567                             if (((*tsi)->getEnd() == node) && ((*tsi) != seg)) {
1568                                 (*tsi)->block();
1569                             }
1570                         }
1571                     }
1572                 }
1573             }
1574         }
1575     }
1576     for   (TrafficVectorIterator i = activeTraffic.begin(); i != activeTraffic.end(); i++) {
1577         i->setPriority(priority++);
1578         int pos = i->getCurrentPosition();
1579         if (pos > 0) {
1580             if (segments[pos-1]->hasBlock()) {
1581                 //SG_LOG(SG_GENERAL, SG_ALERT, "Taxiway incursion for AI aircraft" << i->getAircraft()->getCallSign());
1582             }
1583
1584         }
1585         intVecIterator ivi;
1586         for (ivi = i->getIntentions().begin(); ivi != i->getIntentions().end(); ivi++) {
1587             int segIndex = (*ivi);
1588             if (segIndex > 0) {
1589                 if (segments[segIndex-1]->hasBlock())
1590                     break;
1591             }
1592         }
1593         //after this, ivi points just behind the last valid unblocked taxi segment.
1594         for (intVecIterator j = i->getIntentions().begin(); j != ivi; j++) {
1595             int pos = (*j);
1596             if (pos > 0) {
1597                 FGTaxiSegment *seg = segments[pos-1];
1598                 FGTaxiNode *node = seg->getEnd();
1599                 for (FGTaxiSegmentVectorIterator tsi = segments.begin(); tsi != segments.end(); tsi++) {
1600                     if (((*tsi)->getEnd() == node) && ((*tsi) != seg)) {
1601                         (*tsi)->block();
1602                     }
1603                 }
1604             }
1605         }
1606     }
1607 }
1608