juli/OrcaSlicer
1
1 Fork 0
mirror of https://github.com/OrcaSlicer/OrcaSlicer.git synced 2026-10-21 06:21:16 +00:00
Code Issues Packages Projects Releases Wiki Activity
Files
0ed0c57ed3c2e0c8de3a9b11cea0500be1e59ec7
OrcaSlicer/src/libslic3r/ShortestPath.cpp
T
Ian BassiandTobias Gloth 69193d5681 Optimize travel: exact/refined path chaining, object tours, and joined infill lines (#16492) 
* Shorten the travel between extrusion paths

chain_and_reorder_extrusion_entities() builds the print order of the infill,
ironing and support paths of a region from the nozzle position. It took the
greedy chain, which leaves its longest joins for last; it now shortens it:

- Up to eight paths, chain_segments_exact() finds the shortest chain over
  every order and allowed reversal, by dynamic programming over the subsets
  of paths, with a fixed start and a free end.
- Longer chains are refined by local search: 2-opt, or-opt of up to three
  paths, and moving a run of any length next to the nozzle to the front, each
  tried only toward the 32 end points nearest a link, which come from a
  uniform grid. A move is applied only if it shortens the chain, so the result
  is never longer than the greedy one.
- A nearest-next chain is built too, and refined and used instead when it is
  already shorter than the refined greedy chain, which happens when the greedy
  chain sweeps away from the nozzle and has to come back with a long jump.

The shared greedy chaining also chains two to four segments exactly, which
covers the islands of a layer and the loops of a perimeter collection. Paths
that cannot reverse, such as Grid's, are only reordered, and collections that
keep their order are left alone. The overload without a start point keeps the
greedy chain.

Co-authored-by: Tobias Gloth <tobias.gloth@3dsystems.com>

* Order objects and islands along shorter tours

Objects under print_order Default, and the tour of islands across objects, are
visited in the same order on every layer, so chain_points_with_postprocessing()
orders them as a closed tour. Up to ten points, tsp_exact_cycle() finds the
shortest tour by dynamic programming over subsets, starting at the point
nearest the start point when one is given. Larger sets add or-opt to the
existing 2-opt and crossing removal: runs of up to three points move next to
one of the eight points nearest an end of the run, reversed or not, and
crossing removal runs last so no crossing is left. Snake ordering is
unchanged; Best of strategies gets the shorter Default tour as a candidate.

* Join infill lines into fewer paths

Every separate infill path costs a travel to it, so the fill generators now
leave fewer of them:

- connect_infill() pairs the line ends along each wall in the order that
  joins the most lines: besides shortest arc first, it replays arcs of about
  the same length taken along each contour, forwards and backwards, including
  the last pass that joins neighboring open ends, and keeps the order that
  joins the most. Regular patterns no longer leave loose ends where the
  pairs along a wall fall out of step, and multiline copies print out and back.
- Ends whose anchors would meet are joined along the wall instead, up to two
  anchors and a line width apart.
- An unpaired end anchors toward the nearest open end of another path, where
  the next path usually starts, instead of the side with more free wall. With
  multiline infill, an anchor toward a copy of the same line stops short of
  the copy instead of printing over it.
- Last, sparse infill lines whose ends are at most two line spacings apart are
  joined with a straight piece, shortest first, without closing a loop or
  crossing a line; with anchors off this is the only joining. Copies of
  neighboring multiline groups are joined at the same side, and lines that
  cannot reverse only join end to start.

* Describe the travel optimization design

How the extrusion chains, the object and island tours and the infill line joining shorten the travel, and why each part is built the way it is.

---------

Co-authored-by: Tobias Gloth <tobias.gloth@3dsystems.com>
2026-10-19 15:53:32 -03:00

2619 lines
121 KiB
C++
Raw Blame History

#include "ExtrusionEntity.hpp"
#include <vector>
#include <utility>
#include <cstddef>
#include "Point.hpp"
#include <limits>
#include <algorithm>
#include <memory>
#include "ExPolygon.hpp"
#include "BoundingBox.hpp"
#include <Eigen/Core>
#include "Polyline.hpp"
#include <iterator>
#include "Line.hpp"
#include "libslic3r.h"
#if 0
#pragma optimize("", off)
#undef NDEBUG
#undef assert
#endif
#include "ShortestPath.hpp"
#include "ExtrusionEntityCollection.hpp"
#include "KDTreeIndirect.hpp"
#include "MutablePriorityQueue.hpp"
#include "Print.hpp"
#include "GCode/OrderingStrategies.hpp"
#include <array>
#include <cmath>
#include <cassert>
#include <deque>
#include "MultiMaterialSegmentation.hpp"
namespace Slic3r {
// Orca: Some support entities may contain empty nested paths, which cannot be reordered safely.
static bool extrusion_entity_has_endpoints(const ExtrusionEntity *entity)
{
auto paths_have_endpoints = [](const ExtrusionPaths &paths) {
return !paths.empty() &&
!paths.front().polyline.points.empty() &&
!paths.back().polyline.points.empty();
};
if (entity == nullptr)
return false;
if (const auto *collection = dynamic_cast<const ExtrusionEntityCollection *>(entity))
return !collection->entities.empty() &&
extrusion_entity_has_endpoints(collection->entities.front()) &&
extrusion_entity_has_endpoints(collection->entities.back());
if (const auto *path = dynamic_cast<const ExtrusionPath *>(entity))
return !path->polyline.points.empty();
if (const auto *multipath = dynamic_cast<const ExtrusionMultiPath *>(entity))
return paths_have_endpoints(multipath->paths);
if (const auto *loop = dynamic_cast<const ExtrusionLoop *>(entity))
return paths_have_endpoints(loop->paths);
return true;
}
// Naive implementation of the Traveling Salesman Problem, it works by always taking the next closest neighbor.
// This implementation will always produce valid result even if some segments cannot reverse.
template<typename EndPointType, typename KDTreeType, typename CouldReverseFunc>
std::vector<std::pair<size_t, bool>> chain_segments_closest_point(std::vector<EndPointType> &end_points, KDTreeType &kdtree, CouldReverseFunc &could_reverse_func, EndPointType &first_point)
{
assert((end_points.size() & 1) == 0);
size_t num_segments = end_points.size() / 2;
assert(num_segments >= 2);
for (EndPointType &ep : end_points)
ep.chain_id = 0;
std::vector<std::pair<size_t, bool>> out;
out.reserve(num_segments);
size_t first_point_idx = &first_point - end_points.data();
out.emplace_back(first_point_idx / 2, (first_point_idx & 1) != 0);
first_point.chain_id = 1;
size_t this_idx = first_point_idx ^ 1;
for (int iter = (int)num_segments - 2; iter >= 0; -- iter) {
EndPointType &this_point = end_points[this_idx];
this_point.chain_id = 1;
// Find the closest point to this end_point, which lies on a different extrusion path (filtered by the lambda).
// Ignore the starting point as the starting point is considered to be occupied, no end point coud connect to it.
size_t next_idx = find_closest_point(kdtree, this_point.pos,
[this_idx, &end_points, &could_reverse_func](size_t idx) {
return (idx ^ this_idx) > 1 && end_points[idx].chain_id == 0 && ((idx & 1) == 0 || could_reverse_func(idx >> 1));
});
assert(next_idx < end_points.size());
EndPointType &end_point = end_points[next_idx];
end_point.chain_id = 1;
assert((next_idx & 1) == 0 || could_reverse_func(next_idx >> 1));
out.emplace_back(next_idx / 2, (next_idx & 1) != 0);
this_idx = next_idx ^ 1;
}
#ifndef NDEBUG
assert(end_points[this_idx].chain_id == 0);
for (EndPointType &ep : end_points)
assert(&ep == &end_points[this_idx] || ep.chain_id == 1);
#endif /* NDEBUG */
return out;
}
// The shortest chain over every order and allowed reversal, by dynamic programming over the subsets of segments, so only
// for a few of them. end_points holds the first and the last point of each segment; the chain starts at start when given
// and its end is free. Return index and "reversed" flag.
static std::vector<std::pair<size_t, bool>> chain_segments_exact(const std::vector<Vec2d> &end_points, const std::vector<char> &could_reverse, const Vec2d *start)
{
const size_t n = could_reverse.size();
assert(n >= 1 && n <= 12 && end_points.size() == 2 * n);
// State (mask * n + j) * 2 + o: the segments of mask visited, the last one j, flipped when o.
const size_t num_states = (size_t(1) << n) * n * 2;
std::vector<double> cost(num_states, std::numeric_limits<double>::max());
std::vector<size_t> from(num_states, size_t(-1));
auto entry = [&end_points](size_t j, size_t o) -> const Vec2d & { return end_points[2 * j + o]; };
auto exit = [&end_points](size_t j, size_t o) -> const Vec2d & { return end_points[2 * j + 1 - o]; };
for (size_t j = 0; j < n; ++ j)
for (size_t o = 0; o <= size_t(could_reverse[j] != 0); ++ o)
cost[((size_t(1) << j) * n + j) * 2 + o] = start == nullptr ? 0. : (entry(j, o) - *start).norm();
for (size_t mask = 1; mask < (size_t(1) << n); ++ mask)
for (size_t s = mask * n * 2; s < (mask + 1) * n * 2; ++ s)
if (cost[s] < std::numeric_limits<double>::max())
for (size_t k = 0; k < n; ++ k)
if (! (mask >> k & 1))
for (size_t o = 0; o <= size_t(could_reverse[k] != 0); ++ o) {
const size_t next = (((mask | size_t(1) << k) * n) + k) * 2 + o;
const double c = cost[s] + (entry(k, o) - exit(s / 2 % n, s & 1)).norm();
if (c < cost[next]) {
cost[next] = c;
from[next] = s;
}
}
const size_t full = (size_t(1) << n) - 1;
size_t best = full * n * 2;
for (size_t s = best; s < num_states; ++ s)
if (cost[s] < cost[best])
best = s;
std::vector<std::pair<size_t, bool>> out(n);
for (size_t s = best, k = n; k -- > 0; s = from[s])
out[k] = { s / 2 % n, (s & 1) != 0 };
return out;
}
// Chain perimeters (always closed) and thin fills (closed or open) using a greedy algorithm.
// Solving a Traveling Salesman Problem (TSP) with the modification, that the sites are not always points, but points and segments.
// Solving using a greedy algorithm, where a shortest edge is added to the solution if it does not produce a bifurcation or a cycle.
// Return index and "reversed" flag.
// https://en.wikipedia.org/wiki/Multi-fragment_algorithm
// The algorithm builds a tour for the traveling salesman one edge at a time and thus maintains multiple tour fragments, each of which
// is a simple path in the complete graph of cities. At each stage, the algorithm selects the edge of minimal cost that either creates
// a new fragment, extends one of the existing paths or creates a cycle of length equal to the number of cities.
// Orca: two to four segments are chained exactly instead.
template<typename PointType, typename SegmentEndPointFunc, bool REVERSE_COULD_FAIL, typename CouldReverseFunc>
std::vector<std::pair<size_t, bool>> chain_segments_greedy_constrained_reversals_(SegmentEndPointFunc end_point_func, CouldReverseFunc could_reverse_func, size_t num_segments, const PointType *start_near)
{
std::vector<std::pair<size_t, bool>> out;
if (num_segments == 0) {
// Nothing to do.
}
else if (num_segments == 1)
{
// Just sort the end points so that the first point visited is closest to start_near.
out.emplace_back(0, could_reverse_func(0) && start_near != nullptr &&
(end_point_func(0, false) - *start_near).template cast<double>().squaredNorm() < (end_point_func(0, true) - *start_near).template cast<double>().squaredNorm());
}
else if (num_segments <= 4)
{
// Orca: so few segments are cheap to chain exactly. The first one is then not always the one nearest start_near.
std::vector<Vec2d> end_points(2 * num_segments);
std::vector<char> could_reverse(num_segments);
for (size_t i = 0; i < num_segments; ++ i) {
end_points[2 * i] = end_point_func(i, true).template cast<double>();
end_points[2 * i + 1] = end_point_func(i, false).template cast<double>();
could_reverse[i] = could_reverse_func(i);
}
const Vec2d start = start_near != nullptr ? Vec2d(start_near->template cast<double>()) : Vec2d::Zero();
out = chain_segments_exact(end_points, could_reverse, start_near != nullptr ? &start : nullptr);
}
else
{
// End points of segments for the KD tree closest point search.
// A single end point is inserted into the search structure for loops, two end points are entered for open paths.
struct EndPoint {
EndPoint(const Vec2d &pos) : pos(pos) {}
Vec2d pos;
// Identifier of the chain, to which this end point belongs. Zero means unassigned.
size_t chain_id = 0;
// Link to the closest currently valid end point.
EndPoint *edge_out = nullptr;
// Distance to the next end point following the link.
// Zero value -> start of the final path.
double distance_out = std::numeric_limits<double>::max();
size_t heap_idx = std::numeric_limits<size_t>::max();
};
std::vector<EndPoint> end_points;
end_points.reserve(num_segments * 2);
for (size_t i = 0; i < num_segments; ++ i) {
end_points.emplace_back(end_point_func(i, true ).template cast<double>());
end_points.emplace_back(end_point_func(i, false).template cast<double>());
}
// Construct the closest point KD tree over end points of segments.
auto coordinate_fn = [&end_points](size_t idx, size_t dimension) -> double { return end_points[idx].pos[dimension]; };
KDTreeIndirect<2, double, decltype(coordinate_fn)> kdtree(coordinate_fn, end_points.size());
// Helper to detect loops in already connected paths.
// Unique chain IDs are assigned to paths. If paths are connected, end points will not have their chain IDs updated, but the chain IDs
// will remember an "equivalent" chain ID, which is the lowest ID of all the IDs in the path, and the lowest ID is equivalent to itself.
class EquivalentChains {
public:
// Zero'th chain ID is invalid.
EquivalentChains(size_t reserve) { m_equivalent_with.reserve(reserve); m_equivalent_with.emplace_back(0); }
// Generate next equivalence class.
size_t next() {
m_equivalent_with.emplace_back(++ m_last_chain_id);
return m_last_chain_id;
}
// Get equivalence class for chain ID.
size_t operator()(size_t chain_id) {
if (chain_id != 0) {
for (size_t last = chain_id;;) {
size_t lower = m_equivalent_with[last];
if (lower == last) {
m_equivalent_with[chain_id] = lower;
chain_id = lower;
break;
}
last = lower;
}
}
return chain_id;
}
size_t merge(size_t chain_id1, size_t chain_id2) {
size_t chain_id = std::min((*this)(chain_id1), (*this)(chain_id2));
m_equivalent_with[chain_id1] = chain_id;
m_equivalent_with[chain_id2] = chain_id;
return chain_id;
}
#ifndef NDEBUG
bool validate()
{
assert(m_last_chain_id >= 0);
assert(m_last_chain_id + 1 == m_equivalent_with.size());
for (size_t i = 0; i < m_equivalent_with.size(); ++ i) {
for (size_t last = i;;) {
size_t lower = m_equivalent_with[last];
assert(lower <= last);
if (lower == last)
break;
last = lower;
}
}
return true;
}
#endif /* NDEBUG */
private:
// Unique chain ID assigned to chains of end points of segments.
size_t m_last_chain_id = 0;
std::vector<size_t> m_equivalent_with;
} equivalent_chain(num_segments);
// Find the first end point closest to start_near.
EndPoint *first_point = nullptr;
size_t first_point_idx = std::numeric_limits<size_t>::max();
if (start_near != nullptr) {
size_t idx = find_closest_point(kdtree, start_near->template cast<double>(),
// Don't start with a reverse segment, if flipping of the segment is not allowed.
[&could_reverse_func](size_t idx) { return (idx & 1) == 0 || could_reverse_func(idx >> 1); });
assert(idx < end_points.size());
first_point = &end_points[idx];
first_point->distance_out = 0.;
first_point->chain_id = equivalent_chain.next();
first_point_idx = idx;
}
EndPoint *initial_point = first_point;
EndPoint *last_point = nullptr;
// Assign the closest point and distance to the end points.
for (EndPoint &end_point : end_points) {
assert(end_point.edge_out == nullptr);
if (&end_point != first_point) {
size_t this_idx = &end_point - &end_points.front();
// Find the closest point to this end_point, which lies on a different extrusion path (filtered by the lambda).
// Ignore the starting point as the starting point is considered to be occupied, no end point coud connect to it.
size_t next_idx = find_closest_point(kdtree, end_point.pos,
[this_idx, first_point_idx](size_t idx){ return idx != first_point_idx && (idx ^ this_idx) > 1; });
assert(next_idx < end_points.size());
EndPoint &end_point2 = end_points[next_idx];
end_point.edge_out = &end_point2;
end_point.distance_out = (end_point2.pos - end_point.pos).squaredNorm();
}
}
// Initialize a heap of end points sorted by the lowest distance to the next valid point of a path.
auto queue = make_mutable_priority_queue<EndPoint*, false>(
[](EndPoint *ep, size_t idx){ ep->heap_idx = idx; },
[](EndPoint *l, EndPoint *r){ return l->distance_out < r->distance_out; });
queue.reserve(end_points.size() * 2 - 1);
for (EndPoint &ep : end_points)
if (first_point != &ep)
queue.push(&ep);
#ifndef NDEBUG
auto validate_graph_and_queue = [&equivalent_chain, &end_points, &queue, first_point]() -> bool {
assert(equivalent_chain.validate());
for (EndPoint &ep : end_points) {
if (ep.heap_idx < queue.size()) {
// End point is on the heap.
assert(*(queue.cbegin() + ep.heap_idx) == &ep);
assert(ep.chain_id == 0);
} else {
// End point is NOT on the heap, therefore it is part of the output path.
assert(ep.heap_idx == std::numeric_limits<size_t>::max());
assert(ep.chain_id != 0);
if (&ep == first_point) {
assert(ep.edge_out == nullptr);
} else {
assert(ep.edge_out != nullptr);
// Detect loops.
for (EndPoint *pt = &ep; pt != nullptr;) {
// Out of queue. It is a final point.
assert(pt->heap_idx == std::numeric_limits<size_t>::max());
EndPoint *pt_other = &end_points[(pt - &end_points.front()) ^ 1];
if (pt_other->heap_idx < queue.size())
// The other side of this segment is undecided yet.
break;
pt = pt_other->edge_out;
}
}
}
}
for (EndPoint *ep : queue)
// Points in the queue are not connected yet.
assert(ep->chain_id == 0);
return true;
};
#endif /* NDEBUG */
// Chain the end points: find (num_segments - 1) shortest links not forming bifurcations or loops.
assert(num_segments >= 2);
#ifndef NDEBUG
double distance_taken_last = 0.;
#endif /* NDEBUG */
for (int iter = int(num_segments) - 2;; -- iter) {
assert(validate_graph_and_queue());
// Take the first end point, for which the link points to the currently closest valid neighbor.
EndPoint &end_point1 = *queue.top();
#ifndef NDEBUG
// Each edge added shall be longer than the previous one taken.
assert(end_point1.distance_out > distance_taken_last - SCALED_EPSILON);
distance_taken_last = end_point1.distance_out;
#endif /* NDEBUG */
assert(end_point1.edge_out != nullptr);
// No point on the queue may be connected yet.
assert(end_point1.chain_id == 0);
// Take the closest end point to the first end point,
EndPoint &end_point2 = *end_point1.edge_out;
bool valid = true;
size_t end_point1_other_chain_id = 0;
size_t end_point2_other_chain_id = 0;
if (end_point2.chain_id > 0) {
// The other side is part of the output path. Don't connect to end_point2, update end_point1 and try another one.
valid = false;
} else {
// End points of the opposite ends of the segments.
end_point1_other_chain_id = equivalent_chain(end_points[(&end_point1 - &end_points.front()) ^ 1].chain_id);
end_point2_other_chain_id = equivalent_chain(end_points[(&end_point2 - &end_points.front()) ^ 1].chain_id);
if (end_point1_other_chain_id == end_point2_other_chain_id && end_point1_other_chain_id != 0)
// This edge forms a loop. Update end_point1 and try another one.
valid = false;
}
if (valid) {
// Remove the first and second point from the queue.
queue.pop();
queue.remove(end_point2.heap_idx);
assert(end_point1.edge_out = &end_point2);
end_point2.edge_out = &end_point1;
end_point2.distance_out = end_point1.distance_out;
// Assign chain IDs to the newly connected end points, set equivalent_chain if two chains were merged.
size_t chain_id =
(end_point1_other_chain_id == 0) ?
((end_point2_other_chain_id == 0) ? equivalent_chain.next() : end_point2_other_chain_id) :
((end_point2_other_chain_id == 0) ? end_point1_other_chain_id :
(end_point1_other_chain_id == end_point2_other_chain_id) ?
end_point1_other_chain_id :
equivalent_chain.merge(end_point1_other_chain_id, end_point2_other_chain_id));
end_point1.chain_id = chain_id;
end_point2.chain_id = chain_id;
assert(validate_graph_and_queue());
if (iter == 0) {
// Last iteration. There shall be exactly one or two end points waiting to be connected.
assert(queue.size() == ((first_point == nullptr) ? 2 : 1));
if (first_point == nullptr) {
first_point = queue.top();
queue.pop();
first_point->edge_out = nullptr;
}
last_point = queue.top();
last_point->edge_out = nullptr;
queue.pop();
assert(queue.empty());
break;
}
} else {
// This edge forms a loop. Update end_point1 and try another one.
++ iter;
end_point1.edge_out = nullptr;
// Update edge_out and distance.
size_t this_idx = &end_point1 - &end_points.front();
// Find the closest point to this end_point, which lies on a different extrusion path (filtered by the filter lambda).
size_t next_idx = find_closest_point(kdtree, end_point1.pos, [&end_points, &equivalent_chain, this_idx](size_t idx) {
assert(end_points[this_idx].edge_out == nullptr);
assert(end_points[this_idx].chain_id == 0);
if ((idx ^ this_idx) <= 1 || end_points[idx].chain_id != 0)
// Points of the same segment shall not be connected,
// cannot connect to an already connected point (ideally those would be removed from the KD tree, but the update is difficult).
return false;
size_t chain1 = equivalent_chain(end_points[this_idx ^ 1].chain_id);
size_t chain2 = equivalent_chain(end_points[idx ^ 1].chain_id);
return chain1 != chain2 || chain1 == 0;
});
assert(next_idx < end_points.size());
end_point1.edge_out = &end_points[next_idx];
end_point1.distance_out = (end_points[next_idx].pos - end_point1.pos).squaredNorm();
#ifndef NDEBUG
// Each edge shall be longer than the last one removed from the queue.
assert(end_point1.distance_out > distance_taken_last - SCALED_EPSILON);
#endif /* NDEBUG */
// Update position of this end point in the queue based on the distance calculated at the line above.
queue.update(end_point1.heap_idx);
//FIXME Remove the other end point from the KD tree.
// As the KD tree update is expensive, do it only after some larger number of points is removed from the queue.
assert(validate_graph_and_queue());
}
}
assert(queue.empty());
// Now interconnect pairs of segments into a chain.
assert(first_point != nullptr);
out.reserve(num_segments);
bool failed = false;
do {
assert(out.size() < num_segments);
size_t first_point_id = first_point - &end_points.front();
size_t segment_id = first_point_id >> 1;
bool reverse = (first_point_id & 1) != 0;
EndPoint *second_point = &end_points[first_point_id ^ 1];
if (REVERSE_COULD_FAIL) {
if (reverse && ! could_reverse_func(segment_id)) {
failed = true;
break;
}
} else {
assert(! reverse || could_reverse_func(segment_id));
}
out.emplace_back(segment_id, reverse);
first_point = second_point->edge_out;
} while (first_point != nullptr);
if (REVERSE_COULD_FAIL) {
if (failed) {
if (start_near == nullptr) {
// We may try the reverse order.
out.clear();
first_point = last_point;
failed = false;
do {
assert(out.size() < num_segments);
size_t first_point_id = first_point - &end_points.front();
size_t segment_id = first_point_id >> 1;
bool reverse = (first_point_id & 1) != 0;
EndPoint *second_point = &end_points[first_point_id ^ 1];
if (reverse && ! could_reverse_func(segment_id)) {
failed = true;
break;
}
out.emplace_back(segment_id, reverse);
first_point = second_point->edge_out;
} while (first_point != nullptr);
}
}
if (failed)
// As a last resort, try a dumb algorithm, which is not sensitive to edge reversal constraints.
out = chain_segments_closest_point<EndPoint, decltype(kdtree), CouldReverseFunc>(end_points, kdtree, could_reverse_func, (initial_point != nullptr) ? *initial_point : end_points.front());
} else {
assert(! failed);
}
}
assert(out.size() == num_segments);
return out;
}
template<typename QueueType, typename KDTreeType, typename ChainsType, typename EndPointType>
void update_end_point_in_queue(QueueType &queue, const KDTreeType &kdtree, ChainsType &chains, std::vector<EndPointType> &end_points, EndPointType &end_point, size_t first_point_idx, const EndPointType *first_point)
{
// Updating an end point or a 2nd from an end point.
size_t this_idx = end_point.index(end_points);
// If this segment is not the starting segment, then this end point or the opposite is unconnected.
assert(first_point_idx == this_idx || first_point_idx == (this_idx ^ 1) || end_point.chain_id == 0 || end_point.opposite(end_points).chain_id == 0);
end_point.edge_candidate = nullptr;
if (first_point_idx == this_idx || (end_point.chain_id > 0 && first_point_idx == (this_idx ^ 1)))
{
// One may never flip the 1st edge, don't try it again.
if (! end_point.heap_idx_invalid())
queue.remove(end_point.heap_idx);
}
else
{
// Update edge_candidate and distance.
size_t chain1a = end_point.chain_id;
size_t chain1b = end_points[this_idx ^ 1].chain_id;
size_t this_chain = chains.equivalent(std::max(chain1a, chain1b));
// Find the closest point to this end_point, which lies on a different extrusion path (filtered by the filter lambda).
size_t next_idx = find_closest_point(kdtree, end_point.pos, [&end_points, &chains, this_idx, first_point_idx, first_point, this_chain](size_t idx) {
assert(end_points[this_idx].edge_candidate == nullptr);
// Either this end of the edge or the other end of the edge is not yet connected.
assert((end_points[this_idx ].chain_id == 0 && end_points[this_idx ].edge_out == nullptr) ||
(end_points[this_idx ^ 1].chain_id == 0 && end_points[this_idx ^ 1].edge_out == nullptr));
if ((idx ^ this_idx) <= 1 || idx == first_point_idx)
// Points of the same segment shall not be connected.
// Don't connect to the first point, we must not flip the 1st edge.
return false;
size_t chain2a = end_points[idx].chain_id;
size_t chain2b = end_points[idx ^ 1].chain_id;
if (chain2a > 0 && chain2b > 0)
// Only unconnected end point or a point next to an unconnected end point may be connected to.
// Ideally those would be removed from the KD tree, but the update is difficult.
return false;
assert(chain2a == 0 || chain2b == 0);
size_t chain2 = chains.equivalent(std::max(chain2a, chain2b));
if (this_chain == chain2)
// Don't connect back to the same chain, don't create a loop.
return this_chain == 0;
// Don't connect to a segment requiring flipping if the segment starts or ends with the first point.
if (chain2a > 0) {
// Chain requires flipping.
assert(chain2b == 0);
auto &chain = chains.chain(chain2);
if (chain.begin == first_point || chain.end == first_point)
return false;
}
// Everything is all right, try to connect.
return true;
});
assert(next_idx < end_points.size());
assert(chains.equivalent(end_points[next_idx].chain_id) != chains.equivalent(end_points[next_idx ^ 1].chain_id) || end_points[next_idx].chain_id == 0);
end_point.edge_candidate = &end_points[next_idx];
end_point.distance_out = (end_points[next_idx].pos - end_point.pos).norm();
if (end_point.chain_id > 0)
end_point.distance_out += chains.chain_flip_penalty(this_chain);
if (end_points[next_idx].chain_id > 0)
// The candidate chain is flipped.
end_point.distance_out += chains.chain_flip_penalty(end_points[next_idx].chain_id);
// Update position of this end point in the queue based on the distance calculated at the line above.
if (end_point.heap_idx_invalid())
queue.push(&end_point);
else
queue.update(end_point.heap_idx);
}
}
template<typename PointType, typename SegmentEndPointFunc, bool REVERSE_COULD_FAIL, typename CouldReverseFunc>
std::vector<std::pair<size_t, bool>> chain_segments_greedy_constrained_reversals2_(SegmentEndPointFunc end_point_func, CouldReverseFunc could_reverse_func, size_t num_segments, const PointType *start_near)
{
std::vector<std::pair<size_t, bool>> out;
if (num_segments == 0) {
// Nothing to do.
}
else if (num_segments == 1)
{
// Just sort the end points so that the first point visited is closest to start_near.
out.emplace_back(0, start_near != nullptr &&
(end_point_func(0, true) - *start_near).template cast<double>().squaredNorm() < (end_point_func(0, false) - *start_near).template cast<double>().squaredNorm());
}
else
{
// End points of segments for the KD tree closest point search.
// A single end point is inserted into the search structure for loops, two end points are entered for open paths.
struct EndPoint {
EndPoint(const Vec2d &pos) : pos(pos) {}
Vec2d pos;
// Candidate for a new connection link.
EndPoint *edge_candidate = nullptr;
// Distance to the next end point following the link.
// Zero value -> start of the final path.
double distance_out = std::numeric_limits<double>::max();
size_t heap_idx = std::numeric_limits<size_t>::max();
bool heap_idx_invalid() const { return this->heap_idx == std::numeric_limits<size_t>::max(); }
// Identifier of the chain, to which this end point belongs. Zero means unassigned.
size_t chain_id = 0;
// Double linked chain of segment end points in current path.
EndPoint *edge_out = nullptr;
size_t index(std::vector<EndPoint> &endpoints) const { return this - endpoints.data(); }
// Opposite end point of the same segment.
EndPoint& opposite(std::vector<EndPoint> &endpoints) { return endpoints[(this - endpoints.data()) ^ 1]; }
const EndPoint& opposite(const std::vector<EndPoint> &endpoints) const { return endpoints[(this - endpoints.data()) ^ 1]; }
};
std::vector<EndPoint> end_points;
end_points.reserve(num_segments * 2);
for (size_t i = 0; i < num_segments; ++ i) {
end_points.emplace_back(end_point_func(i, true ).template cast<double>());
end_points.emplace_back(end_point_func(i, false).template cast<double>());
}
// Construct the closest point KD tree over end points of segments.
auto coordinate_fn = [&end_points](size_t idx, size_t dimension) -> double { return end_points[idx].pos[dimension]; };
KDTreeIndirect<2, double, decltype(coordinate_fn)> kdtree(coordinate_fn, end_points.size());
// Chained segments with their sum of connection lengths.
// The chain supports flipping all the segments, connecting the segments at the opposite ends.
// (this is a very useful path optimization for infill lines).
struct Chain {
size_t num_segments = 0;
double cost = 0.;
double cost_flipped = 0.;
EndPoint *begin = nullptr;
EndPoint *end = nullptr;
size_t equivalent_with = 0;
// Flipping the chain has a time complexity of O(n).
void flip(std::vector<EndPoint> &endpoints)
{
assert(this->num_segments > 1);
assert(this->begin->edge_out == nullptr);
assert(this->end ->edge_out == nullptr);
assert(this->begin->opposite(endpoints).edge_out != nullptr);
assert(this->end ->opposite(endpoints).edge_out != nullptr);
// Start of the current segment processed.
EndPoint *ept = this->begin;
// Previous end point to connect the other side of ept to.
EndPoint *ept_prev = nullptr;
do {
EndPoint *ept_end = &ept->opposite(endpoints);
EndPoint *ept_next = ept_end->edge_out;
assert(ept_next == nullptr || ept_next->edge_out == ept_end);
// Connect to the preceding segment.
ept_end->edge_out = ept_prev;
if (ept_prev != nullptr)
ept_prev->edge_out = ept_end;
ept_prev = ept;
ept = ept_next;
} while (ept != nullptr);
ept_prev->edge_out = nullptr;
// Swap the costs.
std::swap(this->cost, this->cost_flipped);
// Swap the ends.
EndPoint *new_begin = &this->begin->opposite(endpoints);
EndPoint *new_end = &this->end->opposite(endpoints);
std::swap(this->begin->chain_id, new_begin->chain_id);
std::swap(this->end ->chain_id, new_end ->chain_id);
this->begin = new_begin;
this->end = new_end;
assert(this->begin->edge_out == nullptr);
assert(this->end ->edge_out == nullptr);
assert(this->begin->opposite(endpoints).edge_out != nullptr);
assert(this->end ->opposite(endpoints).edge_out != nullptr);
}
double flip_penalty() const { return this->cost_flipped - this->cost; }
};
// Helper to detect loops in already connected paths and to accomodate flipping of chains.
//
// Unique chain IDs are assigned to paths. If paths are connected, end points will not have their chain IDs updated, but the chain IDs
// will remember an "equivalent" chain ID, which is the lowest ID of all the IDs in the path, and the lowest ID is equivalent to itself.
// Chain IDs are indexed starting with 1.
//
// Chains remember their lengths and their lengths when each segment of the chain is flipped.
class Chains {
public:
// Zero'th chain ID is invalid.
Chains(size_t reserve) {
m_chains.reserve(reserve / 2);
// Indexing starts with 1.
m_chains.emplace_back();
}
// Generate next equivalence class.
size_t next_id() {
m_chains.emplace_back();
m_chains.back().equivalent_with = ++ m_last_chain_id;
return m_last_chain_id;
}
// Get equivalence class for chain ID, update the "equivalent_with" along the equivalence path.
size_t equivalent(size_t chain_id) {
if (chain_id != 0) {
for (size_t last = chain_id;;) {
size_t lower = m_chains[last].equivalent_with;
if (lower == last) {
m_chains[chain_id].equivalent_with = lower;
chain_id = lower;
break;
}
last = lower;
}
}
return chain_id;
}
// Return a lowest chain ID of the two input chains.
// Produce a new chain ID of both chain IDs are zero.
size_t merge(size_t chain_id1, size_t chain_id2) {
if (chain_id1 == 0)
return (chain_id2 == 0) ? this->next_id() : chain_id2;
if (chain_id2 == 0)
return chain_id1;
assert(m_chains[chain_id1].equivalent_with == chain_id1);
assert(m_chains[chain_id2].equivalent_with == chain_id2);
size_t chain_id = std::min(chain_id1, chain_id2);
m_chains[chain_id1].equivalent_with = chain_id;
m_chains[chain_id2].equivalent_with = chain_id;
return chain_id;
}
Chain& chain(size_t chain_id) { return m_chains[chain_id]; }
const Chain& chain(size_t chain_id) const { return m_chains[chain_id]; }
double chain_flip_penalty(size_t chain_id) {
chain_id = this->equivalent(chain_id);
return m_chains[chain_id].flip_penalty();
}
#ifndef NDEBUG
bool validate()
{
// Validate that the segments merged chain IDs make up a directed acyclic graph
// with edges oriented towards the lower chain ID, therefore all ending up
// in the lowest chain ID of all of them.
assert(m_last_chain_id >= 0);
assert(m_last_chain_id + 1 == m_chains.size());
for (size_t i = 0; i < m_chains.size(); ++ i) {
for (size_t last = i;;) {
size_t lower = m_chains[last].equivalent_with;
assert(lower <= last);
if (lower == last)
break;
last = lower;
}
}
return true;
}
#endif /* NDEBUG */
private:
std::vector<Chain> m_chains;
// Unique chain ID assigned to chains of end points of segments.
size_t m_last_chain_id = 0;
} chains(num_segments);
// Find the first end point closest to start_near.
EndPoint *first_point = nullptr;
size_t first_point_idx = std::numeric_limits<size_t>::max();
if (start_near != nullptr) {
size_t idx = find_closest_point(kdtree, start_near->template cast<double>());
assert(idx < end_points.size());
first_point = &end_points[idx];
first_point->distance_out = 0.;
first_point->chain_id = chains.next_id();
Chain &chain = chains.chain(first_point->chain_id);
chain.begin = first_point;
chain.end = &first_point->opposite(end_points);
first_point_idx = idx;
}
EndPoint *initial_point = first_point;
EndPoint *last_point = nullptr;
// Assign the closest point and distance to the end points.
for (EndPoint &end_point : end_points) {
assert(end_point.edge_candidate == nullptr);
if (&end_point != first_point) {
size_t this_idx = end_point.index(end_points);
// Find the closest point to this end_point, which lies on a different extrusion path (filtered by the lambda).
// Ignore the starting point as the starting point is considered to be occupied, no end point coud connect to it.
size_t next_idx = find_closest_point(kdtree, end_point.pos,
[this_idx, first_point_idx](size_t idx){ return idx != first_point_idx && (idx ^ this_idx) > 1; });
assert(next_idx < end_points.size());
EndPoint &end_point2 = end_points[next_idx];
end_point.edge_candidate = &end_point2;
end_point.distance_out = (end_point2.pos - end_point.pos).norm();
}
}
// Initialize a heap of end points sorted by the lowest distance to the next valid point of a path.
auto queue = make_mutable_priority_queue<EndPoint*, true>(
[](EndPoint *ep, size_t idx){ ep->heap_idx = idx; },
[](EndPoint *l, EndPoint *r){ return l->distance_out < r->distance_out; });
queue.reserve(end_points.size() * 2);
for (EndPoint &ep : end_points)
if (first_point != &ep)
queue.push(&ep);
#ifndef NDEBUG
auto validate_graph_and_queue = [&chains, &end_points, &queue, first_point]() -> bool {
assert(chains.validate());
for (EndPoint &ep : end_points) {
if (ep.heap_idx < queue.size()) {
// End point is on the heap.
assert(*(queue.cbegin() + ep.heap_idx) == &ep);
// One side or the other of the segment is not yet connected.
assert(ep.chain_id == 0 || ep.opposite(end_points).chain_id == 0);
} else {
// End point is NOT on the heap, therefore it must part of the output path.
assert(ep.heap_idx_invalid());
assert(ep.chain_id != 0);
if (&ep == first_point) {
assert(ep.edge_out == nullptr);
} else {
assert(ep.edge_out != nullptr);
// Detect loops.
for (EndPoint *pt = &ep; pt != nullptr;) {
// Out of queue. It is a final point.
EndPoint *pt_other = &pt->opposite(end_points);
if (pt_other->heap_idx < queue.size()) {
// The other side of this segment is undecided yet.
// assert(pt_other->edge_out == nullptr);
break;
}
pt = pt_other->edge_out;
}
}
}
}
for (EndPoint *ep : queue)
// Points in the queue or the opposites of the same segment are not connected yet.
assert(ep->chain_id == 0 || ep->opposite(end_points).chain_id == 0);
return true;
};
#endif /* NDEBUG */
// Chain the end points: find (num_segments - 1) shortest links not forming bifurcations or loops.
assert(num_segments >= 2);
#ifndef NDEBUG
double distance_taken_last = 0.;
#endif /* NDEBUG */
// Some links stored onto the priority queue are being invalidated during the calculation and they are not
// updated immediately. If such a situation is detected for an end point pulled from the priority queue,
// the end point is being updated and re-inserted into the priority queue. Therefore the number of iterations
// required is higher than expected (it would be the number of links, num_segments - 1).
// The limit here may not be necessary, but it guards us against an endless loop if something goes wrong.
size_t num_iter = num_segments * 16;
for (size_t num_connections_to_end = num_segments - 1; num_iter > 0; -- num_iter) {
assert(validate_graph_and_queue());
// Take the first end point, for which the link points to the currently closest valid neighbor.
EndPoint *end_point1 = queue.top();
assert(end_point1 != first_point);
EndPoint *end_point1_other = &end_point1->opposite(end_points);
// true if end_point1 is not the end of its chain, but the 2nd point. When connecting to the 2nd point, this chain needs
// to be flipped first.
bool chain1_flip = end_point1->chain_id > 0;
// Either this point at the queue is not connected, or it is the 2nd point of a chain.
// If connecting to a 2nd point of a chain, the 1st point shall not yet be connected and this chain will need
// to be flipped.
assert( chain1_flip || (end_point1->chain_id == 0 && end_point1->edge_out == nullptr));
assert(! chain1_flip || (end_point1_other->chain_id == 0 && end_point1_other->edge_out == nullptr));
assert(end_point1->edge_candidate != nullptr);
#ifndef NDEBUG
// Each edge added shall be longer than the previous one taken.
//assert(end_point1->distance_out > distance_taken_last - SCALED_EPSILON);
if (end_point1->distance_out < distance_taken_last - SCALED_EPSILON) {
// printf("Warning: taking shorter length than previously is suspicious\n");
}
distance_taken_last = end_point1->distance_out;
#endif /* NDEBUG */
// Take the closest end point to the first end point,
EndPoint *end_point2 = end_point1->edge_candidate;
EndPoint *end_point2_other = &end_point2->opposite(end_points);
bool chain2_flip = end_point2->chain_id > 0;
// Is the link from end_point1 to end_point2 still valid? If yes, the link may be taken. Otherwise the link needs to be refreshed.
bool valid = true;
size_t end_point1_chain_id = 0;
size_t end_point2_chain_id = 0;
if (end_point2->chain_id > 0 && end_point2_other->chain_id > 0) {
// The other side is part of the output path. Don't connect to end_point2, update end_point1 and try another one.
valid = false;
} else {
// End points of the opposite ends of the segments.
end_point1_chain_id = chains.equivalent((chain1_flip ? end_point1 : end_point1_other)->chain_id);
end_point2_chain_id = chains.equivalent((chain2_flip ? end_point2 : end_point2_other)->chain_id);
if (end_point1_chain_id == end_point2_chain_id && end_point1_chain_id != 0)
// This edge forms a loop. Update end_point1 and try another one.
valid = false;
else {
// Verify whether end_point1.distance_out still matches the current state of the two end points to be connected and their chains.
// Namely, the other chain may have been flipped in the meantime.
double dist = (end_point2->pos - end_point1->pos).norm();
if (chain1_flip)
dist += chains.chain_flip_penalty(end_point1_chain_id);
if (chain2_flip)
dist += chains.chain_flip_penalty(end_point2_chain_id);
if (std::abs(dist - end_point1->distance_out) > SCALED_EPSILON)
// The distance changed due to flipping of one of the chains. Refresh this end point in the queue.
valid = false;
}
if (valid && first_point != nullptr) {
// Verify that a chain starting or ending with the first_point does not get flipped.
if (chain1_flip) {
Chain &chain = chains.chain(end_point1_chain_id);
if (chain.begin == first_point || chain.end == first_point)
valid = false;
}
if (valid && chain2_flip) {
Chain &chain = chains.chain(end_point2_chain_id);
if (chain.begin == first_point || chain.end == first_point)
valid = false;
}
}
}
if (valid) {
// Remove the first and second point from the queue.
queue.pop();
queue.remove(end_point2->heap_idx);
assert(end_point1->edge_candidate == end_point2);
end_point1->edge_candidate = nullptr;
Chain *chain1 = (end_point1_chain_id == 0) ? nullptr : &chains.chain(end_point1_chain_id);
Chain *chain2 = (end_point2_chain_id == 0) ? nullptr : &chains.chain(end_point2_chain_id);
assert(chain1 == nullptr || (chain1_flip ? (chain1->begin == end_point1_other || chain1->end == end_point1_other) : (chain1->begin == end_point1 || chain1->end == end_point1)));
assert(chain2 == nullptr || (chain2_flip ? (chain2->begin == end_point2_other || chain2->end == end_point2_other) : (chain2->begin == end_point2 || chain2->end == end_point2)));
if (chain1_flip)
chain1->flip(end_points);
if (chain2_flip)
chain2->flip(end_points);
assert(chain1 == nullptr || chain1->begin == end_point1 || chain1->end == end_point1);
assert(chain2 == nullptr || chain2->begin == end_point2 || chain2->end == end_point2);
size_t chain_id = chains.merge(end_point1_chain_id, end_point2_chain_id);
Chain &chain = chains.chain(chain_id);
{
Chain chain_dst;
chain_dst.begin = (chain1 == nullptr) ? end_point1_other : (chain1->begin == end_point1) ? chain1->end : chain1->begin;
chain_dst.end = (chain2 == nullptr) ? end_point2_other : (chain2->begin == end_point2) ? chain2->end : chain2->begin;
chain_dst.cost = (chain1 == 0 ? 0. : chain1->cost) + (chain2 == 0 ? 0. : chain2->cost) + (end_point2->pos - end_point1->pos).norm();
chain_dst.cost_flipped = (chain1 == 0 ? 0. : chain1->cost_flipped) + (chain2 == 0 ? 0. : chain2->cost_flipped) + (end_point2_other->pos - end_point1_other->pos).norm();
chain_dst.num_segments = (chain1 == 0 ? 1 : chain1->num_segments) + (chain2 == 0 ? 1 : chain2->num_segments);
chain_dst.equivalent_with = chain_id;
chain = chain_dst;
}
if (chain.begin != end_point1_other && ! end_point1_other->heap_idx_invalid())
queue.remove(end_point1_other->heap_idx);
if (chain.end != end_point2_other && ! end_point2_other->heap_idx_invalid())
queue.remove(end_point2_other->heap_idx);
end_point1->edge_out = end_point2;
end_point2->edge_out = end_point1;
end_point1->chain_id = chain_id;
end_point2->chain_id = chain_id;
end_point1_other->chain_id = chain_id;
end_point2_other->chain_id = chain_id;
if (chain.begin != first_point)
chain.begin->chain_id = 0;
if (chain.end != first_point)
chain.end->chain_id = 0;
if (-- num_connections_to_end == 0) {
assert(validate_graph_and_queue());
// Last iteration. There shall be exactly one or two end points waiting to be connected.
assert(queue.size() <= ((first_point == nullptr) ? 4 : 2));
if (first_point == nullptr) {
// Find the first remaining end point.
do {
first_point = queue.top();
queue.pop();
} while (first_point->edge_out != nullptr);
assert(first_point->edge_out == nullptr);
}
// Find the first remaining end point.
do {
last_point = queue.top();
queue.pop();
} while (last_point->edge_out != nullptr);
assert(last_point->edge_out == nullptr);
#ifndef NDEBUG
while (! queue.empty()) {
assert(queue.top()->edge_out != nullptr && queue.top()->chain_id > 0);
queue.pop();
}
#endif /* NDEBUG */
break;
} else {
//FIXME update the 2nd end points on the queue.
// Update end points of the flipped segments.
update_end_point_in_queue(queue, kdtree, chains, end_points, chain.begin->opposite(end_points), first_point_idx, first_point);
update_end_point_in_queue(queue, kdtree, chains, end_points, chain.end->opposite(end_points), first_point_idx, first_point);
if (chain1_flip)
update_end_point_in_queue(queue, kdtree, chains, end_points, *chain.begin, first_point_idx, first_point);
if (chain2_flip)
update_end_point_in_queue(queue, kdtree, chains, end_points, *chain.end, first_point_idx, first_point);
// End points of chains shall certainly stay in the queue.
assert(chain.begin == first_point || chain.begin->heap_idx < queue.size());
assert(chain.end == first_point || chain.end ->heap_idx < queue.size());
assert(&chain.begin->opposite(end_points) != first_point &&
(chain.begin == first_point ? chain.begin->opposite(end_points).heap_idx_invalid() : chain.begin->opposite(end_points).heap_idx < queue.size()));
assert(&chain.end ->opposite(end_points) != first_point &&
(chain.end == first_point ? chain.end ->opposite(end_points).heap_idx_invalid() : chain.end ->opposite(end_points).heap_idx < queue.size()));
}
} else {
// This edge forms a loop. Update end_point1 and try another one.
update_end_point_in_queue(queue, kdtree, chains, end_points, *end_point1, first_point_idx, first_point);
#ifndef NDEBUG
// Each edge shall be longer than the last one removed from the queue.
//assert(end_point1->distance_out > distance_taken_last - SCALED_EPSILON);
if (end_point1->distance_out < distance_taken_last - SCALED_EPSILON) {
// printf("Warning: taking shorter length than previously is suspicious\n");
}
#endif /* NDEBUG */
//FIXME Remove the other end point from the KD tree.
// As the KD tree update is expensive, do it only after some larger number of points is removed from the queue.
}
assert(validate_graph_and_queue());
}
assert(queue.empty());
// Now interconnect pairs of segments into a chain.
assert(first_point != nullptr);
out.reserve(num_segments);
bool failed = false;
do {
assert(out.size() < num_segments);
size_t first_point_id = first_point - &end_points.front();
size_t segment_id = first_point_id >> 1;
bool reverse = (first_point_id & 1) != 0;
EndPoint *second_point = &end_points[first_point_id ^ 1];
if (REVERSE_COULD_FAIL) {
if (reverse && ! could_reverse_func(segment_id)) {
failed = true;
break;
}
} else {
assert(! reverse || could_reverse_func(segment_id));
}
out.emplace_back(segment_id, reverse);
first_point = second_point->edge_out;
} while (first_point != nullptr);
if (REVERSE_COULD_FAIL) {
if (failed) {
if (start_near == nullptr) {
// We may try the reverse order.
out.clear();
first_point = last_point;
failed = false;
do {
assert(out.size() < num_segments);
size_t first_point_id = first_point - &end_points.front();
size_t segment_id = first_point_id >> 1;
bool reverse = (first_point_id & 1) != 0;
EndPoint *second_point = &end_points[first_point_id ^ 1];
if (reverse && ! could_reverse_func(segment_id)) {
failed = true;
break;
}
out.emplace_back(segment_id, reverse);
first_point = second_point->edge_out;
} while (first_point != nullptr);
}
}
if (failed)
// As a last resort, try a dumb algorithm, which is not sensitive to edge reversal constraints.
out = chain_segments_closest_point<EndPoint, decltype(kdtree), CouldReverseFunc>(end_points, kdtree, could_reverse_func, (initial_point != nullptr) ? *initial_point : end_points.front());
} else {
assert(! failed);
}
}
assert(out.size() == num_segments);
return out;
}
template<typename PointType, typename SegmentEndPointFunc, typename CouldReverseFunc>
std::vector<std::pair<size_t, bool>> chain_segments_greedy_constrained_reversals(SegmentEndPointFunc end_point_func, CouldReverseFunc could_reverse_func, size_t num_segments, const PointType *start_near)
{
return chain_segments_greedy_constrained_reversals_<PointType, SegmentEndPointFunc, true, CouldReverseFunc>(end_point_func, could_reverse_func, num_segments, start_near);
}
template<typename PointType, typename SegmentEndPointFunc>
std::vector<std::pair<size_t, bool>> chain_segments_greedy(SegmentEndPointFunc end_point_func, size_t num_segments, const PointType *start_near)
{
auto could_reverse_func = [](size_t /* idx */) -> bool { return true; };
return chain_segments_greedy_constrained_reversals_<PointType, SegmentEndPointFunc, false, decltype(could_reverse_func)>(end_point_func, could_reverse_func, num_segments, start_near);
}
template<typename PointType, typename SegmentEndPointFunc, typename CouldReverseFunc>
std::vector<std::pair<size_t, bool>> chain_segments_greedy_constrained_reversals2(SegmentEndPointFunc end_point_func, CouldReverseFunc could_reverse_func, size_t num_segments, const PointType *start_near)
{
return chain_segments_greedy_constrained_reversals2_<PointType, SegmentEndPointFunc, true, CouldReverseFunc>(end_point_func, could_reverse_func, num_segments, start_near);
}
template<typename PointType, typename SegmentEndPointFunc>
std::vector<std::pair<size_t, bool>> chain_segments_greedy2(SegmentEndPointFunc end_point_func, size_t num_segments, const PointType *start_near)
{
auto could_reverse_func = [](size_t /* idx */) -> bool { return true; };
return chain_segments_greedy_constrained_reversals2_<PointType, SegmentEndPointFunc, false, decltype(could_reverse_func)>(end_point_func, could_reverse_func, num_segments, start_near);
}
// The K points nearest to a query, nearest first, ties by index, from a uniform grid of about two points per cell.
template<size_t K>
class NearestPoints
{
public:
static constexpr size_t npos = size_t(-1);
explicit NearestPoints(const std::vector<Vec2d> &points) : m_points(points)
{
BoundingBoxf bbox;
for (const Vec2d &p : points)
bbox.merge(p);
const Vec2d size = bbox.size();
const double count = double(std::max<size_t>(points.size(), 1));
m_cell = std::max(std::sqrt(2. * size.x() * size.y() / count), std::max(size.x(), size.y()) / count);
if (m_cell <= 0.)
m_cell = 1.;
m_origin = bbox.min;
m_cols = int(size.x() / m_cell) + 1;
m_rows = int(size.y() / m_cell) + 1;
m_first.assign(size_t(m_cols) * m_rows + 1, 0);
for (const Vec2d &p : points)
++ m_first[cell(p) + 1];
for (size_t c = 1; c < m_first.size(); ++ c)
m_first[c] += m_first[c - 1];
m_index.resize(points.size());
std::vector<size_t> next(m_first.begin(), m_first.end() - 1);
for (size_t i = 0; i < points.size(); ++ i)
m_index[next[cell(points[i])] ++] = i;
}
// Indices of the nearest points that filter accepts, npos past the last one.
template<typename Filter>
std::array<size_t, K> query(const Vec2d &q, Filter filter) const
{
std::array<std::pair<double, size_t>, K> best;
size_t found = 0;
const int cx = int(std::clamp(std::floor((q.x() - m_origin.x()) / m_cell), 0., double(m_cols - 1)));
const int cy = int(std::clamp(std::floor((q.y() - m_origin.y()) / m_cell), 0., double(m_rows - 1)));
auto visit = [&](int x, int y) {
if (x < 0 || y < 0 || x >= m_cols || y >= m_rows)
return;
const size_t c = size_t(y) * m_cols + x;
for (size_t k = m_first[c]; k < m_first[c + 1]; ++ k) {
const size_t i = m_index[k];
if (! filter(i))
continue;
const std::pair<double, size_t> candidate((m_points[i] - q).squaredNorm(), i);
if (found == K && ! (candidate < best[K - 1]))
continue;
auto it = std::upper_bound(best.begin(), best.begin() + found, candidate);
std::move_backward(it, best.begin() + std::min(found, K - 1), best.begin() + std::min(found + 1, K));
*it = candidate;
found = std::min(found + 1, K);
}
};
// Points beyond ring r are at least r cells away.
for (int r = 0; r <= std::max(m_cols, m_rows); ++ r) {
if (r == 0)
visit(cx, cy);
for (int d = -r; d <= r && r > 0; ++ d) {
visit(cx + d, cy - r);
visit(cx + d, cy + r);
if (d != -r && d != r) {
visit(cx - r, cy + d);
visit(cx + r, cy + d);
}
}
if (found == K && best[K - 1].first <= sqr(r * m_cell))
break;
}
std::array<size_t, K> out;
out.fill(npos);
for (size_t k = 0; k < found; ++ k)
out[k] = best[k].second;
return out;
}
private:
size_t cell(const Vec2d &p) const
{
const int x = std::clamp(int((p.x() - m_origin.x()) / m_cell), 0, m_cols - 1);
const int y = std::clamp(int((p.y() - m_origin.y()) / m_cell), 0, m_rows - 1);
return size_t(y) * m_cols + x;
}
const std::vector<Vec2d> &m_points;
Vec2d m_origin;
double m_cell;
int m_cols;
int m_rows;
std::vector<size_t> m_first;
std::vector<size_t> m_index;
};
// Shortens the travel of a chain with 2-opt moves (reversing a run of segments) and or-opt moves (moving a run of up to
// three segments elsewhere, or a longer one to the front, flipped or not). end_points holds the first and the last point
// of each segment. The chain starts at start_near when given, and its end is free. A move that would flip a segment that
// cannot reverse is skipped. Moves only create links between nearest neighbor end points, and after a move only the
// segments next to a changed link are examined again, until a sweep of all segments finds no move. This keeps the search
// close to linear.
static void improve_chain_by_local_search(std::vector<std::pair<size_t, bool>> &chain, const std::vector<Vec2d> &end_points,
const std::vector<char> &could_reverse, const Vec2d *start_near)
{
const size_t n = chain.size();
if (n < 2)
return;
assert(end_points.size() == 2 * n && could_reverse.size() == n);
std::vector<size_t> order(n), pos(n), fixed_before(n + 1, 0);
std::vector<char> flipped(n);
for (size_t k = 0; k < n; ++ k) {
order[k] = chain[k].first;
flipped[order[k]] = chain[k].second;
}
// Updates positions lo..hi after a move, which keeps the number of segments that cannot reverse past hi.
auto refresh = [&](size_t lo, size_t hi) {
for (size_t k = lo; k <= hi; ++ k) {
pos[order[k]] = k;
fixed_before[k + 1] = fixed_before[k] + ! could_reverse[order[k]];
}
};
refresh(0, n - 1);
auto entry_idx = [&](size_t k) { return 2 * order[k] + flipped[order[k]]; };
auto exit_idx = [&](size_t k) { return 2 * order[k] + 1 - flipped[order[k]]; };
auto entry = [&](size_t k) -> const Vec2d& { return end_points[entry_idx(k)]; };
auto exit = [&](size_t k) -> const Vec2d& { return end_points[exit_idx(k)]; };
auto is_exit = [&](size_t e) { return char(e & 1) != flipped[e >> 1]; };
auto reversible = [&](size_t i, size_t j) { return fixed_before[j + 1] == fixed_before[i]; };
auto dist = [](const Vec2d *a, const Vec2d *b) { return a == nullptr || b == nullptr ? 0. : (*a - *b).norm(); };
// Travel end points around position k, null where the chain is open.
auto prev_exit = [&](size_t k) -> const Vec2d* { return k > 0 ? &exit(k - 1) : start_near; };
auto next_entry = [&](size_t k) -> const Vec2d* { return k + 1 < n ? &entry(k + 1) : nullptr; };
// The 32 nearest end points are the candidates, which covers every move on short chains; 8 missed much of the gain.
static constexpr size_t K = 32;
static constexpr size_t npos = NearestPoints<K>::npos;
const NearestPoints<K> nearest(end_points);
std::vector<std::array<size_t, K>> neighbors(end_points.size());
for (size_t e = 0; e < end_points.size(); ++ e)
neighbors[e] = nearest.query(end_points[e], [e](size_t idx) { return (idx >> 1) != (e >> 1); });
std::array<size_t, K> start_neighbors;
start_neighbors.fill(npos);
if (start_near != nullptr)
start_neighbors = nearest.query(*start_near, [](size_t) { return true; });
// Segments to examine, by id, each queued once.
std::deque<size_t> queue;
std::vector<char> queued(n, 0);
auto enqueue = [&](size_t k) {
if (k < n && ! queued[order[k]]) {
queued[order[k]] = 1;
queue.push_back(order[k]);
}
};
const double min_gain = SCALED_EPSILON;
// Reverses positions i..j if that shortens the chain.
auto try_2opt = [&](size_t i, size_t j) {
if (! reversible(i, j))
return false;
const Vec2d *a = prev_exit(i), *b = next_entry(j);
if (dist(a, &exit(j)) + dist(&entry(i), b) > dist(a, &entry(i)) + dist(&exit(j), b) - min_gain)
return false;
std::reverse(order.begin() + i, order.begin() + j + 1);
for (size_t k = i; k <= j; ++ k)
flipped[order[k]] ^= 1;
refresh(i, j);
for (size_t k : { i - 1, i, j, j + 1 })
enqueue(k);
return true;
};
// Moves positions i..last after position gap (size_t(-1) for the front) if that shortens the chain.
auto try_oropt = [&](size_t i, size_t last, size_t gap) {
if (gap + 1 == i || (gap >= i && gap <= last))
return false;
const Vec2d *a = gap == size_t(-1) ? start_near : &exit(gap);
const Vec2d *b = gap + 1 < n ? &entry(gap + 1) : nullptr;
const double removal_gain = dist(prev_exit(i), &entry(i)) + dist(&exit(last), next_entry(last)) - dist(prev_exit(i), next_entry(last));
const double base = dist(a, b);
const double cost_kept = dist(a, &entry(i)) + dist(&exit(last), b) - base;
const double cost_flipped = reversible(i, last) ? dist(a, &exit(last)) + dist(&entry(i), b) - base : std::numeric_limits<double>::max();
if (std::min(cost_kept, cost_flipped) > removal_gain - min_gain)
return false;
const size_t len = last - i + 1;
// The run lands on first..first+len-1; lo..hi spans every position that changed.
const bool forward = gap > last && gap != size_t(-1);
const size_t first = forward ? gap + 1 - len : gap + 1;
const size_t lo = forward ? i : first;
const size_t hi = forward ? gap : last;
if (forward)
std::rotate(order.begin() + i, order.begin() + last + 1, order.begin() + gap + 1);
else
std::rotate(order.begin() + first, order.begin() + i, order.begin() + last + 1);
if (cost_flipped < cost_kept) {
std::reverse(order.begin() + first, order.begin() + first + len);
for (size_t k = first; k < first + len; ++ k)
flipped[order[k]] ^= 1;
}
refresh(lo, hi);
// The links around the run's new place, and the link closing its old place.
for (size_t k : { first - 1, first, first + len - 1, first + len, forward ? i - 1 : last, forward ? i : last + 1 })
enqueue(k);
return true;
};
// The gap before position k of the chain with positions i..last taken out.
auto gap_before = [](size_t k, size_t i, size_t last) { return k == last + 1 ? i - 1 : k - 1; };
// Applies the first move found that breaks a link next to position p.
// Candidates are tried nearest first, while closer than the link the move would replace at that end point.
auto closer = [&](const Vec2d &at, size_t q, double limit) { return q != npos && (end_points[q] - at).norm() < limit; };
auto improve_at = [&](size_t p) {
// 2-opt breaking the link into p, as the first or the last link of the reversed run.
if (const Vec2d *a = prev_exit(p); a != nullptr) {
const double old_link = dist(a, &entry(p));
for (size_t q : p > 0 ? neighbors[exit_idx(p - 1)] : start_neighbors) {
if (! closer(*a, q, old_link))
break;
if (is_exit(q) && pos[q >> 1] >= p && try_2opt(p, pos[q >> 1]))
return true;
}
if (p > 0)
for (size_t q : neighbors[entry_idx(p)]) {
if (! closer(entry(p), q, old_link))
break;
if (! is_exit(q) && pos[q >> 1] < p && try_2opt(pos[q >> 1], p - 1))
return true;
}
}
// 2-opt breaking the link out of p.
if (p + 1 < n) {
const double old_link = dist(&exit(p), &entry(p + 1));
for (size_t q : neighbors[exit_idx(p)]) {
if (! closer(exit(p), q, old_link))
break;
if (is_exit(q) && pos[q >> 1] > p && try_2opt(p + 1, pos[q >> 1]))
return true;
}
for (size_t q : neighbors[entry_idx(p + 1)]) {
if (! closer(entry(p + 1), q, old_link))
break;
if (! is_exit(q) && pos[q >> 1] <= p && try_2opt(pos[q >> 1], p))
return true;
}
}
// A run of any length that starts or ends next to the start point moves to the front.
if (p == 0)
for (size_t q : start_neighbors) {
if (q == npos)
break;
const size_t m = pos[q >> 1];
if (m == 0)
continue;
// Runs m..k enter next to the start, runs k..m leave there and go flipped.
const bool exit_near = is_exit(q);
for (size_t k = exit_near ? 1 : m; k < (exit_near ? m + 1 : n); ++ k)
if (try_oropt(exit_near ? k : m, exit_near ? m : k, size_t(-1)))
return true;
}
// Or-opt of the runs starting at p: next to an exit a run goes after that segment, next to an entry before it.
for (size_t last = p; last < std::min(p + 3, n); ++ last) {
if (try_oropt(p, last, size_t(-1)))
return true;
const double removal_gain = dist(prev_exit(p), &entry(p)) + dist(&exit(last), next_entry(last)) - dist(prev_exit(p), next_entry(last));
for (size_t e : { entry_idx(p), exit_idx(last) })
for (size_t q : neighbors[e]) {
if (! closer(end_points[e], q, removal_gain))
break;
const size_t m = pos[q >> 1];
if ((m < p || m > last) && try_oropt(p, last, is_exit(q) ? m : gap_before(m, p, last)))
return true;
}
}
return false;
};
// A new link can make room for a run that sits elsewhere, hence the sweeps until one finds no move. Every move shortens
// the chain, so the search ends; the cap only bounds its time.
for (size_t k = 0; k < n; ++ k)
enqueue(k);
bool moved_since_sweep = false;
for (size_t moves = 0; moves < 50 * n;) {
if (queue.empty()) {
if (! moved_since_sweep)
break;
moved_since_sweep = false;
for (size_t k = 0; k < n; ++ k)
enqueue(k);
}
const size_t segment = queue.front();
queue.pop_front();
queued[segment] = 0;
if (improve_at(pos[segment])) {
++ moves;
moved_since_sweep = true;
enqueue(pos[segment]);
}
}
for (size_t k = 0; k < n; ++ k)
chain[k] = { order[k], flipped[order[k]] != 0 };
}
// Chain that always goes on to the nearest free end point, from start.
static std::vector<std::pair<size_t, bool>> chain_nearest_next(const std::vector<Vec2d> &end_points, const std::vector<char> &could_reverse, const Vec2d &start)
{
const NearestPoints<1> nearest(end_points);
std::vector<char> taken(could_reverse.size(), 0);
std::vector<std::pair<size_t, bool>> out;
out.reserve(could_reverse.size());
for (Vec2d at = start; out.size() < could_reverse.size();) {
const size_t e = nearest.query(at, [&](size_t idx) { return ! taken[idx >> 1] && ((idx & 1) == 0 || could_reverse[idx >> 1]); })[0];
taken[e >> 1] = 1;
out.emplace_back(e >> 1, (e & 1) != 0);
at = end_points[e ^ 1];
}
return out;
}
static double chain_travel(const std::vector<std::pair<size_t, bool>> &chain, const std::vector<Vec2d> &end_points, const Vec2d &start)
{
double travel = 0.;
Vec2d at = start;
for (const std::pair<size_t, bool> &segment : chain) {
travel += (end_points[2 * segment.first + segment.second] - at).norm();
at = end_points[2 * segment.first + 1 - segment.second];
}
return travel;
}
template<typename EntityPtr>
static std::vector<std::pair<size_t, bool>> chain_extrusion_entities_impl(const std::vector<EntityPtr> &entities, const Point *start_near, bool improve = false)
{
auto segment_end_point = [&entities](size_t idx, bool first_point) -> Point { return first_point ? entities[idx]->first_point() : entities[idx]->last_point(); };
auto could_reverse = [&entities](size_t idx) { const ExtrusionEntity *ee = entities[idx]; return ee->is_loop() || ee->can_reverse(); };
std::vector<std::pair<size_t, bool>> out = chain_segments_greedy_constrained_reversals<Point, decltype(segment_end_point), decltype(could_reverse)>(segment_end_point, could_reverse, entities.size(), start_near);
if (improve && out.size() > 1) {
std::vector<Vec2d> end_points;
std::vector<char> reversible;
end_points.reserve(2 * entities.size());
reversible.reserve(entities.size());
for (size_t idx = 0; idx < entities.size(); ++ idx) {
end_points.emplace_back(segment_end_point(idx, true).template cast<double>());
end_points.emplace_back(segment_end_point(idx, false).template cast<double>());
reversible.emplace_back(could_reverse(idx));
}
Vec2d start = Vec2d::Zero();
if (start_near != nullptr)
start = start_near->cast<double>();
if (entities.size() <= 8) {
// Few paths are chained exactly; the local search below can stop short of the shortest chain.
out = chain_segments_exact(end_points, reversible, start_near != nullptr ? &start : nullptr);
} else {
improve_chain_by_local_search(out, end_points, reversible, start_near != nullptr ? &start : nullptr);
if (start_near != nullptr) {
// The greedy chain can settle into a sweep that local moves cannot undo, which a nearest-next chain may avoid.
std::vector<std::pair<size_t, bool>> nearest_next = chain_nearest_next(end_points, reversible, start);
if (chain_travel(nearest_next, end_points, start) < chain_travel(out, end_points, start) - SCALED_EPSILON) {
improve_chain_by_local_search(nearest_next, end_points, reversible, &start);
out = std::move(nearest_next);
}
}
}
}
for (std::pair<size_t, bool> &segment : out) {
const ExtrusionEntity *ee = entities[segment.first];
if (ee->is_loop())
// Ignore reversals for loops, as the start point equals the end point.
segment.second = false;
// Is can_reverse() respected by the reversals?
assert(ee->can_reverse() || ! segment.second);
}
return out;
}
std::vector<std::pair<size_t, bool>> chain_extrusion_entities(std::vector<ExtrusionEntity*> &entities, const Point *start_near)
{
return chain_extrusion_entities_impl(entities, start_near);
}
// Orca: Reordering queries first_point() / last_point(); drop entities that cannot provide valid endpoints.
template<typename EntityPtr>
static void remove_entities_without_endpoints(std::vector<EntityPtr> &entities)
{
entities.erase(std::remove_if(entities.begin(), entities.end(),
[](const ExtrusionEntity *entity) { return !extrusion_entity_has_endpoints(entity); }),
entities.end());
}
void reorder_extrusion_entities(std::vector<ExtrusionEntity*> &entities, const std::vector<std::pair<size_t, bool>> &chain)
{
assert(entities.size() == chain.size());
std::vector<ExtrusionEntity*> out;
out.reserve(entities.size());
for (const std::pair<size_t, bool> &idx : chain) {
assert(entities[idx.first] != nullptr);
out.emplace_back(entities[idx.first]);
if (idx.second)
out.back()->reverse();
}
entities.swap(out);
}
void chain_and_reorder_extrusion_entities(std::vector<ExtrusionEntity*> &entities, const Point &start_near)
{
remove_entities_without_endpoints(entities);
reorder_extrusion_entities(entities, chain_extrusion_entities_impl(entities, &start_near, true));
}
void chain_and_reorder_extrusion_entities(std::vector<ExtrusionEntity*> &entities, const Point *start_near)
{
remove_entities_without_endpoints(entities);
reorder_extrusion_entities(entities, chain_extrusion_entities(entities, start_near));
}
void chain_and_reorder_extrusion_entities(std::vector<const ExtrusionEntity*> &entities, const Point &start_near,
std::vector<std::unique_ptr<ExtrusionEntity>> &reversed_clones)
{
remove_entities_without_endpoints(entities);
std::vector<const ExtrusionEntity*> out;
out.reserve(entities.size());
for (const auto &[idx, reverse] : chain_extrusion_entities_impl(entities, &start_near, true)) {
if (reverse) {
ExtrusionEntity *clone = reversed_clones.emplace_back(entities[idx]->clone()).get();
clone->reverse();
out.emplace_back(clone);
} else
out.emplace_back(entities[idx]);
}
entities.swap(out);
}
std::vector<std::pair<size_t, bool>> chain_extrusion_paths(std::vector<ExtrusionPath> &extrusion_paths, const Point *start_near)
{
auto segment_end_point = [&extrusion_paths](size_t idx, bool first_point) -> Point { return first_point ? extrusion_paths[idx].first_point() : extrusion_paths[idx].last_point(); };
return chain_segments_greedy<Point, decltype(segment_end_point)>(segment_end_point, extrusion_paths.size(), start_near);
}
void reorder_extrusion_paths(std::vector<ExtrusionPath> &extrusion_paths, const std::vector<std::pair<size_t, bool>> &chain)
{
assert(extrusion_paths.size() == chain.size());
if(extrusion_paths.empty()) return;
std::vector<ExtrusionPath> out;
out.reserve(extrusion_paths.size());
for (const std::pair<size_t, bool> &idx : chain) {
out.emplace_back(std::move(extrusion_paths[idx.first]));
if (idx.second)
out.back().reverse();
}
extrusion_paths.swap(out);
}
void chain_and_reorder_extrusion_paths(std::vector<ExtrusionPath> &extrusion_paths, const Point *start_near)
{
reorder_extrusion_paths(extrusion_paths, chain_extrusion_paths(extrusion_paths, start_near));
}
std::vector<size_t> chain_expolygons(const ExPolygons &input_exploy) {
Points points;
for (const ExPolygon &exploy : input_exploy) {
BoundingBox bbox;
bbox = get_extents(exploy);
points.push_back(bbox.center());
}
return chain_points(points);
}
std::vector<size_t> chain_points(const Points &points, const Point *start_near)
{
auto segment_end_point = [&points](size_t idx, bool /* first_point */) -> const Point& { return points[idx]; };
std::vector<std::pair<size_t, bool>> ordered = chain_segments_greedy<Point, decltype(segment_end_point)>(segment_end_point, points.size(), start_near);
std::vector<size_t> out;
out.reserve(ordered.size());
for (auto &segment_and_reversal : ordered)
out.emplace_back(segment_and_reversal.first);
return out;
}
// Moves runs of up to three points of the cycle next to one of the 8 points nearest to an end of the run, reversed or not,
// while that shortens the cycle. path[0] stays first. Returns true if any move was made.
static bool improve_cycle_by_or_opt(std::vector<size_t> &path, const Points &points)
{
const size_t pn = path.size();
if (pn < 5)
return false;
std::vector<Vec2d> coords(points.size());
for (size_t i = 0; i < points.size(); ++ i)
coords[i] = points[i].cast<double>();
const NearestPoints<8> grid(coords);
std::vector<size_t> pos(points.size());
auto refresh = [&]() { for (size_t k = 0; k < pn; ++ k) pos[path[k]] = k; };
refresh();
auto dist = [&](size_t a, size_t b) { return (coords[path[a % pn]] - coords[path[b % pn]]).norm(); };
bool improved = false;
for (bool moved = true; moved;) {
moved = false;
for (size_t len = 1; len <= 3; ++ len)
for (size_t i = 1; i + len <= pn; ++ i) {
// The run path[i..last] goes between path[k] and path[k + 1].
const size_t last = i + len - 1;
const double removal = dist(i - 1, i) + dist(last, last + 1) - dist(i - 1, last + 1);
bool applied = removal <= SCALED_EPSILON;
for (size_t end : { path[i], path[last] })
for (size_t q : grid.query(coords[end], [end](size_t idx) { return idx != end; }))
for (size_t k : { q == NearestPoints<8>::npos ? pn : (pos[q] + pn - 1) % pn, q == NearestPoints<8>::npos ? pn : pos[q] }) {
if (applied || k == pn || (k + 1 >= i && k <= last))
continue;
const double base = dist(k, k + 1);
const double kept = dist(k, i) + dist(last, k + 1) - base;
const double reversed = dist(k, last) + dist(i, k + 1) - base;
if (std::min(kept, reversed) > removal - SCALED_EPSILON)
continue;
std::vector<size_t> run(path.begin() + i, path.begin() + last + 1);
if (reversed < kept)
std::reverse(run.begin(), run.end());
path.erase(path.begin() + i, path.begin() + last + 1);
path.insert(path.begin() + (k < i ? k + 1 : k + 1 - len), run.begin(), run.end());
refresh();
applied = moved = improved = true;
}
}
}
return improved;
}
std::vector<size_t> chain_points_with_postprocessing(const Points &points, const Point *start_near)
{
std::vector<size_t> path;
if (points.size() <= 10) {
// Orca: few points are toured exactly, from the point nearest start_near toward its nearer neighbor.
path = tsp_exact_cycle(points);
if (start_near != nullptr && path.size() > 2) {
auto dist = [&points](size_t a, const Point &b) { return (points[a] - b).cast<double>().squaredNorm(); };
std::rotate(path.begin(), std::min_element(path.begin(), path.end(), [&](size_t a, size_t b) { return dist(a, *start_near) < dist(b, *start_near); }), path.end());
if (dist(path.back(), points[path.front()]) < dist(path[1], points[path.front()]))
std::reverse(path.begin() + 1, path.end());
}
} else {
path = chain_points(points, start_near);
// Alternate 2-opt, or-opt and crossing removal: each can open moves for the others. Break early if none improves.
for (int iter = 0; iter < 3; ++iter) {
bool improved = tsp_2opt_improve(path, points);
improved |= improve_cycle_by_or_opt(path, points);
improved |= tsp_remove_crossings(path, points);
if (!improved) break;
}
}
if (start_near == nullptr)
tsp_rotate_minimize_closing(path, points);
return path;
}
#ifndef NDEBUG
// #define DEBUG_SVG_OUTPUT
#endif /* NDEBUG */
#ifdef DEBUG_SVG_OUTPUT
void svg_draw_polyline_chain(const char *name, size_t idx, const Polylines &polylines)
{
BoundingBox bbox = get_extents(polylines);
SVG svg(debug_out_path("%s-%d.svg", name, idx).c_str(), bbox);
svg.draw(polylines);
for (size_t i = 1; i < polylines.size(); ++i)
svg.draw(Line(polylines[i - 1].last_point(), polylines[i].first_point()), "red");
}
#endif /* DEBUG_SVG_OUTPUT */
#if 0
// Flip the sequences of polylines to lower the total length of connecting lines.
static inline void improve_ordering_by_segment_flipping(Polylines &polylines, bool fixed_start)
{
#ifndef NDEBUG
auto cost = [&polylines]() {
double sum = 0.;
for (size_t i = 1; i < polylines.size(); ++i)
sum += (polylines[i].first_point() - polylines[i - 1].last_point()).cast<double>().norm();
return sum;
};
double cost_initial = cost();
static int iRun = 0;
++ iRun;
#ifdef DEBUG_SVG_OUTPUT
svg_draw_polyline_chain("improve_ordering_by_segment_flipping-initial", iRun, polylines);
#endif /* DEBUG_SVG_OUTPUT */
#endif /* NDEBUG */
struct Connection {
Connection(size_t heap_idx = std::numeric_limits<size_t>::max(), bool flipped = false) : heap_idx(heap_idx), flipped(flipped) {}
// Position of this object on MutablePriorityHeap.
size_t heap_idx;
// Is segment_idx flipped?
bool flipped;
double squaredNorm(const Polylines &polylines, const std::vector<Connection> &connections) const
{ return ((this + 1)->start_point(polylines, connections) - this->end_point(polylines, connections)).squaredNorm(); }
double norm(const Polylines &polylines, const std::vector<Connection> &connections) const
{ return sqrt(this->squaredNorm(polylines, connections)); }
double squaredNorm(const Polylines &polylines, const std::vector<Connection> &connections, bool try_flip1, bool try_flip2) const
{ return ((this + 1)->start_point(polylines, connections, try_flip2) - this->end_point(polylines, connections, try_flip1)).squaredNorm(); }
double norm(const Polylines &polylines, const std::vector<Connection> &connections, bool try_flip1, bool try_flip2) const
{ return sqrt(this->squaredNorm(polylines, connections, try_flip1, try_flip2)); }
Vec2d start_point(const Polylines &polylines, const std::vector<Connection> &connections, bool flip = false) const
{ const Polyline &pl = polylines[this - connections.data()]; return ((this->flipped == flip) ? pl.points.front() : pl.points.back()).cast<double>(); }
Vec2d end_point(const Polylines &polylines, const std::vector<Connection> &connections, bool flip = false) const
{ const Polyline &pl = polylines[this - connections.data()]; return ((this->flipped == flip) ? pl.points.back() : pl.points.front()).cast<double>(); }
bool in_queue() const { return this->heap_idx != std::numeric_limits<size_t>::max(); }
void flip() { this->flipped = ! this->flipped; }
};
std::vector<Connection> connections(polylines.size());
#ifndef NDEBUG
auto cost_flipped = [fixed_start, &polylines, &connections]() {
assert(! fixed_start || ! connections.front().flipped);
double sum = 0.;
for (size_t i = 1; i < polylines.size(); ++ i)
sum += connections[i - 1].norm(polylines, connections);
return sum;
};
double cost_prev = cost_flipped();
assert(std::abs(cost_initial - cost_prev) < SCALED_EPSILON);
auto print_statistics = [&polylines, &connections]() {
#if 0
for (size_t i = 1; i < polylines.size(); ++ i)
printf("Connecting %d with %d: Current length %lf flip(%d, %d), left flipped: %lf, right flipped: %lf, both flipped: %lf, \n",
int(i - 1), int(i),
unscale<double>(connections[i - 1].norm(polylines, connections)),
int(connections[i - 1].flipped), int(connections[i].flipped),
unscale<double>(connections[i - 1].norm(polylines, connections, true, false)),
unscale<double>(connections[i - 1].norm(polylines, connections, false, true)),
unscale<double>(connections[i - 1].norm(polylines, connections, true, true)));
#endif
};
print_statistics();
#endif /* NDEBUG */
// Initialize a MutablePriorityHeap of connections between polylines.
auto queue = make_mutable_priority_queue<Connection*, false>(
[](Connection *connection, size_t idx){ connection->heap_idx = idx; },
// Sort by decreasing connection distance.
[&polylines, &connections](Connection *l, Connection *r){ return l->squaredNorm(polylines, connections) > r->squaredNorm(polylines, connections); });
queue.reserve(polylines.size() - 1);
for (size_t i = 0; i + 1 < polylines.size(); ++ i)
queue.push(&connections[i]);
static constexpr size_t itercnt = 100;
size_t iter = 0;
for (; ! queue.empty() && iter < itercnt; ++ iter) {
Connection &connection = *queue.top();
queue.pop();
connection.heap_idx = std::numeric_limits<size_t>::max();
size_t idx_first = &connection - connections.data();
// Try to flip segments starting with idx_first + 1 to the end.
// Calculate the last segment to be flipped to improve the total path length.
double length_current = connection.norm(polylines, connections);
double length_flipped = connection.norm(polylines, connections, false, true);
int best_idx_forward = int(idx_first);
double best_improvement_forward = 0.;
for (size_t i = idx_first + 1; i + 1 < connections.size(); ++ i) {
length_current += connections[i].norm(polylines, connections);
double this_improvement = length_current - length_flipped - connections[i].norm(polylines, connections, true, false);
length_flipped += connections[i].norm(polylines, connections, true, true);
if (this_improvement > best_improvement_forward) {
best_improvement_forward = this_improvement;
best_idx_forward = int(i);
}
// if (length_flipped > 1.5 * length_current)
// break;
}
if (length_current - length_flipped > best_improvement_forward)
// Best improvement by flipping up to the end.
best_idx_forward = int(connections.size()) - 1;
// Try to flip segments starting with idx_first - 1 to the start.
// Calculate the last segment to be flipped to improve the total path length.
length_current = connection.norm(polylines, connections);
length_flipped = connection.norm(polylines, connections, true, false);
int best_idx_backwards = int(idx_first);
double best_improvement_backwards = 0.;
for (int i = int(idx_first) - 1; i >= 0; -- i) {
length_current += connections[i].norm(polylines, connections);
double this_improvement = length_current - length_flipped - connections[i].norm(polylines, connections, false, true);
length_flipped += connections[i].norm(polylines, connections, true, true);
if (this_improvement > best_improvement_backwards) {
best_improvement_backwards = this_improvement;
best_idx_backwards = int(i);
}
// if (length_flipped > 1.5 * length_current)
// break;
}
if (! fixed_start && length_current - length_flipped > best_improvement_backwards)
// Best improvement by flipping up to the start including the first polyline.
best_idx_backwards = -1;
int update_begin = int(idx_first);
int update_end = best_idx_forward;
if (best_improvement_backwards > 0. && best_improvement_backwards > best_improvement_forward) {
// Flip the sequence of polylines from idx_first to best_improvement_forward + 1.
update_begin = best_idx_backwards;
update_end = int(idx_first);
}
assert(update_begin <= update_end);
if (update_begin == update_end)
continue;
for (int i = update_begin + 1; i <= update_end; ++ i)
connections[i].flip();
#ifndef NDEBUG
double cost = cost_flipped();
assert(cost < cost_prev);
cost_prev = cost;
print_statistics();
#endif /* NDEBUG */
update_end = std::min(update_end + 1, int(connections.size()) - 1);
for (int i = std::max(0, update_begin); i < update_end; ++ i) {
Connection &c = connections[i];
if (c.in_queue())
queue.update(c.heap_idx);
else
queue.push(&c);
}
}
// Flip the segments based on the flip flag.
for (Polyline &pl : polylines)
if (connections[&pl - polylines.data()].flipped)
pl.reverse();
#ifndef NDEBUG
double cost_final = cost();
#ifdef DEBUG_SVG_OUTPUT
svg_draw_polyline_chain("improve_ordering_by_segment_flipping-final", iRun, polylines);
#endif /* DEBUG_SVG_OUTPUT */
assert(cost_final <= cost_prev);
assert(cost_final <= cost_initial);
#endif /* NDEBUG */
}
#endif
struct FlipEdge {
FlipEdge(const Vec2d &p1, const Vec2d &p2, size_t source_index) : p1(p1), p2(p2), source_index(source_index) {}
void flip() { std::swap(this->p1, this->p2); }
Vec2d p1;
Vec2d p2;
size_t source_index;
};
struct ConnectionCost {
ConnectionCost(double cost, double cost_flipped) : cost(cost), cost_flipped(cost_flipped) {}
ConnectionCost() : cost(0.), cost_flipped(0.) {}
void flip() { std::swap(this->cost, this->cost_flipped); }
double cost = 0;
double cost_flipped = 0;
};
static inline ConnectionCost operator-(const ConnectionCost &lhs, const ConnectionCost& rhs) { return ConnectionCost(lhs.cost - rhs.cost, lhs.cost_flipped - rhs.cost_flipped); }
static inline std::pair<double, size_t> minimum_crossover_cost(
const std::vector<FlipEdge> &edges,
const std::pair<size_t, size_t> &span1, const ConnectionCost &cost1,
const std::pair<size_t, size_t> &span2, const ConnectionCost &cost2,
const std::pair<size_t, size_t> &span3, const ConnectionCost &cost3,
const double cost_current)
{
auto connection_cost = [&edges](
const std::pair<size_t, size_t> &span1, const ConnectionCost &cost1, bool reversed1, bool flipped1,
const std::pair<size_t, size_t> &span2, const ConnectionCost &cost2, bool reversed2, bool flipped2,
const std::pair<size_t, size_t> &span3, const ConnectionCost &cost3, bool reversed3, bool flipped3) {
auto first_point = [&edges](const std::pair<size_t, size_t> &span, bool flipped) { return flipped ? edges[span.first].p2 : edges[span.first].p1; };
auto last_point = [&edges](const std::pair<size_t, size_t> &span, bool flipped) { return flipped ? edges[span.second - 1].p1 : edges[span.second - 1].p2; };
auto point = [first_point, last_point](const std::pair<size_t, size_t> &span, bool start, bool flipped) { return start ? first_point(span, flipped) : last_point(span, flipped); };
auto cost = [](const ConnectionCost &acost, bool flipped) {
assert(acost.cost >= 0. && acost.cost_flipped >= 0.);
return flipped ? acost.cost_flipped : acost.cost;
};
// Ignore reversed single segment spans.
auto simple_span_ignore = [](const std::pair<size_t, size_t>& span, bool reversed) {
return span.first + 1 == span.second && reversed;
};
assert(span1.first < span1.second);
assert(span2.first < span2.second);
assert(span3.first < span3.second);
return
simple_span_ignore(span1, reversed1) || simple_span_ignore(span2, reversed2) || simple_span_ignore(span3, reversed3) ?
// Don't perform unnecessary calculations simulating reversion of single segment spans.
std::numeric_limits<double>::max() :
// Calculate the cost of reverting chains and / or flipping segment orientations.
cost(cost1, flipped1) + cost(cost2, flipped2) + cost(cost3, flipped3) +
(point(span2, ! reversed2, flipped2) - point(span1, reversed1, flipped1)).norm() +
(point(span3, ! reversed3, flipped3) - point(span2, reversed2, flipped2)).norm();
};
#ifndef NDEBUG
{
double c = connection_cost(span1, cost1, false, false, span2, cost2, false, false, span3, cost3, false, false);
assert(std::abs(c - cost_current) < SCALED_EPSILON);
}
#endif /* NDEBUG */
double cost_min = cost_current;
size_t flip_min = 0; // no flip, no improvement
for (size_t i = 0; i < (1 << 6); ++ i) {
// From the three combinations of 1,2,3 ordering, the other three are reversals of the first three.
double c1 = (i == 0) ? cost_current :
connection_cost(span1, cost1, (i & 1) != 0, (i & (1 << 1)) != 0, span2, cost2, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, span3, cost3, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0);
double c2 = connection_cost(span1, cost1, (i & 1) != 0, (i & (1 << 1)) != 0, span3, cost3, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, span2, cost2, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0);
double c3 = connection_cost(span2, cost2, (i & 1) != 0, (i & (1 << 1)) != 0, span1, cost1, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, span3, cost3, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0);
if (c1 < cost_min) {
cost_min = c1;
flip_min = i;
}
if (c2 < cost_min) {
cost_min = c2;
flip_min = i + (1 << 6);
}
if (c3 < cost_min) {
cost_min = c3;
flip_min = i + (2 << 6);
}
}
return std::make_pair(cost_min, flip_min);
}
#if 0
static inline std::pair<double, size_t> minimum_crossover_cost(
const std::vector<FlipEdge> &edges,
const std::pair<size_t, size_t> &span1, const ConnectionCost &cost1,
const std::pair<size_t, size_t> &span2, const ConnectionCost &cost2,
const std::pair<size_t, size_t> &span3, const ConnectionCost &cost3,
const std::pair<size_t, size_t> &span4, const ConnectionCost &cost4,
const double cost_current)
{
auto connection_cost = [&edges](
const std::pair<size_t, size_t> &span1, const ConnectionCost &cost1, bool reversed1, bool flipped1,
const std::pair<size_t, size_t> &span2, const ConnectionCost &cost2, bool reversed2, bool flipped2,
const std::pair<size_t, size_t> &span3, const ConnectionCost &cost3, bool reversed3, bool flipped3,
const std::pair<size_t, size_t> &span4, const ConnectionCost &cost4, bool reversed4, bool flipped4) {
auto first_point = [&edges](const std::pair<size_t, size_t> &span, bool flipped) { return flipped ? edges[span.first].p2 : edges[span.first].p1; };
auto last_point = [&edges](const std::pair<size_t, size_t> &span, bool flipped) { return flipped ? edges[span.second - 1].p1 : edges[span.second - 1].p2; };
auto point = [first_point, last_point](const std::pair<size_t, size_t> &span, bool start, bool flipped) { return start ? first_point(span, flipped) : last_point(span, flipped); };
auto cost = [](const ConnectionCost &acost, bool flipped) {
assert(acost.cost >= 0. && acost.cost_flipped >= 0.);
return flipped ? acost.cost_flipped : acost.cost;
};
// Ignore reversed single segment spans.
auto simple_span_ignore = [](const std::pair<size_t, size_t>& span, bool reversed) {
return span.first + 1 == span.second && reversed;
};
assert(span1.first < span1.second);
assert(span2.first < span2.second);
assert(span3.first < span3.second);
assert(span4.first < span4.second);
return
simple_span_ignore(span1, reversed1) || simple_span_ignore(span2, reversed2) || simple_span_ignore(span3, reversed3) || simple_span_ignore(span4, reversed4) ?
// Don't perform unnecessary calculations simulating reversion of single segment spans.
std::numeric_limits<double>::max() :
// Calculate the cost of reverting chains and / or flipping segment orientations.
cost(cost1, flipped1) + cost(cost2, flipped2) + cost(cost3, flipped3) + cost(cost4, flipped4) +
(point(span2, ! reversed2, flipped2) - point(span1, reversed1, flipped1)).norm() +
(point(span3, ! reversed3, flipped3) - point(span2, reversed2, flipped2)).norm() +
(point(span4, ! reversed4, flipped4) - point(span3, reversed3, flipped3)).norm();
};
#ifndef NDEBUG
{
double c = connection_cost(span1, cost1, false, false, span2, cost2, false, false, span3, cost3, false, false, span4, cost4, false, false);
assert(std::abs(c - cost_current) < SCALED_EPSILON);
}
#endif /* NDEBUG */
double cost_min = cost_current;
size_t flip_min = 0; // no flip, no improvement
for (size_t i = 0; i < (1 << 8); ++ i) {
// From the three combinations of 1,2,3 ordering, the other three are reversals of the first three.
size_t permutation = 0;
for (double c : {
(i == 0) ? cost_current :
connection_cost(span1, cost1, (i & 1) != 0, (i & (1 << 1)) != 0, span2, cost2, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, span3, cost3, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0, span4, cost4, (i & (1 << 6)) != 0, (i & (1 << 7)) != 0),
connection_cost(span1, cost1, (i & 1) != 0, (i & (1 << 1)) != 0, span2, cost2, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, span4, cost4, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0, span3, cost3, (i & (1 << 6)) != 0, (i & (1 << 7)) != 0),
connection_cost(span1, cost1, (i & 1) != 0, (i & (1 << 1)) != 0, span3, cost3, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, span2, cost2, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0, span4, cost4, (i & (1 << 6)) != 0, (i & (1 << 7)) != 0),
connection_cost(span1, cost1, (i & 1) != 0, (i & (1 << 1)) != 0, span3, cost3, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, span4, cost4, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0, span2, cost2, (i & (1 << 6)) != 0, (i & (1 << 7)) != 0),
connection_cost(span1, cost1, (i & 1) != 0, (i & (1 << 1)) != 0, span4, cost4, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, span2, cost2, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0, span3, cost3, (i & (1 << 6)) != 0, (i & (1 << 7)) != 0),
connection_cost(span1, cost1, (i & 1) != 0, (i & (1 << 1)) != 0, span4, cost4, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, span3, cost3, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0, span2, cost2, (i & (1 << 6)) != 0, (i & (1 << 7)) != 0),
connection_cost(span2, cost2, (i & 1) != 0, (i & (1 << 1)) != 0, span1, cost1, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, span3, cost3, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0, span4, cost4, (i & (1 << 6)) != 0, (i & (1 << 7)) != 0),
connection_cost(span2, cost2, (i & 1) != 0, (i & (1 << 1)) != 0, span1, cost1, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, span4, cost4, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0, span3, cost3, (i & (1 << 6)) != 0, (i & (1 << 7)) != 0),
connection_cost(span2, cost2, (i & 1) != 0, (i & (1 << 1)) != 0, span3, cost3, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, span1, cost1, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0, span4, cost4, (i & (1 << 6)) != 0, (i & (1 << 7)) != 0),
connection_cost(span2, cost2, (i & 1) != 0, (i & (1 << 1)) != 0, span4, cost4, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, span1, cost1, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0, span3, cost3, (i & (1 << 6)) != 0, (i & (1 << 7)) != 0),
connection_cost(span3, cost3, (i & 1) != 0, (i & (1 << 1)) != 0, span1, cost1, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, span2, cost2, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0, span4, cost4, (i & (1 << 6)) != 0, (i & (1 << 7)) != 0),
connection_cost(span3, cost3, (i & 1) != 0, (i & (1 << 1)) != 0, span2, cost2, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, span1, cost1, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0, span4, cost4, (i & (1 << 6)) != 0, (i & (1 << 7)) != 0)
}) {
if (c < cost_min) {
cost_min = c;
flip_min = i + (permutation << 8);
}
++ permutation;
}
}
return std::make_pair(cost_min, flip_min);
}
#endif
static inline void do_crossover(const std::vector<FlipEdge> &edges_in, std::vector<FlipEdge> &edges_out,
const std::pair<size_t, size_t> &span1, const std::pair<size_t, size_t> &span2, const std::pair<size_t, size_t> &span3,
size_t i)
{
assert(edges_in.size() == edges_out.size());
auto do_it = [&edges_in, &edges_out](
const std::pair<size_t, size_t> &span1, bool reversed1, bool flipped1,
const std::pair<size_t, size_t> &span2, bool reversed2, bool flipped2,
const std::pair<size_t, size_t> &span3, bool reversed3, bool flipped3) {
auto it_edges_out = edges_out.begin();
auto copy_span = [&edges_in, &it_edges_out](std::pair<size_t, size_t> span, bool reversed, bool flipped) {
assert(span.first < span.second);
auto it = it_edges_out;
if (reversed)
std::reverse_copy(edges_in.begin() + span.first, edges_in.begin() + span.second, it_edges_out);
else
std::copy (edges_in.begin() + span.first, edges_in.begin() + span.second, it_edges_out);
it_edges_out += span.second - span.first;
if (reversed != flipped) {
for (; it != it_edges_out; ++ it)
it->flip();
}
};
copy_span(span1, reversed1, flipped1);
copy_span(span2, reversed2, flipped2);
copy_span(span3, reversed3, flipped3);
};
switch (i >> 6) {
case 0:
do_it(span1, (i & 1) != 0, (i & (1 << 1)) != 0, span2, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, span3, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0);
break;
case 1:
do_it(span1, (i & 1) != 0, (i & (1 << 1)) != 0, span3, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, span2, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0);
break;
default:
assert((i >> 6) == 2);
do_it(span2, (i & 1) != 0, (i & (1 << 1)) != 0, span1, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, span3, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0);
}
assert(edges_in.size() == edges_out.size());
}
#if 0
static inline void do_crossover(const std::vector<FlipEdge> &edges_in, std::vector<FlipEdge> &edges_out,
const std::pair<size_t, size_t> &span1, const std::pair<size_t, size_t> &span2, const std::pair<size_t, size_t> &span3, const std::pair<size_t, size_t> &span4,
size_t i)
{
assert(edges_in.size() == edges_out.size());
auto do_it = [&edges_in, &edges_out](
const std::pair<size_t, size_t> &span1, bool reversed1, bool flipped1,
const std::pair<size_t, size_t> &span2, bool reversed2, bool flipped2,
const std::pair<size_t, size_t> &span3, bool reversed3, bool flipped3,
const std::pair<size_t, size_t> &span4, bool reversed4, bool flipped4) {
auto it_edges_out = edges_out.begin();
auto copy_span = [&edges_in, &it_edges_out](std::pair<size_t, size_t> span, bool reversed, bool flipped) {
assert(span.first < span.second);
auto it = it_edges_out;
if (reversed)
std::reverse_copy(edges_in.begin() + span.first, edges_in.begin() + span.second, it_edges_out);
else
std::copy (edges_in.begin() + span.first, edges_in.begin() + span.second, it_edges_out);
it_edges_out += span.second - span.first;
if (reversed != flipped) {
for (; it != it_edges_out; ++ it)
it->flip();
}
};
copy_span(span1, reversed1, flipped1);
copy_span(span2, reversed2, flipped2);
copy_span(span3, reversed3, flipped3);
copy_span(span4, reversed4, flipped4);
};
switch (i >> 8) {
case 0:
assert(i != 0); // otherwise it would be a no-op
do_it(span1, (i & 1) != 0, (i & (1 << 1)) != 0, span2, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, span3, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0, span4, (i & (1 << 6)) != 0, (i & (1 << 7)) != 0);
break;
case 1:
do_it(span1, (i & 1) != 0, (i & (1 << 1)) != 0, span2, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, span4, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0, span3, (i & (1 << 6)) != 0, (i & (1 << 7)) != 0);
break;
case 2:
do_it(span1, (i & 1) != 0, (i & (1 << 1)) != 0, span3, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, span2, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0, span4, (i & (1 << 6)) != 0, (i & (1 << 7)) != 0);
break;
case 3:
do_it(span1, (i & 1) != 0, (i & (1 << 1)) != 0, span3, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, span4, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0, span2, (i & (1 << 6)) != 0, (i & (1 << 7)) != 0);
break;
case 4:
do_it(span1, (i & 1) != 0, (i & (1 << 1)) != 0, span4, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, span2, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0, span3, (i & (1 << 6)) != 0, (i & (1 << 7)) != 0);
break;
case 5:
do_it(span1, (i & 1) != 0, (i & (1 << 1)) != 0, span4, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, span3, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0, span2, (i & (1 << 6)) != 0, (i & (1 << 7)) != 0);
break;
case 6:
do_it(span2, (i & 1) != 0, (i & (1 << 1)) != 0, span1, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, span3, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0, span4, (i & (1 << 6)) != 0, (i & (1 << 7)) != 0);
break;
case 7:
do_it(span2, (i & 1) != 0, (i & (1 << 1)) != 0, span1, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, span4, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0, span3, (i & (1 << 6)) != 0, (i & (1 << 7)) != 0);
break;
case 8:
do_it(span2, (i & 1) != 0, (i & (1 << 1)) != 0, span3, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, span1, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0, span4, (i & (1 << 6)) != 0, (i & (1 << 7)) != 0);
break;
case 9:
do_it(span2, (i & 1) != 0, (i & (1 << 1)) != 0, span4, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, span1, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0, span3, (i & (1 << 6)) != 0, (i & (1 << 7)) != 0);
break;
case 10:
do_it(span3, (i & 1) != 0, (i & (1 << 1)) != 0, span1, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, span2, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0, span4, (i & (1 << 6)) != 0, (i & (1 << 7)) != 0);
break;
default:
assert((i >> 8) == 11);
do_it(span3, (i & 1) != 0, (i & (1 << 1)) != 0, span2, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, span1, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0, span4, (i & (1 << 6)) != 0, (i & (1 << 7)) != 0);
break;
}
assert(edges_in.size() == edges_out.size());
}
#endif
// Worst time complexity: O(min(n, 100) * (n * log n + n^2)
// Expected time complexity: O(min(n, 100) * (n * log n + k * n)
// where n is the number of edges and k is the number of connection_lengths candidates after the first one
// is found that improves the total cost.
//FIXME there are likley better heuristics to lower the time complexity.
static inline void reorder_by_two_exchanges_with_segment_flipping(std::vector<FlipEdge> &edges)
{
if (edges.size() < 2)
return;
std::vector<ConnectionCost> connections(edges.size());
std::vector<FlipEdge> edges_tmp(edges);
std::vector<std::pair<double, size_t>> connection_lengths(edges.size() - 1, std::pair<double, size_t>(0., 0));
std::vector<char> connection_tried(edges.size(), false);
const size_t max_iterations = std::min(edges.size(), size_t(100));
for (size_t iter = 0; iter < max_iterations; ++ iter) {
// Initialize connection costs and connection lengths.
for (size_t i = 1; i < edges.size(); ++ i) {
const FlipEdge &e1 = edges[i - 1];
const FlipEdge &e2 = edges[i];
ConnectionCost &c = connections[i];
c = connections[i - 1];
double l = (e2.p1 - e1.p2).norm();
c.cost += l;
c.cost_flipped += (e2.p2 - e1.p1).norm();
connection_lengths[i - 1] = std::make_pair(l, i);
}
std::sort(connection_lengths.begin(), connection_lengths.end(), [](const std::pair<double, size_t> &l, const std::pair<double, size_t> &r) { return l.first > r.first; });
std::fill(connection_tried.begin(), connection_tried.end(), false);
size_t crossover1_pos_final = std::numeric_limits<size_t>::max();
size_t crossover2_pos_final = std::numeric_limits<size_t>::max();
size_t crossover_flip_final = 0;
for (const std::pair<double, size_t>& first_crossover_candidate : connection_lengths) {
size_t longest_connection_idx = first_crossover_candidate.second;
connection_tried[longest_connection_idx] = true;
// Find the second crossover connection with the lowest total chain cost.
size_t crossover_pos_min = std::numeric_limits<size_t>::max();
double crossover_cost_min = connections.back().cost;
size_t crossover_flip_min = 0;
for (size_t j = 1; j < connections.size(); ++ j)
if (! connection_tried[j]) {
size_t a = j;
size_t b = longest_connection_idx;
if (a > b)
std::swap(a, b);
std::pair<double, size_t> cost_and_flip = minimum_crossover_cost(edges,
std::make_pair(size_t(0), a), connections[a - 1], std::make_pair(a, b), connections[b - 1] - connections[a], std::make_pair(b, edges.size()), connections.back() - connections[b],
connections.back().cost);
if (cost_and_flip.second > 0 && cost_and_flip.first < crossover_cost_min) {
crossover_pos_min = j;
crossover_cost_min = cost_and_flip.first;
crossover_flip_min = cost_and_flip.second;
assert(crossover_cost_min < connections.back().cost + EPSILON);
}
}
if (crossover_cost_min < connections.back().cost) {
// The cost of the chain with the proposed two crossovers has a lower total cost than the current chain. Apply the crossover.
crossover1_pos_final = longest_connection_idx;
crossover2_pos_final = crossover_pos_min;
crossover_flip_final = crossover_flip_min;
break;
} else {
// Continue with another long candidate edge.
}
}
if (crossover_flip_final > 0) {
// Pair of cross over positions and flip / reverse constellation has been found, which improves the total cost of the connection.
// Perform a crossover.
if (crossover1_pos_final > crossover2_pos_final)
std::swap(crossover1_pos_final, crossover2_pos_final);
do_crossover(edges, edges_tmp, std::make_pair(size_t(0), crossover1_pos_final), std::make_pair(crossover1_pos_final, crossover2_pos_final), std::make_pair(crossover2_pos_final, edges.size()), crossover_flip_final);
edges.swap(edges_tmp);
} else {
// No valid pair of cross over positions was found improving the total cost. Giving up.
break;
}
}
}
#if 0
// Currently not used, too slow.
static inline void reorder_by_three_exchanges_with_segment_flipping(std::vector<FlipEdge> &edges)
{
if (edges.size() < 3) {
reorder_by_two_exchanges_with_segment_flipping(edges);
return;
}
std::vector<ConnectionCost> connections(edges.size());
std::vector<FlipEdge> edges_tmp(edges);
std::vector<std::pair<double, size_t>> connection_lengths(edges.size() - 1, std::pair<double, size_t>(0., 0));
std::vector<char> connection_tried(edges.size(), false);
for (size_t iter = 0; iter < edges.size(); ++ iter) {
// Initialize connection costs and connection lengths.
for (size_t i = 1; i < edges.size(); ++ i) {
const FlipEdge &e1 = edges[i - 1];
const FlipEdge &e2 = edges[i];
ConnectionCost &c = connections[i];
c = connections[i - 1];
double l = (e2.p1 - e1.p2).norm();
c.cost += l;
c.cost_flipped += (e2.p2 - e1.p1).norm();
connection_lengths[i - 1] = std::make_pair(l, i);
}
std::sort(connection_lengths.begin(), connection_lengths.end(), [](const std::pair<double, size_t> &l, const std::pair<double, size_t> &r) { return l.first > r.first; });
std::fill(connection_tried.begin(), connection_tried.end(), false);
size_t crossover1_pos_final = std::numeric_limits<size_t>::max();
size_t crossover2_pos_final = std::numeric_limits<size_t>::max();
size_t crossover3_pos_final = std::numeric_limits<size_t>::max();
size_t crossover_flip_final = 0;
for (const std::pair<double, size_t> &first_crossover_candidate : connection_lengths) {
size_t longest_connection_idx = first_crossover_candidate.second;
connection_tried[longest_connection_idx] = true;
// Find the second crossover connection with the lowest total chain cost.
double crossover_cost_min = connections.back().cost;
for (size_t j = 1; j < connections.size(); ++ j)
if (! connection_tried[j]) {
for (size_t k = j + 1; k < connections.size(); ++ k)
if (! connection_tried[k]) {
size_t a = longest_connection_idx;
size_t b = j;
size_t c = k;
if (a > c)
std::swap(a, c);
if (a > b)
std::swap(a, b);
if (b > c)
std::swap(b, c);
std::pair<double, size_t> cost_and_flip = minimum_crossover_cost(edges,
std::make_pair(size_t(0), a), connections[a - 1], std::make_pair(a, b), connections[b - 1] - connections[a],
std::make_pair(b, c), connections[c - 1] - connections[b], std::make_pair(c, edges.size()), connections.back() - connections[c],
connections.back().cost);
if (cost_and_flip.second > 0 && cost_and_flip.first < crossover_cost_min) {
crossover_cost_min = cost_and_flip.first;
crossover1_pos_final = a;
crossover2_pos_final = b;
crossover3_pos_final = c;
crossover_flip_final = cost_and_flip.second;
assert(crossover_cost_min < connections.back().cost + EPSILON);
}
}
}
if (crossover_flip_final > 0) {
// The cost of the chain with the proposed two crossovers has a lower total cost than the current chain. Apply the crossover.
break;
} else {
// Continue with another long candidate edge.
}
}
if (crossover_flip_final > 0) {
// Pair of cross over positions and flip / reverse constellation has been found, which improves the total cost of the connection.
// Perform a crossover.
do_crossover(edges, edges_tmp, std::make_pair(size_t(0), crossover1_pos_final), std::make_pair(crossover1_pos_final, crossover2_pos_final),
std::make_pair(crossover2_pos_final, crossover3_pos_final), std::make_pair(crossover3_pos_final, edges.size()), crossover_flip_final);
edges.swap(edges_tmp);
} else {
// No valid pair of cross over positions was found improving the total cost. Giving up.
break;
}
}
}
#endif
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::DontAlign> Matrixd;
class FourOptCosts {
public:
FourOptCosts(const ConnectionCost &c1, const ConnectionCost &c2, const ConnectionCost &c3, const ConnectionCost &c4) : costs { &c1, &c2, &c3, &c4 } {}
double operator()(size_t piece_idx, bool flipped) const { return flipped ? costs[piece_idx]->cost_flipped : costs[piece_idx]->cost; }
private:
const ConnectionCost* costs[4];
};
#if 0
static inline std::pair<double, size_t> minimum_crossover_cost(
const FourOptCosts &segment_costs,
const Matrixd &segment_end_point_distance_matrix,
const double cost_current)
{
// Distance from the end of span1 to the start of span2.
auto end_point_distance = [&segment_end_point_distance_matrix](size_t span1, bool reversed1, bool flipped1, size_t span2, bool reversed2, bool flipped2) {
return segment_end_point_distance_matrix(span1 * 4 + (! reversed1) * 2 + flipped1, span2 * 4 + reversed2 * 2 + flipped2);
};
auto connection_cost = [&segment_costs, end_point_distance](
const size_t span1, bool reversed1, bool flipped1,
const size_t span2, bool reversed2, bool flipped2,
const size_t span3, bool reversed3, bool flipped3,
const size_t span4, bool reversed4, bool flipped4) {
// Calculate the cost of reverting chains and / or flipping segment orientations.
return segment_costs(span1, flipped1) + segment_costs(span2, flipped2) + segment_costs(span3, flipped3) + segment_costs(span4, flipped4) +
end_point_distance(span1, reversed1, flipped1, span2, reversed2, flipped2) +
end_point_distance(span2, reversed2, flipped2, span3, reversed3, flipped3) +
end_point_distance(span3, reversed3, flipped3, span4, reversed4, flipped4);
};
#ifndef NDEBUG
{
double c = connection_cost(0, false, false, 1, false, false, 2, false, false, 3, false, false);
assert(std::abs(c - cost_current) < SCALED_EPSILON);
}
#endif /* NDEBUG */
double cost_min = cost_current;
size_t flip_min = 0; // no flip, no improvement
for (size_t i = 0; i < (1 << 8); ++ i) {
// From the three combinations of 1,2,3 ordering, the other three are reversals of the first three.
size_t permutation = 0;
for (double c : {
(i == 0) ? cost_current :
connection_cost(0, (i & 1) != 0, (i & (1 << 1)) != 0, 1, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, 2, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0, 3, (i & (1 << 6)) != 0, (i & (1 << 7)) != 0),
connection_cost(0, (i & 1) != 0, (i & (1 << 1)) != 0, 1, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, 3, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0, 2, (i & (1 << 6)) != 0, (i & (1 << 7)) != 0),
connection_cost(0, (i & 1) != 0, (i & (1 << 1)) != 0, 2, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, 1, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0, 3, (i & (1 << 6)) != 0, (i & (1 << 7)) != 0),
connection_cost(0, (i & 1) != 0, (i & (1 << 1)) != 0, 2, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, 3, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0, 1, (i & (1 << 6)) != 0, (i & (1 << 7)) != 0),
connection_cost(0, (i & 1) != 0, (i & (1 << 1)) != 0, 3, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, 1, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0, 2, (i & (1 << 6)) != 0, (i & (1 << 7)) != 0),
connection_cost(0, (i & 1) != 0, (i & (1 << 1)) != 0, 3, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, 2, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0, 1, (i & (1 << 6)) != 0, (i & (1 << 7)) != 0),
connection_cost(1, (i & 1) != 0, (i & (1 << 1)) != 0, 0, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, 2, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0, 3, (i & (1 << 6)) != 0, (i & (1 << 7)) != 0),
connection_cost(1, (i & 1) != 0, (i & (1 << 1)) != 0, 0, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, 3, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0, 2, (i & (1 << 6)) != 0, (i & (1 << 7)) != 0),
connection_cost(1, (i & 1) != 0, (i & (1 << 1)) != 0, 2, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, 0, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0, 3, (i & (1 << 6)) != 0, (i & (1 << 7)) != 0),
connection_cost(1, (i & 1) != 0, (i & (1 << 1)) != 0, 3, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, 0, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0, 2, (i & (1 << 6)) != 0, (i & (1 << 7)) != 0),
connection_cost(2, (i & 1) != 0, (i & (1 << 1)) != 0, 0, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, 1, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0, 3, (i & (1 << 6)) != 0, (i & (1 << 7)) != 0),
connection_cost(2, (i & 1) != 0, (i & (1 << 1)) != 0, 1, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, 0, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0, 3, (i & (1 << 6)) != 0, (i & (1 << 7)) != 0)
}) {
if (c < cost_min) {
cost_min = c;
flip_min = i + (permutation << 8);
}
++ permutation;
}
}
return std::make_pair(cost_min, flip_min);
}
// Currently not used, too slow.
static inline void reorder_by_three_exchanges_with_segment_flipping2(std::vector<FlipEdge> &edges)
{
if (edges.size() < 3) {
reorder_by_two_exchanges_with_segment_flipping(edges);
return;
}
std::vector<ConnectionCost> connections(edges.size());
std::vector<FlipEdge> edges_tmp(edges);
std::vector<std::pair<double, size_t>> connection_lengths(edges.size() - 1, std::pair<double, size_t>(0., 0));
std::vector<char> connection_tried(edges.size(), false);
for (size_t iter = 0; iter < edges.size(); ++ iter) {
// Initialize connection costs and connection lengths.
for (size_t i = 1; i < edges.size(); ++ i) {
const FlipEdge &e1 = edges[i - 1];
const FlipEdge &e2 = edges[i];
ConnectionCost &c = connections[i];
c = connections[i - 1];
double l = (e2.p1 - e1.p2).norm();
c.cost += l;
c.cost_flipped += (e2.p2 - e1.p1).norm();
connection_lengths[i - 1] = std::make_pair(l, i);
}
std::sort(connection_lengths.begin(), connection_lengths.end(), [](const std::pair<double, size_t> &l, const std::pair<double, size_t> &r) { return l.first > r.first; });
std::fill(connection_tried.begin(), connection_tried.end(), false);
size_t crossover1_pos_final = std::numeric_limits<size_t>::max();
size_t crossover2_pos_final = std::numeric_limits<size_t>::max();
size_t crossover3_pos_final = std::numeric_limits<size_t>::max();
size_t crossover_flip_final = 0;
// Distances between the end points of the four pieces of the current segment sequence.
#ifdef NDEBUG
Matrixd segment_end_point_distance_matrix(4 * 4, 4 * 4);
#else /* NDEBUG */
Matrixd segment_end_point_distance_matrix = Matrixd::Constant(4 * 4, 4 * 4, std::numeric_limits<double>::max());
#endif /* NDEBUG */
for (const std::pair<double, size_t> &first_crossover_candidate : connection_lengths) {
size_t longest_connection_idx = first_crossover_candidate.second;
connection_tried[longest_connection_idx] = true;
// Find the second crossover connection with the lowest total chain cost.
double crossover_cost_min = connections.back().cost;
for (size_t j = 1; j < connections.size(); ++ j)
if (! connection_tried[j]) {
for (size_t k = j + 1; k < connections.size(); ++ k)
if (! connection_tried[k]) {
size_t a = longest_connection_idx;
size_t b = j;
size_t c = k;
if (a > c)
std::swap(a, c);
if (a > b)
std::swap(a, b);
if (b > c)
std::swap(b, c);
const Vec2d* endpts[16] = {
&edges[0].p1, &edges[0].p2, &edges[a - 1].p2, &edges[a - 1].p1,
&edges[a].p1, &edges[a].p2, &edges[b - 1].p2, &edges[b - 1].p1,
&edges[b].p1, &edges[b].p2, &edges[c - 1].p2, &edges[c - 1].p1,
&edges[c].p1, &edges[c].p2, &edges.back().p2, &edges.back().p1 };
for (size_t v = 0; v < 16; ++ v) {
const Vec2d &p1 = *endpts[v];
for (size_t u = (v & (~3)) + 4; u < 16; ++ u)
segment_end_point_distance_matrix(u, v) = segment_end_point_distance_matrix(v, u) = (*endpts[u] - p1).norm();
}
FourOptCosts segment_costs(connections[a - 1], connections[b - 1] - connections[a], connections[c - 1] - connections[b], connections.back() - connections[c]);
std::pair<double, size_t> cost_and_flip = minimum_crossover_cost(segment_costs, segment_end_point_distance_matrix, connections.back().cost);
if (cost_and_flip.second > 0 && cost_and_flip.first < crossover_cost_min) {
crossover_cost_min = cost_and_flip.first;
crossover1_pos_final = a;
crossover2_pos_final = b;
crossover3_pos_final = c;
crossover_flip_final = cost_and_flip.second;
assert(crossover_cost_min < connections.back().cost + EPSILON);
}
}
}
if (crossover_flip_final > 0) {
// The cost of the chain with the proposed two crossovers has a lower total cost than the current chain. Apply the crossover.
break;
} else {
// Continue with another long candidate edge.
}
}
if (crossover_flip_final > 0) {
// Pair of cross over positions and flip / reverse constellation has been found, which improves the total cost of the connection.
// Perform a crossover.
do_crossover(edges, edges_tmp, std::make_pair(size_t(0), crossover1_pos_final), std::make_pair(crossover1_pos_final, crossover2_pos_final),
std::make_pair(crossover2_pos_final, crossover3_pos_final), std::make_pair(crossover3_pos_final, edges.size()), crossover_flip_final);
edges.swap(edges_tmp);
} else {
// No valid pair of cross over positions was found improving the total cost. Giving up.
break;
}
}
}
#endif
// Flip the sequences of polylines to lower the total length of connecting lines.
// Used by the infill generator if the infill is not connected with perimeter lines
// and to order the brim lines.
static inline void improve_ordering_by_two_exchanges_with_segment_flipping(Polylines &polylines, bool fixed_start)
{
#ifndef NDEBUG
auto cost = [&polylines]() {
double sum = 0.;
for (size_t i = 1; i < polylines.size(); ++i)
sum += (polylines[i].first_point() - polylines[i - 1].last_point()).cast<double>().norm();
return sum;
};
double cost_initial = cost();
static int iRun = 0;
++ iRun;
#ifdef DEBUG_SVG_OUTPUT
svg_draw_polyline_chain("improve_ordering_by_two_exchanges_with_segment_flipping-initial", iRun, polylines);
#endif /* DEBUG_SVG_OUTPUT */
#endif /* NDEBUG */
std::vector<FlipEdge> edges;
edges.reserve(polylines.size());
std::transform(polylines.begin(), polylines.end(), std::back_inserter(edges),
[&polylines](const Polyline &pl){ return FlipEdge(pl.first_point().cast<double>(), pl.last_point().cast<double>(), &pl - polylines.data()); });
#if 1
reorder_by_two_exchanges_with_segment_flipping(edges);
#else
// reorder_by_three_exchanges_with_segment_flipping(edges);
reorder_by_three_exchanges_with_segment_flipping2(edges);
#endif
Polylines out;
out.reserve(polylines.size());
for (const FlipEdge &edge : edges) {
Polyline &pl = polylines[edge.source_index];
out.emplace_back(std::move(pl));
if (edge.p2 == out.back().first_point().cast<double>()) {
// Polyline is flipped.
out.back().reverse();
} else {
// Polyline is not flipped.
assert(edge.p1 == out.back().first_point().cast<double>());
}
}
polylines = out;
#ifndef NDEBUG
double cost_final = cost();
#ifdef DEBUG_SVG_OUTPUT
svg_draw_polyline_chain("improve_ordering_by_two_exchanges_with_segment_flipping-final", iRun, out);
#endif /* DEBUG_SVG_OUTPUT */
assert(cost_final <= cost_initial);
#endif /* NDEBUG */
}
// Used to optimize order of infill lines and brim lines.
Polylines chain_polylines(Polylines &&polylines, const Point *start_near)
{
#ifdef DEBUG_SVG_OUTPUT
static int iRun = 0;
++ iRun;
svg_draw_polyline_chain("chain_polylines-initial", iRun, polylines);
#endif /* DEBUG_SVG_OUTPUT */
Polylines out;
if (! polylines.empty()) {
auto segment_end_point = [&polylines](size_t idx, bool first_point) -> const Point& { return first_point ? polylines[idx].first_point() : polylines[idx].last_point(); };
std::vector<std::pair<size_t, bool>> ordered = chain_segments_greedy2<Point, decltype(segment_end_point)>(segment_end_point, polylines.size(), start_near);
out.reserve(polylines.size());
for (auto &segment_and_reversal : ordered) {
out.emplace_back(std::move(polylines[segment_and_reversal.first]));
if (segment_and_reversal.second)
out.back().reverse();
}
if (out.size() > 1 && start_near == nullptr) {
improve_ordering_by_two_exchanges_with_segment_flipping(out, start_near != nullptr);
//improve_ordering_by_segment_flipping(out, start_near != nullptr);
}
}
#ifdef DEBUG_SVG_OUTPUT
svg_draw_polyline_chain("chain_polylines-final", iRun, out);
#endif /* DEBUG_SVG_OUTPUT */
return out;
}
// BBS
std::vector<const PrintInstance*> chain_print_object_instances(const std::vector<const PrintObject*>& print_objects, const Point* start_near)
{
// Order objects using a nearest neighbor search.
Points object_reference_points;
std::vector<std::pair<size_t, size_t>> instances;
for (size_t i = 0; i < print_objects.size(); ++i) {
const PrintObject& object = *print_objects[i];
for (size_t j = 0; j < object.instances().size(); ++j) {
// Sliced PrintObjects are centered, object.instances()[j].shift is the center of the PrintObject in G-code coordinates.
object_reference_points.emplace_back(object.instances()[j].shift);
instances.emplace_back(i, j);
}
}
// Order objects using nearest neighbor + post-processing (crossing removal + 2-opt).
std::vector<size_t> path = chain_points_with_postprocessing(object_reference_points, start_near);
std::vector<const PrintInstance*> out;
out.reserve(path.size());
for (size_t idx : path) {
const std::pair<size_t, size_t>& inst = instances[idx];
out.emplace_back(&print_objects[inst.first]->instances()[inst.second]);
}
return out;
}
std::vector<const PrintInstance*> chain_print_object_instances(const Print &print)
{
return chain_print_object_instances(print.objects().vector(), nullptr);
}
Polylines chain_lines(const std::vector<Line> &lines, const double point_distance_epsilon)
{
// Create line end point lookup.
struct LineEnd {
LineEnd(const Line *line, bool start) : line(line), start(start) {}
const Line *line;
// Is it the start or end point?
bool start;
const Point& point() const { return start ? line->a : line->b; }
const Point& other_point() const { return start ? line->b : line->a; }
LineEnd other_end() const { return LineEnd(line, ! start); }
bool operator==(const LineEnd &rhs) const { return this->line == rhs.line && this->start == rhs.start; }
};
struct LineEndAccessor {
const Point* operator()(const LineEnd &pt) const { return &pt.point(); }
};
typedef ClosestPointInRadiusLookup<LineEnd, LineEndAccessor> ClosestPointLookupType;
ClosestPointLookupType closest_end_point_lookup(point_distance_epsilon);
for (const Line &line : lines) {
closest_end_point_lookup.insert(LineEnd(&line, true));
closest_end_point_lookup.insert(LineEnd(&line, false));
}
// Chain the lines.
std::vector<char> line_consumed(lines.size(), false);
static const double point_distance_epsilon2 = point_distance_epsilon * point_distance_epsilon;
Polylines out;
for (const Line &seed : lines)
if (! line_consumed[&seed - lines.data()]) {
line_consumed[&seed - lines.data()] = true;
closest_end_point_lookup.erase(LineEnd(&seed, false));
closest_end_point_lookup.erase(LineEnd(&seed, true));
Polyline pl { seed.a, seed.b };
for (size_t round = 0; round < 2; ++ round) {
for (;;) {
auto [line_end, dist2] = closest_end_point_lookup.find(pl.last_point());
if (line_end == nullptr || dist2 >= point_distance_epsilon2)
// Cannot extent in this direction.
break;
// Average the last point.
pl.points.back() = (0.5 * (pl.points.back().cast<double>() + line_end->point().cast<double>())).cast<coord_t>();
// and extend with the new line segment.
pl.points.emplace_back(line_end->other_point());
closest_end_point_lookup.erase(*line_end);
closest_end_point_lookup.erase(line_end->other_end());
line_consumed[line_end->line - lines.data()] = true;
}
// reverse and try the oter direction.
pl.reverse();
}
out.emplace_back(std::move(pl));
}
return out;
}
} // namespace Slic3r
Reference in new issue View Git Blame Copy Permalink