Skip to content

Commit a13502d

Browse files
sliptonicsliptonic
authored
Merge pull request FreeCAD#26205 from Connor9220/PreserveTSPTunnelExtraData
CAM: Update TSP tunnel solver
2 parents b773504 + 646b0f4 commit a13502d

File tree

3 files changed

+278
-70
lines changed

3 files changed

+278
-70
lines changed

‎src/Mod/CAM/App/tsp_solver.cpp‎

Lines changed: 185 additions & 66 deletions
Original file line numberDiff line numberDiff line change
@@ -200,7 +200,7 @@ std::vector<int> solve_impl(
200200
// New edges after reversal: (i+1)→j and i→(j-1)
201201
// Add epsilon to prevent cycles from floating point errors
202202
double newLen = dist(pts[route[i + 1]], pts[route[j]])
203-
+ dist(pts[route[i]], pts[route[j - 1]]) + 1e-5;
203+
+ dist(pts[route[i]], pts[route[j - 1]]) + Base::Precision::Confusion();
204204

205205
if (newLen < curLen) {
206206
// Reverse the segment between i+1 and j (exclusive)
@@ -230,7 +230,7 @@ std::vector<int> solve_impl(
230230
// New cost: bypass i, insert i after j
231231
double newLen = dist(pts[route[i - 1]], pts[route[i + 1]])
232232
+ dist(pts[route[j]], pts[route[i]])
233-
+ dist(pts[route[i]], pts[route[j + 1]]) + 1e-5;
233+
+ dist(pts[route[i]], pts[route[j + 1]]) + Base::Precision::Confusion();
234234

235235
if (newLen < curLen) {
236236
// Move point i to position after j
@@ -250,7 +250,7 @@ std::vector<int> solve_impl(
250250

251251
double newLen = dist(pts[route[i - 1]], pts[route[i + 1]])
252252
+ dist(pts[route[j]], pts[route[i]])
253-
+ dist(pts[route[i]], pts[route[j + 1]]) + 1e-5;
253+
+ dist(pts[route[i]], pts[route[j + 1]]) + Base::Precision::Confusion();
254254

255255
if (newLen < curLen) {
256256
int node = route[i];
@@ -402,26 +402,37 @@ std::vector<TSPTunnel> TSPSolver::solveTunnels(
402402
}
403403

404404
// STEP 4: Additional improvement of the route
405-
bool improvementFound = true;
406-
while (improvementFound) {
407-
improvementFound = false;
405+
size_t limitReorderI = route.size() - 2;
406+
if (routeEndPoint) {
407+
limitReorderI -= 1;
408+
}
409+
size_t limitReorderJ = route.size();
410+
size_t limitFlipI = route.size() - 1;
411+
size_t limitRelocationI = route.size() - 1;
412+
size_t limitRelocationJ = route.size() - 1;
413+
int lastImprovementAtStep = 0;
414+
415+
while (true) {
408416

409417
if (allowFlipping) {
410418
// STEP 4.1: Apply 2-opt
411-
bool improvementReorderFound = true;
412-
while (improvementReorderFound) {
413-
improvementReorderFound = false;
414-
for (size_t i = 0; i + 3 < route.size(); ++i) {
415-
for (size_t j = i + 3; j < route.size(); ++j) {
416-
double subRouteLengthCurrent = std::sqrt(
417-
std::pow(route[i].endX - route[i + 1].startX, 2)
418-
+ std::pow(route[i].endY - route[i + 1].startY, 2)
419-
);
419+
if (lastImprovementAtStep == 1) {
420+
break;
421+
}
422+
bool improvementFound = true;
423+
while (improvementFound) {
424+
improvementFound = false;
425+
for (size_t i = 0; i < limitReorderI; ++i) {
426+
double subRouteLengthCurrentPart = std::sqrt(
427+
std::pow(route[i].endX - route[i + 1].startX, 2)
428+
+ std::pow(route[i].endY - route[i + 1].startY, 2)
429+
);
430+
for (size_t j = i + 3; j < limitReorderJ; ++j) {
431+
double subRouteLengthCurrent = subRouteLengthCurrentPart;
420432
subRouteLengthCurrent += std::sqrt(
421433
std::pow(route[j - 1].endX - route[j].startX, 2)
422434
+ std::pow(route[j - 1].endY - route[j].startY, 2)
423435
);
424-
425436
double subRouteLengthNew = std::sqrt(
426437
std::pow(route[i + 1].startX - route[j].startX, 2)
427438
+ std::pow(route[i + 1].startY - route[j].startY, 2)
@@ -430,31 +441,63 @@ std::vector<TSPTunnel> TSPSolver::solveTunnels(
430441
std::pow(route[i].endX - route[j - 1].endX, 2)
431442
+ std::pow(route[i].endY - route[j - 1].endY, 2)
432443
);
433-
subRouteLengthNew += 1e-6;
444+
subRouteLengthNew += Base::Precision::Confusion();
434445

435446
if (subRouteLengthNew < subRouteLengthCurrent) {
436-
// Flip direction of each tunnel between i-th and j-th element
447+
// Flip direction of each tunnel between i-th and j-th tunnel
437448
for (size_t k = i + 1; k < j; ++k) {
438449
if (route[k].isOpen) {
439450
route[k].flipped = !route[k].flipped;
440451
std::swap(route[k].startX, route[k].endX);
441452
std::swap(route[k].startY, route[k].endY);
442453
}
443454
}
444-
// Reverse the order of tunnels between i-th and j-th element
455+
// Reverse the order of tunnels between i-th and j-th tunnel
445456
std::reverse(route.begin() + i + 1, route.begin() + j);
446-
improvementReorderFound = true;
457+
subRouteLengthCurrentPart = std::sqrt(
458+
std::pow(route[i].endX - route[i + 1].startX, 2)
459+
+ std::pow(route[i].endY - route[i + 1].startY, 2)
460+
);
461+
improvementFound = true;
462+
lastImprovementAtStep = 1;
463+
}
464+
}
465+
if (!routeEndPoint) {
466+
double subRouteLengthCurrent = std::sqrt(
467+
std::pow(route[i].endX - route[i + 1].startX, 2)
468+
+ std::pow(route[i].endY - route[i + 1].startY, 2)
469+
);
470+
double subRouteLengthNew = std::sqrt(
471+
std::pow(route[i].endX - route[route.size() - 1].endX, 2)
472+
+ std::pow(route[i].endY - route[route.size() - 1].endY, 2)
473+
);
474+
subRouteLengthNew += Base::Precision::Confusion();
475+
if (subRouteLengthNew < subRouteLengthCurrent) {
476+
// Flip direction of each tunnel after i-th to the last tunnel
477+
for (size_t k = i + 1; k < limitReorderJ; ++k) {
478+
if (route[k].isOpen) {
479+
route[k].flipped = !route[k].flipped;
480+
std::swap(route[k].startX, route[k].endX);
481+
std::swap(route[k].startY, route[k].endY);
482+
}
483+
}
484+
// Reverse the order of tunnels after i-th to the last tunnel
485+
std::reverse(route.begin() + i + 1, route.begin() + limitReorderJ);
447486
improvementFound = true;
487+
lastImprovementAtStep = 1;
448488
}
449489
}
450490
}
451491
}
452492

453493
// STEP 4.2: Apply flipping
454-
bool improvementFlipFound = true;
455-
while (improvementFlipFound) {
456-
improvementFlipFound = false;
457-
for (size_t i = 1; i + 1 < route.size(); ++i) {
494+
if (lastImprovementAtStep == 2) {
495+
break;
496+
}
497+
improvementFound = true;
498+
while (improvementFound) {
499+
improvementFound = false;
500+
for (size_t i = 1; i < limitFlipI; ++i) {
458501
if (route[i].isOpen) {
459502
double subRouteLengthCurrent = std::sqrt(
460503
std::pow(route[i - 1].endX - route[i].startX, 2)
@@ -473,45 +516,72 @@ std::vector<TSPTunnel> TSPSolver::solveTunnels(
473516
std::pow(route[i].startX - route[i + 1].startX, 2)
474517
+ std::pow(route[i].startY - route[i + 1].startY, 2)
475518
);
476-
subRouteLengthNew += 1e-6;
519+
subRouteLengthNew += Base::Precision::Confusion();
477520

478521
if (subRouteLengthNew < subRouteLengthCurrent) {
479522
// Flip direction of i-th tunnel
480523
route[i].flipped = !route[i].flipped;
481524
std::swap(route[i].startX, route[i].endX);
482525
std::swap(route[i].startY, route[i].endY);
483-
improvementFlipFound = true;
484526
improvementFound = true;
527+
lastImprovementAtStep = 2;
528+
}
529+
}
530+
}
531+
if (!routeEndPoint) {
532+
if (route[route.size() - 1].isOpen) {
533+
double subRouteLengthCurrent = std::sqrt(
534+
std::pow(route[route.size() - 2].endX - route[route.size() - 1].startX, 2)
535+
+ std::pow(route[route.size() - 2].endY - route[route.size() - 1].startY, 2)
536+
);
537+
double subRouteLengthNew = std::sqrt(
538+
std::pow(route[route.size() - 2].endX - route[route.size() - 1].endX, 2)
539+
+ std::pow(route[route.size() - 2].endY - route[route.size() - 1].endY, 2)
540+
);
541+
subRouteLengthNew += Base::Precision::Confusion();
542+
if (subRouteLengthNew < subRouteLengthCurrent) {
543+
// Flip direction of the last tunnel
544+
route[route.size() - 1].flipped = !route[route.size() - 1].flipped;
545+
std::swap(route[route.size() - 1].startX, route[route.size() - 1].endX);
546+
std::swap(route[route.size() - 1].startY, route[route.size() - 1].endY);
547+
improvementFound = true;
548+
lastImprovementAtStep = 2;
485549
}
486550
}
487551
}
488552
}
489553
}
490554

491555
// STEP 4.3: Apply relocation
492-
bool improvementRelocateFound = true;
493-
while (improvementRelocateFound) {
494-
improvementRelocateFound = false;
495-
for (size_t i = 1; i + 1 < route.size(); ++i) {
556+
if (lastImprovementAtStep == 3) {
557+
break;
558+
}
559+
bool improvementFound = true;
560+
while (improvementFound) {
561+
improvementFound = false;
562+
for (size_t i = 1; i < limitRelocationI; ++i) {
563+
double subRouteLengthCurrentPart = std::sqrt(
564+
std::pow(route[i - 1].endX - route[i].startX, 2)
565+
+ std::pow(route[i - 1].endY - route[i].startY, 2)
566+
);
567+
subRouteLengthCurrentPart += std::sqrt(
568+
std::pow(route[i].endX - route[i + 1].startX, 2)
569+
+ std::pow(route[i].endY - route[i + 1].startY, 2)
570+
);
571+
double subRouteLengthNewPart = std::sqrt(
572+
std::pow(route[i - 1].endX - route[i + 1].startX, 2)
573+
+ std::pow(route[i - 1].endY - route[i + 1].startY, 2)
574+
);
575+
subRouteLengthNewPart += Base::Precision::Confusion();
576+
496577
// Try relocating backward
497-
for (size_t j = 1; j + 2 < i; ++j) {
498-
double subRouteLengthCurrent = std::sqrt(
499-
std::pow(route[i - 1].endX - route[i].startX, 2)
500-
+ std::pow(route[i - 1].endY - route[i].startY, 2)
501-
);
502-
subRouteLengthCurrent += std::sqrt(
503-
std::pow(route[i].endX - route[i + 1].startX, 2)
504-
+ std::pow(route[i].endY - route[i + 1].startY, 2)
505-
);
578+
for (size_t j = 0; j + 2 < i; ++j) {
579+
double subRouteLengthCurrent = subRouteLengthCurrentPart;
506580
subRouteLengthCurrent += std::sqrt(
507581
std::pow(route[j].endX - route[j + 1].startX, 2)
508582
+ std::pow(route[j].endY - route[j + 1].startY, 2)
509583
);
510-
511-
double subRouteLengthNew = std::sqrt(
512-
std::pow(route[i - 1].endX - route[i + 1].startX, 2)
513-
+ std::pow(route[i - 1].endY - route[i + 1].startY, 2)
514-
);
584+
double subRouteLengthNew = subRouteLengthNewPart;
515585
subRouteLengthNew += std::sqrt(
516586
std::pow(route[j].endX - route[i].startX, 2)
517587
+ std::pow(route[j].endY - route[i].startY, 2)
@@ -520,37 +590,37 @@ std::vector<TSPTunnel> TSPSolver::solveTunnels(
520590
std::pow(route[i].endX - route[j + 1].startX, 2)
521591
+ std::pow(route[i].endY - route[j + 1].startY, 2)
522592
);
523-
subRouteLengthNew += 1e-6;
524-
525593
if (subRouteLengthNew < subRouteLengthCurrent) {
526-
// Relocate the i-th tunnel backward (after j-th element)
594+
// Relocate the i-th tunnel backward (after j-th tunnel)
527595
TSPTunnel temp = route[i];
528596
route.erase(route.begin() + i);
529597
route.insert(route.begin() + j + 1, temp);
530-
improvementRelocateFound = true;
598+
subRouteLengthCurrentPart = std::sqrt(
599+
std::pow(route[i - 1].endX - route[i].startX, 2)
600+
+ std::pow(route[i - 1].endY - route[i].startY, 2)
601+
);
602+
subRouteLengthCurrentPart += std::sqrt(
603+
std::pow(route[i].endX - route[i + 1].startX, 2)
604+
+ std::pow(route[i].endY - route[i + 1].startY, 2)
605+
);
606+
subRouteLengthNewPart = std::sqrt(
607+
std::pow(route[i - 1].endX - route[i + 1].startX, 2)
608+
+ std::pow(route[i - 1].endY - route[i + 1].startY, 2)
609+
);
610+
subRouteLengthNewPart += Base::Precision::Confusion();
531611
improvementFound = true;
612+
lastImprovementAtStep = 3;
532613
}
533614
}
534615

535616
// Try relocating forward
536-
for (size_t j = i + 1; j + 1 < route.size(); ++j) {
537-
double subRouteLengthCurrent = std::sqrt(
538-
std::pow(route[i - 1].endX - route[i].startX, 2)
539-
+ std::pow(route[i - 1].endY - route[i].startY, 2)
540-
);
541-
subRouteLengthCurrent += std::sqrt(
542-
std::pow(route[i].endX - route[i + 1].startX, 2)
543-
+ std::pow(route[i].endY - route[i + 1].startY, 2)
544-
);
617+
for (size_t j = i + 1; j < limitRelocationJ; ++j) {
618+
double subRouteLengthCurrent = subRouteLengthCurrentPart;
545619
subRouteLengthCurrent += std::sqrt(
546620
std::pow(route[j].endX - route[j + 1].startX, 2)
547621
+ std::pow(route[j].endY - route[j + 1].startY, 2)
548622
);
549-
550-
double subRouteLengthNew = std::sqrt(
551-
std::pow(route[i - 1].endX - route[i + 1].startX, 2)
552-
+ std::pow(route[i - 1].endY - route[i + 1].startY, 2)
553-
);
623+
double subRouteLengthNew = subRouteLengthNewPart;
554624
subRouteLengthNew += std::sqrt(
555625
std::pow(route[j].endX - route[i].startX, 2)
556626
+ std::pow(route[j].endY - route[i].startY, 2)
@@ -559,18 +629,67 @@ std::vector<TSPTunnel> TSPSolver::solveTunnels(
559629
std::pow(route[i].endX - route[j + 1].startX, 2)
560630
+ std::pow(route[i].endY - route[j + 1].startY, 2)
561631
);
562-
subRouteLengthNew += 1e-6;
563-
564632
if (subRouteLengthNew < subRouteLengthCurrent) {
565-
// Relocate the i-th tunnel forward (after j-th element)
633+
// Relocate the i-th tunnel forward (after j-th tunnel)
566634
TSPTunnel temp = route[i];
567635
route.erase(route.begin() + i);
568636
route.insert(route.begin() + j, temp);
569-
improvementRelocateFound = true;
637+
subRouteLengthCurrentPart = std::sqrt(
638+
std::pow(route[i - 1].endX - route[i].startX, 2)
639+
+ std::pow(route[i - 1].endY - route[i].startY, 2)
640+
);
641+
subRouteLengthCurrentPart += std::sqrt(
642+
std::pow(route[i].endX - route[i + 1].startX, 2)
643+
+ std::pow(route[i].endY - route[i + 1].startY, 2)
644+
);
645+
subRouteLengthNewPart = std::sqrt(
646+
std::pow(route[i - 1].endX - route[i + 1].startX, 2)
647+
+ std::pow(route[i - 1].endY - route[i + 1].startY, 2)
648+
);
649+
subRouteLengthNewPart += Base::Precision::Confusion();
570650
improvementFound = true;
651+
lastImprovementAtStep = 3;
571652
}
572653
}
573654
}
655+
if (!routeEndPoint) {
656+
double subRouteLengthCurrentPart = std::sqrt(
657+
std::pow(route[route.size() - 2].endX - route[route.size() - 1].startX, 2)
658+
+ std::pow(route[route.size() - 2].endY - route[route.size() - 1].startY, 2)
659+
);
660+
for (size_t j = 0; j + 2 < route.size(); ++j) {
661+
double subRouteLengthCurrent = subRouteLengthCurrentPart;
662+
subRouteLengthCurrent += std::sqrt(
663+
std::pow(route[j].endX - route[j + 1].startX, 2)
664+
+ std::pow(route[j].endY - route[j + 1].startY, 2)
665+
);
666+
double subRouteLengthNew = std::sqrt(
667+
std::pow(route[j].endX - route[route.size() - 1].startX, 2)
668+
+ std::pow(route[j].endY - route[route.size() - 1].startY, 2)
669+
);
670+
subRouteLengthNew += std::sqrt(
671+
std::pow(route[route.size() - 1].endX - route[j + 1].startX, 2)
672+
+ std::pow(route[route.size() - 1].endY - route[j + 1].startY, 2)
673+
);
674+
subRouteLengthNew += Base::Precision::Confusion();
675+
if (subRouteLengthNew < subRouteLengthCurrent) {
676+
// Relocate the last tunnel after j-th tunnel
677+
TSPTunnel temp = route[route.size() - 1];
678+
route.erase(route.begin() + route.size() - 1);
679+
route.insert(route.begin() + j + 1, temp);
680+
subRouteLengthCurrentPart = std::sqrt(
681+
std::pow(route[route.size() - 2].endX - route[route.size() - 1].startX, 2)
682+
+ std::pow(route[route.size() - 2].endY - route[route.size() - 1].startY, 2)
683+
);
684+
improvementFound = true;
685+
lastImprovementAtStep = 3;
686+
}
687+
}
688+
}
689+
}
690+
691+
if (lastImprovementAtStep == 0) {
692+
break; // No additional improvements could be made
574693
}
575694
}
576695

0 commit comments

Comments
 (0)