-
// groundnet.cxx - Implimentation of the FlightGear airport ground handling code
//
// Written by Durk Talsma, started June 2005.
# 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());
+}
-// There is probably a computationally cheaper way of
-// doing this.
-void FGTaxiSegment::setDimensions(double elevation)
+double FGTaxiSegment::getHeading() const
{
- length = SGGeodesy::distanceM(start->getGeod(), end->getGeod());
- //heading = SGGeodesy::headingDeg(start->getGeod(), end->getGeod());
-
- double az2; //, distanceM;
- SGGeodesy::inverse(start->getGeod(), end->getGeod(), heading, az2, length);
- double coveredDistance = length * 0.5;
- SGGeodesy::direct(start->getGeod(), heading, coveredDistance, center, az2);
- //start->setElevation(elevation);
- //end->setElevation(elevation);
- //cerr << "Centerpoint = (" << center.getLatitudeDeg() << ", " << center.getLongitudeDeg() << "). Heading = " << heading << endl;
+ return SGGeodesy::courseDeg(getStart()->geod(), getEnd()->geod());
}
-//void FGTaxiSegment::setCourseDiff(double crse)
-//{
-// headingDiff = fabs(course - crse);
+void FGTaxiSegment::block(int id, time_t blockTime, time_t now)
+{
+ 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::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)
+bool compare_trafficrecords(FGTrafficRecord a, FGTrafficRecord b)
{
- return (*a) < (*b);
+ return (a.getIntentions().size() < b.getIntentions().size());
}
-FGGroundNetwork::FGGroundNetwork()
+FGGroundNetwork::FGGroundNetwork() :
+ parent(NULL)
{
hasNetwork = false;
- foundRoute = false;
totalDistance = 0;
maxDistance = 0;
//maxDepth = 1000;
count = 0;
currTraffic = activeTraffic.begin();
group = 0;
+ version = 0;
networkInitialized = false;
}
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;
- }
- }
- for (FGTaxiNodeVectorIterator node = nodes.begin();
- node != nodes.end(); node++) {
- if (saveData) {
- cachefile << (*node)->getIndex () << " "
- << (*node)->getElevation () << " "
- << 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());
saveData = true;
}
}
- for (FGTaxiNodeVectorIterator node = nodes.begin();
- node != nodes.end(); node++) {
- if (saveData) {
- cachefile << (*node)->getIndex () << " "
- << (*node)->getElevation () << " "
- << endl;
- }
+ cachefile << "[GroundNetcachedata:ref:2011:09:04]" << endl;
+ for (IndexTaxiNodeMap::iterator node = nodes.begin();
+ node != nodes.end(); node++) {
+ if (saveData) {
+ 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());
- 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());
- 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;
+ 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));
+ }
+ }
+}
+
+void FGGroundNetwork::parseCache()
+{
+ 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);
}
+ }
}
+ }
+#endif
+}
- return index;
+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
+}
-FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end,
- bool fullSearch)
+static int edgePenalty(FGTaxiNode* tn)
{
-//implements Dijkstra's algorithm to find shortest distance route from start to end
-//taken from http://en.wikipedia.org/wiki/Dijkstra's_algorithm
+ return (tn->type() == FGPositioned::PARKING ? 10000 : 0) +
+ (tn->getIsOnRunway() ? 1000 : 0);
+}
- //double INFINITE = 100000000000.0;
- // initialize scoring values
- int nParkings = parent->getDynamics()->getNrOfParkings();
- FGTaxiNodeVector *currNodesSet;
- if (fullSearch) {
- currNodesSet = &nodes;
- } else {
- currNodesSet = &pushBackNodes;
- }
+class ShortestPathData
+{
+public:
+ ShortestPathData() :
+ score(HUGE_VAL)
+ {}
+
+ double score;
+ FGTaxiNodeRef previousNode;
+};
- for (FGTaxiNodeVectorIterator
- itr = currNodesSet->begin(); itr != currNodesSet->end(); itr++) {
- (*itr)->setPathScore(HUGE_VAL); //infinity by all practical means
- (*itr)->setPreviousNode(0); //
- (*itr)->setPreviousSeg(0); //
+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
+ 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;
+
+ return FGTaxiRoute();
}
- if (end->getIsOnRunway()) { // For now. In future versions, need to find out whether runway is active.
- penalty += 1000;
+
+ // 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_back(rec);
+ if (leg == 2) {
+ activeTraffic.push_front(rec);
+ } else {
+ activeTraffic.push_back(rec);
+ }
+
} else {
i->setPositionAndIntentions(currentPosition, intendedRoute);
i->setPositionAndHeading(lat, lon, heading, speed, alt);
}
if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
SG_LOG(SG_GENERAL, SG_ALERT,
- "AI error: Aircraft without traffic record is signing off");
+ "AI error: Aircraft without traffic record is signing off at " << SG_ORIGIN);
} else {
i = activeTraffic.erase(i);
}
* 9 = Acknowledge switch tower frequency
*************************************************************************************************************************/
bool FGGroundNetwork::checkTransmissionState(int minState, int maxState, TrafficVectorIterator i, time_t now, AtcMsgId msgId,
- AtcMsgDir msgDir)
+ AtcMsgDir msgDir)
{
int state = i->getState();
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");
- atc->getATCDialog()->removeEntry(1);
+ // PopupCallback(n);
+ //cerr << "Selected transmission message " << n << endl;
+ //FGATCManager *atc = (FGATCManager*) globals->get_subsystem("atc");
+ FGATCDialogNew::instance()->removeEntry(1);
} else {
//cerr << "creating message for " << i->getAircraft()->getCallSign() << endl;
transmit(&(*i), &(*parent->getDynamics()), msgId, msgDir, false);
}
void FGGroundNetwork::updateAircraftInformation(int id, double lat, double lon,
- double heading, double speed, double alt,
- double dt)
+ double heading, double speed, double alt,
+ double dt)
{
time_t currentTime = time(NULL);
if (nextSave < currentTime) {
saveElevationCache();
nextSave = currentTime + 100 + rand() % 200;
}
- // Check whether aircraft are on hold due to a preceding pushback. If so, make sure to
+ // 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 ...
+ // Transmit confirmation ...
// Probably use a status mechanism similar to the Engine start procedure in the startup controller.
// 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");
+ "AI error: updating aircraft without traffic record at " << SG_ORIGIN);
} else {
i->setPositionAndHeading(lat, lon, heading, speed, alt);
current = i;
bool needsTaxiClearance = current->getAircraft()->getTaxiClearanceRequest();
if (!needsTaxiClearance) {
checkHoldPosition(id, lat, lon, heading, speed, alt);
- if (checkForCircularWaits(id)) {
- i->setResolveCircularWait();
- }
+ //if (checkForCircularWaits(id)) {
+ // i->setResolveCircularWait();
+ //}
} else {
current->setHoldPosition(true);
int state = current->getState();
aircraft. For traffic that is on other routes we need to issue a "HOLD Position"
instruction. See below for the hold position instruction.
- Note that there currently still is one flaw in the logic that needs to be addressed.
+ Note that there currently still is one flaw in the logic that needs to be addressed.
There can be situations where one aircraft is in front of the current aircraft, on a separate
route, but really close after an intersection coming off the current route. This
aircraft is still close enough to block the current aircraft. This situation is currently
*/
void FGGroundNetwork::checkSpeedAdjustment(int id, double lat,
- double lon, double heading,
- double speed, double alt)
+ double lon, double heading,
+ double speed, double alt)
{
- TrafficVectorIterator current, closest;
+ 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()) {
}
if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
SG_LOG(SG_GENERAL, SG_ALERT,
- "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkSpeedAdjustment");
+ "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkSpeedAdjustment at " << SG_ORIGIN);
}
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++) {
+ i != activeTraffic.end(); i++) {
if (i == current) {
continue;
}
if ((dist < mindist) && (bearing < 60.0)) {
mindist = dist;
closest = i;
- minbearing = bearing;
+ closestOnNetwork = i;
+// minbearing = bearing;
+
}
}
//Check traffic at the tower controller
if (towerController->hasActiveTraffic()) {
for (TrafficVectorIterator i =
- towerController->getActiveTraffic().begin();
- i != towerController->getActiveTraffic().end(); 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(),
bearing = 360 - bearing;
if ((dist < mindist) && (bearing < 60.0)) {
//cerr << "Current aircraft " << current->getAircraft()->getTrafficRef()->getCallSign()
- // << " is closest to " << i->getAircraft()->getTrafficRef()->getCallSign()
- // << ", which has status " << i->getAircraft()->isScheduledForTakeoff()
+ // << " is closest to " << i->getAircraft()->getTrafficRef()->getCallSign()
+ // << ", which has status " << i->getAircraft()->isScheduledForTakeoff()
// << endl;
mindist = dist;
closest = i;
- minbearing = bearing;
+// minbearing = bearing;
otherReasonToSlowDown = true;
}
}
}
*/
current->clearSpeedAdjustment();
-
+ bool needBraking = false;
if (current->checkPositionAndIntentions(*closest)
- || otherReasonToSlowDown) {
+ || otherReasonToSlowDown) {
double maxAllowableDistance =
(1.1 * current->getRadius()) +
(1.1 * closest->getRadius());
if (closest->getId() != current->getId()) {
current->setSpeedAdjustment(closest->getSpeed() *
(mindist / 100));
- if (
- closest->getAircraft()->getTakeOffStatus() &&
- (current->getAircraft()->getTrafficRef()->getDepartureAirport() == closest->getAircraft()->getTrafficRef()->getDepartureAirport()) &&
- (current->getAircraft()->GetFlightPlan()->getRunway() == closest->getAircraft()->GetFlightPlan()->getRunway())
- )
- current->getAircraft()->scheduleForATCTowerDepartureControl(1);
+ needBraking = true;
+
+// if (
+// closest->getAircraft()->getTakeOffStatus() &&
+// (current->getAircraft()->getTrafficRef()->getDepartureAirport() == closest->getAircraft()->getTrafficRef()->getDepartureAirport()) &&
+// (current->getAircraft()->GetFlightPlan()->getRunway() == closest->getAircraft()->GetFlightPlan()->getRunway())
+// )
+// current->getAircraft()->scheduleForATCTowerDepartureControl(1);
} else {
current->setSpeedAdjustment(0); // This can only happen when the user aircraft is the one closest
}
}
}
}
+ if ((closest->getId() == closestOnNetwork->getId()) && (current->getPriority() < closest->getPriority()) && needBraking) {
+ swap(current, closest);
+ }
}
}
TrafficVectorIterator current;
TrafficVectorIterator i = activeTraffic.begin();
if (activeTraffic.size()) {
- //while ((i->getId() != id) && i != activeTraffic.end())
+ //while ((i->getId() != id) && i != activeTraffic.end())
while (i != activeTraffic.end()) {
if (i->getId() == id) {
break;
} 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");
+ "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkHoldPosition at " << SG_ORIGIN);
}
current = i;
+ //
+ if (current->getAircraft()->getTakeOffStatus() == 1) {
+ current->setHoldPosition(true);
+ 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));
-
- 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;
- }
- }
+ //SGGeod curr(SGGeod::fromDegM(lon, lat, alt));
+ int currentRoute = i->getCurrentPosition();
+ int nextRoute;
+ if (i->getIntentions().size()) {
+ nextRoute = (*(i->getIntentions().begin()));
+ } else {
+ nextRoute = 0;
+ }
+ if (currentRoute > 0) {
+ FGTaxiSegment *tx = findSegment(currentRoute);
+ FGTaxiSegment *nx;
+ if (nextRoute) {
+ nx = findSegment(nextRoute);
+ } else {
+ nx = tx;
+ }
+ //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;
}
//int state = current->getState();
if (checkTransmissionState(1,1, current, now, MSG_ACKNOWLEDGE_HOLD_POSITION, ATC_AIR_TO_GROUND)) {
- current->setState(0);
- current->setHoldPosition(true);
+ current->setState(0);
+ current->setHoldPosition(true);
}
if (checkTransmissionState(2,2, current, now, MSG_ACKNOWLEDGE_RESUME_TAXI, ATC_AIR_TO_GROUND)) {
- current->setState(0);
- current->setHoldPosition(false);
+ current->setState(0);
+ current->setHoldPosition(false);
}
if (current->getAircraft()->getTakeOffStatus() && (current->getState() == 0)) {
- //cerr << "Scheduling " << current->getAircraft()->getCallSign() << " for hold short" << endl;
- current->setState(6);
- }
+ //cerr << "Scheduling " << current->getAircraft()->getCallSign() << " for hold short" << endl;
+ current->setState(6);
+ }
if (checkTransmissionState(6,6, current, now, MSG_REPORT_RUNWAY_HOLD_SHORT, ATC_AIR_TO_GROUND)) {
}
if (checkTransmissionState(7,7, current, now, MSG_ACKNOWLEDGE_REPORT_RUNWAY_HOLD_SHORT, ATC_GROUND_TO_AIR)) {
- //current->setState(0);
-}
+ //current->setState(0);
+}
/**
* Check whether situations occur where the current aircraft is waiting for itself
- * due to higher order interactions.
+ * due to higher order interactions.
* A 'circular' wait is a situation where a waits for b, b waits for c, and c waits
- * for a. Ideally each aircraft only waits for one other aircraft, so by tracing
- * through this list of waiting aircraft, we can check if we'd eventually end back
+ * for a. Ideally each aircraft only waits for one other aircraft, so by tracing
+ * through this list of waiting aircraft, we can check if we'd eventually end back
* at the current aircraft.
*
* Note that we should consider the situation where we are actually checking aircraft
}
if (i == activeTraffic.end() || (trafficSize == 0)) {
SG_LOG(SG_GENERAL, SG_ALERT,
- "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits");
+ "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits at " << SG_ORIGIN);
}
current = i;
//printed = true;
TrafficVectorIterator i = activeTraffic.begin();
if (trafficSize) {
- //while ((i->getId() != id) && i != activeTraffic.end())
+ //while ((i->getId() != id) && i != activeTraffic.end())
while (i != activeTraffic.end()) {
if (i->getId() == target) {
break;
// 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.
+ // is waiting for the user controlled aircraft.
//if (current->getId() == other->getId()) {
// cerr << "Caught the impossible trap" << endl;
// cerr << "Current = " << current->getId() << endl;
if (current->getId() == other->getId())
return false;
//}
- //cerr << current->getCallSign() << " (" << current->getId() << ") " << " -> " << other->getCallSign()
+ //cerr << current->getCallSign() << " (" << current->getId() << ") " << " -> " << other->getCallSign()
// << " (" << other->getId() << "); " << endl;;
//current = other;
}
}
if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
SG_LOG(SG_GENERAL, SG_ALERT,
- "AI error: checking ATC instruction for aircraft without traffic record");
+ "AI error: checking ATC instruction for aircraft without traffic record at " << SG_ORIGIN);
} else {
return i->hasInstruction();
}
}
if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
SG_LOG(SG_GENERAL, SG_ALERT,
- "AI error: requesting ATC instruction for aircraft without traffic record");
+ "AI error: requesting ATC instruction for aircraft without traffic record at " << SG_ORIGIN);
} else {
return i->getInstruction();
}
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 = ;
//while (group->getNumChildren()) {
// cerr << "Number of children: " << group->getNumChildren() << endl;
//simgear::EffectGeode* geode = (simgear::EffectGeode*) group->getChild(0);
- //osg::MatrixTransform *obj_trans = (osg::MatrixTransform*) group->getChild(0);
- //geode->releaseGLObjects();
- //group->removeChild(geode);
- //delete geode;
+ //osg::MatrixTransform *obj_trans = (osg::MatrixTransform*) group->getChild(0);
+ //geode->releaseGLObjects();
+ //group->removeChild(geode);
+ //delete geode;
group = 0;
}
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;
osg::MatrixTransform *obj_trans = new osg::MatrixTransform;
if (isUserAircraft((i)->getAircraft())) {
elevationStart = fgGetDouble("/position/ground-elev-m");
} else {
- elevationStart = ((i)->getAircraft()->_getAltitude());
+ elevationStart = ((i)->getAircraft()->_getAltitude());
}
- double elevationEnd = segments[pos]->getEnd()->getElevation();
+ double elevationEnd = segments[pos]->getEnd()->getElevationM();
//cerr << "Using elevation " << elevationEnd << endl;
- if (elevationEnd == 0) {
+ 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_meters += 0.5;
+// elevation_feet = elevationEnd * SG_METER_TO_FEET + 0.5;
+ //elevation_meters += 0.5;
}
- else {
- elevationEnd = parent->getElevation()+8+dx;
+ else {
+ elevationEnd = parent->getElevation();
}
segments[pos]->getEnd()->setElevation(elevationEnd);
}
double elevationMean = (elevationStart + elevationEnd) / 2.0;
double elevDiff = elevationEnd - elevationStart;
-
- double slope = atan2(elevDiff, length) * SGD_RADIANS_TO_DEGREES;
-
- //cerr << "1. Using mean elevation : " << elevationMean << " and " << slope << endl;
+
+ double slope = atan2(elevDiff, length) * SGD_RADIANS_TO_DEGREES;
+
+ //cerr << "1. Using mean elevation : " << elevationMean << " and " << slope << endl;
WorldCoordinate( obj_pos, center.getLatitudeDeg(), center.getLongitudeDeg(), elevationMean+ 0.5, -(heading), slope );
geode->setName("test");
geode->addDrawable(geometry);
//osg::Node *custom_obj;
- SGMaterial *mat = matlib->find("UnidirectionalTaper");
+ SGMaterial *mat;
+ if (segments[pos]->hasBlock(now)) {
+ mat = matlib->find("UnidirectionalTaperRed", center);
+ } else {
+ mat = matlib->find("UnidirectionalTaperGreen", center);
+ }
if (mat)
geode->setEffect(mat->get_effect());
obj_trans->addChild(geode);
// wire as much of the scene graph together as we can
//->addChild( obj_trans );
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;
}
- for(intVecIterator j = (i)->getIntentions().begin(); j != (i)->getIntentions().end(); j++) {
+ // 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;
if (k >= 0) {
obj_trans->setDataVariance(osg::Object::STATIC);
// Experimental: Calculate slope here, based on length, and the individual elevations
- double elevationStart = segments[k]->getStart()->getElevation();
- double elevationEnd = segments[k]->getEnd ()->getElevation();
- if (elevationStart == 0) {
- SGGeod center2 = segments[k]->getStart()->getGeod();
+ double elevationStart = segments[k]->getStart()->getElevationM();
+ double elevationEnd = segments[k]->getEnd ()->getElevationM();
+ if ((elevationStart == 0) || (elevationStart == parent->getElevation())) {
+ 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 {
- elevationStart = parent->getElevation()+8+dx;
+ else {
+ elevationStart = parent->getElevation();
}
segments[k]->getStart()->setElevation(elevationStart);
}
- if (elevationEnd == 0) {
- SGGeod center2 = segments[k]->getEnd()->getGeod();
+ if ((elevationEnd == 0) || (elevationEnd == parent->getElevation())) {
+ 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 {
- elevationEnd = parent->getElevation()+8+dx;
+ else {
+ elevationEnd = parent->getElevation();
}
segments[k]->getEnd()->setElevation(elevationEnd);
}
double elevDiff = elevationEnd - elevationStart;
double length = segments[k]->getLength();
double slope = atan2(elevDiff, length) * SGD_RADIANS_TO_DEGREES;
-
- // cerr << "2. Using mean elevation : " << elevationMean << " and " << slope << endl;
+ // 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->setName("test");
geode->addDrawable(geometry);
//osg::Node *custom_obj;
- SGMaterial *mat = matlib->find("UnidirectionalTaper");
+ SGMaterial *mat;
+ if (segments[k]->hasBlock(now)) {
+ mat = matlib->find("UnidirectionalTaperRed", segCenter);
+ } else {
+ mat = matlib->find("UnidirectionalTaperGreen", segCenter);
+ }
if (mat)
geode->setEffect(mat->get_effect());
obj_trans->addChild(geode);
string FGGroundNetwork::getName() {
return string(parent->getId() + "-ground");
}
+
+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(now);
+ }
+ int priority = 1;
+ //sort(activeTraffic.begin(), activeTraffic.end(), compare_trafficrecords);
+ // Handle traffic that is under ground control first; this way we'll prevent clutter at the gate areas.
+ // Don't allow an aircraft to pushback when a taxiing aircraft is currently using part of the intended route.
+ for (TrafficVectorIterator i = parent->getDynamics()->getStartupController()->getActiveTraffic().begin();
+ i != parent->getDynamics()->getStartupController()->getActiveTraffic().end(); i++) {
+ i->allowPushBack();
+ i->setPriority(priority++);
+ // 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 (TrafficVectorIterator j = activeTraffic.begin(); j != activeTraffic.end(); j++) {
+ int pos = j->getCurrentPosition();
+ if (pos > 0) {
+ FGTaxiSegment *seg = segments[pos-1]->opposite();
+ if (seg) {
+ int posReverse = seg->getIndex();
+ for (intVecIterator k = i->getIntentions().begin(); k != i->getIntentions().end(); k++) {
+ if ((*k) == posReverse) {
+ i->denyPushBack();
+ 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(i->getId(), now, now);
+ }
+ }
+ }
+ for (intVecIterator j = i->getIntentions().begin(); j != i->getIntentions().end(); j++) {
+ int pos = (*j);
+ 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(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) {
+ 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());
+ }
+
+ }
+ intVecIterator ivi;
+ for (ivi = i->getIntentions().begin(); ivi != i->getIntentions().end(); ivi++) {
+ int segIndex = (*ivi);
+ if (segIndex > 0) {
+ if (segments[segIndex-1]->hasBlock(now))
+ break;
+ }
+ }
+ //after this, ivi points just behind the last valid unblocked taxi segment.
+ for (intVecIterator j = i->getIntentions().begin(); j != ivi; j++) {
+ int pos = (*j);
+ 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)) {
+ time_t blockTime = now + (length / vTaxi);
+ (*tsi)->block(i->getId(), blockTime - 30, now);
+ }
+ }
+ }
+ }
+ }
+}
+
projects / flightgear.git / blobdiff