C#高斯正算和高斯反算代码

public class ANGLERAD
    {
        static int intSignOfDms, intSignOfRad;
        static double radAngle, dmsAngle;
        private static double DMSTORAD(double dmsAngle1)
        {
            intSignOfDms = 1;
            if (dmsAngle1 < 0) intSignOfDms = -1;
            dmsAngle1 = Math.Abs(dmsAngle1);


            radAngle = dmsAngle1 * Math.PI / 180.0;
            radAngle = radAngle * intSignOfDms;
            return radAngle;
        }
        private static double RADTODMS(double radAngle)
        {
            intSignOfRad = 1;
            if (radAngle < 0) intSignOfRad = -1;
            radAngle = Math.Abs(radAngle);

            dmsAngle = radAngle * 180 / Math.PI;
            dmsAngle = dmsAngle * intSignOfRad;
            return dmsAngle;
        }


        private static void a0a2a4a6a8(double a, double e, double[] Coeficient_a0)
        {
            double m0, m2, m4, m6, m8;
            m0 = a * (1 - e * e);
            m2 = 3 * e * e * m0 / 2;
            m4 = 5 * e * e * m2 / 4;
            m6 = 7 * e * e * m4 / 6;
            m8 = 9 * e * e * m6 / 8;

            /*计算a0 a2 a4 a6 a8*/

            Coeficient_a0[0] = m0 + m2 / 2 + 3 * m4 / 8 + 5 * m6 / 16 + 35 * m8 / 128;
            Coeficient_a0[1] = m2 / 2 + m4 / 2 + 15 * m6 / 32 + 7 * m8 / 16;
            Coeficient_a0[2] = m4 / 8 + 3 * m6 / 16 + 7 * m8 / 32;
            Coeficient_a0[3] = m6 / 32 + m8 / 16;
            Coeficient_a0[4] = m8 / 128;
        }

        /// <summary>
        /// 中央经度
        /// </summary>
        private static double _l0 = 118.5;

        /// <summary>
        /// 东纬偏移FE = 500000米 
        /// </summary>
        private static double _FE = 499925;

        /// <summary>
        /// 长半轴 a（米）
        /// </summary>
        private static double _a = 6378240;

        /// <summary>
        /// 扁率α
        /// </summary>
        private static double _af = 1 / 298.25722101;

        /// <summary>
        /// WGS84坐标转为北京54坐标
        /// </summary>
        /// <param name="dmslon"></param>
        /// <param name="dmslat"></param>
        /// <param name="dmsl0"></param>
        /// <param name="coor_x"></param>
        /// <param name="coor_y"></param>
        public static void WGS84ToBJ54(double dmslon, double dmslat, out double coor_x, out double coor_y)
        {
            //int h = 0, k = 0;
            double a = 0, Alfa = 0;
            double dmsl0 = 0;
            double a0, a2, a4, a6, a8;
            double radlat, radlon, radl0, l;
            double b, t, sb, cb, ita, e1, ee;
            double X, l0;
            double N, c, v;
            //double Bf0, Bf1, dB, FBf, bf;
            //double itaf, tf;
            //double Nf, Mf;
            //double B, L, dietaB, dietal;
            //double sinBf, cosBf;
            double[] Coeficient_a0 = new double[5];

            a = _a; //长半轴 a（米）
            Alfa = _af;
            dmsl0 = _l0;


            /*将角度转化为弧度*/
            radlat = ANGLERAD.DMSTORAD(dmslat);
            radlon = ANGLERAD.DMSTORAD(dmslon);
            radl0 = ANGLERAD.DMSTORAD(dmsl0);
            l = radlon - radl0;

            /*计算椭球的基本参数和中间变量*/

            b = a * (1 - Alfa);
            sb = Math.Sin(radlat);
            cb = Math.Cos(radlat);
            t = sb / cb;
            e1 = Math.Sqrt((a / b) * (a / b) - 1);
            ee = Math.Sqrt(1 - (b / a) * (b / a));
            ita = e1 * cb;

            /*计算a0 a2 a4 a6 a8*/
            ANGLERAD.a0a2a4a6a8(a, ee, Coeficient_a0);

            a0 = Coeficient_a0[0];
            a2 = Coeficient_a0[1];
            a4 = Coeficient_a0[2];
            a6 = Coeficient_a0[3];
            a8 = Coeficient_a0[4];

            /*计算X*/
            X = a0 * radlat - sb * cb * ((a2 - a4 + a6) + (2 * a4 - 16 * a6 / 3) * sb * sb + 16 * a6 * sb * sb * sb * sb / 3.0);

            /*计算卯酉圈半径N*/

            c = a * a / b;
            v = Math.Sqrt(1 + e1 * e1 * cb * cb);
            N = c / v;

            /*计算未知点的坐标*/

            coor_x = X + N * sb * cb * l * l / 2 + N * sb * cb * cb * cb * (5 - t * t + 9 * ita * ita + 4 * ita * ita * ita * ita) * l * l * l * l / 24 + N * sb * cb * cb * cb * cb * cb * (61 - 58 * t * t + t * t * t * t) * l * l * l * l * l * l / 720;
            coor_y = N * cb * l + N * cb * cb * cb * (1 - t * t + ita * ita) * l * l * l / 6 + N * cb * cb * cb * cb * cb * (5 - 18 * t * t + t * t * t * t + 14 * ita * ita - 58 * ita * ita * t * t) * l * l * l * l * l / 120;

            //东纬偏移FE = 500000米 + 带号 * 1000000；
            coor_y += _FE;
        }

        public static void BJ54ToWGS84(double coor_x, double coor_y, out double dmslon, out double dmslat)
        {
            //int h = 0, k = 0;
            double a = 0, Alfa = 0;
            double a0, a2, a4, a6, a8;
            //double radlat, radlon, radl0, l;
            double b, t, sb, cb, ita, e1, ee;
            double X, l0;
            double N, c, v;
            double Bf0, Bf1, dB, FBf, bf;
            double itaf, tf;
            double Nf, Mf;
            double B, L, dietaB, dietal;
            double sinBf, cosBf;
            double[] Coeficient_a0 = new double[5];

            l0 = _l0 * Math.PI / 180.0;
            a = _a; //长半轴 a（米）
            Alfa = _af; //扁率

            //东纬偏移FE = 500000米 + 带号 * 1000000；
            coor_y -= _FE;

            /*计算b,e1,e*/
            b = a * (1 - Alfa);
            e1 = Math.Sqrt((a / b) * (a / b) - 1);
            ee = Math.Sqrt(1 - (b / a) * (b / a));

            /*计算a0 a2 a4 a6 a8*/

            ANGLERAD.a0a2a4a6a8(a, ee, Coeficient_a0);

            a0 = Coeficient_a0[0];
            a2 = Coeficient_a0[1];
            a4 = Coeficient_a0[2];
            a6 = Coeficient_a0[3];
            a8 = Coeficient_a0[4];

            X = coor_x;
            Bf0 = X / a0;

            do
            {
                sinBf = Math.Sin(Bf0); cosBf = Math.Cos(Bf0);
                FBf = -sinBf * cosBf * ((a2 - a4 + a6) + (2.0 * a4 - 16.0 * a6 / 3.0) * sinBf * sinBf +
                  (16.0 / 3.0) * a6 * (sinBf * sinBf * sinBf * sinBf));
                Bf1 = (X - FBf) / a0;
                dB = Bf1 - Bf0;
                Bf0 = Bf1;
            } while (Math.Abs(dB * 180.0 / Math.PI * 3600.0) > 0.00001);

            bf = Bf1;
            /*计算c,v,N,M*/
            c = a * a / b;
            v = Math.Sqrt(1 + e1 * e1 * Math.Cos(bf) * Math.Cos(bf));
            Nf = c / v;
            Mf = c / (v * v * v);
            tf = Math.Sin(bf) / Math.Cos(bf);
            itaf = e1 * Math.Cos(bf);

            /*计算dietaB,dietal*/
            dietaB = tf * coor_y * coor_y / (2 * Mf * Nf) - tf * (5 + 3 * tf * tf + itaf * itaf - 9 * tf * tf * itaf * itaf) * coor_y * coor_y * coor_y * coor_y / (24 * Mf * Nf * Nf * Nf) + (61 + 90 * tf * tf + 45 * tf * tf * tf * tf) * coor_y * coor_y * coor_y * coor_y * coor_y * coor_y / (720 * Mf * Nf * Nf * Nf * Nf * Nf);
            dietal = coor_y / (Nf * Math.Cos(bf) + (1 + 2 * tf * tf + itaf * itaf) * Math.Cos(bf) * coor_y * coor_y / (6 * Nf)) + (5 + 44 * tf * tf + 32 * tf * tf * tf * tf - 2 * itaf * itaf - 16 * itaf * itaf * tf * tf) / (360 * Nf * Nf * Nf * Mf * Mf * Math.Cos(bf));



            B = bf - dietaB;//0.43685200205184482
            L = l0 + dietal;//1.88716896792179

            dmslat = ANGLERAD.RADTODMS(B);
            dmslon = ANGLERAD.RADTODMS(L);
        }
    }