三维计算几何模板 hdu 5733 tetrahedron（不知为何WA）

#include <cstdio>#include <iostream>#include <cmath>#include <set>using namespace std;#define L(i) i<<1#define R(i) i<<1|1#define INF  0x3f3f3f3f#define pi acos(-1.0)#define eps 1e-9#define maxn 2000010#define MOD 1000000007#define zero(x) (((x)>0?(x):-(x))<eps)struct Point3{    double x,y,z;    Point3(){}    Point3(const Point3 &a):x(a.x),y(a.y),z(a.z){} Point3(double x,double y,double z):x(x),y(y),z(z){}};struct Line3{    Point3 a,b;    Line3(){} Line3(Point3 a,Point3 b):a(a),b(b){}};struct Plane3{    Point3 a,b,c;    Plane3(){} Plane3(Point3 a,Point3 b,Point3 c):a(a),b(b),c(c){}};Point3 Point[4],Vector[4],center;Plane3 Plane[4],angle_plane[4];Line3 Line;double x,y,z,ans;Point3 operator + (Point3 A,Point3 B){return Point3(A.x + B.x,A.y + B.y,A.z + B.z);}Point3 operator - (Point3 A,Point3 B){return Point3(A.x-B.x,A.y-B.y,A.z-B.z);}Point3 operator * (Point3 A,double p){return Point3(A.x * p,A.y * p,A.z * p);}Point3 operator / (Point3 A,double p){return Point3(A.x / p,A.y / p,A.z / p);}//计算cross product U x VPoint3 xmult(Point3 u,Point3 v){ Point3 ret; ret.x=u.y*v.z-u.z*v.y; 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 . Vdouble dmult(Point3 u,Point3 v){ return u.x*v.x+u.y*v.y+u.z*v.z;}//矢量差 U - VPoint3 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.b,s.a),subt(s.c,s.a));}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;}//判两点在平面同侧,点在平面上返回0int 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;}//判两点在平面异侧,点在平面上返回0int 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.b,v.c,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));}int main(){   // freopen("std")    while(scanf("%lf%lf%lf",&x,&y,&z) != EOF)    {        Point[0] = Point3(x,y,z);        for(int i = 1; i < 4; i++)        {            scanf("%lf%lf%lf",&x,&y,&z);            Point[i] = Point3(x,y,z);        }        if(dots_onplane(Point[0],Point[1],Point[2],Point[3]))        {            printf("O O O O\n");            continue;        }        Plane[0] = Plane3(Point[2],Point[1],Point[0]);        Plane[1] = Plane3(Point[0],Point[3],Point[2]);        Plane[2] = Plane3(Point[3],Point[0],Point[1]);        Plane[3] = Plane3(Point[1],Point[2],Point[3]);        for(int i = 1; i < 4; i++)        {            Vector[i] = pvec(Plane[i-1])/vlen(pvec(Plane[i-1])) + pvec(Plane[3])/vlen(pvec(Plane[3]));            Point3 p = Vector[i] + Point[i];            angle_plane[i] = Plane3(Point[i],Point[i+1==4?1:i+1],p);        }        Line = intersection(angle_plane[1],angle_plane[2]);        center = intersection(Line,angle_plane[3]);        ans = ptoplane(center,Plane[0]);        printf("%.4f %.4f %.4f %.4f\n",center.x,center.y,center.z,ans);    }    return 0;}