365 lines
12 KiB
C++
365 lines
12 KiB
C++
#pragma once
|
||
|
||
/*
|
||
|
||
Line simplification based of an improved Douglas-Peucker algorithm with only
|
||
O(n) extra memory. Memory structures are inlined so that most simplifications
|
||
can be done with zero heap allocations.
|
||
|
||
There are two versions here, one that simplifies using a threshold, and another
|
||
version which will simplify to an exact number of points, however the latter is
|
||
expensive since it must re-run the algorithm multiple times to find the right
|
||
threshold. The first version is much faster and should be used in most cases.
|
||
|
||
*/
|
||
|
||
#include "fl/bitset.h"
|
||
#include "fl/math.h"
|
||
#include "fl/math_macros.h"
|
||
#include "fl/pair.h"
|
||
#include "fl/point.h"
|
||
#include "fl/span.h"
|
||
#include "fl/vector.h"
|
||
|
||
namespace fl {
|
||
|
||
template <typename NumberT = float> class LineSimplifier {
|
||
public:
|
||
// This line simplification algorithm will remove vertices that are close
|
||
// together upto a distance of mMinDistance. The algorithm is based on the
|
||
// Douglas-Peucker but with some tweaks for memory efficiency. Most common
|
||
// usage of this class for small sized inputs (~20) will produce no heap
|
||
// allocations.
|
||
using Point = fl::vec2<NumberT>;
|
||
using VectorPoint = fl::vector<Point>;
|
||
|
||
LineSimplifier() : mMinDistance(EPSILON_F) {}
|
||
LineSimplifier(const LineSimplifier &other) = default;
|
||
LineSimplifier &operator=(const LineSimplifier &other) = default;
|
||
LineSimplifier(LineSimplifier &&other) = default;
|
||
LineSimplifier &operator=(LineSimplifier &&other) = default;
|
||
|
||
explicit LineSimplifier(NumberT e) : mMinDistance(e) {}
|
||
void setMinimumDistance(NumberT eps) { mMinDistance = eps; }
|
||
|
||
// simplifyInPlace.
|
||
void simplifyInplace(fl::vector<Point> *polyline) {
|
||
simplifyInplaceT(polyline);
|
||
}
|
||
template <typename VectorType> void simplifyInplace(VectorType *polyLine) {
|
||
simplifyInplaceT(polyLine);
|
||
}
|
||
|
||
// simplify to the output vector.
|
||
void simplify(const fl::span<const Point> &polyLine,
|
||
fl::vector<Point> *out) {
|
||
simplifyT(polyLine, out);
|
||
}
|
||
template <typename VectorType>
|
||
void simplify(const fl::span<Point> &polyLine, VectorType *out) {
|
||
simplifyT(polyLine, out);
|
||
}
|
||
|
||
template <typename VectorType>
|
||
static void removeOneLeastError(VectorType *_poly) {
|
||
bitset<256> keep;
|
||
VectorType &poly = *_poly;
|
||
keep.assign(poly.size(), 1);
|
||
const int n = poly.size();
|
||
NumberT bestErr = INFINITY_DOUBLE;
|
||
int bestIdx = -1;
|
||
|
||
// scan all interior “alive” points
|
||
for (int i = 1; i + 1 < n; ++i) {
|
||
if (!keep[i])
|
||
continue;
|
||
|
||
// find previous alive
|
||
int L = i - 1;
|
||
while (L >= 0 && !keep[L])
|
||
--L;
|
||
// find next alive
|
||
int R = i + 1;
|
||
while (R < n && !keep[R])
|
||
++R;
|
||
|
||
if (L < 0 || R >= n)
|
||
continue; // endpoints
|
||
|
||
// compute perp‐distance² to the chord L→R
|
||
NumberT dx = poly[R].x - poly[L].x;
|
||
NumberT dy = poly[R].y - poly[L].y;
|
||
NumberT vx = poly[i].x - poly[L].x;
|
||
NumberT vy = poly[i].y - poly[L].y;
|
||
NumberT len2 = dx * dx + dy * dy;
|
||
NumberT err =
|
||
(len2 > NumberT(0))
|
||
? ((dx * vy - dy * vx) * (dx * vy - dy * vx) / len2)
|
||
: (vx * vx + vy * vy);
|
||
|
||
if (err < bestErr) {
|
||
bestErr = err;
|
||
bestIdx = i;
|
||
}
|
||
}
|
||
|
||
// now “remove” that one point
|
||
if (bestIdx >= 0)
|
||
// keep[bestIdx] = 0;
|
||
poly.erase(poly.begin() + bestIdx);
|
||
}
|
||
|
||
private:
|
||
template <typename VectorType> void simplifyInplaceT(VectorType *polyLine) {
|
||
// run the simplification algorithm
|
||
span<Point> slice(polyLine->data(), polyLine->size());
|
||
simplifyT(slice, polyLine);
|
||
}
|
||
|
||
template <typename VectorType>
|
||
void simplifyT(const fl::span<const Point> &polyLine, VectorType *out) {
|
||
// run the simplification algorithm
|
||
simplifyInternal(polyLine);
|
||
|
||
// copy the result to the output slice
|
||
out->assign(mSimplified.begin(), mSimplified.end());
|
||
}
|
||
// Runs in O(n) allocations: one bool‐array + one index stack + one output
|
||
// vector
|
||
void simplifyInternal(const fl::span<const Point> &polyLine) {
|
||
mSimplified.clear();
|
||
int n = polyLine.size();
|
||
if (n < 2) {
|
||
if (n) {
|
||
mSimplified.assign(polyLine.data(), polyLine.data() + n);
|
||
}
|
||
return;
|
||
}
|
||
const NumberT minDist2 = mMinDistance * mMinDistance;
|
||
|
||
// mark all points as “kept” initially
|
||
keep.assign(n, 1);
|
||
|
||
// explicit stack of (start,end) index pairs
|
||
indexStack.clear();
|
||
// indexStack.reserve(64);
|
||
indexStack.push_back({0, n - 1});
|
||
|
||
// process segments
|
||
while (!indexStack.empty()) {
|
||
// auto [i0, i1] = indexStack.back();
|
||
auto pair = indexStack.back();
|
||
int i0 = pair.first;
|
||
int i1 = pair.second;
|
||
indexStack.pop_back();
|
||
const bool has_interior = (i1 - i0) > 1;
|
||
if (!has_interior) {
|
||
// no interior points, just keep the endpoints
|
||
// keep[i0] = 1;
|
||
// keep[i1] = 1;
|
||
continue;
|
||
}
|
||
|
||
// find farthest point in [i0+1 .. i1-1]
|
||
NumberT maxDist2 = 0;
|
||
int split = i0;
|
||
for (int i = i0 + 1; i < i1; ++i) {
|
||
if (!keep[i])
|
||
continue;
|
||
NumberT d2 = PerpendicularDistance2(polyLine[i], polyLine[i0],
|
||
polyLine[i1]);
|
||
|
||
// FASTLED_WARN("Perpendicular distance2 between "
|
||
// << polyLine[i] << " and " << polyLine[i0]
|
||
// << " and " << polyLine[i1] << " is " << d2);
|
||
|
||
if (d2 > maxDist2) {
|
||
maxDist2 = d2;
|
||
split = i;
|
||
}
|
||
}
|
||
|
||
if (maxDist2 > minDist2) {
|
||
// need to keep that split point and recurse on both halves
|
||
indexStack.push_back({i0, split});
|
||
indexStack.push_back({split, i1});
|
||
} else {
|
||
// drop all interior points in this segment
|
||
for (int i = i0 + 1; i < i1; ++i) {
|
||
keep[i] = 0;
|
||
}
|
||
}
|
||
}
|
||
|
||
// collect survivors
|
||
mSimplified.clear();
|
||
mSimplified.reserve(n);
|
||
for (int i = 0; i < n; ++i) {
|
||
if (keep[i])
|
||
mSimplified.push_back(polyLine[i]);
|
||
}
|
||
}
|
||
|
||
private:
|
||
NumberT mMinDistance;
|
||
|
||
// workspace buffers
|
||
fl::bitset<256> keep; // marks which points survive
|
||
fl::vector_inlined<fl::pair<int, int>, 64>
|
||
indexStack; // manual recursion stack
|
||
VectorPoint mSimplified; // output buffer
|
||
|
||
static NumberT PerpendicularDistance2(const Point &pt, const Point &a,
|
||
const Point &b) {
|
||
// vector AB
|
||
NumberT dx = b.x - a.x;
|
||
NumberT dy = b.y - a.y;
|
||
// vector AP
|
||
NumberT vx = pt.x - a.x;
|
||
NumberT vy = pt.y - a.y;
|
||
|
||
// squared length of AB
|
||
NumberT len2 = dx * dx + dy * dy;
|
||
if (len2 <= NumberT(0)) {
|
||
// A and B coincide — just return squared dist from A to P
|
||
return vx * vx + vy * vy;
|
||
}
|
||
|
||
// cross‐product magnitude (AB × AP) in 2D is (dx*vy − dy*vx)
|
||
NumberT cross = dx * vy - dy * vx;
|
||
// |cross|/|AB| is the perpendicular distance; we want squared:
|
||
return (cross * cross) / len2;
|
||
}
|
||
};
|
||
|
||
template <typename NumberT = float> class LineSimplifierExact {
|
||
public:
|
||
LineSimplifierExact() = default;
|
||
using Point = vec2<NumberT>;
|
||
|
||
LineSimplifierExact(int count) : mCount(count) {}
|
||
|
||
void setCount(u32 count) { mCount = count; }
|
||
|
||
template <typename VectorType = fl::vector<Point>>
|
||
void simplifyInplace(VectorType *polyLine) {
|
||
return simplify(*polyLine, polyLine);
|
||
}
|
||
|
||
template <typename VectorType = fl::vector<Point>>
|
||
void simplify(const fl::span<const Point> &polyLine, VectorType *out) {
|
||
if (mCount > polyLine.size()) {
|
||
safeCopy(polyLine, out);
|
||
return;
|
||
} else if (mCount == polyLine.size()) {
|
||
safeCopy(polyLine, out);
|
||
return;
|
||
} else if (mCount < 2) {
|
||
fl::vector_fixed<Point, 2> temp;
|
||
if (polyLine.size() > 0) {
|
||
temp.push_back(polyLine[0]);
|
||
}
|
||
if (polyLine.size() > 1) {
|
||
temp.push_back(polyLine[polyLine.size() - 1]);
|
||
}
|
||
out->assign(temp.begin(), temp.end());
|
||
return;
|
||
}
|
||
NumberT est_max_dist = estimateMaxDistance(polyLine);
|
||
NumberT min = 0;
|
||
NumberT max = est_max_dist;
|
||
NumberT mid = (min + max) / 2.0f;
|
||
while (true) {
|
||
// min < max;
|
||
auto diff = max - min;
|
||
const bool done = (diff < 0.01f);
|
||
out->clear();
|
||
mLineSimplifier.setMinimumDistance(mid);
|
||
mLineSimplifier.simplify(polyLine, out);
|
||
|
||
fl::size n = out->size();
|
||
|
||
if (n == mCount) {
|
||
return; // we are done
|
||
}
|
||
|
||
// Handle the last few iterations manually. Often the algo will get
|
||
// stuck here.
|
||
if (n == mCount + 1) {
|
||
// Just one more left, so peel it off.
|
||
mLineSimplifier.removeOneLeastError(out);
|
||
return;
|
||
}
|
||
|
||
if (n == mCount + 2) {
|
||
// Just two more left, so peel them off.
|
||
mLineSimplifier.removeOneLeastError(out);
|
||
mLineSimplifier.removeOneLeastError(out);
|
||
return;
|
||
}
|
||
|
||
if (done) {
|
||
while (out->size() > mCount) {
|
||
// we have too many points, so we need to increase the
|
||
// distance
|
||
mLineSimplifier.removeOneLeastError(out);
|
||
}
|
||
return;
|
||
}
|
||
if (out->size() < mCount) {
|
||
max = mid;
|
||
} else {
|
||
min = mid;
|
||
}
|
||
mid = (min + max) / 2.0f;
|
||
}
|
||
}
|
||
|
||
private:
|
||
static NumberT estimateMaxDistance(const fl::span<const Point> &polyLine) {
|
||
// Rough guess: max distance between endpoints
|
||
if (polyLine.size() < 2)
|
||
return 0;
|
||
|
||
const Point &first = polyLine[0];
|
||
const Point &last = polyLine[polyLine.size() - 1];
|
||
NumberT dx = last.x - first.x;
|
||
NumberT dy = last.y - first.y;
|
||
return sqrt(dx * dx + dy * dy);
|
||
}
|
||
|
||
template <typename VectorType>
|
||
void safeCopy(const fl::span<const Point> &polyLine, VectorType *out) {
|
||
auto *first_out = out->data();
|
||
// auto* last_out = first_out + mCount;
|
||
auto *other_first_out = polyLine.data();
|
||
// auto* other_last_out = other_first_out + polyLine.size();
|
||
const bool is_same = first_out == other_first_out;
|
||
if (is_same) {
|
||
return;
|
||
}
|
||
auto *last_out = first_out + mCount;
|
||
auto *other_last_out = other_first_out + polyLine.size();
|
||
|
||
const bool is_overlapping =
|
||
(first_out >= other_first_out && first_out < other_last_out) ||
|
||
(other_first_out >= first_out && other_first_out < last_out);
|
||
|
||
if (!is_overlapping) {
|
||
out->assign(polyLine.data(), polyLine.data() + polyLine.size());
|
||
return;
|
||
}
|
||
|
||
// allocate a temporary buffer
|
||
fl::vector_inlined<Point, 64> temp;
|
||
temp.assign(polyLine.begin(), polyLine.end());
|
||
out->assign(temp.begin(), temp.end());
|
||
return;
|
||
}
|
||
|
||
u32 mCount = 10;
|
||
LineSimplifier<NumberT> mLineSimplifier;
|
||
};
|
||
|
||
} // namespace fl
|