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

【日志】
最新版本见于“整个源码”部分,其他部分还是首发代码。
6.12 首发此篇博客,球心坐标算法有问题待解决
6.21 优化了代码:求圆心算法简化了一下;求心前先重心化,计算的数字就会很小,易算;求球心那个“简单算法”太复杂,找到一个前辈写的用克莱姆法则求四点球心的代码,稍加改编,用作求球心的初值。并且更新了用克莱姆法则求解球心坐标的原理见于原理部分(现在觉得之前的解法太蠢太笨了!)并且开发了另一个平差方法,相较于之前的平差方法其优点是不用迭代直接平差可解!
前辈的克莱姆法则求球心算法原文链接:

https://blog.csdn.net/yrc1993/article/details/7907894

6.25 加入结构图,方便下次观看。

一、算法原理

1. 求圆心坐标

C# 求取圆心/球心坐标 ∈ C# 编程笔记_第1张图片
如上图所示,已知圆上两点P1, P2 点坐标和圆半径,求圆心点O 的坐标。
1.先来个简单
C# 求取圆心/球心坐标 ∈ C# 编程笔记_第2张图片
所以:三点唯一确定一个圆。
但是如果点数不止三个,有多个点呢,这个时候就要平差了。
2.平差法
C# 求取圆心/球心坐标 ∈ C# 编程笔记_第3张图片
可用上面的 简单求解法 来求初值(最开始迭代的近似值)。

2. 求球心坐标

球坐标系如下图(图片来源于百度百科):
C# 求取圆心/球心坐标 ∈ C# 编程笔记_第4张图片
首先来想一下,几个点 + 一个半径可以确定一个球?直接想不好想,那么可以以已知点为球心,R 为半径画球面。(为了偷个懒,我从GNSS课件上盗了几个图)
C# 求取圆心/球心坐标 ∈ C# 编程笔记_第5张图片
得出的结论为:四个点唯一确定一个球心。

如下图所示,已知四点坐标及半径,求球心坐标(图中坐标系只是辅助看图,不一定是实际坐标系)。
C# 求取圆心/球心坐标 ∈ C# 编程笔记_第6张图片
类似于上面推导圆心坐标的方式,我们也可以先用三个点来求出两个球心这种简单方法。当有多于四个(含四)点时,用平差的方法。
1.简单解法
C# 求取圆心/球心坐标 ∈ C# 编程笔记_第7张图片
2020/6/21
上面的解法是硬刚出来的,超级麻烦,下面介绍一种用四个点求球心(不需知道半径)的简单(这次是真简单)方法:
假设,已知的四个点的坐标为:(x1,y1,z1), (x2,y2,z2), (x3,y3,z3), (x4,y4,z4),于是有:
C# 求取圆心/球心坐标 ∈ C# 编程笔记_第8张图片
上面这种算法好处很多:1 小区了未知数的二次项 2 将非线性方程转化成了线性方程,这样就不用迭代,直接可平差,特别优秀!

当然也可以根据克莱姆法则来求解:

https://blog.csdn.net/wodownload2/article/details/105724964

2.平差法
C# 求取圆心/球心坐标 ∈ C# 编程笔记_第9张图片

二、源码

源码结构图:
C# 求取圆心/球心坐标 ∈ C# 编程笔记_第10张图片

