Initial commit

src/NeighborlyCore/plane.h

@@ -0,0 +1,31 @@
+#pragma once
+#include <Eigen/Dense>
+
+using Eigen::Vector2d;
+using Eigen::Vector3d;
+using Eigen::Matrix3d;
+
+class Plane {
+public:
+    double d;
+    Vector3d n;
+
+    Plane(const Vector3d& _nd) : d(_nd.norm()), n(_nd / d) {}
+    Plane(const Vector3d& _n, double _d) : d(_d), n(_n) {}
+    Plane(const Vector3d& a, const Vector3d& b, const Vector3d& c) {
+        n = (b - a).cross(c - a).normalized();
+        d = a.dot(n);
+    }
+
+    bool intersect(const Vector3d& a, const Vector3d& b, Vector3d& result) const {
+        const double ad = a.dot(n) - d;
+        const double bd = b.dot(n) - d;
+        if (ad * bd >= 0.0) { return false; }
+        result = (ad * b - bd * a) / (ad - bd);
+        return true;
+    }
+
+    double signed_distance(Vector3d p) const {
+        return p.dot(n) - d;
+    }
+};

src/NeighborlyCore/precise_geometry.h

@@ -0,0 +1,155 @@
+#pragma once
+
+#include "plane.h"
+#include "wide_real.h"
+
+#include <Eigen/Dense>
+#include <algorithm>
+#include <cmath>
+#include <vector>
+
+struct WidePoint2 {
+    WideReal x;
+    WideReal y;
+};
+
+inline WidePoint2 wide_point(const Eigen::Vector2d& point) {
+    return {WideReal(point.x()), WideReal(point.y())};
+}
+
+inline WideReal wide_cross_2d(const WidePoint2& a, const WidePoint2& b) {
+    return a.x * b.y - a.y * b.x;
+}
+
+inline WidePoint2 operator-(const WidePoint2& lhs, const WidePoint2& rhs) {
+    return {lhs.x - rhs.x, lhs.y - rhs.y};
+}
+
+inline bool precise_segment_intersection_2d(
+    const Eigen::Vector2d& a,
+    const Eigen::Vector2d& b,
+    const Eigen::Vector2d& c,
+    const Eigen::Vector2d& d,
+    double eps,
+    double* t_out = nullptr,
+    double* u_out = nullptr
+) {
+    const WidePoint2 aw = wide_point(a);
+    const WidePoint2 r = wide_point(b) - aw;
+    const WidePoint2 cw = wide_point(c);
+    const WidePoint2 s = wide_point(d) - cw;
+    const WideReal denominator = wide_cross_2d(r, s);
+    const WideReal tolerance(std::abs(eps));
+    if (abs(denominator) <= tolerance) {
+        return false;
+    }
+    const WidePoint2 ca = cw - aw;
+    const WideReal t = wide_cross_2d(ca, s) / denominator;
+    const WideReal u = wide_cross_2d(ca, r) / denominator;
+    if (t_out != nullptr) {
+        *t_out = t.to_double();
+    }
+    if (u_out != nullptr) {
+        *u_out = u.to_double();
+    }
+    const WideReal one(1.0);
+    return t > tolerance && t < one - tolerance &&
+        u > tolerance && u < one - tolerance;
+}
+
+inline bool precise_point_on_segment_2d(
+    const WidePoint2& point,
+    const WidePoint2& a,
+    const WidePoint2& b,
+    double eps
+) {
+    const WidePoint2 ab = b - a;
+    const WidePoint2 ap = point - a;
+    const WideReal tolerance(std::abs(eps));
+    if (abs(wide_cross_2d(ab, ap)) > tolerance) {
+        return false;
+    }
+    return point.x >= (a.x < b.x ? a.x : b.x) - tolerance &&
+        point.x <= (a.x > b.x ? a.x : b.x) + tolerance &&
+        point.y >= (a.y < b.y ? a.y : b.y) - tolerance &&
+        point.y <= (a.y > b.y ? a.y : b.y) + tolerance;
+}
+
+inline bool precise_point_in_polygon_2d(
+    const WidePoint2& point,
+    const std::vector<Eigen::Vector2d>& polygon,
+    double eps,
+    bool* on_boundary = nullptr
+) {
+    bool inside = false;
+    bool boundary = false;
+    const WideReal tolerance(std::abs(eps));
+    for (size_t i = 0; i < polygon.size(); ++i) {
+        const WidePoint2 a = wide_point(polygon[i]);
+        const WidePoint2 b = wide_point(polygon[(i + 1) % polygon.size()]);
+        if (precise_point_on_segment_2d(point, a, b, eps)) {
+            boundary = true;
+            inside = true;
+            break;
+        }
+        const bool crosses_ray = (a.y > point.y) != (b.y > point.y);
+        if (crosses_ray) {
+            const WideReal x_intersection =
+                (b.x - a.x) * (point.y - a.y) / (b.y - a.y) + a.x;
+            if (point.x < x_intersection + tolerance) {
+                inside = !inside;
+            }
+        }
+    }
+    if (on_boundary != nullptr) {
+        *on_boundary = boundary;
+    }
+    return inside;
+}
+
+inline WideReal wide_dot_3d(const Eigen::Vector3d& a, const Eigen::Vector3d& b) {
+    return WideReal(a.x()) * WideReal(b.x()) +
+        WideReal(a.y()) * WideReal(b.y()) +
+        WideReal(a.z()) * WideReal(b.z());
+}
+
+inline bool precise_edge_plane_parameter(
+    const Eigen::Vector3d& a,
+    const Eigen::Vector3d& b,
+    const Plane& plane,
+    double eps,
+    WideReal& t
+) {
+    const WideReal d0 = wide_dot_3d(a, plane.n) - WideReal(plane.d);
+    const WideReal d1 = wide_dot_3d(b, plane.n) - WideReal(plane.d);
+    const WideReal denominator = d0 - d1;
+    const WideReal tolerance(std::abs(eps));
+    if (abs(denominator) <= tolerance) {
+        return false;
+    }
+    t = d0 / denominator;
+    const WideReal one(1.0);
+    return t > tolerance && t < one - tolerance;
+}
+
+inline WidePoint2 precise_projected_edge_hit(
+    const Eigen::Vector3d& a,
+    const Eigen::Vector3d& b,
+    const WideReal& t,
+    const Eigen::Matrix3d& basis,
+    const Eigen::Vector3d& origin
+) {
+    WideReal coordinates[3];
+    for (int axis = 0; axis < 3; ++axis) {
+        coordinates[axis] = WideReal(a[axis]) +
+            t * WideReal(b[axis] - a[axis]) - WideReal(origin[axis]);
+    }
+    WidePoint2 projected;
+    projected.x = WideReal(basis(0, 0)) * coordinates[0] +
+        WideReal(basis(0, 1)) * coordinates[1] +
+        WideReal(basis(0, 2)) * coordinates[2];
+    projected.y = WideReal(basis(1, 0)) * coordinates[0] +
+        WideReal(basis(1, 1)) * coordinates[1] +
+        WideReal(basis(1, 2)) * coordinates[2];
+    return projected;
+}

