模板：三维计算几何

三维几何

struct Coord3
{
    double X,Y,Z;
    Coord3(double X=0,double Y=0,double Z=0):X(X),Y(Y),Z(Z) {}
};

int sgn(double d)
{
    return (d>EPS)-(d<-EPS);
}
bool operator!=(const Coord3 &a,const Coord3 &b)//不等运算符，涉及到浮点数比较要重写
{
    return sgn(a.X-b.X)||sgn(a.Y-b.Y)||sgn(a.Z-b.Z);
}
bool operator==(const Coord3 &a,const Coord3 &b)
{
    return !(a!=b);
}

Coord3& operator+=(Coord3 &a,const Coord3 &b)
{
    return a.X+=b.X,a.Y+=b.Y,a.Z+=b.Z,a;
}
Coord3 operator+(Coord3 a,const Coord3 &b)
{
    return a+=b;
}

Coord3& operator-=(Coord3 &a,const Coord3 &b)
{
    return a.X-=b.X,a.Y-=b.Y,a.Z-=b.Z,a;
}
Coord3 operator-(Coord3 a,const Coord3 &b)
{
    return a-=b;
}

Coord3& operator*=(Coord3 &a,double d)
{
    return a.X*=d,a.Y*=d,a.Z*=d,a;
}
Coord3 operator*(Coord3 a,double d)
{
    return a*=d;
}
Coord3 operator*(double d,Coord3 a)
{
    return a*=d;
}

Coord3& operator/=(Coord3 &a,double d)
{
    return a.X/=d,a.Y/=d,a.Z/=d,a;
}
Coord3 operator/(Coord3 a,double d)
{
    return a/=d;
}

double Dot(const Coord3& A,const Coord3& B)
{
    return A.X*B.X+A.Y*B.Y+A.Z*B.Z;
}

Coord3 Cross(const Coord3& A,const Coord3& B)
{
    return Coord3(A.Y*B.Z-A.Z*B.Y,A.Z*B.X-A.X*B.Z,A.X*B.Y-A.Y*B.X);
}

double norm(const Coord3& A)
{
    return Dot(A,A);
}

double abs(const Coord3& A)
{
    return sqrt(norm(A));
}

double Angle(const Coord3& A,const Coord3& B)
{
    return acos(Dot(A,B)/abs(A)/abs(B));
}

double Area2(Coord3 A,Coord3 B,Coord3 C)
{
    return abs(Cross(B-A,C-A));
}

double Volume6(Coord3 A, Coord3 B, Coord3 C, Coord3 D)//四面体体积
{
    return Dot(D-A,Cross(B-A,C-A));
}

Coord3 Centroid(Coord3 A, Coord3 B, Coord3 C, Coord3 D)//四面体的重心
{
    return (A+B+C+D)/4.0;
}

//线和平面
double DistanceToPlane(Coord3 p,Coord3 p0,const Coord3& n)// 点p到平面p0-n的有向距离
{
    return Dot(p-p0,n)/abs(n);
}

Coord3 getPlaneProjection(Coord3 p, Coord3 p0, const Coord3& n)// 点p在平面p0-n上的投影。n必须为单位向量
{
    return p-n*Dot(p-p0,n);
}

Coord3 LinePlaneIntersection(Coord3 p1, Coord3 p2, Coord3 p0, Coord3 n)//直线p1-p2 与平面p0-n的交点，假设交点唯一存在
{
    Coord3 v = p2-p1;
    double t = Dot(n, p0-p1) / Dot(n, p2-p1);//分母为0，直线与平面平行或在平面上
    return p1 + v*t; //如果是线段 判断t是否在0~1之间
}

double DistanceToLine(Coord3 P,Coord3 A,Coord3 B)// 点P到直线AB的距离
{
    Coord3 v1=B-A,v2=P-A;
    return abs(Cross(v1,v2))/abs(v1);
}

double DistanceToSeg(Coord3 P, Coord3 A, Coord3 B)//点到线段的距离
{
    if(A==B)return abs(P-A);
    Coord3 v1=B-A,v2=P-A,v3=P-B;
    if(sgn(Dot(v1,v2))<0)return abs(v2);
    if(sgn(Dot(v1,v3))>0)return abs(v3);
    return fabs(DistanceToLine(P,A,B));
}

bool LineDistance3D(Coord3 p1, Coord3 u, Coord3 p2, Coord3 v, double& s)//求异面直线 p1+s*u与p2+t*v的公垂线对应的s，如果平行|重合，返回0
{
    double b = Dot(u, u) * Dot(v, v) - Dot(u, v) * Dot(u, v);
    if(!sgn(b)) return 0;
    double a = Dot(u, v) * Dot(v, p1-p2) - Dot(v, v) * Dot(u, p1-p2);
    s = a/b;
    return 1;
}

bool SameSide(Coord3 p1, Coord3 p2, Coord3 a, Coord3 b)// p1和p2是否在线段a-b的同侧
{
    return sgn(Dot(Cross(b-a, p1-a), Cross(b-a, p2-a)))>=0;
}

bool PointInTri(Coord3 PP, Coord3 *P)// 点P在三角形P0, P1, P2中
{
    return SameSide(PP,P[0],P[1],P[2])&&
           SameSide(PP,P[1],P[0],P[2])&&
           SameSide(PP,P[2],P[0],P[1]);
}

