Mathias Fr�hlich:

The patch adds a vector lib I have put together during the last time,
it is just handy and interfaces well with s(s)g*. Together with some small
modifications this will later also interface well with OpenSceneGraphs
vectors/matrices. Using this vector kernel is targeted to have a handy
matrix/vector lib available and to provide a scenegraph independent vector
type for use with a small scenegraph wrapper aimed for a smooth transition to
openscenegraph.

That vector code also includes an improoved geodetic conversion routine I
have found some time ago published in the 'journal of geodesy' which avoids
iterative computations for that purpose.

Also the geodetic position class is more typesafe and unitsafe than the
Point3D currently is.

That part is relatively old and in use in my local trees for several months
now.

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)

simgear/math/SGGeoc.hxx

@@ -0,0 +1,278 @@
+#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

simgear/math/SGGeod.hxx

@@ -0,0 +1,305 @@
+#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

simgear/math/SGGeodesy.cxx

@@ -0,0 +1,136 @@
+#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);
+}

simgear/math/SGGeodesy.hxx

@@ -0,0 +1,39 @@
+#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

simgear/math/SGLimits.hxx

@@ -0,0 +1,15 @@
+#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

simgear/math/SGMath.hxx

@@ -0,0 +1,18 @@
+#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

simgear/math/SGMathTest.cxx

@@ -0,0 +1,339 @@
+#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;
+}

simgear/math/SGMatrix.hxx

@@ -0,0 +1,581 @@
+#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

simgear/math/SGMisc.hxx

@@ -0,0 +1,55 @@
+#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

simgear/math/SGQuat.hxx

@@ -0,0 +1,521 @@
+#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

simgear/math/SGVec3.hxx

@@ -0,0 +1,268 @@
+#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

simgear/math/SGVec4.hxx

@@ -0,0 +1,259 @@
+#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

simgear/math/point3d.hxx

@@ -52,7 +52,7 @@
 # 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
@@ -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<double>& 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<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)
@@ -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
 

simgear/math/polar3d.cxx

@@ -23,74 +23,11 @@
 
 
 #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

simgear/math/polar3d.hxx

@@ -34,11 +34,10 @@
 #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
@@ -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<double>(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<double> 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<double>(p.x(), p.y(), p.z()), geoc);
+  return Point3D::fromSGGeoc(geoc);
+}
 
 
 /**

simgear/math/sg_geodesy.cxx

@@ -1,4 +1,5 @@
 #include <simgear/constants.h>
+#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 

simgear/math/sg_geodesy.hxx

@@ -2,6 +2,19 @@
 #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
@@ -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<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.
@@ -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<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.
@@ -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<double>(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<double>(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<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.
@@ -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<double> cart;
+  SGGeodesy::SGGeodToCart(SGGeod::fromRadM(geod.lon(), geod.lat(), geod.elev()), cart);
+  return Point3D::fromSGVec3(cart);
 }
 
 /**