Shader之ECEF——LLH

uniform mat4 osg_ViewMatrix;
uniform mat4 osg_ViewMatrixInverse;
uniform mat4 osg_ModeViewMatrix;
uniform mat4 osg_ProjectionMatrix;
uniform float osg_SimulationTime;
uniform sampler2D ocean_data;
const float radiusEquator = 6378137.0;
const float radiusPolar = 6356752.3142;
vec3 ECEF_TO_LLH(in vec3 xyz)
{
    float p = sqrt(xyz.x * xyz.x + xyz.y * xyz.y);
    float theta = atan(xyz.z * radiusEquator,(p * radiusPolar));
    float eDashSquared = (radiusEquator * radiusEquator - radiusPolar * radiusPolar)/(radiusPolar * radiusPolar);
    float sin_theta = sin(theta);
    float cos_theta = cos(theta);

    float flattening = (radiusEquator - radiusPolar)/radiusEquator;
    float eccentricitySquared = 2 * flattening - flattening * flattening;    
    float latitude = atan((xyz.z + eDashSquared * radiusPolar * sin_theta * sin_theta * sin_theta)/(p - eccentricitySquared * radiusEquator * cos_theta * cos_theta * cos_theta));
    float longitude = atan(xyz.y,xyz.x);
    float sin_latitude = sin(latitude);
    float N = radiusEquator /sqrt(1.0 - eccentricitySquared * sin_latitude * sin_latitude);
    float h = p/cos(latitude) - N;
    latitude = degrees(latitude);
    longitude = degrees(longitude);
    return vec3(latitude, longitude, h);
}

vec3 LLH_TO_ECEF(in vec3 llh)
{
    float flattening = (radiusEquator - radiusPolar)/radiusEquator;
    float eccentricitySquared = 2 * flattening - flattening * flattening;
    float sin_latitude = sin(llh.x);
    float cos_latitude = cos(llh.x);
    float sin_longitude = sin(llh.y);
    float cos_longitude = cos(llh.y);
    float N = radiusEquator / sqrt(1.0 - eccentricitySquared * sin_latitude * sin_latitude);
    float x = (N + llh.z) * cos_latitude * cos_longitude;
    float y = (N + llh.z) * cos_latitude * sin_longitude; 
    float z = (N * (1 - eccentricitySquared) + llh.z) * sin_latitude;
    return vec3(x, y, z);
}