javascript wgs84与ecef与enu坐标转换

参考   https://blog.csdn.net/taiyang1987912/article/details/112982150

class CTran {
    wgs84_to_ecef(lat, lon, h) {
        var a = 6378137;
        var b = 6356752.3142;
        var f = (a - b) / a;
        var e_sq = f * (2 - f);
        var lamb = lat / 180 * Math.PI;
        var phi = lon / 180 * Math.PI;
        var s = Math.sin(lamb);
        var N = a / Math.sqrt(1 - e_sq * s * s);

        var sin_lambda = Math.sin(lamb);
        var cos_lambda = Math.cos(lamb);
        var sin_phi = Math.sin(phi);
        var cos_phi = Math.cos(phi);

        var x = (h + N) * cos_lambda * cos_phi;
        var y = (h + N) * cos_lambda * sin_phi;
        var z = (h + (1 - e_sq) * N) * sin_lambda;
        return { x, y, z };
    }
    //lat0, lng0, h0站心坐标
    ecef_to_enu(x, y, z, lat0, lng0, h0) {
        var a = 6378137;
        var b = 6356752.3142;
        var f = (a - b) / a;
        var e_sq = f * (2 - f);

        var lamb = lat0 / 180 * Math.PI;
        var phi = lng0 / 180 * Math.PI;
        var s = Math.sin(lamb);
        var N = a / Math.sqrt(1 - e_sq * s * s);

        var sin_lambda = Math.sin(lamb);
        var cos_lambda = Math.cos(lamb);
        var sin_phi = Math.sin(phi);
        var cos_phi = Math.cos(phi);

        var x0 = (h0 + N) * cos_lambda * cos_phi;
        var y0 = (h0 + N) * cos_lambda * sin_phi;
        var z0 = (h0 + (1 - e_sq) * N) * sin_lambda;

        var xd = x - x0;
        var yd = y - y0;
        var zd = z - z0;

        var t = -cos_phi * xd - sin_phi * yd;
        var xEast = -sin_phi * xd + cos_phi * yd;
        var yNorth = t * sin_lambda + cos_lambda * zd;
        var zUp = cos_lambda * cos_phi * xd + cos_lambda * sin_phi * yd + sin_lambda * zd;

        return { xEast, yNorth, zUp };
    }
    //lat0, lon0, h0 站心坐标
    enu_to_ecef(xEast, yNorth, zUp, lat0, lon0, h0) {
        var a = 6378137
        var b = 6356752.3142
        var f = (a - b) / a
        var e_sq = f * (2 - f)

        var lamb = lat0 / 180 * Math.PI;
        var phi = lon0 / 180 * Math.PI;
        var s = Math.sin(lamb)
        var N = a / Math.sqrt(1 - e_sq * s * s)

        var sin_lambda = Math.sin(lamb)
        var cos_lambda = Math.cos(lamb)
        var sin_phi = Math.sin(phi)
        var cos_phi = Math.cos(phi)

        var x0 = (h0 + N) * cos_lambda * cos_phi
        var y0 = (h0 + N) * cos_lambda * sin_phi
        var z0 = (h0 + (1 - e_sq) * N) * sin_lambda

        var t = cos_lambda * zUp - sin_lambda * yNorth

        var zd = sin_lambda * zUp + cos_lambda * yNorth
        var xd = cos_phi * t - sin_phi * xEast
        var yd = sin_phi * t + cos_phi * xEast

        var x = xd + x0
        var y = yd + y0
        var z = zd + z0

        return { x, y, z }
    }
    ecef_to_wgs84(x, y, z) {
        var x2 = x ** 2
        var y2 = y ** 2
        var z2 = z ** 2

        var a = 6378137.0000    
        var b = 6356752.3142   
        var e = Math.sqrt(1 - (b / a) ** 2)
        var b2 = b * b
        var e2 = e ** 2
        var ep = e * (a / b)
        var r = Math.sqrt(x2 + y2)
        var r2 = r * r
        var E2 = a ** 2 - b ** 2
        var F = 54 * b2 * z2
        var G = r2 + (1 - e2) * z2 - e2 * E2
        var c = (e2 * e2 * F * r2) / (G * G * G)
        var s = (1 + c + Math.sqrt(c * c + 2 * c)) ** (1 / 3)
        var P = F / (3 * (s + 1 / s + 1) ** 2 * G * G)
        var Q = Math.sqrt(1 + 2 * e2 * e2 * P)
        var ro = -(P * e2 * r) / (1 + Q) + Math.sqrt((a * a / 2) * (1 + 1 / Q) - (P * (1 - e2) * z2) / (Q * (1 + Q)) - P * r2 / 2)
        var tmp = (r - e2 * ro) ** 2
        var U = Math.sqrt(tmp + z2)
        var V = Math.sqrt(tmp + (1 - e2) * z2)
        var zo = (b2 * z) / (a * V)

        var height = U * (1 - b2 / (a * V))

        var lat = Math.atan((z + ep * ep * zo) / r)

        var temp = Math.atan(y / x)
        if (x >= 0) {
            var long = temp
        } else if ((x < 0) & (y >= 0)) {
            var long = Math.PI + temp
        } else {
            var long = temp - Math.PI
        }

        var lat0 = lat / (Math.PI / 180)
        var lon0 = long / (Math.PI / 180)
        var h0 = height

        return { 'lat': lat0, 'lon': lon0, 'h': h0 }
    }
    // lat0,lon0,h0 站心坐标
    wgs84_to_enu(lat, lon, h, lat0, lon0, h0) {
        var ecef = this.wgs84_to_ecef(lat, lon, h);
        var enu = this.ecef_to_enu(ecef.x, ecef.y, ecef.z, lat0, lon0, h0);
        return enu;
    }
    enu_to_wgs84(xEast, yNorth, zUp, lat0, lon0, h0) {
        var ecef = this.enu_to_ecef(xEast, yNorth, zUp, lat0, lon0, h0)
        var wgs = this.ecef_to_wgs84(ecef.x, ecef.y, ecef.z)
        return wgs
    }
}