三维计算几何模板

#include <iostream>
#include <cstring>
#include <cstdio>
#include <cmath>
#include <algorithm>
#include <set>
#include <map>
#include <vector>
#include <queue>
#include <ctime>
using namespace std;

#define LL long long
#define inf 0x3f3f3f3f
#define Pi acos(-1.0)
#define N 410
#define M 200020
#define eps 1e-10

double dcmp(double x) {
    if (fabs(x) < eps) return 0;
    return x < 0 ? -1 : 1;
}
struct point {
    double x, y, z;
    point(double x = 0, double y = 0, double z = 0) : x(x), y(y), z(z) {}
    point operator - (const point &b) const {
        return point(x - b.x, y - b.y, z - b.z);
    }
    point operator + (const point &b) const {
        return point(x + b.x, y + b.y, z + b.z);
    }
    point operator * (const double &k) const {
        return point(x * k, y * k, z * k);
    }
    point operator / (const double &k) const {
        return point(x / k, y / k, z / k);
    }
    double len() {
        return sqrt(x * x + y * y + z * z);
    }
    point fix() {
        double l = len();
        return point(x / l, y / l, z / l);
    }
};
double dot(point a, point b) {
    return a.x * b.x + a.y * b.y + a.z * b.z;
}
point cross(point a, point b) {
    return point(a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x);
}
// 三角形abc面积的两倍
double area2(point a, point b, point c) {
    return cross(b - a, c - a).len();
}
// 返回ab，ac，ad的混合积。它等于四面体abcd的有向体积的6倍
double volume6(point a, point b, point c, point d) {
    return dot(d - a, cross(b - a, c - a));
}
// 点到直线的距离
double point_to_line(point p, point a, point b) {
    return cross(b - a, p - a).len() / (b - a).len();
}
// 点到线段距离
double point_to_seg(point p, point a, point b) {
    if (dcmp(dot(p - a, b - a)) < 0) return (p - a).len();
    if (dcmp(dot(p - b, a - b)) < 0) return (p - b).len();
    return (cross(p - a, b - a)).len() / (b - a).len();
}
// 点到平面的距离(
double point_to_plane(point p, point p0, point n) {
    return fabs(dot(p - p0, n)) / n.len();
}
// 点在平面上的投影
point point_plane_projection(point p, point p0, point n) {
    double d = dot(p - p0, n) / n.len();
    return p - n * d;
}
//点 p 关于平面 p0-n 的对称点
point p_plane_q(point p, point p0, point n) {
    double d = 2 * dot(p - p0, n) / n.len();
    return p - n * d;
}
//点关于直线的对称点
point p_line_q(point p, point a, point b) {
    point k = cross(b - a, p - a);
    if (dcmp(k.len()) == 0) return p;
    k = cross(k, b - a);
    return p_plane_q(p, a, k);
}
//两直线距离。n.len() = 0说明直线平行
double LineDis(point a, point b, point c, point d) {
    point n = cross(a - b, c - d);
    if (dcmp(n.len()) == 0) return point_to_line(a, c, d);
    return fabs(dot(a - c, n)) / n.len();
}
// 两线段距离
double SegDis(point a, point b, point c, point d) {
    point n = cross(a - b, c - d);
    if (dcmp(n.len()) != 0) {
        point cc = point_plane_projection(c, a, n);
        point dd = point_plane_projection(d, a, n);
        point res;
        if (SegCross(a, b, cc, dd, res) == 1)
            return LineDis(a, b, c, d);
    }
    double ret = point_to_seg(a, c, d);
    ret = min(ret, point_to_seg(b, c, d));
    ret = min(ret, point_to_seg(c, a, b));
    ret = min(ret, point_to_seg(d, a, b));
    return ret;
}
//直线相交判定 0:不相交(平行); 1:规范相交; 2:非规范相交(重合); 3:异面不相交 
int LineCross(point a, point b, point c, point d, point &res) {
    point n = cross(a - b, c - d);
    if (dcmp(n.len()) == 0) {
        if (dcmp(cross(a - b, c - b).len()) == 0) return 2;
        return 0;
    }
    if (dcmp(point_to_line(a, c, d)) == 0) { res = a; return 1; }
    if (dcmp(point_to_line(b, c, d)) == 0) { res = b; return 1; }
    if (dcmp(point_to_line(c, a, b)) == 0) { res = c; return 1; }
    if (dcmp(point_to_line(d, a, b)) == 0) { res = d; return 1; }
    if (dcmp(dot(cross(b - a, c - a), d - a)) != 0) return 3;
    n = d + n;
    point f = cross(d - c, n - c);
    double t = dot(f, c - a) / dot(f, b - a);
    res = a + (b - a) * t;
    return 1;
}
// 线段相交判定 0:不相交; 1:规范相交; 2:非规范相交
int SegCross(point a, point b, point c, point d, point &res) {
    int k = LineCross(a, b, c, d, res);
    if (k == 0 || k == 3) return 0;
    if (k == 1) {
        double d1 = dot(a - res, b - res);
        double d2 = dot(c - res, d - res);
        if (d1 < 0 && d2 < 0) return 1;
        if (d1 == 0 && d2 <= 0 || d2 == 0 && d1 <= 0) return 2;
        return 0;
    }
    if (dot(a - c, b - c) <= 0 || dot(a - d, b - d) <= 0
        || dot(c - a, d - a) <= 0 || dot(c - b, d - b) <= 0)
        return 2;
    return 0;
}
// 直线p1 - p2到平面p0 - n的交点
bool LinePlaneInter(point p1, point p2, point p0, point n, point &s) {
    point v = p2 - p1;
    if (dcmp(dot(n, p2 - p1)) == 0) return 0;   //直线和平面平行或者直线在平面上
    double t = (dot(n, p0 - p1) / dot(n, p2 - p1));
    //if( t < 0 || t > 1 ) return 0;       如果是线段，判断t是不是在0 和 1之间
    s = p1 + v * t;
    return 1;
}
//直线 p1-p2 与平面 p0-n 的位置关系
//0:相交 1:平行 2:垂直
int LineAndPlane(point p1, point p2, point p0, point n) {
        point v = p2 - p1;
    if (dcmp(dot(v, n)) == 0) return 1;
    if (dcmp(cross(v, n).len()) == 0) return 2;
    return 0;
}
//平面 p0-n0 和 p1-n1 的位置关系
//0:有唯一交线 1:两平面垂直 2:两平面重合 3:两平面平行不重合
int PlaneAndPlane(point p0, point n0, point p1, point n1) {
    if (dcmp(dot(n0, n1)) == 0) return 1;
    if (dcmp(cross(n0, n1).len()) == 0) {
        if (dcmp(dot(n0, p1 - p0)) == 0) return 2;
        return 3;
    }
    return 0;
}
//平面 p0-n0 和 p1-n1 的距离
double PlaneDis(point p0, point n0, point p1, point n1) {
    if (PlaneAndPlane(p0, n0, p1, n1) != 3) return 0;
    return fabs(dot(p1 - p0, n0)) / n0.len();
}
// 球面距离
double cal(point a, point b, double R) {
    double d = (a - b).len();
    return 2 * R * asin(d / (2 * R));
}
// 求异面直线p1＋su 和 p2+tv 的公垂线对应的s，如果平行/重合，return 0;
bool line_distance(point p1, point u, point p2, point v, double &s) {
    double b = dot(u, v) * dot(v, u) - dot(u, u) * dot(v, v);
    if (dcmp(b) == 0) return 0;
    double a = dot(p2 - p1, v) * dot(v, u) - dot(p2 - p1, u) * dot(v, v);
    s = a / b;
    return true;
}
//求异面直线的距离 和 距离最近的两点
double line_distance(point a, point b, point c, point d, point &ans1, point &ans2) {
    point p1 = b - a, p2 = c - d;
    point p = cross(p1, p2);
    double ret = dot(p, c - a) / p.len(), s, t;
    s = (dot(cross(c - a, p2), p));
    s /= p.len() * p.len();
    t = (dot(cross(c - a, p1), p));
    t /= p.len() * p.len();
    ans1 = a + p1 * s, ans2 = c + p2 * t;
    return ret;
}
//平面反射:射线起点 s，方向 v，平面 p0 - n
void reflect(point s, point v, point p0, point n, point &rs, point &rv) {
    LinePlaneInter(s, s + v, p0, n, rs);
    point tmp = p_plane_q(s, p0, n);
    rv = rs - tmp;
}
//球面反射:射线起点 s，方向 v，球心 p，半径 r
bool reflect(point s, point v, point p, double r, point &rs, point &rv) {
    double a = dot(v, v);
    double b = dot(s - p, v) * 2;
    double c = dot(s - p, s - p) - r * r;
    double dlt = b * b - 4 * a * c;
    if (dlt < 0) return false;
    double t = (-b - sqrt(dlt)) / a / 2;
    rs = s + v * t;
    point tn = p - rs;
    rv = v - tn * (dot(v, tn) * 2 / dot(tn, tn));
    return true;
}
struct face {
    int a, b, c;    //表示凸包一个面上的三个点的编号
    bool ok;    //表示该面是否属于最终凸包上的面
};
struct CH3D {
    int n;    //初始顶点数
    point P[N];    //初始顶点
    int num;    //凸包表面的三角形数
    face F[8 * N];    //凸包表面的三角形
    int g[N][N];    //凸包表面的三角形
                    //向量长度
    double vlen(point a) {
        return a.len();
    }
    // 三角形面积*2
    double area2(point a, point b, point c) {
        return vlen(cross(b - a, c - a));
    }
    //四面体有向体积*6
    double volume6(point a, point b, point c, point d) {
        return dot(d - a, cross(b - a, c - a));
    }
    //正：点在面同向
    double dblcmp(point &p, face &f) {
        point m = P[f.b] - P[f.a];
        point n = P[f.c] - P[f.a];
        point t = p - P[f.a];
        double ret = dot(cross(m, n), t);
        return ret;
    }
    void deal(int p, int a, int b) {
        int f = g[a][b];        //搜索与该边相邻的另一个平面
        face add;
        if (F[f].ok) {
            if (dblcmp(P[p], F[f]) > eps)
                dfs(p, f);
            else {
                add.a = b;
                add.b = a;
                add.c = p; //这里注意顺序，要成右手系
                add.ok = 1;
                g[p][b] = g[a][p] = g[b][a] = num;
                F[num++] = add;
            }
        }
    }
    void dfs(int p, int now) { //递归搜索所有应该从凸包内删除的面
        F[now].ok = 0;
        deal(p, F[now].b, F[now].a);
        deal(p, F[now].c, F[now].b);
        deal(p, F[now].a, F[now].c);
    }
    bool same(int s, int t) {
        point &a = P[F[s].a];
        point &b = P[F[s].b];
        point &c = P[F[s].c];
        return dcmp(volume6(a, b, c, P[F[t].a])) == 0 &&
            dcmp(volume6(a, b, c, P[F[t].b])) == 0 &&
            dcmp(volume6(a, b, c, P[F[t].c])) == 0;
    }
    void create() {
        face add;
        num = 0;
        if (n < 4) return;
        //下面一段是为了保证前四个点不共面
        bool flag = 1;
        for (int i = 1; i < n; ++i) {
            if (vlen(P[0] - P[i]) > eps) {
                swap(P[1], P[i]);
                flag = 0;
                break;
            }
        }
        if (flag) return;
        flag = 1;
        for (int i = 2; i < n; ++i) {
            if (vlen(cross(P[1] - P[0], P[i] - P[0])) > eps) {
                swap(P[2], P[i]);
                flag = 0;
                break;
            }
        }
        if (flag) return;
        flag = 1;
        for (int i = 3; i < n; ++i) {
            if (fabs(volume6(P[0], P[1], P[2], P[i])) > eps) {
                swap(P[3], P[i]);
                flag = 0;
                break;
            }
        }
        if (flag) return;
        for (int i = 0; i < 4; ++i) {
            add.a = (i + 1) % 4;
            add.b = (i + 2) % 4;
            add.c = (i + 3) % 4;
            add.ok = 1;
            if (dblcmp(P[i], add) > 0)
                swap(add.b, add.c);
            g[add.a][add.b] = g[add.b][add.c] = g[add.c][add.a] = num;
            F[num++] = add;
        }
        for (int i = 4; i < n; ++i) {
            for (int j = 0; j < num; ++j) {
                if (F[j].ok && dblcmp(P[i], F[j]) > eps) {
                    dfs(i, j);
                    break;
                }
            }
        }
        int tmp = num; num = 0;
        for (int i = 0; i < tmp; ++i)
            if (F[i].ok)
                F[num++] = F[i];
    }
    //凸包表面积
    double area() {
        double ret = 0;
        if (n == 3)
            return area2(P[0], P[1], P[2]) / 2.0;
        for (int i = 0; i < num; ++i)
            ret += area2(P[F[i].a], P[F[i].b], P[F[i].c]);
        return ret / 2.0;
    }
    //凸包体积
    double volume() {
        double ret = 0;
        point tmp(0, 0, 0);
        for (int i = 0; i < num; ++i)
            ret += volume6(tmp, P[F[i].a], P[F[i].b], P[F[i].c]);
        return fabs(ret) / 6.0;
    }
    //凸包表面三角形个数
    int triangle() {
        return num;
    }
    //凸包表面多边形个数
    int polygon() {
        int ret = 0, flag = 0;
        for (int i = 0; i < num; ++i) {
            flag = 1;
            for (int j = 0; j < i; ++j)
                if (same(i, j)) {
                    flag = 0;
                    break;
                }
            ret += flag;
        }
        return ret;
    }
    //三维凸包重心
    point masscenter() {
        point ans(0, 0, 0), o(0, 0, 0);
        double all = 0;
        for (int i = 0; i < num; ++i) {
            double vol = volume6(o, P[F[i].a], P[F[i].b], P[F[i].c]);
            ans = ans + (o + P[F[i].a] + P[F[i].b] + P[F[i].c]) / 4 * vol;
            all += vol;
        }
        ans = ans / all;
        return ans;
    }
    //点到面的距离
    double point_to_face(point p, int i) {
        return fabs(volume6(P[F[i].a], P[F[i].b], P[F[i].c], p) / area2(P[F[i].a], P[F[i].b], P[F[i].c]));
    }
};
// 把法向量为t的平面投影到x-y平面上，并旋转所有点(把平面P:a*x+b*y+c*z=d旋转平移到平面z=0)
void change(point *p, point *pp, point t, int n) {
    double a = t.x, b = t.y, c = t.z;
    if (dcmp(a * a + b * b) == 0) {
        for (int i = 0; i < n; ++i)
            p[i].x = pp[i].x, p[i].y = pp[i].y;
        return;
    }
    for (int i = 0; i < n; ++i) {
        double cosC = b / sqrt(a * a + b * b);
        double sinC = a / sqrt(a * a + b * b);
        point temp;
        temp.x = pp[i].x * cosC - pp[i].y * sinC;
        temp.y = pp[i].x * sinC + pp[i].y * cosC;
        temp.z = pp[i].z;
        pp[i] = temp;
        cosC = c / sqrt(a * a + b * b + c * c);
        sinC = sqrt(a * a + b * b) / sqrt(a * a + b * b + c * c);
        temp.x = pp[i].x;
        temp.y = pp[i].y * cosC - pp[i].z * sinC;
        temp.z = pp[i].y * sinC + pp[i].z * cosC;
        pp[i] = temp;
        p[i].x = pp[i].x, p[i].y = pp[i].y;
    }
}