Support island points are generated as unique ptr for option to move with them

+ add fix for cell 2 polygon transformation
This commit is contained in:
Filip Sykala 2021-03-26 16:57:03 +01:00 committed by Lukas Matena
parent 942443429e
commit 85984ca189
11 changed files with 270 additions and 138 deletions

View File

@ -314,11 +314,11 @@ bool liang_barsky_line_clipping(
template<typename T> template<typename T>
int ray_circle_intersections_r2_lv2_c(T r2, T a, T b, T lv2, T c, std::pair<Eigen::Matrix<T, 2, 1, Eigen::DontAlign>, Eigen::Matrix<T, 2, 1, Eigen::DontAlign>> &out) int ray_circle_intersections_r2_lv2_c(T r2, T a, T b, T lv2, T c, std::pair<Eigen::Matrix<T, 2, 1, Eigen::DontAlign>, Eigen::Matrix<T, 2, 1, Eigen::DontAlign>> &out)
{ {
T x0 = - a * c / lv2;
T y0 = - b * c / lv2;
T d = r2 - c * c / lv2; T d = r2 - c * c / lv2;
if (d < T(0)) if (d < T(0))
return 0; return 0;
T x0 = - a * c / lv2;
T y0 = - b * c / lv2;
T mult = sqrt(d / lv2); T mult = sqrt(d / lv2);
out.first.x() = x0 + b * mult; out.first.x() = x0 + b * mult;
out.first.y() = y0 - a * mult; out.first.y() = y0 - a * mult;

View File

@ -59,7 +59,7 @@ std::optional<Slic3r::Linef> LineUtils::crop_ray(const Linef &ray,
Vec2d center_d = center.cast<double>(); Vec2d center_d = center.cast<double>();
if (is_parallel_y(ray)) { if (is_parallel_y(ray)) {
double x = ray.a.x(); double x = ray.a.x();
double diff = x - center.x(); double diff = x - center_d.x();
double abs_diff = fabs(diff); double abs_diff = fabs(diff);
if (abs_diff > radius) return {}; if (abs_diff > radius) return {};
// create cross points // create cross points
@ -247,6 +247,13 @@ void LineUtils::draw(SVG & svg,
} }
} }
double LineUtils::perp_distance(const Linef &line, Vec2d p)
{
Vec2d v = line.b - line.a; // direction
Vec2d va = p - line.a;
return std::abs(cross2(v, va)) / v.norm();
}
void LineUtils::draw(SVG & svg, void LineUtils::draw(SVG & svg,
const Lines &lines, const Lines &lines,
const char * color, const char * color,

View File

@ -84,6 +84,14 @@ public:
static std::tuple<double, double, double> get_param(const Line &line); static std::tuple<double, double, double> get_param(const Line &line);
static std::tuple<double, double, double> get_param(const Linef &line); static std::tuple<double, double, double> get_param(const Linef &line);
/// <summary>
/// Calculate distance between point and ray
/// </summary>
/// <param name="line">a and b are only for direction, not limit</param>
/// <param name="p">Point in space</param>
/// <returns>Euclid perpedicular distance</returns>
static double perp_distance(const Linef &line, Vec2d p);
static void draw(SVG & svg, static void draw(SVG & svg,
const Line &line, const Line &line,
const char *color = "gray", const char *color = "gray",

View File

@ -19,18 +19,17 @@
using namespace Slic3r::sla; using namespace Slic3r::sla;
std::unique_ptr<SupportIslandPoint> SampleIslandUtils::create_point(
SupportIslandPoint SampleIslandUtils::create_point(
const VoronoiGraph::Node::Neighbor *neighbor, const VoronoiGraph::Node::Neighbor *neighbor,
double ratio, double ratio,
SupportIslandPoint::Type type) SupportIslandPoint::Type type)
{ {
VoronoiGraph::Position position(neighbor, ratio); VoronoiGraph::Position position(neighbor, ratio);
Slic3r::Point p = VoronoiGraphUtils::create_edge_point(position); Slic3r::Point p = VoronoiGraphUtils::create_edge_point(position);
return SupportIslandPoint(p, type, position); return std::make_unique<SupportCenterIslandPoint>(p, position, type);
} }
SupportIslandPoint SampleIslandUtils::create_point_on_path( std::unique_ptr<SupportIslandPoint> SampleIslandUtils::create_point_on_path(
const VoronoiGraph::Nodes &path, const VoronoiGraph::Nodes &path,
double distance, double distance,
SupportIslandPoint::Type type) SupportIslandPoint::Type type)
@ -57,12 +56,10 @@ SupportIslandPoint SampleIslandUtils::create_point_on_path(
// distance must be inside path // distance must be inside path
// this means bad input params // this means bad input params
assert(false); assert(false);
return SupportIslandPoint(Point(0, 0), return nullptr; // unreachable
SupportIslandPoint::Type::undefined,
VoronoiGraph::Position(nullptr,0.));
} }
SupportIslandPoint SampleIslandUtils::create_middle_path_point( SupportIslandPointPtr SampleIslandUtils::create_middle_path_point(
const VoronoiGraph::Path &path, SupportIslandPoint::Type type) const VoronoiGraph::Path &path, SupportIslandPoint::Type type)
{ {
return create_point_on_path(path.nodes, path.length / 2, type); return create_point_on_path(path.nodes, path.length / 2, type);
@ -73,10 +70,11 @@ SupportIslandPoints SampleIslandUtils::create_side_points(
{ {
VoronoiGraph::Nodes reverse_path = path; // copy VoronoiGraph::Nodes reverse_path = path; // copy
std::reverse(reverse_path.begin(), reverse_path.end()); std::reverse(reverse_path.begin(), reverse_path.end());
return { SupportIslandPoints result;
create_point_on_path(path, side_distance, SupportIslandPoint::Type::two_points), result.reserve(2);
create_point_on_path(reverse_path, side_distance, SupportIslandPoint::Type::two_points) result.push_back(create_point_on_path(path, side_distance, SupportIslandPoint::Type::two_points));
}; result.push_back(create_point_on_path(reverse_path, side_distance, SupportIslandPoint::Type::two_points));
return std::move(result);
} }
SupportIslandPoints SampleIslandUtils::sample_side_branch( SupportIslandPoints SampleIslandUtils::sample_side_branch(
@ -92,7 +90,11 @@ SupportIslandPoints SampleIslandUtils::sample_side_branch(
VoronoiGraph::Nodes reverse_path = side_path.nodes; VoronoiGraph::Nodes reverse_path = side_path.nodes;
std::reverse(reverse_path.begin(), reverse_path.end()); std::reverse(reverse_path.begin(), reverse_path.end());
reverse_path.push_back(first_node); reverse_path.push_back(first_node);
return {create_point_on_path(reverse_path, cfg.side_distance, SupportIslandPoint::Type::center_line_end)}; SupportIslandPoints result;
result.push_back(
create_point_on_path(reverse_path, cfg.side_distance,
SupportIslandPoint::Type::center_line_end));
return std::move(result);
} }
// count of segment between points on main path // count of segment between points on main path
size_t segment_count = static_cast<size_t>( size_t segment_count = static_cast<size_t>(
@ -113,13 +115,13 @@ SupportIslandPoints SampleIslandUtils::sample_side_branch(
if (side_item->second.top().length > cfg.min_length) { if (side_item->second.top().length > cfg.min_length) {
auto side_samples = sample_side_branches(side_item, auto side_samples = sample_side_branches(side_item,
start_offset, cfg); start_offset, cfg);
result.insert(result.end(), side_samples.begin(), result.insert(result.end(), std::move_iterator(side_samples.begin()),
side_samples.end()); std::move_iterator(side_samples.end()));
} }
} }
while (distance < neighbor->edge_length) { while (distance < neighbor->edge_length) {
double edge_ratio = distance / neighbor->edge_length; double edge_ratio = distance / neighbor->edge_length;
result.emplace_back( result.push_back(
create_point(neighbor, edge_ratio, SupportIslandPoint::Type::center_line) create_point(neighbor, edge_ratio, SupportIslandPoint::Type::center_line)
); );
distance += sample_distance; distance += sample_distance;
@ -128,8 +130,8 @@ SupportIslandPoints SampleIslandUtils::sample_side_branch(
prev_node = node; prev_node = node;
} }
assert(fabs(distance - (sample_distance - cfg.side_distance)) < 1e-5); assert(fabs(distance - (sample_distance - cfg.side_distance)) < 1e-5);
result.back().type = SupportIslandPoint::Type::center_line_end; result.back()->type = SupportIslandPoint::Type::center_line_end;
return result; return std::move(result);
} }
SupportIslandPoints SampleIslandUtils::sample_side_branches( SupportIslandPoints SampleIslandUtils::sample_side_branches(
@ -150,10 +152,12 @@ SupportIslandPoints SampleIslandUtils::sample_side_branches(
while (side_branches_cpy.top().length > cfg.min_length) { while (side_branches_cpy.top().length > cfg.min_length) {
auto samples = sample_side_branch(first_node, side_branches_cpy.top(), auto samples = sample_side_branch(first_node, side_branches_cpy.top(),
start_offset, cfg); start_offset, cfg);
result.insert(result.end(), samples.begin(), samples.end()); result.insert(result.end(),
std::move_iterator(samples.begin()),
std::move_iterator(samples.end()));
side_branches_cpy.pop(); side_branches_cpy.pop();
} }
return result; return std::move(result);
} }
std::vector<std::set<const VoronoiGraph::Node *>> create_circles_sets( std::vector<std::set<const VoronoiGraph::Node *>> create_circles_sets(
@ -187,14 +191,16 @@ std::vector<std::set<const VoronoiGraph::Node *>> create_circles_sets(
Slic3r::Points SampleIslandUtils::to_points(const SupportIslandPoints &support_points) Slic3r::Points SampleIslandUtils::to_points(const SupportIslandPoints &support_points)
{ {
std::function<Point(const SupportIslandPoint &)> transform_func = &SupportIslandPoint::point; std::function<Point(const std::unique_ptr<SupportIslandPoint> &)> transform_func = &SupportIslandPoint::point;
return VectorUtils::transform(support_points, transform_func); return VectorUtils::transform(support_points, transform_func);
} }
std::vector<Slic3r::Vec2f> SampleIslandUtils::to_points_f(const SupportIslandPoints &support_points) std::vector<Slic3r::Vec2f> SampleIslandUtils::to_points_f(const SupportIslandPoints &support_points)
{ {
std::function<Vec2f(const SupportIslandPoint &p)> transform_func = std::function<Vec2f(const std::unique_ptr<SupportIslandPoint> &)> transform_func =
[](const SupportIslandPoint &p) { return p.point.cast<float>(); }; [](const std::unique_ptr<SupportIslandPoint> &p) {
return p->point.cast<float>();
};
return VectorUtils::transform(support_points, transform_func); return VectorUtils::transform(support_points, transform_func);
} }
@ -246,13 +252,13 @@ coord_t SampleIslandUtils::align_once(SupportIslandPoints &samples,
construct_voronoi(points.begin(), points.end(), &vd); construct_voronoi(points.begin(), points.end(), &vd);
coord_t max_move = 0; coord_t max_move = 0;
for (const VD::cell_type &cell : vd.cells()) { for (const VD::cell_type &cell : vd.cells()) {
SupportIslandPoint &sample = samples[cell.source_index()]; SupportIslandPointPtr &sample = samples[cell.source_index()];
if (!sample.can_move()) continue; if (!sample->can_move()) continue;
Polygon polygon = VoronoiGraphUtils::to_polygon(cell, points, config.max_distance); Polygon polygon = VoronoiGraphUtils::to_polygon(cell, points, config.max_distance);
Polygons intersections = Slic3r::intersection(island, ExPolygon(polygon)); Polygons intersections = Slic3r::intersection(island, ExPolygon(polygon));
const Polygon *island_cell = nullptr; const Polygon *island_cell = nullptr;
for (const Polygon &intersection : intersections) { for (const Polygon &intersection : intersections) {
if (intersection.contains(sample.point)) { if (intersection.contains(sample->point)) {
island_cell = &intersection; island_cell = &intersection;
break; break;
} }
@ -265,27 +271,13 @@ coord_t SampleIslandUtils::align_once(SupportIslandPoints &samples,
svg.draw(*island_cell, "gray"); svg.draw(*island_cell, "gray");
svg.draw(sample.point, "black", config.head_radius); svg.draw(sample.point, "black", config.head_radius);
svg.draw(Line(sample.point, center), "blue", config.head_radius / 5); svg.draw(Line(sample.point, center), "blue", config.head_radius / 5);
#endif // VISUALIZE_SAMPLE_ISLAND_UTILS_ALIGN_ONCE #endif // VISUALIZE_SAMPLE_ISLAND_UTILS_ALIGN_ONCE
coord_t act_move = align_support(sample, center, config.max_align_distance); coord_t act_move = sample->move(center);
if (max_move < act_move) max_move = act_move; if (max_move < act_move) max_move = act_move;
} }
return max_move; return max_move;
} }
coord_t SampleIslandUtils::align_support(SupportIslandPoint &support,
const Point & wanted,
double max_distance)
{
//move only along VD
VoronoiGraph::Position position =
VoronoiGraphUtils::align(support.position, wanted, max_distance);
Point new_point = VoronoiGraphUtils::create_edge_point(position);
Point move = new_point - support.point;
support.position= position;
support.point = new_point;
return abs(move.x()) + abs(move.y());
}
SupportIslandPoints SampleIslandUtils::sample_center_line( SupportIslandPoints SampleIslandUtils::sample_center_line(
const VoronoiGraph::ExPath &path, const CenterLineConfiguration &cfg) const VoronoiGraph::ExPath &path, const CenterLineConfiguration &cfg)
{ {
@ -300,7 +292,7 @@ SupportIslandPoints SampleIslandUtils::sample_center_line(
if (path.circles.empty()) return result; if (path.circles.empty()) return result;
sample_center_circles(path, cfg, result); sample_center_circles(path, cfg, result);
return result; return std::move(result);
} }
void SampleIslandUtils::sample_center_circle_end( void SampleIslandUtils::sample_center_circle_end(
@ -325,7 +317,7 @@ void SampleIslandUtils::sample_center_circle_end(
double distance_between = distance / (count_supports + 1); double distance_between = distance / (count_supports + 1);
if (distance_between < neighbor_distance) { if (distance_between < neighbor_distance) {
// point is calculated to be in done path, SP will be on edge point // point is calculated to be in done path, SP will be on edge point
result.emplace_back(create_point(&neighbor, 1., SupportIslandPoint::Type::center_circle_end)); result.push_back(create_point(&neighbor, 1., SupportIslandPoint::Type::center_circle_end));
neighbor_distance = 0.; neighbor_distance = 0.;
count_supports -= 1; count_supports -= 1;
if (count_supports == 0) { if (count_supports == 0) {
@ -340,7 +332,7 @@ void SampleIslandUtils::sample_center_circle_end(
nodes.insert(nodes.begin(), neighbor.node); nodes.insert(nodes.begin(), neighbor.node);
for (int i = 1; i <= count_supports; ++i) { for (int i = 1; i <= count_supports; ++i) {
double distance_from_neighbor = i * (distance_between) - neighbor_distance; double distance_from_neighbor = i * (distance_between) - neighbor_distance;
result.emplace_back( result.push_back(
create_point_on_path(nodes, distance_from_neighbor, SupportIslandPoint::Type::center_circle_end2)); create_point_on_path(nodes, distance_from_neighbor, SupportIslandPoint::Type::center_circle_end2));
double distance_support_to_node = fabs(neighbor.edge_length - double distance_support_to_node = fabs(neighbor.edge_length -
distance_from_neighbor); distance_from_neighbor);
@ -435,11 +427,13 @@ SupportDistanceMap create_path_distances(
return path_distances; return path_distances;
} }
// do not use
SupportDistanceMap create_support_distance_map(const SupportIslandPoints &support_points) SupportDistanceMap create_support_distance_map(const SupportIslandPoints &support_points)
{ {
SupportDistanceMap support_distance_map; SupportDistanceMap support_distance_map;
for (const SupportIslandPoint &support_point : support_points) { for (const SupportIslandPointPtr &support_point : support_points) {
const VoronoiGraph::Position &position = support_point.position; auto ptr = dynamic_cast<SupportCenterIslandPoint*>(support_point.get()); // bad use
const VoronoiGraph::Position &position = ptr->position;
const VoronoiGraph::Node *node = position.neighbor->node; const VoronoiGraph::Node *node = position.neighbor->node;
const VoronoiGraph::Node *twin_node = VoronoiGraphUtils::get_twin_node(position.neighbor); const VoronoiGraph::Node *twin_node = VoronoiGraphUtils::get_twin_node(position.neighbor);
double distance = (1 - position.ratio) * position.neighbor->edge_length; double distance = (1 - position.ratio) * position.neighbor->edge_length;
@ -516,7 +510,9 @@ void SampleIslandUtils::sample_center_circles(
for (const auto &circle_set : circles_sets) { for (const auto &circle_set : circles_sets) {
SupportDistanceMap path_distances = create_path_distances(circle_set, path_set, support_distance_map, cfg.max_sample_distance/2); SupportDistanceMap path_distances = create_path_distances(circle_set, path_set, support_distance_map, cfg.max_sample_distance/2);
SupportIslandPoints circle_result = sample_center_circle(circle_set, path_distances, cfg); SupportIslandPoints circle_result = sample_center_circle(circle_set, path_distances, cfg);
result.insert(result.end(), circle_result.begin(), circle_result.end()); result.insert(result.end(),
std::make_move_iterator(circle_result.begin()),
std::make_move_iterator(circle_result.end()));
} }
} }
@ -589,7 +585,7 @@ SupportIslandPoints SampleIslandUtils::sample_center_circle(
.distance_from_support_point - .distance_from_support_point -
nd.distance_from_support_point; nd.distance_from_support_point;
double ratio = distance_from_node / neighbor.edge_length; double ratio = distance_from_node / neighbor.edge_length;
result.emplace_back( result.push_back(
create_point(&neighbor, ratio, SupportIslandPoint::Type::center_circle)); create_point(&neighbor, ratio, SupportIslandPoint::Type::center_circle));
next_nd.distance_from_support_point -= cfg.max_sample_distance; next_nd.distance_from_support_point -= cfg.max_sample_distance;
} }
@ -605,9 +601,10 @@ SupportIslandPoints SampleIslandUtils::sample_expath(
// 1) One support point // 1) One support point
if (path.length < config.max_length_for_one_support_point) { if (path.length < config.max_length_for_one_support_point) {
// create only one point in center // create only one point in center
return { SupportIslandPoints result;
create_middle_path_point(path, SupportIslandPoint::Type::one_center_point) result.push_back(create_middle_path_point(
}; path, SupportIslandPoint::Type::one_center_point));
return std::move(result);
} }
double max_width = VoronoiGraphUtils::get_max_width(path); double max_width = VoronoiGraphUtils::get_max_width(path);
@ -624,8 +621,8 @@ SupportIslandPoints SampleIslandUtils::sample_expath(
config.max_distance, config.max_distance,
config.minimal_distance_from_outline); config.minimal_distance_from_outline);
SupportIslandPoints samples = sample_center_line(path, centerLineConfiguration); SupportIslandPoints samples = sample_center_line(path, centerLineConfiguration);
samples.front().type = SupportIslandPoint::Type::center_line_end2; samples.front()->type = SupportIslandPoint::Type::center_line_end2;
return samples; return std::move(samples);
} }
// line of zig zag points // line of zig zag points
@ -641,7 +638,7 @@ SupportIslandPoints SampleIslandUtils::sample_expath(
SupportIslandPoints points; SupportIslandPoints points;
points.emplace_back(create_point_on_path(path.nodes, config.minimal_distance_from_outline)); points.emplace_back(create_point_on_path(path.nodes, config.minimal_distance_from_outline));
return points; return std::move(points);
} }
SupportIslandPoints SampleIslandUtils::sample_voronoi_graph( SupportIslandPoints SampleIslandUtils::sample_voronoi_graph(
@ -665,10 +662,10 @@ void SampleIslandUtils::draw(SVG & svg,
bool write_type) bool write_type)
{ {
for (const auto &p : supportIslandPoints) { for (const auto &p : supportIslandPoints) {
svg.draw(p.point, color, size); svg.draw(p->point, color, size);
if (write_type && p.type != SupportIslandPoint::Type::undefined) { if (write_type && p->type != SupportIslandPoint::Type::undefined) {
auto type_name = magic_enum::enum_name(p.type); auto type_name = magic_enum::enum_name(p->type);
Point start = p.point + Point(size, 0.); Point start = p->point + Point(size, 0.);
svg.draw_text(start, std::string(type_name).c_str(), color); svg.draw_text(start, std::string(type_name).c_str(), color);
} }
} }

View File

@ -28,7 +28,7 @@ public:
/// <param name="ratio">Source distance ratio for position on edge</param> /// <param name="ratio">Source distance ratio for position on edge</param>
/// <param name="type">Type of point</param> /// <param name="type">Type of point</param>
/// <returns>created support island point</returns> /// <returns>created support island point</returns>
static SupportIslandPoint create_point( static SupportIslandPointPtr create_point(
const VoronoiGraph::Node::Neighbor *neighbor, const VoronoiGraph::Node::Neighbor *neighbor,
double ratio, double ratio,
SupportIslandPoint::Type type = SupportIslandPoint::Type::undefined); SupportIslandPoint::Type type = SupportIslandPoint::Type::undefined);
@ -39,7 +39,7 @@ public:
/// <param name="path">Neighbor connected Nodes</param> /// <param name="path">Neighbor connected Nodes</param>
/// <param name="distance">Distance to final point</param> /// <param name="distance">Distance to final point</param>
/// <returns>Points with distance to first node</returns> /// <returns>Points with distance to first node</returns>
static SupportIslandPoint create_point_on_path( static SupportIslandPointPtr create_point_on_path(
const VoronoiGraph::Nodes &path, const VoronoiGraph::Nodes &path,
double distance, double distance,
SupportIslandPoint::Type type = SupportIslandPoint::Type::undefined); SupportIslandPoint::Type type = SupportIslandPoint::Type::undefined);
@ -52,7 +52,7 @@ public:
/// <param name="path">Queue of neighbor nodes.(must be neighbor)</param> /// <param name="path">Queue of neighbor nodes.(must be neighbor)</param>
/// <param name="path_length">length of path</param> /// <param name="path_length">length of path</param>
/// <returns>Point laying on voronoi diagram</returns> /// <returns>Point laying on voronoi diagram</returns>
static SupportIslandPoint create_middle_path_point( static SupportIslandPointPtr create_middle_path_point(
const VoronoiGraph::Path &path, const VoronoiGraph::Path &path,
SupportIslandPoint::Type type = SupportIslandPoint::Type::undefined); SupportIslandPoint::Type type = SupportIslandPoint::Type::undefined);

View File

@ -2,6 +2,7 @@
#define slic3r_SLA_SuppotstIslands_SupportIslandPoint_hpp_ #define slic3r_SLA_SuppotstIslands_SupportIslandPoint_hpp_
#include <set> #include <set>
#include <memory>
#include <libslic3r/Point.hpp> #include <libslic3r/Point.hpp>
#include "VoronoiGraph.hpp" #include "VoronoiGraph.hpp"
@ -12,59 +13,106 @@ namespace Slic3r::sla {
/// </summary> /// </summary>
struct SupportIslandPoint struct SupportIslandPoint
{ {
enum class Type : unsigned char { enum class Type: unsigned char {
one_center_point, one_center_point,
two_points, two_points,
center_line, center_line,
center_line_end, // end of branch center_line_end, // end of branch
center_line_end2, // start of main path(only one per VD) center_line_end2, // start of main path(only one per VD)
center_circle, center_circle,
center_circle_end, // circle finish by one point (one end per circle - need allign) center_circle_end, // circle finish by one point (one end per circle -
center_circle_end2, // circle finish by multi points (one end per circle - need allign) // need allign)
center_circle_end2, // circle finish by multi points (one end per
// circle - need allign)
outline, // keep position align with island outline
undefined undefined
}; };
Slic3r::Point point; // 2 point in layer coordinate Type type;
Slic3r::Point point; // 2 coordinate point in a layer (in one slice)
//SupportIslandPoint() : point(0, 0), type(Type::undefined) {}
SupportIslandPoint(Slic3r::Point point, Type type = Type::undefined);
virtual ~SupportIslandPoint() = default;
static bool can_move(const Type &type)
{
// use shorter list
/*
static const std::set<Type> can_move({
Type::center_line,
Type::center_circle,
Type::center_circle_end,
Type::center_circle_end2});
return can_move.find(type) != can_move.end();
/*/ // switch comment center
static const std::set<Type> cant_move({
Type::one_center_point,
Type::two_points,
Type::center_line_end,
Type::center_line_end2});
return cant_move.find(type) == cant_move.end();
//*/
}
virtual bool can_move() const { return can_move(type); }
/// <summary>
/// Move position of support point close to destination
/// with support point restrictions
/// </summary>
/// <param name="destination">Wanted position</param>
/// <returns>Move distance</returns>
virtual coord_t move(const Point &destination);
};
using SupportIslandPointPtr = std::unique_ptr<SupportIslandPoint>;
using SupportIslandPoints = std::vector<SupportIslandPointPtr>;
struct SupportCenterIslandPoint : public SupportIslandPoint
{
// Define position on voronoi graph // Define position on voronoi graph
// Lose data when voronoi graph does NOT exist // Lose data when voronoi graph does NOT exist
VoronoiGraph::Position position; VoronoiGraph::Position position;
// IMPROVE: not need ratio, only neighbor
// const VoronoiGraph::Node::Neighbor* neighbor;
Type type; // TODO: should earn when created
const double max_distance = 1e6; // [in nm] --> 1 mm
SupportIslandPoint(Slic3r::Point point, SupportCenterIslandPoint(Slic3r::Point point,
Type type, VoronoiGraph::Position position,
VoronoiGraph::Position position) Type type = Type::center_line)
: point(std::move(point)), type(type), position(position) : SupportIslandPoint(point, type), position(position)
{} {}
bool can_move() const{ bool can_move() const override{ return true; }
// use shorter list coord_t move(const Point &destination) override;
/* };
static const std::set<Type> can_move({ struct SupportOutlineIslandPoint : public SupportIslandPoint
Type::center_line, {
Type::center_circle, // index of line form island outline
Type::center_circle_end, size_t index;
Type::center_circle_end2 SupportOutlineIslandPoint(Slic3r::Point point,
}); size_t index,
return can_move.find(type) != can_move.end(); Type type = Type::outline)
: SupportIslandPoint(point, type), index(index)
/*/ {}
static const std::set<Type> cant_move({ bool can_move() const override { return true; }
Type::one_center_point,
Type::two_points, coord_t move(const Point &destination) override
Type::center_line_end, {
Type::center_line_end2 // TODO: For decide of move need information about
}); // + island outlines \ May be
return cant_move.find(type) == cant_move.end(); // + distance from outline / offseted outlines
// + search distance for allowed move over outlines(count, distance)
//*/ assert(false); // Not implemented
return 0;
} }
}; };
using SupportIslandPoints = std::vector<SupportIslandPoint>;
} // namespace Slic3r::sla } // namespace Slic3r::sla
#endif // slic3r_SLA_SuppotstIslands_SupportIslandPoint_hpp_ #endif // slic3r_SLA_SuppotstIslands_SupportIslandPoint_hpp_

View File

@ -26,6 +26,7 @@ public:
template<typename T1, typename T2> template<typename T1, typename T2>
static void sort_by(std::vector<T1> &data, std::function<T2(const T1 &)> &calc) static void sort_by(std::vector<T1> &data, std::function<T2(const T1 &)> &calc)
{ {
assert(!data.empty());
// initialize original index locations // initialize original index locations
std::vector<size_t> idx(data.size()); std::vector<size_t> idx(data.size());
iota(idx.begin(), idx.end(), 0); iota(idx.begin(), idx.end(), 0);

View File

@ -13,10 +13,10 @@
#include <libslic3r/VoronoiVisualUtils.hpp> #include <libslic3r/VoronoiVisualUtils.hpp>
// comment definition of NDEBUG to enable assert() // comment definition of NDEBUG to enable assert()
// #define NDEBUG //#define NDEBUG
#include <cassert> #include <cassert>
//#define SLA_CELL_2_POLYGON_DEBUG //#define SLA_SVG_VISUALIZATION_CELL_2_POLYGON
using namespace Slic3r::sla; using namespace Slic3r::sla;
@ -38,7 +38,7 @@ Slic3r::Point VoronoiGraphUtils::to_point(const VD::vertex_type *vertex)
Slic3r::Vec2d VoronoiGraphUtils::to_point_d(const VD::vertex_type *vertex) Slic3r::Vec2d VoronoiGraphUtils::to_point_d(const VD::vertex_type *vertex)
{ {
return Vec2d(to_coord(vertex->x()), to_coord(vertex->y())); return Vec2d(vertex->x(), vertex->y());
} }
bool VoronoiGraphUtils::is_coord_in_limits(const VD::coordinate_type &coord, bool VoronoiGraphUtils::is_coord_in_limits(const VD::coordinate_type &coord,
@ -78,13 +78,6 @@ Slic3r::Line VoronoiGraphUtils::create_line_between_source_points(
return Line(middle - side_dir, middle + side_dir); return Line(middle - side_dir, middle + side_dir);
} }
bool is_oposit_direction(const Slic3r::Point &p1, const Slic3r::Point &p2) {
if (abs(p1.x()) > abs(p1.y())) {
return (p1.x() > 0) != (p2.x() > 0);
}
return (p1.y() > 0) != (p2.y() > 0);
}
std::optional<Slic3r::Line> VoronoiGraphUtils::to_line( std::optional<Slic3r::Line> VoronoiGraphUtils::to_line(
const VD::edge_type &edge, const Points &points, double maximal_distance) const VD::edge_type &edge, const Points &points, double maximal_distance)
{ {
@ -137,12 +130,22 @@ std::optional<Slic3r::Line> VoronoiGraphUtils::to_line(
} }
std::optional<Linef> segment; std::optional<Linef> segment;
if (use_both) { if (use_both) {
Linef edge_segment(Vec2d(v0->x(), v0->y()), Vec2d(v1->x(), v1->y())); Linef edge_segment(to_point_d(v0), to_point_d(v1));
segment = LineUtils::crop_line(edge_segment, p1, maximal_distance); segment = LineUtils::crop_line(edge_segment, p1, maximal_distance);
} else { } else {
Vec2d ray_point(edge_vertex->x(), edge_vertex->y()); // Vertex can't be used as start point because data type limitation
Linef ray(ray_point, ray_point + direction.cast<double>()); // Explanation for shortening line is in Test::bad_vertex
segment = LineUtils::crop_half_ray(ray, p1, maximal_distance); Vec2d middle = (p1.cast<double>() + p2.cast<double>()) / 2.;
Vec2d vertex = to_point_d(edge_vertex);
Vec2d vertex_direction = (vertex - middle);
Vec2d vertex_dir_abs(fabs(vertex_direction.x()), fabs(vertex_direction.y()));
double divider = (vertex_dir_abs.x() > vertex_dir_abs.y()) ?
vertex_dir_abs.x() / maximal_distance :
vertex_dir_abs.y() / maximal_distance;
Vec2d vertex_dir_short = vertex_direction / divider;
Vec2d start_point = middle + vertex_dir_short;
Linef line_short(start_point, start_point + direction.cast<double>());
segment = LineUtils::crop_half_ray(line_short, p1, maximal_distance);
} }
if (!segment.has_value()) return {}; if (!segment.has_value()) return {};
return Line(segment->a.cast<coord_t>(), segment->b.cast<coord_t>()); return Line(segment->a.cast<coord_t>(), segment->b.cast<coord_t>());
@ -218,32 +221,24 @@ Slic3r::Polygon VoronoiGraphUtils::to_polygon(const VD::cell_type & cell,
const VD::edge_type *edge = cell.incident_edge(); const VD::edge_type *edge = cell.incident_edge();
do { do {
assert(edge->is_linear()); assert(edge->is_linear());
if (!edge->is_primary()) { if (!edge->is_primary()) continue;
edge = edge->next();
continue;
}
std::optional<Line> line = to_line(*edge, points, maximal_distance); std::optional<Line> line = to_line(*edge, points, maximal_distance);
if (!line.has_value()) { if (!line.has_value()) continue;
edge = edge->next();
continue;
}
Geometry::Orientation orientation = Geometry::orient(center, line->a, line->b); Geometry::Orientation orientation = Geometry::orient(center, line->a, line->b);
if (orientation == Geometry::Orientation::ORIENTATION_COLINEAR) // Can be rich on circle over source point edge
{ // on circle over source point edge if (orientation == Geometry::Orientation::ORIENTATION_COLINEAR)
edge = edge->next();
continue; continue;
}
if (orientation == Geometry::Orientation::ORIENTATION_CW) if (orientation == Geometry::Orientation::ORIENTATION_CW)
std::swap(line->a, line->b); std::swap(line->a, line->b);
lines.push_back(line.value()); lines.push_back(line.value());
edge = edge->next(); } while ((edge = edge->next()) && edge != cell.incident_edge());
} while (edge != cell.incident_edge()); assert(!lines.empty());
LineUtils::sort_CCW(lines, center); LineUtils::sort_CCW(lines, center);
// preccission to decide when not connect neighbor points // preccission to decide when not connect neighbor points
double min_distance = maximal_distance / 1000.; double min_distance = maximal_distance / 1000.;
size_t count_point = 6; // count added points size_t count_point = 6; // count added points
Slic3r::Polygon polygon = to_polygon(lines, center, maximal_distance, min_distance, count_point); Slic3r::Polygon polygon = to_polygon(lines, center, maximal_distance, min_distance, count_point);
#ifdef SLA_CELL_2_POLYGON_DEBUG #ifdef SLA_SVG_VISUALIZATION_CELL_2_POLYGON
{ {
std::cout << "cell " << cell.source_index() << " has " << lines.size() << "edges" << std::endl; std::cout << "cell " << cell.source_index() << " has " << lines.size() << "edges" << std::endl;
BoundingBox bbox(center - Point(maximal_distance, maximal_distance), BoundingBox bbox(center - Point(maximal_distance, maximal_distance),
@ -261,7 +256,7 @@ Slic3r::Polygon VoronoiGraphUtils::to_polygon(const VD::cell_type & cell,
} }
svg.draw(center, "red", maximal_distance / 100); svg.draw(center, "red", maximal_distance / 100);
} }
#endif /* SLA_CELL_2_POLYGON_DEBUG */ #endif /* SLA_SVG_VISUALIZATION_CELL_2_POLYGON */
return polygon; return polygon;
} }

View File

@ -2284,3 +2284,79 @@ TEST_CASE("Voronoi cell doesn't contain a source point - SPE-2298", "[VoronoiCel
REQUIRE(vd.is_valid()); REQUIRE(vd.is_valid());
} }
// */ // */
//#include <libslic3r/SLA/SupportIslands/LineUtils.hpp>
TEST_CASE("bad vertex cause overflow of data type precisin when use VD result", "[VoronoiDiagram]")
{
// Points are almost in line
Points points = {
{-106641371, 61644934},
{-56376476, 32588892},
{0, 0}
};
//auto perp_distance = sla::LineUtils::perp_distance;
auto perp_distance = [](const Linef &line, Vec2d p) {
Vec2d v = line.b - line.a; // direction
Vec2d va = p - line.a;
return std::abs(cross2(v, va)) / v.norm();
};
using VD = Slic3r::Geometry::VoronoiDiagram;
VD vd;
construct_voronoi(points.begin(), points.end(), &vd);
// edge between source index 0 and 1 has bad vertex
size_t bad_index0 = 0;
size_t bad_index1 = 1;
for (auto &edge : vd.edges()) {
size_t i1 = edge.cell()->source_index();
size_t i2 = edge.twin()->cell()->source_index();
if (i1 == bad_index0 && i2 == bad_index1 ||
i1 == bad_index1 && i2 == bad_index0) {
Vec2d p1 = points[bad_index0].cast<double>();
Vec2d p2 = points[bad_index1].cast<double>();
Vec2d middle = (p1 + p2) / 2;
// direction for edge is perpendicular point connection
Vec2d direction(p2.y() - p1.y(), p1.x() - p2.x());
const VD::vertex_type *vrtx = (edge.vertex0() == nullptr) ?
edge.vertex1() :
edge.vertex0();
if (vrtx == nullptr) continue;
Vec2d vertex(vrtx->x(), vrtx->y());
double point_distance = (p1 - p2).norm();
double half_point_distance = point_distance/2;
Linef line_from_middle(middle, middle + direction); // line between source points
double distance_vertex = perp_distance(line_from_middle, vertex);
double distance_p1 = perp_distance(line_from_middle, p1);
double distance_p2 = perp_distance(line_from_middle, p2);
Linef line_from_vertex(vertex, vertex + direction);
double distance_middle = perp_distance(line_from_vertex, middle);
double distance_p1_ = perp_distance(line_from_vertex, p1);
double distance_p2_ = perp_distance(line_from_vertex, p2);
double maximal_distance = 9e6;
Vec2d vertex_direction = (vertex - middle);
Vec2d vertex_dir_abs(fabs(vertex_direction.x()),
fabs(vertex_direction.y()));
double divider = (vertex_dir_abs.x() > vertex_dir_abs.y()) ?
vertex_dir_abs.x() / maximal_distance :
vertex_dir_abs.y() / maximal_distance;
Vec2d vertex_dir_short = vertex_direction / divider;
Vec2d start_point = middle + vertex_dir_short;
Linef line_short(start_point, start_point + direction);
double distance_short_vertex = perp_distance(line_short, vertex);
double distance_short_middle = perp_distance(line_short, middle);
double distance_p1_short = perp_distance(line_short, p1);
double distance_p2_short = perp_distance(line_short, p2);
CHECK(distance_vertex < 10);
//CHECK(distance_middle < 10); // This is bad
CHECK(distance_short_vertex < 10);
CHECK(distance_short_middle < 10);
}
}
}

View File

@ -331,8 +331,8 @@ SupportIslandPoints test_island_sampling(const ExPolygon & island,
const Point &chck_point = chck_points[index]; const Point &chck_point = chck_points[index];
double &min_distance = point_distances[index]; double &min_distance = point_distances[index];
bool exist_close_support_point = false; bool exist_close_support_point = false;
for (auto &island_point : points) { for (const auto &island_point : points) {
Point& p = island_point.point; const Point& p = island_point->point;
Point abs_diff(fabs(p.x() - chck_point.x()), Point abs_diff(fabs(p.x() - chck_point.x()),
fabs(p.y() - chck_point.y())); fabs(p.y() - chck_point.y()));
if (abs_diff.x() < min_distance && abs_diff.y() < min_distance) { if (abs_diff.x() < min_distance && abs_diff.y() < min_distance) {
@ -354,7 +354,7 @@ SupportIslandPoints test_island_sampling(const ExPolygon & island,
SVG svg("Error" + std::to_string(++counter) + ".svg", bb); SVG svg("Error" + std::to_string(++counter) + ".svg", bb);
svg.draw(island, "blue", 0.5f); svg.draw(island, "blue", 0.5f);
for (auto& p : points) for (auto& p : points)
svg.draw(p.point, "lightgreen", config.head_radius); svg.draw(p->point, "lightgreen", config.head_radius);
for (size_t index = 0; index < chck_points.size(); ++index) { for (size_t index = 0; index < chck_points.size(); ++index) {
const Point &chck_point = chck_points[index]; const Point &chck_point = chck_points[index];
double distance = point_distances[index]; double distance = point_distances[index];
@ -367,7 +367,7 @@ SupportIslandPoints test_island_sampling(const ExPolygon & island,
//CHECK(is_ok); //CHECK(is_ok);
// all points must be inside of island // all points must be inside of island
for (const auto &point : points) { CHECK(island.contains(point.point)); } for (const auto &point : points) { CHECK(island.contains(point->point)); }
return points; return points;
} }
@ -428,10 +428,10 @@ TEST_CASE("Speed align", "[VoronoiSkeleton]")
Slic3r::Voronoi::annotate_inside_outside(vd, lines); Slic3r::Voronoi::annotate_inside_outside(vd, lines);
VoronoiGraph::ExPath longest_path; VoronoiGraph::ExPath longest_path;
VoronoiGraph skeleton = VoronoiGraphUtils::create_skeleton(vd, lines); VoronoiGraph skeleton = VoronoiGraphUtils::create_skeleton(vd, lines);
auto samples = SampleIslandUtils::sample_voronoi_graph(skeleton, cfg, longest_path);
for (int i = 0; i < 100; ++i) { auto sample_copy = samples; // copy for (int i = 0; i < 100; ++i) {
SampleIslandUtils::align_samples(sample_copy, island, cfg); auto samples = SampleIslandUtils::sample_voronoi_graph(skeleton, cfg, longest_path);
SampleIslandUtils::align_samples(samples, island, cfg);
} }
} }

View File

@ -47,7 +47,7 @@ void check(Slic3r::Points points, double max_distance) {
TEST_CASE("Polygon from cell", "[Voronoi]") TEST_CASE("Polygon from cell", "[Voronoi]")
{ {
// for debug #define SLA_CELL_2_POLYGON_DEBUG in VoronoiGraphUtils // for debug #define SLA_SVG_VISUALIZATION_CELL_2_POLYGON in VoronoiGraphUtils
double max_distance = 1e7; double max_distance = 1e7;
coord_t size = (int) (4e6); coord_t size = (int) (4e6);
coord_t half_size = size/2; coord_t half_size = size/2;