X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FAirports%2Fgroundnetwork.cxx;h=a6ad1f5c44a8c0f4653d3314814f1cb7e643581e;hb=076f61f2b7e1f774bc827f34ab4a24dcc1998571;hp=4802b97c57cecafb9140340f326dbce836ac5c01;hpb=0025dfb9bccc45d7f96b1badbe7c867679669c9e;p=flightgear.git diff --git a/src/Airports/groundnetwork.cxx b/src/Airports/groundnetwork.cxx index 4802b97c5..a6ad1f5c4 100644 --- a/src/Airports/groundnetwork.cxx +++ b/src/Airports/groundnetwork.cxx @@ -24,10 +24,11 @@ # include #endif -#include +#include #include #include - +#include +#include #include #include @@ -35,17 +36,21 @@ #include #include -#include #include #include #include +#include +#include +#include -#include +#include #include +#include #include #include #include +#include #include @@ -53,68 +58,50 @@ #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); -} +}; -void FGTaxiSegment::setEnd(FGTaxiNodeVector * nodes) +SGGeod FGTaxiSegment::getCenter() 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); + 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); } +FGTaxiNodeRef FGTaxiSegment::getEnd() const +{ + return FGPositioned::loadById(endNode); +} - -// There is probably a computationally cheaper way of -// doing this. -void FGTaxiSegment::setDimensions(double elevation) +FGTaxiNodeRef FGTaxiSegment::getStart() const { - 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; + return FGPositioned::loadById(startNode); } +double FGTaxiSegment::getLength() const +{ + return dist(getStart()->cart(), getEnd()->cart()); +} -//void FGTaxiSegment::setCourseDiff(double crse) -//{ -// headingDiff = fabs(course - crse); +double FGTaxiSegment::getHeading() const +{ + return SGGeodesy::courseDeg(getStart()->geod(), getEnd()->geod()); +} -// if (headingDiff > 180) -// headingDiff = fabs(headingDiff - 360); -//} void FGTaxiSegment::block(int id, time_t blockTime, time_t now) { @@ -158,103 +145,39 @@ void FGTaxiSegment::unblock(time_t now) /*************************************************************************** * 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; @@ -268,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(); @@ -326,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()); @@ -334,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); +} -FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end, +class ShortestPathData +{ +public: + ShortestPathData() : + score(HUGE_VAL) + {} + + double score; + FGTaxiNodeRef previousNode; +}; + +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 */ @@ -670,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 @@ -746,7 +584,7 @@ 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); @@ -872,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()) { @@ -891,10 +729,10 @@ 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; @@ -916,7 +754,7 @@ void FGGroundNetwork::checkSpeedAdjustment(int id, double lat, mindist = dist; closest = i; closestOnNetwork = i; - minbearing = bearing; +// minbearing = bearing; } } @@ -940,7 +778,7 @@ void FGGroundNetwork::checkSpeedAdjustment(int id, double lat, // << endl; mindist = dist; closest = i; - minbearing = bearing; +// minbearing = bearing; otherReasonToSlowDown = true; } } @@ -1049,7 +887,7 @@ void FGGroundNetwork::checkHoldPosition(int id, double lat, } 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()) { @@ -1069,7 +907,7 @@ void FGGroundNetwork::checkHoldPosition(int id, double lat, // current->setHoldPosition(true); //} SGGeod start(SGGeod::fromDeg((i->getLongitude()), (i->getLatitude()))); - SGGeod end (SGGeod::fromDeg(nx->getStart()->getLongitude(), nx->getStart()->getLatitude())); + SGGeod end (nx->getStart()->geod()); double distance = SGGeodesy::distanceM(start, end); if (nx->hasBlock(now) && (distance < i->getRadius() * 4)) { @@ -1306,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, @@ -1320,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 = ; @@ -1337,28 +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; @@ -1371,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 { @@ -1410,9 +1248,9 @@ void FGGroundNetwork::render(bool visible) //osg::Node *custom_obj; SGMaterial *mat; if (segments[pos]->hasBlock(now)) { - mat = matlib->find("UnidirectionalTaperRed"); + mat = matlib->find("UnidirectionalTaperRed", center); } else { - mat = matlib->find("UnidirectionalTaperGreen"); + mat = matlib->find("UnidirectionalTaperGreen", center); } if (mat) geode->setEffect(mat->get_effect()); @@ -1422,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; @@ -1432,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 { @@ -1447,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 { @@ -1466,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) @@ -1484,9 +1323,9 @@ void FGGroundNetwork::render(bool visible) //osg::Node *custom_obj; SGMaterial *mat; if (segments[k]->hasBlock(now)) { - mat = matlib->find("UnidirectionalTaperRed"); + mat = matlib->find("UnidirectionalTaperRed", segCenter); } else { - mat = matlib->find("UnidirectionalTaperGreen"); + mat = matlib->find("UnidirectionalTaperGreen", segCenter); } if (mat) geode->setEffect(mat->get_effect()); @@ -1522,7 +1361,7 @@ void FGGroundNetwork::update(double dt) i->setPriority(priority++); // in meters per second; double vTaxi = (i->getAircraft()->getPerformance()->vTaxi() * SG_NM_TO_METER) / 3600; - if (i->isActive(60)) { + 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