PMClassifier/include/solid.hpp

#pragma once

#include "common.hpp"
#include "real.hpp"
#include <utility>
#include "vec.hpp"
#include "line.hpp"

class ISolid {
public:
    virtual ~ISolid() = default;

    virtual real sdf(const Vec3 &p) = 0;
};

Vec2 get2DRepOf3DPt(const Vec3 &pt3D, const Vec3 &u, const Vec3 &v, const Vec3 &localO) {
    Vec3 OP = pt3D - localO;
    return {OP.dot(u), OP.dot(v)};
}

Vec2 get2DRepOf3DDir(const Vec3 &dir, const Vec3 &u, const Vec3 &v) {
    return Vec2{dir.dot(u), dir.dot(v)}.normalize();
}

class IExtrudedSolid : public ISolid {
public:
    Polyline _profile; // TODO: may be replaced by const ref to profile
    real _rScale;

public:
    IExtrudedSolid(Polyline profile, real rScale) : _profile(std::move(profile)), _rScale(rScale) {}
};

/**
*  calculate winding number of a point w.r.t. a segment ab
*/
real unsignedWindingNumberSegment(const Vec3 &p, const Vec3 &a, const Vec3 &b,
                                  const Vec3 &refNormal) {
    Vec3 pa = a - p;
    Vec3 pb = b - p;
    return std::acos(std::clamp(pa.dot(pb) / (pa.norm() * pb.norm()), static_cast<real>(-1.),
                                static_cast<real>(1.)))
           / (std::numbers::pi * 2);
}

class ExtrudedSolidPolyline : public IExtrudedSolid {
private:
    Polyline _axis;
    Pt2Array _localProfile2D;
    std::vector<CircularArc<Vec2>> _localArcs2d;
    // Pt2Array _localCircleCenter2D;
    // Pt2Array _localInCircleDir;

public:
    ExtrudedSolidPolyline(Polyline profile, Polyline axis, real rScale)
        : IExtrudedSolid(std::move(profile), rScale), _axis(std::move(axis)) {
        assert(_profile.isClosed());
        // TODO: project profile at st point to 2D
        Vec3 T = _axis.der1(0).normalize();
        Vec3 N = _axis.der2(0).normalize();
        Vec3 B = T.cross(N);
        Vec3 Q = _axis.eval(0);
        int segCount = _profile.getPoints().size();
        _localProfile2D.resize(segCount);
        _localArcs2d.resize(segCount);
        // _localInCircleDir.resize(segCount);
        // _localCircleCenter2D.resize(segCount);
        for (int i = 0; i < segCount; ++i) {
            _localProfile2D[i] = get2DRepOf3DPt(_profile.getPoints()[i] - Q, N, B, Q);
            auto &arc2d = _localArcs2d[i];
            const auto &arc3d = _profile.getCircularArcs()[i];
            arc2d.center = get2DRepOf3DPt(arc3d.center - Q, N, B, Q);
            arc2d.inCircleDir = get2DRepOf3DDir(arc3d.inCircleDir, N, B);
            arc2d.radius = arc3d.radius;
        }
    }

