From: frohlich Date: Thu, 15 Jun 2006 08:27:31 +0000 (+0000) Subject: Small cleanups to the SGGeo[dc] classes, provide more hooks to use them directly X-Git-Url: https://git.mxchange.org/?a=commitdiff_plain;h=c5d677ac7b879036aa241379cbc32f868c1599bf;p=simgear.git Small cleanups to the SGGeo[dc] classes, provide more hooks to use them directly --- diff --git a/simgear/math/SGGeoc.hxx b/simgear/math/SGGeoc.hxx index a835deb1..14929dbf 100644 --- a/simgear/math/SGGeoc.hxx +++ b/simgear/math/SGGeoc.hxx @@ -29,9 +29,11 @@ public: SGGeoc(void); /// Initialize from a cartesian vector assumed to be in meters /// Note that this conversion is relatively expensive to compute + /// depricated SGGeoc(const SGVec3& cart); /// Initialize from a geodetic position /// Note that this conversion is relatively expensive to compute + /// depricated SGGeoc(const SGGeod& geod); /// Factory from angular values in radians and radius in ft @@ -42,6 +44,13 @@ public: static SGGeoc fromRadM(double lon, double lat, double radius); /// Factory from angular values in degrees and radius in m static SGGeoc fromDegM(double lon, double lat, double radius); + /// Factory to convert position from a cartesian position assumed to be + /// in wgs84 measured in meters + /// Note that this conversion is relatively expensive to compute + static SGGeoc fromCart(const SGVec3& cart); + /// Factory to convert position from a geodetic position + /// Note that this conversion is relatively expensive to compute + static SGGeoc fromGeod(const SGGeod& geod); /// Return the geocentric longitude in radians double getLongitudeRad(void) const; @@ -165,6 +174,26 @@ SGGeoc::fromDegM(double lon, double lat, double radius) #endif } +inline +SGGeoc +SGGeoc::fromCart(const SGVec3& cart) +{ + SGGeoc geoc; + SGGeodesy::SGCartToGeoc(cart, geoc); + return geoc; +} + +inline +SGGeoc +SGGeoc::fromGeod(const SGGeod& geod) +{ + SGVec3 cart; + SGGeodesy::SGGeodToCart(geod, cart); + SGGeoc geoc; + SGGeodesy::SGCartToGeoc(cart, geoc); + return geoc; +} + inline double SGGeoc::getLongitudeRad(void) const diff --git a/simgear/math/SGGeod.hxx b/simgear/math/SGGeod.hxx index 3fb6659b..7124fdd1 100644 --- a/simgear/math/SGGeod.hxx +++ b/simgear/math/SGGeod.hxx @@ -29,9 +29,11 @@ public: SGGeod(void); /// Initialize from a cartesian vector assumed to be in meters /// Note that this conversion is relatively expensive to compute + /// depricated SGGeod(const SGVec3& cart); /// Initialize from a geocentric position /// Note that this conversion is relatively expensive to compute + /// depricated SGGeod(const SGGeoc& geoc); /// Factory from angular values in radians and elevation is 0 @@ -46,6 +48,13 @@ public: static SGGeod fromRadM(double lon, double lat, double elevation); /// Factory from angular values in degrees and elevation in m static SGGeod fromDegM(double lon, double lat, double elevation); + /// Factory to convert position from a cartesian position assumed to be + /// in wgs84 measured in meters + /// Note that this conversion is relatively expensive to compute + static SGGeod fromCart(const SGVec3& cart); + /// Factory to convert position from a geocentric position + /// Note that this conversion is relatively expensive to compute + static SGGeod fromGeoc(const SGGeoc& geoc); /// Return the geodetic longitude in radians double getLongitudeRad(void) const; @@ -191,6 +200,26 @@ SGGeod::fromDegM(double lon, double lat, double elevation) #endif } +inline +SGGeod +SGGeod::fromCart(const SGVec3& cart) +{ + SGGeod geod; + SGGeodesy::SGCartToGeod(cart, geod); + return geod; +} + +inline +SGGeod +SGGeod::fromGeoc(const SGGeoc& geoc) +{ + SGVec3 cart; + SGGeodesy::SGGeocToCart(geoc, cart); + SGGeod geod; + SGGeodesy::SGCartToGeod(cart, geod); + return geod; +} + inline double SGGeod::getLongitudeRad(void) const diff --git a/simgear/math/SGMath.hxx b/simgear/math/SGMath.hxx index 024fe4c1..73ec59a0 100644 --- a/simgear/math/SGMath.hxx +++ b/simgear/math/SGMath.hxx @@ -27,9 +27,9 @@ #include "SGGeodesy.hxx" #include "SGVec3.hxx" #include "SGVec4.hxx" -#include "SGQuat.hxx" -#include "SGMatrix.hxx" #include "SGGeoc.hxx" #include "SGGeod.hxx" +#include "SGQuat.hxx" +#include "SGMatrix.hxx" #endif diff --git a/simgear/math/SGMathTest.cxx b/simgear/math/SGMathTest.cxx index cd193177..3f0d5cc6 100644 --- a/simgear/math/SGMathTest.cxx +++ b/simgear/math/SGMathTest.cxx @@ -213,16 +213,16 @@ GeodesyTest(void) geod0 = SGGeod::fromDegM(30, 20, 17); // Test the conversion routines to cartesian coordinates - cart0 = geod0; - geod1 = cart0; + cart0 = SGVec3::fromGeod(geod0); + geod1 = SGGeod::fromCart(cart0); if (epsDeg < fabs(geod0.getLongitudeDeg() - geod1.getLongitudeDeg()) || epsDeg < fabs(geod0.getLatitudeDeg() - geod1.getLatitudeDeg()) || epsM < fabs(geod0.getElevationM() - geod1.getElevationM())) return false; // Test the conversion routines to radial coordinates - geoc0 = cart0; - cart1 = geoc0; + geoc0 = SGGeoc::fromCart(cart0); + cart1 = SGVec3::fromGeoc(geoc0); if (!equivalent(cart0, cart1)) return false; @@ -346,8 +346,8 @@ main(void) return EXIT_FAILURE; // Check geodetic/geocentric/cartesian conversions -// if (!GeodesyTest()) -// return EXIT_FAILURE; + if (!GeodesyTest()) + return EXIT_FAILURE; // Check interaction with sg*/sgd* if (!sgInterfaceTest()) diff --git a/simgear/math/SGQuat.hxx b/simgear/math/SGQuat.hxx index 36a140d6..0a4aee06 100644 --- a/simgear/math/SGQuat.hxx +++ b/simgear/math/SGQuat.hxx @@ -110,6 +110,11 @@ public: static SGQuat fromLonLatDeg(T lon, T lat) { return fromLonLatRad(SGMisc::deg2rad(lon), SGMisc::deg2rad(lat)); } + /// Return a quaternion rotation the the horizontal local frame from given + /// longitude and latitude + static SGQuat fromLonLat(const SGGeod& geod) + { return fromLonLatRad(geod.getLongitudeRad(), geod.getLatitudeRad()); } + /// Create a quaternion from the angle axis representation static SGQuat fromAngleAxis(T angle, const SGVec3& axis) { diff --git a/simgear/math/SGVec3.hxx b/simgear/math/SGVec3.hxx index 4bd1cc38..1d2a1627 100644 --- a/simgear/math/SGVec3.hxx +++ b/simgear/math/SGVec3.hxx @@ -45,10 +45,12 @@ public: { _data[0] = data[0]; _data[1] = data[1]; _data[2] = data[2]; } /// Constructor. Initialize by a geodetic coordinate /// Note that this conversion is relatively expensive to compute + /// depricated SGVec3(const SGGeod& geod) { SGGeodesy::SGGeodToCart(geod, *this); } /// Constructor. Initialize by a geocentric coordinate /// Note that this conversion is relatively expensive to compute + /// depricated SGVec3(const SGGeoc& geoc) { SGGeodesy::SGGeocToCart(geoc, *this); } @@ -125,11 +127,58 @@ public: static SGVec3 e3(void) { return SGVec3(0, 0, 1); } + /// Constructor. Initialize by a geodetic coordinate + /// Note that this conversion is relatively expensive to compute + static SGVec3 fromGeod(const SGGeod& geod); + /// Constructor. Initialize by a geocentric coordinate + /// Note that this conversion is relatively expensive to compute + static SGVec3 fromGeoc(const SGGeoc& geoc); + private: /// The actual data T _data[3]; }; +template<> +inline +SGVec3 +SGVec3::fromGeod(const SGGeod& geod) +{ + SGVec3 cart; + SGGeodesy::SGGeodToCart(geod, cart); + return cart; +} + +template<> +inline +SGVec3 +SGVec3::fromGeod(const SGGeod& geod) +{ + SGVec3 cart; + SGGeodesy::SGGeodToCart(geod, cart); + return SGVec3(cart(0), cart(1), cart(2)); +} + +template<> +inline +SGVec3 +SGVec3::fromGeoc(const SGGeoc& geoc) +{ + SGVec3 cart; + SGGeodesy::SGGeocToCart(geoc, cart); + return cart; +} + +template<> +inline +SGVec3 +SGVec3::fromGeoc(const SGGeoc& geoc) +{ + SGVec3 cart; + SGGeodesy::SGGeocToCart(geoc, cart); + return SGVec3(cart(0), cart(1), cart(2)); +} + /// Unary +, do nothing ... template inline diff --git a/simgear/math/sg_geodesy.cxx b/simgear/math/sg_geodesy.cxx index 4de126d5..adba76bc 100644 --- a/simgear/math/sg_geodesy.cxx +++ b/simgear/math/sg_geodesy.cxx @@ -73,10 +73,9 @@ static inline double M0( double e2 ) { } -// given, alt, lat1, lon1, az1 and distance (s), calculate lat2, lon2 +// given, lat1, lon1, az1 and distance (s), calculate lat2, lon2 // and az2. Lat, lon, and azimuth are in degrees. distance in meters -int geo_direct_wgs_84 ( double alt, double lat1, - double lon1, double az1, +int geo_direct_wgs_84 ( double lat1, double lon1, double az1, double s, double *lat2, double *lon2, double *az2 ) { @@ -160,16 +159,15 @@ int geo_direct_wgs_84 ( double alt, double lat1, double dM = a*M0(e2) - s; double paz = ( phi1 < 0.0 ? 180.0 : 0.0 ); double zero = 0.0f; - return geo_direct_wgs_84( alt, zero, lon1, paz, dM, lat2, lon2, az2 ); + return geo_direct_wgs_84( zero, lon1, paz, dM, lat2, lon2, az2 ); } } -// given alt, lat1, lon1, lat2, lon2, calculate starting and ending +// given lat1, lon1, lat2, lon2, calculate starting and ending // az1, az2 and distance (s). Lat, lon, and azimuth are in degrees. // distance in meters -int geo_inverse_wgs_84( double alt, double lat1, - double lon1, double lat2, +int geo_inverse_wgs_84( double lat1, double lon1, double lat2, double lon2, double *az1, double *az2, double *s ) { @@ -192,14 +190,14 @@ int geo_inverse_wgs_84( double alt, double lat1, return 0; } else if( fabs(cosphi1) < testv ) { // initial point is polar - int k = geo_inverse_wgs_84( alt, lat2,lon2,lat1,lon1, az1,az2,s ); + int k = geo_inverse_wgs_84( lat2,lon2,lat1,lon1, az1,az2,s ); k = k; // avoid compiler error since return result is unused b = *az1; *az1 = *az2; *az2 = b; return 0; } else if( fabs(cosphi2) < testv ) { // terminal point is polar double _lon1 = lon1 + 180.0f; - int k = geo_inverse_wgs_84( alt, lat1, lon1, lat1, _lon1, + int k = geo_inverse_wgs_84( lat1, lon1, lat1, _lon1, az1, az2, s ); k = k; // avoid compiler error since return result is unused *s /= 2.0; @@ -211,8 +209,8 @@ int geo_inverse_wgs_84( double alt, double lat1, { // Geodesic passes through the pole (antipodal) double s1,s2; - geo_inverse_wgs_84( alt, lat1,lon1, lat1,lon2, az1,az2, &s1 ); - geo_inverse_wgs_84( alt, lat2,lon2, lat1,lon2, az1,az2, &s2 ); + geo_inverse_wgs_84( lat1,lon1, lat1,lon2, az1,az2, &s1 ); + geo_inverse_wgs_84( lat2,lon2, lat1,lon2, az1,az2, &s2 ); *az2 = *az1; *s = s1 + s2; return 0; diff --git a/simgear/math/sg_geodesy.hxx b/simgear/math/sg_geodesy.hxx index c2f9b291..32df3008 100644 --- a/simgear/math/sg_geodesy.hxx +++ b/simgear/math/sg_geodesy.hxx @@ -4,18 +4,6 @@ #include #include "SGMath.hxx" -// Returns the insersection of the line joining the center of the -// earth and the specified cylindrical point with the surface of the -// WGS84 ellipsoid. Works by finding a normalization constant (in -// squashed space) that places the squashed point on the surface of -// the sphere. -inline double seaLevelRadius(double r, double z) -{ - double sr = r * SGGeodesy::SQUASH; - double zz = z*z; - return SGGeodesy::POLRAD*sqrt((r*r + zz)/(sr*sr + zz)); -} - /** * Convert from geocentric coordinates to geodetic coordinates * @param lat_geoc (in) Geocentric latitude, radians, + = North @@ -56,11 +44,12 @@ inline void sgGeodToGeoc(double lat_geod, double alt, double *sl_radius, double *lat_geoc) { SGVec3 cart; - SGGeodesy::SGGeodToCart(SGGeod::fromRadM(0, lat_geod, alt), cart); + SGGeod geod = SGGeod::fromRadM(0, lat_geod, alt); + SGGeodesy::SGGeodToCart(geod, cart); SGGeoc geoc; SGGeodesy::SGCartToGeoc(cart, geoc); *lat_geoc = geoc.getLatitudeRad(); - *sl_radius = seaLevelRadius(cart(0), cart(2)); + *sl_radius = SGGeodesy::SGGeodToSeaLevelRadius(geod); } @@ -130,7 +119,7 @@ inline Point3D sgGeodToCart(const Point3D& geod) /** * Given a starting position and an offset radial and distance, * calculate an ending positon on a wgs84 ellipsoid. - * @param alt (in) meters + * @param alt (in) meters (unused) * @param lat1 (in) degrees * @param lon1 (in) degrees * @param az1 (in) degrees @@ -139,16 +128,39 @@ inline Point3D sgGeodToCart(const Point3D& geod) * @param lon2 (out) degrees * @param az2 (out) return course in degrees */ -int geo_direct_wgs_84 ( double alt, double lat1, - double lon1, double az1, +int geo_direct_wgs_84 ( double lat1, double lon1, double az1, double s, double *lat2, double *lon2, double *az2 ); +inline int geo_direct_wgs_84 ( double alt, double lat1, + double lon1, double az1, + double s, double *lat2, double *lon2, + double *az2 ) +{ return geo_direct_wgs_84(lat1, lon1, az1, s, lat2, lon2, az2); } +/** + * Given a starting position and an offset radial and distance, + * calculate an ending positon on a wgs84 ellipsoid. + * @param p1 (in) geodetic position + * @param az1 (in) degrees + * @param s (in) distance in meters + * @param p2 (out) geodetic position + * @param az2 (out) return course in degrees + */ +inline int geo_direct_wgs_84(const SGGeod& p1, double az1, + double s, SGGeod& p2, double *az2 ) +{ + double lat2, lon2; + int ret = geo_direct_wgs_84(p1.getLatitudeDeg(), p1.getLongitudeDeg(), + az1, s, &lat2, &lon2, az2); + p2.setLatitudeDeg(lat2); + p2.setLongitudeDeg(lon2); + return ret; +} /** * Given an altitude and two sets of (lat, lon) calculate great circle * distance between them as well as the starting and ending azimuths. - * @param alt (in) meters + * @param alt (in) meters (unused) * @param lat1 (in) degrees * @param lon1 (in) degrees * @param lat2 (in) degrees @@ -157,9 +169,31 @@ int geo_direct_wgs_84 ( double alt, double lat1, * @param az2 (out) end heading degrees * @param s (out) distance meters */ -int geo_inverse_wgs_84( double alt, double lat1, - double lon1, double lat2, +int geo_inverse_wgs_84( double lat1, double lon1, double lat2, double lon2, double *az1, double *az2, double *s ); +inline int geo_inverse_wgs_84( double alt, double lat1, + double lon1, double lat2, + double lon2, double *az1, double *az2, + double *s ) +{ return geo_inverse_wgs_84(lat1, lon1, lat2, lon2, az1, az2, s); } + + +/** + * Given an altitude and two sets of (lat, lon) calculate great circle + * distance between them as well as the starting and ending azimuths. + * @param p1 (in) first position + * @param p2 (in) fsecond position + * @param az1 (out) start heading degrees + * @param az2 (out) end heading degrees + * @param s (out) distance meters + */ +inline int geo_inverse_wgs_84(const SGGeod& p1, const SGGeod& p2, + double *az1, double *az2, double *s ) +{ + return geo_inverse_wgs_84(p1.getLatitudeDeg(), p1.getLongitudeDeg(), + p2.getLatitudeDeg(), p2.getLongitudeDeg(), + az1, az2, s); +} #endif // _SG_GEODESY_HXX diff --git a/simgear/scene/model/placement.cxx b/simgear/scene/model/placement.cxx index f5c483fc..32cfbe2c 100644 --- a/simgear/scene/model/placement.cxx +++ b/simgear/scene/model/placement.cxx @@ -102,6 +102,14 @@ SGModelPlacement::setPosition (double lon_deg, double lat_deg, double elev_ft) _elev_ft = elev_ft; } +void +SGModelPlacement::setPosition(const SGGeod& position) +{ + _lon_deg = position.getLongitudeDeg(); + _lat_deg = position.getLatitudeDeg(); + _elev_ft = position.getElevationFt(); +} + void SGModelPlacement::setRollDeg (double roll_deg) { @@ -129,4 +137,10 @@ SGModelPlacement::setOrientation (double roll_deg, double pitch_deg, _heading_deg = heading_deg; } +void +SGModelPlacement::setOrientation (const SGQuatd& orientation) +{ + orientation.getEulerDeg(_heading_deg, _pitch_deg, _roll_deg); +} + // end of model.cxx diff --git a/simgear/scene/model/placement.hxx b/simgear/scene/model/placement.hxx index 1b6654fe..962139ea 100644 --- a/simgear/scene/model/placement.hxx +++ b/simgear/scene/model/placement.hxx @@ -66,6 +66,7 @@ public: virtual void setLatitudeDeg (double lat_deg); virtual void setElevationFt (double elev_ft); virtual void setPosition (double lon_deg, double lat_deg, double elev_ft); + void setPosition(const SGGeod& position); virtual double getRollDeg () const { return _roll_deg; } virtual double getPitchDeg () const { return _pitch_deg; } @@ -76,6 +77,7 @@ public: virtual void setHeadingDeg (double heading_deg); virtual void setOrientation (double roll_deg, double pitch_deg, double heading_deg); + void setOrientation(const SGQuatd& orientation); ssgPlacementTransform * getTransform(void) { return _position; } diff --git a/simgear/timing/geocoord.cxx b/simgear/timing/geocoord.cxx index 56b7f1a2..9cb9f38b 100644 --- a/simgear/timing/geocoord.cxx +++ b/simgear/timing/geocoord.cxx @@ -27,8 +27,8 @@ * written for FlightGear, in order to store Timezone control points. * ************************************************************************/ +#include #include "geocoord.h" -#include SGGeoCoord::SGGeoCoord(const SGGeoCoord& other) { @@ -36,57 +36,20 @@ SGGeoCoord::SGGeoCoord(const SGGeoCoord& other) lon = other.lon; } -// double SGGeoCoord::getAngle(const SGGeoCoord& other) const -// { -// Vector first( getX(), getY(), getZ()); -// Vector secnd(other.getX(), other.getY(), other.getZ()); -// double -// dot = VecDot(first, secnd), -// len1 = first.VecLen(), -// len2 = secnd.VecLen(), -// len = len1 * len2, -// angle = 0; -// //printf ("Dot: %f, len1: %f len2: %f\n", dot, len1, len2); -// /*Vector pPos = prevPos - Reference->prevPos; -// Vector pVel = prevVel - Reference->prevVel;*/ - - -// if ( ( (dot / len) < 1) && (dot / len > -1) && len ) -// angle = acos(dot / len); -// return angle; -// } - -// SGGeoCoord* SGGeoCoordContainer::getNearest(const SGGeoCoord& ref) const -// { -// float angle, maxAngle = 180; - -// SGGeoCoordVectorConstIterator i, nearest; -// for (i = data.begin(); i != data.end(); i++) -// { -// angle = SGD_RADIANS_TO_DEGREES * (*i)->getAngle(ref); -// if (angle < maxAngle) -// { -// maxAngle = angle; -// nearest = i; -// } -// } -// return *nearest; -// } - - SGGeoCoord* SGGeoCoordContainer::getNearest(const SGGeoCoord& ref) const { - sgVec3 first, secnd; - float dist, maxDist=SG_MAX; - sgSetVec3( first, ref.getX(), ref.getY(), ref.getZ()); + if (data.empty()) + return 0; + + float maxCosAng = -2; + SGVec3f refVec(ref.getX(), ref.getY(), ref.getZ()); SGGeoCoordVectorConstIterator i, nearest; - for (i = data.begin(); i != data.end(); i++) + for (i = data.begin(); i != data.end(); ++i) { - sgSetVec3(secnd, (*i)->getX(), (*i)->getY(), (*i)->getZ()); - dist = sgDistanceSquaredVec3(first, secnd); - if (dist < maxDist) + float cosAng = dot(refVec, SGVec3f((*i)->getX(), (*i)->getY(), (*i)->getZ())); + if (maxCosAng < cosAng) { - maxDist = dist; + maxCosAng = cosAng; nearest = i; } } diff --git a/simgear/timing/geocoord.h b/simgear/timing/geocoord.h index dbea68c2..956e1189 100644 --- a/simgear/timing/geocoord.h +++ b/simgear/timing/geocoord.h @@ -36,10 +36,6 @@ #include -#include -#include STL_IOSTREAM -//#include // looks like streambuf does not exist on linux. -// But it looks like it isn't used anyways -:) #include SG_USING_NAMESPACE(std); @@ -66,8 +62,6 @@ public: float getZ() const { return sin(SGD_DEGREES_TO_RADIANS*lat); }; - //double getAngle(const SGGeoCoord& other) const; - virtual void print() {} ; virtual const char * getDescription() {return 0;}; }; diff --git a/simgear/timing/timezone.h b/simgear/timing/timezone.h index 2efdafff..aa282b54 100644 --- a/simgear/timing/timezone.h +++ b/simgear/timing/timezone.h @@ -28,6 +28,7 @@ #define _TIMEZONE_H_ #include +#include #include @@ -84,11 +85,6 @@ public: */ virtual ~SGTimeZone() { }; - /** - * Print the descriptor string - */ - virtual void print() { printf("%s", descriptor.c_str()); } - /** * Return the descriptor string * @return descriptor string (char array)