这是周末写的空间几何c++代码 :)欢迎使用啊 #ifndef G_MATH_3D_HPP #define G_MATH_3D_HPP /* www.gaimo.net ccsdu2004@yahoo.com.cn ccsdu2004 核动力机器人 hi.csdn.net/ccsdu2004 28.03.2009 */ #include <cmath> #include "Vect3D.hpp" #define MIN_VALUE 0.0000001 namespace g { namespace math { //定义直线方程 struct LINE_3D { double a; double b; double c; double x; double y; double z; }; //定义平面方程 struct Plane { double a; double b; double c; double d; }; //获取直线与平面交点 Vect3D<double> Point3D(LINE_3D line, Plane plane); //获取点到平面的距离 double Distance(Vect3D<double> p1, Plane plane); //检测点是否在平面上 bool IsIn(Vect3D<double> p, Plane plane); //检测直线与直线是否垂直 bool IsVertical(LINE_3D l1, LINE_3D l2); //获取平面夹角 double GetAngle(Plane plane1, Plane plane2); //获取直线夹角 double GetAngle(LINE_3D l1, LINE_3D l2); //检测直线与平面是否垂直 bool IsVertical(LINE_3D l, Plane plane); //检测直线是否在平面上 bool IsIn(LINE_3D line, Plane plane); //检测直线是否与轴垂直 //axis = 1, 2, 3for x, y, z bool IsPrallel(LINE_3D l, int axis); //检测直线与直线是否平行 bool IsPrallel(LINE_3D l1, LINE_3D l2); //检测平面与平面是否平行 bool IsPrallel(Plane plane1, Plane plane2); //获取点的最短距离 double Distance(Vect3D<double> p1, Vect3D<double> p2); //检测点是否在直线上 bool IsIn(Vect3D<double> p, LINE_3D l); //获取绕轴旋转的平面方程 Plane RotationAxis(LINE_3D line, double aux); //检测平面是否相交 bool IsCross(Plane p1, Plane p2); //获取直线和平面的夹角 double Angle(LINE_3D line, Plane plane); //获取点与直线构成的平面 Plane GetPlane(Vect3D<double> p, LINE_3D line); //获取点到直线的垂线 LINE_3D VerticalLine(Vect3D<double> p, LINE_3D l); //获取点到直线的距离 double Distance(Vect3D<double> p1, LINE_3D l); //获取2直线的最短距离 double Distance(LINE_3D p1, LINE_3D p2); //检测直线是否与直线相交 bool IsCross(LINE_3D l1, LINE_3D l2); //获取直线交点 Vect3D<double> GetPoint(LINE_3D l1, LINE_3D l2); //获取2平面角线 LINE_3D GetLine(Plane p1, Plane p2); //获取过直线垂直给定平面的垂直平面 Plane GetLine(LINE_3D l, Plane plane); } } #endif #include "3D.hpp" namespace g { namespace math { Vect3D<double> Point3D(LINE_3D point, Plane plane) { double a = point.a; double b = point.b; double c = point.c; double x = point.x; double y = point.y; double z = point.z; double A = plane.a; double B = plane.b; double C = plane.c; double D = plane.d; Vect3D<double> p; p[0] = -(B*a*y-B*b*x+C*a*z-C*x*c-D*a)/(A*a+b*B+c*C); p[1] = (A*a*y-A*b*x-b*C*z+b*D+c*C*y)/(A*a+b*B+c*C); p[2] = (A*a*z+b*B*z-B*c*y-A*x*c+D*c)/(A*a+b*B+c*C); return p; } //获取点到平面的距离 double Distance(Vect3D<double> p, Plane plane) { double x = p[0]; double y = p[1]; double z = p[2]; double A = plane.a; double B = plane.b; double C = plane.c; double D = plane.d; double v =fabs(x*A + y*B +z*C +D); double bom = sqrt((double)(A*A + B*B + C*C)); return v/bom; } //检测点是否在平面上 bool IsIn(Vect3D<double> p, Plane plane) { double x = p[0]; double y = p[1]; double z = p[2]; double A = plane.a; double B = plane.b; double C = plane.c; double D = plane.d; double v = x*A + y*B + z*C +D; v = fabs(v); if(v - MIN_VALUE > 0) return false; return true; } //检测直线与直线是否垂直 bool IsVertical(LINE_3D l1, LINE_3D l2) { double a1 = l1.a; double b1 = l1.b; double c1 = l1.c; double a2 = l2.a; double b2 = l2.b; double c2 = l2.c; double dotproduct = a1*a2 + b1*b2 + c1*c2; dotproduct = fabs(dotproduct); if( dotproduct - MIN_VALUE > 0) return false; return true; } //获取平面夹角 double GetAngle(Plane plane1, Plane plane2) { double A1 = plane1.a; double B1 = plane1.b; double C1 = plane1.c; double D1 = plane1.d; double A2 = plane2.a; double B2 = plane2.b; double C2 = plane2.c; double D2 = plane2.d; double dotproduct = fabs(A1*A2 +B1*B2 +C1*C2); double bom = A1*A1 +B1*B1 +C1*C1; bom *= (A2*A2 +B2*B2 +C2*C2); double v = dotproduct/sqrt((double)bom); double angle = acos(v); return angle; } //获取直线夹角 double GetAngle(LINE_3D l1, LINE_3D l2) { double a1 = l1.a; double b1 = l1.b; double c1 = l1.c; double a2 = l2.a; double b2 = l2.b; double c2 = l2.c; double dotproduct = fabs(a1*a2 +b1*b2 +c1*c2); double bom = a1*a1 +a1*a1 +c1*c1; bom *= (a2*a2 +b2*b2 +c2*c2); double v = dotproduct/sqrt((double)bom); double angle = acos(v); return angle; } //检测直线与平面是否垂直 bool IsVertical(LINE_3D l, Plane plane) { double A = plane.a; double B = plane.b; double C = plane.c; double D = plane.d; double a = l.a; double b = l.b; double c = l.c; double dotproduct = fabs(A*a +B*b +C*c); if(dotproduct > MIN_VALUE) return false; return true; } //检测直线是否在平面上 bool IsIn(LINE_3D line, Plane plane) { if(false == IsVertical(line, plane)) return false; else { Vect3D<double> p(line.x, line.y, line.z); if(true == IsIn(p, plane)) return true; return false; } } bool IsPrallel(LINE_3D l, int axis) { if(fabs((int)(axis -2)) > 1) return false; if(axis == 1) { if(fabs(l.a) > MIN_VALUE) return false; } else if(axis == 2) { if(fabs(l.b) > MIN_VALUE) return false; } else { if(fabs(l.c) > MIN_VALUE) return false; } return true; } //检测直线与直线是否平行 bool IsPrallel(LINE_3D l1, LINE_3D l2) { double a1 = l1.a; double b1 = l1.b; double c1 = l1.c; double a2 = l2.a; double b2 = l2.b; double c2 = l2.c; //检测是否平行轴 for(int i =1; i< 4; i++) if ((IsPrallel(l1, i) == true && IsPrallel(l2, i) == true)) return true; double k1 = a1/a2; double k2 = b1/b2; double k3 = c1/c2; double o1 = fabs(k1 - k2); double o2 = fabs(k3 - k2); if(fabs(o1 - o2) > MIN_VALUE) return false; return true; } //检测平面与平面是否平行 bool IsPrallel(Plane plane1, Plane plane2) { LINE_3D l1, l2; l1.a = plane1.a; l1.b = plane1.b; l1.c = plane1.c; l2.a = plane2.a; l2.b = plane2.b; l2.c = plane2.c; return IsPrallel(l1, l2); } //获取点的最短距离 double Distance(Vect3D<double> p1, Vect3D<double> p2) { double d = (p1[0] - p2[0])*(p1[0] - p2[0]) + (p1[1] - p2[1])*(p1[1] - p2[1]) + (p1[2] - p2[2])*(p1[2] - p2[2]); return sqrt(d); } //检测点是否在直线上 bool IsIn(Vect3D<double> p, LINE_3D l) { LINE_3D line; line.x = p[0]; line.y = p[1]; line.z = p[2]; line.a = p[0] - l.x; line.b = p[1] - l.y; line.c = p[2] - l.z; if(fabs(GetAngle(l, line)) - MIN_VALUE > 0) return false; return true; } //获取绕轴旋转的平面方程 Plane RotationAxis(LINE_3D line, double aux) { Plane plane; plane.a = line.b; plane.b = -line.a + aux*line.c; plane.c = aux*line.b; plane.d = line.b*line.x - line.a*line.y + aux*(line.c*line.y - line.b*line.z); return plane; } //检测平面是否相交 bool IsCross(Plane p1, Plane p2) { if(fabs(GetAngle(p1, p2)) > MIN_VALUE) return true; return false; } //获取直线和平面的夹角 double Angle(LINE_3D line, Plane plane) { LINE_3D l; l.a = plane.a; l.b = plane.b; l.c = plane.c; return fabs(90 -GetAngle(l, line)); } //获取点与直线构成的平面 Plane GetPlane(Vect3D<double> p, LINE_3D line) { double x = p[0]; double y = p[1]; double z = p[2]; double px = line.x; double py = line.y; double pz = line.z; double a = line.a; double b = line.b; double c = line.c; double bom = a*pz*y-px*c*y-a*py*z+px*b*z+py*x*c-pz*x*b; Plane plane; plane.a = pz*b-b*z+c*y-py*c; plane.b = -a*pz+a*z-x*c+px*c; plane.c = -(a*y+px*b-a*py-x*b); plane.d = 1; return plane; } //获取点到直线的垂线 /* LINE_3D VerticalLine(Vect3D<double> p, LINE_3D l) { LINE_3D line; if(true == IsIn(p, l)) { line.a = 0; line.b = 0; line.c = 0; line.x = 0; line.y = 0; line.z = 0; return line; } }*/ } } 其中的线性方程求解是采用matlab做的 还没做完! 以后补完