三維幾何模板

  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 }