mirror of
https://git.mirrors.martin98.com/https://github.com/prusa3d/PrusaSlicer.git
synced 2025-08-01 04:42:00 +08:00
1426 lines
56 KiB
C++
1426 lines
56 KiB
C++
#include "VoronoiGraphUtils.hpp"
|
|
|
|
#include <cmath>
|
|
#include <set>
|
|
#include <libslic3r/Geometry/VoronoiOffset.hpp>
|
|
#include "IStackFunction.hpp"
|
|
#include "EvaluateNeighbor.hpp"
|
|
#include "ParabolaUtils.hpp"
|
|
#include "LineUtils.hpp"
|
|
#include "PointUtils.hpp"
|
|
#include "PolygonUtils.hpp"
|
|
|
|
#include <libslic3r/Geometry/VoronoiVisualUtils.hpp>
|
|
|
|
// comment definition of NDEBUG to enable assert()
|
|
//#define NDEBUG
|
|
#include <cassert>
|
|
|
|
//#define SLA_SVG_VISUALIZATION_CELL_2_POLYGON
|
|
|
|
using namespace Slic3r::sla;
|
|
|
|
coord_t VoronoiGraphUtils::to_coord(const VD::coordinate_type &coord)
|
|
{
|
|
static const VD::coordinate_type min_val =
|
|
static_cast<VD::coordinate_type>(std::numeric_limits<coord_t>::min());
|
|
static const VD::coordinate_type max_val =
|
|
static_cast<VD::coordinate_type>(std::numeric_limits<coord_t>::max());
|
|
if (coord > max_val) return std::numeric_limits<coord_t>::max();
|
|
if (coord < min_val) return std::numeric_limits<coord_t>::min();
|
|
return static_cast<coord_t>(std::round(coord));
|
|
}
|
|
|
|
Slic3r::Point VoronoiGraphUtils::to_point(const VD::vertex_type *vertex)
|
|
{
|
|
return Point(to_coord(vertex->x()), to_coord(vertex->y()));
|
|
}
|
|
|
|
VoronoiGraphUtils::VD::point_type VoronoiGraphUtils::to_point(const Point &point)
|
|
{
|
|
return VD::point_type(point.x(), point.y());
|
|
}
|
|
|
|
Slic3r::Vec2d VoronoiGraphUtils::to_point_d(const VD::vertex_type *vertex)
|
|
{
|
|
return Vec2d(vertex->x(), vertex->y());
|
|
}
|
|
|
|
VoronoiGraphUtils::VD::segment_type VoronoiGraphUtils::to_segment(const Line &line)
|
|
{
|
|
return VD::segment_type(to_point(line.a), to_point(line.b));
|
|
}
|
|
|
|
Slic3r::Point VoronoiGraphUtils::to_direction(const VD::edge_type *edge)
|
|
{
|
|
return to_direction_d(edge).cast<coord_t>();
|
|
}
|
|
|
|
Slic3r::Vec2d VoronoiGraphUtils::to_direction_d(const VD::edge_type *edge)
|
|
{
|
|
const VD::vertex_type *v0 = edge->vertex0();
|
|
const VD::vertex_type *v1 = edge->vertex1();
|
|
return Vec2d(v1->x() - v0->x(), v1->y() - v0->y());
|
|
}
|
|
|
|
bool VoronoiGraphUtils::is_coord_in_limits(const VD::coordinate_type &coord,
|
|
const coord_t & source,
|
|
double max_distance)
|
|
{
|
|
VD::coordinate_type min_val = source - max_distance;
|
|
VD::coordinate_type max_val = source + max_distance;
|
|
if (coord > max_val) return false;
|
|
if (coord < min_val) return false;
|
|
return true;
|
|
}
|
|
|
|
bool VoronoiGraphUtils::is_point_in_limits(const VD::vertex_type *vertex,
|
|
const Point & source,
|
|
double max_distance)
|
|
{
|
|
if (vertex == nullptr) return false;
|
|
return is_coord_in_limits(vertex->x(), source.x(), max_distance) &&
|
|
is_coord_in_limits(vertex->y(), source.y(), max_distance);
|
|
}
|
|
|
|
// create line segment between (in the middle) points. With size depend on their distance
|
|
Slic3r::Line VoronoiGraphUtils::create_line_between_source_points(
|
|
const Point &point1, const Point &point2, double maximal_distance)
|
|
{
|
|
Point middle = (point1 + point2) / 2;
|
|
Point diff = point1 - point2;
|
|
double distance_2 = diff.x() * static_cast<double>(diff.x()) +
|
|
diff.y() * static_cast<double>(diff.y());
|
|
double half_distance = sqrt(distance_2) / 2.;
|
|
double half_distance_2 = distance_2 / 4;
|
|
double size = sqrt(maximal_distance * maximal_distance - half_distance_2);
|
|
// normalized direction to side multiplied by size/2
|
|
double scale = size / half_distance / 2;
|
|
Point side_dir(-diff.y() * scale, diff.x() * scale);
|
|
return Line(middle - side_dir, middle + side_dir);
|
|
}
|
|
|
|
std::optional<Slic3r::Line> VoronoiGraphUtils::to_line(
|
|
const VD::edge_type &edge, const Points &points, double maximal_distance)
|
|
{
|
|
assert(edge.is_linear());
|
|
assert(edge.is_primary());
|
|
const Point &p1 = retrieve_point(points, *edge.cell());
|
|
const Point &p2 = retrieve_point(points, *edge.twin()->cell());
|
|
const VD::vertex_type *v0 = edge.vertex0();
|
|
const VD::vertex_type *v1 = edge.vertex1();
|
|
|
|
bool use_v1 = false; // v0 == nullptr or out of limits
|
|
bool use_double_precision = false;
|
|
bool use_both = false;
|
|
if (edge.is_finite()) {
|
|
bool is_v0_in_limit = is_point_in_limits(v0, p1, maximal_distance);
|
|
bool is_v1_in_limit = is_point_in_limits(v1, p1, maximal_distance);
|
|
if (!is_v0_in_limit) {
|
|
use_v1 = true;
|
|
if (!is_v1_in_limit) {
|
|
use_double_precision = true;
|
|
use_both = true;
|
|
}
|
|
} else if (is_v1_in_limit) {
|
|
// normal full edge line segment
|
|
return Line(to_point(v0), to_point(v1));
|
|
}
|
|
} else if (v0 == nullptr) {
|
|
if (v1 == nullptr)
|
|
{// both vertex are nullptr, create edge between points
|
|
return create_line_between_source_points(p1, p2, maximal_distance);
|
|
}
|
|
if (!is_point_in_limits(v1, p1, maximal_distance))
|
|
use_double_precision = true;
|
|
use_v1 = true;
|
|
} else if (!is_point_in_limits(v0, p1, maximal_distance)) {
|
|
use_double_precision = true;
|
|
if (v1 != nullptr)
|
|
use_v1 = true; // v1 is in
|
|
}
|
|
|
|
Point direction = (use_v1) ?
|
|
Point(p2.y() - p1.y(), p1.x() - p2.x()) :
|
|
Point(p1.y() - p2.y(), p2.x() - p1.x());
|
|
const VD::vertex_type* edge_vertex = (use_v1) ? v1 : v0;
|
|
// koeficient for crop line
|
|
if (!use_double_precision) {
|
|
Point ray_point = to_point(edge_vertex);
|
|
Line ray(ray_point, ray_point + direction);
|
|
return LineUtils::crop_half_ray(ray, p1, maximal_distance);
|
|
}
|
|
std::optional<Linef> segment;
|
|
if (use_both) {
|
|
Linef edge_segment(to_point_d(v0), to_point_d(v1));
|
|
segment = LineUtils::crop_line(edge_segment, p1, maximal_distance);
|
|
} else {
|
|
// Vertex can't be used as start point because data type limitation
|
|
// Explanation for shortening line is in Test::bad_vertex
|
|
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 {};
|
|
return Line(segment->a.cast<coord_t>(), segment->b.cast<coord_t>());
|
|
}
|
|
|
|
|
|
Slic3r::Polygon VoronoiGraphUtils::to_polygon(const Lines &lines,
|
|
const Point ¢er,
|
|
double maximal_distance,
|
|
double minimal_distance,
|
|
size_t count_points)
|
|
{
|
|
assert(minimal_distance > 0.);
|
|
assert(maximal_distance > minimal_distance);
|
|
assert(count_points >= 3);
|
|
if (lines.empty())
|
|
return PolygonUtils::create_regular(count_points, maximal_distance, center);
|
|
|
|
Points points;
|
|
points.reserve(std::max(lines.size(), count_points));
|
|
const Line *prev_line = &lines.back();
|
|
double max_angle = 2 * M_PI / count_points;
|
|
for (const Line &line : lines) {
|
|
const Point &p1 = prev_line->b;
|
|
const Point &p2 = line.a;
|
|
prev_line = &line;
|
|
Point diff = p1-p2;
|
|
if (abs(diff.x()) < minimal_distance &&
|
|
abs(diff.y()) < minimal_distance) {
|
|
Point avg = (p1 + p2) / 2;
|
|
points.push_back(avg);
|
|
continue;
|
|
}
|
|
Point v1 = p1 - center;
|
|
Point v2 = p2 - center;
|
|
double a1 = std::atan2(v1.y(), v1.x());
|
|
double a2 = std::atan2(v2.y(), v2.x());
|
|
|
|
double diff_angle = a2 - a1;
|
|
if(diff_angle < 0.) diff_angle += 2 * M_PI;
|
|
if(diff_angle > 2 * M_PI) diff_angle -= 2 * M_PI;
|
|
|
|
size_t count_segment = std::floor(fabs(diff_angle) / max_angle) + 1;
|
|
double increase_angle = diff_angle / count_segment;
|
|
points.push_back(p1);
|
|
for (size_t i = 1; i < count_segment; i++) {
|
|
double angle = a1 + i*increase_angle;
|
|
double x = cos(angle) * maximal_distance + center.x();
|
|
assert(x < std::numeric_limits<coord_t>::max());
|
|
assert(x > std::numeric_limits<coord_t>::min());
|
|
double y = sin(angle) * maximal_distance + center.y();
|
|
assert(y < std::numeric_limits<coord_t>::max());
|
|
assert(y > std::numeric_limits<coord_t>::min());
|
|
points.emplace_back(x,y);
|
|
}
|
|
points.push_back(p2);
|
|
}
|
|
Polygon polygon(points);
|
|
if (!polygon.contains(center)) {
|
|
draw(polygon, lines, center);
|
|
}
|
|
assert(polygon.is_valid());
|
|
assert(polygon.contains(center));
|
|
assert(PolygonUtils::is_not_self_intersect(polygon, center));
|
|
return polygon;
|
|
}
|
|
|
|
Slic3r::Polygon VoronoiGraphUtils::to_polygon(const VD::cell_type & cell,
|
|
const Slic3r::Points &points,
|
|
double maximal_distance)
|
|
{
|
|
Lines lines;
|
|
Point center = points[cell.source_index()];
|
|
// Convenient way to iterate edges around Voronoi cell.
|
|
const VD::edge_type *edge = cell.incident_edge();
|
|
do {
|
|
assert(edge->is_linear());
|
|
if (!edge->is_primary()) continue;
|
|
std::optional<Line> line = to_line(*edge, points, maximal_distance);
|
|
if (!line.has_value()) continue;
|
|
Geometry::Orientation orientation = Geometry::orient(center, line->a, line->b);
|
|
// Can be rich on circle over source point edge
|
|
if (orientation == Geometry::Orientation::ORIENTATION_COLINEAR)
|
|
continue;
|
|
if (orientation == Geometry::Orientation::ORIENTATION_CW)
|
|
std::swap(line->a, line->b);
|
|
lines.push_back(*line);
|
|
} while ((edge = edge->next()) && edge != cell.incident_edge());
|
|
assert(!lines.empty());
|
|
if (lines.size() > 1)
|
|
LineUtils::sort_CCW(lines, center);
|
|
// preccission to decide when not connect neighbor points
|
|
double min_distance = maximal_distance / 1000.;
|
|
size_t count_point = 6; // count added points
|
|
Slic3r::Polygon polygon = to_polygon(lines, center, maximal_distance, min_distance, count_point);
|
|
#ifdef SLA_SVG_VISUALIZATION_CELL_2_POLYGON
|
|
{
|
|
std::cout << "cell " << cell.source_index() << " has " << lines.size() << "edges" << std::endl;
|
|
BoundingBox bbox(center - Point(maximal_distance, maximal_distance),
|
|
center + Point(maximal_distance, maximal_distance));
|
|
static int counter = 0;
|
|
std::string filename = "polygon" + std::to_string(counter++) + ".svg";
|
|
SVG svg(filename.c_str(), bbox);
|
|
svg.draw(center, "lightgreen", maximal_distance);
|
|
svg.draw(polygon, "lightblue");
|
|
int index = 0;
|
|
for (auto &line : lines) {
|
|
svg.draw(line);
|
|
svg.draw_text(line.a, ("A"+std::to_string(++index)).c_str(), "green");
|
|
svg.draw_text(line.b, ("B" + std::to_string(index)).c_str(), "blue");
|
|
}
|
|
svg.draw(center, "red", maximal_distance / 100);
|
|
}
|
|
#endif /* SLA_SVG_VISUALIZATION_CELL_2_POLYGON */
|
|
return polygon;
|
|
}
|
|
|
|
VoronoiGraph::Node *VoronoiGraphUtils::getNode(VoronoiGraph & graph,
|
|
const VD::vertex_type *vertex,
|
|
const VD::edge_type * edge,
|
|
const Lines & lines)
|
|
{
|
|
std::map<const VD::vertex_type *, VoronoiGraph::Node> &data = graph.data;
|
|
auto mapItem = data.find(vertex);
|
|
// return when exists
|
|
if (mapItem != data.end()) return &mapItem->second;
|
|
|
|
// is new vertex (first edge to this vertex)
|
|
// calculate distance to islad border + fill item0
|
|
const VD::cell_type *cell = edge->cell();
|
|
// const VD::cell_type * cell2 = edge.twin()->cell();
|
|
const Line &line = lines[cell->source_index()];
|
|
// const Line & line1 = lines[cell2->source_index()];
|
|
Point point = to_point(vertex);
|
|
double distance = line.distance_to(point);
|
|
|
|
auto [iterator, success] = data.emplace(vertex, VoronoiGraph::Node(vertex, distance));
|
|
|
|
assert(success);
|
|
if (!success) return nullptr;
|
|
|
|
return &iterator->second;
|
|
}
|
|
|
|
Slic3r::Point VoronoiGraphUtils::retrieve_point(const Lines & lines,
|
|
const VD::cell_type &cell)
|
|
{
|
|
using namespace boost::polygon;
|
|
assert(cell.source_category() == SOURCE_CATEGORY_SEGMENT_START_POINT ||
|
|
cell.source_category() == SOURCE_CATEGORY_SEGMENT_END_POINT);
|
|
return (cell.source_category() == SOURCE_CATEGORY_SEGMENT_START_POINT) ?
|
|
lines[cell.source_index()].a :
|
|
lines[cell.source_index()].b;
|
|
}
|
|
|
|
const Slic3r::Point &VoronoiGraphUtils::retrieve_point(
|
|
const Points &points, const VD::cell_type &cell)
|
|
{
|
|
assert(cell.contains_point());
|
|
assert(cell.source_category() == boost::polygon::SOURCE_CATEGORY_SINGLE_POINT);
|
|
return points[cell.source_index()];
|
|
}
|
|
|
|
Slic3r::Point VoronoiGraphUtils::get_parabola_point(
|
|
const VD::edge_type ¶bola, const Slic3r::Lines &lines)
|
|
{
|
|
using namespace boost::polygon;
|
|
assert(parabola.is_curved());
|
|
const VD::cell_type& cell = (parabola.cell()->contains_point())?
|
|
*parabola.cell() : *parabola.twin()->cell();
|
|
assert(cell.contains_point());
|
|
assert(cell.source_category() == SOURCE_CATEGORY_SEGMENT_START_POINT ||
|
|
cell.source_category() == SOURCE_CATEGORY_SEGMENT_END_POINT);
|
|
return (cell.source_category() == SOURCE_CATEGORY_SEGMENT_START_POINT) ?
|
|
lines[cell.source_index()].a :
|
|
lines[cell.source_index()].b;
|
|
}
|
|
|
|
Slic3r::Line VoronoiGraphUtils::get_parabola_line(
|
|
const VD::edge_type ¶bola, const Slic3r::Lines &lines)
|
|
{
|
|
assert(parabola.is_curved());
|
|
const VD::cell_type& cell = (parabola.cell()->contains_segment())?
|
|
*parabola.cell() : *parabola.twin()->cell();
|
|
assert(cell.contains_segment());
|
|
return lines[cell.source_index()];
|
|
}
|
|
|
|
Parabola VoronoiGraphUtils::get_parabola(
|
|
const VD::edge_type &edge, const Lines &lines)
|
|
{
|
|
Point point = get_parabola_point(edge, lines);
|
|
Line line = get_parabola_line(edge, lines);
|
|
return Parabola(line, point);
|
|
}
|
|
|
|
double VoronoiGraphUtils::calculate_length_of_parabola(
|
|
const VD::edge_type & edge,
|
|
const Lines & lines)
|
|
{
|
|
Point v0 = to_point(edge.vertex0());
|
|
Point v1 = to_point(edge.vertex1());
|
|
ParabolaSegment parabola(get_parabola(edge, lines), v0, v1);
|
|
return ParabolaUtils::length(parabola);
|
|
}
|
|
|
|
double VoronoiGraphUtils::calculate_length(
|
|
const VD::edge_type &edge, const Lines &lines)
|
|
{
|
|
if (edge.is_linear()) {
|
|
const VD::vertex_type* v0 = edge.vertex0();
|
|
const VD::vertex_type* v1 = edge.vertex1();
|
|
double diffX = v0->x() - v1->x();
|
|
double diffY = v0->y() - v1->y();
|
|
return sqrt(diffX * diffX + diffY * diffY);
|
|
}
|
|
assert(edge.is_curved());
|
|
return calculate_length_of_parabola(edge, lines);
|
|
}
|
|
|
|
double VoronoiGraphUtils::calculate_max_width(
|
|
const VD::edge_type &edge, const Lines &lines)
|
|
{
|
|
auto get_squared_distance = [&](const VD::vertex_type *vertex,
|
|
const Point &point) -> double {
|
|
Point point_v = to_point(vertex);
|
|
Vec2d vector = (point - point_v).cast<double>();
|
|
return vector.x() * vector.x() + vector.y() * vector.y();
|
|
};
|
|
auto max_width = [&](const Point& point)->double{
|
|
return 2. *
|
|
sqrt(std::max(get_squared_distance(edge.vertex0(), point),
|
|
get_squared_distance(edge.vertex1(), point)));
|
|
};
|
|
|
|
if (edge.is_linear()) {
|
|
// edge line could be initialized by 2 points
|
|
if (edge.cell()->contains_point()) {
|
|
const Line &source_line = lines[edge.cell()->source_index()];
|
|
Point source_point;
|
|
if (edge.cell()->source_category() ==
|
|
boost::polygon::SOURCE_CATEGORY_SEGMENT_START_POINT)
|
|
source_point = source_line.a;
|
|
else {
|
|
assert(edge.cell()->source_category() ==
|
|
boost::polygon::SOURCE_CATEGORY_SEGMENT_END_POINT);
|
|
source_point = source_line.b;
|
|
}
|
|
return max_width(source_point);
|
|
}
|
|
assert(edge.cell()->contains_segment());
|
|
assert(!edge.twin()->cell()->contains_point());
|
|
assert(edge.twin()->cell()->contains_segment());
|
|
|
|
const Line &line = lines[edge.cell()->source_index()];
|
|
|
|
Point v0 = to_point(edge.vertex0());
|
|
Point v1 = to_point(edge.vertex1());
|
|
double distance0 = line.perp_distance_to(v0);
|
|
double distance1 = line.perp_distance_to(v1);
|
|
return 2 * std::max(distance0, distance1);
|
|
}
|
|
assert(edge.is_curved());
|
|
Parabola parabola = get_parabola(edge, lines);
|
|
// distance to point and line is same
|
|
// vector from edge vertex to parabola focus point
|
|
return max_width(parabola.focus);
|
|
}
|
|
|
|
std::pair<coord_t, coord_t> VoronoiGraphUtils::calculate_width(
|
|
const VD::edge_type &edge, const Lines &lines)
|
|
{
|
|
if (edge.is_linear())
|
|
return calculate_width_for_line(edge, lines);
|
|
return calculate_width_for_parabola(edge, lines);
|
|
}
|
|
|
|
std::pair<coord_t, coord_t> VoronoiGraphUtils::calculate_width_for_line(
|
|
const VD::edge_type &line_edge, const Lines &lines)
|
|
{
|
|
assert(line_edge.is_linear());
|
|
// edge line could be initialized by 2 points
|
|
if (line_edge.cell()->contains_point()) {
|
|
const Line &source_line = lines[line_edge.cell()->source_index()];
|
|
Point source_point;
|
|
if (line_edge.cell()->source_category() ==
|
|
boost::polygon::SOURCE_CATEGORY_SEGMENT_START_POINT)
|
|
source_point = source_line.a;
|
|
else {
|
|
assert(line_edge.cell()->source_category() ==
|
|
boost::polygon::SOURCE_CATEGORY_SEGMENT_END_POINT);
|
|
source_point = source_line.b;
|
|
}
|
|
return min_max_width(line_edge, source_point);
|
|
}
|
|
assert(line_edge.cell()->contains_segment());
|
|
assert(!line_edge.twin()->cell()->contains_point());
|
|
assert(line_edge.twin()->cell()->contains_segment());
|
|
const Line & line = lines[line_edge.cell()->source_index()];
|
|
Point v0 = to_point(line_edge.vertex0());
|
|
Point v1 = to_point(line_edge.vertex1());
|
|
double distance0 = line.perp_distance_to(v0);
|
|
double distance1 = line.perp_distance_to(v1);
|
|
std::pair<coord_t, coord_t> min_max(2 * static_cast<coord_t>(distance0),
|
|
2 * static_cast<coord_t>(distance1));
|
|
if (min_max.first > min_max.second)
|
|
std::swap(min_max.first, min_max.second);
|
|
return min_max;
|
|
}
|
|
|
|
std::pair<coord_t, coord_t> VoronoiGraphUtils::calculate_width_for_parabola(
|
|
const VD::edge_type ¶bola_edge, const Lines &lines)
|
|
{
|
|
assert(parabola_edge.is_curved());
|
|
// distance to point and line on parabola is same
|
|
Parabola parabola = get_parabola(parabola_edge, lines);
|
|
Point v0 = to_point(parabola_edge.vertex0());
|
|
Point v1 = to_point(parabola_edge.vertex1());
|
|
ParabolaSegment parabola_segment(parabola, v0, v1);
|
|
std::pair<coord_t, coord_t> min_max = min_max_width(parabola_edge, parabola.focus);
|
|
if (ParabolaUtils::is_over_zero(parabola_segment)) {
|
|
min_max.first = parabola.directrix.perp_distance_to(parabola.focus);
|
|
}
|
|
return min_max;
|
|
}
|
|
|
|
std::pair<coord_t, coord_t> VoronoiGraphUtils::min_max_width(
|
|
const VD::edge_type &edge, const Point &point)
|
|
{
|
|
auto distance = [](const VD::vertex_type *vertex,
|
|
const Point & point) -> coord_t {
|
|
Vec2d point_d = point.cast<double>();
|
|
Vec2d diff = point_d - to_point_d(vertex);
|
|
double distance = diff.norm();
|
|
return static_cast<coord_t>(std::round(distance));
|
|
};
|
|
std::pair<coord_t, coord_t> result(2 * distance(edge.vertex0(), point),
|
|
2 * distance(edge.vertex1(), point));
|
|
if (result.first > result.second) std::swap(result.first, result.second);
|
|
return result;
|
|
};
|
|
|
|
VoronoiGraph VoronoiGraphUtils::create_skeleton(const VD &vd, const Lines &lines)
|
|
{
|
|
// vd should be annotated.
|
|
// assert(Voronoi::debug::verify_inside_outside_annotations(vd));
|
|
|
|
VoronoiGraph skeleton;
|
|
for (const VD::edge_type &edge : vd.edges()) {
|
|
if (
|
|
// Ignore secondary and unbounded edges, they shall never be part
|
|
// of the skeleton.
|
|
edge.is_secondary() || edge.is_infinite() ||
|
|
// Skip the twin edge of an edge, that has already been processed.
|
|
&edge > edge.twin() ||
|
|
// Ignore outer edges.
|
|
(Voronoi::edge_category(edge) !=
|
|
Voronoi::EdgeCategory::PointsInside &&
|
|
Voronoi::edge_category(edge.twin()) !=
|
|
Voronoi::EdgeCategory::PointsInside))
|
|
continue;
|
|
|
|
const VD::vertex_type * v0 = edge.vertex0();
|
|
const VD::vertex_type * v1 = edge.vertex1();
|
|
Voronoi::VertexCategory category0 = Voronoi::vertex_category(*v0);
|
|
Voronoi::VertexCategory category1 = Voronoi::vertex_category(*v1);
|
|
if (category0 == Voronoi::VertexCategory::Outside ||
|
|
category1 == Voronoi::VertexCategory::Outside)
|
|
continue;
|
|
// only debug check annotation
|
|
if (category0 == Voronoi::VertexCategory::Unknown ||
|
|
category1 == Voronoi::VertexCategory::Unknown)
|
|
return {}; // vd must be annotated
|
|
|
|
double length = calculate_length(edge, lines);
|
|
coord_t min_width, max_width;
|
|
std::tie(min_width, max_width) = calculate_width(edge, lines);
|
|
auto neighbor_size = std::make_shared<VoronoiGraph::Node::Neighbor::Size>(
|
|
length, min_width, max_width);
|
|
|
|
VoronoiGraph::Node *node0 = getNode(skeleton, v0, &edge, lines);
|
|
VoronoiGraph::Node *node1 = getNode(skeleton, v1, &edge, lines);
|
|
// add extended Edge to graph, both side
|
|
node0->neighbors.emplace_back(&edge, node1, neighbor_size);
|
|
node1->neighbors.emplace_back(edge.twin(), node0, std::move(neighbor_size));
|
|
}
|
|
return skeleton;
|
|
}
|
|
|
|
const VoronoiGraph::Node::Neighbor *VoronoiGraphUtils::get_neighbor(
|
|
const VoronoiGraph::Node *from, const VoronoiGraph::Node *to)
|
|
{
|
|
for (const VoronoiGraph::Node::Neighbor &neighbor : from->neighbors)
|
|
if (neighbor.node == to) return &neighbor;
|
|
return nullptr;
|
|
}
|
|
|
|
double VoronoiGraphUtils::get_neighbor_distance(const VoronoiGraph::Node *from,
|
|
const VoronoiGraph::Node *to)
|
|
{
|
|
const VoronoiGraph::Node::Neighbor *neighbor = get_neighbor(from, to);
|
|
assert(neighbor != nullptr);
|
|
return neighbor->length();
|
|
}
|
|
|
|
VoronoiGraph::Path VoronoiGraphUtils::find_longest_path_on_circle(
|
|
const VoronoiGraph::Circle & circle,
|
|
const VoronoiGraph::ExPath::SideBranchesMap &side_branches)
|
|
{
|
|
double half_circle_length = circle.length / 2.;
|
|
double distance_on_circle = 0;
|
|
|
|
bool is_longest_revers_direction = false;
|
|
const VoronoiGraph::Node *longest_circle_node = nullptr;
|
|
const VoronoiGraph::Path *longest_circle_branch = nullptr;
|
|
double longest_branch_length = 0;
|
|
|
|
bool is_short_revers_direction = false;
|
|
// find longest side branch
|
|
const VoronoiGraph::Node *prev_circle_node = nullptr;
|
|
for (const VoronoiGraph::Node *circle_node : circle.nodes) {
|
|
if (prev_circle_node != nullptr)
|
|
distance_on_circle += get_neighbor_distance(circle_node,
|
|
prev_circle_node);
|
|
prev_circle_node = circle_node;
|
|
|
|
auto side_branches_item = side_branches.find(circle_node);
|
|
if (side_branches_item != side_branches.end()) {
|
|
// side_branches should be sorted by length
|
|
if (distance_on_circle > half_circle_length)
|
|
is_short_revers_direction = true;
|
|
const auto &longest_node_branch = side_branches_item->second.top();
|
|
double circle_branch_length = longest_node_branch.length +
|
|
((is_short_revers_direction) ?
|
|
(circle.length -
|
|
distance_on_circle) :
|
|
distance_on_circle);
|
|
if (longest_branch_length < circle_branch_length) {
|
|
longest_branch_length = circle_branch_length;
|
|
is_longest_revers_direction = is_short_revers_direction;
|
|
longest_circle_node = circle_node;
|
|
longest_circle_branch = &longest_node_branch;
|
|
}
|
|
}
|
|
}
|
|
assert(longest_circle_node !=
|
|
nullptr); // only circle with no side branches
|
|
assert(longest_circle_branch != nullptr);
|
|
// almost same - double preccission
|
|
// distance_on_circle += get_neighbor_distance(circle.path.back(),
|
|
// circle.path.front()); assert(distance_on_circle == circle.length);
|
|
|
|
// circlePath
|
|
auto circle_iterator = std::find(circle.nodes.begin(), circle.nodes.end(),
|
|
longest_circle_node);
|
|
VoronoiGraph::Nodes circle_path;
|
|
if (is_longest_revers_direction) {
|
|
circle_path = VoronoiGraph::Nodes(circle_iterator, circle.nodes.end());
|
|
std::reverse(circle_path.begin(), circle_path.end());
|
|
} else {
|
|
if (longest_circle_node != circle.nodes.front())
|
|
circle_path = VoronoiGraph::Nodes(circle.nodes.begin() + 1,
|
|
circle_iterator + 1);
|
|
}
|
|
// append longest side branch
|
|
circle_path.insert(circle_path.end(),
|
|
longest_circle_branch->nodes.begin(),
|
|
longest_circle_branch->nodes.end());
|
|
return {circle_path, longest_branch_length};
|
|
}
|
|
|
|
VoronoiGraph::Path VoronoiGraphUtils::find_longest_path_on_circles(
|
|
const VoronoiGraph::Node & input_node,
|
|
size_t finished_circle_index,
|
|
const VoronoiGraph::ExPath &ex_path)
|
|
{
|
|
const std::vector<VoronoiGraph::Circle> &circles = ex_path.circles;
|
|
const auto &circle = circles[finished_circle_index];
|
|
auto connected_circle_item = ex_path.connected_circle.find(
|
|
finished_circle_index);
|
|
// is only one circle
|
|
if (connected_circle_item == ex_path.connected_circle.end()) {
|
|
// find longest path over circle and store it into next_path
|
|
return find_longest_path_on_circle(circle, ex_path.side_branches);
|
|
}
|
|
|
|
// multi circle
|
|
// find longest path over circles
|
|
const std::set<size_t> &connected_circles = connected_circle_item->second;
|
|
|
|
// collect all circle ndoes
|
|
std::set<const VoronoiGraph::Node *> nodes;
|
|
nodes.insert(circle.nodes.begin(), circle.nodes.end());
|
|
for (size_t circle_index : connected_circles) {
|
|
const auto &circle = circles[circle_index];
|
|
nodes.insert(circle.nodes.begin(), circle.nodes.end());
|
|
}
|
|
|
|
// nodes are path throw circles
|
|
// length is sum path throw circles PLUS length of longest side_branch
|
|
VoronoiGraph::Path longest_path;
|
|
|
|
// wide search by shortest distance for path over circle's node
|
|
// !! Do NOT use recursion, may cause stack overflow
|
|
std::set<const VoronoiGraph::Node *> done; // all ready checked
|
|
// on top is shortest path
|
|
std::priority_queue<VoronoiGraph::Path, std::vector<VoronoiGraph::Path>,
|
|
VoronoiGraph::Path::OrderLengthFromShortest>
|
|
search_queue;
|
|
VoronoiGraph::Path start_path({&input_node}, 0.);
|
|
search_queue.emplace(start_path);
|
|
while (!search_queue.empty()) {
|
|
// shortest path from input_node
|
|
VoronoiGraph::Path path(std::move(search_queue.top()));
|
|
search_queue.pop();
|
|
const VoronoiGraph::Node &node = *path.nodes.back();
|
|
if (done.find(&node) != done.end()) { // already checked
|
|
continue;
|
|
}
|
|
done.insert(&node);
|
|
for (const VoronoiGraph::Node::Neighbor &neighbor : node.neighbors) {
|
|
if (nodes.find(neighbor.node) == nodes.end())
|
|
continue; // out of circles
|
|
if (done.find(neighbor.node) != done.end()) continue;
|
|
VoronoiGraph::Path neighbor_path = path; // make copy
|
|
neighbor_path.append(neighbor.node, neighbor.length());
|
|
search_queue.push(neighbor_path);
|
|
|
|
auto branches_item = ex_path.side_branches.find(neighbor.node);
|
|
// exist side from this neighbor node ?
|
|
if (branches_item == ex_path.side_branches.end()) continue;
|
|
const VoronoiGraph::Path &longest_branch = branches_item->second
|
|
.top();
|
|
double length = longest_branch.length + neighbor_path.length;
|
|
if (longest_path.length < length) {
|
|
longest_path.length = length;
|
|
longest_path.nodes = neighbor_path.nodes; // copy path
|
|
}
|
|
}
|
|
}
|
|
|
|
// create result path
|
|
assert(!longest_path.nodes.empty());
|
|
longest_path.nodes.erase(longest_path.nodes.begin()); // remove input_node
|
|
assert(!longest_path.nodes.empty());
|
|
auto branches_item = ex_path.side_branches.find(longest_path.nodes.back());
|
|
if (branches_item == ex_path.side_branches.end()) {
|
|
// longest path ends on circle
|
|
return longest_path;
|
|
}
|
|
const VoronoiGraph::Path &longest_branch = branches_item->second.top();
|
|
longest_path.nodes.insert(longest_path.nodes.end(),
|
|
longest_branch.nodes.begin(),
|
|
longest_branch.nodes.end());
|
|
return longest_path;
|
|
}
|
|
|
|
std::optional<VoronoiGraph::Circle> VoronoiGraphUtils::create_circle(
|
|
const VoronoiGraph::Path & path,
|
|
const VoronoiGraph::Node::Neighbor &neighbor)
|
|
{
|
|
VoronoiGraph::Nodes passed_nodes = path.nodes;
|
|
// detection of circle
|
|
// not neccesary to check last one in path
|
|
auto end_find = passed_nodes.end() - 1;
|
|
const auto &path_item = std::find(passed_nodes.begin(), end_find,
|
|
neighbor.node);
|
|
if (path_item == end_find) return {}; // circle not detected
|
|
// separate Circle:
|
|
VoronoiGraph::Nodes circle_path(path_item, passed_nodes.end());
|
|
// !!! Real circle lenght is calculated on detection of end circle
|
|
// now circle_length contain also lenght of path before circle
|
|
double circle_length = path.length + neighbor.length();
|
|
// solve of branch length will be at begin of cirlce
|
|
return VoronoiGraph::Circle(std::move(circle_path), circle_length);
|
|
};
|
|
|
|
void VoronoiGraphUtils::merge_connected_circle(
|
|
VoronoiGraph::ExPath::ConnectedCircles &dst,
|
|
VoronoiGraph::ExPath::ConnectedCircles &src,
|
|
size_t dst_circle_count)
|
|
{
|
|
std::set<size_t> done;
|
|
for (const auto &item : src) {
|
|
size_t dst_index = dst_circle_count + item.first;
|
|
if (done.find(dst_index) != done.end()) continue;
|
|
done.insert(dst_index);
|
|
|
|
std::set<size_t> connected_circle;
|
|
for (const size_t &src_index : item.second)
|
|
connected_circle.insert(dst_circle_count + src_index);
|
|
|
|
auto &dst_set = dst[dst_index];
|
|
dst_set.merge(connected_circle);
|
|
|
|
// write same information into connected circles
|
|
connected_circle = dst_set; // copy
|
|
connected_circle.insert(dst_index);
|
|
for (size_t prev_connection_idx : dst_set) {
|
|
done.insert(prev_connection_idx);
|
|
for (size_t connected_circle_idx : connected_circle) {
|
|
if (connected_circle_idx == prev_connection_idx) continue;
|
|
dst[prev_connection_idx].insert(connected_circle_idx);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void VoronoiGraphUtils::append_neighbor_branch(VoronoiGraph::ExPath &dst,
|
|
VoronoiGraph::ExPath &src)
|
|
{
|
|
// move side branches
|
|
if (!src.side_branches.empty())
|
|
dst.side_branches
|
|
.insert(std::make_move_iterator(src.side_branches.begin()),
|
|
std::make_move_iterator(src.side_branches.end()));
|
|
|
|
// move circles
|
|
if (!src.circles.empty()) {
|
|
// copy connected circles indexes
|
|
if (!src.connected_circle.empty()) {
|
|
merge_connected_circle(dst.connected_circle, src.connected_circle,
|
|
dst.circles.size());
|
|
}
|
|
dst.circles.insert(dst.circles.end(),
|
|
std::make_move_iterator(src.circles.begin()),
|
|
std::make_move_iterator(src.circles.end()));
|
|
}
|
|
}
|
|
|
|
void VoronoiGraphUtils::reshape_longest_path(VoronoiGraph::ExPath &path)
|
|
{
|
|
assert(path.nodes.size() >= 1);
|
|
|
|
double actual_length = 0.;
|
|
const VoronoiGraph::Node *prev_node = nullptr;
|
|
VoronoiGraph::Nodes origin_path = path.nodes; // make copy
|
|
// index to path
|
|
size_t path_index = 0;
|
|
for (const VoronoiGraph::Node *node : origin_path) {
|
|
if (prev_node != nullptr) {
|
|
++path_index;
|
|
actual_length += get_neighbor_distance(prev_node, node);
|
|
}
|
|
prev_node = node;
|
|
// increase actual length
|
|
|
|
auto side_branches_item = path.side_branches.find(node);
|
|
if (side_branches_item == path.side_branches.end())
|
|
continue; // no side branches
|
|
VoronoiGraph::ExPath::SideBranches &branches = side_branches_item
|
|
->second;
|
|
if (actual_length >= branches.top().length)
|
|
continue; // no longer branch
|
|
|
|
auto end_path = path.nodes.begin() + path_index;
|
|
VoronoiGraph::Path side_branch({path.nodes.begin(), end_path},
|
|
actual_length);
|
|
std::reverse(side_branch.nodes.begin(), side_branch.nodes.end());
|
|
VoronoiGraph::Path new_main_branch(std::move(branches.top()));
|
|
branches.pop();
|
|
std::reverse(new_main_branch.nodes.begin(),
|
|
new_main_branch.nodes.end());
|
|
// add old main path store into side branches - may be it is not neccessary
|
|
branches.push(std::move(side_branch));
|
|
|
|
// swap side branch with main branch
|
|
path.nodes.erase(path.nodes.begin(), end_path);
|
|
path.nodes.insert(path.nodes.begin(), new_main_branch.nodes.begin(),
|
|
new_main_branch.nodes.end());
|
|
|
|
path.length += new_main_branch.length;
|
|
path.length -= actual_length;
|
|
path_index = new_main_branch.nodes.size();
|
|
actual_length = new_main_branch.length;
|
|
}
|
|
}
|
|
|
|
VoronoiGraph::ExPath VoronoiGraphUtils::create_longest_path(
|
|
const VoronoiGraph::Node *start_node)
|
|
{
|
|
VoronoiGraph::ExPath longest_path;
|
|
CallStack call_stack;
|
|
call_stack.emplace(
|
|
std::make_unique<EvaluateNeighbor>(longest_path, start_node));
|
|
|
|
// depth search for longest path in graph
|
|
while (!call_stack.empty()) {
|
|
std::unique_ptr<IStackFunction> stack_function = std::move(
|
|
call_stack.top());
|
|
call_stack.pop();
|
|
stack_function->process(call_stack);
|
|
// stack function deleted
|
|
}
|
|
|
|
reshape_longest_path(longest_path);
|
|
// after reshape it shoud be longest path for whole Voronoi Graph
|
|
return longest_path;
|
|
}
|
|
|
|
const VoronoiGraph::Node::Neighbor *VoronoiGraphUtils::get_twin(const VoronoiGraph::Node::Neighbor& neighbor)
|
|
{
|
|
auto twin_edge = neighbor.edge->twin();
|
|
for (const VoronoiGraph::Node::Neighbor &twin_neighbor : neighbor.node->neighbors) {
|
|
if (twin_neighbor.edge == twin_edge) return &twin_neighbor;
|
|
}
|
|
assert(false);
|
|
return nullptr;
|
|
}
|
|
|
|
const VoronoiGraph::Node *VoronoiGraphUtils::get_twin_node(const VoronoiGraph::Node::Neighbor &neighbor)
|
|
{
|
|
return get_twin(neighbor)->node;
|
|
}
|
|
|
|
bool VoronoiGraphUtils::is_opposit_direction(const VD::edge_type *edge, const Line &line)
|
|
{
|
|
Point dir_line = LineUtils::direction(line);
|
|
Point dir_edge = VoronoiGraphUtils::to_direction(edge);
|
|
return !PointUtils::is_same_direction(dir_line, dir_edge);
|
|
}
|
|
|
|
Slic3r::Point VoronoiGraphUtils::create_edge_point(
|
|
const VoronoiGraph::Position &position)
|
|
{
|
|
return create_edge_point(position.neighbor->edge, position.ratio);
|
|
}
|
|
|
|
Slic3r::Point VoronoiGraphUtils::create_edge_point(const VD::edge_type *edge,
|
|
double ratio)
|
|
{
|
|
const VD::vertex_type *v0 = edge->vertex0();
|
|
const VD::vertex_type *v1 = edge->vertex1();
|
|
if (ratio <= std::numeric_limits<double>::epsilon())
|
|
return Point(v0->x(), v0->y());
|
|
if (ratio >= 1. - std::numeric_limits<double>::epsilon())
|
|
return Point(v1->x(), v1->y());
|
|
|
|
if (edge->is_linear()) {
|
|
Point dir(v1->x() - v0->x(), v1->y() - v0->y());
|
|
// normalize
|
|
dir *= ratio;
|
|
return Point(v0->x() + dir.x(), v0->y() + dir.y());
|
|
}
|
|
|
|
assert(edge->is_curved());
|
|
// TODO: distance on curve
|
|
|
|
// approx by line
|
|
Point dir(v1->x() - v0->x(), v1->y() - v0->y());
|
|
dir *= ratio;
|
|
return Point(v0->x() + dir.x(), v0->y() + dir.y());
|
|
}
|
|
|
|
// NOTE: Heuristic is bad -> Width is not linear on edge e.g. VD of hexagon
|
|
// Solution: Edge has to know width changes.
|
|
VoronoiGraph::Position VoronoiGraphUtils::get_position_with_width(
|
|
const VoronoiGraph::Node::Neighbor *neighbor, coord_t width, const Slic3r::Lines &lines)
|
|
{
|
|
VoronoiGraph::Position result(neighbor, 0.);
|
|
const VD::edge_type *edge = neighbor->edge;
|
|
if (edge->is_curved()) {
|
|
// Every point on curve has same distance from outline
|
|
// !!! NOT TRUE !!!
|
|
// Only same distance from point and line !!!
|
|
// TODO: Fix it
|
|
return result;
|
|
}
|
|
assert(edge->is_finite());
|
|
Slic3r::Line edge_line(to_point(edge->vertex0()), to_point(edge->vertex1()));
|
|
const Slic3r::Line &source_line = lines[edge->cell()->source_index()];
|
|
if (LineUtils::is_parallel(edge_line, source_line)) {
|
|
// Every point on parallel lines has same distance
|
|
return result;
|
|
}
|
|
|
|
double half_width = width / 2.;
|
|
|
|
double a_dist = source_line.perp_distance_to(edge_line.a);
|
|
double b_dist = source_line.perp_distance_to(edge_line.b);
|
|
|
|
// check if half_width is in range from a_dist to b_dist
|
|
if (a_dist > b_dist) {
|
|
if (b_dist >= half_width) {
|
|
// vertex1 is closer to width
|
|
result.ratio = 1.;
|
|
return result;
|
|
} else if (a_dist <= half_width) {
|
|
// vertex0 is closer to width
|
|
return result;
|
|
}
|
|
} else {
|
|
// a_dist < b_dist
|
|
if (a_dist >= half_width) {
|
|
// vertex0 is closer to width
|
|
return result;
|
|
} else if (b_dist <= half_width) {
|
|
// vertex1 is closer to width
|
|
result.ratio = 1.;
|
|
return result;
|
|
}
|
|
}
|
|
result.ratio = fabs((a_dist - half_width) / (a_dist - b_dist));
|
|
return result;
|
|
}
|
|
|
|
std::pair<Slic3r::Point, Slic3r::Point> VoronoiGraphUtils::point_on_lines(
|
|
const VoronoiGraph::Position &position, const Lines &lines)
|
|
{
|
|
const VD::edge_type *edge = position.neighbor->edge;
|
|
|
|
// TODO: solve point on parabola
|
|
//assert(edge->is_linear());
|
|
|
|
Point edge_point = create_edge_point(position);
|
|
auto point_on_line = [&](const VD::edge_type *edge) -> Point {
|
|
assert(edge->is_finite());
|
|
const VD::cell_type *cell = edge->cell();
|
|
size_t line_index = cell->source_index();
|
|
const Line &line = lines[line_index];
|
|
using namespace boost::polygon;
|
|
if (cell->source_category() == SOURCE_CATEGORY_SEGMENT_START_POINT) {
|
|
return line.a;
|
|
}
|
|
if (cell->source_category() == SOURCE_CATEGORY_SEGMENT_END_POINT) {
|
|
return line.b;
|
|
}
|
|
|
|
Point dir = LineUtils::direction(line);
|
|
Line intersecting_line(edge_point, edge_point + PointUtils::perp(dir));
|
|
std::optional<Vec2d> intersection = LineUtils::intersection(line, intersecting_line);
|
|
assert(intersection.has_value());
|
|
return intersection->cast<coord_t>();
|
|
};
|
|
|
|
return {point_on_line(edge), point_on_line(edge->twin())};
|
|
}
|
|
|
|
VoronoiGraph::Position VoronoiGraphUtils::align(
|
|
const VoronoiGraph::Position &position, const Point &to, double max_distance)
|
|
{
|
|
// for each neighbor in max distance try align edge
|
|
struct NodeDistance
|
|
{
|
|
const VoronoiGraph::Node *node;
|
|
double distance; // distance to search for closest point
|
|
NodeDistance(const VoronoiGraph::Node *node, double distance)
|
|
: node(node), distance(distance)
|
|
{}
|
|
};
|
|
std::queue<NodeDistance> process;
|
|
const VoronoiGraph::Node::Neighbor* neighbor = position.neighbor;
|
|
double from_distance = neighbor->length() * position.ratio;
|
|
if (from_distance < max_distance) {
|
|
const VoronoiGraph::Node *from_node = VoronoiGraphUtils::get_twin_node(*neighbor);
|
|
process.emplace(from_node, from_distance);
|
|
}
|
|
double to_distance = neighbor->length() * (1 - position.ratio);
|
|
if (to_distance < max_distance) {
|
|
const VoronoiGraph::Node *to_node = neighbor->node;
|
|
process.emplace(to_node, to_distance);
|
|
}
|
|
if (process.empty()) {
|
|
const VoronoiGraph::Node *node = (position.ratio < 0.5) ?
|
|
VoronoiGraphUtils::get_twin_node(*neighbor) : neighbor->node;
|
|
process.emplace(node, max_distance);
|
|
}
|
|
|
|
double closest_distance = std::numeric_limits<double>::max();
|
|
VoronoiGraph::Position closest;
|
|
|
|
std::set<const VoronoiGraph::Node *> done;
|
|
while (!process.empty()) {
|
|
NodeDistance nd = process.front(); // copy
|
|
process.pop();
|
|
if (done.find(nd.node) != done.end()) continue;
|
|
done.insert(nd.node);
|
|
for (const auto &neighbor : nd.node->neighbors) {
|
|
if (done.find(neighbor.node) != done.end()) continue;
|
|
double ratio;
|
|
double distance = get_distance(*neighbor.edge, to, ratio);
|
|
if (closest_distance > distance) {
|
|
closest_distance = distance;
|
|
closest = VoronoiGraph::Position(&neighbor, ratio);
|
|
}
|
|
double from_start = nd.distance + neighbor.length();
|
|
if (from_start < max_distance)
|
|
process.emplace(neighbor.node, from_start);
|
|
}
|
|
}
|
|
return closest;
|
|
}
|
|
|
|
double VoronoiGraphUtils::get_distance(const VD::edge_type &edge,
|
|
const Point & point,
|
|
double & edge_ratio)
|
|
{
|
|
// TODO: find closest point on curve edge
|
|
//if (edge.is_linear()) {
|
|
|
|
// get line foot point, inspired Geometry::foot_pt
|
|
Vec2d v0 = to_point_d(edge.vertex0());
|
|
Vec2d v = point.cast<double>() - v0;
|
|
Vec2d v1 = to_point_d(edge.vertex1());
|
|
Vec2d edge_dir = v1 - v0;
|
|
double l2 = edge_dir.squaredNorm();
|
|
edge_ratio = v.dot(edge_dir) / l2;
|
|
// IMPROVE: not neccesary to calculate point if (edge_ratio > 1 || edge_ratio < 0)
|
|
Point edge_point;
|
|
if (edge_ratio > 1.) edge_point = v1.cast<coord_t>();
|
|
else if (edge_ratio < 0.) edge_point = v0.cast<coord_t>();
|
|
else { // foot point
|
|
edge_point = (v0 + edge_dir * edge_ratio).cast<coord_t>();
|
|
}
|
|
double distance = (point - edge_point).cast<double>().norm();
|
|
return distance;
|
|
}
|
|
|
|
|
|
const VoronoiGraph::Node *VoronoiGraphUtils::getFirstContourNode(
|
|
const VoronoiGraph &graph)
|
|
{
|
|
for (const auto &[key, value] : graph.data) {
|
|
const VD::vertex_type & vertex = *key;
|
|
Voronoi::VertexCategory category = Voronoi::vertex_category(vertex);
|
|
if (category == Voronoi::VertexCategory::OnContour) {
|
|
return &value;
|
|
}
|
|
}
|
|
return nullptr;
|
|
}
|
|
|
|
coord_t VoronoiGraphUtils::get_max_width(const VoronoiGraph::Nodes &path)
|
|
{
|
|
coord_t max = 0;
|
|
const VoronoiGraph::Node *prev_node = nullptr;
|
|
for (const VoronoiGraph::Node *node : path) {
|
|
if (prev_node == nullptr) {
|
|
prev_node = node;
|
|
continue;
|
|
}
|
|
const VoronoiGraph::Node::Neighbor *neighbor = get_neighbor(prev_node, node);
|
|
if (max < neighbor->max_width()) max = neighbor->max_width();
|
|
prev_node = node;
|
|
}
|
|
return max;
|
|
}
|
|
|
|
coord_t VoronoiGraphUtils::get_max_width(
|
|
const VoronoiGraph::ExPath &longest_path)
|
|
{
|
|
coord_t max = get_max_width(longest_path.nodes);
|
|
for (const auto &side_branches_item : longest_path.side_branches) {
|
|
const VoronoiGraph::Node *prev_node = side_branches_item.first;
|
|
VoronoiGraph::ExPath::SideBranches side_branches = side_branches_item.second; // !!! copy
|
|
while (!side_branches.empty()) {
|
|
const VoronoiGraph::Path &side_path = side_branches.top();
|
|
const VoronoiGraph::Node::Neighbor *first_neighbor =
|
|
get_neighbor(prev_node, side_path.nodes.front());
|
|
coord_t max_side_branch = std::max(
|
|
get_max_width(side_path.nodes), first_neighbor->max_width());
|
|
if (max < max_side_branch) max = max_side_branch;
|
|
side_branches.pop();
|
|
}
|
|
}
|
|
|
|
for (const VoronoiGraph::Circle &circle : longest_path.circles) {
|
|
const VoronoiGraph::Node::Neighbor *first_neighbor =
|
|
get_neighbor(circle.nodes.front(), circle.nodes.back());
|
|
double max_circle = std::max(
|
|
first_neighbor->max_width(), get_max_width(circle.nodes));
|
|
if (max < max_circle) max = max_circle;
|
|
}
|
|
|
|
return max;
|
|
}
|
|
|
|
// !!! is slower than go along path
|
|
coord_t VoronoiGraphUtils::get_max_width(const VoronoiGraph::Node *node)
|
|
{
|
|
coord_t max = 0;
|
|
std::set<const VoronoiGraph::Node *> done;
|
|
std::queue<const VoronoiGraph::Node *> process;
|
|
process.push(node);
|
|
while (!process.empty()) {
|
|
const VoronoiGraph::Node *actual_node = process.front();
|
|
process.pop();
|
|
if (done.find(actual_node) != done.end()) continue;
|
|
for (const VoronoiGraph::Node::Neighbor& neighbor: actual_node->neighbors) {
|
|
if (done.find(neighbor.node) != done.end()) continue;
|
|
process.push(neighbor.node);
|
|
if (max < neighbor.max_width()) max = neighbor.max_width();
|
|
}
|
|
done.insert(actual_node);
|
|
}
|
|
return max;
|
|
}
|
|
|
|
// START use instead of is_last_neighbor
|
|
bool VoronoiGraphUtils::ends_in_distanace(const VoronoiGraph::Position &position, coord_t max_distance) {
|
|
const VoronoiGraph::Node *node = position.neighbor->node;
|
|
coord_t rest_distance = max_distance - position.calc_rest_distance();
|
|
if (rest_distance < 0)
|
|
return false;
|
|
|
|
// speed up - end of gpraph is no need investigate further
|
|
if (node->neighbors.size() == 1)
|
|
return true;
|
|
|
|
// Already processed nodes
|
|
std::set<const VoronoiGraph::Node *> done;
|
|
done.insert(get_twin_node(*position.neighbor));
|
|
|
|
struct Next{
|
|
const VoronoiGraph::Node *node;
|
|
coord_t rest_distance;
|
|
};
|
|
// sorted by distance from position from biggest
|
|
std::vector<Next> process_queue;
|
|
do {
|
|
done.insert(node);
|
|
for (const VoronoiGraph::Node::Neighbor &neighbor: node->neighbors){
|
|
const VoronoiGraph::Node *neighbor_node = neighbor.node;
|
|
// Check whether node is already done
|
|
// Nodes are processed from closer to position
|
|
// soo done neighbor have to has bigger rest_distance
|
|
if (done.find(neighbor_node) != done.end())
|
|
// node is already explore
|
|
continue;
|
|
|
|
coord_t neighbor_rest = rest_distance - static_cast<coord_t>(neighbor.length());
|
|
if (neighbor_rest < 0)
|
|
// exist node far than max distance
|
|
return false;
|
|
|
|
// speed up - end of gpraph is no need to add to the process queue
|
|
if (neighbor_node->neighbors.size() == 1)
|
|
continue;
|
|
|
|
// check whether exist in queue this node with farer path and fix it
|
|
auto it = std::find_if(process_queue.begin(), process_queue.end(),
|
|
[neighbor_node](const Next &n) { return n.node == neighbor_node;});
|
|
if (it == process_queue.end()){
|
|
process_queue.emplace_back(Next{neighbor_node, neighbor_rest});
|
|
} else if (it->rest_distance < neighbor_rest) {
|
|
// found shorter path to node
|
|
it->rest_distance = neighbor_rest;
|
|
}
|
|
}
|
|
|
|
if (process_queue.empty())
|
|
return true;
|
|
|
|
// find biggest rest distance -> closest to input position
|
|
auto next = std::max_element(process_queue.begin(), process_queue.end(),
|
|
[](const Next& n1, const Next& n2){
|
|
return n1.rest_distance < n2.rest_distance;
|
|
});
|
|
rest_distance = next->rest_distance;
|
|
node = next->node;
|
|
process_queue.erase(next); // process queue pop
|
|
} while (true);
|
|
}
|
|
|
|
void VoronoiGraphUtils::for_neighbor_at_distance(
|
|
const VoronoiGraph::Position &position,
|
|
coord_t max_distance,
|
|
std::function<void(const VoronoiGraph::Node::Neighbor &, coord_t)> fnc)
|
|
{
|
|
coord_t act_distance = position.calc_distance();
|
|
const VoronoiGraph::Node *act_node = position.neighbor->node;
|
|
const VoronoiGraph::Node *twin_node = get_twin_node(*position.neighbor);
|
|
std::set<const VoronoiGraph::Node *> done;
|
|
done.insert(twin_node);
|
|
done.insert(act_node);
|
|
std::queue<std::pair<const VoronoiGraph::Node *, coord_t>> process;
|
|
coord_t distance = position.calc_rest_distance();
|
|
if (distance < max_distance) process.push({twin_node, distance});
|
|
|
|
while (true) {
|
|
const VoronoiGraph::Node *next_node = nullptr;
|
|
coord_t next_distance = 0;
|
|
for (const auto &neighbor : act_node->neighbors) {
|
|
if (done.find(neighbor.node) != done.end())
|
|
continue; // already checked
|
|
done.insert(neighbor.node);
|
|
|
|
fnc(neighbor, act_distance);
|
|
|
|
coord_t length = static_cast<coord_t>(neighbor.length());
|
|
coord_t distance = act_distance + length;
|
|
if (distance >= max_distance) continue;
|
|
if (next_node == nullptr) {
|
|
next_node = neighbor.node;
|
|
next_distance = distance;
|
|
} else {
|
|
process.push({neighbor.node, distance});
|
|
}
|
|
}
|
|
if (next_node != nullptr) { // exist next node
|
|
act_node = next_node;
|
|
act_distance = next_distance;
|
|
} else if (!process.empty()) { // exist next process
|
|
act_node = process.front().first;
|
|
act_distance = process.front().second;
|
|
process.pop();
|
|
} else { // no next node neither process
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
double VoronoiGraphUtils::outline_angle(const VoronoiGraph::Node::Neighbor &neighbor, const Lines& lines)
|
|
{
|
|
assert(neighbor.edge->is_linear());
|
|
assert(neighbor.min_width() == 0);
|
|
const VD::cell_type *c1 = neighbor.edge->cell();
|
|
const VD::cell_type *c2 = neighbor.edge->twin()->cell();
|
|
|
|
const Line &l1 = lines[c1->source_index()];
|
|
const Line &l2 = lines[c2->source_index()];
|
|
|
|
Vec2d d1 = LineUtils::direction(l1).cast<double>();
|
|
Vec2d d2 = LineUtils::direction(l2).cast<double>();
|
|
|
|
double dot = d1.dot(-d2);
|
|
return std::acos(dot/d1.norm() / d2.norm());
|
|
}
|
|
|
|
void VoronoiGraphUtils::draw(SVG & svg,
|
|
const VoronoiGraph &graph,
|
|
const Lines & lines,
|
|
coord_t width,
|
|
bool pointer_caption)
|
|
{
|
|
LineUtils::draw(svg, lines, "black", 0., true);
|
|
|
|
auto print_address = [&](const Point& p, const char* prefix, void * addr, const char* color){
|
|
if (pointer_caption) {
|
|
std::stringstream ss;
|
|
ss << prefix << std::hex << reinterpret_cast<intptr_t>(addr);
|
|
std::string s = ss.str();
|
|
svg.draw_text(p, s.c_str(), color);
|
|
}
|
|
};
|
|
|
|
for (const auto &[key, value] : graph.data) {
|
|
Point p(key->x(), key->y());
|
|
svg.draw(p, "lightgray", width);
|
|
print_address(p, "vertex ptr ",(void*)key, "lightgray");
|
|
for (const auto &n : value.neighbors) {
|
|
Point from = to_point(n.edge->vertex0());
|
|
Point to = to_point(n.edge->vertex1());
|
|
bool is_second = n.edge->vertex0() > n.edge->vertex1();
|
|
Point center = (from + to) / 2;
|
|
Point p = center + ((is_second) ? Point(0., -2e6) :
|
|
Point(0., 2e6));
|
|
print_address(p, "neighbor ptr ", (void *) &n, "gray");
|
|
if (is_second) continue;
|
|
std::string width_str = "width min=" + std::to_string(n.min_width()) +
|
|
" max=" + std::to_string(n.max_width());
|
|
svg.draw_text(center + Point(-6e6, 0.), width_str.c_str(), "gray");
|
|
draw(svg, *n.edge, lines, "gray", width);
|
|
}
|
|
}
|
|
}
|
|
|
|
void VoronoiGraphUtils::draw(SVG & svg,
|
|
const VD::edge_type &edge,
|
|
const Lines & lines,
|
|
const char * color,
|
|
coord_t width)
|
|
{
|
|
Point from = to_point(edge.vertex0());
|
|
Point to = to_point(edge.vertex1());
|
|
if (edge.is_curved()) {
|
|
Parabola p = get_parabola(edge, lines);
|
|
ParabolaSegment ps(p, from, to);
|
|
ParabolaUtils::draw(svg, ps, color, width);
|
|
return;
|
|
}
|
|
svg.draw(Line(from, to), color, width);
|
|
}
|
|
|
|
|
|
void VoronoiGraphUtils::draw(SVG & svg,
|
|
const VoronoiGraph::Nodes &path,
|
|
coord_t width,
|
|
const char * color,
|
|
bool finish)
|
|
{
|
|
const VoronoiGraph::Node *prev_node = (finish) ? path.back() : nullptr;
|
|
int index = 0;
|
|
for (auto &node : path) {
|
|
++index;
|
|
if (prev_node == nullptr) {
|
|
prev_node = node;
|
|
continue;
|
|
}
|
|
|
|
Point from = to_point(prev_node->vertex);
|
|
Point to = to_point(node->vertex);
|
|
svg.draw(Line(from, to), color, width);
|
|
|
|
svg.draw_text(from, std::to_string(index - 1).c_str(), color);
|
|
svg.draw_text(to, std::to_string(index).c_str(), color);
|
|
prev_node = node;
|
|
}
|
|
}
|
|
|
|
void VoronoiGraphUtils::draw(SVG & svg,
|
|
const VoronoiGraph::ExPath &path,
|
|
coord_t width)
|
|
{
|
|
const char *circlePathColor = "green";
|
|
const char *sideBranchesColor = "blue";
|
|
const char *mainPathColor = "red";
|
|
|
|
for (auto &circle : path.circles) {
|
|
draw(svg, circle.nodes, width, circlePathColor, true);
|
|
Point center(0, 0);
|
|
for (auto p : circle.nodes) {
|
|
center += to_point(p->vertex);
|
|
}
|
|
center.x() /= circle.nodes.size();
|
|
center.y() /= circle.nodes.size();
|
|
|
|
svg.draw_text(center,
|
|
("C" + std::to_string(&circle - &path.circles.front()))
|
|
.c_str(),
|
|
circlePathColor);
|
|
}
|
|
|
|
for (const auto &branches : path.side_branches) {
|
|
auto tmp = branches.second; // copy
|
|
while (!tmp.empty()) {
|
|
const auto &branch = tmp.top();
|
|
auto path = branch.nodes;
|
|
path.insert(path.begin(), branches.first);
|
|
draw(svg, path, width, sideBranchesColor);
|
|
tmp.pop();
|
|
}
|
|
}
|
|
|
|
draw(svg, path.nodes, width, mainPathColor);
|
|
}
|
|
|
|
void VoronoiGraphUtils::draw(const Polygon &polygon,
|
|
const Lines & lines,
|
|
const Point & center)
|
|
{
|
|
SVG svg("Bad_polygon.svg", {polygon.points});
|
|
svg.draw(polygon, "orange");
|
|
LineUtils::draw(svg, lines, "red", 0., true, true);
|
|
svg.draw(center);
|
|
} |