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         if (towerController->hasActiveTraffic()) {
 714             for (TrafficVectorIterator i =
 715                  towerController->getActiveTraffic().begin();
 716                  i != towerController->getActiveTraffic().end(); i++) {
 717                 //cerr << "Comparing " << current->getId() << " and " << i->getId() << endl;
 718                 SGGeod other(SGGeod::fromDegM(i->getLongitude(),
 719                                               i->getLatitude(),
 720                                               i->getAltitude()));
 721                 SGGeodesy::inverse(curr, other, course, az2, dist);
 722                 bearing = fabs(heading - course);
 723                 if (bearing > 180)
 724                     bearing = 360 - bearing;
 725                 if ((dist < mindist) && (bearing < 60.0)) {
 726                     mindist = dist;
 727                     closest = i;
 728                     minbearing = bearing;
 729                     otherReasonToSlowDown = true;
 730                 }
 731             }
 732         }
 733         // Finally, check UserPosition
 734         double userLatitude = fgGetDouble("/position/latitude-deg");
 735         double userLongitude = fgGetDouble("/position/longitude-deg");
 736         SGGeod user(SGGeod::fromDeg(userLongitude, userLatitude));
 737         SGGeodesy::inverse(curr, user, course, az2, dist);
 738
 739         bearing = fabs(heading - course);
 740         if (bearing > 180)
 741             bearing = 360 - bearing;
 742         if ((dist < mindist) && (bearing < 60.0)) {
 743             mindist = dist;
 744             //closest = i;
 745             minbearing = bearing;
 746             otherReasonToSlowDown = true;
 747         }
 748
 749         current->clearSpeedAdjustment();
 750
 751         if (current->checkPositionAndIntentions(*closest)
 752             || otherReasonToSlowDown) {
 753             double maxAllowableDistance =
 754                 (1.1 * current->getRadius()) +
 755                 (1.1 * closest->getRadius());
 756             if (mindist < 2 * maxAllowableDistance) {
 757                 if (current->getId() == closest->getWaitsForId())
 758                     return;
 759                 else
 760                     current->setWaitsForId(closest->getId());
 761                 if (closest->getId() != current->getId())
 762                     current->setSpeedAdjustment(closest->getSpeed() *
 763                                                 (mindist / 100));
 764                 else
 765                     current->setSpeedAdjustment(0);     // This can only happen when the user aircraft is the one closest
 766                 if (mindist < maxAllowableDistance) {
 767                     //double newSpeed = (maxAllowableDistance-mindist);
 768                     //current->setSpeedAdjustment(newSpeed);
 769                     //if (mindist < 0.5* maxAllowableDistance)
 770                     //  {
 771                     current->setSpeedAdjustment(0);
 772                     //  }
 773                 }
 774             }
 775         }
 776     }
 777 }
 778
 779 /**
 780    Check for "Hold position instruction".
 781    The hold position should be issued under the following conditions:
 782    1) For aircraft entering or crossing a runway with active traffic on it, or landing aircraft near it
 783    2) For taxiing aircraft that use one taxiway in opposite directions
 784    3) For crossing or merging taxiroutes.
 785 */
 786
 787 void FGGroundNetwork::checkHoldPosition(int id, double lat,
 788                                         double lon, double heading,
 789                                         double speed, double alt)
 790 {
 791     TrafficVectorIterator current;
 792     TrafficVectorIterator i = activeTraffic.begin();
 793     if (activeTraffic.size()) {
 794         //while ((i->getId() != id) && i != activeTraffic.end())
 795         while (i != activeTraffic.end()) {
 796             if (i->getId() == id) {
 797                 break;
 798             }
 799             i++;
 800         }
 801     } else {
 802         return;
 803     }
 804     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 805         SG_LOG(SG_GENERAL, SG_ALERT,
 806                "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkHoldPosition");
 807     }
 808     current = i;
 809     bool origStatus = current->hasHoldPosition();
 810     current->setHoldPosition(false);
 811     SGGeod curr(SGGeod::fromDegM(lon, lat, alt));
 812
 813     for (i = activeTraffic.begin(); i != activeTraffic.end(); i++) {
 814         if (i->getId() != current->getId()) {
 815             int node = current->crosses(this, *i);
 816             if (node != -1) {
 817                 FGTaxiNode *taxiNode = findNode(node);
 818
 819                 // Determine whether it's save to continue or not.
 820                 // If we have a crossing route, there are two possibilities:
 821                 // 1) This is an interestion
 822                 // 2) This is oncoming two-way traffic, using the same taxiway.
 823                 //cerr << "Hold check 1 : " << id << " has common node " << node << endl;
 824
 825                 SGGeod other(SGGeod::
 826                              fromDegM(i->getLongitude(), i->getLatitude(),
 827                                       i->getAltitude()));
 828                 bool needsToWait;
 829                 bool opposing;
 830                 if (current->isOpposing(this, *i, node)) {
 831                     needsToWait = true;
 832                     opposing = true;
 833                     //cerr << "Hold check 2 : " << node << "  has opposing segment " << endl;
 834                     // issue a "Hold Position" as soon as we're close to the offending node
 835                     // For now, I'm doing this as long as the other aircraft doesn't
 836                     // have a hold instruction as soon as we're within a reasonable
 837                     // distance from the offending node.
 838                     // This may be a bit of a conservative estimate though, as it may
 839                     // be well possible that both aircraft can both continue to taxi
 840                     // without crashing into each other.
 841                 } else {
 842                     opposing = false;
 843                     if (SGGeodesy::distanceM(other, taxiNode->getGeod()) > 200) // 2.0*i->getRadius())
 844                     {
 845                         needsToWait = false;
 846                         //cerr << "Hold check 3 : " << id <<"  Other aircraft approaching node is still far away. (" << dist << " nm). Can safely continue "
 847                         //           << endl;
 848                     } else {
 849                         needsToWait = true;
 850                         //cerr << "Hold check 4: " << id << "  Would need to wait for other aircraft : distance = " << dist << " meters" << endl;
 851                     }
 852                 }
 853
 854                 double dist =
 855                     SGGeodesy::distanceM(curr, taxiNode->getGeod());
 856                 if (!(i->hasHoldPosition())) {
 857
 858                     if ((dist < 200) && //2.5*current->getRadius()) &&
 859                         (needsToWait) && (i->onRoute(this, *current)) &&
 860                         //((i->onRoute(this, *current)) || ((!(i->getSpeedAdjustment())))) &&
 861                         (!(current->getId() == i->getWaitsForId())))
 862                         //(!(i->getSpeedAdjustment()))) // &&
 863                         //(!(current->getSpeedAdjustment())))
 864
 865                     {
 866                         current->setHoldPosition(true);
 867                         current->setWaitsForId(i->getId());
 868                         //cerr << "Hold check 5: " << current->getCallSign() <<"  Setting Hold Position: distance to node ("  << node << ") "
 869                         //           << dist << " meters. Waiting for " << i->getCallSign();
 870                         //if (opposing)
 871                         //cerr <<" [opposing] " << endl;
 872                         //else
 873                         //        cerr << "[non-opposing] " << endl;
 874                         //if (i->hasSpeefAdjustment())
 875                         //        {
 876                         //          cerr << " (which in turn waits for ) " << i->
 877                     } else {
 878                         //cerr << "Hold check 6: " << id << "  No need to hold yet: Distance to node : " << dist << " nm"<< endl;
 879                     }
 880                 }
 881             }
 882         }
 883     }
 884     bool currStatus = current->hasHoldPosition();
 885
 886     // Either a Hold Position or a resume taxi transmission has been issued
 887     time_t now = time(NULL) + fgGetLong("/sim/time/warp");
 888     if ((now - lastTransmission) > 2) {
 889         available = true;
 890     }
 891     if ((origStatus != currStatus) && available) {
 892         //cerr << "Issueing hold short instrudtion " << currStatus << " " << available << endl;
 893         if (currStatus == true) { // No has a hold short instruction
 894            transmit(&(*current), MSG_HOLD_POSITION, ATC_GROUND_TO_AIR, true);
 895            //cerr << "Transmittin hold short instrudtion " << currStatus << " " << available << endl;
 896            current->setState(1);
 897         } else {
 898            transmit(&(*current), MSG_RESUME_TAXI, ATC_GROUND_TO_AIR, true);
 899            //cerr << "Transmittig resume instrudtion " << currStatus << " " << available << endl;
 900            current->setState(2);
 901         }
 902         lastTransmission = now;
 903         available = false;
 904         // Don't act on the changed instruction until the transmission is confirmed
 905         // So set back to original status
 906         current->setHoldPosition(origStatus);
 907         //cerr << "Current state " << current->getState() << endl;
 908     } else {
 909     }
 910     //int state = current->getState();
 911     if (checkTransmissionState(1,1, current, now, MSG_ACKNOWLEDGE_HOLD_POSITION, ATC_AIR_TO_GROUND)) {
 912             current->setState(0);
 913             current->setHoldPosition(true);
 914     }
 915     if (checkTransmissionState(2,2, current, now, MSG_ACKNOWLEDGE_RESUME_TAXI, ATC_AIR_TO_GROUND)) {
 916             current->setState(0);
 917             current->setHoldPosition(false);
 918     }
 919 }
 920
 921 /**
 922  * Check whether situations occur where the current aircraft is waiting for itself
 923  * due to higher order interactions.
 924  * A 'circular' wait is a situation where a waits for b, b waits for c, and c waits
 925  * for a. Ideally each aircraft only waits for one other aircraft, so by tracing
 926  * through this list of waiting aircraft, we can check if we'd eventually end back
 927  * at the current aircraft.
 928  *
 929  * Note that we should consider the situation where we are actually checking aircraft
 930  * d, which is waiting for aircraft a. d is not part of the loop, but is held back by
 931  * the looping aircraft. If we don't check for that, this function will get stuck into
 932  * endless loop.
 933  */
 934
 935 bool FGGroundNetwork::checkForCircularWaits(int id)
 936 {
 937     //cerr << "Performing Wait check " << id << endl;
 938     int target = 0;
 939     TrafficVectorIterator current, other;
 940     TrafficVectorIterator i = activeTraffic.begin();
 941     int trafficSize = activeTraffic.size();
 942     if (trafficSize) {
 943         while (i != activeTraffic.end()) {
 944             if (i->getId() == id) {
 945                 break;
 946             }
 947             i++;
 948         }
 949     } else {
 950         return false;
 951     }
 952     if (i == activeTraffic.end() || (trafficSize == 0)) {
 953         SG_LOG(SG_GENERAL, SG_ALERT,
 954                "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits");
 955     }
 956
 957     current = i;
 958     target = current->getWaitsForId();
 959     //bool printed = false; // Note that this variable is for debugging purposes only.
 960     int counter = 0;
 961
 962     if (id == target) {
 963         //cerr << "aircraft waits for user" << endl;
 964         return false;
 965     }
 966
 967
 968     while ((target > 0) && (target != id) && counter++ < trafficSize) {
 969         //printed = true;
 970         TrafficVectorIterator i = activeTraffic.begin();
 971         if (trafficSize) {
 972             //while ((i->getId() != id) && i != activeTraffic.end())
 973             while (i != activeTraffic.end()) {
 974                 if (i->getId() == target) {
 975                     break;
 976                 }
 977                 i++;
 978             }
 979         } else {
 980             return false;
 981         }
 982         if (i == activeTraffic.end() || (trafficSize == 0)) {
 983             //cerr << "[Waiting for traffic at Runway: DONE] " << endl << endl;;
 984             // The target id is not found on the current network, which means it's at the tower
 985             //SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits");
 986             return false;
 987         }
 988         other = i;
 989         target = other->getWaitsForId();
 990
 991         // actually this trap isn't as impossible as it first seemed:
 992         // the setWaitsForID(id) is set to current when the aircraft
 993         // is waiting for the user controlled aircraft.
 994         //if (current->getId() == other->getId()) {
 995         //    cerr << "Caught the impossible trap" << endl;
 996         //    cerr << "Current = " << current->getId() << endl;
 997         //    cerr << "Other   = " << other  ->getId() << endl;
 998         //    for (TrafficVectorIterator at = activeTraffic.begin();
 999         //          at != activeTraffic.end();
1000         //          at++) {
1001         //        cerr << "currently active aircraft : " << at->getCallSign() << " with Id " << at->getId() << " waits for " << at->getWaitsForId() << endl;
1002         //    }
1003         //    exit(1);
1004         if (current->getId() == other->getId())
1005             return false;
1006         //}
1007         //cerr << current->getCallSign() << " (" << current->getId()  << ") " << " -> " << other->getCallSign()
1008         //     << " (" << other->getId()  << "); " << endl;;
1009         //current = other;
1010     }
1011
1012
1013
1014
1015
1016
1017     //if (printed)
1018     //   cerr << "[done] " << endl << endl;;
1019     if (id == target) {
1020         SG_LOG(SG_GENERAL, SG_WARN,
1021                "Detected circular wait condition: Id = " << id <<
1022                "target = " << target);
1023         return true;
1024     } else {
1025         return false;
1026     }
1027 }
1028
1029 // Note that this function is probably obsolete...
1030 bool FGGroundNetwork::hasInstruction(int id)
1031 {
1032     TrafficVectorIterator i = activeTraffic.begin();
1033     // Search search if the current id has an entry
1034     // This might be faster using a map instead of a vector, but let's start by taking a safe route
1035     if (activeTraffic.size()) {
1036         //while ((i->getId() != id) && i != activeTraffic.end()) {
1037         while (i != activeTraffic.end()) {
1038             if (i->getId() == id) {
1039                 break;
1040             }
1041             i++;
1042         }
1043     }
1044     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
1045         SG_LOG(SG_GENERAL, SG_ALERT,
1046                "AI error: checking ATC instruction for aircraft without traffic record");
1047     } else {
1048         return i->hasInstruction();
1049     }
1050     return false;
1051 }
1052
1053 FGATCInstruction FGGroundNetwork::getInstruction(int id)
1054 {
1055     TrafficVectorIterator i = activeTraffic.begin();
1056     // Search search if the current id has an entry
1057     // This might be faster using a map instead of a vector, but let's start by taking a safe route
1058     if (activeTraffic.size()) {
1059         //while ((i->getId() != id) && i != activeTraffic.end()) {
1060         while (i != activeTraffic.end()) {
1061             if (i->getId() == id) {
1062                 break;
1063             }
1064             i++;
1065         }
1066     }
1067     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
1068         SG_LOG(SG_GENERAL, SG_ALERT,
1069                "AI error: requesting ATC instruction for aircraft without traffic record");
1070     } else {
1071         return i->getInstruction();
1072     }
1073     return FGATCInstruction();
1074 }
1075
1076 // Note that this function is copied from simgear. for maintanance purposes, it's probabtl better to make a general function out of that.
1077 static void WorldCoordinate(osg::Matrix& obj_pos, double lat,
1078                             double lon, double elev, double hdg)
1079 {
1080     SGGeod geod = SGGeod::fromDegM(lon, lat, elev);
1081     obj_pos = geod.makeZUpFrame();
1082     // hdg is not a compass heading, but a counter-clockwise rotation
1083     // around the Z axis
1084     obj_pos.preMult(osg::Matrix::rotate(hdg * SGD_DEGREES_TO_RADIANS,
1085                                         0.0, 0.0, 1.0));
1086 }
1087
1088
1089
1090
1091 void FGGroundNetwork::render(bool visible)
1092 {
1093
1094     SGMaterialLib *matlib = globals->get_matlib();
1095     if (group) {
1096         //int nr = ;
1097         globals->get_scenery()->get_scene_graph()->removeChild(group);
1098         //while (group->getNumChildren()) {
1099         //  cerr << "Number of children: " << group->getNumChildren() << endl;
1100         simgear::EffectGeode* geode = (simgear::EffectGeode*) group->getChild(0);
1101           //osg::MatrixTransform *obj_trans = (osg::MatrixTransform*) group->getChild(0);
1102            //geode->releaseGLObjects();
1103            //group->removeChild(geode);
1104            //delete geode;
1105         group = 0;
1106     }
1107     if (visible) {
1108         group = new osg::Group;
1109
1110         //for ( FGTaxiSegmentVectorIterator i = segments.begin(); i != segments.end(); i++) {
1111         double dx = 0;
1112         for   (TrafficVectorIterator i = activeTraffic.begin(); i != activeTraffic.end(); i++) {
1113             // Handle start point
1114             int pos = i->getCurrentPosition() - 1;
1115             if (pos >= 0) {
1116
1117                 SGGeod start(SGGeod::fromDeg((i->getLongitude()), (i->getLatitude())));
1118                 SGGeod end  (SGGeod::fromDeg(segments[pos]->getEnd()->getLongitude(), segments[pos]->getEnd()->getLatitude()));
1119
1120                 double length = SGGeodesy::distanceM(start, end);
1121                 //heading = SGGeodesy::headingDeg(start->getGeod(), end->getGeod());
1122
1123                 double az2, heading; //, distanceM;
1124                 SGGeodesy::inverse(start, end, heading, az2, length);
1125                 double coveredDistance = length * 0.5;
1126                 SGGeod center;
1127                 SGGeodesy::direct(start, heading, coveredDistance, center, az2);
1128                 //cerr << "Active Aircraft : Centerpoint = (" << center.getLatitudeDeg() << ", " << center.getLongitudeDeg() << "). Heading = " << heading << endl;
1129             ///////////////////////////////////////////////////////////////////////////////
1130                 // Make a helper function out of this
1131                 osg::Matrix obj_pos;
1132                 osg::MatrixTransform *obj_trans = new osg::MatrixTransform;
1133                 obj_trans->setDataVariance(osg::Object::STATIC);
1134
1135                 WorldCoordinate( obj_pos, center.getLatitudeDeg(), center.getLongitudeDeg(), parent->elevation()+8+dx, -(heading) );
1136
1137                 obj_trans->setMatrix( obj_pos );
1138                 //osg::Vec3 center(0, 0, 0)
1139
1140                 float width = length /2.0;
1141                 osg::Vec3 corner(-width, 0, 0.25f);
1142                 osg::Vec3 widthVec(2*width + 1, 0, 0);
1143                 osg::Vec3 heightVec(0, 1, 0);
1144                 osg::Geometry* geometry;
1145                 geometry = osg::createTexturedQuadGeometry(corner, widthVec, heightVec);
1146                 simgear::EffectGeode* geode = new simgear::EffectGeode;
1147                 geode->setName("test");
1148                 geode->addDrawable(geometry);
1149                 //osg::Node *custom_obj;
1150                 SGMaterial *mat = matlib->find("UnidirectionalTaper");
1151                 if (mat)
1152                     geode->setEffect(mat->get_effect());
1153                 obj_trans->addChild(geode);
1154                 // wire as much of the scene graph together as we can
1155                 //->addChild( obj_trans );
1156                 group->addChild( obj_trans );
1157             /////////////////////////////////////////////////////////////////////
1158             } else {
1159                 //cerr << "BIG FAT WARNING: current position is here : " << pos << endl;
1160             }
1161             for(intVecIterator j = (i)->getIntentions().begin(); j != (i)->getIntentions().end(); j++) {
1162                 osg::Matrix obj_pos;
1163                 int k = (*j)-1;
1164                 if (k >= 0) {
1165                     osg::MatrixTransform *obj_trans = new osg::MatrixTransform;
1166                     obj_trans->setDataVariance(osg::Object::STATIC);
1167
1168                     WorldCoordinate( obj_pos, segments[k]->getLatitude(), segments[k]->getLongitude(), parent->elevation()+8+dx, -(segments[k]->getHeading()) );
1169
1170                     obj_trans->setMatrix( obj_pos );
1171                     //osg::Vec3 center(0, 0, 0)
1172
1173                     float width = segments[k]->getLength() /2.0;
1174                     osg::Vec3 corner(-width, 0, 0.25f);
1175                     osg::Vec3 widthVec(2*width + 1, 0, 0);
1176                     osg::Vec3 heightVec(0, 1, 0);
1177                     osg::Geometry* geometry;
1178                     geometry = osg::createTexturedQuadGeometry(corner, widthVec, heightVec);
1179                     simgear::EffectGeode* geode = new simgear::EffectGeode;
1180                     geode->setName("test");
1181                     geode->addDrawable(geometry);
1182                     //osg::Node *custom_obj;
1183                     SGMaterial *mat = matlib->find("UnidirectionalTaper");
1184                     if (mat)
1185                         geode->setEffect(mat->get_effect());
1186                     obj_trans->addChild(geode);
1187                     // wire as much of the scene graph together as we can
1188                     //->addChild( obj_trans );
1189                     group->addChild( obj_trans );
1190                 }
1191             }
1192             //dx += 0.1;
1193         }
1194         globals->get_scenery()->get_scene_graph()->addChild(group);
1195     }
1196 }
1197
1198 string FGGroundNetwork::getName() {
1199     return string(parent->getId() + "-ground");
1200 }