geofence_utils: use shoelace formula to calculate proper inward vector

at vertice location

Signed-off-by: RomanBapst <bapstroman@gmail.com>

src/lib/geofence/geofence_utils.cpp

@@ -98,45 +98,59 @@ bool expandOrShrinkPolygon(const matrix::Vector2f *vertices_in, int num_vertices
 			   float margin, bool expand,
 			   matrix::Vector2f *vertices_out)
 {
+	if (num_vertices < 3) {
+		return false;
+	}
+
+	// shoelace formula used to check if the polygon is CCW or CW
+	// https://en.wikipedia.org/wiki/Shoelace_formula
+	float signed_area_2x = 0.f;
+
 	for (int i = 0; i < num_vertices; i++) {
-		int prev = (i + num_vertices - 1) % num_vertices;
+		const int j = (i + 1) % num_vertices;
-		int next = (i + 1) % num_vertices;
+		signed_area_2x += vertices_in[i](0) * vertices_in[j](1)
+				  - vertices_in[j](0) * vertices_in[i](1);
+	}
 
-		const matrix::Vector2f edge_prev = vertices_in[prev] - vertices_in[i];
+	if (fabsf(signed_area_2x) < FLT_EPSILON) {
-		const matrix::Vector2f edge_next = vertices_in[next] - vertices_in[i];
+		return false; // degenerate (zero-area) polygon
+	}
 
-		if (edge_prev.norm() < FLT_EPSILON || edge_next.norm() < FLT_EPSILON) {
+	// If area is positive, polygon is CCW and we want to rotate vector to the left to get inward normal
+	const float rot_sign = (signed_area_2x > 0.f) ? 1.f : -1.f;
+
+	// Expand pushes vertices outward, shrink pushes them inward.
+	const float step_sign = expand ? -1.f : 1.f;
+
+	for (int i = 0; i < num_vertices; i++) {
+		const int prev = (i + num_vertices - 1) % num_vertices;
+		const int next = (i + 1) % num_vertices;
+
+		const matrix::Vector2f edge_in  = vertices_in[i] - vertices_in[prev];
+		const matrix::Vector2f edge_out = vertices_in[next] - vertices_in[i];
+
+		if (edge_in.norm() < FLT_EPSILON || edge_out.norm() < FLT_EPSILON) {
 			return false;
 		}
 
-		// Normalized directions from vertex to its neighbors
+		const matrix::Vector2f edge_in_unit  = edge_in.normalized();
-		const matrix::Vector2f to_prev = edge_prev.normalized();
+		const matrix::Vector2f edge_out_unit = edge_out.normalized();
-		const matrix::Vector2f to_next = edge_next.normalized();
 
-		// Bisector points inward (toward the polygon interior)
+		// Unit inward normals of the two adjacent edges.
-		matrix::Vector2f bisector = to_prev + to_next;
+		const matrix::Vector2f n_in {-rot_sign * edge_in_unit(1), rot_sign * edge_in_unit(0)};
+		const matrix::Vector2f n_out{-rot_sign * edge_out_unit(1), rot_sign * edge_out_unit(0)};
 
-		if (bisector.length() < FLT_EPSILON) {
+		matrix::Vector2f bisector = n_in + n_out;
-			// this happens when all three vertices are exactly on one line
+		const float bisector_len = bisector.length();
-			bisector = matrix::Vector2f(-to_prev(1), to_prev(0)); // rotate 90 degrees to get a perpendicular direction
 
-		} else {
+		if (bisector_len < FLT_EPSILON) {
-			bisector.normalize();
+			// degenerate case, edges are antiparallel
+			return false;
 		}
 
-		const matrix::Vector2f test_point = vertices_in[i] + bisector;
+		bisector.normalize();
-		const bool test_point_inside = geofence_utils::insidePolygon(vertices_in, num_vertices, test_point);
 
-		float direction = 1.f;
+		vertices_out[i] = vertices_in[i] + bisector * margin * step_sign;
-
-		if (expand) {
-			direction = (test_point_inside) ? -1.f : 1.f;
-
-		} else {
-			direction = (test_point_inside) ? 1.f : -1.f;
-		}
-
-		vertices_out[i] = vertices_in[i] + bisector * margin * direction;
 	}
 
 	return true;

src/lib/geofence/geofence_utils.h

@@ -110,9 +110,14 @@ bool lineSegmentIntersectsCircle(const matrix::Vector2f &start, const matrix::Ve
 				 const matrix::Vector2f &center, float radius);
 
 /**
- * Offset polygon vertices expand or inward by computing the bisector
+ * Offset polygon vertices outward (expand) or inward (shrink) by `margin`.
- * of the two normalized edge directions at each vertex.
+ *
- * Works in local frame (meters).
+ * Determines winding direction once via the shoelace formula, then at each vertex
+ * averages the inward normals of the two adjacent edges to get the bisector.
+ * O(n) overall. Works in local frame (meters).
+ *
+ * Returns false for degenerate polygons: fewer than 3 vertices, zero signed area,
+ * zero-length edges, or antiparallel adjacent edges (polygon doubles back).
  *
  * @param vertices_in   input vertices in local frame
  * @param num_vertices  number of vertices