Implement polygon/polyline simplification and smoothing

core/math/2d/geometry/goost_geometry_2d.cpp

@@ -4,6 +4,8 @@
 #include "poly/decomp/poly_decomp.h"
 #include "poly/offset/poly_offset.h"
 
+#include "core/local_vector.h"
+
 Vector<Vector<Point2>> GoostGeometry2D::merge_polygons(const Vector<Point2> &p_polygon_a, const Vector<Point2> &p_polygon_b) {
 	Vector<Vector<Point2>> subject;
 	subject.push_back(p_polygon_a);
@@ -86,10 +88,143 @@ Vector<Vector<Point2>> GoostGeometry2D::decompose_polygon(const Vector<Point2> &
 	return PolyDecomp2D::decompose_polygons(polygons, PolyDecomp2D::DECOMP_CONVEX_HM);
 }
 
+struct IndicesStack {
+	LocalVector<int> stack;
+	uint32_t stack_size = 0;
+	uint32_t back = 0;
+
+	_FORCE_INLINE_ void push_back(int p_index) {
+		if (stack.size() == back) {
+			stack.push_back(p_index);
+		} else {
+			stack[back] = p_index;
+		}
+		++back;
+		++stack_size;
+	}
+	_FORCE_INLINE_ int pop_back() {
+		--stack_size;
+		return stack[--back];
+	}
+	_FORCE_INLINE_ bool is_empty() {
+		return stack_size == 0;
+	}
+	_FORCE_INLINE_ const int &operator[](int p_index) const {
+		return stack[p_index];
+	}
+};
+
+// Polyline decimation using Ramer-Douglas-Peucker (RDP) algorithm.
+Vector<Point2> GoostGeometry2D::simplify_polyline(const Vector<Point2> &p_polyline, real_t p_epsilon) {
+	if (p_polyline.size() <= 2) {
+		return p_polyline;
+	}
+	Vector<bool> points_to_retain;
+	points_to_retain.resize(p_polyline.size());
+
+	bool *retain = points_to_retain.ptrw();
+	memset(retain, 0, points_to_retain.size());
+
+	real_t eps = MAX(0.0, p_epsilon);
+	real_t distance_max = 0;
+	real_t distance = 0;
+
+	IndicesStack parts;
+	parts.stack.reserve(p_polyline.size() * 2);
+	parts.push_back(0);
+	parts.push_back(p_polyline.size() - 1);
+
+	retain[parts[0]] = true;
+	retain[parts[1]] = true;
+	int index = 0;
+
+	while (!parts.is_empty()) {
+		int second = parts.pop_back(); // Pop back in other order.
+		int first = parts.pop_back();
+
+		distance_max = 0;
+		Vector2 a = p_polyline[first];
+		Vector2 b = p_polyline[second];
+		Vector2 n = b - a;
+
+		for (int i = first + 1; i < second; ++i) {
+			Vector2 pa = a - p_polyline[i];
+			Vector2 c = n * pa.dot(n) / n.dot(n);
+			Vector2 d = pa - c;
+			distance = d.dot(d);
+			if (distance > distance_max) {
+				index = i;
+				distance_max = distance;
+			}
+		}
+		if (distance_max >= eps) {
+			retain[index] = true;
+			parts.push_back(first);
+			parts.push_back(index);
+			parts.push_back(index);
+			parts.push_back(second);
+		}
+	}
+	Vector<Vector2> ret;
+	for (int i = 0; i < p_polyline.size(); ++i) {
+		if (retain[i]) {
+			ret.push_back(p_polyline[i]);
+		}
+	}
+	return ret;
+}
+
+// Approximate polygon smoothing using Chaikin algorithm.
+// https://www.cs.unc.edu/~dm/UNC/COMP258/LECTURES/Chaikins-Algorithm.pdf
+//
+Vector<Point2> GoostGeometry2D::smooth_polygon_approx(const Vector<Point2> &p_polygon, int p_iterations, real_t p_cut_distance) {
+	ERR_FAIL_COND_V_MSG(p_polygon.size() < 3, Vector<Point2>(), "Bad polygon!");
+
+	Vector<Point2> subject = p_polygon;
+	const real_t cd = CLAMP(p_cut_distance, 0.0, 0.5);
+	for (int i = 0; i < p_iterations; ++i) {
+		Vector<Point2> smoothed;
+		for (int j = 0; j < subject.size(); ++j) {
+			const Point2 &p1 = subject[j];
+			const Point2 &p2 = subject[(j + 1) % subject.size()];
+			smoothed.push_back((1.0 - cd) * p1 + cd * p2); // Q
+			smoothed.push_back(cd * p1 + (1.0 - cd) * p2); // R
+		}
+		subject = smoothed;
+	}
+	return subject;
+}
+
+// Approximate polyline smoothing using Chaikin algorithm:
+// https://www.cs.unc.edu/~dm/UNC/COMP258/LECTURES/Chaikins-Algorithm.pdf
+//
+// Unlike polygon version, the endpoints are always retained.
+//
+Vector<Point2> GoostGeometry2D::smooth_polyline_approx(const Vector<Point2> &p_polyline, int p_iterations, real_t p_cut_distance) {
+	if (p_polyline.size() <= 2) {
+		return p_polyline;
+	}
+	Vector<Point2> subject = p_polyline;
+	const real_t cd = CLAMP(p_cut_distance, 0.0, 0.5);
+	for (int i = 0; i < p_iterations; ++i) {
+		Vector<Point2> smoothed;
+		smoothed.push_back(subject[0]); // Always add first point.
+		for (int j = 0; j < subject.size() - 1; ++j) {
+			const Point2 &p1 = subject[j];
+			const Point2 &p2 = subject[j + 1];
+			smoothed.push_back((1.0 - cd) * p1 + cd * p2); // Q
+			smoothed.push_back(cd * p1 + (1.0 - cd) * p2); // R
+		}
+		smoothed.push_back(subject[subject.size() - 1]); // Always add last point.
+		subject = smoothed;
+	}
+	return subject;
+}
+
+// "Calculating The Area And Centroid Of A Polygon" Written by Paul Bourke July 1988
+// https://www.seas.upenn.edu/~sys502/extra_materials/Polygon%20Area%20and%20Centroid.pdf
+//
 Point2 GoostGeometry2D::polygon_centroid(const Vector<Point2> &p_polygon) {
-	// Based on formulae from:
-	// "Calculating The Area And Centroid Of A Polygon" Written by Paul Bourke July 1988
-	// https://www.seas.upenn.edu/~sys502/extra_materials/Polygon%20Area%20and%20Centroid.pdf
 	int c = p_polygon.size();
 	ERR_FAIL_COND_V(c < 3, Vector2());
 
@@ -162,7 +297,7 @@ Rect2 GoostGeometry2D::bounding_rect(const Vector<Point2> &p_points) {
 	return rect;
 }
 
-// See "The Point in Polygon Problem for Arbitrary Polygons" by Hormann & Agathos
+// "The Point in Polygon Problem for Arbitrary Polygons" by Hormann & Agathos
 // http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.88.5498&rep=rep1&type=pdf
 //
 // Implementation ported from Clipper 6.4.2.

core/math/2d/geometry/goost_geometry_2d.h

@@ -22,6 +22,11 @@ class GoostGeometry2D {
 	static Vector<Vector<Point2>> triangulate_polygon(const Vector<Point2> &p_polygon);
 	static Vector<Vector<Point2>> decompose_polygon(const Vector<Point2> &p_polygon);
 
+	/* Polygon/Polyline simplification and smoothing */
+	static Vector<Point2> simplify_polyline(const Vector<Point2> &p_polyline, real_t p_epsilon);
+	static Vector<Point2> smooth_polygon_approx(const Vector<Point2> &p_polygon, int p_iterations = 1, real_t p_cut_distance = 0.25);
+	static Vector<Point2> smooth_polyline_approx(const Vector<Point2> &p_polyline, int p_iterations = 1, real_t p_cut_distance = 0.25);
+
 	/* Polygon/Polyline attributes */
 	static Point2 polygon_centroid(const Vector<Point2> &p_polygon);
 	static real_t polygon_area(const Vector<Point2> &p_polygon);

core/math/2d/geometry/goost_geometry_2d_bind.cpp

@@ -102,6 +102,18 @@ Array _GoostGeometry2D::decompose_polygon(const Vector<Point2> &p_polygon) const
 	return ret;
 }
 
+Vector<Point2> _GoostGeometry2D::simplify_polyline(const Vector<Point2> &p_polyline, real_t p_epsilon) const {
+	return GoostGeometry2D::simplify_polyline(p_polyline, p_epsilon);
+}
+
+Vector<Point2> _GoostGeometry2D::smooth_polygon_approx(const Vector<Point2> &p_polygon, int p_iterations, real_t cut_distance) const {
+	return GoostGeometry2D::smooth_polygon_approx(p_polygon, p_iterations, cut_distance);
+}
+
+Vector<Point2> _GoostGeometry2D::smooth_polyline_approx(const Vector<Point2> &p_polyline, int p_iterations, real_t cut_distance) const {
+	return GoostGeometry2D::smooth_polyline_approx(p_polyline, p_iterations, cut_distance);
+}
+
 Vector2 _GoostGeometry2D::polygon_centroid(const Vector<Vector2> &p_polygon) const {
 	return GoostGeometry2D::polygon_centroid(p_polygon);
 }
@@ -159,6 +171,10 @@ void _GoostGeometry2D::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("triangulate_polygon", "polygon"), &_GoostGeometry2D::triangulate_polygon);
 	ClassDB::bind_method(D_METHOD("decompose_polygon", "polygon"), &_GoostGeometry2D::decompose_polygon);
 
+	ClassDB::bind_method(D_METHOD("simplify_polyline", "polyline", "epsilon"), &_GoostGeometry2D::simplify_polyline);
+	ClassDB::bind_method(D_METHOD("smooth_polygon_approx", "polygon", "iterations", "cut_distance"), &_GoostGeometry2D::smooth_polygon_approx, DEFVAL(1), DEFVAL(0.25));
+	ClassDB::bind_method(D_METHOD("smooth_polyline_approx", "polyline", "iterations", "cut_distance"), &_GoostGeometry2D::smooth_polyline_approx, DEFVAL(1), DEFVAL(0.25));
+
 	ClassDB::bind_method(D_METHOD("polygon_centroid", "polygon"), &_GoostGeometry2D::polygon_centroid);
 	ClassDB::bind_method(D_METHOD("polygon_area", "polygon"), &_GoostGeometry2D::polygon_area);
 	ClassDB::bind_method(D_METHOD("polygon_perimeter", "polygon"), &_GoostGeometry2D::polygon_perimeter);

core/math/2d/geometry/goost_geometry_2d_bind.h

@@ -30,6 +30,10 @@ class _GoostGeometry2D : public Object {
 	Array triangulate_polygon(const Vector<Point2> &p_polygon) const;
 	Array decompose_polygon(const Vector<Point2> &p_polygon) const;
 
+	Vector<Point2> simplify_polyline(const Vector<Point2> &p_polyline, real_t p_epsilon) const;
+	Vector<Point2> smooth_polygon_approx(const Vector<Point2> &p_polygon, int p_iterations = 1, real_t cut_distance = 0.25) const;
+	Vector<Point2> smooth_polyline_approx(const Vector<Point2> &p_polyline, int p_iterations = 1, real_t cut_distance = 0.25) const;
+
 	Vector2 polygon_centroid(const Vector<Vector2> &p_polygon) const;
 	real_t polygon_area(const Vector<Vector2> &p_polygon) const;
 	real_t polygon_perimeter(const Vector<Vector2> &p_polygon) const;

doc/GoostGeometry2D.xml

@@ -210,6 +210,44 @@
 				The order of vertices returned is counterclockwise which makes it an outer polygon by default. To convert it to an inner polygon specifically, use [method PoolVector2Array.invert].
 			</description>
 		</method>
+		<method name="simplify_polyline" qualifiers="const">
+			<return type="PoolVector2Array">
+			</return>
+			<argument index="0" name="polyline" type="PoolVector2Array">
+			</argument>
+			<argument index="1" name="epsilon" type="float">
+			</argument>
+			<description>
+				Simplifies a polyline by reducing the number of points using the Ramer-Douglas-Peucker (RDP) algorithm. Higher [code]epsilon[/code] values result in fewer points retained.
+			</description>
+		</method>
+		<method name="smooth_polygon_approx" qualifiers="const">
+			<return type="PoolVector2Array">
+			</return>
+			<argument index="0" name="polygon" type="PoolVector2Array">
+			</argument>
+			<argument index="1" name="iterations" type="int" default="1">
+			</argument>
+			<argument index="2" name="cut_distance" type="float" default="0.25">
+			</argument>
+			<description>
+				Approximately smoothers the polygon using the Chaikin's algorithm resulting in larger number of vertices. Number of [code]iterations[/code] can be specified to produce smoother polygons. The [code]cut_distance[/code] determines at what distance new control points are selected from segments.
+			</description>
+		</method>
+		<method name="smooth_polyline_approx" qualifiers="const">
+			<return type="PoolVector2Array">
+			</return>
+			<argument index="0" name="polyline" type="PoolVector2Array">
+			</argument>
+			<argument index="1" name="iterations" type="int" default="1">
+			</argument>
+			<argument index="2" name="cut_distance" type="float" default="0.25">
+			</argument>
+			<description>
+				Approximately smoothers the polyline using the Chaikin's algorithm resulting in larger number of vertices. Number of [code]iterations[/code] can be specified to produce smoother polylines. The [code]cut_distance[/code] determines at what distance new control points are selected from segments.
+				Unlike [method smooth_polygon_approx], this method always retains start and end points from the original polyline.
+			</description>
+		</method>
 		<method name="triangulate_polygon" qualifiers="const">
 			<return type="Array">
 			</return>

tests/project/goost/core/math/2d/geometry/test_geometry_2d.gd

@@ -185,3 +185,58 @@ func test_bresenham_line():
 	assert_eq(line[13], Vector2(8, 2))
 	assert_eq(line[14], Vector2(9, 2))
 	assert_eq(line[15], Vector2(10, 3))
+
+
+func test_simplify_polyline():
+	var input = [Vector2(20, 51), Vector2(32, 13), Vector2(34, 13), Vector2(37, 13), Vector2(40, 18), Vector2(47, 46)]
+	var control = [Vector2(20, 51), Vector2(32, 13), Vector2(40, 18), Vector2(47, 46)]
+	var simplified = GoostGeometry2D.simplify_polyline(input, 10.0)
+
+	if simplified.size() != control.size():
+		assert_eq(simplified.size(), control.size(), "Point count mismatch")
+		return
+	for i in simplified.size():
+		assert_eq(simplified[i], control[i])
+
+
+func test_smooth_polygon_approx():
+	var input = [Vector2(25, 83), Vector2(49, 16), Vector2(66, 79)]
+	var control = [Vector2(31, 66.25), Vector2(43, 32.75), Vector2(53.25, 31.75), Vector2(61.75, 63.25), Vector2(55.75, 80), Vector2(35.25, 82)]
+	var smoothed = GoostGeometry2D.smooth_polygon_approx(input)
+
+	if smoothed.size() != control.size():
+		assert_eq(smoothed.size(), control.size(), "Point count mismatch")
+		return
+	for i in smoothed.size():
+		assert_eq(smoothed[i], control[i])
+
+
+func test_smooth_polyline_approx():
+	var input = [Vector2(25, 83), Vector2(49, 16), Vector2(66, 79)]
+	var control = [Vector2(25, 83), Vector2(31, 66.25), Vector2(43, 32.75), Vector2(53.25, 31.75), Vector2(61.75, 63.25), Vector2(66, 79)]
+	var smoothed = GoostGeometry2D.smooth_polyline_approx(input)
+
+	if smoothed.size() != control.size():
+		assert_eq(smoothed.size(), control.size(), "Point count mismatch")
+		return
+
+	# Always includes first and last points from input polyline.
+	assert_eq(smoothed[0], input[0])
+	assert_eq(smoothed[smoothed.size() - 1], input[input.size() - 1])
+
+	for i in smoothed.size():
+		assert_eq(smoothed[i], control[i])
+
+
+class Stress extends "res://addons/gut/test.gd":
+	func test_simplify_polyline():
+		var time = 0
+		var input = GoostGeometry2D.circle(1024)
+		for i in input.size():
+			input[i] += Random2D.point_in_circle(100)
+		for i in 100000:
+			var t1 = OS.get_ticks_msec()
+			var _simplified = GoostGeometry2D.simplify_polyline(input, 100.0)
+			var t2 = OS.get_ticks_msec()
+			time += t2 - t1
+		gut.p(time / 100000.0)