GIS坐标转换为Unity3D坐标工具，实测可用

GIS坐标转换为Unity3D坐标工具，已在项目中使用
应用场景：
在用Unity开发数字孪生系统项目中，难免会遇到这种需求：线下设备的位置（如物流车）实实的位置如何映射在Unity虚拟场景中并跟随移动。
解决方案：
本项目采用的线下海康的单兵设备与物流车辆进行进行绑定，海康的单兵设备具有先云平台推送GIS的功能，然后云平台在通过WebSocket把GIS数据传输给Unity中。
位置转换：
Unity在拿到这个数据后如果不进行处理，是没法将值赋给场景中的3D模型的，需要借用咱们的坐标转换工具，将GIS坐标转换为Unity3D坐标。
准备：
单兵设备可通过与海康那边对接购买并部署，由于工具底层算法需要提前知道当地的经纬度，已某个地区的经纬度为标准，进行计算。所以我们先获取下经纬度信息。经纬度获取方式，附上链接：高德获取某地的经纬度

工具核心接口：

public static double[] ECEFfromLLA(double lat,double lon,double alt)
    {
        lat = Math.PI * lat / 180.0;
        lon = Math.PI * lon / 180.0;

        double cosLat = Math.Cos(lat);
        double sinLat = Math.Sin(lat);

        double cosLong = Math.Cos(lon);
        double sinLong = Math.Sin(lon);

        double c = 1 / Math.Sqrt(cosLat * cosLat + (1 - f) * (1 - f) * sinLat * sinLat);
        double s = (1 - f) * (1 - f) * c;

        double x = (WGS84a * c + alt) * cosLat * cosLong;
        double y = (WGS84a * c + alt) * cosLat * sinLong;
        double z = (WGS84a * s + alt) * sinLat;

        return new double[3] { x, y, z };
    }
    public static double[] LLAfromECEF(double x, double y, double z)
    {
        double ea = Math.Sqrt((WGS84a * WGS84a - WGS84b * WGS84b) / (WGS84a * WGS84a));
        double eb = Math.Sqrt((WGS84a * WGS84a - WGS84b * WGS84b) / (WGS84b * WGS84b));
        double p = Math.Sqrt(x * x + y * y);

        double theta = Math.Atan2(z * WGS84a, p * WGS84b);
        double lon = Math.Atan2(y, x);
        double lat = Math.Atan2(z + eb * eb * WGS84b * Math.Pow(Math.Sin(theta), 3),
                                p - ea * ea * WGS84a * Math.Pow(Math.Cos(theta), 3));
        double N = WGS84a / Math.Sqrt(1 - ea * ea * Math.Sin(lat) * Math.Sin(lat));
        double alt = p / Math.Cos(lat) - N;

        return new double[3] { lat * (180.0 / Math.PI), lon * (180.0 / Math.PI), alt };
    }
    private static double[,] ECEFfromENUTransform(double lat, double lon, double alt)
    {
        double[] p = ECEFfromLLA(lat, lon, alt);

        lat = Math.PI * lat / 180.0;
        lon = Math.PI * lon / 180.0;
        double sa = Math.Sin(lat);
        double ca = Math.Cos(lat);
        double so = Math.Sin(lon);
        double co = Math.Cos(lon);

        return new double[,] {{ -so, -sa * co, ca * co, p[0] },
                                  { co, -sa * so, ca * so, p[1] },
                                  { 0, ca, sa, p[2] },
                                  { 0, 0, 0, 1 }};
    }
    public static double[] LLAfromENU(double x, double y, double z, double reflat, double reflon, double refalt)
    {
        double[,] T = ECEFfromENUTransform(reflat, reflon, refalt);
        double ex = T[0, 0] * x + T[0, 1] * y + T[0, 2] * z + T[0, 3];
        double ey = T[1, 0] * x + T[1, 1] * y + T[1, 2] * z + T[1, 3];
        double ez = T[2, 0] * x + T[2, 1] * y + T[2, 2] * z + T[2, 3];

        return LLAfromECEF(ex, ey, ez);
    }
    public static double[] ENUfromLLA(double lat, double lon, double alt, double reflat, double reflon, double refalt)
    {
        double[,] T = MatrixWorker.Invert(ECEFfromENUTransform(reflat, reflon, refalt));
        double[] p = ECEFfromLLA(lat, lon, alt);

        double tx = T[0, 0] * p[0] + T[0, 1] * p[1] + T[0, 2] * p[2] + T[0, 3];
        double ty = T[1, 0] * p[0] + T[1, 1] * p[1] + T[1, 2] * p[2] + T[1, 3];
        double tz = T[2, 0] * p[0] + T[2, 1] * p[1] + T[2, 2] * p[2] + T[2, 3];

        return new double[3] { tx, ty, tz };
    }

    public static void ENUfromLLA(double x, double y, out double lat, out double lon)
    {
        double[] ll = new double[2];
        lat = y / WGS84a * 180 / Math.PI;
        lon = x / (WGS84a * Math.Cos(lat / 180 * Math.PI)) * 180 / Math.PI;
    }
    public static double[,] Invert(double[,] matrix)
    {
        if (matrix == null)
            throw new ArgumentNullException("matrix");

        int rows = matrix.GetLength(0);
        int cols = matrix.GetLength(1);
        int n, r;
        double scale;

        if (rows != cols)
            throw new ArgumentException("Given matrix is not a square!", "matrix");

        n = rows;
        double[,] inverse = new double[rows, cols];

        for (r = 0; r < n; ++r)
            for (int c = 0; c < n; ++c)
                inverse[r, c] = (r == c) ? 1.0 : 0.0;

        for (int c = 0; c < n; ++c)
        {
            if (Math.Abs(matrix[c, c]) <= 2.0 * double.Epsilon)
            {
                for (r = c + 1; r < n; ++r)
                    if (Math.Abs(matrix[r, c]) <= 2.0 * double.Epsilon)
                    {
                        RowSwap(matrix, n, c, r);
                        RowSwap(inverse, n, c, r);
                        break;
                    }
                if (r >= n)
                    throw new Exception("Given matrix is singular!");
            }
            scale = 1.0 / matrix[c, c];
            RowScale(matrix, n, scale, c);
            RowScale(inverse, n, scale, c);

            for (r = 0; r < n; ++r)
            {
                if (r != c)
                {
                    scale = -matrix[r, c];
                    RowScaleAdd(matrix, n, scale, c, r);
                    RowScaleAdd(inverse, n, scale, c, r);
                }
            }
        }

        return inverse;
    }
    public class WGSPoint
{
    public double longitude;//经度
    public double latitude;//weid
    public double altitude;//高度
    public float angle;
    public float direction;
}
public enum CoordinatePointType
{
    CangKu,
    BiJieShi,
    CangKuChaChe,
    XingZhenQu,
    DuJuanZhan,
    DaFangMap
}
public static void GetPosition(CoordinatePointType pointType,WGSPoint wgsPoint,out Vector3 position,bool isXingZhenQu,string layerStr=null)
    {
        SetPointData(pointType);
        var pos = GeoConvertor.ENUfromLLA(wgsPoint.latitude, wgsPoint.longitude, wgsPoint.altitude, latitude, longitude, altitude);
        position = new Vector3((float)pos[0], (float)pos[2], (float)pos[1]);
        position = Quaternion.Euler(0, rotation, 0) * position;

        LayerMask templayer = string.IsNullOrEmpty(layerStr) == false ? (LayerMask)int.Parse(layerStr) : raycastMask;

        if (isXingZhenQu == true)
        {
            position += transPos / rate; 
            position.y = GetHeight(position.x, position.z, templayer);
            position = position * rate;
        }
        else
        {
            position += transPos;
            position.y = GetHeight(position.x, position.z, templayer);
        }
    }
    public static Vector3 GisPointToVector3(string longitude,string latitude, CoordinatePointType pointType)
    {
        Vector3 position = new Vector3();
        WGSPoint point = new WGSPoint();
        double.TryParse(longitude, out point.longitude);
        double.TryParse(latitude, out point.latitude);
        point.altitude = 0;
        GetPosition(pointType,point, out position,true);
        return position;
    }

附上项目应用部分截图：

转换工具源码链接：
源码地址