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
  40 #include <Airports/simple.hxx>
  41 #include <Airports/dynamics.hxx>
  42
  43 #include <AIModel/AIAircraft.hxx>
  44 #include <AIModel/AIFlightPlan.hxx>
  45
  46 #include "groundnetwork.hxx"
  47
  48 /***************************************************************************
  49  * FGTaxiSegment
  50  **************************************************************************/
  51
  52 void FGTaxiSegment::setStart(FGTaxiNodeVector * nodes)
  53 {
  54     FGTaxiNodeVectorIterator i = nodes->begin();
  55     while (i != nodes->end()) {
  56         //cerr << "Scanning start node index" << (*i)->getIndex() << endl;
  57         if ((*i)->getIndex() == startNode) {
  58             start = (*i)->getAddress();
  59             (*i)->addSegment(this);
  60             return;
  61         }
  62         i++;
  63     }
  64     SG_LOG(SG_GENERAL, SG_ALERT,
  65            "Could not find start node " << startNode << endl);
  66 }
  67
  68 void FGTaxiSegment::setEnd(FGTaxiNodeVector * nodes)
  69 {
  70     FGTaxiNodeVectorIterator i = nodes->begin();
  71     while (i != nodes->end()) {
  72         //cerr << "Scanning end node index" << (*i)->getIndex() << endl;
  73         if ((*i)->getIndex() == endNode) {
  74             end = (*i)->getAddress();
  75             return;
  76         }
  77         i++;
  78     }
  79     SG_LOG(SG_GENERAL, SG_ALERT,
  80            "Could not find end node " << endNode << endl);
  81 }
  82
  83
  84
  85 // There is probably a computationally cheaper way of
  86 // doing this.
  87 void FGTaxiSegment::setDimensions(double elevation)
  88 {
  89     length = SGGeodesy::distanceM(start->getGeod(), end->getGeod());
  90     //heading = SGGeodesy::headingDeg(start->getGeod(), end->getGeod());
  91
  92     double az2; //, distanceM;
  93     SGGeodesy::inverse(start->getGeod(), end->getGeod(), heading, az2, length);
  94     double coveredDistance = length * 0.5;
  95     SGGeodesy::direct(start->getGeod(), heading, coveredDistance, center, az2);
  96     cerr << "Centerpoint = (" << center.getLatitudeDeg() << ", " << center.getLongitudeDeg() << "). Heading = " << heading << endl;
  97 }
  98
  99
 100 //void FGTaxiSegment::setCourseDiff(double crse)
 101 //{
 102 //    headingDiff = fabs(course - crse);
 103
 104 //    if (headingDiff > 180)
 105 //        headingDiff = fabs(headingDiff - 360);
 106 //}
 107
 108
 109 /***************************************************************************
 110  * FGTaxiRoute
 111  **************************************************************************/
 112 bool FGTaxiRoute::next(int *nde)
 113 {
 114     //for (intVecIterator i = nodes.begin(); i != nodes.end(); i++)
 115     //  cerr << "FGTaxiRoute contains : " << *(i) << endl;
 116     //cerr << "Offset from end: " << nodes.end() - currNode << endl;
 117     //if (currNode != nodes.end())
 118     //  cerr << "true" << endl;
 119     //else
 120     //  cerr << "false" << endl;
 121     //if (nodes.size() != (routes.size()) +1)
 122     //  cerr << "ALERT: Misconfigured TaxiRoute : " << nodes.size() << " " << routes.size() << endl;
 123
 124     if (currNode == nodes.end())
 125         return false;
 126     *nde = *(currNode);
 127     if (currNode != nodes.begin())      // make sure route corresponds to the end node
 128         currRoute++;
 129     currNode++;
 130     return true;
 131 };
 132
 133 bool FGTaxiRoute::next(int *nde, int *rte)
 134 {
 135     //for (intVecIterator i = nodes.begin(); i != nodes.end(); i++)
 136     //  cerr << "FGTaxiRoute contains : " << *(i) << endl;
 137     //cerr << "Offset from end: " << nodes.end() - currNode << endl;
 138     //if (currNode != nodes.end())
 139     //  cerr << "true" << endl;
 140     //else
 141     //  cerr << "false" << endl;
 142     if (nodes.size() != (routes.size()) + 1) {
 143         SG_LOG(SG_GENERAL, SG_ALERT,
 144                "ALERT: Misconfigured TaxiRoute : " << nodes.
 145                size() << " " << routes.size());
 146         exit(1);
 147     }
 148     if (currNode == nodes.end())
 149         return false;
 150     *nde = *(currNode);
 151     //*rte = *(currRoute);
 152     if (currNode != nodes.begin())      // Make sure route corresponds to the end node
 153     {
 154         *rte = *(currRoute);
 155         currRoute++;
 156     } else {
 157         // If currNode points to the first node, this means the aircraft is not on the taxi node
 158         // yet. Make sure to return a unique identifyer in this situation though, because otherwise
 159         // the speed adjust AI code may be unable to resolve whether two aircraft are on the same
 160         // taxi route or not. the negative of the preceding route seems a logical choice, as it is
 161         // unique for any starting location.
 162         // Note that this is probably just a temporary fix until I get Parking / tower control working.
 163         *rte = -1 * *(currRoute);
 164     }
 165     currNode++;
 166     return true;
 167 };
 168
 169 void FGTaxiRoute::rewind(int route)
 170 {
 171     int currPoint;
 172     int currRoute;
 173     first();
 174     do {
 175         if (!(next(&currPoint, &currRoute))) {
 176             SG_LOG(SG_GENERAL, SG_ALERT,
 177                    "Error in rewinding TaxiRoute: current" << currRoute <<
 178                    " goal " << route);
 179         }
 180     } while (currRoute != route);
 181 }
 182
 183
 184
 185
 186 /***************************************************************************
 187  * FGGroundNetwork()
 188  **************************************************************************/
 189 bool compare_nodes(FGTaxiNode * a, FGTaxiNode * b)
 190 {
 191     return (*a) < (*b);
 192 }
 193
 194 bool compare_segments(FGTaxiSegment * a, FGTaxiSegment * b)
 195 {
 196     return (*a) < (*b);
 197 }
 198
 199 FGGroundNetwork::FGGroundNetwork()
 200 {
 201     hasNetwork = false;
 202     foundRoute = false;
 203     totalDistance = 0;
 204     maxDistance = 0;
 205     //maxDepth    = 1000;
 206     count = 0;
 207     currTraffic = activeTraffic.begin();
 208
 209 }
 210
 211 FGGroundNetwork::~FGGroundNetwork()
 212 {
 213     for (FGTaxiNodeVectorIterator node = nodes.begin();
 214          node != nodes.end(); node++) {
 215         delete(*node);
 216     }
 217     nodes.clear();
 218     pushBackNodes.clear();
 219     for (FGTaxiSegmentVectorIterator seg = segments.begin();
 220          seg != segments.end(); seg++) {
 221         delete(*seg);
 222     }
 223     segments.clear();
 224 }
 225
 226 void FGGroundNetwork::addSegment(const FGTaxiSegment & seg)
 227 {
 228     segments.push_back(new FGTaxiSegment(seg));
 229 }
 230
 231 void FGGroundNetwork::addNode(const FGTaxiNode & node)
 232 {
 233     nodes.push_back(new FGTaxiNode(node));
 234 }
 235
 236 void FGGroundNetwork::addNodes(FGParkingVec * parkings)
 237 {
 238     FGTaxiNode n;
 239     FGParkingVecIterator i = parkings->begin();
 240     while (i != parkings->end()) {
 241         n.setIndex(i->getIndex());
 242         n.setLatitude(i->getLatitude());
 243         n.setLongitude(i->getLongitude());
 244         n.setElevation(parent->getElevation());
 245         nodes.push_back(new FGTaxiNode(n));
 246
 247         i++;
 248     }
 249 }
 250
 251
 252
 253 void FGGroundNetwork::init()
 254 {
 255     hasNetwork = true;
 256     int index = 1;
 257     sort(nodes.begin(), nodes.end(), compare_nodes);
 258     //sort(segments.begin(), segments.end(), compare_segments());
 259     FGTaxiSegmentVectorIterator i = segments.begin();
 260     while (i != segments.end()) {
 261         (*i)->setStart(&nodes);
 262         (*i)->setEnd(&nodes);
 263         (*i)->setDimensions(parent->getElevation());
 264         (*i)->setIndex(index);
 265         if ((*i)->isPushBack()) {
 266             pushBackNodes.push_back((*i)->getEnd());
 267         }
 268         //SG_LOG(SG_GENERAL, SG_BULK,  "initializing segment " << (*i)->getIndex() << endl);
 269         //SG_LOG(SG_GENERAL, SG_BULK, "Track distance = "     << (*i)->getLength() << endl);
 270         //SG_LOG(SG_GENERAL, SG_BULK, "Track runs from "      << (*i)->getStart()->getIndex() << " to "
 271         //                                                    << (*i)->getEnd()->getIndex() << endl);
 272         i++;
 273         index++;
 274     }
 275
 276     i = segments.begin();
 277     while (i != segments.end()) {
 278         FGTaxiSegmentVectorIterator j = (*i)->getEnd()->getBeginRoute();
 279         while (j != (*i)->getEnd()->getEndRoute()) {
 280             if ((*j)->getEnd()->getIndex() == (*i)->getStart()->getIndex()) {
 281 //          int start1 = (*i)->getStart()->getIndex();
 282 //          int end1   = (*i)->getEnd()  ->getIndex();
 283 //          int start2 = (*j)->getStart()->getIndex();
 284 //          int end2   = (*j)->getEnd()->getIndex();
 285 //          int oppIndex = (*j)->getIndex();
 286                 //cerr << "Opposite of  " << (*i)->getIndex() << " (" << start1 << "," << end1 << ") "
 287                 //   << "happens to be " << oppIndex      << " (" << start2 << "," << end2 << ") " << endl;
 288                 (*i)->setOpposite(*j);
 289                 break;
 290             }
 291             j++;
 292         }
 293         i++;
 294     }
 295     //FGTaxiNodeVectorIterator j = nodes.begin();
 296     //while (j != nodes.end()) {
 297     //    if ((*j)->getHoldPointType() == 3) {
 298     //        pushBackNodes.push_back((*j));
 299     //    }
 300     //    j++;
 301     //}
 302     //cerr << "Done initializing ground network" << endl;
 303     //exit(1);
 304 }
 305
 306 int FGGroundNetwork::findNearestNode(const SGGeod & aGeod)
 307 {
 308     double minDist = HUGE_VAL;
 309     int index = -1;
 310
 311     for (FGTaxiNodeVectorIterator itr = nodes.begin(); itr != nodes.end();
 312          itr++) {
 313         double d = SGGeodesy::distanceM(aGeod, (*itr)->getGeod());
 314         if (d < minDist) {
 315             minDist = d;
 316             index = (*itr)->getIndex();
 317             //cerr << "Minimum distance of " << minDist << " for index " << index << endl;
 318         }
 319     }
 320
 321     return index;
 322 }
 323
 324 int FGGroundNetwork::findNearestNode(double lat, double lon)
 325 {
 326     return findNearestNode(SGGeod::fromDeg(lon, lat));
 327 }
 328
 329 FGTaxiNode *FGGroundNetwork::findNode(unsigned idx)
 330 {                               /*
 331                                    for (FGTaxiNodeVectorIterator
 332                                    itr = nodes.begin();
 333                                    itr != nodes.end(); itr++)
 334                                    {
 335                                    if (itr->getIndex() == idx)
 336                                    return itr->getAddress();
 337                                    } */
 338
 339     if ((idx >= 0) && (idx < nodes.size()))
 340         return nodes[idx]->getAddress();
 341     else
 342         return 0;
 343 }
 344
 345 FGTaxiSegment *FGGroundNetwork::findSegment(unsigned idx)
 346 {                               /*
 347                                    for (FGTaxiSegmentVectorIterator
 348                                    itr = segments.begin();
 349                                    itr != segments.end(); itr++)
 350                                    {
 351                                    if (itr->getIndex() == idx)
 352                                    return itr->getAddress();
 353                                    }
 354                                  */
 355     if ((idx > 0) && (idx <= segments.size()))
 356         return segments[idx - 1]->getAddress();
 357     else {
 358         //cerr << "Alert: trying to find invalid segment " << idx << endl;
 359         return 0;
 360     }
 361 }
 362
 363
 364 FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end,
 365                                                bool fullSearch)
 366 {
 367 //implements Dijkstra's algorithm to find shortest distance route from start to end
 368 //taken from http://en.wikipedia.org/wiki/Dijkstra's_algorithm
 369
 370     //double INFINITE = 100000000000.0;
 371     // initialize scoring values
 372     int nParkings = parent->getDynamics()->getNrOfParkings();
 373     FGTaxiNodeVector *currNodesSet;
 374     if (fullSearch) {
 375         currNodesSet = &nodes;
 376     } else {
 377         currNodesSet = &pushBackNodes;
 378     }
 379
 380     for (FGTaxiNodeVectorIterator
 381          itr = currNodesSet->begin(); itr != currNodesSet->end(); itr++) {
 382         (*itr)->setPathScore(HUGE_VAL); //infinity by all practical means
 383         (*itr)->setPreviousNode(0);     //
 384         (*itr)->setPreviousSeg(0);      //
 385     }
 386
 387     FGTaxiNode *firstNode = findNode(start);
 388     firstNode->setPathScore(0);
 389
 390     FGTaxiNode *lastNode = findNode(end);
 391
 392     FGTaxiNodeVector unvisited(*currNodesSet);  // working copy
 393
 394     while (!unvisited.empty()) {
 395         FGTaxiNode *best = *(unvisited.begin());
 396         for (FGTaxiNodeVectorIterator
 397              itr = unvisited.begin(); itr != unvisited.end(); itr++) {
 398             if ((*itr)->getPathScore() < best->getPathScore())
 399                 best = (*itr);
 400         }
 401
 402         FGTaxiNodeVectorIterator newend =
 403             remove(unvisited.begin(), unvisited.end(), best);
 404         unvisited.erase(newend, unvisited.end());
 405
 406         if (best == lastNode) { // found route or best not connected
 407             break;
 408         } else {
 409             for (FGTaxiSegmentVectorIterator
 410                  seg = best->getBeginRoute();
 411                  seg != best->getEndRoute(); seg++) {
 412                 if (fullSearch || (*seg)->isPushBack()) {
 413                     FGTaxiNode *tgt = (*seg)->getEnd();
 414                     double alt =
 415                         best->getPathScore() + (*seg)->getLength() +
 416                         (*seg)->getPenalty(nParkings);
 417                     if (alt < tgt->getPathScore()) {    // Relax (u,v)
 418                         tgt->setPathScore(alt);
 419                         tgt->setPreviousNode(best);
 420                         tgt->setPreviousSeg(*seg);      //
 421                     }
 422                 } else {
 423                     //   // cerr << "Skipping TaxiSegment " << (*seg)->getIndex() << endl;
 424                 }
 425             }
 426         }
 427     }
 428
 429     if (lastNode->getPathScore() == HUGE_VAL) {
 430         // no valid route found
 431         if (fullSearch) {
 432             SG_LOG(SG_GENERAL, SG_ALERT,
 433                    "Failed to find route from waypoint " << start << " to "
 434                    << end << " at " << parent->getId());
 435         }
 436         FGTaxiRoute empty;
 437         return empty;
 438         //exit(1); //TODO exit more gracefully, no need to stall the whole sim with broken GN's
 439     } else {
 440         // assemble route from backtrace information
 441         intVec nodes, routes;
 442         FGTaxiNode *bt = lastNode;
 443         while (bt->getPreviousNode() != 0) {
 444             nodes.push_back(bt->getIndex());
 445             routes.push_back(bt->getPreviousSegment()->getIndex());
 446             bt = bt->getPreviousNode();
 447         }
 448         nodes.push_back(start);
 449         reverse(nodes.begin(), nodes.end());
 450         reverse(routes.begin(), routes.end());
 451
 452         return FGTaxiRoute(nodes, routes, lastNode->getPathScore(), 0);
 453     }
 454 }
 455
 456 int FGTaxiSegment::getPenalty(int nGates)
 457 {
 458     int penalty = 0;
 459     if (end->getIndex() < nGates) {
 460         penalty += 10000;
 461     }
 462     if (end->getIsOnRunway()) { // For now. In future versions, need to find out whether runway is active.
 463         penalty += 1000;
 464     }
 465     return penalty;
 466 }
 467
 468 /* ATC Related Functions */
 469
 470 void FGGroundNetwork::announcePosition(int id,
 471                                        FGAIFlightPlan * intendedRoute,
 472                                        int currentPosition, double lat,
 473                                        double lon, double heading,
 474                                        double speed, double alt,
 475                                        double radius, int leg,
 476                                        FGAIAircraft * aircraft)
 477 {
 478     init();
 479     TrafficVectorIterator i = activeTraffic.begin();
 480     // Search search if the current id alread has an entry
 481     // This might be faster using a map instead of a vector, but let's start by taking a safe route
 482     if (activeTraffic.size()) {
 483         //while ((i->getId() != id) && i != activeTraffic.end()) {
 484         while (i != activeTraffic.end()) {
 485             if (i->getId() == id) {
 486                 break;
 487             }
 488             i++;
 489         }
 490     }
 491     // Add a new TrafficRecord if no one exsists for this aircraft.
 492     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 493         FGTrafficRecord rec;
 494         rec.setId(id);
 495         rec.setLeg(leg);
 496         rec.setPositionAndIntentions(currentPosition, intendedRoute);
 497         rec.setPositionAndHeading(lat, lon, heading, speed, alt);
 498         rec.setRadius(radius);  // only need to do this when creating the record.
 499         rec.setAircraft(aircraft);
 500         activeTraffic.push_back(rec);
 501     } else {
 502         i->setPositionAndIntentions(currentPosition, intendedRoute);
 503         i->setPositionAndHeading(lat, lon, heading, speed, alt);
 504     }
 505 }
 506
 507 void FGGroundNetwork::signOff(int id)
 508 {
 509     TrafficVectorIterator i = activeTraffic.begin();
 510     // Search search if the current id alread has an entry
 511     // This might be faster using a map instead of a vector, but let's start by taking a safe route
 512     if (activeTraffic.size()) {
 513         //while ((i->getId() != id) && i != activeTraffic.end()) {
 514         while (i != activeTraffic.end()) {
 515             if (i->getId() == id) {
 516                 break;
 517             }
 518             i++;
 519         }
 520     }
 521     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 522         SG_LOG(SG_GENERAL, SG_ALERT,
 523                "AI error: Aircraft without traffic record is signing off");
 524     } else {
 525         i = activeTraffic.erase(i);
 526     }
 527 }
 528
 529 bool FGGroundNetwork::checkTransmissionState(int minState, int maxState, TrafficVectorIterator i, time_t now, AtcMsgId msgId,
 530                                AtcMsgDir msgDir)
 531 {
 532     int state = i->getState();
 533     if ((state >= minState) && (state <= maxState) && available) {
 534         if ((msgDir == ATC_AIR_TO_GROUND) && isUserAircraft(i->getAircraft())) {
 535
 536             cerr << "Checking state " << state << " for " << i->getAircraft()->getCallSign() << endl;
 537             static SGPropertyNode_ptr trans_num = globals->get_props()->getNode("/sim/atc/transmission-num", true);
 538             int n = trans_num->getIntValue();
 539             if (n >= 0) {
 540                 trans_num->setIntValue(-1);
 541                  // PopupCallback(n);
 542                  cerr << "Selected transmission message" << n << endl;
 543             } else {
 544                 cerr << "creading message for " << i->getAircraft()->getCallSign() << endl;
 545                 transmit(&(*i), msgId, msgDir, false);
 546                 return false;
 547             }
 548         }
 549         //cerr << "Transmitting startup msg" << endl;
 550         transmit(&(*i), msgId, msgDir, true);
 551         i->updateState();
 552         lastTransmission = now;
 553         available = false;
 554         return true;
 555     }
 556     return false;
 557 }
 558
 559 void FGGroundNetwork::updateAircraftInformation(int id, double lat, double lon,
 560                                                 double heading, double speed, double alt,
 561                                                 double dt)
 562 {
 563     // Check whether aircraft are on hold due to a preceding pushback. If so, make sure to
 564     // Transmit air-to-ground "Ready to taxi request:
 565     // Transmit ground to air approval / hold
 566     // Transmit confirmation ...
 567     // Probably use a status mechanism similar to the Engine start procedure in the startup controller.
 568
 569
 570     TrafficVectorIterator i = activeTraffic.begin();
 571     // Search search if the current id has an entry
 572     // This might be faster using a map instead of a vector, but let's start by taking a safe route
 573     TrafficVectorIterator current, closest;
 574     if (activeTraffic.size()) {
 575         //while ((i->getId() != id) && i != activeTraffic.end()) {
 576         while (i != activeTraffic.end()) {
 577             if (i->getId() == id) {
 578                 break;
 579             }
 580             i++;
 581         }
 582     }
 583     // update position of the current aircraft
 584     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 585         SG_LOG(SG_GENERAL, SG_ALERT,
 586                "AI error: updating aircraft without traffic record");
 587     } else {
 588         i->setPositionAndHeading(lat, lon, heading, speed, alt);
 589         current = i;
 590     }
 591
 592     setDt(getDt() + dt);
 593
 594     // Update every three secs, but add some randomness
 595     // to prevent all IA objects doing this in synchrony
 596     //if (getDt() < (3.0) + (rand() % 10))
 597     //  return;
 598     //else
 599     //  setDt(0);
 600     current->clearResolveCircularWait();
 601     current->setWaitsForId(0);
 602     checkSpeedAdjustment(id, lat, lon, heading, speed, alt);
 603     bool needsTaxiClearance = current->getAircraft()->getTaxiClearanceRequest();
 604     if (!needsTaxiClearance) {
 605         checkHoldPosition(id, lat, lon, heading, speed, alt);
 606         if (checkForCircularWaits(id)) {
 607             i->setResolveCircularWait();
 608         }
 609     } else {
 610         current->setHoldPosition(true);
 611         int state = current->getState();
 612         time_t now = time(NULL) + fgGetLong("/sim/time/warp");
 613         if ((now - lastTransmission) > 15) {
 614             available = true;
 615         }
 616         if (checkTransmissionState(0,2, current, now, MSG_REQUEST_TAXI_CLEARANCE, ATC_AIR_TO_GROUND)) {
 617             current->setState(3);
 618         }
 619         if (checkTransmissionState(3,3, current, now, MSG_ISSUE_TAXI_CLEARANCE, ATC_GROUND_TO_AIR)) {
 620             current->setState(4);
 621         }
 622         if (checkTransmissionState(4,4, current, now, MSG_ACKNOWLEDGE_TAXI_CLEARANCE, ATC_AIR_TO_GROUND)) {
 623             current->setState(5);
 624         }
 625         if ((state == 5) && available) {
 626             current->setState(0);
 627             current->getAircraft()->setTaxiClearanceRequest(false);
 628             current->setHoldPosition(true);
 629             available = false;
 630         }
 631
 632     }
 633
 634 }
 635
 636 /**
 637    Scan for a speed adjustment change. Find the nearest aircraft that is in front
 638    and adjust speed when we get too close. Only do this when current position and/or
 639    intentions of the current aircraft match current taxiroute position of the proximate
 640    aircraft. For traffic that is on other routes we need to issue a "HOLD Position"
 641    instruction. See below for the hold position instruction.
 642
 643    Note that there currently still is one flaw in the logic that needs to be addressed.
 644    There can be situations where one aircraft is in front of the current aircraft, on a separate
 645    route, but really close after an intersection coming off the current route. This
 646    aircraft is still close enough to block the current aircraft. This situation is currently
 647    not addressed yet, but should be.
 648 */
 649
 650 void FGGroundNetwork::checkSpeedAdjustment(int id, double lat,
 651                                            double lon, double heading,
 652                                            double speed, double alt)
 653 {
 654
 655     TrafficVectorIterator current, closest;
 656     TrafficVectorIterator i = activeTraffic.begin();
 657     bool otherReasonToSlowDown = false;
 658     bool previousInstruction;
 659     if (activeTraffic.size()) {
 660         //while ((i->getId() != id) && (i != activeTraffic.end()))
 661         while (i != activeTraffic.end()) {
 662             if (i->getId() == id) {
 663                 break;
 664             }
 665             i++;
 666         }
 667     } else {
 668         return;
 669     }
 670     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 671         SG_LOG(SG_GENERAL, SG_ALERT,
 672                "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkSpeedAdjustment");
 673     }
 674     current = i;
 675     //closest = current;
 676
 677     previousInstruction = current->getSpeedAdjustment();
 678     double mindist = HUGE_VAL;
 679     if (activeTraffic.size()) {
 680         double course, dist, bearing, minbearing, az2;
 681         SGGeod curr(SGGeod::fromDegM(lon, lat, alt));
 682         //TrafficVector iterator closest;
 683         closest = current;
 684         for (TrafficVectorIterator i = activeTraffic.begin();
 685              i != activeTraffic.end(); i++) {
 686             if (i == current) {
 687                 continue;
 688             }
 689
 690             SGGeod other(SGGeod::fromDegM(i->getLongitude(),
 691                                           i->getLatitude(),
 692                                           i->getAltitude()));
 693             SGGeodesy::inverse(curr, other, course, az2, dist);
 694             bearing = fabs(heading - course);
 695             if (bearing > 180)
 696                 bearing = 360 - bearing;
 697             if ((dist < mindist) && (bearing < 60.0)) {
 698                 mindist = dist;
 699                 closest = i;
 700                 minbearing = bearing;
 701             }
 702         }
 703         //Check traffic at the tower controller
 704         if (towerController->hasActiveTraffic()) {
 705             for (TrafficVectorIterator i =
 706                  towerController->getActiveTraffic().begin();
 707                  i != towerController->getActiveTraffic().end(); i++) {
 708                 //cerr << "Comparing " << current->getId() << " and " << i->getId() << endl;
 709                 SGGeod other(SGGeod::fromDegM(i->getLongitude(),
 710                                               i->getLatitude(),
 711                                               i->getAltitude()));
 712                 SGGeodesy::inverse(curr, other, course, az2, dist);
 713                 bearing = fabs(heading - course);
 714                 if (bearing > 180)
 715                     bearing = 360 - bearing;
 716                 if ((dist < mindist) && (bearing < 60.0)) {
 717                     mindist = dist;
 718                     closest = i;
 719                     minbearing = bearing;
 720                     otherReasonToSlowDown = true;
 721                 }
 722             }
 723         }
 724         // Finally, check UserPosition
 725         double userLatitude = fgGetDouble("/position/latitude-deg");
 726         double userLongitude = fgGetDouble("/position/longitude-deg");
 727         SGGeod user(SGGeod::fromDeg(userLongitude, userLatitude));
 728         SGGeodesy::inverse(curr, user, course, az2, dist);
 729
 730         bearing = fabs(heading - course);
 731         if (bearing > 180)
 732             bearing = 360 - bearing;
 733         if ((dist < mindist) && (bearing < 60.0)) {
 734             mindist = dist;
 735             //closest = i;
 736             minbearing = bearing;
 737             otherReasonToSlowDown = true;
 738         }
 739
 740         current->clearSpeedAdjustment();
 741
 742         if (current->checkPositionAndIntentions(*closest)
 743             || otherReasonToSlowDown) {
 744             double maxAllowableDistance =
 745                 (1.1 * current->getRadius()) +
 746                 (1.1 * closest->getRadius());
 747             if (mindist < 2 * maxAllowableDistance) {
 748                 if (current->getId() == closest->getWaitsForId())
 749                     return;
 750                 else
 751                     current->setWaitsForId(closest->getId());
 752                 if (closest->getId() != current->getId())
 753                     current->setSpeedAdjustment(closest->getSpeed() *
 754                                                 (mindist / 100));
 755                 else
 756                     current->setSpeedAdjustment(0);     // This can only happen when the user aircraft is the one closest
 757                 if (mindist < maxAllowableDistance) {
 758                     //double newSpeed = (maxAllowableDistance-mindist);
 759                     //current->setSpeedAdjustment(newSpeed);
 760                     //if (mindist < 0.5* maxAllowableDistance)
 761                     //  {
 762                     current->setSpeedAdjustment(0);
 763                     //  }
 764                 }
 765             }
 766         }
 767     }
 768 }
 769
 770 /**
 771    Check for "Hold position instruction".
 772    The hold position should be issued under the following conditions:
 773    1) For aircraft entering or crossing a runway with active traffic on it, or landing aircraft near it
 774    2) For taxiing aircraft that use one taxiway in opposite directions
 775    3) For crossing or merging taxiroutes.
 776 */
 777
 778 void FGGroundNetwork::checkHoldPosition(int id, double lat,
 779                                         double lon, double heading,
 780                                         double speed, double alt)
 781 {
 782     TrafficVectorIterator current;
 783     TrafficVectorIterator i = activeTraffic.begin();
 784     if (activeTraffic.size()) {
 785         //while ((i->getId() != id) && i != activeTraffic.end())
 786         while (i != activeTraffic.end()) {
 787             if (i->getId() == id) {
 788                 break;
 789             }
 790             i++;
 791         }
 792     } else {
 793         return;
 794     }
 795     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 796         SG_LOG(SG_GENERAL, SG_ALERT,
 797                "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkHoldPosition");
 798     }
 799     current = i;
 800     bool origStatus = current->hasHoldPosition();
 801     current->setHoldPosition(false);
 802     SGGeod curr(SGGeod::fromDegM(lon, lat, alt));
 803
 804     for (i = activeTraffic.begin(); i != activeTraffic.end(); i++) {
 805         if (i->getId() != current->getId()) {
 806             int node = current->crosses(this, *i);
 807             if (node != -1) {
 808                 FGTaxiNode *taxiNode = findNode(node);
 809
 810                 // Determine whether it's save to continue or not.
 811                 // If we have a crossing route, there are two possibilities:
 812                 // 1) This is an interestion
 813                 // 2) This is oncoming two-way traffic, using the same taxiway.
 814                 //cerr << "Hold check 1 : " << id << " has common node " << node << endl;
 815
 816                 SGGeod other(SGGeod::
 817                              fromDegM(i->getLongitude(), i->getLatitude(),
 818                                       i->getAltitude()));
 819                 bool needsToWait;
 820                 bool opposing;
 821                 if (current->isOpposing(this, *i, node)) {
 822                     needsToWait = true;
 823                     opposing = true;
 824                     //cerr << "Hold check 2 : " << node << "  has opposing segment " << endl;
 825                     // issue a "Hold Position" as soon as we're close to the offending node
 826                     // For now, I'm doing this as long as the other aircraft doesn't
 827                     // have a hold instruction as soon as we're within a reasonable
 828                     // distance from the offending node.
 829                     // This may be a bit of a conservative estimate though, as it may
 830                     // be well possible that both aircraft can both continue to taxi
 831                     // without crashing into each other.
 832                 } else {
 833                     opposing = false;
 834                     if (SGGeodesy::distanceM(other, taxiNode->getGeod()) > 200) // 2.0*i->getRadius())
 835                     {
 836                         needsToWait = false;
 837                         //cerr << "Hold check 3 : " << id <<"  Other aircraft approaching node is still far away. (" << dist << " nm). Can safely continue "
 838                         //           << endl;
 839                     } else {
 840                         needsToWait = true;
 841                         //cerr << "Hold check 4: " << id << "  Would need to wait for other aircraft : distance = " << dist << " meters" << endl;
 842                     }
 843                 }
 844
 845                 double dist =
 846                     SGGeodesy::distanceM(curr, taxiNode->getGeod());
 847                 if (!(i->hasHoldPosition())) {
 848
 849                     if ((dist < 200) && //2.5*current->getRadius()) &&
 850                         (needsToWait) && (i->onRoute(this, *current)) &&
 851                         //((i->onRoute(this, *current)) || ((!(i->getSpeedAdjustment())))) &&
 852                         (!(current->getId() == i->getWaitsForId())))
 853                         //(!(i->getSpeedAdjustment()))) // &&
 854                         //(!(current->getSpeedAdjustment())))
 855
 856                     {
 857                         current->setHoldPosition(true);
 858                         current->setWaitsForId(i->getId());
 859                         //cerr << "Hold check 5: " << current->getCallSign() <<"  Setting Hold Position: distance to node ("  << node << ") "
 860                         //           << dist << " meters. Waiting for " << i->getCallSign();
 861                         //if (opposing)
 862                         //cerr <<" [opposing] " << endl;
 863                         //else
 864                         //        cerr << "[non-opposing] " << endl;
 865                         //if (i->hasSpeefAdjustment())
 866                         //        {
 867                         //          cerr << " (which in turn waits for ) " << i->
 868                     } else {
 869                         //cerr << "Hold check 6: " << id << "  No need to hold yet: Distance to node : " << dist << " nm"<< endl;
 870                     }
 871                 }
 872             }
 873         }
 874     }
 875     bool currStatus = current->hasHoldPosition();
 876
 877     // Either a Hold Position or a resume taxi transmission has been issued
 878     time_t now = time(NULL) + fgGetLong("/sim/time/warp");
 879     if ((now - lastTransmission) > 2) {
 880         available = true;
 881     }
 882     if ((origStatus != currStatus) && available) {
 883         //cerr << "Issueing hold short instrudtion " << currStatus << " " << available << endl;
 884         if (currStatus == true) { // No has a hold short instruction
 885            transmit(&(*current), MSG_HOLD_POSITION, ATC_GROUND_TO_AIR, true);
 886            //cerr << "Transmittin hold short instrudtion " << currStatus << " " << available << endl;
 887            current->setState(1);
 888         } else {
 889            transmit(&(*current), MSG_RESUME_TAXI, ATC_GROUND_TO_AIR, true);
 890            //cerr << "Transmittig resume instrudtion " << currStatus << " " << available << endl;
 891            current->setState(2);
 892         }
 893         lastTransmission = now;
 894         available = false;
 895         // Don't act on the changed instruction until the transmission is confirmed
 896         // So set back to original status
 897         current->setHoldPosition(origStatus);
 898         //cerr << "Current state " << current->getState() << endl;
 899     } else {
 900     }
 901     //int state = current->getState();
 902     if (checkTransmissionState(1,1, current, now, MSG_ACKNOWLEDGE_HOLD_POSITION, ATC_AIR_TO_GROUND)) {
 903             current->setState(0);
 904             current->setHoldPosition(true);
 905     }
 906     if (checkTransmissionState(2,2, current, now, MSG_ACKNOWLEDGE_RESUME_TAXI, ATC_AIR_TO_GROUND)) {
 907             current->setState(0);
 908             current->setHoldPosition(false);
 909     }
 910
 911     /*if ((state == 1) && (available)) {
 912         //cerr << "ACKNOWLEDGE HOLD" << endl;
 913         transmit(&(*current), MSG_ACKNOWLEDGE_HOLD_POSITION, ATC_AIR_TO_GROUND, true);
 914         current->setState(0);
 915         current->setHoldPosition(true);
 916         lastTransmission = now;
 917         available = false;
 918
 919     }
 920     if ((state == 2) && (available)) {
 921         //cerr << "ACKNOWLEDGE RESUME" << endl;
 922         transmit(&(*current), MSG_ACKNOWLEDGE_RESUME_TAXI, ATC_AIR_TO_GROUND, true);
 923         current->setState(0);
 924         current->setHoldPosition(false);
 925         lastTransmission = now;
 926         available = false;
 927     }*/
 928 }
 929
 930 /**
 931  * Check whether situations occur where the current aircraft is waiting for itself
 932  * due to higher order interactions.
 933  * A 'circular' wait is a situation where a waits for b, b waits for c, and c waits
 934  * for a. Ideally each aircraft only waits for one other aircraft, so by tracing
 935  * through this list of waiting aircraft, we can check if we'd eventually end back
 936  * at the current aircraft.
 937  *
 938  * Note that we should consider the situation where we are actually checking aircraft
 939  * d, which is waiting for aircraft a. d is not part of the loop, but is held back by
 940  * the looping aircraft. If we don't check for that, this function will get stuck into
 941  * endless loop.
 942  */
 943
 944 bool FGGroundNetwork::checkForCircularWaits(int id)
 945 {
 946     //cerr << "Performing Wait check " << id << endl;
 947     int target = 0;
 948     TrafficVectorIterator current, other;
 949     TrafficVectorIterator i = activeTraffic.begin();
 950     int trafficSize = activeTraffic.size();
 951     if (trafficSize) {
 952         while (i != activeTraffic.end()) {
 953             if (i->getId() == id) {
 954                 break;
 955             }
 956             i++;
 957         }
 958     } else {
 959         return false;
 960     }
 961     if (i == activeTraffic.end() || (trafficSize == 0)) {
 962         SG_LOG(SG_GENERAL, SG_ALERT,
 963                "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits");
 964     }
 965
 966     current = i;
 967     target = current->getWaitsForId();
 968     //bool printed = false; // Note that this variable is for debugging purposes only.
 969     int counter = 0;
 970
 971     if (id == target) {
 972         //cerr << "aircraft waits for user" << endl;
 973         return false;
 974     }
 975
 976
 977     while ((target > 0) && (target != id) && counter++ < trafficSize) {
 978         //printed = true;
 979         TrafficVectorIterator i = activeTraffic.begin();
 980         if (trafficSize) {
 981             //while ((i->getId() != id) && i != activeTraffic.end())
 982             while (i != activeTraffic.end()) {
 983                 if (i->getId() == target) {
 984                     break;
 985                 }
 986                 i++;
 987             }
 988         } else {
 989             return false;
 990         }
 991         if (i == activeTraffic.end() || (trafficSize == 0)) {
 992             //cerr << "[Waiting for traffic at Runway: DONE] " << endl << endl;;
 993             // The target id is not found on the current network, which means it's at the tower
 994             //SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits");
 995             return false;
 996         }
 997         other = i;
 998         target = other->getWaitsForId();
 999
1000         // actually this trap isn't as impossible as it first seemed:
1001         // the setWaitsForID(id) is set to current when the aircraft
1002         // is waiting for the user controlled aircraft.
1003         //if (current->getId() == other->getId()) {
1004         //    cerr << "Caught the impossible trap" << endl;
1005         //    cerr << "Current = " << current->getId() << endl;
1006         //    cerr << "Other   = " << other  ->getId() << endl;
1007         //    for (TrafficVectorIterator at = activeTraffic.begin();
1008         //          at != activeTraffic.end();
1009         //          at++) {
1010         //        cerr << "currently active aircraft : " << at->getCallSign() << " with Id " << at->getId() << " waits for " << at->getWaitsForId() << endl;
1011         //    }
1012         //    exit(1);
1013         if (current->getId() == other->getId())
1014             return false;
1015         //}
1016         //cerr << current->getCallSign() << " (" << current->getId()  << ") " << " -> " << other->getCallSign()
1017         //     << " (" << other->getId()  << "); " << endl;;
1018         //current = other;
1019     }
1020
1021
1022
1023
1024
1025
1026     //if (printed)
1027     //   cerr << "[done] " << endl << endl;;
1028     if (id == target) {
1029         SG_LOG(SG_GENERAL, SG_WARN,
1030                "Detected circular wait condition: Id = " << id <<
1031                "target = " << target);
1032         return true;
1033     } else {
1034         return false;
1035     }
1036 }
1037
1038 // Note that this function is probably obsolete...
1039 bool FGGroundNetwork::hasInstruction(int id)
1040 {
1041     TrafficVectorIterator i = activeTraffic.begin();
1042     // Search search if the current id has an entry
1043     // This might be faster using a map instead of a vector, but let's start by taking a safe route
1044     if (activeTraffic.size()) {
1045         //while ((i->getId() != id) && i != activeTraffic.end()) {
1046         while (i != activeTraffic.end()) {
1047             if (i->getId() == id) {
1048                 break;
1049             }
1050             i++;
1051         }
1052     }
1053     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
1054         SG_LOG(SG_GENERAL, SG_ALERT,
1055                "AI error: checking ATC instruction for aircraft without traffic record");
1056     } else {
1057         return i->hasInstruction();
1058     }
1059     return false;
1060 }
1061
1062 FGATCInstruction FGGroundNetwork::getInstruction(int id)
1063 {
1064     TrafficVectorIterator i = activeTraffic.begin();
1065     // Search search if the current id has an entry
1066     // This might be faster using a map instead of a vector, but let's start by taking a safe route
1067     if (activeTraffic.size()) {
1068         //while ((i->getId() != id) && i != activeTraffic.end()) {
1069         while (i != activeTraffic.end()) {
1070             if (i->getId() == id) {
1071                 break;
1072             }
1073             i++;
1074         }
1075     }
1076     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
1077         SG_LOG(SG_GENERAL, SG_ALERT,
1078                "AI error: requesting ATC instruction for aircraft without traffic record");
1079     } else {
1080         return i->getInstruction();
1081     }
1082     return FGATCInstruction();
1083 }
1084
1085 // Note that this function is copied from simgear. for maintanance purposes, it's probabtl better to make a general function out of that.
1086 static void WorldCoordinate(osg::Matrix& obj_pos, double lat,
1087                             double lon, double elev, double hdg)
1088 {
1089     SGGeod geod = SGGeod::fromDegM(lon, lat, elev);
1090     obj_pos = geod.makeZUpFrame();
1091     // hdg is not a compass heading, but a counter-clockwise rotation
1092     // around the Z axis
1093     obj_pos.preMult(osg::Matrix::rotate(hdg * SGD_DEGREES_TO_RADIANS,
1094                                         0.0, 0.0, 1.0));
1095 }
1096
1097
1098
1099 osg::Node* FGGroundNetwork::getRenderNode()
1100 {
1101     osg::Group* group = new osg::Group;
1102
1103     for ( FGTaxiSegmentVectorIterator i = segments.begin(); i != segments.end(); i++) {
1104         osg::Matrix obj_pos;
1105         osg::MatrixTransform *obj_trans = new osg::MatrixTransform;
1106         obj_trans->setDataVariance(osg::Object::STATIC);
1107         WorldCoordinate( obj_pos, (*i)->getLatitude(), (*i)->getLongitude(), parent->elevation()+10, -((*i)->getHeading()) );
1108
1109                 obj_trans->setMatrix( obj_pos );
1110         //osg::Vec3 center(0, 0, 0)
1111
1112         float width = (*i)->getLength() /2.0;
1113         osg::Vec3 corner(-width, 0, 0.25f);
1114         osg::Vec3 widthVec(2*width + 1, 0, 0);
1115         osg::Vec3 heightVec(0, 0, 1);
1116         osg::Geometry* geometry;
1117         geometry = osg::createTexturedQuadGeometry(corner, widthVec, heightVec);
1118         simgear::EffectGeode* geode = new simgear::EffectGeode;
1119         geode->setName("test");
1120         geode->addDrawable(geometry);
1121         //osg::Node *custom_obj;
1122
1123         obj_trans->addChild(geode);
1124             // wire as much of the scene graph together as we can
1125         //->addChild( obj_trans );
1126         group->addChild( obj_trans );
1127     }
1128
1129
1130     return group;
1131 }