From f9347c781b0c8b7623af4d9dd77585744d69ffa8 Mon Sep 17 00:00:00 2001 From: Billy Huddleston Date: Fri, 17 Oct 2025 14:08:31 -0400 Subject: [PATCH 1/5] Add 2-Opt TSP solver This update introduces a new C++ 2-Opt TSP solver with Python bindings. src/Mod/CAM/App/CMakeLists.txt: - Add build and install rules for the new pybind11-based tsp_solver Python module src/Mod/CAM/App/tsp_solver.cpp: - Add new C++ implementation of a 2-Opt TSP solver with nearest-neighbor initialization src/Mod/CAM/App/tsp_solver.h: - Add TSPPoint struct and TSPSolver class with 2-Opt solve method src/Mod/CAM/App/tsp_solver_pybind.cpp: - Add pybind11 wrapper exposing the TSP solver to Python as tsp_solver.solve src/Mod/CAM/PathScripts/PathUtils.py: - Add sort_locations_tsp Python wrapper for the C++ TSP solver - Use tsp_solver.solve for TSP-based --- src/Mod/CAM/App/CMakeLists.txt | 12 +++ src/Mod/CAM/App/tsp_solver.cpp | 105 ++++++++++++++++++++++++++ src/Mod/CAM/App/tsp_solver.h | 42 +++++++++++ src/Mod/CAM/App/tsp_solver_pybind.cpp | 44 +++++++++++ src/Mod/CAM/PathScripts/PathUtils.py | 13 ++++ 5 files changed, 216 insertions(+) create mode 100644 src/Mod/CAM/App/tsp_solver.cpp create mode 100644 src/Mod/CAM/App/tsp_solver.h create mode 100644 src/Mod/CAM/App/tsp_solver_pybind.cpp diff --git a/src/Mod/CAM/App/CMakeLists.txt b/src/Mod/CAM/App/CMakeLists.txt index 3eda1821b5..4ec9936d3c 100644 --- a/src/Mod/CAM/App/CMakeLists.txt +++ b/src/Mod/CAM/App/CMakeLists.txt @@ -138,3 +138,15 @@ SET_BIN_DIR(Path PathApp /Mod/CAM) SET_PYTHON_PREFIX_SUFFIX(Path) INSTALL(TARGETS Path DESTINATION ${CMAKE_INSTALL_LIBDIR}) + +# --- TSP Solver Python module (pybind11) --- +find_package(pybind11 REQUIRED) + +pybind11_add_module(tsp_solver MODULE tsp_solver_pybind.cpp tsp_solver.cpp) +target_include_directories(tsp_solver PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}) + +SET_BIN_DIR(tsp_solver tsp_solver /Mod/CAM) +SET_PYTHON_PREFIX_SUFFIX(tsp_solver) + +INSTALL(TARGETS tsp_solver DESTINATION ${CMAKE_INSTALL_LIBDIR}) +# ------------------------------------------- diff --git a/src/Mod/CAM/App/tsp_solver.cpp b/src/Mod/CAM/App/tsp_solver.cpp new file mode 100644 index 0000000000..ea8aec161e --- /dev/null +++ b/src/Mod/CAM/App/tsp_solver.cpp @@ -0,0 +1,105 @@ +/*************************************************************************** + * 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 * + * * + ***************************************************************************/ + +#include "tsp_solver.h" +#include +#include +#include +#include +#include + +namespace +{ +// Euclidean distance between two points +double dist(const TSPPoint& a, const TSPPoint& b) +{ + double dx = a.x - b.x; + double dy = a.y - b.y; + return std::sqrt(dx * dx + dy * dy); +} + +// Calculate total path length +double pathLength(const std::vector& points, const std::vector& path) +{ + double total = 0.0; + size_t n = path.size(); + for (size_t i = 0; i < n - 1; ++i) { + total += dist(points[path[i]], points[path[i + 1]]); + } + // Optionally, close the loop: total += dist(points[path[n-1]], points[path[0]]); + return total; +} + +// 2-Opt swap +void twoOptSwap(std::vector& path, size_t i, size_t k) +{ + std::reverse(path.begin() + static_cast(i), path.begin() + static_cast(k) + 1); +} +} // namespace + +std::vector TSPSolver::solve(const std::vector& points) +{ + size_t n = points.size(); + if (n == 0) { + return {}; + } + // Start with a simple nearest neighbor path + std::vector visited(n, false); + std::vector path; + size_t current = 0; + path.push_back(static_cast(current)); + visited[current] = true; + for (size_t step = 1; step < n; ++step) { + double min_dist = std::numeric_limits::max(); + size_t next = n; // Use n as an invalid index + for (size_t i = 0; i < n; ++i) { + if (!visited[i]) { + double d = dist(points[current], points[i]); + if (d < min_dist) { + min_dist = d; + next = i; + } + } + } + current = next; + path.push_back(static_cast(current)); + visited[current] = true; + } + + // 2-Opt optimization + bool improved = true; + while (improved) { + improved = false; + for (size_t i = 1; i < n - 1; ++i) { + for (size_t k = i + 1; k < n; ++k) { + double delta = dist(points[path[i - 1]], points[path[k]]) + + dist(points[path[i]], points[path[(k + 1) % n]]) + - dist(points[path[i - 1]], points[path[i]]) + - dist(points[path[k]], points[path[(k + 1) % n]]); + if (delta < -Base::Precision::Confusion()) { + twoOptSwap(path, i, k); + improved = true; + } + } + } + } + return path; +} diff --git a/src/Mod/CAM/App/tsp_solver.h b/src/Mod/CAM/App/tsp_solver.h new file mode 100644 index 0000000000..adf17f6497 --- /dev/null +++ b/src/Mod/CAM/App/tsp_solver.h @@ -0,0 +1,42 @@ +/*************************************************************************** + * 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 * + * * + ***************************************************************************/ + +#pragma once +#include +#include +#include +#include + +struct TSPPoint +{ + double x, y; + TSPPoint(double x_, double y_) + : x(x_) + , y(y_) + {} +}; + +class TSPSolver +{ +public: + // Returns a vector of indices representing the visit order using 2-Opt + static std::vector solve(const std::vector& points); +}; diff --git a/src/Mod/CAM/App/tsp_solver_pybind.cpp b/src/Mod/CAM/App/tsp_solver_pybind.cpp new file mode 100644 index 0000000000..d742ccc69e --- /dev/null +++ b/src/Mod/CAM/App/tsp_solver_pybind.cpp @@ -0,0 +1,44 @@ +/*************************************************************************** + * 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 * + * * + ***************************************************************************/ + +#include +#include +#include "tsp_solver.h" + +namespace py = pybind11; + +std::vector tspSolvePy(const std::vector>& points) +{ + std::vector pts; + for (const auto& p : points) { + pts.emplace_back(p.first, p.second); + } + return TSPSolver::solve(pts); +} + +PYBIND11_MODULE(tsp_solver, m) +{ + m.doc() = "Simple TSP solver (2-Opt) for FreeCAD"; + m.def("solve", + &tspSolvePy, + py::arg("points"), + "Solve TSP for a list of (x, y) points using 2-Opt, returns visit order"); +} diff --git a/src/Mod/CAM/PathScripts/PathUtils.py b/src/Mod/CAM/PathScripts/PathUtils.py index 02e03a0544..cfdd8f35c1 100644 --- a/src/Mod/CAM/PathScripts/PathUtils.py +++ b/src/Mod/CAM/PathScripts/PathUtils.py @@ -2,6 +2,7 @@ # *************************************************************************** # * Copyright (c) 2014 Dan Falck * +# * 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) * @@ -29,6 +30,7 @@ import Path import Path.Main.Job as PathJob import math from numpy import linspace +import tsp_solver # lazily loaded modules from lazy_loader.lazy_loader import LazyLoader @@ -611,6 +613,17 @@ def sort_locations(locations, keys, attractors=None): return out +def sort_locations_tsp(locations, keys, attractors=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). + """ + points = [(loc[keys[0]], loc[keys[1]]) for loc in locations] + order = tsp_solver.solve(points) + return [locations[i] for i in order] + + def guessDepths(objshape, subs=None): """ takes an object shape and optional list of subobjects and returns a depth_params From 428948699ae136585419ec07a55c3579fb61c554 Mon Sep 17 00:00:00 2001 From: Billy Huddleston Date: Fri, 17 Oct 2025 15:03:10 -0400 Subject: [PATCH 2/5] 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 From 005cbb937d5827849595466ceabd2794e8b4ec61 Mon Sep 17 00:00:00 2001 From: Billy Huddleston Date: Sun, 19 Oct 2025 17:12:08 -0400 Subject: [PATCH 3/5] Rewrite TSP solver for improved path optimization and clarity - Completely re-implemented the TSP algorithm in C++ for better path quality - Added detailed comments and documentation to clarify each step - Improved nearest neighbor, 2-opt, and relocation logic - Enhanced handling of start/end point constraints - Updated PathUtils.py docstring to accurately describe start point behavior --- src/Mod/CAM/App/tsp_solver.cpp | 452 +++++++++++++++------------ src/Mod/CAM/PathScripts/PathUtils.py | 3 +- 2 files changed, 255 insertions(+), 200 deletions(-) diff --git a/src/Mod/CAM/App/tsp_solver.cpp b/src/Mod/CAM/App/tsp_solver.cpp index 7d37e9847b..9927b8aa30 100644 --- a/src/Mod/CAM/App/tsp_solver.cpp +++ b/src/Mod/CAM/App/tsp_solver.cpp @@ -28,7 +28,15 @@ namespace { -// Euclidean distance between two points +/** + * @brief Calculate Euclidean distance between two points + * + * Used for 2-opt and relocation steps where actual distance matters for path length optimization. + * + * @param a First point + * @param b Second point + * @return Actual distance: sqrt(dx² + dy²) + */ double dist(const TSPPoint& a, const TSPPoint& b) { double dx = a.x - b.x; @@ -36,22 +44,251 @@ double dist(const TSPPoint& a, const TSPPoint& b) return std::sqrt(dx * dx + dy * dy); } -// Calculate total path length -double pathLength(const std::vector& points, const std::vector& path) +/** + * @brief Calculate squared distance between two points (no sqrt) + * + * Used for nearest neighbor selection for performance (avoids expensive sqrt operation). + * Since we only need to compare distances, squared distance preserves ordering: + * if dist(A,B) < dist(A,C), then distSquared(A,B) < distSquared(A,C) + * + * @param a First point + * @param b Second point + * @return Squared distance: dx² + dy² + */ +double distSquared(const TSPPoint& a, const TSPPoint& b) { - double total = 0.0; - size_t n = path.size(); - for (size_t i = 0; i < n - 1; ++i) { - total += dist(points[path[i]], points[path[i + 1]]); - } - // Optionally, close the loop: total += dist(points[path[n-1]], points[path[0]]); - return total; + double dx = a.x - b.x; + double dy = a.y - b.y; + return dx * dx + dy * dy; } -// 2-Opt swap -void twoOptSwap(std::vector& path, size_t i, size_t k) +/** + * @brief Core TSP solver implementation using nearest neighbor + iterative improvement + * + * Algorithm steps: + * 1. Add temporary start/end points if specified + * 2. Build initial route using nearest neighbor heuristic + * 3. Optimize route with 2-opt and relocation moves + * 4. Remove temporary points and map back to original indices + * + * @param points Input points to visit + * @param startPoint Optional starting location constraint + * @param endPoint Optional ending location constraint + * @return Vector of indices representing optimized visit order + */ +std::vector solve_impl(const std::vector& points, + const TSPPoint* startPoint, + const TSPPoint* endPoint) { - std::reverse(path.begin() + static_cast(i), path.begin() + static_cast(k) + 1); + // ======================================================================== + // STEP 1: Prepare point set with temporary start/end markers + // ======================================================================== + // We insert temporary points to enforce start/end constraints. + // These will be removed after optimization and won't appear in final result. + std::vector pts = points; + int tempStartIdx = -1, tempEndIdx = -1; + + if (startPoint) { + // Insert user-specified start point at beginning + pts.insert(pts.begin(), TSPPoint(startPoint->x, startPoint->y)); + tempStartIdx = 0; + } + else if (!pts.empty()) { + // No start specified: duplicate first point as anchor + pts.insert(pts.begin(), TSPPoint(pts[0].x, pts[0].y)); + tempStartIdx = 0; + } + + if (endPoint) { + // Add user-specified end point at the end + pts.push_back(TSPPoint(endPoint->x, endPoint->y)); + tempEndIdx = static_cast(pts.size()) - 1; + } + + // ======================================================================== + // STEP 2: Build initial route using Nearest Neighbor algorithm + // ======================================================================== + // Greedy approach: always visit the closest unvisited point next. + // This gives a decent initial solution quickly (O(n²) complexity). + // + // Tie-breaking rule: + // - If distances are within ±0.1, prefer point with y-value closer to start + // - This provides deterministic results when points are nearly equidistant + std::vector route; + std::vector visited(pts.size(), false); + route.push_back(0); // Start from temp start point (index 0) + visited[0] = true; + + for (size_t step = 1; step < pts.size(); ++step) { + double minDist = std::numeric_limits::max(); + int next = -1; + double nextYDiff = std::numeric_limits::max(); + + // Find nearest unvisited neighbor + for (size_t i = 0; i < pts.size(); ++i) { + if (!visited[i]) { + // Use squared distance for speed (no sqrt needed for comparison) + double d = distSquared(pts[route.back()], pts[i]); + double yDiff = std::abs(pts[route.front()].y - pts[i].y); + + // Tie-breaking logic: + if (d > minDist + 0.1) { + continue; // Clearly farther, skip + } + else if (d < minDist - 0.1) { + // Clearly closer, use it + minDist = d; + next = static_cast(i); + nextYDiff = yDiff; + } + else if (yDiff < nextYDiff) { + // Tie: prefer point closer to start in Y-axis + minDist = d; + next = static_cast(i); + nextYDiff = yDiff; + } + } + } + + if (next == -1) { + break; // No more unvisited points + } + route.push_back(next); + visited[next] = true; + } + + // Ensure temporary end point is at the end of route + if (tempEndIdx != -1 && route.back() != tempEndIdx) { + auto it = std::find(route.begin(), route.end(), tempEndIdx); + if (it != route.end()) { + route.erase(it); + } + route.push_back(tempEndIdx); + } + + // ======================================================================== + // STEP 3: Iterative improvement using 2-Opt and Relocation + // ======================================================================== + // Repeatedly apply local optimizations until no improvement is possible. + // This typically converges quickly (a few iterations) to a near-optimal solution. + // + // Two optimization techniques: + // 1. 2-Opt: Reverse segments of the route to eliminate crossing paths + // 2. Relocation: Move individual points to better positions in the route + bool improvementFound = true; + while (improvementFound) { + improvementFound = false; + + // --- 2-Opt Optimization --- + // Try reversing every possible segment of the route. + // If reversing segment [i+1...j-1] reduces total distance, keep it. + // + // Example: Route A-B-C-D-E becomes A-D-C-B-E if reversing B-C-D is better + bool reorderFound = true; + while (reorderFound) { + reorderFound = false; + for (size_t i = 0; i + 3 < route.size(); ++i) { + for (size_t j = i + 3; j < route.size(); ++j) { + // Current edges: i→(i+1) and (j-1)→j + double curLen = dist(pts[route[i]], pts[route[i + 1]]) + + dist(pts[route[j - 1]], pts[route[j]]); + + // New edges after reversal: (i+1)→j and i→(j-1) + // Add epsilon to prevent cycles from floating point errors + double newLen = dist(pts[route[i + 1]], pts[route[j]]) + + dist(pts[route[i]], pts[route[j - 1]]) + 1e-5; + + if (newLen < curLen) { + // Reverse the segment between i+1 and j (exclusive) + std::reverse(route.begin() + i + 1, route.begin() + j); + reorderFound = true; + improvementFound = true; + } + } + } + } + + // --- Relocation Optimization --- + // Try moving each point to a different position in the route. + // If moving point i to position j improves the route, do it. + bool relocateFound = true; + while (relocateFound) { + relocateFound = false; + for (size_t i = 1; i + 1 < route.size(); ++i) { + + // Try moving point i backward (to positions before i) + for (size_t j = 1; j + 2 < i; ++j) { + // Current cost: edges around point i and edge j→(j+1) + double curLen = dist(pts[route[i - 1]], pts[route[i]]) + + dist(pts[route[i]], pts[route[i + 1]]) + + dist(pts[route[j]], pts[route[j + 1]]); + + // New cost: bypass i, insert i after j + double newLen = dist(pts[route[i - 1]], pts[route[i + 1]]) + + dist(pts[route[j]], pts[route[i]]) + + dist(pts[route[i]], pts[route[j + 1]]) + 1e-5; + + if (newLen < curLen) { + // Move point i to position after j + int node = route[i]; + route.erase(route.begin() + i); + route.insert(route.begin() + j + 1, node); + relocateFound = true; + improvementFound = true; + } + } + + // Try moving point i forward (to positions after i) + for (size_t j = i + 1; j + 1 < route.size(); ++j) { + double curLen = dist(pts[route[i - 1]], pts[route[i]]) + + dist(pts[route[i]], pts[route[i + 1]]) + + dist(pts[route[j]], pts[route[j + 1]]); + + double newLen = dist(pts[route[i - 1]], pts[route[i + 1]]) + + dist(pts[route[j]], pts[route[i]]) + + dist(pts[route[i]], pts[route[j + 1]]) + 1e-5; + + if (newLen < curLen) { + int node = route[i]; + route.erase(route.begin() + i); + route.insert(route.begin() + j, node); + relocateFound = true; + improvementFound = true; + } + } + } + } + } + + // ======================================================================== + // STEP 4: Remove temporary start/end points + // ======================================================================== + // The temporary markers served their purpose during optimization. + // Now remove them so they don't appear in the final result. + if (tempEndIdx != -1 && !route.empty() && route.back() == tempEndIdx) { + route.pop_back(); + } + if (tempStartIdx != -1 && !route.empty() && route.front() == tempStartIdx) { + route.erase(route.begin()); + } + + // ======================================================================== + // STEP 5: Map route indices back to original point array + // ======================================================================== + // Since we inserted a temp start point at index 0, all subsequent indices + // are offset by 1. Adjust them back to match the original points array. + std::vector result; + for (int idx : route) { + // Adjust for temp start offset + if (tempStartIdx != -1) { + --idx; + } + // Only include valid indices from the original points array + if (idx >= 0 && idx < static_cast(points.size())) { + result.push_back(idx); + } + } + return result; } } // namespace @@ -64,194 +301,11 @@ void twoOptSwap(std::vector& path, size_t i, size_t k) * - 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) { - double min_dist = std::numeric_limits::max(); - size_t next = n; // Use n as an invalid index - for (size_t i = 0; i < n; ++i) { - if (!visited[i]) { - double d = dist(points[current], points[i]); - if (d < min_dist) { - min_dist = d; - next = i; - } - } - } - current = next; - path.push_back(static_cast(current)); - visited[current] = true; - } - - // 2-Opt optimization - bool improved = true; - while (improved) { - improved = false; - for (size_t i = 1; i < n - 1; ++i) { - for (size_t k = i + 1; k < n; ++k) { - double delta = dist(points[path[i - 1]], points[path[k]]) - + dist(points[path[i]], points[path[(k + 1) % n]]) - - dist(points[path[i - 1]], points[path[i]]) - - dist(points[path[k]], points[path[(k + 1) % n]]); - if (delta < -Base::Precision::Confusion()) { - twoOptSwap(path, i, k); - improved = true; - } - } - } - } - - // 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; + return solve_impl(points, startPoint, endPoint); } diff --git a/src/Mod/CAM/PathScripts/PathUtils.py b/src/Mod/CAM/PathScripts/PathUtils.py index a3b99664bf..ce5d9d60a1 100644 --- a/src/Mod/CAM/PathScripts/PathUtils.py +++ b/src/Mod/CAM/PathScripts/PathUtils.py @@ -626,7 +626,8 @@ def sort_locations_tsp(locations, keys, attractors=None, startPoint=None, endPoi - 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. + If startPoint is None, the path is optimized to start near the first point in the original list, + but may not start exactly at that point. """ # Extract points from locations points = [(loc[keys[0]], loc[keys[1]]) for loc in locations] From d940c499b76d958eb1e6282c52eaa1a65f283af5 Mon Sep 17 00:00:00 2001 From: Billy Huddleston Date: Wed, 22 Oct 2025 10:46:11 -0400 Subject: [PATCH 4/5] CAM: Add TSP tunnel solver with flipping and Python bindings Introduce TSPTunnel struct and implement TSPSolver::solveTunnels for optimizing tunnel order with support for flipping and start/end points. Expose the new functionality to Python via pybind11, returning tunnel dictionaries with flipped status. src/Mod/CAM/App/tsp_solver.cpp: - Add solveTunnels implementation for tunnel TSP with flipping and route endpoints src/Mod/CAM/App/tsp_solver.h: - Define TSPTunnel struct - Declare solveTunnels static method in TSPSolver src/Mod/CAM/App/tsp_solver_pybind.cpp: - Add Python wrapper for solveTunnels - Expose solveTunnels to Python with argument parsing and result conversion --- src/Mod/CAM/App/tsp_solver.cpp | 287 +++++++++++++++++++++++++- src/Mod/CAM/App/tsp_solver.h | 39 +++- src/Mod/CAM/App/tsp_solver_pybind.cpp | 144 +++++++++++-- src/Mod/CAM/CAMTests/TestTSPSolver.py | 2 + 4 files changed, 449 insertions(+), 23 deletions(-) diff --git a/src/Mod/CAM/App/tsp_solver.cpp b/src/Mod/CAM/App/tsp_solver.cpp index 9927b8aa30..5c0dc3ad2d 100644 --- a/src/Mod/CAM/App/tsp_solver.cpp +++ b/src/Mod/CAM/App/tsp_solver.cpp @@ -1,3 +1,5 @@ +// SPDX-License-Identifier: LGPL-2.1-or-later + /*************************************************************************** * Copyright (c) 2025 Billy Huddleston * * * @@ -76,9 +78,11 @@ double distSquared(const TSPPoint& a, const TSPPoint& b) * @param endPoint Optional ending location constraint * @return Vector of indices representing optimized visit order */ -std::vector solve_impl(const std::vector& points, - const TSPPoint* startPoint, - const TSPPoint* endPoint) +std::vector solve_impl( + const std::vector& points, + const TSPPoint* startPoint, + const TSPPoint* endPoint +) { // ======================================================================== // STEP 1: Prepare point set with temporary start/end markers @@ -303,9 +307,280 @@ std::vector solve_impl(const std::vector& points, */ -std::vector TSPSolver::solve(const std::vector& points, - const TSPPoint* startPoint, - const TSPPoint* endPoint) +std::vector TSPSolver::solve( + const std::vector& points, + const TSPPoint* startPoint, + const TSPPoint* endPoint +) { return solve_impl(points, startPoint, endPoint); } + +std::vector TSPSolver::solveTunnels( + std::vector tunnels, + bool allowFlipping, + const TSPPoint* routeStartPoint, + const TSPPoint* routeEndPoint +) +{ + if (tunnels.empty()) { + return tunnels; + } + + // Set original indices + for (size_t i = 0; i < tunnels.size(); ++i) { + tunnels[i].originalIdx = static_cast(i); + } + + // STEP 1: Add the routeStartPoint (will be deleted at the end) + if (routeStartPoint) { + tunnels.insert( + tunnels.begin(), + TSPTunnel(routeStartPoint->x, routeStartPoint->y, routeStartPoint->x, routeStartPoint->y, false) + ); + } + else { + tunnels.insert(tunnels.begin(), TSPTunnel(0.0, 0.0, 0.0, 0.0, false)); + } + + // STEP 2: Apply nearest neighbor algorithm + std::vector potentialNeighbours(tunnels.begin() + 1, tunnels.end()); + std::vector route; + route.push_back(tunnels[0]); + + while (!potentialNeighbours.empty()) { + double costCurrent = std::numeric_limits::max(); + bool toBeFlipped = false; + auto nearestNeighbour = potentialNeighbours.begin(); + + // Check normal orientation + for (auto it = potentialNeighbours.begin(); it != potentialNeighbours.end(); ++it) { + double dx = route.back().endX - it->startX; + double dy = route.back().endY - it->startY; + double costNew = dx * dx + dy * dy; + + if (costNew < costCurrent) { + costCurrent = costNew; + toBeFlipped = false; + nearestNeighbour = it; + } + } + + // Check flipped orientation if allowed + if (allowFlipping) { + for (auto it = potentialNeighbours.begin(); it != potentialNeighbours.end(); ++it) { + if (it->isOpen) { + double dx = route.back().endX - it->endX; + double dy = route.back().endY - it->endY; + double costNew = dx * dx + dy * dy; + + if (costNew < costCurrent) { + costCurrent = costNew; + toBeFlipped = true; + nearestNeighbour = it; + } + } + } + } + + // Apply flipping if needed + if (toBeFlipped) { + nearestNeighbour->flipped = !nearestNeighbour->flipped; + std::swap(nearestNeighbour->startX, nearestNeighbour->endX); + std::swap(nearestNeighbour->startY, nearestNeighbour->endY); + } + + route.push_back(*nearestNeighbour); + potentialNeighbours.erase(nearestNeighbour); + } + + // STEP 3: Add the routeEndPoint (will be deleted at the end) + if (routeEndPoint) { + route.push_back( + TSPTunnel(routeEndPoint->x, routeEndPoint->y, routeEndPoint->x, routeEndPoint->y, false) + ); + } + + // STEP 4: Additional improvement of the route + bool improvementFound = true; + while (improvementFound) { + improvementFound = false; + + if (allowFlipping) { + // STEP 4.1: Apply 2-opt + bool improvementReorderFound = true; + while (improvementReorderFound) { + improvementReorderFound = false; + for (size_t i = 0; i + 3 < route.size(); ++i) { + for (size_t j = i + 3; j < route.size(); ++j) { + double subRouteLengthCurrent = std::sqrt( + std::pow(route[i].endX - route[i + 1].startX, 2) + + std::pow(route[i].endY - route[i + 1].startY, 2) + ); + subRouteLengthCurrent += std::sqrt( + std::pow(route[j - 1].endX - route[j].startX, 2) + + std::pow(route[j - 1].endY - route[j].startY, 2) + ); + + double subRouteLengthNew = std::sqrt( + std::pow(route[i + 1].startX - route[j].startX, 2) + + std::pow(route[i + 1].startY - route[j].startY, 2) + ); + subRouteLengthNew += std::sqrt( + std::pow(route[i].endX - route[j - 1].endX, 2) + + std::pow(route[i].endY - route[j - 1].endY, 2) + ); + subRouteLengthNew += 1e-6; + + if (subRouteLengthNew < subRouteLengthCurrent) { + // Flip direction of each tunnel between i-th and j-th element + for (size_t k = i + 1; k < j; ++k) { + if (route[k].isOpen) { + route[k].flipped = !route[k].flipped; + std::swap(route[k].startX, route[k].endX); + std::swap(route[k].startY, route[k].endY); + } + } + // Reverse the order of tunnels between i-th and j-th element + std::reverse(route.begin() + i + 1, route.begin() + j); + improvementReorderFound = true; + improvementFound = true; + } + } + } + } + + // STEP 4.2: Apply flipping + bool improvementFlipFound = true; + while (improvementFlipFound) { + improvementFlipFound = false; + for (size_t i = 1; i + 1 < route.size(); ++i) { + if (route[i].isOpen) { + double subRouteLengthCurrent = std::sqrt( + std::pow(route[i - 1].endX - route[i].startX, 2) + + std::pow(route[i - 1].endY - route[i].startY, 2) + ); + subRouteLengthCurrent += std::sqrt( + std::pow(route[i].endX - route[i + 1].startX, 2) + + std::pow(route[i].endY - route[i + 1].startY, 2) + ); + + double subRouteLengthNew = std::sqrt( + std::pow(route[i - 1].endX - route[i].endX, 2) + + std::pow(route[i - 1].endY - route[i].endY, 2) + ); + subRouteLengthNew += std::sqrt( + std::pow(route[i].startX - route[i + 1].startX, 2) + + std::pow(route[i].startY - route[i + 1].startY, 2) + ); + subRouteLengthNew += 1e-6; + + if (subRouteLengthNew < subRouteLengthCurrent) { + // Flip direction of i-th tunnel + route[i].flipped = !route[i].flipped; + std::swap(route[i].startX, route[i].endX); + std::swap(route[i].startY, route[i].endY); + improvementFlipFound = true; + improvementFound = true; + } + } + } + } + } + + // STEP 4.3: Apply relocation + bool improvementRelocateFound = true; + while (improvementRelocateFound) { + improvementRelocateFound = false; + for (size_t i = 1; i + 1 < route.size(); ++i) { + // Try relocating backward + for (size_t j = 1; j + 2 < i; ++j) { + double subRouteLengthCurrent = std::sqrt( + std::pow(route[i - 1].endX - route[i].startX, 2) + + std::pow(route[i - 1].endY - route[i].startY, 2) + ); + subRouteLengthCurrent += std::sqrt( + std::pow(route[i].endX - route[i + 1].startX, 2) + + std::pow(route[i].endY - route[i + 1].startY, 2) + ); + subRouteLengthCurrent += std::sqrt( + std::pow(route[j].endX - route[j + 1].startX, 2) + + std::pow(route[j].endY - route[j + 1].startY, 2) + ); + + double subRouteLengthNew = std::sqrt( + std::pow(route[i - 1].endX - route[i + 1].startX, 2) + + std::pow(route[i - 1].endY - route[i + 1].startY, 2) + ); + subRouteLengthNew += std::sqrt( + std::pow(route[j].endX - route[i].startX, 2) + + std::pow(route[j].endY - route[i].startY, 2) + ); + subRouteLengthNew += std::sqrt( + std::pow(route[i].endX - route[j + 1].startX, 2) + + std::pow(route[i].endY - route[j + 1].startY, 2) + ); + subRouteLengthNew += 1e-6; + + if (subRouteLengthNew < subRouteLengthCurrent) { + // Relocate the i-th tunnel backward (after j-th element) + TSPTunnel temp = route[i]; + route.erase(route.begin() + i); + route.insert(route.begin() + j + 1, temp); + improvementRelocateFound = true; + improvementFound = true; + } + } + + // Try relocating forward + for (size_t j = i + 1; j + 1 < route.size(); ++j) { + double subRouteLengthCurrent = std::sqrt( + std::pow(route[i - 1].endX - route[i].startX, 2) + + std::pow(route[i - 1].endY - route[i].startY, 2) + ); + subRouteLengthCurrent += std::sqrt( + std::pow(route[i].endX - route[i + 1].startX, 2) + + std::pow(route[i].endY - route[i + 1].startY, 2) + ); + subRouteLengthCurrent += std::sqrt( + std::pow(route[j].endX - route[j + 1].startX, 2) + + std::pow(route[j].endY - route[j + 1].startY, 2) + ); + + double subRouteLengthNew = std::sqrt( + std::pow(route[i - 1].endX - route[i + 1].startX, 2) + + std::pow(route[i - 1].endY - route[i + 1].startY, 2) + ); + subRouteLengthNew += std::sqrt( + std::pow(route[j].endX - route[i].startX, 2) + + std::pow(route[j].endY - route[i].startY, 2) + ); + subRouteLengthNew += std::sqrt( + std::pow(route[i].endX - route[j + 1].startX, 2) + + std::pow(route[i].endY - route[j + 1].startY, 2) + ); + subRouteLengthNew += 1e-6; + + if (subRouteLengthNew < subRouteLengthCurrent) { + // Relocate the i-th tunnel forward (after j-th element) + TSPTunnel temp = route[i]; + route.erase(route.begin() + i); + route.insert(route.begin() + j, temp); + improvementRelocateFound = true; + improvementFound = true; + } + } + } + } + } + + // STEP 5: Delete temporary start and end point + if (!route.empty()) { + route.erase(route.begin()); // Remove temp start + } + if (routeEndPoint && !route.empty()) { + route.pop_back(); // Remove temp end + } + + return route; +} diff --git a/src/Mod/CAM/App/tsp_solver.h b/src/Mod/CAM/App/tsp_solver.h index f9c50c0713..416b68ed9f 100644 --- a/src/Mod/CAM/App/tsp_solver.h +++ b/src/Mod/CAM/App/tsp_solver.h @@ -1,3 +1,5 @@ +// SPDX-License-Identifier: LGPL-2.1-or-later + /*************************************************************************** * Copyright (c) 2025 Billy Huddleston * * * @@ -34,13 +36,44 @@ struct TSPPoint {} }; +struct TSPTunnel +{ + double startX, startY; + double endX, endY; + bool isOpen; // Whether the tunnel can be flipped (entry/exit can be swapped) + bool flipped; // Tracks if tunnel has been flipped from original orientation + int originalIdx; // Original index in input array + + TSPTunnel(double sx, double sy, double ex, double ey, bool open = true) + : startX(sx) + , startY(sy) + , endX(ex) + , endY(ey) + , isOpen(open) + , flipped(false) + , originalIdx(-1) + {} +}; + class TSPSolver { public: // Returns a vector of indices representing the visit order using 2-Opt // 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); + static std::vector solve( + const std::vector& points, + const TSPPoint* startPoint = nullptr, + const TSPPoint* endPoint = nullptr + ); + + // Solves TSP for tunnels (path segments with entry/exit points) + // allowFlipping: whether tunnels can be reversed (entry becomes exit) + // Returns vector of tunnels in optimized order (tunnels may be flipped) + static std::vector solveTunnels( + std::vector tunnels, + bool allowFlipping = false, + const TSPPoint* routeStartPoint = nullptr, + const TSPPoint* routeEndPoint = nullptr + ); }; diff --git a/src/Mod/CAM/App/tsp_solver_pybind.cpp b/src/Mod/CAM/App/tsp_solver_pybind.cpp index a7c394f779..e5074f9a5d 100644 --- a/src/Mod/CAM/App/tsp_solver_pybind.cpp +++ b/src/Mod/CAM/App/tsp_solver_pybind.cpp @@ -1,3 +1,5 @@ +// SPDX-License-Identifier: LGPL-2.1-or-later + /*************************************************************************** * Copyright (c) 2025 Billy Huddleston * * * @@ -25,9 +27,11 @@ namespace py = pybind11; -std::vector tspSolvePy(const std::vector>& points, - const py::object& startPoint = py::none(), - const py::object& endPoint = py::none()) +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) { @@ -93,18 +97,130 @@ std::vector tspSolvePy(const std::vector>& points return TSPSolver::solve(pts, pStartPoint, pEndPoint); } +// Python wrapper for solveTunnels function +std::vector tspSolveTunnelsPy( + const std::vector& tunnels, + bool allowFlipping = false, + const py::object& routeStartPoint = py::none(), + const py::object& routeEndPoint = py::none() +) +{ + std::vector cppTunnels; + + // Convert Python dictionaries to C++ TSPTunnel objects + for (const auto& tunnel : tunnels) { + double startX = py::cast(tunnel["startX"]); + double startY = py::cast(tunnel["startY"]); + double endX = py::cast(tunnel["endX"]); + double endY = py::cast(tunnel["endY"]); + bool isOpen = tunnel.contains("isOpen") ? py::cast(tunnel["isOpen"]) : true; + + cppTunnels.emplace_back(startX, startY, endX, endY, isOpen); + } + + // Handle optional start point + TSPPoint* pStartPoint = nullptr; + TSPPoint startPointObj(0, 0); + if (!routeStartPoint.is_none()) { + try { + auto sp = routeStartPoint.cast>(); + if (sp.size() >= 2) { + startPointObj.x = sp[0]; + startPointObj.y = sp[1]; + pStartPoint = &startPointObj; + } + } + catch (py::cast_error&) { + try { + if (py::len(routeStartPoint) >= 2) { + startPointObj.x = py::cast(routeStartPoint.attr("__getitem__")(0)); + startPointObj.y = py::cast(routeStartPoint.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 (!routeEndPoint.is_none()) { + try { + auto ep = routeEndPoint.cast>(); + if (ep.size() >= 2) { + endPointObj.x = ep[0]; + endPointObj.y = ep[1]; + pEndPoint = &endPointObj; + } + } + catch (py::cast_error&) { + try { + if (py::len(routeEndPoint) >= 2) { + endPointObj.x = py::cast(routeEndPoint.attr("__getitem__")(0)); + endPointObj.y = py::cast(routeEndPoint.attr("__getitem__")(1)); + pEndPoint = &endPointObj; + } + } + catch (py::error_already_set&) { + // Ignore if we can't access the elements + } + } + } + + // Solve the tunnel TSP + auto result = TSPSolver::solveTunnels(cppTunnels, allowFlipping, pStartPoint, pEndPoint); + + // Convert result back to Python dictionaries + std::vector pyResult; + for (const auto& tunnel : result) { + py::dict tunnelDict; + tunnelDict["startX"] = tunnel.startX; + tunnelDict["startY"] = tunnel.startY; + tunnelDict["endX"] = tunnel.endX; + tunnelDict["endY"] = tunnel.endY; + tunnelDict["isOpen"] = tunnel.isOpen; + tunnelDict["flipped"] = tunnel.flipped; + tunnelDict["originalIdx"] = tunnel.originalIdx; + pyResult.push_back(tunnelDict); + } + + return pyResult; +} + PYBIND11_MODULE(tsp_solver, m) { m.doc() = "Simple TSP solver (2-Opt) for FreeCAD"; - m.def("solve", - &tspSolvePy, - py::arg("points"), - 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)"); + + m.def( + "solve", + &tspSolvePy, + py::arg("points"), + 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)" + ); + + m.def( + "solveTunnels", + &tspSolveTunnelsPy, + py::arg("tunnels"), + py::arg("allowFlipping") = false, + py::arg("routeStartPoint") = py::none(), + py::arg("routeEndPoint") = py::none(), + "Solve TSP for tunnels (path segments with entry/exit points).\n" + "Arguments:\n" + "- tunnels: List of dictionaries with keys: startX, startY, endX, endY, isOpen (optional)\n" + "- allowFlipping: Whether tunnels can be reversed (entry becomes exit)\n" + "- routeStartPoint: Optional [x, y] point where route should start\n" + "- routeEndPoint: Optional [x, y] point where route should end\n" + "Returns: List of tunnel dictionaries in optimized order with flipped status" + ); } diff --git a/src/Mod/CAM/CAMTests/TestTSPSolver.py b/src/Mod/CAM/CAMTests/TestTSPSolver.py index 1eaaa35cc0..9a169f5574 100644 --- a/src/Mod/CAM/CAMTests/TestTSPSolver.py +++ b/src/Mod/CAM/CAMTests/TestTSPSolver.py @@ -1,3 +1,5 @@ +# SPDX-License-Identifier: LGPL-2.1-or-later + # *************************************************************************** # * Copyright (c) 2025 Billy Huddleston * # * * From 095cdb14a773c1461c60f25435c2f941d3f6de4f Mon Sep 17 00:00:00 2001 From: Billy Huddleston Date: Sun, 14 Dec 2025 15:51:16 -0500 Subject: [PATCH 5/5] CAM: Add comprehensive TSP tunnel solver tests and wrapper function Added extensive test coverage for the TSP tunnel solver including linear tunnels, pentagram diagonals, and complex wire geometries with various constraint combinations. Introduced a Python wrapper function for tunnel sorting that integrates with the C++ solver. src/Mod/CAM/CAMTests/TestTSPSolver.py: - Renumbered existing tests to run in sequential order (test_01_simple_tsp, test_02_start_point, etc.) - Added print_tunnels() helper function for displaying tunnel information with original indices - Added test_06_tunnels_tsp: Tests 7 linear tunnels with varying lengths, connectivity, and flipping behavior - Added test_07_pentagram_tunnels_tsp: Tests pentagram diagonal tunnels with no constraints, start+end constraints, and start-only constraints - Added test_08_open_wire_end_only: Tests end-only constraint on complex wire with 8 tunnels including crossings and diagonals src/Mod/CAM/PathScripts/PathUtils.py: - Added sort_tunnels_tsp() wrapper function that interfaces with the C++ tsp_solver.solveTunnels() - Supports allowFlipping, routeStartPoint, and routeEndPoint parameters --- src/Mod/CAM/App/tsp_solver.cpp | 2 +- src/Mod/CAM/App/tsp_solver.h | 8 +- src/Mod/CAM/App/tsp_solver_pybind.cpp | 2 +- src/Mod/CAM/CAMTests/TestTSPSolver.py | 241 +++++++++++++++++++++++++- src/Mod/CAM/PathScripts/PathUtils.py | 30 ++++ 5 files changed, 272 insertions(+), 11 deletions(-) diff --git a/src/Mod/CAM/App/tsp_solver.cpp b/src/Mod/CAM/App/tsp_solver.cpp index 5c0dc3ad2d..480014cecd 100644 --- a/src/Mod/CAM/App/tsp_solver.cpp +++ b/src/Mod/CAM/App/tsp_solver.cpp @@ -329,7 +329,7 @@ std::vector TSPSolver::solveTunnels( // Set original indices for (size_t i = 0; i < tunnels.size(); ++i) { - tunnels[i].originalIdx = static_cast(i); + tunnels[i].index = static_cast(i); } // STEP 1: Add the routeStartPoint (will be deleted at the end) diff --git a/src/Mod/CAM/App/tsp_solver.h b/src/Mod/CAM/App/tsp_solver.h index 416b68ed9f..ff1d2f91ce 100644 --- a/src/Mod/CAM/App/tsp_solver.h +++ b/src/Mod/CAM/App/tsp_solver.h @@ -40,9 +40,9 @@ struct TSPTunnel { double startX, startY; double endX, endY; - bool isOpen; // Whether the tunnel can be flipped (entry/exit can be swapped) - bool flipped; // Tracks if tunnel has been flipped from original orientation - int originalIdx; // Original index in input array + bool isOpen; // Whether the tunnel can be flipped (entry/exit can be swapped) + bool flipped; // Tracks if tunnel has been flipped from original orientation + int index; // Original index in input array TSPTunnel(double sx, double sy, double ex, double ey, bool open = true) : startX(sx) @@ -51,7 +51,7 @@ struct TSPTunnel , endY(ey) , isOpen(open) , flipped(false) - , originalIdx(-1) + , index(-1) {} }; diff --git a/src/Mod/CAM/App/tsp_solver_pybind.cpp b/src/Mod/CAM/App/tsp_solver_pybind.cpp index e5074f9a5d..021bc1ad3c 100644 --- a/src/Mod/CAM/App/tsp_solver_pybind.cpp +++ b/src/Mod/CAM/App/tsp_solver_pybind.cpp @@ -183,7 +183,7 @@ std::vector tspSolveTunnelsPy( tunnelDict["endY"] = tunnel.endY; tunnelDict["isOpen"] = tunnel.isOpen; tunnelDict["flipped"] = tunnel.flipped; - tunnelDict["originalIdx"] = tunnel.originalIdx; + tunnelDict["index"] = tunnel.index; pyResult.push_back(tunnelDict); } diff --git a/src/Mod/CAM/CAMTests/TestTSPSolver.py b/src/Mod/CAM/CAMTests/TestTSPSolver.py index 9a169f5574..854b790365 100644 --- a/src/Mod/CAM/CAMTests/TestTSPSolver.py +++ b/src/Mod/CAM/CAMTests/TestTSPSolver.py @@ -52,7 +52,17 @@ class TestTSPSolver(PathTestBase): # 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): + def print_tunnels(self, tunnels, title): + """Helper function to print tunnel information.""" + print(f"\n{title}:") + for i, tunnel in enumerate(tunnels): + orig_idx = tunnel.get("index", "N/A") + flipped_str = f" flipped={tunnel.get('flipped', 'N/A')}" if "flipped" in tunnel else "" + print( + f" {i} (orig {orig_idx}): ({tunnel['startX']:.2f},{tunnel['startY']:.2f}) -> ({tunnel['endX']:.2f},{tunnel['endY']:.2f}){flipped_str}" + ) + + def test_01_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) @@ -73,7 +83,7 @@ class TestTSPSolver(PathTestBase): # Allow for small numerical errors self.assertRoughly(total_distance, 30.0, 0.001) - def test_start_point(self): + def test_02_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] @@ -84,7 +94,7 @@ class TestTSPSolver(PathTestBase): closest_pt_idx = 3 self.assertEqual(route[0], closest_pt_idx) - def test_end_point(self): + def test_03_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] @@ -94,7 +104,7 @@ class TestTSPSolver(PathTestBase): closest_pt_idx = 4 self.assertEqual(route[-1], closest_pt_idx) - def test_start_end_points(self): + def test_04_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) @@ -104,7 +114,7 @@ class TestTSPSolver(PathTestBase): 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): + def test_05_path_utils_integration(self): """Test integration with PathUtils.sort_locations_tsp.""" keys = ["x", "y"] start_point = [0, 0] @@ -123,6 +133,227 @@ class TestTSPSolver(PathTestBase): self.assertRoughly(sorted_locations[-1]["x"], 9, 0.001) self.assertRoughly(sorted_locations[-1]["y"], 8, 0.001) + def test_06_tunnels_tsp(self): + """Test TSP solver for tunnels with varying lengths, connections, and flipping.""" + # Create 7 tunnels with varying lengths and connectivity + tunnels = [ + {"startX": 0, "startY": 0, "endX": 5, "endY": 0}, # Short horizontal, idx 0 + { + "startX": 5, + "startY": 0, + "endX": 15, + "endY": 0, + }, # Long horizontal, connects to 0, idx 1 + { + "startX": 20, + "startY": 5, + "endX": 25, + "endY": 5, + }, # Short horizontal, doesn't connect, idx 2 + { + "startX": 15, + "startY": 0, + "endX": 20, + "endY": 0, + }, # Medium horizontal, connects to 1, idx 3 + { + "startX": 30, + "startY": 10, + "endX": 35, + "endY": 10, + }, # Short horizontal, doesn't connect, idx 4 + { + "startX": 25, + "startY": 5, + "endX": 30, + "endY": 5, + }, # Medium horizontal, connects to 2, idx 5 + { + "startX": 40, + "startY": 15, + "endX": 50, + "endY": 15, + }, # Long horizontal, doesn't connect, idx 6 + ] + + self.print_tunnels(tunnels, "Input tunnels") + + # Test without flipping + sorted_tunnels_no_flip = PathUtils.sort_tunnels_tsp(tunnels, allowFlipping=False) + self.print_tunnels(sorted_tunnels_no_flip, "Sorted tunnels (no flipping)") + + self.assertEqual(len(sorted_tunnels_no_flip), 7) + # All should have flipped=False + for tunnel in sorted_tunnels_no_flip: + self.assertFalse(tunnel["flipped"]) + + # Test with flipping allowed + sorted_tunnels_with_flip = PathUtils.sort_tunnels_tsp(tunnels, allowFlipping=True) + self.print_tunnels(sorted_tunnels_with_flip, "Sorted tunnels (flipping allowed)") + + self.assertEqual(len(sorted_tunnels_with_flip), 7) + # Check flipped status (may or may not flip depending on optimization) + flipped_count = sum(1 for tunnel in sorted_tunnels_with_flip if tunnel["flipped"]) + # Note: flipping may or may not occur depending on the specific optimization + + # Verify that flipped tunnels have swapped coordinates + for tunnel in sorted_tunnels_with_flip: + self.assertIn("flipped", tunnel) + self.assertIn("index", tunnel) + # Coordinates are already updated by C++ solver if flipped + + def test_07_pentagram_tunnels_tsp(self): + """Test TSP solver for pentagram tunnels with diagonals.""" + # Create pentagram points (scaled for readability) + scale = 10 + pentagram_points = [ + (0 * scale, 1 * scale), # Point 0 - top + (0.951 * scale, 0.309 * scale), # Point 1 - top right + (0.588 * scale, -0.809 * scale), # Point 2 - bottom right + (-0.588 * scale, -0.809 * scale), # Point 3 - bottom left + (-0.951 * scale, 0.309 * scale), # Point 4 - top left + ] + + # Create diagonal tunnels (the crossing lines of the pentagram) + tunnels = [ + { + "startX": pentagram_points[0][0], + "startY": pentagram_points[0][1], + "endX": pentagram_points[2][0], + "endY": pentagram_points[2][1], + }, # 0 -> 2 + { + "startX": pentagram_points[0][0], + "startY": pentagram_points[0][1], + "endX": pentagram_points[3][0], + "endY": pentagram_points[3][1], + }, # 0 -> 3 + { + "startX": pentagram_points[1][0], + "startY": pentagram_points[1][1], + "endX": pentagram_points[3][0], + "endY": pentagram_points[3][1], + }, # 1 -> 3 + { + "startX": pentagram_points[1][0], + "startY": pentagram_points[1][1], + "endX": pentagram_points[4][0], + "endY": pentagram_points[4][1], + }, # 1 -> 4 + { + "startX": pentagram_points[2][0], + "startY": pentagram_points[2][1], + "endX": pentagram_points[4][0], + "endY": pentagram_points[4][1], + }, # 2 -> 4 + ] + + # Test 1: No start/end constraints + print("\n=== Pentagram Test: No start/end constraints ===") + self.print_tunnels(tunnels, "Input pentagram tunnels") + + sorted_no_constraints = PathUtils.sort_tunnels_tsp(tunnels, allowFlipping=True) + self.print_tunnels(sorted_no_constraints, "Sorted (no constraints)") + self.assertEqual(len(sorted_no_constraints), 5) + + # Test 2: With start and end points + start_point = [pentagram_points[0][0], pentagram_points[0][1]] # Start at point 0 + end_point = [pentagram_points[2][0], pentagram_points[2][1]] # End at point 2 + + print(f"\n=== Pentagram Test: Start at {start_point}, End at {end_point} ===") + sorted_with_start_end = PathUtils.sort_tunnels_tsp( + tunnels, + allowFlipping=True, + routeStartPoint=start_point, + routeEndPoint=end_point, + ) + self.print_tunnels(sorted_with_start_end, "Sorted (start+end constraints)") + self.assertEqual(len(sorted_with_start_end), 5) + + # Test 3: With just start point + print(f"\n=== Pentagram Test: Start at {start_point}, no end constraint ===") + sorted_with_start_only = PathUtils.sort_tunnels_tsp( + tunnels, allowFlipping=True, routeStartPoint=start_point + ) + self.print_tunnels(sorted_with_start_only, "Sorted (start only constraint)") + self.assertEqual(len(sorted_with_start_only), 5) + + def test_08_open_wire_end_only(self): + """Test TSP solver for tunnels with end-only constraint on a complex wire with crossings and diagonals.""" + # Create a complex wire with 6 points in random positions and multiple crossings + points = [ + (0, 0), # Point 0 + (15, 5), # Point 1 + (30, -5), # Point 2 + (10, -10), # Point 3 + (25, 10), # Point 4 + (5, 15), # Point 5 + ] + + tunnels = [ + { + "startX": points[2][0], + "startY": points[2][1], + "endX": points[3][0], + "endY": points[3][1], + }, # 2 -> 3 + { + "startX": points[1][0], + "startY": points[1][1], + "endX": points[2][0], + "endY": points[2][1], + }, # 1 -> 2 + { + "startX": points[3][0], + "startY": points[3][1], + "endX": points[4][0], + "endY": points[4][1], + }, # 3 -> 4 + { + "startX": points[0][0], + "startY": points[0][1], + "endX": points[1][0], + "endY": points[1][1], + }, # 0 -> 1 + { + "startX": points[4][0], + "startY": points[4][1], + "endX": points[5][0], + "endY": points[5][1], + }, # 4 -> 5 + { + "startX": points[0][0], + "startY": points[0][1], + "endX": points[2][0], + "endY": points[2][1], + }, # 0 -> 2 (diagonal) + { + "startX": points[1][0], + "startY": points[1][1], + "endX": points[4][0], + "endY": points[4][1], + }, # 1 -> 4 (crossing) + { + "startX": points[3][0], + "startY": points[3][1], + "endX": points[5][0], + "endY": points[5][1], + }, # 3 -> 5 (diagonal) + ] + + print("\n=== Complex Wire Test: End at (25, 10), no start constraint ===") + self.print_tunnels(tunnels, "Input complex wire tunnels") + + end_point = [25.0, 10.0] # End at point 4 + sorted_tunnels = PathUtils.sort_tunnels_tsp( + tunnels, allowFlipping=False, routeEndPoint=end_point + ) + self.print_tunnels(sorted_tunnels, "Sorted (end only constraint)") + self.assertEqual(len(sorted_tunnels), 8) + + # The route should end at the specified end point + # Note: Due to current implementation limitations, this may not be enforced + if __name__ == "__main__": import unittest diff --git a/src/Mod/CAM/PathScripts/PathUtils.py b/src/Mod/CAM/PathScripts/PathUtils.py index ce5d9d60a1..cb248e7cc9 100644 --- a/src/Mod/CAM/PathScripts/PathUtils.py +++ b/src/Mod/CAM/PathScripts/PathUtils.py @@ -637,6 +637,36 @@ def sort_locations_tsp(locations, keys, attractors=None, startPoint=None, endPoi return [locations[i] for i in order] +def sort_tunnels_tsp(tunnels, allowFlipping=False, routeStartPoint=None, routeEndPoint=None): + """ + Python wrapper for the C++ TSP tunnel solver. Takes a list of dicts (tunnels), + a list of keys for start/end coordinates, and optional parameters. + + Parameters: + - tunnels: List of dictionaries with tunnel data. Each tunnel dictionary should contain: + - startX: X-coordinate of the tunnel start point + - startY: Y-coordinate of the tunnel start point + - endX: X-coordinate of the tunnel end point + - endY: Y-coordinate of the tunnel end point + - isOpen: Boolean indicating if the tunnel is open (optional, defaults to True) + - allowFlipping: Whether tunnels can be reversed (entry becomes exit) + - routeStartPoint: Optional starting point [x, y] for the entire route + - routeEndPoint: Optional ending point [x, y] for the entire route + + Returns the sorted list of tunnels in TSP order. Each returned tunnel dictionary + will include the original keys plus: + - flipped: Boolean indicating if the tunnel was reversed during optimization + - index: Original index of the tunnel in the input list + """ + # Call C++ TSP tunnel solver directly - it handles all the processing + return tsp_solver.solveTunnels( + tunnels=tunnels, + allowFlipping=allowFlipping, + routeStartPoint=routeStartPoint, + routeEndPoint=routeEndPoint, + ) + + def guessDepths(objshape, subs=None): """ takes an object shape and optional list of subobjects and returns a depth_params