omp/Calculator.cs

namespace omp
{
    using CalculationResults = Dictionary<string, double[,]>;

    public class CalculationException : Exception;

    public class OverlappingPoints : CalculationException;
    public class SingularMatrix : CalculationException;

    public interface ICalculator
    {
        CalculationResults Calculate();
    }

    public class Calculator2D(List<Point2D> stations, int xSize, int ySize) : ICalculator
    {
        private readonly List<Point2D> stations = stations;
        private readonly int xSize = xSize, ySize = ySize;

        public CalculationResults Calculate()
        {
            double[,] xdop = new double[xSize, ySize];
            double[,] ydop = new double[xSize, ySize];
            double[,] hdop = new double[xSize, ySize];

            Parallel.For(0, xSize, i =>
            {
                for (int j = 0; j < ySize; ++j)
                {
                    try
                    {
                        double[,] JR = Jacobian(stations, new Point2D(i, j));

                        double[,] mult = JR.MultTrans();

                        double det = mult.Det();

                        if (det == 0)
                        {
                            throw new SingularMatrix();
                        }

                        double sx2 = mult.Adj(0, 0) / det;
                        double sy2 = mult.Adj(1, 1) / det;

                        xdop[i, j] = Math.Sqrt(sx2);
                        ydop[i, j] = Math.Sqrt(sy2);
                        hdop[i, j] = Math.Sqrt(sx2 + sy2);
                    }
                    catch (CalculationException)
                    {
                        xdop[i, j] = double.NaN;
                        ydop[i, j] = double.NaN;
                        hdop[i, j] = double.NaN;
                    }
                }
            });

            return new CalculationResults
            {
                ["xdop"] = xdop,
                ["ydop"] = ydop,
                ["hdop"] = hdop,
            };
        }

        public static double[,] Jacobian(List<Point2D> stations, Point2D position)
        {
            int n = stations.Count;
            double[,] J = new double[n, 2];

            for (int i = 0; i < n; ++i)
            {
                double distance = position.DistanceTo(stations[i]);

                if (distance == 0)
                {
                    throw new OverlappingPoints();
                }

                J[i, 0] = -(stations[i].x - position.x) / distance;
                J[i, 1] = -(stations[i].y - position.y) / distance;
            }

            return J;
        }
    }

    public class Calculator3D(List<Point3D> stations, int xSize, int ySize, int zCoordinate) : ICalculator
    {
        private readonly List<Point3D> stations = stations;
        private readonly int xSize = xSize, ySize = ySize;
        private readonly int z = zCoordinate;

        public CalculationResults Calculate()
        {
            double[,] xdop = new double[xSize, ySize];
            double[,] ydop = new double[xSize, ySize];
            double[,] hdop = new double[xSize, ySize];
            double[,] vdop = new double[xSize, ySize];
            double[,] pdop = new double[xSize, ySize];

            Parallel.For(0, xSize, i =>
            {
                for (int j = 0; j < ySize; ++j)
                {
                    try
                    {
                        double[,] JR = Jacobian(stations, new Point3D(i, j, z));

                        double[,] mult = JR.MultTrans();

                        double det = mult.Det();

                        double sx2 = mult.Adj(0, 0) / det;
                        double sy2 = mult.Adj(1, 1) / det;
                        double sz2 = mult.Adj(2, 2) / det;

                        xdop[i, j] = Math.Sqrt(sx2);
                        ydop[i, j] = Math.Sqrt(sy2);
                        hdop[i, j] = Math.Sqrt(sx2 + sy2);
                        vdop[i, j] = Math.Sqrt(sz2);
                        pdop[i, j] = Math.Sqrt(sx2 + sy2 + sz2);
                    }
                    catch (OverlappingPoints)
                    {
                        xdop[i, j] = double.NaN;
                        ydop[i, j] = double.NaN;
                        hdop[i, j] = double.NaN;
                        vdop[i, j] = double.NaN;
                        pdop[i, j] = double.NaN;
                    }
                }
            });

            return new CalculationResults
            {
                ["xdop"] = xdop,
                ["ydop"] = ydop,
                ["hdop"] = hdop,
                ["vdop"] = vdop,
                ["pdop"] = pdop,
            };
        }

        public static double[,] Jacobian(List<Point3D> stations, Point3D position)
        {
            int n = stations.Count;
            double[,] J = new double[n, 3];

            for (int i = 0; i < n; ++i)
            {
                double distance = position.DistanceTo(stations[i]);

                if (distance == 0)
                {
                    throw new OverlappingPoints();
                }

                J[i, 0] = -(stations[i].x - position.x) / distance;
                J[i, 1] = -(stations[i].y - position.y) / distance;
                J[i, 2] = -(stations[i].z - position.z) / distance;
            }

            return J;
        }
    }

    public static class CalculatorFactory
    {
        public static ICalculator Create(IConfigurator configurator)
        {
            var stations = configurator.Stations;

            return configurator.PointType switch
            {
                _ when configurator.PointType == typeof(Point2D) =>
                    new Calculator2D(stations.Cast<Point2D>().ToList(), configurator.XSize, configurator.YSize),
                _ when configurator.PointType == typeof(Point3D) =>
                    new Calculator3D(stations.Cast<Point3D>().ToList(), configurator.XSize, configurator.YSize, configurator.ZCoordinate),
                _ => throw new InvalidOperationException("Unsupported point type")
            };
        }
    }
}