已知空间三点的坐标，求这三个点所确定的空间圆的圆心坐标和半径
在机器人轨迹规划处理中经常用到，查找资料找到一份可用的，经代码测试没问题特此记录：
原理
- 已知空间三点(x1,y1,z1), (x2,y2,z2),(x3,y3,z3)，求圆心(x0, y0, z0)和半径R
- 2个约束条件：1、三点共面 2、 三点到圆心距离相等
- 根据三点到圆心距离相等可以建立方程组：
image.png
用消元法消去R，联立(1)(2)可得(5)
image.png
联立(1)(3)可得(6)
image.png
image.png
- 根据三点共面约束可确定平面方程得(4)
image.png
- 通过以上(4)(5)(6)获得得A~D系数建立线性方程组，三个未知数三个方程即可求解圆心和半径
image.png
image.png
代码
#include "Eigen\dense"
using namespace Eigen;
/*
* 根据不共线的空间三点计算圆心坐标
* @points 三个点的坐标数组
* @return 返回圆心坐标
*/
Vector3d getCenterOfCircle(const vector<Vector3d>& points)
{
if (points.size() > 3 || points.size() <= 0) {
#ifndef NDEBUG
puts("[getCenterOfCircle] 参数的数量有误");
#endif
return Vector3d::Zero();
}
double x1 = points[0].x(),
x2 = points[1].x(),
x3 = points[2].x();
double y1 = points[0].y(),
y2 = points[1].y(),
y3 = points[2].y();
double z1 = points[0].z(),
z2 = points[1].z(),
z3 = points[2].z();
double a1 = (y1*z2 - y2*z1 - y1*z3 + y3*z1 + y2*z3 - y3*z2),
b1 = -(x1*z2 - x2*z1 - x1*z3 + x3*z1 + x2*z3 - x3*z2),
c1 = (x1*y2 - x2*y1 - x1*y3 + x3*y1 + x2*y3 - x3*y2),
d1 = -(x1*y2*z3 - x1*y3*z2 - x2*y1*z3 + x2*y3*z1 + x3*y1*z2 - x3*y2*z1);
double a2 = 2 * (x2 - x1),
b2 = 2 * (y2 - y1),
c2 = 2 * (z2 - z1),
d2 = x1*x1 + y1*y1 + z1*z1 - x2*x2 - y2*y2 - z2*z2;
double a3 = 2 * (x3 - x1),
b3 = 2 * (y3 - y1),
c3 = 2 * (z3 - z1),
d3 = x1*x1 + y1*y1 + z1*z1 - x3*x3 - y3*y3 - z3*z3;
double cx = -(b1*c2*d3 - b1*c3*d2 - b2*c1*d3 + b2*c3*d1 + b3*c1*d2 - b3*c2*d1)
/(a1*b2*c3 - a1*b3*c2 - a2*b1*c3 + a2*b3*c1 + a3*b1*c2 - a3*b2*c1);
double cy = (a1*c2*d3 - a1*c3*d2 - a2*c1*d3 + a2*c3*d1 + a3*c1*d2 - a3*c2*d1)
/(a1*b2*c3 - a1*b3*c2 - a2*b1*c3 + a2*b3*c1 + a3*b1*c2 - a3*b2*c1);
double cz = -(a1*b2*d3 - a1*b3*d2 - a2*b1*d3 + a2*b3*d1 + a3*b1*d2 - a3*b2*d1)
/(a1*b2*c3 - a1*b3*c2 - a2*b1*c3 + a2*b3*c1 + a3*b1*c2 - a3*b2*c1);
return Vector3d(cx, cy, cz);
}
