1 //三维几何函数库 2 #include <math.h> 3 #define eps 1e-8 4 #define zero(x) (((x)>0?(x):-(x))<eps) 5 struct point3{double x,y,z;}; 6 struct line3{point3 a,b;}; 7 struct plane3{point3 a,b,c;}; 8 9 //计算cross product U x V 10 point3 xmult(point3 u,point3 v){ 11 point3 ret; 12 ret.x=u.y*v.z-v.y*u.z; 13 ret.y=u.z*v.x-u.x*v.z; 14 ret.z=u.x*v.y-u.y*v.x; 15 return ret; 16 } 17 18 //计算dot product U . V 19 double dmult(point3 u,point3 v){ 20 return u.x*v.x+u.y*v.y+u.z*v.z; 21 } 22 23 //矢量差 U - V 24 point3 subt(point3 u,point3 v){ 25 point3 ret; 26 ret.x=u.x-v.x; 27 ret.y=u.y-v.y; 28 ret.z=u.z-v.z; 29 return ret; 30 } 31 32 //取平面法向量 33 point3 pvec(plane3 s){ 34 return xmult(subt(s.a,s.b),subt(s.b,s.c)); 35 } 36 point3 pvec(point3 s1,point3 s2,point3 s3){ 37 return xmult(subt(s1,s2),subt(s2,s3)); 38 } 39 40 //两点距离,单参数取向量大小 41 double distance(point3 p1,point3 p2){ 42 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)); 43 } 44 45 //向量大小 46 double vlen(point3 p){ 47 return sqrt(p.x*p.x+p.y*p.y+p.z*p.z); 48 } 49 50 //判三点共线 51 int dots_inline(point3 p1,point3 p2,point3 p3){ 52 return vlen(xmult(subt(p1,p2),subt(p2,p3)))<eps; 53 } 54 55 //判四点共面 56 int dots_onplane(point3 a,point3 b,point3 c,point3 d){ 57 return zero(dmult(pvec(a,b,c),subt(d,a))); 58 } 59 60 //判点是否在线段上,包括端点和共线 61 int dot_online_in(point3 p,line3 l){ 62 return zero(vlen(xmult(subt(p,l.a),subt(p,l.b))))&&(l.a.x-p.x)*(l.b.x-p.x)<eps&& 63 (l.a.y-p.y)*(l.b.y-p.y)<eps&&(l.a.z-p.z)*(l.b.z-p.z)<eps; 64 } 65 int dot_online_in(point3 p,point3 l1,point3 l2){ 66 return zero(vlen(xmult(subt(p,l1),subt(p,l2))))&&(l1.x-p.x)*(l2.x-p.x)<eps&& 67 (l1.y-p.y)*(l2.y-p.y)<eps&&(l1.z-p.z)*(l2.z-p.z)<eps; 68 } 69 70 //判点是否在线段上,不包括端点 71 int dot_online_ex(point3 p,line3 l){ 72 return dot_online_in(p,l)&&(!zero(p.x-l.a.x)||!zero(p.y-l.a.y)||!zero(p.z-l.a.z))&& 73 (!zero(p.x-l.b.x)||!zero(p.y-l.b.y)||!zero(p.z-l.b.z)); 74 } 75 int dot_online_ex(point3 p,point3 l1,point3 l2){ 76 return dot_online_in(p,l1,l2)&&(!zero(p.x-l1.x)||!zero(p.y-l1.y)||!zero(p.z-l1.z))&& 77 (!zero(p.x-l2.x)||!zero(p.y-l2.y)||!zero(p.z-l2.z)); 78 } 79 80 //判点是否在空间三角形上,包括边界,三点共线无意义 81 int dot_inplane_in(point3 p,plane3 s){ 82 return zero(vlen(xmult(subt(s.a,s.b),subt(s.a,s.c)))-vlen(xmult(subt(p,s.a),subt(p,s.b)))- 83 vlen(xmult(subt(p,s.b),subt(p,s.c)))-vlen(xmult(subt(p,s.c),subt(p,s.a)))); 84 } 85 int dot_inplane_in(point3 p,point3 s1,point3 s2,point3 s3){ 86 return zero(vlen(xmult(subt(s1,s2),subt(s1,s3)))-vlen(xmult(subt(p,s1),subt(p,s2)))- 87 vlen(xmult(subt(p,s2),subt(p,s3)))-vlen(xmult(subt(p,s3),subt(p,s1)))); 88 } 89 90 //判点是否在空间三角形上,不包括边界,三点共线无意义 91 int dot_inplane_ex(point3 p,plane3 s){ 92 return dot_inplane_in(p,s)&&vlen(xmult(subt(p,s.a),subt(p,s.b)))>eps&& 93 vlen(xmult(subt(p,s.b),subt(p,s.c)))>eps&&vlen(xmult(subt(p,s.c),subt(p,s.a)))>eps; 94 } 95 int dot_inplane_ex(point3 p,point3 s1,point3 s2,point3 s3){ 96 return dot_inplane_in(p,s1,s2,s3)&&vlen(xmult(subt(p,s1),subt(p,s2)))>eps&& 97 vlen(xmult(subt(p,s2),subt(p,s3)))>eps&&vlen(xmult(subt(p,s3),subt(p,s1)))>eps; 98 } 99 100 //判两点在线段同侧,点在线段上返回0,不共面无意义 101 int same_side(point3 p1,point3 p2,line3 l){ 102 return dmult(xmult(subt(l.a,l.b),subt(p1,l.b)),xmult(subt(l.a,l.b),subt(p2,l.b)))>eps; 103 } 104 int same_side(point3 p1,point3 p2,point3 l1,point3 l2){ 105 return dmult(xmult(subt(l1,l2),subt(p1,l2)),xmult(subt(l1,l2),subt(p2,l2)))>eps; 106 } 107 108 //判两点在线段异侧,点在线段上返回0,不共面无意义 109 int opposite_side(point3 p1,point3 p2,line3 l){ 110 return dmult(xmult(subt(l.a,l.b),subt(p1,l.b)),xmult(subt(l.a,l.b),subt(p2,l.b)))<-eps; 111 } 112 int opposite_side(point3 p1,point3 p2,point3 l1,point3 l2){ 113 return dmult(xmult(subt(l1,l2),subt(p1,l2)),xmult(subt(l1,l2),subt(p2,l2)))<-eps; 114 } 115 116 //判两点在平面同侧,点在平面上返回0 117 int same_side(point3 p1,point3 p2,plane3 s){ 118 return dmult(pvec(s),subt(p1,s.a))*dmult(pvec(s),subt(p2,s.a))>eps; 119 } 120 int same_side(point3 p1,point3 p2,point3 s1,point3 s2,point3 s3){ 121 return dmult(pvec(s1,s2,s3),subt(p1,s1))*dmult(pvec(s1,s2,s3),subt(p2,s1))>eps; 122 } 123 124 //判两点在平面异侧,点在平面上返回0 125 int opposite_side(point3 p1,point3 p2,plane3 s){ 126 return dmult(pvec(s),subt(p1,s.a))*dmult(pvec(s),subt(p2,s.a))<-eps; 127 } 128 int opposite_side(point3 p1,point3 p2,point3 s1,point3 s2,point3 s3){ 129 return dmult(pvec(s1,s2,s3),subt(p1,s1))*dmult(pvec(s1,s2,s3),subt(p2,s1))<-eps; 130 } 131 132 //判两直线平行 133 int parallel(line3 u,line3 v){ 134 return vlen(xmult(subt(u.a,u.b),subt(v.a,v.b)))<eps; 135 } 136 int parallel(point3 u1,point3 u2,point3 v1,point3 v2){ 137 return vlen(xmult(subt(u1,u2),subt(v1,v2)))<eps; 138 } 139 140 //判两平面平行 141 int parallel(plane3 u,plane3 v){ 142 return vlen(xmult(pvec(u),pvec(v)))<eps; 143 } 144 int parallel(point3 u1,point3 u2,point3 u3,point3 v1,point3 v2,point3 v3){ 145 return vlen(xmult(pvec(u1,u2,u3),pvec(v1,v2,v3)))<eps; 146 } 147 148 //判直线与平面平行 149 int parallel(line3 l,plane3 s){ 150 return zero(dmult(subt(l.a,l.b),pvec(s))); 151 } 152 int parallel(point3 l1,point3 l2,point3 s1,point3 s2,point3 s3){ 153 return zero(dmult(subt(l1,l2),pvec(s1,s2,s3))); 154 } 155 156 //判两直线垂直 157 int perpendicular(line3 u,line3 v){ 158 return zero(dmult(subt(u.a,u.b),subt(v.a,v.b))); 159 } 160 int perpendicular(point3 u1,point3 u2,point3 v1,point3 v2){ 161 return zero(dmult(subt(u1,u2),subt(v1,v2))); 162 } 163 164 //判两平面垂直 165 int perpendicular(plane3 u,plane3 v){ 166 return zero(dmult(pvec(u),pvec(v))); 167 } 168 int perpendicular(point3 u1,point3 u2,point3 u3,point3 v1,point3 v2,point3 v3){ 169 return zero(dmult(pvec(u1,u2,u3),pvec(v1,v2,v3))); 170 } 171 172 //判直线与平面平行 173 int perpendicular(line3 l,plane3 s){ 174 return vlen(xmult(subt(l.a,l.b),pvec(s)))<eps; 175 } 176 int perpendicular(point3 l1,point3 l2,point3 s1,point3 s2,point3 s3){ 177 return vlen(xmult(subt(l1,l2),pvec(s1,s2,s3)))<eps; 178 } 179 180 //判两线段相交,包括端点和部分重合 181 int intersect_in(line3 u,line3 v){ 182 if (!dots_onplane(u.a,u.b,v.a,v.b)) 183 return 0; 184 if (!dots_inline(u.a,u.b,v.a)||!dots_inline(u.a,u.b,v.b)) 185 return !same_side(u.a,u.b,v)&&!same_side(v.a,v.b,u); 186 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); 187 } 188 int intersect_in(point3 u1,point3 u2,point3 v1,point3 v2){ 189 if (!dots_onplane(u1,u2,v1,v2)) 190 return 0; 191 if (!dots_inline(u1,u2,v1)||!dots_inline(u1,u2,v2)) 192 return !same_side(u1,u2,v1,v2)&&!same_side(v1,v2,u1,u2); 193 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); 194 } 195 196 //判两线段相交,不包括端点和部分重合 197 int intersect_ex(line3 u,line3 v){ 198 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); 199 } 200 int intersect_ex(point3 u1,point3 u2,point3 v1,point3 v2){ 201 return dots_onplane(u1,u2,v1,v2)&&opposite_side(u1,u2,v1,v2)&&opposite_side(v1,v2,u1,u2); 202 } 203 204 //判线段与空间三角形相交,包括交于边界和(部分)包含 205 int intersect_in(line3 l,plane3 s){ 206 return !same_side(l.a,l.b,s)&&!same_side(s.a,s.b,l.a,l.b,s.c)&& 207 !same_side(s.b,s.c,l.a,l.b,s.a)&&!same_side(s.c,s.a,l.a,l.b,s.b); 208 } 209 int intersect_in(point3 l1,point3 l2,point3 s1,point3 s2,point3 s3){ 210 return !same_side(l1,l2,s1,s2,s3)&&!same_side(s1,s2,l1,l2,s3)&& 211 !same_side(s2,s3,l1,l2,s1)&&!same_side(s3,s1,l1,l2,s2); 212 } 213 214 //判线段与空间三角形相交,不包括交于边界和(部分)包含 215 int intersect_ex(line3 l,plane3 s){ 216 return opposite_side(l.a,l.b,s)&&opposite_side(s.a,s.b,l.a,l.b,s.c)&& 217 opposite_side(s.b,s.c,l.a,l.b,s.a)&&opposite_side(s.c,s.a,l.a,l.b,s.b); 218 } 219 int intersect_ex(point3 l1,point3 l2,point3 s1,point3 s2,point3 s3){ 220 return opposite_side(l1,l2,s1,s2,s3)&&opposite_side(s1,s2,l1,l2,s3)&& 221 opposite_side(s2,s3,l1,l2,s1)&&opposite_side(s3,s1,l1,l2,s2); 222 } 223 224 //计算两直线交点,注意事先判断直线是否共面和平行! 225 //线段交点请另外判线段相交(同时还是要判断是否平行!) 226 point3 intersection(line3 u,line3 v){ 227 point3 ret=u.a; 228 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)) 229 /((u.a.x-u.b.x)*(v.a.y-v.b.y)-(u.a.y-u.b.y)*(v.a.x-v.b.x)); 230 ret.x+=(u.b.x-u.a.x)*t; 231 ret.y+=(u.b.y-u.a.y)*t; 232 ret.z+=(u.b.z-u.a.z)*t; 233 return ret; 234 } 235 point3 intersection(point3 u1,point3 u2,point3 v1,point3 v2){ 236 point3 ret=u1; 237 double t=((u1.x-v1.x)*(v1.y-v2.y)-(u1.y-v1.y)*(v1.x-v2.x)) 238 /((u1.x-u2.x)*(v1.y-v2.y)-(u1.y-u2.y)*(v1.x-v2.x)); 239 ret.x+=(u2.x-u1.x)*t; 240 ret.y+=(u2.y-u1.y)*t; 241 ret.z+=(u2.z-u1.z)*t; 242 return ret; 243 } 244 245 //计算直线与平面交点,注意事先判断是否平行,并保证三点不共线! 246 //线段和空间三角形交点请另外判断 247 point3 intersection(line3 l,plane3 s){ 248 point3 ret=pvec(s); 249 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))/ 250 (ret.x*(l.b.x-l.a.x)+ret.y*(l.b.y-l.a.y)+ret.z*(l.b.z-l.a.z)); 251 ret.x=l.a.x+(l.b.x-l.a.x)*t; 252 ret.y=l.a.y+(l.b.y-l.a.y)*t; 253 ret.z=l.a.z+(l.b.z-l.a.z)*t; 254 return ret; 255 } 256 point3 intersection(point3 l1,point3 l2,point3 s1,point3 s2,point3 s3){ 257 point3 ret=pvec(s1,s2,s3); 258 double t=(ret.x*(s1.x-l1.x)+ret.y*(s1.y-l1.y)+ret.z*(s1.z-l1.z))/ 259 (ret.x*(l2.x-l1.x)+ret.y*(l2.y-l1.y)+ret.z*(l2.z-l1.z)); 260 ret.x=l1.x+(l2.x-l1.x)*t; 261 ret.y=l1.y+(l2.y-l1.y)*t; 262 ret.z=l1.z+(l2.z-l1.z)*t; 263 return ret; 264 } 265 266 //计算两平面交线,注意事先判断是否平行,并保证三点不共线! 267 line3 intersection(plane3 u,plane3 v){ 268 line3 ret; 269 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); 270 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); 271 return ret; 272 } 273 line3 intersection(point3 u1,point3 u2,point3 u3,point3 v1,point3 v2,point3 v3){ 274 line3 ret; 275 ret.a=parallel(v1,v2,u1,u2,u3)?intersection(v2,v3,u1,u2,u3):intersection(v1,v2,u1,u2,u3); 276 ret.b=parallel(v3,v1,u1,u2,u3)?intersection(v2,v3,u1,u2,u3):intersection(v3,v1,u1,u2,u3); 277 return ret; 278 } 279 280 //点到直线距离 281 double ptoline(point3 p,line3 l){ 282 return vlen(xmult(subt(p,l.a),subt(l.b,l.a)))/distance(l.a,l.b); 283 } 284 double ptoline(point3 p,point3 l1,point3 l2){ 285 return vlen(xmult(subt(p,l1),subt(l2,l1)))/distance(l1,l2); 286 } 287 288 //点到平面距离 289 double ptoplane(point3 p,plane3 s){ 290 return fabs(dmult(pvec(s),subt(p,s.a)))/vlen(pvec(s)); 291 } 292 double ptoplane(point3 p,point3 s1,point3 s2,point3 s3){ 293 return fabs(dmult(pvec(s1,s2,s3),subt(p,s1)))/vlen(pvec(s1,s2,s3)); 294 } 295 296 //直线到直线距离 297 double linetoline(line3 u,line3 v){ 298 point3 n=xmult(subt(u.a,u.b),subt(v.a,v.b)); 299 return fabs(dmult(subt(u.a,v.a),n))/vlen(n); 300 } 301 double linetoline(point3 u1,point3 u2,point3 v1,point3 v2){ 302 point3 n=xmult(subt(u1,u2),subt(v1,v2)); 303 return fabs(dmult(subt(u1,v1),n))/vlen(n); 304 } 305 306 //两直线夹角cos值 307 double angle_cos(line3 u,line3 v){ 308 return dmult(subt(u.a,u.b),subt(v.a,v.b))/vlen(subt(u.a,u.b))/vlen(subt(v.a,v.b)); 309 } 310 double angle_cos(point3 u1,point3 u2,point3 v1,point3 v2){ 311 return dmult(subt(u1,u2),subt(v1,v2))/vlen(subt(u1,u2))/vlen(subt(v1,v2)); 312 } 313 314 //两平面夹角cos值 315 double angle_cos(plane3 u,plane3 v){ 316 return dmult(pvec(u),pvec(v))/vlen(pvec(u))/vlen(pvec(v)); 317 } 318 double angle_cos(point3 u1,point3 u2,point3 u3,point3 v1,point3 v2,point3 v3){ 319 return dmult(pvec(u1,u2,u3),pvec(v1,v2,v3))/vlen(pvec(u1,u2,u3))/vlen(pvec(v1,v2,v3)); 320 } 321 322 //直线平面夹角sin值 323 double angle_sin(line3 l,plane3 s){ 324 return dmult(subt(l.a,l.b),pvec(s))/vlen(subt(l.a,l.b))/vlen(pvec(s)); 325 } 326 double angle_sin(point3 l1,point3 l2,point3 s1,point3 s2,point3 s3){ 327 return dmult(subt(l1,l2),pvec(s1,s2,s3))/vlen(subt(l1,l2))/vlen(pvec(s1,s2,s3)); 328 }