车载终端项目GPS模块算法详述

　　项目系统使用WINCE6.0，应用层语言采用C#。

　　项目中GPS模块的工作过程为：读取串口中的GPS数据，解析封装到类中，然后进行数据库的保存操作，上传到后台，并进行位置和限制路径以及限制区域之间关系的判断。将判断结果进行报警处理。其中限制路径和区域的判断是模块中的重点。

　　对于其中算法，我分为两大部分：1是比较策略，2是调度策略。比较策略是指具体的比较算法的复杂度，比如，如何判断点是否在多边形区域内；而调度策略是指如何调度使每一次判断的比较总次数最少。比如：当前位置为（121，31），而所有的限制区域和限制路径所在范围能达到的最大经度都小于121，并且此时方向为正东，即远离所有的限制区域和路径，则最佳的调度调度策略会使比较的次数为0。当然，这种调度策略也会带来一定的复杂度，所以当限制路径和区域个数较少时，可以不予考虑。

　　下面将着重介绍计较策略，在介绍之前，先描述一个限制路径和限制区域的数据结构。

　　1、限制路径

　　限制路径是由一系列的拐点组成，拐点为经纬度坐标，相邻的两个拐点组成一条路段，路段有宽度属性。所以，路段矩形为相邻两拐点和路宽所围成的矩形。路段空间为 过相邻两拐点分别做垂直与两点连线的垂线所围成的无穷大区域。

　　点与路径之间的位置关系为：

　　　　1、点在所有的路段空间外，则称为：点在路径之外；　　

　　　　2、点在某个路段（如A）矩形上，称为：点在路径上；

　　　　3、点在某个路段（如A）空间内，且不在路段空间上，称为：点偏移路段（A）；

　　点与路径之间的动作关系为：

　　　　1、点进入第一个路段空间，称为：点进入路径；

　　　　2、点退出最后一个路段空间，称为：点离开路径；

　　2、限制区域

　　　　限制区域由3种区域组成：圆形区域、矩形区域和多边形区域。（若不做说明，坐标均为经纬度）

　　　　a、圆形区域：由原点和半径R来定义

　　　　b、矩形区域：由左上点和右下点定义

　　　　c、多边形区域：由一系列点所围成的闭合区域

　　结合上面的数据结构，可以很容易在平面几何中得到相应的比较算法，然而，我们采集到的是大地坐标，即经纬度，不可以直接运用公式求解，所以要先将大地坐标系转换到平面坐标系，可采用高斯投影公式来转换。下面贴上项目使用的工具类，里面都是一些工具函数：

using System;
using System.Collections.Generic;
using System.Text;
using System.Data;
using MotorTerminal.Data;

namespace MotorTerminal.GPS
{
    class Utils
    {
        public const double EARTH_RADIUS = 6378.137;   //地球半径

        //根据角度求弧度
        public static double rad(double d)
        {
            return d * Math.PI / 180.0;
        }

        //求点集中的最大x和y
        public static CartesianPoint Max(IList<GPSLocation> list)
        {
            CartesianPoint result = new CartesianPoint();

            foreach (GPSLocation location in list)
            {
                result.x = result.x > location.Longitude ? result.x : location.Longitude;
                result.y = result.y > location.Latitude ? result.y : location.Latitude;
            }

            return result;
        }

        //求点集中的最小x和y
        public static CartesianPoint Min(IList<GPSLocation> list)
        {
            CartesianPoint result = new CartesianPoint(999, 999);

            foreach (GPSLocation location in list)
            {
                result.x = result.x < location.Longitude ? result.x : location.Longitude;
                result.y = result.y < location.Latitude ? result.y : location.Latitude;
            }

            return result;
        }

        //同经度上指定距离的纬度差，返回值单位为：度
        public static double DistanceToDegreeByLon(int distance)
        {
            return (distance * 180) / (Math.PI * EARTH_RADIUS * 1000);
        }

        //同纬度上的指定距离的经度差，返回值单位为：度
        public static double DistanceToDegreeByLat(int distance, double latitude)
        {
            double result = 2 * Math.Asin((Math.Sin(distance / (2 * EARTH_RADIUS * 1000))) / Math.Cos(rad(latitude)));
            result = result * 180 / Math.PI;
            return result;
        }

