GPS路线偏移

GPS.cs

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace GPS1
{
    class GPS
    {
        //将角度转换为弧度  
        public static double ToRad( double degree ) { return degree / 180 * Math.PI; }
        //将弧度转换为角度  
        public static double ToDeg( double radian ) { return radian * 180 / Math.PI; }

        /// <summary>
        /// 地球半径，单位千米
        /// </summary>
        public const double EarthRadiusKm = 6371.004; // WGS-84

        /// <summary>
        /// 两点球面距离
        /// </summary>
        /// <param name="p1Lat">点1纬度</param>
        /// <param name="p1Lng">点1经度</param>
        /// <param name="p2Lat">点2纬度</param>
        /// <param name="p2Lng">点2经度</param>
        /// <returns>单位千米</returns>
        public static double GetDistance( double p1Lat, double p1Lng,
                                          double p2Lat, double p2Lng ) {
            double p1LatInRad = ToRad( p1Lat );
            double p1LngInRad = ToRad( p1Lng );
            double p2LatInRad = ToRad( p2Lat );
            double p2LngInRad = ToRad( p2Lng );
            double i = Math.Cos( p1LatInRad ) * Math.Cos( p2LatInRad ) * Math.Cos( p1LngInRad - p2LngInRad ) + Math.Sin( p1LatInRad ) * Math.Sin( p2LatInRad );
            double distance = EarthRadiusKm * Math.Acos( i );
            return distance;
        }

        /// <summary>
        /// 计算Gamma值
        /// </summary>
        /// <param name="pALat">路线参考点A纬度</param>
        /// <param name="pALng">路线参考点A经度</param>
        /// <param name="pBLat">路线参考点B纬度</param>
        /// <param name="pBLng">路线参考点B经度</param>
        /// <param name="pXLat">待测点X纬度</param>
        /// <param name="pXLng">待测点X经度</param>
        /// <returns></returns>
        public static double GetGamma( double pALat, double pALng,
                                       double pBLat, double pBLng,
                                       double pXLat, double pXLng ) {
            /*
             *  Points definitions
             * A(x0, y0)
             * B(x1, y1)
             * X(x2, y2)
             *  Equation
             * gamma = (X10*X20 + Y10*Y20) / (X10^2 + Y10^2)
             */
            double dBALat = pBLat - pALat;
            double dBALng = pBLng - pALng;
            double dXALat = pXLat - pALat;
            double dXALng = pXLng - pALng;
            double gamma = ( dBALng * dXALng + dBALat * dXALat ) / ( Math.Pow( dBALng, 2 ) + Math.Pow( dBALat, 2 ) );
            return gamma;
        }

        /// <summary>
        /// 待测车辆C是否在预定路线上，AB点分别为路线参考点
        /// </summary>
        /// <param name="pALat">参考点A纬度</param>
        /// <param name="pALng">参考点A经度</param>
        /// <param name="pBLat">参考点B纬度</param>
        /// <param name="pBLng">参考点B经度</param>
        /// <param name="pCLat">车辆C点纬度</param>
        /// <param name="pCLng">车辆C点经度</param>
        /// <param name="width">预计路宽，单位千米</param>
        /// <returns></returns>
        public static bool IsOnline( double pALat, double pALng,
                                     double pBLat, double pBLng,
                                     double pCLat, double pCLng,
                                     double width ) {
            /*
             *   |           |     <--- width/2
             *  A|-----------|B
             *   |           |     <--- width/2
             */
            // 两个同样的点不能构造矩形区域
            // 两点相等判断精度， 两点直接相减可能由于四舍五入丧失精度，故而取模 |PointA - PointB|
            double TOLERANCE = 0.000001;
            if ( Math.Abs( pALat - pBLat ) < TOLERANCE && Math.Abs( pALng - pBLng ) < TOLERANCE ) return false;

            double Gamma = GetGamma( pALat, pALng, pBLat, pBLng, pCLat, pCLng );
            if ( Gamma > 1 ) {
                double d = GetDistance( pCLat, pCLng, pBLat, pBLng );
                return ! ( d > width );
            }
            if ( Gamma < 0 ) {
                double d = GetDistance( pCLat, pCLng, pALat, pALng );
                return ! ( d > width );
            } else {
                // 计算待测点与路线垂线的交点坐标
                double tmpLng = Gamma * ( pBLng - pALng ) + pALng;
                double tmpLat = Gamma * ( pBLat - pALat ) + pALat;
                double d = GetDistance( pCLat, pCLng, tmpLat, tmpLng );
                return ! ( d > width );
            }
        }
    }
}

GPSTest.cs

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.VisualStudio.TestTools.UnitTesting;

namespace GPS1
{
    [ TestClass ]
    public class GPSTest
    {
        [ TestMethod ]
        public void TestIsOnline() {
            //Assert.IsTrue( GPS.IsOnline() );
            Assert.IsFalse( GPS.IsOnline( 39.200162, 117.262389, 39.200162, 117.262389, 18.527924, 109.519965, 0.01 ) );
        }

        [ TestMethod ]
        public void TestToRad() {
            //Assert.AreEqual( GPS.ToRad(  ), );
        }

        [ TestMethod ]
        public void TestToDeg() {
            //Assert.AreEqual( GPS.ToDeg(  ), );
        }
    }
}