C++实现三维空间中点到点、点到直线、点到平面的距离计算

#include <iostream> #include <cmath> #include "Point.h" #include "PlaneEquation.h" using namespace std; double dist(Point &pt1, Point &pt2) //Distance between two points { return (pt2-pt1).mod(); } double dist(Point &ptC, Point &ptA, Point &ptB) //Distance between point C to line AB. { double d=0.0; Point ac,ab; ac=ptC-ptA; ab=ptB-ptA; if (ab.mod()) { d=(ac*ab).mod()/ab.mod(); } else { d=dist(ptC,ptA); } //The line is reduced to a point. point(ptC) to point(ptA) distance. return d; } double dist(Point &pt, PlaneEquation &pe) //Distance between point and plane { double dt=0.0; double mA,mB,mC,mD,mX,mY,mZ; mA=pe.GetA(); mB=pe.GetB(); mC=pe.GetC(); mD=pe.GetD(); mX=pt.GetX(); mY=pt.GetY(); mZ=pt.GetZ(); if (mA*mA+mB*mB+mC*mC) dt=abs(mA*mX+mB*mY+mC*mZ+mD)/sqrt(mA*mA+mB*mB+mC*mC); else dt=pt.mod(); // The plane is reduced to the origin. point(pt) to point(zero) distance. return dt; } int main() { Point p0,p1(1.0,0.0,0.0),p2(0.0,1.0,0.0),p3(0.0,0.0,1.0),p4(1.0,1.0,1.0),p1p3,p1p2,p0p4; PlaneEquation pL0, pL(1.0,1.0,1.0,-1.0); cout<<"\n---------------- Points --------------------"<<endl; cout<<"\nThe point P0 is : "<<p0<<" |P0|="<<p0.mod()<<endl; cout<<"\nThe point P1 is : "<<p1<<" |P1|="<<p1.mod()<<endl; cout<<"\nThe point P2 is : "<<p2<<" |P2|="<<p2.mod()<<endl; cout<<"\nThe point P3 is : "<<p3<<" |P3|="<<p3.mod()<<endl; cout<<"\nThe point P4 is : "<<p4<<" |P4|="<<p4.mod()<<endl; cout<<"\n---------------- Point to point --------------------"<<endl; cout<<"\nThe distance between point P0 and p1 : "<<dist(p0,p1)<<endl; cout<<"\nThe distance between point P0 and p2 : "<<dist(p0,p2)<<endl; cout<<"\nThe distance between point P0 and p3 : "<<dist(p0,p3)<<endl; cout<<"\nThe distance between point P0 and p4 : "<<dist(p0,p4)<<endl; cout<<"\n---------------- Point to line --------------------"<<endl; cout<<"\nThe distance between point P3 and the line p1,p2 is : "<<dist(p3,p1,p2)<<endl; cout<<"\nThe distance between point P3 and the line p1,p3 is : "<<dist(p3,p1,p3)<<endl; cout<<"\nThe distance between point P3 and the line p2,p3 is : "<<dist(p3,p2,p3)<<endl; cout<<"\nThe distance between point P3 and the line p1,p1 is : "<<dist(p3,p1,p1)<<endl; cout<<"\n---------------- Point to plane --------------------"<<endl; cout<<"\nThe plane equation is : "<<pL<<endl; cout<<"\nThe distance between point P0 and the plane is : "<<dist(p0,pL)<<endl; cout<<"\nThe distance between point P1 and the plane is : "<<dist(p1,pL)<<endl; cout<<"\nThe plane equation is : "<<pL0<<endl; cout<<"\nThe distance between point P4 and the plane is : "<<dist(p4,pL0)<<endl; p1p3=p3-p1; p1p2=p2-p1; p0p4=p4-p0; cout<<"\n---------------- Vector --------------------"<<endl; cout<<"\nThe vector p1p3 is : "<<p1p3<<" |P1P3|="<<p1p3.mod()<<endl; cout<<"\nThe vector p1p2 is : "<<p1p2<<" |P1P2|="<<p1p2.mod()<<endl; cout<<"\nThe vector p0p4 is : "<<p0p4<<" |P0P4|="<<p0p4.mod()<<endl; cout<<"\np1p2 x p1p3 = "<<p1p2*p1p3<<endl; cout<<"\np1p3 x p1p2 = "<<p1p3*p1p2<<endl; cout<<"\n|p1p3 x p1p2| = "<<(p1p3*p1p2).mod()<<endl; cout<<"\nThe distance between point P3 and the line p1,p2 is : |p1p3 x p1p2|/|p1p2|="<<dist(p3,p1,p2)<<endl; return 0; }