aabb & closet point

include/aabb.hpp

@@ -0,0 +1,49 @@
+#pragma once
+#include "vec.hpp"
+#include "common.hpp"
+
+class AABB {
+public:
+    Vec3 min;
+    Vec3 max;
+    AABB() : min(Vec3(INFINITY, INFINITY, INFINITY)), max(Vec3(-INFINITY, -INFINITY, -INFINITY)) {}
+    AABB(const Vec3 &minPoint, const Vec3 &maxPoint) : min(minPoint), max(maxPoint) {}
+
+    void extend(const Vec3 &point) {
+        min = minValue(min, point);
+        max = maxValue(max, point);
+    }
+
+    void extend(const AABB &other) {
+        min = minValue(min, other.min);
+        max = maxValue(max, other.max);
+    }
+
+    void expand(real s = EPS_AABB_EXTENSION) {
+        min = min - s;
+        max = max + s;
+    }
+
+    void expand(const Vec3 &s) {
+        min = min - s;
+        max = max + s;
+    }
+
+    [[nodiscard]] Vec3 center() const { return (min + max) * HALF; }
+
+    [[nodiscard]] Vec3 halfSize() const { return (max - min) * HALF; }
+
+    void move(const Vec3 &v) {
+        min = min + v;
+        max = max + v;
+    }
+
+private:
+    static Vec3 minValue(const Vec3 &a, const Vec3 &b) {
+        return Vec3{std::min(a.x(), b.x()), std::min(a.y(), b.y()), std::min(a.z(), b.z())};
+    }
+
+    static Vec3 maxValue(const Vec3 &a, const Vec3 &b) {
+        return Vec3{std::max(a.x(), b.x()), std::max(a.y(), b.y()), std::max(a.z(), b.z())};
+    }
+};

include/common.hpp

@@ -10,6 +10,7 @@ const real PI2 = 2 * PI;
 const real EPS = std::numeric_limits<real>::epsilon() * 1e2;
 const real EPS_END_PARAM = std::numeric_limits<real>::epsilon() * 1e3;
 const real EPS_NEWTON_ITERATION = std::numeric_limits<real>::epsilon() * 1e6;
+const real EPS_AABB_EXTENSION = std::numeric_limits<real>::epsilon() * 1e6;
 
 const real HALF = 0.5;
 const real ONE_FOURTH = 0.25;

include/line.hpp

@@ -11,6 +11,7 @@
 #include <limits.h>
 #include <limits>
 #include <vector>
+#include "aabb.hpp"
 
 // class ILineParam {
 // public:
@@ -26,12 +27,19 @@
 //     real t;
 // };
 
