X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FAirports%2Fgroundnetwork.cxx;h=b4f48caeeea2f158dc3a8d0042933d1fb94efb99;hb=49030e700ba276d5cd6ffa4f4e661a266fa0105c;hp=7f77222e65f222465a872cb1decf18b71d978520;hpb=c9813d1b5d79b4aad13c263690a0223086af25ac;p=flightgear.git diff --git a/src/Airports/groundnetwork.cxx b/src/Airports/groundnetwork.cxx index 7f77222e6..b4f48caee 100644 --- a/src/Airports/groundnetwork.cxx +++ b/src/Airports/groundnetwork.cxx @@ -27,8 +27,6 @@ #include #include -#include - //#include //#include @@ -43,39 +41,33 @@ //#include
//#include -//#include STL_STRING +#include -#include "groundnetwork.hxx" +#include -SG_USING_STD(sort); +//#include -/************************************************************************** - * FGTaxiNode - *************************************************************************/ -FGTaxiNode::FGTaxiNode() -{ -} +#include "groundnetwork.hxx" /*************************************************************************** * FGTaxiSegment **************************************************************************/ -FGTaxiSegment::FGTaxiSegment() -{ -} void FGTaxiSegment::setStart(FGTaxiNodeVector *nodes) { FGTaxiNodeVectorIterator i = nodes->begin(); while (i != nodes->end()) { - if (i->getIndex() == startNode) + //cerr << "Scanning start node index" << (*i)->getIndex() << endl; + if ((*i)->getIndex() == startNode) { - start = i->getAddress(); - i->addSegment(this); + 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) @@ -83,20 +75,24 @@ void FGTaxiSegment::setEnd(FGTaxiNodeVector *nodes) FGTaxiNodeVectorIterator i = nodes->begin(); while (i != nodes->end()) { - if (i->getIndex() == endNode) + //cerr << "Scanning end node index" << (*i)->getIndex() << endl; + if ((*i)->getIndex() == endNode) { - end = i->getAddress(); + end = (*i)->getAddress(); return; } i++; } + SG_LOG(SG_GENERAL, SG_ALERT, "Could not find end node " << endNode << endl); } + + // There is probably a computationally cheaper way of // doing this. void FGTaxiSegment::setTrackDistance() { - double course; + //double course; SGWayPoint first (start->getLongitude(), start->getLatitude(), 0); @@ -104,10 +100,22 @@ void FGTaxiSegment::setTrackDistance() end->getLatitude(), 0); first.CourseAndDistance(second, &course, &length); +} + + +void FGTaxiSegment::setCourseDiff(double crse) +{ + headingDiff = fabs(course-crse); + if (headingDiff > 180) + headingDiff = fabs(headingDiff - 360); } -bool FGTaxiRoute::next(int *val) + +/*************************************************************************** + * FGTaxiRoute + **************************************************************************/ +bool FGTaxiRoute::next(int *nde) { //for (intVecIterator i = nodes.begin(); i != nodes.end(); i++) // cerr << "FGTaxiRoute contains : " << *(i) << endl; @@ -116,16 +124,80 @@ bool FGTaxiRoute::next(int *val) // 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; - *val = *(currNode); + *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) +{ + //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); + } + if (currNode == nodes.end()) + return false; + *nde = *(currNode); + //*rte = *(currRoute); + if (currNode != nodes.begin()) // Make sure route corresponds to the end node + { + *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. + *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); +} FGGroundNetwork::FGGroundNetwork() { @@ -133,16 +205,39 @@ FGGroundNetwork::FGGroundNetwork() foundRoute = false; totalDistance = 0; maxDistance = 0; + //maxDepth = 1000; + count = 0; + currTraffic = activeTraffic.begin(); + +} + +FGGroundNetwork::~FGGroundNetwork() +{ + for (FGTaxiNodeVectorIterator node = nodes.begin(); + node != nodes.end(); + node++) + { + delete (*node); + } + nodes.clear(); + pushBackNodes.clear(); + for (FGTaxiSegmentVectorIterator seg = segments.begin(); + seg != segments.end(); + seg++) + { + delete (*seg); + } + segments.clear(); } void FGGroundNetwork::addSegment(const FGTaxiSegment &seg) { - segments.push_back(seg); + segments.push_back(new FGTaxiSegment(seg)); } void FGGroundNetwork::addNode(const FGTaxiNode &node) { - nodes.push_back(node); + nodes.push_back(new FGTaxiNode(node)); } void FGGroundNetwork::addNodes(FGParkingVec *parkings) @@ -154,7 +249,7 @@ void FGGroundNetwork::addNodes(FGParkingVec *parkings) n.setIndex(i->getIndex()); n.setLatitude(i->getLatitude()); n.setLongitude(i->getLongitude()); - nodes.push_back(n); + nodes.push_back(new FGTaxiNode(n)); i++; } @@ -165,23 +260,64 @@ void FGGroundNetwork::addNodes(FGParkingVec *parkings) void FGGroundNetwork::init() { hasNetwork = true; + 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()) { - //cerr << "initializing node " << i->getIndex() << endl; - i->setStart(&nodes); - i->setEnd (&nodes); - i->setTrackDistance(); - //cerr << "Track distance = " << i->getLength() << endl; - //cerr << "Track ends at" << i->getEnd()->getIndex() << endl; + (*i)->setStart(&nodes); + (*i)->setEnd (&nodes); + (*i)->setTrackDistance(); + (*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++; + } + + 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); } + + int FGGroundNetwork::findNearestNode(double lat, double lon) { double minDist = HUGE_VAL; - double course, dist; + double dist; int index; SGWayPoint first (lon, lat, @@ -192,14 +328,14 @@ int FGGroundNetwork::findNearestNode(double lat, double lon) itr != nodes.end(); itr++) { double course; - SGWayPoint second (itr->getLongitude(), - itr->getLatitude(), + SGWayPoint second ((*itr)->getLongitude(), + (*itr)->getLatitude(), 0); first.CourseAndDistance(second, &course, &dist); if (dist < minDist) { minDist = dist; - index = itr->getIndex(); + index = (*itr)->getIndex(); //cerr << "Minimum distance of " << minDist << " for index " << index << endl; } } @@ -207,128 +343,665 @@ int FGGroundNetwork::findNearestNode(double lat, double lon) } FGTaxiNode *FGGroundNetwork::findNode(int idx) -{ - for (FGTaxiNodeVectorIterator - itr = nodes.begin(); - itr != nodes.end(); itr++) +{ /* + 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; +} + +FGTaxiSegment *FGGroundNetwork::findSegment(int idx) +{/* + for (FGTaxiSegmentVectorIterator + itr = segments.begin(); + itr != segments.end(); itr++) { if (itr->getIndex() == idx) return itr->getAddress(); + } + */ + if ((idx > 0) && (idx <= segments.size())) + return segments[idx-1]->getAddress(); + else + { + //cerr << "Alert: trying to find invalid segment " << idx << endl; + return 0; } - return 0; } -FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end) + +FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end, bool fullSearch) { - foundRoute = false; - totalDistance = 0; - FGTaxiNode *firstNode = findNode(start); - FGTaxiNode *lastNode = findNode(end); - //prevNode = prevPrevNode = -1; - //prevNode = start; - routes.clear(); - traceStack.clear(); - trace(firstNode, end, 0, 0); - FGTaxiRoute empty; - - if (!foundRoute) - { - SG_LOG( SG_GENERAL, SG_INFO, "Failed to find route from waypoint " << start << " to " << end ); - exit(1); +//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); // + } + + FGTaxiNode *firstNode = findNode(start); + firstNode->setPathScore(0); + + FGTaxiNode *lastNode = findNode(end); + + FGTaxiNodeVector unvisited(*currNodesSet); // working copy + + while (!unvisited.empty()) { + FGTaxiNode* best = *(unvisited.begin()); + for (FGTaxiNodeVectorIterator + itr = unvisited.begin(); + itr != unvisited.end(); itr++) { + if ((*itr)->getPathScore() < best->getPathScore()) + best = (*itr); + } + + 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; + } + } + } + } + + if (lastNode->getPathScore() == 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); } - sort(routes.begin(), routes.end()); - //for (intVecIterator i = route.begin(); i != route.end(); i++) - // { - // rte->push_back(*i); - // } +} + +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, + FGAIAircraft *aircraft) +{ + 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 + if (activeTraffic.size()) { + //while ((i->getId() != id) && i != activeTraffic.end()) { + while (i != activeTraffic.end()) { + if (i->getId() == id) { + break; + } + i++; + } + } + // Add a new TrafficRecord if no one exsists for this aircraft. + if (i == activeTraffic.end() || (activeTraffic.size() == 0)) { + FGTrafficRecord rec; + rec.setId(id); + rec.setPositionAndIntentions(currentPosition, intendedRoute); + rec.setPositionAndHeading(lat, lon, heading, speed, alt); + rec.setRadius(radius); // only need to do this when creating the record. + rec.setAircraft(aircraft); + activeTraffic.push_back(rec); + } else { + i->setPositionAndIntentions(currentPosition, intendedRoute); + i->setPositionAndHeading(lat, lon, heading, speed, alt); + } +} + +void FGGroundNetwork::signOff(int id) { + 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 + if (activeTraffic.size()) { + //while ((i->getId() != id) && i != activeTraffic.end()) { + while (i != activeTraffic.end()) { + if (i->getId() == id) { + break; + } + i++; + } + } + if (i == activeTraffic.end() || (activeTraffic.size() == 0)) { + SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Aircraft without traffic record is signing off"); + } else { + i = activeTraffic.erase(i); + } +} + +void FGGroundNetwork::update(int id, double lat, double lon, double heading, double speed, double alt, + double dt) { + TrafficVectorIterator i = activeTraffic.begin(); + // Search search if the current id has an entry + // This might be faster using a map instead of a vector, but let's start by taking a safe route + TrafficVectorIterator current, closest; + if (activeTraffic.size()) { + //while ((i->getId() != id) && i != activeTraffic.end()) { + while (i != activeTraffic.end()) { + if (i->getId() == id) { + break; + } + i++; + } + } + // update position of the current aircraft + if (i == activeTraffic.end() || (activeTraffic.size() == 0)) { + SG_LOG(SG_GENERAL, SG_ALERT, "AI error: updating aircraft without traffic record"); + } else { + i->setPositionAndHeading(lat, lon, heading, speed, alt); + current = i; + } - if (routes.begin() != routes.end()) - return *(routes.begin()); - else - return empty; + setDt(getDt() + dt); + + // Update every three secs, but add some randomness + // to prevent all IA objects doing this in synchrony + //if (getDt() < (3.0) + (rand() % 10)) + // return; + //else + // setDt(0); + current->clearResolveCircularWait(); + current->setWaitsForId(0); + checkSpeedAdjustment(id, lat, lon, heading, speed, alt); + checkHoldPosition (id, lat, lon, heading, speed, alt); + if (checkForCircularWaits(id)) { + i->setResolveCircularWait(); + } } +/** + Scan for a speed adjustment change. Find the nearest aircraft that is in front + and adjust speed when we get too close. Only do this when current position and/or + intentions of the current aircraft match current taxiroute position of the proximate + aircraft. For traffic that is on other routes we need to issue a "HOLD Position" + instruction. See below for the hold position instruction. -void FGGroundNetwork::trace(FGTaxiNode *currNode, int end, int depth, double distance) + Note that there currently still is one flaw in the logic that needs to be addressed. + can be situations where one aircraft is in front of the current aircraft, on a separate + route, but really close after an intersection coming off the current route. This + aircraft is still close enough to block the current aircraft. This situation is currently + not addressed yet, but should be. +*/ + +void FGGroundNetwork::checkSpeedAdjustment(int id, double lat, + double lon, double heading, + double speed, double alt) { - traceStack.push_back(currNode->getIndex()); - totalDistance += distance; - //cerr << "Starting trace " << depth << " 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) + + TrafficVectorIterator current, closest; + TrafficVectorIterator i = activeTraffic.begin(); + bool otherReasonToSlowDown = false; + bool previousInstruction; + if (activeTraffic.size()) { - //cerr << "Found route : " << totalDistance << "" << " " << *(traceStack.end()-1) << endl; - routes.push_back(FGTaxiRoute(traceStack,totalDistance)); - traceStack.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 traceStack.end()-1; and we can continue - // if i is not traceStack.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 = traceStack.begin(); - while ((*i) != currNode->getIndex()) { - //cerr << "Route so far : " << (*i) << endl; - i++; + //while ((i->getId() != id) && (i != activeTraffic.end())) + while (i != activeTraffic.end()) { + if (i->getId() == id) { + break; + } + i++; + } } - if (i != traceStack.end()-1) { - traceStack.pop_back(); - totalDistance -= distance; + else + { 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) - { - //cerr << "Stopping rediculously long trace: " << totalDistance << endl; - traceStack.pop_back(); - totalDistance -= distance; - return; - } + if (i == activeTraffic.end() || (activeTraffic.size() == 0)) { + SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkSpeedAdjustment"); } + current = i; + //closest = current; - //cerr << "2" << endl; - if (currNode->getBeginRoute() != currNode->getEndRoute()) + previousInstruction = current->getSpeedAdjustment(); + double mindist = HUGE_VAL; + if (activeTraffic.size()) { - //cerr << "3" << endl; - for (FGTaxiSegmentPointerVectorIterator - i = currNode->getBeginRoute(); - i != currNode->getEndRoute(); - i++) + double course, dist, bearing, minbearing; + SGWayPoint curr (lon, + lat, + alt); + //TrafficVector iterator closest; + closest = current; + for (TrafficVectorIterator i = activeTraffic.begin(); + i != activeTraffic.end(); i++) + { + if (i != current) { + //SGWayPoint curr (lon, + // lat, + // alt); + SGWayPoint other (i->getLongitude (), + i->getLatitude (), + i->getAltitude ()); + other.CourseAndDistance(curr, &course, &dist); + bearing = fabs(heading-course); + if (bearing > 180) + bearing = 360-bearing; + if ((dist < mindist) && (bearing < 60.0)) + { + mindist = dist; + closest = i; + minbearing = bearing; + } + } + } + //Check traffic at the tower controller + if (towerController->hasActiveTraffic()) + { + for (TrafficVectorIterator i = towerController->getActiveTraffic().begin(); + i != towerController->getActiveTraffic().end(); i++) + { + //cerr << "Comparing " << current->getId() << " and " << i->getId() << endl; + //SGWayPoint curr (lon, + // lat, + // alt); + SGWayPoint other (i->getLongitude (), + i->getLatitude (), + i->getAltitude ()); + other.CourseAndDistance(curr, &course, &dist); + bearing = fabs(heading-course); + if (bearing > 180) + bearing = 360-bearing; + if ((dist < mindist) && (bearing < 60.0)) + { + mindist = dist; + closest = i; + minbearing = bearing; + otherReasonToSlowDown = true; + } + } + } + // Finally, check UserPosition + double userLatitude = fgGetDouble("/position/latitude-deg"); + double userLongitude = fgGetDouble("/position/longitude-deg"); + SGWayPoint user (userLongitude, + userLatitude, + alt); // Alt is not really important here. + user.CourseAndDistance(curr, &course, &dist); + bearing = fabs(heading-course); + if (bearing > 180) + bearing = 360-bearing; + if ((dist < mindist) && (bearing < 60.0)) + { + mindist = dist; + //closest = i; + minbearing = bearing; + otherReasonToSlowDown = true; + } + + // if (closest == current) { + // //SG_LOG(SG_GENERAL, SG_ALERT, "AI error: closest and current match"); + // //return; + // } + //cerr << "Distance : " << dist << " bearing : " << bearing << " heading : " << heading + // << " course : " << course << endl; + current->clearSpeedAdjustment(); + + if (current->checkPositionAndIntentions(*closest) || otherReasonToSlowDown) { - //cerr << (*i)->getLenght() << endl; - trace((*i)->getEnd(), end, depth+1, (*i)->getLength()); - // { - // // cerr << currNode -> getIndex() << " "; - // route.push_back(currNode->getIndex()); - // return true; - // } + double maxAllowableDistance = (1.1*current->getRadius()) + (1.1*closest->getRadius()); + if (mindist < 2*maxAllowableDistance) + { + if (current->getId() == closest->getWaitsForId()) + return; + else + current->setWaitsForId(closest->getId()); + 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 + if (mindist < maxAllowableDistance) + { + //double newSpeed = (maxAllowableDistance-mindist); + //current->setSpeedAdjustment(newSpeed); + //if (mindist < 0.5* maxAllowableDistance) + // { + current->setSpeedAdjustment(0); + // } + } + } + } + } +} + +/** + Check for "Hold position instruction". + The hold position should be issued under the following conditions: + 1) For aircraft entering or crossing a runway with active traffic on it, or landing aircraft near it + 2) For taxiing aircraft that use one taxiway in opposite directions + 3) For crossing or merging taxiroutes. +*/ + +void FGGroundNetwork::checkHoldPosition(int id, double lat, + double lon, double heading, + double speed, double alt) +{ + + TrafficVectorIterator current; + TrafficVectorIterator i = activeTraffic.begin(); + if (activeTraffic.size()) + { + //while ((i->getId() != id) && i != activeTraffic.end()) + while (i != activeTraffic.end()) { + if (i->getId() == id) { + break; } + i++; + } } else { - SG_LOG( SG_GENERAL, SG_DEBUG, "4" ); + return ; + } + if (i == activeTraffic.end() || (activeTraffic.size() == 0)) { + SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkHoldPosition"); + } + current = i; + current->setHoldPosition(false); + SGWayPoint curr (lon, + lat, + alt); + double course, dist, bearing, minbearing; + for (i = activeTraffic.begin(); + i != activeTraffic.end(); i++) + { + if (i->getId() != current->getId()) + { + int node = current->crosses(this, *i); + if (node != -1) + { + // 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; + SGWayPoint nodePos(findNode(node)->getLongitude (), + findNode(node)->getLatitude (), + alt); + + SGWayPoint other (i->getLongitude (), + i->getLatitude (), + i->getAltitude ()); + //other.CourseAndDistance(curr, &course, &dist); + 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; + other.CourseAndDistance(nodePos, &course, &dist); + if (dist > 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; + } + } + curr.CourseAndDistance(nodePos, &course, &dist); + 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()))) + + { + 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; + } + } + } + } + } +} + +/** + * Check whether situations occur where the current aircraft is waiting for itself + * due to higher order interactions. + * A 'circular' wait is a situation where a waits for b, b waits for c, and c waits + * for a. Ideally each aircraft only waits for one other aircraft, so by tracing + * through this list of waiting aircraft, we can check if we'd eventually end back + * at the current aircraft. + * + * Note that we should consider the situation where we are actually checking aircraft + * d, which is waiting for aircraft a. d is not part of the loop, but is held back by + * the looping aircraft. If we don't check for that, this function will get stuck into + * endless loop. + */ + +bool FGGroundNetwork::checkForCircularWaits(int id) +{ + //cerr << "Performing Wait check " << id << endl; + int target = 0; + TrafficVectorIterator current, other; + TrafficVectorIterator i = activeTraffic.begin(); + int trafficSize = activeTraffic.size(); + if (trafficSize) { + while (i != activeTraffic.end()) { + if (i->getId() == id) { + break; + } + i++; + } + } + else { + return false; + } + if (i == activeTraffic.end() || (trafficSize == 0)) { + SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits"); + } + + current = i; + target = current->getWaitsForId(); + //bool printed = false; // Note that this variable is for debugging purposes only. + int counter = 0; + + if (id == target) { + //cerr << "aircraft waits for user" << endl; + return false; + } + + + while ((target > 0) && (target != id) && counter++ < trafficSize) { + //printed = true; + TrafficVectorIterator i = activeTraffic.begin(); + if (trafficSize) { + //while ((i->getId() != id) && i != activeTraffic.end()) + while (i != activeTraffic.end()) { + if (i->getId() == target) { + break; + } + i++; + } + } + else { + return false; + } + if (i == activeTraffic.end() || (trafficSize == 0)) { + //cerr << "[Waiting for traffic at Runway: DONE] " << endl << endl;; + // The target id is not found on the current network, which means it's at the tower + //SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits"); + return false; } - traceStack.pop_back(); - totalDistance -= distance; - return; + other = i; + target = other->getWaitsForId(); + + // actually this trap isn't as impossible as it first seemed: + // the setWaitsForID(id) is set to current when the aircraft + // is waiting for the user controlled aircraft. + //if (current->getId() == other->getId()) { + // cerr << "Caught the impossible trap" << endl; + // cerr << "Current = " << current->getId() << endl; + // cerr << "Other = " << other ->getId() << endl; + // for (TrafficVectorIterator at = activeTraffic.begin(); + // at != activeTraffic.end(); + // at++) { + // cerr << "currently active aircraft : " << at->getCallSign() << " with Id " << at->getId() << " waits for " << at->getWaitsForId() << endl; + // } + // exit(1); + if (current->getId() == other->getId()) + return false; + //} + //cerr << current->getCallSign() << " (" << current->getId() << ") " << " -> " << other->getCallSign() + // << " (" << other->getId() << "); " << endl;; + //current = other; + } + + + + + + + //if (printed) + // cerr << "[done] " << endl << endl;; + if (id == target) { + SG_LOG(SG_GENERAL, SG_WARN, "Detected circular wait condition: Id = " << id << "target = " << target); + return true; + } else { + return false; + } +} + +// Note that this function is probably obsolete... +bool FGGroundNetwork::hasInstruction(int id) +{ + TrafficVectorIterator i = activeTraffic.begin(); + // Search search if the current id has an entry + // This might be faster using a map instead of a vector, but let's start by taking a safe route + if (activeTraffic.size()) + { + //while ((i->getId() != id) && i != activeTraffic.end()) { + while (i != activeTraffic.end()) { + if (i->getId() == id) { + break; + } + i++; + } + } + if (i == activeTraffic.end() || (activeTraffic.size() == 0)) { + SG_LOG(SG_GENERAL, SG_ALERT, "AI error: checking ATC instruction for aircraft without traffic record"); + } else { + return i->hasInstruction(); + } + return false; } +FGATCInstruction FGGroundNetwork::getInstruction(int id) +{ + TrafficVectorIterator i = activeTraffic.begin(); + // Search search if the current id has an entry + // This might be faster using a map instead of a vector, but let's start by taking a safe route + if (activeTraffic.size()) { + //while ((i->getId() != id) && i != activeTraffic.end()) { + while (i != activeTraffic.end()) { + if (i->getId() == id) { + break; + } + i++; + } + } + if (i == activeTraffic.end() || (activeTraffic.size() == 0)) { + SG_LOG(SG_GENERAL, SG_ALERT, "AI error: requesting ATC instruction for aircraft without traffic record"); + } else { + return i->getInstruction(); + } + return FGATCInstruction(); +} + +