src/NeighborlyCore/util.h

@@ -0,0 +1,103 @@
+#pragma once
+#include "plane.h"
+#include <Eigen/Dense>
+#include <unordered_set>
+#include <vector>
+#include <map>
+
+using Eigen::Vector2d;
+using Eigen::Vector3d;
+using Eigen::VectorXd;
+using Eigen::Matrix3d;
+
+using Verts2D = std::vector<Vector2d>;
+using Verts3D = std::vector<Vector3d>;
+using Planes = std::vector<Plane>;
+using Face = std::vector<int>;
+using Faces = std::vector<Face>;
+using Edge = std::pair<int, int>;
+using Edges = std::vector<Edge>;
+
+using FaceMap = std::map<int, std::vector<int>>;
+using EdgeMap = std::map<Edge, std::vector<int>>;
+
+std::vector<std::string> split(const std::string& s, char delim);
+bool is_finite(const Verts3D& verts);
+void print_faces(const Faces& faces);
+void open_face_file(const char* fname, Faces& tris);
+void open_topology(const char* fname, Faces& tris, int ix);
+bool verify_topology(const Faces& tris);
+void dual_graph(const Faces& faces, Faces& dual_faces, Edges& dual_edges);
+void save_sample(const char* name, const Planes& planes, const Verts3D& verts, int iter, bool can_save);
+void save_dot_graph(const char* fname, const Edges& edges);
+void make_edges(const Faces& faces, Edges& edges);
+void fix_face_ordering(Faces& polys, const Edges& edges);
+bool test_face_ordering(const Faces& polys);
+void import_obj(const char* fname, Verts3D& verts, Faces& polys);
+void export_obj(const char* fname, const Verts3D& verts, const Faces& polys);
+void line_line_intersection(const Vector3d& a1, const Vector3d& a2, const Vector3d& b1, const Vector3d& b2, Vector3d& pa, Vector3d& pb);
+
+Plane get_plane(const Verts3D& pts, const Face& poly);
+void make_2d_projection(const Verts3D& v3ds, const Face& poly, const Plane& plane, Verts2D& v2ds);
+void make_2d_projection(const Verts3D& v3ds, const Face& poly, const Plane& plane, Verts2D& v2ds, Matrix3d& basis, Vector3d& p);
+Vector3d plane_intersection(const Plane& p1, const Plane& p2, const Plane& p3);
+void spread_planes(VectorXd& x, double spread, int iters);
+
+void y_to_v3ds(const VectorXd& y, Verts3D& verts);
+void v3ds_to_y(const Verts3D& verts, VectorXd& y);
+void x_to_planes(const VectorXd& x, Planes& planes);
+void planes_to_x(const Planes& planes, VectorXd& x);
+void v3ds_to_planes(const Verts3D& pts, const Faces& polys, Planes& planes);
+void planes_to_v3ds(const Faces& dual_tris, const Planes& planes, Verts3D& verts);
+void x_to_v3ds(const VectorXd& x, const Faces& tris, Verts3D& verts);
+void v3ds_to_x(const Verts3D& pts, const Faces& polys, VectorXd& x);
+void y_to_x(const VectorXd& n, const VectorXd& y, VectorXd& x);
+
+bool point_in_polygon(const Vector2d& p, const Verts2D& pts, int& onEdge);
+int count_crossings(const Verts3D& v3ds, const Plane& plane, const Face& poly);
+int count_crossings(const Verts3D& v3ds, const Planes& planes);
+int count_self_crossings_strict(
+    const Verts3D& v3ds,
+    const Planes& planes,
+    double eps = 1e-8,
+    double* smooth_loss = nullptr);
+int count_self_crossings_precise(
+    const Verts3D& v3ds,
+    const Planes& planes,
+    double eps = 1e-12);
+int count_intersections(const Verts3D& v3ds, const Planes& planes, const Plane& plane, const Face& poly, const Edges& other_edges);
+int count_intersections(const Verts3D& v3ds, const Planes& planes);
+int count_edge_face_intersections_strict(const Verts3D& v3ds, const Planes& planes, double eps = 1e-8);
+int count_edge_face_intersections_precise(const Verts3D& v3ds, const Planes& planes, double eps = 1e-12);
+double angle_penalty(const Verts3D& verts);
+double dist_penalty(const Verts3D& verts);
+double length_penalty(const Verts3D& verts);
+double plane_penalty(const Planes& planes);
+double q_penalty(const Verts3D& verts);
+
+double objective_cross_int_qlim(const VectorXd& x);
+double objective_cross_int_q(const VectorXd& x);
+double objective_cross_zint(const VectorXd& x);
+double objective_cross_int(const VectorXd& x);
+double objective_cross(const VectorXd& x);
+double objective_int_cross_q(const VectorXd& x);
+double objective_int_cross(const VectorXd& x);
+double objective_int_zcross_q(const VectorXd& x);
+double objective_int_zcross(const VectorXd& x);
+double objective_int_qlim(const VectorXd& x);
+double objective_int_q(const VectorXd& x);
+double objective_int(const VectorXd& x);
+double objective_sum_qlim(const VectorXd& x);
+double objective_sum_q(const VectorXd& x);
+double objective_sum(const VectorXd& x);
+double objective_wsum(const VectorXd& x);
+double objective_wsum2(const VectorXd& x);
+double objective_wsum_q(const VectorXd& x);
+
+double objective_dual(const VectorXd& x);
+double objective_dual_q(const VectorXd& x);
+
+extern Faces g_polys;
+extern Faces g_tris;
+extern Edges g_edges;
+extern int g_topology;