-struct ClosestDescOnSeg {
+struct ClosestDescOnLine {
     real t;
     real dis;
-    ClosestDescOnSeg(real _t, real _dis) : t(_t), dis(_dis) {}
-    ClosestDescOnSeg() : t(0), dis(std::numeric_limits<real>::max()) {}
+    ClosestDescOnLine(real _t, real _dis) : t(_t), dis(_dis) {}
+    ClosestDescOnLine() : t(0), dis(std::numeric_limits<real>::max()) {}
 };
+
+struct ClosestDescOnProfile : public ClosestDescOnLine {
+    int i; // line idx
+    ClosestDescOnProfile(real _t, real _dis, int _i) : ClosestDescOnLine(_t, _dis), i(_i) {}
+    ClosestDescOnProfile() : i(-1) {}
+};
+
 // vscode C++ override跳转插件
 class ILine {
 public:
@@ -43,29 +51,34 @@ public:
     ILine(ILine &&) = default;
     ILine &operator=(ILine &&) = default;
 
-    virtual Vec3 eval(real t) const = 0;
+    [[nodiscard]] virtual Vec3 eval(real t) const = 0;
 
-    virtual Vec3 der1(real t) const = 0;
+    [[nodiscard]] virtual Vec3 der1(real t) const = 0;
 
-    virtual Vec3 der2(real t) const = 0;
+    [[nodiscard]] virtual Vec3 der2(real t) const = 0;
 
-    virtual Vec3 tangent(real t) const = 0;
+    [[nodiscard]] virtual Vec3 tangent(real t) const = 0;
 
-    virtual Vec3 normal(real t, const Vec3 &tan = -1.) const = 0;
+    [[nodiscard]] virtual Vec3 normal(real t, const Vec3 &tan = -1.) const = 0;
 
-    virtual ClosestDescOnSeg getClosestParam(const Vec3 &p) const = 0;
+    [[nodiscard]] virtual ClosestDescOnLine getClosestParam(const Vec3 &p) const = 0;
 
     [[nodiscard]] virtual bool isEndParam(real t) const = 0;
+
+    [[nodiscard]] virtual real startT() const = 0;
+    [[nodiscard]] virtual real endT() const = 0;
+
+    [[nodiscard]] virtual AABB getAABB() const = 0;
 };
 
-inline static ClosestDescOnSeg segPtDist(const Vec3 &p, const Vec3 &A, const Vec3 &B) {
+inline static ClosestDescOnLine segPtDist(const Vec3 &p, const Vec3 &A, const Vec3 &B) {
     Vec3 AB = B - A;
     Vec3 AP = p - A;
     real h = std::clamp(AP.dot(AB) / AB.dot(AB), 0., 1.);
     return {h, (AP - AB * h).norm()};
 }
 
-inline static ClosestDescOnSeg segPtDist(const Vec2 &p, const Vec2 &A, const Vec2 &B) {
+inline static ClosestDescOnLine segPtDist(const Vec2 &p, const Vec2 &A, const Vec2 &B) {
     Vec2 AB = B - A;
     Vec2 AP = p - A;
     real h = std::clamp(AP.dot(AB) / AB.dot(AB), 0., 1.);
@@ -109,6 +122,12 @@ public:
         }
         circularArcs.resize(_bugles.size());
         initSegInfo();
+        for (size_t i = 0; i < _bugles.size(); ++i) {
+            AABB tmp{_points[i], _points[i]};
+            tmp.extend(AABB{_points[(i + 1) % _points.size()], _points[(i + 1) % _points.size()]});
+            tmp.expand(circularArcs[i].radius - circularArcs[i].h);
+            aabb.extend(tmp);
+        }
     }
 
     [[nodiscard]] const Pt3Array &getPoints() const { return _points; }
@@ -123,6 +142,9 @@ public:
         return circularArcs;
     }
 
+    [[nodiscard]] real startT() const override { return 0; };
+    [[nodiscard]] real endT() const override { return static_cast<real>(_bugles.size()); };
+
 protected:
     Pt3Array _points;
     std::vector<real> _bugles;
@@ -130,6 +152,8 @@ protected:
     bool _closed;
     std::vector<CircularArc<Vec3>> circularArcs;
 
+    AABB aabb;
+
 public:
     void initSegInfo() {
         for (size_t i = 0; i < _bugles.size(); ++i) {
@@ -138,7 +162,7 @@ public:
         }
     }
 
-    Vec3 eval(real t) const override {
+    [[nodiscard]] Vec3 eval(real t) const override {
         // if (circularArcs.empty())
         //     initSegInfo();
         int seg = static_cast<int>(t);
@@ -151,7 +175,7 @@ public:
         return arc.center + arc.radius * (arc.u * std::cos(phi) + arc.v * std::sin(phi));
     }
 
-    Vec3 der1(real t) const override {
+    [[nodiscard]] Vec3 der1(real t) const override {
         int seg = static_cast<int>(t);
         if (isEqual(_bugles[seg], 0)) {
             return _points[(seg + 1) % _points.size()] - _points[seg];
@@ -162,7 +186,7 @@ public:
         return arc.radius * (arc.u * -std::sin(phi) + arc.v * std::cos(phi));
     }
 
-    Vec3 der2(real t) const override {
+    [[nodiscard]] Vec3 der2(real t) const override {
         int seg = static_cast<int>(t);
         if (isEqual(_bugles[seg], 0)) {
             int aaa = 1;
@@ -245,14 +269,14 @@ public:
     //     return {closestParam, closestDis};
     // }
 
-    ClosestDescOnSeg getClosestParam(const Vec3 &p) const override {
-        ClosestDescOnSeg closestDes{};
+    [[nodiscard]] ClosestDescOnLine getClosestParam(const Vec3 &p) const override {
+        ClosestDescOnLine closestDes{};
         for (int i = 0; i < _bugles.size(); ++i) {
             const Vec3 &a = _points[i];
             const Vec3 &b = _points[(i + 1) % _points.size()];
             if (isEqual(_bugles[i], 0)) {
                 // 点到线段最近距离
-                ClosestDescOnSeg segPtDistRes = segPtDist(p, a, b);
+                ClosestDescOnLine segPtDistRes = segPtDist(p, a, b);
                 if (segPtDistRes.dis < closestDes.dis) {
                     closestDes = segPtDistRes;
                     closestDes.t = i + segPtDistRes.t;
@@ -320,6 +344,8 @@ public:
         return t < EPS_END_PARAM || t > static_cast<real>(_bugles.size()) - EPS_END_PARAM;
     }
 
+    [[nodiscard]] AABB getAABB() const override { return aabb; }
+
 private:
     void initCircularArcInfo(const Vec3 &a, const Vec3 &b, real bugle, const Vec3 &refNormal,
                              CircularArc<Vec3> &res) {
@@ -365,25 +391,30 @@ public:
         real _4pi2r = PI2 * PI2 * _r;
         // _k = _4pi2r / (advancePerRound * advancePerRound + _4pi2r * _r);
         _arcDeltaMaxFactor = _4pi2r / (advancePerRound * advancePerRound + _4pi2r * _r) * ONE_EIGHT;
+
+        // init aabb
+        aabb.extend(_axisStart);
+        aabb.extend(axisEnd);
+        aabb.extend(r);
     }
-    Vec3 eval(real t) const override {
+    [[nodiscard]] Vec3 eval(real t) const override {
         real theta = _frequency * t;
         return _axisStart + _axisDir * t + (_u * std::cos(theta) + _v * std::sin(theta)) * _r;
     };
 
-    Vec3 der1(real param) const override {
+    [[nodiscard]] Vec3 der1(real param) const override {
         real theta = _frequency * param;
         return _axisDir + _2pir_p * (_v * std::cos(theta) - _u * std::sin(theta));
     };
 
-    Vec3 der2(real param) const override {
+    [[nodiscard]] Vec3 der2(real param) const override {
         real theta = _frequency * param;
         return -_4pi2r_p2 * (_u * std::cos(theta) + _v * std::sin(theta));
     };
 
-    Vec3 tangent(real t) const override { return der1(t).normalize(); }
+    [[nodiscard]] Vec3 tangent(real t) const override { return der1(t).normalize(); }
 
-    Vec3 normal(real t, const Vec3 &tan = -1.) const override {
+    [[nodiscard]] Vec3 normal(real t, const Vec3 &tan = -1.) const override {
         Vec3 der2Vec = this->der2(t);
         if (tan == -1.) {
             Vec3 realTan = tangent(t);
@@ -392,15 +423,20 @@ public:
         return (der2Vec - der2Vec.dot(tan) * tan).normalize();
     }
 
-    ClosestDescOnSeg getClosestParam(const Vec3 &p) const override {
+    [[nodiscard]] real startT() const override { return 0; }
+    [[nodiscard]] real endT() const override { return 1; }
+
+    [[nodiscard]] AABB getAABB() const override { return aabb; }
+
+    ClosestDescOnLine getClosestParam(const Vec3 &p) const override {
         // discretization and traversal
         real startT = 0;
         real endT = SEG_T;
         auto segCount = static_cast<size_t>(std::ceil(_advanceLen / SEG_T));
-        std::vector<ClosestDescOnSeg> sampledSegs(segCount + 2); // 加上首尾
+        std::vector<ClosestDescOnLine> sampledSegs(segCount + 2); // 加上首尾
         std::vector<Vec3> samplePoints(segCount + 2);
 
-        ClosestDescOnSeg closestSampleDes;
+        ClosestDescOnLine closestSampleDes;
         for (size_t i = 0; i < segCount; ++i, startT = endT, endT += SEG_T) {
             real sampledT = fmin(startT + SEG_T_HALF, _advanceLen);
             samplePoints[i] = eval(sampledT);
@@ -474,6 +510,7 @@ private:
     real _r, _2pir_p, _4pi2r_p2, _arcDeltaMaxFactor;
     const int SEG_PER_ROUND = 12;
     const real SEG_T, SEG_T_HALF;
+    AABB aabb;
 };
 
 class SingleLine : public ILine {
@@ -507,5 +544,5 @@ public:
 
     Vec3 normal(real t, const Vec3 &tan = -1.) const override { return {}; }
 
-    ClosestDescOnSeg getClosestParam(const Vec3 &p) const override { return {}; };
+    ClosestDescOnLine getClosestParam(const Vec3 &p) const override { return {}; };
 };

include/solid.hpp

@@ -19,7 +19,8 @@ public:
     ISolid(ISolid &&) = default;
     ISolid &operator=(ISolid &&) = default;
 
-    virtual real sdf(const Vec3 &p) const = 0;
+    // virtual real sdf(const Vec3 &p) const = 0;
+    virtual real sdf(const Vec3 &p, Vec3 &closestPoint) const = 0;
 };
 inline Vec2 get2DRepOf3DPt(const Vec3 &pt3D, const Vec3 &u, const Vec3 &v, const Vec3 &localO) {
     Vec3 OP = pt3D - localO;
@@ -28,6 +29,9 @@ inline Vec2 get2DRepOf3DPt(const Vec3 &pt3D, const Vec3 &u, const Vec3 &v, const
 inline Vec2 get2DRepOf3DDir(const Vec3 &dir, const Vec3 &u, const Vec3 &v) {
     return Vec2{dir.dot(u), dir.dot(v)}.normalize();
 }
+inline Vec3 get3DRepOf2DPt(const Vec2 &pt2D, const Vec3 &u, const Vec3 &v, const Vec3 &localO) {
+    return localO + pt2D[0] * u + pt2D[1] * v;
+}
 
 /**
 *  calculate winding number of a point w.r.t. a segment ab
@@ -50,6 +54,9 @@ protected:
     std::vector<Pt2Array> _localProfiles2D;
     std::vector<std::vector<CircularArc<Vec2>>> _localArcs2d;
     Vec3 _biNormalStartPt;
+    Vec3 _axisStart, _axisStartTangent, _axisEnd, _axisEndTangent;
+
+    AABB aabb;
 
 public:
     IExtrudedSolidBase(std::vector<Polyline> profiles, AxisLineType axis, real rScale)
@@ -59,10 +66,12 @@ public:
             assert(_profile.isClosed());
         }
         // project profile at st point to 2D
-        Vec3 tangent = _axis.tangent(0);
+        _axisStartTangent = _axis.tangent(0);
         Vec3 normal = _axis.normal(0);
-        _biNormalStartPt = tangent.cross(normal);
-        Vec3 q = _axis.eval(0);
+        _biNormalStartPt = _axisStartTangent.cross(normal);
+        _axisStart = _axis.eval(0);
+        _axisEnd = _axis.eval(_axis.endT());
+        _axisEndTangent = _axis.tangent(_axis.endT());
         size_t profileCount = _profiles.size();
         _localProfiles2D.resize(profileCount);
         _localArcs2d.resize(profileCount);
@@ -74,20 +83,25 @@ public:
             for (int j = 0; j < segCount; ++j) {
                 // TODO:
                 _localProfiles2D[i][j] =
-                    get2DRepOf3DPt(profile.getPoints()[j], normal, _biNormalStartPt, q);
+                    get2DRepOf3DPt(profile.getPoints()[j], normal, _biNormalStartPt, _axisStart);
                 auto &arc2d = _localArcs2d[i][j];
                 const auto &arc3d = profile.getCircularArcs()[j];
-                arc2d.center = get2DRepOf3DPt(arc3d.center, normal, _biNormalStartPt, q);
+                arc2d.center = get2DRepOf3DPt(arc3d.center, normal, _biNormalStartPt, _axisStart);
                 arc2d.inCircleDir = get2DRepOf3DDir(arc3d.inCircleDir, normal, _biNormalStartPt);
                 arc2d.radius = arc3d.radius;
                 arc2d.theta = arc3d.theta;
                 arc2d.h = arc3d.h;
             }
         }
+        AABB aabbOfProfile = _profiles[0].getAABB();
+        aabb = _axis.getAABB();
+        aabb.move(aabbOfProfile.center() - _axisStart);
+        aabb.expand(aabbOfProfile.halfSize());
+        aabb.expand();
     }
 
-    real sdf(const Vec3 &p) const override {
-        ClosestDescOnSeg closestDescToAxis = _axis.getClosestParam(p);
+    real sdf(const Vec3 &p, Vec3 &closestPoint) const override {
+        ClosestDescOnLine closestDescToAxis = _axis.getClosestParam(p);
         // TNB coordinate system
         auto t = closestDescToAxis.t;
         Vec3 q = _axis.eval(t); // closest point on axis
@@ -97,35 +111,62 @@ public:
         const Vec3 &normal = TBN[1];
         const Vec3 &biNormal = TBN[2];
         if (_axis.isEndParam(t) && fabs(qp.dot(tangent)) > EPS) {
+            // cases away from two sides
             // project p to the plane passing through Q and perpendicular to T
             real pqDotT = -qp.dot(tangent);
             Vec3 projP = p + tangent * pqDotT;
             Vec2 p2D = get2DRepOf3DPt(projP, normal, biNormal, q);
             PtBoundaryRelation ptProfileRelation = pmcProfile2d(p2D);
             real projectedDis = 0;
-            if (ptProfileRelation == Outside)
-                projectedDis = disProfile2D(p2D).dis;
-            // must be positive (outside)
+            if (ptProfileRelation == Outside) {
+                auto closestDesOnProfile = disProfile2D(p2D);
+                projectedDis = closestDesOnProfile.dis;
+                closestPoint = _profiles[closestDesOnProfile.i].eval(closestDesOnProfile.t);
+            } else {
+                closestPoint = projP;
+            }
+            // p must be positive (outside)
             return sqrt(projectedDis * projectedDis + pqDotT * pqDotT);
         }
 
         Vec2 p2D = get2DRepOf3DPt(p, normal, biNormal, q);
         PtBoundaryRelation ptProfileRelation = pmcProfile2d(p2D);
-        if (ptProfileRelation == OnBoundary)
+        if (ptProfileRelation == OnBoundary) {
+            closestPoint = p;
             return 0;
+        }
         auto closestDescToProfile = disProfile2D(p2D);
+        // consider whether either of two sides is closer
+        if (ptProfileRelation == Inside) {
+            real pqDotT = (_axisStart - p).dot(_axisStartTangent);
+            real disTmp = sqrt(pqDotT * pqDotT);
+            if (disTmp < closestDescToProfile.dis) {
+                closestPoint = p - tangent * pqDotT;
+                return -disTmp; // return here may be fault when is closer to the other side, but this does not exit for thick extrusion solid
+            }
+            pqDotT = (_axisEnd - p).dot(_axisEndTangent);
+            disTmp = sqrt(pqDotT * pqDotT);
+            if (disTmp < closestDescToProfile.dis) {
+                closestPoint = p + tangent * pqDotT;
+                return -disTmp;
+            }
+        }
+        closestPoint = _profiles[closestDescToProfile.i].eval(closestDescToProfile.t);
         return closestDescToProfile.dis * static_cast<int>(ptProfileRelation);
     }
 
 private:
     virtual std::array<Vec3, 3> getTBN(const Vec3 &p, const Vec3 &q, real t) const = 0;
 
-    inline ClosestDescOnSeg disProfile2D(const Vec2 &p2D) const {
-        ClosestDescOnSeg closestDescToProfile{};
+    [[nodiscard]] inline ClosestDescOnProfile disProfile2D(const Vec2 &p2D) const {
+        ClosestDescOnProfile closestDescToProfile{};
         for (int i = 0; i < _localArcs2d.size(); ++i) {
-            ClosestDescOnSeg closestDescTemp = disLoop2D(p2D, _localProfiles2D[i], _localArcs2d[i]);
+            ClosestDescOnLine closestDescTemp =
+                disLoop2D(p2D, _localProfiles2D[i], _localArcs2d[i]);
             if (closestDescTemp.dis < closestDescToProfile.dis) {
-                closestDescToProfile = closestDescTemp;
+                closestDescToProfile.dis = closestDescTemp.dis;
+                closestDescToProfile.t = closestDescTemp.t;
+                closestDescToProfile.i = i;
             }
         }
         return closestDescToProfile;
@@ -248,11 +289,11 @@ private:
         return ptInPolygon(p2D) ^ inFan ? Inside : Outside;
     }
 
-    static ClosestDescOnSeg disLoop2D(const Vec2 &p2D, const Pt2Array &loop2D,
-                                      const std::vector<CircularArc<Vec2>> &arcs2d) {
+    static ClosestDescOnLine disLoop2D(const Vec2 &p2D, const Pt2Array &loop2D,
+                                       const std::vector<CircularArc<Vec2>> &arcs2d) {
         size_t segCount = arcs2d.size();
         assert(loop2D.size() == segCount);
-        ClosestDescOnSeg res{};
+        ClosestDescOnLine res{};
         for (int i = 0; i < segCount; ++i) {
             auto disDesc = distance2Arc2D(p2D, loop2D[i], loop2D[(i + 1) % segCount], arcs2d[i]);
             if (res.dis > disDesc.dis) {
@@ -263,8 +304,8 @@ private:
         return res;
     }
 
-    static ClosestDescOnSeg distance2Arc2D(const Vec2 &p2D, const Vec2 &a, const Vec2 &b,
-                                           const CircularArc<Vec2> &arc) {
+    static ClosestDescOnLine distance2Arc2D(const Vec2 &p2D, const Vec2 &a, const Vec2 &b,
+                                            const CircularArc<Vec2> &arc) {
         if (isEqual(arc.theta, 0)) {
             return segPtDist(p2D, a, b);
         }

main.cpp

@@ -3,7 +3,8 @@
 #include "solid.hpp"
 
 void testPrint(const ISolid &solid, const Vec3 &queryPt) {
-    std::cout << "sdf on " << queryPt << " = " << solid.sdf(queryPt) << std::endl;
+    Vec3 closestPt;
+    std::cout << "sdf on " << queryPt << " = " << solid.sdf(queryPt, closestPt) << std::endl;
 }
 
 void caseCyliner() {
@@ -12,6 +13,8 @@ void caseCyliner() {
     ExtrudedSolidPolyline extrusion({profile}, axis);
     testPrint(extrusion, {1, 2, 1});
     testPrint(extrusion, {1, 2, -1});
+
+    profile.getAABB();
 }
 
 int main() {