已知三維空間兩條直線，如何計算兩條直線距離最近的位置的中點
阿新 • • 發佈：2018-12-17
------------------------------------------------------- -- 2018-01-18 建立人：Ruo_Xiao -- 開發環境：Matlab 2010 -- 郵箱：[email protected] -------------------------------------------------------- -- 2018-10-22 修改人：Ruo_Xiao -- 內容：修正計算P6的X的錯誤。 -------------------------------------------------------- -- 2018-10-25 修改人：Ruo_Xiao -- 內容： -- 1、修改Matlab程式； -- 2、增加測試程式； -- 3、增加Cpp程式。 --------------------------------------------------------
假設：
一、思路
1、由P1和P2計算出直線L1的方向向量D1和方程，P3和P4計算出直線L2的方向向量D2和方程。
2、由兩直線的方向向量計算法向量N。
3、N和P1可以確定平面Plane1，N和P3可以確定平面Plane2。
4、N和P1可以確定直線L3，L3和Plane2的交點為P5。
5、P5和D1可以確定直線方程L4，L4和L2的交點就是L1和L2兩條直線最短距離上的一個點P6。
6、N和P6可以算出法線L5。
7、L5和L1的交點就是就是L1和L2兩條直線最短距離上的一個點P7。
7、中點座標為P = （P6 + P7）/2。
二、計算公式
1、直線L1的方向向量D1：
2、直線L2的方向向量D2：
3、直線L1的方程：
4、直線L2的方程：
5、D1和D2的法向量N：
6、平面Plane2的方程：
7、直線L3的方程：
8、L3和Plane2的交點P5：
9、P5和D1組成的直線方程L4：
10、L4和L2的交點P6：
12、法線N的方程：
13、法線N和L1的交點P7：
14、中點座標：
三、原始碼
function [CrossPt,P6,P7] = CalCrossPt(P11,P12,P21,P22) x1 = P11(1); y1 = P11(2);z1 = P11(3); x2 = P12(1); y2 = P12(2);z2 = P12(3); x3 = P21(1); y3 = P21(2);z3 = P21(3); x4 = P22(1); y4 = P22(2);z4 = P22(3); a1 = x2 - x1; b1 = y2 - y1; c1 = z2 - z1; a2 = x4 - x3; b2 = y4 - y3; c2 = z4 - z3; a3 = b1*c2 - c1*b2; b3 = c1*a2 - a1*c2; c3 = a1*b2 - b1*a2; x5 = (a3*x3 + b3^2/a3*x1 - b3*(y1-y3) + c3^2/a3*x1 - c3*(z1 - z3))/(a3 + b3^2/a3 + c3^2/a3); y5 = b3/a3*(x5 - x1)+y1; z5 = c3/a3*(x5 - x1)+z1; x6 = (b1/a1*x5 - y5 - b2/a2*x3 + y3)/(b1/a1 - b2/a2); y6 = b1/a1*(x6 - x5) + y5; z6 = c1/a1*(x6 - x5) + z5; x7 = (b3/a3*x6 - y6 - b1/a1*x1 + y1)/(b3/a3 - b1/a1); y7 = b3/a3*(x7 - x6) + y6; z7 = c3/a3*(x7 - x6) + z6; P6 = [x6 , y6 , z6]; P7 = [x7 , y7 , z7]; CrossPt = [(x6+x7)/2 , (y6+y7)/2 , (z6+z7)/2]; end
clear;
clc;
% P11 = [-508.697 , 462.178 , -1203.02];
% P12 = [-507.968 , 614.376 , -1203.19];
% P21 = [-348.43 , 796.119 , -1258.63];
% P22 = [-158.094 , 793.653 , -1259.07];
P11 = [-42.4921 , 557.079 , -1421.21];
P12 = [-41.9771 , 546.407 , -1421.21];
P21 = [-177.645 , 238.873 , -1420.34];
P22 = [-344.014 , 238.244 , -1420.41];
[CrossPt,P6,P7] = CalCrossPt(P11,P12,P21,P22)
X1 = [P11(1),P12(1),P7(1)];
Y1 = [P11(2),P12(2),P7(2)];
Z1 = [P11(3),P12(3),P7(3)];
X2 = [P21(1),P22(1),P6(1)];
Y2 = [P21(2),P22(2),P6(2)];
Z2 = [P21(3),P22(3),P6(3)];
plot3(X1,Y1,Z1,'r');
hold on;
scatter3(X1,Y1,Z1,'r*');
hold on;
plot3(X2,Y2,Z2,'b');
hold on;
scatter3(X2,Y2,Z2,'b*');
hold on;
scatter3(CrossPt(1),CrossPt(2),CrossPt(3),'ko');
xlabel('X');
ylabel('Y');
zlabel('Z');
void Cal3DCrossPt(array<array<double,3>,4> dData , array<double,3> &dR)
{
double v1[3];
double v2[3];
for (size_t i=0;i<3;++i)
{
v1[i] = dData[1][i] - dData[0][i];
v2[i] = dData[3][i] - dData[2][i];
}
double x1 = dData[0][0];
double y1 = dData[0][1];
double z1 = dData[0][2];
double x3 = dData[2][0];
double y3 = dData[2][1];
double z3 = dData[2][2];
double a1 = v1[0];
double b1 = v1[1];
double c1 = v1[2];
double a2 = v2[0];
double b2 = v2[1];
double c2 = v2[2];
double a3 = b1*c2 - b2*c1;
double b3 = c1*a2 - c2*a1;
double c3 = a1*b2 - a2*b1;
if (fabs(a3)<=DBL_EPSILON)
{
a3 = 0.00001;
}
if (fabs(a1)<=DBL_EPSILON)
{
a1 = 0.00001;
}
if (fabs(a2)<=DBL_EPSILON)
{
a2 = 0.00001;
}
double x5h = a3*x3 + b3*b3*x1/a3 - b3*(y1 - y3) + c3*c3*x1/a3 - c3*(z1-z3);
double x51 = a3 + b3*b3/a3 + c3*c3/a3;
if (fabs(x51)<=DBL_EPSILON)
{
x51 = 0.00001;
}
double x5 = x5h/x51;
double y5 = b3*(x5-x1)/a3+y1;
double z5 = c3*(x5-x1)/a3+z1;
double x6h = b1*x5/a1 - y5 - b2*x3/a2 + y3;
double x61 = b1/a1 - b2/a2;
if (fabs(x61)<=DBL_EPSILON)
{
x61 = 0.00001;
}
double x6 = x6h/x61;
double y6 = b1*(x6 - x5)/a1 + y5;
double z6 = c1*(x6 - x5)/a1 + z5;
double x7h = b3*x6/a3 - y6 - b1*x1/a1 + y1;
double x71 = b3/a3 - b1/a1;
if (fabs(x71)<=DBL_EPSILON)
{
x71 = 0.00001;
}
double x7 = x7h / x71;
double y7 = b3*(x7 - x6)/a3 + y6;
double z7 = c3*(x7 - x6)/a3 + z6;
double xn = (x6 + x7)/2;
double yn = (y6 + y7)/2;
double zn = (z6 + z7)/2;
dR[0] = xn;
dR[1] = yn;
dR[2] = zn;
}
結果：
（SAW：Game Over！）