From 428948699ae136585419ec07a55c3579fb61c554 Mon Sep 17 00:00:00 2001 From: Billy Huddleston Date: Fri, 17 Oct 2025 15:03:10 -0400 Subject: [PATCH] Add startPoint and endPoint support to TSP solver and Python wrapper; add tests - Enhanced the C++ TSP solver to accept optional start and end points, so the route can begin and/or end at the closest point to specified coordinates. - Updated the Python pybind11 wrapper and PathUtils.sort_locations_tsp to support startPoint and endPoint as named parameters. - Added a new Python test suite (TestTSPSolver.py) to verify correct handling of start/end points and integration with PathUtils. - Registered the new test in TestCAMApp.py and CMakeLists.txt for automatic test discovery. --- src/Mod/CAM/App/tsp_solver.cpp | 154 +++++++++++++++++++++++++- src/Mod/CAM/App/tsp_solver.h | 6 +- src/Mod/CAM/App/tsp_solver_pybind.cpp | 72 +++++++++++- src/Mod/CAM/CAMTests/TestTSPSolver.py | 128 +++++++++++++++++++++ src/Mod/CAM/CMakeLists.txt | 1 + src/Mod/CAM/PathScripts/PathUtils.py | 20 +++- src/Mod/CAM/TestCAMApp.py | 1 + 7 files changed, 373 insertions(+), 9 deletions(-) create mode 100644 src/Mod/CAM/CAMTests/TestTSPSolver.py diff --git a/src/Mod/CAM/App/tsp_solver.cpp b/src/Mod/CAM/App/tsp_solver.cpp index ea8aec161e..7d37e9847b 100644 --- a/src/Mod/CAM/App/tsp_solver.cpp +++ b/src/Mod/CAM/App/tsp_solver.cpp @@ -55,16 +55,41 @@ void twoOptSwap(std::vector& path, size_t i, size_t k) } } // namespace -std::vector TSPSolver::solve(const std::vector& points) +/** + * @brief Solve the Traveling Salesperson Problem using 2-opt algorithm + * + * This implementation handles optional start and end point constraints: + * - If startPoint is provided, the path will begin at the point closest to startPoint + * - If endPoint is provided, the path will end at the point closest to endPoint + * - If both are provided, the path will respect both constraints while optimizing the middle path + * - The algorithm ensures all points are visited exactly once + */ +std::vector TSPSolver::solve(const std::vector& points, + const TSPPoint* startPoint, + const TSPPoint* endPoint) { size_t n = points.size(); if (n == 0) { return {}; } + // Start with a simple nearest neighbor path std::vector visited(n, false); std::vector path; + + // If startPoint provided, find the closest point to it size_t current = 0; + if (startPoint) { + double minDist = std::numeric_limits::max(); + for (size_t i = 0; i < n; ++i) { + double d = dist(points[i], *startPoint); + if (d < minDist) { + minDist = d; + current = i; + } + } + } + path.push_back(static_cast(current)); visited[current] = true; for (size_t step = 1; step < n; ++step) { @@ -101,5 +126,132 @@ std::vector TSPSolver::solve(const std::vector& points) } } } + + // Handle end point constraint if specified + if (endPoint) { + // If both start and end points are specified, we need to handle them differently + if (startPoint) { + // Find the closest points to start and end + size_t startIdx = 0; + size_t endIdx = 0; + double minStartDist = std::numeric_limits::max(); + double minEndDist = std::numeric_limits::max(); + + // Find the indices of the closest points to both start and end points + for (size_t i = 0; i < n; ++i) { + // Find closest to start + double dStart = dist(points[i], *startPoint); + if (dStart < minStartDist) { + minStartDist = dStart; + startIdx = i; + } + + // Find closest to end + double dEnd = dist(points[i], *endPoint); + if (dEnd < minEndDist) { + minEndDist = dEnd; + endIdx = i; + } + } + + // If start and end are different points, create a new path + if (startIdx != endIdx) { + // Create a new path starting with the start point and ending with the end point + // This ensures both constraints are met + std::vector visited(n, false); + std::vector newPath; + + // Add start point + newPath.push_back(static_cast(startIdx)); + visited[startIdx] = true; + + // Add all other points except end point using nearest neighbor algorithm + // This builds a path that starts at startIdx and visits all intermediate points + size_t current = startIdx; + for (size_t step = 1; step < n - 1; ++step) { + double minDist = std::numeric_limits::max(); + size_t next = n; // Invalid index (n is out of bounds) + + for (size_t i = 0; i < n; ++i) { + if (!visited[i] && i != endIdx) { + double d = dist(points[current], points[i]); + if (d < minDist) { + minDist = d; + next = i; + } + } + } + + if (next == n) { + break; // No more points to add + } + + current = next; + newPath.push_back(static_cast(current)); + visited[current] = true; + } + + // Add end point as the final stop in the path + newPath.push_back(static_cast(endIdx)); + + // Apply 2-opt optimization while preserving the start and end points + // The algorithm only swaps edges between interior points + bool improved = true; + while (improved) { + improved = false; + // Start from 1 and end before the last point to preserve start/end constraints + for (size_t i = 1; i < newPath.size() - 1; ++i) { + for (size_t k = i + 1; k < newPath.size() - 1; ++k) { + // Calculate improvement in distance if we swap these edges + double delta = dist(points[newPath[i - 1]], points[newPath[k]]) + + dist(points[newPath[i]], points[newPath[k + 1]]) + - dist(points[newPath[i - 1]], points[newPath[i]]) + - dist(points[newPath[k]], points[newPath[k + 1]]); + + // If the swap reduces the total distance, make the swap + if (delta < -Base::Precision::Confusion()) { + std::reverse(newPath.begin() + static_cast(i), + newPath.begin() + static_cast(k) + 1); + improved = true; + } + } + } + } + + path = newPath; + } + // If start and end are the same point, keep path as is + } + else { + // Only end point specified (no start point constraint) + // Find the point in the current path that's closest to the desired end point + double minDist = std::numeric_limits::max(); + size_t endIdx = 0; + for (size_t i = 0; i < n; ++i) { + double d = dist(points[path[i]], *endPoint); + if (d < minDist) { + minDist = d; + endIdx = i; + } + } + + // Rotate the path so that endIdx is at the end + // This preserves the relative order of points while ensuring the path ends + // at the point closest to the specified end coordinates + if (endIdx != n - 1) { + std::vector newPath; + // Start with points after endIdx + for (size_t i = endIdx + 1; i < n; ++i) { + newPath.push_back(path[i]); + } + // Then add points from beginning up to and including endIdx + for (size_t i = 0; i <= endIdx; ++i) { + newPath.push_back(path[i]); + } + path = newPath; + } + } + } + return path; } diff --git a/src/Mod/CAM/App/tsp_solver.h b/src/Mod/CAM/App/tsp_solver.h index adf17f6497..f9c50c0713 100644 --- a/src/Mod/CAM/App/tsp_solver.h +++ b/src/Mod/CAM/App/tsp_solver.h @@ -38,5 +38,9 @@ class TSPSolver { public: // Returns a vector of indices representing the visit order using 2-Opt - static std::vector solve(const std::vector& points); + // If startPoint or endPoint are provided, the path will start/end at the closest point to these + // coordinates + static std::vector solve(const std::vector& points, + const TSPPoint* startPoint = nullptr, + const TSPPoint* endPoint = nullptr); }; diff --git a/src/Mod/CAM/App/tsp_solver_pybind.cpp b/src/Mod/CAM/App/tsp_solver_pybind.cpp index d742ccc69e..a7c394f779 100644 --- a/src/Mod/CAM/App/tsp_solver_pybind.cpp +++ b/src/Mod/CAM/App/tsp_solver_pybind.cpp @@ -25,13 +25,72 @@ namespace py = pybind11; -std::vector tspSolvePy(const std::vector>& points) +std::vector tspSolvePy(const std::vector>& points, + const py::object& startPoint = py::none(), + const py::object& endPoint = py::none()) { std::vector pts; for (const auto& p : points) { pts.emplace_back(p.first, p.second); } - return TSPSolver::solve(pts); + + // Handle optional start point + TSPPoint* pStartPoint = nullptr; + TSPPoint startPointObj(0, 0); + if (!startPoint.is_none()) { + try { + // Use py::cast to convert to standard C++ types + auto sp = startPoint.cast>(); + if (sp.size() >= 2) { + startPointObj.x = sp[0]; + startPointObj.y = sp[1]; + pStartPoint = &startPointObj; + } + } + catch (py::cast_error&) { + // If casting fails, try accessing elements directly + try { + if (py::len(startPoint) >= 2) { + startPointObj.x = py::cast(startPoint.attr("__getitem__")(0)); + startPointObj.y = py::cast(startPoint.attr("__getitem__")(1)); + pStartPoint = &startPointObj; + } + } + catch (py::error_already_set&) { + // Ignore if we can't access the elements + } + } + } + + // Handle optional end point + TSPPoint* pEndPoint = nullptr; + TSPPoint endPointObj(0, 0); + if (!endPoint.is_none()) { + try { + // Use py::cast to convert to standard C++ types + auto ep = endPoint.cast>(); + if (ep.size() >= 2) { + endPointObj.x = ep[0]; + endPointObj.y = ep[1]; + pEndPoint = &endPointObj; + } + } + catch (py::cast_error&) { + // If casting fails, try accessing elements directly + try { + if (py::len(endPoint) >= 2) { + endPointObj.x = py::cast(endPoint.attr("__getitem__")(0)); + endPointObj.y = py::cast(endPoint.attr("__getitem__")(1)); + pEndPoint = &endPointObj; + } + } + catch (py::error_already_set&) { + // Ignore if we can't access the elements + } + } + } + + return TSPSolver::solve(pts, pStartPoint, pEndPoint); } PYBIND11_MODULE(tsp_solver, m) @@ -40,5 +99,12 @@ PYBIND11_MODULE(tsp_solver, m) m.def("solve", &tspSolvePy, py::arg("points"), - "Solve TSP for a list of (x, y) points using 2-Opt, returns visit order"); + py::arg("startPoint") = py::none(), + py::arg("endPoint") = py::none(), + "Solve TSP for a list of (x, y) points using 2-Opt, returns visit order.\n" + "Optional arguments:\n" + "- startPoint: Optional [x, y] point where the path should start (closest point will be " + "chosen)\n" + "- endPoint: Optional [x, y] point where the path should end (closest point will be " + "chosen)"); } diff --git a/src/Mod/CAM/CAMTests/TestTSPSolver.py b/src/Mod/CAM/CAMTests/TestTSPSolver.py new file mode 100644 index 0000000000..1eaaa35cc0 --- /dev/null +++ b/src/Mod/CAM/CAMTests/TestTSPSolver.py @@ -0,0 +1,128 @@ +# *************************************************************************** +# * Copyright (c) 2025 Billy Huddleston * +# * * +# * This program is free software; you can redistribute it and/or modify * +# * it under the terms of the GNU Lesser General Public License (LGPL) * +# * as published by the Free Software Foundation; either version 2 of * +# * the License, or (at your option) any later version. * +# * for detail see the LICENCE text file. * +# * * +# * This program is distributed in the hope that it will be useful, * +# * but WITHOUT ANY WARRANTY; without even the implied warranty of * +# * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * +# * GNU Library General Public License for more details. * +# * * +# * You should have received a copy of the GNU Library General Public * +# * License along with this program; if not, write to the Free Software * +# * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 * +# * USA * +# * * +# *************************************************************************** + +import FreeCAD +import math +import tsp_solver +import PathScripts.PathUtils as PathUtils +from CAMTests.PathTestUtils import PathTestBase + + +class TestTSPSolver(PathTestBase): + """Test class for the TSP (Traveling Salesman Problem) solver.""" + + def setUp(self): + """Set up test environment.""" + # Create test points arranged in a simple pattern + self.square_points = [ + (0, 0), # 0 - bottom left + (10, 0), # 1 - bottom right + (10, 10), # 2 - top right + (0, 10), # 3 - top left + ] + + self.random_points = [ + (5, 5), # 0 - center + (8, 2), # 1 + (3, 7), # 2 + (1, 3), # 3 + (9, 8), # 4 + ] + + # Create dictionary points for PathUtils.sort_locations_tsp + self.dict_points = [{"x": x, "y": y} for x, y in self.random_points] + + def test_simple_tsp(self): + """Test TSP solver with a simple square of points.""" + # Test the TSP solver on a simple square + route = tsp_solver.solve(self.square_points) + + # Check that the route contains all points exactly once + self.assertEqual(len(route), len(self.square_points)) + self.assertEqual(set(route), set(range(len(self.square_points)))) + + # Check that the route forms a logical path (each point is adjacent to previous) + total_distance = 0 + for i in range(len(route) - 1): + pt1 = self.square_points[route[i]] + pt2 = self.square_points[route[i + 1]] + total_distance += math.sqrt((pt2[0] - pt1[0]) ** 2 + (pt2[1] - pt1[1]) ** 2) + + # The path length should be 30 for a non-closed tour of a 10x10 square + # (10 + 10 + 10 = 30 for three sides of a square) + # Allow for small numerical errors + self.assertRoughly(total_distance, 30.0, 0.001) + + def test_start_point(self): + """Test that the path starts from the point closest to the specified start.""" + # Force start point at (0, 0) + start_point = [0, 0] + route = tsp_solver.solve(self.random_points, startPoint=start_point) + + # Based on observed behavior, the solver is choosing point 3 + # This may be using a different distance metric or have different implementation details + closest_pt_idx = 3 + self.assertEqual(route[0], closest_pt_idx) + + def test_end_point(self): + """Test that the path ends at the point closest to the specified end.""" + # Force end point at (10, 10) + end_point = [10, 10] + route = tsp_solver.solve(self.random_points, endPoint=end_point) + + # The last point should be the closest to (10, 10), which is point 4 (9, 8) + closest_pt_idx = 4 + self.assertEqual(route[-1], closest_pt_idx) + + def test_start_end_points(self): + """Test that path respects both start and end points.""" + start_point = [0, 0] # Solver should choose point 3 (1, 3) - closest to (0,0) + end_point = [10, 10] # Closest is point 4 (9, 8) + + route = tsp_solver.solve(self.random_points, startPoint=start_point, endPoint=end_point) + + self.assertEqual(route[0], 3) # Should start with point 3 (closest to start) + self.assertEqual(route[-1], 4) # Should end with point 4 (closest to end) + + def test_path_utils_integration(self): + """Test integration with PathUtils.sort_locations_tsp.""" + keys = ["x", "y"] + start_point = [0, 0] + end_point = [10, 10] + + # Test with both start and end points + sorted_locations = PathUtils.sort_locations_tsp( + self.dict_points, keys=keys, startPoint=start_point, endPoint=end_point + ) + + # First point should be closest to (0,0), which is point 3 (1,3) + self.assertRoughly(sorted_locations[0]["x"], 1, 0.001) + self.assertRoughly(sorted_locations[0]["y"], 3, 0.001) + + # Last point should have coordinates closest to (10, 10) + self.assertRoughly(sorted_locations[-1]["x"], 9, 0.001) + self.assertRoughly(sorted_locations[-1]["y"], 8, 0.001) + + +if __name__ == "__main__": + import unittest + + unittest.main() diff --git a/src/Mod/CAM/CMakeLists.txt b/src/Mod/CAM/CMakeLists.txt index d0ed7a1cb2..f44ce6771b 100644 --- a/src/Mod/CAM/CMakeLists.txt +++ b/src/Mod/CAM/CMakeLists.txt @@ -571,6 +571,7 @@ SET(Tests_SRCS CAMTests/TestPostGCodes.py CAMTests/TestPostMCodes.py CAMTests/TestSnapmakerPost.py + CAMTests/TestTSPSolver.py CAMTests/Tools/Bit/test-path-tool-bit-bit-00.fctb CAMTests/Tools/Library/test-path-tool-bit-library-00.fctl CAMTests/Tools/Shape/test-path-tool-bit-shape-00.fcstd diff --git a/src/Mod/CAM/PathScripts/PathUtils.py b/src/Mod/CAM/PathScripts/PathUtils.py index cfdd8f35c1..a3b99664bf 100644 --- a/src/Mod/CAM/PathScripts/PathUtils.py +++ b/src/Mod/CAM/PathScripts/PathUtils.py @@ -613,14 +613,26 @@ def sort_locations(locations, keys, attractors=None): return out -def sort_locations_tsp(locations, keys, attractors=None): +def sort_locations_tsp(locations, keys, attractors=None, startPoint=None, endPoint=None): """ Python wrapper for the C++ TSP solver. Takes a list of dicts (locations), - a list of keys (e.g. ['x', 'y']), and optional attractors. - Returns the sorted list of locations in TSP order, starting at (0,0). + a list of keys (e.g. ['x', 'y']), and optional parameters. + + Parameters: + - locations: List of dictionaries with point coordinates + - keys: List of keys to use for coordinates (e.g. ['x', 'y']) + - attractors: Optional parameter (not used, kept for compatibility) + - startPoint: Optional starting point [x, y] + - endPoint: Optional ending point [x, y] + + Returns the sorted list of locations in TSP order. + If startPoint is None, path starts from the first point in the original order. """ + # Extract points from locations points = [(loc[keys[0]], loc[keys[1]]) for loc in locations] - order = tsp_solver.solve(points) + order = tsp_solver.solve(points=points, startPoint=startPoint, endPoint=endPoint) + + # Return the reordered locations return [locations[i] for i in order] diff --git a/src/Mod/CAM/TestCAMApp.py b/src/Mod/CAM/TestCAMApp.py index abf4a26933..f03cd14422 100644 --- a/src/Mod/CAM/TestCAMApp.py +++ b/src/Mod/CAM/TestCAMApp.py @@ -117,3 +117,4 @@ from CAMTests.TestCentroidLegacyPost import TestCentroidLegacyPost from CAMTests.TestMach3Mach4LegacyPost import TestMach3Mach4LegacyPost from CAMTests.TestSnapmakerPost import TestSnapmakerPost +from CAMTests.TestTSPSolver import TestTSPSolver