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 } } 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