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 * # * *