    real sdf(const Vec3 &p) override {
        ClosestDescOnSeg closestDesc = _axis.getClosestParam(p);
        // TNB coordinate system
        auto t = closestDesc.t;
        Vec3 T = _axis.der1(t).normalize();
        Vec3 N = _axis.der2(t).normalize();
        Vec3 B = T.cross(N);
        Vec3 Q = _axis.eval(t);
        Vec3 QP = p - Q;
        auto p2D = get2DRepOf3DPt(QP, N, B, Q);
        // TODO: to test if p2D is in _localProfile2D
        // for (auto i = 0; i < _localProfile2D.size(); ++i) {
        // }
        PtBoundaryRelation ptProfileRelation = getPtProfileRelation(p2D);
        if (ptProfileRelation == OnBoundary) {
            return 0; // TODO: 判断OnBoundary的过程可以加一点容差
        }
        ClosestDescOnSeg closestDescOnProfile = distance2Profile2D(p2D);
        return closestDescOnProfile.dis * static_cast<int>(ptProfileRelation);
    }

private:
    /**
     * in + in = out
     * in + out = in
     * out + in = in
     * out + out = out
     */
    PtBoundaryRelation getPtProfileRelation(const Vec2 &p2D) {
        assert(_profile.isClosed());

        int segCount = _profile.getBugles().size();
        // 先判断是否在outline上
        // 顺便判断点-扇的位置关系
        bool inFan = false;
        int onLinesegButHasBugle = -1;
        for (int i = 0; i < segCount; ++i) {
            const Vec2 &a = _localProfile2D[i];
            const Vec2 &b = _localProfile2D[(i + 1) % segCount];
            if (_profile.getBugles()[i] == 0) {
                //line segment
                if (isPointOnSegment(p2D, a, b)) {
                    return OnBoundary;
                }
                continue;
            }
            if (isPointOnSegment(p2D, a, b)) {
                onLinesegButHasBugle = i;
                break;
            }
            const auto &arc = _profile.getCircularArcs()[i];
            real po = (p2D - _localArcs2d[i].center).norm();
            if ((p2D - a).dot(_localArcs2d[i].inCircleDir) > 0) {
                if (po == arc.radius) {
                    return OnBoundary;
                }
                if (po < arc.radius) {
                    inFan = true;
                    break;
                }
            } else {
                if (po <= arc.radius) {
                    inFan = true;
                    break;
                }
            }
        }

        // 判断点-直线多边形的关系
        const auto ptInPolygon = [&](const Vec2 &p) {
            int intersectionCount = 0;
            // int onSegIdx = -1;
            constexpr int numRays = 3; // 射线数量
            int majorityIn = 0;        // 在多边形内的射线计数
            int majorityOut = 0;       // 在多边形外的射线计数
            for (int rayIdx = 0; rayIdx < numRays; ++rayIdx) {
                double angle = (2.0 * std::numbers::pi * rayIdx) / numRays;
                Vec2 rayDir(cos(angle), sin(angle));
                int crossings = 0;

                for (int i = 0; i < segCount; ++i) {
                    const Vec2 &a = _localProfile2D[i];
                    const Vec2 &b = _localProfile2D[(i + 1) % segCount];
                    assert(isPointOnSegment(p, a, b));
                    // if (isPointOnSegment(p2D, a, b))
                    // {
                    //     onSegIdx = i;
                    //     break;
                    // }
                    // 使用向量方法计算射线和边的交点
                    double dx1 = b[0] - a[0];
                    double dy1 = b[1] - a[1];
                    double dx2 = rayDir[0];
                    double dy2 = rayDir[1];
                    double determinant = dx1 * dy2 - dy1 * dx2;

                    // 如果determinant为0，则射线和边平行，不计算交点
                    if (isEqual(determinant, 0))
                        continue;

                    double t1 = ((p[0] - a[0]) * dy2 - (p[1] - a[1]) * dx2) / determinant;
                    double t2 = ((p[0] - a[0]) * dy1 - (p[1] - a[1]) * dx1) / determinant;

                    // 检查交点是否在边上（0 <= t1 <= 1）且射线上（t2 >= 0）
                    if (t1 >= 0 && t1 <= 1 && t2 >= 0) {
                        crossings++;
                    }
                }
                if (crossings % 2 == 0) {
                    majorityOut++;
                } else {
                    majorityIn++;
                }
            }
            return majorityIn > majorityOut;
        };

        if (onLinesegButHasBugle != -1) {
            // 需要特殊考虑的情况
            // 从p2D向inCircle方向前进一小步
            Vec2 samplePt = p2D
                            + _localArcs2d[onLinesegButHasBugle].center
                                  * std::numeric_limits<real>::epsilon() * 1e6;
            return !ptInPolygon(samplePt) ? Inside : Outside; // 取反
        }
        return ptInPolygon(p2D) ^ inFan ? Inside : Outside;
        // TODO: 返回on的情况
    }

