From 9102cb5e7aaf91eed35475a123d253f828fa9933 Mon Sep 17 00:00:00 2001 From: CSTBook Date: Fri, 31 Oct 2025 21:37:45 -0700 Subject: [PATCH 1/3] Initial nuking --- include/utilities/obc_config.hpp | 18 +- src/pathing/environment.cpp | 939 ++++++++++++++++--------------- src/pathing/static.cpp | 540 +++++++++--------- src/pathing/tree.cpp | 868 ++++++++++++++-------------- 4 files changed, 1196 insertions(+), 1169 deletions(-) diff --git a/include/utilities/obc_config.hpp b/include/utilities/obc_config.hpp index 54e2b877..f9425a5f 100644 --- a/include/utilities/obc_config.hpp +++ b/include/utilities/obc_config.hpp @@ -57,15 +57,15 @@ struct DubinsConfig { double point_separation; }; -struct RRTConfig { - int iterations_per_waypoint; // number of iterations run between two waypoints - double rewire_radius; // maximum distance from sampled point to optimize during RRT* - bool optimize; // run RRT* if true - PointFetchMethod::Enum point_fetch_method; - bool allowed_to_skip_waypoints; // if true, will skip waypoints if it can not connect after 1 - // RRT iteration - bool generate_deviations; -}; +// struct RRTConfig { +// int iterations_per_waypoint; // number of iterations run between two waypoints +// double rewire_radius; // maximum distance from sampled point to optimize during RRT* //DELETE +// bool optimize; // run RRT* if true //DELETE +// PointFetchMethod::Enum point_fetch_method; // DELETE +// bool allowed_to_skip_waypoints; // if true, will skip waypoints if it can not connect after 1 +// // RRT iteration +// bool generate_deviations; //DELETE +// }; namespace AirdropCoverageMethod { enum class Enum { HOVER, FORWARD }; diff --git a/src/pathing/environment.cpp b/src/pathing/environment.cpp index 60f19393..695c7d9b 100644 --- a/src/pathing/environment.cpp +++ b/src/pathing/environment.cpp @@ -10,103 +10,176 @@ #include "utilities/datatypes.hpp" #include "utilities/rng.hpp" -Environment::Environment(const Polygon& valid_region, const Polygon& airdrop_zone, - const Polygon& mapping_region, const std::vector& goals, - const std::vector& obstacles) - : valid_region(valid_region), - airdrop_zone(airdrop_zone), - mapping_region(mapping_region), - goals(goals), - goals_found(0), - bounds(findBounds(valid_region)), - obstacles(obstacles) {} - -bool Environment::isPointInBounds(const XYZCoord& point) const { - if (!isPointInPolygon(valid_region, point)) { - return false; - } - - for (const Polygon& obstacle : obstacles) { - if (isPointInPolygon(obstacle, point)) { - return false; - } - } - - return true; -} - -bool Environment::isPathInBounds(const std::vector& path) const { - /* - * starts from the center, and walks in botht directions in intervals, this - * is motivated by the idea that if an endpoint is in bounds, then the path - * is most likely out of bounds in the center of the path. Furthermore, - * moving by larger step would be more efficient than checking every point. - * - * ^only for small step length does this make much sense - */ - const int center = path.size() / 2; - const int interval = ENV_PATH_VALIDATION_STEP_SIZE; - - // special case not checked in the loop - if (!isPointInBounds(path[center])) { - return false; - } - - for (int i = 1; i < interval; i++) { - int count = center + i; - - // to the right - while (count < path.size()) { - if (!isPointInBounds(path[count])) { - return false; - } - count += interval; - } - - // to the left - count = center - i; - while (count >= 0) { - if (!isPointInBounds(path[count])) { - return false; - } - count -= interval; - } - } - - return true; -} - -// bool Environment::isPathInBoundsAdv(const std::vector& path, -// const RRTOption& option) const { -// if (!option.has_straight) { -// return isPathInBounds(path); -// } -// // finds the last point on the first curve, and the first point on the second curve -// // does this using the option, using arclength and the point separation -// const int first_curve_end = -// std::abs(option.dubins_path.beta_0) * TURNING_RADIUS / POINT_SEPARATION + 1; -// const int second_curve_start = -// path.size() - std::abs(option.dubins_path.beta_2) * TURNING_RADIUS / POINT_SEPARATION; +/* +To nuke or not to nuke, that is the question +Whether 'tis nobler in the mind to suffer +The pointers and recursion of outrageous fortune, +Or to take arms against a sea of segfaults +And by opposing end them. +*/ + +// Environment::Environment(const Polygon& valid_region, const Polygon& airdrop_zone, +// const Polygon& mapping_region, const std::vector& goals, +// const std::vector& obstacles) +// : valid_region(valid_region), +// airdrop_zone(airdrop_zone), +// mapping_region(mapping_region), +// goals(goals), +// goals_found(0), +// bounds(findBounds(valid_region)), +// obstacles(obstacles) {} + +// bool Environment::isPointInBounds(const XYZCoord& point) const { +// if (!isPointInPolygon(valid_region, point)) { +// return false; +// } -// // sanity check -// if (first_curve_end >= second_curve_start) { -// return isPathInBounds(path); +// for (const Polygon& obstacle : obstacles) { +// if (isPointInPolygon(obstacle, point)) { +// return false; +// } // } -// if (!isLineInBounds(path[first_curve_end], path[second_curve_start])) { +// return true; +// } + +// bool Environment::isPathInBounds(const std::vector& path) const { +// /* +// * starts from the center, and walks in botht directions in intervals, this +// * is motivated by the idea that if an endpoint is in bounds, then the path +// * is most likely out of bounds in the center of the path. Furthermore, +// * moving by larger step would be more efficient than checking every point. +// * +// * ^only for small step length does this make much sense +// */ +// const int center = path.size() / 2; +// const int interval = ENV_PATH_VALIDATION_STEP_SIZE; + +// // special case not checked in the loop +// if (!isPointInBounds(path[center])) { // return false; // } -// // checks the points manually in the curve -// for (int i = 0; i <= first_curve_end; i++) { -// if (!isPointInBounds(path[i])) { -// return false; +// for (int i = 1; i < interval; i++) { +// int count = center + i; + +// // to the right +// while (count < path.size()) { +// if (!isPointInBounds(path[count])) { +// return false; +// } +// count += interval; +// } + +// // to the left +// count = center - i; +// while (count >= 0) { +// if (!isPointInBounds(path[count])) { +// return false; +// } +// count -= interval; // } // } -// for (int i = second_curve_start; i < path.size(); i++) { -// if (!isPointInBounds(path[i])) { +// return true; +// } + +// // bool Environment::isPathInBoundsAdv(const std::vector& path, +// // const RRTOption& option) const { +// // if (!option.has_straight) { +// // return isPathInBounds(path); +// // } + +// // // finds the last point on the first curve, and the first point on the second curve +// // // does this using the option, using arclength and the point separation +// // const int first_curve_end = +// // std::abs(option.dubins_path.beta_0) * TURNING_RADIUS / POINT_SEPARATION + 1; +// // const int second_curve_start = +// // path.size() - std::abs(option.dubins_path.beta_2) * TURNING_RADIUS / POINT_SEPARATION; + +// // // sanity check +// // if (first_curve_end >= second_curve_start) { +// // return isPathInBounds(path); +// // } + +// // if (!isLineInBounds(path[first_curve_end], path[second_curve_start])) { +// // return false; +// // } + +// // // checks the points manually in the curve +// // for (int i = 0; i <= first_curve_end; i++) { +// // if (!isPointInBounds(path[i])) { +// // return false; +// // } +// // } + +// // for (int i = second_curve_start; i < path.size(); i++) { +// // if (!isPointInBounds(path[i])) { +// // return false; +// // } +// // } + +// // return true; +// // } + +// const XYZCoord& Environment::getGoal() const { return goals[goals_found]; } + +// const XYZCoord& Environment::getGoal(int index) const { return goals[index]; } + +// XYZCoord Environment::getRandomPoint(bool use_mapping_region = false) const { +// // TODO - use some heuristic to more efficiently generate direction +// // vector (and make it toggleable) +// std::pair, std::pair> polygon_bounds = bounds; +// if (use_mapping_region) { +// polygon_bounds = findBounds(mapping_region); +// } + +// for (int i = 0; i < TRIES_FOR_RANDOM_POINT; i++) { +// // generates a random point in the rectangle contianing the valid region +// XYZCoord generated_point = { +// random(polygon_bounds.first.first, polygon_bounds.first.second), +// random(polygon_bounds.second.first, polygon_bounds.second.second), 0}; + +// if (use_mapping_region) { +// if (isPointInPolygon(mapping_region, generated_point)) { +// return generated_point; +// } +// } else { +// if (isPointInBounds(generated_point)) { +// return generated_point; +// } +// } +// } + +// return goals[goals_found]; +// } + +// int Environment::getNumGoals() const { return goals.size(); } + +// bool Environment::isPointInPolygon(const Polygon& polygon, const XYZCoord& point) { +// bool is_inside = false; + +// // point in polygon +// for (int i = 0, j = polygon.size() - 1; i < polygon.size(); j = i++) { +// if (((polygon[i].y > point.y) != (polygon[j].y > point.y)) && +// (point.x < (polygon[j].x - polygon[i].x) * (point.y - polygon[i].y) / +// (polygon[j].y - polygon[i].y) + +// polygon[i].x)) { +// is_inside = !is_inside; +// } +// } + +// return is_inside; +// } + +// bool Environment::isLineInBounds(const XYZCoord& start_point, const XYZCoord& end_point) const { +// if (doesLineIntersectPolygon(start_point, end_point, valid_region)) { +// return false; +// } + +// for (const Polygon& obstacle : obstacles) { +// if (doesLineIntersectPolygon(start_point, end_point, obstacle)) { // return false; // } // } @@ -114,381 +187,317 @@ bool Environment::isPathInBounds(const std::vector& path) const { // return true; // } -const XYZCoord& Environment::getGoal() const { return goals[goals_found]; } - -const XYZCoord& Environment::getGoal(int index) const { return goals[index]; } - -XYZCoord Environment::getRandomPoint(bool use_mapping_region = false) const { - // TODO - use some heuristic to more efficiently generate direction - // vector (and make it toggleable) - std::pair, std::pair> polygon_bounds = bounds; - if (use_mapping_region) { - polygon_bounds = findBounds(mapping_region); - } - - for (int i = 0; i < TRIES_FOR_RANDOM_POINT; i++) { - // generates a random point in the rectangle contianing the valid region - XYZCoord generated_point = { - random(polygon_bounds.first.first, polygon_bounds.first.second), - random(polygon_bounds.second.first, polygon_bounds.second.second), 0}; - - if (use_mapping_region) { - if (isPointInPolygon(mapping_region, generated_point)) { - return generated_point; - } - } else { - if (isPointInBounds(generated_point)) { - return generated_point; - } - } - } - - return goals[goals_found]; -} - -int Environment::getNumGoals() const { return goals.size(); } - -bool Environment::isPointInPolygon(const Polygon& polygon, const XYZCoord& point) { - bool is_inside = false; - - // point in polygon - for (int i = 0, j = polygon.size() - 1; i < polygon.size(); j = i++) { - if (((polygon[i].y > point.y) != (polygon[j].y > point.y)) && - (point.x < (polygon[j].x - polygon[i].x) * (point.y - polygon[i].y) / - (polygon[j].y - polygon[i].y) + - polygon[i].x)) { - is_inside = !is_inside; - } - } - - return is_inside; -} - -bool Environment::isLineInBounds(const XYZCoord& start_point, const XYZCoord& end_point) const { - if (doesLineIntersectPolygon(start_point, end_point, valid_region)) { - return false; - } - - for (const Polygon& obstacle : obstacles) { - if (doesLineIntersectPolygon(start_point, end_point, obstacle)) { - return false; - } - } - - return true; -} - -bool Environment::doesLineIntersectPolygon(const XYZCoord& start_point, const XYZCoord& end_point, - const Polygon& polygon) const { - for (int i = 0, j = polygon.size() - 1; i < polygon.size(); j = i++) { - if (intersect(start_point, end_point, polygon[i], polygon[j])) { - return true; - } - } - - return false; -} - -// Given three colinear points p, q, r, the function checks if -// point q lies on line segment 'pr' -bool Environment::onSegment(XYZCoord p, XYZCoord q, XYZCoord r) const { - if (q.x <= std::max(p.x, r.x) && q.x >= std::min(p.x, r.x) && q.y <= std::max(p.y, r.y) && - q.y >= std::min(p.y, r.y)) - return true; - return false; -} - -// To find orientation of ordered triplet (p, q, r). -// The function returns following values: -// 0 : Colinear points -// 1 : Clockwise points -// 2 : Counterclockwise points -int Environment::orientation(XYZCoord p, XYZCoord q, XYZCoord r) const { - int val = (q.y - p.y) * (r.x - q.x) - (q.x - p.x) * (r.y - q.y); - if (val == 0) return 0; // colinear - return (val > 0) ? 1 : 2; // clock or counterclock wise -} - -// Function to check if segments intersect -bool Environment::intersect(XYZCoord p1, XYZCoord q1, XYZCoord p2, XYZCoord q2) const { - // Find the four orientations needed for general and - // special cases - int o1 = orientation(p1, q1, p2); - int o2 = orientation(p1, q1, q2); - int o3 = orientation(p2, q2, p1); - int o4 = orientation(p2, q2, q1); - - // General case - if (o1 != o2 && o3 != o4) return true; - - // Special Cases - // p1, q1 and p2 are colinear and p2 lies on segment p1q1 - if (o1 == 0 && onSegment(p1, p2, q1)) return true; - - // p1, q1 and q2 are colinear and q2 lies on segment p1q1 - if (o2 == 0 && onSegment(p1, q2, q1)) return true; - - // p2, q2 and p1 are colinear and p1 lies on segment p2q2 - if (o3 == 0 && onSegment(p2, p1, q2)) return true; - - // p2, q2 and q1 are colinear and q1 lies on segment p2q2 - if (o4 == 0 && onSegment(p2, q1, q2)) return true; - - return false; // Doesn't fall in any of the above cases -} - -std::vector Environment::getAirdropEndpoints(int scan_radius, bool vertical) const { - auto bounds = findBounds(airdrop_zone); - auto [x_min, x_max] = bounds.first; - auto [y_min, y_max] = bounds.second; - - std::vector endpoints; - bool fly_down = true; - double start = vertical ? x_min + scan_radius : -1 * (y_max - scan_radius); - double end = vertical ? x_max - scan_radius : -1 * (y_min + scan_radius); - double iteration = scan_radius * 2; - - for (double coordinate = start; coordinate <= end; coordinate += iteration) { - // finds where this x-coordinate intersects with the airdrop zone (always convex) - XYZCoord top(0, 0, 0); - XYZCoord bottom(0, 0, 0); - if (vertical) { - top = XYZCoord(coordinate, y_max, 0); - bottom = XYZCoord(coordinate, y_min, 0); - } else { - top = XYZCoord(x_min, -coordinate, 0); - bottom = XYZCoord(x_max, -coordinate, 0); - } - - std::vector intersections = - findIntersections(airdrop_zone, top, bottom, vertical); - - if (vertical) { - if (intersections[0].y < intersections[1].y) { - endpoints.push_back(intersections[1]); - endpoints.push_back(intersections[0]); - - } else { - endpoints.push_back(intersections[0]); - endpoints.push_back(intersections[1]); - } - } else { - if (intersections[0].x < intersections[1].x) { - endpoints.push_back(intersections[0]); - endpoints.push_back(intersections[1]); - - } else { - endpoints.push_back(intersections[1]); - endpoints.push_back(intersections[0]); - } - } - } - - return endpoints; -} - -std::vector Environment::getAirdropWaypoints(int scan_radius, bool one_way, - bool vertical) const { - std::vector waypoints; - std::vector endpoints = getAirdropEndpoints(scan_radius, vertical); - double angle = vertical ? 3.0 / 2.0 * M_PI : 0; - - if (one_way) { - for (const XYZCoord& endpoint : endpoints) { - waypoints.push_back(RRTPoint(endpoint, angle)); - } - } else { - bool fly_down = true; - for (int i = 0; i < endpoints.size(); i += 2) { - if (fly_down) { - waypoints.push_back(RRTPoint(endpoints[i], angle)); - waypoints.push_back(RRTPoint(endpoints[i + 1], angle)); - } else { - waypoints.push_back(RRTPoint(endpoints[i + 1], angle)); - waypoints.push_back(RRTPoint(endpoints[i], angle)); - } - - angle = mod(angle + M_PI, TWO_PI); - fly_down = !fly_down; - } - } - - return waypoints; -} - -bool Environment::verticalRayIntersectsEdge(const XYZCoord& p1, const XYZCoord& p2, - const XYZCoord& rayStart, const XYZCoord& rayEnd, - XYZCoord& intersection) const { - // if the x coordinate lines between the edge - if ((p2.x <= rayStart.x && p1.x >= rayStart.x) || (p1.x <= rayStart.x && p2.x >= rayStart.x)) { - double slope = (p2.y - p1.y) / (p2.x - p1.x); - // finds where they intersect using a line y - y' = m(x - x') - intersection = XYZCoord(rayStart.x, slope * (rayStart.x - p1.x) + p1.y, 0); - return true; - } - return false; -} - -bool Environment::horizontalRayIntersectsEdge(const XYZCoord& p1, const XYZCoord& p2, - const XYZCoord& rayStart, const XYZCoord& rayEnd, - XYZCoord& intersection) const { - // if the x coordinate lines between the edge - if ((p2.y <= rayStart.y && p1.y >= rayStart.y) || (p1.y <= rayStart.y && p2.y >= rayStart.y)) { - double inverse_slope = (p2.x - p1.x) / (p2.y - p1.y); - // finds where they intersect using a line y - y' = m(x - x') - intersection = XYZCoord(inverse_slope * (rayStart.y - p1.y) + p1.x, rayStart.y, 0); - return true; - } - return false; -} - -std::vector Environment::findIntersections(const Polygon& polygon, - const XYZCoord& rayStart, - const XYZCoord& rayEnd, bool vertical) const { - // array to be filled - std::vector intersections; - int n = polygon.size(); - - for (int i = 0; i < n; ++i) { - const XYZCoord& p1 = polygon[i]; - const XYZCoord& p2 = polygon[(i + 1) % n]; - - // temporary variable to be changed if an intersection is found - XYZCoord intersection(0, 0, 0); - bool found_intersection = - vertical ? verticalRayIntersectsEdge(p1, p2, rayStart, rayEnd, intersection) - : horizontalRayIntersectsEdge(p1, p2, rayStart, rayEnd, intersection); - if (found_intersection) { - intersections.push_back(intersection); - } - } - - return intersections; -} - -std::vector Environment::findIntersectionsWithPolygon(const Polygon& polygon, - const XYZCoord& start_point, - const XYZCoord& end_point) const { - // for loop through each edge of the polygon - // for each edge, find the intersection point with the line segment - // if an intersection point is found, add it to the list of intersection - // points - std::vector intersections; - for (int i = 0; i < polygon.size(); ++i) { - XYZCoord p1 = polygon[i]; - XYZCoord p2 = polygon[(i + 1) % polygon.size()]; - if (intersect(start_point, end_point, p1, p2)) { - // finds the intersection point - double x1 = start_point.x, y1 = start_point.y; - double x2 = end_point.x, y2 = end_point.y; - double x3 = p1.x, y3 = p1.y; - double x4 = p2.x, y4 = p2.y; - - double a1 = y2 - y1; - double b1 = x1 - x2; - double c1 = a1 * x1 + b1 * y1; - - double a2 = y4 - y3; - double b2 = x3 - x4; - double c2 = a2 * x3 + b2 * y3; - - double det = a1 * b2 - a2 * b1; - - double x = (b2 * c1 - b1 * c2) / det; - double y = (a1 * c2 - a2 * c1) / det; - if (std::find(intersections.begin(), intersections.end(), XYZCoord(x, y, 0)) == - intersections.end()) { - intersections.push_back(XYZCoord(x, y, 0)); - } - } - } - - return intersections; -} - -std::pair Environment::estimateAreaCoveredAndPathLength( - const std::vector& new_goals) const { - double area_covered = 0.0; - double path_length = 0.0; - - for (int i = 0; i < new_goals.size(); ++i) { - XYZCoord start_point = new_goals[i]; - XYZCoord end_point = new_goals[(i + 1) % new_goals.size()]; - path_length += start_point.distanceTo(end_point); - // Calculates area covered by adding the part of the line segment that is in bounds - // Caulcate the intersections and whether the start and end points are in bounds in order to - // find the sections of the line that is in bounds - if (!doesLineIntersectPolygon(start_point, end_point, mapping_region)) { - area_covered += start_point.distanceTo(end_point) * SEARCH_RADIUS * 2; - } else { - std::vector intersections = - findIntersectionsWithPolygon(mapping_region, start_point, end_point); - bool start_in_bounds = isPointInPolygon(mapping_region, start_point); - bool end_in_bounds = isPointInPolygon(mapping_region, end_point); - - if (start_in_bounds) { - area_covered += start_point.distanceTo(intersections[0]) * SEARCH_RADIUS; - intersections.erase(intersections.begin()); - } - - if (end_in_bounds) { - area_covered += end_point.distanceTo(intersections.back()) * SEARCH_RADIUS; - intersections.pop_back(); - } - - for (int i = 0; i < intersections.size(); i += 2) { - area_covered += intersections[i].distanceTo(intersections[i + 1]) * SEARCH_RADIUS; - } - } - } - - return {area_covered, path_length}; -} - -Polygon Environment::scale(double scale, const Polygon& source_polygon) const { - Polygon scaled_polygon; - - // square bounds of the polygon - auto bounds = findBounds(source_polygon); - auto [x_min, x_max] = bounds.first; - auto [y_min, y_max] = bounds.second; - - // finds the center of the polygon - double x_center = (x_max + x_min) / 2; - double y_center = (y_max + y_min) / 2; - - for (const XYZCoord& point : source_polygon) { - // shifts the polygon to the center so scaling doesn't shift the centter (readability) - // scales the polygon - // retranslates the center to the original center - double scaled_x = (point.x - x_center) * scale + x_center; - double scaled_y = (point.y - y_center) * scale + y_center; - - scaled_polygon.push_back(XYZCoord(scaled_x, scaled_y, 0)); - } - - return scaled_polygon; -} - -std::pair, std::pair> Environment::findBounds( - const Polygon& region) const { - if (region.empty()) { - return std::make_pair(std::make_pair(0, 0), std::make_pair(0, 0)); - } - - // initial values - double min_x = region[0].x; - double max_x = region[0].x; - double min_y = region[0].y; - double max_y = region[0].y; - - // finds the min and max x and y values - for (const XYZCoord& point : region) { - min_x = std::min(min_x, point.x); - max_x = std::max(max_x, point.x); - min_y = std::min(min_y, point.y); - max_y = std::max(max_y, point.y); - } - - return {{min_x, max_x}, {min_y, max_y}}; -} +// bool Environment::doesLineIntersectPolygon(const XYZCoord& start_point, const XYZCoord& end_point, +// const Polygon& polygon) const { +// for (int i = 0, j = polygon.size() - 1; i < polygon.size(); j = i++) { +// if (intersect(start_point, end_point, polygon[i], polygon[j])) { +// return true; +// } +// } + +// return false; +// } + +// // Given three colinear points p, q, r, the function checks if +// // point q lies on line segment 'pr' +// bool Environment::onSegment(XYZCoord p, XYZCoord q, XYZCoord r) const { +// if (q.x <= std::max(p.x, r.x) && q.x >= std::min(p.x, r.x) && q.y <= std::max(p.y, r.y) && +// q.y >= std::min(p.y, r.y)) +// return true; +// return false; +// } + +// // To find orientation of ordered triplet (p, q, r). +// // The function returns following values: +// // 0 : Colinear points +// // 1 : Clockwise points +// // 2 : Counterclockwise points +// int Environment::orientation(XYZCoord p, XYZCoord q, XYZCoord r) const { +// int val = (q.y - p.y) * (r.x - q.x) - (q.x - p.x) * (r.y - q.y); +// if (val == 0) return 0; // colinear +// return (val > 0) ? 1 : 2; // clock or counterclock wise +// } + +// // Function to check if segments intersect +// bool Environment::intersect(XYZCoord p1, XYZCoord q1, XYZCoord p2, XYZCoord q2) const { +// // Find the four orientations needed for general and +// // special cases +// int o1 = orientation(p1, q1, p2); +// int o2 = orientation(p1, q1, q2); +// int o3 = orientation(p2, q2, p1); +// int o4 = orientation(p2, q2, q1); + +// // General case +// if (o1 != o2 && o3 != o4) return true; + +// // Special Cases +// // p1, q1 and p2 are colinear and p2 lies on segment p1q1 +// if (o1 == 0 && onSegment(p1, p2, q1)) return true; + +// // p1, q1 and q2 are colinear and q2 lies on segment p1q1 +// if (o2 == 0 && onSegment(p1, q2, q1)) return true; + +// // p2, q2 and p1 are colinear and p1 lies on segment p2q2 +// if (o3 == 0 && onSegment(p2, p1, q2)) return true; + +// // p2, q2 and q1 are colinear and q1 lies on segment p2q2 +// if (o4 == 0 && onSegment(p2, q1, q2)) return true; + +// return false; // Doesn't fall in any of the above cases +// } + +// std::vector Environment::getAirdropEndpoints(int scan_radius, bool vertical) const { +// auto bounds = findBounds(airdrop_zone); +// auto [x_min, x_max] = bounds.first; +// auto [y_min, y_max] = bounds.second; + +// std::vector endpoints; +// bool fly_down = true; +// double start = vertical ? x_min + scan_radius : -1 * (y_max - scan_radius); +// double end = vertical ? x_max - scan_radius : -1 * (y_min + scan_radius); +// double iteration = scan_radius * 2; + +// for (double coordinate = start; coordinate <= end; coordinate += iteration) { +// // finds where this x-coordinate intersects with the airdrop zone (always convex) +// XYZCoord top(0, 0, 0); +// XYZCoord bottom(0, 0, 0); +// if (vertical) { +// top = XYZCoord(coordinate, y_max, 0); +// bottom = XYZCoord(coordinate, y_min, 0); +// } else { +// top = XYZCoord(x_min, -coordinate, 0); +// bottom = XYZCoord(x_max, -coordinate, 0); +// } + +// std::vector intersections = +// findIntersections(airdrop_zone, top, bottom, vertical); + +// if (vertical) { +// if (intersections[0].y < intersections[1].y) { +// endpoints.push_back(intersections[1]); +// endpoints.push_back(intersections[0]); + +// } else { +// endpoints.push_back(intersections[0]); +// endpoints.push_back(intersections[1]); +// } +// } else { +// if (intersections[0].x < intersections[1].x) { +// endpoints.push_back(intersections[0]); +// endpoints.push_back(intersections[1]); + +// } else { +// endpoints.push_back(intersections[1]); +// endpoints.push_back(intersections[0]); +// } +// } +// } + +// return endpoints; +// } + +// std::vector Environment::getAirdropWaypoints(int scan_radius, bool one_way, +// bool vertical) const { +// std::vector waypoints; +// std::vector endpoints = getAirdropEndpoints(scan_radius, vertical); +// double angle = vertical ? 3.0 / 2.0 * M_PI : 0; + +// if (one_way) { +// for (const XYZCoord& endpoint : endpoints) { +// waypoints.push_back(RRTPoint(endpoint, angle)); +// } +// } else { +// bool fly_down = true; +// for (int i = 0; i < endpoints.size(); i += 2) { +// if (fly_down) { +// waypoints.push_back(RRTPoint(endpoints[i], angle)); +// waypoints.push_back(RRTPoint(endpoints[i + 1], angle)); +// } else { +// waypoints.push_back(RRTPoint(endpoints[i + 1], angle)); +// waypoints.push_back(RRTPoint(endpoints[i], angle)); +// } + +// angle = mod(angle + M_PI, TWO_PI); +// fly_down = !fly_down; +// } +// } + +// return waypoints; +// } + +// bool Environment::verticalRayIntersectsEdge(const XYZCoord& p1, const XYZCoord& p2, +// const XYZCoord& rayStart, const XYZCoord& rayEnd, +// XYZCoord& intersection) const { +// // if the x coordinate lines between the edge +// if ((p2.x <= rayStart.x && p1.x >= rayStart.x) || (p1.x <= rayStart.x && p2.x >= rayStart.x)) { +// double slope = (p2.y - p1.y) / (p2.x - p1.x); +// // finds where they intersect using a line y - y' = m(x - x') +// intersection = XYZCoord(rayStart.x, slope * (rayStart.x - p1.x) + p1.y, 0); +// return true; +// } +// return false; +// } + +// bool Environment::horizontalRayIntersectsEdge(const XYZCoord& p1, const XYZCoord& p2, +// const XYZCoord& rayStart, const XYZCoord& rayEnd, +// XYZCoord& intersection) const { +// // if the x coordinate lines between the edge +// if ((p2.y <= rayStart.y && p1.y >= rayStart.y) || (p1.y <= rayStart.y && p2.y >= rayStart.y)) { +// double inverse_slope = (p2.x - p1.x) / (p2.y - p1.y); +// // finds where they intersect using a line y - y' = m(x - x') +// intersection = XYZCoord(inverse_slope * (rayStart.y - p1.y) + p1.x, rayStart.y, 0); +// return true; +// } +// return false; +// } + +// std::vector Environment::findIntersections(const Polygon& polygon, +// const XYZCoord& rayStart, +// const XYZCoord& rayEnd, bool vertical) const { +// // array to be filled +// std::vector intersections; +// int n = polygon.size(); + +// for (int i = 0; i < n; ++i) { +// const XYZCoord& p1 = polygon[i]; +// const XYZCoord& p2 = polygon[(i + 1) % n]; + +// // temporary variable to be changed if an intersection is found +// XYZCoord intersection(0, 0, 0); +// bool found_intersection = +// vertical ? verticalRayIntersectsEdge(p1, p2, rayStart, rayEnd, intersection) +// : horizontalRayIntersectsEdge(p1, p2, rayStart, rayEnd, intersection); +// if (found_intersection) { +// intersections.push_back(intersection); +// } +// } + +// return intersections; +// } + +// std::vector Environment::findIntersectionsWithPolygon(const Polygon& polygon, +// const XYZCoord& start_point, +// const XYZCoord& end_point) const { +// // for loop through each edge of the polygon +// // for each edge, find the intersection point with the line segment +// // if an intersection point is found, add it to the list of intersection +// // points +// std::vector intersections; +// for (int i = 0; i < polygon.size(); ++i) { +// XYZCoord p1 = polygon[i]; +// XYZCoord p2 = polygon[(i + 1) % polygon.size()]; +// if (intersect(start_point, end_point, p1, p2)) { +// // finds the intersection point +// double x1 = start_point.x, y1 = start_point.y; +// double x2 = end_point.x, y2 = end_point.y; +// double x3 = p1.x, y3 = p1.y; +// double x4 = p2.x, y4 = p2.y; + +// double a1 = y2 - y1; +// double b1 = x1 - x2; +// double c1 = a1 * x1 + b1 * y1; + +// double a2 = y4 - y3; +// double b2 = x3 - x4; +// double c2 = a2 * x3 + b2 * y3; + +// double det = a1 * b2 - a2 * b1; + +// double x = (b2 * c1 - b1 * c2) / det; +// double y = (a1 * c2 - a2 * c1) / det; +// if (std::find(intersections.begin(), intersections.end(), XYZCoord(x, y, 0)) == +// intersections.end()) { +// intersections.push_back(XYZCoord(x, y, 0)); +// } +// } +// } + +// return intersections; +// } + +// std::pair Environment::estimateAreaCoveredAndPathLength( +// const std::vector& new_goals) const { +// double area_covered = 0.0; +// double path_length = 0.0; + +// for (int i = 0; i < new_goals.size(); ++i) { +// XYZCoord start_point = new_goals[i]; +// XYZCoord end_point = new_goals[(i + 1) % new_goals.size()]; +// path_length += start_point.distanceTo(end_point); +// // Calculates area covered by adding the part of the line segment that is in bounds +// // Caulcate the intersections and whether the start and end points are in bounds in order to +// // find the sections of the line that is in bounds +// if (!doesLineIntersectPolygon(start_point, end_point, mapping_region)) { +// area_covered += start_point.distanceTo(end_point) * SEARCH_RADIUS * 2; +// } else { +// std::vector intersections = +// findIntersectionsWithPolygon(mapping_region, start_point, end_point); +// bool start_in_bounds = isPointInPolygon(mapping_region, start_point); +// bool end_in_bounds = isPointInPolygon(mapping_region, end_point); + +// if (start_in_bounds) { +// area_covered += start_point.distanceTo(intersections[0]) * SEARCH_RADIUS; +// intersections.erase(intersections.begin()); +// } + +// if (end_in_bounds) { +// area_covered += end_point.distanceTo(intersections.back()) * SEARCH_RADIUS; +// intersections.pop_back(); +// } + +// for (int i = 0; i < intersections.size(); i += 2) { +// area_covered += intersections[i].distanceTo(intersections[i + 1]) * SEARCH_RADIUS; +// } +// } +// } + +// return {area_covered, path_length}; +// } + +// Polygon Environment::scale(double scale, const Polygon& source_polygon) const { +// Polygon scaled_polygon; + +// // square bounds of the polygon +// auto bounds = findBounds(source_polygon); +// auto [x_min, x_max] = bounds.first; +// auto [y_min, y_max] = bounds.second; + +// // finds the center of the polygon +// double x_center = (x_max + x_min) / 2; +// double y_center = (y_max + y_min) / 2; + +// for (const XYZCoord& point : source_polygon) { +// // shifts the polygon to the center so scaling doesn't shift the centter (readability) +// // scales the polygon +// // retranslates the center to the original center +// double scaled_x = (point.x - x_center) * scale + x_center; +// double scaled_y = (point.y - y_center) * scale + y_center; + +// scaled_polygon.push_back(XYZCoord(scaled_x, scaled_y, 0)); +// } + +// return scaled_polygon; +// } + +// std::pair, std::pair> Environment::findBounds( +// const Polygon& region) const { +// if (region.empty()) { +// return std::make_pair(std::make_pair(0, 0), std::make_pair(0, 0)); +// } + +// // initial values +// double min_x = region[0].x; +// double max_x = region[0].x; +// double min_y = region[0].y; +// double max_y = region[0].y; + +// // finds the min and max x and y values +// for (const XYZCoord& point : region) { +// min_x = std::min(min_x, point.x); +// max_x = std::max(max_x, point.x); +// min_y = std::min(min_y, point.y); +// max_y = std::max(max_y, point.y); +// } + +// return {{min_x, max_x}, {min_y, max_y}}; +// } diff --git a/src/pathing/static.cpp b/src/pathing/static.cpp index eedf4b05..f241f430 100644 --- a/src/pathing/static.cpp +++ b/src/pathing/static.cpp @@ -20,269 +20,278 @@ #include "utilities/obc_config.hpp" #include "utilities/rng.hpp" -RRT::RRT(RRTPoint start, std::vector goals, double search_radius, Polygon bounds, - const OBCConfig &config, std::vector obstacles, std::vector angles) - : 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), - Dubins(config.pathing.dubins.turning_radius, config.pathing.dubins.point_separation)), - config(config.pathing.rrt) { - if (angles.size() != 0) { - this->angles = angles; - } -} - -RRT::RRT(RRTPoint start, std::vector goals, double search_radius, Environment airspace, - const OBCConfig &config, std::vector angles) - : iterations_per_waypoint(config.pathing.rrt.iterations_per_waypoint), - search_radius(search_radius), - rewire_radius(config.pathing.rrt.rewire_radius), - tree(start, airspace, - Dubins(config.pathing.dubins.turning_radius, config.pathing.dubins.point_separation)), - config(config.pathing.rrt) { - if (angles.size() != 0) { - this->angles = angles; - } -} - -void RRT::run() { - /* - * RRT algorithm - * - Treats each waypoint as a goal, DOES NOT reuse trees between waypoints, - * basically calls RRT for each waypoint - * - For Each Waypoint - * - Tries to connect directly to the goal - * - If it can't, it runs the RRT algorithm - * - Attempts to converge based on epoch intervals - * - If it can't, it connects to the goal with whatever it has - */ - const int total_goals = tree.getAirspace().getNumGoals(); - - for (int current_goal_index = 0; current_goal_index < total_goals; current_goal_index++) { - // tries to connect directly to the goal - if (connectToGoal(current_goal_index)) { - continue; - } - - // run the RRT algorithm if it can not connect - RRTIteration(iterations_per_waypoint, current_goal_index); - } -} - -std::vector RRT::getPointsToGoal() const { - // return tree.getPathToGoal(); - return flight_path; -} - -bool RRT::RRTIteration(int tries, int current_goal_index) { - const int epoch_interval = tries / NUM_EPOCHS; - int current_epoch = epoch_interval; - - RRTNode *goal_node = nullptr; - - for (int i = 0; i < tries; i++) { - if (i == current_epoch) { - // generates a new node (not connect), and adds and breaks if it is - // within X% of the last generation - if (epochEvaluation(goal_node, current_goal_index)) { - return true; - } - - current_epoch += epoch_interval; - } - // generate a sample point - const RRTPoint sample = generateSamplePoint(); - - // returns all dubins options from the tree to the sample - const std::vector> &options = - tree.pathingOptions(sample, config.point_fetch_method); - - // returns true if the node is successfully added to the tree - RRTNode *new_node = parseOptions(options, sample); - - if (new_node != nullptr && config.optimize) { - optimizeTree(new_node); - } - } - - // frees memory - delete (goal_node); - if (!connectToGoal(current_goal_index)) { - loguru::set_thread_name("Static Pathing"); - LOG_F(WARNING, "Failed to connect to goal on iteration: [%s]. Trying again...", - std::to_string(current_goal_index).c_str()); - - if (!config.allowed_to_skip_waypoints && - !connectToGoal(current_goal_index, std::numeric_limits::max())) { - // will always return true (unless it turns into a pseudo-infinite loop) - return RRTIteration(tries, current_goal_index); - } else { - return false; - } - } - - return true; -} - -bool RRT::epochEvaluation(RRTNode *goal_node, int current_goal_index) { - // If a single epoch has not been passed, mark this goal as the first - // benchmark. - if (goal_node == nullptr) { - goal_node = sampleToGoal(current_goal_index, TOTAL_OPTIONS_FOR_GOAL_CONNECTION); - return false; - } - - RRTNode *new_node = sampleToGoal(current_goal_index, TOTAL_OPTIONS_FOR_GOAL_CONNECTION); - - if (new_node == nullptr) { - return false; - } - - /* If the new node is within ~X% of the goal, then we are done. - * It should be impossible for new_node to be more inefficient than - * goal_node as it uses a superset of the tree goal_node used - */ - if (new_node->getCost() < EPOCH_TEST_MARGIN * goal_node->getCost()) { - delete (goal_node); - goal_node = new_node; - return false; - } - - addNodeToTree(new_node, current_goal_index); - return true; -} - -RRTPoint RRT::generateSamplePoint() const { return tree.getRandomPoint(search_radius); } - -std::vector>> RRT::getOptionsToGoal( - int current_goal_index, int total_options) const { - // attempts to connect to the goal, should always connect - std::vector goal_points; - - // Generates goal specific points based on current Waypoints and list og - // Angles - for (const double angle : angles) { - const XYZCoord &goal = tree.getAirspace().getGoal(current_goal_index); - goal_points.push_back(RRTPoint(goal, angle)); - } - - // RRTPoint is the goal that is to be connected - // RRTNode is the node in the tree that is the anchor - // RRTOPtion Node-->Point - std::vector>> all_options; - - // limit amount of options to sort, defined in constants.hpp - const int NUMBER_OPTIONS_EACH = total_options / angles.size(); - - // gets all options for each of the goals, and puts them into a unified list - // TODO ? maybe better for a max heap? - for (const RRTPoint &goal : goal_points) { - const std::vector> &options = - // For now, we use optimal pathing - tree.pathingOptions(goal, PointFetchMethod::Enum::NONE, NUMBER_OPTIONS_EACH); - - for (const auto &[node, option] : options) { - all_options.push_back({goal, {node, option}}); - } - } - - std::sort(all_options.begin(), all_options.end(), [](const auto &a, const auto &b) { - auto &[a_goal, a_paths] = a; - auto &[a_node, a_option] = a_paths; - auto &[b_goal, b_paths] = b; - auto &[b_node, b_option] = b_paths; - return a_option.length + a_node->getCost() < b_option.length + b_node->getCost(); - }); - - return all_options; -} - -RRTNode *RRT::sampleToGoal(int current_goal_index, int total_options) const { - // gets all options for each of the goals - const auto &all_options = getOptionsToGoal(current_goal_index, total_options); - - // - for (const auto &[goal, pair] : all_options) { - auto &[anchor_node, option] = pair; - - RRTNode *new_node = tree.generateNode(anchor_node, goal, option); - - if (new_node != nullptr) { - return new_node; - } - } - - return nullptr; -} - -bool RRT::connectToGoal(int current_goal_index, int total_options) { - RRTNode *goal_node = sampleToGoal(current_goal_index, total_options); - - if (goal_node == nullptr) { - return false; - } - - addNodeToTree(goal_node, current_goal_index); - return true; -} - -void RRT::addNodeToTree(RRTNode *goal_node, int current_goal_index) { - // add the node to the tree - tree.addNode(goal_node->getParent(), goal_node); - - // inserts the altitude into the path - std::vector local_path = tree.getPathSegment(goal_node); - - double start_height; - if (current_goal_index == 0) { - start_height = tree.getStart().coord.z; - } else { - start_height = tree.getAirspace().getGoal(current_goal_index - 1).z; - } - - double height_difference = tree.getAirspace().getGoal(current_goal_index).z - start_height; - double height_increment = height_difference / local_path.size(); - - for (XYZCoord &point : local_path) { - point.z = start_height; - start_height += height_increment; - } - - // adds local path to the flight path, and updates the tree - flight_path.insert(flight_path.end(), local_path.begin(), local_path.end()); - tree.setCurrentHead(goal_node); -} - -RRTNode *RRT::parseOptions(const std::vector> &options, - const RRTPoint &sample) { - for (auto &[node, option] : options) { - /* - * stop if - * 1. the node is null - * 2. the node is the same as the sample - * - * The idea is that any further options will have the same if not more - * issues - * - * This shouldn't ever happen? - */ - // if (node == nullptr || node->getPoint() == sample) { - // return nullptr; - - // else, attempt to add the node to the tree - RRTNode *sucessful_addition = tree.addSample(node, sample, option); - - if (sucessful_addition != nullptr) { - return sucessful_addition; - } - } - - return nullptr; -} - -void RRT::optimizeTree(RRTNode *sample) { tree.RRTStar(sample, rewire_radius); } +/* +I remained to dream the nightmare out to the end, and to show my loyalty to TUAS once more. +Path-planning. My Path-planning! Droll thing RRT is—that mysterious arrangement of merciless logic for a futile purpose. +The most you can hope from it is some knowledge of yourself—that comes too late—a crop of unextinguishable errors. +I have wrestled with C++. It is the most unexciting contest you can imagine. +*/ +// RRT::RRT(RRTPoint start, std::vector goals, double search_radius, Polygon bounds, +// const OBCConfig &config, std::vector obstacles, std::vector angles) +// : 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), +// Dubins(config.pathing.dubins.turning_radius, config.pathing.dubins.point_separation)), +// config(config.pathing.rrt) { +// if (angles.size() != 0) { +// this->angles = angles; +// } +// } + +// RRT::RRT(RRTPoint start, std::vector goals, double search_radius, Environment airspace, +// const OBCConfig &config, std::vector angles) +// : iterations_per_waypoint(config.pathing.rrt.iterations_per_waypoint), +// search_radius(search_radius), +// rewire_radius(config.pathing.rrt.rewire_radius), +// tree(start, airspace, +// Dubins(config.pathing.dubins.turning_radius, config.pathing.dubins.point_separation)), +// config(config.pathing.rrt) { +// if (angles.size() != 0) { +// this->angles = angles; +// } +// } + +// void RRT::run() { +// /* +// * RRT algorithm +// * - Treats each waypoint as a goal, DOES NOT reuse trees between waypoints, +// * basically calls RRT for each waypoint +// * - For Each Waypoint +// * - Tries to connect directly to the goal +// * - If it can't, it runs the RRT algorithm +// * - Attempts to converge based on epoch intervals +// * - If it can't, it connects to the goal with whatever it has +// */ +// const int total_goals = tree.getAirspace().getNumGoals(); + +// for (int current_goal_index = 0; current_goal_index < total_goals; current_goal_index++) { +// // tries to connect directly to the goal +// if (connectToGoal(current_goal_index)) { +// continue; +// } + +// // run the RRT algorithm if it can not connect +// RRTIteration(iterations_per_waypoint, current_goal_index); +// } +// } + +// std::vector RRT::getPointsToGoal() const { +// // return tree.getPathToGoal(); +// return flight_path; +// } + +// bool RRT::RRTIteration(int tries, int current_goal_index) { +// const int epoch_interval = tries / NUM_EPOCHS; +// int current_epoch = epoch_interval; + +// RRTNode *goal_node = nullptr; + +// for (int i = 0; i < tries; i++) { +// if (i == current_epoch) { +// // generates a new node (not connect), and adds and breaks if it is +// // within X% of the last generation +// if (epochEvaluation(goal_node, current_goal_index)) { +// return true; +// } + +// current_epoch += epoch_interval; +// } +// // generate a sample point +// const RRTPoint sample = generateSamplePoint(); + +// // returns all dubins options from the tree to the sample +// const std::vector> &options = +// tree.pathingOptions(sample, config.point_fetch_method); + +// // returns true if the node is successfully added to the tree +// RRTNode *new_node = parseOptions(options, sample); + +// if (new_node != nullptr && config.optimize) { +// optimizeTree(new_node); +// } +// } + +// // frees memory +// delete (goal_node); +// if (!connectToGoal(current_goal_index)) { +// loguru::set_thread_name("Static Pathing"); +// LOG_F(WARNING, "Failed to connect to goal on iteration: [%s]. Trying again...", +// std::to_string(current_goal_index).c_str()); + +// if (!config.allowed_to_skip_waypoints && +// !connectToGoal(current_goal_index, std::numeric_limits::max())) { +// // will always return true (unless it turns into a pseudo-infinite loop) +// return RRTIteration(tries, current_goal_index); +// } else { +// return false; +// } +// } + +// return true; +// } + +// bool RRT::epochEvaluation(RRTNode *goal_node, int current_goal_index) { +// // If a single epoch has not been passed, mark this goal as the first +// // benchmark. +// if (goal_node == nullptr) { +// goal_node = sampleToGoal(current_goal_index, TOTAL_OPTIONS_FOR_GOAL_CONNECTION); +// return false; +// } + +// RRTNode *new_node = sampleToGoal(current_goal_index, TOTAL_OPTIONS_FOR_GOAL_CONNECTION); + +// if (new_node == nullptr) { +// return false; +// } + +// /* If the new node is within ~X% of the goal, then we are done. +// * It should be impossible for new_node to be more inefficient than +// * goal_node as it uses a superset of the tree goal_node used +// */ +// if (new_node->getCost() < EPOCH_TEST_MARGIN * goal_node->getCost()) { +// delete (goal_node); +// goal_node = new_node; +// return false; +// } + +// addNodeToTree(new_node, current_goal_index); +// return true; +// } + +// RRTPoint RRT::generateSamplePoint() const { return tree.getRandomPoint(search_radius); } + +// std::vector>> RRT::getOptionsToGoal( +// int current_goal_index, int total_options) const { +// // attempts to connect to the goal, should always connect +// std::vector goal_points; + +// // Generates goal specific points based on current Waypoints and list og +// // Angles +// for (const double angle : angles) { +// const XYZCoord &goal = tree.getAirspace().getGoal(current_goal_index); +// goal_points.push_back(RRTPoint(goal, angle)); +// } + +// // RRTPoint is the goal that is to be connected +// // RRTNode is the node in the tree that is the anchor +// // RRTOPtion Node-->Point +// std::vector>> all_options; + +// // limit amount of options to sort, defined in constants.hpp +// const int NUMBER_OPTIONS_EACH = total_options / angles.size(); + +// // gets all options for each of the goals, and puts them into a unified list +// // TODO ? maybe better for a max heap? +// for (const RRTPoint &goal : goal_points) { +// const std::vector> &options = +// // For now, we use optimal pathing +// tree.pathingOptions(goal, PointFetchMethod::Enum::NONE, NUMBER_OPTIONS_EACH); + +// for (const auto &[node, option] : options) { +// all_options.push_back({goal, {node, option}}); +// } +// } + +// std::sort(all_options.begin(), all_options.end(), [](const auto &a, const auto &b) { +// auto &[a_goal, a_paths] = a; +// auto &[a_node, a_option] = a_paths; +// auto &[b_goal, b_paths] = b; +// auto &[b_node, b_option] = b_paths; +// return a_option.length + a_node->getCost() < b_option.length + b_node->getCost(); +// }); + +// return all_options; +// } + +// RRTNode *RRT::sampleToGoal(int current_goal_index, int total_options) const { +// // gets all options for each of the goals +// const auto &all_options = getOptionsToGoal(current_goal_index, total_options); + +// // +// for (const auto &[goal, pair] : all_options) { +// auto &[anchor_node, option] = pair; + +// RRTNode *new_node = tree.generateNode(anchor_node, goal, option); + +// if (new_node != nullptr) { +// return new_node; +// } +// } + +// return nullptr; +// } + +// bool RRT::connectToGoal(int current_goal_index, int total_options) { +// RRTNode *goal_node = sampleToGoal(current_goal_index, total_options); + +// if (goal_node == nullptr) { +// return false; +// } + +// addNodeToTree(goal_node, current_goal_index); +// return true; +// } + +// void RRT::addNodeToTree(RRTNode *goal_node, int current_goal_index) { +// // add the node to the tree +// tree.addNode(goal_node->getParent(), goal_node); + +// // inserts the altitude into the path +// std::vector local_path = tree.getPathSegment(goal_node); + +// double start_height; +// if (current_goal_index == 0) { +// start_height = tree.getStart().coord.z; +// } else { +// start_height = tree.getAirspace().getGoal(current_goal_index - 1).z; +// } + +// double height_difference = tree.getAirspace().getGoal(current_goal_index).z - start_height; +// double height_increment = height_difference / local_path.size(); + +// for (XYZCoord &point : local_path) { +// point.z = start_height; +// start_height += height_increment; +// } + +// // adds local path to the flight path, and updates the tree +// flight_path.insert(flight_path.end(), local_path.begin(), local_path.end()); +// tree.setCurrentHead(goal_node); +// } + +// RRTNode *RRT::parseOptions(const std::vector> &options, +// const RRTPoint &sample) { +// for (auto &[node, option] : options) { +// /* +// * stop if +// * 1. the node is null +// * 2. the node is the same as the sample +// * +// * The idea is that any further options will have the same if not more +// * issues +// * +// * This shouldn't ever happen? +// */ +// // if (node == nullptr || node->getPoint() == sample) { +// // return nullptr; + +// // else, attempt to add the node to the tree +// RRTNode *sucessful_addition = tree.addSample(node, sample, option); + +// if (sucessful_addition != nullptr) { +// return sucessful_addition; +// } +// } + +// return nullptr; +// } + +// void RRT::optimizeTree(RRTNode *sample) { tree.RRTStar(sample, rewire_radius); } + + +// sanctuary of safety ForwardCoveragePathing::ForwardCoveragePathing(const RRTPoint &start, double scan_radius, Polygon bounds, Polygon airdrop_zone, const OBCConfig &config, @@ -594,6 +603,7 @@ std::vector> generateRankedNewGoalsList(const std::vector< 2. we omit the first waypoint (FATAL) - this means we never tell the computer to hit it 3. We don't know where home location is - we rely on geofencing to not fly out of bounds */ +// DO NOT TOUCH THIS MissionPath generateInitialPath(std::shared_ptr state) { // first waypoint is start @@ -609,16 +619,20 @@ MissionPath generateInitialPath(std::shared_ptr state) { // Copy elements from the second element to the last element of source into // destination all other methods of copying over crash??? + + // dont start second, incorrect asumption. Change in the RRT object for (int i = 1; i < state->mission_params.getWaypoints().size(); i++) { goals.emplace_back(state->mission_params.getWaypoints()[i]); } // update goals here + // DELETE if (state->config.pathing.rrt.generate_deviations) { Environment mapping_bounds(state->mission_params.getMappingBoundary(), {}, {}, goals, {}); goals = generateRankedNewGoalsList(goals, mapping_bounds)[0]; } + // assumes that first waypoint is home pos double init_angle = std::atan2(goals.front().y - state->mission_params.getWaypoints().front().y, goals.front().x - state->mission_params.getWaypoints().front().x); @@ -640,6 +654,7 @@ MissionPath generateInitialPath(std::shared_ptr state) { return MissionPath(MissionPath::Type::FORWARD, output_coords); } +// DO NOT TOUCH THIS MissionPath generateSearchPath(std::shared_ptr state) { std::vector gps_coords; if (state->config.pathing.coverage.method == AirdropCoverageMethod::Enum::FORWARD) { @@ -669,6 +684,7 @@ MissionPath generateSearchPath(std::shared_ptr state) { } } +// DO NOT TOUCH MissionPath generateAirdropApproach(std::shared_ptr state, const GPSCoord &goal) { // finds starting location std::shared_ptr mav = state->getMav(); diff --git a/src/pathing/tree.cpp b/src/pathing/tree.cpp index f330f70e..5bca6414 100644 --- a/src/pathing/tree.cpp +++ b/src/pathing/tree.cpp @@ -11,501 +11,503 @@ #include "utilities/rng.hpp" #include "utilities/obc_config.hpp" -RRTNode::RRTNode(const RRTPoint& point, double cost, double path_length, - const std::vector path) - : point{point}, cost{cost}, path_length(path_length), path(path) {} - -RRTNode::RRTNode(const RRTPoint& point, double cost, double path_length, - const std::vector path, RRTNodeList reachable) - : point{point}, cost{cost}, path_length(path_length), path(path), reachable{reachable} {} - -RRTNode::~RRTNode() { - for (RRTNode* node : reachable) { - delete node; - } -} - -// bool RRTNode::operator==(const RRTNode& other_node) const { -// return this->point == other_node.point && this->cost == other_node.cost; +// When we are born, we cry that we are come to this great stage of fools + +// RRTNode::RRTNode(const RRTPoint& point, double cost, double path_length, +// const std::vector path) +// : point{point}, cost{cost}, path_length(path_length), path(path) {} + +// RRTNode::RRTNode(const RRTPoint& point, double cost, double path_length, +// const std::vector path, RRTNodeList reachable) +// : point{point}, cost{cost}, path_length(path_length), path(path), reachable{reachable} {} + +// RRTNode::~RRTNode() { +// for (RRTNode* node : reachable) { +// delete node; +// } +// } + +// // bool RRTNode::operator==(const RRTNode& other_node) const { +// // return this->point == other_node.point && this->cost == other_node.cost; +// // } + +// RRTPoint& RRTNode::getPoint() { return this->point; } + +// void RRTNode::setReachable(const RRTNodeList& reachable) { +// this->reachable = reachable; +// for (RRTNode* node : reachable) { +// node->parent = this; +// } +// } + +// void RRTNode::addReachable(RRTNode* new_node) { +// this->reachable.push_back(new_node); +// new_node->parent = this; +// } + +// void RRTNode::removeReachable(RRTNode* old_node) { +// for (int i = 0; i < reachable.size(); i++) { +// if (reachable.at(i) == old_node) { +// // TODO - UNSAFE +// reachable.at(i)->parent = nullptr; +// reachable.erase(reachable.begin() + i); +// } +// } +// } + +// const RRTNodeList& RRTNode::getReachable() { return (this->reachable); } + +// double RRTNode::getCost() const { return this->cost; } + +// void RRTNode::setCost(double new_cost) { this->cost = new_cost; } + +// RRTNode* RRTNode::getParent() const { return this->parent; } + +// void RRTNode::setParent(RRTNode* new_parent) { this->parent = new_parent; } + +// const std::vector& RRTNode::getPath() const { return this->path; } + +// void RRTNode::setPath(const std::vector& path) { this->path = path; } + +// double RRTNode::getPathLength() const { return this->path_length; } + +// void RRTNode::setPathLength(double new_path_length) { this->path_length = new_path_length; } +// /* + + + + +// */ +// /** RRTTree */ +// /* + + + + +// */ + +// RRTTree::RRTTree(RRTPoint root_point, Environment airspace, Dubins dubins) +// : airspace(airspace), dubins(dubins), tree_size(1) { +// RRTNode* new_node = new RRTNode(root_point, 0, 0, {}); +// root = new_node; +// current_head = new_node; +// } + +// // TODO - seems a bit sketchy +// RRTTree::~RRTTree() { delete root; } + +// bool RRTTree::validatePath(const std::vector& path, const RRTOption& option) const { +// return airspace.isPathInBounds(path); +// // return airspace.isPathInBoundsAdv(path, option); +// } + +// RRTNode* RRTTree::generateNode(RRTNode* anchor_node, const RRTPoint& new_point, +// const RRTOption& option) const { +// const std::vector& path = dubins.generatePoints( +// anchor_node->getPoint(), new_point, option.dubins_path, option.has_straight); + +// if (!validatePath(path, option)) { +// return nullptr; +// } + +// // needs to add the node to the tree +// RRTNode* new_node = +// new RRTNode(new_point, anchor_node->getCost() + option.length, option.length, path); +// new_node->setParent(anchor_node); + +// return new_node; +// } + +// bool RRTTree::addNode(RRTNode* anchor_node, RRTNode* new_node) { +// if (new_node == nullptr || anchor_node == nullptr) { +// return false; +// } + +// anchor_node->addReachable(new_node); +// tree_size++; +// return true; +// } + +// // TODO - convert from old to new +// RRTNode* RRTTree::addSample(RRTNode* anchor_node, const RRTPoint& new_point, +// const RRTOption& option) { +// RRTNode* new_node = generateNode(anchor_node, new_point, option); + +// if (addNode(anchor_node, new_node)) { +// return new_node; +// } + +// return nullptr; +// } + +// void RRTTree::rewireEdge(RRTNode* current_node, RRTNode* previous_parent, RRTNode* new_parent, +// const std::vector& path, double path_cost) { +// // ORDER MATTERS, REMOVE THEN ADD TO PRESERVE THE CURR_NODE HAS A PARENT +// previous_parent->removeReachable(current_node); +// new_parent->addReachable(current_node); + +// // bubbles down the tree to reassign the costs +// current_node->setPath(path); +// current_node->setCost(new_parent->getCost() + path_cost); +// current_node->setPathLength(path_cost); +// reassignCosts(current_node); // } -RRTPoint& RRTNode::getPoint() { return this->point; } +// std::vector RRTTree::getKRandomNodes(int k) const { +// std::vector nodes; +// // proabability that any given node should be added +// double chance = 1.0 * k / tree_size; +// getKRandomNodesRecursive(nodes, current_head, chance); -void RRTNode::setReachable(const RRTNodeList& reachable) { - this->reachable = reachable; - for (RRTNode* node : reachable) { - node->parent = this; - } -} +// return nodes; +// } -void RRTNode::addReachable(RRTNode* new_node) { - this->reachable.push_back(new_node); - new_node->parent = this; -} +// void RRTTree::getKRandomNodesRecursive(std::vector& nodes, RRTNode* current_node, +// double chance) const { +// if (current_node == nullptr) { +// return; +// } -void RRTNode::removeReachable(RRTNode* old_node) { - for (int i = 0; i < reachable.size(); i++) { - if (reachable.at(i) == old_node) { - // TODO - UNSAFE - reachable.at(i)->parent = nullptr; - reachable.erase(reachable.begin() + i); - } - } -} +// // if the chance is less than the random number, then add the node to the list +// // TODO maybe make some check that prevents the random calls if the tree is small enough +// if (random(0, 1) < chance) { +// nodes.emplace_back(current_node); +// } -const RRTNodeList& RRTNode::getReachable() { return (this->reachable); } +// for (RRTNode* node : current_node->getReachable()) { +// getKRandomNodesRecursive(nodes, node, chance); +// } +// } + +// std::vector RRTTree::getKClosestNodes(const RRTPoint& sample, int k) const { +// std::vector closest_nodes; -double RRTNode::getCost() const { return this->cost; } +// // helper vector that associates nodes with distances +// // TODO - do some benchmarks with max-heaps to see which one is more efficient +// std::vector> nodes_by_distance; +// getKClosestNodesRecursive(nodes_by_distance, sample, current_head); -void RRTNode::setCost(double new_cost) { this->cost = new_cost; } +// // sorts the nodes by distance +// std::sort(nodes_by_distance.begin(), nodes_by_distance.end(), +// [](auto& left, auto& right) { return left.first < right.first; }); -RRTNode* RRTNode::getParent() const { return this->parent; } +// // gets either the k closest nodes, or the entire list +// int size = nodes_by_distance.size(); +// int stop_condition = std::min(k, size); +// for (int i = 0; i < stop_condition; i++) { +// closest_nodes.emplace_back(nodes_by_distance[i].second); +// } -void RRTNode::setParent(RRTNode* new_parent) { this->parent = new_parent; } +// return closest_nodes; +// } -const std::vector& RRTNode::getPath() const { return this->path; } +// void RRTTree::getKClosestNodesRecursive(std::vector>& nodes_by_distance, +// const RRTPoint& sample, RRTNode* current_node) const { +// if (current_node == nullptr) { +// return; +// } -void RRTNode::setPath(const std::vector& path) { this->path = path; } +// // ONLY considers the distance, and not the path length required to get to the node +// double distance = sample.coord.distanceToSquared(current_node->getPoint().coord); +// nodes_by_distance.push_back({distance, current_node}); -double RRTNode::getPathLength() const { return this->path_length; } +// for (RRTNode* node : current_node->getReachable()) { +// getKClosestNodesRecursive(nodes_by_distance, sample, node); +// } +// } -void RRTNode::setPathLength(double new_path_length) { this->path_length = new_path_length; } -/* +// void RRTTree::fillOptionsNodes(std::vector>& options, +// const std::vector& nodes, const RRTPoint& sample) const { +// for (RRTNode* node : nodes) { +// const std::vector& local_options = dubins.allOptions(node->getPoint(), sample); +// for (const RRTOption& option : local_options) { +// if (std::isnan(option.length) || +// option.length == std::numeric_limits::infinity()) { +// continue; +// } +// options.push_back({node, option}); +// } +// } +// } +// RRTNode* RRTTree::getRoot() const { return this->root; } -*/ -/** RRTTree */ -/* +// const XYZCoord& RRTTree::getGoal() const { return airspace.getGoal(); } +// const XYZCoord& RRTTree::getGoal(int index) const { return airspace.getGoal(index); } +// const Environment& RRTTree::getAirspace() const { return this->airspace; } +// RRTPoint RRTTree::getRandomPoint(double search_radius) const { +// // gets random point if the goal is not being used +// const XYZCoord& sample = airspace.getRandomPoint(); -*/ +// // // picks the nearest node to the sample, and then returns a point `search_radius` distance +// // away +// // // from tat point in the direction of the sample +// // std::pair nearest_node = getNearestNode(sample); -RRTTree::RRTTree(RRTPoint root_point, Environment airspace, Dubins dubins) - : airspace(airspace), dubins(dubins), tree_size(1) { - RRTNode* new_node = new RRTNode(root_point, 0, 0, {}); - root = new_node; - current_head = new_node; -} +// // int count = 0; -// TODO - seems a bit sketchy -RRTTree::~RRTTree() { delete root; } +// // while (nearest_node.second < search_radius && count < TRIES_FOR_RANDOM_POINT) { +// // sample = airspace.getRandomPoint(); +// // nearest_node = getNearestNode(sample); +// // count++; +// // } +// // // shouldn't happen, it is here for memory safety +// // if (nearest_node == nullptr) { +// // return sample; +// // } -bool RRTTree::validatePath(const std::vector& path, const RRTOption& option) const { - return airspace.isPathInBounds(path); - // return airspace.isPathInBoundsAdv(path, option); -} +// // const RRTPoint& nearest_point = nearest_node->getPoint(); -RRTNode* RRTTree::generateNode(RRTNode* anchor_node, const RRTPoint& new_point, - const RRTOption& option) const { - const std::vector& path = dubins.generatePoints( - anchor_node->getPoint(), new_point, option.dubins_path, option.has_straight); +// // const XYZCoord displacement_vector = sample.coord - nearest_point.coord; - if (!validatePath(path, option)) { - return nullptr; - } +// // // TODO - use some heuristic to make this angle as optimal as possible +// // const double angle = +// // std::atan2(displacement_vector.y, displacement_vector.x) + random(-M_PI / 2, M_PI / 2); - // needs to add the node to the tree - RRTNode* new_node = - new RRTNode(new_point, anchor_node->getCost() + option.length, option.length, path); - new_node->setParent(anchor_node); +// // // distance between the vectors, if it is less than the search radius, then it will return +// // the +// // // sample point, otherwise, it will return a point `search_radius` away from the nearest +// // point const double distance = displacement_vector.norm(); - return new_node; -} +// // if (distance < search_radius) { +// // return RRTPoint(sample.coord, angle); +// // } -bool RRTTree::addNode(RRTNode* anchor_node, RRTNode* new_node) { - if (new_node == nullptr || anchor_node == nullptr) { - return false; - } +// // RRTPoint new_point(nearest_point.coord + (search_radius / distance) * displacement_vector, +// // angle); - anchor_node->addReachable(new_node); - tree_size++; - return true; -} +// // return new_point; +// return RRTPoint(sample, random(0, TWO_PI)); +// } + +// /* +// TODO - investigate whether a max heap is better or worse +// */ +// std::vector> RRTTree::pathingOptions( +// const RRTPoint& end, PointFetchMethod::Enum point_fetch_method, int quantity_options) const { +// // fills the options list with valid values +// std::vector> options; + +// switch (point_fetch_method) { +// case PointFetchMethod::Enum::RANDOM: { +// const std::vector& nodes = getKRandomNodes(K_RANDOM_NODES); +// fillOptionsNodes(options, nodes, end); +// } break; +// case PointFetchMethod::Enum::NEAREST: { +// const std::vector& nodes = getKClosestNodes(end, K_CLOESEST_NODES); +// fillOptionsNodes(options, nodes, end); +// } break; +// case PointFetchMethod::Enum::NONE: +// fillOptions(options, current_head, end); +// break; +// default: +// fillOptions(options, current_head, end); +// break; +// } + +// // sorts the list +// std::sort(options.begin(), options.end(), [](auto& a, auto& b) { +// auto& [a_node, a_option] = a; +// auto& [b_node, b_option] = b; +// return a_option.length + a_node->getCost() < b_option.length + b_node->getCost(); +// }); + +// // the options are already sorted, why return a truncated list? +// // 2024-03-11 : because pathing to goal req a certain amount, you can change this later if you +// // want, idk c++ memory management well enough to know if tht is a good idea (for speed) + +// // if there are less options than needed amount, then just reurn the xisting list, else, +// // return a truncated list. +// if (options.size() < quantity_options) { +// return options; +// } + +// // erases everything after the last wanted element +// options.erase(options.begin() + quantity_options, options.end()); + +// return options; +// } + +// void RRTTree::fillOptions(std::vector>& options, RRTNode* node, +// const RRTPoint& end) const { +// /* +// TODO - try to limit the scope of the search to prevent too many calls to dubins +// */ +// if (node == nullptr) { +// return; +// } + +// // gets all dubins curves from the current node to the end point +// const std::vector& local_options = dubins.allOptions(node->getPoint(), end); + +// // filters out the options that are not valid +// for (const RRTOption& option : local_options) { +// if (std::isnan(option.length) || option.length == std::numeric_limits::infinity()) { +// continue; +// } -// TODO - convert from old to new -RRTNode* RRTTree::addSample(RRTNode* anchor_node, const RRTPoint& new_point, - const RRTOption& option) { - RRTNode* new_node = generateNode(anchor_node, new_point, option); +// options.push_back({node, option}); +// } - if (addNode(anchor_node, new_node)) { - return new_node; - } +// // recursively calls the function for all reachable nodes +// for (RRTNode* child : node->getReachable()) { +// fillOptions(options, child, end); +// } +// } - return nullptr; -} +// void RRTTree::RRTStar(RRTNode* sample, double rewire_radius) { +// // last element takes in the squared value of rewire_radius to prevent the need for sqrt() +// RRTStarRecursive(current_head, sample, rewire_radius * rewire_radius); +// } -void RRTTree::rewireEdge(RRTNode* current_node, RRTNode* previous_parent, RRTNode* new_parent, - const std::vector& path, double path_cost) { - // ORDER MATTERS, REMOVE THEN ADD TO PRESERVE THE CURR_NODE HAS A PARENT - previous_parent->removeReachable(current_node); - new_parent->addReachable(current_node); +// void RRTTree::setCurrentHead(RRTNode* goal) { +// if (goal == nullptr) { +// LOG_F(ERROR, "FAILURE: Goal is not in the tree"); +// return; +// } - // bubbles down the tree to reassign the costs - current_node->setPath(path); - current_node->setCost(new_parent->getCost() + path_cost); - current_node->setPathLength(path_cost); - reassignCosts(current_node); -} +// // update local parametters +// tree_size = 1; +// current_head = goal; +// } -std::vector RRTTree::getKRandomNodes(int k) const { - std::vector nodes; - // proabability that any given node should be added - double chance = 1.0 * k / tree_size; - getKRandomNodesRecursive(nodes, current_head, chance); +// // std::vector RRTTree::getPathToGoal() const { +// // RRTNode* current_node = current_head; +// // std::vector path = {}; - return nodes; -} +// // while (current_node != nullptr && current_node->getParent() != nullptr) { +// // const std::vector& edge_path = current_node->getPath(); -void RRTTree::getKRandomNodesRecursive(std::vector& nodes, RRTNode* current_node, - double chance) const { - if (current_node == nullptr) { - return; - } +// // path.insert(path.begin(), edge_path.begin() + 1, edge_path.end()); +// // current_node = current_node->getParent(); +// // } - // if the chance is less than the random number, then add the node to the list - // TODO maybe make some check that prevents the random calls if the tree is small enough - if (random(0, 1) < chance) { - nodes.emplace_back(current_node); - } +// // // loop above misses the first node, this adds it manually +// // path.insert(path.begin(), current_node->getPoint().coord); +// // return path; +// // } - for (RRTNode* node : current_node->getReachable()) { - getKRandomNodesRecursive(nodes, node, chance); - } -} - -std::vector RRTTree::getKClosestNodes(const RRTPoint& sample, int k) const { - std::vector closest_nodes; - - // helper vector that associates nodes with distances - // TODO - do some benchmarks with max-heaps to see which one is more efficient - std::vector> nodes_by_distance; - getKClosestNodesRecursive(nodes_by_distance, sample, current_head); - - // sorts the nodes by distance - std::sort(nodes_by_distance.begin(), nodes_by_distance.end(), - [](auto& left, auto& right) { return left.first < right.first; }); - - // gets either the k closest nodes, or the entire list - int size = nodes_by_distance.size(); - int stop_condition = std::min(k, size); - for (int i = 0; i < stop_condition; i++) { - closest_nodes.emplace_back(nodes_by_distance[i].second); - } - - return closest_nodes; -} - -void RRTTree::getKClosestNodesRecursive(std::vector>& nodes_by_distance, - const RRTPoint& sample, RRTNode* current_node) const { - if (current_node == nullptr) { - return; - } - - // ONLY considers the distance, and not the path length required to get to the node - double distance = sample.coord.distanceToSquared(current_node->getPoint().coord); - nodes_by_distance.push_back({distance, current_node}); - - for (RRTNode* node : current_node->getReachable()) { - getKClosestNodesRecursive(nodes_by_distance, sample, node); - } -} - -void RRTTree::fillOptionsNodes(std::vector>& options, - const std::vector& nodes, const RRTPoint& sample) const { - for (RRTNode* node : nodes) { - const std::vector& local_options = dubins.allOptions(node->getPoint(), sample); - - for (const RRTOption& option : local_options) { - if (std::isnan(option.length) || - option.length == std::numeric_limits::infinity()) { - continue; - } - - options.push_back({node, option}); - } - } -} - -RRTNode* RRTTree::getRoot() const { return this->root; } - -const XYZCoord& RRTTree::getGoal() const { return airspace.getGoal(); } - -const XYZCoord& RRTTree::getGoal(int index) const { return airspace.getGoal(index); } - -const Environment& RRTTree::getAirspace() const { return this->airspace; } - -RRTPoint RRTTree::getRandomPoint(double search_radius) const { - // gets random point if the goal is not being used - const XYZCoord& sample = airspace.getRandomPoint(); - - // // picks the nearest node to the sample, and then returns a point `search_radius` distance - // away - // // from tat point in the direction of the sample - // std::pair nearest_node = getNearestNode(sample); - - // int count = 0; - - // while (nearest_node.second < search_radius && count < TRIES_FOR_RANDOM_POINT) { - // sample = airspace.getRandomPoint(); - // nearest_node = getNearestNode(sample); - // count++; - // } - // // shouldn't happen, it is here for memory safety - // if (nearest_node == nullptr) { - // return sample; - // } - - // const RRTPoint& nearest_point = nearest_node->getPoint(); - - // const XYZCoord displacement_vector = sample.coord - nearest_point.coord; - - // // TODO - use some heuristic to make this angle as optimal as possible - // const double angle = - // std::atan2(displacement_vector.y, displacement_vector.x) + random(-M_PI / 2, M_PI / 2); - - // // distance between the vectors, if it is less than the search radius, then it will return - // the - // // sample point, otherwise, it will return a point `search_radius` away from the nearest - // point const double distance = displacement_vector.norm(); - - // if (distance < search_radius) { - // return RRTPoint(sample.coord, angle); - // } - - // RRTPoint new_point(nearest_point.coord + (search_radius / distance) * displacement_vector, - // angle); - - // return new_point; - return RRTPoint(sample, random(0, TWO_PI)); -} - -/* - TODO - investigate whether a max heap is better or worse -*/ -std::vector> RRTTree::pathingOptions( - const RRTPoint& end, PointFetchMethod::Enum point_fetch_method, int quantity_options) const { - // fills the options list with valid values - std::vector> options; - - switch (point_fetch_method) { - case PointFetchMethod::Enum::RANDOM: { - const std::vector& nodes = getKRandomNodes(K_RANDOM_NODES); - fillOptionsNodes(options, nodes, end); - } break; - case PointFetchMethod::Enum::NEAREST: { - const std::vector& nodes = getKClosestNodes(end, K_CLOESEST_NODES); - fillOptionsNodes(options, nodes, end); - } break; - case PointFetchMethod::Enum::NONE: - fillOptions(options, current_head, end); - break; - default: - fillOptions(options, current_head, end); - break; - } - - // sorts the list - std::sort(options.begin(), options.end(), [](auto& a, auto& b) { - auto& [a_node, a_option] = a; - auto& [b_node, b_option] = b; - return a_option.length + a_node->getCost() < b_option.length + b_node->getCost(); - }); - - // the options are already sorted, why return a truncated list? - // 2024-03-11 : because pathing to goal req a certain amount, you can change this later if you - // want, idk c++ memory management well enough to know if tht is a good idea (for speed) - - // if there are less options than needed amount, then just reurn the xisting list, else, - // return a truncated list. - if (options.size() < quantity_options) { - return options; - } - - // erases everything after the last wanted element - options.erase(options.begin() + quantity_options, options.end()); - - return options; -} - -void RRTTree::fillOptions(std::vector>& options, RRTNode* node, - const RRTPoint& end) const { - /* - TODO - try to limit the scope of the search to prevent too many calls to dubins - */ - if (node == nullptr) { - return; - } - - // gets all dubins curves from the current node to the end point - const std::vector& local_options = dubins.allOptions(node->getPoint(), end); - - // filters out the options that are not valid - for (const RRTOption& option : local_options) { - if (std::isnan(option.length) || option.length == std::numeric_limits::infinity()) { - continue; - } - - options.push_back({node, option}); - } - - // recursively calls the function for all reachable nodes - for (RRTNode* child : node->getReachable()) { - fillOptions(options, child, end); - } -} - -void RRTTree::RRTStar(RRTNode* sample, double rewire_radius) { - // last element takes in the squared value of rewire_radius to prevent the need for sqrt() - RRTStarRecursive(current_head, sample, rewire_radius * rewire_radius); -} - -void RRTTree::setCurrentHead(RRTNode* goal) { - if (goal == nullptr) { - LOG_F(ERROR, "FAILURE: Goal is not in the tree"); - return; - } - - // update local parametters - tree_size = 1; - current_head = goal; -} - -// std::vector RRTTree::getPathToGoal() const { -// RRTNode* current_node = current_head; +// std::vector RRTTree::getPathSegment(RRTNode* node) const { +// RRTNode* current_node = node; // std::vector path = {}; -// while (current_node != nullptr && current_node->getParent() != nullptr) { +// while (current_node != current_head) { +// if (current_node == nullptr) { +// LOG_F(ERROR, "TREE: Segement does not find a path"); +// return {}; +// } + // const std::vector& edge_path = current_node->getPath(); // path.insert(path.begin(), edge_path.begin() + 1, edge_path.end()); // current_node = current_node->getParent(); // } -// // loop above misses the first node, this adds it manually -// path.insert(path.begin(), current_node->getPoint().coord); // return path; // } -std::vector RRTTree::getPathSegment(RRTNode* node) const { - RRTNode* current_node = node; - std::vector path = {}; +// RRTPoint& RRTTree::getStart() const { return root->getPoint(); } + +// /*-----------------*/ +// /* RRTTree Private */ +// /*-----------------*/ - while (current_node != current_head) { - if (current_node == nullptr) { - LOG_F(ERROR, "TREE: Segement does not find a path"); - return {}; - } +// // std::pair RRTTree::getNearestNode(const XYZCoord& point) const { +// // RRTNode* nearest = nullptr; +// // double min_distance = std::numeric_limits::infinity(); - const std::vector& edge_path = current_node->getPath(); +// // for (auto& node : node_set) { +// // double distance = point.distanceToSquared(node->getPoint().coord); +// // if (distance < min_distance) { +// // min_distance = distance; +// // nearest = node; +// // } +// // } - path.insert(path.begin(), edge_path.begin() + 1, edge_path.end()); - current_node = current_node->getParent(); - } +// // return {nearest, min_distance}; +// // } + +// void RRTTree::RRTStarRecursive(RRTNode* current_node, RRTNode* sample, +// double rewire_radius_squared) { +// // base case +// if (current_node == nullptr) { +// return; +// } - return path; -} +// // for all nodes past the current node, attempt to rewire them +// for (RRTNode* child : current_node->getReachable()) { +// // get the distance between the current node and the nearest node +// if (child->getPoint().distanceToSquared(sample->getPoint()) > rewire_radius_squared) { +// continue; +// } + +// // the sample shouldn't have any children +// if (child == sample) { +// return; +// } -RRTPoint& RRTTree::getStart() const { return root->getPoint(); } +// // get the dubins options (sorted) +// const std::vector& options = +// dubins.allOptions(sample->getPoint(), child->getPoint(), true); -/*-----------------*/ -/* RRTTree Private */ -/*-----------------*/ +// // for each option +// for (const RRTOption& option : options) { +// if (std::isnan(option.length) || +// option.length == std::numeric_limits::infinity()) { +// break; +// } -// std::pair RRTTree::getNearestNode(const XYZCoord& point) const { -// RRTNode* nearest = nullptr; -// double min_distance = std::numeric_limits::infinity(); +// // if the node is uncompetitive, move onto the next node +// double new_cost = sample->getCost() + option.length; +// double cost = child->getCost(); -// for (auto& node : node_set) { -// double distance = point.distanceToSquared(node->getPoint().coord); -// if (distance < min_distance) { -// min_distance = distance; -// nearest = node; +// if (new_cost >= cost) { +// break; +// } + +// // if the new cost is less than the current cost +// // check if new path is valid +// const std::vector& path = dubins.generatePoints( +// sample->getPoint(), child->getPoint(), option.dubins_path, option.has_straight); + +// if (!validatePath(path, option)) { +// continue; +// } + +// // rewire the edge +// rewireEdge(child, current_node, sample, path, option.length); // } // } -// return {nearest, min_distance}; +// // recurse +// for (RRTNode* child : current_node->getReachable()) { +// RRTStarRecursive(child, sample, rewire_radius_squared); +// } // } -void RRTTree::RRTStarRecursive(RRTNode* current_node, RRTNode* sample, - double rewire_radius_squared) { - // base case - if (current_node == nullptr) { - return; - } - - // for all nodes past the current node, attempt to rewire them - for (RRTNode* child : current_node->getReachable()) { - // get the distance between the current node and the nearest node - if (child->getPoint().distanceToSquared(sample->getPoint()) > rewire_radius_squared) { - continue; - } - - // the sample shouldn't have any children - if (child == sample) { - return; - } - - // get the dubins options (sorted) - const std::vector& options = - dubins.allOptions(sample->getPoint(), child->getPoint(), true); - - // for each option - for (const RRTOption& option : options) { - if (std::isnan(option.length) || - option.length == std::numeric_limits::infinity()) { - break; - } - - // if the node is uncompetitive, move onto the next node - double new_cost = sample->getCost() + option.length; - double cost = child->getCost(); - - if (new_cost >= cost) { - break; - } - - // if the new cost is less than the current cost - // check if new path is valid - const std::vector& path = dubins.generatePoints( - sample->getPoint(), child->getPoint(), option.dubins_path, option.has_straight); - - if (!validatePath(path, option)) { - continue; - } - - // rewire the edge - rewireEdge(child, current_node, sample, path, option.length); - } - } - - // recurse - for (RRTNode* child : current_node->getReachable()) { - RRTStarRecursive(child, sample, rewire_radius_squared); - } -} - -void RRTTree::reassignCosts(RRTNode* changed_node) { - if (changed_node == nullptr) { - return; - } - - for (RRTNode* child : changed_node->getReachable()) { - reassignCostsRecursive(changed_node, child, changed_node->getCost()); - } -} - -void RRTTree::reassignCostsRecursive(RRTNode* parent, RRTNode* current_node, double path_cost) { - if (current_node == nullptr) { - return; - } - - // reassigns the cost: cost to get to the parent + known path length between parent and child - current_node->setCost(path_cost + current_node->getPathLength()); - for (RRTNode* neighbor : current_node->getReachable()) { - reassignCostsRecursive(current_node, neighbor, current_node->getCost()); - } -} +// void RRTTree::reassignCosts(RRTNode* changed_node) { +// if (changed_node == nullptr) { +// return; +// } + +// for (RRTNode* child : changed_node->getReachable()) { +// reassignCostsRecursive(changed_node, child, changed_node->getCost()); +// } +// } + +// void RRTTree::reassignCostsRecursive(RRTNode* parent, RRTNode* current_node, double path_cost) { +// if (current_node == nullptr) { +// return; +// } + +// // reassigns the cost: cost to get to the parent + known path length between parent and child +// current_node->setCost(path_cost + current_node->getPathLength()); +// for (RRTNode* neighbor : current_node->getReachable()) { +// reassignCostsRecursive(current_node, neighbor, current_node->getCost()); +// } +// } From a06a5a94357c76d929f2428b9ee7cea59e4b818c Mon Sep 17 00:00:00 2001 From: CSTBook Date: Sun, 2 Nov 2025 23:55:01 +0000 Subject: [PATCH 2/3] Replaced RRTNode* with a smart pointer and removed all references to parent within RRTNode --- include/pathing/tree.hpp | 85 +++++++++++++------------------ src/pathing/tree.cpp | 106 +++++++++++++++++---------------------- 2 files changed, 80 insertions(+), 111 deletions(-) diff --git a/include/pathing/tree.hpp b/include/pathing/tree.hpp index f834877b..db9d92c4 100644 --- a/include/pathing/tree.hpp +++ b/include/pathing/tree.hpp @@ -6,6 +6,7 @@ #include #include #include +#include #include "pathing/dubins.hpp" #include "pathing/environment.hpp" @@ -14,15 +15,14 @@ #include "utilities/rng.hpp" class RRTNode; -typedef std::vector RRTNodeList; -typedef XYZCoord Vector; +typedef std::shared_ptr RRTNodePtr; class RRTNode { public: RRTNode(const RRTPoint& point, double cost, double path_length, const std::vector path); RRTNode(const RRTPoint& point, double cost, double path_length, - const std::vector path, RRTNodeList reachable); + const std::vector path, std::vector reachable); /** * Destructor for RRTNode object @@ -44,24 +44,24 @@ class RRTNode { /* * Set the reachable (neighbors) list for this RRTNode object */ - void setReachable(const RRTNodeList& reachable); + void setReachable(const std::vector& reachable); /* * Add a new node to the end of this node's reachable list. * Set the new node's parent to be this node. */ - void addReachable(RRTNode* new_node); + void addReachable(RRTNodePtr new_node); /* * Remove a specific node from this node's reachable list. * Set the removed node's parent pointer to be null. */ - void removeReachable(RRTNode* old_node); + void removeReachable(RRTNodePtr old_node); /* * Return a reference to this node's reachable list */ - const RRTNodeList& getReachable(); + const std::vector& getReachable(); /* * Get the cost associated with this node @@ -73,20 +73,6 @@ class RRTNode { */ void setCost(double new_cost); - /** - * Get the parent of this node - * - * @return RRTNode* pointer to parent node - */ - RRTNode* getParent() const; - - /** - * Set the parent of this node - * - * @param new_parent pointer to new parent node - */ - void setParent(RRTNode* new_parent); - /** * Get the path associated with this node * @@ -117,10 +103,9 @@ class RRTNode { private: RRTPoint point; - RRTNodeList reachable{}; + std::vector reachable{}; double cost; double path_length; - RRTNode* parent{}; std::vector path{}; }; @@ -132,7 +117,7 @@ class RRTTree { /** * Generates node without adding it to the tree */ - RRTNode* generateNode(RRTNode* anchor_node, const RRTPoint& new_point, + RRTNodePtr generateNode(RRTNodePtr anchor_node, const RRTPoint& new_point, const RRTOption& option) const; /** @@ -141,20 +126,20 @@ class RRTTree { * @param anchor_node ==> the node to connect to * @param new_node ==> the node to add */ - bool addNode(RRTNode* anchor_node, RRTNode* new_node); + bool addNode(RRTNodePtr anchor_node, RRTNodePtr new_node); /* * Add a node to the RRTTree. * If adding the first node to the tree, connectTo can be anything. */ - RRTNode* addSample(RRTNode* anchor_node, const RRTPoint& new_point, const RRTOption& option); + RRTNodePtr addSample(RRTNodePtr anchor_node, const RRTPoint& new_point, const RRTOption& option); /** * Returns a pointer to the root node * - * @return RRTNode* pointer to root node + * @return RRTNodePtr pointer to root node */ - RRTNode* getRoot() const; + RRTNodePtr getRoot() const; /** * Get goal point @@ -184,7 +169,7 @@ class RRTTree { */ RRTPoint getRandomPoint(double search_radius) const; - bool validatePath(const std::vector& path, const RRTOption& options) const; + bool validatePath(const std::vector& path, const RRTOption& options) const; /** * Returns a sorted list of the paths to get from a given node to the sampled @@ -196,7 +181,7 @@ class RRTTree { * function * @return ==> mininum sorted list of pairs of */ - std::vector> pathingOptions( + std::vector> pathingOptions( const RRTPoint& end, PointFetchMethod::Enum path_option = PointFetchMethod::Enum::NONE, int quantity_options = MAX_DUBINS_OPTIONS_TO_PARSE) const; @@ -205,14 +190,14 @@ class RRTTree { * @param sample ==> the point to used as the base * @param rewire_radius ==> the radius to search for nodes to rewire */ - void RRTStar(RRTNode* sample, double rewire_radius); + void RRTStar(RRTNodePtr sample, double rewire_radius); /** * Changes the currentHead to the given goal * * @param goal ==> the goal to change the currentHead to */ - void setCurrentHead(RRTNode* goal); + void setCurrentHead(RRTNodePtr goal); // /** // * _____| UNUSED |_____ @@ -234,8 +219,8 @@ class RRTTree { * @param path ==> the new path new_parrent --> current_point * @param path_cost ==> the cost of the new path */ - void rewireEdge(RRTNode* current_point, RRTNode* previous_parent, RRTNode* new_parent, - const std::vector& path, double path_cost); + void rewireEdge(RRTNodePtr current_point, RRTNodePtr previous_parent, RRTNodePtr new_parent, + const std::vector& path, double path_cost); /** * Gets K random nodes from the tree, starting at the current head @@ -243,7 +228,7 @@ class RRTTree { * @param k ==> the number of nodes to get * @return ==> list of k random nodes (unordered) */ - std::vector getKRandomNodes(int k) const; + std::vector getKRandomNodes(int k) const; /** * __Recursive Helper__ @@ -254,7 +239,7 @@ class RRTTree { * @param k ==> the number of nodes to get * @param chance ==> the chance to add the current node to the list */ - void getKRandomNodesRecursive(std::vector& nodes, RRTNode* current_node, + void getKRandomNodesRecursive(std::vector& nodes, RRTNodePtr current_node, double chance) const; /** @@ -264,7 +249,7 @@ class RRTTree { * @param k ==> the number of nodes to get * @return ==> list (ordered) of k closest nodes */ - std::vector getKClosestNodes(const RRTPoint& sample, int k) const; + std::vector getKClosestNodes(const RRTPoint& sample, int k) const; /** * __Recursive Helper__ @@ -274,8 +259,8 @@ class RRTTree { * @param sample ==> the point to find the closest nodes to * @param current_node ==> the current node that is being accessed */ - void getKClosestNodesRecursive(std::vector>& nodes_by_distance, - const RRTPoint& sample, RRTNode* current_node) const; + void getKClosestNodesRecursive(std::vector>& nodes_by_distance, + const RRTPoint& sample, RRTNodePtr current_node) const; /** * Fills in a list of options from an existing list of nodes @@ -284,8 +269,8 @@ class RRTTree { * @param nodes ==> the list of nodes to parse * @param sample ==> the end point that the options will be connected to */ - void fillOptionsNodes(std::vector>& options, - const std::vector& nodes, const RRTPoint& sample) const; + void fillOptionsNodes(std::vector>& options, + const std::vector& nodes, const RRTPoint& sample) const; /** * Returns the segment of path from the given node to the current head @@ -293,7 +278,7 @@ class RRTTree { * @param node ==> the node to start the path from * @return ==> the path from the node to the current head */ - std::vector getPathSegment(RRTNode* node) const; + std::vector getPathSegment(RRTNodePtr node) const; /** * Returns the start RRTPoint @@ -303,8 +288,8 @@ class RRTTree { RRTPoint& getStart() const; private: - RRTNode* root; - RRTNode* current_head; + RRTNodePtr root; + RRTNodePtr current_head; Environment airspace; Dubins dubins; int tree_size; @@ -314,7 +299,7 @@ class RRTTree { * * @param node ==> the root of the tree to delete */ - void deleteTree(RRTNode* node); + void deleteTree(RRTNodePtr node); /** * traverses the tree, and puts in all RRTOptions from dubins into a list @@ -324,7 +309,7 @@ class RRTTree { * @param node ==> current node that will be traversed (DFS) * @param sample ==> the end point that the options will be connected to */ - void fillOptions(std::vector>& options, RRTNode* node, + void fillOptions(std::vector>& options, RRTNodePtr node, const RRTPoint& sample) const; /** @@ -333,7 +318,7 @@ class RRTTree { * @param point ==> the point to find the nearest node to * @return ==> the nearest node to the point */ - // std::pair getNearestNode(const XYZCoord& point) const; + // std::pair getNearestNode(const XYZCoord& point) const; /** * RRTStar Recursive @@ -346,7 +331,7 @@ class RRTTree { * @param sample ==> sampled point * @param search_radius ==> the radius to search for nodes to rewire */ - void RRTStarRecursive(RRTNode* current_node, RRTNode* sample, double rewire_radius); + void RRTStarRecursive(RRTNodePtr current_node, RRTNodePtr sample, double rewire_radius); /** * After rewire edge, it goes down the tree and reassigns the cost of the @@ -354,7 +339,7 @@ class RRTTree { * * @param changed_node the node that has been changed */ - void reassignCosts(RRTNode* changed_node); + void reassignCosts(RRTNodePtr changed_node); /** * Recurses down the tree to reassign the costs of the nodes @@ -364,7 +349,7 @@ class RRTTree { * @param node ==> the current node * @param path_cost ==> the cost of the path to the current node */ - void reassignCostsRecursive(RRTNode* parent, RRTNode* current_node, double cost_difference); + void reassignCostsRecursive(RRTNodePtr parent, RRTNodePtr current_node, double cost_difference); }; #endif // INCLUDE_PATHING_TREE_HPP_ diff --git a/src/pathing/tree.cpp b/src/pathing/tree.cpp index 5bca6414..12916f4a 100644 --- a/src/pathing/tree.cpp +++ b/src/pathing/tree.cpp @@ -13,88 +13,72 @@ // When we are born, we cry that we are come to this great stage of fools -// RRTNode::RRTNode(const RRTPoint& point, double cost, double path_length, -// const std::vector path) -// : point{point}, cost{cost}, path_length(path_length), path(path) {} -// RRTNode::RRTNode(const RRTPoint& point, double cost, double path_length, -// const std::vector path, RRTNodeList reachable) -// : point{point}, cost{cost}, path_length(path_length), path(path), reachable{reachable} {} +/* +TODO: rewrite the RRT Node implementation slightly to utilize smart pointers for the reachable +remove the parents since that introduces insecurity +*/ -// RRTNode::~RRTNode() { -// for (RRTNode* node : reachable) { -// delete node; -// } -// } +RRTNode::RRTNode(const RRTPoint& point, double cost, double path_length, + const std::vector path) + : point{point}, cost{cost}, path_length(path_length), path(path) {} + +RRTNode::RRTNode(const RRTPoint& point, double cost, double path_length, + const std::vector path, std::vector reachable) + : point{point}, cost{cost}, path_length(path_length), path(path), reachable{reachable} {} + +RRTNode::~RRTNode() { + reachable.clear(); +} // // bool RRTNode::operator==(const RRTNode& other_node) const { // // return this->point == other_node.point && this->cost == other_node.cost; // // } -// RRTPoint& RRTNode::getPoint() { return this->point; } - -// void RRTNode::setReachable(const RRTNodeList& reachable) { -// this->reachable = reachable; -// for (RRTNode* node : reachable) { -// node->parent = this; -// } -// } +RRTPoint& RRTNode::getPoint() { return this->point; } -// void RRTNode::addReachable(RRTNode* new_node) { -// this->reachable.push_back(new_node); -// new_node->parent = this; -// } +void RRTNode::setReachable(const std::vector& reachable) { + this->reachable = reachable; +} +void RRTNode::addReachable(RRTNodePtr new_node) { + this->reachable.push_back(new_node); +} -// void RRTNode::removeReachable(RRTNode* old_node) { -// for (int i = 0; i < reachable.size(); i++) { -// if (reachable.at(i) == old_node) { -// // TODO - UNSAFE -// reachable.at(i)->parent = nullptr; -// reachable.erase(reachable.begin() + i); -// } -// } -// } +void RRTNode::removeReachable(RRTNodePtr old_node) { + for (int i = 0; i < reachable.size(); i++) { + if (reachable.at(i) == old_node) { + reachable.erase(reachable.begin() + i); + return; + } + } +} -// const RRTNodeList& RRTNode::getReachable() { return (this->reachable); } +const std::vector& RRTNode::getReachable() { return (this->reachable); } -// double RRTNode::getCost() const { return this->cost; } +double RRTNode::getCost() const { return this->cost; } -// void RRTNode::setCost(double new_cost) { this->cost = new_cost; } +void RRTNode::setCost(double new_cost) { this->cost = new_cost; } -// RRTNode* RRTNode::getParent() const { return this->parent; } +const std::vector& RRTNode::getPath() const { return this->path; } -// void RRTNode::setParent(RRTNode* new_parent) { this->parent = new_parent; } +void RRTNode::setPath(const std::vector& path) { this->path = path; } -// const std::vector& RRTNode::getPath() const { return this->path; } +double RRTNode::getPathLength() const { return this->path_length; } -// void RRTNode::setPath(const std::vector& path) { this->path = path; } +void RRTNode::setPathLength(double new_path_length) { this->path_length = new_path_length; } -// double RRTNode::getPathLength() const { return this->path_length; } -// void RRTNode::setPathLength(double new_path_length) { this->path_length = new_path_length; } -// /* +/** RRTTree */ +RRTTree::RRTTree(RRTPoint root_point, Environment airspace, Dubins dubins) + : airspace(airspace), dubins(dubins), tree_size(1) { + RRTNodePtr new_node = std::make_shared(root_point, 0, 0, {}); + root = new_node; + current_head = new_node; +} - -// */ -// /** RRTTree */ -// /* - - - - -// */ - -// RRTTree::RRTTree(RRTPoint root_point, Environment airspace, Dubins dubins) -// : airspace(airspace), dubins(dubins), tree_size(1) { -// RRTNode* new_node = new RRTNode(root_point, 0, 0, {}); -// root = new_node; -// current_head = new_node; -// } - -// // TODO - seems a bit sketchy -// RRTTree::~RRTTree() { delete root; } +RRTTree::~RRTTree() { root.reset(); } // bool RRTTree::validatePath(const std::vector& path, const RRTOption& option) const { // return airspace.isPathInBounds(path); From 4eed9010781a5a0896f8bc95baeecf3a9dd836f3 Mon Sep 17 00:00:00 2001 From: CSTBook Date: Wed, 5 Nov 2025 01:53:23 +0000 Subject: [PATCH 3/3] small changes --- include/pathing/environment.hpp | 43 +- include/pathing/tree.hpp | 91 ---- src/pathing/environment.cpp | 897 +++++++++++++++----------------- src/pathing/static.cpp | 2 + src/pathing/tree.cpp | 537 +++++++------------ 5 files changed, 594 insertions(+), 976 deletions(-) diff --git a/include/pathing/environment.hpp b/include/pathing/environment.hpp index 892b6194..0b826dab 100644 --- a/include/pathing/environment.hpp +++ b/include/pathing/environment.hpp @@ -17,25 +17,11 @@ * * [FUTURE] * - add dynamic shrinking and enlarging of the boundary - * - add dynamic obstacles */ class Environment { public: Environment(const Polygon& valid_region, const Polygon& airdrop_zone, - const Polygon& mapping_region, const std::vector& goals, - const std::vector& obstacles); - - /** - * Check if a point is in the valid region - * - * TODO - analysis if checking all regions for a given point at one time - * is optimal. The alternative would be to check each region individually - * for all points - * - * @param point the point to check - * @return true if the point is in the valid region, false otherwise - */ - bool isPointInBounds(const XYZCoord& point) const; + const Polygon& mapping_region, const std::vector& goals); /** * Check if an entire flight path is in bounds @@ -49,20 +35,6 @@ class Environment { */ bool isPathInBounds(const std::vector& path) const; - /** - * - * Check if an entire flight path is in bounds - * - * Attemps to skip a straight section by checking line segments instead of - * points, this doesn't actually end up making a large differernce with small - * path length? - * - * @param path the path to check - * @param option the RRT option associated with the path - * @return true if the path is in bounds, false otherwise - */ - bool isPathInBoundsAdv(const std::vector& path, const RRTOption& option) const; - /** * Get the goal point * can be unsafe if goals_found is not in bounds @@ -118,18 +90,6 @@ class Environment { */ static bool isPointInPolygon(const Polygon& polygon, const XYZCoord& point); - /** - * Checks wheter a line segment is in bounds or not, it must NOT intersect - * either the valid region or the obstacles - * - * ASSUMES THAT THE END POINTS ARE IN THE POLYGON - * - * @param start_point ==> start point of the line segment - * @param end_point ==> end point of the line segment - * @return ==> whether or not the line segment is in bounds - */ - bool isLineInBounds(const XYZCoord& start_point, const XYZCoord& end_point) const; - /** * Determines whether a line segment intersects the polygon * @@ -272,7 +232,6 @@ class Environment { const Polygon airdrop_zone; // boundary of the airdrop zone (subset of valid_region) 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 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/pathing/tree.hpp b/include/pathing/tree.hpp index db9d92c4..97c2c15e 100644 --- a/include/pathing/tree.hpp +++ b/include/pathing/tree.hpp @@ -185,13 +185,6 @@ class RRTTree { const RRTPoint& end, PointFetchMethod::Enum path_option = PointFetchMethod::Enum::NONE, int quantity_options = MAX_DUBINS_OPTIONS_TO_PARSE) const; - /** DOES RRT* for the program - * - * @param sample ==> the point to used as the base - * @param rewire_radius ==> the radius to search for nodes to rewire - */ - void RRTStar(RRTNodePtr sample, double rewire_radius); - /** * Changes the currentHead to the given goal * @@ -209,59 +202,6 @@ class RRTTree { // */ // std::vector getPathToGoal() const; - /** - * Rewires an edge from an old path to a new path. - * preserves ALL elements of the tree (i.e. NO elements are removed). - * - * @param current_point ==> the current/end point to be rewired - * @param previous_parent ==> the previous parent to the current point - * @param new_parent ==> the new parent to the current point - * @param path ==> the new path new_parrent --> current_point - * @param path_cost ==> the cost of the new path - */ - void rewireEdge(RRTNodePtr current_point, RRTNodePtr previous_parent, RRTNodePtr new_parent, - const std::vector& path, double path_cost); - - /** - * Gets K random nodes from the tree, starting at the current head - * - * @param k ==> the number of nodes to get - * @return ==> list of k random nodes (unordered) - */ - std::vector getKRandomNodes(int k) const; - - /** - * __Recursive Helper__ - * Gets K random nodes from the tree, starting at the current head - * - * @param nodes ==> the list (reference) of nodes to add to - * @param current_node ==> the current node that is being accessed - * @param k ==> the number of nodes to get - * @param chance ==> the chance to add the current node to the list - */ - void getKRandomNodesRecursive(std::vector& nodes, RRTNodePtr current_node, - double chance) const; - - /** - * Gets the k closest nodes to a given point - * - * @param sample ==> the point to find the closest nodes to - * @param k ==> the number of nodes to get - * @return ==> list (ordered) of k closest nodes - */ - std::vector getKClosestNodes(const RRTPoint& sample, int k) const; - - /** - * __Recursive Helper__ - * Gets the k closest nodes to a given point - * - * @param nodes_by_distance ==> the list (reference) of {distance, node} to add to - * @param sample ==> the point to find the closest nodes to - * @param current_node ==> the current node that is being accessed - */ - void getKClosestNodesRecursive(std::vector>& nodes_by_distance, - const RRTPoint& sample, RRTNodePtr current_node) const; - /** * Fills in a list of options from an existing list of nodes * @@ -319,37 +259,6 @@ class RRTTree { * @return ==> the nearest node to the point */ // std::pair getNearestNode(const XYZCoord& point) const; - - /** - * RRTStar Recursive - * (RECURSIVE HELPER) - * Rewires the tree by finding paths that are more efficintly routed through - * the sample. Only searches for nodes a specific radius around the sample - * to reduce computational expense - * - * @param current_node ==> current node (DFS) - * @param sample ==> sampled point - * @param search_radius ==> the radius to search for nodes to rewire - */ - void RRTStarRecursive(RRTNodePtr current_node, RRTNodePtr sample, double rewire_radius); - - /** - * After rewire edge, it goes down the tree and reassigns the cost of the - * nodes - * - * @param changed_node the node that has been changed - */ - void reassignCosts(RRTNodePtr changed_node); - - /** - * Recurses down the tree to reassign the costs of the nodes - * (RECURSIVE HELPER) - * - * @param parent ==> the parent node - * @param node ==> the current node - * @param path_cost ==> the cost of the path to the current node - */ - void reassignCostsRecursive(RRTNodePtr parent, RRTNodePtr current_node, double cost_difference); }; #endif // INCLUDE_PATHING_TREE_HPP_ diff --git a/src/pathing/environment.cpp b/src/pathing/environment.cpp index 695c7d9b..0493fd8f 100644 --- a/src/pathing/environment.cpp +++ b/src/pathing/environment.cpp @@ -19,485 +19,418 @@ Or to take arms against a sea of segfaults And by opposing end them. */ -// Environment::Environment(const Polygon& valid_region, const Polygon& airdrop_zone, -// const Polygon& mapping_region, const std::vector& goals, -// const std::vector& obstacles) -// : valid_region(valid_region), -// airdrop_zone(airdrop_zone), -// mapping_region(mapping_region), -// goals(goals), -// goals_found(0), -// bounds(findBounds(valid_region)), -// obstacles(obstacles) {} - -// bool Environment::isPointInBounds(const XYZCoord& point) const { -// if (!isPointInPolygon(valid_region, point)) { -// return false; -// } - -// for (const Polygon& obstacle : obstacles) { -// if (isPointInPolygon(obstacle, point)) { -// return false; -// } -// } - -// return true; -// } - -// bool Environment::isPathInBounds(const std::vector& path) const { -// /* -// * starts from the center, and walks in botht directions in intervals, this -// * is motivated by the idea that if an endpoint is in bounds, then the path -// * is most likely out of bounds in the center of the path. Furthermore, -// * moving by larger step would be more efficient than checking every point. -// * -// * ^only for small step length does this make much sense -// */ -// const int center = path.size() / 2; -// const int interval = ENV_PATH_VALIDATION_STEP_SIZE; - -// // special case not checked in the loop -// if (!isPointInBounds(path[center])) { -// return false; -// } - -// for (int i = 1; i < interval; i++) { -// int count = center + i; - -// // to the right -// while (count < path.size()) { -// if (!isPointInBounds(path[count])) { -// return false; -// } -// count += interval; -// } - -// // to the left -// count = center - i; -// while (count >= 0) { -// if (!isPointInBounds(path[count])) { -// return false; -// } -// count -= interval; -// } -// } - -// return true; -// } - -// // bool Environment::isPathInBoundsAdv(const std::vector& path, -// // const RRTOption& option) const { -// // if (!option.has_straight) { -// // return isPathInBounds(path); -// // } - -// // // finds the last point on the first curve, and the first point on the second curve -// // // does this using the option, using arclength and the point separation -// // const int first_curve_end = -// // std::abs(option.dubins_path.beta_0) * TURNING_RADIUS / POINT_SEPARATION + 1; -// // const int second_curve_start = -// // path.size() - std::abs(option.dubins_path.beta_2) * TURNING_RADIUS / POINT_SEPARATION; - -// // // sanity check -// // if (first_curve_end >= second_curve_start) { -// // return isPathInBounds(path); -// // } - -// // if (!isLineInBounds(path[first_curve_end], path[second_curve_start])) { -// // return false; -// // } - -// // // checks the points manually in the curve -// // for (int i = 0; i <= first_curve_end; i++) { -// // if (!isPointInBounds(path[i])) { -// // return false; -// // } -// // } - -// // for (int i = second_curve_start; i < path.size(); i++) { -// // if (!isPointInBounds(path[i])) { -// // return false; -// // } -// // } - -// // return true; -// // } - -// const XYZCoord& Environment::getGoal() const { return goals[goals_found]; } - -// const XYZCoord& Environment::getGoal(int index) const { return goals[index]; } - -// XYZCoord Environment::getRandomPoint(bool use_mapping_region = false) const { -// // TODO - use some heuristic to more efficiently generate direction -// // vector (and make it toggleable) -// std::pair, std::pair> polygon_bounds = bounds; -// if (use_mapping_region) { -// polygon_bounds = findBounds(mapping_region); -// } - -// for (int i = 0; i < TRIES_FOR_RANDOM_POINT; i++) { -// // generates a random point in the rectangle contianing the valid region -// XYZCoord generated_point = { -// random(polygon_bounds.first.first, polygon_bounds.first.second), -// random(polygon_bounds.second.first, polygon_bounds.second.second), 0}; - -// if (use_mapping_region) { -// if (isPointInPolygon(mapping_region, generated_point)) { -// return generated_point; -// } -// } else { -// if (isPointInBounds(generated_point)) { -// return generated_point; -// } -// } -// } - -// return goals[goals_found]; -// } - -// int Environment::getNumGoals() const { return goals.size(); } - -// bool Environment::isPointInPolygon(const Polygon& polygon, const XYZCoord& point) { -// bool is_inside = false; - -// // point in polygon -// for (int i = 0, j = polygon.size() - 1; i < polygon.size(); j = i++) { -// if (((polygon[i].y > point.y) != (polygon[j].y > point.y)) && -// (point.x < (polygon[j].x - polygon[i].x) * (point.y - polygon[i].y) / -// (polygon[j].y - polygon[i].y) + -// polygon[i].x)) { -// is_inside = !is_inside; -// } -// } - -// return is_inside; -// } - -// bool Environment::isLineInBounds(const XYZCoord& start_point, const XYZCoord& end_point) const { -// if (doesLineIntersectPolygon(start_point, end_point, valid_region)) { -// return false; -// } - -// for (const Polygon& obstacle : obstacles) { -// if (doesLineIntersectPolygon(start_point, end_point, obstacle)) { -// return false; -// } -// } - -// return true; -// } - -// bool Environment::doesLineIntersectPolygon(const XYZCoord& start_point, const XYZCoord& end_point, -// const Polygon& polygon) const { -// for (int i = 0, j = polygon.size() - 1; i < polygon.size(); j = i++) { -// if (intersect(start_point, end_point, polygon[i], polygon[j])) { -// return true; -// } -// } - -// return false; -// } - -// // Given three colinear points p, q, r, the function checks if -// // point q lies on line segment 'pr' -// bool Environment::onSegment(XYZCoord p, XYZCoord q, XYZCoord r) const { -// if (q.x <= std::max(p.x, r.x) && q.x >= std::min(p.x, r.x) && q.y <= std::max(p.y, r.y) && -// q.y >= std::min(p.y, r.y)) -// return true; -// return false; -// } - -// // To find orientation of ordered triplet (p, q, r). -// // The function returns following values: -// // 0 : Colinear points -// // 1 : Clockwise points -// // 2 : Counterclockwise points -// int Environment::orientation(XYZCoord p, XYZCoord q, XYZCoord r) const { -// int val = (q.y - p.y) * (r.x - q.x) - (q.x - p.x) * (r.y - q.y); -// if (val == 0) return 0; // colinear -// return (val > 0) ? 1 : 2; // clock or counterclock wise -// } - -// // Function to check if segments intersect -// bool Environment::intersect(XYZCoord p1, XYZCoord q1, XYZCoord p2, XYZCoord q2) const { -// // Find the four orientations needed for general and -// // special cases -// int o1 = orientation(p1, q1, p2); -// int o2 = orientation(p1, q1, q2); -// int o3 = orientation(p2, q2, p1); -// int o4 = orientation(p2, q2, q1); - -// // General case -// if (o1 != o2 && o3 != o4) return true; - -// // Special Cases -// // p1, q1 and p2 are colinear and p2 lies on segment p1q1 -// if (o1 == 0 && onSegment(p1, p2, q1)) return true; - -// // p1, q1 and q2 are colinear and q2 lies on segment p1q1 -// if (o2 == 0 && onSegment(p1, q2, q1)) return true; - -// // p2, q2 and p1 are colinear and p1 lies on segment p2q2 -// if (o3 == 0 && onSegment(p2, p1, q2)) return true; - -// // p2, q2 and q1 are colinear and q1 lies on segment p2q2 -// if (o4 == 0 && onSegment(p2, q1, q2)) return true; - -// return false; // Doesn't fall in any of the above cases -// } - -// std::vector Environment::getAirdropEndpoints(int scan_radius, bool vertical) const { -// auto bounds = findBounds(airdrop_zone); -// auto [x_min, x_max] = bounds.first; -// auto [y_min, y_max] = bounds.second; - -// std::vector endpoints; -// bool fly_down = true; -// double start = vertical ? x_min + scan_radius : -1 * (y_max - scan_radius); -// double end = vertical ? x_max - scan_radius : -1 * (y_min + scan_radius); -// double iteration = scan_radius * 2; - -// for (double coordinate = start; coordinate <= end; coordinate += iteration) { -// // finds where this x-coordinate intersects with the airdrop zone (always convex) -// XYZCoord top(0, 0, 0); -// XYZCoord bottom(0, 0, 0); -// if (vertical) { -// top = XYZCoord(coordinate, y_max, 0); -// bottom = XYZCoord(coordinate, y_min, 0); -// } else { -// top = XYZCoord(x_min, -coordinate, 0); -// bottom = XYZCoord(x_max, -coordinate, 0); -// } - -// std::vector intersections = -// findIntersections(airdrop_zone, top, bottom, vertical); - -// if (vertical) { -// if (intersections[0].y < intersections[1].y) { -// endpoints.push_back(intersections[1]); -// endpoints.push_back(intersections[0]); - -// } else { -// endpoints.push_back(intersections[0]); -// endpoints.push_back(intersections[1]); -// } -// } else { -// if (intersections[0].x < intersections[1].x) { -// endpoints.push_back(intersections[0]); -// endpoints.push_back(intersections[1]); - -// } else { -// endpoints.push_back(intersections[1]); -// endpoints.push_back(intersections[0]); -// } -// } -// } - -// return endpoints; -// } - -// std::vector Environment::getAirdropWaypoints(int scan_radius, bool one_way, -// bool vertical) const { -// std::vector waypoints; -// std::vector endpoints = getAirdropEndpoints(scan_radius, vertical); -// double angle = vertical ? 3.0 / 2.0 * M_PI : 0; - -// if (one_way) { -// for (const XYZCoord& endpoint : endpoints) { -// waypoints.push_back(RRTPoint(endpoint, angle)); -// } -// } else { -// bool fly_down = true; -// for (int i = 0; i < endpoints.size(); i += 2) { -// if (fly_down) { -// waypoints.push_back(RRTPoint(endpoints[i], angle)); -// waypoints.push_back(RRTPoint(endpoints[i + 1], angle)); -// } else { -// waypoints.push_back(RRTPoint(endpoints[i + 1], angle)); -// waypoints.push_back(RRTPoint(endpoints[i], angle)); -// } - -// angle = mod(angle + M_PI, TWO_PI); -// fly_down = !fly_down; -// } -// } - -// return waypoints; -// } - -// bool Environment::verticalRayIntersectsEdge(const XYZCoord& p1, const XYZCoord& p2, -// const XYZCoord& rayStart, const XYZCoord& rayEnd, -// XYZCoord& intersection) const { -// // if the x coordinate lines between the edge -// if ((p2.x <= rayStart.x && p1.x >= rayStart.x) || (p1.x <= rayStart.x && p2.x >= rayStart.x)) { -// double slope = (p2.y - p1.y) / (p2.x - p1.x); -// // finds where they intersect using a line y - y' = m(x - x') -// intersection = XYZCoord(rayStart.x, slope * (rayStart.x - p1.x) + p1.y, 0); -// return true; -// } -// return false; -// } - -// bool Environment::horizontalRayIntersectsEdge(const XYZCoord& p1, const XYZCoord& p2, -// const XYZCoord& rayStart, const XYZCoord& rayEnd, -// XYZCoord& intersection) const { -// // if the x coordinate lines between the edge -// if ((p2.y <= rayStart.y && p1.y >= rayStart.y) || (p1.y <= rayStart.y && p2.y >= rayStart.y)) { -// double inverse_slope = (p2.x - p1.x) / (p2.y - p1.y); -// // finds where they intersect using a line y - y' = m(x - x') -// intersection = XYZCoord(inverse_slope * (rayStart.y - p1.y) + p1.x, rayStart.y, 0); -// return true; -// } -// return false; -// } - -// std::vector Environment::findIntersections(const Polygon& polygon, -// const XYZCoord& rayStart, -// const XYZCoord& rayEnd, bool vertical) const { -// // array to be filled -// std::vector intersections; -// int n = polygon.size(); - -// for (int i = 0; i < n; ++i) { -// const XYZCoord& p1 = polygon[i]; -// const XYZCoord& p2 = polygon[(i + 1) % n]; - -// // temporary variable to be changed if an intersection is found -// XYZCoord intersection(0, 0, 0); -// bool found_intersection = -// vertical ? verticalRayIntersectsEdge(p1, p2, rayStart, rayEnd, intersection) -// : horizontalRayIntersectsEdge(p1, p2, rayStart, rayEnd, intersection); -// if (found_intersection) { -// intersections.push_back(intersection); -// } -// } - -// return intersections; -// } - -// std::vector Environment::findIntersectionsWithPolygon(const Polygon& polygon, -// const XYZCoord& start_point, -// const XYZCoord& end_point) const { -// // for loop through each edge of the polygon -// // for each edge, find the intersection point with the line segment -// // if an intersection point is found, add it to the list of intersection -// // points -// std::vector intersections; -// for (int i = 0; i < polygon.size(); ++i) { -// XYZCoord p1 = polygon[i]; -// XYZCoord p2 = polygon[(i + 1) % polygon.size()]; -// if (intersect(start_point, end_point, p1, p2)) { -// // finds the intersection point -// double x1 = start_point.x, y1 = start_point.y; -// double x2 = end_point.x, y2 = end_point.y; -// double x3 = p1.x, y3 = p1.y; -// double x4 = p2.x, y4 = p2.y; - -// double a1 = y2 - y1; -// double b1 = x1 - x2; -// double c1 = a1 * x1 + b1 * y1; - -// double a2 = y4 - y3; -// double b2 = x3 - x4; -// double c2 = a2 * x3 + b2 * y3; - -// double det = a1 * b2 - a2 * b1; - -// double x = (b2 * c1 - b1 * c2) / det; -// double y = (a1 * c2 - a2 * c1) / det; -// if (std::find(intersections.begin(), intersections.end(), XYZCoord(x, y, 0)) == -// intersections.end()) { -// intersections.push_back(XYZCoord(x, y, 0)); -// } -// } -// } - -// return intersections; -// } - -// std::pair Environment::estimateAreaCoveredAndPathLength( -// const std::vector& new_goals) const { -// double area_covered = 0.0; -// double path_length = 0.0; - -// for (int i = 0; i < new_goals.size(); ++i) { -// XYZCoord start_point = new_goals[i]; -// XYZCoord end_point = new_goals[(i + 1) % new_goals.size()]; -// path_length += start_point.distanceTo(end_point); -// // Calculates area covered by adding the part of the line segment that is in bounds -// // Caulcate the intersections and whether the start and end points are in bounds in order to -// // find the sections of the line that is in bounds -// if (!doesLineIntersectPolygon(start_point, end_point, mapping_region)) { -// area_covered += start_point.distanceTo(end_point) * SEARCH_RADIUS * 2; -// } else { -// std::vector intersections = -// findIntersectionsWithPolygon(mapping_region, start_point, end_point); -// bool start_in_bounds = isPointInPolygon(mapping_region, start_point); -// bool end_in_bounds = isPointInPolygon(mapping_region, end_point); - -// if (start_in_bounds) { -// area_covered += start_point.distanceTo(intersections[0]) * SEARCH_RADIUS; -// intersections.erase(intersections.begin()); -// } - -// if (end_in_bounds) { -// area_covered += end_point.distanceTo(intersections.back()) * SEARCH_RADIUS; -// intersections.pop_back(); -// } - -// for (int i = 0; i < intersections.size(); i += 2) { -// area_covered += intersections[i].distanceTo(intersections[i + 1]) * SEARCH_RADIUS; -// } -// } -// } - -// return {area_covered, path_length}; -// } - -// Polygon Environment::scale(double scale, const Polygon& source_polygon) const { -// Polygon scaled_polygon; - -// // square bounds of the polygon -// auto bounds = findBounds(source_polygon); -// auto [x_min, x_max] = bounds.first; -// auto [y_min, y_max] = bounds.second; - -// // finds the center of the polygon -// double x_center = (x_max + x_min) / 2; -// double y_center = (y_max + y_min) / 2; - -// for (const XYZCoord& point : source_polygon) { -// // shifts the polygon to the center so scaling doesn't shift the centter (readability) -// // scales the polygon -// // retranslates the center to the original center -// double scaled_x = (point.x - x_center) * scale + x_center; -// double scaled_y = (point.y - y_center) * scale + y_center; - -// scaled_polygon.push_back(XYZCoord(scaled_x, scaled_y, 0)); -// } - -// return scaled_polygon; -// } - -// std::pair, std::pair> Environment::findBounds( -// const Polygon& region) const { -// if (region.empty()) { -// return std::make_pair(std::make_pair(0, 0), std::make_pair(0, 0)); -// } - -// // initial values -// double min_x = region[0].x; -// double max_x = region[0].x; -// double min_y = region[0].y; -// double max_y = region[0].y; - -// // finds the min and max x and y values -// for (const XYZCoord& point : region) { -// min_x = std::min(min_x, point.x); -// max_x = std::max(max_x, point.x); -// min_y = std::min(min_y, point.y); -// max_y = std::max(max_y, point.y); -// } - -// return {{min_x, max_x}, {min_y, max_y}}; -// } +Environment::Environment(const Polygon& valid_region, const Polygon& airdrop_zone, + const Polygon& mapping_region, const std::vector& goals) + : valid_region(valid_region), + airdrop_zone(airdrop_zone), + mapping_region(mapping_region), + goals(goals), + goals_found(0), + bounds(findBounds(valid_region)) {} + +bool Environment::isPathInBounds(const std::vector& path) const { + /* + * starts from the center, and walks in botht directions in intervals, this + * is motivated by the idea that if an endpoint is in bounds, then the path + * is most likely out of bounds in the center of the path. Furthermore, + * moving by larger step would be more efficient than checking every point. + * + * ^only for small step length does this make much sense + */ + const int center = path.size() / 2; + const int interval = ENV_PATH_VALIDATION_STEP_SIZE; + + // special case not checked in the loop + if (!isPointInPolygon(valid_region, [center])) { + return false; + } + + for (int i = 1; i < interval; i++) { + int count = center + i; + + // to the right + while (count < path.size()) { + if (!isPointInPolygon(valid_region, path[count])) { + return false; + } + count += interval; + } + + // to the left + count = center - i; + while (count >= 0) { + if (!isPointInPolygon(valid_region, path[count])) { + return false; + } + count -= interval; + } + } + + return true; +} + +const XYZCoord& Environment::getGoal() const { return goals[goals_found]; } + +const XYZCoord& Environment::getGoal(int index) const { return goals[index]; } + +XYZCoord Environment::getRandomPoint(bool use_mapping_region = false) const { + // TODO - use some heuristic to more efficiently generate direction + // vector (and make it toggleable) + // potentially utilize ellipse sampling or goal biasing + std::pair, std::pair> polygon_bounds = bounds; + if (use_mapping_region) { + polygon_bounds = findBounds(mapping_region); + } + + for (int i = 0; i < TRIES_FOR_RANDOM_POINT; i++) { + // generates a random point in the rectangle containing the valid region + XYZCoord generated_point = { + random(polygon_bounds.first.first, polygon_bounds.first.second), + random(polygon_bounds.second.first, polygon_bounds.second.second), 0}; + + if (use_mapping_region) { + if (isPointInPolygon(mapping_region, generated_point)) { + return generated_point; + } + } else { + if (isPointInPolygon(valid_region, generated_point)) { + return generated_point; + } + } + } + + return goals[goals_found]; +} + +int Environment::getNumGoals() const { return goals.size(); } + +bool Environment::isPointInPolygon(const Polygon& polygon, const XYZCoord& point) { + bool is_inside = false; + + // point in polygon + for (int i = 0, j = polygon.size() - 1; i < polygon.size(); j = i++) { + if (((polygon[i].y > point.y) != (polygon[j].y > point.y)) && + (point.x < (polygon[j].x - polygon[i].x) * (point.y - polygon[i].y) / + (polygon[j].y - polygon[i].y) + + polygon[i].x)) { + is_inside = !is_inside; + } + } + + return is_inside; +} + +bool Environment::doesLineIntersectPolygon(const XYZCoord& start_point, const XYZCoord& end_point, + const Polygon& polygon) const { + for (int i = 0, j = polygon.size() - 1; i < polygon.size(); j = i++) { + if (intersect(start_point, end_point, polygon[i], polygon[j])) { + return true; + } + } + + return false; +} + +// Given three colinear points p, q, r, the function checks if +// point q lies on line segment 'pr' +bool Environment::onSegment(XYZCoord p, XYZCoord q, XYZCoord r) const { + if (q.x <= std::max(p.x, r.x) && q.x >= std::min(p.x, r.x) && q.y <= std::max(p.y, r.y) && + q.y >= std::min(p.y, r.y)) + return true; + return false; +} + +// To find orientation of ordered triplet (p, q, r). +// The function returns following values: +// 0 : Colinear points +// 1 : Clockwise points +// 2 : Counterclockwise points +int Environment::orientation(XYZCoord p, XYZCoord q, XYZCoord r) const { + int val = (q.y - p.y) * (r.x - q.x) - (q.x - p.x) * (r.y - q.y); + if (val == 0) return 0; // colinear + return (val > 0) ? 1 : 2; // clock or counterclock wise +} + +// Function to check if segments intersect +bool Environment::intersect(XYZCoord p1, XYZCoord q1, XYZCoord p2, XYZCoord q2) const { + // Find the four orientations needed for general and + // special cases + int o1 = orientation(p1, q1, p2); + int o2 = orientation(p1, q1, q2); + int o3 = orientation(p2, q2, p1); + int o4 = orientation(p2, q2, q1); + + // General case + if (o1 != o2 && o3 != o4) return true; + + // Special Cases + // p1, q1 and p2 are colinear and p2 lies on segment p1q1 + if (o1 == 0 && onSegment(p1, p2, q1)) return true; + + // p1, q1 and q2 are colinear and q2 lies on segment p1q1 + if (o2 == 0 && onSegment(p1, q2, q1)) return true; + + // p2, q2 and p1 are colinear and p1 lies on segment p2q2 + if (o3 == 0 && onSegment(p2, p1, q2)) return true; + + // p2, q2 and q1 are colinear and q1 lies on segment p2q2 + if (o4 == 0 && onSegment(p2, q1, q2)) return true; + + return false; // Doesn't fall in any of the above cases +} + +std::vector Environment::getAirdropEndpoints(int scan_radius, bool vertical) const { + auto bounds = findBounds(airdrop_zone); + auto [x_min, x_max] = bounds.first; + auto [y_min, y_max] = bounds.second; + + std::vector endpoints; + bool fly_down = true; + double start = vertical ? x_min + scan_radius : -1 * (y_max - scan_radius); + double end = vertical ? x_max - scan_radius : -1 * (y_min + scan_radius); + double iteration = scan_radius * 2; + + for (double coordinate = start; coordinate <= end; coordinate += iteration) { + // finds where this x-coordinate intersects with the airdrop zone (always convex) + XYZCoord top(0, 0, 0); + XYZCoord bottom(0, 0, 0); + if (vertical) { + top = XYZCoord(coordinate, y_max, 0); + bottom = XYZCoord(coordinate, y_min, 0); + } else { + top = XYZCoord(x_min, -coordinate, 0); + bottom = XYZCoord(x_max, -coordinate, 0); + } + + std::vector intersections = + findIntersections(airdrop_zone, top, bottom, vertical); + + if (vertical) { + if (intersections[0].y < intersections[1].y) { + endpoints.push_back(intersections[1]); + endpoints.push_back(intersections[0]); + + } else { + endpoints.push_back(intersections[0]); + endpoints.push_back(intersections[1]); + } + } else { + if (intersections[0].x < intersections[1].x) { + endpoints.push_back(intersections[0]); + endpoints.push_back(intersections[1]); + + } else { + endpoints.push_back(intersections[1]); + endpoints.push_back(intersections[0]); + } + } + } + + return endpoints; +} + +std::vector Environment::getAirdropWaypoints(int scan_radius, bool one_way, + bool vertical) const { + std::vector waypoints; + std::vector endpoints = getAirdropEndpoints(scan_radius, vertical); + double angle = vertical ? 3.0 / 2.0 * M_PI : 0; + + if (one_way) { + for (const XYZCoord& endpoint : endpoints) { + waypoints.push_back(RRTPoint(endpoint, angle)); + } + } else { + bool fly_down = true; + for (int i = 0; i < endpoints.size(); i += 2) { + if (fly_down) { + waypoints.push_back(RRTPoint(endpoints[i], angle)); + waypoints.push_back(RRTPoint(endpoints[i + 1], angle)); + } else { + waypoints.push_back(RRTPoint(endpoints[i + 1], angle)); + waypoints.push_back(RRTPoint(endpoints[i], angle)); + } + + angle = mod(angle + M_PI, TWO_PI); + fly_down = !fly_down; + } + } + + return waypoints; +} + +bool Environment::verticalRayIntersectsEdge(const XYZCoord& p1, const XYZCoord& p2, + const XYZCoord& rayStart, const XYZCoord& rayEnd, + XYZCoord& intersection) const { + // if the x coordinate lines between the edge + if ((p2.x <= rayStart.x && p1.x >= rayStart.x) || (p1.x <= rayStart.x && p2.x >= rayStart.x)) { + double slope = (p2.y - p1.y) / (p2.x - p1.x); + // finds where they intersect using a line y - y' = m(x - x') + intersection = XYZCoord(rayStart.x, slope * (rayStart.x - p1.x) + p1.y, 0); + return true; + } + return false; +} + +bool Environment::horizontalRayIntersectsEdge(const XYZCoord& p1, const XYZCoord& p2, + const XYZCoord& rayStart, const XYZCoord& rayEnd, + XYZCoord& intersection) const { + // if the x coordinate lines between the edge + if ((p2.y <= rayStart.y && p1.y >= rayStart.y) || (p1.y <= rayStart.y && p2.y >= rayStart.y)) { + double inverse_slope = (p2.x - p1.x) / (p2.y - p1.y); + // finds where they intersect using a line y - y' = m(x - x') + intersection = XYZCoord(inverse_slope * (rayStart.y - p1.y) + p1.x, rayStart.y, 0); + return true; + } + return false; +} + +std::vector Environment::findIntersections(const Polygon& polygon, + const XYZCoord& rayStart, + const XYZCoord& rayEnd, bool vertical) const { + // array to be filled + std::vector intersections; + int n = polygon.size(); + + for (int i = 0; i < n; ++i) { + const XYZCoord& p1 = polygon[i]; + const XYZCoord& p2 = polygon[(i + 1) % n]; + + // temporary variable to be changed if an intersection is found + XYZCoord intersection(0, 0, 0); + bool found_intersection = + vertical ? verticalRayIntersectsEdge(p1, p2, rayStart, rayEnd, intersection) + : horizontalRayIntersectsEdge(p1, p2, rayStart, rayEnd, intersection); + if (found_intersection) { + intersections.push_back(intersection); + } + } + + return intersections; +} + +std::vector Environment::findIntersectionsWithPolygon(const Polygon& polygon, + const XYZCoord& start_point, + const XYZCoord& end_point) const { + // for loop through each edge of the polygon + // for each edge, find the intersection point with the line segment + // if an intersection point is found, add it to the list of intersection + // points + std::vector intersections; + for (int i = 0; i < polygon.size(); ++i) { + XYZCoord p1 = polygon[i]; + XYZCoord p2 = polygon[(i + 1) % polygon.size()]; + if (intersect(start_point, end_point, p1, p2)) { + // finds the intersection point + double x1 = start_point.x, y1 = start_point.y; + double x2 = end_point.x, y2 = end_point.y; + double x3 = p1.x, y3 = p1.y; + double x4 = p2.x, y4 = p2.y; + + double a1 = y2 - y1; + double b1 = x1 - x2; + double c1 = a1 * x1 + b1 * y1; + + double a2 = y4 - y3; + double b2 = x3 - x4; + double c2 = a2 * x3 + b2 * y3; + + double det = a1 * b2 - a2 * b1; + + double x = (b2 * c1 - b1 * c2) / det; + double y = (a1 * c2 - a2 * c1) / det; + if (std::find(intersections.begin(), intersections.end(), XYZCoord(x, y, 0)) == + intersections.end()) { + intersections.push_back(XYZCoord(x, y, 0)); + } + } + } + + return intersections; +} + +std::pair Environment::estimateAreaCoveredAndPathLength( + const std::vector& new_goals) const { + double area_covered = 0.0; + double path_length = 0.0; + + for (int i = 0; i < new_goals.size(); ++i) { + XYZCoord start_point = new_goals[i]; + XYZCoord end_point = new_goals[(i + 1) % new_goals.size()]; + path_length += start_point.distanceTo(end_point); + // Calculates area covered by adding the part of the line segment that is in bounds + // Caulcate the intersections and whether the start and end points are in bounds in order to + // find the sections of the line that is in bounds + if (!doesLineIntersectPolygon(start_point, end_point, mapping_region)) { + area_covered += start_point.distanceTo(end_point) * SEARCH_RADIUS * 2; + } else { + std::vector intersections = + findIntersectionsWithPolygon(mapping_region, start_point, end_point); + bool start_in_bounds = isPointInPolygon(mapping_region, start_point); + bool end_in_bounds = isPointInPolygon(mapping_region, end_point); + + if (start_in_bounds) { + area_covered += start_point.distanceTo(intersections[0]) * SEARCH_RADIUS; + intersections.erase(intersections.begin()); + } + + if (end_in_bounds) { + area_covered += end_point.distanceTo(intersections.back()) * SEARCH_RADIUS; + intersections.pop_back(); + } + + for (int i = 0; i < intersections.size(); i += 2) { + area_covered += intersections[i].distanceTo(intersections[i + 1]) * SEARCH_RADIUS; + } + } + } + + return {area_covered, path_length}; +} + +Polygon Environment::scale(double scale, const Polygon& source_polygon) const { + Polygon scaled_polygon; + + // square bounds of the polygon + auto bounds = findBounds(source_polygon); + auto [x_min, x_max] = bounds.first; + auto [y_min, y_max] = bounds.second; + + // finds the center of the polygon + double x_center = (x_max + x_min) / 2; + double y_center = (y_max + y_min) / 2; + + for (const XYZCoord& point : source_polygon) { + // shifts the polygon to the center so scaling doesn't shift the centter (readability) + // scales the polygon + // retranslates the center to the original center + double scaled_x = (point.x - x_center) * scale + x_center; + double scaled_y = (point.y - y_center) * scale + y_center; + + scaled_polygon.push_back(XYZCoord(scaled_x, scaled_y, 0)); + } + + return scaled_polygon; +} + +std::pair, std::pair> Environment::findBounds( + const Polygon& region) const { + if (region.empty()) { + return std::make_pair(std::make_pair(0, 0), std::make_pair(0, 0)); + } + + // initial values + double min_x = region[0].x; + double max_x = region[0].x; + double min_y = region[0].y; + double max_y = region[0].y; + + // finds the min and max x and y values + for (const XYZCoord& point : region) { + min_x = std::min(min_x, point.x); + max_x = std::max(max_x, point.x); + min_y = std::min(min_y, point.y); + max_y = std::max(max_y, point.y); + } + + return {{min_x, max_x}, {min_y, max_y}}; +} diff --git a/src/pathing/static.cpp b/src/pathing/static.cpp index f241f430..8e77377e 100644 --- a/src/pathing/static.cpp +++ b/src/pathing/static.cpp @@ -132,6 +132,8 @@ I have wrestled with C++. It is the most unexciting contest you can imagine. // return true; // } +// This code will disappear, but not immediately. Deo volente, it will provide a useful framework. + // bool RRT::epochEvaluation(RRTNode *goal_node, int current_goal_index) { // // If a single epoch has not been passed, mark this goal as the first // // benchmark. diff --git a/src/pathing/tree.cpp b/src/pathing/tree.cpp index 12916f4a..bbe1a31a 100644 --- a/src/pathing/tree.cpp +++ b/src/pathing/tree.cpp @@ -13,12 +13,6 @@ // When we are born, we cry that we are come to this great stage of fools - -/* -TODO: rewrite the RRT Node implementation slightly to utilize smart pointers for the reachable -remove the parents since that introduces insecurity -*/ - RRTNode::RRTNode(const RRTPoint& point, double cost, double path_length, const std::vector path) : point{point}, cost{cost}, path_length(path_length), path(path) {} @@ -31,9 +25,9 @@ RRTNode::~RRTNode() { reachable.clear(); } -// // bool RRTNode::operator==(const RRTNode& other_node) const { -// // return this->point == other_node.point && this->cost == other_node.cost; -// // } +// bool RRTNode::operator==(const RRTNode& other_node) const { +// return this->point == other_node.point && this->cost == other_node.cost; +// } RRTPoint& RRTNode::getPoint() { return this->point; } @@ -80,329 +74,230 @@ RRTTree::RRTTree(RRTPoint root_point, Environment airspace, Dubins dubins) RRTTree::~RRTTree() { root.reset(); } -// bool RRTTree::validatePath(const std::vector& path, const RRTOption& option) const { -// return airspace.isPathInBounds(path); -// // return airspace.isPathInBoundsAdv(path, option); -// } - -// RRTNode* RRTTree::generateNode(RRTNode* anchor_node, const RRTPoint& new_point, -// const RRTOption& option) const { -// const std::vector& path = dubins.generatePoints( -// anchor_node->getPoint(), new_point, option.dubins_path, option.has_straight); - -// if (!validatePath(path, option)) { -// return nullptr; -// } - -// // needs to add the node to the tree -// RRTNode* new_node = -// new RRTNode(new_point, anchor_node->getCost() + option.length, option.length, path); -// new_node->setParent(anchor_node); - -// return new_node; -// } - -// bool RRTTree::addNode(RRTNode* anchor_node, RRTNode* new_node) { -// if (new_node == nullptr || anchor_node == nullptr) { -// return false; -// } - -// anchor_node->addReachable(new_node); -// tree_size++; -// return true; -// } - -// // TODO - convert from old to new -// RRTNode* RRTTree::addSample(RRTNode* anchor_node, const RRTPoint& new_point, -// const RRTOption& option) { -// RRTNode* new_node = generateNode(anchor_node, new_point, option); - -// if (addNode(anchor_node, new_node)) { -// return new_node; -// } - -// return nullptr; -// } - -// void RRTTree::rewireEdge(RRTNode* current_node, RRTNode* previous_parent, RRTNode* new_parent, -// const std::vector& path, double path_cost) { -// // ORDER MATTERS, REMOVE THEN ADD TO PRESERVE THE CURR_NODE HAS A PARENT -// previous_parent->removeReachable(current_node); -// new_parent->addReachable(current_node); - -// // bubbles down the tree to reassign the costs -// current_node->setPath(path); -// current_node->setCost(new_parent->getCost() + path_cost); -// current_node->setPathLength(path_cost); -// reassignCosts(current_node); -// } - -// std::vector RRTTree::getKRandomNodes(int k) const { -// std::vector nodes; -// // proabability that any given node should be added -// double chance = 1.0 * k / tree_size; -// getKRandomNodesRecursive(nodes, current_head, chance); - -// return nodes; -// } - -// void RRTTree::getKRandomNodesRecursive(std::vector& nodes, RRTNode* current_node, -// double chance) const { -// if (current_node == nullptr) { -// return; -// } - -// // if the chance is less than the random number, then add the node to the list -// // TODO maybe make some check that prevents the random calls if the tree is small enough -// if (random(0, 1) < chance) { -// nodes.emplace_back(current_node); -// } - -// for (RRTNode* node : current_node->getReachable()) { -// getKRandomNodesRecursive(nodes, node, chance); -// } -// } - -// std::vector RRTTree::getKClosestNodes(const RRTPoint& sample, int k) const { -// std::vector closest_nodes; - -// // helper vector that associates nodes with distances -// // TODO - do some benchmarks with max-heaps to see which one is more efficient -// std::vector> nodes_by_distance; -// getKClosestNodesRecursive(nodes_by_distance, sample, current_head); - -// // sorts the nodes by distance -// std::sort(nodes_by_distance.begin(), nodes_by_distance.end(), -// [](auto& left, auto& right) { return left.first < right.first; }); - -// // gets either the k closest nodes, or the entire list -// int size = nodes_by_distance.size(); -// int stop_condition = std::min(k, size); -// for (int i = 0; i < stop_condition; i++) { -// closest_nodes.emplace_back(nodes_by_distance[i].second); -// } +// MAYBE: glorified function wrapper that uses one of its params, need to wipe. +bool RRTTree::validatePath(const std::vector& path, const RRTOption& option) const { + return airspace.isPathInBounds(path); +} -// return closest_nodes; -// } +RRTNodePtr RRTTree::generateNode(RRTNodePtr anchor_node, const RRTPoint& new_point, + const RRTOption& option) const { + const std::vector& path = dubins.generatePoints( + anchor_node->getPoint(), new_point, option.dubins_path, option.has_straight); -// void RRTTree::getKClosestNodesRecursive(std::vector>& nodes_by_distance, -// const RRTPoint& sample, RRTNode* current_node) const { -// if (current_node == nullptr) { -// return; -// } + if (!validatePath(path, option)) { + return nullptr; + } -// // ONLY considers the distance, and not the path length required to get to the node -// double distance = sample.coord.distanceToSquared(current_node->getPoint().coord); -// nodes_by_distance.push_back({distance, current_node}); + // needs to add the node to the tree + RRTNodePtr new_node = + std::make_shared(new_point, anchor_node->getCost() + option.length, option.length, path); -// for (RRTNode* node : current_node->getReachable()) { -// getKClosestNodesRecursive(nodes_by_distance, sample, node); -// } -// } + return new_node; +} -// void RRTTree::fillOptionsNodes(std::vector>& options, -// const std::vector& nodes, const RRTPoint& sample) const { -// for (RRTNode* node : nodes) { -// const std::vector& local_options = dubins.allOptions(node->getPoint(), sample); +bool RRTTree::addNode(RRTNodePtr anchor_node, RRTNodePtr new_node) { + if (new_node == nullptr || anchor_node == nullptr) { + return false; + } -// for (const RRTOption& option : local_options) { -// if (std::isnan(option.length) || -// option.length == std::numeric_limits::infinity()) { -// continue; -// } + anchor_node->addReachable(new_node); + tree_size++; + return true; +} -// options.push_back({node, option}); -// } -// } -// } +// TODO - convert from old to new +RRTNodePtr RRTTree::addSample(RRTNodePtr anchor_node, const RRTPoint& new_point, + const RRTOption& option) { + RRTNodePtr new_node = generateNode(anchor_node, new_point, option); -// RRTNode* RRTTree::getRoot() const { return this->root; } + if (addNode(anchor_node, new_node)) { + return new_node; + } -// const XYZCoord& RRTTree::getGoal() const { return airspace.getGoal(); } + return nullptr; +} -// const XYZCoord& RRTTree::getGoal(int index) const { return airspace.getGoal(index); } +void RRTTree::fillOptionsNodes(std::vector>& options, + const std::vector& nodes, const RRTPoint& sample) const { + for (RRTNodePtr node : nodes) { + const std::vector& local_options = dubins.allOptions(node->getPoint(), sample); -// const Environment& RRTTree::getAirspace() const { return this->airspace; } + for (const RRTOption& option : local_options) { + if (std::isnan(option.length) || + option.length == std::numeric_limits::infinity()) { + continue; + } -// RRTPoint RRTTree::getRandomPoint(double search_radius) const { -// // gets random point if the goal is not being used -// const XYZCoord& sample = airspace.getRandomPoint(); + options.push_back({node, option}); + } + } +} -// // // picks the nearest node to the sample, and then returns a point `search_radius` distance -// // away -// // // from tat point in the direction of the sample -// // std::pair nearest_node = getNearestNode(sample); +RRTNodePtr RRTTree::getRoot() const { return this->root; } -// // int count = 0; +const XYZCoord& RRTTree::getGoal() const { return airspace.getGoal(); } -// // while (nearest_node.second < search_radius && count < TRIES_FOR_RANDOM_POINT) { -// // sample = airspace.getRandomPoint(); -// // nearest_node = getNearestNode(sample); -// // count++; -// // } -// // // shouldn't happen, it is here for memory safety -// // if (nearest_node == nullptr) { -// // return sample; -// // } +const XYZCoord& RRTTree::getGoal(int index) const { return airspace.getGoal(index); } -// // const RRTPoint& nearest_point = nearest_node->getPoint(); +const Environment& RRTTree::getAirspace() const { return this->airspace; } -// // const XYZCoord displacement_vector = sample.coord - nearest_point.coord; +RRTPoint RRTTree::getRandomPoint(double search_radius) const { + // gets random point if the goal is not being used + const XYZCoord& sample = airspace.getRandomPoint(); -// // // TODO - use some heuristic to make this angle as optimal as possible -// // const double angle = -// // std::atan2(displacement_vector.y, displacement_vector.x) + random(-M_PI / 2, M_PI / 2); + // MAYBE: go through this code, if anything of value, take it, otherwise throw away -// // // distance between the vectors, if it is less than the search radius, then it will return -// // the -// // // sample point, otherwise, it will return a point `search_radius` away from the nearest -// // point const double distance = displacement_vector.norm(); + // // picks the nearest node to the sample, and then returns a point `search_radius` distance + // away + // // from tat point in the direction of the sample + // std::pair nearest_node = getNearestNode(sample); -// // if (distance < search_radius) { -// // return RRTPoint(sample.coord, angle); -// // } + // int count = 0; -// // RRTPoint new_point(nearest_point.coord + (search_radius / distance) * displacement_vector, -// // angle); + // while (nearest_node.second < search_radius && count < TRIES_FOR_RANDOM_POINT) { + // sample = airspace.getRandomPoint(); + // nearest_node = getNearestNode(sample); + // count++; + // } + // // shouldn't happen, it is here for memory safety + // if (nearest_node == nullptr) { + // return sample; + // } -// // return new_point; -// return RRTPoint(sample, random(0, TWO_PI)); -// } + // const RRTPoint& nearest_point = nearest_node->getPoint(); -// /* -// TODO - investigate whether a max heap is better or worse -// */ -// std::vector> RRTTree::pathingOptions( -// const RRTPoint& end, PointFetchMethod::Enum point_fetch_method, int quantity_options) const { -// // fills the options list with valid values -// std::vector> options; - -// switch (point_fetch_method) { -// case PointFetchMethod::Enum::RANDOM: { -// const std::vector& nodes = getKRandomNodes(K_RANDOM_NODES); -// fillOptionsNodes(options, nodes, end); -// } break; -// case PointFetchMethod::Enum::NEAREST: { -// const std::vector& nodes = getKClosestNodes(end, K_CLOESEST_NODES); -// fillOptionsNodes(options, nodes, end); -// } break; -// case PointFetchMethod::Enum::NONE: -// fillOptions(options, current_head, end); -// break; -// default: -// fillOptions(options, current_head, end); -// break; -// } + // const XYZCoord displacement_vector = sample.coord - nearest_point.coord; -// // sorts the list -// std::sort(options.begin(), options.end(), [](auto& a, auto& b) { -// auto& [a_node, a_option] = a; -// auto& [b_node, b_option] = b; -// return a_option.length + a_node->getCost() < b_option.length + b_node->getCost(); -// }); - -// // the options are already sorted, why return a truncated list? -// // 2024-03-11 : because pathing to goal req a certain amount, you can change this later if you -// // want, idk c++ memory management well enough to know if tht is a good idea (for speed) - -// // if there are less options than needed amount, then just reurn the xisting list, else, -// // return a truncated list. -// if (options.size() < quantity_options) { -// return options; -// } + // // TODO - use some heuristic to make this angle as optimal as possible + // const double angle = + // std::atan2(displacement_vector.y, displacement_vector.x) + random(-M_PI / 2, M_PI / 2); -// // erases everything after the last wanted element -// options.erase(options.begin() + quantity_options, options.end()); + // // distance between the vectors, if it is less than the search radius, then it will return + // the + // // sample point, otherwise, it will return a point `search_radius` away from the nearest + // point const double distance = displacement_vector.norm(); -// return options; -// } + // if (distance < search_radius) { + // return RRTPoint(sample.coord, angle); + // } -// void RRTTree::fillOptions(std::vector>& options, RRTNode* node, -// const RRTPoint& end) const { -// /* -// TODO - try to limit the scope of the search to prevent too many calls to dubins -// */ -// if (node == nullptr) { -// return; -// } + // RRTPoint new_point(nearest_point.coord + (search_radius / distance) * displacement_vector, + // angle); -// // gets all dubins curves from the current node to the end point -// const std::vector& local_options = dubins.allOptions(node->getPoint(), end); + // return new_point; + return RRTPoint(sample, random(0, TWO_PI)); +} -// // filters out the options that are not valid -// for (const RRTOption& option : local_options) { -// if (std::isnan(option.length) || option.length == std::numeric_limits::infinity()) { -// continue; -// } +/* + TODO - investigate whether a max heap is better or worse +*/ +std::vector> RRTTree::pathingOptions( + const RRTPoint& end, PointFetchMethod::Enum point_fetch_method, int quantity_options) const { + // fills the options list with valid values + std::vector> options; + fillOptions(options, current_head, end); + + // sorts the list + std::sort(options.begin(), options.end(), [](auto& a, auto& b) { + auto& [a_node, a_option] = a; + auto& [b_node, b_option] = b; + return a_option.length + a_node->getCost() < b_option.length + b_node->getCost(); + }); + + // the options are already sorted, why return a truncated list? + // 2024-03-11 : because pathing to goal req a certain amount, you can change this later if you + // want, idk c++ memory management well enough to know if tht is a good idea (for speed) + + // if there are less options than needed amount, then just reurn the xisting list, else, + // return a truncated list. + if (options.size() < quantity_options) { + return options; + } -// options.push_back({node, option}); -// } + // erases everything after the last wanted element + options.erase(options.begin() + quantity_options, options.end()); -// // recursively calls the function for all reachable nodes -// for (RRTNode* child : node->getReachable()) { -// fillOptions(options, child, end); -// } -// } + return options; +} -// void RRTTree::RRTStar(RRTNode* sample, double rewire_radius) { -// // last element takes in the squared value of rewire_radius to prevent the need for sqrt() -// RRTStarRecursive(current_head, sample, rewire_radius * rewire_radius); -// } +void RRTTree::fillOptions(std::vector>& options, RRTNodePtr node, + const RRTPoint& end) const { + /* + TODO - try to limit the scope of the search to prevent too many calls to dubins + */ + if (node == nullptr) { + return; + } -// void RRTTree::setCurrentHead(RRTNode* goal) { -// if (goal == nullptr) { -// LOG_F(ERROR, "FAILURE: Goal is not in the tree"); -// return; -// } + // gets all dubins curves from the current node to the end point + const std::vector& local_options = dubins.allOptions(node->getPoint(), end); -// // update local parametters -// tree_size = 1; -// current_head = goal; -// } + // filters out the options that are not valid + for (const RRTOption& option : local_options) { + if (std::isnan(option.length) || option.length == std::numeric_limits::infinity()) { + continue; + } -// // std::vector RRTTree::getPathToGoal() const { -// // RRTNode* current_node = current_head; -// // std::vector path = {}; + options.push_back({node, option}); + } -// // while (current_node != nullptr && current_node->getParent() != nullptr) { -// // const std::vector& edge_path = current_node->getPath(); + // recursively calls the function for all reachable nodes + for (RRTNodePtr child : node->getReachable()) { + fillOptions(options, child, end); + } +} -// // path.insert(path.begin(), edge_path.begin() + 1, edge_path.end()); -// // current_node = current_node->getParent(); -// // } +void RRTTree::setCurrentHead(RRTNodePtr goal) { + if (goal == nullptr) { + LOG_F(ERROR, "FAILURE: Goal is not in the tree"); + return; + } -// // // loop above misses the first node, this adds it manually -// // path.insert(path.begin(), current_node->getPoint().coord); -// // return path; -// // } + // update local parametters + tree_size = 1; + current_head = goal; +} -// std::vector RRTTree::getPathSegment(RRTNode* node) const { -// RRTNode* current_node = node; +// std::vector RRTTree::getPathToGoal() const { +// RRTNodePtr current_node = current_head; // std::vector path = {}; -// while (current_node != current_head) { -// if (current_node == nullptr) { -// LOG_F(ERROR, "TREE: Segement does not find a path"); -// return {}; -// } - +// while (current_node != nullptr && current_node->getParent() != nullptr) { // const std::vector& edge_path = current_node->getPath(); // path.insert(path.begin(), edge_path.begin() + 1, edge_path.end()); // current_node = current_node->getParent(); // } +// // loop above misses the first node, this adds it manually +// path.insert(path.begin(), current_node->getPoint().coord); // return path; // } -// RRTPoint& RRTTree::getStart() const { return root->getPoint(); } +std::vector RRTTree::getPathSegment(RRTNodePtr node) const { + RRTNodePtr current_node = node; + std::vector path = {}; + + while (current_node != current_head) { + if (current_node == nullptr) { + LOG_F(ERROR, "TREE: Segement does not find a path"); + return {}; + } + + const std::vector& edge_path = current_node->getPath(); -// /*-----------------*/ -// /* RRTTree Private */ -// /*-----------------*/ + path.insert(path.begin(), edge_path.begin() + 1, edge_path.end()); + current_node = current_node->getParent(); + } + + return path; +} -// // std::pair RRTTree::getNearestNode(const XYZCoord& point) const { -// // RRTNode* nearest = nullptr; +RRTPoint& RRTTree::getStart() const { return root->getPoint(); } + +// I'm keeping this code for the time being, because it's prolly useful for implementing RRT +// // std::pair RRTTree::getNearestNode(const XYZCoord& point) const { +// // RRTNodePtr nearest = nullptr; // // double min_distance = std::numeric_limits::infinity(); // // for (auto& node : node_set) { @@ -414,84 +309,4 @@ RRTTree::~RRTTree() { root.reset(); } // // } // // return {nearest, min_distance}; -// // } - -// void RRTTree::RRTStarRecursive(RRTNode* current_node, RRTNode* sample, -// double rewire_radius_squared) { -// // base case -// if (current_node == nullptr) { -// return; -// } - -// // for all nodes past the current node, attempt to rewire them -// for (RRTNode* child : current_node->getReachable()) { -// // get the distance between the current node and the nearest node -// if (child->getPoint().distanceToSquared(sample->getPoint()) > rewire_radius_squared) { -// continue; -// } - -// // the sample shouldn't have any children -// if (child == sample) { -// return; -// } - -// // get the dubins options (sorted) -// const std::vector& options = -// dubins.allOptions(sample->getPoint(), child->getPoint(), true); - -// // for each option -// for (const RRTOption& option : options) { -// if (std::isnan(option.length) || -// option.length == std::numeric_limits::infinity()) { -// break; -// } - -// // if the node is uncompetitive, move onto the next node -// double new_cost = sample->getCost() + option.length; -// double cost = child->getCost(); - -// if (new_cost >= cost) { -// break; -// } - -// // if the new cost is less than the current cost -// // check if new path is valid -// const std::vector& path = dubins.generatePoints( -// sample->getPoint(), child->getPoint(), option.dubins_path, option.has_straight); - -// if (!validatePath(path, option)) { -// continue; -// } - -// // rewire the edge -// rewireEdge(child, current_node, sample, path, option.length); -// } -// } - -// // recurse -// for (RRTNode* child : current_node->getReachable()) { -// RRTStarRecursive(child, sample, rewire_radius_squared); -// } -// } - -// void RRTTree::reassignCosts(RRTNode* changed_node) { -// if (changed_node == nullptr) { -// return; -// } - -// for (RRTNode* child : changed_node->getReachable()) { -// reassignCostsRecursive(changed_node, child, changed_node->getCost()); -// } -// } - -// void RRTTree::reassignCostsRecursive(RRTNode* parent, RRTNode* current_node, double path_cost) { -// if (current_node == nullptr) { -// return; -// } - -// // reassigns the cost: cost to get to the parent + known path length between parent and child -// current_node->setCost(path_cost + current_node->getPathLength()); -// for (RRTNode* neighbor : current_node->getReachable()) { -// reassignCostsRecursive(current_node, neighbor, current_node->getCost()); -// } -// } +// // } \ No newline at end of file