From 345edb42db0f5ecf322b1c5f9013fb25e05d87e9 Mon Sep 17 00:00:00 2001 From: Ishan Agrawal Date: Tue, 10 Feb 2026 07:18:37 +0800 Subject: [PATCH 1/3] Created centroid function. Need to test more and use it to scale the bounds --- include/pathing/environment.hpp | 9 ++++++ src/pathing/environment.cpp | 46 ++++++++++++++++++++++++++++ tests/unit/pathing/centroid_test.cpp | 24 +++++++++++++++ 3 files changed, 79 insertions(+) create mode 100644 tests/unit/pathing/centroid_test.cpp diff --git a/include/pathing/environment.hpp b/include/pathing/environment.hpp index 892b6194..823e2fd8 100644 --- a/include/pathing/environment.hpp +++ b/include/pathing/environment.hpp @@ -258,6 +258,15 @@ class Environment { std::pair estimateAreaCoveredAndPathLength( const std::vector& goals) const; + /** + * Find the centroid of a given valid region. + * This rests on the assumption that the region + * is in an non-intersecting order. + * + * @return the centroid coordinate + */ + XYZCoord findCentroid() const; + /** * Returns a new polygon that is scaled by a given factor * diff --git a/src/pathing/environment.cpp b/src/pathing/environment.cpp index 60f19393..7fbd956f 100644 --- a/src/pathing/environment.cpp +++ b/src/pathing/environment.cpp @@ -492,3 +492,49 @@ std::pair, std::pair> Environment::fin return {{min_x, max_x}, {min_y, max_y}}; } + +XYZCoord Environment::findCentroid() const { + + if (valid_region.size() < 3) { + throw std::invalid_argument("valid_region field must have at least 3 points."); + } + + size_t n_verticies = valid_region.size(); + + double centroid_x = 0.0; + double centroid_y = 0.0; + double signed_area = 0.0; + + for (size_t i = 0; i < n_verticies - 1; ++i) { + + double x0 = valid_region[i].x; + double y0 = valid_region[i].y; + double x1 = valid_region[i+1].x; + double y1 = valid_region[i+1].y; + + double delta_area = (x0 * y1) - (x1 * y0); + signed_area += delta_area; + centroid_x += (x0 + x1) * delta_area; + centroid_y += (y0 + y1) * delta_area; + } + + double x0 = valid_region[n_verticies - 1].x; + double y0 = valid_region[n_verticies - 1].y; + double x1 = valid_region[0].x; + double y1 = valid_region[0].y; + + double delta_area = (x0 * y1) - (x1 * y0); + + signed_area += delta_area; + centroid_x += (x0 + x1) * delta_area; + centroid_y += (y0 + y1) * delta_area; + + if (signed_area == 0.0) { + throw std::runtime_error("The points are colinear. No centroid exists"); + } + + centroid_x /= (3.0*signed_area); + centroid_y /= (3.0*signed_area); + + return XYZCoord{centroid_x, centroid_y, 0}; +} \ No newline at end of file diff --git a/tests/unit/pathing/centroid_test.cpp b/tests/unit/pathing/centroid_test.cpp new file mode 100644 index 00000000..0cf9f427 --- /dev/null +++ b/tests/unit/pathing/centroid_test.cpp @@ -0,0 +1,24 @@ + +#include +#include "pathing/environment.hpp" +#include + +TEST(Centroid, Square) { + std::vector contour { + {0.0, 0.0, 0.0}, + {10.0, 0.0, 0.0}, + {10.0, 10.0, 0.0}, + {0.0, 10.0, 0.0} + }; + + XYZCoord centroid = XYZCoord(5.0, 5.0, 0.0); + Environment env = Environment(contour, {}, {}, {}, {}); + XYZCoord predCentroid = env.findCentroid(); + + std::cout << centroid.x << '\n'; + std::cout << predCentroid.x; + + EXPECT_EQ(centroid.x, predCentroid.x); + EXPECT_EQ(centroid.y, predCentroid.y); + EXPECT_EQ(centroid.z, predCentroid.z); +} \ No newline at end of file From e31212fb636adf7197b3ed2e853de7bd43792b9a Mon Sep 17 00:00:00 2001 From: Ishan Agrawal Date: Thu, 19 Feb 2026 10:54:13 +0800 Subject: [PATCH 2/3] Scale enviornment by fixed amount --- configs/dev.json | 3 +- include/pathing/environment.hpp | 5 +- include/utilities/obc_config.hpp | 1 + src/pathing/environment.cpp | 70 ++++++++++++---------------- src/pathing/static.cpp | 2 +- tests/unit/pathing/centroid_test.cpp | 27 ++++++----- 6 files changed, 54 insertions(+), 54 deletions(-) diff --git a/configs/dev.json b/configs/dev.json index e55ddd1f..c7ac9d88 100644 --- a/configs/dev.json +++ b/configs/dev.json @@ -28,7 +28,8 @@ "optimize": true, "point_fetch_method": "nearest", "allowed_to_skip_waypoints": false, - "generate_deviations": false + "generate_deviations": false, + "environment_offset": 10 }, "coverage": { "altitude_m": 30.0, diff --git a/include/pathing/environment.hpp b/include/pathing/environment.hpp index 823e2fd8..ead0a81b 100644 --- a/include/pathing/environment.hpp +++ b/include/pathing/environment.hpp @@ -23,7 +23,7 @@ class Environment { public: Environment(const Polygon& valid_region, const Polygon& airdrop_zone, const Polygon& mapping_region, const std::vector& goals, - const std::vector& obstacles); + const std::vector& obstacles, double environment_offset); /** * Check if a point is in the valid region @@ -265,7 +265,7 @@ class Environment { * * @return the centroid coordinate */ - XYZCoord findCentroid() const; + Polygon scaleFixedDistance(double distance, const Polygon& shape) const; /** * Returns a new polygon that is scaled by a given factor @@ -282,6 +282,7 @@ class Environment { const Polygon mapping_region; // boundary of the mapping region (subset of valid_region) const std::vector goals; // goal point const std::vector obstacles; // obstacles in the map + const double environment_offset; int goals_found; // whether or not the goal has been found, once it becomes ture, it will never // be false again diff --git a/include/utilities/obc_config.hpp b/include/utilities/obc_config.hpp index 186555a1..05906def 100644 --- a/include/utilities/obc_config.hpp +++ b/include/utilities/obc_config.hpp @@ -68,6 +68,7 @@ struct RRTConfig { bool allowed_to_skip_waypoints; // if true, will skip waypoints if it can not connect after 1 // RRT iteration bool generate_deviations; + double environment_offset; }; namespace AirdropCoverageMethod { diff --git a/src/pathing/environment.cpp b/src/pathing/environment.cpp index 7fbd956f..5296f815 100644 --- a/src/pathing/environment.cpp +++ b/src/pathing/environment.cpp @@ -12,14 +12,15 @@ Environment::Environment(const Polygon& valid_region, const Polygon& airdrop_zone, const Polygon& mapping_region, const std::vector& goals, - const std::vector& obstacles) + const std::vector& obstacles, double environment_offset) : valid_region(valid_region), airdrop_zone(airdrop_zone), mapping_region(mapping_region), goals(goals), goals_found(0), bounds(findBounds(valid_region)), - obstacles(obstacles) {} + obstacles(obstacles), + environment_offset(environment_offset) {} bool Environment::isPointInBounds(const XYZCoord& point) const { if (!isPointInPolygon(valid_region, point)) { @@ -493,48 +494,39 @@ std::pair, std::pair> Environment::fin return {{min_x, max_x}, {min_y, max_y}}; } -XYZCoord Environment::findCentroid() const { - - if (valid_region.size() < 3) { - throw std::invalid_argument("valid_region field must have at least 3 points."); - } - - size_t n_verticies = valid_region.size(); - - double centroid_x = 0.0; - double centroid_y = 0.0; - double signed_area = 0.0; +Polygon Environment::scaleFixedDistance(double distance, const Polygon& shape) const { + + Polygon reduced_polygon; - for (size_t i = 0; i < n_verticies - 1; ++i) { - - double x0 = valid_region[i].x; - double y0 = valid_region[i].y; - double x1 = valid_region[i+1].x; - double y1 = valid_region[i+1].y; + // square bounds of the polygon + auto bounds = findBounds(shape); + auto [x_min, x_max] = bounds.first; + auto [y_min, y_max] = bounds.second; - double delta_area = (x0 * y1) - (x1 * y0); - signed_area += delta_area; - centroid_x += (x0 + x1) * delta_area; - centroid_y += (y0 + y1) * delta_area; - } + // finds the center of the polygon + double x_center = (x_max + x_min) / 2; + double y_center = (y_max + y_min) / 2; - double x0 = valid_region[n_verticies - 1].x; - double y0 = valid_region[n_verticies - 1].y; - double x1 = valid_region[0].x; - double y1 = valid_region[0].y; + for (const XYZCoord& point : shape) { - double delta_area = (x0 * y1) - (x1 * y0); - - signed_area += delta_area; - centroid_x += (x0 + x1) * delta_area; - centroid_y += (y0 + y1) * delta_area; + double distance_x = (point.x - x_center); + double distance_y = (point.y - y_center); - if (signed_area == 0.0) { - throw std::runtime_error("The points are colinear. No centroid exists"); - } + if (distance_x > 0) { + distance_x -= distance; + } else { + distance_x += distance; + } - centroid_x /= (3.0*signed_area); - centroid_y /= (3.0*signed_area); + if (distance_y > 0) { + distance_y -= distance; + } else { + distance_y += distance; + } - return XYZCoord{centroid_x, centroid_y, 0}; + double new_x_coord = x_center + distance_x; + double new_y_coord = y_center + distance_y; + reduced_polygon.push_back(XYZCoord(new_x_coord, new_y_coord, 0)); + } + return reduced_polygon; } \ No newline at end of file diff --git a/src/pathing/static.cpp b/src/pathing/static.cpp index edaf2b1b..647a4ff7 100644 --- a/src/pathing/static.cpp +++ b/src/pathing/static.cpp @@ -25,7 +25,7 @@ RRT::RRT(RRTPoint start, std::vector goals, double search_radius, Poly : iterations_per_waypoint(config.pathing.rrt.iterations_per_waypoint), search_radius(search_radius), rewire_radius(config.pathing.rrt.rewire_radius), - tree(start, Environment(bounds, {}, {}, goals, obstacles), + tree(start, Environment(bounds, {}, {}, goals, obstacles, config.pathing.rrt.environment_offset), Dubins(config.pathing.dubins.turning_radius, config.pathing.dubins.point_separation)), config(config.pathing.rrt) { if (angles.size() != 0) { diff --git a/tests/unit/pathing/centroid_test.cpp b/tests/unit/pathing/centroid_test.cpp index 0cf9f427..b8e26d82 100644 --- a/tests/unit/pathing/centroid_test.cpp +++ b/tests/unit/pathing/centroid_test.cpp @@ -3,7 +3,8 @@ #include "pathing/environment.hpp" #include -TEST(Centroid, Square) { +TEST(ScaleContour, Square) { + std::vector contour { {0.0, 0.0, 0.0}, {10.0, 0.0, 0.0}, @@ -11,14 +12,18 @@ TEST(Centroid, Square) { {0.0, 10.0, 0.0} }; - XYZCoord centroid = XYZCoord(5.0, 5.0, 0.0); - Environment env = Environment(contour, {}, {}, {}, {}); - XYZCoord predCentroid = env.findCentroid(); - - std::cout << centroid.x << '\n'; - std::cout << predCentroid.x; + std::vector contour_scaled { + {2.0, 2.0, 0.0}, + {8.0, 2.0, 0.0}, + {8.0, 8.0, 0.0}, + {2.0, 8.0, 0.0} + }; + + Environment env = Environment({}, {}, {}, {}, {}); + Polygon pred_contour = env.scaleFixedDistance(2, contour); - EXPECT_EQ(centroid.x, predCentroid.x); - EXPECT_EQ(centroid.y, predCentroid.y); - EXPECT_EQ(centroid.z, predCentroid.z); -} \ No newline at end of file + for (int i = 0; i < pred_contour.size(); ++i) { + EXPECT_EQ(pred_contour[i].x, contour_scaled[i].x); + EXPECT_EQ(pred_contour[i].y, contour_scaled[i].y); + } +} From 6d087686ea3cd45fbbb9cf324c3e4af33542bdc3 Mon Sep 17 00:00:00 2001 From: Ishan Agrawal Date: Thu, 19 Feb 2026 11:05:50 +0800 Subject: [PATCH 3/3] default arg for constructor --- include/pathing/environment.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/pathing/environment.hpp b/include/pathing/environment.hpp index ead0a81b..50fa562e 100644 --- a/include/pathing/environment.hpp +++ b/include/pathing/environment.hpp @@ -23,7 +23,7 @@ class Environment { public: Environment(const Polygon& valid_region, const Polygon& airdrop_zone, const Polygon& mapping_region, const std::vector& goals, - const std::vector& obstacles, double environment_offset); + const std::vector& obstacles, double environment_offset=0); /** * Check if a point is in the valid region