    ClosestDescOnSeg distance2Profile2D(const Vec2 &p2D) {
        // TODO: 2D 下点到圆弧的距离应该可以直接算，不用这么迭代！
        assert(_profile.isClosed());
        ClosestDescOnSeg res{};
        for (int i = 0; i < _localArcs2d.size(); ++i) {
            auto disDesc =
                distance2Arc2D(p2D, _localProfile2D[i],
                               _localProfile2D[(i + 1) % _localArcs2d.size()], _localArcs2d[i]);
            if (res.dis > disDesc.dis) {
                res.dis = disDesc.dis;
                res.t = i + disDesc.t;
            }
        }
        return res;
    }

    ClosestDescOnSeg distance2Arc2D(const Vec2 &p2D, const Vec2 &a, const Vec2 &b,
                                    const CircularArc<Vec2> &arc) {
        const Vec2 &center = arc.center;
        Vec2 op = p2D - center;
        Vec2 q = center + arc.radius * op.normalize(); // closest pt on circle
        Vec2 oq = q - center;
        Vec2 oa = a - center;
        // 判断q是否在弧上
        if ((q - a).dot(arc.inCircleDir) > 0) {
            // 计算参数
            real normMulti = arc.radius * oq.norm();
            real cosTheta = (oa).dot(oq) / normMulti;
            real sinTheta = (oa).cross(oq) / normMulti;
            return {atan2(sinTheta, cosTheta), (p2D - q).norm()};
        }

        real paDis = (a - p2D).norm();
        real pbDis = (b - p2D).norm();
        if (paDis < pbDis)
            return {0, paDis};
        return {1, pbDis};
    }

    bool isOn2DPolyline(const Vec2 &p2D) {
        int segCount = _profile.getBugles().size();
        for (int i = 0; i < segCount; ++i) {
            const Vec2 &a = _localProfile2D[i];
            const Vec2 &b = _localProfile2D[(i + 1) % segCount];
            if (_profile.getBugles()[i] == 0) {
                //line segment
                if (isPointOnSegment(p2D, a, b)) {
                    return true;
                }
                continue;
            }
        }
    }

    bool isPointOnSegment(const Vec2 p, const Vec2 &a, const Vec2 &b) {
        // check collinearity
        double crossProduct = (p[1] - a[1]) * (b[0] - a[0]) - (p[0] - a[0]) * (b[1] - a[1]);
        if (!isEqual(crossProduct, 0))
            return false; // Not collinear

        // Check if point is within segment bounds
        return (p[0] >= std::min(a[0], b[0]) && p[0] <= std::max(a[0], b[0])
                && p[1] >= std::min(a[1], b[1]) && p[1] <= std::max(a[1], b[1]));
    }

    // real wnCircularArc(
    //     const Vec3 &p, const Vec3 &a, const Vec3 &b, const Vec3 &plgNormal, const Polyline::CircularArc &arc, int dir)
    // {
    //     Vec3 pa = a - p;
    //     Vec3 pb = b - p;
    //     real wn =
    //         std::acos(std::clamp(pa.dot(pb) / (pa.norm() * pb.norm()), static_cast<real>(-1.), static_cast<real>(1.))) /
    //         (std::numbers::pi * 2);
    //     auto inOutCircle = arc.inCircleCheck(p);
    //     if (inOutCircle == PtBoundaryRelation::Outside || pa.cross(pb).dot(plgNormal) < 0)
    //     {
    //         // outside
    //         // pa.cross(pb).dot(plgNormal) 不会 == 0
    //         return -wn * dir;
    //     }
    //     if (inOutCircle == PtBoundaryRelation::Inside)
    //     {
    //         return wn * dir;
    //     }

    //     return 0;
    // }

    // Vec2 eval2DProfile(real param)
    // {
    //     int seg = static_cast<int>(param);
    //     real tOnSeg = param - seg;
    //     const auto &arc = circularArcs[seg];
    //     real phi = tOnSeg * arc.theta;
    //     return arc.center + arc.radius * (arc.u * std::cos(phi) + arc.v * std::sin(phi));
    // }
};

class ExtrudedSolidPolynomialLine : public IExtrudedSolid {
protected:
    PolynomialLine _axis;
};