FGFS-4.1/simgear
3
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Change SGWaypoint to use SGGeod internally. Remove some unused code, to

Browse Source 
support cartesian waypoints and compute distance off a cartesian route.
Add a helper to access the total route distance.

Should not cause any visible functionality change.
This commit is contained in:
jmt
2009-06-08 23:18:39 +00:00
committed by Tim Moore
parent f913febd71
commit 03a7d72a62
4 changed files with 62 additions and 88 deletions
Download Patch File Download Diff File Expand all files Collapse all files

39
simgear/route/route.cxx
View File

@@ -38,37 +38,6 @@ SGRoute::SGRoute() {
SGRoute::~SGRoute() {
}
// Calculate perpendicular distance from the current route segment
// This routine assumes all points are laying on a flat plane and
// ignores the altitude (or Z) dimension. For best results, use with
// CARTESIAN way points.
double SGRoute::distance_off_route( double x, double y ) const {
if ( current_wp > 0 ) {
int n0 = current_wp - 1;
int n1 = current_wp;
sgdVec3 p, p0, p1, d;
sgdSetVec3( p, x, y, 0.0 );
sgdSetVec3( p0,
route[n0].get_target_lon(), route[n0].get_target_lat(),
0.0 );
sgdSetVec3( p1,
route[n1].get_target_lon(), route[n1].get_target_lat(),
0.0 );
sgdSubVec3( d, p0, p1 );
return sqrt( sgdClosestPointToLineDistSquared( p, p0, d ) );
} else {
// We are tracking the first waypoint so there is no route
// segment. If you add the current location as the first
// waypoint and the actual waypoint as the second, then we
// will have a route segment and calculate distance from it.
return 0;
}
}
/** Update the length of the leg ending at waypoint index */
void SGRoute::update_distance(int index)
{
@@ -115,3 +84,11 @@ void SGRoute::delete_waypoint( int n ) {
if ( n < size - 1 )
update_distance( n );
}
double SGRoute::total_distance() const {
double total = 0.0;
for (unsigned int i=0; i<route.size(); ++i) {
total += route[i].get_distance();
}
return total;
}

9
simgear/route/route.hxx
View File

@@ -142,14 +142,11 @@ public:
/** Delete waypoint waypoint with index n (last one if n < 0) */
void delete_waypoint( int n = 0 );
/**
* Calculate perpendicular distance from the current route segment
* This routine assumes all points are laying on a flat plane and
* ignores the altitude (or Z) dimension. For most accurate
* results, use with CARTESIAN way points.
* Helper, sum the distance members of each waypoint
*/
double distance_off_route( double x, double y ) const;
double total_distance() const;
};

61
simgear/route/waypoint.cxx
View File

@@ -29,23 +29,43 @@
#include "waypoint.hxx"
using std::string;
// Constructor
SGWayPoint::SGWayPoint( const double lon, const double lat, const double alt,
const modetype m, const string& s, const string& n ) {
target_lon = lon;
target_lat = lat;
target_alt = alt;
mode = m;
id = s;
name = n;
const modetype m, const string& s, const string& n ) :
mode(m),
pos(SGGeod::fromDegM(lon, lat, alt)),
distance(0.0),
id(s),
name(n)
{
}
SGWayPoint::SGWayPoint(const SGGeod& geod, const string& s, const string& n ) :
mode(WGS84),
pos(geod),
distance(0.0),
id(s),
name(n)
{
}
// Destructor
SGWayPoint::~SGWayPoint() {
}
void SGWayPoint::CourseAndDistance(const SGGeod& cur, double& course, double& dist ) const {
if ( mode == WGS84 ) {
double reverse;
SGGeodesy::inverse(cur, pos, course, reverse, dist);
} else if ( mode == SPHERICAL ) {
Point3D currentPoint(cur.getLongitudeRad(), cur.getLatitudeRad(), 0.0 );
Point3D targetPoint(pos.getLongitudeRad(), pos.getLatitudeRad(), 0.0 );
calc_gc_course_dist( currentPoint, targetPoint, &course, &dist );
course = 360.0 - course * SGD_RADIANS_TO_DEGREES;
}
}
// Calculate course and distances. For WGS84 and SPHERICAL
// coordinates lat, lon, and course are in degrees, alt and distance
@@ -55,34 +75,11 @@ void SGWayPoint::CourseAndDistance( const double cur_lon,
const double cur_lat,
const double cur_alt,
double *course, double *dist ) const {
if ( mode == WGS84 ) {
double reverse;
geo_inverse_wgs_84( cur_alt, cur_lat, cur_lon, target_lat, target_lon,
course, &reverse, dist );
} else if ( mode == SPHERICAL ) {
Point3D current( cur_lon * SGD_DEGREES_TO_RADIANS, cur_lat * SGD_DEGREES_TO_RADIANS, 0.0 );
Point3D target( target_lon * SGD_DEGREES_TO_RADIANS, target_lat * SGD_DEGREES_TO_RADIANS, 0.0 );
calc_gc_course_dist( current, target, course, dist );
*course = 360.0 - *course * SGD_RADIANS_TO_DEGREES;
} else if ( mode == CARTESIAN ) {
double dx = target_lon - cur_lon;
double dy = target_lat - cur_lat;
*course = -atan2( dy, dx ) * SGD_RADIANS_TO_DEGREES - 90;
while ( *course < 0 ) {
*course += 360.0;
}
while ( *course > 360.0 ) {
*course -= 360.0;
}
*dist = sqrt( dx * dx + dy * dy );
}
CourseAndDistance(SGGeod::fromDegM(cur_lon, cur_lat, cur_alt), *course, *dist);
}
// Calculate course and distances between two waypoints
void SGWayPoint::CourseAndDistance( const SGWayPoint &wp,
double *course, double *dist ) const {
CourseAndDistance( wp.get_target_lon(),
wp.get_target_lat(),
wp.get_target_alt(),
course, dist );
CourseAndDistance( wp.get_target(), course, dist );
}

41
simgear/route/waypoint.hxx
View File

@@ -35,9 +35,10 @@
#include <simgear/compiler.h>
#include <string>
#include <simgear/math/SGMath.hxx>
#include <simgear/math/SGGeod.hxx>
using std::string;
#include <string>
/**
@@ -57,26 +58,21 @@ public:
* the world is a perfect sphere. This is less compuntationally
* expensive than using wgs84 math and still a fairly good
* approximation of the real world, especially over shorter distances.
* <li> CARTESIAN requests all math be done assuming the coordinates specify
* position in a Z = up world.
*/
enum modetype {
WGS84 = 0,
SPHERICAL = 1,
CARTESIAN = 2
};
private:
modetype mode;
double target_lon;
double target_lat;
double target_alt;
SGGeod pos;
double distance;
string id;
string name;
std::string id;
std::string name;
public:
@@ -91,7 +87,12 @@ public:
*/
SGWayPoint( const double lon = 0.0, const double lat = 0.0,
const double alt = 0.0, const modetype m = WGS84,
const string& s = "", const string& n = "" );
const std::string& s = "", const std::string& n = "" );
/**
* Construct from a geodetic position, in WGS84 coordinates
*/
SGWayPoint(const SGGeod& pos, const std::string& s = "", const std::string& n = "" );
/** Destructor */
~SGWayPoint();
@@ -112,6 +113,9 @@ public:
const double cur_alt,
double *course, double *dist ) const;
void CourseAndDistance(const SGGeod& current,
double& course, double& dist ) const;
/**
* Calculate course and distances between a specified starting waypoint
* and this waypoint.
@@ -122,17 +126,16 @@ public:
void CourseAndDistance( const SGWayPoint &wp,
double *course, double *dist ) const;
/** @return waypoint mode */
inline modetype get_mode() const { return mode; }
/** @return waypoint longitude */
inline double get_target_lon() const { return target_lon; }
inline double get_target_lon() const { return pos.getLongitudeDeg(); }
/** @return waypoint latitude */
inline double get_target_lat() const { return target_lat; }
inline double get_target_lat() const { return pos.getLatitudeDeg(); }
/** @return waypoint altitude */
inline double get_target_alt() const { return target_alt; }
inline double get_target_alt() const { return pos.getElevationM(); }
inline const SGGeod& get_target() const { return pos; }
/**
* This value is not calculated by this class. It is simply a
@@ -153,10 +156,10 @@ public:
inline void set_distance( double d ) { distance = d; }
/** @return waypoint id */
inline const string& get_id() const { return id; }
inline const std::string& get_id() const { return id; }
/** @return waypoint name */
inline const string& get_name() const { return name; }
inline const std::string& get_name() const { return name; }
};