Small cleanups to the SGGeo[dc] classes, provide more hooks to use them directly

simgear/math/SGGeoc.hxx

@@ -29,9 +29,11 @@ public:
   SGGeoc(void);
   /// Initialize from a cartesian vector assumed to be in meters
   /// Note that this conversion is relatively expensive to compute
+  /// depricated
   SGGeoc(const SGVec3<double>& cart);
   /// Initialize from a geodetic position
   /// Note that this conversion is relatively expensive to compute
+  /// depricated
   SGGeoc(const SGGeod& geod);
 
   /// Factory from angular values in radians and radius in ft
@@ -42,6 +44,13 @@ public:
   static SGGeoc fromRadM(double lon, double lat, double radius);
   /// Factory from angular values in degrees and radius in m
   static SGGeoc fromDegM(double lon, double lat, double radius);
+  /// Factory to convert position from a cartesian position assumed to be
+  /// in wgs84 measured in meters
+  /// Note that this conversion is relatively expensive to compute
+  static SGGeoc fromCart(const SGVec3<double>& cart);
+  /// Factory to convert position from a geodetic position
+  /// Note that this conversion is relatively expensive to compute
+  static SGGeoc fromGeod(const SGGeod& geod);
 
   /// Return the geocentric longitude in radians
   double getLongitudeRad(void) const;
@@ -165,6 +174,26 @@ SGGeoc::fromDegM(double lon, double lat, double radius)
 #endif
 }
 
+inline
+SGGeoc
+SGGeoc::fromCart(const SGVec3<double>& cart)
+{
+  SGGeoc geoc;
+  SGGeodesy::SGCartToGeoc(cart, geoc);
+  return geoc;
+}
+
+inline
+SGGeoc
+SGGeoc::fromGeod(const SGGeod& geod)
+{
+  SGVec3<double> cart;
+  SGGeodesy::SGGeodToCart(geod, cart);
+  SGGeoc geoc;
+  SGGeodesy::SGCartToGeoc(cart, geoc);
+  return geoc;
+}
+
 inline
 double
 SGGeoc::getLongitudeRad(void) const

simgear/math/SGGeod.hxx

@@ -29,9 +29,11 @@ public:
   SGGeod(void);
   /// Initialize from a cartesian vector assumed to be in meters
   /// Note that this conversion is relatively expensive to compute
+  /// depricated
   SGGeod(const SGVec3<double>& cart);
   /// Initialize from a geocentric position
   /// Note that this conversion is relatively expensive to compute
+  /// depricated
   SGGeod(const SGGeoc& geoc);
 
   /// Factory from angular values in radians and elevation is 0
@@ -46,6 +48,13 @@ public:
   static SGGeod fromRadM(double lon, double lat, double elevation);
   /// Factory from angular values in degrees and elevation in m
   static SGGeod fromDegM(double lon, double lat, double elevation);
+  /// Factory to convert position from a cartesian position assumed to be
+  /// in wgs84 measured in meters
+  /// Note that this conversion is relatively expensive to compute
+  static SGGeod fromCart(const SGVec3<double>& cart);
+  /// Factory to convert position from a geocentric position
+  /// Note that this conversion is relatively expensive to compute
+  static SGGeod fromGeoc(const SGGeoc& geoc);
 
   /// Return the geodetic longitude in radians
   double getLongitudeRad(void) const;
@@ -191,6 +200,26 @@ SGGeod::fromDegM(double lon, double lat, double elevation)
 #endif
 }
 
+inline
+SGGeod
+SGGeod::fromCart(const SGVec3<double>& cart)
+{
+  SGGeod geod;
+  SGGeodesy::SGCartToGeod(cart, geod);
+  return geod;
+}
+
+inline
+SGGeod
+SGGeod::fromGeoc(const SGGeoc& geoc)
+{
+  SGVec3<double> cart;
+  SGGeodesy::SGGeocToCart(geoc, cart);
+  SGGeod geod;
+  SGGeodesy::SGCartToGeod(cart, geod);
+  return geod;
+}
+
 inline
 double
 SGGeod::getLongitudeRad(void) const

