kindred/create
1
1
Fork 0
Code Issues 37 Pull Requests Actions Packages Projects 9 Releases 2 Wiki Activity
Files
b6607a5472d65b1027826435e97b4f52d80c2b5f
create/src/Mod/CAM/App/tsp_solver.cpp
Billy Huddleston b6607a5472 CAM: Add TSP tunnel solver with flipping and Python bindings 
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
2025-12-13 10:59:47 -05:00

587 lines
24 KiB
C++
Raw Blame History

// SPDX-License-Identifier: LGPL-2.1-or-later
/***************************************************************************
* Copyright (c) 2025 Billy Huddleston <billy@ivdc.com> *
* *
* This program is free software; you can redistribute it and/or modify *
* it under the terms of the GNU Lesser General Public License (LGPL) *
* as published by the Free Software Foundation; either version 2 of *
* the License, or (at your option) any later version. *
* for detail see the LICENCE text file. *
* *
* This program is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU Library General Public License for more details. *
* *
* You should have received a copy of the GNU Library General Public *
* License along with this program; if not, write to the Free Software *
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 *
* USA *
* *
***************************************************************************/
#include "tsp_solver.h"
#include <vector>
#include <limits>
#include <cmath>
#include <algorithm>
#include <Base/Precision.h>
namespace
{
/**
* @brief Calculate Euclidean distance between two points
*
* Used for 2-opt and relocation steps where actual distance matters for path length optimization.
*
* @param a First point
* @param b Second point
* @return Actual distance: sqrt(dx² + dy²)
*/
double dist(const TSPPoint& a, const TSPPoint& b)
{
double dx = a.x - b.x;
double dy = a.y - b.y;
return std::sqrt(dx * dx + dy * dy);
}
/**
* @brief Calculate squared distance between two points (no sqrt)
*
* Used for nearest neighbor selection for performance (avoids expensive sqrt operation).
* Since we only need to compare distances, squared distance preserves ordering:
* if dist(A,B) < dist(A,C), then distSquared(A,B) < distSquared(A,C)
*
* @param a First point
* @param b Second point
* @return Squared distance: dx² + dy²
*/
double distSquared(const TSPPoint& a, const TSPPoint& b)
{
double dx = a.x - b.x;
double dy = a.y - b.y;
return dx * dx + dy * dy;
}
/**
* @brief Core TSP solver implementation using nearest neighbor + iterative improvement
*
* Algorithm steps:
* 1. Add temporary start/end points if specified
* 2. Build initial route using nearest neighbor heuristic
* 3. Optimize route with 2-opt and relocation moves
* 4. Remove temporary points and map back to original indices
*
* @param points Input points to visit
* @param startPoint Optional starting location constraint
* @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
)
{
// ========================================================================
// STEP 1: Prepare point set with temporary start/end markers
// ========================================================================
// We insert temporary points to enforce start/end constraints.
// These will be removed after optimization and won't appear in final result.
std::vector<TSPPoint> pts = points;
int tempStartIdx = -1, tempEndIdx = -1;
if (startPoint) {
// Insert user-specified start point at beginning
pts.insert(pts.begin(), TSPPoint(startPoint->x, startPoint->y));
tempStartIdx = 0;
}
else if (!pts.empty()) {
// No start specified: duplicate first point as anchor
pts.insert(pts.begin(), TSPPoint(pts[0].x, pts[0].y));
tempStartIdx = 0;
}
if (endPoint) {
// Add user-specified end point at the end
pts.push_back(TSPPoint(endPoint->x, endPoint->y));
tempEndIdx = static_cast<int>(pts.size()) - 1;
}
// ========================================================================
// STEP 2: Build initial route using Nearest Neighbor algorithm
// ========================================================================
// Greedy approach: always visit the closest unvisited point next.
// This gives a decent initial solution quickly (O(n²) complexity).
//
// Tie-breaking rule:
// - If distances are within ±0.1, prefer point with y-value closer to start
// - This provides deterministic results when points are nearly equidistant
std::vector<int> route;
std::vector<bool> visited(pts.size(), false);
route.push_back(0); // Start from temp start point (index 0)
visited[0] = true;
for (size_t step = 1; step < pts.size(); ++step) {
double minDist = std::numeric_limits<double>::max();
int next = -1;
double nextYDiff = std::numeric_limits<double>::max();
// Find nearest unvisited neighbor
for (size_t i = 0; i < pts.size(); ++i) {
if (!visited[i]) {
// Use squared distance for speed (no sqrt needed for comparison)
double d = distSquared(pts[route.back()], pts[i]);
double yDiff = std::abs(pts[route.front()].y - pts[i].y);
// Tie-breaking logic:
if (d > minDist + 0.1) {
continue; // Clearly farther, skip
}
else if (d < minDist - 0.1) {
// Clearly closer, use it
minDist = d;
next = static_cast<int>(i);
nextYDiff = yDiff;
}
else if (yDiff < nextYDiff) {
// Tie: prefer point closer to start in Y-axis
minDist = d;
next = static_cast<int>(i);
nextYDiff = yDiff;
}
}
}
if (next == -1) {
break; // No more unvisited points
}
route.push_back(next);
visited[next] = true;
}
// Ensure temporary end point is at the end of route
if (tempEndIdx != -1 && route.back() != tempEndIdx) {
auto it = std::find(route.begin(), route.end(), tempEndIdx);
if (it != route.end()) {
route.erase(it);
}
route.push_back(tempEndIdx);
}
// ========================================================================
// STEP 3: Iterative improvement using 2-Opt and Relocation
// ========================================================================
// Repeatedly apply local optimizations until no improvement is possible.
// This typically converges quickly (a few iterations) to a near-optimal solution.
//
// Two optimization techniques:
// 1. 2-Opt: Reverse segments of the route to eliminate crossing paths
// 2. Relocation: Move individual points to better positions in the route
bool improvementFound = true;
while (improvementFound) {
improvementFound = false;
// --- 2-Opt Optimization ---
// Try reversing every possible segment of the route.
// If reversing segment [i+1...j-1] reduces total distance, keep it.
//
// Example: Route A-B-C-D-E becomes A-D-C-B-E if reversing B-C-D is better
bool reorderFound = true;
while (reorderFound) {
reorderFound = false;
for (size_t i = 0; i + 3 < route.size(); ++i) {
for (size_t j = i + 3; j < route.size(); ++j) {
// Current edges: i→(i+1) and (j-1)→j
double curLen = dist(pts[route[i]], pts[route[i + 1]])
+ dist(pts[route[j - 1]], pts[route[j]]);
// 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;
if (newLen < curLen) {
// Reverse the segment between i+1 and j (exclusive)
std::reverse(route.begin() + i + 1, route.begin() + j);
reorderFound = true;
improvementFound = true;
}
}
}
}
// --- Relocation Optimization ---
// Try moving each point to a different position in the route.
// If moving point i to position j improves the route, do it.
bool relocateFound = true;
while (relocateFound) {
relocateFound = false;
for (size_t i = 1; i + 1 < route.size(); ++i) {
// Try moving point i backward (to positions before i)
for (size_t j = 1; j + 2 < i; ++j) {
// Current cost: edges around point i and edge j→(j+1)
double curLen = dist(pts[route[i - 1]], pts[route[i]])
+ dist(pts[route[i]], pts[route[i + 1]])
+ dist(pts[route[j]], pts[route[j + 1]]);
// 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;
if (newLen < curLen) {
// Move point i to position after j
int node = route[i];
route.erase(route.begin() + i);
route.insert(route.begin() + j + 1, node);
relocateFound = true;
improvementFound = true;
}
}
// Try moving point i forward (to positions after i)
for (size_t j = i + 1; j + 1 < route.size(); ++j) {
double curLen = dist(pts[route[i - 1]], pts[route[i]])
+ dist(pts[route[i]], pts[route[i + 1]])
+ dist(pts[route[j]], pts[route[j + 1]]);
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;
if (newLen < curLen) {
int node = route[i];
route.erase(route.begin() + i);
route.insert(route.begin() + j, node);
relocateFound = true;
improvementFound = true;
}
}
}
}
}
// ========================================================================
// STEP 4: Remove temporary start/end points
// ========================================================================
// The temporary markers served their purpose during optimization.
// Now remove them so they don't appear in the final result.
if (tempEndIdx != -1 && !route.empty() && route.back() == tempEndIdx) {
route.pop_back();
}
if (tempStartIdx != -1 && !route.empty() && route.front() == tempStartIdx) {
route.erase(route.begin());
}
// ========================================================================
// STEP 5: Map route indices back to original point array
// ========================================================================
// Since we inserted a temp start point at index 0, all subsequent indices
// are offset by 1. Adjust them back to match the original points array.
std::vector<int> result;
for (int idx : route) {
// Adjust for temp start offset
if (tempStartIdx != -1) {
--idx;
}
// Only include valid indices from the original points array
if (idx >= 0 && idx < static_cast<int>(points.size())) {
result.push_back(idx);
}
}
return result;
}
} // namespace
/**
* @brief Solve the Traveling Salesperson Problem using 2-opt algorithm
*
* This implementation handles optional start and end point constraints:
* - If startPoint is provided, the path will begin at the point closest to startPoint
* - If endPoint is provided, the path will end at the point closest to endPoint
* - If both are provided, the path will respect both constraints while optimizing the middle path
* - The algorithm ensures all points are visited exactly once
*/
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;
}
Reference in New Issue View Git Blame Copy Permalink