【模板】三维几何模板

  1 #include <cstdio>
  2 #include <cstring>
  3 #include <algorithm>
  4  
  5 using namespace std;
  6  
  7 /***********基础*************/
  8  
  9 const double EPS=0.000001;
 10  
 11 typedef struct Point_3D {
 12     double x, y, z;
 13     Point_3D(double xx = 0, double yy = 0, double zz = 0): x(xx), y(yy), z(zz) {}
 14  
 15     bool operator == (const Point_3D& A) const {
 16         return x==A.x && y==A.y && z==A.z;
 17     }
 18 }Vector_3D;
 19  
 20 Point_3D read_Point_3D() {
 21     double x,y,z;
 22     scanf("%lf%lf%lf",&x,&y,&z);
 23     return Point_3D(x,y,z);
 24 }
 25  
 26 Vector_3D operator + (const Vector_3D & A, const Vector_3D & B) {
 27     return Vector_3D(A.x + B.x, A.y + B.y, A.z + B.z);
 28 }
 29  
 30 Vector_3D operator - (const Point_3D & A, const Point_3D & B) {
 31     return Vector_3D(A.x - B.x, A.y - B.y, A.z - B.z);
 32 }
 33  
 34 Vector_3D operator * (const Vector_3D & A, double p) {
 35     return Vector_3D(A.x * p, A.y * p, A.z * p);
 36 }
 37  
 38 Vector_3D operator / (const Vector_3D & A, double p) {
 39     return Vector_3D(A.x / p, A.y / p, A.z / p);
 40 }
 41  
 42 double Dot(const Vector_3D & A, const Vector_3D & B) {
 43     return A.x * B.x + A.y * B.y + A.z * B.z;
 44 }
 45  
 46 double Length(const Vector_3D & A) {
 47     return sqrt(Dot(A, A));
 48 }
 49  
 50 double Angle(const Vector_3D & A, const Vector_3D & B) {
 51     return acos(Dot(A, B) / Length(A) / Length(B));
 52 }
 53  
 54 Vector_3D Cross(const Vector_3D & A, const Vector_3D & B) {
 55     return Vector_3D(A.y * B.z - A.z * B.y, A.z * B.x - A.x * B.z, A.x * B.y - A.y * B.x);
 56 }
 57  
 58 double Area2(const Point_3D & A, const Point_3D & B, const Point_3D & C) {
 59     return Length(Cross(B - A, C - A));
 60 }
 61 //混合积
 62 double Volume6(const Point_3D & A, const Point_3D & B, const Point_3D & C, const Point_3D & D) {
 63     return Dot(D - A, Cross(B - A, C - A));
 64 }
 65  
 66 // 四面体的重心
 67 Point_3D Centroid(const Point_3D & A, const Point_3D & B, const Point_3D & C, const Point_3D & D) {
 68     return (A + B + C + D) / 4.0;
 69 }
 70  
 71 /************点线面*************/
 72 // 点p到平面p0-n的距离。n必须为单位向量
 73 double DistanceToPlane(const Point_3D & p, const Point_3D & p0, const Vector_3D & n)
 74 {
 75     return fabs(Dot(p - p0, n)); // 如果不取绝对值，得到的是有向距离
 76 }
 77  
 78 // 点p在平面p0-n上的投影。n必须为单位向量
 79 Point_3D GetPlaneProjection(const Point_3D & p, const Point_3D & p0, const Vector_3D & n)
 80 {
 81     return p - n * Dot(p - p0, n);
 82 }
 83  
 84 //直线p1-p2 与平面p0-n的交点
 85 Point_3D LinePlaneIntersection(Point_3D p1, Point_3D p2, Point_3D p0, Vector_3D n)
 86 {
 87     Vector_3D v= p2 - p1;
 88     double t = (Dot(n, p0 - p1) / Dot(n, p2 - p1)); //分母为0，直线与平面平行或在平面上
 89     return p1 + v * t; //如果是线段 判断t是否在0~1之间
 90 }
 91  
 92 // 点P到直线AB的距离
 93 double DistanceToLine(const Point_3D & P, const Point_3D & A, const Point_3D & B)
 94 {
 95     Vector_3D v1 = B - A, v2 = P - A;
 96     return Length(Cross(v1, v2)) / Length(v1);
 97 }
 98  
 99 //点到线段的距离
