C# 求取圆心/球心坐标 ∈ C# 编程笔记

一、求圆心坐标

1.算法原理

如上图所示，已知圆上两点P1, P2 点坐标和圆半径，求圆心点O 的坐标。
1.先来个简单

所以：三点唯一确定一个圆。
但是如果点数不止三个，有多个点呢，这个时候就要平差了。
2.平差法

可用上面的 简单求解法 来求初值（最开始迭代的近似值）。

2.代码实现

		/*此函数用简单方法求圆心坐标
         * 输入：R 半径, p1 p2 已知的圆上两点
         * 输出：center 俩圆心点数组
         * 有个弊端：p1,p2 的x 不能相等
         */
        public static Point[] YSx(double R, Point p1, Point p2)
        {
            Point[] center = new Point[2];
            double x1 = p1.x, y1 = p1.y, x2 = p2.x, y2 = p2.y;
            double C1 = (x2 * x2 - x1 * x1 + y2 * y2 - y1 * y1) / (2 * (x2 - x1)),
                C2 = (y2 - y1) / (x2 - x1),
                A = 1 + C2 * C2,
                B = 2 * ((x1 - C1) * C2 - y1),
                C = (x1 - C1) * (x1 - C1) + y1 * y1 - R * R;
            double y = (-B + Math.Sqrt(B * B - 4 * A * C)) / (2 * A);
            center[0] = new Point(C1 - C2 * y, y);
            y = (-B - Math.Sqrt(B * B - 4 * A * C)) / (2 * A);
            center[1] = new Point(C1 - C2 * y, y);
            return center;
        }
        /*此函数用简单方法求圆心坐标
         * 输入：R 半径, p1 p2 已知的圆上两点
         * 输出：center 俩圆心点数组
         * 有个弊端：p1,p2 的y 不能相等
         */
        public static Point[] YSy(double R, Point p1, Point p2)
        {
            Point[] center = new Point[2];
            double x1 = p1.x, y1 = p1.y, x2 = p2.x, y2 = p2.y;
            double C1 = (x2 * x2 - x1 * x1 + y2 * y2 - y1 * y1) / (2 * (y2 - y1)),
                C2 = (x2 - x1) / (y2 - y1),
                A = 1 + C2 * C2,
                B = 2 * ((y1 - C1) * C2 - x1),
                C = (y1 - C1) * (y1 - C1) + x1 * x1 - R * R;
            double x = (-B + Math.Sqrt(B * B - 4 * A * C)) / (2 * A);
            center[0] = new Point(x, C1 - C2 * x);
            x = (-B - Math.Sqrt(B * B - 4 * A * C)) / (2 * A);
            center[1] = new Point(x, C1 - C2 * x);
            return center;
        }
        /*此函数用简单方法求圆心坐标
         * 输入：R 半径, p1 p2 已知的圆上两点
         * 输出：center 俩圆心点数组
         * 解决了上述两弊端
         */
        public static Point[] YS(double R, Point p1, Point p2)
        {
            if ((p1.x - p2.x) != 0) return YSx(R, p1, p2);
            else return YSy(R, p1, p2);
        }
        /*此函数用平差方法求圆心坐标
         * 输入：R 半径, pt 已知点数组
         * 输出：center 圆心坐标
         */
        public static Point YPC(double R, Point[] pt)
        {
            int n = pt.Length, r = n - 2;
            Point center = new Point();
            Point[] c1 = YS(R, pt[0], pt[1]);
            Point[] c2 = YS(R, pt[0], pt[n - 1]);
            Point[] dp = new Point[4];
            dp[0] = c1[1] - c2[1];
            dp[1] = c1[0] - c2[0];
            dp[2] = c1[0] - c2[1];
            dp[3] = c1[1] - c2[0];
            int index = Point.find_Close0(dp);
            switch (index)
            {//找到初值(近似值)
                case 0:
                    center = (c1[1] + c2[1]) / 2;
                    break;
                case 1:
                    center = (c1[0] + c2[0]) / 2;
                    break;
                case 2:
                    center = (c1[0] + c2[1]) / 2;
                    break;
                case 3:
                    center = (c1[1] + c2[0]) / 2;
                    break;
            }

            Matrix B = new Matrix(n, 2),
                L = new Matrix(n, 1),
                X = new Matrix(2, 1),
                Zero = new Matrix(2, 1);

            double x0 = 0, y0 = 0;
            do
            {
                x0 = center.x;
                y0 = center.y;
                for (int i = 0; i < n; i++)
                {
                    double x = pt[i].x, y = pt[i].y;
                    B[i, 0] = 2 * (x0 - x);
                    B[i, 1] = 2 * (y0 - y);
                    L[i, 0] = 2 * x0 * x + 2 * y * y0 - pt[i].Dis2To0() - center.Dis2To0() + R * R;
                }
                Matrix Nbb = B.Transpose() * B;
                Nbb.InvertGaussJordan();
                X = Nbb * B.Transpose() * L;
                center.x += X[0, 0];
                center.y += X[1, 0];
                X.Eps = 1e-6;
            } while (!X.Equals(Zero));
            Matrix V = B * X - L;
            double cgma2 = (V.Transpose() * V)[0, 0] / r;//单位权中误差的平方。
            return center;
        }
        /*此函数再已知哪个是起点（逆时针）的情况下 求圆心坐标
         */
        public static Point Ycenter(double R, Point p1, Point p2)
        {
            Point E = (p1 + p2) / 2;
            Point[] p = new Point[3];
            p[0] = p1;
            p[1] = p2;
            double s1 = p1.Dis(p2) / 2,
                s3 = Math.Sqrt(R * R - s1 * s1),
                s2 = R - s3;
            Angle a = p1.getAn(p2),
                b = new Angle(Math.PI / 2),
                c = a - b;//-
            p[2] = new Point(E, s2, c);//至此，求出第三个点
            return YPC(R, p);
        }

