#include <math.h>
#include <algorithm>
-//#include <plib/sg.h>
-//#include <plib/ul.h>
-
-//#include <Environment/environment_mgr.hxx>
-//#include <Environment/environment.hxx>
-//#include <simgear/misc/sg_path.hxx>
-//#include <simgear/props/props.hxx>
-//#include <simgear/structure/subsystem_mgr.hxx>
#include <simgear/debug/logstream.hxx>
#include <simgear/route/waypoint.hxx>
-//#include <Main/globals.hxx>
-//#include <Main/fg_props.hxx>
-//#include <Airports/runways.hxx>
-#include <AIModel/AIFlightPlan.hxx>
+#include <Airports/dynamics.hxx>
-//#include STL_STRING
+#include <AIModel/AIFlightPlan.hxx>
#include "groundnetwork.hxx"
// doing this.
void FGTaxiSegment::setTrackDistance()
{
- //double course;
- SGWayPoint first (start->getLongitude(),
- start->getLatitude(),
- 0);
- SGWayPoint second (end->getLongitude(),
- end->getLatitude(),
- 0);
- first.CourseAndDistance(second, &course, &length);
+ length = SGGeodesy::distanceM(start->getGeod(), end->getGeod());
}
{
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();
+// 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);
//exit(1);
}
-
-
-int FGGroundNetwork::findNearestNode(double lat, double lon)
+int FGGroundNetwork::findNearestNode(const SGGeod& aGeod)
{
double minDist = HUGE_VAL;
- double dist;
- int index;
- SGWayPoint first (lon,
- lat,
- 0);
+ int index = -1;
- for (FGTaxiNodeVectorIterator
- itr = nodes.begin();
- itr != nodes.end(); itr++)
+ for (FGTaxiNodeVectorIterator itr = nodes.begin(); itr != nodes.end(); itr++)
+ {
+ double d = SGGeodesy::distanceM(aGeod, (*itr)->getGeod());
+ if (d < minDist)
{
- double course;
- SGWayPoint second ((*itr)->getLongitude(),
- (*itr)->getLatitude(),
- 0);
- first.CourseAndDistance(second, &course, &dist);
- if (dist < minDist)
- {
- minDist = dist;
- index = (*itr)->getIndex();
- //cerr << "Minimum distance of " << minDist << " for index " << index << endl;
- }
+ minDist = d;
+ index = (*itr)->getIndex();
+ //cerr << "Minimum distance of " << minDist << " for index " << index << endl;
}
+ }
+
return index;
}
-FGTaxiNode *FGGroundNetwork::findNode(int idx)
+int FGGroundNetwork::findNearestNode(double lat, double lon)
+{
+ return findNearestNode(SGGeod::fromDeg(lon, lat));
+}
+
+FGTaxiNode *FGGroundNetwork::findNode(unsigned idx)
{ /*
for (FGTaxiNodeVectorIterator
itr = nodes.begin();
return 0;
}
-FGTaxiSegment *FGGroundNetwork::findSegment(int idx)
+FGTaxiSegment *FGGroundNetwork::findSegment(unsigned idx)
{/*
for (FGTaxiSegmentVectorIterator
itr = segments.begin();
return penalty;
}
-// void FGGroundNetwork::trace(FGTaxiNode *currNode, int end, int depth, double distance)
-// {
-// // Just check some preconditions of the trace algorithm
-// if (nodesStack.size() != routesStack.size())
-// {
-// SG_LOG(SG_GENERAL, SG_ALERT, "size of nodesStack and routesStack is not equal. NodesStack :"
-// << nodesStack.size() << ". RoutesStack : " << routesStack.size());
-// }
-// nodesStack.push_back(currNode->getIndex());
-// totalDistance += distance;
-// //cerr << "Starting trace " << currNode->getIndex() << " " << "total distance: " << totalDistance << endl;
-// // << currNode->getIndex() << endl;
-//
-// // If the current route matches the required end point we found a valid route
-// // So we can add this to the routing table
-// if (currNode->getIndex() == end)
-// {
-// maxDepth = depth;
-// //cerr << "Found route : " << totalDistance << "" << " " << *(nodesStack.end()-1) << " Depth = " << depth << endl;
-// routes.push_back(FGTaxiRoute(nodesStack,routesStack,totalDistance, depth));
-// if (nodesStack.empty() || routesStack.empty())
-// {
-// printRoutingError(string("while finishing route"));
-// }
-// nodesStack.pop_back();
-// routesStack.pop_back();
-// if (!(foundRoute)) {
-// maxDistance = totalDistance;
-// }
-// else
-// if (totalDistance < maxDistance)
-// maxDistance = totalDistance;
-// foundRoute = true;
-// totalDistance -= distance;
-// return;
-// }
-//
-//
-// // search if the currentNode has been encountered before
-// // if so, we should step back one level, because it is
-// // rather rediculous to proceed further from here.
-// // if the current node has not been encountered before,
-// // i should point to nodesStack.end()-1; and we can continue
-// // if i is not nodesStack.end, the previous node was found,
-// // and we should return.
-// // This only works at trace levels of 1 or higher though
-// if (depth > 0) {
-// intVecIterator i = nodesStack.begin();
-// while ((*i) != currNode->getIndex()) {
-// //cerr << "Route so far : " << (*i) << endl;
-// i++;
-// }
-// if (i != nodesStack.end()-1) {
-// if (nodesStack.empty() || routesStack.empty())
-// {
-// printRoutingError(string("while returning from an already encountered node"));
-// }
-// nodesStack.pop_back();
-// routesStack.pop_back();
-// totalDistance -= distance;
-// return;
-// }
-// if (depth >= maxDepth) {
-// count++;
-// if (!(count % 100000)) {
-// maxDepth--; // Gradually decrease maxdepth, to prevent "eternal searches"
-// //cerr << "Reducing maxdepth to " << maxDepth << endl;
-// }
-// nodesStack.pop_back();
-// routesStack.pop_back();
-// totalDistance -= distance;
-// return;
-// }
-// // If the total distance from start to the current waypoint
-// // is longer than that of a route we can also stop this trace
-// // and go back one level.
-// if ((totalDistance > maxDistance) && foundRoute)
-// //if (foundRoute)
-// {
-// //cerr << "Stopping rediculously long trace: " << totalDistance << endl;
-// if (nodesStack.empty() || routesStack.empty())
-// {
-// printRoutingError(string("while returning from finding a rediculously long route"));
-// }
-// nodesStack.pop_back();
-// routesStack.pop_back();
-// totalDistance -= distance;
-// return;
-// }
-// }
-/*
- //cerr << "2" << endl;
- if (currNode->getBeginRoute() != currNode->getEndRoute())
- {
- double course, length;
- //cerr << "3" << endl;
- // calculate distance and heading "as the crow flies" between starn and end points"
- SGWayPoint first(currNode->getLongitude(),
- currNode->getLatitude(),
- 0);
- //SGWayPoint second (lastNode->getLongitude(),
- // lastNode->getLatitude(),
- // 0);
-
- first.CourseAndDistance(destination, &course, &length);
- //for (FGTaxiSegmentVectorIterator
- // itr = segments.begin();
- // itr != segments.end(); itr++)
- // {
- // (*itr)->setCourseDiff(course);
- // }
- //FGTaxiNodeVectorIterator nde = nodes.begin();
- //while (nde != nodes.end()) {
- //(*nde)->sortEndSegments();
- //nde++;
-
- for (FGTaxiSegmentVectorIterator
- i = currNode->getBeginRoute();
- i != currNode->getEndRoute();
- i++)
- {
- (*i)->setCourseDiff(course);
- }
- currNode->sortEndSegments(foundRoute);
- for (FGTaxiSegmentVectorIterator
- i = currNode->getBeginRoute();
- i != currNode->getEndRoute();
- i++)
- {
- //cerr << (*i)->getLength() << endl;
- //cerr << (*i)->getIndex() << endl;
- int idx = (*i)->getIndex();
- routesStack.push_back((*i)->getIndex());
- trace((*i)->getEnd(), end, depth+1, (*i)->getLength());
- // {
- // // cerr << currNode -> getIndex() << " ";
- // route.push_back(currNode->getIndex());
- // return true;
- // }
- }
- }
- else
- {
- //SG_LOG( SG_GENERAL, SG_DEBUG, "4" );
- }
- if (nodesStack.empty())
- {
- printRoutingError(string("while finishing trace"));
- }
- nodesStack.pop_back();
- // Make sure not to dump the level-zero routesStack entry, because that was never created.
- if (depth)
- {
- routesStack.pop_back();
- //cerr << "leaving trace " << routesStack.size() << endl;
- }
- totalDistance -= distance;
- return;
-}*/
-/*
-void FGGroundNetwork::printRoutingError(string mess)
-{
- SG_LOG(SG_GENERAL, SG_ALERT, "Error in ground network trace algorithm " << mess);
- if (nodesStack.empty())
- {
- SG_LOG(SG_GENERAL, SG_ALERT, " nodesStack is empty. Dumping routesStack");
- for (intVecIterator i = routesStack.begin() ; i != routesStack.end(); i++)
- SG_LOG(SG_GENERAL, SG_ALERT, "Route " << (*i));
- }
- if (routesStack.empty())
- {
- SG_LOG(SG_GENERAL, SG_ALERT, " routesStack is empty. Dumping nodesStack");
- for (intVecIterator i = nodesStack.begin() ; i != nodesStack.end(); i++)
- SG_LOG(SG_GENERAL, SG_ALERT, "Node " << (*i));
- }
- //exit(1);
-}
-*/
+/* ATC Related Functions */
void FGGroundNetwork::announcePosition(int id, FGAIFlightPlan *intendedRoute, int currentPosition,
double lat, double lon, double heading,
double speed, double alt, double radius, int leg,
- string callsign)
+ FGAIAircraft *aircraft)
{
TrafficVectorIterator i = activeTraffic.begin();
// Search search if the current id alread has an entry
rec.setPositionAndIntentions(currentPosition, intendedRoute);
rec.setPositionAndHeading(lat, lon, heading, speed, alt);
rec.setRadius(radius); // only need to do this when creating the record.
- rec.setCallSign(callsign);
+ rec.setAircraft(aircraft);
activeTraffic.push_back(rec);
} else {
i->setPositionAndIntentions(currentPosition, intendedRoute);
instruction. See below for the hold position instruction.
Note that there currently still is one flaw in the logic that needs to be addressed.
- can be situations where one aircraft is in front of the current aircraft, on a separate
+ 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
not addressed yet, but should be.
double mindist = HUGE_VAL;
if (activeTraffic.size())
{
- double course, dist, bearing, minbearing;
- SGWayPoint curr (lon,
- lat,
- alt);
+ double course, dist, bearing, minbearing, az2;
+ SGGeod curr(SGGeod::fromDegM(lon, lat, alt));
//TrafficVector iterator closest;
closest = current;
for (TrafficVectorIterator i = activeTraffic.begin();
i != activeTraffic.end(); i++)
{
- if (i != current) {
- //SGWayPoint curr (lon,
- // lat,
- // alt);
- SGWayPoint other (i->getLongitude (),
- i->getLatitude (),
- i->getAltitude ());
- other.CourseAndDistance(curr, &course, &dist);
- bearing = fabs(heading-course);
+ if (i == current) {
+ continue;
+ }
+
+ SGGeod other(SGGeod::fromDegM(i->getLongitude(),
+ i->getLatitude(), i->getAltitude()));
+ SGGeodesy::inverse(curr, other, course, az2, dist);
+ bearing = fabs(heading-course);
if (bearing > 180)
bearing = 360-bearing;
if ((dist < mindist) && (bearing < 60.0))
closest = i;
minbearing = bearing;
}
- }
}
//Check traffic at the tower controller
if (towerController->hasActiveTraffic())
i != towerController->getActiveTraffic().end(); i++)
{
//cerr << "Comparing " << current->getId() << " and " << i->getId() << endl;
- //SGWayPoint curr (lon,
- // lat,
- // alt);
- SGWayPoint other (i->getLongitude (),
- i->getLatitude (),
- i->getAltitude ());
- other.CourseAndDistance(curr, &course, &dist);
- bearing = fabs(heading-course);
+ SGGeod other(SGGeod::fromDegM(i->getLongitude(),
+ i->getLatitude(),
+ i->getAltitude()));
+ SGGeodesy::inverse(curr, other, course, az2, dist);
+ bearing = fabs(heading-course);
if (bearing > 180)
- bearing = 360-bearing;
+ bearing = 360-bearing;
if ((dist < mindist) && (bearing < 60.0))
{
mindist = dist;
// Finally, check UserPosition
double userLatitude = fgGetDouble("/position/latitude-deg");
double userLongitude = fgGetDouble("/position/longitude-deg");
- SGWayPoint user (userLongitude,
- userLatitude,
- alt); // Alt is not really important here.
- user.CourseAndDistance(curr, &course, &dist);
+ SGGeod user(SGGeod::fromDeg(userLongitude,userLatitude));
+ SGGeodesy::inverse(curr, user, course, az2, dist);
+
bearing = fabs(heading-course);
if (bearing > 180)
- bearing = 360-bearing;
+ bearing = 360-bearing;
if ((dist < mindist) && (bearing < 60.0))
{
mindist = dist;
minbearing = bearing;
otherReasonToSlowDown = true;
}
-
- // if (closest == current) {
- // //SG_LOG(SG_GENERAL, SG_ALERT, "AI error: closest and current match");
- // //return;
- // }
- //cerr << "Distance : " << dist << " bearing : " << bearing << " heading : " << heading
- // << " course : " << course << endl;
+
current->clearSpeedAdjustment();
if (current->checkPositionAndIntentions(*closest) || otherReasonToSlowDown)
}
current = i;
current->setHoldPosition(false);
- SGWayPoint curr (lon,
- lat,
- alt);
- double course, dist, bearing, minbearing;
+ SGGeod curr(SGGeod::fromDegM(lon, lat, alt));
+
for (i = activeTraffic.begin();
i != activeTraffic.end(); i++)
{
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;
- SGWayPoint nodePos(findNode(node)->getLongitude (),
- findNode(node)->getLatitude (),
- alt);
-
- SGWayPoint other (i->getLongitude (),
- i->getLatitude (),
- i->getAltitude ());
- //other.CourseAndDistance(curr, &course, &dist);
- bool needsToWait;
+
+ SGGeod other(SGGeod::fromDegM(i->getLongitude(), i->getLatitude(), i->getAltitude()));
+ bool needsToWait;
bool opposing;
if (current->isOpposing(this, *i, node))
{
else
{
opposing = false;
- other.CourseAndDistance(nodePos, &course, &dist);
- if (dist > 200) // 2.0*i->getRadius())
+ 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 "
//cerr << "Hold check 4: " << id << " Would need to wait for other aircraft : distance = " << dist << " meters" << endl;
}
}
- curr.CourseAndDistance(nodePos, &course, &dist);
- if (!(i->hasHoldPosition()))
+
+ double dist = SGGeodesy::distanceM(curr, taxiNode->getGeod());
+ if (!(i->hasHoldPosition()))
{
if ((dist < 200) && //2.5*current->getRadius()) &&
projects / flightgear.git / blobdiff