X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FAirports%2Fgroundnetwork.cxx;h=b4f48caeeea2f158dc3a8d0042933d1fb94efb99;hb=49030e700ba276d5cd6ffa4f4e661a266fa0105c;hp=9e26374aa52903c069eb10c426f2d0d0308f8f22;hpb=b452234cb203c3336cfc5299be8ff4789a0eb416;p=flightgear.git diff --git a/src/Airports/groundnetwork.cxx b/src/Airports/groundnetwork.cxx index 9e26374aa..b4f48caee 100644 --- a/src/Airports/groundnetwork.cxx +++ b/src/Airports/groundnetwork.cxx @@ -41,26 +41,24 @@ //#include
//#include +#include + #include -//#include STL_STRING +//#include #include "groundnetwork.hxx" /*************************************************************************** * FGTaxiSegment **************************************************************************/ -FGTaxiSegment::FGTaxiSegment() -{ - oppositeDirection = 0; - isActive = true; -} void FGTaxiSegment::setStart(FGTaxiNodeVector *nodes) { FGTaxiNodeVectorIterator i = nodes->begin(); while (i != nodes->end()) { + //cerr << "Scanning start node index" << (*i)->getIndex() << endl; if ((*i)->getIndex() == startNode) { start = (*i)->getAddress(); @@ -69,6 +67,7 @@ void FGTaxiSegment::setStart(FGTaxiNodeVector *nodes) } i++; } + SG_LOG(SG_GENERAL, SG_ALERT, "Could not find start node " << startNode << endl); } void FGTaxiSegment::setEnd(FGTaxiNodeVector *nodes) @@ -76,6 +75,7 @@ void FGTaxiSegment::setEnd(FGTaxiNodeVector *nodes) FGTaxiNodeVectorIterator i = nodes->begin(); while (i != nodes->end()) { + //cerr << "Scanning end node index" << (*i)->getIndex() << endl; if ((*i)->getIndex() == endNode) { end = (*i)->getAddress(); @@ -83,6 +83,7 @@ void FGTaxiSegment::setEnd(FGTaxiNodeVector *nodes) } i++; } + SG_LOG(SG_GENERAL, SG_ALERT, "Could not find end node " << endNode << endl); } @@ -219,6 +220,7 @@ FGGroundNetwork::~FGGroundNetwork() delete (*node); } nodes.clear(); + pushBackNodes.clear(); for (FGTaxiSegmentVectorIterator seg = segments.begin(); seg != segments.end(); seg++) @@ -263,13 +265,17 @@ void FGGroundNetwork::init() //sort(segments.begin(), segments.end(), compare_segments()); FGTaxiSegmentVectorIterator i = segments.begin(); while(i != segments.end()) { - //cerr << "initializing node " << i->getIndex() << endl; (*i)->setStart(&nodes); (*i)->setEnd (&nodes); (*i)->setTrackDistance(); (*i)->setIndex(index); - //cerr << "Track distance = " << i->getLength() << endl; - //cerr << "Track ends at" << i->getEnd()->getIndex() << endl; + 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++; } @@ -295,6 +301,13 @@ void FGGroundNetwork::init() } 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); } @@ -365,34 +378,42 @@ FGTaxiSegment *FGGroundNetwork::findSegment(int idx) } -FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end) +FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int 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 = nodes.begin(); - itr != nodes.end(); itr++) { - (*itr)->pathscore = HUGE_VAL; //infinity by all practical means - (*itr)->previousnode = 0; // - (*itr)->previousseg = 0; // + itr = currNodesSet->begin(); + itr != currNodesSet->end(); itr++) { + (*itr)->setPathScore(HUGE_VAL); //infinity by all practical means + (*itr)->setPreviousNode(0); // + (*itr)->setPreviousSeg (0); // } FGTaxiNode *firstNode = findNode(start); - firstNode->pathscore = 0; + firstNode->setPathScore(0); FGTaxiNode *lastNode = findNode(end); - FGTaxiNodeVector unvisited(nodes); // working copy + FGTaxiNodeVector unvisited(*currNodesSet); // working copy while (!unvisited.empty()) { FGTaxiNode* best = *(unvisited.begin()); for (FGTaxiNodeVectorIterator itr = unvisited.begin(); itr != unvisited.end(); itr++) { - if ((*itr)->pathscore < best->pathscore) + if ((*itr)->getPathScore() < best->getPathScore()) best = (*itr); } @@ -405,223 +426,64 @@ FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end) for (FGTaxiSegmentVectorIterator seg = best->getBeginRoute(); seg != best->getEndRoute(); seg++) { - FGTaxiNode* tgt = (*seg)->getEnd(); - double alt = best->pathscore + (*seg)->getLength(); - if (alt < tgt->pathscore) { // Relax (u,v) - tgt->pathscore = alt; - tgt->previousnode = best; - tgt->previousseg = *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; } } } } - if (lastNode->pathscore == HUGE_VAL) { + if (lastNode->getPathScore() == HUGE_VAL) { // no valid route found - SG_LOG( SG_GENERAL, SG_ALERT, "Failed to find route from waypoint " << start << " to " << end << " at " << - parent->getId()); - exit(1); //TODO exit more gracefully, no need to stall the whole sim with broken GN's + 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->previousnode != 0) { + while (bt->getPreviousNode() != 0) { nodes.push_back(bt->getIndex()); - routes.push_back(bt->previousseg->getIndex()); - bt = bt->previousnode; + 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->pathscore, 0); + return FGTaxiRoute(nodes, routes, lastNode->getPathScore(), 0); } } - -// void FGGroundNetwork::trace(FGTaxiNode *currNode, int end, int depth, double distance) -// { -// // Just check some preconditions of the trace algorithm -// if (nodesStack.size() != routesStack.size()) -// { -// SG_LOG(SG_GENERAL, SG_ALERT, "size of nodesStack and routesStack is not equal. NodesStack :" -// << nodesStack.size() << ". RoutesStack : " << routesStack.size()); -// } -// nodesStack.push_back(currNode->getIndex()); -// totalDistance += distance; -// //cerr << "Starting trace " << currNode->getIndex() << " " << "total distance: " << totalDistance << endl; -// // << currNode->getIndex() << endl; -// -// // If the current route matches the required end point we found a valid route -// // So we can add this to the routing table -// if (currNode->getIndex() == end) -// { -// maxDepth = depth; -// //cerr << "Found route : " << totalDistance << "" << " " << *(nodesStack.end()-1) << " Depth = " << depth << endl; -// routes.push_back(FGTaxiRoute(nodesStack,routesStack,totalDistance, depth)); -// if (nodesStack.empty() || routesStack.empty()) -// { -// printRoutingError(string("while finishing route")); -// } -// nodesStack.pop_back(); -// routesStack.pop_back(); -// if (!(foundRoute)) { -// maxDistance = totalDistance; -// } -// else -// if (totalDistance < maxDistance) -// maxDistance = totalDistance; -// foundRoute = true; -// totalDistance -= distance; -// return; -// } -// -// -// // search if the currentNode has been encountered before -// // if so, we should step back one level, because it is -// // rather rediculous to proceed further from here. -// // if the current node has not been encountered before, -// // i should point to nodesStack.end()-1; and we can continue -// // if i is not nodesStack.end, the previous node was found, -// // and we should return. -// // This only works at trace levels of 1 or higher though -// if (depth > 0) { -// intVecIterator i = nodesStack.begin(); -// while ((*i) != currNode->getIndex()) { -// //cerr << "Route so far : " << (*i) << endl; -// i++; -// } -// if (i != nodesStack.end()-1) { -// if (nodesStack.empty() || routesStack.empty()) -// { -// printRoutingError(string("while returning from an already encountered node")); -// } -// nodesStack.pop_back(); -// routesStack.pop_back(); -// totalDistance -= distance; -// return; -// } -// if (depth >= maxDepth) { -// count++; -// if (!(count % 100000)) { -// maxDepth--; // Gradually decrease maxdepth, to prevent "eternal searches" -// //cerr << "Reducing maxdepth to " << maxDepth << endl; -// } -// nodesStack.pop_back(); -// routesStack.pop_back(); -// totalDistance -= distance; -// return; -// } -// // If the total distance from start to the current waypoint -// // is longer than that of a route we can also stop this trace -// // and go back one level. -// if ((totalDistance > maxDistance) && foundRoute) -// //if (foundRoute) -// { -// //cerr << "Stopping rediculously long trace: " << totalDistance << endl; -// if (nodesStack.empty() || routesStack.empty()) -// { -// printRoutingError(string("while returning from finding a rediculously long route")); -// } -// nodesStack.pop_back(); -// routesStack.pop_back(); -// totalDistance -= distance; -// return; -// } -// } -/* - //cerr << "2" << endl; - if (currNode->getBeginRoute() != currNode->getEndRoute()) - { - double course, length; - //cerr << "3" << endl; - // calculate distance and heading "as the crow flies" between starn and end points" - SGWayPoint first(currNode->getLongitude(), - currNode->getLatitude(), - 0); - //SGWayPoint second (lastNode->getLongitude(), - // lastNode->getLatitude(), - // 0); - - first.CourseAndDistance(destination, &course, &length); - //for (FGTaxiSegmentVectorIterator - // itr = segments.begin(); - // itr != segments.end(); itr++) - // { - // (*itr)->setCourseDiff(course); - // } - //FGTaxiNodeVectorIterator nde = nodes.begin(); - //while (nde != nodes.end()) { - //(*nde)->sortEndSegments(); - //nde++; - - for (FGTaxiSegmentVectorIterator - i = currNode->getBeginRoute(); - i != currNode->getEndRoute(); - i++) - { - (*i)->setCourseDiff(course); - } - currNode->sortEndSegments(foundRoute); - for (FGTaxiSegmentVectorIterator - i = currNode->getBeginRoute(); - i != currNode->getEndRoute(); - i++) - { - //cerr << (*i)->getLength() << endl; - //cerr << (*i)->getIndex() << endl; - int idx = (*i)->getIndex(); - routesStack.push_back((*i)->getIndex()); - trace((*i)->getEnd(), end, depth+1, (*i)->getLength()); - // { - // // cerr << currNode -> getIndex() << " "; - // route.push_back(currNode->getIndex()); - // return true; - // } - } - } - else - { - //SG_LOG( SG_GENERAL, SG_DEBUG, "4" ); - } - if (nodesStack.empty()) - { - printRoutingError(string("while finishing trace")); - } - nodesStack.pop_back(); - // Make sure not to dump the level-zero routesStack entry, because that was never created. - if (depth) - { - routesStack.pop_back(); - //cerr << "leaving trace " << routesStack.size() << endl; - } - totalDistance -= distance; - return; -}*/ - -void FGGroundNetwork::printRoutingError(string mess) -{ - SG_LOG(SG_GENERAL, SG_ALERT, "Error in ground network trace algorithm " << mess); - if (nodesStack.empty()) - { - SG_LOG(SG_GENERAL, SG_ALERT, " nodesStack is empty. Dumping routesStack"); - for (intVecIterator i = routesStack.begin() ; i != routesStack.end(); i++) - SG_LOG(SG_GENERAL, SG_ALERT, "Route " << (*i)); - } - if (routesStack.empty()) - { - SG_LOG(SG_GENERAL, SG_ALERT, " routesStack is empty. Dumping nodesStack"); - for (intVecIterator i = nodesStack.begin() ; i != nodesStack.end(); i++) - SG_LOG(SG_GENERAL, SG_ALERT, "Node " << (*i)); - } - //exit(1); +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 penalty; } +/* ATC Related Functions */ void FGGroundNetwork::announcePosition(int id, FGAIFlightPlan *intendedRoute, int currentPosition, double lat, double lon, double heading, double speed, double alt, double radius, int leg, - string callsign) + FGAIAircraft *aircraft) { TrafficVectorIterator i = activeTraffic.begin(); // Search search if the current id alread has an entry @@ -642,7 +504,7 @@ void FGGroundNetwork::announcePosition(int id, FGAIFlightPlan *intendedRoute, in rec.setPositionAndIntentions(currentPosition, intendedRoute); rec.setPositionAndHeading(lat, lon, heading, speed, alt); rec.setRadius(radius); // only need to do this when creating the record. - rec.setCallSign(callsign); + rec.setAircraft(aircraft); activeTraffic.push_back(rec); } else { i->setPositionAndIntentions(currentPosition, intendedRoute); @@ -754,7 +616,7 @@ void FGGroundNetwork::checkSpeedAdjustment(int id, double lat, //closest = current; previousInstruction = current->getSpeedAdjustment(); - double mindist = HUGE; + double mindist = HUGE_VAL; if (activeTraffic.size()) { double course, dist, bearing, minbearing; @@ -846,7 +708,7 @@ void FGGroundNetwork::checkSpeedAdjustment(int id, double lat, return; else current->setWaitsForId(closest->getId()); - if (closest != current) + if (closest->getId() != current->getId()) current->setSpeedAdjustment(closest->getSpeed()* (mindist/100)); else current->setSpeedAdjustment(0); // This can only happen when the user aircraft is the one closest @@ -905,7 +767,7 @@ void FGGroundNetwork::checkHoldPosition(int id, double lat, for (i = activeTraffic.begin(); i != activeTraffic.end(); i++) { - if (i != current) + if (i->getId() != current->getId()) { int node = current->crosses(this, *i); if (node != -1) @@ -1089,9 +951,7 @@ bool FGGroundNetwork::checkForCircularWaits(int id) //if (printed) // cerr << "[done] " << endl << endl;; if (id == target) { - SG_LOG(SG_GENERAL, SG_ALERT, "Detected circular wait condition"); - cerr << "Id = " << id << endl; - cerr << "target = " << target << endl; + SG_LOG(SG_GENERAL, SG_WARN, "Detected circular wait condition: Id = " << id << "target = " << target); return true; } else { return false;