From 9273f721b0dbda7a9ceded945e9c44ffa3607776 Mon Sep 17 00:00:00 2001 From: stribor14 Date: Tue, 17 Sep 2024 15:12:38 +0200 Subject: [PATCH 01/19] Add static method for Curve fitting onto a given polyline --- CMakeLists.txt | 2 + include/Bezier/bezier.h | 2 + include/Bezier/utils.h | 97 ++++++++++++++++++++++++++ src/bezier.cpp | 147 +++++++++++++++++++++++++--------------- src/utils.cpp | 75 ++++++++++++++++++++ 5 files changed, 267 insertions(+), 56 deletions(-) create mode 100644 include/Bezier/utils.h create mode 100644 src/utils.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index a1cc237..8ae7c5d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -13,12 +13,14 @@ find_package(Eigen3 REQUIRED) include_directories(SYSTEM ${EIGEN3_INCLUDE_DIR} include) set(Bezier_SRC + ${PROJECT_SOURCE_DIR}/src/utils.cpp ${PROJECT_SOURCE_DIR}/src/bezier.cpp ${PROJECT_SOURCE_DIR}/src/polycurve.cpp ) set(Bezier_INC ${PROJECT_SOURCE_DIR}/include/Bezier/declarations.h + ${PROJECT_SOURCE_DIR}/include/Bezier/utils.h ${PROJECT_SOURCE_DIR}/include/Bezier/bezier.h ${PROJECT_SOURCE_DIR}/include/Bezier/polycurve.h ) diff --git a/include/Bezier/bezier.h b/include/Bezier/bezier.h index b9740f0..a9f5639 100644 --- a/include/Bezier/bezier.h +++ b/include/Bezier/bezier.h @@ -280,6 +280,8 @@ class Curve */ void applyContinuity(const Curve& curve, const std::vector& beta_coeffs); + static Curve fromPolyline(const PointVector& polyline, unsigned order = 0); + protected: /*! * \brief N x 2 matrix where each row corresponds to control Point diff --git a/include/Bezier/utils.h b/include/Bezier/utils.h new file mode 100644 index 0000000..31fea40 --- /dev/null +++ b/include/Bezier/utils.h @@ -0,0 +1,97 @@ +#ifndef UTILS_H +#define UTILS_H + +#include +#include +#include + +#include "declarations.h" + +#ifndef __cpp_lib_make_unique +namespace std +{ +template inline std::unique_ptr make_unique(Args&&... args) +{ + return std::unique_ptr(new T(std::forward(args)...)); +} +} // namespace std +#endif + +namespace Bezier +{ + +struct _PolynomialRoots : public std::vector +{ + explicit _PolynomialRoots(unsigned reserve) { std::vector::reserve(reserve); } + void clear(){} // no-op so that PolynomialSolver::RealRoots() doesn't clear it + void push_back(double t) // only allow valid roots + { + if (t >= 0 && t <= 1) + std::vector::push_back(t); + } +}; + +inline unsigned _exp2(unsigned exp) { return 1 << exp; } + +template +inline T _pow(T base, unsigned exp) +{ + T result = exp & 1 ? base : 1; + while (exp >>= 1) + { + base *= base; + if (exp & 1) + result *= base; + } + return result; +} + +inline Eigen::RowVectorXd _powSeries(double base, unsigned exp) +{ + Eigen::RowVectorXd power_series(exp); + power_series(0) = 1; + for (unsigned k = 1; k < exp; k++) + power_series(k) = power_series(k - 1) * base; + return power_series; +} + +inline Eigen::VectorXd _trimZeroes(const Eigen::VectorXd& vec) +{ + auto idx = vec.size(); + while (idx && std::abs(vec(idx - 1)) < _epsilon) + --idx; + return vec.head(idx); +} + +inline double _polylineLength(const PointVector& polyline) +{ + double length{}; + for (size_t k{1}; k < polyline.size(); k++) + length += (polyline[k] - polyline[k-1]).norm(); + return length; +} + +inline double _polylineDist(const PointVector& polyline, const Point& point) +{ + auto distSeg = [&point](const Point& p1, const Point& p2) { + Vector u = p2 - p1; + Vector v = point - p1; + double t = u.dot(v) / u.squaredNorm(); + if (t < 0) + return v.squaredNorm(); + if (t > 1) + return (point - p2).squaredNorm(); + return (p1 + t * u - point).squaredNorm(); + }; + + double dist = std::numeric_limits::max(); + for (size_t k{1}; k < polyline.size(); k++) + dist = std::min(dist, distSeg(polyline[k-1], polyline[k])); + return dist; +} + +PointVector _polylineSimplify(const PointVector& polyline, unsigned N); + +} + +#endif // UTILS_H diff --git a/src/bezier.cpp b/src/bezier.cpp index 413a11d..649f68b 100644 --- a/src/bezier.cpp +++ b/src/bezier.cpp @@ -1,6 +1,8 @@ #include "Bezier/bezier.h" +#include "Bezier/utils.h" #include +#include #include #include @@ -8,62 +10,6 @@ using namespace Bezier; -///// Additional declarations - -#ifndef __cpp_lib_make_unique -namespace std -{ -template inline std::unique_ptr make_unique(Args&&... args) -{ - return std::unique_ptr(new T(std::forward(args)...)); -} -} // namespace std -#endif - -struct _PolynomialRoots : public std::vector -{ - explicit _PolynomialRoots(unsigned reserve) { std::vector::reserve(reserve); } - void clear(){}; // no-op so that PolynomialSolver::RealRoots() doesn't clear it - void push_back(double t) // only allow valid roots - { - if (t >= 0 && t <= 1) - std::vector::push_back(t); - } -}; - -inline unsigned _exp2(unsigned exp) { return 1 << exp; } - -inline double _pow(double base, unsigned exp) -{ - double result = exp & 1 ? base : 1; - while (exp >>= 1) - { - base *= base; - if (exp & 1) - result *= base; - } - return result; -} - -inline Eigen::RowVectorXd _powSeries(double base, unsigned exp) -{ - Eigen::RowVectorXd power_series(exp); - power_series(0) = 1; - for (unsigned k = 1; k < exp; k++) - power_series(k) = power_series(k - 1) * base; - return power_series; -} - -inline Eigen::VectorXd _trimZeroes(const Eigen::VectorXd& vec) -{ - auto idx = vec.size(); - while (idx && std::abs(vec(idx - 1)) < _epsilon) - --idx; - return vec.head(idx); -} - -///// Curve::Curve - Curve::Curve(Eigen::MatrixX2d points) : control_points_(std::move(points)), N_(control_points_.rows()) {} Curve::Curve(const PointVector& points) @@ -617,6 +563,95 @@ void Curve::applyContinuity(const Curve& curve, const std::vector& beta_ resetCache(); } +Curve Curve::fromPolyline(const PointVector& polyline, unsigned order) +{ + // Select N points that most influence the shape of the polyline, + // either based on a specified order or using the full polyline. + const unsigned N = std::min(order ? order + 1 : polyline.size(), polyline.size()); + auto simplified = _polylineSimplify(polyline, N); + + // Initialize vector t where each element represents a normalized cumulative + // distance between consecutive simplified points along the simplified polyline. + Eigen::VectorXd t(N); + Eigen::MatrixXd P(N, 2), M = bernsteinCoeffs(N); + for (unsigned k = 0; k < N; k++) + { + P.row(k) = simplified[k]; + t(k) = k == 0 ? 0 : t(k - 1) + (P.row(k) - P.row(k - 1)).norm(); + } + t /= t(N - 1); + + // Compute the control points for a Bezier curve such that C(t_i) = P_i for all i, + // by solving the matrix form of the Bezier curve equation. + auto getCurve = [&M, &P](const Eigen::VectorXd& t) { + Eigen::MatrixXd T(t.size(), t.size()); + for (unsigned k = 0; k < t.size(); k++) + T.row(k) = _powSeries(t(k), t.size()); + return Curve(M.inverse() * (T.transpose() * T).inverse() * T.transpose() * P); + }; + + // Calculate the root mean square distance (RMSD) between the polyline + // representation of the curve and the original polyline points. + auto rmsd = [&polyline](const Curve& c) + { + double rmsd{}; + auto polyline_c = c.polyline(); + for (const auto& p : polyline_c) + rmsd += _pow(_polylineDist(polyline, p), 2); + return std::sqrt(rmsd / polyline_c.size()); + }; + + // Calculate the absolute difference in length between the polyline representation + // of the curve and the original polyline points. + const double polyline_length = _polylineLength(polyline); + auto length_diff = [&polyline_length](const Curve& c) + { + return std::fabs(_polylineLength(c.polyline()) - polyline_length); + }; + + // Define a cost function for optimization, combining the RMSD and length difference, + // with a +1 offset to handle cases where either is zero. + auto costFun = [&rmsd, &length_diff](const Curve& c) + { + return (1 + rmsd(c)) * (1 + length_diff(c)); // +1 is for cases when either is zero + }; + + // Initialize random number generator for random mutations in the t vector. + std::random_device rd; + std::mt19937 gen(rd()); + std::uniform_int_distribution<> idx_randomizer(1, N - 2); + + // Initialize optimization variables and best_guess + double mutation_std_dev{0.5}; + unsigned no_improvement_counter = 0, no_improvement_threshold = _pow(N, 2); + std::pair best_guess{costFun(getCurve(t)), t}; + + int iter{}; + while (mutation_std_dev > _epsilon) + { + iter++; + do { + t = best_guess.second; + t(idx_randomizer(gen)) += std::normal_distribution<>(0.0, mutation_std_dev)(gen); + } while (t.minCoeff() < 0.0 || t.maxCoeff() > 1.0 || (t.array().head(N-1) >= t.array().tail(N-1)).any()); + + double new_cost = costFun(getCurve(t)); + if (new_cost < best_guess.first) + { + best_guess = std::make_pair(new_cost, t); + no_improvement_counter = 0; + } + else if (++no_improvement_counter > no_improvement_threshold) + { + mutation_std_dev /= 2; + no_improvement_counter = 0; + no_improvement_threshold = std::max(10u, no_improvement_threshold / 2); + } + } + + return getCurve(best_guess.second); +} + void Curve::resetCache() { N_ = control_points_.rows(); diff --git a/src/utils.cpp b/src/utils.cpp new file mode 100644 index 0000000..d610069 --- /dev/null +++ b/src/utils.cpp @@ -0,0 +1,75 @@ +#include "Bezier/utils.h" + +#include +#include + +using namespace Bezier; + +PointVector Bezier::_polylineSimplify(const PointVector &polyline, unsigned int N) +{ + if (polyline.size() < 2) + throw std::logic_error{"Polyline must have at least two points."}; + if (polyline.size() < N) + return PointVector(polyline); + if (N == 2) + return PointVector{polyline.front(), polyline.back()}; + + // Simplification is done using the Visvalingam-Whyatt algorithm + // Helper structure to keep track of each Points neigbours and contribution to the polyline shape + struct Vertex { + size_t prev, next; + double contribution; + }; + std::vector vertices; + vertices.reserve(polyline.size()); + + // Set is used to keep track of sorted contributions + auto cmp=[&vertices](size_t idx1, size_t idx2) { + return vertices[idx1].contribution > vertices[idx2].contribution; + }; + std::vector by_contribution(polyline.size()-2); + std::iota(by_contribution.begin(), by_contribution.end(), 1); + + // Visvalingam-Whyatt measures contribution as an area between 3 consecutive Points + auto area = [&polyline](size_t id1, size_t id2, size_t id3) + { + const auto& A = polyline[id1]; + const auto& B = polyline[id2]; + const auto& C = polyline[id3]; + return std::fabs((B.x() - A.x()) * (C.y() - A.y()) - (C.x() - A.x()) * (B.y() - A.y())); + }; + + // Initialize structures used for the algorithm + vertices.push_back({0, 1, 0.0}); + for(size_t k=1; k+1 N - 2) + { + // Select and erase a Point with smallest current contribution to polyline shape + std::make_heap(by_contribution.begin(), by_contribution.end(), cmp); + std::pop_heap(by_contribution.begin(), by_contribution.end(), cmp); + auto curr = by_contribution.back(); + by_contribution.pop_back(); + std::cout << vertices[curr].contribution << " "; + + // Update previous and next Vertex: + // - update neighbours neighbours + // - update neighbours contribution + auto prev = vertices[curr].prev; + auto next = vertices[curr].next; + vertices[prev].next = vertices[curr].next; + vertices[next].prev = vertices[curr].prev; + vertices[prev].contribution = area(vertices[prev].prev, prev, vertices[prev].next); + vertices[next].contribution = area(vertices[next].prev, next, vertices[next].next); + } +std::cout << std::endl; + // Reconstruct simplified polyline + PointVector simplified; + simplified.reserve(N); + for(size_t k = 0; k < polyline.size(); k = vertices[k].next) + simplified.push_back(polyline[k]); + return simplified; +} From 5f116fac9fc39cb15328e060ae9922a7b6ded007 Mon Sep 17 00:00:00 2001 From: stribor14 Date: Tue, 17 Sep 2024 15:13:15 +0200 Subject: [PATCH 02/19] Add static method for joining two Curves --- example/bezier_example.pro | 2 ++ include/Bezier/bezier.h | 2 ++ src/bezier.cpp | 13 +++++++++++++ 3 files changed, 17 insertions(+) diff --git a/example/bezier_example.pro b/example/bezier_example.pro index 8362d7f..61ff9c9 100644 --- a/example/bezier_example.pro +++ b/example/bezier_example.pro @@ -35,6 +35,7 @@ SOURCES += \ customscene.cpp \ qcurve.cpp \ qpolycurve.cpp \ + ../src/utils.cpp \ ../src/bezier.cpp \ ../src/polycurve.cpp \ @@ -44,6 +45,7 @@ HEADERS += \ customscene.h \ qcurve.h \ qpolycurve.h \ + ../include/Bezier/utils.h \ ../include/Bezier/bezier.h \ ../include/Bezier/polycurve.h \ ../include/Bezier/declarations.h diff --git a/include/Bezier/bezier.h b/include/Bezier/bezier.h index a9f5639..4cb3339 100644 --- a/include/Bezier/bezier.h +++ b/include/Bezier/bezier.h @@ -280,6 +280,8 @@ class Curve */ void applyContinuity(const Curve& curve, const std::vector& beta_coeffs); + static Curve joinCurves(const Curve& curve1, const Curve& curve2, unsigned order = 0); + static Curve fromPolyline(const PointVector& polyline, unsigned order = 0); protected: diff --git a/src/bezier.cpp b/src/bezier.cpp index 649f68b..bda2111 100644 --- a/src/bezier.cpp +++ b/src/bezier.cpp @@ -563,6 +563,19 @@ void Curve::applyContinuity(const Curve& curve, const std::vector& beta_ resetCache(); } +Curve Curve::joinCurves(const Curve &curve1, const Curve &curve2, unsigned int order) +{ + if (order == 1) + return Curve(PointVector{curve1.control_points_.row(0), curve2.control_points_.row(curve2.N_ - 1)}); + + auto polyline = curve1.polyline(); + auto polyline2 = curve2.polyline(); + polyline.reserve(polyline.size() + polyline2.size()); + polyline.insert(polyline.end(), std::make_move_iterator(polyline2.begin()), std::make_move_iterator(polyline2.end())); + + return fromPolyline(polyline, order ? order : curve1.order() + curve2.order()); +} + Curve Curve::fromPolyline(const PointVector& polyline, unsigned order) { // Select N points that most influence the shape of the polyline, From 05be740d5e37c94ef2bf885a94c535c6b1f59a23 Mon Sep 17 00:00:00 2001 From: stribor14 Date: Tue, 17 Sep 2024 15:21:04 +0200 Subject: [PATCH 03/19] Run formatter on all files --- include/Bezier/utils.h | 13 ++++++------- src/bezier.cpp | 28 +++++++++++++--------------- src/utils.cpp | 26 +++++++++++++------------- 3 files changed, 32 insertions(+), 35 deletions(-) diff --git a/include/Bezier/utils.h b/include/Bezier/utils.h index 31fea40..f1f01f4 100644 --- a/include/Bezier/utils.h +++ b/include/Bezier/utils.h @@ -3,11 +3,11 @@ #include #include -#include #include "declarations.h" #ifndef __cpp_lib_make_unique +#include namespace std { template inline std::unique_ptr make_unique(Args&&... args) @@ -23,7 +23,7 @@ namespace Bezier struct _PolynomialRoots : public std::vector { explicit _PolynomialRoots(unsigned reserve) { std::vector::reserve(reserve); } - void clear(){} // no-op so that PolynomialSolver::RealRoots() doesn't clear it + void clear() {} // no-op so that PolynomialSolver::RealRoots() doesn't clear it void push_back(double t) // only allow valid roots { if (t >= 0 && t <= 1) @@ -33,8 +33,7 @@ struct _PolynomialRoots : public std::vector inline unsigned _exp2(unsigned exp) { return 1 << exp; } -template -inline T _pow(T base, unsigned exp) +template inline T _pow(T base, unsigned exp) { T result = exp & 1 ? base : 1; while (exp >>= 1) @@ -67,7 +66,7 @@ inline double _polylineLength(const PointVector& polyline) { double length{}; for (size_t k{1}; k < polyline.size(); k++) - length += (polyline[k] - polyline[k-1]).norm(); + length += (polyline[k] - polyline[k - 1]).norm(); return length; } @@ -86,12 +85,12 @@ inline double _polylineDist(const PointVector& polyline, const Point& point) double dist = std::numeric_limits::max(); for (size_t k{1}; k < polyline.size(); k++) - dist = std::min(dist, distSeg(polyline[k-1], polyline[k])); + dist = std::min(dist, distSeg(polyline[k - 1], polyline[k])); return dist; } PointVector _polylineSimplify(const PointVector& polyline, unsigned N); -} +} // namespace Bezier #endif // UTILS_H diff --git a/src/bezier.cpp b/src/bezier.cpp index bda2111..a80516a 100644 --- a/src/bezier.cpp +++ b/src/bezier.cpp @@ -563,7 +563,7 @@ void Curve::applyContinuity(const Curve& curve, const std::vector& beta_ resetCache(); } -Curve Curve::joinCurves(const Curve &curve1, const Curve &curve2, unsigned int order) +Curve Curve::joinCurves(const Curve& curve1, const Curve& curve2, unsigned int order) { if (order == 1) return Curve(PointVector{curve1.control_points_.row(0), curve2.control_points_.row(curve2.N_ - 1)}); @@ -589,24 +589,23 @@ Curve Curve::fromPolyline(const PointVector& polyline, unsigned order) Eigen::MatrixXd P(N, 2), M = bernsteinCoeffs(N); for (unsigned k = 0; k < N; k++) { - P.row(k) = simplified[k]; - t(k) = k == 0 ? 0 : t(k - 1) + (P.row(k) - P.row(k - 1)).norm(); + P.row(k) = simplified[k]; + t(k) = k == 0 ? 0 : t(k - 1) + (P.row(k) - P.row(k - 1)).norm(); } t /= t(N - 1); // Compute the control points for a Bezier curve such that C(t_i) = P_i for all i, // by solving the matrix form of the Bezier curve equation. auto getCurve = [&M, &P](const Eigen::VectorXd& t) { - Eigen::MatrixXd T(t.size(), t.size()); - for (unsigned k = 0; k < t.size(); k++) - T.row(k) = _powSeries(t(k), t.size()); - return Curve(M.inverse() * (T.transpose() * T).inverse() * T.transpose() * P); + Eigen::MatrixXd T(t.size(), t.size()); + for (unsigned k = 0; k < t.size(); k++) + T.row(k) = _powSeries(t(k), t.size()); + return Curve(M.inverse() * (T.transpose() * T).inverse() * T.transpose() * P); }; // Calculate the root mean square distance (RMSD) between the polyline // representation of the curve and the original polyline points. - auto rmsd = [&polyline](const Curve& c) - { + auto rmsd = [&polyline](const Curve& c) { double rmsd{}; auto polyline_c = c.polyline(); for (const auto& p : polyline_c) @@ -617,15 +616,13 @@ Curve Curve::fromPolyline(const PointVector& polyline, unsigned order) // Calculate the absolute difference in length between the polyline representation // of the curve and the original polyline points. const double polyline_length = _polylineLength(polyline); - auto length_diff = [&polyline_length](const Curve& c) - { + auto length_diff = [&polyline_length](const Curve& c) { return std::fabs(_polylineLength(c.polyline()) - polyline_length); }; // Define a cost function for optimization, combining the RMSD and length difference, // with a +1 offset to handle cases where either is zero. - auto costFun = [&rmsd, &length_diff](const Curve& c) - { + auto costFun = [&rmsd, &length_diff](const Curve& c) { return (1 + rmsd(c)) * (1 + length_diff(c)); // +1 is for cases when either is zero }; @@ -643,10 +640,11 @@ Curve Curve::fromPolyline(const PointVector& polyline, unsigned order) while (mutation_std_dev > _epsilon) { iter++; - do { + do + { t = best_guess.second; t(idx_randomizer(gen)) += std::normal_distribution<>(0.0, mutation_std_dev)(gen); - } while (t.minCoeff() < 0.0 || t.maxCoeff() > 1.0 || (t.array().head(N-1) >= t.array().tail(N-1)).any()); + } while (t.minCoeff() < 0.0 || t.maxCoeff() > 1.0 || (t.array().head(N - 1) >= t.array().tail(N - 1)).any()); double new_cost = costFun(getCurve(t)); if (new_cost < best_guess.first) diff --git a/src/utils.cpp b/src/utils.cpp index d610069..21d8d4b 100644 --- a/src/utils.cpp +++ b/src/utils.cpp @@ -1,11 +1,11 @@ #include "Bezier/utils.h" -#include #include +#include using namespace Bezier; -PointVector Bezier::_polylineSimplify(const PointVector &polyline, unsigned int N) +PointVector Bezier::_polylineSimplify(const PointVector& polyline, unsigned int N) { if (polyline.size() < 2) throw std::logic_error{"Polyline must have at least two points."}; @@ -16,7 +16,8 @@ PointVector Bezier::_polylineSimplify(const PointVector &polyline, unsigned int // Simplification is done using the Visvalingam-Whyatt algorithm // Helper structure to keep track of each Points neigbours and contribution to the polyline shape - struct Vertex { + struct Vertex + { size_t prev, next; double contribution; }; @@ -24,15 +25,14 @@ PointVector Bezier::_polylineSimplify(const PointVector &polyline, unsigned int vertices.reserve(polyline.size()); // Set is used to keep track of sorted contributions - auto cmp=[&vertices](size_t idx1, size_t idx2) { + auto cmp = [&vertices](size_t idx1, size_t idx2) { return vertices[idx1].contribution > vertices[idx2].contribution; }; - std::vector by_contribution(polyline.size()-2); + std::vector by_contribution(polyline.size() - 2); std::iota(by_contribution.begin(), by_contribution.end(), 1); // Visvalingam-Whyatt measures contribution as an area between 3 consecutive Points - auto area = [&polyline](size_t id1, size_t id2, size_t id3) - { + auto area = [&polyline](size_t id1, size_t id2, size_t id3) { const auto& A = polyline[id1]; const auto& B = polyline[id2]; const auto& C = polyline[id3]; @@ -41,12 +41,12 @@ PointVector Bezier::_polylineSimplify(const PointVector &polyline, unsigned int // Initialize structures used for the algorithm vertices.push_back({0, 1, 0.0}); - for(size_t k=1; k+1 N - 2) + while (by_contribution.size() > N - 2) { // Select and erase a Point with smallest current contribution to polyline shape std::make_heap(by_contribution.begin(), by_contribution.end(), cmp); @@ -65,11 +65,11 @@ PointVector Bezier::_polylineSimplify(const PointVector &polyline, unsigned int vertices[prev].contribution = area(vertices[prev].prev, prev, vertices[prev].next); vertices[next].contribution = area(vertices[next].prev, next, vertices[next].next); } -std::cout << std::endl; + std::cout << std::endl; // Reconstruct simplified polyline PointVector simplified; simplified.reserve(N); - for(size_t k = 0; k < polyline.size(); k = vertices[k].next) + for (size_t k = 0; k < polyline.size(); k = vertices[k].next) simplified.push_back(polyline[k]); return simplified; } From b315f52e0b9e8133d74f41bcf4497b6b6b943eb8 Mon Sep 17 00:00:00 2001 From: stribor14 Date: Tue, 17 Sep 2024 16:39:58 +0200 Subject: [PATCH 04/19] Add method for offseting a curve --- include/Bezier/bezier.h | 5 ++++- src/bezier.cpp | 36 +++++++++++++++++++++++++++++------- src/utils.cpp | 4 +--- 3 files changed, 34 insertions(+), 11 deletions(-) diff --git a/include/Bezier/bezier.h b/include/Bezier/bezier.h index 4cb3339..8808e59 100644 --- a/include/Bezier/bezier.h +++ b/include/Bezier/bezier.h @@ -280,6 +280,8 @@ class Curve */ void applyContinuity(const Curve& curve, const std::vector& beta_coeffs); + static Curve offsetCurve(const Curve& curve, double offset); + static Curve joinCurves(const Curve& curve1, const Curve& curve2, unsigned order = 0); static Curve fromPolyline(const PointVector& polyline, unsigned order = 0); @@ -313,7 +315,8 @@ class Curve mutable std::unique_ptr> cached_roots_; /*! If generated, stores roots for later use */ mutable std::unique_ptr cached_bounding_box_; /*! If generated, stores bounding box for later use */ mutable std::unique_ptr cached_polyline_; /*! If generated, stores polyline for later use */ - mutable double cached_polyline_flatness_{}; /*! Flatness of cached polyline */ + mutable std::unique_ptr> cached_polyline_t_; /*! If generated, stores polyline t for later use */ + mutable double cached_polyline_flatness_{}; /*! Flatness of cached polyline */ mutable std::unique_ptr cached_projection_polynomial_part_; /*! Constant part of point projection polynomial */ mutable Eigen::MatrixXd diff --git a/src/bezier.cpp b/src/bezier.cpp index a80516a..01bbf96 100644 --- a/src/bezier.cpp +++ b/src/bezier.cpp @@ -49,9 +49,11 @@ PointVector Curve::polyline(double flatness) const cached_polyline_flatness_ = flatness; cached_polyline_ = std::make_unique(); cached_polyline_->emplace_back(control_points_.row(0)); + cached_polyline_t_ = std::make_unique>(); + cached_polyline_t_->emplace_back(0.0); - std::vector subcurves; - subcurves.emplace_back(control_points_); + std::vector> subcurves; + subcurves.emplace_back(control_points_, 0.0, 1.0); // we calculate in squared distances flatness *= flatness; @@ -66,7 +68,7 @@ PointVector Curve::polyline(double flatness) const while (!subcurves.empty()) { - Eigen::MatrixX2d cp(std::move(subcurves.back())); + auto [cp, t1, t2] = std::move(subcurves.back()); subcurves.pop_back(); const Point& p1 = cp.row(0); const Point& p2 = cp.row(N_ - 1); @@ -84,11 +86,14 @@ PointVector Curve::polyline(double flatness) const }; if (coeff * cp.rowwise().redux(deviation).maxCoeff() <= flatness) + { cached_polyline_->emplace_back(cp.row(N_ - 1)); + cached_polyline_t_->emplace_back(t2); + } else { - subcurves.emplace_back(splittingCoeffsRight(N_) * cp); - subcurves.emplace_back(splittingCoeffsLeft(N_) * cp); + subcurves.emplace_back(splittingCoeffsRight(N_) * cp, (t1 + t2) / 2, t2); + subcurves.emplace_back(splittingCoeffsLeft(N_) * cp, t1, (t1 + t2) / 2); } } } @@ -563,6 +568,18 @@ void Curve::applyContinuity(const Curve& curve, const std::vector& beta_ resetCache(); } +Curve Curve::offsetCurve(const Curve& curve, double offset) +{ + PointVector offset_polyline; + if (!curve.cached_polyline_) + curve.polyline(); + offset_polyline.reserve(curve.cached_polyline_->size()); + for (size_t k{}; k < curve.cached_polyline_->size(); k++) + offset_polyline.emplace_back((*curve.cached_polyline_)[k] + + offset * curve.normalAt((*curve.cached_polyline_t_)[k])); + return fromPolyline(offset_polyline, curve.order() + 1); +} + Curve Curve::joinCurves(const Curve& curve1, const Curve& curve2, unsigned int order) { if (order == 1) @@ -578,6 +595,11 @@ Curve Curve::joinCurves(const Curve& curve1, const Curve& curve2, unsigned int o Curve Curve::fromPolyline(const PointVector& polyline, unsigned order) { + if (polyline.size() < 2) + throw std::logic_error{"Polyline must have at least two points."}; + if (order == 1) + return Curve(PointVector{polyline.front(), polyline.back()}); + // Select N points that most influence the shape of the polyline, // either based on a specified order or using the full polyline. const unsigned N = std::min(order ? order + 1 : polyline.size(), polyline.size()); @@ -633,7 +655,7 @@ Curve Curve::fromPolyline(const PointVector& polyline, unsigned order) // Initialize optimization variables and best_guess double mutation_std_dev{0.5}; - unsigned no_improvement_counter = 0, no_improvement_threshold = _pow(N, 2); + unsigned no_improvement_counter = 0, no_improvement_threshold = _pow(N, 3); std::pair best_guess{costFun(getCurve(t)), t}; int iter{}; @@ -656,7 +678,7 @@ Curve Curve::fromPolyline(const PointVector& polyline, unsigned order) { mutation_std_dev /= 2; no_improvement_counter = 0; - no_improvement_threshold = std::max(10u, no_improvement_threshold / 2); + no_improvement_threshold = std::max(_pow(N, 2), no_improvement_threshold / 2); } } diff --git a/src/utils.cpp b/src/utils.cpp index 21d8d4b..0ff8a46 100644 --- a/src/utils.cpp +++ b/src/utils.cpp @@ -1,6 +1,5 @@ #include "Bezier/utils.h" -#include #include using namespace Bezier; @@ -53,7 +52,6 @@ PointVector Bezier::_polylineSimplify(const PointVector& polyline, unsigned int std::pop_heap(by_contribution.begin(), by_contribution.end(), cmp); auto curr = by_contribution.back(); by_contribution.pop_back(); - std::cout << vertices[curr].contribution << " "; // Update previous and next Vertex: // - update neighbours neighbours @@ -65,7 +63,7 @@ PointVector Bezier::_polylineSimplify(const PointVector& polyline, unsigned int vertices[prev].contribution = area(vertices[prev].prev, prev, vertices[prev].next); vertices[next].contribution = area(vertices[next].prev, next, vertices[next].next); } - std::cout << std::endl; + // Reconstruct simplified polyline PointVector simplified; simplified.reserve(N); From 215b21822fcf950f88f23f02618d8a723698722c Mon Sep 17 00:00:00 2001 From: stribor14 Date: Wed, 18 Sep 2024 17:27:37 +0200 Subject: [PATCH 05/19] Reimplement fitting method from randomized guess to gradient descent Pareto optimization --- include/Bezier/bezier.h | 2 +- src/bezier.cpp | 116 +++++++++++++++++++++++++++------------- 2 files changed, 80 insertions(+), 38 deletions(-) diff --git a/include/Bezier/bezier.h b/include/Bezier/bezier.h index 8808e59..33df198 100644 --- a/include/Bezier/bezier.h +++ b/include/Bezier/bezier.h @@ -280,7 +280,7 @@ class Curve */ void applyContinuity(const Curve& curve, const std::vector& beta_coeffs); - static Curve offsetCurve(const Curve& curve, double offset); + static Curve offsetCurve(const Curve& curve, double offset, unsigned order = 0); static Curve joinCurves(const Curve& curve1, const Curve& curve2, unsigned order = 0); diff --git a/src/bezier.cpp b/src/bezier.cpp index 01bbf96..3daa582 100644 --- a/src/bezier.cpp +++ b/src/bezier.cpp @@ -2,10 +2,11 @@ #include "Bezier/utils.h" #include -#include #include +#include #include +#include #include using namespace Bezier; @@ -568,7 +569,7 @@ void Curve::applyContinuity(const Curve& curve, const std::vector& beta_ resetCache(); } -Curve Curve::offsetCurve(const Curve& curve, double offset) +Curve Curve::offsetCurve(const Curve& curve, double offset, unsigned order) { PointVector offset_polyline; if (!curve.cached_polyline_) @@ -577,7 +578,7 @@ Curve Curve::offsetCurve(const Curve& curve, double offset) for (size_t k{}; k < curve.cached_polyline_->size(); k++) offset_polyline.emplace_back((*curve.cached_polyline_)[k] + offset * curve.normalAt((*curve.cached_polyline_t_)[k])); - return fromPolyline(offset_polyline, curve.order() + 1); + return fromPolyline(offset_polyline, order ? order : curve.order() + 1); } Curve Curve::joinCurves(const Curve& curve1, const Curve& curve2, unsigned int order) @@ -593,16 +594,17 @@ Curve Curve::joinCurves(const Curve& curve1, const Curve& curve2, unsigned int o return fromPolyline(polyline, order ? order : curve1.order() + curve2.order()); } -Curve Curve::fromPolyline(const PointVector& polyline, unsigned order) +Curve Curve::fromPolyline(const PointVector& polyline, unsigned int order) { + const unsigned N = std::min(order ? order + 1 : polyline.size(), polyline.size()); + if (polyline.size() < 2) throw std::logic_error{"Polyline must have at least two points."}; - if (order == 1) + if (N == 2) return Curve(PointVector{polyline.front(), polyline.back()}); // Select N points that most influence the shape of the polyline, // either based on a specified order or using the full polyline. - const unsigned N = std::min(order ? order + 1 : polyline.size(), polyline.size()); auto simplified = _polylineSimplify(polyline, N); // Initialize vector t where each element represents a normalized cumulative @@ -642,47 +644,87 @@ Curve Curve::fromPolyline(const PointVector& polyline, unsigned order) return std::fabs(_polylineLength(c.polyline()) - polyline_length); }; - // Define a cost function for optimization, combining the RMSD and length difference, - // with a +1 offset to handle cases where either is zero. - auto costFun = [&rmsd, &length_diff](const Curve& c) { - return (1 + rmsd(c)) * (1 + length_diff(c)); // +1 is for cases when either is zero + struct CostFunctor : public Eigen::DenseFunctor + { + using GetCurveFun = std::function; + using CostFun = std::function; + GetCurveFun getCurve; + CostFun f1, f2; + + CostFunctor(int N, GetCurveFun getCurve, CostFun f1, CostFun f2) + : DenseFunctor(N - 2, 2), getCurve(getCurve), f1(f1), f2(f2) + { + } + + int operator()(const Eigen::VectorXd& x, Eigen::VectorXd& fvec) const + { + auto c = getCurve((Eigen::VectorXd(inputs() + 2) << 0, x, 1).finished()); + fvec(0) = f1(c); + fvec(1) = f2(c); + return 0; + } }; - // Initialize random number generator for random mutations in the t vector. - std::random_device rd; - std::mt19937 gen(rd()); - std::uniform_int_distribution<> idx_randomizer(1, N - 2); + CostFunctor costFun(N, getCurve, rmsd, length_diff); + Eigen::NumericalDiff num_diff(costFun); - // Initialize optimization variables and best_guess - double mutation_std_dev{0.5}; - unsigned no_improvement_counter = 0, no_improvement_threshold = _pow(N, 3); - std::pair best_guess{costFun(getCurve(t)), t}; + Eigen::MatrixXd J(2, N - 2); + Eigen::VectorXd fvec(2), x = t.segment(1, N - 2); + costFun(x, fvec); + num_diff.df(x, J); - int iter{}; - while (mutation_std_dev > _epsilon) + struct ParetoData { - iter++; - do - { - t = best_guess.second; - t(idx_randomizer(gen)) += std::normal_distribution<>(0.0, mutation_std_dev)(gen); - } while (t.minCoeff() < 0.0 || t.maxCoeff() > 1.0 || (t.array().head(N - 1) >= t.array().tail(N - 1)).any()); + Eigen::VectorXd f, x; + Eigen::MatrixXd J; + double alpha; + }; - double new_cost = costFun(getCurve(t)); - if (new_cost < best_guess.first) - { - best_guess = std::make_pair(new_cost, t); - no_improvement_counter = 0; - } - else if (++no_improvement_counter > no_improvement_threshold) + constexpr double alpha_init = 0.1; + constexpr size_t pareto_max_size = 10; + std::vector pareto_front{{fvec, x, J, alpha_init}}; + pareto_front.reserve(2 * pareto_max_size); + + for (bool finished{false}; !finished;) + { + finished = true; + + for (auto& sample : pareto_front) { - mutation_std_dev /= 2; - no_improvement_counter = 0; - no_improvement_threshold = std::max(_pow(N, 2), no_improvement_threshold / 2); + if (sample.alpha < std::sqrt(_epsilon)) + continue; + finished = false; + + do + { + x = sample.x - sample.alpha * (sample.J.row(0) + sample.J.row(1)).transpose().normalized(); + sample.alpha /= 2; + } while (x.minCoeff() < 0.0 || x.maxCoeff() > 1.0 || (x.array().head(N - 3) >= x.array().tail(N - 3)).any()); + + costFun(x, fvec); + if (std::any_of(pareto_front.begin(), pareto_front.end(), + [&fvec](const auto& p) { return (p.f.array() <= fvec.array()).all(); })) + continue; + + num_diff.df(x, J); + pareto_front.push_back({fvec, x, J, 2 * sample.alpha}); } + + std::sort(pareto_front.begin(), pareto_front.end(), [](const auto& p1, const auto& p2) { + return p1.f(0) == p2.f(0) ? p1.f(1) < p2.f(1) : p1.f(0) < p2.f(0); + }); + + double best_f1 = std::numeric_limits::max(); + auto erase_it = std::remove_if(pareto_front.begin(), pareto_front.end(), [&best_f1](const auto& p) { + return p.f(1) <= best_f1 ? (best_f1 = p.f(1), false) : true; + }); + + if (std::distance(erase_it, pareto_front.begin() + pareto_max_size) < 0) + erase_it = pareto_front.begin() + pareto_max_size; + pareto_front.erase(erase_it, pareto_front.end()); } - return getCurve(best_guess.second); + return getCurve((Eigen::VectorXd(N) << 0, pareto_front.front().x, 1).finished()); } void Curve::resetCache() From 19ea06ad67c986ddd3631ae72f5e2cc39b4cd85e Mon Sep 17 00:00:00 2001 From: stribor14 Date: Wed, 18 Sep 2024 18:29:04 +0200 Subject: [PATCH 06/19] A bit of refactoring. --- include/Bezier/declarations.h | 5 --- include/Bezier/utils.h | 7 ++++ src/bezier.cpp | 69 +++++++++++++++-------------------- src/polycurve.cpp | 2 + src/utils.cpp | 3 +- 5 files changed, 41 insertions(+), 45 deletions(-) diff --git a/include/Bezier/declarations.h b/include/Bezier/declarations.h index 5da44c8..f64ae2b 100644 --- a/include/Bezier/declarations.h +++ b/include/Bezier/declarations.h @@ -72,10 +72,5 @@ using Vector = Eigen::Vector2d; * \brief Bounding box class */ using BoundingBox = Eigen::AlignedBox2d; - -/*! - * \brief Precision for numerical methods - */ -const double _epsilon = std::sqrt(std::numeric_limits::epsilon()); } // namespace Bezier #endif // DECLARATIONS_H diff --git a/include/Bezier/utils.h b/include/Bezier/utils.h index f1f01f4..814da5d 100644 --- a/include/Bezier/utils.h +++ b/include/Bezier/utils.h @@ -19,6 +19,12 @@ template inline std::unique_ptr make_unique(Ar namespace Bezier { +namespace Utils +{ +/*! + * \brief Precision for numerical methods + */ +const double _epsilon = std::sqrt(std::numeric_limits::epsilon()); struct _PolynomialRoots : public std::vector { @@ -91,6 +97,7 @@ inline double _polylineDist(const PointVector& polyline, const Point& point) PointVector _polylineSimplify(const PointVector& polyline, unsigned N); +} // namespace Utils } // namespace Bezier #endif // UTILS_H diff --git a/src/bezier.cpp b/src/bezier.cpp index 3daa582..e78941d 100644 --- a/src/bezier.cpp +++ b/src/bezier.cpp @@ -10,13 +10,14 @@ #include using namespace Bezier; +using namespace Bezier::Utils; Curve::Curve(Eigen::MatrixX2d points) : control_points_(std::move(points)), N_(control_points_.rows()) {} Curve::Curve(const PointVector& points) : control_points_(Eigen::Index(points.size()), Eigen::Index(2)), N_(points.size()) { - for (unsigned k = 0; k < N_; k++) + for (unsigned k{}; k < N_; k++) control_points_.row(k) = points[k]; } @@ -34,7 +35,7 @@ unsigned Curve::order() const { return N_ - 1; } PointVector Curve::controlPoints() const { PointVector points(N_); - for (unsigned k = 0; k < N_; k++) + for (unsigned k{}; k < N_; k++) points[k] = control_points_.row(k); return points; } @@ -59,13 +60,8 @@ PointVector Curve::polyline(double flatness) const // we calculate in squared distances flatness *= flatness; - double coeff{1}; - if (N_ < 10) - { - // for N_ == 10, coeff is 0.9922, so we ignore it for higher orders - coeff -= std::exp2(2. - N_); - coeff *= coeff; - } + // for N_ == 10, coeff is 0.9922, so we ignore it for higher orders + const double coeff{N_ >= 10 ? 1 : _pow(1 - std::exp2(2. - N_), 2)}; while (!subcurves.empty()) { @@ -73,7 +69,7 @@ PointVector Curve::polyline(double flatness) const subcurves.pop_back(); const Point& p1 = cp.row(0); const Point& p2 = cp.row(N_ - 1); - Vector u = p2 - p1; + const Vector u = p2 - p1; auto deviation = [&p1, &p2, &u](double x, double y) { Point q(x, y); @@ -112,7 +108,7 @@ double Curve::length(double t) const auto evaluate_chebyshev = [](double t, const Eigen::VectorXd& coeff) { t = 2 * t - 1; double tn{t}, tn_1{1}, res{coeff(0) + coeff(1) * t}; - for (unsigned k = 2; k < coeff.size(); k++) + for (unsigned k{2}; k < coeff.size(); k++) { std::swap(tn_1, tn); tn = 2 * t * tn_1 - tn; @@ -137,7 +133,7 @@ double Curve::length(double t) const }; derivative_cache.head(2) << derivativeAt(1.0).norm(), derivativeAt(0.0).norm(); - for (unsigned k = 2; k <= n; k *= 2) + for (unsigned k{2}; k <= n; k *= 2) updateDerivativeCache(k); Eigen::VectorXd chebyshev; @@ -154,14 +150,14 @@ double Curve::length(double t) const coeff(0) = derivative_cache(0); coeff(n) = derivative_cache(1); - for (unsigned k = 1; k <= log_n; k++) + for (unsigned k{1}; k <= log_n; k++) { auto lin_spaced = Eigen::ArrayXi::LinSpaced(_exp2(k - 1), 0, _exp2(k - 1) - 1); auto index_c = _exp2(log_n + 1 - (k + 1)) + lin_spaced * _exp2(log_n + 1 - k); auto index_dc = _exp2(k - 1) + 1 + lin_spaced; // TODO: make use of slicing & indexing in Eigen3.4 // coeff(index_c) = coeff(N - index_c) = derivative_cache(index_dc) / n; - for (unsigned i = 0; i < lin_spaced.size(); i++) + for (unsigned i{}; i < lin_spaced.size(); i++) coeff(index_c(i)) = coeff(N - index_c(i)) = derivative_cache(index_dc(i)) / n; } @@ -294,7 +290,7 @@ Point Curve::valueAt(double t) const Eigen::MatrixX2d Curve::valueAt(const std::vector& t_vector) const { Eigen::MatrixXd power_basis(t_vector.size(), N_); - for (unsigned k = 0; k < t_vector.size(); k++) + for (unsigned k{}; k < t_vector.size(); k++) power_basis.row(k) = _powSeries(t_vector[k], N_); return power_basis * bernsteinCoeffs(N_) * control_points_; } @@ -334,18 +330,16 @@ Vector Curve::normalAt(double t, bool normalize) const const Curve& Curve::derivative() const { if (!cached_derivative_) - { cached_derivative_ = N_ == 1 ? std::make_unique(PointVector{Point(0, 0)}) : std::make_unique((N_ - 1) * (control_points_.bottomRows(N_ - 1) - control_points_.topRows(N_ - 1))); - } return *cached_derivative_; } const Curve& Curve::derivative(unsigned n) const { auto nth_derivative = this; - for (unsigned k = 0; k < n; k++) + for (unsigned k{}; k < n; k++) nth_derivative = &nth_derivative->derivative(); return *nth_derivative; } @@ -425,7 +419,7 @@ PointVector Curve::intersections(const Curve& curve) const std::sort(t.begin(), t.end()); std::vector subcurves; subcurves.emplace_back(control_points_); - for (unsigned k = 0; k < t.size(); k++) + for (unsigned k{}; k < t.size(); k++) { Eigen::MatrixX2d new_cp = std::move(subcurves.back()); subcurves.pop_back(); @@ -440,8 +434,8 @@ PointVector Curve::intersections(const Curve& curve) const } // create all pairs of subcurves - for (unsigned k = 0; k < subcurves.size(); k++) - for (unsigned i = k + 1; i < subcurves.size(); i++) + for (unsigned k{}; k < subcurves.size(); k++) + for (unsigned i{k + 1}; i < subcurves.size(); i++) subcurve_pairs.emplace_back(subcurves[k], subcurves[i]); } @@ -461,7 +455,7 @@ PointVector Curve::intersections(const Curve& curve) const Point(cp_b.col(0).maxCoeff(), cp_b.col(1).maxCoeff())); if (!bbox1.intersects(bbox2)) - ; // no intersection + continue; // no intersection else if (bbox1.diagonal().norm() < _epsilon) addIntersection(bbox1.center()); else if (bbox2.diagonal().norm() < _epsilon) @@ -494,7 +488,7 @@ double Curve::projectPoint(const Point& point) const Eigen::MatrixXd derivate_polynomial = (bernsteinCoeffs(N_ - 1) * derivative().control_points_); Eigen::VectorXd polynomial_part = Eigen::VectorXd::Zero(curve_polynomial.rows() + derivate_polynomial.rows() - 1); - for (unsigned k = 0; k < curve_polynomial.rows(); k++) + for (unsigned k{}; k < curve_polynomial.rows(); k++) polynomial_part.middleRows(k, derivate_polynomial.rows()) += derivate_polynomial * curve_polynomial.row(k).transpose(); @@ -509,10 +503,7 @@ double Curve::projectPoint(const Point& point) const auto trimmed = _trimZeroes(polynomial); _PolynomialRoots candidates(trimmed.size()); if (trimmed.size() > 1) - { - Eigen::PolynomialSolver poly_solver(trimmed); - poly_solver.realRoots(candidates); - } + Eigen::PolynomialSolver(trimmed).realRoots(candidates); double min_t = (point - valueAt(0.0)).norm() < (point - valueAt(1.0)).norm() ? 0.0 : 1.0; double min_dist = (point - valueAt(min_t)).norm(); @@ -534,8 +525,8 @@ void Curve::applyContinuity(const Curve& curve, const std::vector& beta_ // pascal triangle matrix (binomial coefficients) - rowwise Eigen::MatrixXd pascal_matrix(Eigen::MatrixXd::Zero(c_order + 1, c_order + 1)); pascal_matrix.row(0).setOnes(); - for (unsigned k = 1; k <= c_order; k++) - for (unsigned i = 1; i <= k; i++) + for (unsigned k{1}; k <= c_order; k++) + for (unsigned i{1}; i <= k; i++) pascal_matrix(i, k) = pascal_matrix(i - 1, k - 1) + pascal_matrix(i, k - 1); // inverse of pascal matrix, i.e., pascal matrix with alternating signs - colwise @@ -544,7 +535,7 @@ void Curve::applyContinuity(const Curve& curve, const std::vector& beta_ // https://en.wikipedia.org/wiki/Bell_polynomials -> equivalent to equations of geometric continuity Eigen::MatrixXd bell_matrix(Eigen::MatrixXd::Zero(c_order + 1, c_order + 1)); bell_matrix(0, c_order) = 1; - for (unsigned k = 0; k < c_order; k++) + for (unsigned k{}; k < c_order; k++) bell_matrix.block(1, c_order - k - 1, k + 1, 1) = bell_matrix.block(0, c_order - k, k + 1, k + 1) * pascal_matrix.block(0, k, k + 1, 1) @@ -553,12 +544,12 @@ void Curve::applyContinuity(const Curve& curve, const std::vector& beta_ // diagonal: (N-1)! / (N-k-1)! Eigen::MatrixXd factorial_matrix(Eigen::MatrixXd::Zero(c_order + 1, c_order + 1)); factorial_matrix(0, 0) = 1; - for (unsigned k = 1; k <= c_order; k++) + for (unsigned k{1}; k <= c_order; k++) factorial_matrix(k, k) = factorial_matrix(k - 1, k - 1) * (N_ - k); // derivatives of given curve Eigen::Matrix2Xd derivatives(Eigen::Index(2), Eigen::Index(c_order + 1)); - for (unsigned k = 0; k < c_order + 1; k++) + for (unsigned k{}; k < c_order + 1; k++) derivatives.col(k) = curve.derivative(k).control_points_.bottomRows(1).transpose(); // based on the beta coefficients and geometric continuity equations, calculate new derivatives @@ -575,7 +566,7 @@ Curve Curve::offsetCurve(const Curve& curve, double offset, unsigned order) if (!curve.cached_polyline_) curve.polyline(); offset_polyline.reserve(curve.cached_polyline_->size()); - for (size_t k{}; k < curve.cached_polyline_->size(); k++) + for (unsigned k{}; k < curve.cached_polyline_->size(); k++) offset_polyline.emplace_back((*curve.cached_polyline_)[k] + offset * curve.normalAt((*curve.cached_polyline_t_)[k])); return fromPolyline(offset_polyline, order ? order : curve.order() + 1); @@ -611,7 +602,7 @@ Curve Curve::fromPolyline(const PointVector& polyline, unsigned int order) // distance between consecutive simplified points along the simplified polyline. Eigen::VectorXd t(N); Eigen::MatrixXd P(N, 2), M = bernsteinCoeffs(N); - for (unsigned k = 0; k < N; k++) + for (unsigned k{}; k < N; k++) { P.row(k) = simplified[k]; t(k) = k == 0 ? 0 : t(k - 1) + (P.row(k) - P.row(k - 1)).norm(); @@ -622,7 +613,7 @@ Curve Curve::fromPolyline(const PointVector& polyline, unsigned int order) // by solving the matrix form of the Bezier curve equation. auto getCurve = [&M, &P](const Eigen::VectorXd& t) { Eigen::MatrixXd T(t.size(), t.size()); - for (unsigned k = 0; k < t.size(); k++) + for (unsigned k{}; k < t.size(); k++) T.row(k) = _powSeries(t(k), t.size()); return Curve(M.inverse() * (T.transpose() * T).inverse() * T.transpose() * P); }; @@ -681,7 +672,7 @@ Curve Curve::fromPolyline(const PointVector& polyline, unsigned int order) }; constexpr double alpha_init = 0.1; - constexpr size_t pareto_max_size = 10; + constexpr unsigned pareto_max_size = 10; std::vector pareto_front{{fvec, x, J, alpha_init}}; pareto_front.reserve(2 * pareto_max_size); @@ -751,7 +742,7 @@ Curve::Coeffs Curve::bernsteinCoeffs(unsigned n) bernstein_coeffs_.insert({n, Coeffs::Zero(n, n)}); bernstein_coeffs_[n].diagonal(-1).setLinSpaced(-1, -static_cast(n - 1)); bernstein_coeffs_[n] = bernstein_coeffs_[n].exp(); - for (unsigned k = 0, binomial = 1; k < n; binomial = binomial * (n - k - 1) / (k + 1), k++) + for (unsigned k{}, binomial = 1; k < n; binomial = binomial * (n - k - 1) / (k + 1), k++) bernstein_coeffs_[n].row(k) *= binomial; } return bernstein_coeffs_[n]; @@ -783,7 +774,7 @@ Curve::Coeffs Curve::splittingCoeffsRight(unsigned n, double t) { splitting_coeffs_right_.insert({n, Coeffs::Zero(n, n)}); Curve::Coeffs temp_splitting_coeffs_left = splittingCoeffsLeft(n); - for (unsigned k = 0; k < n; k++) + for (unsigned k{}; k < n; k++) splitting_coeffs_right_[n].block(0, n - 1 - k, n - k, 1) = temp_splitting_coeffs_left.diagonal(-static_cast(k)).reverse(); } @@ -792,7 +783,7 @@ Curve::Coeffs Curve::splittingCoeffsRight(unsigned n, double t) Curve::Coeffs coeffs(Coeffs::Zero(n, n)); Curve::Coeffs temp_splitting_coeffs_left = splittingCoeffsLeft(n, t); - for (unsigned k = 0; k < n; k++) + for (unsigned k{}; k < n; k++) coeffs.block(0, n - 1 - k, n - k, 1) = temp_splitting_coeffs_left.diagonal(-static_cast(k)).reverse(); return coeffs; } diff --git a/src/polycurve.cpp b/src/polycurve.cpp index d626aaf..350bd2c 100644 --- a/src/polycurve.cpp +++ b/src/polycurve.cpp @@ -1,8 +1,10 @@ #include "Bezier/polycurve.h" +#include "Bezier/utils.h" #include using namespace Bezier; +using namespace Bezier::Utils; PolyCurve::PolyCurve(std::deque curves) : curves_(std::move(curves)) {} diff --git a/src/utils.cpp b/src/utils.cpp index 0ff8a46..b3d5c8a 100644 --- a/src/utils.cpp +++ b/src/utils.cpp @@ -3,8 +3,9 @@ #include using namespace Bezier; +using namespace Bezier::Utils; -PointVector Bezier::_polylineSimplify(const PointVector& polyline, unsigned int N) +PointVector Bezier::Utils::_polylineSimplify(const PointVector& polyline, unsigned int N) { if (polyline.size() < 2) throw std::logic_error{"Polyline must have at least two points."}; From b58675d511a42de21aaab093579ef44c5ff890e3 Mon Sep 17 00:00:00 2001 From: stribor14 Date: Fri, 20 Sep 2024 08:45:21 +0200 Subject: [PATCH 07/19] Remove manipulateCurvature method. Run formatter on example. Close #38 --- example/customscene.cpp | 106 ++++++++++++++-------------------- example/customscene.h | 4 +- example/mainwindow.cpp | 13 +---- example/qcurve.cpp | 10 +--- example/qcurve.h | 2 +- example/qgraphicsviewzoom.cpp | 2 +- example/qgraphicsviewzoom.h | 2 +- example/qpolycurve.cpp | 4 +- example/qpolycurve.h | 3 +- include/Bezier/bezier.h | 10 ---- src/bezier.cpp | 31 ---------- 11 files changed, 57 insertions(+), 130 deletions(-) diff --git a/example/customscene.cpp b/example/customscene.cpp index 393a32e..54121af 100644 --- a/example/customscene.cpp +++ b/example/customscene.cpp @@ -77,7 +77,8 @@ void CustomScene::mousePressEvent(QGraphicsSceneMouseEvent* mouseEvent) QPen(Qt::blue))); auto t2 = c_curve->iterateByLength(t1, 50); auto a = c_curve->valueAt(t2); - byLength.insert(curve, addEllipse(QRectF(QPointF(a.x() - 3, a.y() - 3), QSizeF(6, 6)), QPen(Qt::yellow), QBrush(Qt::red, Qt::SolidPattern))); + byLength.insert(curve, addEllipse(QRectF(QPointF(a.x() - 3, a.y() - 3), QSizeF(6, 6)), QPen(Qt::yellow), + QBrush(Qt::red, Qt::SolidPattern))); } if (is_poly) { @@ -90,7 +91,8 @@ void CustomScene::mousePressEvent(QGraphicsSceneMouseEvent* mouseEvent) QPen(Qt::blue))); auto t2 = c_poly->iterateByLength(t1, 50); auto a = c_poly->valueAt(t2); - byLength.insert(curve, addEllipse(QRectF(QPointF(a.x() - 3, a.y() - 3), QSizeF(6, 6)), QPen(Qt::yellow), QBrush(Qt::red, Qt::SolidPattern))); + byLength.insert(curve, addEllipse(QRectF(QPointF(a.x() - 3, a.y() - 3), QSizeF(6, 6)), QPen(Qt::yellow), + QBrush(Qt::red, Qt::SolidPattern))); } } show_projection = true; @@ -145,18 +147,6 @@ void CustomScene::mousePressEvent(QGraphicsSceneMouseEvent* mouseEvent) } if (update_cp) break; - if (is_curve) - { - double t = c_curve->projectPoint(p); - auto pt = c_curve->valueAt(t); - auto ep = c_curve->endPoints(); - if ((pt - p).norm() < 10 && (pt - ep.first).norm() > 20 && (pt - ep.second).norm() > 20) - { - update_curvature = true; - t_to_update = std::make_pair(c_curve, t); - break; - } - } } } } @@ -235,20 +225,6 @@ void CustomScene::mouseMoveEvent(QGraphicsSceneMouseEvent* mouseEvent) } update(); } - if (update_curvature) - { - try - { - t_to_update.first->prepareGeometryChange(); - t_to_update.first->manipulateCurvature(t_to_update.second, p); - update(); - } - catch (char const* err) - { - update_curvature = false; - QMessageBox::warning(nullptr, "Warning", QString().sprintf("%s", err)); - } - } QGraphicsScene::mouseMoveEvent(mouseEvent); } @@ -273,10 +249,7 @@ void CustomScene::mouseReleaseEvent(QGraphicsSceneMouseEvent* mouseEvent) } } if (mouseEvent->button() == Qt::LeftButton) - { update_cp = false; - update_curvature = false; - } } void CustomScene::keyPressEvent(QKeyEvent* keyEvent) @@ -290,7 +263,6 @@ Ctrl + Scroll - zoom in/out\n\ Ctrl + Left click - select curves\n\ Left click - deselect curves\n\ Left click + drag control point - manipulate control point\n\ -Left click + drag curve - manipulate curve (only for 2nd and 3rd order)\n\ \n\ Keyboard shortcuts:\n\ H - display help\n\ @@ -377,43 +349,53 @@ Delete - delete curve/polycurve"); } update(); } - if (keyEvent->key() == Qt::Key_L) { - for (auto&& curve : selectedItems()) { - if (is_curve) { - c_curve->setLocked(!c_curve->getLocked()); - } + if (keyEvent->key() == Qt::Key_L) + { + for (auto&& curve : selectedItems()) + { + if (is_curve) + { + c_curve->setLocked(!c_curve->getLocked()); } - update(); + } + update(); } - if (keyEvent->key() == Qt::Key_Enter || keyEvent->key() == Qt::Key_Return) { - if (selectedItems().size() != 2) { - return; - } + if (keyEvent->key() == Qt::Key_Enter || keyEvent->key() == Qt::Key_Return) + { + if (selectedItems().size() != 2) + { + return; + } - qCurve* curve_locked = nullptr; - qCurve* curve_unlocked = nullptr; + qCurve* curve_locked = nullptr; + qCurve* curve_unlocked = nullptr; - if (c_curve_item(selectedItems().first())->getLocked()) { - curve_locked = c_curve_item(selectedItems().first()); - curve_unlocked = c_curve_item(selectedItems().last()); + if (c_curve_item(selectedItems().first())->getLocked()) + { + curve_locked = c_curve_item(selectedItems().first()); + curve_unlocked = c_curve_item(selectedItems().last()); + } + else if (c_curve_item(selectedItems().last())->getLocked()) + { + curve_locked = c_curve_item(selectedItems().last()); + curve_unlocked = c_curve_item(selectedItems().first()); + } + + if (curve_locked != nullptr && curve_unlocked != nullptr) + { + while (curve_locked->order() < 5) + { + curve_locked->elevateOrder(); } - else if (c_curve_item(selectedItems().last())->getLocked()) { - curve_locked = c_curve_item(selectedItems().last()); - curve_unlocked = c_curve_item(selectedItems().first()); + while (curve_unlocked->order() < 5) + { + curve_unlocked->elevateOrder(); } - if (curve_locked != nullptr && curve_unlocked != nullptr) { - while (curve_locked->order() < 5) { - curve_locked->elevateOrder(); - } - while (curve_unlocked->order() < 5) { - curve_unlocked->elevateOrder(); - } - - std::vector beta_coeffs = {1, 0, 0}; - curve_unlocked->applyContinuity(*dynamic_cast(curve_locked), beta_coeffs); - update(); - } + std::vector beta_coeffs = {1, 0, 0}; + curve_unlocked->applyContinuity(*dynamic_cast(curve_locked), beta_coeffs); + update(); + } } // if (keyEvent->key() >= 48 && keyEvent->key() <= 59) // num keys diff --git a/example/customscene.h b/example/customscene.h index 0288c02..58dc991 100644 --- a/example/customscene.h +++ b/example/customscene.h @@ -4,8 +4,8 @@ #include #include "qcurve.h" -#include "qpolycurve.h" #include "qgraphicsviewzoom.h" +#include "qpolycurve.h" namespace Ui { @@ -28,8 +28,6 @@ class CustomScene : public QGraphicsScene bool draw_box_ = false; bool draw_inter_ = false; bool show_projection = false; - bool update_curvature = false; - std::pair t_to_update; bool update_cp = false; std::pair cp_to_update; diff --git a/example/mainwindow.cpp b/example/mainwindow.cpp index cdd27ff..f1a4369 100644 --- a/example/mainwindow.cpp +++ b/example/mainwindow.cpp @@ -13,16 +13,9 @@ MainWindow::MainWindow(QWidget* parent) : QMainWindow(parent), ui(new Ui::MainWi Eigen::MatrixX2d cp1, cp2; cp1.resize(4, 2); cp2.resize(5, 2); - cp1 << 84, 162, - 246, 30, - 48, 236, - 180, 110; - - cp2 << 180, 110, - 175, 160, - 60, 48, - 164, 165, - 124, 134; + cp1 << 84, 162, 246, 30, 48, 236, 180, 110; + + cp2 << 180, 110, 175, 160, 60, 48, 164, 165, 124, 134; scene->addItem(new qCurve(cp1 * 5)); scene->addItem(new qCurve(cp2 * 5)); diff --git a/example/qcurve.cpp b/example/qcurve.cpp index b6dd19a..d661d0c 100644 --- a/example/qcurve.cpp +++ b/example/qcurve.cpp @@ -11,15 +11,9 @@ bool qCurve::getDraw_control_points() const { return draw_control_points; } bool qCurve::getDraw_curvature_radious() const { return draw_curvature_radious; } -bool qCurve::getLocked() const -{ - return locked; -} +bool qCurve::getLocked() const { return locked; } -void qCurve::setLocked(bool value) -{ - locked = value; -} +void qCurve::setLocked(bool value) { locked = value; } int qCurve::type() const { return QGraphicsItem::UserType + 1; } diff --git a/example/qcurve.h b/example/qcurve.h index 23efadc..22f6890 100644 --- a/example/qcurve.h +++ b/example/qcurve.h @@ -3,8 +3,8 @@ #include -#include "Bezier/declarations.h" #include "Bezier/bezier.h" +#include "Bezier/declarations.h" class qCurve : public QGraphicsItem, public Bezier::Curve { diff --git a/example/qgraphicsviewzoom.cpp b/example/qgraphicsviewzoom.cpp index ba114a3..9ac3bfc 100644 --- a/example/qgraphicsviewzoom.cpp +++ b/example/qgraphicsviewzoom.cpp @@ -1,7 +1,7 @@ #include "qgraphicsviewzoom.h" -#include #include +#include #include #include diff --git a/example/qgraphicsviewzoom.h b/example/qgraphicsviewzoom.h index ea39aaf..f234c01 100644 --- a/example/qgraphicsviewzoom.h +++ b/example/qgraphicsviewzoom.h @@ -1,9 +1,9 @@ #ifndef QGRAPHICSVIEWZOOM_H #define QGRAPHICSVIEWZOOM_H -#include #include #include +#include /*! * \brief This class adds ability to zoom QGraphicsView using mouse wheel. The point under cursor diff --git a/example/qpolycurve.cpp b/example/qpolycurve.cpp index 80c3702..0aaef65 100644 --- a/example/qpolycurve.cpp +++ b/example/qpolycurve.cpp @@ -55,8 +55,8 @@ void qPolyCurve::paint(QPainter* painter, const QStyleOptionGraphicsItem* option painter->setPen(Qt::green); for (double t = 0; t <= size(); t += 1.0 / 500) { - painter->setPen(QColor(static_cast(std::fabs(255 * (0.5 - t / size()))), - static_cast(255 * t / size()), static_cast(255 * (1 - t / size())))); + painter->setPen(QColor(static_cast(std::fabs(255 * (0.5 - t / size()))), static_cast(255 * t / size()), + static_cast(255 * (1 - t / size())))); auto p = valueAt(t); auto n1 = p + normalAt(t, false) * curvatureDerivativeAt(t); auto n2 = p - normalAt(t, false) * curvatureDerivativeAt(t); diff --git a/example/qpolycurve.h b/example/qpolycurve.h index 0b0529c..294fea2 100644 --- a/example/qpolycurve.h +++ b/example/qpolycurve.h @@ -3,15 +3,16 @@ #include +#include "Bezier/bezier.h" #include "Bezier/declarations.h" #include "Bezier/polycurve.h" -#include "Bezier/bezier.h" class qPolyCurve : public QGraphicsItem, public Bezier::PolyCurve { private: bool draw_control_points = false; bool draw_curvature_radious = false; + public: qPolyCurve(const std::deque& curve_list) : QGraphicsItem(), Bezier::PolyCurve(curve_list) {} qPolyCurve(const Bezier::Curve& curve) : QGraphicsItem(), Bezier::PolyCurve(std::deque{curve}) {} diff --git a/include/Bezier/bezier.h b/include/Bezier/bezier.h index 33df198..d9e9744 100644 --- a/include/Bezier/bezier.h +++ b/include/Bezier/bezier.h @@ -127,16 +127,6 @@ class Curve */ void setControlPoint(unsigned idx, const Point& point); - /*! - * \brief Manipulate the curve so that it passes through wanted point for given 't' - * \param t Curve parameter - * \param point Point where curve should pass for a given t - * - * \warning CAN THROW: Only works for quadratic and cubic curves - * \warning Resets cached data - */ - void manipulateCurvature(double t, const Point& point); - /*! * \brief Raise the curve order by 1 * diff --git a/src/bezier.cpp b/src/bezier.cpp index e78941d..46afa22 100644 --- a/src/bezier.cpp +++ b/src/bezier.cpp @@ -235,37 +235,6 @@ void Curve::setControlPoint(unsigned idx, const Point& point) resetCache(); } -void Curve::manipulateCurvature(double t, const Point& point) -{ - if (N_ < 3 || N_ > 4) - throw std::logic_error{"Only quadratic and cubic curves can be manipulated"}; - - double r = std::fabs((_pow(t, N_ - 1) + _pow(1 - t, N_ - 1) - 1) / (_pow(t, N_ - 1) + _pow(1 - t, N_ - 1))); - double u = _pow(1 - t, N_ - 1) / (_pow(t, N_ - 1) + _pow(1 - t, N_ - 1)); - Point C = u * control_points_.row(0) + (1 - u) * control_points_.row(N_ - 1); - const Point& B = point; - Point A = B - (C - B) / r; - - switch (N_) - { - case 3: - control_points_.row(1) = A; - break; - case 4: - Point e1 = control_points_.row(0) * _pow(1 - t, 2) + control_points_.row(1) * 2 * t * (1 - t) + - control_points_.row(2) * _pow(t, 2); - Point e2 = control_points_.row(1) * _pow(1 - t, 2) + control_points_.row(2) * 2 * t * (1 - t) + - control_points_.row(3) * _pow(t, 2); - e1 = B + e1 - valueAt(t); - e2 = B + e2 - valueAt(t); - Point v1 = A - (A - e1) / (1 - t); - Point v2 = A + (e2 - A) / t; - control_points_.row(1).noalias() = control_points_.row(0) + (v1.transpose() - control_points_.row(0)) / t; - control_points_.row(2).noalias() = control_points_.row(3) - (control_points_.row(3) - v2.transpose()) / (1 - t); - } - resetCache(); -} - void Curve::elevateOrder() { control_points_ = elevateOrderCoeffs(N_) * control_points_; From 325b068a57391fb4d2b35aefad599ae2f77a0cb6 Mon Sep 17 00:00:00 2001 From: stribor14 Date: Fri, 20 Sep 2024 10:44:11 +0200 Subject: [PATCH 08/19] Remove macro ifs for c++17 features --- include/Bezier/utils.h | 11 ----------- src/bezier.cpp | 9 --------- 2 files changed, 20 deletions(-) diff --git a/include/Bezier/utils.h b/include/Bezier/utils.h index 814da5d..c17c4e8 100644 --- a/include/Bezier/utils.h +++ b/include/Bezier/utils.h @@ -6,17 +6,6 @@ #include "declarations.h" -#ifndef __cpp_lib_make_unique -#include -namespace std -{ -template inline std::unique_ptr make_unique(Args&&... args) -{ - return std::unique_ptr(new T(std::forward(args)...)); -} -} // namespace std -#endif - namespace Bezier { namespace Utils diff --git a/src/bezier.cpp b/src/bezier.cpp index 46afa22..dedfb2d 100644 --- a/src/bezier.cpp +++ b/src/bezier.cpp @@ -395,11 +395,7 @@ PointVector Curve::intersections(const Curve& curve) const subcurves.emplace_back(splittingCoeffsLeft(N_, t[k] - _epsilon / 2) * new_cp); subcurves.emplace_back(splittingCoeffsRight(N_, t[k] + _epsilon / 2) * new_cp); -#if __cpp_init_captures std::for_each(t.begin() + k + 1, t.end(), [t = t[k]](double& x) { x = (x - t) / (1 - t); }); -#else - std::for_each(t.begin() + k + 1, t.end(), [&t, k](double& x) { x = (x - t[k]) / (1 - t[k]); }); -#endif } // create all pairs of subcurves @@ -410,12 +406,7 @@ PointVector Curve::intersections(const Curve& curve) const while (!subcurve_pairs.empty()) { -#if __cpp_structured_bindings auto [cp_a, cp_b] = std::move(subcurve_pairs.back()); -#else - Eigen::MatrixX2d cp_a, cp_b; - std::tie(cp_a, cp_b) = std::move(subcurve_pairs.back()); -#endif subcurve_pairs.pop_back(); BoundingBox bbox1(Point(cp_a.col(0).minCoeff(), cp_a.col(1).minCoeff()), From 840726050be1e87238fe2087dd2c66c8729dad68 Mon Sep 17 00:00:00 2001 From: stribor14 Date: Tue, 1 Oct 2024 12:25:51 +0200 Subject: [PATCH 09/19] Use optional instead of dynamic unique_ptr for cache --- include/Bezier/bezier.h | 19 +++++++------- src/bezier.cpp | 58 ++++++++++++++++------------------------- 2 files changed, 33 insertions(+), 44 deletions(-) diff --git a/include/Bezier/bezier.h b/include/Bezier/bezier.h index d9e9744..343646c 100644 --- a/include/Bezier/bezier.h +++ b/include/Bezier/bezier.h @@ -19,6 +19,7 @@ #include #include +#include #include "declarations.h" @@ -301,17 +302,17 @@ class Curve using CoeffsMap = std::map; // private caching - mutable std::unique_ptr cached_derivative_; /*! If generated, stores derivative for later use */ - mutable std::unique_ptr> cached_roots_; /*! If generated, stores roots for later use */ - mutable std::unique_ptr cached_bounding_box_; /*! If generated, stores bounding box for later use */ - mutable std::unique_ptr cached_polyline_; /*! If generated, stores polyline for later use */ - mutable std::unique_ptr> cached_polyline_t_; /*! If generated, stores polyline t for later use */ - mutable double cached_polyline_flatness_{}; /*! Flatness of cached polyline */ - mutable std::unique_ptr + mutable std::unique_ptr cached_derivative_; /*! If generated, stores derivative for later use */ + mutable std::optional> cached_roots_; /*! If generated, stores roots for later use */ + mutable std::optional cached_bounding_box_; /*! If generated, stores bounding box for later use */ + mutable std::optional cached_polyline_; /*! If generated, stores polyline for later use */ + mutable std::optional> cached_polyline_t_; /*! If generated, stores polyline t for later use */ + mutable double cached_polyline_flatness_{}; /*! Flatness of cached polyline */ + mutable std::optional cached_projection_polynomial_part_; /*! Constant part of point projection polynomial */ - mutable Eigen::MatrixXd + mutable std::optional cached_projection_polynomial_derivative_; /*! Polynomial representation of the curve derivative */ - mutable std::unique_ptr cached_chebyshev_coeffs_; /*! If generated, stores chebyshev coefficients + mutable std::optional cached_chebyshev_coeffs_; /*! If generated, stores chebyshev coefficients for calculating the length of the curve */ // static caching diff --git a/src/bezier.cpp b/src/bezier.cpp index dedfb2d..add9c87 100644 --- a/src/bezier.cpp +++ b/src/bezier.cpp @@ -49,10 +49,8 @@ PointVector Curve::polyline(double flatness) const if (!cached_polyline_ || cached_polyline_flatness_ != flatness) { cached_polyline_flatness_ = flatness; - cached_polyline_ = std::make_unique(); - cached_polyline_->emplace_back(control_points_.row(0)); - cached_polyline_t_ = std::make_unique>(); - cached_polyline_t_->emplace_back(0.0); + cached_polyline_.emplace({Point(control_points_.row(0))}); + cached_polyline_t_.emplace({0.0}); std::vector> subcurves; subcurves.emplace_back(control_points_, 0.0, 1.0); @@ -95,7 +93,7 @@ PointVector Curve::polyline(double flatness) const } } - return *cached_polyline_; + return cached_polyline_.value(); } double Curve::length() const { return length(1.0); } @@ -169,11 +167,11 @@ double Curve::length(double t) const unsigned cut = 0; while (std::fabs(chebyshev(cut)) > _epsilon * 1e-2) cut++; - cached_chebyshev_coeffs_ = std::make_unique(cut + 1); - (*cached_chebyshev_coeffs_) << 0, chebyshev.head(cut); - (*cached_chebyshev_coeffs_)(0) = -evaluate_chebyshev(0, *cached_chebyshev_coeffs_); + cached_chebyshev_coeffs_.emplace(cut + 1); + cached_chebyshev_coeffs_.value() << 0, chebyshev.head(cut); + cached_chebyshev_coeffs_.value()(0) = -evaluate_chebyshev(0, cached_chebyshev_coeffs_.value()); } - return evaluate_chebyshev(t, *cached_chebyshev_coeffs_); + return evaluate_chebyshev(t, cached_chebyshev_coeffs_.value()); } double Curve::length(double t1, double t2) const { return length(t2) - length(t1); } @@ -320,29 +318,19 @@ Vector Curve::derivativeAt(unsigned n, double t) const { return derivative(n).va std::vector Curve::roots() const { if (!cached_roots_) - { - cached_roots_ = std::make_unique>(); - if (N_ > 1) - { + cached_roots_ = N_ < 2 ? std::vector{} : [this] { Eigen::MatrixXd bezier_polynomial = bernsteinCoeffs(N_) * control_points_; - Eigen::PolynomialSolver poly_solver; auto trimmed_x = _trimZeroes(bezier_polynomial.col(0)); auto trimmed_y = _trimZeroes(bezier_polynomial.col(1)); _PolynomialRoots roots(trimmed_x.size() + trimmed_y.size()); if (trimmed_x.size() > 1) - { - poly_solver.compute(trimmed_x); - poly_solver.realRoots(roots); - } + Eigen::PolynomialSolver(trimmed_x).realRoots(roots); if (trimmed_y.size() > 1) - { - poly_solver.compute(trimmed_y); - poly_solver.realRoots(roots); - } - std::swap(roots, *cached_roots_); - } - } - return *cached_roots_; + Eigen::PolynomialSolver(trimmed_y).realRoots(roots); + return roots; + }(); + + return cached_roots_.value(); } std::vector Curve::extrema() const { return derivative().roots(); } @@ -356,10 +344,10 @@ BoundingBox Curve::boundingBox() const extremes.row(extremes.rows() - 1) = control_points_.row(0); extremes.row(extremes.rows() - 2) = control_points_.row(N_ - 1); - cached_bounding_box_ = std::make_unique(Point(extremes.col(0).minCoeff(), extremes.col(1).minCoeff()), - Point(extremes.col(0).maxCoeff(), extremes.col(1).maxCoeff())); + cached_bounding_box_.emplace(Point(extremes.col(0).minCoeff(), extremes.col(1).minCoeff()), + Point(extremes.col(0).maxCoeff(), extremes.col(1).maxCoeff())); } - return *cached_bounding_box_; + return cached_bounding_box_.value(); } std::pair Curve::splitCurve(double t) const @@ -442,7 +430,7 @@ PointVector Curve::intersections(const Curve& curve) const double Curve::projectPoint(const Point& point) const { - if (!cached_projection_polynomial_part_) + if (!cached_projection_polynomial_part_ || !cached_projection_polynomial_derivative_) { Eigen::MatrixXd curve_polynomial = (bernsteinCoeffs(N_) * control_points_); Eigen::MatrixXd derivate_polynomial = (bernsteinCoeffs(N_ - 1) * derivative().control_points_); @@ -452,13 +440,13 @@ double Curve::projectPoint(const Point& point) const polynomial_part.middleRows(k, derivate_polynomial.rows()) += derivate_polynomial * curve_polynomial.row(k).transpose(); - cached_projection_polynomial_part_ = std::make_unique(std::move(polynomial_part)); - cached_projection_polynomial_derivative_ = std::move(derivate_polynomial); + cached_projection_polynomial_part_.emplace(std::move(polynomial_part)); + cached_projection_polynomial_derivative_.emplace(std::move(derivate_polynomial)); } - Eigen::VectorXd polynomial = *cached_projection_polynomial_part_; - polynomial.topRows(cached_projection_polynomial_derivative_.rows()) -= - cached_projection_polynomial_derivative_ * point; + Eigen::VectorXd polynomial = cached_projection_polynomial_part_.value(); + polynomial.topRows(cached_projection_polynomial_derivative_->rows()) -= + cached_projection_polynomial_derivative_.value() * point; auto trimmed = _trimZeroes(polynomial); _PolynomialRoots candidates(trimmed.size()); From a95248288b88e13be3215d4ced5676998dd59de8 Mon Sep 17 00:00:00 2001 From: stribor14 Date: Tue, 1 Oct 2024 13:59:38 +0200 Subject: [PATCH 10/19] Rework polynomial solving and refactor a bit --- include/Bezier/utils.h | 22 ++++++---------------- src/bezier.cpp | 25 ++++--------------------- src/utils.cpp | 23 +++++++++++++++++++++++ 3 files changed, 33 insertions(+), 37 deletions(-) diff --git a/include/Bezier/utils.h b/include/Bezier/utils.h index c17c4e8..3245bfb 100644 --- a/include/Bezier/utils.h +++ b/include/Bezier/utils.h @@ -15,17 +15,6 @@ namespace Utils */ const double _epsilon = std::sqrt(std::numeric_limits::epsilon()); -struct _PolynomialRoots : public std::vector -{ - explicit _PolynomialRoots(unsigned reserve) { std::vector::reserve(reserve); } - void clear() {} // no-op so that PolynomialSolver::RealRoots() doesn't clear it - void push_back(double t) // only allow valid roots - { - if (t >= 0 && t <= 1) - std::vector::push_back(t); - } -}; - inline unsigned _exp2(unsigned exp) { return 1 << exp; } template inline T _pow(T base, unsigned exp) @@ -49,12 +38,11 @@ inline Eigen::RowVectorXd _powSeries(double base, unsigned exp) return power_series; } -inline Eigen::VectorXd _trimZeroes(const Eigen::VectorXd& vec) +template inline std::vector _concatenate(std::vector&& v1, std::vector&& v2) { - auto idx = vec.size(); - while (idx && std::abs(vec(idx - 1)) < _epsilon) - --idx; - return vec.head(idx); + v1.reserve(v1.size() + v2.size()); + v1.insert(v1.end(), std::make_move_iterator(v2.begin()), std::make_move_iterator(v2.end())); + return std::move(v1); } inline double _polylineLength(const PointVector& polyline) @@ -86,6 +74,8 @@ inline double _polylineDist(const PointVector& polyline, const Point& point) PointVector _polylineSimplify(const PointVector& polyline, unsigned N); +std::vector _solvePolynomial(const Eigen::VectorXd& polynomial); + } // namespace Utils } // namespace Bezier diff --git a/src/bezier.cpp b/src/bezier.cpp index add9c87..4b0539f 100644 --- a/src/bezier.cpp +++ b/src/bezier.cpp @@ -7,7 +7,6 @@ #include #include #include -#include using namespace Bezier; using namespace Bezier::Utils; @@ -320,14 +319,7 @@ std::vector Curve::roots() const if (!cached_roots_) cached_roots_ = N_ < 2 ? std::vector{} : [this] { Eigen::MatrixXd bezier_polynomial = bernsteinCoeffs(N_) * control_points_; - auto trimmed_x = _trimZeroes(bezier_polynomial.col(0)); - auto trimmed_y = _trimZeroes(bezier_polynomial.col(1)); - _PolynomialRoots roots(trimmed_x.size() + trimmed_y.size()); - if (trimmed_x.size() > 1) - Eigen::PolynomialSolver(trimmed_x).realRoots(roots); - if (trimmed_y.size() > 1) - Eigen::PolynomialSolver(trimmed_y).realRoots(roots); - return roots; + return _concatenate(_solvePolynomial(bezier_polynomial.col(0)), _solvePolynomial(bezier_polynomial.col(1))); }(); return cached_roots_.value(); @@ -448,14 +440,10 @@ double Curve::projectPoint(const Point& point) const polynomial.topRows(cached_projection_polynomial_derivative_->rows()) -= cached_projection_polynomial_derivative_.value() * point; - auto trimmed = _trimZeroes(polynomial); - _PolynomialRoots candidates(trimmed.size()); - if (trimmed.size() > 1) - Eigen::PolynomialSolver(trimmed).realRoots(candidates); - double min_t = (point - valueAt(0.0)).norm() < (point - valueAt(1.0)).norm() ? 0.0 : 1.0; double min_dist = (point - valueAt(min_t)).norm(); + auto candidates = _solvePolynomial(polynomial); return std::accumulate(candidates.begin(), candidates.end(), std::make_pair(min_t, min_dist), [this, &point](std::pair min, double t) { double dist = (point - valueAt(t)).norm(); @@ -524,13 +512,8 @@ Curve Curve::joinCurves(const Curve& curve1, const Curve& curve2, unsigned int o { if (order == 1) return Curve(PointVector{curve1.control_points_.row(0), curve2.control_points_.row(curve2.N_ - 1)}); - - auto polyline = curve1.polyline(); - auto polyline2 = curve2.polyline(); - polyline.reserve(polyline.size() + polyline2.size()); - polyline.insert(polyline.end(), std::make_move_iterator(polyline2.begin()), std::make_move_iterator(polyline2.end())); - - return fromPolyline(polyline, order ? order : curve1.order() + curve2.order()); + return fromPolyline(_concatenate(curve1.polyline(), curve2.polyline()), + order ? order : curve1.order() + curve2.order()); } Curve Curve::fromPolyline(const PointVector& polyline, unsigned int order) diff --git a/src/utils.cpp b/src/utils.cpp index b3d5c8a..800cfdd 100644 --- a/src/utils.cpp +++ b/src/utils.cpp @@ -2,6 +2,8 @@ #include +#include + using namespace Bezier; using namespace Bezier::Utils; @@ -72,3 +74,24 @@ PointVector Bezier::Utils::_polylineSimplify(const PointVector& polyline, unsign simplified.push_back(polyline[k]); return simplified; } + +std::vector Bezier::Utils::_solvePolynomial(const Eigen::VectorXd& polynomial) +{ + auto idx = polynomial.size(); + while (idx && std::abs(polynomial(idx - 1)) < _epsilon) + --idx; + + struct PolynomialRoots : public std::vector + { + void push_back(double t) // only allow valid roots + { + if (t >= 0 && t <= 1) + std::vector::push_back(t); + } + } roots; + roots.reserve(idx); + + if (idx > 1) + Eigen::PolynomialSolver(polynomial.head(idx)).realRoots(roots); + return roots; +} From cd6edb6be65a72bfd3a3edfe1d45288b30cc32ea Mon Sep 17 00:00:00 2001 From: stribor14 Date: Tue, 1 Oct 2024 14:34:25 +0200 Subject: [PATCH 11/19] Remove leading underscore and utilize namespace for helper functions --- include/Bezier/utils.h | 18 +++++------ src/bezier.cpp | 73 +++++++++++++++++++++--------------------- src/polycurve.cpp | 12 +++---- src/utils.cpp | 16 +++++---- 4 files changed, 61 insertions(+), 58 deletions(-) diff --git a/include/Bezier/utils.h b/include/Bezier/utils.h index 3245bfb..647bb86 100644 --- a/include/Bezier/utils.h +++ b/include/Bezier/utils.h @@ -13,11 +13,11 @@ namespace Utils /*! * \brief Precision for numerical methods */ -const double _epsilon = std::sqrt(std::numeric_limits::epsilon()); +const double epsilon = std::sqrt(std::numeric_limits::epsilon()); -inline unsigned _exp2(unsigned exp) { return 1 << exp; } +inline unsigned exp2(unsigned exp) { return 1 << exp; } -template inline T _pow(T base, unsigned exp) +template inline T pow(T base, unsigned exp) { T result = exp & 1 ? base : 1; while (exp >>= 1) @@ -29,7 +29,7 @@ template inline T _pow(T base, unsigned exp) return result; } -inline Eigen::RowVectorXd _powSeries(double base, unsigned exp) +inline Eigen::RowVectorXd powSeries(double base, unsigned exp) { Eigen::RowVectorXd power_series(exp); power_series(0) = 1; @@ -38,14 +38,14 @@ inline Eigen::RowVectorXd _powSeries(double base, unsigned exp) return power_series; } -template inline std::vector _concatenate(std::vector&& v1, std::vector&& v2) +template inline std::vector concatenate(std::vector&& v1, std::vector&& v2) { v1.reserve(v1.size() + v2.size()); v1.insert(v1.end(), std::make_move_iterator(v2.begin()), std::make_move_iterator(v2.end())); return std::move(v1); } -inline double _polylineLength(const PointVector& polyline) +inline double polylineLength(const PointVector& polyline) { double length{}; for (size_t k{1}; k < polyline.size(); k++) @@ -53,7 +53,7 @@ inline double _polylineLength(const PointVector& polyline) return length; } -inline double _polylineDist(const PointVector& polyline, const Point& point) +inline double polylineDist(const PointVector& polyline, const Point& point) { auto distSeg = [&point](const Point& p1, const Point& p2) { Vector u = p2 - p1; @@ -72,9 +72,9 @@ inline double _polylineDist(const PointVector& polyline, const Point& point) return dist; } -PointVector _polylineSimplify(const PointVector& polyline, unsigned N); +PointVector polylineSimplify(const PointVector& polyline, unsigned N); -std::vector _solvePolynomial(const Eigen::VectorXd& polynomial); +std::vector solvePolynomial(const Eigen::VectorXd& polynomial); } // namespace Utils } // namespace Bezier diff --git a/src/bezier.cpp b/src/bezier.cpp index 4b0539f..aa499f7 100644 --- a/src/bezier.cpp +++ b/src/bezier.cpp @@ -9,7 +9,7 @@ #include using namespace Bezier; -using namespace Bezier::Utils; +namespace bu = Bezier::Utils; Curve::Curve(Eigen::MatrixX2d points) : control_points_(std::move(points)), N_(control_points_.rows()) {} @@ -58,7 +58,7 @@ PointVector Curve::polyline(double flatness) const flatness *= flatness; // for N_ == 10, coeff is 0.9922, so we ignore it for higher orders - const double coeff{N_ >= 10 ? 1 : _pow(1 - std::exp2(2. - N_), 2)}; + const double coeff{N_ >= 10 ? 1 : bu::pow(1 - std::exp2(2. - N_), 2)}; while (!subcurves.empty()) { @@ -118,7 +118,7 @@ double Curve::length(double t) const { constexpr unsigned START_LOG_N = 10; unsigned log_n = START_LOG_N - 1; - unsigned n = _exp2(START_LOG_N - 1); + unsigned n = bu::exp2(START_LOG_N - 1); Eigen::VectorXd derivative_cache(2 * n + 1); auto updateDerivativeCache = [this, &derivative_cache](double n) { @@ -149,9 +149,9 @@ double Curve::length(double t) const for (unsigned k{1}; k <= log_n; k++) { - auto lin_spaced = Eigen::ArrayXi::LinSpaced(_exp2(k - 1), 0, _exp2(k - 1) - 1); - auto index_c = _exp2(log_n + 1 - (k + 1)) + lin_spaced * _exp2(log_n + 1 - k); - auto index_dc = _exp2(k - 1) + 1 + lin_spaced; + auto lin_spaced = Eigen::ArrayXi::LinSpaced(bu::exp2(k - 1), 0, bu::exp2(k - 1) - 1); + auto index_c = bu::exp2(log_n + 1 - (k + 1)) + lin_spaced * bu::exp2(log_n + 1 - k); + auto index_dc = bu::exp2(k - 1) + 1 + lin_spaced; // TODO: make use of slicing & indexing in Eigen3.4 // coeff(index_c) = coeff(N - index_c) = derivative_cache(index_dc) / n; for (unsigned i{}; i < lin_spaced.size(); i++) @@ -161,10 +161,10 @@ double Curve::length(double t) const fft.fwd(fft_out, coeff); chebyshev = (fft_out.real().head(n - 1) - fft_out.real().segment(2, n - 1)).array() / Eigen::ArrayXd::LinSpaced(n - 1, 4, 4 * (n - 1)); - } while (std::fabs(chebyshev.tail<1>()[0]) > _epsilon * 1e-2); + } while (std::fabs(chebyshev.tail<1>()[0]) > bu::epsilon * 1e-2); unsigned cut = 0; - while (std::fabs(chebyshev(cut)) > _epsilon * 1e-2) + while (std::fabs(chebyshev(cut)) > bu::epsilon * 1e-2) cut++; cached_chebyshev_coeffs_.emplace(cut + 1); cached_chebyshev_coeffs_.value() << 0, chebyshev.head(cut); @@ -177,7 +177,7 @@ double Curve::length(double t1, double t2) const { return length(t2) - length(t1 double Curve::iterateByLength(double t, double s) const { - if (std::fabs(s) < _epsilon) // no-op + if (std::fabs(s) < bu::epsilon) // no-op return t; double s_t = length(t); @@ -186,19 +186,19 @@ double Curve::iterateByLength(double t, double s) const if (s < 0) { lbracket = {0.0, -s_t}; - if (s < lbracket.second + _epsilon) // out-of-scope + if (s < lbracket.second + bu::epsilon) // out-of-scope return 0.0; rbracket = guess; } else // s > 0 { rbracket = {1.0, length() - s_t}; - if (s > rbracket.second - _epsilon) // out-of-scope + if (s > rbracket.second - bu::epsilon) // out-of-scope return 1.0; lbracket = guess; } - while (std::fabs(guess.second - s) > _epsilon) + while (std::fabs(guess.second - s) > bu::epsilon) { // Halley's method double f = guess.second - s; @@ -210,7 +210,7 @@ double Curve::iterateByLength(double t, double s) const if (guess.first <= lbracket.first || guess.first >= rbracket.first) guess.first = (lbracket.first + rbracket.first) / 2; - if (rbracket.first - lbracket.first < _epsilon) + if (rbracket.first - lbracket.first < bu::epsilon) break; guess.second = length(guess.first) - s_t; @@ -250,14 +250,14 @@ Point Curve::valueAt(double t) const { if (N_ == 0) return {0, 0}; - return (_powSeries(t, N_) * bernsteinCoeffs(N_) * control_points_).transpose(); + return (bu::powSeries(t, N_) * bernsteinCoeffs(N_) * control_points_).transpose(); } Eigen::MatrixX2d Curve::valueAt(const std::vector& t_vector) const { Eigen::MatrixXd power_basis(t_vector.size(), N_); for (unsigned k{}; k < t_vector.size(); k++) - power_basis.row(k) = _powSeries(t_vector[k], N_); + power_basis.row(k) = bu::powSeries(t_vector[k], N_); return power_basis * bernsteinCoeffs(N_) * control_points_; } @@ -266,7 +266,7 @@ double Curve::curvatureAt(double t) const Vector d1 = derivativeAt(t); Vector d2 = derivativeAt(2, t); - return (d1.x() * d2.y() - d1.y() * d2.x()) / _pow(d1.norm(), 3); + return (d1.x() * d2.y() - d1.y() * d2.x()) / bu::pow(d1.norm(), 3); } double Curve::curvatureDerivativeAt(double t) const @@ -275,8 +275,8 @@ double Curve::curvatureDerivativeAt(double t) const Vector d2 = derivativeAt(2, t); Vector d3 = derivativeAt(3, t); - return (d1.x() * d3.y() - d1.y() * d3.x()) / _pow(d1.norm(), 3) - - 3 * d1.dot(d2) * (d1.x() * d2.y() - d1.y() * d2.x()) / _pow(d1.norm(), 5); + return (d1.x() * d3.y() - d1.y() * d3.x()) / bu::pow(d1.norm(), 3) - + 3 * d1.dot(d2) * (d1.x() * d2.y() - d1.y() * d2.x()) / bu::pow(d1.norm(), 5); } Vector Curve::tangentAt(double t, bool normalize) const @@ -319,7 +319,8 @@ std::vector Curve::roots() const if (!cached_roots_) cached_roots_ = N_ < 2 ? std::vector{} : [this] { Eigen::MatrixXd bezier_polynomial = bernsteinCoeffs(N_) * control_points_; - return _concatenate(_solvePolynomial(bezier_polynomial.col(0)), _solvePolynomial(bezier_polynomial.col(1))); + return bu::concatenate(bu::solvePolynomial(bezier_polynomial.col(0)), + bu::solvePolynomial(bezier_polynomial.col(1))); }(); return cached_roots_.value(); @@ -353,7 +354,7 @@ PointVector Curve::intersections(const Curve& curve) const auto addIntersection = [&intersections](Point new_point) { // check if not already found, and add new point if (std::none_of(intersections.begin(), intersections.end(), - [&new_point](const Point& point) { return (point - new_point).norm() < _epsilon; })) + [&new_point](const Point& point) { return (point - new_point).norm() < bu::epsilon; })) intersections.emplace_back(std::move(new_point)); }; @@ -372,8 +373,8 @@ PointVector Curve::intersections(const Curve& curve) const { Eigen::MatrixX2d new_cp = std::move(subcurves.back()); subcurves.pop_back(); - subcurves.emplace_back(splittingCoeffsLeft(N_, t[k] - _epsilon / 2) * new_cp); - subcurves.emplace_back(splittingCoeffsRight(N_, t[k] + _epsilon / 2) * new_cp); + subcurves.emplace_back(splittingCoeffsLeft(N_, t[k] - bu::epsilon / 2) * new_cp); + subcurves.emplace_back(splittingCoeffsRight(N_, t[k] + bu::epsilon / 2) * new_cp); std::for_each(t.begin() + k + 1, t.end(), [t = t[k]](double& x) { x = (x - t) / (1 - t); }); } @@ -396,9 +397,9 @@ PointVector Curve::intersections(const Curve& curve) const if (!bbox1.intersects(bbox2)) continue; // no intersection - else if (bbox1.diagonal().norm() < _epsilon) + else if (bbox1.diagonal().norm() < bu::epsilon) addIntersection(bbox1.center()); - else if (bbox2.diagonal().norm() < _epsilon) + else if (bbox2.diagonal().norm() < bu::epsilon) addIntersection(bbox2.center()); else { @@ -443,7 +444,7 @@ double Curve::projectPoint(const Point& point) const double min_t = (point - valueAt(0.0)).norm() < (point - valueAt(1.0)).norm() ? 0.0 : 1.0; double min_dist = (point - valueAt(min_t)).norm(); - auto candidates = _solvePolynomial(polynomial); + auto candidates = bu::solvePolynomial(polynomial); return std::accumulate(candidates.begin(), candidates.end(), std::make_pair(min_t, min_dist), [this, &point](std::pair min, double t) { double dist = (point - valueAt(t)).norm(); @@ -508,15 +509,15 @@ Curve Curve::offsetCurve(const Curve& curve, double offset, unsigned order) return fromPolyline(offset_polyline, order ? order : curve.order() + 1); } -Curve Curve::joinCurves(const Curve& curve1, const Curve& curve2, unsigned int order) +Curve Curve::joinCurves(const Curve& curve1, const Curve& curve2, unsigned order) { if (order == 1) return Curve(PointVector{curve1.control_points_.row(0), curve2.control_points_.row(curve2.N_ - 1)}); - return fromPolyline(_concatenate(curve1.polyline(), curve2.polyline()), + return fromPolyline(bu::concatenate(curve1.polyline(), curve2.polyline()), order ? order : curve1.order() + curve2.order()); } -Curve Curve::fromPolyline(const PointVector& polyline, unsigned int order) +Curve Curve::fromPolyline(const PointVector& polyline, unsigned order) { const unsigned N = std::min(order ? order + 1 : polyline.size(), polyline.size()); @@ -527,7 +528,7 @@ Curve Curve::fromPolyline(const PointVector& polyline, unsigned int order) // Select N points that most influence the shape of the polyline, // either based on a specified order or using the full polyline. - auto simplified = _polylineSimplify(polyline, N); + auto simplified = bu::polylineSimplify(polyline, N); // Initialize vector t where each element represents a normalized cumulative // distance between consecutive simplified points along the simplified polyline. @@ -545,7 +546,7 @@ Curve Curve::fromPolyline(const PointVector& polyline, unsigned int order) auto getCurve = [&M, &P](const Eigen::VectorXd& t) { Eigen::MatrixXd T(t.size(), t.size()); for (unsigned k{}; k < t.size(); k++) - T.row(k) = _powSeries(t(k), t.size()); + T.row(k) = bu::powSeries(t(k), t.size()); return Curve(M.inverse() * (T.transpose() * T).inverse() * T.transpose() * P); }; @@ -555,15 +556,15 @@ Curve Curve::fromPolyline(const PointVector& polyline, unsigned int order) double rmsd{}; auto polyline_c = c.polyline(); for (const auto& p : polyline_c) - rmsd += _pow(_polylineDist(polyline, p), 2); + rmsd += bu::pow(bu::polylineDist(polyline, p), 2); return std::sqrt(rmsd / polyline_c.size()); }; // Calculate the absolute difference in length between the polyline representation // of the curve and the original polyline points. - const double polyline_length = _polylineLength(polyline); + const double polyline_length = bu::polylineLength(polyline); auto length_diff = [&polyline_length](const Curve& c) { - return std::fabs(_polylineLength(c.polyline()) - polyline_length); + return std::fabs(bu::polylineLength(c.polyline()) - polyline_length); }; struct CostFunctor : public Eigen::DenseFunctor @@ -613,7 +614,7 @@ Curve Curve::fromPolyline(const PointVector& polyline, unsigned int order) for (auto& sample : pareto_front) { - if (sample.alpha < std::sqrt(_epsilon)) + if (sample.alpha < std::sqrt(bu::epsilon)) continue; finished = false; @@ -686,14 +687,14 @@ Curve::Coeffs Curve::splittingCoeffsLeft(unsigned n, double t) if (!splitting_coeffs_left_.count(n)) { splitting_coeffs_left_.insert({n, Coeffs::Zero(n, n)}); - splitting_coeffs_left_[n].diagonal() = _powSeries(0.5, n); + splitting_coeffs_left_[n].diagonal() = bu::powSeries(0.5, n); splitting_coeffs_left_[n] = bernsteinCoeffs(n).inverse() * splitting_coeffs_left_[n] * bernsteinCoeffs(n); } return splitting_coeffs_left_[n]; } Curve::Coeffs coeffs(Coeffs::Zero(n, n)); - coeffs.diagonal() = _powSeries(t, n); + coeffs.diagonal() = bu::powSeries(t, n); return bernsteinCoeffs(n).inverse() * coeffs * bernsteinCoeffs(n); } diff --git a/src/polycurve.cpp b/src/polycurve.cpp index 350bd2c..3091ce2 100644 --- a/src/polycurve.cpp +++ b/src/polycurve.cpp @@ -4,7 +4,7 @@ #include using namespace Bezier; -using namespace Bezier::Utils; +namespace bu = Bezier::Utils; PolyCurve::PolyCurve(std::deque curves) : curves_(std::move(curves)) {} @@ -77,15 +77,15 @@ double PolyCurve::length(double t1, double t2) const double PolyCurve::iterateByLength(double t, double s) const { - if (std::fabs(s) < _epsilon) // no-op + if (std::fabs(s) < bu::epsilon) // no-op return t; double s_t = length(t); - if (s < -s_t + _epsilon) // out-of-scope + if (s < -s_t + bu::epsilon) // out-of-scope return 0.0; - if (s > length() - s_t - _epsilon) // out-of-scope + if (s > length() - s_t - bu::epsilon) // out-of-scope return size(); unsigned idx = curveIdx(t); @@ -93,13 +93,13 @@ double PolyCurve::iterateByLength(double t, double s) const s_t = s < 0 ? s_t - length(idx) : length(idx + 1) - s_t; - while (-s_t > s + _epsilon) + while (-s_t > s + bu::epsilon) { s += s_t; s_t = curves_[--idx].length(); t = 1.0; } - while (s_t < s - _epsilon) + while (s_t < s - bu::epsilon) { s -= s_t; s_t = curves_[++idx].length(); diff --git a/src/utils.cpp b/src/utils.cpp index 800cfdd..b400656 100644 --- a/src/utils.cpp +++ b/src/utils.cpp @@ -5,9 +5,9 @@ #include using namespace Bezier; -using namespace Bezier::Utils; +namespace bu = Bezier::Utils; -PointVector Bezier::Utils::_polylineSimplify(const PointVector& polyline, unsigned int N) +PointVector Bezier::Utils::polylineSimplify(const PointVector& polyline, unsigned N) { if (polyline.size() < 2) throw std::logic_error{"Polyline must have at least two points."}; @@ -75,11 +75,15 @@ PointVector Bezier::Utils::_polylineSimplify(const PointVector& polyline, unsign return simplified; } -std::vector Bezier::Utils::_solvePolynomial(const Eigen::VectorXd& polynomial) +std::vector Bezier::Utils::solvePolynomial(const Eigen::VectorXd& polynomial) { + // Trim leading zeros from the polynomial auto idx = polynomial.size(); - while (idx && std::abs(polynomial(idx - 1)) < _epsilon) + while (idx && std::abs(polynomial(idx - 1)) < bu::epsilon) --idx; + // Polynomial is constant + if (idx < 2) + return {}; struct PolynomialRoots : public std::vector { @@ -90,8 +94,6 @@ std::vector Bezier::Utils::_solvePolynomial(const Eigen::VectorXd& polyn } } roots; roots.reserve(idx); - - if (idx > 1) - Eigen::PolynomialSolver(polynomial.head(idx)).realRoots(roots); + Eigen::PolynomialSolver(polynomial.head(idx)).realRoots(roots); return roots; } From a03ebc2abdc4aa034bd4952c95e1e8d39ce15563 Mon Sep 17 00:00:00 2001 From: stribor14 Date: Tue, 1 Oct 2024 14:42:55 +0200 Subject: [PATCH 12/19] Small refactor for cached roots --- src/bezier.cpp | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/src/bezier.cpp b/src/bezier.cpp index aa499f7..858baaa 100644 --- a/src/bezier.cpp +++ b/src/bezier.cpp @@ -317,11 +317,11 @@ Vector Curve::derivativeAt(unsigned n, double t) const { return derivative(n).va std::vector Curve::roots() const { if (!cached_roots_) - cached_roots_ = N_ < 2 ? std::vector{} : [this] { - Eigen::MatrixXd bezier_polynomial = bernsteinCoeffs(N_) * control_points_; - return bu::concatenate(bu::solvePolynomial(bezier_polynomial.col(0)), - bu::solvePolynomial(bezier_polynomial.col(1))); - }(); + { + Eigen::MatrixXd bezier_polynomial = bernsteinCoeffs(N_) * control_points_; + cached_roots_.emplace(bu::concatenate(bu::solvePolynomial(bezier_polynomial.col(0)), // + bu::solvePolynomial(bezier_polynomial.col(1)))); + } return cached_roots_.value(); } @@ -336,7 +336,6 @@ BoundingBox Curve::boundingBox() const extremes.conservativeResize(extremes.rows() + 2, Eigen::NoChange); extremes.row(extremes.rows() - 1) = control_points_.row(0); extremes.row(extremes.rows() - 2) = control_points_.row(N_ - 1); - cached_bounding_box_.emplace(Point(extremes.col(0).minCoeff(), extremes.col(1).minCoeff()), Point(extremes.col(0).maxCoeff(), extremes.col(1).maxCoeff())); } From ee7d796a10b78718cd1a25fa7799757d0843d282 Mon Sep 17 00:00:00 2001 From: stribor14 Date: Tue, 1 Oct 2024 14:55:35 +0200 Subject: [PATCH 13/19] Add missing cached values to reset() --- src/bezier.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/bezier.cpp b/src/bezier.cpp index 858baaa..7fa4f82 100644 --- a/src/bezier.cpp +++ b/src/bezier.cpp @@ -656,7 +656,9 @@ void Curve::resetCache() cached_roots_.reset(); cached_bounding_box_.reset(); cached_polyline_.reset(); + cached_polyline_t_.reset(); cached_projection_polynomial_part_.reset(); + cached_projection_polynomial_derivative_.reset(); cached_chebyshev_coeffs_.reset(); } From c0828f154da075df57bf2c2870a4400bad2b8b1e Mon Sep 17 00:00:00 2001 From: stribor14 Date: Sat, 21 Sep 2024 19:21:27 +0200 Subject: [PATCH 14/19] Change default flatness parameter to be dynamic (0.1% of bbox diagonal) --- include/Bezier/bezier.h | 9 ++++++++- src/bezier.cpp | 5 +++++ 2 files changed, 13 insertions(+), 1 deletion(-) diff --git a/include/Bezier/bezier.h b/include/Bezier/bezier.h index 33df198..d8606ea 100644 --- a/include/Bezier/bezier.h +++ b/include/Bezier/bezier.h @@ -79,12 +79,19 @@ class Curve */ std::pair endPoints() const; + /*! + * \brief Get a polyline representation of the curve as a vector of points on curve + * \return A vector of polyline vertices + * \note Default flatness parameter is calculated as 0.1% for bounding box diagonal + */ + PointVector polyline() const; + /*! * \brief Get a polyline representation of the curve as a vector of points on curve * \param flatness Error tolerance of approximation * \return A vector of polyline vertices */ - PointVector polyline(double flatness = 0.5) const; + PointVector polyline(double flatness) const; /*! * \brief Compute exact arc length using Chebyshev polynomials diff --git a/src/bezier.cpp b/src/bezier.cpp index e78941d..5d5f9f7 100644 --- a/src/bezier.cpp +++ b/src/bezier.cpp @@ -44,6 +44,11 @@ Point Curve::controlPoint(unsigned idx) const { return control_points_.row(idx); std::pair Curve::endPoints() const { return {control_points_.row(0), control_points_.row(N_ - 1)}; } +PointVector Curve::polyline() const +{ + return polyline(boundingBox().diagonal().norm() / 1000); +} + PointVector Curve::polyline(double flatness) const { if (!cached_polyline_ || cached_polyline_flatness_ != flatness) From 5e10de9489799dd7ba00823dd54e30277b1277b5 Mon Sep 17 00:00:00 2001 From: stribor14 Date: Fri, 20 Sep 2024 23:36:02 +0200 Subject: [PATCH 15/19] Change return type of splitCurve and add a method to split in multiple subcurves --- example/customscene.cpp | 4 ++-- include/Bezier/bezier.h | 11 +++++++++-- src/bezier.cpp | 38 ++++++++++++++++++++++---------------- 3 files changed, 33 insertions(+), 20 deletions(-) diff --git a/example/customscene.cpp b/example/customscene.cpp index 54121af..8b11f4c 100644 --- a/example/customscene.cpp +++ b/example/customscene.cpp @@ -163,8 +163,8 @@ void CustomScene::mousePressEvent(QGraphicsSceneMouseEvent* mouseEvent) auto split = c_curve->splitCurve(t); delete curve; qCurve *c1, *c2; - c1 = new qCurve(split.first); - c2 = new qCurve(split.second); + c1 = new qCurve(split[0]); + c2 = new qCurve(split[1]); this->addItem(c1); this->addItem(c2); update(); diff --git a/include/Bezier/bezier.h b/include/Bezier/bezier.h index 343646c..dec45ae 100644 --- a/include/Bezier/bezier.h +++ b/include/Bezier/bezier.h @@ -236,12 +236,19 @@ class Curve */ BoundingBox boundingBox() const; + /*! + * \brief Split the curve into multiple subcurves + * \param t A vector of curve parameters at which to split the curve + * \return A vector of subcurves + */ + std::vector splitCurve(const std::vector& t) const; + /*! * \brief Split the curve into two subcurves * \param t Curve parameter at which to split the curve - * \return Pair of two subcurves + * \return A vector of two subcurves */ - std::pair splitCurve(double t = 0.5) const; + std::vector splitCurve(double t = 0.5) const; /*! * \brief Get the points of intersection with another curve diff --git a/src/bezier.cpp b/src/bezier.cpp index 7fa4f82..435ec80 100644 --- a/src/bezier.cpp +++ b/src/bezier.cpp @@ -342,7 +342,25 @@ BoundingBox Curve::boundingBox() const return cached_bounding_box_.value(); } -std::pair Curve::splitCurve(double t) const +std::vector Curve::splitCurve(const std::vector& t) const +{ + auto sorted_t = t; + std::sort(sorted_t.begin(), sorted_t.end()); + std::vector subcurves; + subcurves.reserve(sorted_t.size() + 1); + auto leftover_cp = control_points_; + for (unsigned k{}; k < sorted_t.size(); k++) + { + subcurves.emplace_back(splittingCoeffsLeft(N_, sorted_t[k] - bu::epsilon / 2) * leftover_cp); + leftover_cp = splittingCoeffsRight(N_, sorted_t[k] + bu::epsilon / 2) * leftover_cp; + std::for_each(sorted_t.begin() + k + 1, sorted_t.end(), [t = sorted_t[k]](double& x) { x = (x - t) / (1 - t); }); + } + subcurves.emplace_back(std::move(leftover_cp)); + return subcurves; +} + + +std::vector Curve::splitCurve(double t) const { return {Curve(splittingCoeffsLeft(N_, t) * control_points_), Curve(splittingCoeffsRight(N_, t) * control_points_)}; } @@ -363,25 +381,13 @@ PointVector Curve::intersections(const Curve& curve) const subcurve_pairs.emplace_back(control_points_, curve.control_points_); else { - // for self intersections divide curve into subcurves at extrema + // for self intersections divide curve into subcurves at extrema and create all pairs of subcurves auto t = extrema(); std::sort(t.begin(), t.end()); - std::vector subcurves; - subcurves.emplace_back(control_points_); - for (unsigned k{}; k < t.size(); k++) - { - Eigen::MatrixX2d new_cp = std::move(subcurves.back()); - subcurves.pop_back(); - subcurves.emplace_back(splittingCoeffsLeft(N_, t[k] - bu::epsilon / 2) * new_cp); - subcurves.emplace_back(splittingCoeffsRight(N_, t[k] + bu::epsilon / 2) * new_cp); - - std::for_each(t.begin() + k + 1, t.end(), [t = t[k]](double& x) { x = (x - t) / (1 - t); }); - } - - // create all pairs of subcurves + auto subcurves = splitCurve(t); for (unsigned k{}; k < subcurves.size(); k++) for (unsigned i{k + 1}; i < subcurves.size(); i++) - subcurve_pairs.emplace_back(subcurves[k], subcurves[i]); + subcurve_pairs.emplace_back(subcurves[k].control_points_, subcurves[i].control_points_); } while (!subcurve_pairs.empty()) From dbc892efd60c486836d616d2307359ef306e0802 Mon Sep 17 00:00:00 2001 From: stribor14 Date: Sat, 21 Sep 2024 11:07:51 +0200 Subject: [PATCH 16/19] Refactor fromPolyline static method with eigen3 LevenbergMarquadt solver --- include/Bezier/utils.h | 33 ++++++++-- src/bezier.cpp | 141 +++++++++++++++-------------------------- src/utils.cpp | 52 +++++++-------- 3 files changed, 104 insertions(+), 122 deletions(-) diff --git a/include/Bezier/utils.h b/include/Bezier/utils.h index 647bb86..76403b2 100644 --- a/include/Bezier/utils.h +++ b/include/Bezier/utils.h @@ -10,13 +10,14 @@ namespace Bezier { namespace Utils { -/*! - * \brief Precision for numerical methods - */ + +/// Precision for numerical methods const double epsilon = std::sqrt(std::numeric_limits::epsilon()); +/// Calculate unsigned power of number 2 inline unsigned exp2(unsigned exp) { return 1 << exp; } +/// Calculate power for integer exponents template inline T pow(T base, unsigned exp) { T result = exp & 1 ? base : 1; @@ -29,6 +30,7 @@ template inline T pow(T base, unsigned exp) return result; } +/// Compute binomial coefficient inline Eigen::RowVectorXd powSeries(double base, unsigned exp) { Eigen::RowVectorXd power_series(exp); @@ -38,6 +40,7 @@ inline Eigen::RowVectorXd powSeries(double base, unsigned exp) return power_series; } +/// Concatenate two vectors template inline std::vector concatenate(std::vector&& v1, std::vector&& v2) { v1.reserve(v1.size() + v2.size()); @@ -45,6 +48,7 @@ template inline std::vector concatenate(std::vector&& v1, std return std::move(v1); } +/// Length of a polyline inline double polylineLength(const PointVector& polyline) { double length{}; @@ -53,6 +57,7 @@ inline double polylineLength(const PointVector& polyline) return length; } +/// Distance between a point and a polyline inline double polylineDist(const PointVector& polyline, const Point& point) { auto distSeg = [&point](const Point& p1, const Point& p2) { @@ -72,8 +77,28 @@ inline double polylineDist(const PointVector& polyline, const Point& point) return dist; } -PointVector polylineSimplify(const PointVector& polyline, unsigned N); +/// Sort indices of polyline points by their contribution to the polyline shape +std::vector visvalingamWyatt(const PointVector& polyline); + +/// Simplify polyline to N points +inline PointVector polylineSimplify(const PointVector& polyline, unsigned N) +{ + if (polyline.size() < 2) + throw std::logic_error{"Polyline must have at least two points."}; + if (polyline.size() < N) + return PointVector(polyline); + if (N == 2) + return PointVector{polyline.front(), polyline.back()}; + + auto by_contribution = visvalingamWyatt(polyline); + std::sort(by_contribution.begin(), by_contribution.begin() + N); + PointVector simplified(N); + for (size_t k{0}; k < N; k++) + simplified[k] = polyline[by_contribution[k]]; + return simplified; +} +/// Find solutions to polynomial equation (limited to [0, 1]) std::vector solvePolynomial(const Eigen::VectorXd& polynomial); } // namespace Utils diff --git a/src/bezier.cpp b/src/bezier.cpp index 435ec80..7dc7a7b 100644 --- a/src/bezier.cpp +++ b/src/bezier.cpp @@ -2,6 +2,7 @@ #include "Bezier/utils.h" #include +#include #include #include @@ -531,9 +532,22 @@ Curve Curve::fromPolyline(const PointVector& polyline, unsigned order) if (N == 2) return Curve(PointVector{polyline.front(), polyline.back()}); - // Select N points that most influence the shape of the polyline, - // either based on a specified order or using the full polyline. - auto simplified = bu::polylineSimplify(polyline, N); + // Sort the polyline points by their contribution to the Visvalingam-Whyatt + // simplification algorithm, and keep the N most contributing points in original order. + auto vw = bu::visvalingamWyatt(polyline); + std::set by_contribution(vw.begin(), vw.begin() + N); + + // Divide polyline into subparts where the simplified polyline points are located. + std::vector subpolylines; + subpolylines.reserve(N - 1); + subpolylines.emplace_back(std::vector{polyline.front()}); + for(unsigned k{1}; k + 1 < polyline.size(); k++) + { + subpolylines.back().emplace_back(polyline[k]); + if (by_contribution.count(k)) + subpolylines.emplace_back(std::vector{polyline[k]}); + } + subpolylines.back().emplace_back(polyline.back()); // Initialize vector t where each element represents a normalized cumulative // distance between consecutive simplified points along the simplified polyline. @@ -541,13 +555,13 @@ Curve Curve::fromPolyline(const PointVector& polyline, unsigned order) Eigen::MatrixXd P(N, 2), M = bernsteinCoeffs(N); for (unsigned k{}; k < N; k++) { - P.row(k) = simplified[k]; + P.row(k) = polyline[*std::next(by_contribution.begin(), k)]; t(k) = k == 0 ? 0 : t(k - 1) + (P.row(k) - P.row(k - 1)).norm(); } t /= t(N - 1); - // Compute the control points for a Bezier curve such that C(t_i) = P_i for all i, - // by solving the matrix form of the Bezier curve equation. + // Compute the control points for a Bezier curve such that it passes through + // the simplified polyline points at parameter t. auto getCurve = [&M, &P](const Eigen::VectorXd& t) { Eigen::MatrixXd T(t.size(), t.size()); for (unsigned k{}; k < t.size(); k++) @@ -555,104 +569,53 @@ Curve Curve::fromPolyline(const PointVector& polyline, unsigned order) return Curve(M.inverse() * (T.transpose() * T).inverse() * T.transpose() * P); }; - // Calculate the root mean square distance (RMSD) between the polyline - // representation of the curve and the original polyline points. - auto rmsd = [&polyline](const Curve& c) { - double rmsd{}; - auto polyline_c = c.polyline(); - for (const auto& p : polyline_c) - rmsd += bu::pow(bu::polylineDist(polyline, p), 2); - return std::sqrt(rmsd / polyline_c.size()); - }; - - // Calculate the absolute difference in length between the polyline representation - // of the curve and the original polyline points. - const double polyline_length = bu::polylineLength(polyline); - auto length_diff = [&polyline_length](const Curve& c) { - return std::fabs(bu::polylineLength(c.polyline()) - polyline_length); - }; - + // Cost functor calculates RMSD and length difference for each subcurve/subpolyline + // divided at parameter t, where C(t_i) = P_i. struct CostFunctor : public Eigen::DenseFunctor { - using GetCurveFun = std::function; - using CostFun = std::function; + using GetCurveFun = std::function; GetCurveFun getCurve; - CostFun f1, f2; + std::vector subpolylines; - CostFunctor(int N, GetCurveFun getCurve, CostFun f1, CostFun f2) - : DenseFunctor(N - 2, 2), getCurve(getCurve), f1(f1), f2(f2) + CostFunctor(int N, GetCurveFun getCurve, const std::vector& subpolylines) + : DenseFunctor(N - 2, 2 * N - 2), getCurve(getCurve), subpolylines(subpolylines) { } int operator()(const Eigen::VectorXd& x, Eigen::VectorXd& fvec) const { - auto c = getCurve((Eigen::VectorXd(inputs() + 2) << 0, x, 1).finished()); - fvec(0) = f1(c); - fvec(1) = f2(c); - return 0; - } - }; - - CostFunctor costFun(N, getCurve, rmsd, length_diff); - Eigen::NumericalDiff num_diff(costFun); - - Eigen::MatrixXd J(2, N - 2); - Eigen::VectorXd fvec(2), x = t.segment(1, N - 2); - costFun(x, fvec); - num_diff.df(x, J); - - struct ParetoData - { - Eigen::VectorXd f, x; - Eigen::MatrixXd J; - double alpha; - }; - - constexpr double alpha_init = 0.1; - constexpr unsigned pareto_max_size = 10; - std::vector pareto_front{{fvec, x, J, alpha_init}}; - pareto_front.reserve(2 * pareto_max_size); - - for (bool finished{false}; !finished;) - { - finished = true; + auto rmsd = [](const Curve& curve, const PointVector& polyline) { + double rmsd{}; + auto polyline_c = curve.polyline(); + for (const auto& p : polyline_c) + rmsd += bu::pow(bu::polylineDist(polyline, p), 2); + return std::sqrt(rmsd / polyline_c.size()); + }; - for (auto& sample : pareto_front) - { - if (sample.alpha < std::sqrt(bu::epsilon)) - continue; - finished = false; + auto length_diff = [](const Curve& curve, const PointVector& polyline) { + return std::fabs(bu::polylineLength(curve.polyline()) - bu::polylineLength(polyline)); + }; - do + auto curve = getCurve((Eigen::VectorXd(inputs() + 2) << 0, x, 1).finished()); + auto subcurves = curve.splitCurve(std::vector(x.data(), x.data() + inputs())); + for(unsigned k = 0; k < subcurves.size(); k++) { - x = sample.x - sample.alpha * (sample.J.row(0) + sample.J.row(1)).transpose().normalized(); - sample.alpha /= 2; - } while (x.minCoeff() < 0.0 || x.maxCoeff() > 1.0 || (x.array().head(N - 3) >= x.array().tail(N - 3)).any()); - - costFun(x, fvec); - if (std::any_of(pareto_front.begin(), pareto_front.end(), - [&fvec](const auto& p) { return (p.f.array() <= fvec.array()).all(); })) - continue; - - num_diff.df(x, J); - pareto_front.push_back({fvec, x, J, 2 * sample.alpha}); + fvec(k) = rmsd(subcurves[k], subpolylines[k]); + fvec(values() / 2 + k) = length_diff(subcurves[k], subpolylines[k]); + } + return 0; } + }; - std::sort(pareto_front.begin(), pareto_front.end(), [](const auto& p1, const auto& p2) { - return p1.f(0) == p2.f(0) ? p1.f(1) < p2.f(1) : p1.f(0) < p2.f(0); - }); - - double best_f1 = std::numeric_limits::max(); - auto erase_it = std::remove_if(pareto_front.begin(), pareto_front.end(), [&best_f1](const auto& p) { - return p.f(1) <= best_f1 ? (best_f1 = p.f(1), false) : true; - }); - - if (std::distance(erase_it, pareto_front.begin() + pareto_max_size) < 0) - erase_it = pareto_front.begin() + pareto_max_size; - pareto_front.erase(erase_it, pareto_front.end()); - } + // Use Levenberg-Marquardt optimization to find the control points that minimize + // the RMSD and length difference of the Bezier curve and the simplified polyline. + CostFunctor costFun(N, getCurve, subpolylines); + Eigen::NumericalDiff num_diff(costFun); + Eigen::LevenbergMarquardt> lm(num_diff); + Eigen::VectorXd x = t.segment(1, N - 2); + lm.minimize(x); - return getCurve((Eigen::VectorXd(N) << 0, pareto_front.front().x, 1).finished()); + return getCurve((Eigen::VectorXd(N) << 0, x, 1).finished()); } void Curve::resetCache() diff --git a/src/utils.cpp b/src/utils.cpp index b400656..749ece5 100644 --- a/src/utils.cpp +++ b/src/utils.cpp @@ -7,16 +7,12 @@ using namespace Bezier; namespace bu = Bezier::Utils; -PointVector Bezier::Utils::polylineSimplify(const PointVector& polyline, unsigned N) +std::vector Bezier::Utils::visvalingamWyatt(const PointVector& polyline) { - if (polyline.size() < 2) - throw std::logic_error{"Polyline must have at least two points."}; - if (polyline.size() < N) - return PointVector(polyline); - if (N == 2) - return PointVector{polyline.front(), polyline.back()}; + // Vector of indices sorted by contribution to the polyline shape + std::vector by_contribution; + by_contribution.reserve(polyline.size()); - // Simplification is done using the Visvalingam-Whyatt algorithm // Helper structure to keep track of each Points neigbours and contribution to the polyline shape struct Vertex { @@ -27,14 +23,14 @@ PointVector Bezier::Utils::polylineSimplify(const PointVector& polyline, unsigne vertices.reserve(polyline.size()); // Set is used to keep track of sorted contributions - auto cmp = [&vertices](size_t idx1, size_t idx2) { + auto cmp = [&vertices](unsigned idx1, unsigned idx2) { return vertices[idx1].contribution > vertices[idx2].contribution; }; - std::vector by_contribution(polyline.size() - 2); - std::iota(by_contribution.begin(), by_contribution.end(), 1); + std::vector min_heap(polyline.size() - 2); + std::iota(min_heap.begin(), min_heap.end(), 1); // Visvalingam-Whyatt measures contribution as an area between 3 consecutive Points - auto area = [&polyline](size_t id1, size_t id2, size_t id3) { + auto area = [&polyline](unsigned id1, unsigned id2, unsigned id3) { const auto& A = polyline[id1]; const auto& B = polyline[id2]; const auto& C = polyline[id3]; @@ -43,36 +39,34 @@ PointVector Bezier::Utils::polylineSimplify(const PointVector& polyline, unsigne // Initialize structures used for the algorithm vertices.push_back({0, 1, 0.0}); - for (size_t k = 1; k + 1 < polyline.size(); k++) + for (unsigned k = 1; k + 1 < polyline.size(); k++) vertices.push_back({k - 1, k + 1, area(k - 1, k, k + 1)}); vertices.push_back({polyline.size() - 2, polyline.size(), 0.0}); - // Simplify polyline until N points are left (-2 for start/end points) - while (by_contribution.size() > N - 2) + while (!min_heap.empty()) { // Select and erase a Point with smallest current contribution to polyline shape - std::make_heap(by_contribution.begin(), by_contribution.end(), cmp); - std::pop_heap(by_contribution.begin(), by_contribution.end(), cmp); - auto curr = by_contribution.back(); - by_contribution.pop_back(); + std::make_heap(min_heap.begin(), min_heap.end(), cmp); + std::pop_heap(min_heap.begin(), min_heap.end(), cmp); + by_contribution.push_back(min_heap.back()); + min_heap.pop_back(); // Update previous and next Vertex: // - update neighbours neighbours // - update neighbours contribution - auto prev = vertices[curr].prev; - auto next = vertices[curr].next; - vertices[prev].next = vertices[curr].next; - vertices[next].prev = vertices[curr].prev; + auto prev = vertices[by_contribution.back()].prev; + auto next = vertices[by_contribution.back()].next; + vertices[prev].next = vertices[by_contribution.back()].next; + vertices[next].prev = vertices[by_contribution.back()].prev; vertices[prev].contribution = area(vertices[prev].prev, prev, vertices[prev].next); vertices[next].contribution = area(vertices[next].prev, next, vertices[next].next); } - // Reconstruct simplified polyline - PointVector simplified; - simplified.reserve(N); - for (size_t k = 0; k < polyline.size(); k = vertices[k].next) - simplified.push_back(polyline[k]); - return simplified; + by_contribution.push_back(0); + by_contribution.push_back(polyline.size() - 1); + std::reverse(by_contribution.begin(), by_contribution.end()); + + return by_contribution; } std::vector Bezier::Utils::solvePolynomial(const Eigen::VectorXd& polynomial) From 6a7dd2dee769c2b3e7c35b9c8c10e7857c3597da Mon Sep 17 00:00:00 2001 From: stribor14 Date: Mon, 30 Sep 2024 18:48:51 +0200 Subject: [PATCH 17/19] Simplify implementation of Visvalingam-Wyatt algorithm --- src/bezier.cpp | 7 +++--- src/utils.cpp | 58 ++++++++++++++++++++------------------------------ 2 files changed, 26 insertions(+), 39 deletions(-) diff --git a/src/bezier.cpp b/src/bezier.cpp index 7dc7a7b..43b6b0d 100644 --- a/src/bezier.cpp +++ b/src/bezier.cpp @@ -360,7 +360,6 @@ std::vector Curve::splitCurve(const std::vector& t) const return subcurves; } - std::vector Curve::splitCurve(double t) const { return {Curve(splittingCoeffsLeft(N_, t) * control_points_), Curve(splittingCoeffsRight(N_, t) * control_points_)}; @@ -541,7 +540,7 @@ Curve Curve::fromPolyline(const PointVector& polyline, unsigned order) std::vector subpolylines; subpolylines.reserve(N - 1); subpolylines.emplace_back(std::vector{polyline.front()}); - for(unsigned k{1}; k + 1 < polyline.size(); k++) + for (unsigned k{1}; k + 1 < polyline.size(); k++) { subpolylines.back().emplace_back(polyline[k]); if (by_contribution.count(k)) @@ -578,7 +577,7 @@ Curve Curve::fromPolyline(const PointVector& polyline, unsigned order) std::vector subpolylines; CostFunctor(int N, GetCurveFun getCurve, const std::vector& subpolylines) - : DenseFunctor(N - 2, 2 * N - 2), getCurve(getCurve), subpolylines(subpolylines) + : DenseFunctor(N - 2, 2 * N - 2), getCurve(getCurve), subpolylines(subpolylines) { } @@ -598,7 +597,7 @@ Curve Curve::fromPolyline(const PointVector& polyline, unsigned order) auto curve = getCurve((Eigen::VectorXd(inputs() + 2) << 0, x, 1).finished()); auto subcurves = curve.splitCurve(std::vector(x.data(), x.data() + inputs())); - for(unsigned k = 0; k < subcurves.size(); k++) + for (unsigned k = 0; k < subcurves.size(); k++) { fvec(k) = rmsd(subcurves[k], subpolylines[k]); fvec(values() / 2 + k) = length_diff(subcurves[k], subpolylines[k]); diff --git a/src/utils.cpp b/src/utils.cpp index 749ece5..f1f4d1f 100644 --- a/src/utils.cpp +++ b/src/utils.cpp @@ -9,9 +9,11 @@ namespace bu = Bezier::Utils; std::vector Bezier::Utils::visvalingamWyatt(const PointVector& polyline) { - // Vector of indices sorted by contribution to the polyline shape - std::vector by_contribution; - by_contribution.reserve(polyline.size()); + // Vector of indices sorted by contribution to the polyline shape (first and last contribute the most by default) + // Initialized with all indices, taking care to put the first and the last at the start + std::vector by_contribution(polyline.size()); + std::iota(by_contribution.begin(), by_contribution.end(), -1); + by_contribution.front() += polyline.size(); // Helper structure to keep track of each Points neigbours and contribution to the polyline shape struct Vertex @@ -19,15 +21,6 @@ std::vector Bezier::Utils::visvalingamWyatt(const PointVector& polylin size_t prev, next; double contribution; }; - std::vector vertices; - vertices.reserve(polyline.size()); - - // Set is used to keep track of sorted contributions - auto cmp = [&vertices](unsigned idx1, unsigned idx2) { - return vertices[idx1].contribution > vertices[idx2].contribution; - }; - std::vector min_heap(polyline.size() - 2); - std::iota(min_heap.begin(), min_heap.end(), 1); // Visvalingam-Whyatt measures contribution as an area between 3 consecutive Points auto area = [&polyline](unsigned id1, unsigned id2, unsigned id3) { @@ -37,35 +30,30 @@ std::vector Bezier::Utils::visvalingamWyatt(const PointVector& polylin return std::fabs((B.x() - A.x()) * (C.y() - A.y()) - (C.x() - A.x()) * (B.y() - A.y())); }; - // Initialize structures used for the algorithm - vertices.push_back({0, 1, 0.0}); + // Initialize vertices + std::vector vertices(polyline.size()); + vertices.front() = {0, 1, 0.0}; for (unsigned k = 1; k + 1 < polyline.size(); k++) - vertices.push_back({k - 1, k + 1, area(k - 1, k, k + 1)}); - vertices.push_back({polyline.size() - 2, polyline.size(), 0.0}); + vertices[k] = {k - 1, k + 1, area(k - 1, k, k + 1)}; + vertices.back() = {polyline.size() - 2, polyline.size(), 0.0}; - while (!min_heap.empty()) + // Smallest contribution willl be at the end of the vector + for (auto it = by_contribution.rbegin(); it != by_contribution.rend() - 2; ++it) { - // Select and erase a Point with smallest current contribution to polyline shape - std::make_heap(min_heap.begin(), min_heap.end(), cmp); - std::pop_heap(min_heap.begin(), min_heap.end(), cmp); - by_contribution.push_back(min_heap.back()); - min_heap.pop_back(); + // Select and move a Point with smallest current contribution + std::iter_swap(it, std::min_element(it, by_contribution.rend() - 2, [&vertices](unsigned idx1, unsigned idx2) { + return vertices[idx1].contribution < vertices[idx2].contribution; + })); - // Update previous and next Vertex: - // - update neighbours neighbours - // - update neighbours contribution - auto prev = vertices[by_contribution.back()].prev; - auto next = vertices[by_contribution.back()].next; - vertices[prev].next = vertices[by_contribution.back()].next; - vertices[next].prev = vertices[by_contribution.back()].prev; - vertices[prev].contribution = area(vertices[prev].prev, prev, vertices[prev].next); - vertices[next].contribution = area(vertices[next].prev, next, vertices[next].next); + // Update previous and next Vertices (neighbours and contributions) + auto prev = vertices[*it].prev; + auto next = vertices[*it].next; + vertices[prev].next = next; + vertices[next].prev = prev; + vertices[prev].contribution = area(vertices[prev].prev, prev, next); + vertices[next].contribution = area(prev, next, vertices[next].next); } - by_contribution.push_back(0); - by_contribution.push_back(polyline.size() - 1); - std::reverse(by_contribution.begin(), by_contribution.end()); - return by_contribution; } From ebb1d6786aee60b6ec804c4a1c2186450db6bace Mon Sep 17 00:00:00 2001 From: stribor14 Date: Tue, 1 Oct 2024 17:04:47 +0200 Subject: [PATCH 18/19] Small cleanup --- include/Bezier/utils.h | 21 +++------------------ src/utils.cpp | 19 ++++++++++++++++++- 2 files changed, 21 insertions(+), 19 deletions(-) diff --git a/include/Bezier/utils.h b/include/Bezier/utils.h index 76403b2..d47edc1 100644 --- a/include/Bezier/utils.h +++ b/include/Bezier/utils.h @@ -41,11 +41,11 @@ inline Eigen::RowVectorXd powSeries(double base, unsigned exp) } /// Concatenate two vectors -template inline std::vector concatenate(std::vector&& v1, std::vector&& v2) +template inline std::vector concatenate(std::vector v1, std::vector v2) { v1.reserve(v1.size() + v2.size()); v1.insert(v1.end(), std::make_move_iterator(v2.begin()), std::make_move_iterator(v2.end())); - return std::move(v1); + return v1; } /// Length of a polyline @@ -81,22 +81,7 @@ inline double polylineDist(const PointVector& polyline, const Point& point) std::vector visvalingamWyatt(const PointVector& polyline); /// Simplify polyline to N points -inline PointVector polylineSimplify(const PointVector& polyline, unsigned N) -{ - if (polyline.size() < 2) - throw std::logic_error{"Polyline must have at least two points."}; - if (polyline.size() < N) - return PointVector(polyline); - if (N == 2) - return PointVector{polyline.front(), polyline.back()}; - - auto by_contribution = visvalingamWyatt(polyline); - std::sort(by_contribution.begin(), by_contribution.begin() + N); - PointVector simplified(N); - for (size_t k{0}; k < N; k++) - simplified[k] = polyline[by_contribution[k]]; - return simplified; -} +PointVector polylineSimplify(const PointVector& polyline, unsigned N); /// Find solutions to polynomial equation (limited to [0, 1]) std::vector solvePolynomial(const Eigen::VectorXd& polynomial); diff --git a/src/utils.cpp b/src/utils.cpp index f1f4d1f..6565d7e 100644 --- a/src/utils.cpp +++ b/src/utils.cpp @@ -37,7 +37,7 @@ std::vector Bezier::Utils::visvalingamWyatt(const PointVector& polylin vertices[k] = {k - 1, k + 1, area(k - 1, k, k + 1)}; vertices.back() = {polyline.size() - 2, polyline.size(), 0.0}; - // Smallest contribution willl be at the end of the vector + // Smallest contribution will be at the end of the vector for (auto it = by_contribution.rbegin(); it != by_contribution.rend() - 2; ++it) { // Select and move a Point with smallest current contribution @@ -57,6 +57,23 @@ std::vector Bezier::Utils::visvalingamWyatt(const PointVector& polylin return by_contribution; } +PointVector Bezier::Utils::polylineSimplify(const PointVector &polyline, unsigned int N) +{ + if (polyline.size() < 2) + throw std::logic_error{"Polyline must have at least two points."}; + if (polyline.size() < N) + return PointVector(polyline); + if (N == 2) + return PointVector{polyline.front(), polyline.back()}; + + auto by_contribution = visvalingamWyatt(polyline); + std::sort(by_contribution.begin(), by_contribution.begin() + N); + PointVector simplified(N); + for (size_t k{0}; k < N; k++) + simplified[k] = polyline[by_contribution[k]]; + return simplified; +} + std::vector Bezier::Utils::solvePolynomial(const Eigen::VectorXd& polynomial) { // Trim leading zeros from the polynomial From 0229184eba033c037508bc3f998b80ebd4d1f8c5 Mon Sep 17 00:00:00 2001 From: Matt Angus Date: Fri, 18 Oct 2024 17:09:26 +0100 Subject: [PATCH 19/19] Modernize cmake --- CMakeLists.txt | 21 +++++++++++---------- 1 file changed, 11 insertions(+), 10 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 8ae7c5d..dbaa425 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,14 +3,7 @@ project(bezier LANGUAGES CXX VERSION 0.3.2) -set(CMAKE_CXX_STANDARD 17) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - -add_compile_options(-fPIC -O2) - find_package(Eigen3 REQUIRED) -include_directories(SYSTEM ${EIGEN3_INCLUDE_DIR} include) set(Bezier_SRC ${PROJECT_SOURCE_DIR}/src/utils.cpp @@ -37,15 +30,23 @@ else() endif() target_include_directories(bezier PUBLIC - $ + $ $ ) +target_link_libraries(bezier PUBLIC Eigen3::Eigen) -set_target_properties(bezier PROPERTIES VERSION ${PROJECT_VERSION}) -set_target_properties(bezier PROPERTIES PUBLIC_HEADER "${Bezier_INC}") +set_target_properties(bezier PROPERTIES + VERSION ${PROJECT_VERSION} + CXX_EXTENSIONS OFF + POSITION_INDEPENDENT_CODE ON + PUBLIC_HEADER "${Bezier_INC}" +) +target_compile_features(bezier PUBLIC cxx_std_17) # install rules install(TARGETS bezier EXPORT bezier-export DESTINATION "lib" PUBLIC_HEADER DESTINATION "include/Bezier") install(EXPORT bezier-export DESTINATION "lib/cmake/Bezier" FILE BezierConfig.cmake) + +add_library(bezier::bezier ALIAS bezier)