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

CAM: Add TSP tunnel solver with flipping and Python bindings

Browse Source 
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
This commit is contained in:
Billy Huddleston
2025-10-22 10:46:11 -04:00
parent 6d26c3009f
commit b6607a5472
4 changed files with 449 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1,3 +1,5 @@
// SPDX-License-Identifier: LGPL-2.1-or-later
/***************************************************************************
* Copyright (c) 2025 Billy Huddleston <billy@ivdc.com> *
* *
@@ -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<int> solve_impl(const std::vector<TSPPoint>& points,
const TSPPoint* startPoint,
const TSPPoint* endPoint)
std::vector<int> solve_impl(
const std::vector<TSPPoint>& points,
const TSPPoint* startPoint,
const TSPPoint* endPoint
)
{
// ========================================================================
// STEP 1: Prepare point set with temporary start/end markers
@@ -303,9 +307,280 @@ std::vector<int> solve_impl(const std::vector<TSPPoint>& points,
*/
std::vector<int> TSPSolver::solve(const std::vector<TSPPoint>& points,
const TSPPoint* startPoint,
const TSPPoint* endPoint)
std::vector<int> TSPSolver::solve(
const std::vector<TSPPoint>& points,
const TSPPoint* startPoint,
const TSPPoint* endPoint
)
{
return solve_impl(points, startPoint, endPoint);
}
std::vector<TSPTunnel> TSPSolver::solveTunnels(
std::vector<TSPTunnel> 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<int>(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<TSPTunnel> potentialNeighbours(tunnels.begin() + 1, tunnels.end());
std::vector<TSPTunnel> route;
route.push_back(tunnels[0]);
while (!potentialNeighbours.empty()) {
double costCurrent = std::numeric_limits<double>::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;
}

39
src/Mod/CAM/App/tsp_solver.h
View File

@@ -1,3 +1,5 @@
// SPDX-License-Identifier: LGPL-2.1-or-later
/***************************************************************************
* Copyright (c) 2025 Billy Huddleston <billy@ivdc.com> *
* *
@@ -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<int> solve(const std::vector<TSPPoint>& points,
const TSPPoint* startPoint = nullptr,
const TSPPoint* endPoint = nullptr);
static std::vector<int> solve(
const std::vector<TSPPoint>& 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<TSPTunnel> solveTunnels(
std::vector<TSPTunnel> tunnels,
bool allowFlipping = false,
const TSPPoint* routeStartPoint = nullptr,
const TSPPoint* routeEndPoint = nullptr
);
};

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

@@ -1,3 +1,5 @@
// SPDX-License-Identifier: LGPL-2.1-or-later
/***************************************************************************
* Copyright (c) 2025 Billy Huddleston <billy@ivdc.com> *
* *
@@ -25,9 +27,11 @@
namespace py = pybind11;
std::vector<int> tspSolvePy(const std::vector<std::pair<double, double>>& points,
const py::object& startPoint = py::none(),
const py::object& endPoint = py::none())
std::vector<int> tspSolvePy(
const std::vector<std::pair<double, double>>& points,
const py::object& startPoint = py::none(),
const py::object& endPoint = py::none()
)
{
std::vector<TSPPoint> pts;
for (const auto& p : points) {
@@ -93,18 +97,130 @@ std::vector<int> tspSolvePy(const std::vector<std::pair<double, double>>& points
return TSPSolver::solve(pts, pStartPoint, pEndPoint);
}
// Python wrapper for solveTunnels function
std::vector<py::dict> tspSolveTunnelsPy(
const std::vector<py::dict>& tunnels,
bool allowFlipping = false,
const py::object& routeStartPoint = py::none(),
const py::object& routeEndPoint = py::none()
)
{
std::vector<TSPTunnel> cppTunnels;
// Convert Python dictionaries to C++ TSPTunnel objects
for (const auto& tunnel : tunnels) {
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);
}
// Handle optional start point
TSPPoint* pStartPoint = nullptr;
TSPPoint startPointObj(0, 0);
if (!routeStartPoint.is_none()) {
try {
auto sp = routeStartPoint.cast<std::vector<double>>();
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<double>(routeStartPoint.attr("__getitem__")(0));
startPointObj.y = py::cast<double>(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<std::vector<double>>();
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<double>(routeEndPoint.attr("__getitem__")(0));
endPointObj.y = py::cast<double>(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<py::dict> 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"
);
}

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

@@ -1,3 +1,5 @@
# SPDX-License-Identifier: LGPL-2.1-or-later
# ***************************************************************************
# * Copyright (c) 2025 Billy Huddleston <billy@ivdc.com> *
# * *