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

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

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

    using NewGamePhysics.Mathematics;
    using NewGamePhysics.Utilities;

    public class GravityEarthGfcGrid
    {
        private const int LatMin = -90;

        private const int LatMax =  90;

        private const int LonMin = 0;

        private const int LonMax = 360;

        private double[,] grid;

        private int size = 0;

        public GravityEarthGfcGrid()
        {
        }

        public GravityEarthGfcGrid(string gridFilename)
        {
            this.LoadGravityMap(gridFilename);
        }

        public int Size
        {
            get { return this.size; }
        }

        public double Calculate(double lat, double lon, double h)
        {
            if (null == this.grid)
            {
                throw new ApplicationException(
                    "Gravity map 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");
            }

            // Calculate index position
            int intLat = (int)Math.Truncate(lat);
            int intLon = (int)Math.Truncate(lon);

            // Create bicubic interpolator matrix
            double[,] G = new double[4, 4];
            for (int i = -1; i <= 2; i++)
            {
                for (int j = -1; j <= 2; j++)
                {
                    int indexLat = LatWrap(intLat + i) - LatMin;
                    int indexLon = LonWrap(intLon + j) - LonMin;                    
                    G[i+1,j+1] = grid[indexLat, indexLon];
                }
            }

            // Interpolate over grid
            Bicubic bicubic = new Bicubic(G);
            double x = Math.Abs(lat - (double)intLat);
            double y = lon - (double)intLon;
            double g = bicubic.Calc(x,y);

            // Scale from mgal units
            g *= 1e-5;

            // Adjust for elevation
            g -= (3.086e-6 * h);

            return g;
        }

        public void LoadGravityMap(string gridFilename)
        {
            // Create new grid 
            size = 0;
            grid = new double[(LatMax - LatMin + 1), (LonMax - LonMin + 1)];

            // Prep data reader
            Scanf scanf = new Scanf();
            FileStream fileStream = null;
            StreamReader streamReader = null;
            try
            {
                fileStream = new FileStream(gridFilename, FileMode.Open);
                streamReader = new StreamReader(fileStream);
                string line;
                while ((line = streamReader.ReadLine()) != null)
                {
                    object[] o = scanf.Scan(line, " %lf %lf %lf");
                    if ((null != o) && (o.Length == 3))
                    {
                        int lon = (int)Math.Truncate((double)o[0]);
                        int lat = (int)Math.Truncate((double)o[1]);
                        double g = (double)o[2];

                        // Check if the grid indices are in the right range
                        if ((lat < LatMin) || (lat > LatMax) || (lon < LonMin) || (lon > LonMax))
                        {
                            throw new Exception("Invalid data: " + line); 
                        }

                        int latIndex = lat - LatMin;
                        int lonIndex = lon - LonMin;

                        // Make sure we don't double write a grid point 
                        if (grid[latIndex, lonIndex] != 0.0)
                        {
                            throw new Exception("Double grid value: " + line);
                        }

                        // Store value in grid                        
                        grid[latIndex, lonIndex] = g;
                        size++;
                    }
                }
            }
            finally
            {
                if (null != streamReader)
                {
                    streamReader.Close();
                }
                if (null != fileStream)
                {
                    fileStream.Close();
                }
            }
        }

        public void UnloadGravityMap()
        {
            this.grid = null;
        }

        private int LatWrap(int lat)
        {
            while (lat < LatMin) 
            {
                lat =+ (LatMax - LatMin);
            } 

            while (lat > LatMax)
            {
                lat -= (LatMax - LatMin);
            }
 
            return lat;
        }

        private int LonWrap(int lon)
        {
            while (lon < LonMin)
            {
                lon += (LonMax - LonMin);
            }

            while (lon > LonMax)
            {
                lon -= (LonMax - LonMin);
            }

            return lon;
        }
    }
}

---