src/Airports/groundnetwork.cxx

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