simgear/math/SGMath.hxx

@@ -27,9 +27,9 @@
 #include "SGGeodesy.hxx"
 #include "SGVec3.hxx"
 #include "SGVec4.hxx"
-#include "SGQuat.hxx"
-#include "SGMatrix.hxx"
 #include "SGGeoc.hxx"
 #include "SGGeod.hxx"
+#include "SGQuat.hxx"
+#include "SGMatrix.hxx"
 
 #endif

simgear/math/SGMathTest.cxx

@@ -213,16 +213,16 @@ GeodesyTest(void)
   geod0 = SGGeod::fromDegM(30, 20, 17);
 
   // Test the conversion routines to cartesian coordinates
-  cart0 = geod0;
+  cart0 = SGVec3<double>::fromGeod(geod0);
-  geod1 = cart0;
+  geod1 = SGGeod::fromCart(cart0);
   if (epsDeg < fabs(geod0.getLongitudeDeg() - geod1.getLongitudeDeg()) ||
       epsDeg < fabs(geod0.getLatitudeDeg() - geod1.getLatitudeDeg()) ||
       epsM < fabs(geod0.getElevationM() - geod1.getElevationM()))
     return false;
 
   // Test the conversion routines to radial coordinates
-  geoc0 = cart0;
+  geoc0 = SGGeoc::fromCart(cart0);
-  cart1 = geoc0;
+  cart1 = SGVec3<double>::fromGeoc(geoc0);
   if (!equivalent(cart0, cart1))
     return false;
 
@@ -346,8 +346,8 @@ main(void)
     return EXIT_FAILURE;
 
   // Check geodetic/geocentric/cartesian conversions
-//   if (!GeodesyTest())
+  if (!GeodesyTest())
-//     return EXIT_FAILURE;
+    return EXIT_FAILURE;
 
   // Check interaction with sg*/sgd*
   if (!sgInterfaceTest())

simgear/math/SGQuat.hxx

@@ -110,6 +110,11 @@ public:
   static SGQuat fromLonLatDeg(T lon, T lat)
   { return fromLonLatRad(SGMisc<T>::deg2rad(lon), SGMisc<T>::deg2rad(lat)); }
 
+  /// Return a quaternion rotation the the horizontal local frame from given
+  /// longitude and latitude
+  static SGQuat fromLonLat(const SGGeod& geod)
+  { return fromLonLatRad(geod.getLongitudeRad(), geod.getLatitudeRad()); }
+
   /// Create a quaternion from the angle axis representation
   static SGQuat fromAngleAxis(T angle, const SGVec3<T>& axis)
   {

simgear/math/SGVec3.hxx

@@ -45,10 +45,12 @@ public:
   { _data[0] = data[0]; _data[1] = data[1]; _data[2] = data[2]; }
   /// Constructor. Initialize by a geodetic coordinate
   /// Note that this conversion is relatively expensive to compute
+  /// depricated
   SGVec3(const SGGeod& geod)
   { SGGeodesy::SGGeodToCart(geod, *this); }
   /// Constructor. Initialize by a geocentric coordinate
   /// Note that this conversion is relatively expensive to compute
+  /// depricated
   SGVec3(const SGGeoc& geoc)
   { SGGeodesy::SGGeocToCart(geoc, *this); }
 
@@ -125,11 +127,58 @@ public:
   static SGVec3 e3(void)
   { return SGVec3(0, 0, 1); }
 
+  /// Constructor. Initialize by a geodetic coordinate
+  /// Note that this conversion is relatively expensive to compute
+  static SGVec3 fromGeod(const SGGeod& geod);
+  /// Constructor. Initialize by a geocentric coordinate
+  /// Note that this conversion is relatively expensive to compute
+  static SGVec3 fromGeoc(const SGGeoc& geoc);
+
 private:
   /// The actual data
   T _data[3];
 };
 
