三维计算几何模版

网上找了一个三维计算几何模版，完善了一下，使它能使用了...

#include <cstdio>
#include <cstring>
#include <algorithm>

using namespace std;

/***********基础*************/

const double EPS=0.000001;

typedef struct Point_3D {
    double x, y, z;
    Point_3D(double xx = 0, double yy = 0, double zz = 0): x(xx), y(yy), z(zz) {}

    bool operator == (const Point_3D& A) const {
        return x==A.x && y==A.y && z==A.z;
    }
}Vector_3D;

Point_3D read_Point_3D() {
    double x,y,z;
    scanf("%lf%lf%lf",&x,&y,&z);
    return Point_3D(x,y,z);
}

Vector_3D operator + (const Vector_3D & A, const Vector_3D & B) {
    return Vector_3D(A.x + B.x, A.y + B.y, A.z + B.z);
}

Vector_3D operator - (const Point_3D & A, const Point_3D & B) {
    return Vector_3D(A.x - B.x, A.y - B.y, A.z - B.z);
}

Vector_3D operator * (const Vector_3D & A, double p) {
    return Vector_3D(A.x * p, A.y * p, A.z * p);
}

Vector_3D operator / (const Vector_3D & A, double p) {
    return Vector_3D(A.x / p, A.y / p, A.z / p);
}

double Dot(const Vector_3D & A, const Vector_3D & B) {
    return A.x * B.x + A.y * B.y + A.z * B.z;
}

double Length(const Vector_3D & A) {
    return sqrt(Dot(A, A));
}

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

Vector_3D Cross(const Vector_3D & A, const Vector_3D & B) {
    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);
}

double Area2(const Point_3D & A, const Point_3D & B, const Point_3D & C) {
    return Length(Cross(B - A, C - A));
}

double Volume6(const Point_3D & A, const Point_3D & B, const Point_3D & C, const Point_3D & D) {
    return Dot(D - A, Cross(B - A, C - A));
}

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

/************点线面*************/
// 点p到平面p0-n的距离。n必须为单位向量
double DistanceToPlane(const Point_3D & p, const Point_3D & p0, const Vector_3D & n)
{
    return fabs(Dot(p - p0, n)); // 如果不取绝对值，得到的是有向距离
}

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

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

// 点P到直线AB的距离
double DistanceToLine(const Point_3D & P, const Point_3D & A, const Point_3D & B)
{
    Vector_3D v1 = B - A, v2 = P - A;
    return Length(Cross(v1, v2)) / Length(v1);
}

//点到线段的距离
double DistanceToSeg(Point_3D p, Point_3D a, Point_3D b)
{
    if(a == b)
    {
        return Length(p - a);
    }

    Vector_3D v1 = b - a, v2 = p - a, v3 = p - b;

    if(Dot(v1, v2) + EPS < 0)
    {
        return Length(v2);
    }
    else
    {
        if(Dot(v1, v3) - EPS > 0)
        {
            return Length(v3);
        }
        else
        {
            return Length(Cross(v1, v2)) / Length(v1);
        }
    }
}

//求异面直线 p1+s*u与p2+t*v的公垂线对应的s 如果平行|重合，返回false
bool LineDistance3D(Point_3D p1, Vector_3D u, Point_3D p2, Vector_3D v, double & s)
{
    double b = Dot(u, u) * Dot(v, v) - Dot(u, v) * Dot(u, v);

    if(abs(b) <= EPS)
    {
        return false;
    }

    double a = Dot(u, v) * Dot(v, p1 - p2) - Dot(v, v) * Dot(u, p1 - p2);
    s = a / b;
    return true;
}

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

// 点P在三角形P0, P1, P2中
bool PointInTri(const Point_3D & P, const Point_3D & P0, const Point_3D & P1, const Point_3D & P2)
{
    return SameSide(P, P0, P1, P2) && SameSide(P, P1, P0, P2) && SameSide(P, P2, P0, P1);
}

