diff --git a/simgear/math/Makefile.am b/simgear/math/Makefile.am index 36747b2d..cd06c737 100644 --- a/simgear/math/Makefile.am +++ b/simgear/math/Makefile.am @@ -1,11 +1,18 @@ includedir = @includedir@/math +noinst_PROGRAMS = SGMathTest +check_PROGRAMS = SGMathTest +TESTS = $(check_PROGRAMS) + +SGMathTest_SOURCES = SGMathTest.cxx +SGMathTest_LDADD = libsgmath.a $(base_LIBS) + + lib_LIBRARIES = libsgmath.a include_HEADERS = \ interpolater.hxx \ leastsqs.hxx \ - localconsts.hxx \ point3d.hxx \ polar3d.hxx \ sg_geodesy.hxx \ @@ -13,7 +20,18 @@ include_HEADERS = \ sg_random.h \ sg_types.hxx \ vector.hxx \ - fastmath.hxx + fastmath.hxx \ + SGGeoc.hxx \ + SGGeod.hxx \ + SGGeodesy.hxx \ + SGLimits.hxx \ + SGMatrix.hxx \ + SGMath.hxx \ + SGMisc.hxx \ + SGQuat.hxx \ + SGVec4.hxx \ + SGVec3.hxx + EXTRA_DIST = linintp2.h linintp2.inl sphrintp.h sphrintp.inl @@ -24,6 +42,7 @@ libsgmath_a_SOURCES = \ sg_geodesy.cxx \ sg_random.c \ vector.cxx \ - fastmath.cxx + fastmath.cxx \ + SGGeodesy.cxx INCLUDES = -I$(top_srcdir) diff --git a/simgear/math/SGGeoc.hxx b/simgear/math/SGGeoc.hxx new file mode 100644 index 00000000..b7d0a191 --- /dev/null +++ b/simgear/math/SGGeoc.hxx @@ -0,0 +1,278 @@ +#ifndef SGGeoc_H +#define SGGeoc_H + +#include + +// #define SG_GEOC_NATIVE_DEGREE + +/// Class representing a geocentric location +class SGGeoc { +public: + /// Default constructor, initializes the instance to lat = lon = lat = 0 + SGGeoc(void); + /// Initialize from a cartesian vector assumed to be in meters + /// Note that this conversion is relatively expensive to compute + SGGeoc(const SGVec3& cart); + /// Initialize from a geodetic position + /// Note that this conversion is relatively expensive to compute + SGGeoc(const SGGeod& geod); + + /// Factory from angular values in radians and radius in ft + static SGGeoc fromRadFt(double lon, double lat, double radius); + /// Factory from angular values in degrees and radius in ft + static SGGeoc fromDegFt(double lon, double lat, double radius); + /// Factory from angular values in radians and radius in m + 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); + + /// Return the geocentric longitude in radians + double getLongitudeRad(void) const; + /// Set the geocentric longitude from the argument given in radians + void setLongitudeRad(double lon); + + /// Return the geocentric longitude in degrees + double getLongitudeDeg(void) const; + /// Set the geocentric longitude from the argument given in degrees + void setLongitudeDeg(double lon); + + /// Return the geocentric latitude in radians + double getLatitudeRad(void) const; + /// Set the geocentric latitude from the argument given in radians + void setLatitudeRad(double lat); + + /// Return the geocentric latitude in degrees + double getLatitudeDeg(void) const; + /// Set the geocentric latitude from the argument given in degrees + void setLatitudeDeg(double lat); + + /// Return the geocentric radius in meters + double getRadiusM(void) const; + /// Set the geocentric radius from the argument given in meters + void setRadiusM(double radius); + + /// Return the geocentric radius in feet + double getRadiusFt(void) const; + /// Set the geocentric radius from the argument given in feet + void setRadiusFt(double radius); + +private: + /// This one is private since construction is not unique if you do + /// not know the units of the arguments, use the factory methods for + /// that purpose + SGGeoc(double lon, double lat, double radius); + + /// The actual data, angles in degree, radius in meters + /// The rationale for storing the values in degrees is that most code places + /// in flightgear/terragear use degrees as a nativ input and output value. + /// The places where it makes sense to use radians is when we convert + /// to other representations or compute rotation matrices. But both tasks + /// are computionally intensive anyway and that additional 'toRadian' + /// conversion does not hurt too much + double _lon; + double _lat; + double _radius; +}; + +inline +SGGeoc::SGGeoc(void) : + _lon(0), _lat(0), _radius(0) +{ +} + +inline +SGGeoc::SGGeoc(double lon, double lat, double radius) : + _lon(lon), _lat(lat), _radius(radius) +{ +} + +inline +SGGeoc::SGGeoc(const SGVec3& cart) +{ + SGGeodesy::SGCartToGeoc(cart, *this); +} + +inline +SGGeoc::SGGeoc(const SGGeod& geod) +{ + SGVec3 cart; + SGGeodesy::SGGeodToCart(geod, cart); + SGGeodesy::SGCartToGeoc(cart, *this); +} + +inline +SGGeoc +SGGeoc::fromRadFt(double lon, double lat, double radius) +{ +#ifdef SG_GEOC_NATIVE_DEGREE + return SGGeoc(lon*SGD_RADIANS_TO_DEGREES, lat*SGD_RADIANS_TO_DEGREES, + radius*SG_FEET_TO_METER); +#else + return SGGeoc(lon, lat, radius*SG_FEET_TO_METER); +#endif +} + +inline +SGGeoc +SGGeoc::fromDegFt(double lon, double lat, double radius) +{ +#ifdef SG_GEOC_NATIVE_DEGREE + return SGGeoc(lon, lat, radius*SG_FEET_TO_METER); +#else + return SGGeoc(lon*SGD_DEGREES_TO_RADIANS, lat*SGD_DEGREES_TO_RADIANS, + radius*SG_FEET_TO_METER); +#endif +} + +inline +SGGeoc +SGGeoc::fromRadM(double lon, double lat, double radius) +{ +#ifdef SG_GEOC_NATIVE_DEGREE + return SGGeoc(lon*SGD_RADIANS_TO_DEGREES, lat*SGD_RADIANS_TO_DEGREES, + radius); +#else + return SGGeoc(lon, lat, radius); +#endif +} + +inline +SGGeoc +SGGeoc::fromDegM(double lon, double lat, double radius) +{ +#ifdef SG_GEOC_NATIVE_DEGREE + return SGGeoc(lon, lat, radius); +#else + return SGGeoc(lon*SGD_DEGREES_TO_RADIANS, lat*SGD_DEGREES_TO_RADIANS, + radius); +#endif +} + +inline +double +SGGeoc::getLongitudeRad(void) const +{ +#ifdef SG_GEOC_NATIVE_DEGREE + return _lon*SGD_DEGREES_TO_RADIANS; +#else + return _lon; +#endif +} + +inline +void +SGGeoc::setLongitudeRad(double lon) +{ +#ifdef SG_GEOC_NATIVE_DEGREE + _lon = lon*SGD_RADIANS_TO_DEGREES; +#else + _lon = lon; +#endif +} + +inline +double +SGGeoc::getLongitudeDeg(void) const +{ +#ifdef SG_GEOC_NATIVE_DEGREE + return _lon; +#else + return _lon*SGD_DEGREES_TO_RADIANS; +#endif +} + +inline +void +SGGeoc::setLongitudeDeg(double lon) +{ +#ifdef SG_GEOC_NATIVE_DEGREE + _lon = lon; +#else + _lon = lon*SGD_RADIANS_TO_DEGREES; +#endif +} + +inline +double +SGGeoc::getLatitudeRad(void) const +{ +#ifdef SG_GEOC_NATIVE_DEGREE + return _lat*SGD_DEGREES_TO_RADIANS; +#else + return _lat; +#endif +} + +inline +void +SGGeoc::setLatitudeRad(double lat) +{ +#ifdef SG_GEOC_NATIVE_DEGREE + _lat = lat*SGD_RADIANS_TO_DEGREES; +#else + _lat = lat; +#endif +} + +inline +double +SGGeoc::getLatitudeDeg(void) const +{ +#ifdef SG_GEOC_NATIVE_DEGREE + return _lat; +#else + return _lat*SGD_DEGREES_TO_RADIANS; +#endif +} + +inline +void +SGGeoc::setLatitudeDeg(double lat) +{ +#ifdef SG_GEOC_NATIVE_DEGREE + _lat = lat; +#else + _lat = lat*SGD_RADIANS_TO_DEGREES; +#endif +} + +inline +double +SGGeoc::getRadiusM(void) const +{ + return _radius; +} + +inline +void +SGGeoc::setRadiusM(double radius) +{ + _radius = radius; +} + +inline +double +SGGeoc::getRadiusFt(void) const +{ + return _radius*SG_METER_TO_FEET; +} + +inline +void +SGGeoc::setRadiusFt(double radius) +{ + _radius = radius*SG_FEET_TO_METER; +} + +/// Output to an ostream +template +inline +std::basic_ostream& +operator<<(std::basic_ostream& s, const SGGeoc& g) +{ + return s << "lon = " << g.getLongitudeDeg() + << ", lat = " << g.getLatitudeDeg() + << ", radius = " << g.getRadiusM(); +} + +#endif diff --git a/simgear/math/SGGeod.hxx b/simgear/math/SGGeod.hxx new file mode 100644 index 00000000..e89cd4da --- /dev/null +++ b/simgear/math/SGGeod.hxx @@ -0,0 +1,305 @@ +#ifndef SGGeod_H +#define SGGeod_H + +#include + +// #define SG_GEOD_NATIVE_DEGREE + +/// Class representing a geodetic location +class SGGeod { +public: + /// Default constructor, initializes the instance to lat = lon = elev = 0 + SGGeod(void); + /// Initialize from a cartesian vector assumed to be in meters + /// Note that this conversion is relatively expensive to compute + SGGeod(const SGVec3& cart); + /// Initialize from a geocentric position + /// Note that this conversion is relatively expensive to compute + SGGeod(const SGGeoc& geoc); + + /// Factory from angular values in radians and elevation is 0 + static SGGeod fromRad(double lon, double lat); + /// Factory from angular values in degrees and elevation is 0 + static SGGeod fromDeg(double lon, double lat); + /// Factory from angular values in radians and elevation in ft + static SGGeod fromRadFt(double lon, double lat, double elevation); + /// Factory from angular values in degrees and elevation in ft + static SGGeod fromDegFt(double lon, double lat, double elevation); + /// Factory from angular values in radians and elevation in m + 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); + + /// Return the geodetic longitude in radians + double getLongitudeRad(void) const; + /// Set the geodetic longitude from the argument given in radians + void setLongitudeRad(double lon); + + /// Return the geodetic longitude in degrees + double getLongitudeDeg(void) const; + /// Set the geodetic longitude from the argument given in degrees + void setLongitudeDeg(double lon); + + /// Return the geodetic latitude in radians + double getLatitudeRad(void) const; + /// Set the geodetic latitude from the argument given in radians + void setLatitudeRad(double lat); + + /// Return the geodetic latitude in degrees + double getLatitudeDeg(void) const; + /// Set the geodetic latitude from the argument given in degrees + void setLatitudeDeg(double lat); + + /// Return the geodetic elevation in meters + double getElevationM(void) const; + /// Set the geodetic elevation from the argument given in meters + void setElevationM(double elevation); + + /// Return the geodetic elevation in feet + double getElevationFt(void) const; + /// Set the geodetic elevation from the argument given in feet + void setElevationFt(double elevation); + +private: + /// This one is private since construction is not unique if you do + /// not know the units of the arguments. Use the factory methods for + /// that purpose + SGGeod(double lon, double lat, double elevation); + + /// The actual data, angles in degree, elevation in meters + /// The rationale for storing the values in degrees is that most code places + /// in flightgear/terragear use degrees as a nativ input and output value. + /// The places where it makes sense to use radians is when we convert + /// to other representations or compute rotation matrices. But both tasks + /// are computionally intensive anyway and that additional 'toRadian' + /// conversion does not hurt too much + double _lon; + double _lat; + double _elevation; +}; + +inline +SGGeod::SGGeod(void) : + _lon(0), _lat(0), _elevation(0) +{ +} + +inline +SGGeod::SGGeod(double lon, double lat, double elevation) : + _lon(lon), _lat(lat), _elevation(elevation) +{ +} + +inline +SGGeod::SGGeod(const SGVec3& cart) +{ + SGGeodesy::SGCartToGeod(cart, *this); +} + +inline +SGGeod::SGGeod(const SGGeoc& geoc) +{ + SGVec3 cart; + SGGeodesy::SGGeocToCart(geoc, cart); + SGGeodesy::SGCartToGeod(cart, *this); +} + +inline +SGGeod +SGGeod::fromRad(double lon, double lat) +{ +#ifdef SG_GEOD_NATIVE_DEGREE + return SGGeod(lon*SGD_RADIANS_TO_DEGREES, lat*SGD_RADIANS_TO_DEGREES, 0); +#else + return SGGeod(lon, lat, 0); +#endif +} + +inline +SGGeod +SGGeod::fromDeg(double lon, double lat) +{ +#ifdef SG_GEOD_NATIVE_DEGREE + return SGGeod(lon, lat, 0); +#else + return SGGeod(lon*SGD_DEGREES_TO_RADIANS, lat*SGD_DEGREES_TO_RADIANS, 0); +#endif +} + +inline +SGGeod +SGGeod::fromRadFt(double lon, double lat, double elevation) +{ +#ifdef SG_GEOD_NATIVE_DEGREE + return SGGeod(lon*SGD_RADIANS_TO_DEGREES, lat*SGD_RADIANS_TO_DEGREES, + elevation*SG_FEET_TO_METER); +#else + return SGGeod(lon, lat, elevation*SG_FEET_TO_METER); +#endif +} + +inline +SGGeod +SGGeod::fromDegFt(double lon, double lat, double elevation) +{ +#ifdef SG_GEOD_NATIVE_DEGREE + return SGGeod(lon, lat, elevation*SG_FEET_TO_METER); +#else + return SGGeod(lon*SGD_DEGREES_TO_RADIANS, lat*SGD_DEGREES_TO_RADIANS, + elevation*SG_FEET_TO_METER); +#endif +} + +inline +SGGeod +SGGeod::fromRadM(double lon, double lat, double elevation) +{ +#ifdef SG_GEOD_NATIVE_DEGREE + return SGGeod(lon*SGD_RADIANS_TO_DEGREES, lat*SGD_RADIANS_TO_DEGREES, + elevation); +#else + return SGGeod(lon, lat, elevation); +#endif +} + +inline +SGGeod +SGGeod::fromDegM(double lon, double lat, double elevation) +{ +#ifdef SG_GEOD_NATIVE_DEGREE + return SGGeod(lon, lat, elevation); +#else + return SGGeod(lon*SGD_DEGREES_TO_RADIANS, lat*SGD_DEGREES_TO_RADIANS, + elevation); +#endif +} + +inline +double +SGGeod::getLongitudeRad(void) const +{ +#ifdef SG_GEOD_NATIVE_DEGREE + return _lon*SGD_DEGREES_TO_RADIANS; +#else + return _lon; +#endif +} + +inline +void +SGGeod::setLongitudeRad(double lon) +{ +#ifdef SG_GEOD_NATIVE_DEGREE + _lon = lon*SGD_RADIANS_TO_DEGREES; +#else + _lon = lon; +#endif +} + +inline +double +SGGeod::getLongitudeDeg(void) const +{ +#ifdef SG_GEOD_NATIVE_DEGREE + return _lon; +#else + return _lon*SGD_RADIANS_TO_DEGREES; +#endif +} + +inline +void +SGGeod::setLongitudeDeg(double lon) +{ +#ifdef SG_GEOD_NATIVE_DEGREE + _lon = lon; +#else + _lon = lon*SGD_DEGREES_TO_RADIANS; +#endif +} + +inline +double +SGGeod::getLatitudeRad(void) const +{ +#ifdef SG_GEOD_NATIVE_DEGREE + return _lat*SGD_DEGREES_TO_RADIANS; +#else + return _lat; +#endif +} + +inline +void +SGGeod::setLatitudeRad(double lat) +{ +#ifdef SG_GEOD_NATIVE_DEGREE + _lat = lat*SGD_RADIANS_TO_DEGREES; +#else + _lat = lat; +#endif +} + +inline +double +SGGeod::getLatitudeDeg(void) const +{ +#ifdef SG_GEOD_NATIVE_DEGREE + return _lat; +#else + return _lat*SGD_RADIANS_TO_DEGREES; +#endif +} + +inline +void +SGGeod::setLatitudeDeg(double lat) +{ +#ifdef SG_GEOD_NATIVE_DEGREE + _lat = lat; +#else + _lat = lat*SGD_DEGREES_TO_RADIANS; +#endif +} + +inline +double +SGGeod::getElevationM(void) const +{ + return _elevation; +} + +inline +void +SGGeod::setElevationM(double elevation) +{ + _elevation = elevation; +} + +inline +double +SGGeod::getElevationFt(void) const +{ + return _elevation*SG_METER_TO_FEET; +} + +inline +void +SGGeod::setElevationFt(double elevation) +{ + _elevation = elevation*SG_FEET_TO_METER; +} + +/// Output to an ostream +template +inline +std::basic_ostream& +operator<<(std::basic_ostream& s, const SGGeod& g) +{ + return s << "lon = " << g.getLongitudeDeg() + << "deg, lat = " << g.getLatitudeDeg() + << "deg, elev = " << g.getElevationM() + << "m"; +} + +#endif diff --git a/simgear/math/SGGeodesy.cxx b/simgear/math/SGGeodesy.cxx new file mode 100644 index 00000000..57b13434 --- /dev/null +++ b/simgear/math/SGGeodesy.cxx @@ -0,0 +1,136 @@ +#include + +#include "SGMath.hxx" + +// These are hard numbers from the WGS84 standard. DON'T MODIFY +// unless you want to change the datum. +#define _EQURAD 6378137.0 +#define _FLATTENING 298.257223563 + +// These are derived quantities more useful to the code: +#if 0 +#define _SQUASH (1 - 1/_FLATTENING) +#define _STRETCH (1/_SQUASH) +#define _POLRAD (EQURAD * _SQUASH) +#else +// High-precision versions of the above produced with an arbitrary +// precision calculator (the compiler might lose a few bits in the FPU +// operations). These are specified to 81 bits of mantissa, which is +// higher than any FPU known to me: +#define _SQUASH 0.9966471893352525192801545 +#define _STRETCH 1.0033640898209764189003079 +#define _POLRAD 6356752.3142451794975639668 +#endif + +// The constants from the WGS84 standard +const double SGGeodesy::EQURAD = _EQURAD; +const double SGGeodesy::iFLATTENING = _FLATTENING; +const double SGGeodesy::SQUASH = _SQUASH; +const double SGGeodesy::STRETCH = _STRETCH; +const double SGGeodesy::POLRAD = _POLRAD; + +// additional derived and precomputable ones +// for the geodetic conversion algorithm + +#define E2 fabs(1 - _SQUASH*_SQUASH) +static double a = _EQURAD; +static double ra2 = 1/(_EQURAD*_EQURAD); +static double e = sqrt(E2); +static double e2 = E2; +static double e4 = E2*E2; + +#undef _EQURAD +#undef _FLATTENING +#undef _SQUASH +#undef _STRETCH +#undef _POLRAD +#undef E2 + +void +SGGeodesy::SGCartToGeod(const SGVec3& cart, SGGeod& geod) +{ + // according to + // H. Vermeille, + // Direct transformation from geocentric to geodetic ccordinates, + // Journal of Geodesy (2002) 76:451-454 + double X = cart(0); + double Y = cart(1); + double Z = cart(2); + double XXpYY = X*X+Y*Y; + double sqrtXXpYY = sqrt(XXpYY); + double p = XXpYY*ra2; + double q = Z*Z*(1-e2)*ra2; + double r = 1/6.0*(p+q-e4); + double s = e4*p*q/(4*r*r*r); + double t = pow(1+s+sqrt(s*(2+s)), 1/3.0); + double u = r*(1+t+1/t); + double v = sqrt(u*u+e4*q); + double w = e2*(u+v-q)/(2*v); + double k = sqrt(u+v+w*w)-w; + double D = k*sqrtXXpYY/(k+e2); + geod.setLongitudeRad(2*atan2(Y, X+sqrtXXpYY)); + double sqrtDDpZZ = sqrt(D*D+Z*Z); + geod.setLatitudeRad(2*atan2(Z, D+sqrtDDpZZ)); + geod.setElevationM((k+e2-1)*sqrtDDpZZ/k); +} + +void +SGGeodesy::SGGeodToCart(const SGGeod& geod, SGVec3& cart) +{ + // according to + // H. Vermeille, + // Direct transformation from geocentric to geodetic ccordinates, + // Journal of Geodesy (2002) 76:451-454 + double lambda = geod.getLongitudeRad(); + double phi = geod.getLatitudeRad(); + double h = geod.getElevationM(); + double sphi = sin(phi); + double n = a/sqrt(1-e2*sphi*sphi); + double cphi = cos(phi); + double slambda = sin(lambda); + double clambda = cos(lambda); + cart(0) = (h+n)*cphi*clambda; + cart(1) = (h+n)*cphi*slambda; + cart(2) = (h+n-e2*n)*sphi; +} + +double +SGGeodesy::SGGeodToSeaLevelRadius(const SGGeod& geod) +{ + // this is just a simplified version of the SGGeodToCart function above, + // substitute h = 0, take the 2-norm of the cartesian vector and simplify + double phi = geod.getLatitudeRad(); + double sphi = sin(phi); + double sphi2 = sphi*sphi; + return a*sqrt((1 + (e4 - 2*e2)*sphi2)/(1 - e2*sphi2)); +} + +void +SGGeodesy::SGCartToGeoc(const SGVec3& cart, SGGeoc& geoc) +{ + double minVal = SGLimits::min(); + if (fabs(cart(0)) < minVal && fabs(cart(1)) < minVal) + geoc.setLongitudeRad(0); + else + geoc.setLongitudeRad(atan2(cart(1), cart(0))); + + double nxy = sqrt(cart(0)*cart(0) + cart(1)*cart(1)); + if (fabs(nxy) < minVal && fabs(cart(2)) < minVal) + geoc.setLatitudeRad(0); + else + geoc.setLatitudeRad(atan2(cart(2), nxy)); + + geoc.setRadiusM(norm(cart)); +} + +void +SGGeodesy::SGGeocToCart(const SGGeoc& geoc, SGVec3& cart) +{ + double lat = geoc.getLatitudeRad(); + double lon = geoc.getLongitudeRad(); + double slat = sin(lat); + double clat = cos(lat); + double slon = sin(lon); + double clon = cos(lon); + cart = geoc.getRadiusM()*SGVec3(clat*clon, clat*slon, slat); +} diff --git a/simgear/math/SGGeodesy.hxx b/simgear/math/SGGeodesy.hxx new file mode 100644 index 00000000..8f44504b --- /dev/null +++ b/simgear/math/SGGeodesy.hxx @@ -0,0 +1,39 @@ +#ifndef SGGeodesy_H +#define SGGeodesy_H + +class SGGeoc; +class SGGeod; + +template +class SGVec3; + +class SGGeodesy { +public: + // Hard numbers from the WGS84 standard. + static const double EQURAD; + static const double iFLATTENING; + static const double SQUASH; + static const double STRETCH; + static const double POLRAD; + + /// Takes a cartesian coordinate data and returns the geodetic + /// coordinates. + static void SGCartToGeod(const SGVec3& cart, SGGeod& geod); + + /// Takes a geodetic coordinate data and returns the cartesian + /// coordinates. + static void SGGeodToCart(const SGGeod& geod, SGVec3& cart); + + /// Takes a geodetic coordinate data and returns the sea level radius. + static double SGGeodToSeaLevelRadius(const SGGeod& geod); + + /// Takes a cartesian coordinate data and returns the geocentric + /// coordinates. + static void SGCartToGeoc(const SGVec3& cart, SGGeoc& geoc); + + /// Takes a geocentric coordinate data and returns the cartesian + /// coordinates. + static void SGGeocToCart(const SGGeoc& geoc, SGVec3& cart); +}; + +#endif diff --git a/simgear/math/SGLimits.hxx b/simgear/math/SGLimits.hxx new file mode 100644 index 00000000..913fc18b --- /dev/null +++ b/simgear/math/SGLimits.hxx @@ -0,0 +1,15 @@ +#ifndef SGLimits_H +#define SGLimits_H + +#include + +/// Helper class for epsilon and so on +/// This is the possible place to hook in for machines not +/// providing numeric_limits ... +template +class SGLimits : public std::numeric_limits {}; + +typedef SGLimits SGLimitsf; +typedef SGLimits SGLimitsd; + +#endif diff --git a/simgear/math/SGMath.hxx b/simgear/math/SGMath.hxx new file mode 100644 index 00000000..c3c96160 --- /dev/null +++ b/simgear/math/SGMath.hxx @@ -0,0 +1,18 @@ +#ifndef SGMath_H +#define SGMath_H + +/// Just include them all + +#include + +#include "SGLimits.hxx" +#include "SGMisc.hxx" +#include "SGGeodesy.hxx" +#include "SGVec3.hxx" +#include "SGVec4.hxx" +#include "SGQuat.hxx" +#include "SGMatrix.hxx" +#include "SGGeoc.hxx" +#include "SGGeod.hxx" + +#endif diff --git a/simgear/math/SGMathTest.cxx b/simgear/math/SGMathTest.cxx new file mode 100644 index 00000000..4ab0763b --- /dev/null +++ b/simgear/math/SGMathTest.cxx @@ -0,0 +1,339 @@ +#include +#include + +#include + +#include "SGMath.hxx" + +template +bool +Vec3Test(void) +{ + SGVec3 v1, v2, v3; + + // Check if the equivalent function works + v1 = SGVec3(1, 2, 3); + v2 = SGVec3(3, 2, 1); + if (equivalent(v1, v2)) + return false; + + // Check the unary minus operator + v3 = SGVec3(-1, -2, -3); + if (!equivalent(-v1, v3)) + return false; + + // Check the unary plus operator + v3 = SGVec3(1, 2, 3); + if (!equivalent(+v1, v3)) + return false; + + // Check the addition operator + v3 = SGVec3(4, 4, 4); + if (!equivalent(v1 + v2, v3)) + return false; + + // Check the subtraction operator + v3 = SGVec3(-2, 0, 2); + if (!equivalent(v1 - v2, v3)) + return false; + + // Check the scaler multiplication operator + v3 = SGVec3(2, 4, 6); + if (!equivalent(2*v1, v3)) + return false; + + // Check the dot product + if (fabs(dot(v1, v2) - 10) > 10*SGLimits::epsilon()) + return false; + + // Check the cross product + v3 = SGVec3(-4, 8, -4); + if (!equivalent(cross(v1, v2), v3)) + return false; + + // Check the euclidean length + if (fabs(14 - length(v1)*length(v1)) > 14*SGLimits::epsilon()) + return false; + + return true; +} + +template +bool +QuatTest(void) +{ + const SGVec3 e1(1, 0, 0); + const SGVec3 e2(0, 1, 0); + const SGVec3 e3(0, 0, 1); + SGVec3 v1, v2; + SGQuat q1, q2, q3, q4; + // Check a rotation around the x axis + q1 = SGQuat::fromAngleAxis(SGMisc::pi(), e1); + v1 = SGVec3(1, 2, 3); + v2 = SGVec3(1, -2, -3); + if (!equivalent(q1.transform(v1), v2)) + return false; + + // Check a rotation around the x axis + q1 = SGQuat::fromAngleAxis(0.5*SGMisc::pi(), e1); + v2 = SGVec3(1, 3, -2); + if (!equivalent(q1.transform(v1), v2)) + return false; + + // Check a rotation around the y axis + q1 = SGQuat::fromAngleAxis(SGMisc::pi(), e2); + v2 = SGVec3(-1, 2, -3); + if (!equivalent(q1.transform(v1), v2)) + return false; + + // Check a rotation around the y axis + q1 = SGQuat::fromAngleAxis(0.5*SGMisc::pi(), e2); + v2 = SGVec3(-3, 2, 1); + if (!equivalent(q1.transform(v1), v2)) + return false; + + // Check a rotation around the z axis + q1 = SGQuat::fromAngleAxis(SGMisc::pi(), e3); + v2 = SGVec3(-1, -2, 3); + if (!equivalent(q1.transform(v1), v2)) + return false; + + // Check a rotation around the z axis + q1 = SGQuat::fromAngleAxis(0.5*SGMisc::pi(), e3); + v2 = SGVec3(2, -1, 3); + if (!equivalent(q1.transform(v1), v2)) + return false; + + // Now check some successive transforms + // We can reuse the prevously tested stuff + q1 = SGQuat::fromAngleAxis(0.5*SGMisc::pi(), e1); + q2 = SGQuat::fromAngleAxis(0.5*SGMisc::pi(), e2); + q3 = q1*q2; + v2 = q2.transform(q1.transform(v1)); + if (!equivalent(q3.transform(v1), v2)) + return false; + + /// Test from Euler angles + float x = 0.2*SGMisc::pi(); + float y = 0.3*SGMisc::pi(); + float z = 0.4*SGMisc::pi(); + q1 = SGQuat::fromAngleAxis(z, e3); + q2 = SGQuat::fromAngleAxis(y, e2); + q3 = SGQuat::fromAngleAxis(x, e1); + v2 = q3.transform(q2.transform(q1.transform(v1))); + q4 = SGQuat::fromEuler(z, y, x); + if (!equivalent(q4.transform(v1), v2)) + return false; + + /// Test angle axis forward and back transform + q1 = SGQuat::fromAngleAxis(0.2*SGMisc::pi(), e1); + q2 = SGQuat::fromAngleAxis(0.7*SGMisc::pi(), e2); + q3 = q1*q2; + SGVec3 angleAxis; + q1.getAngleAxis(angleAxis); + q4 = SGQuat::fromAngleAxis(angleAxis); + if (!equivalent(q1, q4)) + return false; + q2.getAngleAxis(angleAxis); + q4 = SGQuat::fromAngleAxis(angleAxis); + if (!equivalent(q2, q4)) + return false; + q3.getAngleAxis(angleAxis); + q4 = SGQuat::fromAngleAxis(angleAxis); + if (!equivalent(q3, q4)) + return false; + + return true; +} + +template +bool +MatrixTest(void) +{ + // Create some test matrix + SGVec3 v0(2, 7, 17); + SGQuat q0 = SGQuat::fromAngleAxis(SGMisc::pi(), normalize(v0)); + SGMatrix m0(q0, v0); + + // Check the tqo forms of the inverse for that kind of special matrix + SGMatrix m1, m2; + invert(m1, m0); + m2 = transNeg(m0); + if (!equivalent(m1, m2)) + return false; + + // Check matrix multiplication and inversion + if (!equivalent(m0*m1, SGMatrix::unit())) + return false; + if (!equivalent(m1*m0, SGMatrix::unit())) + return false; + if (!equivalent(m0*m2, SGMatrix::unit())) + return false; + if (!equivalent(m2*m0, SGMatrix::unit())) + return false; + + return true; +} + +bool +GeodesyTest(void) +{ + // We know that the values are on the order of 1 + double epsDeg = 10*SGLimits::epsilon(); + // For the altitude values we need to tolerate relative errors in the order + // of the radius + double epsM = 1e6*SGLimits::epsilon(); + + SGVec3 cart0, cart1; + SGGeod geod0, geod1; + SGGeoc geoc0; + + // create some geodetic position + geod0 = SGGeod::fromDegM(30, 20, 17); + + // Test the conversion routines to cartesian coordinates + cart0 = geod0; + geod1 = 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; + if (!equivalent(cart0, cart1)) + return false; + + return true; +} + + +bool +sgInterfaceTest(void) +{ + SGVec3f v3f = SGVec3f::e2(); + SGVec4f v4f = SGVec4f::e2(); + SGQuatf qf = SGQuatf::fromEuler(1.2, 1.3, -0.4); + SGMatrixf mf(qf, v3f); + + // Copy to and from plibs types check if result is equal, + // test for exact equality + SGVec3f tv3f; + sgVec3 sv3f; + sgCopyVec3(sv3f, v3f.sg()); + sgCopyVec3(tv3f.sg(), sv3f); + if (tv3f != v3f) + return false; + + // Copy to and from plibs types check if result is equal, + // test for exact equality + SGVec4f tv4f; + sgVec4 sv4f; + sgCopyVec4(sv4f, v4f.sg()); + sgCopyVec4(tv4f.sg(), sv4f); + if (tv4f != v4f) + return false; + + // Copy to and from plibs types check if result is equal, + // test for exact equality + SGQuatf tqf; + sgQuat sqf; + sgCopyQuat(sqf, qf.sg()); + sgCopyQuat(tqf.sg(), sqf); + if (tqf != qf) + return false; + + // Copy to and from plibs types check if result is equal, + // test for exact equality + SGMatrixf tmf; + sgMat4 smf; + sgCopyMat4(smf, mf.sg()); + sgCopyMat4(tmf.sg(), smf); + if (tmf != mf) + return false; + + return true; +} + +bool +sgdInterfaceTest(void) +{ + SGVec3d v3d = SGVec3d::e2(); + SGVec4d v4d = SGVec4d::e2(); + SGQuatd qd = SGQuatd::fromEuler(1.2, 1.3, -0.4); + SGMatrixd md(qd, v3d); + + // Copy to and from plibs types check if result is equal, + // test for exact equality + SGVec3d tv3d; + sgdVec3 sv3d; + sgdCopyVec3(sv3d, v3d.sg()); + sgdCopyVec3(tv3d.sg(), sv3d); + if (tv3d != v3d) + return false; + + // Copy to and from plibs types check if result is equal, + // test for exact equality + SGVec4d tv4d; + sgdVec4 sv4d; + sgdCopyVec4(sv4d, v4d.sg()); + sgdCopyVec4(tv4d.sg(), sv4d); + if (tv4d != v4d) + return false; + + // Copy to and from plibs types check if result is equal, + // test for exact equality + SGQuatd tqd; + sgdQuat sqd; + sgdCopyQuat(sqd, qd.sg()); + sgdCopyQuat(tqd.sg(), sqd); + if (tqd != qd) + return false; + + // Copy to and from plibs types check if result is equal, + // test for exact equality + SGMatrixd tmd; + sgdMat4 smd; + sgdCopyMat4(smd, md.sg()); + sgdCopyMat4(tmd.sg(), smd); + if (tmd != md) + return false; + + return true; +} + +int +main(void) +{ + // Do vector tests + if (!Vec3Test()) + return EXIT_FAILURE; + if (!Vec3Test()) + return EXIT_FAILURE; + + // Do quaternion tests + if (!QuatTest()) + return EXIT_FAILURE; + if (!QuatTest()) + return EXIT_FAILURE; + + // Do matrix tests + if (!MatrixTest()) + return EXIT_FAILURE; + if (!MatrixTest()) + return EXIT_FAILURE; + + // Check geodetic/geocentric/cartesian conversions +// if (!GeodesyTest()) +// return EXIT_FAILURE; + + // Check interaction with sg*/sgd* + if (!sgInterfaceTest()) + return EXIT_FAILURE; + if (!sgdInterfaceTest()) + return EXIT_FAILURE; + + std::cout << "Successfully passed all tests!" << std::endl; + return EXIT_SUCCESS; +} diff --git a/simgear/math/SGMatrix.hxx b/simgear/math/SGMatrix.hxx new file mode 100644 index 00000000..3e5628c4 --- /dev/null +++ b/simgear/math/SGMatrix.hxx @@ -0,0 +1,581 @@ +#ifndef SGMatrix_H +#define SGMatrix_H + +/// Expression templates for poor programmers ... :) +template +struct TransNegRef; + +template +class SGMatrix; + +/// 3D Matrix Class +template +class SGMatrix { +public: + enum { nCols = 4, nRows = 4, nEnts = 16 }; + typedef T value_type; + + /// Default constructor. Does not initialize at all. + /// If you need them zero initialized, use SGMatrix::zeros() + SGMatrix(void) + { + /// Initialize with nans in the debug build, that will guarantee to have + /// a fast uninitialized default constructor in the release but shows up + /// uninitialized values in the debug build very fast ... +#ifndef NDEBUG + for (unsigned i = 0; i < nEnts; ++i) + _data.flat[i] = SGLimits::quiet_NaN(); +#endif + } + /// Constructor. Initialize by the content of a plain array, + /// make sure it has at least 16 elements + explicit SGMatrix(const T* data) + { for (unsigned i = 0; i < nEnts; ++i) _data.flat[i] = data[i]; } + + /// Constructor, build up a SGMatrix from given elements + SGMatrix(T m00, T m01, T m02, T m03, + T m10, T m11, T m12, T m13, + T m20, T m21, T m22, T m23, + T m30, T m31, T m32, T m33) + { + _data.flat[0] = m00; _data.flat[1] = m10; + _data.flat[2] = m20; _data.flat[3] = m30; + _data.flat[4] = m01; _data.flat[5] = m11; + _data.flat[6] = m21; _data.flat[7] = m31; + _data.flat[8] = m02; _data.flat[9] = m12; + _data.flat[10] = m22; _data.flat[11] = m32; + _data.flat[12] = m03; _data.flat[13] = m13; + _data.flat[14] = m23; _data.flat[15] = m33; + } + + /// Constructor, build up a SGMatrix from a translation + SGMatrix(const SGVec3& trans) + { set(trans); } + + /// Constructor, build up a SGMatrix from a rotation and a translation + SGMatrix(const SGQuat& quat, const SGVec3& trans) + { set(quat, trans); } + /// Constructor, build up a SGMatrix from a rotation and a translation + SGMatrix(const SGQuat& quat) + { set(quat); } + + /// Copy constructor for a transposed negated matrix + SGMatrix(const TransNegRef& tm) + { set(tm); } + + /// Set from a tranlation + void set(const SGVec3& trans) + { + _data.flat[0] = 1; _data.flat[4] = 0; + _data.flat[8] = 0; _data.flat[12] = -trans(0); + _data.flat[1] = 0; _data.flat[5] = 1; + _data.flat[9] = 0; _data.flat[13] = -trans(1); + _data.flat[2] = 0; _data.flat[6] = 0; + _data.flat[10] = 1; _data.flat[14] = -trans(2); + _data.flat[3] = 0; _data.flat[7] = 0; + _data.flat[11] = 0; _data.flat[15] = 1; + } + + /// Set from a scale/rotation and tranlation + void set(const SGQuat& quat, const SGVec3& trans) + { + T w = quat.w(); T x = quat.x(); T y = quat.y(); T z = quat.z(); + T xx = x*x; T yy = y*y; T zz = z*z; + T wx = w*x; T wy = w*y; T wz = w*z; + T xy = x*y; T xz = x*z; T yz = y*z; + _data.flat[0] = 1-2*(yy+zz); _data.flat[1] = 2*(xy-wz); + _data.flat[2] = 2*(xz+wy); _data.flat[3] = 0; + _data.flat[4] = 2*(xy+wz); _data.flat[5] = 1-2*(xx+zz); + _data.flat[6] = 2*(yz-wx); _data.flat[7] = 0; + _data.flat[8] = 2*(xz-wy); _data.flat[9] = 2*(yz+wx); + _data.flat[10] = 1-2*(xx+yy); _data.flat[11] = 0; + // Well, this one is ugly here, as that xform method on the current + // object needs the above data to be already set ... + SGVec3 t = xformVec(trans); + _data.flat[12] = -t(0); _data.flat[13] = -t(1); + _data.flat[14] = -t(2); _data.flat[15] = 1; + } + /// Set from a scale/rotation and tranlation + void set(const SGQuat& quat) + { + T w = quat.w(); T x = quat.x(); T y = quat.y(); T z = quat.z(); + T xx = x*x; T yy = y*y; T zz = z*z; + T wx = w*x; T wy = w*y; T wz = w*z; + T xy = x*y; T xz = x*z; T yz = y*z; + _data.flat[0] = 1-2*(yy+zz); _data.flat[1] = 2*(xy-wz); + _data.flat[2] = 2*(xz+wy); _data.flat[3] = 0; + _data.flat[4] = 2*(xy+wz); _data.flat[5] = 1-2*(xx+zz); + _data.flat[6] = 2*(yz-wx); _data.flat[7] = 0; + _data.flat[8] = 2*(xz-wy); _data.flat[9] = 2*(yz+wx); + _data.flat[10] = 1-2*(xx+yy); _data.flat[11] = 0; + _data.flat[12] = 0; _data.flat[13] = 0; + _data.flat[14] = 0; _data.flat[15] = 1; + } + + /// set from a transposed negated matrix + void set(const TransNegRef& tm) + { + const SGMatrix& m = tm.m; + _data.flat[0] = m(0,0); + _data.flat[1] = m(0,1); + _data.flat[2] = m(0,2); + _data.flat[3] = m(3,0); + + _data.flat[4] = m(1,0); + _data.flat[5] = m(1,1); + _data.flat[6] = m(1,2); + _data.flat[7] = m(3,1); + + _data.flat[8] = m(2,0); + _data.flat[9] = m(2,1); + _data.flat[10] = m(2,2); + _data.flat[11] = m(3,2); + + // Well, this one is ugly here, as that xform method on the current + // object needs the above data to be already set ... + SGVec3 t = xformVec(SGVec3(m(0,3), m(1,3), m(2,3))); + _data.flat[12] = -t(0); + _data.flat[13] = -t(1); + _data.flat[14] = -t(2); + _data.flat[15] = m(3,3); + } + + /// Access by index, the index is unchecked + const T& operator()(unsigned i, unsigned j) const + { return _data.flat[i + 4*j]; } + /// Access by index, the index is unchecked + T& operator()(unsigned i, unsigned j) + { return _data.flat[i + 4*j]; } + + /// Access raw data by index, the index is unchecked + const T& operator[](unsigned i) const + { return _data.flat[i]; } + /// Access by index, the index is unchecked + T& operator[](unsigned i) + { return _data.flat[i]; } + + /// Get the data pointer + const T* data(void) const + { return _data.flat; } + /// Get the data pointer + T* data(void) + { return _data.flat; } + + /// Readonly interface function to ssg's sgMat4/sgdMat4 + const T (&sg(void) const)[4][4] + { return _data.carray; } + /// Interface function to ssg's sgMat4/sgdMat4 + T (&sg(void))[4][4] + { return _data.carray; } + + /// Inplace addition + SGMatrix& operator+=(const SGMatrix& m) + { for (unsigned i = 0; i < nEnts; ++i) _data.flat[i] += m._data.flat[i]; return *this; } + /// Inplace subtraction + SGMatrix& operator-=(const SGMatrix& m) + { for (unsigned i = 0; i < nEnts; ++i) _data.flat[i] -= m._data.flat[i]; return *this; } + /// Inplace scalar multiplication + template + SGMatrix& operator*=(S s) + { for (unsigned i = 0; i < nEnts; ++i) _data.flat[i] *= s; return *this; } + /// Inplace scalar multiplication by 1/s + template + SGMatrix& operator/=(S s) + { return operator*=(1/T(s)); } + /// Inplace matrix multiplication, post multiply + SGMatrix& operator*=(const SGMatrix& m2); + + SGVec3 xformPt(const SGVec3& pt) const + { + SGVec3 tpt; + tpt(0) = (*this)(0,3); + tpt(1) = (*this)(1,3); + tpt(2) = (*this)(2,3); + for (unsigned i = 0; i < SGMatrix::nCols-1; ++i) { + T tmp = pt(i); + tpt(0) += tmp*(*this)(0,i); + tpt(1) += tmp*(*this)(1,i); + tpt(2) += tmp*(*this)(2,i); + } + return tpt; + } + SGVec3 xformVec(const SGVec3& v) const + { + SGVec3 tv; + T tmp = v(0); + tv(0) = tmp*(*this)(0,0); + tv(1) = tmp*(*this)(1,0); + tv(2) = tmp*(*this)(2,0); + for (unsigned i = 1; i < SGMatrix::nCols-1; ++i) { + T tmp = v(i); + tv(0) += tmp*(*this)(0,i); + tv(1) += tmp*(*this)(1,i); + tv(2) += tmp*(*this)(2,i); + } + return tv; + } + + /// Return an all zero matrix + static SGMatrix zeros(void) + { return SGMatrix(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0); } + + /// Return a unit matrix + static SGMatrix unit(void) + { return SGMatrix(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1); } + +private: + /// Required to make that alias safe. + union Data { + T flat[16]; + T carray[4][4]; + }; + + /// The actual data, the matrix is stored in column major order, + /// that matches the storage format of OpenGL + Data _data; +}; + +/// Class to distinguish between a matrix and the matrix with a transposed +/// rotational part and a negated translational part +template +struct TransNegRef { + TransNegRef(const SGMatrix& _m) : m(_m) {} + const SGMatrix& m; +}; + +/// Unary +, do nothing ... +template +inline +const SGMatrix& +operator+(const SGMatrix& m) +{ return m; } + +/// Unary -, do nearly nothing +template +inline +SGMatrix +operator-(const SGMatrix& m) +{ + SGMatrix ret; + for (unsigned i = 0; i < SGMatrix::nEnts; ++i) + ret[i] = -m[i]; + return ret; +} + +/// Binary + +template +inline +SGMatrix +operator+(const SGMatrix& m1, const SGMatrix& m2) +{ + SGMatrix ret; + for (unsigned i = 0; i < SGMatrix::nEnts; ++i) + ret[i] = m1[i] + m2[i]; + return ret; +} + +/// Binary - +template +inline +SGMatrix +operator-(const SGMatrix& m1, const SGMatrix& m2) +{ + SGMatrix ret; + for (unsigned i = 0; i < SGMatrix::nEnts; ++i) + ret[i] = m1[i] - m2[i]; + return ret; +} + +/// Scalar multiplication +template +inline +SGMatrix +operator*(S s, const SGMatrix& m) +{ + SGMatrix ret; + for (unsigned i = 0; i < SGMatrix::nEnts; ++i) + ret[i] = s*m[i]; + return ret; +} + +/// Scalar multiplication +template +inline +SGMatrix +operator*(const SGMatrix& m, S s) +{ + SGMatrix ret; + for (unsigned i = 0; i < SGMatrix::nEnts; ++i) + ret[i] = s*m[i]; + return ret; +} + +/// Vector multiplication +template +inline +SGVec4 +operator*(const SGMatrix& m, const SGVec4& v) +{ + SGVec4 mv; + T tmp = v(0); + mv(0) = tmp*m(0,0); + mv(1) = tmp*m(1,0); + mv(2) = tmp*m(2,0); + mv(3) = tmp*m(3,0); + for (unsigned i = 1; i < SGMatrix::nCols; ++i) { + T tmp = v(i); + mv(0) += tmp*m(0,i); + mv(1) += tmp*m(1,i); + mv(2) += tmp*m(2,i); + mv(3) += tmp*m(3,i); + } + return mv; +} + +/// Vector multiplication +template +inline +SGVec4 +operator*(const TransNegRef& tm, const SGVec4& v) +{ + const SGMatrix& m = tm.m; + SGVec4 mv; + SGVec3 v2; + T tmp = v(3); + mv(0) = v2(0) = -tmp*m(0,3); + mv(1) = v2(1) = -tmp*m(1,3); + mv(2) = v2(2) = -tmp*m(2,3); + mv(3) = tmp*m(3,3); + for (unsigned i = 0; i < SGMatrix::nCols - 1; ++i) { + T tmp = v(i) + v2(i); + mv(0) += tmp*m(i,0); + mv(1) += tmp*m(i,1); + mv(2) += tmp*m(i,2); + mv(3) += tmp*m(3,i); + } + return mv; +} + +/// Matrix multiplication +template +inline +SGMatrix +operator*(const SGMatrix& m1, const SGMatrix& m2) +{ + SGMatrix m; + for (unsigned j = 0; j < SGMatrix::nCols; ++j) { + T tmp = m2(0,j); + m(0,j) = tmp*m1(0,0); + m(1,j) = tmp*m1(1,0); + m(2,j) = tmp*m1(2,0); + m(3,j) = tmp*m1(3,0); + for (unsigned i = 1; i < SGMatrix::nCols; ++i) { + T tmp = m2(i,j); + m(0,j) += tmp*m1(0,i); + m(1,j) += tmp*m1(1,i); + m(2,j) += tmp*m1(2,i); + m(3,j) += tmp*m1(3,i); + } + } + return m; +} + +/// Inplace matrix multiplication, post multiply +template +inline +SGMatrix& +SGMatrix::operator*=(const SGMatrix& m2) +{ (*this) = operator*(*this, m2); return *this; } + +/// Return a reference to the matrix typed to make sure we use the transposed +/// negated matrix +template +inline +TransNegRef +transNeg(const SGMatrix& m) +{ return TransNegRef(m); } + +/// Compute the inverse if the matrix src. Store the result in dst. +/// Return if the matrix is nonsingular. If it is singular dst contains +/// undefined values +template +inline +bool +invert(SGMatrix& dst, const SGMatrix& src) +{ + // Do a LU decomposition with row pivoting and solve into dst + SGMatrix tmp = src; + dst = SGMatrix::unit(); + + for (unsigned i = 0; i < 4; ++i) { + T val = tmp(i,i); + unsigned ind = i; + + // Find the row with the maximum value in the i-th colum + for (unsigned j = i + 1; j < 4; ++j) { + if (fabs(tmp(j, i)) > fabs(val)) { + ind = j; + val = tmp(j, i); + } + } + + // Do row pivoting + if (ind != i) { + for (unsigned j = 0; j < 4; ++j) { + T t; + t = dst(i,j); dst(i,j) = dst(ind,j); dst(ind,j) = t; + t = tmp(i,j); tmp(i,j) = tmp(ind,j); tmp(ind,j) = t; + } + } + + // Check for singularity + if (fabs(val) <= SGLimits::min()) + return false; + + T ival = 1/val; + for (unsigned j = 0; j < 4; ++j) { + tmp(i,j) *= ival; + dst(i,j) *= ival; + } + + for (unsigned j = 0; j < 4; ++j) { + if (j == i) + continue; + + val = tmp(j,i); + for (unsigned k = 0; k < 4; ++k) { + tmp(j,k) -= tmp(i,k) * val; + dst(j,k) -= dst(i,k) * val; + } + } + } + return true; +} + +/// The 1-norm of the matrix, this is the largest column sum of +/// the absolute values of A. +template +inline +T +norm1(const SGMatrix& m) +{ + T nrm = 0; + for (unsigned i = 0; i < SGMatrix::nRows; ++i) { + T sum = fabs(m(i, 0)) + fabs(m(i, 1)) + fabs(m(i, 2)) + fabs(m(i, 3)); + if (nrm < sum) + nrm = sum; + } + return nrm; +} + +/// The inf-norm of the matrix, this is the largest row sum of +/// the absolute values of A. +template +inline +T +normInf(const SGMatrix& m) +{ + T nrm = 0; + for (unsigned i = 0; i < SGMatrix::nCols; ++i) { + T sum = fabs(m(0, i)) + fabs(m(1, i)) + fabs(m(2, i)) + fabs(m(3, i)); + if (nrm < sum) + nrm = sum; + } + return nrm; +} + +/// Return true if exactly the same +template +inline +bool +operator==(const SGMatrix& m1, const SGMatrix& m2) +{ + for (unsigned i = 0; i < SGMatrix::nEnts; ++i) + if (m1[i] != m2[i]) + return false; + return true; +} + +/// Return true if not exactly the same +template +inline +bool +operator!=(const SGMatrix& m1, const SGMatrix& m2) +{ return ! (m1 == m2); } + +/// Return true if equal to the relative tolerance tol +template +inline +bool +equivalent(const SGMatrix& m1, const SGMatrix& m2, T rtol, T atol) +{ return norm1(m1 - m2) < rtol*(norm1(m1) + norm1(m2)) + atol; } + +/// Return true if equal to the relative tolerance tol +template +inline +bool +equivalent(const SGMatrix& m1, const SGMatrix& m2, T rtol) +{ return norm1(m1 - m2) < rtol*(norm1(m1) + norm1(m2)); } + +/// Return true if about equal to roundoff of the underlying type +template +inline +bool +equivalent(const SGMatrix& m1, const SGMatrix& m2) +{ + T tol = 100*SGLimits::epsilon(); + return equivalent(m1, m2, tol, tol); +} + +#ifndef NDEBUG +template +inline +bool +isNaN(const SGMatrix& m) +{ + for (unsigned i = 0; i < SGMatrix::nEnts; ++i) { + if (SGMisc::isNaN(m[i])) + return true; + } + return false; +} +#endif + +/// Output to an ostream +template +inline +std::basic_ostream& +operator<<(std::basic_ostream& s, const SGMatrix& m) +{ + s << "[ " << m(0,0) << ", " << m(0,1) << ", " << m(0,2) << ", " << m(0,3) << "\n"; + s << " " << m(1,0) << ", " << m(1,1) << ", " << m(1,2) << ", " << m(1,3) << "\n"; + s << " " << m(2,0) << ", " << m(2,1) << ", " << m(2,2) << ", " << m(2,3) << "\n"; + s << " " << m(3,0) << ", " << m(3,1) << ", " << m(3,2) << ", " << m(3,3) << " ]"; + return s; +} + +/// Two classes doing actually the same on different types +typedef SGMatrix SGMatrixf; +typedef SGMatrix SGMatrixd; + +inline +SGMatrixf +toMatrixf(const SGMatrixd& m) +{ + return SGMatrixf(m(0,0), m(0,1), m(0,2), m(0,3), + m(1,0), m(1,1), m(1,2), m(1,3), + m(3,0), m(2,1), m(2,2), m(2,3), + m(4,0), m(4,1), m(4,2), m(4,3)); +} + +inline +SGMatrixd +toMatrixd(const SGMatrixf& m) +{ + return SGMatrixd(m(0,0), m(0,1), m(0,2), m(0,3), + m(1,0), m(1,1), m(1,2), m(1,3), + m(3,0), m(2,1), m(2,2), m(2,3), + m(4,0), m(4,1), m(4,2), m(4,3)); +} + +#endif diff --git a/simgear/math/SGMisc.hxx b/simgear/math/SGMisc.hxx new file mode 100644 index 00000000..b6671f84 --- /dev/null +++ b/simgear/math/SGMisc.hxx @@ -0,0 +1,55 @@ +#ifndef SGMisc_H +#define SGMisc_H + +#include + +template +class SGMisc { +public: + static T pi() { return T(3.1415926535897932384626433832795029L); } + static T min(const T& a, const T& b) + { return a < b ? a : b; } + static T min(const T& a, const T& b, const T& c) + { return min(min(a, b), c); } + static T min(const T& a, const T& b, const T& c, const T& d) + { return min(min(min(a, b), c), d); } + static T max(const T& a, const T& b) + { return a > b ? a : b; } + static T max(const T& a, const T& b, const T& c) + { return max(max(a, b), c); } + static T max(const T& a, const T& b, const T& c, const T& d) + { return max(max(max(a, b), c), d); } + static int sign(const T& a) + { + if (a < -SGLimits::min()) + return -1; + else if (SGLimits::min() < a) + return 1; + else + return 0; + } + +#ifndef NDEBUG + /// Returns true if v is a NaN value + /// Use with care: allways code that you do not need to use that! + static bool isNaN(const T& v) + { +#ifdef HAVE_ISNAN + return isnan(v); +#elif defined HAVE_STD_ISNAN + return std::isnan(v); +#else + // Use that every compare involving a NaN returns false + // But be careful, some usual compiler switches like for example + // -fast-math from gcc might optimize that expression to v != v which + // behaves exactly like the opposite ... + return !(v == v); +#endif + } +#endif +}; + +typedef SGMisc SGMiscf; +typedef SGMisc SGMiscd; + +#endif diff --git a/simgear/math/SGQuat.hxx b/simgear/math/SGQuat.hxx new file mode 100644 index 00000000..a1b14081 --- /dev/null +++ b/simgear/math/SGQuat.hxx @@ -0,0 +1,521 @@ +#ifndef SGQuat_H +#define SGQuat_H + +/// 3D Vector Class +template +class SGQuat { +public: + typedef T value_type; + + /// Default constructor. Does not initialize at all. + /// If you need them zero initialized, SGQuat::zeros() + SGQuat(void) + { + /// Initialize with nans in the debug build, that will guarantee to have + /// a fast uninitialized default constructor in the release but shows up + /// uninitialized values in the debug build very fast ... +#ifndef NDEBUG + for (unsigned i = 0; i < 4; ++i) + _data[i] = SGLimits::quiet_NaN(); +#endif + } + /// Constructor. Initialize by the given values + SGQuat(T _x, T _y, T _z, T _w) + { x() = _x; y() = _y; z() = _z; w() = _w; } + /// Constructor. Initialize by the content of a plain array, + /// make sure it has at least 4 elements + explicit SGQuat(const T* d) + { _data[0] = d[0]; _data[1] = d[1]; _data[2] = d[2]; _data[3] = d[3]; } + + /// Return a unit quaternion + static SGQuat unit(void) + { return fromRealImag(1, SGVec3(0)); } + + /// Return a quaternion from euler angles + static SGQuat fromEuler(T z, T y, T x) + { + SGQuat q; + T zd2 = T(0.5)*z; T yd2 = T(0.5)*y; T xd2 = T(0.5)*x; + T Szd2 = sin(zd2); T Syd2 = sin(yd2); T Sxd2 = sin(xd2); + T Czd2 = cos(zd2); T Cyd2 = cos(yd2); T Cxd2 = cos(xd2); + T Cxd2Czd2 = Cxd2*Czd2; T Cxd2Szd2 = Cxd2*Szd2; + T Sxd2Szd2 = Sxd2*Szd2; T Sxd2Czd2 = Sxd2*Czd2; + q.w() = Cxd2Czd2*Cyd2 + Sxd2Szd2*Syd2; + q.x() = Sxd2Czd2*Cyd2 - Cxd2Szd2*Syd2; + q.y() = Cxd2Czd2*Syd2 + Sxd2Szd2*Cyd2; + q.z() = Cxd2Szd2*Cyd2 - Sxd2Czd2*Syd2; + return q; + } + + /// Return a quaternion from euler angles + static SGQuat fromYawPitchRoll(T y, T p, T r) + { return fromEuler(y, p, r); } + + /// Return a quaternion from euler angles + static SGQuat fromHeadAttBank(T h, T a, T b) + { return fromEuler(h, a, b); } + + /// Return a quaternion rotation the the horizontal local frame from given + /// longitude and latitude + static SGQuat fromLonLat(T lon, T lat) + { + SGQuat q; + T zd2 = T(0.5)*lon; + T yd2 = T(-0.25)*SGMisc::pi() - T(0.5)*lat; + T Szd2 = sin(zd2); + T Syd2 = sin(yd2); + T Czd2 = cos(zd2); + T Cyd2 = cos(yd2); + q.w() = Czd2*Cyd2; + q.x() = -Szd2*Syd2; + q.y() = Czd2*Syd2; + q.z() = Szd2*Cyd2; + return q; + } + + /// Create a quaternion from the angle axis representation + static SGQuat fromAngleAxis(T angle, const SGVec3& axis) + { + T angle2 = 0.5*angle; + return fromRealImag(cos(angle2), T(sin(angle2))*axis); + } + + /// Create a quaternion from the angle axis representation where the angle + /// is stored in the axis' length + static SGQuat fromAngleAxis(const SGVec3& axis) + { + T nAxis = norm(axis); + if (nAxis <= SGLimits::min()) + return SGQuat(1, 0, 0, 0); + T angle2 = 0.5*nAxis; + return fromRealImag(cos(angle2), T(sin(angle2)/nAxis)*axis); + } + + /// Return a quaternion from real and imaginary part + static SGQuat fromRealImag(T r, const SGVec3& i) + { + SGQuat q; + q.w() = r; + q.x() = i(0); + q.y() = i(1); + q.z() = i(2); + return q; + } + + /// Return an all zero vector + static SGQuat zeros(void) + { return SGQuat(0, 0, 0, 0); } + + /// write the euler angles into the references + void getEulerRad(T& zRad, T& yRad, T& xRad) const + { + value_type sqrQW = w()*w(); + value_type sqrQX = x()*x(); + value_type sqrQY = y()*y(); + value_type sqrQZ = z()*z(); + + value_type num = 2*(y()*z() + w()*x()); + value_type den = sqrQW - sqrQX - sqrQY + sqrQZ; + if (fabs(den) < SGLimits::min() && + fabs(num) < SGLimits::min()) + xRad = 0; + else + xRad = atan2(num, den); + + value_type tmp = 2*(x()*z() - w()*y()); + if (tmp < -1) + yRad = 0.5*SGMisc::pi(); + else if (1 < tmp) + yRad = -0.5*SGMisc::pi(); + else + yRad = -asin(tmp); + + num = 2*(x()*y() + w()*z()); + den = sqrQW + sqrQX - sqrQY - sqrQZ; + if (fabs(den) < SGLimits::min() && + fabs(num) < SGLimits::min()) + zRad = 0; + else { + value_type psi = atan2(num, den); + if (psi < 0) + psi += 2*SGMisc::pi(); + zRad = psi; + } + } + + /// write the euler angles in degrees into the references + void getEulerDeg(T& zDeg, T& yDeg, T& xDeg) const + { + getEulerRad(zDeg, yDeg, xDeg); + zDeg *= 180/SGMisc::pi(); + yDeg *= 180/SGMisc::pi(); + xDeg *= 180/SGMisc::pi(); + } + + /// write the angle axis representation into the references + void getAngleAxis(T& angle, SGVec3& axis) const + { + T nrm = norm(*this); + if (nrm < SGLimits::min()) { + angle = 0; + axis = SGVec3(0, 0, 0); + } else { + T rNrm = 1/nrm; + angle = acos(SGMisc::max(-1, SGMisc::min(1, rNrm*w()))); + T sAng = sin(angle); + if (fabs(sAng) < SGLimits::min()) + axis = SGVec3(1, 0, 0); + else + axis = (rNrm/sAng)*imag(*this); + angle *= 2; + } + } + + /// write the angle axis representation into the references + void getAngleAxis(SGVec3& axis) const + { + T angle; + getAngleAxis(angle, axis); + axis *= angle; + } + + /// Access by index, the index is unchecked + const T& operator()(unsigned i) const + { return _data[i]; } + /// Access by index, the index is unchecked + T& operator()(unsigned i) + { return _data[i]; } + + /// Access the x component + const T& x(void) const + { return _data[0]; } + /// Access the x component + T& x(void) + { return _data[0]; } + /// Access the y component + const T& y(void) const + { return _data[1]; } + /// Access the y component + T& y(void) + { return _data[1]; } + /// Access the z component + const T& z(void) const + { return _data[2]; } + /// Access the z component + T& z(void) + { return _data[2]; } + /// Access the w component + const T& w(void) const + { return _data[3]; } + /// Access the w component + T& w(void) + { return _data[3]; } + + /// Get the data pointer, usefull for interfacing with plib's sg*Vec + const T* data(void) const + { return _data; } + /// Get the data pointer, usefull for interfacing with plib's sg*Vec + T* data(void) + { return _data; } + + /// Readonly interface function to ssg's sgQuat/sgdQuat + const T (&sg(void) const)[4] + { return _data; } + /// Interface function to ssg's sgQuat/sgdQuat + T (&sg(void))[4] + { return _data; } + + /// Inplace addition + SGQuat& operator+=(const SGQuat& v) + { _data[0]+=v(0);_data[1]+=v(1);_data[2]+=v(2);_data[3]+=v(3);return *this; } + /// Inplace subtraction + SGQuat& operator-=(const SGQuat& v) + { _data[0]-=v(0);_data[1]-=v(1);_data[2]-=v(2);_data[3]-=v(3);return *this; } + /// Inplace scalar multiplication + template + SGQuat& operator*=(S s) + { _data[0] *= s; _data[1] *= s; _data[2] *= s; _data[3] *= s; return *this; } + /// Inplace scalar multiplication by 1/s + template + SGQuat& operator/=(S s) + { return operator*=(1/T(s)); } + /// Inplace quaternion multiplication + SGQuat& operator*=(const SGQuat& v); + + /// Transform a vector from the current coordinate frame to a coordinate + /// frame rotated with the quaternion + SGVec3 transform(const SGVec3& v) const + { + value_type r = 2/dot(*this, *this); + SGVec3 qimag = imag(*this); + value_type qr = real(*this); + return (r*qr*qr - 1)*v + (r*dot(qimag, v))*qimag - (r*qr)*cross(qimag, v); + } + /// Transform a vector from the coordinate frame rotated with the quaternion + /// to the current coordinate frame + SGVec3 backTransform(const SGVec3& v) const + { + value_type r = 2/dot(*this, *this); + SGVec3 qimag = imag(*this); + value_type qr = real(*this); + return (r*qr*qr - 1)*v + (r*dot(qimag, v))*qimag + (r*qr)*cross(qimag, v); + } + + /// Rotate a given vector with the quaternion + SGVec3 rotate(const SGVec3& v) const + { return backTransform(v); } + /// Rotate a given vector with the inverse quaternion + SGVec3 rotateBack(const SGVec3& v) const + { return transform(v); } + + /// Return the time derivative of the quaternion given the angular velocity + SGQuat + derivative(const SGVec3& angVel) + { + SGQuat deriv; + + deriv.w() = 0.5*(-x()*angVel(0) - y()*angVel(1) - z()*angVel(2)); + deriv.x() = 0.5*( w()*angVel(0) - z()*angVel(1) + y()*angVel(2)); + deriv.y() = 0.5*( z()*angVel(0) + w()*angVel(1) - x()*angVel(2)); + deriv.z() = 0.5*(-y()*angVel(0) + x()*angVel(1) + w()*angVel(2)); + + return deriv; + } + +private: + /// The actual data + T _data[4]; +}; + +/// Unary +, do nothing ... +template +inline +const SGQuat& +operator+(const SGQuat& v) +{ return v; } + +/// Unary -, do nearly nothing +template +inline +SGQuat +operator-(const SGQuat& v) +{ return SGQuat(-v(0), -v(1), -v(2), -v(3)); } + +/// Binary + +template +inline +SGQuat +operator+(const SGQuat& v1, const SGQuat& v2) +{ return SGQuat(v1(0)+v2(0), v1(1)+v2(1), v1(2)+v2(2), v1(3)+v2(3)); } + +/// Binary - +template +inline +SGQuat +operator-(const SGQuat& v1, const SGQuat& v2) +{ return SGQuat(v1(0)-v2(0), v1(1)-v2(1), v1(2)-v2(2), v1(3)-v2(3)); } + +/// Scalar multiplication +template +inline +SGQuat +operator*(S s, const SGQuat& v) +{ return SGQuat(s*v(0), s*v(1), s*v(2), s*v(3)); } + +/// Scalar multiplication +template +inline +SGQuat +operator*(const SGQuat& v, S s) +{ return SGQuat(s*v(0), s*v(1), s*v(2), s*v(3)); } + +/// Quaterion multiplication +template +inline +SGQuat +operator*(const SGQuat& v1, const SGQuat& v2) +{ + SGQuat v; + v.x() = v1.w()*v2.x() + v1.x()*v2.w() + v1.y()*v2.z() - v1.z()*v2.y(); + v.y() = v1.w()*v2.y() - v1.x()*v2.z() + v1.y()*v2.w() + v1.z()*v2.x(); + v.z() = v1.w()*v2.z() + v1.x()*v2.y() - v1.y()*v2.x() + v1.z()*v2.w(); + v.w() = v1.w()*v2.w() - v1.x()*v2.x() - v1.y()*v2.y() - v1.z()*v2.z(); + return v; +} + +/// Now define the inplace multiplication +template +inline +SGQuat& +SGQuat::operator*=(const SGQuat& v) +{ (*this) = (*this)*v; return *this; } + +/// The conjugate of the quaternion, this is also the +/// inverse for normalized quaternions +template +inline +SGQuat +conj(const SGQuat& v) +{ return SGQuat(-v(0), -v(1), -v(2), v(3)); } + +/// The conjugate of the quaternion, this is also the +/// inverse for normalized quaternions +template +inline +SGQuat +inverse(const SGQuat& v) +{ return (1/dot(v, v))*SGQuat(-v(0), -v(1), -v(2), v(3)); } + +/// The imagniary part of the quaternion +template +inline +T +real(const SGQuat& v) +{ return v.w(); } + +/// The imagniary part of the quaternion +template +inline +SGVec3 +imag(const SGQuat& v) +{ return SGVec3(v.x(), v.y(), v.z()); } + +/// Scalar dot product +template +inline +T +dot(const SGQuat& v1, const SGQuat& v2) +{ return v1(0)*v2(0) + v1(1)*v2(1) + v1(2)*v2(2) + v1(3)*v2(3); } + +/// The euclidean norm of the vector, that is what most people call length +template +inline +T +norm(const SGQuat& v) +{ return sqrt(dot(v, v)); } + +/// The euclidean norm of the vector, that is what most people call length +template +inline +T +length(const SGQuat& v) +{ return sqrt(dot(v, v)); } + +/// The 1-norm of the vector, this one is the fastest length function we +/// can implement on modern cpu's +template +inline +T +norm1(const SGQuat& v) +{ return fabs(v(0)) + fabs(v(1)) + fabs(v(2)) + fabs(v(3)); } + +/// The euclidean norm of the vector, that is what most people call length +template +inline +SGQuat +normalize(const SGQuat& q) +{ return (1/norm(q))*q; } + +/// Return true if exactly the same +template +inline +bool +operator==(const SGQuat& v1, const SGQuat& v2) +{ return v1(0)==v2(0) && v1(1)==v2(1) && v1(2)==v2(2) && v1(3)==v2(3); } + +/// Return true if not exactly the same +template +inline +bool +operator!=(const SGQuat& v1, const SGQuat& v2) +{ return ! (v1 == v2); } + +/// Return true if equal to the relative tolerance tol +/// Note that this is not the same than comparing quaternions to represent +/// the same rotation +template +inline +bool +equivalent(const SGQuat& v1, const SGQuat& v2, T tol) +{ return norm1(v1 - v2) < tol*(norm1(v1) + norm1(v2)); } + +/// Return true if about equal to roundoff of the underlying type +/// Note that this is not the same than comparing quaternions to represent +/// the same rotation +template +inline +bool +equivalent(const SGQuat& v1, const SGQuat& v2) +{ return equivalent(v1, v2, 100*SGLimits::epsilon()); } + +#ifndef NDEBUG +template +inline +bool +isNaN(const SGQuat& v) +{ + return SGMisc::isNaN(v(0)) || SGMisc::isNaN(v(1)) + || SGMisc::isNaN(v(2)) || SGMisc::isNaN(v(3)); +} +#endif + +/// quaternion interpolation for t in [0,1] interpolate between src (=0) +/// and dst (=1) +template +inline +SGQuat +interpolate(T t, const SGQuat& src, const SGQuat& dst) +{ + T cosPhi = dot(src, dst); + // need to take the shorter way ... + int signCosPhi = SGMisc::sign(cosPhi); + // cosPhi must be corrected for that sign + cosPhi = fabs(cosPhi); + + // first opportunity to fail - make sure acos will succeed later - + // result is correct + if (1 <= cosPhi) + return dst; + + // now the half angle between the orientations + T o = acos(cosPhi); + + // need the scales now, if the angle is very small, do linear interpolation + // to avoid instabilities + T scale0, scale1; + if (fabs(o) < SGLimits::epsilon()) { + scale0 = 1 - t; + scale1 = t; + } else { + // note that we can give a positive lower bound for sin(o) here + T sino = sin(o); + T so = 1/sino; + scale0 = sin((1 - t)*o)*so; + scale1 = sin(t*o)*so; + } + + return scale0*src + signCosPhi*scale1*dst; +} + +/// Output to an ostream +template +inline +std::basic_ostream& +operator<<(std::basic_ostream& s, const SGQuat& v) +{ return s << "[ " << v(0) << ", " << v(1) << ", " << v(2) << ", " << v(3) << " ]"; } + +/// Two classes doing actually the same on different types +typedef SGQuat SGQuatf; +typedef SGQuat SGQuatd; + +inline +SGQuatf +toQuatf(const SGQuatd& v) +{ return SGQuatf(v(0), v(1), v(2), v(3)); } + +inline +SGQuatd +toQuatd(const SGQuatf& v) +{ return SGQuatd(v(0), v(1), v(2), v(3)); } + +#endif diff --git a/simgear/math/SGVec3.hxx b/simgear/math/SGVec3.hxx new file mode 100644 index 00000000..b299d669 --- /dev/null +++ b/simgear/math/SGVec3.hxx @@ -0,0 +1,268 @@ +#ifndef SGVec3_H +#define SGVec3_H + +/// 3D Vector Class +template +class SGVec3 { +public: + typedef T value_type; + + /// Default constructor. Does not initialize at all. + /// If you need them zero initialized, use SGVec3::zeros() + SGVec3(void) + { + /// Initialize with nans in the debug build, that will guarantee to have + /// a fast uninitialized default constructor in the release but shows up + /// uninitialized values in the debug build very fast ... +#ifndef NDEBUG + for (unsigned i = 0; i < 3; ++i) + _data[i] = SGLimits::quiet_NaN(); +#endif + } + /// Constructor. Initialize by the given values + SGVec3(T x, T y, T z) + { _data[0] = x; _data[1] = y; _data[2] = z; } + /// Constructor. Initialize by the content of a plain array, + /// make sure it has at least 3 elements + explicit SGVec3(const T* data) + { _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 + SGVec3(const SGGeod& geod) + { SGGeodesy::SGGeodToCart(geod, *this); } + /// Constructor. Initialize by a geocentric coordinate + /// Note that this conversion is relatively expensive to compute + SGVec3(const SGGeoc& geoc) + { SGGeodesy::SGGeocToCart(geoc, *this); } + + /// Access by index, the index is unchecked + const T& operator()(unsigned i) const + { return _data[i]; } + /// Access by index, the index is unchecked + T& operator()(unsigned i) + { return _data[i]; } + + /// Access the x component + const T& x(void) const + { return _data[0]; } + /// Access the x component + T& x(void) + { return _data[0]; } + /// Access the y component + const T& y(void) const + { return _data[1]; } + /// Access the y component + T& y(void) + { return _data[1]; } + /// Access the z component + const T& z(void) const + { return _data[2]; } + /// Access the z component + T& z(void) + { return _data[2]; } + + /// Get the data pointer + const T* data(void) const + { return _data; } + /// Get the data pointer + T* data(void) + { return _data; } + + /// Readonly interface function to ssg's sgVec3/sgdVec3 + const T (&sg(void) const)[3] + { return _data; } + /// Interface function to ssg's sgVec3/sgdVec3 + T (&sg(void))[3] + { return _data; } + + /// Inplace addition + SGVec3& operator+=(const SGVec3& v) + { _data[0] += v(0); _data[1] += v(1); _data[2] += v(2); return *this; } + /// Inplace subtraction + SGVec3& operator-=(const SGVec3& v) + { _data[0] -= v(0); _data[1] -= v(1); _data[2] -= v(2); return *this; } + /// Inplace scalar multiplication + template + SGVec3& operator*=(S s) + { _data[0] *= s; _data[1] *= s; _data[2] *= s; return *this; } + /// Inplace scalar multiplication by 1/s + template + SGVec3& operator/=(S s) + { return operator*=(1/T(s)); } + + /// Return an all zero vector + static SGVec3 zeros(void) + { return SGVec3(0, 0, 0); } + /// Return unit vectors + static SGVec3 e1(void) + { return SGVec3(1, 0, 0); } + static SGVec3 e2(void) + { return SGVec3(0, 1, 0); } + static SGVec3 e3(void) + { return SGVec3(0, 0, 1); } + +private: + /// The actual data + T _data[3]; +}; + +/// Unary +, do nothing ... +template +inline +const SGVec3& +operator+(const SGVec3& v) +{ return v; } + +/// Unary -, do nearly nothing +template +inline +SGVec3 +operator-(const SGVec3& v) +{ return SGVec3(-v(0), -v(1), -v(2)); } + +/// Binary + +template +inline +SGVec3 +operator+(const SGVec3& v1, const SGVec3& v2) +{ return SGVec3(v1(0)+v2(0), v1(1)+v2(1), v1(2)+v2(2)); } + +/// Binary - +template +inline +SGVec3 +operator-(const SGVec3& v1, const SGVec3& v2) +{ return SGVec3(v1(0)-v2(0), v1(1)-v2(1), v1(2)-v2(2)); } + +/// Scalar multiplication +template +inline +SGVec3 +operator*(S s, const SGVec3& v) +{ return SGVec3(s*v(0), s*v(1), s*v(2)); } + +/// Scalar multiplication +template +inline +SGVec3 +operator*(const SGVec3& v, S s) +{ return SGVec3(s*v(0), s*v(1), s*v(2)); } + +/// Scalar dot product +template +inline +T +dot(const SGVec3& v1, const SGVec3& v2) +{ return v1(0)*v2(0) + v1(1)*v2(1) + v1(2)*v2(2); } + +/// The euclidean norm of the vector, that is what most people call length +template +inline +T +norm(const SGVec3& v) +{ return sqrt(dot(v, v)); } + +/// The euclidean norm of the vector, that is what most people call length +template +inline +T +length(const SGVec3& v) +{ return sqrt(dot(v, v)); } + +/// The 1-norm of the vector, this one is the fastest length function we +/// can implement on modern cpu's +template +inline +T +norm1(const SGVec3& v) +{ return fabs(v(0)) + fabs(v(1)) + fabs(v(2)); } + +/// Vector cross product +template +inline +SGVec3 +cross(const SGVec3& v1, const SGVec3& v2) +{ + return SGVec3(v1(1)*v2(2) - v1(2)*v2(1), + v1(2)*v2(0) - v1(0)*v2(2), + v1(0)*v2(1) - v1(1)*v2(0)); +} + +/// The euclidean norm of the vector, that is what most people call length +template +inline +SGVec3 +normalize(const SGVec3& v) +{ return (1/norm(v))*v; } + +/// Return true if exactly the same +template +inline +bool +operator==(const SGVec3& v1, const SGVec3& v2) +{ return v1(0) == v2(0) && v1(1) == v2(1) && v1(2) == v2(2); } + +/// Return true if not exactly the same +template +inline +bool +operator!=(const SGVec3& v1, const SGVec3& v2) +{ return ! (v1 == v2); } + +/// Return true if equal to the relative tolerance tol +template +inline +bool +equivalent(const SGVec3& v1, const SGVec3& v2, T rtol, T atol) +{ return norm1(v1 - v2) < rtol*(norm1(v1) + norm1(v2)) + atol; } + +/// Return true if equal to the relative tolerance tol +template +inline +bool +equivalent(const SGVec3& v1, const SGVec3& v2, T rtol) +{ return norm1(v1 - v2) < rtol*(norm1(v1) + norm1(v2)); } + +/// Return true if about equal to roundoff of the underlying type +template +inline +bool +equivalent(const SGVec3& v1, const SGVec3& v2) +{ + T tol = 100*SGLimits::epsilon(); + return equivalent(v1, v2, tol, tol); +} + +#ifndef NDEBUG +template +inline +bool +isNaN(const SGVec3& v) +{ + return SGMisc::isNaN(v(0)) || + SGMisc::isNaN(v(1)) || SGMisc::isNaN(v(2)); +} +#endif + +/// Output to an ostream +template +inline +std::basic_ostream& +operator<<(std::basic_ostream& s, const SGVec3& v) +{ return s << "[ " << v(0) << ", " << v(1) << ", " << v(2) << " ]"; } + +/// Two classes doing actually the same on different types +typedef SGVec3 SGVec3f; +typedef SGVec3 SGVec3d; + +inline +SGVec3f +toVec3f(const SGVec3d& v) +{ return SGVec3f(v(0), v(1), v(2)); } + +inline +SGVec3d +toVec3d(const SGVec3f& v) +{ return SGVec3d(v(0), v(1), v(2)); } + +#endif diff --git a/simgear/math/SGVec4.hxx b/simgear/math/SGVec4.hxx new file mode 100644 index 00000000..e5607ccd --- /dev/null +++ b/simgear/math/SGVec4.hxx @@ -0,0 +1,259 @@ +#ifndef SGVec4_H +#define SGVec4_H + +/// 3D Vector Class +template +class SGVec4 { +public: + typedef T value_type; + + /// Default constructor. Does not initialize at all. + /// If you need them zero initialized, use SGVec4::zeros() + SGVec4(void) + { + /// Initialize with nans in the debug build, that will guarantee to have + /// a fast uninitialized default constructor in the release but shows up + /// uninitialized values in the debug build very fast ... +#ifndef NDEBUG + for (unsigned i = 0; i < 4; ++i) + _data[i] = SGLimits::quiet_NaN(); +#endif + } + /// Constructor. Initialize by the given values + SGVec4(T x, T y, T z, T w) + { _data[0] = x; _data[1] = y; _data[2] = z; _data[3] = w; } + /// Constructor. Initialize by the content of a plain array, + /// make sure it has at least 3 elements + explicit SGVec4(const T* d) + { _data[0] = d[0]; _data[1] = d[1]; _data[2] = d[2]; _data[3] = d[3]; } + + + /// Access by index, the index is unchecked + const T& operator()(unsigned i) const + { return _data[i]; } + /// Access by index, the index is unchecked + T& operator()(unsigned i) + { return _data[i]; } + + /// Access the x component + const T& x(void) const + { return _data[0]; } + /// Access the x component + T& x(void) + { return _data[0]; } + /// Access the y component + const T& y(void) const + { return _data[1]; } + /// Access the y component + T& y(void) + { return _data[1]; } + /// Access the z component + const T& z(void) const + { return _data[2]; } + /// Access the z component + T& z(void) + { return _data[2]; } + /// Access the x component + const T& w(void) const + { return _data[3]; } + /// Access the x component + T& w(void) + { return _data[3]; } + + + /// Get the data pointer, usefull for interfacing with plib's sg*Vec + const T* data(void) const + { return _data; } + /// Get the data pointer, usefull for interfacing with plib's sg*Vec + T* data(void) + { return _data; } + + /// Readonly interface function to ssg's sgVec3/sgdVec3 + const T (&sg(void) const)[4] + { return _data; } + /// Interface function to ssg's sgVec3/sgdVec3 + T (&sg(void))[4] + { return _data; } + + /// Inplace addition + SGVec4& operator+=(const SGVec4& v) + { _data[0]+=v(0);_data[1]+=v(1);_data[2]+=v(2);_data[3]+=v(3);return *this; } + /// Inplace subtraction + SGVec4& operator-=(const SGVec4& v) + { _data[0]-=v(0);_data[1]-=v(1);_data[2]-=v(2);_data[3]-=v(3);return *this; } + /// Inplace scalar multiplication + template + SGVec4& operator*=(S s) + { _data[0] *= s; _data[1] *= s; _data[2] *= s; _data[3] *= s; return *this; } + /// Inplace scalar multiplication by 1/s + template + SGVec4& operator/=(S s) + { return operator*=(1/T(s)); } + + /// Return an all zero vector + static SGVec4 zeros(void) + { return SGVec4(0, 0, 0, 0); } + /// Return unit vectors + static SGVec4 e1(void) + { return SGVec4(1, 0, 0, 0); } + static SGVec4 e2(void) + { return SGVec4(0, 1, 0, 0); } + static SGVec4 e3(void) + { return SGVec4(0, 0, 1, 0); } + static SGVec4 e4(void) + { return SGVec4(0, 0, 0, 1); } + +private: + /// The actual data + T _data[4]; +}; + +/// Unary +, do nothing ... +template +inline +const SGVec4& +operator+(const SGVec4& v) +{ return v; } + +/// Unary -, do nearly nothing +template +inline +SGVec4 +operator-(const SGVec4& v) +{ return SGVec4(-v(0), -v(1), -v(2), -v(3)); } + +/// Binary + +template +inline +SGVec4 +operator+(const SGVec4& v1, const SGVec4& v2) +{ return SGVec4(v1(0)+v2(0), v1(1)+v2(1), v1(2)+v2(2), v1(3)+v2(3)); } + +/// Binary - +template +inline +SGVec4 +operator-(const SGVec4& v1, const SGVec4& v2) +{ return SGVec4(v1(0)-v2(0), v1(1)-v2(1), v1(2)-v2(2), v1(3)-v2(3)); } + +/// Scalar multiplication +template +inline +SGVec4 +operator*(S s, const SGVec4& v) +{ return SGVec4(s*v(0), s*v(1), s*v(2), s*v(3)); } + +/// Scalar multiplication +template +inline +SGVec4 +operator*(const SGVec4& v, S s) +{ return SGVec4(s*v(0), s*v(1), s*v(2), s*v(3)); } + +/// Scalar dot product +template +inline +T +dot(const SGVec4& v1, const SGVec4& v2) +{ return v1(0)*v2(0) + v1(1)*v2(1) + v1(2)*v2(2) + v1(3)*v2(3); } + +/// The euclidean norm of the vector, that is what most people call length +template +inline +T +norm(const SGVec4& v) +{ return sqrt(dot(v, v)); } + +/// The euclidean norm of the vector, that is what most people call length +template +inline +T +length(const SGVec4& v) +{ return sqrt(dot(v, v)); } + +/// The 1-norm of the vector, this one is the fastest length function we +/// can implement on modern cpu's +template +inline +T +norm1(const SGVec4& v) +{ return fabs(v(0)) + fabs(v(1)) + fabs(v(2)) + fabs(v(3)); } + +/// The euclidean norm of the vector, that is what most people call length +template +inline +SGVec4 +normalize(const SGVec4& v) +{ return (1/norm(v))*v; } + +/// Return true if exactly the same +template +inline +bool +operator==(const SGVec4& v1, const SGVec4& v2) +{ return v1(0)==v2(0) && v1(1)==v2(1) && v1(2)==v2(2) && v1(3)==v2(3); } + +/// Return true if not exactly the same +template +inline +bool +operator!=(const SGVec4& v1, const SGVec4& v2) +{ return ! (v1 == v2); } + +/// Return true if equal to the relative tolerance tol +template +inline +bool +equivalent(const SGVec4& v1, const SGVec4& v2, T rtol, T atol) +{ return norm1(v1 - v2) < rtol*(norm1(v1) + norm1(v2)) + atol; } + +/// Return true if equal to the relative tolerance tol +template +inline +bool +equivalent(const SGVec4& v1, const SGVec4& v2, T rtol) +{ return norm1(v1 - v2) < rtol*(norm1(v1) + norm1(v2)); } + +/// Return true if about equal to roundoff of the underlying type +template +inline +bool +equivalent(const SGVec4& v1, const SGVec4& v2) +{ + T tol = 100*SGLimits::epsilon(); + return equivalent(v1, v2, tol, tol); +} + +#ifndef NDEBUG +template +inline +bool +isNaN(const SGVec4& v) +{ + return SGMisc::isNaN(v(0)) || SGMisc::isNaN(v(1)) + || SGMisc::isNaN(v(2)) || SGMisc::isNaN(v(3)); +} +#endif + +/// Output to an ostream +template +inline +std::basic_ostream& +operator<<(std::basic_ostream& s, const SGVec4& v) +{ return s << "[ " << v(0) << ", " << v(1) << ", " << v(2) << ", " << v(3) << " ]"; } + +/// Two classes doing actually the same on different types +typedef SGVec4 SGVec4f; +typedef SGVec4 SGVec4d; + +inline +SGVec4f +toVec4f(const SGVec4d& v) +{ return SGVec4f(v(0), v(1), v(2), v(3)); } + +inline +SGVec4d +toVec4d(const SGVec4f& v) +{ return SGVec4d(v(0), v(1), v(2), v(3)); } + +#endif diff --git a/simgear/math/point3d.hxx b/simgear/math/point3d.hxx index c5b72c93..d3552b45 100644 --- a/simgear/math/point3d.hxx +++ b/simgear/math/point3d.hxx @@ -52,7 +52,7 @@ # include #endif -#include +#include "SGMath.hxx" // I don't understand ... or should be included // already depending on how you defined SG_HAVE_STD_INCLUDES, but I @@ -95,6 +95,10 @@ public: explicit Point3D(const double d); Point3D(const Point3D &p); + static Point3D fromSGGeod(const SGGeod& geod); + static Point3D fromSGGeoc(const SGGeoc& geoc); + static Point3D fromSGVec3(const SGVec3& cart); + // Assignment operators Point3D& operator = ( const Point3D& p ); // assignment of a Point3D @@ -126,6 +130,9 @@ public: double radius() const; // polar radius double elev() const; // geodetic elevation (if specifying a surface point) + SGGeod toSGGeod(void) const; + SGGeoc toSGGeoc(void) const; + // friends friend Point3D operator - (const Point3D& p); // -p1 friend bool operator == (const Point3D& a, const Point3D& b); // p1 == p2? @@ -206,6 +213,33 @@ inline Point3D::Point3D(const Point3D& p) n[PX] = p.n[PX]; n[PY] = p.n[PY]; n[PZ] = p.n[PZ]; } +inline Point3D Point3D::fromSGGeod(const SGGeod& geod) +{ + Point3D pt; + pt.setlon(geod.getLongitudeRad()); + pt.setlat(geod.getLatitudeRad()); + pt.setelev(geod.getElevationM()); + return pt; +} + +inline Point3D Point3D::fromSGGeoc(const SGGeoc& geoc) +{ + Point3D pt; + pt.setlon(geoc.getLongitudeRad()); + pt.setlat(geoc.getLatitudeRad()); + pt.setradius(geoc.getRadiusM()); + return pt; +} + +inline Point3D Point3D::fromSGVec3(const SGVec3& cart) +{ + Point3D pt; + pt.setx(cart.x()); + pt.sety(cart.y()); + pt.setz(cart.z()); + return pt; +} + // ASSIGNMENT OPERATORS inline Point3D& Point3D::operator = (const Point3D& p) @@ -290,6 +324,23 @@ inline double Point3D::radius() const { return n[PZ]; } inline double Point3D::elev() const { return n[PZ]; } +inline SGGeod Point3D::toSGGeod(void) const +{ + SGGeod geod; + geod.setLongitudeRad(lon()); + geod.setLatitudeRad(lat()); + geod.setElevationM(elev()); + return geod; +} + +inline SGGeoc Point3D::toSGGeoc(void) const +{ + SGGeoc geoc; + geoc.setLongitudeRad(lon()); + geoc.setLatitudeRad(lat()); + geoc.setRadiusM(radius()); + return geoc; +} // FRIENDS diff --git a/simgear/math/polar3d.cxx b/simgear/math/polar3d.cxx index 6917fe12..5b27c638 100644 --- a/simgear/math/polar3d.cxx +++ b/simgear/math/polar3d.cxx @@ -23,74 +23,11 @@ #include -#include #include #include "polar3d.hxx" - -/** - * Find the Altitude above the Ellipsoid (WGS84) given the Earth - * Centered Cartesian coordinate vector Distances are specified in - * meters. - * @param cp point specified in cartesian coordinates - * @return altitude above the (wgs84) earth in meters - */ -double sgGeodAltFromCart(const Point3D& cp) -{ - double t_lat, x_alpha, mu_alpha; - double lat_geoc, radius; - double result; - - lat_geoc = SGD_PI_2 - atan2( sqrt(cp.x()*cp.x() + cp.y()*cp.y()), cp.z() ); - radius = sqrt( cp.x()*cp.x() + cp.y()*cp.y() + cp.z()*cp.z() ); - - if( ( (SGD_PI_2 - lat_geoc) < SG_ONE_SECOND ) // near North pole - || ( (SGD_PI_2 + lat_geoc) < SG_ONE_SECOND ) ) // near South pole - { - result = radius - SG_EQUATORIAL_RADIUS_M*E; - } else { - t_lat = tan(lat_geoc); - x_alpha = E*SG_EQUATORIAL_RADIUS_M/sqrt(t_lat*t_lat + E*E); - mu_alpha = atan2(sqrt(SG_EQ_RAD_SQUARE_M - x_alpha*x_alpha),E*x_alpha); - if (lat_geoc < 0) { - mu_alpha = - mu_alpha; - } - result = (radius - x_alpha/cos(lat_geoc))*cos(mu_alpha - lat_geoc); - } - - return(result); -} - -/** - * Convert a polar coordinate to a cartesian coordinate. Lon and Lat - * must be specified in radians. The SG convention is for distances - * to be specified in meters - * @param p point specified in polar coordinates - * @return the same point in cartesian coordinates - */ -Point3D sgPolarToCart3d(const Point3D& p) { - double tmp = cos( p.lat() ) * p.radius(); - - return Point3D( cos( p.lon() ) * tmp, - sin( p.lon() ) * tmp, - sin( p.lat() ) * p.radius() ); -} - -/** - * Convert a cartesian coordinate to polar coordinates (lon/lat - * specified in radians. Distances are specified in meters. - * @param cp point specified in cartesian coordinates - * @return the same point in polar coordinates - */ -Point3D sgCartToPolar3d(const Point3D& cp) { - return Point3D( atan2( cp.y(), cp.x() ), - SGD_PI_2 - - atan2( sqrt(cp.x()*cp.x() + cp.y()*cp.y()), cp.z() ), - sqrt(cp.x()*cp.x() + cp.y()*cp.y() + cp.z()*cp.z()) ); -} - /** * Calculate new lon/lat given starting lon/lat, and offset radial, and * distance. NOTE: starting point is specifed in radians, distance is diff --git a/simgear/math/polar3d.hxx b/simgear/math/polar3d.hxx index 4a0c7ed7..d5c351d9 100644 --- a/simgear/math/polar3d.hxx +++ b/simgear/math/polar3d.hxx @@ -34,11 +34,10 @@ #endif -#include - #include #include +#include "SGMath.hxx" /** * Find the Altitude above the Ellipsoid (WGS84) given the Earth @@ -47,7 +46,12 @@ * @param cp point specified in cartesian coordinates * @return altitude above the (wgs84) earth in meters */ -double sgGeodAltFromCart(const Point3D& cp); +inline double sgGeodAltFromCart(const Point3D& p) +{ + SGGeod geod; + SGGeodesy::SGCartToGeod(SGVec3(p.x(), p.y(), p.z()), geod); + return geod.getElevationM(); +} /** @@ -57,7 +61,12 @@ double sgGeodAltFromCart(const Point3D& cp); * @param p point specified in polar coordinates * @return the same point in cartesian coordinates */ -Point3D sgPolarToCart3d(const Point3D& p); +inline Point3D sgPolarToCart3d(const Point3D& p) +{ + SGVec3 cart; + SGGeodesy::SGGeocToCart(SGGeoc::fromRadM(p.lon(), p.lat(), p.radius()), cart); + return Point3D::fromSGVec3(cart); +} /** @@ -66,7 +75,12 @@ Point3D sgPolarToCart3d(const Point3D& p); * @param cp point specified in cartesian coordinates * @return the same point in polar coordinates */ -Point3D sgCartToPolar3d(const Point3D& cp); +inline Point3D sgCartToPolar3d(const Point3D& p) +{ + SGGeoc geoc; + SGGeodesy::SGCartToGeoc(SGVec3(p.x(), p.y(), p.z()), geoc); + return Point3D::fromSGGeoc(geoc); +} /** diff --git a/simgear/math/sg_geodesy.cxx b/simgear/math/sg_geodesy.cxx index f56e93fa..fa6fcb96 100644 --- a/simgear/math/sg_geodesy.cxx +++ b/simgear/math/sg_geodesy.cxx @@ -1,4 +1,5 @@ #include +#include "SGMath.hxx" #include "sg_geodesy.hxx" // Notes: @@ -40,171 +41,6 @@ static const double STRETCH = 1.0033640898209764189003079; static const double POLRAD = 6356752.3142451794975639668; #endif -// Returns a "local" geodetic latitude: an approximation that will be -// correct only at zero altitude. -static double localLat(double r, double z) -{ - // Squash to a spherical earth, compute a tangent vector to the - // surface circle (in squashed space, the surface is a perfect - // sphere) by swapping the components and negating one, stretch to - // real coordinates, and take an inverse-tangent/perpedicular - // vector to get a local geodetic "up" vector. (Those steps all - // cook down to just a few multiplies). Then just turn it into an - // angle. - double upr = r * SQUASH; - double upz = z * STRETCH; - return atan2(upz, upr); -} - -// This is the inverse of the algorithm in localLat(). It returns the -// (cylindrical) coordinates of a surface latitude expressed as an -// "up" unit vector. -static void surfRZ(double upr, double upz, double* r, double* z) -{ - // We are - // converting a (2D, cylindrical) "up" vector defined by the - // geodetic latitude into unitless R and Z coordinates in - // cartesian space. - double R = upr * STRETCH; - double Z = upz * SQUASH; - - // Now we need to turn R and Z into a surface point. That is, - // pick a coefficient C for them such that the point is on the - // surface when converted to "squashed" space: - // (C*R*SQUASH)^2 + (C*Z)^2 = POLRAD^2 - // C^2 = POLRAD^2 / ((R*SQUASH)^2 + Z^2) - double sr = R * SQUASH; - double c = POLRAD / sqrt(sr*sr + Z*Z); - R *= c; - Z *= c; - - *r = R; *z = Z; -} - -// 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. -static double seaLevelRadius(double r, double z) -{ - double sr = r * SQUASH; - double norm = POLRAD/sqrt(sr*sr + z*z); - r *= norm; - z *= norm; - return sqrt(r*r + z*z); -} - -// Convert a cartexian XYZ coordinate to a geodetic lat/lon/alt. This -// is a "recursion relation". In essence, it iterates over the 2D -// part of sgGeodToCart refining its approximation at each step. The -// MAX_LAT_ERROR threshold is picked carefully to allow us to reach -// the full precision of an IEEE double. While this algorithm might -// look slow, it's not. It actually converges very fast indeed -- -// I've never seen it take more than six iterations under normal -// conditions. Three or four is more typical. (It gets slower as the -// altitude/error gets larger; at 50000m altitude, it starts to need -// seven loops.) One caveat is that at *very* large altitudes, it -// starts making very poor guesses as to latitude. As altitude -// approaches infinity, it should be guessing with geocentric -// coordinates, not "local geodetic up" ones. -void sgCartToGeod(const double* xyz, double* lat, double* lon, double* alt) -{ - // The error is expressed as a radian angle, and we want accuracy - // to 1 part in 2^50 (an IEEE double has between 51 and 52 - // significant bits of magnitude due to the "hidden" digit; leave - // at least one bit free for potential slop). In real units, this - // works out to about 6 nanometers. - static const double MAX_LAT_ERROR = 8.881784197001252e-16; - double x = xyz[0], y = xyz[1], z = xyz[2]; - - // Longitude is trivial. Convert to cylindrical "(r, z)" - // coordinates while we're at it. - *lon = atan2(y, x); - double r = sqrt(x*x + y*y); - - double lat1, lat2 = localLat(r, z); - double r2, z2, dot; - do { - lat1 = lat2; - - // Compute an "up" vector - double upr = cos(lat1); - double upz = sin(lat1); - - // Find the surface point with that latitude - surfRZ(upr, upz, &r2, &z2); - - // Convert r2z2 to the vector pointing from the surface to rz - r2 = r - r2; - z2 = z - z2; - - // Dot it with "up" to get an approximate altitude - dot = r2*upr + z2*upz; - - // And compute an approximate geodetic surface coordinate - // using that altitude, so now: R2Z2 = RZ - ((RZ - SURF) dot - // UP) - r2 = r - dot * upr; - z2 = z - dot * upz; - - // Find the latitude of *that* point, and iterate - lat2 = localLat(r2, z2); - } while(fabs(lat2 - lat1) > MAX_LAT_ERROR); - - // All done! We have an accurate geodetic lattitude, now - // calculate the altitude as a cartesian distance between the - // final geodetic surface point and the initial r/z coordinate. - *lat = lat1; - double dr = r - r2; - double dz = z - z2; - double altsign = (dot > 0) ? 1 : -1; - *alt = altsign * sqrt(dr*dr + dz*dz); -} - -void sgGeodToCart(double lat, double lon, double alt, double* xyz) -{ - // This is the inverse of the algorithm in localLat(). We are - // converting a (2D, cylindrical) "up" vector defined by the - // geodetic latitude into unitless R and Z coordinates in - // cartesian space. - double upr = cos(lat); - double upz = sin(lat); - double r, z; - surfRZ(upr, upz, &r, &z); - - // Add the altitude using the "up" unit vector we calculated - // initially. - r += upr * alt; - z += upz * alt; - - // Finally, convert from cylindrical to cartesian - xyz[0] = r * cos(lon); - xyz[1] = r * sin(lon); - xyz[2] = z; -} - -void sgGeocToGeod(double lat_geoc, double radius, - double *lat_geod, double *alt, double *sea_level_r) -{ - // Build a fake cartesian point, and run it through CartToGeod - double lon_dummy, xyz[3]; - xyz[0] = cos(lat_geoc) * radius; - xyz[1] = 0; - xyz[2] = sin(lat_geoc) * radius; - sgCartToGeod(xyz, lat_geod, &lon_dummy, alt); - *sea_level_r = seaLevelRadius(xyz[0], xyz[2]); -} - -void sgGeodToGeoc(double lat_geod, double alt, - double *sl_radius, double *lat_geoc) -{ - double xyz[3]; - sgGeodToCart(lat_geod, 0, alt, xyz); - *lat_geoc = atan2(xyz[2], xyz[0]); - *sl_radius = seaLevelRadius(xyz[0], xyz[2]); -} - //////////////////////////////////////////////////////////////////////// // // Direct and inverse distance functions diff --git a/simgear/math/sg_geodesy.hxx b/simgear/math/sg_geodesy.hxx index 0024e541..c2f9b291 100644 --- a/simgear/math/sg_geodesy.hxx +++ b/simgear/math/sg_geodesy.hxx @@ -2,6 +2,19 @@ #define _SG_GEODESY_HXX #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 @@ -12,9 +25,17 @@ * @param sea_level_r (out) radius from earth center to sea level at * local vertical (surface normal) of C.G. (meters) */ -void sgGeocToGeod(double lat_geoc, double radius, - double *lat_geod, double *alt, double *sea_level_r); - +inline void sgGeocToGeod(double lat_geoc, double radius, + double *lat_geod, double *alt, double *sea_level_r) +{ + SGVec3 cart; + SGGeodesy::SGGeocToCart(SGGeoc::fromRadM(0, lat_geoc, radius), cart); + SGGeod geod; + SGGeodesy::SGCartToGeod(cart, geod); + *lat_geod = geod.getLatitudeRad(); + *alt = geod.getElevationM(); + *sea_level_r = SGGeodesy::SGGeodToSeaLevelRadius(geod); +} /** * Convert from geodetic coordinates to geocentric coordinates. @@ -31,8 +52,17 @@ void sgGeocToGeod(double lat_geoc, double radius, * @param sl_radius (out) SEA LEVEL radius to earth center (meters) * @param lat_geoc (out) Geocentric latitude, radians, + = North */ -void sgGeodToGeoc(double lat_geod, double alt, - double *sl_radius, double *lat_geoc ); +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); + SGGeoc geoc; + SGGeodesy::SGCartToGeoc(cart, geoc); + *lat_geoc = geoc.getLatitudeRad(); + *sl_radius = seaLevelRadius(cart(0), cart(2)); +} + /** * Convert a cartesian point to a geodetic lat/lon/altitude. @@ -42,7 +72,14 @@ void sgGeodToGeoc(double lat_geod, double alt, * @param lon (out) Longitude, in radians * @param alt (out) Altitude, in meters above the WGS84 ellipsoid */ -void sgCartToGeod(const double* xyz, double* lat, double* lon, double* alt); +inline void sgCartToGeod(const double* xyz, double* lat, double* lon, double* alt) +{ + SGGeod geod; + SGGeodesy::SGCartToGeod(SGVec3(xyz), geod); + *lat = geod.getLatitudeRad(); + *lon = geod.getLongitudeRad(); + *alt = geod.getElevationM(); +} /** * Convert a cartesian point to a geodetic lat/lon/altitude. @@ -53,10 +90,9 @@ void sgCartToGeod(const double* xyz, double* lat, double* lon, double* alt); */ inline Point3D sgCartToGeod(const Point3D& p) { - double lat, lon, alt, xyz[3]; - xyz[0] = p.x(); xyz[1] = p.y(); xyz[2] = p.z(); - sgCartToGeod(xyz, &lat, &lon, &alt); - return Point3D(lon, lat, alt); + SGGeod geod; + SGGeodesy::SGCartToGeod(SGVec3(p.x(), p.y(), p.z()), geod); + return Point3D::fromSGGeod(geod); } @@ -68,7 +104,14 @@ inline Point3D sgCartToGeod(const Point3D& p) * @param alt (in) Altitude, in meters above the WGS84 ellipsoid * @param xyz (out) Pointer to cartesian point. */ -void sgGeodToCart(double lat, double lon, double alt, double* xyz); +inline void sgGeodToCart(double lat, double lon, double alt, double* xyz) +{ + SGVec3 cart; + SGGeodesy::SGGeodToCart(SGGeod::fromRadM(lon, lat, alt), cart); + xyz[0] = cart(0); + xyz[1] = cart(1); + xyz[2] = cart(2); +} /** * Convert a geodetic lat/lon/altitude to a cartesian point. @@ -79,9 +122,9 @@ void sgGeodToCart(double lat, double lon, double alt, double* xyz); */ inline Point3D sgGeodToCart(const Point3D& geod) { - double xyz[3]; - sgGeodToCart(geod.lat(), geod.lon(), geod.elev(), xyz); - return Point3D(xyz[0], xyz[1], xyz[2]); + SGVec3 cart; + SGGeodesy::SGGeodToCart(SGGeod::fromRadM(geod.lon(), geod.lat(), geod.elev()), cart); + return Point3D::fromSGVec3(cart); } /**