fix

Author: Davide Faconti

include/geodetic_utils/geodetic_conv.hpp

@@ -12,17 +12,33 @@ static double kFirstEccentricitySquared = 6.69437999014 * 0.001;
 static double kSecondEccentricitySquared = 6.73949674228 * 0.001;
 static double kFlattening = 1 / 298.257223563;
 
+
+inline double rad2Deg(const double radians)
+{
+  return (radians / M_PI) * 180.0;
+}
+
+inline double deg2Rad(const double degrees)
+{
+  return (degrees / 180.0) * M_PI;
+}
+
 inline void Geodetic2Ecef(const double latitude, const double longitude, const double altitude,
                           double* x, double* y, double* z)
 {
   // Convert geodetic coordinates to ECEF.
   // http://code.google.com/p/pysatel/source/browse/trunk/coord.py?r=22
-  double lat_rad = deg2Rad(latitude);
-  double lon_rad = deg2Rad(longitude);
-  double xi = sqrt(1 - kFirstEccentricitySquared * sin(lat_rad) * sin(lat_rad));
-  *x = (kSemimajorAxis / xi + altitude) * cos(lat_rad) * cos(lon_rad);
-  *y = (kSemimajorAxis / xi + altitude) * cos(lat_rad) * sin(lon_rad);
-  *z = (kSemimajorAxis / xi * (1 - kFirstEccentricitySquared) + altitude) * sin(lat_rad);
+  const double lat_rad = deg2Rad(latitude);
+  const double lon_rad = deg2Rad(longitude);
+  const double sLat = sin(lat_rad);
+  const double sLon = sin(lon_rad);
+  const double cLat = cos(lat_rad);
+  const double cLon = cos(lon_rad);
+
+  double xi = sqrt(1 - kFirstEccentricitySquared * sLat * sLat);
+  *x = (kSemimajorAxis / xi + altitude) * cLat * cLon;
+  *y = (kSemimajorAxis / xi + altitude) * cLat * sLon;
+  *z = (kSemimajorAxis / xi * (1 - kFirstEccentricitySquared) + altitude) * sLat;
 }
 
 void Ecef2Geodetic(const double x, const double y, const double z,
@@ -240,18 +256,6 @@ class GeodeticConverter
     return ret;
   }
 
-  inline static
-  double rad2Deg(const double radians)
-  {
-    return (radians / M_PI) * 180.0;
-  }
-
-  inline static
-  double deg2Rad(const double degrees)
-  {
-    return (degrees / 180.0) * M_PI;
-  }
-
   double initial_latitude_;
   double initial_longitude_;
   double initial_altitude_;