C++ geodetic2ecef - lat lon to cartesian, assuming an ellipsoid

geodetic2ecef.hpp

#include <math.h>

constexpr double R = 6378.137;
constexpr double ESQ (6.69437999014 * 0.001);
constexpr double DEG2RAD = M_PI / 180.0;

template<typename T>
struct point {
    using coordinate_type = T;
    T x;
    T y;
    T z;
};

template<class T> using Point = point<T>;

// Assumes a ellipsoid, this is more accurate than the classic sphere conversion
Point<double> geodetic2ecef( double lat, double lon, double alt ) {
    lat *= DEG2RAD;
    lon *= DEG2RAD;
    auto xi = std::sqrt( 1 - ESQ * std::sin( lat ) * std::sin( lat ));
    auto x = ( R / xi + alt ) * std::cos( lat ) * std::cos( lon );
    auto y = ( R / xi + alt ) * std::cos( lat ) * std::sin( lon );
    auto z = ( R / xi * ( 1 - ESQ) + alt ) * std::sin( lat );
    return Point<double>{ x, y, z };
}