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