From b67858f388be620a562cd8424fe2cb98b7768c01 Mon Sep 17 00:00:00 2001
From: Robert Osfield <robert@openscenegraph.com>
Date: Wed, 31 Mar 2004 15:50:30 +0000
Subject: [PATCH] Added osgTerrain::EllipsodeTransform helper class for
 converting to and from lat, long, height to geocentric X,Y,Z and back.

---
 examples/osgdem/osgdem.cpp          | 18 +++++++
 include/osgTerrain/CoordinateSystem | 82 ++++++++++++++++++++++++-----
 src/osgTerrain/CoordinateSystem.cpp |  6 +--
 3 files changed, 88 insertions(+), 18 deletions(-)

diff --git a/examples/osgdem/osgdem.cpp b/examples/osgdem/osgdem.cpp
index dcb7a8fe6..2c631bb17 100644
--- a/examples/osgdem/osgdem.cpp
+++ b/examples/osgdem/osgdem.cpp
@@ -78,6 +78,24 @@ char *SanitizeSRS( const char *pszUserInput )
 }
 
 
+void ellipsodeTransformTest(double latitude, double longitude, double height)
+{
+    osgTerrain::EllipsodeTransform transform;
+    
+    double X,Y,Z;
+    double newLat, newLong, newHeight;
+    
+    transform.convertLatLongHeightToXYZ(latitude,longitude,height,
+                                        X,Y,Z);
+    
+    transform.convertXYZToLatLongHeight(X,Y,Z,
+                                        newLat,newLong,newHeight);
+                                        
+    std::cout<<"lat = "<<osg::RadiansToDegrees(latitude)<<"\tlong="<<osg::RadiansToDegrees(longitude)<<"\t"<<height<<std::endl;  
+    std::cout<<"X = "<<X<<"\tY="<<Y<<"\tZ="<<Z<<std::endl;  
+    std::cout<<"lat = "<<osg::RadiansToDegrees(newLat)<<"\tlong="<<osg::RadiansToDegrees(newLong)<<"\t"<<newHeight<<std::endl;  
+}
+
 int main( int argc, char **argv )
 {
 
diff --git a/include/osgTerrain/CoordinateSystem b/include/osgTerrain/CoordinateSystem
index 457ec36f0..1779ea9c1 100644
--- a/include/osgTerrain/CoordinateSystem
+++ b/include/osgTerrain/CoordinateSystem
@@ -23,21 +23,81 @@
 namespace osgTerrain
 {
 
+const double WGS_84_RADIUS_EQUATOR = 6378137.0;
+const double WGS_84_RADIUS_POLAR = 6356752.3142;
+
+class EllipsodeTransform
+{
+    public:
+
+        EllipsodeTransform(double radiusEquator = WGS_84_RADIUS_EQUATOR,
+                           double radiusPolar = WGS_84_RADIUS_POLAR):
+            _radiusEquator(radiusEquator),
+            _radiusPolar(radiusPolar) { computeCoefficients(); }
+
+
+        void setRadiusEquator(double radius) { _radiusEquator = radius; computeCoefficients(); }
+        double getRadiusEquator() const { return _radiusEquator; }
+
+        void setRadiusPolar(double radius) { _radiusPolar = radius; computeCoefficients(); }
+        double getRadiusPolar() const { return _radiusPolar; }
+
+        inline void convertLatLongHeightToXYZ(double latitude, double longitude, double height,
+                                              double& X, double& Y, double& Z) const
+        {
+            // for details on maths see http://www.colorado.edu/geography/gcraft/notes/datum/gif/llhxyz.gif
+            double sin_latitude = sin(latitude);
+            double cos_latitude = cos(latitude);
+            double N = _radiusEquator / sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude);
+            X = (N+height)*cos_latitude*cos(longitude);
+            Y = (N+height)*cos_latitude*sin(longitude);
+            Z = (N*(1-_eccentricitySquared)+height)*sin(latitude);
+        }
+
+        inline void convertXYZToLatLongHeight(double X, double Y, double Z,
+                                              double& latitude, double& longitude, double& height) const
+        {
+            // http://www.colorado.edu/geography/gcraft/notes/datum/gif/xyzllh.gif
+            double p = sqrt(X*X + Y*Y);
+            double theta = atan(Z*_radiusEquator/ (p*_radiusPolar));
+            double eDashSquared = (_radiusEquator*_radiusEquator - _radiusPolar*_radiusPolar)/
+                                  (_radiusPolar*_radiusPolar);
+
+            double sin_theta = sin(theta);
+            double cos_theta = cos(theta);
+
+            latitude = atan( (Z + eDashSquared*_radiusPolar*sin_theta*sin_theta*sin_theta) /
+                             (p - _eccentricitySquared*_radiusEquator*cos_theta*cos_theta*cos_theta) );
+            longitude = atan2(Y,X);
+
+            double sin_latitude = sin(latitude);
+            double N = _radiusEquator / sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude);
+
+            height = p/cos(latitude) - N;
+        }
+
+    protected:
+    
+        void computeCoefficients()
+        {
+            double flattening = (_radiusEquator-_radiusPolar)/_radiusEquator;
+            _eccentricitySquared = 2*flattening - flattening*flattening;
+        }
+
+        double _radiusEquator;
+        double _radiusPolar;
+        double _eccentricitySquared;
+
+};
+
 /** CoordinateSystem encapsulate the coordinate system that associated with objects in a scene.*/
 class CoordinateSystem : public osg::Object
 {
     public:
 
-        enum Type
-        {
-            GEOCENTRIC,
-            GEOGRAPHIC,
-            PROJECTED
-        };
-        
         CoordinateSystem();
 
-        CoordinateSystem(const std::string& WTK, Type type=GEOCENTRIC);
+        CoordinateSystem(const std::string& WTK);
 
         /** Copy constructor using CopyOp to manage deep vs shallow copy.*/
         CoordinateSystem(const CoordinateSystem&,const osg::CopyOp& copyop=osg::CopyOp::SHALLOW_COPY);
@@ -47,7 +107,6 @@ class CoordinateSystem : public osg::Object
         inline bool operator == (const CoordinateSystem& cs) const
         {
             if (this == &cs) return true;
-            if (_type!=cs._type) return false;
             if (_WKT != cs._WKT) return false;
             return true;
         }
@@ -57,10 +116,6 @@ class CoordinateSystem : public osg::Object
             return !(*this==cs);
         }
                 
-        void setType(Type type) { _type = type; }
-        Type getType() const { return _type; }
-        
-
         /** Set the CoordinateSystem reference string, should be stored in OpenGIS Well Know Text form.*/
         void setWKT(const std::string& WKT) { _WKT = WKT; }
         
@@ -99,7 +154,6 @@ class CoordinateSystem : public osg::Object
 
         virtual ~CoordinateSystem() {}
                 
-        Type        _type;
         std::string _WKT;
 
 };
diff --git a/src/osgTerrain/CoordinateSystem.cpp b/src/osgTerrain/CoordinateSystem.cpp
index 594230add..6fff2f73c 100644
--- a/src/osgTerrain/CoordinateSystem.cpp
+++ b/src/osgTerrain/CoordinateSystem.cpp
@@ -15,13 +15,11 @@
 
 using namespace osgTerrain;
 
-CoordinateSystem::CoordinateSystem():
-    _type(GEOGRAPHIC)
+CoordinateSystem::CoordinateSystem()
 {
 }
 
-CoordinateSystem::CoordinateSystem(const std::string& WKT, Type type):
-    _type(type),
+CoordinateSystem::CoordinateSystem(const std::string& WKT):
     _WKT(WKT)
 {
 }