【模板】三维几何模板

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

 

posted @ 2020-04-19 23:41  小布鞋  阅读(82)  评论(0编辑  收藏