|
|
@ -10,8 +10,6 @@ |
|
|
|
#include <iostream> |
|
|
|
#include <limits.h> |
|
|
|
#include <limits> |
|
|
|
#include <memory> |
|
|
|
#include <numbers> |
|
|
|
#include <vector> |
|
|
|
|
|
|
|
// class ILineParam {
|
|
|
@ -45,17 +43,17 @@ public: |
|
|
|
ILine(ILine &&) = default; |
|
|
|
ILine &operator=(ILine &&) = default; |
|
|
|
|
|
|
|
virtual Vec3 eval(real t) = 0; |
|
|
|
virtual Vec3 eval(real t) const = 0; |
|
|
|
|
|
|
|
virtual Vec3 der1(real t) = 0; |
|
|
|
virtual Vec3 der1(real t) const = 0; |
|
|
|
|
|
|
|
virtual Vec3 der2(real t) = 0; |
|
|
|
virtual Vec3 der2(real t) const = 0; |
|
|
|
|
|
|
|
virtual Vec3 tangent(real t) = 0; |
|
|
|
virtual Vec3 tangent(real t) const = 0; |
|
|
|
|
|
|
|
virtual Vec3 normal(real t, const Vec3 &tan = -1.) = 0; |
|
|
|
virtual Vec3 normal(real t, const Vec3 &tan = -1.) const = 0; |
|
|
|
|
|
|
|
virtual ClosestDescOnSeg getClosestParam(const Vec3 &p) = 0; |
|
|
|
virtual ClosestDescOnSeg getClosestParam(const Vec3 &p) const = 0; |
|
|
|
|
|
|
|
[[nodiscard]] virtual bool isEndParam(real t) const = 0; |
|
|
|
}; |
|
|
@ -96,7 +94,7 @@ struct AA { |
|
|
|
void print() const { std::cout << a << " " << b << std::endl; } |
|
|
|
}; |
|
|
|
|
|
|
|
const real DISC_ARC_ANGLE = std::numbers::pi * 0.125; |
|
|
|
const real DISC_ARC_ANGLE = PI * 0.125; |
|
|
|
|
|
|
|
class Polyline : public ILine { |
|
|
|
public: |
|
|
@ -105,11 +103,12 @@ public: |
|
|
|
_refNormal(refNormal.normalize()) { |
|
|
|
assert(_points.size() >= 2); |
|
|
|
if (closed) { |
|
|
|
assert(_points.size() == _points.size()); |
|
|
|
assert(_points.size() == _bugles.size()); |
|
|
|
} else { |
|
|
|
assert(_points.size() - 1 == _points.size()); |
|
|
|
assert(_points.size() - 1 == _bugles.size()); |
|
|
|
} |
|
|
|
circularArcs.resize(_bugles.size()); |
|
|
|
initSegInfo(); |
|
|
|
} |
|
|
|
|
|
|
|
[[nodiscard]] const Pt3Array &getPoints() const { return _points; } |
|
|
@ -139,12 +138,12 @@ public: |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
Vec3 eval(real t) override { |
|
|
|
if (circularArcs.empty()) |
|
|
|
initSegInfo(); |
|
|
|
Vec3 eval(real t) const override { |
|
|
|
// if (circularArcs.empty())
|
|
|
|
// initSegInfo();
|
|
|
|
int seg = static_cast<int>(t); |
|
|
|
if (isEqual(_bugles[seg], 0)) { |
|
|
|
return _points[seg] + (_points[(seg + 1) % _bugles.size()] - _points[seg]) * (t - seg); |
|
|
|
return _points[seg] + (_points[(seg + 1) % _points.size()] - _points[seg]) * (t - seg); |
|
|
|
} |
|
|
|
real tOnSeg = t - seg; |
|
|
|
const auto &arc = circularArcs[seg]; |
|
|
@ -152,12 +151,10 @@ public: |
|
|
|
return arc.center + arc.radius * (arc.u * std::cos(phi) + arc.v * std::sin(phi)); |
|
|
|
} |
|
|
|
|
|
|
|
Vec3 der1(real t) override { |
|
|
|
if (circularArcs.empty()) |
|
|
|
initSegInfo(); |
|
|
|
Vec3 der1(real t) const override { |
|
|
|
int seg = static_cast<int>(t); |
|
|
|
if (isEqual(_bugles[seg], 0)) { |
|
|
|
return _points[(seg + 1) % _bugles.size()] - _points[seg]; |
|
|
|
return _points[(seg + 1) % _points.size()] - _points[seg]; |
|
|
|
} |
|
|
|
real tOnSeg = t - seg; |
|
|
|
const auto &arc = circularArcs[seg]; |
|
|
@ -165,21 +162,22 @@ public: |
|
|
|
return arc.radius * (arc.u * -std::sin(phi) + arc.v * std::cos(phi)); |
|
|
|
} |
|
|
|
|
|
|
|
Vec3 der2(real t) override { |
|
|
|
if (circularArcs.empty()) |
|
|
|
initSegInfo(); |
|
|
|
Vec3 der2(real t) const override { |
|
|
|
int seg = static_cast<int>(t); |
|
|
|
assert(!isEqual(_bugles[seg], 0)); |
|
|
|
if (isEqual(_bugles[seg], 0)) { |
|
|
|
int aaa = 1; |
|
|
|
} |
|
|
|
// 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(); } |
|
|
|
Vec3 tangent(real t) const 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)) { |
|
|
|
Vec3 normal(real t, [[maybe_unused]] const Vec3 &tan = -1.) const override { |
|
|
|
if (isEqual(_bugles[static_cast<int>(t)], 0)) { |
|
|
|
return -circularArcs[static_cast<int>(t)].inCircleDir; |
|
|
|
} |
|
|
|
// 只有对于圆弧这样的特殊曲线是这样
|
|
|
@ -247,9 +245,7 @@ public: |
|
|
|
// return {closestParam, closestDis};
|
|
|
|
// }
|
|
|
|
|
|
|
|
ClosestDescOnSeg getClosestParam(const Vec3 &p) override { |
|
|
|
if (circularArcs.empty()) |
|
|
|
initSegInfo(); |
|
|
|
ClosestDescOnSeg getClosestParam(const Vec3 &p) const override { |
|
|
|
ClosestDescOnSeg closestDes{}; |
|
|
|
for (int i = 0; i < _bugles.size(); ++i) { |
|
|
|
const Vec3 &a = _points[i]; |
|
|
@ -280,7 +276,7 @@ public: |
|
|
|
real cosTheta = (oa).dot(oClsPt) / R2; |
|
|
|
real theta = std::acos(cosTheta); // [0, pi]
|
|
|
|
if ((oa.cross(oClsPt)).dot(_refNormal) < 0) { |
|
|
|
theta = 2 * std::numbers::pi - theta; |
|
|
|
theta = PI2 - theta; |
|
|
|
} |
|
|
|
closestDes.t = i + theta / arc.theta; |
|
|
|
} |
|
|
@ -370,24 +366,24 @@ public: |
|
|
|
// _k = _4pi2r / (advancePerRound * advancePerRound + _4pi2r * _r);
|
|
|
|
_arcDeltaMaxFactor = _4pi2r / (advancePerRound * advancePerRound + _4pi2r * _r) * ONE_EIGHT; |
|
|
|
} |
|
|
|
Vec3 eval(real t) override { |
|
|
|
Vec3 eval(real t) const override { |
|
|
|
real theta = _frequency * t; |
|
|
|
return _axisStart + _axisDir * t + (_u * std::cos(theta) + _v * std::sin(theta)) * _r; |
|
|
|
}; |
|
|
|
|
|
|
|
Vec3 der1(real param) override { |
|
|
|
Vec3 der1(real param) const override { |
|
|
|
real theta = _frequency * param; |
|
|
|
return _axisDir + _2pir_p * (_v * std::cos(theta) - _u * std::sin(theta)); |
|
|
|
}; |
|
|
|
|
|
|
|
Vec3 der2(real param) override { |
|
|
|
Vec3 der2(real param) const override { |
|
|
|
real theta = _frequency * param; |
|
|
|
return -_4pi2r_p2 * (_u * std::cos(theta) + _v * std::sin(theta)); |
|
|
|
}; |
|
|
|
|
|
|
|
Vec3 tangent(real t) override { return der1(t).normalize(); } |
|
|
|
Vec3 tangent(real t) const override { return der1(t).normalize(); } |
|
|
|
|
|
|
|
Vec3 normal(real t, const Vec3 &tan = -1.) override { |
|
|
|
Vec3 normal(real t, const Vec3 &tan = -1.) const override { |
|
|
|
Vec3 der2Vec = this->der2(t); |
|
|
|
if (tan == -1.) { |
|
|
|
Vec3 realTan = tangent(t); |
|
|
@ -396,7 +392,7 @@ public: |
|
|
|
return (der2Vec - der2Vec.dot(tan) * tan).normalize(); |
|
|
|
} |
|
|
|
|
|
|
|
ClosestDescOnSeg getClosestParam(const Vec3 &p) override { |
|
|
|
ClosestDescOnSeg getClosestParam(const Vec3 &p) const override { |
|
|
|
// discretization and traversal
|
|
|
|
real startT = 0; |
|
|
|
real endT = SEG_T; |
|
|
@ -490,7 +486,6 @@ public: |
|
|
|
ArcLine(const Vec3 &a, const Vec3 &b, real bugle, const Vec3 &refNormal) |
|
|
|
// : _a(a), _b(b), _bugle(bugle), _refNormal(refNormal.normalize()) {}
|
|
|
|
: Polyline(Pt3Array{{a, b}}, std::vector<real>{bugle}, refNormal, false) {} |
|
|
|
ClosestDescOnSeg getClosestParam(const Vec3 &p) override { return {}; }; |
|
|
|
[[nodiscard]] const std::vector<real> &getBugles() const = delete; |
|
|
|
[[nodiscard]] const real &getBugle() const { return _bugles[0]; } |
|
|
|
|
|
|
@ -502,15 +497,15 @@ public: |
|
|
|
|
|
|
|
class PolynomialLine : public ILine { |
|
|
|
public: |
|
|
|
Vec3 eval(real t) override { return {}; }; |
|
|
|
Vec3 eval(real t) const override { return {}; }; |
|
|
|
|
|
|
|
Vec3 der1(real t) override { return {}; }; |
|
|
|
Vec3 der1(real t) const override { return {}; }; |
|
|
|
|
|
|
|
Vec3 der2(real t) override { return {}; }; |
|
|
|
Vec3 der2(real t) const override { return {}; }; |
|
|
|
|
|
|
|
Vec3 tangent(real t) override { return {}; } |
|
|
|
Vec3 tangent(real t) const override { return {}; } |
|
|
|
|
|
|
|
Vec3 normal(real t, const Vec3 &tan = -1.) override { return {}; } |
|
|
|
Vec3 normal(real t, const Vec3 &tan = -1.) const override { return {}; } |
|
|
|
|
|
|
|
ClosestDescOnSeg getClosestParam(const Vec3 &p) override { return {}; }; |
|
|
|
ClosestDescOnSeg getClosestParam(const Vec3 &p) const override { return {}; }; |
|
|
|
}; |
|
|
|