PMClassifier/include/line.hpp

#pragma once

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

// 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(double param) const = 0;

    virtual Vec3 der1(double param) const = 0;

    virtual Vec3 der2(double param) const = 0;

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

template<size_t N>
using PtArray = std::vector<Vec<N> >;
using Pt3Array = PtArray<3>;
using Pt2Array = PtArray<2>;


class Polyline : public ILine {
public:
    using Point = Vec<3>;

    Polyline(const Pt3Array &points, const std::vector<double> &bugles, const Vec3 &normal, bool closed = false)
            : _points(points), _bugles(bugles), _closed(closed), _normal(normal.normalize()) {
        assert(points.size() >= 2);
        if (closed) {
            assert(points.size() == bugles.size());
        } else {
            assert(points.size() - 1 == bugles.size());
        }
        circularArcs.resize(bugles.size());
        for (size_t i = 0; i < bugles.size(); ++i) {
            const Point &A = points[i];
            const Point &B = points[(i + 1) % points.size()];
            Vec3 AB = B - A;
            Vec3 ABNorm = AB.normalize();
            Vec3 QONorm = normal.cross(ABNorm) * (abs(bugles[i]) > 1 ? -1 : 1);
            real theta = std::atan(bugles[i]) * 4;
            circularArcs[i].h = AB.length() * 0.5 * std::tan(theta * 0.5);
            circularArcs[i].center = A + AB * 0.5 + QONorm * circularArcs[i].h;
            circularArcs[i].theta = theta;
            circularArcs[i].radius = (circularArcs[i].center - A).length();
            circularArcs[i].u = (A - circularArcs[i].center).normalize();
            circularArcs[i].v = ABNorm.cross(circularArcs[i].u);
        }
    }

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

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

    std::vector<CircularArc> circularArcs;

public:
    Vec3 eval(real param) const override {
        assert(param >= 0 && param <= _bugles.size());
        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) const override {
        assert(param >= 0 && param <= _bugles.size());
        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) const override {
        assert(param >= 0 && param <= _bugles.size());
        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) const 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).length() < closestDis) {
                closestDis = (A - p).length();
                closestParam = i;
            }
            if ((B - p).length() < closestDis) {
                closestDis = (B - p).length();
                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).length();
                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).length();
                break;
            } else if (closestParam > seg + 1 + std::numeric_limits<real>::epsilon()) {
                closestParam = seg + 1;
                closestDis = (_points[(seg + 1) % _points.size()] - p).length();
                break;
            }
        }
        return {closestParam, closestDis};
    }

    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;
    }

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

    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).length()};
    }

};

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

    Vec3 der1(double param) const override { return {}; };

    Vec3 der2(double param) const override { return {}; };

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