src/NeighborlyCore/wide_real.h

@@ -0,0 +1,168 @@
+#pragma once
+
+#include <cmath>
+#include <limits>
+
+// A compact double-double number. The unevaluated value is hi + lo, where lo
+// stores the rounding error of hi. Geometry predicates get about 106 bits of
+// significand without bringing arbitrary-precision arithmetic into the search.
+class WideReal {
+public:
+    constexpr WideReal() = default;
+    constexpr WideReal(double value) : hi_(value), lo_(0.0) {}
+
+    static constexpr int decimal_digits = 31;
+
+    static WideReal from_parts(double hi, double lo) {
+        double sum = 0.0;
+        double error = 0.0;
+        two_sum(hi, lo, sum, error);
+        return WideReal(sum, error, RawTag{});
+    }
+
+    double to_double() const {
+        return hi_ + lo_;
+    }
+
+    bool is_finite() const {
+        return std::isfinite(hi_) && std::isfinite(lo_);
+    }
+
+    WideReal& operator+=(const WideReal& rhs) {
+        *this = *this + rhs;
+        return *this;
+    }
+
+    WideReal& operator-=(const WideReal& rhs) {
+        *this = *this - rhs;
+        return *this;
+    }
+
+    WideReal& operator*=(const WideReal& rhs) {
+        *this = *this * rhs;
+        return *this;
+    }
+
+    WideReal& operator/=(const WideReal& rhs) {
+        *this = *this / rhs;
+        return *this;
+    }
+
+    friend WideReal operator+(const WideReal& lhs, const WideReal& rhs) {
+        double sum = 0.0;
+        double error = 0.0;
+        two_sum(lhs.hi_, rhs.hi_, sum, error);
+        error += lhs.lo_ + rhs.lo_;
+        return normalized(sum, error);
+    }
+
+    friend WideReal operator-(const WideReal& lhs, const WideReal& rhs) {
+        double sum = 0.0;
+        double error = 0.0;
+        two_sum(lhs.hi_, -rhs.hi_, sum, error);
+        error += lhs.lo_ - rhs.lo_;
+        return normalized(sum, error);
+    }
+
+    friend WideReal operator*(const WideReal& lhs, const WideReal& rhs) {
+        const double product = lhs.hi_ * rhs.hi_;
+        double error = std::fma(lhs.hi_, rhs.hi_, -product);
+        error += lhs.hi_ * rhs.lo_ + lhs.lo_ * rhs.hi_;
+        error += lhs.lo_ * rhs.lo_;
+        return normalized(product, error);
+    }
+
+    friend WideReal operator/(const WideReal& lhs, const WideReal& rhs) {
+        if (rhs.hi_ == 0.0 && rhs.lo_ == 0.0) {
+            const double value = lhs.to_double();
+            if (value == 0.0) {
+                return WideReal(std::numeric_limits<double>::quiet_NaN());
+            }
+            const bool negative = std::signbit(value) != std::signbit(rhs.hi_);
+            return WideReal(std::copysign(
+                std::numeric_limits<double>::infinity(), negative ? -1.0 : 1.0));
+        }
+        const double q1 = lhs.hi_ / rhs.hi_;
+        WideReal remainder = lhs - rhs * WideReal(q1);
+        const double q2 = remainder.hi_ / rhs.hi_;
+        remainder -= rhs * WideReal(q2);
+        const double q3 = remainder.hi_ / rhs.hi_;
+        return WideReal(q1) + WideReal(q2) + WideReal(q3);
+    }
+
+    WideReal operator-() const {
+        return WideReal(-hi_, -lo_, RawTag{});
+    }
+
+    friend bool operator==(const WideReal& lhs, const WideReal& rhs) {
+        return lhs.hi_ == rhs.hi_ && lhs.lo_ == rhs.lo_;
+    }
+
+    friend bool operator!=(const WideReal& lhs, const WideReal& rhs) {
+        return !(lhs == rhs);
+    }
+
+    friend bool operator<(const WideReal& lhs, const WideReal& rhs) {
+        return lhs.hi_ < rhs.hi_ || (lhs.hi_ == rhs.hi_ && lhs.lo_ < rhs.lo_);
+    }
+
+    friend bool operator>(const WideReal& lhs, const WideReal& rhs) {
+        return rhs < lhs;
+    }
+
+    friend bool operator<=(const WideReal& lhs, const WideReal& rhs) {
+        return !(rhs < lhs);
+    }
+
+    friend bool operator>=(const WideReal& lhs, const WideReal& rhs) {
+        return !(lhs < rhs);
+    }
+
+    friend WideReal abs(const WideReal& value) {
+        return value < WideReal(0.0) ? -value : value;
+    }
+
+    friend WideReal sqrt(const WideReal& value) {
+        if (value < WideReal(0.0)) {
+            return WideReal(std::numeric_limits<double>::quiet_NaN());
+        }
+        if (value == WideReal(0.0) || !value.is_finite()) {
+            return WideReal(std::sqrt(value.to_double()));
+        }
+        WideReal estimate(std::sqrt(value.hi_));
+        estimate += (value - estimate * estimate) / (estimate * WideReal(2.0));
+        return estimate;
+    }
+
+private:
+    struct RawTag {};
+
+    constexpr WideReal(double hi, double lo, RawTag) : hi_(hi), lo_(lo) {}
+
+    static void two_sum(double a, double b, double& sum, double& error) {
+        sum = a + b;
+        const double b_virtual = sum - a;
+        error = (a - (sum - b_virtual)) + (b - b_virtual);
+    }
+
+    static WideReal normalized(double hi, double lo) {
+        double sum = 0.0;
+        double error = 0.0;
+        two_sum(hi, lo, sum, error);
+        return WideReal(sum, error, RawTag{});
+    }
+
+    double hi_ = 0.0;
+    double lo_ = 0.0;
+};
+
+inline WideReal wide_square(const WideReal& value) {
+    return value * value;
+}
+
+inline bool wide_real_self_test() {
+    const WideReal one(1.0);
+    const WideReal recovered = (WideReal(1e16) + one) - WideReal(1e16);
+    const WideReal division_error = abs((one / WideReal(3.0)) * WideReal(3.0) - one);
+    return recovered == one && division_error < WideReal(1e-30);
+}