一些三維計算幾何演算法
阿新 • • 發佈:2019-02-12
//三維幾何函式庫 #include <math.h> #define eps 1e-8 #define zero(x) (((x)>0?(x):-(x))<eps) struct point3{double x,y,z;}; struct line3{point3 a,b;}; struct plane3{point3 a,b,c;}; //計算cross product U x V point3 xmult(point3 u,point3 v){ point3 ret; ret.x=u.y*v.z-v.y*u.z; ret.y=u.z*v.x-u.x*v.z; ret.z=u.x*v.y-u.y*v.x; return ret; } //計算dot product U . V double dmult(point3 u,point3 v){ return u.x*v.x+u.y*v.y+u.z*v.z; } //向量差 U - V point3 subt(point3 u,point3 v){ point3 ret; ret.x=u.x-v.x; ret.y=u.y-v.y; ret.z=u.z-v.z; return ret; } //取平面法向量 point3 pvec(plane3 s){ return xmult(subt(s.a,s.b),subt(s.b,s.c)); } point3 pvec(point3 s1,point3 s2,point3 s3){ return xmult(subt(s1,s2),subt(s2,s3)); } //兩點距離,單引數取向量大小 double distance(point3 p1,point3 p2){ return sqrt((p1.x-p2.x)*(p1.x-p2.x)+(p1.y-p2.y)*(p1.y-p2.y)+(p1.z-p2.z)*(p1.z-p2.z)); } //向量大小 double vlen(point3 p){ return sqrt(p.x*p.x+p.y*p.y+p.z*p.z); } //判三點共線 int dots_inline(point3 p1,point3 p2,point3 p3){ return vlen(xmult(subt(p1,p2),subt(p2,p3)))<eps; } //判四點共面 int dots_onplane(point3 a,point3 b,point3 c,point3 d){ return zero(dmult(pvec(a,b,c),subt(d,a))); } //判點是否線上段上,包括端點和共線 int dot_online_in(point3 p,line3 l){ return zero(vlen(xmult(subt(p,l.a),subt(p,l.b))))&&(l.a.x-p.x)*(l.b.x-p.x)<eps&& (l.a.y-p.y)*(l.b.y-p.y)<eps&&(l.a.z-p.z)*(l.b.z-p.z)<eps; } int dot_online_in(point3 p,point3 l1,point3 l2){ return zero(vlen(xmult(subt(p,l1),subt(p,l2))))&&(l1.x-p.x)*(l2.x-p.x)<eps&& (l1.y-p.y)*(l2.y-p.y)<eps&&(l1.z-p.z)*(l2.z-p.z)<eps; } //判點是否線上段上,不包括端點 int dot_online_ex(point3 p,line3 l){ return dot_online_in(p,l)&&(!zero(p.x-l.a.x)||!zero(p.y-l.a.y)||!zero(p.z-l.a.z))&& (!zero(p.x-l.b.x)||!zero(p.y-l.b.y)||!zero(p.z-l.b.z)); } int dot_online_ex(point3 p,point3 l1,point3 l2){ return dot_online_in(p,l1,l2)&&(!zero(p.x-l1.x)||!zero(p.y-l1.y)||!zero(p.z-l1.z))&& (!zero(p.x-l2.x)||!zero(p.y-l2.y)||!zero(p.z-l2.z)); } //判點是否在空間三角形上,包括邊界,三點共線無意義 int dot_inplane_in(point3 p,plane3 s){ return zero(vlen(xmult(subt(s.a,s.b),subt(s.a,s.c)))-vlen(xmult(subt(p,s.a),subt(p,s.b)))- vlen(xmult(subt(p,s.b),subt(p,s.c)))-vlen(xmult(subt(p,s.c),subt(p,s.a)))); } int dot_inplane_in(point3 p,point3 s1,point3 s2,point3 s3){ return zero(vlen(xmult(subt(s1,s2),subt(s1,s3)))-vlen(xmult(subt(p,s1),subt(p,s2)))- vlen(xmult(subt(p,s2),subt(p,s3)))-vlen(xmult(subt(p,s3),subt(p,s1)))); } //判點是否在空間三角形上,不包括邊界,三點共線無意義 int dot_inplane_ex(point3 p,plane3 s){ return dot_inplane_in(p,s)&&vlen(xmult(subt(p,s.a),subt(p,s.b)))>eps&& vlen(xmult(subt(p,s.b),subt(p,s.c)))>eps&&vlen(xmult(subt(p,s.c),subt(p,s.a)))>eps; } int dot_inplane_ex(point3 p,point3 s1,point3 s2,point3 s3){ return dot_inplane_in(p,s1,s2,s3)&&vlen(xmult(subt(p,s1),subt(p,s2)))>eps&& vlen(xmult(subt(p,s2),subt(p,s3)))>eps&&vlen(xmult(subt(p,s3),subt(p,s1)))>eps; } //判兩點線上段同側,點線上段上返回0,不共面無意義 int same_side(point3 p1,point3 p2,line3 l){ return dmult(xmult(subt(l.a,l.b),subt(p1,l.b)),xmult(subt(l.a,l.b),subt(p2,l.b)))>eps; } int same_side(point3 p1,point3 p2,point3 l1,point3 l2){ return dmult(xmult(subt(l1,l2),subt(p1,l2)),xmult(subt(l1,l2),subt(p2,l2)))>eps; } //判兩點線上段異側,點線上段上返回0,不共面無意義 int opposite_side(point3 p1,point3 p2,line3 l){ return dmult(xmult(subt(l.a,l.b),subt(p1,l.b)),xmult(subt(l.a,l.b),subt(p2,l.b)))<-eps; } int opposite_side(point3 p1,point3 p2,point3 l1,point3 l2){ return dmult(xmult(subt(l1,l2),subt(p1,l2)),xmult(subt(l1,l2),subt(p2,l2)))<-eps; } //判兩點在平面同側,點在平面上返回0 int same_side(point3 p1,point3 p2,plane3 s){ return dmult(pvec(s),subt(p1,s.a))*dmult(pvec(s),subt(p2,s.a))>eps; } int same_side(point3 p1,point3 p2,point3 s1,point3 s2,point3 s3){ return dmult(pvec(s1,s2,s3),subt(p1,s1))*dmult(pvec(s1,s2,s3),subt(p2,s1))>eps; } //判兩點在平面異側,點在平面上返回0 int opposite_side(point3 p1,point3 p2,plane3 s){ return dmult(pvec(s),subt(p1,s.a))*dmult(pvec(s),subt(p2,s.a))<-eps; } int opposite_side(point3 p1,point3 p2,point3 s1,point3 s2,point3 s3){ return dmult(pvec(s1,s2,s3),subt(p1,s1))*dmult(pvec(s1,s2,s3),subt(p2,s1))<-eps; } //判兩直線平行 int parallel(line3 u,line3 v){ return vlen(xmult(subt(u.a,u.b),subt(v.a,v.b)))<eps; } int parallel(point3 u1,point3 u2,point3 v1,point3 v2){ return vlen(xmult(subt(u1,u2),subt(v1,v2)))<eps; } //判兩平面平行 int parallel(plane3 u,plane3 v){ return vlen(xmult(pvec(u),pvec(v)))<eps; } int parallel(point3 u1,point3 u2,point3 u3,point3 v1,point3 v2,point3 v3){ return vlen(xmult(pvec(u1,u2,u3),pvec(v1,v2,v3)))<eps; } //判直線與平面平行 int parallel(line3 l,plane3 s){ return zero(dmult(subt(l.a,l.b),pvec(s))); } int parallel(point3 l1,point3 l2,point3 s1,point3 s2,point3 s3){ return zero(dmult(subt(l1,l2),pvec(s1,s2,s3))); } //判兩直線垂直 int perpendicular(line3 u,line3 v){ return zero(dmult(subt(u.a,u.b),subt(v.a,v.b))); } int perpendicular(point3 u1,point3 u2,point3 v1,point3 v2){ return zero(dmult(subt(u1,u2),subt(v1,v2))); } //判兩平面垂直 int perpendicular(plane3 u,plane3 v){ return zero(dmult(pvec(u),pvec(v))); } int perpendicular(point3 u1,point3 u2,point3 u3,point3 v1,point3 v2,point3 v3){ return zero(dmult(pvec(u1,u2,u3),pvec(v1,v2,v3))); } //判直線與平面平行 int perpendicular(line3 l,plane3 s){ return vlen(xmult(subt(l.a,l.b),pvec(s)))<eps; } int perpendicular(point3 l1,point3 l2,point3 s1,point3 s2,point3 s3){ return vlen(xmult(subt(l1,l2),pvec(s1,s2,s3)))<eps; } //判兩線段相交,包括端點和部分重合 int intersect_in(line3 u,line3 v){ if (!dots_onplane(u.a,u.b,v.a,v.b)) return 0; if (!dots_inline(u.a,u.b,v.a)||!dots_inline(u.a,u.b,v.b)) return !same_side(u.a,u.b,v)&&!same_side(v.a,v.b,u); return dot_online_in(u.a,v)||dot_online_in(u.b,v)||dot_online_in(v.a,u)||dot_online_in(v.b,u); } int intersect_in(point3 u1,point3 u2,point3 v1,point3 v2){ if (!dots_onplane(u1,u2,v1,v2)) return 0; if (!dots_inline(u1,u2,v1)||!dots_inline(u1,u2,v2)) return !same_side(u1,u2,v1,v2)&&!same_side(v1,v2,u1,u2); return dot_online_in(u1,v1,v2)||dot_online_in(u2,v1,v2)||dot_online_in(v1,u1,u2)||dot_online_in(v2,u1,u2); } //判兩線段相交,不包括端點和部分重合 int intersect_ex(line3 u,line3 v){ return dots_onplane(u.a,u.b,v.a,v.b)&&opposite_side(u.a,u.b,v)&&opposite_side(v.a,v.b,u); } int intersect_ex(point3 u1,point3 u2,point3 v1,point3 v2){ return dots_onplane(u1,u2,v1,v2)&&opposite_side(u1,u2,v1,v2)&&opposite_side(v1,v2,u1,u2); } //判線段與空間三角形相交,包括交於邊界和(部分)包含 int intersect_in(line3 l,plane3 s){ return !same_side(l.a,l.b,s)&&!same_side(s.a,s.b,l.a,l.b,s.c)&& !same_side(s.b,s.c,l.a,l.b,s.a)&&!same_side(s.c,s.a,l.a,l.b,s.b); } int intersect_in(point3 l1,point3 l2,point3 s1,point3 s2,point3 s3){ return !same_side(l1,l2,s1,s2,s3)&&!same_side(s1,s2,l1,l2,s3)&& !same_side(s2,s3,l1,l2,s1)&&!same_side(s3,s1,l1,l2,s2); } //判線段與空間三角形相交,不包括交於邊界和(部分)包含 int intersect_ex(line3 l,plane3 s){ return opposite_side(l.a,l.b,s)&&opposite_side(s.a,s.b,l.a,l.b,s.c)&& opposite_side(s.b,s.c,l.a,l.b,s.a)&&opposite_side(s.c,s.a,l.a,l.b,s.b); } int intersect_ex(point3 l1,point3 l2,point3 s1,point3 s2,point3 s3){ return opposite_side(l1,l2,s1,s2,s3)&&opposite_side(s1,s2,l1,l2,s3)&& opposite_side(s2,s3,l1,l2,s1)&&opposite_side(s3,s1,l1,l2,s2); } //計算兩直線交點,注意事先判斷直線是否共面和平行! //線段交點請另外判線段相交(同時還是要判斷是否平行!) point3 intersection(line3 u,line3 v){ point3 ret=u.a; double t=((u.a.x-v.a.x)*(v.a.y-v.b.y)-(u.a.y-v.a.y)*(v.a.x-v.b.x)) /((u.a.x-u.b.x)*(v.a.y-v.b.y)-(u.a.y-u.b.y)*(v.a.x-v.b.x)); ret.x+=(u.b.x-u.a.x)*t; ret.y+=(u.b.y-u.a.y)*t; ret.z+=(u.b.z-u.a.z)*t; return ret; } point3 intersection(point3 u1,point3 u2,point3 v1,point3 v2){ point3 ret=u1; double t=((u1.x-v1.x)*(v1.y-v2.y)-(u1.y-v1.y)*(v1.x-v2.x)) /((u1.x-u2.x)*(v1.y-v2.y)-(u1.y-u2.y)*(v1.x-v2.x)); ret.x+=(u2.x-u1.x)*t; ret.y+=(u2.y-u1.y)*t; ret.z+=(u2.z-u1.z)*t; return ret; } //計算直線與平面交點,注意事先判斷是否平行,並保證三點不共線! //線段和空間三角形交點請另外判斷 point3 intersection(line3 l,plane3 s){ point3 ret=pvec(s); double t=(ret.x*(s.a.x-l.a.x)+ret.y*(s.a.y-l.a.y)+ret.z*(s.a.z-l.a.z))/ (ret.x*(l.b.x-l.a.x)+ret.y*(l.b.y-l.a.y)+ret.z*(l.b.z-l.a.z)); ret.x=l.a.x+(l.b.x-l.a.x)*t; ret.y=l.a.y+(l.b.y-l.a.y)*t; ret.z=l.a.z+(l.b.z-l.a.z)*t; return ret; } point3 intersection(point3 l1,point3 l2,point3 s1,point3 s2,point3 s3){ point3 ret=pvec(s1,s2,s3); double t=(ret.x*(s1.x-l1.x)+ret.y*(s1.y-l1.y)+ret.z*(s1.z-l1.z))/ (ret.x*(l2.x-l1.x)+ret.y*(l2.y-l1.y)+ret.z*(l2.z-l1.z)); ret.x=l1.x+(l2.x-l1.x)*t; ret.y=l1.y+(l2.y-l1.y)*t; ret.z=l1.z+(l2.z-l1.z)*t; return ret; } //計算兩平面交線,注意事先判斷是否平行,並保證三點不共線! line3 intersection(plane3 u,plane3 v){ line3 ret; ret.a=parallel(v.a,v.b,u.a,u.b,u.c)?intersection(v.b,v.c,u.a,u.b,u.c):intersection(v.a,v.b,u.a,u.b,u.c); ret.b=parallel(v.c,v.a,u.a,u.b,u.c)?intersection(v.b,v.c,u.a,u.b,u.c):intersection(v.c,v.a,u.a,u.b,u.c); return ret; } line3 intersection(point3 u1,point3 u2,point3 u3,point3 v1,point3 v2,point3 v3){ line3 ret; ret.a=parallel(v1,v2,u1,u2,u3)?intersection(v2,v3,u1,u2,u3):intersection(v1,v2,u1,u2,u3); ret.b=parallel(v3,v1,u1,u2,u3)?intersection(v2,v3,u1,u2,u3):intersection(v3,v1,u1,u2,u3); return ret; } //點到直線距離 double ptoline(point3 p,line3 l){ return vlen(xmult(subt(p,l.a),subt(l.b,l.a)))/distance(l.a,l.b); } double ptoline(point3 p,point3 l1,point3 l2){ return vlen(xmult(subt(p,l1),subt(l2,l1)))/distance(l1,l2); } //點到平面距離 double ptoplane(point3 p,plane3 s){ return fabs(dmult(pvec(s),subt(p,s.a)))/vlen(pvec(s)); } double ptoplane(point3 p,point3 s1,point3 s2,point3 s3){ return fabs(dmult(pvec(s1,s2,s3),subt(p,s1)))/vlen(pvec(s1,s2,s3)); } //直線到直線距離 double linetoline(line3 u,line3 v){ point3 n=xmult(subt(u.a,u.b),subt(v.a,v.b)); return fabs(dmult(subt(u.a,v.a),n))/vlen(n); } double linetoline(point3 u1,point3 u2,point3 v1,point3 v2){ point3 n=xmult(subt(u1,u2),subt(v1,v2)); return fabs(dmult(subt(u1,v1),n))/vlen(n); } //兩直線夾角cos值 double angle_cos(line3 u,line3 v){ return dmult(subt(u.a,u.b),subt(v.a,v.b))/vlen(subt(u.a,u.b))/vlen(subt(v.a,v.b)); } double angle_cos(point3 u1,point3 u2,point3 v1,point3 v2){ return dmult(subt(u1,u2),subt(v1,v2))/vlen(subt(u1,u2))/vlen(subt(v1,v2)); } //兩平面夾角cos值 double angle_cos(plane3 u,plane3 v){ return dmult(pvec(u),pvec(v))/vlen(pvec(u))/vlen(pvec(v)); } double angle_cos(point3 u1,point3 u2,point3 u3,point3 v1,point3 v2,point3 v3){ return dmult(pvec(u1,u2,u3),pvec(v1,v2,v3))/vlen(pvec(u1,u2,u3))/vlen(pvec(v1,v2,v3)); } //直線平面夾角sin值 double angle_sin(line3 l,plane3 s){ return dmult(subt(l.a,l.b),pvec(s))/vlen(subt(l.a,l.b))/vlen(pvec(s)); } double angle_sin(point3 l1,point3 l2,point3 s1,point3 s2,point3 s3){ return dmult(subt(l1,l2),pvec(s1,s2,s3))/vlen(subt(l1,l2))/vlen(pvec(s1,s2,s3)); }