You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
319 lines
11 KiB
319 lines
11 KiB
#pragma once
|
|
|
|
#include "common.hpp"
|
|
#include "vec.hpp"
|
|
#include <algorithm>
|
|
#include <cassert>
|
|
#include <cmath>
|
|
#include <iostream>
|
|
#include <limits.h>
|
|
#include <memory>
|
|
#include <numbers>
|
|
#include <vector>
|
|
|
|
// class ILineParam {
|
|
// public:
|
|
// virtual ~ILineParam() = default;
|
|
// };
|
|
//
|
|
// class PolylineParam : ILineParam {
|
|
// int segIdx;
|
|
// real tOnSeg;
|
|
// };
|
|
//
|
|
// class PolynomialLineParam : ILineParam {
|
|
// real t;
|
|
// };
|
|
|
|
struct ClosestDescOnSeg {
|
|
real t;
|
|
real dis;
|
|
ClosestDescOnSeg(real _t, real _dis) : t(_t), dis(_dis) {}
|
|
ClosestDescOnSeg() : t(0), dis(std::numeric_limits<real>::max()) {}
|
|
};
|
|
|
|
class ILine {
|
|
public:
|
|
int aaa;
|
|
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;
|
|
};
|
|
|
|
template <typename VecType> // Vec2 or Vec3
|
|
struct CircularArc {
|
|
VecType center;
|
|
real radius;
|
|
real theta;
|
|
real h = -1; // straight line for h <= 0.
|
|
VecType u;
|
|
VecType v;
|
|
VecType inCircleDir;
|
|
PtBoundaryRelation inCircleCheck(const VecType &pt) const {
|
|
real d = (pt - center).norm();
|
|
return d < radius ? Inside : d > radius ? Outside : OnBoundary;
|
|
}
|
|
};
|
|
;
|
|
|
|
struct AA {
|
|
int a;
|
|
int b;
|
|
void print() const { std::cout << a << " " << b << std::endl; }
|
|
};
|
|
|
|
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; }
|
|
|
|
[[nodiscard]] const std::vector<CircularArc<Vec3>> &getCircularArcs() const {
|
|
return circularArcs;
|
|
}
|
|
|
|
private:
|
|
Pt3Array _points;
|
|
std::vector<real> _bugles;
|
|
Vec3 _normal;
|
|
bool _closed;
|
|
|
|
std::vector<CircularArc<Vec3>> 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 (fabs(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;
|
|
// }
|
|
// 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};
|
|
// }
|
|
|
|
ClosestDescOnSeg getClosestParam(const Vec3 &p) override {
|
|
ClosestDescOnSeg closestDes{};
|
|
for (int i = 0; i < _bugles.size(); ++i) {
|
|
const Vec3 &a = _points[i];
|
|
const Vec3 &b = _points[(i + 1) % _points.size()];
|
|
const CircularArc<Vec3> &arc = circularArcs[i];
|
|
const Vec3 &o = arc.center;
|
|
// p 到圆弧平面的投影
|
|
const Vec3 projPt = p - _normal.dot(p - a) * _normal;
|
|
// projPt到圆的最近点
|
|
const Vec3 clsPtOnCircle = o + arc.radius * (projPt - o).normalize();
|
|
if ((clsPtOnCircle - a).dot(arc.inCircleDir) > 0) {
|
|
// 在圆弧上
|
|
real dis = (p - clsPtOnCircle).norm();
|
|
if (dis < closestDes.dis) {
|
|
closestDes.dis = dis;
|
|
Vec3 oa = a - o;
|
|
Vec3 oClsPt = clsPtOnCircle - o;
|
|
real R2 = arc.radius * arc.radius;
|
|
real cosTheta = (oa).dot(oClsPt) / R2;
|
|
real theta = std::acos(cosTheta); // [0, pi]
|
|
if ((oa.cross(oClsPt)).dot(_normal) < 0) {
|
|
theta = 2 * std::numbers::pi - theta;
|
|
}
|
|
closestDes.t = i + theta / arc.theta;
|
|
}
|
|
continue;
|
|
}
|
|
real paDis = (p - a).norm();
|
|
real pbDis = (p - b).norm();
|
|
if (paDis < closestDes.dis) {
|
|
closestDes.dis = paDis;
|
|
closestDes.t = i;
|
|
} else {
|
|
closestDes.dis = pbDis;
|
|
closestDes.t = i + 1;
|
|
}
|
|
}
|
|
return closestDes;
|
|
}
|
|
|
|
void print() const {
|
|
if (_closed)
|
|
std::cout << "Closed Polyline: \n";
|
|
else
|
|
std::cout << "Open Polyline: \n";
|
|
std::cout << "Points: {\n";
|
|
for (int i = 0; i < _points.size(); ++i) {
|
|
std::cout << _points[i].x() << ", " << _points[i].y() << ", " << _points[i].z() << ">";
|
|
if (i != _points.size() - 1)
|
|
std::cout << ", ";
|
|
}
|
|
std::cout << "}\n";
|
|
std::cout << "的可变参数Bugles: {\n";
|
|
for (int i = 0; i < _bugles.size(); ++i) {
|
|
std::cout << _bugles[i];
|
|
if (i != _bugles.size() - 1)
|
|
std::cout << ", ";
|
|
}
|
|
std::cout << "}\n";
|
|
}
|
|
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 HelixLine : 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 {}; };
|
|
|
|
private:
|
|
Vec3 axisStart, axisDir;
|
|
real advanceLen, advancePerRound;
|
|
real startTheta; // 轴线起始时在螺旋投影（圆）上的参数
|
|
};
|
|
|
|
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 {}; };
|
|
};
|
|
|