matlab求异面直线的公垂线,求空间异面直线的公垂线

题设：假设有两条直线 L1，L2 ，以及两条直线的方向向量V1，V2，求其最短距离连线的连接点。

首先，最短距离很好求，也即是两异面直线公垂线的长度，选择L1上任意一点P1连接L2上任意一点P2，则线段P1P2在L1，L2的公垂线上的投影即是长度,这个太简单，而且百度搜索一搜一大堆，不解释

而异面直线的垂足点，则不是那么好求，我找了好一阵网上的代码，坑得要死，无奈只能自己写，

基本思路如下，

因为直线的定义可以由如下式子给出

L(t) = P + t*V

则在L1和L2上分别选择任意的p1,p2,以及对应的t1,t2,得到的L1(t1),L2(t2)。

写作如下等式

L1(t1) = P1 + t1V1--------------------------(1)

L2(t2) = P2 + t2V2--------------------------(2)

如果L1(t1),L2(t2)刚好是各自直线的垂足点，

那么可以得出此时| L2(t2)-L1(t1) | 的模长为最小值

且构成的 L2(t2)-L1(t1) 向量刚好就是公垂线

因为v1,和 v2都和公垂线垂直，所以，v1和v2各自和公垂线的点乘都为0

于是我们可以写出如下等式

( L2(t2)-L1(t1) ) . V1 = 0 //.表示点乘

( L2(t2)-L1(t1) ) . V2 = 0

即

(t2V2-t1V1 + P2 - P1 ).V1 = 0

(t2V2-t1V1 + P2 - P1 ).V2 = 0

整理

t2V2.V1 - t1V1.V1 + (P2 - P1).V1 = 0

t2V2.V2 - t1V1.V2 + (P2 - P1).V2 = 0

令

a = V1 .V2 = V2.V1

b = V1.V1

c = V2.V2

d = (P2 - P1) .V1

e = (P2 - P1).V2

则上式可化简为

at2 - bt1 + d = 0

ct2 - at1 + e = 0

以下分三种情形讨论

当a = 0 ，即 V1 .V2 = V2.V1 = 0时，表明原始两条直线互相垂直

则

t1 = d / b;

t2 = -e / c;

当a != 0时候

解上述方程

t1 = (ae -cd)/(aa-bc)

t2 = b/a * t1 - d/a

这里发现当(a * a - b * c) = 0时，即(V2.V1) * (V2.V1) = (V1.V1) * (V2.V2) = 0时，也就是两条直线平行或共线，此时无意义，所以只要排除即可

综上所述

a = 0 时

t1 = d / b;

t2 = -e / c;

a! = 0 时

t1 = (ae -cd)/(aa-bc)

t2 = b/a * t1 - d/a

#include

#include

#include

using namespace std;

double dotMultiply(double* v1, double* v2){//点乘

double v;

v = v1[0]*v2[0] + v1[1] * v2[1] + v1[2] * v2[2];

return v;

}

void perpend(double* p1, double* v1, double* p2, double* v2, double* t1, double* t2){

double a=dotMultiply(v1,v2);

double b=dotMultiply(v1,v1);

double c=dotMultiply(v2,v2);

double p2p1[3];

p2p1[0]=p2[0]-p1[0];

p2p1[1]=p2[1]-p1[1];

p2p1[2]=p2[2]-p1[2];

double d=dotMultiply(p2p1,v1);

double e=dotMultiply(p2p1,v2);

if(a==0){

*t1=d/b;

*t2=-e/c;

}

if(a!=0){

*t1=(a*e-c*d)/(a*a-b*c);

*t2=(b/a)*(*t1)-d/a;

}

}

void test(){

double v1[3]={1,0,0};

double v2[3]={0,1,0};

double p1[3]={10,0,0};

double p2[3]={1,10,0};

double t1,t2;

perpend(p1,v1,p2,v2,&t1,&t2);

cout<

cout<

double p11[3]; double p22[3];

double p[3]; double b[3];

p[0]=t1*v1[0];

p[1]=t1*v1[1];

p[2]=t1*v1[2];

b[0]=t2*v2[0];

b[1]=t2*v2[1];

b[2]=t2*v2[2];

p11[0]=p1[0]+p[0];

p11[1]=p1[1]+p[1];

p11[2]=p1[2]+p[2];

p22[0]=p2[0]+b[0];

p22[1]=p2[1]+b[1];

p22[2]=p2[2]+b[2];

printf("垂足1：%f %f %f\n",p11[0], p11[1], p11[2]);

printf("垂足2：%f %f %f\n",p22[0], p22[1], p22[2]);

double cha[3];

cha[0]=p22[0]-p11[0];

cha[1]=p22[1]-p11[1];

cha[2]=p22[2]-p11[2];

printf("%f\n",v1[0]*cha[0]+v1[1]*cha[1]+v1[2]*cha[2]);

printf("%f\n",v2[0]*cha[0]+v2[1]*cha[1]+v2[2]*cha[2]);

}

int main(){

test();

return 0;

}