Haversine

haversine.cs

public static class Haversine
    {
        private static double ToRadiant(this double ang)
        {
            return ang * Math.PI / 180.0;
        }
        private static double ToDeg(this double ang)
        {
            return ang / Math.PI * 180.0;
        }
        static readonly double  HEARTRADIUS=6371;
        /// <summary>
        ///   Haversine formula:
	    ///   a = sin²(Δlat/2) + cos(lat1).cos(lat2).sin²(Δlong/2)
        ///   c = 2.atan2(√a, √(1−a))
        ///   d = R.c
        ///  where R is earth’s radius (mean radius = 6,371km);
        ///note that angles need to be in radians to pass to trig functions!
        /// </summary>
        public static double GetDistance(double latStart, double lonStart, double latEnd, double lonEnd)
        {
            double dlat = (latEnd-latStart)/2;
            double dlon = (lonEnd-lonStart)/2;
            double a = Math.Sin(dlat.ToRadiant()) * Math.Sin(dlat.ToRadiant()) + Math.Cos(latStart.ToRadiant()) * Math.Cos(latEnd.ToRadiant()) * Math.Sin(dlon.ToRadiant()) * Math.Sin(dlon.ToRadiant());
            double c = 2 * Math.Atan2(Math.Sqrt(a), Math.Sqrt(1 - a));
            return HEARTRADIUS * c;
        }
        /// <summary>
        /// θ = 	atan2( 	sin(Δlong).cos(lat2),
        /// cos(lat1).sin(lat2) − sin(lat1).cos(lat2).cos(Δlong) )
        /// </summary>
        /// <returns></returns>
        public static double GetInitialBearing(double latStart, double lonStart, double latEnd, double lonEnd)
        {
            double dlon = (lonEnd - lonStart) / 2;
            return (Math.Atan2(Math.Sin(dlon.ToRadiant()) * Math.Cos(latEnd.ToRadiant()), Math.Cos(latStart.ToRadiant())
                * Math.Sin(latEnd.ToRadiant()) - Math.Sin(latStart.ToRadiant())*Math.Cos(latEnd.ToRadiant())*Math.Cos(dlon.ToRadiant())).ToDeg()
                +360)%360;
        }
    }