src/Airports/groundnetwork.cxx

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