kindred/create
1
1
Fork 0
Code Issues 53 Pull Requests Actions Packages Projects 9 Releases 2 Wiki Activity

Merge pull request #26205 from Connor9220/PreserveTSPTunnelExtraData

Browse Source 
CAM: Update TSP tunnel solver
This commit is contained in:
sliptonic
2025-12-16 09:31:14 -06:00
committed by GitHub
parent b7735044c4 646b0f4492
commit a13502dccb
3 changed files with 278 additions and 70 deletions
Download Patch File Download Diff File Expand all files Collapse all files

251
src/Mod/CAM/App/tsp_solver.cpp
View File

@@ -200,7 +200,7 @@ std::vector<int> 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<int> 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<int> 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<TSPTunnel> 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<TSPTunnel> 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<TSPTunnel> 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<TSPTunnel> 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<TSPTunnel> 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<TSPTunnel> 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<TSPTunnel> 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
}
}

13
src/Mod/CAM/App/tsp_solver_pybind.cpp
View File

@@ -108,14 +108,17 @@ std::vector<py::dict> tspSolveTunnelsPy(
std::vector<TSPTunnel> 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<double>(tunnel["startX"]);
double startY = py::cast<double>(tunnel["startY"]);
double endX = py::cast<double>(tunnel["endX"]);
double endY = py::cast<double>(tunnel["endY"]);
bool isOpen = tunnel.contains("isOpen") ? py::cast<bool>(tunnel["isOpen"]) : true;
cppTunnels.emplace_back(startX, startY, endX, endY, isOpen);
TSPTunnel cppTunnel(startX, startY, endX, endY, isOpen);
cppTunnel.index = static_cast<int>(i);
cppTunnels.emplace_back(cppTunnel);
}
// Handle optional start point
@@ -173,10 +176,12 @@ std::vector<py::dict> 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<py::dict> 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;

84
src/Mod/CAM/CAMTests/TestTSPSolver.py
View File

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