Metis_xctg/Adjust/CurvedSurface.cs

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace PaintDotNet.Adjust
{
    public class CurvedSurfacePoint
    {
        public double X;

        public double Y;

        public double Z;

        public CurvedSurfacePoint(double x, double y, double z)
        {
            X = x;
            Y = y;
            Z = z;
        }
    }
    public class CurvedSurface
    {
        public List<CurvedSurfacePoint> Points;

        private double sumX = 0.0;

        private double sumX2;

        private double sumX3;

        private double sumX4;

        private double sumY;

        private double sumY2;

        private double sumY3;

        private double sumY4;

        private double sumXY;

        private double sumXY2;

        private double sumXY3;

        private double sumX2Y;

        private double sumX3Y;

        private double sumX2Y2;

        private double sumZ;

        private double sumZX2;

        private double sumZXY;

        private double sumZY2;

        private double sumZX;

        private double sumZY;

        private double a = 0.0;

        private double b = 0.0;

        private double c = 0.0;

        private double d = 0.0;

        private double e = 0.0;

        private double f = 0.0;

        private int N = 3;

        public void ClearValue()
        {
            sumX = 0.0;
            sumX2 = 0.0;
            sumX3 = 0.0;
            sumX4 = 0.0;
            sumY = 0.0;
            sumY2 = 0.0;
            sumY3 = 0.0;
            sumY4 = 0.0;
            sumXY = 0.0;
            sumXY2 = 0.0;
            sumXY3 = 0.0;
            sumX2Y = 0.0;
            sumX3Y = 0.0;
            sumX2Y2 = 0.0;
            sumZ = 0.0;
            sumZX2 = 0.0;
            sumZXY = 0.0;
            sumZY2 = 0.0;
            sumZX = 0.0;
            sumZY = 0.0;
        }

        public void Init(List<CurvedSurfacePoint> list)
        {
            Points = list;
            N = list.Count;
            ClearValue();
            foreach (CurvedSurfacePoint value in list)
            {
                sumX += value.X;
                sumX2 += value.X * value.X;
                sumX3 += value.X * value.X * value.X;
                sumX4 += value.X * value.X * value.X * value.X;
                sumY += value.Y;
                sumY2 += value.Y * value.Y;
                sumY3 += value.Y * value.Y * value.Y;
                sumY4 += value.Y * value.Y * value.Y * value.Y;
                sumXY += value.X * value.Y;
                sumXY2 += value.X * value.Y * value.Y;
                sumXY3 += value.X * value.Y * value.Y * value.Y;
                sumX2Y += value.X * value.X * value.Y;
                sumX3Y += value.X * value.X * value.X * value.Y;
                sumX2Y2 += value.X * value.X * value.Y * value.Y;
                sumZ += value.Z;
                sumZX2 += value.Z * value.X * value.X;
                sumZXY += value.Z * value.X * value.Y;
                sumZY2 += value.Z * value.Y * value.Y;
                sumZX += value.Z * value.X;
                sumZY += value.Z * value.Y;
            }
        }

        public void CalPara()
        {
            if (N >= 3)
            {
                double[,] valus = new double[3, 4]
                {
                {
                    sumX2,
                    sumXY,
                    sumX,
                    sumZX
                },
                {
                    sumXY,
                    sumY2,
                    sumY,
                    sumZY
                },
                {
                    sumX,
                    sumY,
                    N,
                    sumZ
                }
                };
                Guass guass = new Guass(3, valus);
                a = guass.Result[0];
                b = guass.Result[1];
                c = guass.Result[2];
                if (double.IsNaN(a) || double.IsNaN(b) || double.IsNaN(c))
                    throw new Exception("Calculate failed!");
                return;
            }
            double[,] valus2 = new double[6, 7]
            {
            {
                sumX4,
                sumX3Y,
                sumX2Y2,
                sumX3,
                sumX2Y,
                sumX2,
                sumZX2
            },
            {
                sumX3Y,
                sumX2Y2,
                sumXY3,
                sumX2Y,
                sumXY2,
                sumXY,
                sumZXY
            },
            {
                sumX2Y2,
                sumXY3,
                sumY4,
                sumXY2,
                sumY3,
                sumY2,
                sumZY2
            },
            {
                sumX3,
                sumX2Y,
                sumXY2,
                sumX2,
                sumXY,
                sumX,
                sumZX
            },
            {
                sumX2Y,
                sumXY2,
                sumY3,
                sumXY,
                sumY2,
                sumY,
                sumZY
            },
            {
                sumX2,
                sumXY,
                sumY2,
                sumX,
                sumY,
                N,
                sumZ
            }
            };
            Guass guass2 = new Guass(6, valus2);
            a = guass2.Result[0];
            b = guass2.Result[1];
            c = guass2.Result[2];
            d = guass2.Result[3];
            e = guass2.Result[4];
            f = guass2.Result[5];
        }

        public double GetValue(double x, double y)
        {
            if (N >= 3)
            {
                return a * x + b * y + c;
            }
            return a * x * x + b * x * y + c * y * y + d * x + e * y + f;
        }

        private static double[][] MatrixMult(double[][] matrix1, double[][] matrix2)
        {
            int num = matrix1.Length;
            int num2 = matrix2.Length;
            int num3 = matrix2[0].Length;
            double[][] array = new double[num][];
            for (int i = 0; i < array.Length; i++)
            {
                array[i] = new double[num3];
            }
            for (int j = 0; j < num; j++)
            {
                for (int k = 0; k < num3; k++)
                {
                    for (int l = 0; l < num2; l++)
                    {
                        array[j][k] += matrix1[j][l] * matrix2[l][k];
                    }
                }
            }
            return array;
        }
    }
    class Guass
    {
        private double[,] a;

        private double[] ans;

        private int d;

        public double[] Result => ans;

        private int gauss_jordan(int n)
        {
            int num = 0;
            for (int i = 0; i < n; i++)
            {
                if (num >= n)
                {
                    break;
                }
                int num2 = num;
                for (int j = num + 1; j < n; j++)
                {
                    if (Math.Abs(a[j, i]) > Math.Abs(a[num2, i]))
                    {
                        num2 = j;
                    }
                    if (Math.Abs(a[num2, i]) < 1E-07)
                    {
                        num--;
                        continue;
                    }
                    if (num2 != num)
                    {
                        for (int k = 0; k <= n; k++)
                        {
                            double num3 = a[num2, k];
                            a[num2, k] = a[num, k];
                            a[num, k] = num3;
                        }
                    }
                    for (int l = 0; l < n; l++)
                    {
                        if (l != num)
                        {
                            for (int num4 = n; num4 >= num; num4--)
                            {
                                a[l, num4] -= a[l, i] / a[num, i] * a[num, num4];
                            }
                        }
                    }
                }
                num++;
            }
            return num;
        }

        private void gauss_jordan2(int n)
        {
            int num = 0;
            for (int i = 0; i < n - 1; i++)
            {
                if (a[i, num] != 0.0)
                {
                    double num2 = a[i, num];
                    for (int j = i; j < n; j++)
                    {
                        if (a[j, num] == 0.0)
                        {
                            continue;
                        }
                        double num3 = a[j, num];
                        for (int k = num; k < n + 1; k++)
                        {
                            if (j == i)
                            {
                                a[j, k] /= num2;
                            }
                            else
                            {
                                a[j, k] -= num3 * a[i, k];
                            }
                        }
                    }
                }
                num++;
            }
        }

        public Guass(int rowCount, double[,] valus)
        {
            a = valus;
            gauss_jordan2(rowCount);
            d--;
            ans = new double[rowCount];
            d = rowCount - 1;
            for (int num = d; num >= 0; num--)
            {
                for (int i = num + 1; i < rowCount; i++)
                {
                    a[num, rowCount] -= a[num, i] * ans[i];
                }
                ans[num] = a[num, rowCount] / a[num, num];
            }
            for (int j = 0; j < rowCount; j++)
            {
                ans[j] = a[j, rowCount] / a[j, j];
            }
        }
    }
}