|
|
@ -40,11 +40,15 @@ public: |
|
|
|
int aaa; |
|
|
|
virtual ~ILine() = default; |
|
|
|
|
|
|
|
virtual Vec3 eval(real param) = 0; |
|
|
|
virtual Vec3 eval(real t) = 0; |
|
|
|
|
|
|
|
virtual Vec3 der1(real param) = 0; |
|
|
|
virtual Vec3 der1(real t) = 0; |
|
|
|
|
|
|
|
virtual Vec3 der2(real param) = 0; |
|
|
|
virtual Vec3 der2(real t) = 0; |
|
|
|
|
|
|
|
virtual Vec3 tangent(real t) = 0; |
|
|
|
|
|
|
|
virtual Vec3 normal(real t, const Vec3 &tan = -1.) = 0; |
|
|
|
|
|
|
|
virtual ClosestDescOnSeg getClosestParam(const Vec3 &p) = 0; |
|
|
|
|
|
|
@ -75,13 +79,42 @@ struct AA { |
|
|
|
|
|
|
|
const real DISC_ARC_ANGLE = std::numbers::pi * 0.125; |
|
|
|
|
|
|
|
namespace detail { |
|
|
|
void initCircularArcInfo(const Vec3 &a, const Vec3 &b, real bugle, const Vec3 &refNormal, |
|
|
|
CircularArc<Vec3> &res) { |
|
|
|
if (isEqual(bugle, 0)) { |
|
|
|
res.radius = INFINITY; |
|
|
|
res.theta = 0; |
|
|
|
res.h = INFINITY; |
|
|
|
res.inCircleDir = refNormal.cross(b - a).normalize(); |
|
|
|
res.u = res.inCircleDir; |
|
|
|
res.v = refNormal.cross(res.u); |
|
|
|
return; |
|
|
|
} |
|
|
|
|
|
|
|
Vec3 abHalf = (b - a) * HALF; |
|
|
|
Vec3 abNorm = abHalf.normalize(); |
|
|
|
real theta = std::atan(fabs(bugle)) * 4; |
|
|
|
res.inCircleDir = abNorm.cross(refNormal) * (bugle > 0 ? 1 : -1); |
|
|
|
|
|
|
|
if (fabs(bugle) == 1) { |
|
|
|
res.h = 0; |
|
|
|
} else { |
|
|
|
res.h = abHalf.norm() / std::tan(theta * HALF); |
|
|
|
} |
|
|
|
res.center = a + abHalf - res.inCircleDir * res.h; |
|
|
|
res.theta = theta; |
|
|
|
res.radius = (res.center - a).norm(); |
|
|
|
res.u = (a - res.center).normalize(); |
|
|
|
res.v = refNormal.cross(res.u); |
|
|
|
} |
|
|
|
} // namespace detail
|
|
|
|
|
|
|
|
class Polyline : public ILine { |
|
|
|
public: |
|
|
|
using Point = Vec3; |
|
|
|
|
|
|
|
Polyline(Pt3Array points, std::vector<real> bugles, const Vec3 &normal, bool closed = false) |
|
|
|
Polyline(Pt3Array points, std::vector<real> bugles, const Vec3 &refNormal, bool closed = false) |
|
|
|
: _points(std::move(points)), _bugles(std::move(bugles)), _closed(closed), |
|
|
|
_normal(normal.normalize()) { |
|
|
|
_refNormal(refNormal.normalize()) { |
|
|
|
assert(_points.size() >= 2); |
|
|
|
if (closed) { |
|
|
|
assert(_points.size() == _points.size()); |
|
|
@ -95,7 +128,7 @@ public: |
|
|
|
|
|
|
|
[[nodiscard]] const std::vector<real> &getBugles() const { return _bugles; } |
|
|
|
|
|
|
|
[[nodiscard]] const Vec3 &getNormal() const { return _normal; } |
|
|
|
[[nodiscard]] const Vec3 &getRefNormal() const { return _refNormal; } |
|
|
|
|
|
|
|
[[nodiscard]] bool isClosed() const { return _closed; } |
|
|
|
|
|
|
@ -106,7 +139,7 @@ public: |
|
|
|
private: |
|
|
|
Pt3Array _points; |
|
|
|
std::vector<real> _bugles; |
|
|
|
Vec3 _normal; |
|
|
|
Vec3 _refNormal; |
|
|
|
bool _closed; |
|
|
|
|
|
|
|
std::vector<CircularArc<Vec3>> circularArcs; |
|
|
@ -114,51 +147,58 @@ private: |
|
|
|
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) * HALF; |
|
|
|
Vec3 ABNorm = ABHalf.normalize(); |
|
|
|
real theta = std::atan(_bugles[i]) * 4; |
|
|
|
circularArcs[i].inCircleDir = _normal.cross(ABNorm) * (fabs(_bugles[i]) > 1 ? -1 : 1); |
|
|
|
circularArcs[i].h = ABHalf.norm() * std::tan(theta * HALF); |
|
|
|
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); |
|
|
|
detail::initCircularArcInfo(_points[i], _points[(i + 1) % _points.size()], _bugles[i], |
|
|
|
_refNormal, circularArcs[i]); |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
Vec3 eval(real param) override { |
|
|
|
Vec3 eval(real t) override { |
|
|
|
if (circularArcs.empty()) |
|
|
|
initSegInfo(); |
|
|
|
int seg = static_cast<int>(param); |
|
|
|
real tOnSeg = param - seg; |
|
|
|
int seg = static_cast<int>(t); |
|
|
|
if (isEqual(_bugles[seg], 0)) { |
|
|
|
return _points[seg] + (_points[(seg + 1) % _bugles.size()] - _points[seg]) * (t - seg); |
|
|
|
} |
|
|
|
real tOnSeg = t - 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 { |
|
|
|
Vec3 der1(real t) override { |
|
|
|
if (circularArcs.empty()) |
|
|
|
initSegInfo(); |
|
|
|
int seg = static_cast<int>(param); |
|
|
|
real tOnSeg = param - seg; |
|
|
|
int seg = static_cast<int>(t); |
|
|
|
if (isEqual(_bugles[seg], 0)) { |
|
|
|
return _points[(seg + 1) % _bugles.size()] - _points[seg]; |
|
|
|
} |
|
|
|
real tOnSeg = t - 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 { |
|
|
|
Vec3 der2(real t) override { |
|
|
|
if (circularArcs.empty()) |
|
|
|
initSegInfo(); |
|
|
|
int seg = static_cast<int>(param); |
|
|
|
real tOnSeg = param - seg; |
|
|
|
int seg = static_cast<int>(t); |
|
|
|
assert(!isEqual(_bugles[seg], 0)); |
|
|
|
real tOnSeg = t - seg; |
|
|
|
const auto &arc = circularArcs[seg]; |
|
|
|
real phi = tOnSeg * arc.theta; |
|
|
|
return -arc.radius * (arc.u * std::cos(phi) + arc.v * std::cos(phi)); |
|
|
|
} |
|
|
|
|
|
|
|
Vec3 tangent(real t) override { return der1(t).normalize(); } |
|
|
|
// TODO: 试试https://www.jianshu.com/p/9e4877e3965e算出来的结果
|
|
|
|
Vec3 normal(real t, [[maybe_unused]] const Vec3 &tan = -1.) override { |
|
|
|
if (!isEqual(_bugles[static_cast<int>(t)], 0)) { |
|
|
|
return -circularArcs[static_cast<int>(t)].inCircleDir; |
|
|
|
} |
|
|
|
// 只有对于圆弧这样的特殊曲线是这样
|
|
|
|
return der2(t).normalize(); |
|
|
|
} |
|
|
|
|
|
|
|
// ClosestDescOnSeg getClosestParam(const Vec3 &p) override {
|
|
|
|
// real closestDis = std::numeric_limits<real>::max();
|
|
|
|
// real closestParam;
|
|
|
@ -221,14 +261,25 @@ public: |
|
|
|
// }
|
|
|
|
|
|
|
|
ClosestDescOnSeg getClosestParam(const Vec3 &p) override { |
|
|
|
if (circularArcs.empty()) |
|
|
|
initSegInfo(); |
|
|
|
ClosestDescOnSeg 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); |
|
|
|
if (segPtDistRes.dis < closestDes.dis) { |
|
|
|
closestDes = segPtDistRes; |
|
|
|
closestDes.t = i + segPtDistRes.t; |
|
|
|
} |
|
|
|
continue; |
|
|
|
} |
|
|
|
const CircularArc<Vec3> &arc = circularArcs[i]; |
|
|
|
const Vec3 &o = arc.center; |
|
|
|
// p 到圆弧平面的投影
|
|
|
|
const Vec3 projPt = p - _normal.dot(p - a) * _normal; |
|
|
|
const Vec3 projPt = p - _refNormal.dot(p - a) * _refNormal; |
|
|
|
// projPt到圆的最近点
|
|
|
|
const Vec3 clsPtOnCircle = o + arc.radius * (projPt - o).normalize(); |
|
|
|
if ((clsPtOnCircle - a).dot(arc.inCircleDir) > 0) { |
|
|
@ -241,7 +292,7 @@ public: |
|
|
|
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) { |
|
|
|
if ((oa.cross(oClsPt)).dot(_refNormal) < 0) { |
|
|
|
theta = 2 * std::numbers::pi - theta; |
|
|
|
} |
|
|
|
closestDes.t = i + theta / arc.theta; |
|
|
@ -296,7 +347,7 @@ public: |
|
|
|
} |
|
|
|
|
|
|
|
[[nodiscard]] bool isEndParam(real t) const override { |
|
|
|
return t < EPS || t > static_cast<real>(_bugles.size()) - EPS; |
|
|
|
return t < EPS_END_PARAM || t > static_cast<real>(_bugles.size()) - EPS_END_PARAM; |
|
|
|
} |
|
|
|
}; |
|
|
|
|
|
|
@ -330,6 +381,17 @@ public: |
|
|
|
return -_4pi2r_p2 * (_u * std::cos(theta) + _v * std::sin(theta)); |
|
|
|
}; |
|
|
|
|
|
|
|
Vec3 tangent(real t) override { return der1(t).normalize(); } |
|
|
|
|
|
|
|
Vec3 normal(real t, const Vec3 &tan = -1.) override { |
|
|
|
Vec3 der2Vec = this->der2(t); |
|
|
|
if (tan == -1.) { |
|
|
|
Vec3 realTan = tangent(t); |
|
|
|
return (der2Vec - der2Vec.dot(realTan) * realTan).normalize(); |
|
|
|
} |
|
|
|
return (der2Vec - der2Vec.dot(tan) * tan).normalize(); |
|
|
|
} |
|
|
|
|
|
|
|
ClosestDescOnSeg getClosestParam(const Vec3 &p) override { |
|
|
|
// discretization and traversal
|
|
|
|
real startT = 0; |
|
|
@ -418,13 +480,41 @@ class SingleLine : public ILine { |
|
|
|
public: |
|
|
|
}; |
|
|
|
|
|
|
|
// 单段圆弧
|
|
|
|
// 其实就是
|
|
|
|
class ArcLine : public ILine { |
|
|
|
public: |
|
|
|
ArcLine(const Vec3 &a, const Vec3 &b, real bugle, const Vec3 &refNormal) |
|
|
|
: _a(a), _b(b), _bugle(bugle), _refNormal(refNormal.normalize()) {} |
|
|
|
|
|
|
|
bool isEndParam(real t) const override { return t < EPS || t > 1 - EPS; } |
|
|
|
Vec3 eval(real t) override { return {}; }; |
|
|
|
|
|
|
|
Vec3 der1(real t) override { return {}; }; |
|
|
|
|
|
|
|
Vec3 der2(real t) override { return {}; }; |
|
|
|
|
|
|
|
ClosestDescOnSeg getClosestParam(const Vec3 &p) override { return {}; }; |
|
|
|
|
|
|
|
private: |
|
|
|
Vec3 _a, _b, _refNormal; |
|
|
|
real _bugle; |
|
|
|
CircularArc<Vec3> _circularArc; |
|
|
|
|
|
|
|
void initArcInfo() { detail::initCircularArcInfo(_a, _b, _bugle, _refNormal, _circularArc); } |
|
|
|
}; |
|
|
|
|
|
|
|
class PolynomialLine : public ILine { |
|
|
|
public: |
|
|
|
Vec3 eval(real param) override { return {}; }; |
|
|
|
Vec3 eval(real t) override { return {}; }; |
|
|
|
|
|
|
|
Vec3 der1(real t) override { return {}; }; |
|
|
|
|
|
|
|
Vec3 der2(real t) override { return {}; }; |
|
|
|
|
|
|
|
Vec3 der1(real param) override { return {}; }; |
|
|
|
Vec3 tangent(real t) override { return {}; } |
|
|
|
|
|
|
|
Vec3 der2(real param) override { return {}; }; |
|
|
|
Vec3 normal(real t, const Vec3 &tan = -1.) override { return {}; } |
|
|
|
|
|
|
|
ClosestDescOnSeg getClosestParam(const Vec3 &p) override { return {}; }; |
|
|
|
}; |
|
|
|