100 double DistanceToSeg(Point_3D p, Point_3D a, Point_3D b)
101 {
102     if(a == b)
103     {
104         return Length(p - a);
105     }
106  
107     Vector_3D v1 = b - a, v2 = p - a, v3 = p - b;
108  
109     if(Dot(v1, v2) + EPS < 0)
110     {
111         return Length(v2);
112     }
113     else
114     {
115         if(Dot(v1, v3) - EPS > 0)
116         {
117             return Length(v3);
118         }
119         else
120         {
121             return Length(Cross(v1, v2)) / Length(v1);
122         }
123     }
124 }
125 
126 //点 p 关于平面 p0-n 的对称点
127 point p_plane_q(point p, point p0, point n) {
128     double d = 2 * dot(p - p0, n) / n.len();
129     return p - n * d;
130 }
131 //点关于直线的对称点
132 point p_line_q(point p, point a, point b) {
133     point k = cross(b - a, p - a);
134     if (dcmp(k.len()) == 0) return p;
135     k = cross(k, b - a);
136     return p_plane_q(p, a, k);
137 }
138 
139 //求异面直线 p1+s*u与p2+t*v的公垂线对应的s 如果平行|重合，返回false
140 bool LineDistance3D(Point_3D p1, Vector_3D u, Point_3D p2, Vector_3D v, double & s)
141 {
142     double b = Dot(u, u) * Dot(v, v) - Dot(u, v) * Dot(u, v);
143  
144     if(abs(b) <= EPS)
145     {
146         return false;
147     }
148  
149     double a = Dot(u, v) * Dot(v, p1 - p2) - Dot(v, v) * Dot(u, p1 - p2);
150     s = a / b;
151     return true;
152 }
153  
154 // p1和p2是否在线段a-b的同侧,(p1,p2,a,b 同一平面)
155 // p1,p2,a,b不同平面时，表示p1ab,p2ab两个半平面夹角是否钝角
156 bool SameSide(const Point_3D & p1, const Point_3D & p2, const Point_3D & a, const Point_3D & b)
157 {
158     return Dot(Cross(b - a, p1 - a), Cross(b - a, p2 - a)) - EPS >= 0;
159 }
160  
161 // 点P在三角形P0, P1, P2中 (p,p0,p1,p2 同一平面)
162 // p和p0p1,p2不同一面时，判断点p是否在p0,p1,p2形成的三角形柱形内
163 bool PointInTri(const Point_3D & P, const Point_3D & P0, const Point_3D & P1, const Point_3D & P2)
164 {
165     return SameSide(P, P0, P1, P2) && SameSide(P, P1, P0, P2) && SameSide(P, P2, P0, P1);
166 }
167  
168 // 三角形P0P1P2是否和线段AB相交
169 bool TriSegIntersection(const Point_3D & P0, const Point_3D & P1, const Point_3D & P2, const Point_3D & A, const Point_3D & B, Point_3D & P)
170 {
171     Vector_3D n = Cross(P1 - P0, P2 - P0);
172  
173     if(abs(Dot(n, B - A)) <= EPS)
174     {
175         return false;    // 线段A-B和平面P0P1P2平行或共面
176     }
177     else   // 平面A和直线P1-P2有惟一交点
178     {
179         double t = Dot(n, P0 - A) / Dot(n, B - A);
180  
181         if(t + EPS < 0 || t - 1 - EPS > 0)
182         {
183             return false;    // 不在线段AB上
184         }
185  
186         P = A + (B - A) * t; // 交点
187         return PointInTri(P, P0, P1, P2);
188     }
189 }
190  
191 //空间两三角形是否相交
192 bool TriTriIntersection(Point_3D * T1, Point_3D * T2)
193 {
194     Point_3D P;
195  
196     for(int i = 0; i < 3; i++)
197     {
198         if(TriSegIntersection(T1[0], T1[1], T1[2], T2[i], T2[(i + 1) % 3], P))
199         {
200             return true;
201         }
202  
203         if(TriSegIntersection(T2[0], T2[1], T2[2], T1[i], T1[(i + 1) % 3], P))
204         {
205             return true;
206         }
207     }
208  
209     return false;
210 }
211  
212 //空间两直线上最近点对 返回最近距离 点对保存在ans1 ans2中
213 double SegSegDistance(Point_3D a1, Point_3D b1, Point_3D a2, Point_3D b2, Point_3D& ans1, Point_3D& ans2)
214 {
215     Vector_3D v1 = (a1 - b1), v2 = (a2 - b2);
216     Vector_3D N = Cross(v1, v2);
217     Vector_3D ab = (a1 - a2);
218     double ans = Dot(N, ab) / Length(N);
219     Point_3D p1 = a1, p2 = a2;
220     Vector_3D d1 = b1 - a1, d2 = b2 - a2;
221     double t1, t2;
222     t1 = Dot((Cross(p2 - p1, d2)), Cross(d1, d2));
223     t2 = Dot((Cross(p2 - p1, d1)), Cross(d1, d2));
224     double dd = Length((Cross(d1, d2)));
225     t1 /= dd * dd;
226     t2 /= dd * dd;
227     ans1 = (a1 + (b1 - a1) * t1);
228     ans2 = (a2 + (b2 - a2) * t2);
229     return fabs(ans);
230 }
231  
232 // 判断P是否在凸四边形ABCD（顺时针或逆时针）中，并且到四条边的距离都至少为mindist。保证P, A, B, C, D共面
233 bool InsideWithMinDistance(const Point_3D & P, const Point_3D & A, const Point_3D & B, const Point_3D & C, const Point_3D & D, double mindist)
234 {
235     if(!PointInTri(P, A, B, C)) return false;
236     if(!PointInTri(P, A, B, D)) return false;
237     if(!PointInTri(P, B, C, D)) return false;
238     if(!PointInTri(P, A, C, D)) return false;
239     
240     return true;
241 }
242 
243 //两直线距离。n.len() = 0说明直线平行
244 double LineDis(point a, point b, point c, point d) {
245     point n = cross(a - b, c - d);
246     if (dcmp(n.len()) == 0) return point_to_line(a, c, d);
247     return fabs(dot(a - c, n)) / n.len();
248 }
249 // 两线段距离
250 double SegDis(point a, point b, point c, point d) {
251     point n = cross(a - b, c - d);
252     if (dcmp(n.len()) != 0) {
253         point cc = point_plane_projection(c, a, n);
254         point dd = point_plane_projection(d, a, n);
255         point res;
256         if (SegCross(a, b, cc, dd, res) == 1)
257             return LineDis(a, b, c, d);
258     }
259     double ret = point_to_seg(a, c, d);
260     ret = min(ret, point_to_seg(b, c, d));
261     ret = min(ret, point_to_seg(c, a, b));
262     ret = min(ret, point_to_seg(d, a, b));
263     return ret;
264 }
265 //直线相交判定 0:不相交(平行); 1:规范相交; 2:非规范相交(重合); 3:异面不相交 
266 int LineCross(point a, point b, point c, point d, point &res) {
267     point n = cross(a - b, c - d);
268     if (dcmp(n.len()) == 0) {
269         if (dcmp(cross(a - b, c - b).len()) == 0) return 2;
270         return 0;
271     }
272     if (dcmp(point_to_line(a, c, d)) == 0) { res = a; return 1; }
273     if (dcmp(point_to_line(b, c, d)) == 0) { res = b; return 1; }
274     if (dcmp(point_to_line(c, a, b)) == 0) { res = c; return 1; }
275     if (dcmp(point_to_line(d, a, b)) == 0) { res = d; return 1; }
276     if (dcmp(dot(cross(b - a, c - a), d - a)) != 0) return 3;
277     n = d + n;
278     point f = cross(d - c, n - c);
279     double t = dot(f, c - a) / dot(f, b - a);
280     res = a + (b - a) * t;
281     return 1;
282 }
283 // 线段相交判定 0:不相交; 1:规范相交; 2:非规范相交
284 int SegCross(point a, point b, point c, point d, point &res) {
285     int k = LineCross(a, b, c, d, res);
286     if (k == 0 || k == 3) return 0;
287     if (k == 1) {
288         double d1 = dot(a - res, b - res);
289         double d2 = dot(c - res, d - res);
290         if (d1 < 0 && d2 < 0) return 1;
291         if (d1 == 0 && d2 <= 0 || d2 == 0 && d1 <= 0) return 2;
292         return 0;
293     }
294     if (dot(a - c, b - c) <= 0 || dot(a - d, b - d) <= 0
295         || dot(c - a, d - a) <= 0 || dot(c - b, d - b) <= 0)
296         return 2;
297     return 0;
298 }
299 
300 //直线 p1-p2 与平面 p0-n 的位置关系
301 //0:相交 1:平行 2:垂直
302 int LineAndPlane(point p1, point p2, point p0, point n) {
303     point v = p2 - p1;
304     if (dcmp(dot(v, n)) == 0) return 1;
305     if (dcmp(cross(v, n).len()) == 0) return 2;
306     return 0;
307 }
308 //平面 p0-n0 和 p1-n1 的位置关系
309 //0:有唯一交线 1:两平面垂直 2:两平面重合 3:两平面平行不重合
310 int PlaneAndPlane(point p0, point n0, point p1, point n1) {
311     if (dcmp(dot(n0, n1)) == 0) return 1;
312     if (dcmp(cross(n0, n1).len()) == 0) {
313         if (dcmp(dot(n0, p1 - p0)) == 0) return 2;
314         return 3;
315     }
316     return 0;
317 }
318 //平面 p0-n0 和 p1-n1 的距离
319 double PlaneDis(point p0, point n0, point p1, point n1) {
320     if (PlaneAndPlane(p0, n0, p1, n1) != 3) return 0;
321     return fabs(dot(p1 - p0, n0)) / n0.len();
322 }
323 
324 
325 /**********************反射*******************************/
326 //平面反射:射线起点 s，方向 v，平面 p0 - n
327 void reflect(point s, point v, point p0, point n, point &rs, point &rv) {
328     LinePlaneInter(s, s + v, p0, n, rs);
329     point tmp = p_plane_q(s, p0, n);
330     rv = rs - tmp;
331 }
332 //球面反射:射线起点 s，方向 v，球心 p，半径 r
333 bool reflect(point s, point v, point p, double r, point &rs, point &rv) {
334     double a = dot(v, v);
335     double b = dot(s - p, v) * 2;
336     double c = dot(s - p, s - p) - r * r;
337     double dlt = b * b - 4 * a * c;
338     if (dlt < 0) return false;
339     double t = (-b - sqrt(dlt)) / a / 2;
340     rs = s + v * t;
341     point tn = p - rs;
342     rv = v - tn * (dot(v, tn) * 2 / dot(tn, tn));
343     return true;
344 }
345 
346 /*************凸包相关问题*******************/
347 //加干扰
348 double rand01()
349 {
350     return rand() / (double)RAND_MAX;
351 }
352 double randeps()
353 {
354     return (rand01() - 0.5) * EPS;
355 }
356 Point_3D add_noise(const Point_3D & p)
357 {
358     return Point_3D(p.x + randeps(), p.y + randeps(), p.z + randeps());
359 }
360  
361 struct Face
362 {
363     int v[3];
364     Face(int a, int b, int c)
365     {
366         v[0] = a;
367         v[1] = b;
368         v[2] = c;
369     }
370     Vector_3D Normal(const vector<Point_3D> & P) const
371     {
372         return Cross(P[v[1]] - P[v[0]], P[v[2]] - P[v[0]]);
373     }
374     // f是否能看见P[i]
375     int CanSee(const vector<Point_3D> & P, int i) const
376     {
377         return Dot(P[i] - P[v[0]], Normal(P)) > 0;
378     }
379 };
380  
381 // 增量法求三维凸包
382 // 注意：没有考虑各种特殊情况（如四点共面）。实践中，请在调用前对输入点进行微小扰动
383 vector<Face> CH3D(const vector<Point_3D> & P)
384 {
385     int n = P.size();
386     vector<vector<int> > vis(n);
387  
388     for(int i = 0; i < n; i++)
389     {
390         vis[i].resize(n);
391     }
392  
393     vector<Face> cur;
394     cur.push_back(Face(0, 1, 2)); // 由于已经进行扰动，前三个点不共线
395     cur.push_back(Face(2, 1, 0));
396  
397     for(int i = 3; i < n; i++)
398     {
399         vector<Face> next;
400  
401         // 计算每条边的“左面”的可见性
402         for(int j = 0; j < cur.size(); j++)
403         {
404             Face & f = cur[j];
405             int res = f.CanSee(P, i);
406  
407             if(!res)
408             {
409                 next.push_back(f);
410             }
411  
412             for(int k = 0; k < 3; k++)
413             {
414                 vis[f.v[k]][f.v[(k + 1) % 3]] = res;
415             }
416         }
417  
418         for(int j = 0; j < cur.size(); j++)
419             for(int k = 0; k < 3; k++)
420             {
421                 int a = cur[j].v[k], b = cur[j].v[(k + 1) % 3];
422  
423                 if(vis[a][b] != vis[b][a] && vis[a][b]) // (a,b)是分界线，左边对P[i]可见
424                 {
425                     next.push_back(Face(a, b, i));
426                 }
427             }
428  
429         cur = next;
430     }
431  
432     return cur;
433 }
434  
435 struct ConvexPolyhedron
436 {
437     int n;
438     vector<Point_3D> P, P2;
439     vector<Face> faces;
440  
441     bool read()
442     {
443         if(scanf("%d", &n) != 1)
444         {
445             return false;
446         }
447  
448         P.resize(n);
449         P2.resize(n);
450  
451         for(int i = 0; i < n; i++)
452         {
453             P[i] = read_Point_3D();
454             P2[i] = add_noise(P[i]);
455         }
456  
457         faces = CH3D(P2);
458         return true;
459     }
460  
461     //三维凸包重心
462     Point_3D centroid()
463     {
464         Point_3D C = P[0];
465         double totv = 0;
466         Point_3D tot(0, 0, 0);
467  
468         for(int i = 0; i < faces.size(); i++)
469         {
470             Point_3D p1 = P[faces[i].v[0]], p2 = P[faces[i].v[1]], p3 = P[faces[i].v[2]];
471             double v = -Volume6(p1, p2, p3, C);
472             totv += v;
473             tot = tot + Centroid(p1, p2, p3, C) * v;
474         }
475  
476         return tot / totv;
477     }
478     //凸包重心到表面最近距离
479     double mindist(Point_3D C)
480     {
481         double ans = 1e30;
482  
483         for(int i = 0; i < faces.size(); i++)
484         {
485             Point_3D p1 = P[faces[i].v[0]], p2 = P[faces[i].v[1]], p3 = P[faces[i].v[2]];
486             ans = min(ans, fabs(-Volume6(p1, p2, p3, C) / Area2(p1, p2, p3)));
487         }
488  
489         return ans;
490     }
491 };
492  
493 int main() {
494     
495     return 0;
496 }