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