PMClassifier/include/line.hpp

#pragma once

#include "vec.hpp"
#include <vector>
#include <iostream>
#include <memory>
#include <cassert>
#include <cmath>
#include <algorithm>
#include <numbers>
#include "common.hpp"

// class ILineParam {
// public:
//     virtual ~ILineParam() = default;
// };
//
// class PolylineParam : ILineParam {
//     int segIdx;
//     real tOnSeg;
// };
//
// class PolynomialLineParam : ILineParam {
//     real t;
// };

struct ClosestDescOnSeg
{
    real t;
    real dis;
};

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

    virtual Vec3 eval(real param) = 0;

    virtual Vec3 der1(real param) = 0;

    virtual Vec3 der2(real param) = 0;

    virtual ClosestDescOnSeg getClosestParam(const Vec3 &p) = 0;
};

const real DISC_ARC_ANGLE = std::numbers::pi * 0.125;

class Polyline : public ILine
{
public:
    using Point = Vec3;

    Polyline(Pt3Array points, std::vector<real> bugles, const Vec3 &normal, bool closed = false)
    : _points(std::move(points))
    , _bugles(std::move(bugles))
    , _closed(closed)
    , _normal(normal.normalize())
    {
        assert(_points.size() >= 2);
        if (closed)
        {
            assert(_points.size() == _points.size());
        }
        else
        {
            assert(_points.size() - 1 == _points.size());
        }
        circularArcs.resize(_bugles.size());
    }

    [[nodiscard]] const Pt3Array &getPoints() const { return _points; }

    [[nodiscard]] const std::vector<real> &getBugles() const { return _bugles; }

    [[nodiscard]] const Vec3 &getNormal() const { return _normal; }

    [[nodiscard]] bool isClosed() const { return _closed; }

    struct CircularArc
    {
        Vec3 center;
        real radius;
        real theta;
        real h;
        Vec3 u; // dir of OA
        Vec3 v; // u X v = normal
        Vec3 inCircleDir;

        PtBoundaryRelation inCircleCheck(const Vec3 &pt) const
        {
            real d = (pt - center).norm();
            return d < radius ? Inside : d > radius ? Outside : OnBoundary;
        }
    };

private:
    Pt3Array _points;
    std::vector<real> _bugles;
    Vec3 _normal;
    bool _closed;

    std::vector<CircularArc> circularArcs;

public:
    void initSegInfo()
    {
        for (size_t i = 0; i < _bugles.size(); ++i)
        {
            const Point &A = _points[i];
            const Point &B = _points[(i + 1) % _points.size()];
            Vec3 ABHalf = (B - A) * 0.5;
            Vec3 ABNorm = ABHalf.normalize();
            real theta = std::atan(_bugles[i]) * 4;
            circularArcs[i].inCircleDir = _normal.cross(ABNorm) * (abs(_bugles[i]) > 1 ? -1 : 1);
            circularArcs[i].h = ABHalf.norm() * std::tan(theta * 0.5);
            circularArcs[i].center = A + ABHalf + circularArcs[i].inCircleDir * circularArcs[i].h;
            circularArcs[i].theta = theta;
            circularArcs[i].radius = (circularArcs[i].center - A).norm();
            circularArcs[i].u = (A - circularArcs[i].center).normalize();
            circularArcs[i].v = _normal.cross(circularArcs[i].u);
        }
    }

