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