C:/Users/Andreas Schiffler/Desktop/SVN/NewGamePhysics/NewGamePhysics/Physics/GravityEarthGfcModel.cs

// --------------------------------------------------------
// GravityEarthGfcModel.cs
// (A. Schiffler, 2009)
// --------------------------------------------------------

namespace NewGamePhysics.Physics
{
    using System;
    using System.Collections.Generic;
    using System.IO;

    using NewGamePhysics.Mathematics;
    using NewGamePhysics.Utilities;

    public class GravityEarthGfcModel
    {
        public const double Gal = 0.01;
        
        public const double G = 6.67428E-11;

        public const double GM = 0.3986004415E+15;

        public const double R = 0.6378136300E+07;

        public const double omega = 7.292115E-05;

        private double[,] C_lm;

        private double[,] S_lm;

        private int modelOrder;

        private int calcOrder;

        public GravityEarthGfcModel()
        {
        }

        public GravityEarthGfcModel(string modelFilename)
        {
            this.LoadGfcModel(modelFilename);
        }

        public int ModelOrder
        {
            get { return this.modelOrder; }
        }

        public int CalcOrder
        {
            get 
            { 
                return this.calcOrder; 
            }
            
            set 
            { 
                if ((value > 0) && (value <= this.modelOrder))
                {
                    this.calcOrder = value; 
                }
            }
        }

        public double Calculate(double lat, double lon, double h)
        {
            if (this.calcOrder == 0)
            {
                throw new ApplicationException(
                                   "Coefficient model not initialized. Load one first.");
            }

            if ((lat < -90.0) || (lat>90.0))
            {
                throw new ArgumentOutOfRangeException("lat");
            }

            if ((lon < 0.0) || (lon > 360.0))
            {
                throw new ArgumentOutOfRangeException("lon");
            }

            if ((h < 0.0) || (h > 100000.0))
            {
                throw new ArgumentOutOfRangeException("h");
            }

            if (this.calcOrder<1)
            {
                return 0.0;
            }
            
            // Scale input variables
            double r = R + h;
            double phi = Math.PI * lat / 180.0;
            double lambda = Math.PI * lon / 180.0;

            // Precalculations
            double cos_phi = Math.Cos(phi);
            double sin_phi = Math.Sin(phi);
            double R_r = R / r;

            // The components of the centrifugal potential
            double cpr = omega * omega * r * cos_phi * cos_phi;
            double cpl = 0.0;
            double cpp = -omega * omega * r * r * cos_phi * sin_phi;

            // The components of the gradient of the model potential
            double war = 0.0;
            double wal = 0.0;
            double wap = 0.0;

            // Generated normalized Legendre functions
            double[][] P_lm;
            double[][] dP_lm;
            Legendre.NormalizedAssociatedFunctionAndDerivative(this.calcOrder + 1, sin_phi, out P_lm, out dP_lm);

            // Sum over L
            for (int l = 0; l <= this.calcOrder; l++)
            {
                double war_inner = 0.0;
                double wal_inner = 0.0;
                double wap_inner = 0.0;

                // Sum over M
                for (int m = 0; m <= l; m++)
                {
                    double m_lam = (double)m * lambda;
                    double sin_m_lam = Math.Sin(m_lam);
                    double cos_m_lam = Math.Cos(m_lam);

                    double f1, f2;
                    f1 = (S_lm[l, m] * cos_m_lam - C_lm[l, m] * sin_m_lam);
                    f2 = (C_lm[l, m] * cos_m_lam + S_lm[l, m] * sin_m_lam);

                    double s1, s2, s3;
                    s1 = P_lm[l][m] * f2;
                    s2 = (double)m * P_lm[l][m] * f1;
                    s3 = dP_lm[l][m] * f2;

                    war_inner += s1;
                    wal_inner += s2;
                    wap_inner += s3;                    
                }

                double R_r_l = Math.Pow(R_r, (double)l);
                war += (R_r_l  * (double)(l + 1) * war_inner);
                wal += (R_r_l * wal_inner);
                wap += (R_r_l * wap_inner);
            }

            double GM_r = GM / r;
            war *= (- GM_r / r);
            wal *= GM_r;
            wap *= GM_r;

            // Aggregate components
            double c1 = (war + cpr);
            double c2 = (wal + cpl) / (r * cos_phi);
            double c3 = (wap + cpp) / r;

            double g = Math.Sqrt(c1 * c1 + c2 * c2 + c3 * c3);

            return g;
        }

        public void LoadGfcModel(string modelFilename)
        {
            // New list of coefficients
            List<GravityFieldCoefficient> gfcCoefficients = 
                new List<GravityFieldCoefficient>();

            // Reset model order and coefficient arrays
            this.modelOrder = 0;
            this.C_lm = null;
            this.S_lm = null;

            // Prep data reader
            Scanf scanf = new Scanf();
            FileStream fileStream = null;
            StreamReader streamReader = null;
            try
            {
                fileStream = new FileStream(modelFilename, FileMode.Open);
                streamReader = new StreamReader(fileStream);
                string line;
                while ((line = streamReader.ReadLine()) != null)
                {
                    object[] o = scanf.Scan(line, "gfc    %i    %i  %lf  %lf ");

                    if ((null != o) && (o.Length == 4))
                    {
                        int l = (int)o[0];
                        
                        // Track the bigges L as order
                        if (l > this.modelOrder)
                        {
                            this.modelOrder = l;
                        }

                        GravityFieldCoefficient gcf = new GravityFieldCoefficient(l, (int)o[1], (double)o[2], (double)o[3]);
                        gfcCoefficients.Add(gcf);
                    }
                }
            }
            finally
            {
                if (null != streamReader)
                {
                    streamReader.Close();
                }
                if (null != fileStream)
                {
                    fileStream.Close();
                }
            }

            // Repack coefficients into two-dimensional arrays 
            // for easy access in iterative calculations
            int imax = modelOrder + 1;
            this.C_lm = new double[imax, imax];
            this.S_lm = new double[imax, imax];
            foreach (GravityFieldCoefficient gfcCoefficient in gfcCoefficients)
            {
                if ((gfcCoefficient.L < imax) && (gfcCoefficient.M < imax))
                {
                    C_lm[gfcCoefficient.L, gfcCoefficient.M] = gfcCoefficient.C;
                    S_lm[gfcCoefficient.L, gfcCoefficient.M] = gfcCoefficient.S;
                }
            }

            this.calcOrder = this.modelOrder;
        }

        public void UnloadGfcModel()
        {
            this.C_lm = null;
            this.S_lm = null;
            this.calcOrder = 0;
        }
    }
}

---