fly/flightgear
1
0
Fork 0
Code Activity

Convert waypoint::CourseAndDistance users to use SGGeodesy helper functions.

Browse source
This commit is contained in:
jmt 2009-06-09 19:39:18 +00:00 committed by Tim Moore
parent e1583cb28c
commit 6e42458a55
8 changed files with 120 additions and 162 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

14
src/AIModel/AIAircraft.cxx
View file

@ -644,16 +644,10 @@ bool FGAIAircraft::leadPointReached(FGAIFlightPlan::waypoint* curr) {
bool FGAIAircraft::aiTrafficVisible() {
double userLatitude = fgGetDouble("/position/latitude-deg");
double userLongitude = fgGetDouble("/position/longitude-deg");
double course, distance;
SGWayPoint current(pos.getLongitudeDeg(), pos.getLatitudeDeg(), 0);
SGWayPoint user (userLongitude, userLatitude, 0);
user.CourseAndDistance(current, &course, &distance);
return ((distance * SG_METER_TO_NM) <= TRAFFICTOAIDISTTODIE);
SGGeod userPos(SGGeod::fromDeg(fgGetDouble("/position/longitude-deg"),
fgGetDouble("/position/latitude-deg")));
return (SGGeodesy::distanceNm(userPos, pos) <= TRAFFICTOAIDISTTODIE);
}

4
src/AIModel/AIFlightPlanCreate.cxx
View file

@ -286,7 +286,7 @@ void FGAIFlightPlan::createTakeoffTaxi(FGAIAircraft *ac, bool firstFlight,
char buffer[10];
snprintf (buffer, 10, "%d", node);
FGTaxiNode *tn = apt->getDynamics()->getGroundNetwork()->findNode(node);
waypoint* wpt = createOnGround(ac, buffer, tn->geod(), apt->getElevation(), ac->getPerformance()->vTaxi());
waypoint* wpt = createOnGround(ac, buffer, tn->getGeod(), apt->getElevation(), ac->getPerformance()->vTaxi());
wpt->routeIndex = route;
waypoints.push_back(wpt);
}
@ -358,7 +358,7 @@ void FGAIFlightPlan::createLandingTaxi(FGAIAircraft *ac, FGAirport *apt,
char buffer[10];
snprintf (buffer, 10, "%d", node);
FGTaxiNode *tn = gn->findNode(node);
waypoint* wpt = createOnGround(ac, buffer, tn->geod(), apt->getElevation(), ac->getPerformance()->vTaxi());
waypoint* wpt = createOnGround(ac, buffer, tn->getGeod(), apt->getElevation(), ac->getPerformance()->vTaxi());
wpt->routeIndex = route;
waypoints.push_back(wpt);
}

52
src/AIModel/AIFlightPlanCreateCruise.cxx
View file

@ -51,37 +51,30 @@ void FGAIFlightPlan::evaluateRoutePart(double deplat,
intVec nodes;
int tmpNode, prevNode;
SGWayPoint first (deplon,
deplat,
100);
SGWayPoint sec (arrlon,
arrlat,
100);
double course, distance;
first.CourseAndDistance(sec, &course, &distance);
distance *= SG_METER_TO_NM;
SGVec3d a = SGVec3d::fromGeoc(SGGeoc::fromDegM(deplon, deplat, 1));
SGVec3d b = SGVec3d::fromGeoc(SGGeoc::fromDegM(arrlon, arrlat, 1));
SGGeoc dep(SGGeoc::fromDegM(deplon, deplat, 100.0));
SGGeoc arr(SGGeoc::fromDegM(arrlon, arrlat, 100.0));
SGVec3d a = SGVec3d::fromGeoc(dep);
SGVec3d b = SGVec3d::fromGeoc(arr);
SGVec3d _cross = cross(b, a);
double angle = sgACos(dot(a, b));
tmpNode = 0;
for (double ang = 0.0; ang < angle; ang += 0.05)
{
{
sgdVec3 newPos;
sgdMat4 matrix;
//cerr << "Angle = " << ang << endl;
sgdMakeRotMat4(matrix, ang, _cross.sg());
for(int j = 0; j < 3; j++)
{
newPos[j] =0.0;
for (int k = 0; k<3; k++)
{
newPos[j] += matrix[j][k]*a[k];
}
}
{
newPos[j] =0.0;
for (int k = 0; k<3; k++)
{
newPos[j] += matrix[j][k]*a[k];
}
}
//cerr << "1"<< endl;
SGGeoc geoc = SGGeoc::fromCart(SGVec3d(newPos[0], newPos[1], newPos[2]));
@ -91,17 +84,12 @@ void FGAIFlightPlan::evaluateRoutePart(double deplat,
prevNode = tmpNode;
tmpNode = globals->get_airwaynet()->findNearestNode(midlat, midlon);
double nodelat = globals->get_airwaynet()->findNode(tmpNode)->getLatitude ();
double nodelon = globals->get_airwaynet()->findNode(tmpNode)->getLongitude ();
SGWayPoint curr(midlat,
midlon,
100);
SGWayPoint node(nodelat,
nodelon,
100);
curr.CourseAndDistance(node, &course, &distance);
if ((distance < 25000) && (tmpNode != prevNode))
nodes.push_back(tmpNode);
FGNode* node = globals->get_airwaynet()->findNode(tmpNode);
SGGeoc nodePos(SGGeoc::fromGeod(node->getPosition()));
if ((tmpNode != prevNode) && (SGGeodesy::distanceM(geoc, nodePos) < 25000)) {
nodes.push_back(tmpNode);
}
}
intVecIterator i = nodes.begin();

20
src/Airports/gnnode.cxx
View file

@ -44,6 +44,26 @@ bool sortByLength(FGTaxiSegment *a, FGTaxiSegment *b) {
*************************************************************************/
void FGTaxiNode::setLatitude (double val)
{
geod.setLatitudeDeg(val);
}
void FGTaxiNode::setLongitude(double val)
{
geod.setLongitudeDeg(val);
}
void FGTaxiNode::setLatitude (const string& val)
{
geod.setLatitudeDeg(processPosition(val));
}
void FGTaxiNode::setLongitude(const string& val)
{
geod.setLongitudeDeg(processPosition(val));
}
void FGTaxiNode::sortEndSegments(bool byLength)
{
if (byLength)

33
src/Airports/gnnode.hxx
View file

@ -22,22 +22,17 @@
#include <simgear/compiler.h>
#include <simgear/math/sg_geodesy.hxx>
using std::string;
using std::vector;
class FGTaxiSegment;
typedef vector<FGTaxiSegment*> FGTaxiSegmentVector;
typedef std::vector<FGTaxiSegment*> FGTaxiSegmentVector;
typedef FGTaxiSegmentVector::iterator FGTaxiSegmentVectorIterator;
bool sortByHeadingDiff(FGTaxiSegment *a, FGTaxiSegment *b);
bool sortByLength (FGTaxiSegment *a, FGTaxiSegment *b);
double processPosition(const string& pos);
class FGTaxiNode
{
private:
double lat;
double lon;
SGGeod geod;
int index;
bool isOnRunway;
@ -51,8 +46,6 @@ private:
public:
FGTaxiNode() :
lat (0.0),
lon (0.0),
index(0),
isOnRunway(false),
holdType(0),
@ -63,8 +56,7 @@ public:
};
FGTaxiNode(const FGTaxiNode &other) :
lat(other.lat),
lon(other.lon),
geod(other.geod),
index(other.index),
isOnRunway(other.isOnRunway),
holdType(other.holdType),
@ -77,8 +69,7 @@ public:
FGTaxiNode &operator =(const FGTaxiNode &other)
{
lat = other.lat;
lon = other.lon;
geod = other.geod;
index = other.index;
isOnRunway = other.isOnRunway;
holdType = other.holdType;
@ -90,10 +81,10 @@ FGTaxiNode &operator =(const FGTaxiNode &other)
};
void setIndex(int idx) { index = idx; };
void setLatitude (double val) { lat = val; };
void setLongitude(double val) { lon = val; };
void setLatitude (const string& val) { lat = processPosition(val); };
void setLongitude(const string& val) { lon = processPosition(val); };
void setLatitude (double val);
void setLongitude(double val);
void setLatitude (const std::string& val);
void setLongitude(const std::string& val);
void addSegment(FGTaxiSegment *segment) { next.push_back(segment); };
void setHoldPointType(int val) { holdType = val; };
void setOnRunway(bool val) { isOnRunway = val; };
@ -106,10 +97,10 @@ FGTaxiNode &operator =(const FGTaxiNode &other)
FGTaxiSegment *getPreviousSegment() { return previousSeg; };
double getPathScore() { return pathScore; };
double getLatitude() { return lat;};
double getLongitude(){ return lon;};
double getLatitude() { return geod.getLatitudeDeg();};
double getLongitude(){ return geod.getLongitudeDeg();};
SGGeod geod() const { return SGGeod::fromDeg(lon, lat); }
const SGGeod& getGeod() const { return geod; }
int getIndex() { return index; };
int getHoldPointType() { return holdType; };
@ -124,7 +115,7 @@ FGTaxiNode &operator =(const FGTaxiNode &other)
};
typedef vector<FGTaxiNode*> FGTaxiNodeVector;
typedef std::vector<FGTaxiNode*> FGTaxiNodeVector;
typedef FGTaxiNodeVector::iterator FGTaxiNodeVectorIterator;
#endif

123
src/Airports/groundnetwork.cxx
View file

@ -79,14 +79,7 @@ void FGTaxiSegment::setEnd(FGTaxiNodeVector *nodes)
// 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());
}
@ -301,35 +294,26 @@ void FGGroundNetwork::init()
int FGGroundNetwork::findNearestNode(const SGGeod& aGeod)
{
return findNearestNode(aGeod.getLatitudeDeg(), aGeod.getLongitudeDeg());
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;
}
int FGGroundNetwork::findNearestNode(double lat, double lon)
{
double minDist = HUGE_VAL;
double dist;
int index;
SGWayPoint first (lon,
lat,
0);
for (FGTaxiNodeVectorIterator
itr = nodes.begin();
itr != nodes.end(); itr++)
{
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;
}
}
return index;
return findNearestNode(SGGeod::fromDeg(lon, lat));
}
FGTaxiNode *FGGroundNetwork::findNode(int idx)
@ -609,24 +593,21 @@ void FGGroundNetwork::checkSpeedAdjustment(int id, double lat,
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(other, curr, course, az2, dist);
bearing = fabs(heading-course);
if (bearing > 180)
bearing = 360-bearing;
if ((dist < mindist) && (bearing < 60.0))
@ -635,7 +616,6 @@ void FGGroundNetwork::checkSpeedAdjustment(int id, double lat,
closest = i;
minbearing = bearing;
}
}
}
//Check traffic at the tower controller
if (towerController->hasActiveTraffic())
@ -644,16 +624,12 @@ void FGGroundNetwork::checkSpeedAdjustment(int id, double lat,
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(other, curr, course, az2, dist);
bearing = fabs(heading-course);
if (bearing > 180)
bearing = 360-bearing;
bearing = 360-bearing;
if ((dist < mindist) && (bearing < 60.0))
{
mindist = dist;
@ -666,10 +642,9 @@ void FGGroundNetwork::checkSpeedAdjustment(int id, double lat,
// 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;
@ -750,10 +725,8 @@ void FGGroundNetwork::checkHoldPosition(int id, double lat,
}
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++)
{
@ -762,20 +735,16 @@ void FGGroundNetwork::checkHoldPosition(int id, double lat,
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))
{
@ -793,8 +762,7 @@ void FGGroundNetwork::checkHoldPosition(int id, double lat,
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 "
@ -806,8 +774,9 @@ void FGGroundNetwork::checkHoldPosition(int id, double lat,
//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()) &&

21
src/Navaids/awynet.cxx
View file

@ -47,11 +47,18 @@ FGNode::FGNode()
{
}
FGNode::FGNode(double lt, double ln, int idx, std::string id) :
ident(id),
geod(SGGeod::fromDeg(ln, lt)),
index(idx)
{
}
bool FGNode::matches(string id, double lt, double ln)
{
if ((ident == id) &&
(fabs(lt - lat) < 1.0) &&
(fabs(ln - lon) < 1.0))
(fabs(lt - geod.getLatitudeDeg()) < 1.0) &&
(fabs(ln - geod.getLongitudeDeg()) < 1.0))
return true;
else
return false;
@ -92,15 +99,7 @@ void FGAirway::setEnd(node_map *nodes)
// doing this.
void FGAirway::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->getPosition(), end->getPosition());
}
/***************************************************************************

15
src/Navaids/awynet.hxx
View file

@ -50,24 +50,21 @@ class FGNode
{
private:
std::string ident;
double lat;
double lon;
SGGeod geod;
int index;
FGAirwayPointerVector next; // a vector to all the segments leaving from this node
public:
FGNode();
FGNode(double lt, double ln, int idx, std::string id) { lat = lt; lon = ln; index = idx; ident = id;};
FGNode(double lt, double ln, int idx, std::string id);
void setIndex(int idx) { index = idx;};
void setLatitude (double val) { lat = val;};
void setLongitude(double val) { lon = val;};
//void setLatitude (const std::string& val) { lat = processPosition(val); };
//void setLongitude(const std::string& val) { lon = processPosition(val); };
void addAirway(FGAirway *segment) { next.push_back(segment); };
double getLatitude() { return lat;};
double getLongitude(){ return lon;};
double getLatitude() { return geod.getLatitudeDeg();};
double getLongitude(){ return geod.getLongitudeDeg();};
const SGGeod& getPosition() {return geod;}
int getIndex() { return index; };
std::string getIdent() { return ident; };