#define PROBLEM "https://judge.yosupo.jp/problem/aplusb"
#include "template/template.hpp"
#include "geometry-3d/sphere.hpp"
#include "geometry-3d/triangle.hpp"
bool near(Real a, Real b, Real eps = 1e-8) { return abs(a - b) < eps; }
bool near(const Point3D& a, const Point3D& b, Real eps = 1e-8) {
return abs(a - b) < eps;
}
int main() {
{
Point3D x{1, 0, 0}, y{0, 1, 0}, z{0, 0, 1};
assert(dot(x, y) == 0);
assert(cross(x, y) == z);
assert(triple(x, y, z) == 1);
assert(near(angle(x, y), PI / 2));
assert(near(normalize(Point3D{2, 0, 0}), x));
}
{
Line3D l{{0, 0, 0}, {1, 0, 0}};
Line3D m{{0, -1, 0}, {0, 1, 0}};
assert(is_orthogonal(l, m));
assert(is_intersect_ll(l, m));
assert(near(*cross_point_ll(l, m), Point3D{0, 0, 0}));
assert(near(projection(l, Point3D{2, 3, 4}), Point3D{2, 0, 0}));
assert(near(reflection(l, Point3D{2, 3, 4}), Point3D{2, -3, -4}));
Line3D skew{{0, 1, 1}, {0, 2, 1}};
assert(!is_intersect_ll(l, skew));
assert(!cross_point_ll(l, skew));
assert(near(distance_ll(l, skew), 1));
Line3D parallel{{0, 2, 0}, {1, 2, 0}};
assert(is_parallel(l, parallel));
assert(near(distance_ll(l, parallel), 2));
assert(!cross_point_ll(l, parallel));
Line3D same{{-2, 0, 0}, {3, 0, 0}};
assert(is_intersect_ll(l, same));
assert(!cross_point_ll(l, same));
}
{
Segment3D s{{0, 0, 0}, {2, 0, 0}};
Segment3D t{{1, -1, 0}, {1, 1, 0}};
assert(is_intersect_ss(s, t));
assert(near(distance_ss(s, t), 0));
assert(near(closest_point_sp(s, Point3D{3, 4, 0}), Point3D{2, 0, 0}));
assert(near(distance_sp(s, Point3D{1, 2, 2}), sqrt((Real)8)));
Segment3D u{{0, 0, 0}, {0, 0, 0}};
assert(is_intersect_sp(u, Point3D{0, 0, 0}));
assert(near(distance_sp(u, Point3D{1, 2, 2}), 3));
}
{
Plane3D xy{{0, 0, 0}, {0, 0, 1}};
assert(is_intersect_pp(xy, Point3D{2, 3, 0}));
assert(near(projection(xy, Point3D{2, 3, 4}), Point3D{2, 3, 0}));
assert(near(reflection(xy, Point3D{2, 3, 4}), Point3D{2, 3, -4}));
assert(near(signed_distance_pp(xy, Point3D{2, 3, 4}), 4));
Line3D l{{1, 2, -1}, {1, 2, 1}};
assert(near(*cross_point_lp(l, xy), Point3D{1, 2, 0}));
Line3D parallel{{0, 0, 2}, {1, 0, 2}};
assert(!is_intersect_lp(parallel, xy));
assert(!cross_point_lp(parallel, xy));
assert(near(distance_lp(parallel, xy), 2));
Line3D included{{0, 0, 0}, {1, 0, 0}};
assert(is_intersect_lp(included, xy));
assert(!cross_point_lp(included, xy));
Plane3D x2{{2, 0, 0}, {1, 0, 0}};
Plane3D y3{{0, 3, 0}, {0, 1, 0}};
auto line = cross_line_pp(x2, y3);
assert(line);
assert(is_intersect_pp(x2, line->a));
assert(is_intersect_pp(y3, line->a));
assert(is_intersect_pp(x2, line->b));
assert(is_intersect_pp(y3, line->b));
Plane3D z2{{0, 0, 2}, {0, 0, 2}};
assert(is_parallel(xy, z2));
assert(!cross_line_pp(xy, z2));
assert(near(distance_pp(xy, z2), 2));
}
{
Triangle3D t{{0, 0, 0}, {2, 0, 0}, {0, 2, 0}};
assert(near(area(t), 2));
assert(near(signed_volume(Point3D{0, 0, 0}, Point3D{1, 0, 0},
Point3D{0, 1, 0}, Point3D{0, 0, 1}),
1.0L / 6));
assert(near(volume(Point3D{0, 0, 0}, Point3D{1, 0, 0},
Point3D{0, 1, 0}, Point3D{0, 0, -1}),
1.0L / 6));
auto b = barycentric_coordinates(t, Point3D{0.5, 0.5, 0});
assert(b && near((*b)[0], 0.5) && near((*b)[1], 0.25) &&
near((*b)[2], 0.25));
assert(is_intersect_tp(t, Point3D{0.5, 0.5, 0}));
assert(!is_intersect_tp(t, Point3D{1.5, 1.5, 0}));
assert(near(closest_point_tp(t, Point3D{0.5, 0.5, 3}),
Point3D{0.5, 0.5, 0}));
assert(near(distance_tp(t, Point3D{0.5, 0.5, 3}), 3));
assert(near(closest_point_tp(t, Point3D{2, 2, 0}), Point3D{1, 1, 0}));
}
{
Sphere3D s{{0, 0, 0}, 2};
assert(contains_sphere(s, Point3D{0, 0, 0}) == 2);
assert(contains_sphere(s, Point3D{2, 0, 0}) == 1);
assert(contains_sphere(s, Point3D{3, 0, 0}) == 0);
assert(near(surface_area(s), 16 * PI));
assert(near(volume(s), 32 * PI / 3));
auto ps = cross_point_ls(Line3D{{-3, 0, 0}, {3, 0, 0}}, s);
assert(ps);
assert(near(ps->first, Point3D{-2, 0, 0}));
assert(near(ps->second, Point3D{2, 0, 0}));
auto tangent = cross_point_ls(Line3D{{-3, 2, 0}, {3, 2, 0}}, s);
assert(tangent && near(tangent->first, Point3D{0, 2, 0}) &&
near(tangent->second, Point3D{0, 2, 0}));
assert(!cross_point_ls(Line3D{{-3, 3, 0}, {3, 3, 0}}, s));
auto c1 = cross_circle_ps(Plane3D{{0, 0, 1}, {0, 0, 1}}, s);
assert(c1 && near(c1->p, Point3D{0, 0, 1}) && near(c1->r, sqrt((Real)3)));
assert(!cross_circle_ps(Plane3D{{0, 0, 3}, {0, 0, 1}}, s));
Sphere3D t{{2, 0, 0}, 2};
auto c2 = cross_circle_ss(s, t);
assert(c2 && near(c2->p, Point3D{1, 0, 0}) && near(c2->r, sqrt((Real)3)));
assert(intersect(s, t) == 2);
assert(intersect(s, Sphere3D{{4, 0, 0}, 2}) == 3);
assert(intersect(s, Sphere3D{{5, 0, 0}, 2}) == 4);
assert(intersect(s, Sphere3D{{1, 0, 0}, 1}) == 1);
assert(intersect(s, Sphere3D{{0, 0, 0}, 1}) == 0);
}
mt19937_64 rng(1234567);
uniform_real_distribution<Real> dist(-10, 10);
auto random_point = [&]() { return Point3D{dist(rng), dist(rng), dist(rng)}; };
for (int qi = 0; qi < 5000; ++qi) {
Point3D a = random_point(), b = random_point(), p = random_point();
if (abs(a - b) < 0.1) {
--qi;
continue;
}
Line3D l{a, b};
Point3D q = projection(l, p);
assert(abs(cross(l.b - l.a, q - l.a)) < 1e-7);
assert(abs(dot(l.b - l.a, p - q)) < 1e-7);
assert(near(distance_lp(l, p), abs(p - q), 1e-7));
Point3D c = random_point(), d = random_point();
if (abs(c - d) < 0.1) {
--qi;
continue;
}
Line3D m{c, d};
auto [x, y] = closest_points_ll(l, m);
assert(is_intersect_lp(l, x));
assert(is_intersect_lp(m, y));
assert(near(distance_ll(l, m), distance_ll(m, l), 1e-7));
if (!is_parallel(l, m)) {
assert(abs(dot(l.b - l.a, x - y)) < 1e-7);
assert(abs(dot(m.b - m.a, x - y)) < 1e-7);
}
Segment3D s{a, b}, t{c, d};
auto [u, v] = closest_points_ss(s, t);
assert(is_intersect_sp(s, u));
assert(is_intersect_sp(t, v));
assert(near(distance_ss(s, t), distance_ss(t, s), 1e-7));
Point3D n = random_point();
if (abs(n) < 0.1) {
--qi;
continue;
}
Plane3D pl{a, n};
Point3D z = projection(pl, p);
assert(abs(plane_value(pl, z)) < 1e-7);
assert(near(distance_pp(pl, p), abs(p - z), 1e-7));
}
long long a, b;
in(a, b);
out(a + b);
}
#line 1 "verify/geometry-3d/UNIT_geometry_3d.test.cpp"
#define PROBLEM "https://judge.yosupo.jp/problem/aplusb"
#line 2 "template/template.hpp"
#include <bits/stdc++.h>
using namespace std;
#line 2 "template/macro.hpp"
#define rep(i, a, b) for (int i = (a); i < (int)(b); i++)
#define rrep(i, a, b) for (int i = (int)(b) - 1; i >= (a); i--)
#define ALL(v) (v).begin(), (v).end()
#define UNIQUE(v) sort(ALL(v)), (v).erase(unique(ALL(v)), (v).end())
#define SZ(v) (int)v.size()
#define MIN(v) *min_element(ALL(v))
#define MAX(v) *max_element(ALL(v))
#define LB(v, x) int(lower_bound(ALL(v), (x)) - (v).begin())
#define UB(v, x) int(upper_bound(ALL(v), (x)) - (v).begin())
#define YN(b) cout << ((b) ? "YES" : "NO") << "\n";
#define Yn(b) cout << ((b) ? "Yes" : "No") << "\n";
#define yn(b) cout << ((b) ? "yes" : "no") << "\n";
#line 6 "template/template.hpp"
#line 2 "template/util.hpp"
using uint = unsigned int;
using ll = long long int;
using ull = unsigned long long;
using i128 = __int128_t;
using u128 = __uint128_t;
template <class T>
using priority_queue_asc = priority_queue<T, vector<T>, greater<T>>;
template <class T, class S = T>
S SUM(const vector<T>& a) {
return accumulate(ALL(a), S(0));
}
template <class T1, class T2>
inline bool chmin(T1& a, T2 b) {
if (a > b) {
a = b;
return true;
}
return false;
}
template <class T1, class T2>
inline bool chmax(T1& a, T2 b) {
if (a < b) {
a = b;
return true;
}
return false;
}
template <class T1, class T2>
inline bool chmin_opt(optional<T1>& a, T2 b) {
if (!a || a > b) {
a = b;
return true;
}
return false;
}
template <class T1, class T2>
inline bool chmax_opt(optional<T1>& a, T2 b) {
if (!a || a < b) {
a = b;
return true;
}
return false;
}
template <class T>
int popcnt(T x) {
return __builtin_popcountll(x);
}
template <class T>
int topbit(T x) {
return (x == 0 ? -1 : 63 - __builtin_clzll(x));
}
template <class T>
int lowbit(T x) {
return (x == 0 ? -1 : __builtin_ctzll(x));
}
#line 8 "template/template.hpp"
#line 2 "template/inout.hpp"
struct Fast {
Fast() {
cin.tie(nullptr);
ios_base::sync_with_stdio(false);
cout << fixed << setprecision(15);
}
} fast;
ostream& operator<<(ostream& os, __uint128_t x) {
char buf[40];
size_t k = 0;
while (x > 0) buf[k++] = (char)(x % 10 + '0'), x /= 10;
if (k == 0) buf[k++] = '0';
while (k) os << buf[--k];
return os;
}
ostream& operator<<(ostream& os, __int128_t x) {
return x < 0 ? (os << '-' << (__uint128_t)(-x)) : (os << (__uint128_t)x);
}
template <class T, size_t N>
ostream& operator<<(ostream& os, const array<T, N>& a);
template <class T1, class T2>
istream& operator>>(istream& is, pair<T1, T2>& p) {
return is >> p.first >> p.second;
}
template <class T1, class T2>
ostream& operator<<(ostream& os, const pair<T1, T2>& p) {
return os << p.first << " " << p.second;
}
template <class T>
istream& operator>>(istream& is, vector<T>& a) {
for (auto& v : a) is >> v;
return is;
}
template <class T>
ostream& operator<<(ostream& os, const vector<T>& a) {
for (auto it = a.begin(); it != a.end();) {
os << *it;
if (++it != a.end()) os << " ";
}
return os;
}
template <class T, size_t N>
ostream& operator<<(ostream& os, const array<T, N>& a) {
for (auto it = a.begin(); it != a.end();) {
os << *it;
if (++it != a.end()) os << " ";
}
return os;
}
template <class T>
ostream& operator<<(ostream& os, const set<T>& st) {
os << "{";
for (auto it = st.begin(); it != st.end();) {
os << *it;
if (++it != st.end()) os << ",";
}
os << "}";
return os;
}
template <class T1, class T2>
ostream& operator<<(ostream& os, const map<T1, T2>& mp) {
os << "{";
for (auto it = mp.begin(); it != mp.end();) {
os << it->first << ":" << it->second;
if (++it != mp.end()) os << ",";
}
os << "}";
return os;
}
void in() {}
template <typename T, class... U>
void in(T& t, U&... u) {
cin >> t;
in(u...);
}
template <class... T>
void in_zip(int n, T&... t) {
assert(n >= 0 && ((size(t) >= static_cast<size_t>(n)) && ...));
for (int i = 0; i < n; i++) in(t[i]...);
}
void out() { cout << "\n"; }
template <typename T, class... U, char sep = ' '>
void out(const T& t, const U&... u) {
cout << t;
if (sizeof...(u)) cout << sep;
out(u...);
}
template <class T, class U>
void out_opt(const optional<T>& opt, const U& fallback, ostream& os = cout) {
if (opt.has_value())
os << opt.value();
else
os << fallback;
os << "\n";
}
template <class T, class U>
void out_opt(const vector<optional<T>>& vec, const U& fallback, ostream& os = cout) {
for (auto it = vec.begin(); it != vec.end();) {
if ((*it).has_value())
os << (*it).value();
else
os << fallback;
if (++it != vec.end()) os << " ";
}
os << "\n";
}
namespace IO {
template <class T, class... U>
T read(U&&... u) {
T t = T(forward<U>(u)...);
in(t);
return t;
}
namespace Graph {
vector<vector<int>> unweighted(int n, int m, bool directed = false, int offset = 1) {
vector<vector<int>> g(n);
for (int i = 0; i < m; i++) {
int u, v;
cin >> u >> v;
u -= offset, v -= offset;
g[u].push_back(v);
if (!directed) g[v].push_back(u);
}
return g;
}
template <class T>
vector<vector<pair<int, T>>> weighted(int n, int m, bool directed = false, int offset = 1) {
vector<vector<pair<int, T>>> g(n);
for (int i = 0; i < m; i++) {
int u, v;
T w;
cin >> u >> v >> w;
u -= offset, v -= offset;
g[u].push_back({v, w});
if (!directed) g[v].push_back({u, w});
}
return g;
}
} // namespace Graph
namespace Tree {
vector<vector<int>> unweighted(int n, bool directed = false, int offset = 1) {
return Graph::unweighted(n, n - 1, directed, offset);
}
template <class T>
vector<vector<pair<int, T>>> weighted(int n, bool directed = false, int offset = 1) {
return Graph::weighted<T>(n, n - 1, directed, offset);
}
vector<vector<int>> rooted(int n, bool to_root = true, bool to_leaf = true, int offset = 1) {
vector<vector<int>> g(n);
for (int i = 1; i < n; i++) {
int p;
cin >> p;
p -= offset;
if (to_root) g[i].push_back(p);
if (to_leaf) g[p].push_back(i);
}
return g;
}
} // namespace Tree
} // namespace IO
#line 10 "template/template.hpp"
#line 2 "template/debug.hpp"
#ifdef LOCAL
#define debug 1
#define show(...) _show(0, #__VA_ARGS__, __VA_ARGS__)
#else
#define debug 0
#define show(...) true
#endif
template <class T>
void _show(int, T) {
cerr << '\n';
}
template <class T1, class T2, class... T3>
void _show(int i, const T1& a, const T2& b, const T3&... c) {
for (; a[i] != ',' && a[i] != '\0'; i++) cerr << a[i];
cerr << ":" << b << " ";
_show(i + 1, a, c...);
}
#line 2 "geometry-3d/sphere.hpp"
#line 2 "geometry-3d/plane.hpp"
#line 2 "geometry-3d/line.hpp"
#line 2 "geometry-3d/geometry-base.hpp"
#line 2 "geometry/geometry-base.hpp"
#line 4 "geometry/geometry-base.hpp"
using Real = long double;
constexpr Real EPS = 1e-10;
constexpr Real PI = 3.141592653589793238462643383279L;
bool equals(Real x, Real y) { return fabs(x - y) < EPS; }
int sign(Real a) { return equals(a, 0) ? 0 : (a > 0 ? 1 : -1); }
template <class R>
struct PointBase {
using P = PointBase;
R x, y;
PointBase() : x(0), y(0) {}
PointBase(R _x, R _y) : x(_x), y(_y) {}
template <typename T, typename U>
PointBase(const pair<T, U>& p) : x(p.first), y(p.second) {}
P operator+(const P& r) const { return P{x + r.x, y + r.y}; }
P operator-(const P& r) const { return P{x - r.x, y - r.y}; }
P operator-() const { return P{-x, -y}; }
P operator*(R r) const { return P{x * r, y * r}; }
P operator/(R r) const { return P{x / r, y / r}; }
P& operator+=(const P& r) { return (*this) = (*this) + r; }
P& operator-=(const P& r) { return (*this) = (*this) - r; }
P& operator*=(R r) { return (*this) = (*this) * r; }
P& operator/=(R r) { return (*this) = (*this) / r; }
bool operator<(const P& r) const { return x != r.x ? x < r.x : y < r.y; }
bool operator==(const P& r) const { return x == r.x and y == r.y; }
bool operator!=(const P& r) const { return !((*this) == r); }
P rotate(R rad) const {
return {x * cos(rad) - y * sin(rad), x * sin(rad) + y * cos(rad)};
}
P rotate90() const { return {-y, x}; }
R real() const { return x; }
R imag() const { return y; }
friend P operator*(R r, const P& p) { return p * r; }
friend R real(const P& p) { return p.x; }
friend R imag(const P& p) { return p.y; }
friend R dot(const P& l, const P& r) { return l.x * r.x + l.y * r.y; }
friend R cross(const P& l, const P& r) { return l.x * r.y - l.y * r.x; }
friend R abs(const P& p) { return sqrt(p.x * p.x + p.y * p.y); }
friend R norm(const P& p) { return p.x * p.x + p.y * p.y; }
friend R arg(const P& p) { return atan2(p.y, p.x); }
friend istream& operator>>(istream& is, P& p) {
R a, b;
is >> a >> b;
p = P{a, b};
return is;
}
friend ostream& operator<<(ostream& os, const P& p) {
return os << p.x << " " << p.y;
}
};
using Point = PointBase<Real>;
using Points = vector<Point>;
// relative position of c from a->b
int ccw(const Point& a, const Point& b, const Point& c) {
Point x = b - a, y = c - a;
if (cross(x, y) > EPS) return +1; // counter-clockwise
if (cross(x, y) < -EPS) return -1; // clockwise
if (dot(x, y) < -EPS) return +2; // collinear in the order c-a-b
if (norm(x) + EPS < norm(y)) return -2; // collinear in the order a-b-c
return 0; // collinear in the order a-c-b
}
/**
* @brief 二次元幾何の基本要素
* @docs docs/geometry/geometry-base.md
*/
#line 4 "geometry-3d/geometry-base.hpp"
template <class R>
struct Point3DBase {
using P = Point3DBase;
R x, y, z;
Point3DBase() : x(0), y(0), z(0) {}
Point3DBase(R _x, R _y, R _z) : x(_x), y(_y), z(_z) {}
P operator+(const P& r) const { return {x + r.x, y + r.y, z + r.z}; }
P operator-(const P& r) const { return {x - r.x, y - r.y, z - r.z}; }
P operator-() const { return {-x, -y, -z}; }
P operator*(R r) const { return {x * r, y * r, z * r}; }
P operator/(R r) const { return {x / r, y / r, z / r}; }
P& operator+=(const P& r) { return (*this) = (*this) + r; }
P& operator-=(const P& r) { return (*this) = (*this) - r; }
P& operator*=(R r) { return (*this) = (*this) * r; }
P& operator/=(R r) { return (*this) = (*this) / r; }
bool operator<(const P& r) const {
if (x != r.x) return x < r.x;
return y != r.y ? y < r.y : z < r.z;
}
bool operator==(const P& r) const {
return x == r.x && y == r.y && z == r.z;
}
bool operator!=(const P& r) const { return !((*this) == r); }
friend P operator*(R r, const P& p) { return p * r; }
friend R dot(const P& l, const P& r) {
return l.x * r.x + l.y * r.y + l.z * r.z;
}
friend P cross(const P& l, const P& r) {
return {l.y * r.z - l.z * r.y, l.z * r.x - l.x * r.z,
l.x * r.y - l.y * r.x};
}
friend R norm(const P& p) { return dot(p, p); }
friend R abs(const P& p) { return sqrt(norm(p)); }
friend istream& operator>>(istream& is, P& p) {
return is >> p.x >> p.y >> p.z;
}
friend ostream& operator<<(ostream& os, const P& p) {
return os << p.x << " " << p.y << " " << p.z;
}
};
using Point3D = Point3DBase<Real>;
using Points3D = vector<Point3D>;
Real triple(const Point3D& a, const Point3D& b, const Point3D& c) {
return dot(a, cross(b, c));
}
Point3D normalize(const Point3D& p) {
Real len = abs(p);
assert(len > EPS);
return p / len;
}
Real angle(const Point3D& a, const Point3D& b) {
Real d = abs(a) * abs(b);
assert(d > EPS);
return acos(clamp(dot(a, b) / d, (Real)-1, (Real)1));
}
bool equals(const Point3D& a, const Point3D& b) { return abs(a - b) < EPS; }
/**
* @brief 三次元幾何の基本要素
* @docs docs/geometry-3d/geometry-base.md
*/
#line 4 "geometry-3d/line.hpp"
struct Line3D {
Point3D a, b;
Line3D() = default;
Line3D(const Point3D& _a, const Point3D& _b) : a(_a), b(_b) {
assert(abs(b - a) > EPS);
}
friend istream& operator>>(istream& is, Line3D& l) { return is >> l.a >> l.b; }
friend ostream& operator<<(ostream& os, const Line3D& l) {
return os << l.a << " to " << l.b;
}
};
using Lines3D = vector<Line3D>;
bool is_intersect_lp(const Line3D& l, const Point3D& p) {
Point3D d = l.b - l.a;
assert(abs(d) > EPS);
return abs(cross(d, p - l.a)) < EPS * abs(d);
}
bool is_parallel(const Line3D& l, const Line3D& m) {
Point3D u = l.b - l.a, v = m.b - m.a;
assert(abs(u) > EPS && abs(v) > EPS);
return abs(cross(u, v)) < EPS * abs(u) * abs(v);
}
bool is_orthogonal(const Line3D& l, const Line3D& m) {
Point3D u = l.b - l.a, v = m.b - m.a;
assert(abs(u) > EPS && abs(v) > EPS);
return abs(dot(u, v)) < EPS * abs(u) * abs(v);
}
Point3D projection(const Line3D& l, const Point3D& p) {
Point3D d = l.b - l.a;
assert(norm(d) > EPS * EPS);
return l.a + d * (dot(p - l.a, d) / norm(d));
}
Point3D reflection(const Line3D& l, const Point3D& p) {
return projection(l, p) * 2 - p;
}
Real distance_lp(const Line3D& l, const Point3D& p) {
return abs(p - projection(l, p));
}
pair<Point3D, Point3D> closest_points_ll(const Line3D& l, const Line3D& m) {
Point3D u = l.b - l.a, v = m.b - m.a, w = l.a - m.a;
Real a = norm(u), b = dot(u, v), c = norm(v);
assert(a > EPS * EPS && c > EPS * EPS);
Real d = dot(u, w), e = dot(v, w);
Real det = a * c - b * b;
if (abs(det) < EPS * EPS * a * c) {
Point3D q = projection(m, l.a);
return {l.a, q};
}
Real s = (b * e - c * d) / det;
Real t = (a * e - b * d) / det;
return {l.a + u * s, m.a + v * t};
}
Real distance_ll(const Line3D& l, const Line3D& m) {
auto [p, q] = closest_points_ll(l, m);
return abs(p - q);
}
bool is_intersect_ll(const Line3D& l, const Line3D& m) {
return distance_ll(l, m) < EPS;
}
optional<Point3D> cross_point_ll(const Line3D& l, const Line3D& m) {
if (is_parallel(l, m)) return nullopt;
auto [p, q] = closest_points_ll(l, m);
if (abs(p - q) >= EPS) return nullopt;
return (p + q) / 2;
}
/**
* @brief 三次元空間上の直線
* @docs docs/geometry-3d/line.md
*/
#line 4 "geometry-3d/plane.hpp"
struct Plane3D {
Point3D p, n;
Plane3D() = default;
Plane3D(const Point3D& _p, const Point3D& _n) : p(_p), n(_n) {
assert(abs(n) > EPS);
}
Plane3D(const Point3D& a, const Point3D& b, const Point3D& c)
: p(a), n(cross(b - a, c - a)) {
assert(abs(n) > EPS);
}
};
using Planes3D = vector<Plane3D>;
Real plane_value(const Plane3D& pl, const Point3D& p) {
return dot(pl.n, p - pl.p);
}
bool is_intersect_pp(const Plane3D& pl, const Point3D& p) {
return abs(plane_value(pl, p)) < EPS * abs(pl.n);
}
bool is_parallel(const Plane3D& a, const Plane3D& b) {
return abs(cross(a.n, b.n)) < EPS * abs(a.n) * abs(b.n);
}
bool is_orthogonal(const Plane3D& a, const Plane3D& b) {
return abs(dot(a.n, b.n)) < EPS * abs(a.n) * abs(b.n);
}
Point3D projection(const Plane3D& pl, const Point3D& p) {
assert(norm(pl.n) > EPS * EPS);
return p - pl.n * (plane_value(pl, p) / norm(pl.n));
}
Point3D reflection(const Plane3D& pl, const Point3D& p) {
return projection(pl, p) * 2 - p;
}
Real signed_distance_pp(const Plane3D& pl, const Point3D& p) {
return plane_value(pl, p) / abs(pl.n);
}
Real distance_pp(const Plane3D& pl, const Point3D& p) {
return abs(signed_distance_pp(pl, p));
}
bool is_intersect_lp(const Line3D& l, const Plane3D& pl) {
Point3D d = l.b - l.a;
bool parallel = abs(dot(pl.n, d)) < EPS * abs(pl.n) * abs(d);
return !parallel || is_intersect_pp(pl, l.a);
}
optional<Point3D> cross_point_lp(const Line3D& l, const Plane3D& pl) {
Point3D v = l.b - l.a;
Real d = dot(pl.n, v);
if (abs(d) < EPS * abs(pl.n) * abs(v)) return nullopt;
Real t = dot(pl.n, pl.p - l.a) / d;
return l.a + v * t;
}
Real distance_lp(const Line3D& l, const Plane3D& pl) {
return is_intersect_lp(l, pl) ? 0 : distance_pp(pl, l.a);
}
optional<Line3D> cross_line_pp(const Plane3D& a, const Plane3D& b) {
Point3D d = cross(a.n, b.n);
Real d2 = norm(d);
if (d2 < EPS * EPS * norm(a.n) * norm(b.n)) return nullopt;
Real da = dot(a.n, a.p), db = dot(b.n, b.p);
Point3D p = cross(da * b.n - db * a.n, d) / d2;
return Line3D{p, p + d};
}
Real distance_pp(const Plane3D& a, const Plane3D& b) {
return is_parallel(a, b) ? distance_pp(a, b.p) : 0;
}
/**
* @brief 三次元空間上の平面
* @docs docs/geometry-3d/plane.md
*/
#line 4 "geometry-3d/sphere.hpp"
struct Circle3D {
Point3D p, n;
Real r;
Circle3D() = default;
Circle3D(const Point3D& _p, const Point3D& _n, Real _r)
: p(_p), n(_n), r(_r) {
assert(abs(n) > EPS && r >= 0);
}
};
struct Sphere3D {
Point3D p;
Real r;
Sphere3D() = default;
Sphere3D(const Point3D& _p, Real _r) : p(_p), r(_r) { assert(r >= 0); }
};
using Circles3D = vector<Circle3D>;
using Spheres3D = vector<Sphere3D>;
Real area(const Circle3D& c) { return PI * c.r * c.r; }
Real circumference(const Circle3D& c) { return 2 * PI * c.r; }
Real surface_area(const Sphere3D& s) { return 4 * PI * s.r * s.r; }
Real volume(const Sphere3D& s) { return 4 * PI * s.r * s.r * s.r / 3; }
int contains_sphere(const Sphere3D& s, const Point3D& p) {
int z = sign(abs(p - s.p) - s.r);
return z > 0 ? 0 : z == 0 ? 1 : 2;
}
// 0: contained, 1: internally tangent, 2: intersecting,
// 3: externally tangent, 4: separate.
int intersect(Sphere3D s, Sphere3D t) {
if (s.r < t.r) swap(s, t);
Real d = abs(s.p - t.p);
if (s.r + t.r < d - EPS) return 4;
if (equals(s.r + t.r, d)) return 3;
if (s.r - t.r < d - EPS) return 2;
if (equals(s.r - t.r, d)) return 1;
return 0;
}
optional<pair<Point3D, Point3D>> cross_point_ls(const Line3D& l,
const Sphere3D& s) {
Point3D q = projection(l, s.p);
Real h2 = s.r * s.r - norm(q - s.p);
if (h2 < -EPS) return nullopt;
h2 = max<Real>(0, h2);
Point3D d = normalize(l.b - l.a) * sqrt(h2);
return pair<Point3D, Point3D>{q - d, q + d};
}
optional<Circle3D> cross_circle_ps(const Plane3D& pl, const Sphere3D& s) {
Point3D q = projection(pl, s.p);
Real r2 = s.r * s.r - norm(q - s.p);
if (r2 < -EPS) return nullopt;
return Circle3D{q, normalize(pl.n), sqrt(max<Real>(0, r2))};
}
optional<Circle3D> cross_circle_ss(const Sphere3D& s, const Sphere3D& t) {
int relation = intersect(s, t);
if (relation == 0 || relation == 4) return nullopt;
Point3D d = t.p - s.p;
Real len = abs(d);
if (len < EPS) return nullopt;
Real x = (s.r * s.r - t.r * t.r + len * len) / (2 * len);
Real r2 = s.r * s.r - x * x;
Point3D n = d / len;
return Circle3D{s.p + n * x, n, sqrt(max<Real>(0, r2))};
}
/**
* @brief 三次元空間上の円と球
* @docs docs/geometry-3d/sphere.md
*/
#line 2 "geometry-3d/triangle.hpp"
#line 2 "geometry-3d/segment.hpp"
#line 4 "geometry-3d/segment.hpp"
struct Segment3D : Line3D {
Segment3D() = default;
Segment3D(const Point3D& _a, const Point3D& _b) {
a = _a;
b = _b;
}
};
using Segments3D = vector<Segment3D>;
Point3D closest_point_sp(const Segment3D& s, const Point3D& p) {
Point3D d = s.b - s.a;
if (norm(d) <= EPS * EPS) return s.a;
Real t = clamp(dot(p - s.a, d) / norm(d), (Real)0, (Real)1);
return s.a + d * t;
}
pair<Point3D, Point3D> closest_points_ss(const Segment3D& s, const Segment3D& t) {
Point3D d1 = s.b - s.a, d2 = t.b - t.a, r = s.a - t.a;
Real a = norm(d1), e = norm(d2), x = 0, y = 0;
if (a <= EPS * EPS && e <= EPS * EPS) return {s.a, t.a};
if (a <= EPS * EPS) {
y = clamp(dot(d2, r) / e, (Real)0, (Real)1);
} else {
Real c = dot(d1, r);
if (e <= EPS * EPS) {
x = clamp(-c / a, (Real)0, (Real)1);
} else {
Real b = dot(d1, d2), f = dot(d2, r);
Real det = a * e - b * b;
if (abs(det) > EPS * EPS * a * e) {
x = clamp((b * f - c * e) / det, (Real)0, (Real)1);
}
y = (b * x + f) / e;
if (y < 0) {
y = 0;
x = clamp(-c / a, (Real)0, (Real)1);
} else if (y > 1) {
y = 1;
x = clamp((b - c) / a, (Real)0, (Real)1);
}
}
}
return {s.a + d1 * x, t.a + d2 * y};
}
Real distance_sp(const Segment3D& s, const Point3D& p) {
return abs(p - closest_point_sp(s, p));
}
Real distance_ss(const Segment3D& s, const Segment3D& t) {
auto [p, q] = closest_points_ss(s, t);
return abs(p - q);
}
bool is_intersect_sp(const Segment3D& s, const Point3D& p) {
return distance_sp(s, p) < EPS;
}
bool is_intersect_ss(const Segment3D& s, const Segment3D& t) {
return distance_ss(s, t) < EPS;
}
/**
* @brief 三次元空間上の線分
* @docs docs/geometry-3d/segment.md
*/
#line 5 "geometry-3d/triangle.hpp"
struct Triangle3D {
Point3D a, b, c;
Triangle3D() = default;
Triangle3D(const Point3D& _a, const Point3D& _b, const Point3D& _c)
: a(_a), b(_b), c(_c) {
assert(abs(cross(b - a, c - a)) > EPS);
}
};
Point3D triangle_normal(const Triangle3D& t) {
return cross(t.b - t.a, t.c - t.a);
}
Real area(const Triangle3D& t) { return abs(triangle_normal(t)) / 2; }
Real signed_volume(const Point3D& a, const Point3D& b, const Point3D& c,
const Point3D& d) {
return triple(b - a, c - a, d - a) / 6;
}
Real volume(const Point3D& a, const Point3D& b, const Point3D& c,
const Point3D& d) {
return abs(signed_volume(a, b, c, d));
}
optional<array<Real, 3>> barycentric_coordinates(const Triangle3D& t,
const Point3D& p) {
Point3D u = t.b - t.a, v = t.c - t.a, w = p - t.a;
Real uu = norm(u), uv = dot(u, v), vv = norm(v);
Real wu = dot(w, u), wv = dot(w, v);
Real d = uu * vv - uv * uv;
if (abs(d) < EPS * EPS) return nullopt;
Real y = (vv * wu - uv * wv) / d;
Real z = (uu * wv - uv * wu) / d;
return array<Real, 3>{1 - y - z, y, z};
}
bool is_intersect_tp(const Triangle3D& t, const Point3D& p) {
Plane3D pl{t.a, t.b, t.c};
if (distance_pp(pl, p) >= EPS) return false;
auto b = barycentric_coordinates(t, p);
if (!b) return false;
return (*b)[0] >= -EPS && (*b)[1] >= -EPS && (*b)[2] >= -EPS;
}
Point3D closest_point_tp(const Triangle3D& t, const Point3D& p) {
Plane3D pl{t.a, t.b, t.c};
assert(norm(pl.n) > EPS * EPS);
Point3D q = projection(pl, p);
if (is_intersect_tp(t, q)) return q;
array<Point3D, 3> ps = {closest_point_sp(Segment3D{t.a, t.b}, p),
closest_point_sp(Segment3D{t.b, t.c}, p),
closest_point_sp(Segment3D{t.c, t.a}, p)};
return *min_element(begin(ps), end(ps), [&](const Point3D& x, const Point3D& y) {
return norm(x - p) < norm(y - p);
});
}
Real distance_tp(const Triangle3D& t, const Point3D& p) {
return abs(p - closest_point_tp(t, p));
}
/**
* @brief 三次元空間上の三角形
* @docs docs/geometry-3d/triangle.md
*/
#line 6 "verify/geometry-3d/UNIT_geometry_3d.test.cpp"
bool near(Real a, Real b, Real eps = 1e-8) { return abs(a - b) < eps; }
bool near(const Point3D& a, const Point3D& b, Real eps = 1e-8) {
return abs(a - b) < eps;
}
int main() {
{
Point3D x{1, 0, 0}, y{0, 1, 0}, z{0, 0, 1};
assert(dot(x, y) == 0);
assert(cross(x, y) == z);
assert(triple(x, y, z) == 1);
assert(near(angle(x, y), PI / 2));
assert(near(normalize(Point3D{2, 0, 0}), x));
}
{
Line3D l{{0, 0, 0}, {1, 0, 0}};
Line3D m{{0, -1, 0}, {0, 1, 0}};
assert(is_orthogonal(l, m));
assert(is_intersect_ll(l, m));
assert(near(*cross_point_ll(l, m), Point3D{0, 0, 0}));
assert(near(projection(l, Point3D{2, 3, 4}), Point3D{2, 0, 0}));
assert(near(reflection(l, Point3D{2, 3, 4}), Point3D{2, -3, -4}));
Line3D skew{{0, 1, 1}, {0, 2, 1}};
assert(!is_intersect_ll(l, skew));
assert(!cross_point_ll(l, skew));
assert(near(distance_ll(l, skew), 1));
Line3D parallel{{0, 2, 0}, {1, 2, 0}};
assert(is_parallel(l, parallel));
assert(near(distance_ll(l, parallel), 2));
assert(!cross_point_ll(l, parallel));
Line3D same{{-2, 0, 0}, {3, 0, 0}};
assert(is_intersect_ll(l, same));
assert(!cross_point_ll(l, same));
}
{
Segment3D s{{0, 0, 0}, {2, 0, 0}};
Segment3D t{{1, -1, 0}, {1, 1, 0}};
assert(is_intersect_ss(s, t));
assert(near(distance_ss(s, t), 0));
assert(near(closest_point_sp(s, Point3D{3, 4, 0}), Point3D{2, 0, 0}));
assert(near(distance_sp(s, Point3D{1, 2, 2}), sqrt((Real)8)));
Segment3D u{{0, 0, 0}, {0, 0, 0}};
assert(is_intersect_sp(u, Point3D{0, 0, 0}));
assert(near(distance_sp(u, Point3D{1, 2, 2}), 3));
}
{
Plane3D xy{{0, 0, 0}, {0, 0, 1}};
assert(is_intersect_pp(xy, Point3D{2, 3, 0}));
assert(near(projection(xy, Point3D{2, 3, 4}), Point3D{2, 3, 0}));
assert(near(reflection(xy, Point3D{2, 3, 4}), Point3D{2, 3, -4}));
assert(near(signed_distance_pp(xy, Point3D{2, 3, 4}), 4));
Line3D l{{1, 2, -1}, {1, 2, 1}};
assert(near(*cross_point_lp(l, xy), Point3D{1, 2, 0}));
Line3D parallel{{0, 0, 2}, {1, 0, 2}};
assert(!is_intersect_lp(parallel, xy));
assert(!cross_point_lp(parallel, xy));
assert(near(distance_lp(parallel, xy), 2));
Line3D included{{0, 0, 0}, {1, 0, 0}};
assert(is_intersect_lp(included, xy));
assert(!cross_point_lp(included, xy));
Plane3D x2{{2, 0, 0}, {1, 0, 0}};
Plane3D y3{{0, 3, 0}, {0, 1, 0}};
auto line = cross_line_pp(x2, y3);
assert(line);
assert(is_intersect_pp(x2, line->a));
assert(is_intersect_pp(y3, line->a));
assert(is_intersect_pp(x2, line->b));
assert(is_intersect_pp(y3, line->b));
Plane3D z2{{0, 0, 2}, {0, 0, 2}};
assert(is_parallel(xy, z2));
assert(!cross_line_pp(xy, z2));
assert(near(distance_pp(xy, z2), 2));
}
{
Triangle3D t{{0, 0, 0}, {2, 0, 0}, {0, 2, 0}};
assert(near(area(t), 2));
assert(near(signed_volume(Point3D{0, 0, 0}, Point3D{1, 0, 0},
Point3D{0, 1, 0}, Point3D{0, 0, 1}),
1.0L / 6));
assert(near(volume(Point3D{0, 0, 0}, Point3D{1, 0, 0},
Point3D{0, 1, 0}, Point3D{0, 0, -1}),
1.0L / 6));
auto b = barycentric_coordinates(t, Point3D{0.5, 0.5, 0});
assert(b && near((*b)[0], 0.5) && near((*b)[1], 0.25) &&
near((*b)[2], 0.25));
assert(is_intersect_tp(t, Point3D{0.5, 0.5, 0}));
assert(!is_intersect_tp(t, Point3D{1.5, 1.5, 0}));
assert(near(closest_point_tp(t, Point3D{0.5, 0.5, 3}),
Point3D{0.5, 0.5, 0}));
assert(near(distance_tp(t, Point3D{0.5, 0.5, 3}), 3));
assert(near(closest_point_tp(t, Point3D{2, 2, 0}), Point3D{1, 1, 0}));
}
{
Sphere3D s{{0, 0, 0}, 2};
assert(contains_sphere(s, Point3D{0, 0, 0}) == 2);
assert(contains_sphere(s, Point3D{2, 0, 0}) == 1);
assert(contains_sphere(s, Point3D{3, 0, 0}) == 0);
assert(near(surface_area(s), 16 * PI));
assert(near(volume(s), 32 * PI / 3));
auto ps = cross_point_ls(Line3D{{-3, 0, 0}, {3, 0, 0}}, s);
assert(ps);
assert(near(ps->first, Point3D{-2, 0, 0}));
assert(near(ps->second, Point3D{2, 0, 0}));
auto tangent = cross_point_ls(Line3D{{-3, 2, 0}, {3, 2, 0}}, s);
assert(tangent && near(tangent->first, Point3D{0, 2, 0}) &&
near(tangent->second, Point3D{0, 2, 0}));
assert(!cross_point_ls(Line3D{{-3, 3, 0}, {3, 3, 0}}, s));
auto c1 = cross_circle_ps(Plane3D{{0, 0, 1}, {0, 0, 1}}, s);
assert(c1 && near(c1->p, Point3D{0, 0, 1}) && near(c1->r, sqrt((Real)3)));
assert(!cross_circle_ps(Plane3D{{0, 0, 3}, {0, 0, 1}}, s));
Sphere3D t{{2, 0, 0}, 2};
auto c2 = cross_circle_ss(s, t);
assert(c2 && near(c2->p, Point3D{1, 0, 0}) && near(c2->r, sqrt((Real)3)));
assert(intersect(s, t) == 2);
assert(intersect(s, Sphere3D{{4, 0, 0}, 2}) == 3);
assert(intersect(s, Sphere3D{{5, 0, 0}, 2}) == 4);
assert(intersect(s, Sphere3D{{1, 0, 0}, 1}) == 1);
assert(intersect(s, Sphere3D{{0, 0, 0}, 1}) == 0);
}
mt19937_64 rng(1234567);
uniform_real_distribution<Real> dist(-10, 10);
auto random_point = [&]() { return Point3D{dist(rng), dist(rng), dist(rng)}; };
for (int qi = 0; qi < 5000; ++qi) {
Point3D a = random_point(), b = random_point(), p = random_point();
if (abs(a - b) < 0.1) {
--qi;
continue;
}
Line3D l{a, b};
Point3D q = projection(l, p);
assert(abs(cross(l.b - l.a, q - l.a)) < 1e-7);
assert(abs(dot(l.b - l.a, p - q)) < 1e-7);
assert(near(distance_lp(l, p), abs(p - q), 1e-7));
Point3D c = random_point(), d = random_point();
if (abs(c - d) < 0.1) {
--qi;
continue;
}
Line3D m{c, d};
auto [x, y] = closest_points_ll(l, m);
assert(is_intersect_lp(l, x));
assert(is_intersect_lp(m, y));
assert(near(distance_ll(l, m), distance_ll(m, l), 1e-7));
if (!is_parallel(l, m)) {
assert(abs(dot(l.b - l.a, x - y)) < 1e-7);
assert(abs(dot(m.b - m.a, x - y)) < 1e-7);
}
Segment3D s{a, b}, t{c, d};
auto [u, v] = closest_points_ss(s, t);
assert(is_intersect_sp(s, u));
assert(is_intersect_sp(t, v));
assert(near(distance_ss(s, t), distance_ss(t, s), 1e-7));
Point3D n = random_point();
if (abs(n) < 0.1) {
--qi;
continue;
}
Plane3D pl{a, n};
Point3D z = projection(pl, p);
assert(abs(plane_value(pl, z)) < 1e-7);
assert(near(distance_pp(pl, p), abs(p - z), 1e-7));
}
long long a, b;
in(a, b);
out(a + b);
}
verify/geometry-3d/UNIT_geometry_3d.test.cpp
template/debug.hpp