三维计算几何

Point_3 rotate(Point_3 a, Vec_3 b, db angle)
向量 oa 以向量 b 为轴逆时针旋转 angle 度

#include <bits/stdc++.h>
using namespace std;
typedef double db;
const db eps = 1e-8;
inline int cmp(db a) {
   return a < -eps ? -1 : a > eps;
}
inline db Sqr(db a) {
   return a * a;
}
inline db Sqrt(db a) {
   return a <= 0 ? 0 : sqrt(a);
}
class Point_3 {
public:
   double x, y, z;
   Point_3() {}
   Point_3(db x, db y, db z) : x(x), y(y), z(z) {}
   double Length() const {
       return Sqrt(Sqr(x) + Sqr(y) + Sqr(z));
   }
   Point_3 Unit() const;
};
typedef Point_3 Vec_3;

Point_3 operator + (const Point_3 &a, const Point_3 &b) {
   return Point_3(a.x + b.x, a.y + b.y, a.z + b.z);
}
Point_3 operator - (const Point_3 &a, const Point_3 &b) {
   return Point_3(a.x - b.x, a.y - b.y, a.z - b.z);
}
Point_3 operator * (const Point_3 &a, db b) {
   return Point_3(a.x * b, a.y * b, a.z * b);
}
Point_3 operator / (const Point_3 &a, db b) {
   return Point_3(a.x / b, a.y / b, a.z / b);
}
Point_3 Point_3::Unit() const {
   return *this / Length();
}
Point_3 Det(const Point_3 &a, const Point_3 &b) {
   return Point_3(a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x);
}
db Dot(const Point_3 &a, const Point_3 &b) {
   return a.x * b.x + a.y * b.y + a.z * b.z;
}
db dis(const Point_3 &a, const Point_3 &b) {
   return (a - b).Length();
}

class Line_3 {
public:
   Point_3 a, b;
   Line_3() {}
   Line_3(Point_3 a, Point_3 b) : a(a), b(b) {}
};
db vlen(Point_3 P) {
   return P.Length();
}
db ptoline(Point_3 p, Line_3 l) {
   return vlen(Det(p - l.a, l.b - l.a)) / dis(l.a, l.b);
}
db ptoseg(Point_3 p, Line_3 l) {
    Vec_3 v1 = l.b - l.a, v2 = p - l.a, v3 = p - l.b;
    if (cmp(v1.Length()) == 0) return v2.Length();
    if (cmp(Dot(v1, v2)) < 0) return v2.Length();
    if (cmp(Dot(v1, v3)) > 0) return v3.Length();
    return ptoline(p, l);
}

Point_3 rotate(Point_3 a, Vec_3 b, db angle) {
    db x[3][3], y[3], ang = 1 - cos(angle);
    x[0][0] = ang * b.x * b.x + cos(angle);
    x[0][1] = ang * b.x * b.y - sin(angle) * b.z;
    x[0][2] = ang * b.x * b.z + sin(angle) * b.y;
    x[1][0] = ang * b.y * b.x + sin(angle) * b.z;
    x[1][1] = ang * b.y * b.y + cos(angle);
    x[1][2] = ang * b.y * b.z - sin(angle) * b.x;
    x[2][0] = ang * b.z * b.x - sin(angle) * b.y;
    x[2][1] = ang * b.z * b.y + sin(angle) * b.x;
    x[2][2] = ang * b.z * b.z + cos(angle);
    for (int i = 0; i < 3; i++) {
        y[i] = x[i][0] * a.x + x[i][1] * a.y + x[i][2] * a.z;
    }
    return Point_3(y[0], y[1], y[2]);
}

int main() {
   int T; scanf("%d", &T);
   while (T--) {
       db x, y, z;
       db ans = 1e18;
       scanf("%lf%lf%lf", &x, &y, &z);
       Point_3 start = Point_3(x, y, z);
       scanf("%lf%lf%lf", &x, &y, &z);
       Point_3 end = Point_3(x, y, z);
       Point_3 now = start, to;
       Vec_3 face(1, 0, 0), head(0, 0, 1);

       int n; scanf("%d", &n);
       while (n--) {
           db d, t; char s[5];
           scanf("%lf", &d);
           db dx = d * face.x, dy = d * face.y, dz = d * face.z;
           to = now + Point_3(dx, dy, dz);
           ans = min(ans, ptoseg(end, Line_3(now, to)));
           now = to;
           scanf("%s%lf", s, &t);
           if (s[0] == 'L') {
               face = rotate(face, head, t);
           }
           if (s[0] == 'R') {
               face = rotate(face, head * (-1), t);
           }
           if (s[0] == 'U') {
               Vec_3 dir = Det(face, head);
               face = rotate(face, dir, t);
               head = rotate(head, dir, t);
           }
           if (s[0] == 'D') {
               Vec_3 dir = Det(head, face);
               face = rotate(face, dir, t);
               head = rotate(head, dir, t);
           }
       }
       printf("%.2f\n", ans);
   }
    return 0;
}

平面最近点对

db minDist(vector<pt> &a, vector<int> &s, int l, int r) {
    db ans = 1e18;
    if (r - l < 20) {
        for (int i = l; i < r; i++) {
            for (int j = i + 1; j < r; j++) {
                ans = min(ans, (a[s[i]] - a[s[j]]).norm());
            }
        }
        return ans;
    }
    int tl, tr, m = (l + r) >> 1;
    ans = min(minDist(a, s, l, m), minDist(a, s, m, r));
    for (tl = l; a[s[tl]].x < a[s[m]].x - ans; tl++);
    for (tr = r - 1; a[s[tr]].x > a[s[m]].x + ans; tr--);
    sort(s.begin() + tl, s.begin() + tr, [&](int i, int j) {
        return cmp(a[i].y - a[j].y) < 0;
    });
    for (int i = tl; i < tr; i++) {
        for (int j = i + 1; j <= min(tr, i + 6); j++) {
            ans = min(ans, (a[s[i]] - a[s[j]]).norm());
        }
    }
    sort(s.begin() + tl, s.begin() + tr, [&](int i, int j) {
        return cmp(a[i].x - a[j].x) < 0;
    });
    return ans;
}

db minDist(vector<pt> &a) {
    int n = (int)a.size();
    vector<int> s(n);
    for (int i = 0; i < n; i++) s[i] = i;
    sort(s.begin(), s.end(), [&](int i, int j) {
        return cmp(a[i].x - a[j].x) < 0;
    });
    return minDist(a, s, 0, n);
}