+template<>
+inline
+SGVec3<double>
+SGVec3<double>::fromGeod(const SGGeod& geod)
+{
+  SGVec3<double> cart;
+  SGGeodesy::SGGeodToCart(geod, cart);
+  return cart;
+}
+
+template<>
+inline
+SGVec3<float>
+SGVec3<float>::fromGeod(const SGGeod& geod)
+{
+  SGVec3<double> cart;
+  SGGeodesy::SGGeodToCart(geod, cart);
+  return SGVec3<float>(cart(0), cart(1), cart(2));
+}
+
+template<>
+inline
+SGVec3<double>
+SGVec3<double>::fromGeoc(const SGGeoc& geoc)
+{
+  SGVec3<double> cart;
+  SGGeodesy::SGGeocToCart(geoc, cart);
+  return cart;
+}
+
+template<>
+inline
+SGVec3<float>
+SGVec3<float>::fromGeoc(const SGGeoc& geoc)
+{
+  SGVec3<double> cart;
+  SGGeodesy::SGGeocToCart(geoc, cart);
+  return SGVec3<float>(cart(0), cart(1), cart(2));
+}
+
 /// Unary +, do nothing ...
 template<typename T>
 inline

simgear/math/sg_geodesy.cxx

@@ -73,10 +73,9 @@ static inline double M0( double e2 ) {
 }
 
 
-// given, alt, lat1, lon1, az1 and distance (s), calculate lat2, lon2
+// given, lat1, lon1, az1 and distance (s), calculate lat2, lon2
 // and az2.  Lat, lon, and azimuth are in degrees.  distance in meters