1. 整个源码

	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 俩圆心点数组
         * 解决了 x 或 y 不能相等的弊端
         */
        public static Point[] YS(double R, Point p1, Point p2)
        {
            Point[] center = new Point[2];
            double a1 = 0, a2 = 0, b1 = 0, b2 = 0;
            int mode = 1;//一个状态参量 x不等1  y不等2;
            if ((p1.x - p2.x) == 0)
            {
                a1 = p1.x; b1 = p1.y; a2 = p2.x; b2 = p2.y;
                mode = 2;
            }
            else{
                a1 = p1.y; b1 = p1.x; a2 = p2.y; b2 = p2.x;
            }

            double C1 = (a2 *a2 - a1 * a1 + b2 * b2 - b1 * b1) / (2 * (b2 - b1)),
                C2 = (a2 - a1) / (b2 - b1),
                A = 1 + C2 * C2,
                B = 2 * ((b1 - C1) * C2 - a1),
                C = (b1 - C1) * (b1 - C1) + a1 * a1 - R * R;
            double a = (-B + Math.Sqrt(B * B - 4 * A * C)) / (2 * A);
            if(mode==1)
                center[0] = new Point(a, C1 - C2 * a);
            else
                center[0] = new Point(C1 - C2 * a,a);
            a = (-B - Math.Sqrt(B * B - 4 * A * C)) / (2 * A);
            if (mode == 1)
                center[1] = new Point(a, C1 - C2 * a);
            else
                center[1] = new Point(C1 - C2 * a, a);
            return center;         
        }
        /*此函数用平差方法求圆心坐标
         * 输入:R 半径, pt 已知点数组
         * 输出:center 圆心坐标
         */
        public static Point YPC(double R, Point[] Pt)
        {
            int n = Pt.Length, r = n - 2;
            Point[] pt = Point.ZXH(Pt);//重心化
            Point center = new Point();
            Point[] c1 = YS(R, pt[0], pt[1]);
            if ((pt[n - 1].Dis(c1[0]) - R) > (pt[n - 1].Dis(c1[1]) - R))
                center = c1[1];
            else
                center = c1[0];

            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+pt[n];
        }
        /*此函数再已知哪个是起点 弧长短的情况下 求圆心坐标
         * 输入: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;
        }
        /*空间四点确定球心坐标(克莱姆法则)
         * 输入:点数组,用前四个点算球心
         * 输出:球心坐标, 半径。
         */ 
        public static double[] get_xyz(Point[] pt)
        {
            Point p1=pt[0],
                p2=pt[1],
                p3=pt[2],
                p4=pt[3];
            double x1 = p1.x, x2 = p2.x, x3 = p3.x, x4 = p4.x,
                y1 = p1.y, y2 = p2.y, y3 = p3.y, y4 = p4.y,
                z1 = p1.z, z2 = p2.z, z3 = p3.z, z4 = p4.z;
            double a11,a12,a13,a21,a22,a23,a31,a32,a33,b1,b2,b3,d,d1,d2,d3,x,y,z,R;
            a11=2*(x2-x1); a12=2*(y2-y1); a13=2*(z2-z1);
            a21=2*(x3-x2); a22=2*(y3-y2); a23=2*(z3-z2);
            a31=2*(x4-x3); a32=2*(y4-y3); a33=2*(z4-z3);
            b1=x2*x2-x1*x1+y2*y2-y1*y1+z2*z2-z1*z1;
            b2=x3*x3-x2*x2+y3*y3-y2*y2+z3*z3-z2*z2;
            b3=x4*x4-x3*x3+y4*y4-y3*y3+z4*z4-z3*z3;
            d=a11*a22*a33+a12*a23*a31+a13*a21*a32-a11*a23*a32-a12*a21*a33-a13*a22*a31;
            d1=b1*a22*a33+a12*a23*b3+a13*b2*a32-b1*a23*a32-a12*b2*a33-a13*a22*b3;
            d2=a11*b2*a33+b1*a23*a31+a13*a21*b3-a11*a23*b3-b1*a21*a33-a13*b2*a31;
            d3=a11*a22*b3+a12*b2*a31+b1*a21*a32-a11*b2*a32-a12*a21*b3-b1*a22*a31;
            x=d1/d;
            y=d2/d;
            z=d3/d;
            R = new Point(x, y, z).Dis(pt[0]);
            return new double[]{x,y,z,R};
        }
        /* 此函数用一种巧妙的方法,将球面方程线性化,然后平差求解。 - 由get_xyz 函数启发而来,独立函数
         * 输入:点数组,
         * 输出:球心坐标, 半径。
         */
        public static double[] get_xyz1(Point[] pt)
        {
            int n = pt.Length;
            Matrix B = new Matrix(n - 1, 3);
            Matrix L = new Matrix(n - 1, 1);
            for (int i = 0; i < n - 1; i++)
            {
                Point p1 = pt[i], p2 = pt[i + 1];
                double x1 = p1.x, y1 = p1.y, z1 = p1.z,
                    x2 = p2.x, y2 = p2.y, z2 = p2.z;
                B[i, 0] = 2 * (x2 - x1);
                B[i, 1] = 2 * (y2 - y1);
                B[i, 2] = 2 * (z2 - z1);
                L[i, 0] = p2.Dis2To0() - p1.Dis2To0();
            }
            Matrix Nbb = B.Transpose() * B;
            Nbb.InvertGaussJordan();
            Matrix X = Nbb * B.Transpose() * L;
            double x = X[0, 0], y = X[1, 0], z = X[2, 0], R, Rsum = 0;
            Point center = new Point(x, y, z);
            for (int i = 0; i < n; i++)
            {
                Rsum += pt[i].Dis(center);
            }
            R = Rsum / n;
            return new double[] { x, y, z, R };
        }

        //下面的俩函数仍有缺陷
        /*此函数用简单方法求球心坐标 - 暂未搞定
         * 输入: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;
        }
        /*此函数用平差方法求球心坐标 - 有缺陷,实验数据2得到的结果不好
         * 输入: pt 已知点数组
         * 输出: xyz R
         * 
         */
        public static double[] QPC(Point[] Pt)
        {
            int n = Pt.Length, r = n - 4;
            Point[] pt = Point.ZXH(Pt);//先对这些坐标重心化,然后在平差
            double[] aa = get_xyz(pt);
            Point center = new Point(aa[0], aa[1], aa[2]);
            double R = aa[3];

            Matrix B = new Matrix(n, 4),
                L = new Matrix(n, 1),
                X = new Matrix(4, 1),
                Zero = new Matrix(4, 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, 2] = 2 * (z0 - z);
                    B[i, 3] = 2 * R;
                    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];
                center.z += X[2, 0];
                R += X[3, 0];
                X.Eps = 1e-3;
            } while (!X.Equals(Zero));
            Matrix V = B * X - L;
            double cgma2 = (V.Transpose() * V)[0, 0] / r;//单位权中误差的平方。
            x0 += pt[n].x; y0 += pt[n].y; z0 += pt[n].z;
            return new double[] { x0, y0, z0, R };//再加回到重心上。
        }
    }

