求异面直线距离及公垂线
公式转自: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; }
HDU 4741
时间: 2024-11-08 20:10:48