首页 > 代码库 > HDU 4741

HDU 4741

求异面直线距离及公垂线

公式转自:http://blog.csdn.net/zhengnanlee/article/details/11870595  (求公垂线)

两条直线:

 

构造方程:

 

求出公垂线向量:

 

记公垂线和l1形成的平面为alpha,下面求它:

 

令:

 

联立:

 

 

#include <iostream>#include <cstdio>#include <cstring>#include <algorithm>#include <cmath>using namespace std;struct point {	double x,y,z;};struct line{	point a,b;};point a,b,c,d;point vectical;point A,B;point operator - (const point &x, const point &y){	point t;	t.x=x.x-y.x; t.y=x.y-y.y; t.z=x.z-y.z;	return t;}point Xmulti(point u,point v){	point t;	t.x=u.y*v.z-v.y*u.z; t.y=u.z*v.x-u.x*v.z; t.z=u.x*v.y-u.y*v.x;	return t;}double dmulti(point &u,point &v){	return u.x*v.x+u.y*v.y+u.z*v.z;}double len(point s){	return s.x*s.x+s.y*s.y+s.z*s.z;}int main(){	int t;	scanf("%d",&t);	while(t--){		scanf("%lf%lf%lf",&a.x,&a.y,&a.z);		scanf("%lf%lf%lf",&b.x,&b.y,&b.z);		scanf("%lf%lf%lf",&c.x,&c.y,&c.z);		scanf("%lf%lf%lf",&d.x,&d.y,&d.z);		vectical=Xmulti(b-a,d-c);		point tt=c-a;		double ans=fabs(dmulti(tt,vectical)/sqrt(len(vectical)));        double H = b.x - a.x, I = b.y - a.y, J = b.z - a.z;        double K = d.x - c.x, L = d.y - c.y, M = d.z - c.z;        double N = H * I * L - I * I * K - J * J * K + H * J * M;        double O = H * H * L - H * I * K - I * J * M + J * J * L;        double P = H * J * K - H * H * M - I * I * M + I *J * L;        double Q = -a.x * N + a.y * O - a.z * P;        double k = (O * c.y - N * c.x - P * c.z - Q) / (N * K - O * L + P * M);        A.x=K * k + c.x; A.y=L * k + c.y; A.z=M * k + c.z;        swap(a, c);        swap(b, d);        H = b.x - a.x, I = b.y - a.y, J = b.z - a.z;        K = d.x - c.x, L = d.y - c.y, M = d.z - c.z;        N = H * I * L - I * I * K - J * J * K + H * J * M;        O = H * H * L - H * I * K - I * J * M + J * J * L;        P = H * J * K - H * H * M - I * I * M + I *J * L;        Q = -a.x * N + a.y * O - a.z * P;        k = (O * c.y - N * c.x - P * c.z - Q) / (N * K - O * L + P * M);        B.x=K * k + c.x; B.y=L * k + c.y; B.z=M * k + c.z;		printf("%.6lf\n",ans);		printf("%.6lf %.6lf %.6lf %.6lf %.6lf %.6lf\n",B.x,B.y,B.z,A.x,A.y,A.z);	}	return 0;}