-int geo_direct_wgs_84 ( double alt, double lat1,
+int geo_direct_wgs_84 ( double lat1, double lon1, double az1,
-                        double lon1, double az1,
                         double s, double *lat2, double *lon2,
                         double *az2 )
 {
@@ -160,16 +159,15 @@ int geo_direct_wgs_84 ( double alt, double lat1,
 	double dM = a*M0(e2) - s;
 	double paz = ( phi1 < 0.0 ? 180.0 : 0.0 );
         double zero = 0.0f;
-	return geo_direct_wgs_84( alt, zero, lon1, paz, dM, lat2, lon2, az2 );
+	return geo_direct_wgs_84( zero, lon1, paz, dM, lat2, lon2, az2 );
     } 
 }
 
 
-// given alt, lat1, lon1, lat2, lon2, calculate starting and ending
+// given lat1, lon1, lat2, lon2, calculate starting and ending
 // az1, az2 and distance (s).  Lat, lon, and azimuth are in degrees.
 // distance in meters
-int geo_inverse_wgs_84( double alt, double lat1,
+int geo_inverse_wgs_84( double lat1, double lon1, double lat2,
-                        double lon1, double lat2,
 			double lon2, double *az1, double *az2,
                         double *s )
 {
@@ -192,14 +190,14 @@ int geo_inverse_wgs_84( double alt, double lat1,
 	return 0;
     } else if(  fabs(cosphi1) < testv ) {
 	// initial point is polar
-	int k = geo_inverse_wgs_84( alt, lat2,lon2,lat1,lon1, az1,az2,s );
+	int k = geo_inverse_wgs_84( lat2,lon2,lat1,lon1, az1,az2,s );
 	k = k; // avoid compiler error since return result is unused
 	b = *az1; *az1 = *az2; *az2 = b;
 	return 0;
     } else if( fabs(cosphi2) < testv ) {
 	// terminal point is polar
         double _lon1 = lon1 + 180.0f;
-	int k = geo_inverse_wgs_84( alt, lat1, lon1, lat1, _lon1, 
+	int k = geo_inverse_wgs_84( lat1, lon1, lat1, _lon1, 
 				    az1, az2, s );
 	k = k; // avoid compiler error since return result is unused
 	*s /= 2.0;
@@ -211,8 +209,8 @@ int geo_inverse_wgs_84( double alt, double lat1,
     {
 	// Geodesic passes through the pole (antipodal)
 	double s1,s2;
-	geo_inverse_wgs_84( alt, lat1,lon1, lat1,lon2, az1,az2, &s1 );
+	geo_inverse_wgs_84( lat1,lon1, lat1,lon2, az1,az2, &s1 );
-	geo_inverse_wgs_84( alt, lat2,lon2, lat1,lon2, az1,az2, &s2 );
+	geo_inverse_wgs_84( lat2,lon2, lat1,lon2, az1,az2, &s2 );
 	*az2 = *az1;
 	*s = s1 + s2;
 	return 0;

simgear/math/sg_geodesy.hxx

@@ -4,18 +4,6 @@
 #include <simgear/math/point3d.hxx>
 #include "SGMath.hxx"
 
-// Returns the insersection of the line joining the center of the
-// earth and the specified cylindrical point with the surface of the
-// WGS84 ellipsoid.  Works by finding a normalization constant (in
-// squashed space) that places the squashed point on the surface of
-// the sphere.
-inline double seaLevelRadius(double r, double z)
-{
-    double sr = r * SGGeodesy::SQUASH;
-    double zz = z*z;
-    return SGGeodesy::POLRAD*sqrt((r*r + zz)/(sr*sr + zz));
-}
-
 /** 
  * Convert from geocentric coordinates to geodetic coordinates
  * @param lat_geoc (in) Geocentric latitude, radians, + = North
@@ -56,11 +44,12 @@ inline void sgGeodToGeoc(double lat_geod, double alt,
                          double *sl_radius, double *lat_geoc)
 {
   SGVec3<double> cart;
-  SGGeodesy::SGGeodToCart(SGGeod::fromRadM(0, lat_geod, alt), cart);
+  SGGeod geod = SGGeod::fromRadM(0, lat_geod, alt);
+  SGGeodesy::SGGeodToCart(geod, cart);
   SGGeoc geoc;
   SGGeodesy::SGCartToGeoc(cart, geoc);
   *lat_geoc = geoc.getLatitudeRad();
-  *sl_radius = seaLevelRadius(cart(0), cart(2));
+  *sl_radius = SGGeodesy::SGGeodToSeaLevelRadius(geod);
 }
 
 
@@ -130,7 +119,7 @@ inline Point3D sgGeodToCart(const Point3D& geod)
 /**
  * Given a starting position and an offset radial and distance,
  * calculate an ending positon on a wgs84 ellipsoid.
- * @param alt (in) meters
+ * @param alt (in) meters (unused)
  * @param lat1 (in) degrees
  * @param lon1 (in) degrees
  * @param az1 (in) degrees
@@ -139,16 +128,39 @@ inline Point3D sgGeodToCart(const Point3D& geod)
  * @param lon2 (out) degrees
  * @param az2 (out) return course in degrees
  */
-int geo_direct_wgs_84 ( double alt, double lat1,
+int geo_direct_wgs_84 ( double lat1, double lon1, double az1, 
-                        double lon1, double az1, 
 			double s, double *lat2, double *lon2,
                         double *az2 );
+inline int geo_direct_wgs_84 ( double alt, double lat1,
+                        double lon1, double az1, 
+			double s, double *lat2, double *lon2,
+                        double *az2 )
+{ return geo_direct_wgs_84(lat1, lon1, az1, s, lat2, lon2, az2); }
 
+/**
+ * Given a starting position and an offset radial and distance,
+ * calculate an ending positon on a wgs84 ellipsoid.
+ * @param p1 (in) geodetic position
+ * @param az1 (in) degrees
+ * @param s (in) distance in meters
+ * @param p2 (out) geodetic position
+ * @param az2 (out) return course in degrees
+ */
+inline int geo_direct_wgs_84(const SGGeod& p1, double az1,
+                             double s, SGGeod& p2, double *az2 )
+{
+  double lat2, lon2;
+  int ret = geo_direct_wgs_84(p1.getLatitudeDeg(), p1.getLongitudeDeg(),
+                              az1, s, &lat2, &lon2, az2);
+  p2.setLatitudeDeg(lat2);
+  p2.setLongitudeDeg(lon2);
+  return ret;
+}
 
 /**
  * Given an altitude and two sets of (lat, lon) calculate great circle
  * distance between them as well as the starting and ending azimuths.
- * @param alt (in) meters
+ * @param alt (in) meters (unused)
  * @param lat1 (in) degrees
  * @param lon1 (in) degrees
  * @param lat2 (in) degrees
@@ -157,9 +169,31 @@ int geo_direct_wgs_84 ( double alt, double lat1,
  * @param az2 (out) end heading degrees
  * @param s (out) distance meters
  */
-int geo_inverse_wgs_84( double alt, double lat1,
+int geo_inverse_wgs_84( double lat1, double lon1, double lat2,
-                        double lon1, double lat2,
 			double lon2, double *az1, double *az2,
                         double *s );
+inline int geo_inverse_wgs_84( double alt, double lat1,
+                               double lon1, double lat2,
+                               double lon2, double *az1, double *az2,
+                               double *s )
+{ return geo_inverse_wgs_84(lat1, lon1, lat2, lon2, az1, az2, s); }
+
+
+/**
+ * Given an altitude and two sets of (lat, lon) calculate great circle
+ * distance between them as well as the starting and ending azimuths.
+ * @param p1 (in) first position
+ * @param p2 (in) fsecond position
+ * @param az1 (out) start heading degrees
+ * @param az2 (out) end heading degrees
+ * @param s (out) distance meters
+ */
+inline int geo_inverse_wgs_84(const SGGeod& p1, const SGGeod& p2,
+                              double *az1, double *az2, double *s )
+{
+  return geo_inverse_wgs_84(p1.getLatitudeDeg(), p1.getLongitudeDeg(),
+                            p2.getLatitudeDeg(), p2.getLongitudeDeg(),
+                            az1, az2, s);
+}
 
 #endif // _SG_GEODESY_HXX

simgear/scene/model/placement.cxx

@@ -102,6 +102,14 @@ SGModelPlacement::setPosition (double lon_deg, double lat_deg, double elev_ft)
   _elev_ft = elev_ft;
 }
 
+void
+SGModelPlacement::setPosition(const SGGeod& position)
+{
+  _lon_deg = position.getLongitudeDeg();
+  _lat_deg = position.getLatitudeDeg();
+  _elev_ft = position.getElevationFt();
+}
+
 void
 SGModelPlacement::setRollDeg (double roll_deg)
 {
@@ -129,4 +137,10 @@ SGModelPlacement::setOrientation (double roll_deg, double pitch_deg,
   _heading_deg = heading_deg;
 }
 
+void
+SGModelPlacement::setOrientation (const SGQuatd& orientation)
+{
+  orientation.getEulerDeg(_heading_deg, _pitch_deg, _roll_deg);
+}
+
 // end of model.cxx

simgear/scene/model/placement.hxx

@@ -66,6 +66,7 @@ public:
   virtual void setLatitudeDeg (double lat_deg);
   virtual void setElevationFt (double elev_ft);
   virtual void setPosition (double lon_deg, double lat_deg, double elev_ft);
+  void setPosition(const SGGeod& position);
 
   virtual double getRollDeg () const { return _roll_deg; }
   virtual double getPitchDeg () const { return _pitch_deg; }
@@ -76,6 +77,7 @@ public:
   virtual void setHeadingDeg (double heading_deg);
   virtual void setOrientation (double roll_deg, double pitch_deg,
                                double heading_deg);
+  void setOrientation(const SGQuatd& orientation);
   
   ssgPlacementTransform * getTransform(void)
   { return _position; }

simgear/timing/geocoord.cxx

@@ -27,8 +27,8 @@
  * written for FlightGear, in order to store Timezone control points. 
  *
  ************************************************************************/
+#include <simgear/math/SGMath.hxx>
 #include "geocoord.h"
-#include <plib/sg.h>
 
 SGGeoCoord::SGGeoCoord(const SGGeoCoord& other)
 {
@@ -36,57 +36,20 @@ SGGeoCoord::SGGeoCoord(const SGGeoCoord& other)
   lon = other.lon;
 }
 
-// double SGGeoCoord::getAngle(const SGGeoCoord& other) const
-// {
-//   Vector first(      getX(),       getY(),       getZ());
-//   Vector secnd(other.getX(), other.getY(), other.getZ());
-//     double
-//       dot = VecDot(first, secnd),
-//       len1 = first.VecLen(),
-//       len2 = secnd.VecLen(),
-//       len = len1 * len2,
-//       angle = 0;
-//     //printf ("Dot: %f, len1: %f len2: %f\n", dot, len1, len2);
-//     /*Vector pPos = prevPos - Reference->prevPos;
-//       Vector pVel = prevVel - Reference->prevVel;*/
-
-
-//     if ( ( (dot / len) < 1) && (dot / len > -1) && len )
-//     	angle = acos(dot / len);
-//     return angle;
-// }
-
-// SGGeoCoord* SGGeoCoordContainer::getNearest(const SGGeoCoord& ref) const
-// {
-//   float angle, maxAngle = 180;
-
-//   SGGeoCoordVectorConstIterator i, nearest;
-//   for (i = data.begin(); i != data.end(); i++)
-//     {
-//       angle = SGD_RADIANS_TO_DEGREES * (*i)->getAngle(ref);
-//       if (angle < maxAngle)
-// 	{
-// 	  maxAngle = angle;
-// 	  nearest = i;
-// 	}
-//     }
-//   return *nearest;
-// }
-
-
 SGGeoCoord* SGGeoCoordContainer::getNearest(const SGGeoCoord& ref) const
 {
-  sgVec3 first, secnd;
+  if (data.empty())
-  float dist, maxDist=SG_MAX;
+    return 0;
-  sgSetVec3( first, ref.getX(), ref.getY(), ref.getZ());
+
+  float maxCosAng = -2;
+  SGVec3f refVec(ref.getX(), ref.getY(), ref.getZ());
   SGGeoCoordVectorConstIterator i, nearest;
-  for (i = data.begin(); i != data.end(); i++)
+  for (i = data.begin(); i != data.end(); ++i)
     {
-      sgSetVec3(secnd, (*i)->getX(), (*i)->getY(), (*i)->getZ());
+      float cosAng = dot(refVec, SGVec3f((*i)->getX(), (*i)->getY(), (*i)->getZ()));
-      dist = sgDistanceSquaredVec3(first, secnd);
+      if (maxCosAng < cosAng)
-      if (dist < maxDist)
 	{
-	  maxDist = dist;
+	  maxCosAng = cosAng;
 	  nearest = i;
 	}
     }

simgear/timing/geocoord.h

@@ -36,10 +36,6 @@
 
 
 #include <math.h>
-#include <string>
-#include STL_IOSTREAM
-//#include <streambuf> // looks like streambuf does not exist on linux.
-// But it looks like it isn't used anyways -:)
 #include <vector>
 
 SG_USING_NAMESPACE(std);
@@ -66,8 +62,6 @@ public:
   float getZ()   const { return sin(SGD_DEGREES_TO_RADIANS*lat); };
 
 
-  //double getAngle(const SGGeoCoord& other) const;
-  virtual void print() {} ; 
   virtual const char * getDescription() {return 0;};
 };
 

simgear/timing/timezone.h

@@ -28,6 +28,7 @@
 #define _TIMEZONE_H_
 
 #include <stdio.h>
+#include <string>
 
 #include <simgear/timing/geocoord.h>
 
@@ -84,11 +85,6 @@ public:
      */
     virtual ~SGTimeZone() { };
   
-    /**
-     * Print the descriptor string
-     */
-    virtual void print() { printf("%s", descriptor.c_str()); }
-
     /**
      * Return the descriptor string
      * @return descriptor string (char array)