    Vec3 eval(real param) override
    {
        if (circularArcs.empty())
            initSegInfo();
        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));
    }

    Vec3 der1(real param) override
    {
        if (circularArcs.empty())
            initSegInfo();
        int seg = static_cast<int>(param);
        real tOnSeg = param - seg;
        const auto &arc = circularArcs[seg];
        real phi = tOnSeg * arc.theta;
        return arc.radius * (arc.u * -std::sin(phi) + arc.v * std::cos(phi));
    }

    Vec3 der2(real param) override
    {
        if (circularArcs.empty())
            initSegInfo();
        int seg = static_cast<int>(param);
        real tOnSeg = param - seg;
        const auto &arc = circularArcs[seg];
        real phi = tOnSeg * arc.theta;
        return -arc.radius * (arc.u * std::cos(phi) + arc.v * std::cos(phi));
    }

    ClosestDescOnSeg getClosestParam(const Vec3 &p) override
    {
        real closestDis = std::numeric_limits<real>::max();
        real closestParam;
        for (int i = 0; i < _bugles.size(); ++i)
        {
            const Vec3 &A = _points[i];
            const Vec3 &B = _points[(i + 1) % _points.size()];
            const auto &arc = circularArcs[i];
            real dis2Seg = segPtDist(p, A, B).dis;
            if (dis2Seg - arc.h > closestDis)
                continue;
            if ((A - p).norm() < closestDis)
            {
                closestDis = (A - p).norm();
                closestParam = i;
            }
            if ((B - p).norm() < closestDis)
            {
                closestDis = (B - p).norm();
                closestParam = i + 1;
            }
            int segInsertedCnt = arc.theta / DISC_ARC_ANGLE;
            for (int j = 0; j < segInsertedCnt; ++j)
            {
                real insertParam = i + j * DISC_ARC_ANGLE / arc.theta;
                const Vec3 insertPt = eval(insertParam);
                real dis2InsertPt = (p - insertPt).norm();
                if (dis2InsertPt < closestDis)
                {
                    closestDis = dis2InsertPt;
                    closestParam = insertParam;
                }
            }
        }
        // TODO: 为了鲁棒和精度，应该在每个可能最近的seg上做newton iteration
        int seg = static_cast<int>(closestParam);
        // Q = arc.center + arc.radius * (arc.u * std::cos(phi) + arc.v * std::sin(phi))
        // d2 = (Q - p)^2
        Vec3 q = eval(closestParam);
        Vec3 qDer1 = der1(closestParam);
        Vec3 qDer2 = der2(closestParam);
        real lDer1 = (q - p).dot(qDer1);
        int iter = 0;
        while (abs(lDer1) > std::numeric_limits<real>::epsilon() * 1e6)
        {
            closestParam -= lDer1 / (qDer1.dot(qDer1) + (q - p).dot(qDer2)); // -der1 / der2
            q = eval(closestParam);
            qDer1 = der1(closestParam);
            qDer2 = der2(closestParam);
            lDer1 = (q - p).dot(qDer1);
            printf("After iter %d, dL is %lf\n", iter, lDer1);
            if (closestParam < seg - std::numeric_limits<real>::epsilon())
            {
                closestParam = seg;
                closestDis = (_points[seg] - p).norm();
                break;
            }
            else if (closestParam > seg + 1 + std::numeric_limits<real>::epsilon())
            {
                closestParam = seg + 1;
                closestDis = (_points[(seg + 1) % _points.size()] - p).norm();
                break;
            }
            closestDis = (q - p).norm();
            iter++;
        }
        return {closestParam, closestDis};
    }

    const std::vector<CircularArc> &getCircularArcs() const { return circularArcs; }

    void print() const
    {
        if (_closed)
            printf("Closed Polyline: \n");
        else
            printf("Open Polyline: \n");
        printf("Points: {\n");
        for (int i = 0; i < _points.size(); ++i)
        {
            printf("<%lf, %lf, %lf>", _points[i].x(), _points[i].y(), _points[i].z());
            if (i != _points.size() - 1)
                printf(", ");
        }
        std::cout << "}" << std::endl;
        printf("Bugles: {\n");
        for (int i = 0; i < _bugles.size(); ++i)
        {
            printf("%lf", _bugles[i]);
            if (i != _bugles.size() - 1)
                printf(", ");
        }
        std::cout << "}" << std::endl;
    }

    static ClosestDescOnSeg 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()};
    }

    static ClosestDescOnSeg 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.);
        return {h, (AP - AB * h).norm()};
    }
};

class PolynomialLine : public ILine
{
public:
    Vec3 eval(real param) override { return {}; };

    Vec3 der1(real param) override { return {}; };

    Vec3 der2(real param) override { return {}; };

    ClosestDescOnSeg getClosestParam(const Vec3 &p) override { return {}; };
};