Fix polygon generation in BitMap

2 files changed, 124 insertions, 40 deletions

diff --git a/scene/resources/bit_map.cpp b/scene/resources/bit_map.cpp
index 1b06e09bb8..4afc82576d 100644
--- a/scene/resources/bit_map.cpp
+++ b/scene/resources/bit_map.cpp
@@ -169,7 +169,15 @@ Dictionary BitMap::_get_data() const {
 	return d;
 }
 
-Vector<Vector2> BitMap::_march_square(const Rect2i &p_rect, const Point2i &p_start) const {
+struct CrossStackEntry {
+	Point2i cross;
+	Vector<int> ranges;
+
+	_FORCE_INLINE_ bool operator==(const CrossStackEntry &p_other) const { return cross == p_other.cross; }
+	_FORCE_INLINE_ bool operator!=(const CrossStackEntry &p_other) const { return cross != p_other.cross; }
+};
+
+Vector<Vector<Vector2>> BitMap::_march_square(const Rect2i &p_rect, const Point2i &p_start) const {
 	int stepx = 0;
 	int stepy = 0;
 	int prevx = 0;
@@ -179,9 +187,20 @@ Vector<Vector2> BitMap::_march_square(const Rect2i &p_rect, const Point2i &p_sta
 	int curx = startx;
 	int cury = starty;
 	unsigned int count = 0;
-	HashSet<Point2i> case9s;
-	HashSet<Point2i> case6s;
-	Vector<Vector2> _points;
+
+	Vector<CrossStackEntry> cross_stack;
+	int cross_stack_size = 0;
+
+	// Add starting point to stack as the default entry.
+	cross_stack.push_back({ Point2i(-1, -1), Vector<int>({ 0 }) });
+	cross_stack_size++;
+
+	Vector<Point2i> _points;
+	Vector<Vector<Vector2>> ret;
+
+	// Add starting entry at start of return.
+	ret.resize(1);
+
 	do {
 		int sv = 0;
 		{ // Square value
@@ -202,7 +221,7 @@ Vector<Vector2> BitMap::_march_square(const Rect2i &p_rect, const Point2i &p_sta
 			sv += (p_rect.has_point(bl) && get_bitv(bl)) ? 4 : 0;
 			Point2i br = Point2i(curx, cury);
 			sv += (p_rect.has_point(br) && get_bitv(br)) ? 8 : 0;
-			ERR_FAIL_COND_V(sv == 0 || sv == 15, Vector<Vector2>());
+			ERR_FAIL_COND_V(sv == 0 || sv == 15, Vector<Vector<Vector2>>());
 		}
 
 		switch (sv) {
@@ -266,70 +285,139 @@ Vector<Vector2> BitMap::_march_square(const Rect2i &p_rect, const Point2i &p_sta
 				stepy = 0;
 				break;
 			case 9:
-				/*
+				/* Going DOWN if coming from the LEFT, otherwise go UP.
+				9
 				+---+---+
 				| 1 |   |
 				+---+---+
 				|   | 8 |
 				+---+---+
-				this should normally go UP, but if we already been here, we go down
 				*/
-				if (case9s.has(Point2i(curx, cury))) {
-					//found, so we go down, and delete from case9s;
+
+				if (prevx == 1) {
 					stepx = 0;
 					stepy = 1;
-					case9s.erase(Point2i(curx, cury));
 				} else {
-					//not found, we go up, and add to case9s;
 					stepx = 0;
 					stepy = -1;
-					case9s.insert(Point2i(curx, cury));
 				}
 				break;
 			case 6:
-				/*
+				/* Going RIGHT if coming from BELOW, otherwise go LEFT.
 				6
 				+---+---+
 				|   | 2 |
 				+---+---+
 				| 4 |   |
 				+---+---+
-				this normally go RIGHT, but if it's coming from RIGHT, it should go LEFT
 				*/
-				if (case6s.has(Point2i(curx, cury))) {
-					//found, so we go left, and delete from case6s;
-					stepx = -1;
+
+				if (prevy == -1) {
+					stepx = 1;
 					stepy = 0;
-					case6s.erase(Point2i(curx, cury));
 				} else {
-					//not found, we go right, and add to case6s;
-					stepx = 1;
+					stepx = -1;
 					stepy = 0;
-					case6s.insert(Point2i(curx, cury));
 				}
 				break;
 			default:
 				ERR_PRINT("this shouldn't happen.");
 		}
+
+		// Handle crossing points.
+		if (sv == 6 || sv == 9) {
+			const int new_index = _points.size() - 1;
+
+			// Add previous point to last stack entry.
+			cross_stack.write[cross_stack_size - 1].ranges.push_back(new_index);
+
+			// Create temporary entry to maybe insert, for searching.
+			const CrossStackEntry new_entry = { _points[new_index], Vector<int>({ new_index }) };
+
+			// Attempt to find matching entry.
+			const int found = cross_stack.rfind(new_entry, cross_stack_size - 1);
+
+			if (found != -1) {
+				Vector<Vector2> tmp;
+
+				// Iterate over entries between end of stack and found, adding ranges to result.
+				for (int i = found; i < cross_stack_size; i++) {
+					const Vector<int> &ranges = cross_stack[i].ranges;
+
+					for (int j = 0; j < ranges.size() / 2; j++) {
+						int first = ranges[2 * j];
+						const int last = ranges[2 * j + 1];
+
+						int new_pos = tmp.size();
+
+						tmp.resize(tmp.size() + (last - first));
+
+						Vector2 *tmp_ptrw = tmp.ptrw();
+
+						for (; first < last; first++, new_pos++) {
+							tmp_ptrw[new_pos].x = (float)(_points[first].x - p_rect.position.x);
+							tmp_ptrw[new_pos].y = (float)(_points[first].y - p_rect.position.y);
+						}
+					}
+				}
+
+				ret.push_back(tmp);
+
+				// Shrink stack.
+				cross_stack_size = found;
+
+				// Add previous point to last stack entry.
+				cross_stack.write[cross_stack_size - 1].ranges.push_back(new_index);
+			} else {
+				cross_stack.resize(MAX(cross_stack_size + 1, cross_stack.size()));
+				cross_stack.set(cross_stack_size, new_entry);
+				cross_stack_size++;
+			}
+		}
+
 		// Small optimization:
 		// If the previous direction is same as the current direction,
 		// then we should modify the last vector to current.
 		curx += stepx;
 		cury += stepy;
 		if (stepx == prevx && stepy == prevy) {
-			_points.write[_points.size() - 1].x = (float)(curx - p_rect.position.x);
-			_points.write[_points.size() - 1].y = (float)(cury + p_rect.position.y);
+			_points.write[_points.size() - 1].x = curx;
+			_points.write[_points.size() - 1].y = cury;
 		} else {
-			_points.push_back(Vector2((float)(curx - p_rect.position.x), (float)(cury + p_rect.position.y)));
+			_points.push_back(Point2i(curx, cury));
 		}
 
 		count++;
 		prevx = stepx;
 		prevy = stepy;
 
-		ERR_FAIL_COND_V((int)count > width * height, _points);
+		ERR_FAIL_COND_V((int)count > width * height, Vector<Vector<Vector2>>());
 	} while (curx != startx || cury != starty);
-	return _points;
+
+	// Add last position to last stack entry.
+	cross_stack.write[cross_stack_size - 1].ranges.push_back(_points.size());
+
+	for (int i = 0; i < cross_stack_size; i++) {
+		const Vector<int> &ranges = cross_stack[i].ranges;
+
+		for (int j = 0; j < ranges.size() / 2; j++) {
+			int first = ranges[2 * j];
+			const int last = ranges[2 * j + 1];
+
+			int new_pos = ret[0].size();
+
+			ret.write[0].resize(ret[0].size() + (last - first));
+
+			Vector2 *tmp_ptrw = ret.write[0].ptrw();
+
+			for (; first < last; first++, new_pos++) {
+				tmp_ptrw[new_pos].x = (float)(_points[first].x - p_rect.position.x);
+				tmp_ptrw[new_pos].y = (float)(_points[first].y - p_rect.position.y);
+			}
+		}
+	}
+
+	return ret;
 }
 
 static float perpendicular_distance(const Vector2 &i, const Vector2 &start, const Vector2 &end) {
@@ -442,7 +530,7 @@ static void fill_bits(const BitMap *p_src, Ref<BitMap> &p_map, const Point2i &p_
 			for (int j = next_j; j <= pos.y + 1; j++) {
 				if (popped) {
 					// The next loop over j must start normally.
-					next_j = pos.y;
+					next_j = pos.y - 1;
 					popped = false;
 					// Skip because an iteration was already executed with current counter values.
 					continue;
@@ -486,13 +574,10 @@ static void fill_bits(const BitMap *p_src, Ref<BitMap> &p_map, const Point2i &p_
 			}
 		}
 	} while (reenter || popped);
-
-	print_verbose("BitMap: Max stack size: " + itos(stack.size()));
 }
 
 Vector<Vector<Vector2>> BitMap::clip_opaque_to_polygons(const Rect2i &p_rect, float p_epsilon) const {
 	Rect2i r = Rect2i(0, 0, width, height).intersection(p_rect);
-	print_verbose("BitMap: Rect: " + r);
 
 	Point2i from;
 	Ref<BitMap> fill;
@@ -505,17 +590,16 @@ Vector<Vector<Vector2>> BitMap::clip_opaque_to_polygons(const Rect2i &p_rect, fl
 			if (!fill->get_bit(j, i) && get_bit(j, i)) {
 				fill_bits(this, fill, Point2i(j, i), r);
 
-				Vector<Vector2> polygon = _march_square(r, Point2i(j, i));
-				print_verbose("BitMap: Pre reduce: " + itos(polygon.size()));
-				polygon = reduce(polygon, r, p_epsilon);
-				print_verbose("BitMap: Post reduce: " + itos(polygon.size()));
+				for (Vector<Vector2> polygon : _march_square(r, Point2i(j, i))) {
+					polygon = reduce(polygon, r, p_epsilon);
 
-				if (polygon.size() < 3) {
-					print_verbose("Invalid polygon, skipped");
-					continue;
-				}
+					if (polygon.size() < 3) {
+						print_verbose("Invalid polygon, skipped");
+						continue;
+					}
 
-				polygons.push_back(polygon);
+					polygons.push_back(polygon);
+				}
 			}
 		}
 	}
diff --git a/scene/resources/bit_map.h b/scene/resources/bit_map.h
index 291ed8c4d0..0ec5772fd1 100644
--- a/scene/resources/bit_map.h
+++ b/scene/resources/bit_map.h
@@ -46,7 +46,7 @@ class BitMap : public Resource {
 	int width = 0;
 	int height = 0;
 
-	Vector<Vector2> _march_square(const Rect2i &p_rect, const Point2i &p_start) const;
+	Vector<Vector<Vector2>> _march_square(const Rect2i &p_rect, const Point2i &p_start) const;
 
 	TypedArray<PackedVector2Array> _opaque_to_polygons_bind(const Rect2i &p_rect, float p_epsilon) const;