src/Airports/groundnetwork.cxx

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