一、原理
二、代码实现
c++
/// <summary>
/// 四个不共面的点 用克拉默法则 计算球心和半径
/// </summary>
/// <param name="p1"></param>
/// <param name="p2"></param>
/// <param name="p3"></param>
/// <param name="p4"></param>
/// <param name="center"></param>
/// <returns></returns>
int CramerFindCenterByFourPoints(Eigen::Vector3d& p1, Eigen::Vector3d& p2, Eigen::Vector3d& p3, Eigen::Vector3d& p4, Eigen::Vector4d& center)
{
//1 1-2 3-4 2-3
//
double a = p1[0] - p2[0];
double b = p1[1] - p2[1];
double c = p1[2] - p2[2];
//
double a1 = p3[0] - p4[0];
double b1 = p3[1] - p4[1];
double c1 = p3[2] - p4[2];
double a2 = p2[0] - p3[0];
double b2 = p2[1] - p3[1];
double c2 = p2[2] - p3[2];
//2 、
// [P] p=1/2(......)
// L =[Q] Q=1/2(......)
// [R] R=1/2(......)
double P = (p1[0]* p1[0]- p2[0] * p2[0] + p1[1] * p1[1] - p2[1] * p2[1] + p1[2] * p1[2] - p2[2] * p2[2])/2;
double Q= (p3[0] * p3[0] - p4[0] * p4[0] + p3[1] * p3[1] - p4[1] * p4[1] + p3[2] * p3[2] - p4[2] * p4[2]) / 2;
double R = (p2[0] * p2[0] - p3[0] * p3[0] + p2[1] * p2[1] - p3[1] * p3[1] + p2[2] * p2[2] - p3[2] * p3[2]) / 2;
// 3 计算行列式
// [ a b c ]
// D = [ a1 b1 c1]
// [ a2 b2 c2]
double D= a * b1 * c2 + a2 * b * c1 + c * a1 * b2 - (a2 * b1 * c + a1 * b * c2 + a * b2 * c1);
// 4 Cramer 计算
double Dx = P * b1 * c2 + b * c1 * R + c * Q * b2 - (c * b1 * R + P * c1 * b2 + Q * b * c2);
double Dy = a * Q * c2 + P * c1 * a2 + c * a1 * R - (c * Q * a2 + a * c1 * R + c2 * P * a1);
double Dz = a * b1 * R + b * Q * a2 + P * a1 * b2 - (a2 * b1 * P + a * Q * b2 + R * b * a1);
if (D==0)
{
return -1;
}
else
{
//
center[0] = Dx / D;
center[1] = Dy / D;
center[2] = Dz / D;
center[3] = sqrt((p1[0] - center[0]) * (p1[0] - center[0]) + (p1[1] - center[1]) * (p1[1] - center[1]) + (p1[2] - center[2]) * (p1[2] - center[2]));
}
}
对比![](https://i-blog.csdnimg.cn/blog_migrate/47ddc20f58ddb8fa5683df18a23e8cb3.png)
cc 拟合球