#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();
void FGGroundNetwork::update(int id, double lat, double lon, double heading, double speed, double alt,
double dt) {
+ // Check whether aircraft are on hold due to a preceding pushback. If so, make sure to
+ // Transmit air-to-ground "Ready to taxi request:
+ // Transmit ground to air approval / hold
+ // Transmit confirmation ...
+ // Probably use a status mechanism similar to the Engine start procedure in the startup controller.
+
+
TrafficVectorIterator i = activeTraffic.begin();
// Search search if the current id has an entry
// This might be faster using a map instead of a vector, but let's start by taking a safe route
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(userLatitude,userLongitude));
+ SGGeodesy::inverse(user, curr, course, az2, dist);
+
bearing = fabs(heading-course);
if (bearing > 180)
bearing = 360-bearing;
}
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