X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FAirports%2Fgroundnetwork.cxx;h=a6ad1f5c44a8c0f4653d3314814f1cb7e643581e;hb=076f61f2b7e1f774bc827f34ab4a24dcc1998571;hp=a3a3245fe9af7d711be60af249d1cd22abc483d7;hpb=894f066370ee71b36577f25f30d77378149a60eb;p=flightgear.git diff --git a/src/Airports/groundnetwork.cxx b/src/Airports/groundnetwork.cxx index a3a3245fe..a6ad1f5c4 100644 --- a/src/Airports/groundnetwork.cxx +++ b/src/Airports/groundnetwork.cxx @@ -24,9 +24,11 @@ # include #endif -#include +#include #include - +#include +#include +#include #include #include @@ -34,16 +36,21 @@ #include #include -#include #include #include #include +#include +#include +#include -#include +#include #include +#include #include +#include #include +#include #include @@ -51,169 +58,126 @@ #include "groundnetwork.hxx" +using std::string; +using flightgear::NavDataCache; + /*************************************************************************** * FGTaxiSegment **************************************************************************/ -void FGTaxiSegment::setStart(FGTaxiNodeVector * nodes) +FGTaxiSegment::FGTaxiSegment(PositionedID aStart, PositionedID aEnd) : + startNode(aStart), + endNode(aEnd), + isActive(0), + index(0), + oppositeDirection(0) { - FGTaxiNodeVectorIterator i = nodes->begin(); - while (i != nodes->end()) { - //cerr << "Scanning start node index" << (*i)->getIndex() << endl; - if ((*i)->getIndex() == startNode) { - start = (*i)->getAddress(); - (*i)->addSegment(this); - return; - } - i++; - } - SG_LOG(SG_GENERAL, SG_ALERT, - "Could not find start node " << startNode << endl); +}; + +SGGeod FGTaxiSegment::getCenter() const +{ + FGTaxiNode* start(getStart()), *end(getEnd()); + double heading, length, az2; + SGGeodesy::inverse(start->geod(), end->geod(), heading, az2, length); + return SGGeodesy::direct(start->geod(), heading, length * 0.5); } -void FGTaxiSegment::setEnd(FGTaxiNodeVector * nodes) +FGTaxiNodeRef FGTaxiSegment::getEnd() const { - FGTaxiNodeVectorIterator i = nodes->begin(); - while (i != nodes->end()) { - //cerr << "Scanning end node index" << (*i)->getIndex() << endl; - if ((*i)->getIndex() == endNode) { - end = (*i)->getAddress(); - return; - } - i++; - } - SG_LOG(SG_GENERAL, SG_ALERT, - "Could not find end node " << endNode << endl); + return FGPositioned::loadById(endNode); +} + +FGTaxiNodeRef FGTaxiSegment::getStart() const +{ + return FGPositioned::loadById(startNode); } +double FGTaxiSegment::getLength() const +{ + return dist(getStart()->cart(), getEnd()->cart()); +} + +double FGTaxiSegment::getHeading() const +{ + return SGGeodesy::courseDeg(getStart()->geod(), getEnd()->geod()); +} -// There is probably a computationally cheaper way of -// doing this. -void FGTaxiSegment::setDimensions(double elevation) +void FGTaxiSegment::block(int id, time_t blockTime, time_t now) { - length = SGGeodesy::distanceM(start->getGeod(), end->getGeod()); - //heading = SGGeodesy::headingDeg(start->getGeod(), end->getGeod()); - - double az2; //, distanceM; - SGGeodesy::inverse(start->getGeod(), end->getGeod(), heading, az2, length); - double coveredDistance = length * 0.5; - SGGeodesy::direct(start->getGeod(), heading, coveredDistance, center, az2); - //start->setElevation(elevation); - //end->setElevation(elevation); - //cerr << "Centerpoint = (" << center.getLatitudeDeg() << ", " << center.getLongitudeDeg() << "). Heading = " << heading << endl; + BlockListIterator i = blockTimes.begin(); + while (i != blockTimes.end()) { + if (i->getId() == id) + break; + i++; + } + if (i == blockTimes.end()) { + blockTimes.push_back(Block(id, blockTime, now)); + sort(blockTimes.begin(), blockTimes.end()); + } else { + i->updateTimeStamps(blockTime, now); + } } +// The segment has a block if any of the block times listed in the block list is +// smaller than the current time. +bool FGTaxiSegment::hasBlock(time_t now) +{ + for (BlockListIterator i = blockTimes.begin(); i != blockTimes.end(); i++) { + if (i->getBlockTime() < now) + return true; + } + return false; +} -//void FGTaxiSegment::setCourseDiff(double crse) -//{ -// headingDiff = fabs(course - crse); +void FGTaxiSegment::unblock(time_t now) +{ + if (blockTimes.size()) { + BlockListIterator i = blockTimes.begin(); + if (i->getTimeStamp() < (now - 30)) { + blockTimes.erase(i); + } + } +} -// if (headingDiff > 180) -// headingDiff = fabs(headingDiff - 360); -//} /*************************************************************************** * FGTaxiRoute **************************************************************************/ -bool FGTaxiRoute::next(int *nde) -{ - //for (intVecIterator i = nodes.begin(); i != nodes.end(); i++) - // cerr << "FGTaxiRoute contains : " << *(i) << endl; - //cerr << "Offset from end: " << nodes.end() - currNode << endl; - //if (currNode != nodes.end()) - // cerr << "true" << endl; - //else - // cerr << "false" << endl; - //if (nodes.size() != (routes.size()) +1) - // cerr << "ALERT: Misconfigured TaxiRoute : " << nodes.size() << " " << routes.size() << endl; - - if (currNode == nodes.end()) - return false; - *nde = *(currNode); - if (currNode != nodes.begin()) // make sure route corresponds to the end node - currRoute++; - currNode++; - return true; -}; - -bool FGTaxiRoute::next(int *nde, int *rte) +bool FGTaxiRoute::next(PositionedID *nde, int *rte) { - //for (intVecIterator i = nodes.begin(); i != nodes.end(); i++) - // cerr << "FGTaxiRoute contains : " << *(i) << endl; - //cerr << "Offset from end: " << nodes.end() - currNode << endl; - //if (currNode != nodes.end()) - // cerr << "true" << endl; - //else - // cerr << "false" << endl; if (nodes.size() != (routes.size()) + 1) { - SG_LOG(SG_GENERAL, SG_ALERT, - "ALERT: Misconfigured TaxiRoute : " << nodes. - size() << " " << routes.size()); - exit(1); + SG_LOG(SG_GENERAL, SG_ALERT, "ALERT: Misconfigured TaxiRoute : " << nodes.size() << " " << routes.size()); + throw sg_range_exception("Misconfigured taxi route"); } if (currNode == nodes.end()) return false; *nde = *(currNode); - //*rte = *(currRoute); - if (currNode != nodes.begin()) // Make sure route corresponds to the end node - { + if (currNode != nodes.begin()) { *rte = *(currRoute); currRoute++; } else { - // If currNode points to the first node, this means the aircraft is not on the taxi node - // yet. Make sure to return a unique identifyer in this situation though, because otherwise - // the speed adjust AI code may be unable to resolve whether two aircraft are on the same - // taxi route or not. the negative of the preceding route seems a logical choice, as it is - // unique for any starting location. - // Note that this is probably just a temporary fix until I get Parking / tower control working. + // Handle special case for the first node. *rte = -1 * *(currRoute); } currNode++; return true; }; - -void FGTaxiRoute::rewind(int route) -{ - int currPoint; - int currRoute; - first(); - do { - if (!(next(&currPoint, &currRoute))) { - SG_LOG(SG_GENERAL, SG_ALERT, - "Error in rewinding TaxiRoute: current" << currRoute << - " goal " << route); - } - } while (currRoute != route); -} - - - - /*************************************************************************** * FGGroundNetwork() **************************************************************************/ -bool compare_nodes(FGTaxiNode * a, FGTaxiNode * b) -{ - return (*a) < (*b); -} - -bool compare_segments(FGTaxiSegment * a, FGTaxiSegment * b) -{ - return (*a) < (*b); -} bool compare_trafficrecords(FGTrafficRecord a, FGTrafficRecord b) { return (a.getIntentions().size() < b.getIntentions().size()); } -FGGroundNetwork::FGGroundNetwork() +FGGroundNetwork::FGGroundNetwork() : + parent(NULL) { hasNetwork = false; - foundRoute = false; totalDistance = 0; maxDistance = 0; //maxDepth = 1000; @@ -227,55 +191,27 @@ FGGroundNetwork::FGGroundNetwork() FGGroundNetwork::~FGGroundNetwork() { - //cerr << "Running Groundnetwork Destructor " << endl; - bool saveData = false; - ofstream cachefile; - if (fgGetBool("/sim/ai/groundnet-cache")) { - SGPath cacheData(fgGetString("/sim/fg-home")); - cacheData.append("ai"); - string airport = parent->getId(); - - if ((airport) != "") { - char buffer[128]; - ::snprintf(buffer, 128, "%c/%c/%c/", - airport[0], airport[1], airport[2]); - cacheData.append(buffer); - if (!cacheData.exists()) { - cacheData.create_dir(0777); - } - cacheData.append(airport + "-groundnet-cache.txt"); - cachefile.open(cacheData.str().c_str()); - saveData = true; - } - } - cachefile << "[GroundNetcachedata:ref:2011:09:04]" << endl; - for (FGTaxiNodeVectorIterator node = nodes.begin(); - node != nodes.end(); node++) { - if (saveData) { - cachefile << (*node)->getIndex () << " " - << (*node)->getElevationM (parent->getElevation()*SG_FEET_TO_METER) << " " - << endl; - } - delete(*node); - } - nodes.clear(); - pushBackNodes.clear(); - for (FGTaxiSegmentVectorIterator seg = segments.begin(); - seg != segments.end(); seg++) { - delete(*seg); - } - segments.clear(); - if (saveData) { - cachefile.close(); - } +// JMT 2012-09-8 - disabling the groundnet-caching as part of enabling the +// navcache. The problem isn't the NavCache - it's that for the past few years, +// we have not being running destructors on FGPositioned classes, and hence, +// not running this code. +// When I fix FGPositioned lifetimes (unloading-at-runtime support), this +// will need to be re-visited so it can run safely during shutdown. +#if 0 + saveElevationCache(); +#endif + BOOST_FOREACH(FGTaxiSegment* seg, segments) { + delete seg; + } } -void FGGroundNetwork::saveElevationCache() { - //cerr << "Running Groundnetwork Destructor " << endl; +void FGGroundNetwork::saveElevationCache() +{ +#if 0 bool saveData = false; ofstream cachefile; if (fgGetBool("/sim/ai/groundnet-cache")) { - SGPath cacheData(fgGetString("/sim/fg-home")); + SGPath cacheData(globals->get_fg_home()); cacheData.append("ai"); string airport = parent->getId(); @@ -285,7 +221,7 @@ void FGGroundNetwork::saveElevationCache() { airport[0], airport[1], airport[2]); cacheData.append(buffer); if (!cacheData.exists()) { - cacheData.create_dir(0777); + cacheData.create_dir(0755); } cacheData.append(airport + "-groundnet-cache.txt"); cachefile.open(cacheData.str().c_str()); @@ -293,330 +229,270 @@ void FGGroundNetwork::saveElevationCache() { } } cachefile << "[GroundNetcachedata:ref:2011:09:04]" << endl; - for (FGTaxiNodeVectorIterator node = nodes.begin(); + for (IndexTaxiNodeMap::iterator node = nodes.begin(); node != nodes.end(); node++) { if (saveData) { - cachefile << (*node)->getIndex () << " " - << (*node)->getElevationM (parent->getElevation()*SG_FEET_TO_METER) << " " + cachefile << node->second->getIndex () << " " + << node->second->getElevationM (parent->getElevation()*SG_FEET_TO_METER) << " " << endl; } } if (saveData) { cachefile.close(); } +#endif } -void FGGroundNetwork::addSegment(const FGTaxiSegment & seg) -{ - segments.push_back(new FGTaxiSegment(seg)); -} - -void FGGroundNetwork::addNode(const FGTaxiNode & node) -{ - nodes.push_back(new FGTaxiNode(node)); -} - -void FGGroundNetwork::addNodes(FGParkingVec * parkings) -{ - FGTaxiNode n; - FGParkingVecIterator i = parkings->begin(); - while (i != parkings->end()) { - n.setIndex(i->getIndex()); - n.setLatitude(i->getLatitude()); - n.setLongitude(i->getLongitude()); - n.setElevation(parent->getElevation()*SG_FEET_TO_METER); - nodes.push_back(new FGTaxiNode(n)); - - i++; - } -} - - - -void FGGroundNetwork::init() +void FGGroundNetwork::init(FGAirport* pr) { if (networkInitialized) { FGATCController::init(); //cerr << "FGground network already initialized" << endl; return; } + + parent = pr; + assert(parent); hasNetwork = true; nextSave = 0; int index = 1; - sort(nodes.begin(), nodes.end(), compare_nodes); - //sort(segments.begin(), segments.end(), compare_segments()); - FGTaxiSegmentVectorIterator i = segments.begin(); - while (i != segments.end()) { - (*i)->setStart(&nodes); - (*i)->setEnd(&nodes); - (*i)->setDimensions(parent->getElevation() * SG_FEET_TO_METER); - (*i)->setIndex(index); - if ((*i)->isPushBack()) { - pushBackNodes.push_back((*i)->getEnd()); - } - //SG_LOG(SG_GENERAL, SG_BULK, "initializing segment " << (*i)->getIndex() << endl); - //SG_LOG(SG_GENERAL, SG_BULK, "Track distance = " << (*i)->getLength() << endl); - //SG_LOG(SG_GENERAL, SG_BULK, "Track runs from " << (*i)->getStart()->getIndex() << " to " - // << (*i)->getEnd()->getIndex() << endl); - i++; - index++; + + loadSegments(); + + // establish pairing of segments + BOOST_FOREACH(FGTaxiSegment* segment, segments) { + segment->setIndex(index++); + + if (segment->oppositeDirection) { + continue; // already established + } + + FGTaxiSegment* opp = findSegment(segment->endNode, segment->startNode); + if (opp) { + assert(opp->oppositeDirection == NULL); + segment->oppositeDirection = opp; + opp->oppositeDirection = segment; + } } - i = segments.begin(); - while (i != segments.end()) { - FGTaxiSegmentVectorIterator j = (*i)->getEnd()->getBeginRoute(); - while (j != (*i)->getEnd()->getEndRoute()) { - if ((*j)->getEnd()->getIndex() == (*i)->getStart()->getIndex()) { -// int start1 = (*i)->getStart()->getIndex(); -// int end1 = (*i)->getEnd() ->getIndex(); -// int start2 = (*j)->getStart()->getIndex(); -// int end2 = (*j)->getEnd()->getIndex(); -// int oppIndex = (*j)->getIndex(); - //cerr << "Opposite of " << (*i)->getIndex() << " (" << start1 << "," << end1 << ") " - // << "happens to be " << oppIndex << " (" << start2 << "," << end2 << ") " << endl; - (*i)->setOpposite(*j); - break; - } - j++; - } - i++; - } - //FGTaxiNodeVectorIterator j = nodes.begin(); - //while (j != nodes.end()) { - // if ((*j)->getHoldPointType() == 3) { - // pushBackNodes.push_back((*j)); - // } - // j++; - //} - //cerr << "Done initializing ground network" << endl; - //exit(1); if (fgGetBool("/sim/ai/groundnet-cache")) { - SGPath cacheData(fgGetString("/sim/fg-home")); - cacheData.append("ai"); - string airport = parent->getId(); - - if ((airport) != "") { - char buffer[128]; - ::snprintf(buffer, 128, "%c/%c/%c/", - airport[0], airport[1], airport[2]); - cacheData.append(buffer); - if (!cacheData.exists()) { - cacheData.create_dir(0777); - } - int index; - double elev; - cacheData.append(airport + "-groundnet-cache.txt"); - if (cacheData.exists()) { - ifstream data(cacheData.c_str()); - string revisionStr; - data >> revisionStr; - if (revisionStr != "[GroundNetcachedata:ref:2011:09:04]") { - SG_LOG(SG_GENERAL, SG_ALERT,"GroundNetwork Warning: discarding outdated cachefile " << - cacheData.c_str() << " for Airport " << airport); - } else { - for (FGTaxiNodeVectorIterator i = nodes.begin(); - i != nodes.end(); - i++) { - (*i)->setElevation(parent->getElevation() * SG_FEET_TO_METER); - data >> index >> elev; - if (data.eof()) - break; - if (index != (*i)->getIndex()) { - SG_LOG(SG_GENERAL, SG_ALERT, "Index read from ground network cache at airport " << airport << " does not match index in the network itself"); - } else { - (*i)->setElevation(elev); - } - } - } - } - } + parseCache(); } - //cerr << "Finished initializing " << parent->getId() << " groundnetwork " << endl; + networkInitialized = true; } -int FGGroundNetwork::findNearestNode(const SGGeod & aGeod) +void FGGroundNetwork::loadSegments() { - double minDist = HUGE_VAL; - int index = -1; - - for (FGTaxiNodeVectorIterator itr = nodes.begin(); itr != nodes.end(); - itr++) { - double d = SGGeodesy::distanceM(aGeod, (*itr)->getGeod()); - if (d < minDist) { - minDist = d; - index = (*itr)->getIndex(); - //cerr << "Minimum distance of " << minDist << " for index " << index << endl; - } - } - - return index; + flightgear::NavDataCache* cache = flightgear::NavDataCache::instance(); +// iterate over all ground-net nodes in this airport + BOOST_FOREACH(PositionedID node, cache->groundNetNodes(parent->guid(), false)) { + // find all segments leaving the node + BOOST_FOREACH(PositionedID end, cache->groundNetEdgesFrom(node, false)) { + segments.push_back(new FGTaxiSegment(node, end)); + } + } } -int FGGroundNetwork::findNearestNodeOnRunway(const SGGeod & aGeod) +void FGGroundNetwork::parseCache() { - double minDist = HUGE_VAL; - int index = -1; - - for (FGTaxiNodeVectorIterator itr = nodes.begin(); itr != nodes.end(); - itr++) { - if (!((*itr)->getIsOnRunway())) { - continue; - } - double d = SGGeodesy::distanceM(aGeod, (*itr)->getGeod()); - if (d < minDist) { - minDist = d; - index = (*itr)->getIndex(); - //cerr << "Minimum distance of " << minDist << " for index " << index << endl; + SGPath cacheData(globals->get_fg_home()); + cacheData.append("ai"); + string airport = parent->getId(); + + if (airport.empty()) { + return; + } +#if 0 + char buffer[128]; + ::snprintf(buffer, 128, "%c/%c/%c/", + airport[0], airport[1], airport[2]); + cacheData.append(buffer); + if (!cacheData.exists()) { + cacheData.create_dir(0755); + } + int index; + double elev; + cacheData.append(airport + "-groundnet-cache.txt"); + if (cacheData.exists()) { + ifstream data(cacheData.c_str()); + string revisionStr; + data >> revisionStr; + if (revisionStr != "[GroundNetcachedata:ref:2011:09:04]") { + SG_LOG(SG_GENERAL, SG_ALERT,"GroundNetwork Warning: discarding outdated cachefile " << + cacheData.c_str() << " for Airport " << airport); + } else { + for (IndexTaxiNodeMap::iterator i = nodes.begin(); + i != nodes.end(); + i++) { + i->second->setElevation(parent->elevation() * SG_FEET_TO_METER); + data >> index >> elev; + if (data.eof()) + break; + if (index != i->second->getIndex()) { + SG_LOG(SG_GENERAL, SG_ALERT, "Index read from ground network cache at airport " << airport << " does not match index in the network itself"); + } else { + i->second->setElevation(elev); } + } } - - return index; + } +#endif } +int FGGroundNetwork::findNearestNode(const SGGeod & aGeod) const +{ + const bool onRunway = false; + return NavDataCache::instance()->findGroundNetNode(parent->guid(), aGeod, onRunway); +} -int FGGroundNetwork::findNearestNode(double lat, double lon) +int FGGroundNetwork::findNearestNodeOnRunway(const SGGeod & aGeod, FGRunway* aRunway) const { - return findNearestNode(SGGeod::fromDeg(lon, lat)); + const bool onRunway = true; + return NavDataCache::instance()->findGroundNetNode(parent->guid(), aGeod, onRunway, aRunway); } -FGTaxiNode *FGGroundNetwork::findNode(unsigned idx) -{ /* - for (FGTaxiNodeVectorIterator - itr = nodes.begin(); - itr != nodes.end(); itr++) - { - if (itr->getIndex() == idx) - return itr->getAddress(); - } */ - - if ((idx >= 0) && (idx < nodes.size())) - return nodes[idx]->getAddress(); - else - return 0; +FGTaxiNodeRef FGGroundNetwork::findNode(PositionedID idx) const +{ + return FGPositioned::loadById(idx); } -FGTaxiSegment *FGGroundNetwork::findSegment(unsigned idx) -{ /* - for (FGTaxiSegmentVectorIterator - itr = segments.begin(); - itr != segments.end(); itr++) - { - if (itr->getIndex() == idx) - return itr->getAddress(); - } - */ +FGTaxiSegment *FGGroundNetwork::findSegment(unsigned idx) const +{ if ((idx > 0) && (idx <= segments.size())) - return segments[idx - 1]->getAddress(); + return segments[idx - 1]; else { //cerr << "Alert: trying to find invalid segment " << idx << endl; return 0; } } +FGTaxiSegment* FGGroundNetwork::findSegment(PositionedID from, PositionedID to) const +{ + if (from == 0) { + return NULL; + } + + // completely boring linear search of segments. Can be improved if/when + // this ever becomes a hot-spot + BOOST_FOREACH(FGTaxiSegment* seg, segments) { + if (seg->startNode != from) { + continue; + } + + if ((to == 0) || (seg->endNode == to)) { + return seg; + } + } + + return NULL; // not found +} + +static int edgePenalty(FGTaxiNode* tn) +{ + return (tn->type() == FGPositioned::PARKING ? 10000 : 0) + + (tn->getIsOnRunway() ? 1000 : 0); +} + +class ShortestPathData +{ +public: + ShortestPathData() : + score(HUGE_VAL) + {} + + double score; + FGTaxiNodeRef previousNode; +}; -FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end, +FGTaxiRoute FGGroundNetwork::findShortestRoute(PositionedID start, PositionedID end, bool fullSearch) { //implements Dijkstra's algorithm to find shortest distance route from start to end //taken from http://en.wikipedia.org/wiki/Dijkstra's_algorithm - - //double INFINITE = 100000000000.0; - // initialize scoring values - int nParkings = parent->getDynamics()->getNrOfParkings(); - FGTaxiNodeVector *currNodesSet; - if (fullSearch) { - currNodesSet = &nodes; - } else { - currNodesSet = &pushBackNodes; - } - - for (FGTaxiNodeVectorIterator - itr = currNodesSet->begin(); itr != currNodesSet->end(); itr++) { - (*itr)->setPathScore(HUGE_VAL); //infinity by all practical means - (*itr)->setPreviousNode(0); // - (*itr)->setPreviousSeg(0); // - } - + FGTaxiNodeVector unvisited; + flightgear::NavDataCache* cache = flightgear::NavDataCache::instance(); + std::map searchData; + + BOOST_FOREACH(PositionedID n, cache->groundNetNodes(parent->guid(), !fullSearch)) { + unvisited.push_back(findNode(n)); + } + FGTaxiNode *firstNode = findNode(start); - firstNode->setPathScore(0); + if (!firstNode) + { + SG_LOG(SG_GENERAL, SG_ALERT, + "Error in ground network. Failed to find first waypoint: " << start + << " at " << ((parent) ? parent->getId() : "")); + return FGTaxiRoute(); + } + searchData[firstNode].score = 0.0; FGTaxiNode *lastNode = findNode(end); - - FGTaxiNodeVector unvisited(*currNodesSet); // working copy + if (!lastNode) + { + SG_LOG(SG_GENERAL, SG_ALERT, + "Error in ground network. Failed to find last waypoint: " << end + << " at " << ((parent) ? parent->getId() : "")); + return FGTaxiRoute(); + } while (!unvisited.empty()) { - FGTaxiNode *best = *(unvisited.begin()); - for (FGTaxiNodeVectorIterator - itr = unvisited.begin(); itr != unvisited.end(); itr++) { - if ((*itr)->getPathScore() < best->getPathScore()) - best = (*itr); + FGTaxiNode *best = unvisited.front(); + BOOST_FOREACH(FGTaxiNode* i, unvisited) { + if (searchData[i].score < searchData[best].score) { + best = i; + } } - + + // remove 'best' from the unvisited set FGTaxiNodeVectorIterator newend = remove(unvisited.begin(), unvisited.end(), best); unvisited.erase(newend, unvisited.end()); if (best == lastNode) { // found route or best not connected break; - } else { - for (FGTaxiSegmentVectorIterator - seg = best->getBeginRoute(); - seg != best->getEndRoute(); seg++) { - if (fullSearch || (*seg)->isPushBack()) { - FGTaxiNode *tgt = (*seg)->getEnd(); - double alt = - best->getPathScore() + (*seg)->getLength() + - (*seg)->getPenalty(nParkings); - if (alt < tgt->getPathScore()) { // Relax (u,v) - tgt->setPathScore(alt); - tgt->setPreviousNode(best); - tgt->setPreviousSeg(*seg); // - } - } else { - // // cerr << "Skipping TaxiSegment " << (*seg)->getIndex() << endl; - } - } } - } + + BOOST_FOREACH(PositionedID targetId, cache->groundNetEdgesFrom(best->guid(), !fullSearch)) { + FGTaxiNodeRef tgt = FGPositioned::loadById(targetId); + double edgeLength = dist(best->cart(), tgt->cart()); + double alt = searchData[best].score + edgeLength + edgePenalty(tgt); + if (alt < searchData[tgt].score) { // Relax (u,v) + searchData[tgt].score = alt; + searchData[tgt].previousNode = best; + } + } // of outgoing arcs/segments from current best node iteration + } // of unvisited nodes remaining - if (lastNode->getPathScore() == HUGE_VAL) { + if (searchData[lastNode].score == HUGE_VAL) { // no valid route found if (fullSearch) { SG_LOG(SG_GENERAL, SG_ALERT, "Failed to find route from waypoint " << start << " to " << end << " at " << parent->getId()); } - FGTaxiRoute empty; - return empty; - //exit(1); //TODO exit more gracefully, no need to stall the whole sim with broken GN's - } else { - // assemble route from backtrace information - intVec nodes, routes; - FGTaxiNode *bt = lastNode; - while (bt->getPreviousNode() != 0) { - nodes.push_back(bt->getIndex()); - routes.push_back(bt->getPreviousSegment()->getIndex()); - bt = bt->getPreviousNode(); - } - nodes.push_back(start); - reverse(nodes.begin(), nodes.end()); - reverse(routes.begin(), routes.end()); - - return FGTaxiRoute(nodes, routes, lastNode->getPathScore(), 0); - } -} - -int FGTaxiSegment::getPenalty(int nGates) -{ - int penalty = 0; - if (end->getIndex() < nGates) { - penalty += 10000; - } - if (end->getIsOnRunway()) { // For now. In future versions, need to find out whether runway is active. - penalty += 1000; + + return FGTaxiRoute(); + } + + // assemble route from backtrace information + PositionedIDVec nodes; + intVec routes; + FGTaxiNode *bt = lastNode; + + while (searchData[bt].previousNode != 0) { + nodes.push_back(bt->guid()); + FGTaxiSegment *segment = findSegment(searchData[bt].previousNode->guid(), bt->guid()); + int idx = segment->getIndex(); + routes.push_back(idx); + bt = searchData[bt].previousNode; + } - return penalty; + nodes.push_back(start); + reverse(nodes.begin(), nodes.end()); + reverse(routes.begin(), routes.end()); + return FGTaxiRoute(nodes, routes, searchData[lastNode].score, 0); } /* ATC Related Functions */ @@ -629,7 +505,10 @@ void FGGroundNetwork::announcePosition(int id, double radius, int leg, FGAIAircraft * aircraft) { - init(); + + assert(parent); + init(parent); + TrafficVectorIterator i = activeTraffic.begin(); // Search search if the current id alread has an entry // This might be faster using a map instead of a vector, but let's start by taking a safe route @@ -651,7 +530,11 @@ void FGGroundNetwork::announcePosition(int id, rec.setPositionAndHeading(lat, lon, heading, speed, alt); rec.setRadius(radius); // only need to do this when creating the record. rec.setAircraft(aircraft); - activeTraffic.push_front(rec); + if (leg == 2) { + activeTraffic.push_front(rec); + } else { + activeTraffic.push_back(rec); + } } else { i->setPositionAndIntentions(currentPosition, intendedRoute); @@ -701,21 +584,21 @@ bool FGGroundNetwork::checkTransmissionState(int minState, int maxState, Traffic if ((state >= minState) && (state <= maxState) && available) { if ((msgDir == ATC_AIR_TO_GROUND) && isUserAircraft(i->getAircraft())) { //cerr << "Checking state " << state << " for " << i->getAircraft()->getCallSign() << endl; - static SGPropertyNode_ptr trans_num = globals->get_props()->getNode("/sim/atc/transmission-num", true); + SGPropertyNode_ptr trans_num = globals->get_props()->getNode("/sim/atc/transmission-num", true); int n = trans_num->getIntValue(); if (n == 0) { trans_num->setIntValue(-1); // PopupCallback(n); //cerr << "Selected transmission message " << n << endl; - FGATCManager *atc = (FGATCManager*) globals->get_subsystem("atc"); + //FGATCManager *atc = (FGATCManager*) globals->get_subsystem("atc"); FGATCDialogNew::instance()->removeEntry(1); } else { //cerr << "creating message for " << i->getAircraft()->getCallSign() << endl; - transmit(&(*i), msgId, msgDir, false); + transmit(&(*i), &(*parent->getDynamics()), msgId, msgDir, false); return false; } } - transmit(&(*i), msgId, msgDir, true); + transmit(&(*i), &(*parent->getDynamics()), msgId, msgDir, true); i->updateState(); lastTransmission = now; available = false; @@ -827,7 +710,7 @@ void FGGroundNetwork::checkSpeedAdjustment(int id, double lat, TrafficVectorIterator current, closest, closestOnNetwork; TrafficVectorIterator i = activeTraffic.begin(); bool otherReasonToSlowDown = false; - bool previousInstruction; +// bool previousInstruction; if (activeTraffic.size()) { //while ((i->getId() != id) && (i != activeTraffic.end())) while (i != activeTraffic.end()) { @@ -846,13 +729,14 @@ void FGGroundNetwork::checkSpeedAdjustment(int id, double lat, current = i; //closest = current; - previousInstruction = current->getSpeedAdjustment(); +// previousInstruction = current->getSpeedAdjustment(); double mindist = HUGE_VAL; if (activeTraffic.size()) { - double course, dist, bearing, minbearing, az2; + double course, dist, bearing, az2; // minbearing, SGGeod curr(SGGeod::fromDegM(lon, lat, alt)); //TrafficVector iterator closest; closest = current; + closestOnNetwork = current; for (TrafficVectorIterator i = activeTraffic.begin(); i != activeTraffic.end(); i++) { if (i == current) { @@ -870,7 +754,7 @@ void FGGroundNetwork::checkSpeedAdjustment(int id, double lat, mindist = dist; closest = i; closestOnNetwork = i; - minbearing = bearing; +// minbearing = bearing; } } @@ -894,7 +778,7 @@ void FGGroundNetwork::checkSpeedAdjustment(int id, double lat, // << endl; mindist = dist; closest = i; - minbearing = bearing; +// minbearing = bearing; otherReasonToSlowDown = true; } } @@ -953,7 +837,7 @@ void FGGroundNetwork::checkSpeedAdjustment(int id, double lat, } } } - if ((closest == closestOnNetwork) && (current->getPriority() < closest->getPriority()) && needBraking) { + if ((closest->getId() == closestOnNetwork->getId()) && (current->getPriority() < closest->getPriority()) && needBraking) { swap(current, closest); } } @@ -984,6 +868,7 @@ void FGGroundNetwork::checkHoldPosition(int id, double lat, } else { return; } + time_t now = time(NULL) + fgGetLong("/sim/time/warp"); if (i == activeTraffic.end() || (activeTraffic.size() == 0)) { SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkHoldPosition at " << SG_ORIGIN); @@ -995,12 +880,14 @@ void FGGroundNetwork::checkHoldPosition(int id, double lat, return; } if (current->getAircraft()->getTakeOffStatus() == 2) { + //cerr << current->getAircraft()->getCallSign() << ". Taxi in position and hold" << endl; current->setHoldPosition(false); + current->clearSpeedAdjustment(); return; } bool origStatus = current->hasHoldPosition(); current->setHoldPosition(false); - SGGeod curr(SGGeod::fromDegM(lon, lat, alt)); + //SGGeod curr(SGGeod::fromDegM(lon, lat, alt)); int currentRoute = i->getCurrentPosition(); int nextRoute; if (i->getIntentions().size()) { @@ -1016,89 +903,32 @@ void FGGroundNetwork::checkHoldPosition(int id, double lat, } else { nx = tx; } - if (tx->hasBlock() || nx->hasBlock() ) { - current->setHoldPosition(true); - } - } - - - /* for (i = activeTraffic.begin(); i != activeTraffic.end(); i++) { - if (i->getId() != current->getId()) { - int node = current->crosses(this, *i); - if (node != -1) { - FGTaxiNode *taxiNode = findNode(node); - - // Determine whether it's save to continue or not. - // If we have a crossing route, there are two possibilities: - // 1) This is an interestion - // 2) This is oncoming two-way traffic, using the same taxiway. - //cerr << "Hold check 1 : " << id << " has common node " << node << endl; - - SGGeod other(SGGeod:: - fromDegM(i->getLongitude(), i->getLatitude(), - i->getAltitude())); - bool needsToWait; - bool opposing; - if (current->isOpposing(this, *i, node)) { - needsToWait = true; - opposing = true; - //cerr << "Hold check 2 : " << node << " has opposing segment " << endl; - // issue a "Hold Position" as soon as we're close to the offending node - // For now, I'm doing this as long as the other aircraft doesn't - // have a hold instruction as soon as we're within a reasonable - // distance from the offending node. - // This may be a bit of a conservative estimate though, as it may - // be well possible that both aircraft can both continue to taxi - // without crashing into each other. - } else { - opposing = false; - if (SGGeodesy::distanceM(other, taxiNode->getGeod()) > 200) // 2.0*i->getRadius()) - { - needsToWait = false; - //cerr << "Hold check 3 : " << id <<" Other aircraft approaching node is still far away. (" << dist << " nm). Can safely continue " - // << endl; - } else { - needsToWait = true; - //cerr << "Hold check 4: " << id << " Would need to wait for other aircraft : distance = " << dist << " meters" << endl; - } - } + //if (tx->hasBlock(now) || nx->hasBlock(now) ) { + // current->setHoldPosition(true); + //} + SGGeod start(SGGeod::fromDeg((i->getLongitude()), (i->getLatitude()))); + SGGeod end (nx->getStart()->geod()); - double dist = - SGGeodesy::distanceM(curr, taxiNode->getGeod()); - if (!(i->hasHoldPosition())) { - - if ((dist < 200) && //2.5*current->getRadius()) && - (needsToWait) && (i->onRoute(this, *current)) && - //((i->onRoute(this, *current)) || ((!(i->getSpeedAdjustment())))) && - (!(current->getId() == i->getWaitsForId()))) - //(!(i->getSpeedAdjustment()))) // && - //(!(current->getSpeedAdjustment()))) - - { - if (!(isUserAircraft(i->getAircraft()))) { // test code. Don't wait for the user, let the user wait for you. - current->setHoldPosition(true); - current->setWaitsForId(i->getId()); - } - //cerr << "Hold check 5: " << current->getCallSign() <<" Setting Hold Position: distance to node (" << node << ") " - // << dist << " meters. Waiting for " << i->getCallSign(); - //if (opposing) - //cerr <<" [opposing] " << endl; - //else - // cerr << "[non-opposing] " << endl; - //if (i->hasSpeefAdjustment()) - // { - // cerr << " (which in turn waits for ) " << i-> - } else { - //cerr << "Hold check 6: " << id << " No need to hold yet: Distance to node : " << dist << " nm"<< endl; - } + double distance = SGGeodesy::distanceM(start, end); + if (nx->hasBlock(now) && (distance < i->getRadius() * 4)) { + current->setHoldPosition(true); + } else { + intVecIterator ivi = i->getIntentions().begin(); + while (ivi != i->getIntentions().end()) { + if ((*ivi) > 0) { + distance += segments[(*ivi)-1]->getLength(); + if ((segments[(*ivi)-1]->hasBlock(now)) && (distance < i->getRadius() * 4)) { + current->setHoldPosition(true); + break; } } + ivi++; } - } */ + } + } bool currStatus = current->hasHoldPosition(); current->setHoldPosition(origStatus); // Either a Hold Position or a resume taxi transmission has been issued - time_t now = time(NULL) + fgGetLong("/sim/time/warp"); if ((now - lastTransmission) > 2) { available = true; } @@ -1106,11 +936,11 @@ void FGGroundNetwork::checkHoldPosition(int id, double lat, if ((origStatus != currStatus) && available) { //cerr << "Issueing hold short instrudtion " << currStatus << " " << available << endl; if (currStatus == true) { // No has a hold short instruction - transmit(&(*current), MSG_HOLD_POSITION, ATC_GROUND_TO_AIR, true); + transmit(&(*current), &(*parent->getDynamics()), MSG_HOLD_POSITION, ATC_GROUND_TO_AIR, true); //cerr << "Transmittin hold short instrudtion " << currStatus << " " << available << endl; current->setState(1); } else { - transmit(&(*current), MSG_RESUME_TAXI, ATC_GROUND_TO_AIR, true); + transmit(&(*current), &(*parent->getDynamics()), MSG_RESUME_TAXI, ATC_GROUND_TO_AIR, true); //cerr << "Transmittig resume instrudtion " << currStatus << " " << available << endl; current->setState(2); } @@ -1314,7 +1144,7 @@ static void WorldCoordinate(osg::Matrix& obj_pos, double lat, double lon, double elev, double hdg, double slope) { SGGeod geod = SGGeod::fromDegM(lon, lat, elev); - obj_pos = geod.makeZUpFrame(); + obj_pos = makeZUpFrame(geod); // hdg is not a compass heading, but a counter-clockwise rotation // around the Z axis obj_pos.preMult(osg::Matrix::rotate(hdg * SGD_DEGREES_TO_RADIANS, @@ -1328,7 +1158,6 @@ static void WorldCoordinate(osg::Matrix& obj_pos, double lat, void FGGroundNetwork::render(bool visible) { - SGMaterialLib *matlib = globals->get_matlib(); if (group) { //int nr = ; @@ -1345,27 +1174,29 @@ void FGGroundNetwork::render(bool visible) if (visible) { group = new osg::Group; FGScenery * local_scenery = globals->get_scenery(); - double elevation_meters = 0.0; - double elevation_feet = 0.0; + // double elevation_meters = 0.0; +// double elevation_feet = 0.0; + time_t now = time(NULL) + fgGetLong("/sim/time/warp"); //for ( FGTaxiSegmentVectorIterator i = segments.begin(); i != segments.end(); i++) { - double dx = 0; + //double dx = 0; for (TrafficVectorIterator i = activeTraffic.begin(); i != activeTraffic.end(); i++) { - // Handle start point + // Handle start point i.e. the segment that is connected to the aircraft itself on the starting end + // and to the the first "real" taxi segment on the other end. int pos = i->getCurrentPosition() - 1; if (pos >= 0) { SGGeod start(SGGeod::fromDeg((i->getLongitude()), (i->getLatitude()))); - SGGeod end (SGGeod::fromDeg(segments[pos]->getEnd()->getLongitude(), segments[pos]->getEnd()->getLatitude())); + SGGeod end (segments[pos]->getEnd()->geod()); double length = SGGeodesy::distanceM(start, end); - //heading = SGGeodesy::headingDeg(start->getGeod(), end->getGeod()); + //heading = SGGeodesy::headingDeg(start->geod(), end->geod()); double az2, heading; //, distanceM; SGGeodesy::inverse(start, end, heading, az2, length); double coveredDistance = length * 0.5; SGGeod center; SGGeodesy::direct(start, heading, coveredDistance, center, az2); - //cerr << "Active Aircraft : Centerpoint = (" << center.getLatitudeDeg() << ", " << center.getLongitudeDeg() << "). Heading = " << heading << endl; + //std::cerr << "Active Aircraft : Centerpoint = (" << center.getLatitudeDeg() << ", " << center.getLongitudeDeg() << "). Heading = " << heading << std::endl; /////////////////////////////////////////////////////////////////////////////// // Make a helper function out of this osg::Matrix obj_pos; @@ -1378,14 +1209,14 @@ void FGGroundNetwork::render(bool visible) } else { elevationStart = ((i)->getAircraft()->_getAltitude()); } - double elevationEnd = segments[pos]->getEnd()->getElevationM(parent->getElevation()*SG_FEET_TO_METER); + double elevationEnd = segments[pos]->getEnd()->getElevationM(); //cerr << "Using elevation " << elevationEnd << endl; if ((elevationEnd == 0) || (elevationEnd = parent->getElevation())) { SGGeod center2 = end; center2.setElevationM(SG_MAX_ELEVATION_M); if (local_scenery->get_elevation_m( center2, elevationEnd, NULL )) { - elevation_feet = elevationEnd * SG_METER_TO_FEET + 0.5; +// elevation_feet = elevationEnd * SG_METER_TO_FEET + 0.5; //elevation_meters += 0.5; } else { @@ -1416,10 +1247,10 @@ void FGGroundNetwork::render(bool visible) geode->addDrawable(geometry); //osg::Node *custom_obj; SGMaterial *mat; - if (segments[pos]->hasBlock()) { - mat = matlib->find("UnidirectionalTaperRed"); + if (segments[pos]->hasBlock(now)) { + mat = matlib->find("UnidirectionalTaperRed", center); } else { - mat = matlib->find("UnidirectionalTaperGreen"); + mat = matlib->find("UnidirectionalTaperGreen", center); } if (mat) geode->setEffect(mat->get_effect()); @@ -1429,8 +1260,9 @@ void FGGroundNetwork::render(bool visible) group->addChild( obj_trans ); ///////////////////////////////////////////////////////////////////// } else { - //cerr << "BIG FAT WARNING: current position is here : " << pos << endl; + //std::cerr << "BIG FAT WARNING: current position is here : " << pos << std::endl; } + // Next: Draw the other taxi segments. for (intVecIterator j = (i)->getIntentions().begin(); j != (i)->getIntentions().end(); j++) { osg::Matrix obj_pos; int k = (*j)-1; @@ -1439,13 +1271,13 @@ void FGGroundNetwork::render(bool visible) obj_trans->setDataVariance(osg::Object::STATIC); // Experimental: Calculate slope here, based on length, and the individual elevations - double elevationStart = segments[k]->getStart()->getElevationM(parent->getElevation()*SG_FEET_TO_METER); - double elevationEnd = segments[k]->getEnd ()->getElevationM(parent->getElevation()*SG_FEET_TO_METER); + double elevationStart = segments[k]->getStart()->getElevationM(); + double elevationEnd = segments[k]->getEnd ()->getElevationM(); if ((elevationStart == 0) || (elevationStart == parent->getElevation())) { - SGGeod center2 = segments[k]->getStart()->getGeod(); + SGGeod center2 = segments[k]->getStart()->geod(); center2.setElevationM(SG_MAX_ELEVATION_M); if (local_scenery->get_elevation_m( center2, elevationStart, NULL )) { - elevation_feet = elevationStart * SG_METER_TO_FEET + 0.5; +// elevation_feet = elevationStart * SG_METER_TO_FEET + 0.5; //elevation_meters += 0.5; } else { @@ -1454,10 +1286,10 @@ void FGGroundNetwork::render(bool visible) segments[k]->getStart()->setElevation(elevationStart); } if ((elevationEnd == 0) || (elevationEnd == parent->getElevation())) { - SGGeod center2 = segments[k]->getEnd()->getGeod(); + SGGeod center2 = segments[k]->getEnd()->geod(); center2.setElevationM(SG_MAX_ELEVATION_M); if (local_scenery->get_elevation_m( center2, elevationEnd, NULL )) { - elevation_feet = elevationEnd * SG_METER_TO_FEET + 0.5; +// elevation_feet = elevationEnd * SG_METER_TO_FEET + 0.5; //elevation_meters += 0.5; } else { @@ -1473,8 +1305,8 @@ void FGGroundNetwork::render(bool visible) // cerr << "2. Using mean elevation : " << elevationMean << " and " << slope << endl; - - WorldCoordinate( obj_pos, segments[k]->getLatitude(), segments[k]->getLongitude(), elevationMean+ 0.5, -(segments[k]->getHeading()), slope ); + SGGeod segCenter = segments[k]->getCenter(); + WorldCoordinate( obj_pos, segCenter.getLatitudeDeg(), segCenter.getLongitudeDeg(), elevationMean+ 0.5, -(segments[k]->getHeading()), slope ); obj_trans->setMatrix( obj_pos ); //osg::Vec3 center(0, 0, 0) @@ -1490,10 +1322,10 @@ void FGGroundNetwork::render(bool visible) geode->addDrawable(geometry); //osg::Node *custom_obj; SGMaterial *mat; - if (segments[k]->hasBlock()) { - mat = matlib->find("UnidirectionalTaperRed"); + if (segments[k]->hasBlock(now)) { + mat = matlib->find("UnidirectionalTaperRed", segCenter); } else { - mat = matlib->find("UnidirectionalTaperGreen"); + mat = matlib->find("UnidirectionalTaperGreen", segCenter); } if (mat) geode->setEffect(mat->get_effect()); @@ -1515,8 +1347,9 @@ string FGGroundNetwork::getName() { void FGGroundNetwork::update(double dt) { + time_t now = time(NULL) + fgGetLong("/sim/time/warp"); for (FGTaxiSegmentVectorIterator tsi = segments.begin(); tsi != segments.end(); tsi++) { - (*tsi)->unblock(); + (*tsi)->unblock(now); } int priority = 1; //sort(activeTraffic.begin(), activeTraffic.end(), compare_trafficrecords); @@ -1526,7 +1359,9 @@ void FGGroundNetwork::update(double dt) i != parent->getDynamics()->getStartupController()->getActiveTraffic().end(); i++) { i->allowPushBack(); i->setPriority(priority++); - if (i->isActive(60)) { + // in meters per second; + double vTaxi = (i->getAircraft()->getPerformance()->vTaxi() * SG_NM_TO_METER) / 3600; + if (i->isActive(0)) { // Check for all active aircraft whether it's current pos segment is // an opposite of one of the departing aircraft's intentions @@ -1539,7 +1374,7 @@ void FGGroundNetwork::update(double dt) for (intVecIterator k = i->getIntentions().begin(); k != i->getIntentions().end(); k++) { if ((*k) == posReverse) { i->denyPushBack(); - segments[posReverse-1]->block(); + segments[posReverse-1]->block(i->getId(), now, now); } } } @@ -1547,13 +1382,15 @@ void FGGroundNetwork::update(double dt) } // if the current aircraft is still allowed to pushback, we can start reserving a route for if by blocking all the entry taxiways. if (i->pushBackAllowed()) { + double length = 0; int pos = i->getCurrentPosition(); if (pos > 0) { FGTaxiSegment *seg = segments[pos-1]; FGTaxiNode *node = seg->getEnd(); + length = seg->getLength(); for (FGTaxiSegmentVectorIterator tsi = segments.begin(); tsi != segments.end(); tsi++) { if (((*tsi)->getEnd() == node) && ((*tsi) != seg)) { - (*tsi)->block(); + (*tsi)->block(i->getId(), now, now); } } } @@ -1562,9 +1399,11 @@ void FGGroundNetwork::update(double dt) if (pos > 0) { FGTaxiSegment *seg = segments[pos-1]; FGTaxiNode *node = seg->getEnd(); + length += seg->getLength(); + time_t blockTime = now + (length / vTaxi); for (FGTaxiSegmentVectorIterator tsi = segments.begin(); tsi != segments.end(); tsi++) { if (((*tsi)->getEnd() == node) && ((*tsi) != seg)) { - (*tsi)->block(); + (*tsi)->block(i->getId(), blockTime-30, now); } } } @@ -1573,10 +1412,13 @@ void FGGroundNetwork::update(double dt) } } for (TrafficVectorIterator i = activeTraffic.begin(); i != activeTraffic.end(); i++) { + double length = 0; + double vTaxi = (i->getAircraft()->getPerformance()->vTaxi() * SG_NM_TO_METER) / 3600; i->setPriority(priority++); int pos = i->getCurrentPosition(); if (pos > 0) { - if (segments[pos-1]->hasBlock()) { + length = segments[pos-1]->getLength(); + if (segments[pos-1]->hasBlock(now)) { //SG_LOG(SG_GENERAL, SG_ALERT, "Taxiway incursion for AI aircraft" << i->getAircraft()->getCallSign()); } @@ -1585,7 +1427,7 @@ void FGGroundNetwork::update(double dt) for (ivi = i->getIntentions().begin(); ivi != i->getIntentions().end(); ivi++) { int segIndex = (*ivi); if (segIndex > 0) { - if (segments[segIndex-1]->hasBlock()) + if (segments[segIndex-1]->hasBlock(now)) break; } } @@ -1595,9 +1437,11 @@ void FGGroundNetwork::update(double dt) if (pos > 0) { FGTaxiSegment *seg = segments[pos-1]; FGTaxiNode *node = seg->getEnd(); + length += seg->getLength(); for (FGTaxiSegmentVectorIterator tsi = segments.begin(); tsi != segments.end(); tsi++) { if (((*tsi)->getEnd() == node) && ((*tsi) != seg)) { - (*tsi)->block(); + time_t blockTime = now + (length / vTaxi); + (*tsi)->block(i->getId(), blockTime - 30, now); } } }