3.调用实例

			double R = 10.1;
            Point[] pt = new Point[4];
            pt[0] = new Point(1, 13.03);
            pt[1] = new Point(2, -5.8125);
            pt[2] = new Point(3, 13.679);
            pt[3] = new Point(4, -6.24);
            GetCenter.YPC(R, pt).show();
===============================================
X 5.101,   Y 3.800,   Z 0.000,   Name 0

二、求球心坐标

1.算法原理

球坐标系如下图（图片来源于百度百科）：

首先来想一下，几个点 + 一个半径可以确定一个球？直接想不好想，那么可以以已知点为球心，R 为半径画球面。（为了偷个懒，我从GNSS课件上盗了几个图）

得出的结论为：四个点唯一确定一个球心。

如下图所示，已知四点坐标及半径，求球心坐标（图中坐标系只是辅助看图，不一定是实际坐标系）。

类似于上面推导圆心坐标的方式，我们也可以先用三个点来求出两个球心这种简单方法。当有多于四个（含四）点时，用平差的方法。
1.简单解法

2.平差法

2.代码实现

2020/6/12
此代码有bug，目前找不出来，有时间来补坑。

		/*此函数用简单方法求球心坐标
         * 输入：R 半径, p1 p2 p3已知的上三点
         * 输出：center 俩圆心点数组
         */
        public static Point[] QS(double R, Point p1, Point p2, Point p3)
        {
            Point[] center = new Point[2];
            double x1 = p1.x, y1 = p1.y, z1 = p1.z,
                x2 = p2.x, y2 = p2.y, z2 = p2.z,
                x3 = p3.x, y3 = p3.y, z3=p3.z;
            double C12 = p1.Dis2To0() - p2.Dis2To0(),
                C23 = p2.Dis2To0() - p3.Dis2To0(),
                x21 = x2 - x1, y21 = y2 - y1, z21 = z2 - z1,
                x32 = x3 - x2, y32 = y3 - y2, z32 = z3 - z2,
                C1=(C23*x21-C12*x32) / (2*(x32*z21-x21*z32)),
                C2=-(x32*y21-x21*y32) / (x32*z21-x21*z32),

                D1=-(C12+2*C1*z21)/(2*(y21+z21*C2)),
                D2=-x21/(y21+z21*C2),
                A = 1 + D2 * D2 + C2 * C2 * D2 * D2,
                B = -2 * x1 - 2 * y1 * D2 - 2 * z1 * C2 * D2 + 2 * D1 * D2 + 2 * (C1 + C2 * D1) * C2 * D2,
                C = p1.Dis2To0() - 2 * y1 * D1 - 2 * z1 * (C1 + C2 * D1) + D1 * D1 + (C1 + C2 * D1) * (C1 + C2 * D1);
            double x = (-B + Math.Sqrt(B * B - 4 * A * C)) / (2 * A),
                y = D1 + D2 * x,
                z = C1 + C2 * y;
            center[0] = new Point(x, y, z);
            x = (-B - Math.Sqrt(B * B - 4 * A * C)) / (2 * A);
            y = D1 + D2 * x;
            z = C1 + C2 * y;
            center[1] = new Point(x, y, z);
            return center;
        }
        /*此函数用平差方法求球心坐标
         * 输入：R 半径, pt 已知点数组
         * 输出：center 球心坐标
         */
        public static Point QPC(double R, Point[] pt)
        {
            int n = pt.Length, r = n - 2;
            Point center = new Point();
            Point[] c1 = QS(R, pt[0], pt[1], pt[2]);
            Point[] c2 = QS(R, pt[0], pt[n-2],pt[n - 1]);
            Point[] dp = new Point[4];
            dp[0] = c1[1] - c2[1];
            dp[1] = c1[0] - c2[0];
            dp[2] = c1[0] - c2[1];
            dp[3] = c1[1] - c2[0];
            int index = Point.find_Close0(dp);
            switch (index)
            {//找到初值(近似值)
                case 0:
                    center = (c1[1] + c2[1]) / 2;
                    break;
                case 1:
                    center = (c1[0] + c2[0]) / 2;
                    break;
                case 2:
                    center = (c1[0] + c2[1]) / 2;
                    break;
                case 3:
                    center = (c1[1] + c2[0]) / 2;
                    break;
            }

            Matrix B = new Matrix(n, 3),
                L = new Matrix(n, 1),
                X = new Matrix(3, 1),
                Zero = new Matrix(3, 1);

            double x0 = 0, y0 = 0, z0=0;
            do
            {
                x0 = center.x;
                y0 = center.y;
                z0 = center.z;
                for (int i = 0; i < n; i++)
                {
                    double x = pt[i].x, y = pt[i].y, z = pt[i].z;
                    B[i, 0] = 2 * (x0 - x);
                    B[i, 1] = 2 * (y0 - y);
                    B[i, 1] = 2 * (z0 - z);
                    L[i, 0] = 2 * x0 * x + 2 * y * y0 + 2 * z * z0 - pt[i].Dis2To0() - center.Dis2To0()+ R * R;
                }
                Matrix Nbb = B.Transpose() * B;
                Nbb.InvertGaussJordan();
                X = Nbb * B.Transpose() * L;
                center.x += X[0, 0];
                center.y += X[1, 0];
                X.Eps = 1e-6;
            } while (!X.Equals(Zero));
            Matrix V = B * X - L;
            double cgma2 = (V.Transpose() * V)[0, 0] / r;//单位权中误差的平方。
            return center;
        }