        //射线法判断点是否在多边形区域内
        public static bool PointInFences(CartesianPoint pnt1, IList<CartesianPoint> fencePnts)
        {
            int j = 0, cnt = 0;
            for (int i = 0; i < fencePnts.Count; i++)
            {
                j = (i == fencePnts.Count - 1) ? 0 : j + 1;
                if ((fencePnts[i].y != fencePnts[j].y) && (((pnt1.y >= fencePnts[i].y) && (pnt1.y < fencePnts[j].y))
                    || ((pnt1.y >= fencePnts[j].y) && (pnt1.y < fencePnts[i].y)))
                    && (pnt1.x < (fencePnts[j].x - fencePnts[i].x) * (pnt1.y - fencePnts[i].y) / (fencePnts[j].y - fencePnts[i].y) + fencePnts[i].x))
                {
                    cnt++;
                }
            }
            return (cnt % 2 > 0) ? true : false;
        }

        /// <summary>
        /// 获得地球上两点的距离，公式由弧长公式和余弦定理推导而出。
        /// </summary>
        /// <param name="lat1">起点的纬度</param>
        /// <param name="lng1">起点的经度</param>
        /// <param name="lat2">终点的纬度</param>
        /// <param name="lng2">终点的经度</param>
        /// <returns>返回距离，单位为：米，3位精度</returns>
        public static double GetDistance(double lat1, double lng1, double lat2, double lng2)
        {
            double radLat1 = rad(lat1);
            double radLat2 = rad(lat2);
            double a = radLat1 - radLat2;
            double b = rad(lng1) - rad(lng2);
            double s = 2 * Math.Asin(Math.Sqrt(Math.Pow(Math.Sin(a / 2), 2) +
             Math.Cos(radLat1) * Math.Cos(radLat2) * Math.Pow(Math.Sin(b / 2), 2)));
            s = s * EARTH_RADIUS;
            s = Math.Round(s * 1000, 3);
            return s;
        }

        /// <summary>
        /// 获得地球上两点的距离
        /// </summary>
        /// <param name="lat1">起点的纬度</param>
        /// <param name="lng1">起点的经度</param>
        /// <param name="lat2">终点的纬度</param>
        /// <param name="lng2">终点的经度</param>
        /// <returns>返回距离，单位为：米</returns>
        public static int GetDistanceInt(double lat1, double lng1, double lat2, double lng2)
        {
            int result = -1;
            double s = GetDistance(lat1, lng1, lat2, lng2);
            try
            {
                result = Convert.ToInt32(s);
            }
            catch (System.Exception ex)
            {
                Console.WriteLine("错误函数为：GPS模块【GetDistanceInt】，异常为：{0}", ex.Message);
            }

            return result;
        }

        /// <summary>
        /// 大地坐标系（经纬度坐标）转换到笛卡尔坐标系，使用高斯投影公式求解
        /// </summary>
        /// <param name="longitude">要转换点的经度</param>
        /// <param name="latitude">要转换点的纬度</param>
        /// <param name="type">投影基准面类型</param>
        /// <returns>笛卡尔坐标点</returns>
        public static CartesianPoint GeodeticToCartesian(double longitude, double latitude, CoordinateType type)

