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 \
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
sg_geodesy.cxx \
sg_random.c \
vector.cxx \
- fastmath.cxx
+ fastmath.cxx \
+ SGGeodesy.cxx
INCLUDES = -I$(top_srcdir)
--- /dev/null
+#ifndef SGGeoc_H
+#define SGGeoc_H
+
+#include <simgear/constants.h>
+
+// #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<double>& 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<double>& cart)
+{
+ SGGeodesy::SGCartToGeoc(cart, *this);
+}
+
+inline
+SGGeoc::SGGeoc(const SGGeod& geod)
+{
+ SGVec3<double> 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<typename char_type, typename traits_type>
+inline
+std::basic_ostream<char_type, traits_type>&
+operator<<(std::basic_ostream<char_type, traits_type>& s, const SGGeoc& g)
+{
+ return s << "lon = " << g.getLongitudeDeg()
+ << ", lat = " << g.getLatitudeDeg()
+ << ", radius = " << g.getRadiusM();
+}
+
+#endif
--- /dev/null
+#ifndef SGGeod_H
+#define SGGeod_H
+
+#include <simgear/constants.h>
+
+// #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<double>& 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<double>& cart)
+{
+ SGGeodesy::SGCartToGeod(cart, *this);
+}
+
+inline
+SGGeod::SGGeod(const SGGeoc& geoc)
+{
+ SGVec3<double> 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<typename char_type, typename traits_type>
+inline
+std::basic_ostream<char_type, traits_type>&
+operator<<(std::basic_ostream<char_type, traits_type>& s, const SGGeod& g)
+{
+ return s << "lon = " << g.getLongitudeDeg()
+ << "deg, lat = " << g.getLatitudeDeg()
+ << "deg, elev = " << g.getElevationM()
+ << "m";
+}
+
+#endif
--- /dev/null
+#include <cmath>
+
+#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<double>& 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<double>& 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<double>& cart, SGGeoc& geoc)
+{
+ double minVal = SGLimits<double>::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<double>& 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<double>(clat*clon, clat*slon, slat);
+}
--- /dev/null
+#ifndef SGGeodesy_H
+#define SGGeodesy_H
+
+class SGGeoc;
+class SGGeod;
+
+template<typename T>
+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<double>& cart, SGGeod& geod);
+
+ /// Takes a geodetic coordinate data and returns the cartesian
+ /// coordinates.
+ static void SGGeodToCart(const SGGeod& geod, SGVec3<double>& 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<double>& cart, SGGeoc& geoc);
+
+ /// Takes a geocentric coordinate data and returns the cartesian
+ /// coordinates.
+ static void SGGeocToCart(const SGGeoc& geoc, SGVec3<double>& cart);
+};
+
+#endif
--- /dev/null
+#ifndef SGLimits_H
+#define SGLimits_H
+
+#include <limits>
+
+/// Helper class for epsilon and so on
+/// This is the possible place to hook in for machines not
+/// providing numeric_limits ...
+template<typename T>
+class SGLimits : public std::numeric_limits<T> {};
+
+typedef SGLimits<float> SGLimitsf;
+typedef SGLimits<double> SGLimitsd;
+
+#endif
--- /dev/null
+#ifndef SGMath_H
+#define SGMath_H
+
+/// Just include them all
+
+#include <iosfwd>
+
+#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
--- /dev/null
+#include <cstdlib>
+#include <iostream>
+
+#include <plib/sg.h>
+
+#include "SGMath.hxx"
+
+template<typename T>
+bool
+Vec3Test(void)
+{
+ SGVec3<T> v1, v2, v3;
+
+ // Check if the equivalent function works
+ v1 = SGVec3<T>(1, 2, 3);
+ v2 = SGVec3<T>(3, 2, 1);
+ if (equivalent(v1, v2))
+ return false;
+
+ // Check the unary minus operator
+ v3 = SGVec3<T>(-1, -2, -3);
+ if (!equivalent(-v1, v3))
+ return false;
+
+ // Check the unary plus operator
+ v3 = SGVec3<T>(1, 2, 3);
+ if (!equivalent(+v1, v3))
+ return false;
+
+ // Check the addition operator
+ v3 = SGVec3<T>(4, 4, 4);
+ if (!equivalent(v1 + v2, v3))
+ return false;
+
+ // Check the subtraction operator
+ v3 = SGVec3<T>(-2, 0, 2);
+ if (!equivalent(v1 - v2, v3))
+ return false;
+
+ // Check the scaler multiplication operator
+ v3 = SGVec3<T>(2, 4, 6);
+ if (!equivalent(2*v1, v3))
+ return false;
+
+ // Check the dot product
+ if (fabs(dot(v1, v2) - 10) > 10*SGLimits<T>::epsilon())
+ return false;
+
+ // Check the cross product
+ v3 = SGVec3<T>(-4, 8, -4);
+ if (!equivalent(cross(v1, v2), v3))
+ return false;
+
+ // Check the euclidean length
+ if (fabs(14 - length(v1)*length(v1)) > 14*SGLimits<T>::epsilon())
+ return false;
+
+ return true;
+}
+
+template<typename T>
+bool
+QuatTest(void)
+{
+ const SGVec3<T> e1(1, 0, 0);
+ const SGVec3<T> e2(0, 1, 0);
+ const SGVec3<T> e3(0, 0, 1);
+ SGVec3<T> v1, v2;
+ SGQuat<T> q1, q2, q3, q4;
+ // Check a rotation around the x axis
+ q1 = SGQuat<T>::fromAngleAxis(SGMisc<T>::pi(), e1);
+ v1 = SGVec3<T>(1, 2, 3);
+ v2 = SGVec3<T>(1, -2, -3);
+ if (!equivalent(q1.transform(v1), v2))
+ return false;
+
+ // Check a rotation around the x axis
+ q1 = SGQuat<T>::fromAngleAxis(0.5*SGMisc<T>::pi(), e1);
+ v2 = SGVec3<T>(1, 3, -2);
+ if (!equivalent(q1.transform(v1), v2))
+ return false;
+
+ // Check a rotation around the y axis
+ q1 = SGQuat<T>::fromAngleAxis(SGMisc<T>::pi(), e2);
+ v2 = SGVec3<T>(-1, 2, -3);
+ if (!equivalent(q1.transform(v1), v2))
+ return false;
+
+ // Check a rotation around the y axis
+ q1 = SGQuat<T>::fromAngleAxis(0.5*SGMisc<T>::pi(), e2);
+ v2 = SGVec3<T>(-3, 2, 1);
+ if (!equivalent(q1.transform(v1), v2))
+ return false;
+
+ // Check a rotation around the z axis
+ q1 = SGQuat<T>::fromAngleAxis(SGMisc<T>::pi(), e3);
+ v2 = SGVec3<T>(-1, -2, 3);
+ if (!equivalent(q1.transform(v1), v2))
+ return false;
+
+ // Check a rotation around the z axis
+ q1 = SGQuat<T>::fromAngleAxis(0.5*SGMisc<T>::pi(), e3);
+ v2 = SGVec3<T>(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<T>::fromAngleAxis(0.5*SGMisc<T>::pi(), e1);
+ q2 = SGQuat<T>::fromAngleAxis(0.5*SGMisc<T>::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<T>::pi();
+ float y = 0.3*SGMisc<T>::pi();
+ float z = 0.4*SGMisc<T>::pi();
+ q1 = SGQuat<T>::fromAngleAxis(z, e3);
+ q2 = SGQuat<T>::fromAngleAxis(y, e2);
+ q3 = SGQuat<T>::fromAngleAxis(x, e1);
+ v2 = q3.transform(q2.transform(q1.transform(v1)));
+ q4 = SGQuat<T>::fromEuler(z, y, x);
+ if (!equivalent(q4.transform(v1), v2))
+ return false;
+
+ /// Test angle axis forward and back transform
+ q1 = SGQuat<T>::fromAngleAxis(0.2*SGMisc<T>::pi(), e1);
+ q2 = SGQuat<T>::fromAngleAxis(0.7*SGMisc<T>::pi(), e2);
+ q3 = q1*q2;
+ SGVec3<T> angleAxis;
+ q1.getAngleAxis(angleAxis);
+ q4 = SGQuat<T>::fromAngleAxis(angleAxis);
+ if (!equivalent(q1, q4))
+ return false;
+ q2.getAngleAxis(angleAxis);
+ q4 = SGQuat<T>::fromAngleAxis(angleAxis);
+ if (!equivalent(q2, q4))
+ return false;
+ q3.getAngleAxis(angleAxis);
+ q4 = SGQuat<T>::fromAngleAxis(angleAxis);
+ if (!equivalent(q3, q4))
+ return false;
+
+ return true;
+}
+
+template<typename T>
+bool
+MatrixTest(void)
+{
+ // Create some test matrix
+ SGVec3<T> v0(2, 7, 17);
+ SGQuat<T> q0 = SGQuat<T>::fromAngleAxis(SGMisc<T>::pi(), normalize(v0));
+ SGMatrix<T> m0(q0, v0);
+
+ // Check the tqo forms of the inverse for that kind of special matrix
+ SGMatrix<T> m1, m2;
+ invert(m1, m0);
+ m2 = transNeg(m0);
+ if (!equivalent(m1, m2))
+ return false;
+
+ // Check matrix multiplication and inversion
+ if (!equivalent(m0*m1, SGMatrix<T>::unit()))
+ return false;
+ if (!equivalent(m1*m0, SGMatrix<T>::unit()))
+ return false;
+ if (!equivalent(m0*m2, SGMatrix<T>::unit()))
+ return false;
+ if (!equivalent(m2*m0, SGMatrix<T>::unit()))
+ return false;
+
+ return true;
+}
+
+bool
+GeodesyTest(void)
+{
+ // We know that the values are on the order of 1
+ double epsDeg = 10*SGLimits<double>::epsilon();
+ // For the altitude values we need to tolerate relative errors in the order
+ // of the radius
+ double epsM = 1e6*SGLimits<double>::epsilon();
+
+ SGVec3<double> 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<float>())
+ return EXIT_FAILURE;
+ if (!Vec3Test<double>())
+ return EXIT_FAILURE;
+
+ // Do quaternion tests
+ if (!QuatTest<float>())
+ return EXIT_FAILURE;
+ if (!QuatTest<double>())
+ return EXIT_FAILURE;
+
+ // Do matrix tests
+ if (!MatrixTest<float>())
+ return EXIT_FAILURE;
+ if (!MatrixTest<double>())
+ 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;
+}
--- /dev/null
+#ifndef SGMatrix_H
+#define SGMatrix_H
+
+/// Expression templates for poor programmers ... :)
+template<typename T>
+struct TransNegRef;
+
+template<typename T>
+class SGMatrix;
+
+/// 3D Matrix Class
+template<typename T>
+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<T>::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<T>& trans)
+ { set(trans); }
+
+ /// Constructor, build up a SGMatrix from a rotation and a translation
+ SGMatrix(const SGQuat<T>& quat, const SGVec3<T>& trans)
+ { set(quat, trans); }
+ /// Constructor, build up a SGMatrix from a rotation and a translation
+ SGMatrix(const SGQuat<T>& quat)
+ { set(quat); }
+
+ /// Copy constructor for a transposed negated matrix
+ SGMatrix(const TransNegRef<T>& tm)
+ { set(tm); }
+
+ /// Set from a tranlation
+ void set(const SGVec3<T>& 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<T>& quat, const SGVec3<T>& 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> 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<T>& 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<T>& 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> t = xformVec(SGVec3<T>(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<typename S>
+ SGMatrix& operator*=(S s)
+ { for (unsigned i = 0; i < nEnts; ++i) _data.flat[i] *= s; return *this; }
+ /// Inplace scalar multiplication by 1/s
+ template<typename S>
+ SGMatrix& operator/=(S s)
+ { return operator*=(1/T(s)); }
+ /// Inplace matrix multiplication, post multiply
+ SGMatrix& operator*=(const SGMatrix<T>& m2);
+
+ SGVec3<T> xformPt(const SGVec3<T>& pt) const
+ {
+ SGVec3<T> tpt;
+ tpt(0) = (*this)(0,3);
+ tpt(1) = (*this)(1,3);
+ tpt(2) = (*this)(2,3);
+ for (unsigned i = 0; i < SGMatrix<T>::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<T> xformVec(const SGVec3<T>& v) const
+ {
+ SGVec3<T> 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<T>::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<typename T>
+struct TransNegRef {
+ TransNegRef(const SGMatrix<T>& _m) : m(_m) {}
+ const SGMatrix<T>& m;
+};
+
+/// Unary +, do nothing ...
+template<typename T>
+inline
+const SGMatrix<T>&
+operator+(const SGMatrix<T>& m)
+{ return m; }
+
+/// Unary -, do nearly nothing
+template<typename T>
+inline
+SGMatrix<T>
+operator-(const SGMatrix<T>& m)
+{
+ SGMatrix<T> ret;
+ for (unsigned i = 0; i < SGMatrix<T>::nEnts; ++i)
+ ret[i] = -m[i];
+ return ret;
+}
+
+/// Binary +
+template<typename T>
+inline
+SGMatrix<T>
+operator+(const SGMatrix<T>& m1, const SGMatrix<T>& m2)
+{
+ SGMatrix<T> ret;
+ for (unsigned i = 0; i < SGMatrix<T>::nEnts; ++i)
+ ret[i] = m1[i] + m2[i];
+ return ret;
+}
+
+/// Binary -
+template<typename T>
+inline
+SGMatrix<T>
+operator-(const SGMatrix<T>& m1, const SGMatrix<T>& m2)
+{
+ SGMatrix<T> ret;
+ for (unsigned i = 0; i < SGMatrix<T>::nEnts; ++i)
+ ret[i] = m1[i] - m2[i];
+ return ret;
+}
+
+/// Scalar multiplication
+template<typename S, typename T>
+inline
+SGMatrix<T>
+operator*(S s, const SGMatrix<T>& m)
+{
+ SGMatrix<T> ret;
+ for (unsigned i = 0; i < SGMatrix<T>::nEnts; ++i)
+ ret[i] = s*m[i];
+ return ret;
+}
+
+/// Scalar multiplication
+template<typename S, typename T>
+inline
+SGMatrix<T>
+operator*(const SGMatrix<T>& m, S s)
+{
+ SGMatrix<T> ret;
+ for (unsigned i = 0; i < SGMatrix<T>::nEnts; ++i)
+ ret[i] = s*m[i];
+ return ret;
+}
+
+/// Vector multiplication
+template<typename T>
+inline
+SGVec4<T>
+operator*(const SGMatrix<T>& m, const SGVec4<T>& v)
+{
+ SGVec4<T> 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<T>::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<typename T>
+inline
+SGVec4<T>
+operator*(const TransNegRef<T>& tm, const SGVec4<T>& v)
+{
+ const SGMatrix<T>& m = tm.m;
+ SGVec4<T> mv;
+ SGVec3<T> 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<T>::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<typename T>
+inline
+SGMatrix<T>
+operator*(const SGMatrix<T>& m1, const SGMatrix<T>& m2)
+{
+ SGMatrix<T> m;
+ for (unsigned j = 0; j < SGMatrix<T>::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<T>::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<typename T>
+inline
+SGMatrix<T>&
+SGMatrix<T>::operator*=(const SGMatrix<T>& m2)
+{ (*this) = operator*(*this, m2); return *this; }
+
+/// Return a reference to the matrix typed to make sure we use the transposed
+/// negated matrix
+template<typename T>
+inline
+TransNegRef<T>
+transNeg(const SGMatrix<T>& m)
+{ return TransNegRef<T>(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<typename T>
+inline
+bool
+invert(SGMatrix<T>& dst, const SGMatrix<T>& src)
+{
+ // Do a LU decomposition with row pivoting and solve into dst
+ SGMatrix<T> tmp = src;
+ dst = SGMatrix<T>::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<T>::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<typename T>
+inline
+T
+norm1(const SGMatrix<T>& m)
+{
+ T nrm = 0;
+ for (unsigned i = 0; i < SGMatrix<T>::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<typename T>
+inline
+T
+normInf(const SGMatrix<T>& m)
+{
+ T nrm = 0;
+ for (unsigned i = 0; i < SGMatrix<T>::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<typename T>
+inline
+bool
+operator==(const SGMatrix<T>& m1, const SGMatrix<T>& m2)
+{
+ for (unsigned i = 0; i < SGMatrix<T>::nEnts; ++i)
+ if (m1[i] != m2[i])
+ return false;
+ return true;
+}
+
+/// Return true if not exactly the same
+template<typename T>
+inline
+bool
+operator!=(const SGMatrix<T>& m1, const SGMatrix<T>& m2)
+{ return ! (m1 == m2); }
+
+/// Return true if equal to the relative tolerance tol
+template<typename T>
+inline
+bool
+equivalent(const SGMatrix<T>& m1, const SGMatrix<T>& 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<typename T>
+inline
+bool
+equivalent(const SGMatrix<T>& m1, const SGMatrix<T>& m2, T rtol)
+{ return norm1(m1 - m2) < rtol*(norm1(m1) + norm1(m2)); }
+
+/// Return true if about equal to roundoff of the underlying type
+template<typename T>
+inline
+bool
+equivalent(const SGMatrix<T>& m1, const SGMatrix<T>& m2)
+{
+ T tol = 100*SGLimits<T>::epsilon();
+ return equivalent(m1, m2, tol, tol);
+}
+
+#ifndef NDEBUG
+template<typename T>
+inline
+bool
+isNaN(const SGMatrix<T>& m)
+{
+ for (unsigned i = 0; i < SGMatrix<T>::nEnts; ++i) {
+ if (SGMisc<T>::isNaN(m[i]))
+ return true;
+ }
+ return false;
+}
+#endif
+
+/// Output to an ostream
+template<typename char_type, typename traits_type, typename T>
+inline
+std::basic_ostream<char_type, traits_type>&
+operator<<(std::basic_ostream<char_type, traits_type>& s, const SGMatrix<T>& 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<float> SGMatrixf;
+typedef SGMatrix<double> 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
--- /dev/null
+#ifndef SGMisc_H
+#define SGMisc_H
+
+#include <cmath>
+
+template<typename T>
+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<T>::min())
+ return -1;
+ else if (SGLimits<T>::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<float> SGMiscf;
+typedef SGMisc<double> SGMiscd;
+
+#endif
--- /dev/null
+#ifndef SGQuat_H
+#define SGQuat_H
+
+/// 3D Vector Class
+template<typename T>
+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<T>::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<T>(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<value_type>::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<T>& 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<T>& axis)
+ {
+ T nAxis = norm(axis);
+ if (nAxis <= SGLimits<T>::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<T>& 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<value_type>::min() &&
+ fabs(num) < SGLimits<value_type>::min())
+ xRad = 0;
+ else
+ xRad = atan2(num, den);
+
+ value_type tmp = 2*(x()*z() - w()*y());
+ if (tmp < -1)
+ yRad = 0.5*SGMisc<value_type>::pi();
+ else if (1 < tmp)
+ yRad = -0.5*SGMisc<value_type>::pi();
+ else
+ yRad = -asin(tmp);
+
+ num = 2*(x()*y() + w()*z());
+ den = sqrQW + sqrQX - sqrQY - sqrQZ;
+ if (fabs(den) < SGLimits<value_type>::min() &&
+ fabs(num) < SGLimits<value_type>::min())
+ zRad = 0;
+ else {
+ value_type psi = atan2(num, den);
+ if (psi < 0)
+ psi += 2*SGMisc<value_type>::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<value_type>::pi();
+ yDeg *= 180/SGMisc<value_type>::pi();
+ xDeg *= 180/SGMisc<value_type>::pi();
+ }
+
+ /// write the angle axis representation into the references
+ void getAngleAxis(T& angle, SGVec3<T>& axis) const
+ {
+ T nrm = norm(*this);
+ if (nrm < SGLimits<T>::min()) {
+ angle = 0;
+ axis = SGVec3<T>(0, 0, 0);
+ } else {
+ T rNrm = 1/nrm;
+ angle = acos(SGMisc<T>::max(-1, SGMisc<T>::min(1, rNrm*w())));
+ T sAng = sin(angle);
+ if (fabs(sAng) < SGLimits<T>::min())
+ axis = SGVec3<T>(1, 0, 0);
+ else
+ axis = (rNrm/sAng)*imag(*this);
+ angle *= 2;
+ }
+ }
+
+ /// write the angle axis representation into the references
+ void getAngleAxis(SGVec3<T>& 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<typename S>
+ 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<typename S>
+ 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<T> transform(const SGVec3<T>& v) const
+ {
+ value_type r = 2/dot(*this, *this);
+ SGVec3<T> 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<T> backTransform(const SGVec3<T>& v) const
+ {
+ value_type r = 2/dot(*this, *this);
+ SGVec3<T> 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<T> rotate(const SGVec3<T>& v) const
+ { return backTransform(v); }
+ /// Rotate a given vector with the inverse quaternion
+ SGVec3<T> rotateBack(const SGVec3<T>& v) const
+ { return transform(v); }
+
+ /// Return the time derivative of the quaternion given the angular velocity
+ SGQuat
+ derivative(const SGVec3<T>& 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<typename T>
+inline
+const SGQuat<T>&
+operator+(const SGQuat<T>& v)
+{ return v; }
+
+/// Unary -, do nearly nothing
+template<typename T>
+inline
+SGQuat<T>
+operator-(const SGQuat<T>& v)
+{ return SGQuat<T>(-v(0), -v(1), -v(2), -v(3)); }
+
+/// Binary +
+template<typename T>
+inline
+SGQuat<T>
+operator+(const SGQuat<T>& v1, const SGQuat<T>& v2)
+{ return SGQuat<T>(v1(0)+v2(0), v1(1)+v2(1), v1(2)+v2(2), v1(3)+v2(3)); }
+
+/// Binary -
+template<typename T>
+inline
+SGQuat<T>
+operator-(const SGQuat<T>& v1, const SGQuat<T>& v2)
+{ return SGQuat<T>(v1(0)-v2(0), v1(1)-v2(1), v1(2)-v2(2), v1(3)-v2(3)); }
+
+/// Scalar multiplication
+template<typename S, typename T>
+inline
+SGQuat<T>
+operator*(S s, const SGQuat<T>& v)
+{ return SGQuat<T>(s*v(0), s*v(1), s*v(2), s*v(3)); }
+
+/// Scalar multiplication
+template<typename S, typename T>
+inline
+SGQuat<T>
+operator*(const SGQuat<T>& v, S s)
+{ return SGQuat<T>(s*v(0), s*v(1), s*v(2), s*v(3)); }
+
+/// Quaterion multiplication
+template<typename T>
+inline
+SGQuat<T>
+operator*(const SGQuat<T>& v1, const SGQuat<T>& v2)
+{
+ SGQuat<T> 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<typename T>
+inline
+SGQuat<T>&
+SGQuat<T>::operator*=(const SGQuat& v)
+{ (*this) = (*this)*v; return *this; }
+
+/// The conjugate of the quaternion, this is also the
+/// inverse for normalized quaternions
+template<typename T>
+inline
+SGQuat<T>
+conj(const SGQuat<T>& v)
+{ return SGQuat<T>(-v(0), -v(1), -v(2), v(3)); }
+
+/// The conjugate of the quaternion, this is also the
+/// inverse for normalized quaternions
+template<typename T>
+inline
+SGQuat<T>
+inverse(const SGQuat<T>& v)
+{ return (1/dot(v, v))*SGQuat<T>(-v(0), -v(1), -v(2), v(3)); }
+
+/// The imagniary part of the quaternion
+template<typename T>
+inline
+T
+real(const SGQuat<T>& v)
+{ return v.w(); }
+
+/// The imagniary part of the quaternion
+template<typename T>
+inline
+SGVec3<T>
+imag(const SGQuat<T>& v)
+{ return SGVec3<T>(v.x(), v.y(), v.z()); }
+
+/// Scalar dot product
+template<typename T>
+inline
+T
+dot(const SGQuat<T>& v1, const SGQuat<T>& 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<typename T>
+inline
+T
+norm(const SGQuat<T>& v)
+{ return sqrt(dot(v, v)); }
+
+/// The euclidean norm of the vector, that is what most people call length
+template<typename T>
+inline
+T
+length(const SGQuat<T>& 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<typename T>
+inline
+T
+norm1(const SGQuat<T>& 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<typename T>
+inline
+SGQuat<T>
+normalize(const SGQuat<T>& q)
+{ return (1/norm(q))*q; }
+
+/// Return true if exactly the same
+template<typename T>
+inline
+bool
+operator==(const SGQuat<T>& v1, const SGQuat<T>& 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<typename T>
+inline
+bool
+operator!=(const SGQuat<T>& v1, const SGQuat<T>& 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<typename T>
+inline
+bool
+equivalent(const SGQuat<T>& v1, const SGQuat<T>& 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<typename T>
+inline
+bool
+equivalent(const SGQuat<T>& v1, const SGQuat<T>& v2)
+{ return equivalent(v1, v2, 100*SGLimits<T>::epsilon()); }
+
+#ifndef NDEBUG
+template<typename T>
+inline
+bool
+isNaN(const SGQuat<T>& v)
+{
+ return SGMisc<T>::isNaN(v(0)) || SGMisc<T>::isNaN(v(1))
+ || SGMisc<T>::isNaN(v(2)) || SGMisc<T>::isNaN(v(3));
+}
+#endif
+
+/// quaternion interpolation for t in [0,1] interpolate between src (=0)
+/// and dst (=1)
+template<typename T>
+inline
+SGQuat<T>
+interpolate(T t, const SGQuat<T>& src, const SGQuat<T>& dst)
+{
+ T cosPhi = dot(src, dst);
+ // need to take the shorter way ...
+ int signCosPhi = SGMisc<T>::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<T>::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<typename char_type, typename traits_type, typename T>
+inline
+std::basic_ostream<char_type, traits_type>&
+operator<<(std::basic_ostream<char_type, traits_type>& s, const SGQuat<T>& v)
+{ return s << "[ " << v(0) << ", " << v(1) << ", " << v(2) << ", " << v(3) << " ]"; }
+
+/// Two classes doing actually the same on different types
+typedef SGQuat<float> SGQuatf;
+typedef SGQuat<double> 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
--- /dev/null
+#ifndef SGVec3_H
+#define SGVec3_H
+
+/// 3D Vector Class
+template<typename T>
+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<T>::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<typename S>
+ SGVec3& operator*=(S s)
+ { _data[0] *= s; _data[1] *= s; _data[2] *= s; return *this; }
+ /// Inplace scalar multiplication by 1/s
+ template<typename S>
+ 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<typename T>
+inline
+const SGVec3<T>&
+operator+(const SGVec3<T>& v)
+{ return v; }
+
+/// Unary -, do nearly nothing
+template<typename T>
+inline
+SGVec3<T>
+operator-(const SGVec3<T>& v)
+{ return SGVec3<T>(-v(0), -v(1), -v(2)); }
+
+/// Binary +
+template<typename T>
+inline
+SGVec3<T>
+operator+(const SGVec3<T>& v1, const SGVec3<T>& v2)
+{ return SGVec3<T>(v1(0)+v2(0), v1(1)+v2(1), v1(2)+v2(2)); }
+
+/// Binary -
+template<typename T>
+inline
+SGVec3<T>
+operator-(const SGVec3<T>& v1, const SGVec3<T>& v2)
+{ return SGVec3<T>(v1(0)-v2(0), v1(1)-v2(1), v1(2)-v2(2)); }
+
+/// Scalar multiplication
+template<typename S, typename T>
+inline
+SGVec3<T>
+operator*(S s, const SGVec3<T>& v)
+{ return SGVec3<T>(s*v(0), s*v(1), s*v(2)); }
+
+/// Scalar multiplication
+template<typename S, typename T>
+inline
+SGVec3<T>
+operator*(const SGVec3<T>& v, S s)
+{ return SGVec3<T>(s*v(0), s*v(1), s*v(2)); }
+
+/// Scalar dot product
+template<typename T>
+inline
+T
+dot(const SGVec3<T>& v1, const SGVec3<T>& 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<typename T>
+inline
+T
+norm(const SGVec3<T>& v)
+{ return sqrt(dot(v, v)); }
+
+/// The euclidean norm of the vector, that is what most people call length
+template<typename T>
+inline
+T
+length(const SGVec3<T>& 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<typename T>
+inline
+T
+norm1(const SGVec3<T>& v)
+{ return fabs(v(0)) + fabs(v(1)) + fabs(v(2)); }
+
+/// Vector cross product
+template<typename T>
+inline
+SGVec3<T>
+cross(const SGVec3<T>& v1, const SGVec3<T>& v2)
+{
+ return SGVec3<T>(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<typename T>
+inline
+SGVec3<T>
+normalize(const SGVec3<T>& v)
+{ return (1/norm(v))*v; }
+
+/// Return true if exactly the same
+template<typename T>
+inline
+bool
+operator==(const SGVec3<T>& v1, const SGVec3<T>& v2)
+{ return v1(0) == v2(0) && v1(1) == v2(1) && v1(2) == v2(2); }
+
+/// Return true if not exactly the same
+template<typename T>
+inline
+bool
+operator!=(const SGVec3<T>& v1, const SGVec3<T>& v2)
+{ return ! (v1 == v2); }
+
+/// Return true if equal to the relative tolerance tol
+template<typename T>
+inline
+bool
+equivalent(const SGVec3<T>& v1, const SGVec3<T>& 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<typename T>
+inline
+bool
+equivalent(const SGVec3<T>& v1, const SGVec3<T>& v2, T rtol)
+{ return norm1(v1 - v2) < rtol*(norm1(v1) + norm1(v2)); }
+
+/// Return true if about equal to roundoff of the underlying type
+template<typename T>
+inline
+bool
+equivalent(const SGVec3<T>& v1, const SGVec3<T>& v2)
+{
+ T tol = 100*SGLimits<T>::epsilon();
+ return equivalent(v1, v2, tol, tol);
+}
+
+#ifndef NDEBUG
+template<typename T>
+inline
+bool
+isNaN(const SGVec3<T>& v)
+{
+ return SGMisc<T>::isNaN(v(0)) ||
+ SGMisc<T>::isNaN(v(1)) || SGMisc<T>::isNaN(v(2));
+}
+#endif
+
+/// Output to an ostream
+template<typename char_type, typename traits_type, typename T>
+inline
+std::basic_ostream<char_type, traits_type>&
+operator<<(std::basic_ostream<char_type, traits_type>& s, const SGVec3<T>& v)
+{ return s << "[ " << v(0) << ", " << v(1) << ", " << v(2) << " ]"; }
+
+/// Two classes doing actually the same on different types
+typedef SGVec3<float> SGVec3f;
+typedef SGVec3<double> 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
--- /dev/null
+#ifndef SGVec4_H
+#define SGVec4_H
+
+/// 3D Vector Class
+template<typename T>
+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<T>::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<typename S>
+ 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<typename S>
+ 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<typename T>
+inline
+const SGVec4<T>&
+operator+(const SGVec4<T>& v)
+{ return v; }
+
+/// Unary -, do nearly nothing
+template<typename T>
+inline
+SGVec4<T>
+operator-(const SGVec4<T>& v)
+{ return SGVec4<T>(-v(0), -v(1), -v(2), -v(3)); }
+
+/// Binary +
+template<typename T>
+inline
+SGVec4<T>
+operator+(const SGVec4<T>& v1, const SGVec4<T>& v2)
+{ return SGVec4<T>(v1(0)+v2(0), v1(1)+v2(1), v1(2)+v2(2), v1(3)+v2(3)); }
+
+/// Binary -
+template<typename T>
+inline
+SGVec4<T>
+operator-(const SGVec4<T>& v1, const SGVec4<T>& v2)
+{ return SGVec4<T>(v1(0)-v2(0), v1(1)-v2(1), v1(2)-v2(2), v1(3)-v2(3)); }
+
+/// Scalar multiplication
+template<typename S, typename T>
+inline
+SGVec4<T>
+operator*(S s, const SGVec4<T>& v)
+{ return SGVec4<T>(s*v(0), s*v(1), s*v(2), s*v(3)); }
+
+/// Scalar multiplication
+template<typename S, typename T>
+inline
+SGVec4<T>
+operator*(const SGVec4<T>& v, S s)
+{ return SGVec4<T>(s*v(0), s*v(1), s*v(2), s*v(3)); }
+
+/// Scalar dot product
+template<typename T>
+inline
+T
+dot(const SGVec4<T>& v1, const SGVec4<T>& 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<typename T>
+inline
+T
+norm(const SGVec4<T>& v)
+{ return sqrt(dot(v, v)); }
+
+/// The euclidean norm of the vector, that is what most people call length
+template<typename T>
+inline
+T
+length(const SGVec4<T>& 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<typename T>
+inline
+T
+norm1(const SGVec4<T>& 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<typename T>
+inline
+SGVec4<T>
+normalize(const SGVec4<T>& v)
+{ return (1/norm(v))*v; }
+
+/// Return true if exactly the same
+template<typename T>
+inline
+bool
+operator==(const SGVec4<T>& v1, const SGVec4<T>& 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<typename T>
+inline
+bool
+operator!=(const SGVec4<T>& v1, const SGVec4<T>& v2)
+{ return ! (v1 == v2); }
+
+/// Return true if equal to the relative tolerance tol
+template<typename T>
+inline
+bool
+equivalent(const SGVec4<T>& v1, const SGVec4<T>& 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<typename T>
+inline
+bool
+equivalent(const SGVec4<T>& v1, const SGVec4<T>& v2, T rtol)
+{ return norm1(v1 - v2) < rtol*(norm1(v1) + norm1(v2)); }
+
+/// Return true if about equal to roundoff of the underlying type
+template<typename T>
+inline
+bool
+equivalent(const SGVec4<T>& v1, const SGVec4<T>& v2)
+{
+ T tol = 100*SGLimits<T>::epsilon();
+ return equivalent(v1, v2, tol, tol);
+}
+
+#ifndef NDEBUG
+template<typename T>
+inline
+bool
+isNaN(const SGVec4<T>& v)
+{
+ return SGMisc<T>::isNaN(v(0)) || SGMisc<T>::isNaN(v(1))
+ || SGMisc<T>::isNaN(v(2)) || SGMisc<T>::isNaN(v(3));
+}
+#endif
+
+/// Output to an ostream
+template<typename char_type, typename traits_type, typename T>
+inline
+std::basic_ostream<char_type, traits_type>&
+operator<<(std::basic_ostream<char_type, traits_type>& s, const SGVec4<T>& v)
+{ return s << "[ " << v(0) << ", " << v(1) << ", " << v(2) << ", " << v(3) << " ]"; }
+
+/// Two classes doing actually the same on different types
+typedef SGVec4<float> SGVec4f;
+typedef SGVec4<double> 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
# include <math.h>
#endif
-#include <simgear/math/localconsts.hxx>
+#include "SGMath.hxx"
// I don't understand ... <math.h> or <cmath> should be included
// already depending on how you defined SG_HAVE_STD_INCLUDES, but I
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<double>& cart);
+
// Assignment operators
Point3D& operator = ( const Point3D& p ); // assignment of a Point3D
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?
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<double>& 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)
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
#include <math.h>
-#include <stdio.h>
#include <simgear/constants.h>
#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
#endif
-#include <math.h>
-
#include <simgear/constants.h>
#include <simgear/math/point3d.hxx>
+#include "SGMath.hxx"
/**
* Find the Altitude above the Ellipsoid (WGS84) given the Earth
* @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<double>(p.x(), p.y(), p.z()), geod);
+ return geod.getElevationM();
+}
/**
* @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<double> cart;
+ SGGeodesy::SGGeocToCart(SGGeoc::fromRadM(p.lon(), p.lat(), p.radius()), cart);
+ return Point3D::fromSGVec3(cart);
+}
/**
* @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<double>(p.x(), p.y(), p.z()), geoc);
+ return Point3D::fromSGGeoc(geoc);
+}
/**
#include <simgear/constants.h>
+#include "SGMath.hxx"
#include "sg_geodesy.hxx"
// Notes:
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
#define _SG_GEODESY_HXX
#include <simgear/math/point3d.hxx>
+#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 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<double> 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.
* @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<double> 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.
* @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<double>(xyz), geod);
+ *lat = geod.getLatitudeRad();
+ *lon = geod.getLongitudeRad();
+ *alt = geod.getElevationM();
+}
/**
* Convert a cartesian point to a geodetic lat/lon/altitude.
*/
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<double>(p.x(), p.y(), p.z()), geod);
+ return Point3D::fromSGGeod(geod);
}
* @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<double> 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.
*/
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<double> cart;
+ SGGeodesy::SGGeodToCart(SGGeod::fromRadM(geod.lon(), geod.lat(), geod.elev()), cart);
+ return Point3D::fromSGVec3(cart);
}
/**
projects / simgear.git / commitdiff