From 04df03cea131fe7a44a14e14f38fb3db7ae273fe Mon Sep 17 00:00:00 2001 From: Billy Huddleston Date: Mon, 15 Dec 2025 16:32:03 -0500 Subject: [PATCH 1/2] CAM: Preserve extra tunnel data through TSP solver Ensures that all extra keys in tunnel dictionaries are preserved after TSP solving by copying the original input dict and updating solver results. Adds a dedicated test to verify passthrough of extra data and prints extra keys for debugging. src/Mod/CAM/App/tsp_solver_pybind.cpp: - Copy original tunnel dict and update with solver results to preserve extra keys src/Mod/CAM/CAMTests/TestTSPSolver.py: - Add test_09_tunnels_extra_data_passthrough to verify extra data preservation - Print extra tunnel data in print_tunnels for easier debugging --- src/Mod/CAM/App/tsp_solver_pybind.cpp | 13 +++-- src/Mod/CAM/CAMTests/TestTSPSolver.py | 84 +++++++++++++++++++++++++++ 2 files changed, 93 insertions(+), 4 deletions(-) diff --git a/src/Mod/CAM/App/tsp_solver_pybind.cpp b/src/Mod/CAM/App/tsp_solver_pybind.cpp index 021bc1ad3c..24a2ddac95 100644 --- a/src/Mod/CAM/App/tsp_solver_pybind.cpp +++ b/src/Mod/CAM/App/tsp_solver_pybind.cpp @@ -108,14 +108,17 @@ std::vector tspSolveTunnelsPy( std::vector cppTunnels; // Convert Python dictionaries to C++ TSPTunnel objects - for (const auto& tunnel : tunnels) { + for (size_t i = 0; i < tunnels.size(); ++i) { + const auto& tunnel = tunnels[i]; 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); + TSPTunnel cppTunnel(startX, startY, endX, endY, isOpen); + cppTunnel.index = static_cast(i); + cppTunnels.emplace_back(cppTunnel); } // Handle optional start point @@ -173,10 +176,12 @@ std::vector tspSolveTunnelsPy( // Solve the tunnel TSP auto result = TSPSolver::solveTunnels(cppTunnels, allowFlipping, pStartPoint, pEndPoint); - // Convert result back to Python dictionaries + // Convert result back to Python dictionaries, preserving extra keys from input std::vector pyResult; for (const auto& tunnel : result) { - py::dict tunnelDict; + // Start with a copy of the original input dict to preserve extra keys + py::dict tunnelDict = py::dict(tunnels[tunnel.index]); + // Update with solver results (may have changed due to flipping) tunnelDict["startX"] = tunnel.startX; tunnelDict["startY"] = tunnel.startY; tunnelDict["endX"] = tunnel.endX; diff --git a/src/Mod/CAM/CAMTests/TestTSPSolver.py b/src/Mod/CAM/CAMTests/TestTSPSolver.py index 854b790365..ca908fd363 100644 --- a/src/Mod/CAM/CAMTests/TestTSPSolver.py +++ b/src/Mod/CAM/CAMTests/TestTSPSolver.py @@ -62,6 +62,13 @@ class TestTSPSolver(PathTestBase): f" {i} (orig {orig_idx}): ({tunnel['startX']:.2f},{tunnel['startY']:.2f}) -> ({tunnel['endX']:.2f},{tunnel['endY']:.2f}){flipped_str}" ) + # Print extra data if present + standard_keys = {"startX", "startY", "endX", "endY", "isOpen", "flipped", "index"} + extra_keys = [k for k in tunnel.keys() if k not in standard_keys] + if extra_keys: + extra_data = {k: tunnel[k] for k in extra_keys} + print(f" Extra data: {extra_data}") + def test_01_simple_tsp(self): """Test TSP solver with a simple square of points.""" # Test the TSP solver on a simple square @@ -354,6 +361,83 @@ class TestTSPSolver(PathTestBase): # The route should end at the specified end point # Note: Due to current implementation limitations, this may not be enforced + def test_09_tunnels_extra_data_passthrough(self): + """Test that extra data in tunnel dictionaries is preserved through TSP solving.""" + tunnels = [ + { + "startX": 0, + "startY": 0, + "endX": 5, + "endY": 0, + "tool": "drill_1mm", + "speed": 1000, + "feed": 500, + "custom_id": "tunnel_0", + }, + { + "startX": 20, + "startY": 5, + "endX": 25, + "endY": 5, + "tool": "drill_3mm", + "speed": 600, + "feed": 200, + "notes": "high precision", + "custom_id": "tunnel_2", + }, + { + "startX": 5, + "startY": 17, + "endX": 15, + "endY": 0, + "tool": "mill_2mm", + "speed": 800, + "feed": 300, + "material": "aluminum", + "custom_id": "tunnel_1", + }, + ] + + self.print_tunnels(tunnels, "Input tunnels with extra data") + + # Test with flipping allowed to ensure extra data survives optimization + result = PathUtils.sort_tunnels_tsp(tunnels, allowFlipping=True) + + self.print_tunnels(result, "Sorted tunnels with extra data preserved") + + # Verify all tunnels are present + self.assertEqual(len(result), 3) + + # Verify extra data is preserved for each tunnel + for tunnel in result: + # Check that solver-added keys are present + self.assertIn("startX", tunnel) + self.assertIn("startY", tunnel) + self.assertIn("endX", tunnel) + self.assertIn("endY", tunnel) + self.assertIn("isOpen", tunnel) + self.assertIn("flipped", tunnel) + self.assertIn("index", tunnel) + + # Check that extra keys are preserved + self.assertIn("tool", tunnel) + self.assertIn("speed", tunnel) + self.assertIn("feed", tunnel) + self.assertIn("custom_id", tunnel) + + # Verify specific values based on original index + original_tunnel = tunnels[tunnel["index"]] + self.assertEqual(tunnel["tool"], original_tunnel["tool"]) + self.assertEqual(tunnel["speed"], original_tunnel["speed"]) + self.assertEqual(tunnel["feed"], original_tunnel["feed"]) + self.assertEqual(tunnel["custom_id"], original_tunnel["custom_id"]) + + # Check tunnel-specific extra data + if tunnel["index"] == 2: + self.assertEqual(tunnel["material"], "aluminum") + elif tunnel["index"] == 1: + self.assertEqual(tunnel["notes"], "high precision") + if __name__ == "__main__": import unittest From 646b0f44921d414956da42bc56a1423e747f049f Mon Sep 17 00:00:00 2001 From: Billy Huddleston Date: Mon, 15 Dec 2025 17:52:55 -0500 Subject: [PATCH 2/2] CAM: Update TSP tunnel solver Update TSP tunnel solver to match revised Pythonlogic, including improved early exit, open-ended route handling, and performance tweaks. Also ensure extra data in tunnel dictionaries is preserved through the C++/Python interface and add a test for passthrough of extra keys. src/Mod/CAM/App/tsp_solver.cpp: - Refactor optimization loop to use lastImprovementAtStep and limit variables - Add special handling for open-ended routes (no end point) - Change epsilon to 10e-6 for consistency with Python - Cache distance calculations for relocation step src/Mod/CAM/App/tsp_solver_pybind.cpp: - Preserve extra keys from input tunnel dicts in output - Set tunnel index for passthrough src/Mod/CAM/CAMTests/TestTSPSolver.py: - Add test to verify extra data in tunnel dicts is preserved through TSP solver - Print extra data for debugging --- src/Mod/CAM/App/tsp_solver.cpp | 251 ++++++++++++++++++++++++--------- 1 file changed, 185 insertions(+), 66 deletions(-) diff --git a/src/Mod/CAM/App/tsp_solver.cpp b/src/Mod/CAM/App/tsp_solver.cpp index 480014cecd..9fb7961096 100644 --- a/src/Mod/CAM/App/tsp_solver.cpp +++ b/src/Mod/CAM/App/tsp_solver.cpp @@ -200,7 +200,7 @@ std::vector solve_impl( // 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; + + dist(pts[route[i]], pts[route[j - 1]]) + Base::Precision::Confusion(); if (newLen < curLen) { // Reverse the segment between i+1 and j (exclusive) @@ -230,7 +230,7 @@ std::vector solve_impl( // 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; + + dist(pts[route[i]], pts[route[j + 1]]) + Base::Precision::Confusion(); if (newLen < curLen) { // Move point i to position after j @@ -250,7 +250,7 @@ std::vector solve_impl( 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; + + dist(pts[route[i]], pts[route[j + 1]]) + Base::Precision::Confusion(); if (newLen < curLen) { int node = route[i]; @@ -402,26 +402,37 @@ std::vector TSPSolver::solveTunnels( } // STEP 4: Additional improvement of the route - bool improvementFound = true; - while (improvementFound) { - improvementFound = false; + size_t limitReorderI = route.size() - 2; + if (routeEndPoint) { + limitReorderI -= 1; + } + size_t limitReorderJ = route.size(); + size_t limitFlipI = route.size() - 1; + size_t limitRelocationI = route.size() - 1; + size_t limitRelocationJ = route.size() - 1; + int lastImprovementAtStep = 0; + + while (true) { 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) - ); + if (lastImprovementAtStep == 1) { + break; + } + bool improvementFound = true; + while (improvementFound) { + improvementFound = false; + for (size_t i = 0; i < limitReorderI; ++i) { + double subRouteLengthCurrentPart = std::sqrt( + std::pow(route[i].endX - route[i + 1].startX, 2) + + std::pow(route[i].endY - route[i + 1].startY, 2) + ); + for (size_t j = i + 3; j < limitReorderJ; ++j) { + double subRouteLengthCurrent = subRouteLengthCurrentPart; 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) @@ -430,10 +441,10 @@ std::vector TSPSolver::solveTunnels( std::pow(route[i].endX - route[j - 1].endX, 2) + std::pow(route[i].endY - route[j - 1].endY, 2) ); - subRouteLengthNew += 1e-6; + subRouteLengthNew += Base::Precision::Confusion(); if (subRouteLengthNew < subRouteLengthCurrent) { - // Flip direction of each tunnel between i-th and j-th element + // Flip direction of each tunnel between i-th and j-th tunnel for (size_t k = i + 1; k < j; ++k) { if (route[k].isOpen) { route[k].flipped = !route[k].flipped; @@ -441,20 +452,52 @@ std::vector TSPSolver::solveTunnels( std::swap(route[k].startY, route[k].endY); } } - // Reverse the order of tunnels between i-th and j-th element + // Reverse the order of tunnels between i-th and j-th tunnel std::reverse(route.begin() + i + 1, route.begin() + j); - improvementReorderFound = true; + subRouteLengthCurrentPart = std::sqrt( + std::pow(route[i].endX - route[i + 1].startX, 2) + + std::pow(route[i].endY - route[i + 1].startY, 2) + ); improvementFound = true; + lastImprovementAtStep = 1; + } + } + if (!routeEndPoint) { + double 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].endX - route[route.size() - 1].endX, 2) + + std::pow(route[i].endY - route[route.size() - 1].endY, 2) + ); + subRouteLengthNew += Base::Precision::Confusion(); + if (subRouteLengthNew < subRouteLengthCurrent) { + // Flip direction of each tunnel after i-th to the last tunnel + for (size_t k = i + 1; k < limitReorderJ; ++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 after i-th to the last tunnel + std::reverse(route.begin() + i + 1, route.begin() + limitReorderJ); + improvementFound = true; + lastImprovementAtStep = 1; } } } } // STEP 4.2: Apply flipping - bool improvementFlipFound = true; - while (improvementFlipFound) { - improvementFlipFound = false; - for (size_t i = 1; i + 1 < route.size(); ++i) { + if (lastImprovementAtStep == 2) { + break; + } + improvementFound = true; + while (improvementFound) { + improvementFound = false; + for (size_t i = 1; i < limitFlipI; ++i) { if (route[i].isOpen) { double subRouteLengthCurrent = std::sqrt( std::pow(route[i - 1].endX - route[i].startX, 2) @@ -473,15 +516,36 @@ std::vector TSPSolver::solveTunnels( std::pow(route[i].startX - route[i + 1].startX, 2) + std::pow(route[i].startY - route[i + 1].startY, 2) ); - subRouteLengthNew += 1e-6; + subRouteLengthNew += Base::Precision::Confusion(); 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; + lastImprovementAtStep = 2; + } + } + } + if (!routeEndPoint) { + if (route[route.size() - 1].isOpen) { + double subRouteLengthCurrent = std::sqrt( + std::pow(route[route.size() - 2].endX - route[route.size() - 1].startX, 2) + + std::pow(route[route.size() - 2].endY - route[route.size() - 1].startY, 2) + ); + double subRouteLengthNew = std::sqrt( + std::pow(route[route.size() - 2].endX - route[route.size() - 1].endX, 2) + + std::pow(route[route.size() - 2].endY - route[route.size() - 1].endY, 2) + ); + subRouteLengthNew += Base::Precision::Confusion(); + if (subRouteLengthNew < subRouteLengthCurrent) { + // Flip direction of the last tunnel + route[route.size() - 1].flipped = !route[route.size() - 1].flipped; + std::swap(route[route.size() - 1].startX, route[route.size() - 1].endX); + std::swap(route[route.size() - 1].startY, route[route.size() - 1].endY); + improvementFound = true; + lastImprovementAtStep = 2; } } } @@ -489,29 +553,35 @@ std::vector TSPSolver::solveTunnels( } // STEP 4.3: Apply relocation - bool improvementRelocateFound = true; - while (improvementRelocateFound) { - improvementRelocateFound = false; - for (size_t i = 1; i + 1 < route.size(); ++i) { + if (lastImprovementAtStep == 3) { + break; + } + bool improvementFound = true; + while (improvementFound) { + improvementFound = false; + for (size_t i = 1; i < limitRelocationI; ++i) { + double subRouteLengthCurrentPart = std::sqrt( + std::pow(route[i - 1].endX - route[i].startX, 2) + + std::pow(route[i - 1].endY - route[i].startY, 2) + ); + subRouteLengthCurrentPart += std::sqrt( + std::pow(route[i].endX - route[i + 1].startX, 2) + + std::pow(route[i].endY - route[i + 1].startY, 2) + ); + double subRouteLengthNewPart = std::sqrt( + std::pow(route[i - 1].endX - route[i + 1].startX, 2) + + std::pow(route[i - 1].endY - route[i + 1].startY, 2) + ); + subRouteLengthNewPart += Base::Precision::Confusion(); + // 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) - ); + for (size_t j = 0; j + 2 < i; ++j) { + double subRouteLengthCurrent = subRouteLengthCurrentPart; 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) - ); + double subRouteLengthNew = subRouteLengthNewPart; subRouteLengthNew += std::sqrt( std::pow(route[j].endX - route[i].startX, 2) + std::pow(route[j].endY - route[i].startY, 2) @@ -520,37 +590,37 @@ std::vector TSPSolver::solveTunnels( 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) + // Relocate the i-th tunnel backward (after j-th tunnel) TSPTunnel temp = route[i]; route.erase(route.begin() + i); route.insert(route.begin() + j + 1, temp); - improvementRelocateFound = true; + subRouteLengthCurrentPart = std::sqrt( + std::pow(route[i - 1].endX - route[i].startX, 2) + + std::pow(route[i - 1].endY - route[i].startY, 2) + ); + subRouteLengthCurrentPart += std::sqrt( + std::pow(route[i].endX - route[i + 1].startX, 2) + + std::pow(route[i].endY - route[i + 1].startY, 2) + ); + subRouteLengthNewPart = std::sqrt( + std::pow(route[i - 1].endX - route[i + 1].startX, 2) + + std::pow(route[i - 1].endY - route[i + 1].startY, 2) + ); + subRouteLengthNewPart += Base::Precision::Confusion(); improvementFound = true; + lastImprovementAtStep = 3; } } // 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) - ); + for (size_t j = i + 1; j < limitRelocationJ; ++j) { + double subRouteLengthCurrent = subRouteLengthCurrentPart; 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) - ); + double subRouteLengthNew = subRouteLengthNewPart; subRouteLengthNew += std::sqrt( std::pow(route[j].endX - route[i].startX, 2) + std::pow(route[j].endY - route[i].startY, 2) @@ -559,18 +629,67 @@ std::vector TSPSolver::solveTunnels( 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) + // Relocate the i-th tunnel forward (after j-th tunnel) TSPTunnel temp = route[i]; route.erase(route.begin() + i); route.insert(route.begin() + j, temp); - improvementRelocateFound = true; + subRouteLengthCurrentPart = std::sqrt( + std::pow(route[i - 1].endX - route[i].startX, 2) + + std::pow(route[i - 1].endY - route[i].startY, 2) + ); + subRouteLengthCurrentPart += std::sqrt( + std::pow(route[i].endX - route[i + 1].startX, 2) + + std::pow(route[i].endY - route[i + 1].startY, 2) + ); + subRouteLengthNewPart = std::sqrt( + std::pow(route[i - 1].endX - route[i + 1].startX, 2) + + std::pow(route[i - 1].endY - route[i + 1].startY, 2) + ); + subRouteLengthNewPart += Base::Precision::Confusion(); improvementFound = true; + lastImprovementAtStep = 3; } } } + if (!routeEndPoint) { + double subRouteLengthCurrentPart = std::sqrt( + std::pow(route[route.size() - 2].endX - route[route.size() - 1].startX, 2) + + std::pow(route[route.size() - 2].endY - route[route.size() - 1].startY, 2) + ); + for (size_t j = 0; j + 2 < route.size(); ++j) { + double subRouteLengthCurrent = subRouteLengthCurrentPart; + 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[j].endX - route[route.size() - 1].startX, 2) + + std::pow(route[j].endY - route[route.size() - 1].startY, 2) + ); + subRouteLengthNew += std::sqrt( + std::pow(route[route.size() - 1].endX - route[j + 1].startX, 2) + + std::pow(route[route.size() - 1].endY - route[j + 1].startY, 2) + ); + subRouteLengthNew += Base::Precision::Confusion(); + if (subRouteLengthNew < subRouteLengthCurrent) { + // Relocate the last tunnel after j-th tunnel + TSPTunnel temp = route[route.size() - 1]; + route.erase(route.begin() + route.size() - 1); + route.insert(route.begin() + j + 1, temp); + subRouteLengthCurrentPart = std::sqrt( + std::pow(route[route.size() - 2].endX - route[route.size() - 1].startX, 2) + + std::pow(route[route.size() - 2].endY - route[route.size() - 1].startY, 2) + ); + improvementFound = true; + lastImprovementAtStep = 3; + } + } + } + } + + if (lastImprovementAtStep == 0) { + break; // No additional improvements could be made } }