其所依附的几个类:

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

2. 调用示例

double R = 10.1;         
Point cen = new Point(5.1, 10.2, 2.5);//用这两行生成求圆心、球心的数据
GetCenter.YPC(R, GetCenter.YData(R, cen)).show();//平差法 求圆心
double[] ans=GetCenter.QPC(GetCenter.QData(R, cen));//平差法1 求球心
Console.WriteLine("X:{0}   Y:{1}    Z:{2}    R:{3}", ans[0], ans[1], ans[2], ans[3]);
ans = GetCenter.get_xyz1(GetCenter.QData(R, cen));//平差法2 求球心
Console.WriteLine("X:{0}   Y:{1}    Z:{2}    R:{3}", ans[0], ans[1], ans[2], ans[3]);
double R1 = 10.1;
Point[] pt1 = new Point[4];
pt1[0] = new Point(1, 13.03);
pt1[1] = new Point(2, -5.8125);
pt1[2] = new Point(3, 13.679);
pt1[3] = new Point(4, -6.24);
GetCenter.YPC(R1, pt1).show();
==============================================
X 5.100,   Y 10.200,   Z 0.000,   Name 0
X:5.1   Y:10.2    Z:2.5    R:10.1
X:5.1   Y:10.2    Z:2.5    R:10.1
X 5.101,   Y 3.800,   Z 0.000,   Name 0

参考/引用 文章

[1] wolves_liu-CSDN博主:https://blog.csdn.net/yaodaoji/article/details/81540883
[2] yrc1993-CSDN博主:https://blog.csdn.net/yrc1993/article/details/7907894
[3] wodownload2-CSDN博主:https://blog.csdn.net/wodownload2/article/details/105724964

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

你可能感兴趣的:(#,C#,算法,c#)