366 lines
17 KiB
C++
366 lines
17 KiB
C++
|
#ifndef SRC_LIBSLIC3R_AABBTREELINES_HPP_
|
||
|
|
#define SRC_LIBSLIC3R_AABBTREELINES_HPP_
|
||
|
|
|
||
|
|
#include "Point.hpp"
|
||
|
|
#include "Utils.hpp"
|
||
|
|
#include "libslic3r.h"
|
||
|
|
#include "libslic3r/AABBTreeIndirect.hpp"
|
||
|
|
#include "libslic3r/Line.hpp"
|
||
|
|
#include <algorithm>
|
||
|
|
#include <cmath>
|
||
|
|
#include <type_traits>
|
||
|
|
#include <vector>
|
||
|
|
|
||
|
|
namespace Slic3r { namespace AABBTreeLines {
|
||
|
|
|
||
|
|
namespace detail {
|
||
|
|
|
||
|
|
template<typename ALineType, typename ATreeType, typename AVectorType> struct IndexedLinesDistancer
|
||
|
|
{
|
||
|
|
using LineType = ALineType;
|
||
|
|
using TreeType = ATreeType;
|
||
|
|
using VectorType = AVectorType;
|
||
|
|
using ScalarType = typename VectorType::Scalar;
|
||
|
|
|
||
|
|
const std::vector<LineType> &lines;
|
||
|
|
const TreeType &tree;
|
||
|
|
|
||
|
|
const VectorType origin;
|
||
|
|
|
||
|
|
inline VectorType closest_point_to_origin(size_t primitive_index, ScalarType &squared_distance) const
|
||
|
|
{
|
||
|
|
Vec<LineType::Dim, typename LineType::Scalar> nearest_point;
|
||
|
|
const LineType &line = lines[primitive_index];
|
||
|
|
squared_distance = line_alg::distance_to_squared(line, origin.template cast<typename LineType::Scalar>(), &nearest_point);
|
||
|
|
return nearest_point.template cast<ScalarType>();
|
||
|
|
}
|
||
|
|
};
|
||
|
|
|
||
|
|
// returns number of intersections of ray starting in ray_origin and following the specified coordinate line with lines in tree
|
||
|
|
// first number is hits in positive direction of ray, second number hits in negative direction. returns neagtive numbers when ray_origin is
|
||
|
|
// on some line exactly.
|
||
|
|
template<typename LineType, typename TreeType, typename VectorType, int coordinate>
|
||
|
|
inline std::tuple<int, int> coordinate_aligned_ray_hit_count(size_t node_idx,
|
||
|
|
const TreeType &tree,
|
||
|
|
const std::vector<LineType> &lines,
|
||
|
|
const VectorType &ray_origin)
|
||
|
|
{
|
||
|
|
static constexpr int other_coordinate = (coordinate + 1) % 2;
|
||
|
|
using Scalar = typename LineType::Scalar;
|
||
|
|
using Floating = typename std::conditional<std::is_floating_point<Scalar>::value, Scalar, double>::type;
|
||
|
|
const auto &node = tree.node(node_idx);
|
||
|
|
assert(node.is_valid());
|
||
|
|
if (node.is_leaf()) {
|
||
|
|
const LineType &line = lines[node.idx];
|
||
|
|
if (ray_origin[other_coordinate] < std::min(line.a[other_coordinate], line.b[other_coordinate]) ||
|
||
|
|
ray_origin[other_coordinate] >= std::max(line.a[other_coordinate], line.b[other_coordinate])) {
|
||
|
|
// the second inequality is nonsharp for a reason
|
||
|
|
// without it, we may count contour border twice when the lines meet exactly at the spot of intersection. this prevents is
|
||
|
|
return {0, 0};
|
||
|
|
}
|
||
|
|
|
||
|
|
Scalar line_max = std::max(line.a[coordinate], line.b[coordinate]);
|
||
|
|
Scalar line_min = std::min(line.a[coordinate], line.b[coordinate]);
|
||
|
|
if (ray_origin[coordinate] > line_max) {
|
||
|
|
return {1, 0};
|
||
|
|
} else if (ray_origin[coordinate] < line_min) {
|
||
|
|
return {0, 1};
|
||
|
|
} else {
|
||
|
|
// find intersection of ray with line
|
||
|
|
// that is when ( line.a + t * (line.b - line.a) )[other_coordinate] == ray_origin[other_coordinate]
|
||
|
|
// t = ray_origin[oc] - line.a[oc] / (line.b[oc] - line.a[oc]);
|
||
|
|
// then we want to get value of intersection[ coordinate]
|
||
|
|
// val_c = line.a[c] + t * (line.b[c] - line.a[c]);
|
||
|
|
// Note that ray and line may overlap, when (line.b[oc] - line.a[oc]) is zero
|
||
|
|
// In that case, we return negative number
|
||
|
|
Floating distance_oc = line.b[other_coordinate] - line.a[other_coordinate];
|
||
|
|
Floating t = (ray_origin[other_coordinate] - line.a[other_coordinate]) / distance_oc;
|
||
|
|
Floating val_c = line.a[coordinate] + t * (line.b[coordinate] - line.a[coordinate]);
|
||
|
|
if (ray_origin[coordinate] > val_c) {
|
||
|
|
return {1, 0};
|
||
|
|
} else if (ray_origin[coordinate] < val_c) {
|
||
|
|
return {0, 1};
|
||
|
|
} else { // ray origin is on boundary
|
||
|
|
return {-1, -1};
|
||
|
|
}
|
||
|
|
}
|
||
|
|
} else {
|
||
|
|
int intersections_above = 0;
|
||
|
|
int intersections_below = 0;
|
||
|
|
size_t left_node_idx = node_idx * 2 + 1;
|
||
|
|
size_t right_node_idx = left_node_idx + 1;
|
||
|
|
const auto &node_left = tree.node(left_node_idx);
|
||
|
|
const auto &node_right = tree.node(right_node_idx);
|
||
|
|
assert(node_left.is_valid());
|
||
|
|
assert(node_right.is_valid());
|
||
|
|
|
||
|
|
if (node_left.bbox.min()[other_coordinate] <= ray_origin[other_coordinate] &&
|
||
|
|
node_left.bbox.max()[other_coordinate] >=
|
||
|
|
ray_origin[other_coordinate]) {
|
||
|
|
auto [above, below] = coordinate_aligned_ray_hit_count<LineType, TreeType, VectorType, coordinate>(left_node_idx, tree, lines,
|
||
|
|
ray_origin);
|
||
|
|
if (above < 0 || below < 0) return {-1, -1};
|
||
|
|
intersections_above += above;
|
||
|
|
intersections_below += below;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (node_right.bbox.min()[other_coordinate] <= ray_origin[other_coordinate] &&
|
||
|
|
node_right.bbox.max()[other_coordinate] >= ray_origin[other_coordinate]) {
|
||
|
|
auto [above, below] = coordinate_aligned_ray_hit_count<LineType, TreeType, VectorType, coordinate>(right_node_idx, tree, lines,
|
||
|
|
ray_origin);
|
||
|
|
if (above < 0 || below < 0) return {-1, -1};
|
||
|
|
intersections_above += above;
|
||
|
|
intersections_below += below;
|
||
|
|
}
|
||
|
|
return {intersections_above, intersections_below};
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
template<typename LineType, typename TreeType, typename VectorType>
|
||
|
|
inline std::vector<std::pair<VectorType, size_t>> get_intersections_with_line(size_t node_idx,
|
||
|
|
const TreeType &tree,
|
||
|
|
const std::vector<LineType> &lines,
|
||
|
|
const LineType &line,
|
||
|
|
const typename TreeType::BoundingBox &line_bb)
|
||
|
|
{
|
||
|
|
const auto &node = tree.node(node_idx);
|
||
|
|
assert(node.is_valid());
|
||
|
|
if (node.is_leaf()) {
|
||
|
|
VectorType intersection_pt;
|
||
|
|
if (line_alg::intersection(line, lines[node.idx], &intersection_pt)) {
|
||
|
|
return {std::pair<VectorType, size_t>(intersection_pt, node.idx)};
|
||
|
|
} else {
|
||
|
|
return {};
|
||
|
|
}
|
||
|
|
} else {
|
||
|
|
size_t left_node_idx = node_idx * 2 + 1;
|
||
|
|
size_t right_node_idx = left_node_idx + 1;
|
||
|
|
const auto &node_left = tree.node(left_node_idx);
|
||
|
|
const auto &node_right = tree.node(right_node_idx);
|
||
|
|
assert(node_left.is_valid());
|
||
|
|
assert(node_right.is_valid());
|
||
|
|
|
||
|
|
std::vector<std::pair<VectorType, size_t>> result;
|
||
|
|
|
||
|
|
if (node_left.bbox.intersects(line_bb)) {
|
||
|
|
std::vector<std::pair<VectorType, size_t>> intersections =
|
||
|
|
get_intersections_with_line<LineType, TreeType, VectorType>(left_node_idx, tree, lines, line, line_bb);
|
||
|
|
result.insert(result.end(), intersections.begin(), intersections.end());
|
||
|
|
}
|
||
|
|
|
||
|
|
if (node_right.bbox.intersects(line_bb)) {
|
||
|
|
std::vector<std::pair<VectorType, size_t>> intersections =
|
||
|
|
get_intersections_with_line<LineType, TreeType, VectorType>(right_node_idx, tree, lines, line, line_bb);
|
||
|
|
result.insert(result.end(), intersections.begin(), intersections.end());
|
||
|
|
}
|
||
|
|
|
||
|
|
return result;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
} // namespace detail
|
||
|
|
|
||
|
|
// Build a balanced AABB Tree over a vector of lines, balancing the tree
|
||
|
|
// on centroids of the lines.
|
||
|
|
// Epsilon is applied to the bounding boxes of the AABB Tree to cope with numeric inaccuracies
|
||
|
|
// during tree traversal.
|
||
|
|
template<typename LineType>
|
||
|
|
inline AABBTreeIndirect::Tree<2, typename LineType::Scalar> build_aabb_tree_over_indexed_lines(const std::vector<LineType> &lines)
|
||
|
|
{
|
||
|
|
using TreeType = AABBTreeIndirect::Tree<2, typename LineType::Scalar>;
|
||
|
|
// using CoordType = typename TreeType::CoordType;
|
||
|
|
using VectorType = typename TreeType::VectorType;
|
||
|
|
using BoundingBox = typename TreeType::BoundingBox;
|
||
|
|
|
||
|
|
struct InputType
|
||
|
|
{
|
||
|
|
size_t idx() const { return m_idx; }
|
||
|
|
const BoundingBox &bbox() const { return m_bbox; }
|
||
|
|
const VectorType ¢roid() const { return m_centroid; }
|
||
|
|
|
||
|
|
size_t m_idx;
|
||
|
|
BoundingBox m_bbox;
|
||
|
|
VectorType m_centroid;
|
||
|
|
};
|
||
|
|
|
||
|
|
std::vector<InputType> input;
|
||
|
|
input.reserve(lines.size());
|
||
|
|
for (size_t i = 0; i < lines.size(); ++i) {
|
||
|
|
const LineType &line = lines[i];
|
||
|
|
InputType n;
|
||
|
|
n.m_idx = i;
|
||
|
|
n.m_centroid = (line.a + line.b) * 0.5;
|
||
|
|
n.m_bbox = BoundingBox(line.a, line.a);
|
||
|
|
n.m_bbox.extend(line.b);
|
||
|
|
input.emplace_back(n);
|
||
|
|
}
|
||
|
|
|
||
|
|
TreeType out;
|
||
|
|
out.build(std::move(input));
|
||
|
|
return out;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Finding a closest line, its closest point and squared distance to the closest point
|
||
|
|
// Returns squared distance to the closest point or -1 if the input is empty.
|
||
|
|
// or no closer point than max_sq_dist
|
||
|
|
template<typename LineType, typename TreeType, typename VectorType>
|
||
|
|
inline typename VectorType::Scalar squared_distance_to_indexed_lines(
|
||
|
|
const std::vector<LineType> &lines,
|
||
|
|
const TreeType &tree,
|
||
|
|
const VectorType &point,
|
||
|
|
size_t &hit_idx_out,
|
||
|
|
Eigen::PlainObjectBase<VectorType> &hit_point_out,
|
||
|
|
typename VectorType::Scalar max_sqr_dist = std::numeric_limits<typename VectorType::Scalar>::infinity())
|
||
|
|
{
|
||
|
|
using Scalar = typename VectorType::Scalar;
|
||
|
|
if (tree.empty()) return Scalar(-1);
|
||
|
|
auto distancer = detail::IndexedLinesDistancer<LineType, TreeType, VectorType>{lines, tree, point};
|
||
|
|
return AABBTreeIndirect::detail::squared_distance_to_indexed_primitives_recursive(distancer, size_t(0), Scalar(0), max_sqr_dist,
|
||
|
|
hit_idx_out, hit_point_out);
|
||
|
|
}
|
||
|
|
|
||
|
|
// Returns all lines within the given radius limit
|
||
|
|
template<typename LineType, typename TreeType, typename VectorType>
|
||
|
|
inline std::vector<size_t> all_lines_in_radius(const std::vector<LineType> &lines,
|
||
|
|
const TreeType &tree,
|
||
|
|
const VectorType &point,
|
||
|
|
typename VectorType::Scalar max_distance_squared)
|
||
|
|
{
|
||
|
|
auto distancer = detail::IndexedLinesDistancer<LineType, TreeType, VectorType>{lines, tree, point};
|
||
|
|
|
||
|
|
if (tree.empty()) { return {}; }
|
||
|
|
|
||
|
|
std::vector<size_t> found_lines{};
|
||
|
|
AABBTreeIndirect::detail::indexed_primitives_within_distance_squared_recurisve(distancer, size_t(0), max_distance_squared, found_lines);
|
||
|
|
return found_lines;
|
||
|
|
}
|
||
|
|
|
||
|
|
// return 1 if true, -1 if false, 0 for point on contour (or if cannot be determined)
|
||
|
|
template<typename LineType, typename TreeType, typename VectorType>
|
||
|
|
inline int point_outside_closed_contours(const std::vector<LineType> &lines, const TreeType &tree, const VectorType &point)
|
||
|
|
{
|
||
|
|
if (tree.empty()) { return 1; }
|
||
|
|
|
||
|
|
auto [hits_above, hits_below] = detail::coordinate_aligned_ray_hit_count<LineType, TreeType, VectorType, 0>(0, tree, lines, point);
|
||
|
|
if (hits_above < 0 || hits_below < 0) {
|
||
|
|
return 0;
|
||
|
|
} else if (hits_above % 2 == 1 && hits_below % 2 == 1) {
|
||
|
|
return -1;
|
||
|
|
} else if (hits_above % 2 == 0 && hits_below % 2 == 0) {
|
||
|
|
return 1;
|
||
|
|
} else { // this should not happen with closed contours. lets check it in Y direction
|
||
|
|
auto [hits_above, hits_below] = detail::coordinate_aligned_ray_hit_count<LineType, TreeType, VectorType, 1>(0, tree, lines, point);
|
||
|
|
if (hits_above < 0 || hits_below < 0) {
|
||
|
|
return 0;
|
||
|
|
} else if (hits_above % 2 == 1 && hits_below % 2 == 1) {
|
||
|
|
return -1;
|
||
|
|
} else if (hits_above % 2 == 0 && hits_below % 2 == 0) {
|
||
|
|
return 1;
|
||
|
|
} else { // both results were unclear
|
||
|
|
return 0;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
template<bool sorted, typename VectorType, typename LineType, typename TreeType>
|
||
|
|
inline std::vector<std::pair<VectorType, size_t>> get_intersections_with_line(const std::vector<LineType> &lines,
|
||
|
|
const TreeType &tree,
|
||
|
|
const LineType &line)
|
||
|
|
{
|
||
|
|
if (tree.empty()) {
|
||
|
|
return {};
|
||
|
|
}
|
||
|
|
auto line_bb = typename TreeType::BoundingBox(line.a, line.a);
|
||
|
|
line_bb.extend(line.b);
|
||
|
|
|
||
|
|
auto intersections = detail::get_intersections_with_line<LineType, TreeType, VectorType>(0, tree, lines, line, line_bb);
|
||
|
|
if (sorted) {
|
||
|
|
using Floating =
|
||
|
|
typename std::conditional<std::is_floating_point<typename LineType::Scalar>::value, typename LineType::Scalar, double>::type;
|
||
|
|
|
||
|
|
std::vector<std::pair<Floating, std::pair<VectorType, size_t>>> points_with_sq_distance{};
|
||
|
|
for (const auto &p : intersections) {
|
||
|
|
points_with_sq_distance.emplace_back((p.first - line.a).template cast<Floating>().squaredNorm(), p);
|
||
|
|
}
|
||
|
|
std::sort(points_with_sq_distance.begin(), points_with_sq_distance.end(),
|
||
|
|
[](const std::pair<Floating, std::pair<VectorType, size_t>> &left,
|
||
|
|
std::pair<Floating, std::pair<VectorType, size_t>> &right) { return left.first < right.first; });
|
||
|
|
for (size_t i = 0; i < points_with_sq_distance.size(); i++) {
|
||
|
|
intersections[i] = points_with_sq_distance[i].second;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
return intersections;
|
||
|
|
}
|
||
|
|
|
||
|
|
template<typename LineType> class LinesDistancer
|
||
|
|
{
|
||
|
|
public:
|
||
|
|
using Scalar = typename LineType::Scalar;
|
||
|
|
using Floating = typename std::conditional<std::is_floating_point<Scalar>::value, Scalar, double>::type;
|
||
|
|
private:
|
||
|
|
std::vector<LineType> lines;
|
||
|
|
AABBTreeIndirect::Tree<2, Scalar> tree;
|
||
|
|
|
||
|
|
public:
|
||
|
|
explicit LinesDistancer(const std::vector<LineType> &lines) : lines(lines)
|
||
|
|
{
|
||
|
|
tree = AABBTreeLines::build_aabb_tree_over_indexed_lines(this->lines);
|
||
|
|
}
|
||
|
|
|
||
|
|
explicit LinesDistancer(std::vector<LineType> &&lines) : lines(lines)
|
||
|
|
{
|
||
|
|
tree = AABBTreeLines::build_aabb_tree_over_indexed_lines(this->lines);
|
||
|
|
}
|
||
|
|
|
||
|
|
LinesDistancer() = default;
|
||
|
|
|
||
|
|
// 1 true, -1 false, 0 cannot determine
|
||
|
|
int outside(const Vec<2, Scalar> &point) const { return point_outside_closed_contours(lines, tree, point); }
|
||
|
|
|
||
|
|
// negative sign means inside
|
||
|
|
template<bool SIGNED_DISTANCE>
|
||
|
|
std::tuple<Floating, size_t, Vec<2, Floating>> distance_from_lines_extra(const Vec<2, Scalar> &point) const
|
||
|
|
{
|
||
|
|
size_t nearest_line_index_out = size_t(-1);
|
||
|
|
Vec<2, Floating> nearest_point_out = Vec<2, Floating>::Zero();
|
||
|
|
Vec<2, Floating> p = point.template cast<Floating>();
|
||
|
|
auto distance = AABBTreeLines::squared_distance_to_indexed_lines(lines, tree, p, nearest_line_index_out, nearest_point_out);
|
||
|
|
|
||
|
|
if (distance < 0) {
|
||
|
|
return {std::numeric_limits<Floating>::infinity(), nearest_line_index_out, nearest_point_out};
|
||
|
|
}
|
||
|
|
distance = sqrt(distance);
|
||
|
|
|
||
|
|
if (SIGNED_DISTANCE) {
|
||
|
|
distance *= outside(point);
|
||
|
|
}
|
||
|
|
|
||
|
|
return {distance, nearest_line_index_out, nearest_point_out};
|
||
|
|
}
|
||
|
|
|
||
|
|
template<bool SIGNED_DISTANCE> Floating distance_from_lines(const Vec<2, typename LineType::Scalar> &point) const
|
||
|
|
{
|
||
|
|
auto [dist, idx, np] = distance_from_lines_extra<SIGNED_DISTANCE>(point);
|
||
|
|
return dist;
|
||
|
|
}
|
||
|
|
|
||
|
|
std::vector<size_t> all_lines_in_radius(const Vec<2, typename LineType::Scalar> &point, Floating radius)
|
||
|
|
{
|
||
|
|
return all_lines_in_radius(this->lines, this->tree, point, radius * radius);
|
||
|
|
}
|
||
|
|
|
||
|
|
template<bool sorted> std::vector<std::pair<Vec<2, Scalar>, size_t>> intersections_with_line(const LineType &line) const
|
||
|
|
{
|
||
|
|
return get_intersections_with_line<sorted, Vec<2, Scalar>>(lines, tree, line);
|
||
|
|
}
|
||
|
|
|
||
|
|
const LineType &get_line(size_t line_idx) const { return lines[line_idx]; }
|
||
|
|
|
||
|
|
const std::vector<LineType> &get_lines() const { return lines; }
|
||
|
|
};
|
||
|
|
|
||
|
|
}} // namespace Slic3r::AABBTreeLines
|
||
|
|
|
||
|
|
#endif /* SRC_LIBSLIC3R_AABBTREELINES_HPP_ */
|