X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FAirports%2Fgroundnetwork.cxx;h=3ba65015859e7a13d1d02197797223b9162c4c90;hb=52b0baace1d3ef636b4e1c8f8f1bd047f8d0b023;hp=321a3af68833c7212d8f67d037e6b3e651d872e5;hpb=4e0e5c0b98b2b72d9c63e1851560939b143a7b78;p=flightgear.git diff --git a/src/Airports/groundnetwork.cxx b/src/Airports/groundnetwork.cxx index 321a3af68..3ba650158 100644 --- a/src/Airports/groundnetwork.cxx +++ b/src/Airports/groundnetwork.cxx @@ -30,8 +30,10 @@ #include #include +#include #include +#include #include #include "groundnetwork.hxx" @@ -40,37 +42,35 @@ * FGTaxiSegment **************************************************************************/ -void FGTaxiSegment::setStart(FGTaxiNodeVector *nodes) +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(); - (*i)->addSegment(this); - return; - } - i++; - } - SG_LOG(SG_GENERAL, SG_ALERT, "Could not find start node " << startNode << endl); + 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) +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(); - return; - } - i++; - } - SG_LOG(SG_GENERAL, SG_ALERT, "Could not find end node " << endNode << endl); + 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); } @@ -79,90 +79,91 @@ void FGTaxiSegment::setEnd(FGTaxiNodeVector *nodes) // doing this. void FGTaxiSegment::setTrackDistance() { - length = SGGeodesy::distanceM(start->getGeod(), end->getGeod()); + length = SGGeodesy::distanceM(start->getGeod(), end->getGeod()); } void FGTaxiSegment::setCourseDiff(double crse) { - headingDiff = fabs(course-crse); - - if (headingDiff > 180) - headingDiff = fabs(headingDiff - 360); + headingDiff = fabs(course - crse); + + 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) +{ + //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) -{ - //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++; +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); } - else + if (currNode == nodes.end()) + return false; + *nde = *(currNode); + //*rte = *(currRoute); + if (currNode != nodes.begin()) // Make sure route corresponds to the end node { - // 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; + *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); + 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); } @@ -171,67 +172,64 @@ void FGTaxiRoute::rewind(int route) /*************************************************************************** * FGGroundNetwork() **************************************************************************/ -bool compare_nodes(FGTaxiNode *a, FGTaxiNode *b) { -return (*a) < (*b); +bool compare_nodes(FGTaxiNode * a, FGTaxiNode * b) +{ + return (*a) < (*b); } -bool compare_segments(FGTaxiSegment *a, FGTaxiSegment *b) { -return (*a) < (*b); +bool compare_segments(FGTaxiSegment * a, FGTaxiSegment * b) +{ + return (*a) < (*b); } FGGroundNetwork::FGGroundNetwork() { - hasNetwork = false; - foundRoute = false; - totalDistance = 0; - maxDistance = 0; - //maxDepth = 1000; - count = 0; - currTraffic = activeTraffic.begin(); + hasNetwork = false; + 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); + 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); + nodes.clear(); + pushBackNodes.clear(); + for (FGTaxiSegmentVectorIterator seg = segments.begin(); + seg != segments.end(); seg++) { + delete(*seg); } - segments.clear(); + segments.clear(); } -void FGGroundNetwork::addSegment(const FGTaxiSegment &seg) +void FGGroundNetwork::addSegment(const FGTaxiSegment & seg) { - segments.push_back(new FGTaxiSegment(seg)); + segments.push_back(new FGTaxiSegment(seg)); } -void FGGroundNetwork::addNode(const FGTaxiNode &node) +void FGGroundNetwork::addNode(const FGTaxiNode & node) { - nodes.push_back(new FGTaxiNode(node)); + nodes.push_back(new FGTaxiNode(node)); } -void FGGroundNetwork::addNodes(FGParkingVec *parkings) +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()); - nodes.push_back(new FGTaxiNode(n)); - - i++; + FGTaxiNode n; + FGParkingVecIterator i = parkings->begin(); + while (i != parkings->end()) { + n.setIndex(i->getIndex()); + n.setLatitude(i->getLatitude()); + n.setLongitude(i->getLongitude()); + nodes.push_back(new FGTaxiNode(n)); + + i++; } } @@ -239,120 +237,117 @@ 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()) { - (*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); + 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()) { + (*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(const SGGeod& aGeod) +int FGGroundNetwork::findNearestNode(const SGGeod & aGeod) { - 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; + 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; + + return index; } int FGGroundNetwork::findNearestNode(double lat, double lon) { - return findNearestNode(SGGeod::fromDeg(lon, lat)); + return findNearestNode(SGGeod::fromDeg(lon, lat)); } 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; +{ /* + 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(unsigned 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; +{ /* + 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; } } -FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end, bool fullSearch) +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 @@ -362,38 +357,37 @@ FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end, bool fullSear int nParkings = parent->getDynamics()->getNrOfParkings(); FGTaxiNodeVector *currNodesSet; if (fullSearch) { - currNodesSet = &nodes; + currNodesSet = &nodes; } else { - currNodesSet = &pushBackNodes; + 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); // - } + 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); + FGTaxiNode *lastNode = findNode(end); - FGTaxiNodeVector unvisited(*currNodesSet); // working copy + FGTaxiNodeVector unvisited(*currNodesSet); // working copy while (!unvisited.empty()) { - FGTaxiNode* best = *(unvisited.begin()); + FGTaxiNode *best = *(unvisited.begin()); for (FGTaxiNodeVectorIterator - itr = unvisited.begin(); - itr != unvisited.end(); itr++) { - if ((*itr)->getPathScore() < best->getPathScore()) - best = (*itr); + itr = unvisited.begin(); itr != unvisited.end(); itr++) { + if ((*itr)->getPathScore() < best->getPathScore()) + best = (*itr); } - FGTaxiNodeVectorIterator newend = remove(unvisited.begin(), unvisited.end(), best); + FGTaxiNodeVectorIterator newend = + remove(unvisited.begin(), unvisited.end(), best); unvisited.erase(newend, unvisited.end()); - + if (best == lastNode) { // found route or best not connected break; } else { @@ -401,15 +395,17 @@ FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end, bool fullSear 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) + 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); // - } + tgt->setPreviousSeg(*seg); // + } } else { - // // cerr << "Skipping TaxiSegment " << (*seg)->getIndex() << endl; + // // cerr << "Skipping TaxiSegment " << (*seg)->getIndex() << endl; } } } @@ -417,17 +413,18 @@ FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end, bool fullSear 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()); + if (fullSearch) { + SG_LOG(SG_GENERAL, SG_ALERT, + "Failed to find route from waypoint " << start << " to " + << end << " at " << parent->getId()); } - FGTaxiRoute empty; - return empty; + 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; + FGTaxiNode *bt = lastNode; while (bt->getPreviousNode() != 0) { nodes.push_back(bt->getIndex()); routes.push_back(bt->getPreviousSegment()->getIndex()); @@ -441,116 +438,162 @@ FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end, bool fullSear } } -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; +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) +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); - } + 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.setLeg(leg); + 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::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) { - // Check whether aircraft are on hold due to a preceding pushback. If so, make sure to - // Transmit air-to-ground "Ready to taxi request: - // Transmit ground to air approval / hold - // Transmit confirmation ... - // Probably use a status mechanism similar to the Engine start procedure in the startup controller. - - - 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; - } - - 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(); - } +void FGGroundNetwork::updateAircraftInformation(int id, double lat, double lon, + double heading, double speed, double alt, + double dt) +{ + // Check whether aircraft are on hold due to a preceding pushback. If so, make sure to + // Transmit air-to-ground "Ready to taxi request: + // Transmit ground to air approval / hold + // Transmit confirmation ... + // Probably use a status mechanism similar to the Engine start procedure in the startup controller. + + + 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; + } + + 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); + bool needsTaxiClearance = current->getAircraft()->getTaxiClearanceRequest(); + if (!needsTaxiClearance) { + checkHoldPosition(id, lat, lon, heading, speed, alt); + if (checkForCircularWaits(id)) { + i->setResolveCircularWait(); + } + } else { + current->setHoldPosition(true); + int state = current->getState(); + time_t now = time(NULL) + fgGetLong("/sim/time/warp"); + if ((now - lastTransmission) > 15) { + available = true; + } + if ((state < 3) && available) { + transmit(&(*current), MSG_REQUEST_TAXI_CLEARANCE, ATC_AIR_TO_GROUND); + current->setState(3); + lastTransmission = now; + available = false; + } + if ((state == 3) && available) { + transmit(&(*current), MSG_ISSUE_TAXI_CLEARANCE, ATC_GROUND_TO_AIR); + current->setState(4); + lastTransmission = now; + available = false; + } + if ((state == 4) && available) { + transmit(&(*current), MSG_ACKNOWLEDGE_TAXI_CLEARANCE, ATC_AIR_TO_GROUND); + current->setState(5); + lastTransmission = now; + available = false; + } + if ((state == 5) && available) { + current->setState(0); + current->getAircraft()->setTaxiClearanceRequest(false); + current->setHoldPosition(true); + available = false; + } + + } + } /** @@ -567,129 +610,123 @@ void FGGroundNetwork::update(int id, double lat, double lon, double heading, dou not addressed yet, but should be. */ -void FGGroundNetwork::checkSpeedAdjustment(int id, double lat, - double lon, double heading, - double speed, double alt) +void FGGroundNetwork::checkSpeedAdjustment(int id, double lat, + double lon, double heading, + double speed, double alt) { - - TrafficVectorIterator current, closest; - TrafficVectorIterator i = activeTraffic.begin(); - bool otherReasonToSlowDown = false; - bool previousInstruction; - if (activeTraffic.size()) - { - //while ((i->getId() != id) && (i != activeTraffic.end())) - while (i != activeTraffic.end()) { - if (i->getId() == id) { - break; - } - i++; - } - } - else - { - 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; - - previousInstruction = current->getSpeedAdjustment(); - double mindist = HUGE_VAL; - if (activeTraffic.size()) - { - double course, dist, bearing, minbearing, az2; - SGGeod curr(SGGeod::fromDegM(lon, lat, alt)); - //TrafficVector iterator closest; - closest = current; - for (TrafficVectorIterator i = activeTraffic.begin(); - i != activeTraffic.end(); i++) - { - if (i == current) { - continue; - } - - SGGeod other(SGGeod::fromDegM(i->getLongitude(), - i->getLatitude(), i->getAltitude())); - SGGeodesy::inverse(curr, other, course, az2, 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; - SGGeod other(SGGeod::fromDegM(i->getLongitude(), - i->getLatitude(), - i->getAltitude())); - SGGeodesy::inverse(curr, other, course, az2, 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"); - SGGeod user(SGGeod::fromDeg(userLongitude,userLatitude)); - SGGeodesy::inverse(curr, user, course, az2, 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; - } - - current->clearSpeedAdjustment(); - - if (current->checkPositionAndIntentions(*closest) || otherReasonToSlowDown) - { - 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); - // } - } - } - } + + TrafficVectorIterator current, closest; + TrafficVectorIterator i = activeTraffic.begin(); + bool otherReasonToSlowDown = false; + bool previousInstruction; + if (activeTraffic.size()) { + //while ((i->getId() != id) && (i != activeTraffic.end())) + while (i != activeTraffic.end()) { + if (i->getId() == id) { + break; + } + i++; + } + } else { + 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; + + previousInstruction = current->getSpeedAdjustment(); + double mindist = HUGE_VAL; + if (activeTraffic.size()) { + double course, dist, bearing, minbearing, az2; + SGGeod curr(SGGeod::fromDegM(lon, lat, alt)); + //TrafficVector iterator closest; + closest = current; + for (TrafficVectorIterator i = activeTraffic.begin(); + i != activeTraffic.end(); i++) { + if (i == current) { + continue; + } + + SGGeod other(SGGeod::fromDegM(i->getLongitude(), + i->getLatitude(), + i->getAltitude())); + SGGeodesy::inverse(curr, other, course, az2, 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; + SGGeod other(SGGeod::fromDegM(i->getLongitude(), + i->getLatitude(), + i->getAltitude())); + SGGeodesy::inverse(curr, other, course, az2, 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"); + SGGeod user(SGGeod::fromDeg(userLongitude, userLatitude)); + SGGeodesy::inverse(curr, user, course, az2, 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; + } + + current->clearSpeedAdjustment(); + + if (current->checkPositionAndIntentions(*closest) + || otherReasonToSlowDown) { + 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); + // } + } + } + } } } @@ -701,114 +738,146 @@ void FGGroundNetwork::checkSpeedAdjustment(int id, double lat, 3) For crossing or merging taxiroutes. */ -void FGGroundNetwork::checkHoldPosition(int id, double lat, - double lon, double heading, - double speed, double alt) +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 - { - 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); - SGGeod curr(SGGeod::fromDegM(lon, lat, alt)); - - 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; - } - } - - 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()))) - - { - 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; - } - } - } - } + 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 { + 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; + bool origStatus = current->hasHoldPosition(); + current->setHoldPosition(false); + SGGeod curr(SGGeod::fromDegM(lon, lat, alt)); + + 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; + } + } + + 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()))) + + { + 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; + } + } + } + } + } + bool currStatus = current->hasHoldPosition(); + + // 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; + } + 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); + //cerr << "Transmittin hold short instrudtion " << currStatus << " " << available << endl; + current->setState(1); + } else { + transmit(&(*current), MSG_RESUME_TAXI, ATC_GROUND_TO_AIR); + //cerr << "Transmittig resume instrudtion " << currStatus << " " << available << endl; + current->setState(2); + } + lastTransmission = now; + available = false; + // Don't act on the changed instruction until the transmission is confirmed + // So set back to original status + current->setHoldPosition(origStatus); + //cerr << "Current state " << current->getState() << endl; + } else { + } + int state = current->getState(); + if ((state == 1) && (available)) { + //cerr << "ACKNOWLEDGE HOLD" << endl; + transmit(&(*current), MSG_ACKNOWLEDGE_HOLD_POSITION, ATC_AIR_TO_GROUND); + current->setState(0); + current->setHoldPosition(true); + lastTransmission = now; + available = false; + + } + if ((state == 2) && (available)) { + //cerr << "ACKNOWLEDGE RESUME" << endl; + transmit(&(*current), MSG_ACKNOWLEDGE_RESUME_TAXI, ATC_AIR_TO_GROUND); + current->setState(0); + current->setHoldPosition(false); + lastTransmission = now; + available = false; } } @@ -827,142 +896,142 @@ void FGGroundNetwork::checkHoldPosition(int id, double lat, */ 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) { +{ + //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 { + if (i->getId() == id) { + 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; - } - 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; - } + 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; + } + 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; - } + + //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; + // 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(); + 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(); } - -