|
@ -32,50 +32,37 @@ class ILine { |
|
|
public: |
|
|
public: |
|
|
virtual ~ILine() = default; |
|
|
virtual ~ILine() = default; |
|
|
|
|
|
|
|
|
virtual Vec3 eval(double param) const = 0; |
|
|
virtual Vec3 eval(real param) = 0; |
|
|
|
|
|
|
|
|
virtual Vec3 der1(double param) const = 0; |
|
|
virtual Vec3 der1(real param) = 0; |
|
|
|
|
|
|
|
|
virtual Vec3 der2(double param) const = 0; |
|
|
virtual Vec3 der2(real param) = 0; |
|
|
|
|
|
|
|
|
virtual ClosestDescOnSeg getClosestParam(const Vec3 &p) const = 0; |
|
|
virtual ClosestDescOnSeg getClosestParam(const Vec3 &p) = 0; |
|
|
}; |
|
|
}; |
|
|
|
|
|
|
|
|
template<size_t N> |
|
|
|
|
|
using PtArray = std::vector<Vec<N> >; |
|
|
|
|
|
using Pt3Array = PtArray<3>; |
|
|
|
|
|
using Pt2Array = PtArray<2>; |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
class Polyline : public ILine { |
|
|
class Polyline : public ILine { |
|
|
public: |
|
|
public: |
|
|
using Point = Vec<3>; |
|
|
using Point = Vec3; |
|
|
|
|
|
|
|
|
Polyline(const Pt3Array &points, const std::vector<double> &bugles, const Vec3 &normal, bool closed = false) |
|
|
Polyline(Pt3Array points, std::vector<real> bugles, const Vec3 &normal, bool closed = false) |
|
|
: _points(points), _bugles(bugles), _closed(closed), _normal(normal.normalize()) { |
|
|
: _points(std::move(points)), _bugles(std::move(bugles)), _closed(closed), _normal(normal.normalize()) { |
|
|
assert(points.size() >= 2); |
|
|
assert(_points.size() >= 2); |
|
|
if (closed) { |
|
|
if (closed) { |
|
|
assert(points.size() == bugles.size()); |
|
|
assert(_points.size() == _points.size()); |
|
|
} else { |
|
|
} else { |
|
|
assert(points.size() - 1 == bugles.size()); |
|
|
assert(_points.size() - 1 == _points.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); |
|
|
|
|
|
} |
|
|
} |
|
|
} |
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
[[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; } |
|
|
|
|
|
|
|
|
private: |
|
|
private: |
|
|
Pt3Array _points; |
|
|
Pt3Array _points; |
|
|
std::vector<real> _bugles; |
|
|
std::vector<real> _bugles; |
|
@ -94,8 +81,27 @@ private: |
|
|
std::vector<CircularArc> circularArcs; |
|
|
std::vector<CircularArc> circularArcs; |
|
|
|
|
|
|
|
|
public: |
|
|
public: |
|
|
Vec3 eval(real param) const override { |
|
|
void initSegInfo() { |
|
|
|
|
|
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 ABHalf = (B - A) * 0.5; |
|
|
|
|
|
Vec3 ABNorm = ABHalf.normalize(); |
|
|
|
|
|
Vec3 QONorm = _normal.cross(ABNorm) * (abs(_bugles[i]) > 1 ? -1 : 1); |
|
|
|
|
|
real theta = std::atan(_bugles[i]) * 4; |
|
|
|
|
|
circularArcs[i].h = ABHalf.norm() * std::tan(theta * 0.5); |
|
|
|
|
|
circularArcs[i].center = A + ABHalf + QONorm * 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 = ABNorm.cross(circularArcs[i].u); |
|
|
|
|
|
} |
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
Vec3 eval(real param) override { |
|
|
assert(param >= 0 && param <= _bugles.size()); |
|
|
assert(param >= 0 && param <= _bugles.size()); |
|
|
|
|
|
if (circularArcs.empty()) initSegInfo(); |
|
|
int seg = static_cast<int>(param); |
|
|
int seg = static_cast<int>(param); |
|
|
real tOnSeg = param - seg; |
|
|
real tOnSeg = param - seg; |
|
|
const auto &arc = circularArcs[seg]; |
|
|
const auto &arc = circularArcs[seg]; |
|
@ -103,8 +109,9 @@ public: |
|
|
return arc.center + arc.radius * (arc.u * std::cos(phi) + arc.v * std::sin(phi)); |
|
|
return arc.center + arc.radius * (arc.u * std::cos(phi) + arc.v * std::sin(phi)); |
|
|
} |
|
|
} |
|
|
|
|
|
|
|
|
Vec3 der1(real param) const override { |
|
|
Vec3 der1(real param) override { |
|
|
assert(param >= 0 && param <= _bugles.size()); |
|
|
assert(param >= 0 && param <= _bugles.size()); |
|
|
|
|
|
if (circularArcs.empty()) initSegInfo(); |
|
|
int seg = static_cast<int>(param); |
|
|
int seg = static_cast<int>(param); |
|
|
real tOnSeg = param - seg; |
|
|
real tOnSeg = param - seg; |
|
|
const auto &arc = circularArcs[seg]; |
|
|
const auto &arc = circularArcs[seg]; |
|
@ -112,8 +119,9 @@ public: |
|
|
return arc.radius * (arc.u * -std::sin(phi) + arc.v * std::cos(phi)); |
|
|
return arc.radius * (arc.u * -std::sin(phi) + arc.v * std::cos(phi)); |
|
|
} |
|
|
} |
|
|
|
|
|
|
|
|
Vec3 der2(real param) const override { |
|
|
Vec3 der2(real param) override { |
|
|
assert(param >= 0 && param <= _bugles.size()); |
|
|
assert(param >= 0 && param <= _bugles.size()); |
|
|
|
|
|
if (circularArcs.empty()) initSegInfo(); |
|
|
int seg = static_cast<int>(param); |
|
|
int seg = static_cast<int>(param); |
|
|
real tOnSeg = param - seg; |
|
|
real tOnSeg = param - seg; |
|
|
const auto &arc = circularArcs[seg]; |
|
|
const auto &arc = circularArcs[seg]; |
|
@ -121,7 +129,7 @@ public: |
|
|
return -arc.radius * (arc.u * std::cos(phi) + arc.v * std::cos(phi)); |
|
|
return -arc.radius * (arc.u * std::cos(phi) + arc.v * std::cos(phi)); |
|
|
} |
|
|
} |
|
|
|
|
|
|
|
|
ClosestDescOnSeg getClosestParam(const Vec3 &p) const override { |
|
|
ClosestDescOnSeg getClosestParam(const Vec3 &p) override { |
|
|
real closestDis = std::numeric_limits<real>::max(); |
|
|
real closestDis = std::numeric_limits<real>::max(); |
|
|
real closestParam; |
|
|
real closestParam; |
|
|
for (int i = 0; i < _bugles.size(); ++i) { |
|
|
for (int i = 0; i < _bugles.size(); ++i) { |
|
@ -130,19 +138,19 @@ public: |
|
|
const auto &arc = circularArcs[i]; |
|
|
const auto &arc = circularArcs[i]; |
|
|
real dis2Seg = segPtDist(p, A, B).dis; |
|
|
real dis2Seg = segPtDist(p, A, B).dis; |
|
|
if (dis2Seg - arc.h > closestDis) continue; |
|
|
if (dis2Seg - arc.h > closestDis) continue; |
|
|
if ((A - p).length() < closestDis) { |
|
|
if ((A - p).norm() < closestDis) { |
|
|
closestDis = (A - p).length(); |
|
|
closestDis = (A - p).norm(); |
|
|
closestParam = i; |
|
|
closestParam = i; |
|
|
} |
|
|
} |
|
|
if ((B - p).length() < closestDis) { |
|
|
if ((B - p).norm() < closestDis) { |
|
|
closestDis = (B - p).length(); |
|
|
closestDis = (B - p).norm(); |
|
|
closestParam = i + 1; |
|
|
closestParam = i + 1; |
|
|
} |
|
|
} |
|
|
int segInsertedCnt = arc.theta / DISC_ARC_ANGLE; |
|
|
int segInsertedCnt = arc.theta / DISC_ARC_ANGLE; |
|
|
for (int j = 0; j < segInsertedCnt; ++j) { |
|
|
for (int j = 0; j < segInsertedCnt; ++j) { |
|
|
real insertParam = i + j * DISC_ARC_ANGLE / arc.theta; |
|
|
real insertParam = i + j * DISC_ARC_ANGLE / arc.theta; |
|
|
const Vec3 insertPt = eval(insertParam); |
|
|
const Vec3 insertPt = eval(insertParam); |
|
|
real dis2InsertPt = (p - insertPt).length(); |
|
|
real dis2InsertPt = (p - insertPt).norm(); |
|
|
if (dis2InsertPt < closestDis) { |
|
|
if (dis2InsertPt < closestDis) { |
|
|
closestDis = dis2InsertPt; |
|
|
closestDis = dis2InsertPt; |
|
|
closestParam = insertParam; |
|
|
closestParam = insertParam; |
|
@ -167,17 +175,18 @@ public: |
|
|
printf("After iter %d, dL is %lf\n", iter, lDer1); |
|
|
printf("After iter %d, dL is %lf\n", iter, lDer1); |
|
|
if (closestParam < seg - std::numeric_limits<real>::epsilon()) { |
|
|
if (closestParam < seg - std::numeric_limits<real>::epsilon()) { |
|
|
closestParam = seg; |
|
|
closestParam = seg; |
|
|
closestDis = (_points[seg] - p).length(); |
|
|
closestDis = (_points[seg] - p).norm(); |
|
|
break; |
|
|
break; |
|
|
} else if (closestParam > seg + 1 + std::numeric_limits<real>::epsilon()) { |
|
|
} else if (closestParam > seg + 1 + std::numeric_limits<real>::epsilon()) { |
|
|
closestParam = seg + 1; |
|
|
closestParam = seg + 1; |
|
|
closestDis = (_points[(seg + 1) % _points.size()] - p).length(); |
|
|
closestDis = (_points[(seg + 1) % _points.size()] - p).norm(); |
|
|
break; |
|
|
break; |
|
|
} |
|
|
} |
|
|
} |
|
|
} |
|
|
return {closestParam, closestDis}; |
|
|
return {closestParam, closestDis}; |
|
|
} |
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
void print() const { |
|
|
void print() const { |
|
|
if (_closed) printf("Closed Polyline: \n"); |
|
|
if (_closed) printf("Closed Polyline: \n"); |
|
|
else printf("Open Polyline: \n"); |
|
|
else printf("Open Polyline: \n"); |
|
@ -202,18 +211,18 @@ private: |
|
|
Vec3 AB = B - A; |
|
|
Vec3 AB = B - A; |
|
|
Vec3 AP = p - A; |
|
|
Vec3 AP = p - A; |
|
|
real h = std::clamp(AP.dot(AB) / AB.dot(AB), 0., 1.); |
|
|
real h = std::clamp(AP.dot(AB) / AB.dot(AB), 0., 1.); |
|
|
return {h, (AP - AB * h).length()}; |
|
|
return {h, (AP - AB * h).norm()}; |
|
|
} |
|
|
} |
|
|
|
|
|
|
|
|
}; |
|
|
}; |
|
|
|
|
|
|
|
|
class PolynomialLine : public ILine { |
|
|
class PolynomialLine : public ILine { |
|
|
public: |
|
|
public: |
|
|
Vec3 eval(double param) const override { return {}; }; |
|
|
Vec3 eval(real param) override { return {}; }; |
|
|
|
|
|
|
|
|
Vec3 der1(double param) const override { return {}; }; |
|
|
Vec3 der1(real param) override { return {}; }; |
|
|
|
|
|
|
|
|
Vec3 der2(double param) const override { return {}; }; |
|
|
Vec3 der2(real param) override { return {}; }; |
|
|
|
|
|
|
|
|
ClosestDescOnSeg getClosestParam(const Vec3 &p) const override { return {}; }; |
|
|
ClosestDescOnSeg getClosestParam(const Vec3 &p) override { return {}; }; |
|
|
}; |
|
|
}; |
|
|