三维计算几何模板整理

/***********基础*************/
struct Point3 {
  double x, y, z;
  Point3(double x=0, double y=0, double z=0):x(x),y(y),z(z) { }
};

typedef Point3 Vector3;

Vector3 operator + (const Vector3& A, const Vector3& B) { return Vector3(A.x+B.x, A.y+B.y, A.z+B.z); }
Vector3 operator - (const Point3& A, const Point3& B) { return Vector3(A.x-B.x, A.y-B.y, A.z-B.z); }
Vector3 operator * (const Vector3& A, double p) { return Vector3(A.x*p, A.y*p, A.z*p); }
Vector3 operator / (const Vector3& A, double p) { return Vector3(A.x/p, A.y/p, A.z/p); }

double Dot(const Vector3& A, const Vector3& B) { return A.x*B.x + A.y*B.y + A.z*B.z; }
double Length(const Vector3& A) { return sqrt(Dot(A, A)); }
double Angle(const Vector3& A, const Vector3& B) { return acos(Dot(A, B) / Length(A) / Length(B)); }
Vector3 Cross(const Vector3& A, const Vector3& B) { return Vector3(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 Point3& A, const Point3& B, const Point3& C) { return Length(Cross(B-A, C-A)); }
double Volume6(const Point3& A, const Point3& B, const Point3& C, const Point3& D) { return Dot(D-A, Cross(B-A, C-A)); }
// 四面体的重心
Point3 Centroid(const Point3& A, const Point3& B, const Point3& C, const Point3& D) { return (A + B + C + D)/4.0; }

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

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

//直线p1-p2 与平面p0-n的交点
Point3 LinePlaneIntersection(Point3 p1, Point3 p2, Point3 p0, Vector3 n)
{
    vector3 = 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 Point3& P, const Point3& A, const Point3& B) {
  Vector3 v1 = B - A, v2 = P - A;
  return Length(Cross(v1, v2)) / Length(v1);
}

//点到线段的距离
double DistanceToSeg(Point3 p, Point3 a, Point3 b)
{
    if(a == b) return Length(p-a);
    Vector3 v1 = b-a, v2 = p-a, v3 = p-b;
    if(dcmp(Dot(v1, v2)) < 0) return Length(v2);
    else if(dcmp(Dot(v1, v3)) > 0) return Length(v3);
    else return Length(Cross(v1, v2)) / Length(v1);  
}

//求异面直线 p1+s*u与p2+t*v的公垂线对应的s 如果平行|重合，返回false
bool LineDistance3D(Point3 p1, Vector3 u, Point3 p2, Vector3 v, double& s)
{
    double b = Dot(u, u) * Dot(v, v) - Dot(u, v) * Dot(u, v);
    if(dcmp(b) == 0) 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 Point3& p1, const Point3& p2, const Point3& a, const Point3& b) {
  return dcmp(Dot(Cross(b-a, p1-a), Cross(b-a, p2-a))) >= 0;
}

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

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

//空间两三角形是否相交
bool TriTriIntersection(Point3* T1, Point3* T2) {
  Point3 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(Point3 a1, Point3 b1, Point3 a2, Point b2)
{
    Vector v1 = (a1-b1), v2 = (a2-b2);
    Vector N = Cross(v1, v2);
    Vector ab = (a1-a2);
    double ans = Dot(N, ab) / Length(N);
    Point p1 = a1, p2 = a2;
    Vector 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 Point3& P, const Point3& A, const Point3& B, const Point3& 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 Point3& P, const Point3& A, const Point3& B, const Point3& C, const Point3& 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; }
Point3 add_noise(const Point3& p) {
  return Point3(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; }
  Vector3 Normal(const vector<Point3>& P) const {
    return Cross(P[v[1]]-P[v[0]], P[v[2]]-P[v[0]]);
  }
  // f是否能看见P[i]
  int CanSee(const vector<Point3>& P, int i) const {
    return Dot(P[i]-P[v[0]], Normal(P)) > 0;
  }
};

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

  //三维凸包重心
  Point3 centroid() {
    Point3 C = P[0];
    double totv = 0;
    Point3 tot(0,0,0);
    for(int i = 0; i < faces.size(); i++) {
      Point3 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(Point3 C) {
    double ans = 1e30;
    for(int i = 0; i < faces.size(); i++) {
      Point3 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;
  }
};

//三维凸包
struct Point  
{  
    double x, y, z;  
    Point(double x = 0, double y = 0, double z = 0) : x(x), y(y), z(z) {}  
  
    inline void read()  
    {  
        scanf("%lf%lf%lf", &x, &y, &z);  
    }  
  
    //两向量之差   
    inline Point operator- (Point p)  
    {  
        return Point(x - p.x, y - p.y, z - p.z);  
    }  
  
    //两向量之和   
    inline Point operator+ (Point p)  
    {  
        return Point(x + p.x, y + p.y, z + p.z);  
    }  
  
    //叉乘   
    inline Point operator* (Point p)  
    {  
        return Point(y * p.z - z * p.y, z * p.x - x * p.z, x * p.y - y * p.x);  
    }  
  
    inline Point operator* (double d)  
    {  
        return Point(x * d, y * d, z * d);  
    }  
  
    inline Point operator/ (double d)  
    {  
        return Point(x / d, y / d, z / d);  
    }  
  
    //点乘   
    inline double operator^ (Point p)  
    {  
        return (x * p.x + y * p.y + z * p.z);  
    }  
};  
  
struct CH3D  
{  
    struct face  
    {  
        //表示凸包一个面上的三个点的编号   
        int a,b,c;  
        //表示该面是否属于最终凸包上的面   
        bool ok;  
    };  
    //初始顶点数   
    int n;  
    //初始顶点   
    Point P[MAXN];  
    //凸包表面的三角形数   
    int num;  
    //凸包表面的三角形   
    face F[8*MAXN];  
    //凸包表面的三角形   
    int g[MAXN][MAXN];  
    //向量长度   
    inline double Length(Point a)  
    {  
        return sqrt(a.x * a.x + a.y * a.y + a.z * a.z);  
    }  
    //叉乘   
    inline Point cross(Point a, Point b, Point c)  
    {  
        return Point((b.y - a.y) * (c.z - a.z) - (b.z - a.z) * (c.y - a.y),  
                     (b.z - a.z) * (c.x - a.x) - (b.x - a.x) * (c.z - a.z),  
                     (b.x - a.x) * (c.y - a.y) - (b.y - a.y) * (c.x - a.x)  
                    );  
    }  
    //三角形面积*2   
    inline double area(Point a, Point b, Point c)  
    {  
        return Length((b - a) * (c - a));  
    }  
    //四面体有向体积*6   
    inline double volume(Point a, Point b, Point c, Point d)  
    {  
        return (b - a) * (c - a) ^ (d - a);  
    }  
    //正：点在面同向   
    inline 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];  
        return (m * n) ^ t;  
    }  
    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 = true;  
                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 fabs(volume(a, b, c, P[F[t].a])) < eps &&  
               fabs(volume(a, b, c, P[F[t].b])) < eps &&  
               fabs(volume(a, b, c, P[F[t].c])) < eps;  
    }  
    //构建三维凸包   
    void create()  
    {  
        face add;  
  
        num = 0;  
        if(n < 4) return;  
        //**********************************************   
        //此段是为了保证前四个点不共面   
        bool flag = true;  
        FF(i, 1, n)  
        {  
            if(Length(P[0] - P[i]) > eps)  
            {  
                swap(P[1], P[i]);  
                flag=false;  
                break;  
            }  
        }  
        if(flag) return;  
        flag = true;  
        //使前三个点不共线   
        FF(i, 2, n)  
        {  
            if(Length((P[0] - P[1]) * (P[1] - P[i])) > eps)  
            {  
                swap(P[2], P[i]);  
                flag = false;  
                break;  
            }  
        }  
        if(flag) return;  
        flag = true;  
        //使前四个点不共面   
        FF(i, 3, n)  
        {  
            if(fabs((P[0] - P[1]) * (P[1] - P[2]) ^ (P[0] - P[i])) > eps)  
            {  
                swap(P[3], P[i]);  
                flag = false;  
                break;  
            }  
        }  
        if(flag) return;  
        //*****************************************   
        REP(i, 4)  
        {  
            add.a = (i + 1) % 4;  
            add.b = (i + 2) % 4;  
            add.c = (i + 3) % 4;  
            add.ok = true;  
            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;  
        }  
        FF(i, 4, n)  
        {  
            REP(j, num)  
            {  
                if(F[j].ok && dblcmp(P[i],F[j]) > eps)  
                {  
                    dfs(i, j);  
                    break;  
                }  
            }  
        }  
        int tmp = num;  
        num = 0;  
        REP(i, tmp)  
            if(F[i].ok)  
                F[num++] = F[i];  
  
    }  
    //表面积   
    double area()  
    {  
        double res = 0;  
        if(n == 3)  
        {  
            Point p = cross(P[0], P[1], P[2]);  
            res = Length(p) / 2.0;  
            return res;  
        }  
        REP(i, num)  
            res += area(P[F[i].a], P[F[i].b], P[F[i].c]);  
        return res / 2.0;  
    }  
    double volume()  
    {  
        double res = 0;  
        Point tmp(0, 0, 0);  
        REP(i, num)  
            res += volume(tmp, P[F[i].a], P[F[i].b], P[F[i].c]);  
        return fabs(res / 6.0);  
    }  
    //表面三角形个数   
    inline int triangle()  
    {  
        return num;  
    }  
    //表面多边形个数   
    int polygon()  
    {  
        int res = 0, flag;  
        REP(i, num)  
        {  
            flag = 1;  
            REP(j, i)  
                if(same(i, j))  
                {  
                    flag = 0;  
                    break;  
                }  
            res += flag;  
        }  
        return res;  
    }  
    //三维凸包重心   
    Point barycenter()  
    {  
        Point ans(0,0,0), t(0,0,0);  
        double all = 0, vol;  
        REP(i, num)  
        {  
            vol = volume(t, P[F[i].a], P[F[i].b], P[F[i].c]);  
            ans = ans + (t + P[F[i].a] + P[F[i].b] + P[F[i].c]) / 4.0 * vol;  
            all += vol;  
        }  
        return ans / all;  
    }  
    //点到面的距离   
    inline double ptoface(Point p, int i)  
    {  
        double Len = Length((P[F[i].b] - P[F[i].a]) * (P[F[i].c] - P[F[i].a]));  
        return fabs(volume(P[F[i].a], P[F[i].b], P[F[i].c],p) / Len);  
    }  
} hull;