使用克拉默法则进行四点定球
目录
- 1.算法过程
- 2.C++代码
- 3.python代码
- 4.计算结果
本文由CSDN点云侠原创，爬虫网站请自重。
1.算法过程
已知空间内不共面的四个点，设其坐标为 A ( x 1 , y 1 , z 1 ) A(x_1,y_1,z_1) A(x1,y1,z1)、 B ( x 2 , y 2 , z 2 ) B(x_2,y_2,z_2) B(x2,y2,z2)、 C ( x 3 , y 3 , z 3 ) 、 D ( x 4 , y 4 , z 4 ) C(x_3,y_3,z_3)、D(x_4,y_4,z_4) C(x3,y3,z3)、D(x4,y4,z4)，设半径为 r r r，球心 O O O坐标为 ( x , y , z ) (x,y,z) (x,y,z)。利用四点到球心距离相等的性质得到如下四个方程。
( x − x 1 ) 2 + ( y − y 1 ) 2 + ( z − z 1 ) 2 = r 2 ; ( x − x 2 ) 2 + ( y − y 2 ) 2 + ( z − z 2 ) 2 = r 2 ; ( x − x 3 ) 2 + ( y − y 3 ) 2 + ( z − z 3 ) 2 = r 2 ; ( x − x 4 ) 2 + ( y − y 4 ) 2 + ( z − z 4 ) 2 = r 2 ; (x-x_1)^2 + (y-y_1)^2 +(z-z_1)^2 =r^2;\\ (x-x_2)^2 + (y-y_2)^2 +(z-z_2)^2 =r^2;\\ (x-x_3)^2 + (y-y_3)^2 +(z-z_3)^2 =r^2;\\ (x-x_4)^2 + (y-y_4)^2 +(z-z_4)^2 =r^2; (x−x1)2+(y−y1)2+(z−z1)2=r2;(x−x2)2+(y−y2)2+(z−z2)2=r2;(x−x3)2+(y−y3)2+(z−z3)2=r2;(x−x4)2+(y−y4)2+(z−z4)2=r2;
展开得:
x 2 + y 2 + z 2 − 2 ( x 1 x + y 1 y + z 1 z ) + x 1 2 + y 1 2 + z 1 2 = r 2 ① x 2 + y 2 + z 2 − 2 ( x 2 x + y 2 y + z 2 z ) + x 2 2 + y 2 2 + z 2 2 = r 2 ② x 2 + y 2 + z 2 − 2 ( x 3 x + y 3 y + z 3 z ) + x 3 2 + y 3 2 + z 3 2 = r 2 ③ x 2 + y 2 + z 2 − 2 ( x 4 x + y 4 y + z 4 z ) + x 4 2 + y 4 2 + z 4 2 = r 2 ④ x^2 + y^2 + z^2- 2(x_1x+y_1y+z_1z)+x_1^2+y_1^2 + z_1^2 = r^2 ①\\ x^2 + y^2 + z^2- 2(x_2x+y_2y+z_2z)+x_2^2+y_2^2 + z_2^2 = r^2②\\ x^2 + y^2 + z^2- 2(x_3x+y_3y+z_3z)+x_3^2+y_3^2 + z_3^2 = r^2③\\ x^2 + y^2 + z^2- 2(x_4x+y_4y+z_4z)+x_4^2+y_4^2 + z_4^2 = r^2④ x2+y2+z2−2(x1x+y1y+z1z)+x12+y12+z12=r2①x2+y2+z2−2(x2x+y2y+z2z)+x22+y22+z22=r2②x2+y2+z2−2(x3x+y3y+z3z)+x32+y32+z32=r2③x2+y2+z2−2(x4x+y4y+z4z)+x42+y42+z42=r2④
分别作①-②、③ - ④、② - ③得：
( x 1 − x 2 ) x + ( y 1 − y 2 ) y + ( z 1 − z 2 ) z = 1 / 2 ( x 1 2 − x 2 2 + y 1 2 − y 2 2 + z 1 2 − z 2 2 ) ( x 3 − x 4 ) x + ( y 3 − y 4 ) y + ( z 3 − z 4 ) z = 1 / 2 ( x 3 2 − x 4 2 + y 3 2 − y 4 2 + z 3 2 − z 4 2 ) ( x 2 − x 3 ) x + ( y 2 − y 3 ) y + ( z 2 − z 3 ) z = 1 / 2 ( x 2 2 − x 3 2 + y 2 2 − y 3 2 + z 2 2 − z 3 2 ) (x_1-x_2)x+(y_1-y_2)y+(z_1-z_2)z=1/2(x_1^2 -x_2^2 + y_1^2 -y_2^2 + z_1^2 -z_2^2 )\\ (x_3-x_4)x+(y_3-y_4)y+(z_3-z_4)z=1/2(x_3^2 -x_4^2 + y_3^2 -y_4^2 + z_3^2 -z_4^2 )\\ (x_2-x_3)x+(y_2-y_3)y+(z_2-z_3)z=1/2(x_2^2 -x_3^2 + y_2^2 -y_3^2 + z_2^2 -z_3^2 )\\ (x1−x2)x+(y1−y2)y+(z1−z2)z=1/2(x12−x22+y12−y22+z12−z22)(x3−x4)x+(y3−y4)y+(z3−z4)z=1/2(x32−x42+y32−y42+z32−z42)(x2−x3)x+(y2−y3)y+(z2−z3)z=1/2(x22−x32+y22−y32+z22−z32)
其对应的系数行列式可设为：
D = ∣ a b c a 1 b 1 c 1 a 2 b 2 c 2 ∣ D=\left| \begin{matrix} a & b & c\\ a_1 & b_1 & c_1 \\ a_2 & b_2 & c_2 \end{matrix} \right| D= aa1a2bb1b2cc1c2
则： a = ( x 1 − x 2 ) , b = ( y 1 − y 2 ) , c = ( z 1 − z 2 ) , a 1 = ( x 3 − x 4 ) , b 1 = ( y 3 − y 4 ) ， c 1 = ( z 3 − z 4 ) , a 2 = ( x 2 − x 3 ) , b 2 = ( y 2 − y 3 ) ， c 2 = ( z 2 − z 3 ) a=(x_1-x_2),b=(y_1-y_2),c=(z_1-z_2),\\a_1=(x_3-x_4),b_1=(y_3-y_4)，c_1=(z_3-z_4),\\ a_2=(x_2-x_3),b_2=(y_2-y_3)，c_2=(z_2-z_3) a=(x1−x2),b=(y1−y2),c=(z1−z2),a1=(x3−x4),b1=(y3−y4)，c1=(z3−z4),a2=(x2−x3),b2=(y2−y3)，c2=(z2−z3)
常数项行列式为:
L = ∣ P Q R ∣ L=\left| \begin{matrix} P\\ Q \\ R \end{matrix} \right| L= PQR
则：
P = 1 2 ( x 1 2 − x 2 2 + y 1 2 − y 2 2 + z 1 2 − z 2 2 ) P=\frac{1}{2}(x_1^2 -x_2^2 + y_1^2 -y_2^2 + z_1^2 - z_2^2 ) P=21(x12−x22+y12−y22+z12−z22)
Q = 1 2 ( x 3 2 − x 4 2 + y 3 2 − y 4 2 + z 3 2 − z 4 2 ) Q=\frac{1}{2}(x_3^2 -x_4^2 + y_3^2 -y_4^2 + z_3^2 - z_4^2 ) Q=21(x32−x42+y32−y42+z32−z42)
R = 1 2 ( x 2 2 − x 3 2 + y 2 2 − y 3 2 + z 2 2 − z 3 2 ) R=\frac{1}{2}(x_2^2 -x_3^2 + y_2^2 -y_3^2 + z_2^2 - z_3^2 ) R=21(x22−x32+y22−y32+z22−z32)
现设：
D x = ∣ P b c Q b 1 c 1 R b 2 c 2 ∣ Dx=\left| \begin{matrix} P & b & c\\ Q & b_1 & c_1 \\ R & b_2 & c_2 \end{matrix} \right| Dx= PQRbb1b2cc1c2
D y = ∣ a P c a 1 Q c 1 a 2 R c 2 ∣ Dy=\left| \begin{matrix} a & P & c\\ a_1 & Q & c_1 \\ a_2 &R & c_2 \end{matrix} \right| Dy= aa1a2PQRcc1c2
D z = ∣ a b P a 1 b 1 Q a 2 b 2 R ∣ Dz=\left| \begin{matrix} a & b & P\\ a_1 & b_1 & Q \\ a_2 &b_2 & R \end{matrix} \right| Dz= aa1a2bb1b2PQR
由线性代数中的克拉默法则可知:
x = D x D x=\frac{Dx}{D} x=DDx
y = D y D y=\frac{Dy}{D} y=DDy
z = D z D z=\frac{Dz}{D} z=DDz
2.C++代码
bool calculatesphere_oAndsphere_r(Eigen::Vector3d& p1, Eigen::Vector3d& p2,
Eigen::Vector3d& p3, Eigen::Vector3d& p4,
Eigen::Vector3d& sphere_o, double& sphere_r)
{
double a = p1[0] - p2[0], b = p1[1] - p2[1], c = p1[2] - p2[2];
double a1 = p3[0] - p4[0], b1 = p3[1] - p4[1], c1 = p3[2] - p4[2];
double a2 = p2[0] - p3[0], b2 = p2[1] - p3[1], c2 = p2[2] - p3[2];
double A = p1[0] * p1[0] - p2[0] * p2[0];
double B = p1[1] * p1[1] - p2[1] * p2[1];
double C = p1[2] * p1[2] - p2[2] * p2[2];
double A1 = p3[0] * p3[0] - p4[0] * p4[0];
double B1 = p3[1] * p3[1] - p4[1] * p4[1];
double C1 = p3[2] * p3[2] - p4[2] * p4[2];
double A2 = p2[0] * p2[0] - p3[0] * p3[0];
double B2 = p2[1] * p2[1] - p3[1] * p3[1];
double C2 = p2[2] * p2[2] - p3[2] * p3[2];
double P = (A + B + C) / 2;
double Q = (A1 + B1 + C1) / 2;
double R = (A2 + B2 + C2) / 2;
// D是系数行列式，利用克拉默法则
double D = a * b1 * c2 + a2 * b * c1 + c * a1 * b2 - (a2 * b1 * c + a1 * b * c2 + a * b2 * c1);
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)
{
cerr << "四点共面" << endl;
return false;
}
else
{
sphere_o[0] = Dx / D;
sphere_o[1] = Dy / D;
sphere_o[2] = Dz / D;
sphere_r = sqrt((p1[0] - sphere_o[0]) * (p1[0] - sphere_o[0]) +
(p1[1] - sphere_o[1]) * (p1[1] - sphere_o[1]) +
(p1[2] - sphere_o[2]) * (p1[2] - sphere_o[2]));
return true;
}
}
3.python代码
def four_point_fix_sphere(points):
# 四个点的坐标
P1 = points[0]
P2 = points[1]
P3 = points[2]
P4 = points[3]
# 互相相减
l = P1 - P2
l1 = P3 - P4
l2 = P2 - P3
# 系数D
D = np.ones((3, 3))
D[0, :] = l
D[1, :] = l1
D[2, :] = l2
detD = np.linalg.det(D)
# P,Q,R
P = (np.linalg.norm(P1) ** 2 - np.linalg.norm(P2) ** 2) * 0.5
Q = (np.linalg.norm(P3) ** 2 - np.linalg.norm(P4) ** 2) * 0.5
R = (np.linalg.norm(P2) ** 2 - np.linalg.norm(P3) ** 2) * 0.5
# 常数项系数
L = np.ones((3, 1))
L[0] = P
L[1] = Q
L[2] = R
# 系数Dx
Dx = np.ones((3, 3))
Dx[:, :] = D[:, :]
Dx[:, 0] = L[:, 0]
detDx = np.linalg.det(Dx)
# 系数Dy
Dy = np.ones((3, 3))
Dy[:, :] = D[:, :]
Dy[:, 1] = L[:, 0]
detDy = np.linalg.det(Dy)
# 系数Dz
Dz = np.ones((3, 3))
Dz[:, :] = D[:, :]
Dz[:, 2] = L[:, 0]
detDz = np.linalg.det(Dz)
# 计算球心
xo = detDx / detD
yo = detDy / detD
zo = detDz / detD
sphere_o = [xo, yo, zo]
# 计算半径
sphere_r = np.linalg.norm(P1 - sphere_o)
return sphere_o, sphere_r
4.计算结果
球心坐标:[-5.480253504546088, -0.08827109437986415, -0.01369803960562648]
球体半径:1.9874710684528454