// 三角形P0P1P2是否和线段AB相交
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)
{
    Vector_3D n = Cross(P1 - P0, P2 - P0);

    if(abs(Dot(n, B - A)) <= EPS)
    {
        return false;    // 线段A-B和平面P0P1P2平行或共面
    }
    else   // 平面A和直线P1-P2有惟一交点
    {
        double t = Dot(n, P0 - A) / Dot(n, B - A);

        if(t + EPS < 0 || t - 1 - EPS > 0)
        {
            return false;    // 不在线段AB上
        }

        P = A + (B - A) * t; // 交点
        return PointInTri(P, P0, P1, P2);
    }
}

//空间两三角形是否相交
bool TriTriIntersection(Point_3D * T1, Point_3D * T2)
{
    Point_3D P;

    for(int i = 0; i < 3; i++)
    {
        if(TriSegIntersection(T1[0], T1[1], T1[2], T2[i], T2[(i + 1) % 3], P))
        {
            return true;
        }

        if(TriSegIntersection(T2[0], T2[1], T2[2], T1[i], T1[(i + 1) % 3], P))
        {
            return true;
        }
    }

    return false;
}

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

// 判断P是否在三角形A, B, C中，并且到三条边的距离都至少为mindist。保证P, A, B, C共面
bool InsideWithMinDistance(const Point_3D & P, const Point_3D & A, const Point_3D & B, const Point_3D & C, double mindist)
{
    if(!PointInTri(P, A, B, C))
    {
        return false;
    }

    if(DistanceToLine(P, A, B) < mindist)
    {
        return false;
    }

    if(DistanceToLine(P, B, C) < mindist)
    {
        return false;
    }

    if(DistanceToLine(P, C, A) < mindist)
    {
        return false;
    }

    return true;
}

// 判断P是否在凸四边形ABCD（顺时针或逆时针）中，并且到四条边的距离都至少为mindist。保证P, A, B, C, D共面
bool InsideWithMinDistance(const Point_3D & P, const Point_3D & A, const Point_3D & B, const Point_3D & C, const Point_3D & D, double mindist)
{
    if(!PointInTri(P, A, B, C))
    {
        return false;
    }

    if(!PointInTri(P, C, D, A))
    {
        return false;
    }

    if(DistanceToLine(P, A, B) < mindist)
    {
        return false;
    }

    if(DistanceToLine(P, B, C) < mindist)
    {
        return false;
    }

    if(DistanceToLine(P, C, D) < mindist)
    {
        return false;
    }

    if(DistanceToLine(P, D, A) < mindist)
    {
        return false;
    }

    return true;
}


/*************凸包相关问题*******************/
//加干扰
double rand01()
{
    return rand() / (double)RAND_MAX;
}
double randeps()
{
    return (rand01() - 0.5) * EPS;
}
Point_3D add_noise(const Point_3D & p)
{
    return Point_3D(p.x + randeps(), p.y + randeps(), p.z + randeps());
}

struct Face
{
    int v[3];
    Face(int a, int b, int c)
    {
        v[0] = a;
        v[1] = b;
        v[2] = c;
    }
    Vector_3D Normal(const vector<Point_3D> & P) const
    {
        return Cross(P[v[1]] - P[v[0]], P[v[2]] - P[v[0]]);
    }
    // f是否能看见P[i]
    int CanSee(const vector<Point_3D> & P, int i) const
    {
        return Dot(P[i] - P[v[0]], Normal(P)) > 0;
    }
};

// 增量法求三维凸包
// 注意：没有考虑各种特殊情况（如四点共面）。实践中，请在调用前对输入点进行微小扰动
vector<Face> CH3D(const vector<Point_3D> & 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;
}

struct ConvexPolyhedron
{
    int n;
    vector<Point_3D> P, P2;
    vector<Face> faces;

    bool read()
    {
        if(scanf("%d", &n) != 1)
        {
            return false;
        }

        P.resize(n);
        P2.resize(n);

        for(int i = 0; i < n; i++)
        {
            P[i] = read_Point_3D();
            P2[i] = add_noise(P[i]);
        }

        faces = CH3D(P2);
        return true;
    }

    //三维凸包重心
    Point_3D centroid()
    {
        Point_3D C = P[0];
        double totv = 0;
        Point_3D tot(0, 0, 0);

        for(int i = 0; i < faces.size(); i++)
        {
            Point_3D 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(Point_3D C)
    {
        double ans = 1e30;

        for(int i = 0; i < faces.size(); i++)
        {
            Point_3D 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;
    }
};

int main() {
    
    return 0;
}