src/Airports/groundnetwork.cxx

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