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