        {
            double L, B;//大地经度，纬度
            double l, L0;//投影带中央线起算的经差，投影带中央子午线
            double X, Y;//直角坐标
            double f, e2, a, b, e12;////参考椭球体扁率,第一偏心率，长半轴,短半轴，第二偏心率
            double t;//保存tan B
            double m;//保存lcosB
            double q2;//卯酉圈的分量 的平方
            double M, N;//子午，卯酉圈曲半径
            double S;//子午线弧长
            double A0, B0, C0, D0, E0;
            double n;//投影带号
            double a_1_e2;//a*(1-e*e)的值
            int zonewide = 3;//3度带比较准，可以用6度带

            CoordinateType base_type = type;    //投影基准面类型：北京54基准面为54，西安80基准面为80，WGS84基准面
            //  const double PI = 3.1415926535897932;
            //----------初始化已知参数-----------
            L = longitude;//经度
            B = latitude;//纬度
            if (CoordinateType.WGS84 == base_type)
            {
                a = 6378137;
                b = 6356752.3142;
                f = 1 / 298.257223563;
                e2 = 0.0066943799013;//--第一偏心率
                e12 = 0.00673949674227;//--第二偏心率
            }
            else if (CoordinateType.BJ54 == base_type)
            {
                a = 6378245;
                b = 6356863.0187730473;
                f = 1 / 298.3;
                e2 = 0.006693421622966;//--第一偏心率
                e12 = 0.006738525414683;//--第二偏心率
            }
            else if (CoordinateType.XIAN80 == base_type)
            {
                a = 6378140;
                b = 6356755.2881575287;
                f = 1 / 298.257;
                e2 = 0.006694384999588;//--第一偏心率
                e12 = 0.006739501819473;//--第二偏心率
            }
            else
            {
                a = 6378137;
                b = 6356752.3142;
                f = 1 / 298.257223563;
                e2 = 0.0066943799013;//--第一偏心率
                e12 = 0.00673949674227;//--第二偏心率
            }
            //---基本计算公式 a 长半轴，b 短半轴--------
            //f = (a-b)/a;//--椭球体扁率
            //e2 = (a*a-b*b)/(a*a);//--第一偏心率
            //e12 = (a*a-b*b)/(b*b);//--第二偏心率
            a_1_e2 = a * (1 - e2);
            //-----简化计算参数
            if (zonewide == 6)
            {
                n = (int)(L / 6) + 1;
                L0 = n * zonewide - 3;
            }
            else
            {
                n = (int)((L - 1.5) / 3) + 1;
                L0 = n * 3;
            }

            //---转化为弧度,sin,cos 等运算都是以弧度为基础的
            L0 = L0 * Math.PI / 180;
            L = L * Math.PI / 180;
            B = B * Math.PI / 180;
            l = L - L0;
            t = Math.Tan(B);
            q2 = e12 * (Math.Cos(B) * Math.Cos(B));
            N = a / Math.Sqrt(1 - e2 * Math.Sin(B) * Math.Sin(B));////卯酉圈的曲率半径
            m = l * Math.Cos(B);
            //---子午线弧长
            //--以下算法是对原始公式的多项式进行了合并处理，保存计算更准
            double m0 = a_1_e2;
            double m2 = 3.0 * e2 * m0 / 2.0;
            double m4 = 5.0 * e2 * m2 / 4.0;
            double m6 = 7.0 * e2 * m4 / 6.0;
            double m8 = 9.0 * e2 * m6 / 8.0;
            double a0 = m0 + m2 / 2.0 + 3.0 * m4 / 8.0 + 5.0 * m6 / 16.0 + 35.0 * m8 / 128;
            double a2 = m2 / 2.0 + m4 / 2.0 + 15.0 * m6 / 32.0 + 7.0 * m8 / 16.0;
            double a4 = m4 / 8.0 + 3.0 * m6 / 16.0 + 7.0 * m8 / 32.0;
            double a6 = m6 / 32.0 + m8 / 16.0;
            double a8 = m8 / 128.0;
            A0 = a0;
            B0 = a2;
            C0 = a4;
            D0 = a6;
            E0 = a8;
            S = (A0 * B - B0 * Math.Sin(2.0 * B) / 2.0 + C0 * Math.Sin(4.0 * B) / 4.0 - D0 * Math.Sin(6.0 * B) / 6.0 + E0 * Math.Sin(8.0 * B) / 8.0);

            //----高斯投影公式-------
            X = S + N * t * ((0.5 + ((5.0 - t * t + 9 * q2 + 4 * q2 * q2) / 24 + (61.0 - 58.0 * t * t + Math.Pow(t, 4)) * m * m / 720.0) * m * m) * m * m);//pow(t,4)为t的4次方
            Y = N * ((1 + ((1 - t * t + q2) / 6.0 + (5.0 - 18.0 * t * t + Math.Pow(t, 4) + 14 * q2 - 58 * q2 * t * t) * m * m / 120.0) * m * m) * m);
            //----根据中国国情坐标纵轴西移500公里当作起始轴
            Y = 1000000 * n + 500000 + Y;

            CartesianPoint point = new CartesianPoint(Y, X);

            return point;
        }

        /// <summary>
        /// 大地坐标系（经纬度坐标）转换到笛卡尔坐标系，投影基准面默认为：WGS84。使用高斯投影公式求解
        /// </summary>
        /// <param name="longitude">要转换点的经度</param>
        /// <param name="latitude">要转换点的纬度</param>
        /// <returns>笛卡尔坐标点</returns>
        public static CartesianPoint GeodeticToCartesian(double longitude, double latitude)
        {
            return GeodeticToCartesian(longitude, latitude, CoordinateType.WGS84);
        }

        //将大地坐标点集（经纬度）转换成笛卡尔坐标点集
        public static IList<CartesianPoint> GeodeticToCartesian(IList<GPSLocation> points)
        {
            IList<CartesianPoint> result = new List<CartesianPoint>();

            for (int i = 0; i < points.Count; i++)
            {
                CartesianPoint temp = new CartesianPoint();
                temp = GeodeticToCartesian(points[i].Longitude, points[i].Latitude);

                result.Add(temp);
            }

            return result;
        }

