C# 求取圆心/球心坐标 ∈ C# 编程笔记
【日志】
最新版本见于“整个源码”部分，其他部分还是首发代码。
6.12 首发此篇博客，球心坐标算法有问题待解决
6.21 优化了代码：求圆心算法简化了一下；求心前先重心化，计算的数字就会很小，易算；求球心那个“简单算法”太复杂，找到一个前辈写的用克莱姆法则求四点球心的代码，稍加改编，用作求球心的初值。并且更新了用克莱姆法则求解球心坐标的原理见于原理部分（现在觉得之前的解法太蠢太笨了！）并且开发了另一个平差方法，相较于之前的平差方法其优点是不用迭代直接平差可解！
前辈的克莱姆法则求球心算法原文链接：
https://blog.csdn.net/yrc1993/article/details/7907894
6.25 加入结构图，方便下次观看。
一、算法原理
1. 求圆心坐标
如上图所示，已知圆上两点P1, P2 点坐标和圆半径，求圆心点O 的坐标。
1.先来个简单
所以：三点唯一确定一个圆。
但是如果点数不止三个，有多个点呢，这个时候就要平差了。
2.平差法
可用上面的 简单求解法 来求初值（最开始迭代的近似值）。
2. 求球心坐标
球坐标系如下图（图片来源于百度百科）：
首先来想一下，几个点 + 一个半径可以确定一个球？直接想不好想，那么可以以已知点为球心，R 为半径画球面。（为了偷个懒，我从GNSS课件上盗了几个图）
得出的结论为：四个点唯一确定一个球心。
如下图所示，已知四点坐标及半径，求球心坐标（图中坐标系只是辅助看图，不一定是实际坐标系）。
类似于上面推导圆心坐标的方式，我们也可以先用三个点来求出两个球心这种简单方法。当有多于四个（含四）点时，用平差的方法。
1.简单解法
2020/6/21
上面的解法是硬刚出来的，超级麻烦，下面介绍一种用四个点求球心（不需知道半径）的简单（这次是真简单）方法：
假设，已知的四个点的坐标为：(x1,y1,z1), (x2,y2,z2), (x3,y3,z3), (x4,y4,z4)，于是有：
上面这种算法好处很多：1 小区了未知数的二次项 2 将非线性方程转化成了线性方程，这样就不用迭代，直接可平差，特别优秀！
当然也可以根据克莱姆法则来求解：
https://blog.csdn.net/wodownload2/article/details/105724964
2.平差法
二、源码
源码结构图：
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， 如有发现，请尽快与博主联系！