3.调用实例

			double R = 10.1;
            Point[] pt = new Point[5];
            pt[0] = new Point(1, 2, 11.553);
            pt[1] = new Point(2, 3, -7.079);
            pt[2] = new Point(5, 6, 12.357);
            pt[3] = new Point(7, 8, -6.4866);
            pt[4] = new Point(4, 3, 12.508);
            GetCenter.QPC(R, pt).show();

三、整个源码

	class GetCenter
    {
        /* 辅助函数-求随机数
         * 可选输入：a 下界, b 上界
         */
        public static double getRandom(double a = 0, double b = 100)
        {

            byte[] bytes = new byte[4];
            System.Security.Cryptography.RNGCryptoServiceProvider r = new System.Security.Cryptography.RNGCryptoServiceProvider();
            r.GetBytes(bytes);
            double g = Math.Abs(BitConverter.ToInt32(bytes, 0)) / 100.0;
            double c = b - a;
            return g % c + a;//得到在[a,b)范围内的随机数
        }
        /*此函数用于生成求圆心的实验数据
         * 输入：R 半径 cen 圆心坐标
         */
        public static Point[] YData(double R, Point cen)
        {
            Point[] pt = new Point[10];
            double x = cen.x, y = cen.y;
            for (int i = 0; i < 10; i++)
            {
                double x1 = getRandom(x - R, x + R),
                    dx = x1 - x,
                    dy = Math.Sqrt(R * R - dx * dx),
                    y1 = dy + y;
                if (i % 2 == 0) y1 = y1 - 2 * dy;
                pt[i] = new Point(x1, y1);
            }
            return pt;
        }
        /*此函数用简单方法求圆心坐标
         * 输入：R 半径, p1 p2 已知的圆上两点
         * 输出：center 俩圆心点数组
         * 有个弊端：p1,p2 的x 不能相等
         */
        public static Point[] YSx(double R, Point p1, Point p2)
        {
            Point[] center = new Point[2];
            double x1 = p1.x, y1 = p1.y, x2 = p2.x, y2 = p2.y;
            double C1 = (x2 * x2 - x1 * x1 + y2 * y2 - y1 * y1) / (2 * (x2 - x1)),
                C2 = (y2 - y1) / (x2 - x1),
                A = 1 + C2 * C2,
                B = 2 * ((x1 - C1) * C2 - y1),
                C = (x1 - C1) * (x1 - C1) + y1 * y1 - R * R;
            double y = (-B + Math.Sqrt(B * B - 4 * A * C)) / (2 * A);
            center[0] = new Point(C1 - C2 * y, y);
            y = (-B - Math.Sqrt(B * B - 4 * A * C)) / (2 * A);
            center[1] = new Point(C1 - C2 * y, y);
            return center;
        }
        /*此函数用简单方法求圆心坐标
         * 输入：R 半径, p1 p2 已知的圆上两点
         * 输出：center 俩圆心点数组
         * 有个弊端：p1,p2 的y 不能相等
         */
        public static Point[] YSy(double R, Point p1, Point p2)
        {
            Point[] center = new Point[2];
            double x1 = p1.x, y1 = p1.y, x2 = p2.x, y2 = p2.y;
            double C1 = (x2 * x2 - x1 * x1 + y2 * y2 - y1 * y1) / (2 * (y2 - y1)),
                C2 = (x2 - x1) / (y2 - y1),
                A = 1 + C2 * C2,
                B = 2 * ((y1 - C1) * C2 - x1),
                C = (y1 - C1) * (y1 - C1) + x1 * x1 - R * R;
            double x = (-B + Math.Sqrt(B * B - 4 * A * C)) / (2 * A);
            center[0] = new Point(x, C1 - C2 * x);
            x = (-B - Math.Sqrt(B * B - 4 * A * C)) / (2 * A);
            center[1] = new Point(x, C1 - C2 * x);
            return center;
        }
        /*此函数用简单方法求圆心坐标
         * 输入：R 半径, p1 p2 已知的圆上两点
         * 输出：center 俩圆心点数组
         * 解决了上述两弊端
         */
        public static Point[] YS(double R, Point p1, Point p2)
        {
            if ((p1.x - p2.x) != 0) return YSx(R, p1, p2);
            else return YSy(R, p1, p2);
        }
        /*此函数用平差方法求圆心坐标
         * 输入：R 半径, pt 已知点数组
         * 输出：center 圆心坐标
         */
        public static Point YPC(double R, Point[] pt)
        {
            int n = pt.Length, r = n - 2;
            Point center = new Point();
            Point[] c1 = YS(R, pt[0], pt[1]);
            Point[] c2 = YS(R, pt[0], pt[n - 1]);
            Point[] dp = new Point[4];
            dp[0] = c1[1] - c2[1];
            dp[1] = c1[0] - c2[0];
            dp[2] = c1[0] - c2[1];
            dp[3] = c1[1] - c2[0];
            int index = Point.find_Close0(dp);
            switch (index)
            {//找到初值(近似值)
                case 0:
                    center = (c1[1] + c2[1]) / 2;
                    break;
                case 1:
                    center = (c1[0] + c2[0]) / 2;
                    break;
                case 2:
                    center = (c1[0] + c2[1]) / 2;
                    break;
                case 3:
                    center = (c1[1] + c2[0]) / 2;
                    break;
            }

            Matrix B = new Matrix(n, 2),
                L = new Matrix(n, 1),
                X = new Matrix(2, 1),
                Zero = new Matrix(2, 1);

            double x0 = 0, y0 = 0;
            do
            {
                x0 = center.x;
                y0 = center.y;
                for (int i = 0; i < n; i++)
                {
                    double x = pt[i].x, y = pt[i].y;
                    B[i, 0] = 2 * (x0 - x);
                    B[i, 1] = 2 * (y0 - y);
                    L[i, 0] = 2 * x0 * x + 2 * y * y0 - pt[i].Dis2To0() - center.Dis2To0() + R * R;
                }
                Matrix Nbb = B.Transpose() * B;
                Nbb.InvertGaussJordan();
                X = Nbb * B.Transpose() * L;
                center.x += X[0, 0];
                center.y += X[1, 0];
                X.Eps = 1e-6;
            } while (!X.Equals(Zero));
            Matrix V = B * X - L;
            double cgma2 = (V.Transpose() * V)[0, 0] / r;//单位权中误差的平方。
            return center;
        }
        /*此函数再已知哪个是起点 弧长短的情况下 求圆心坐标
         * 输入：R 半径, p1 起点, p2 终点, k 优弧=1 劣弧=2
         */
        public static Point Ycenter(double R, Point p1, Point p2, int k=1)
        {
            Point E = (p1 + p2) / 2;
            Point[] p = new Point[3];
            p[0] = p1;
            p[1] = p2;
            double s1 = p1.Dis(p2) / 2,
                s3 = Math.Sqrt(R * R - s1 * s1),
                s2 = R - s3;
            if (k == 2) s2 = R + s3;//劣弧的情况下
            Angle a = p1.getAn(p2),
                b = new Angle(Math.PI / 2),
                c = a - b;//-
            p[2] = new Point(E, s2, c);//至此，求出第三个点
            return YPC(R, p);
        }

        /*此函数用于生成求球心的实验数据
         * 输入：R 半径 cen 圆心坐标
         */
        public static Point[] QData(double R, Point cen)
        {
            Point[] pt = new Point[16];
            double x = cen.x, y = cen.y, z = cen.z;
            for (int i = 0; i < 4; i++)
            {
                for (int j = 0; j < 4; j++)
                {
                    double x1 = getRandom(x - R, x + R),
                        dx = x1 - x,
                        R0 = Math.Sqrt(R * R - dx * dx),
                        y1 = getRandom(y - R0, y + R0),
                        dy = y1 - y,
                        dz = Math.Sqrt(R * R - dx * dx - dy * dy),
                        z1 = dz + z;
                    if (j % 2 == 0) z1 = z1 - 2 * dz;
                    int k = j * 4 + i;
                    pt[k] = new Point(x1, y1, z1);
                }
            }
            return pt;
        }
        /*此函数用简单方法求球心坐标
         * 输入：R 半径, p1 p2 p3已知的上三点
         * 输出：center 俩圆心点数组
         */
        public static Point[] QS(double R, Point p1, Point p2, Point p3)
        {
            Point[] center = new Point[2];
            double x1 = p1.x, y1 = p1.y, z1 = p1.z,
                x2 = p2.x, y2 = p2.y, z2 = p2.z,
                x3 = p3.x, y3 = p3.y, z3 = p3.z;
            double C12 = p1.Dis2To0() - p2.Dis2To0(),
                C23 = p2.Dis2To0() - p3.Dis2To0(),
                x21 = x2 - x1, y21 = y2 - y1, z21 = z2 - z1,
                x32 = x3 - x2, y32 = y3 - y2, z32 = z3 - z2,
                C1 = (C23 * x21 - C12 * x32) / (2 * (x32 * z21 - x21 * z32)),
                C2 = -(x32 * y21 - x21 * y32) / (x32 * z21 - x21 * z32),

                D1 = -(C12 + 2 * C1 * z21) / (2 * (y21 + z21 * C2)),
                D2 = -x21 / (y21 + z21 * C2),
                A = 1 + D2 * D2 + C2 * C2 * D2 * D2,
                B = -2 * x1 - 2 * y1 * D2 - 2 * z1 * C2 * D2 + 2 * D1 * D2 + 2 * (C1 + C2 * D1) * C2 * D2,
                C = p1.Dis2To0() - 2 * y1 * D1 - 2 * z1 * (C1 + C2 * D1) + D1 * D1 + (C1 + C2 * D1) * (C1 + C2 * D1);
            double x = (-B + Math.Sqrt(B * B - 4 * A * C)) / (2 * A),
                y = D1 + D2 * x,
                z = C1 + C2 * y;
            center[0] = new Point(x, y, z);
            x = (-B - Math.Sqrt(B * B - 4 * A * C)) / (2 * A);
            y = D1 + D2 * x;
            z = C1 + C2 * y;
            center[1] = new Point(x, y, z);
            return center;
        }
        /*此函数用平差方法求球心坐标
         * 输入：R 半径, pt 已知点数组
         * 输出：center 球心坐标
         */
        public static Point QPC(double R, Point[] pt)
        {
            int n = pt.Length, r = n - 2;
            Point center = new Point();
            Point[] c1 = QS(R, pt[0], pt[1], pt[2]);
            Point[] c2 = QS(R, pt[0], pt[n - 2], pt[n - 1]);
            Point[] dp = new Point[4];
            dp[0] = c1[1] - c2[1];
            dp[1] = c1[0] - c2[0];
            dp[2] = c1[0] - c2[1];
            dp[3] = c1[1] - c2[0];
            int index = Point.find_Close0(dp);
            switch (index)
            {//找到初值(近似值)
                case 0:
                    center = (c1[1] + c2[1]) / 2;
                    break;
                case 1:
                    center = (c1[0] + c2[0]) / 2;
                    break;
                case 2:
                    center = (c1[0] + c2[1]) / 2;
                    break;
                case 3:
                    center = (c1[1] + c2[0]) / 2;
                    break;
            }

            Matrix B = new Matrix(n, 3),
                L = new Matrix(n, 1),
                X = new Matrix(3, 1),
                Zero = new Matrix(3, 1);

            double x0 = 0, y0 = 0, z0 = 0;
            do
            {
                x0 = center.x;
                y0 = center.y;
                z0 = center.z;
                for (int i = 0; i < n; i++)
                {
                    double x = pt[i].x, y = pt[i].y, z = pt[i].z;
                    B[i, 0] = 2 * (x0 - x);
                    B[i, 1] = 2 * (y0 - y);
                    B[i, 1] = 2 * (z0 - z);
                    L[i, 0] = 2 * x0 * x + 2 * y * y0 + 2 * z * z0 - pt[i].Dis2To0() - center.Dis2To0() + R * R;
                }
                Matrix Nbb = B.Transpose() * B;
                Nbb.InvertGaussJordan();
                X = Nbb * B.Transpose() * L;
                center.x += X[0, 0];
                center.y += X[1, 0];
                X.Eps = 1e-6;
            } while (!X.Equals(Zero));
            Matrix V = B * X - L;
            double cgma2 = (V.Transpose() * V)[0, 0] / r;//单位权中误差的平方。
            return center;
        }
    }

其所依附的几个类：

https://blog.csdn.net/Gou_Hailong/article/details/88989274
https://blog.csdn.net/Gou_Hailong/article/details/98451032

调用示例

double R = 10.1;
Point cen = new Point(5.1, 3.8, 2.5);
//GetCenter.YPC(R, GetCenter.YData(R, cen)).show();
//GetCenter.QPC(R, GetCenter.QData(R, cen)).show();

参考/引用 文章

[1] wolves_liu-CSDN博主：https://blog.csdn.net/yaodaoji/article/details/81540883

【注1】其中的代码也许并不完整，您可以作为伪码参看，或者您可以去我主博客逛逛，也许有意外之喜！
【注2】此篇博客是 C# 编程笔记 的子博客。
【注3】由于博主水平有限，程序可能存在漏洞或bug， 如有发现，请尽快与博主联系！

转载：https://blog.csdn.net/Gou_Hailong/article/details/106686355