        /// <summary>
        /// 计算两点之间的距离，直接用距离公式
        /// </summary>
        /// <param name="p1">起点的笛卡尔坐标</param>
        /// <param name="p2">终点的笛卡尔坐标</param>
        /// <returns>距离</returns>
        public static double GetPointDistance(CartesianPoint p1, CartesianPoint p2)
        {
            return Math.Sqrt((p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y));
        }

        /// <summary>
        /// 计算点到线段的距离，注：此函数计算距离的条件为点在过线段两端点的垂线所围成的范围内。
        /// </summary>
        /// <param name="pointStart">线段起点</param>
        /// <param name="pointEnd">线段终点</param>
        /// <param name="point">要计算到线段距离的点</param>
        /// <returns>距离</returns>
        public static double DistancePointToLine(CartesianPoint pointStart, CartesianPoint pointEnd, CartesianPoint point)
        { 
            double a, b, c;
            a = GetPointDistance(pointEnd, point);
            if (a <= 0.00001)
                return 0.0;
            b = GetPointDistance(pointStart, point);
            if (b <= 0.00001)
                return 0.0;
            c = GetPointDistance(pointStart, pointEnd);
            if (c <= 0.00001)
                return a;

            if (a * a > b * b + c * c)      //点未进入线段区域
                return -1;       
            if (b * b > a * a + c * c)      //点已出线段区域
                return -2;       

            double l = (a + b + c) / 2;     //周长的一半  
            double s = Math.Sqrt(l * (l - a) * (l - b) * (l - c));  //海伦公式求面积，也可以用矢量求  
            return 2 * s / c;
        }

        /// <summary>
        /// 计算点到线段的距离，注：此函数计算距离的条件为点在过线段两端点的垂线所围成的范围内。
        /// </summary>
        /// <param name="pointStart">线段起点</param>
        /// <param name="pointEnd">线段终点</param>
        /// <param name="point">要计算到线段距离的点</param>
        /// <returns>距离</returns>
        public static int DistancePointToLineInt(CartesianPoint pointStart, CartesianPoint pointEnd, CartesianPoint point)
        {
            double result = DistancePointToLine(pointStart, pointEnd, point);

            return Convert.ToInt32(result);
        }
    }

    #region 内部使用的封装结构

    /// <summary>
    /// 笛卡尔点，即平面坐标系上的点
    /// </summary>
    struct CartesianPoint
    {
        public CartesianPoint(double x, double y)
        {
            this.x = x;
            this.y = y;
        }
        public double x;
        public double y;
    }

    /// <summary>
    /// 圆形区域的封装结构，用来存储转换后的圆形笛卡尔坐标和外包矩形的阀值
    /// </summary>
    struct RoundCapsule
    {
        public IRoundedArea area;              //圆形区域
        public CartesianPoint centerPoint;     //圆心(笛卡尔坐标)
        public CartesianPoint max;             //外包矩形的最大x和y，即右上点
        public CartesianPoint min;             //外包矩形的最小x和y，即左下点
    }

    /// <summary>
    /// 矩形区域的封装结构，用来存储转换后的左上点和右下点的笛卡尔坐标，此2点即可作为阀值
    /// </summary>
    struct RectCapsule
    {
        public IRectangularArea area;          //矩形区域
        public CartesianPoint leftUpPoint;     //左上点
        public CartesianPoint rightDownPoint;  //右下点
    }

    /// <summary>
    /// 多边形区域的封装结构，用来存储转换后的各点的笛卡尔坐标和外包矩形的阀值
    /// </summary>
    struct PolygonCapsule
    {
        public IMultilateralArea area;         //多边形区域
        public CartesianPoint max;             //外包矩形的最大x和y，即右上点
        public CartesianPoint min;             //外包矩形的最小x和y，即左下点
        public IList<CartesianPoint> points;    //多边形区域的笛卡尔坐标点集
    }

    /// <summary>
    /// 限制路径中路段的包装结构
    /// </summary>
    struct RoutePartCapsule
    {
        public IRoutePart routePart;            //路段
        public CartesianPoint startPoint;       //起始点
        public CartesianPoint endPoint;         //终点
    }

    /// <summary>
    /// 限制路径的封装结构，用来
    /// </summary>
    struct RouteCapsule
    {
        public IRestrictedRoute route;              //限制路径
        public IList<RoutePartCapsule> routeParts;  //路段集合
    }

    //坐标系类型
    enum CoordinateType
    {
        WGS84,      //北京54基准面为
        BJ54,       //WGS84基准面为
        XIAN80      //西安80基准面为
    }

    #endregion
}