bool TriSegIntersection(Coord3* P,Coord3 A,Coord3 B,Coord3& PP)// 三角形P0P1P2是否和线段AB相交
{
    Coord3 n = Cross(P[1]-P[0], P[2]-P[0]);
    if(sgn(Dot(n, B-A)) == 0) return false; // 线段A-B和平面P0P1P2平行或共面
    double t = Dot(n, P[0]-A) / Dot(n, B-A);// 平面A和直线P1-P2有惟一交点
    if(sgn(t) < 0 || sgn(t-1) > 0) return false; // 不在线段AB上
    PP = A + (B-A)*t; // 交点
    return PointInTri(PP, P);
}

bool TriTriIntersection(Coord3* T1, Coord3* T2)//空间两三角形是否相交
{
    Coord3 P;
    for(int i = 0; i < 3; i++)
        if(TriSegIntersection(T1, T2[i], T2[(i+1)%3], P)||
                TriSegIntersection(T2, T1[i], T1[(i+1)%3], P))
            return 1;
    return 0;
}

double SegSegDistance(Coord3 a1, Coord3 b1, Coord3 a2, Coord3 b2,Coord3 &ans1,Coord3 &ans2)//空间两直线上最近点对 返回最近距离 点对保存在ans1 ans2中
{
    Coord3 v1 = (a1-b1), v2 = (a2-b2);
    Coord3 N = Cross(v1, v2);
    Coord3 ab = (a1-a2);
    double ans = Dot(N, ab) / abs(N);
    Coord3 p1 = a1, p2 = a2;
    Coord3 d1 = b1-a1, d2 = b2-a2;
    double t1, t2;
    t1 = Dot((Cross(p2-p1, d2)), Cross(d1, d2));
    t2 = Dot((Cross(p2-p1, d1)), Cross(d1, d2));
    double dd = abs((Cross(d1, d2)));
    t1 /= dd*dd;
    t2 /= dd*dd;
    ans1 = (a1 + (b1-a1)*t1);
    ans2 = (a2 + (b2-a2)*t2);
    return fabs(ans);
}

bool InsideWithMinDistance(Coord3 PP, Coord3 *P, double mindist)// 判断PP是否在三角形P中，并且到三条边的距离都至少为mindist。保证P, A, B, C共面
{
    return PointInTri(PP,P)&&
           DistanceToLine(PP,P[0],P[1])>=mindist||
           DistanceToLine(PP,P[1],P[2])>=mindist||
           DistanceToLine(PP,P[2],P[0])>=mindist;
}

//空间多边形和凸包
struct ConveXPolYhedron//凸包问题
{
    struct Face
    {
        int v[3];
        Face(int a, int b, int c)
        {
            v[0] = a,v[1] = b,v[2] = c;
        }
        Coord3 Normal(const vector<Coord3>& P) const
        {
            return Cross(P[v[1]]-P[v[0]], P[v[2]]-P[v[0]]);
        }
        bool CanSee(const vector<Coord3>& P, int i) const// f是否能看见P[i]
        {
            return Dot(P[i]-P[v[0]], Normal(P)) > 0;
        }
    };
    double randeps()
    {
        return (rand()/(double)RAND_MAX-0.5)*EPS;
    }
    void add_noise(Coord3 &p)//使坐标微小偏移
    {
        p+=Coord3(randeps(),randeps(),randeps());
    }
    vector<Coord3> P;
    vector<Face> faces;
    void read(vector<Coord3> P2)
    {
        P=P2;
        for(int i = 0; i < P2.size(); i++)
            add_noise(P2[i]);
        faces = CH3D(P2);
    }
    vector<Face> CH3D(const vector<Coord3>& P)//增量法求三维凸包，P已经过扰动，没有考虑各种特殊情况（如四点共面）
    {
        int n = P.size();
        vector<vector<int> > vis(n);
        for(int i = 0; i < n; i++) vis[i].resize(n);
        vector<Face> cur;
        cur.push_back(Face(0, 1, 2)); // 由于已经进行扰动，前三个点不共线
        cur.push_back(Face(2, 1, 0));
        for(int i = 3; i < n; i++)
        {
            vector<Face> neXt;
            for(int j = 0; j < cur.size(); j++)// 计算每条边的“左面”的可见性
            {
                Face& f = cur[j];
                int res = f.CanSee(P, i);
                if(!res) neXt.push_back(f);
                for(int k = 0; k < 3; k++) vis[f.v[k]][f.v[(k+1)%3]] = res;
            }
            for(int j = 0; j < cur.size(); j++)
                for(int k = 0; k < 3; k++)
                {
                    int a = cur[j].v[k], b = cur[j].v[(k+1)%3];
                    if(vis[a][b] != vis[b][a] && vis[a][b]) // (a,b)是分界线，左边对P[i]可见
                        neXt.push_back(Face(a, b, i));
                }
            cur = neXt;
        }
        return cur;
    }
    Coord3 centroid()//三维凸包重心
    {
        Coord3 C = P[0],tot(0,0,0);
        double totv = 0;
        for(int i = 0; i < faces.size(); i++)
        {
            Coord3 p1 = P[faces[i].v[0]], p2 = P[faces[i].v[1]], p3 = P[faces[i].v[2]];
            double v = -Volume6(p1, p2, p3, C);
            totv += v;
            tot = tot + Centroid(p1, p2, p3, C)*v;
        }
        return tot / totv;
    }
    double mindist(Coord3 C)//凸包重心到表面最近距离
    {
        double ans = 1e30;//INF
        for(int i = 0; i < faces.size(); i++)
        {
            Coord3 p1 = P[faces[i].v[0]], p2 = P[faces[i].v[1]], p3 = P[faces[i].v[2]];
            ans = min(ans, fabs(-Volume6(p1, p2, p3, C) / Area2(p1, p2, p3)));
        }
        return ans;
    }
};