mirror of
https://github.com/SoftFever/OrcaSlicer.git
synced 2025-10-31 04:31:15 -06:00
d73142c2f9
As title. Thanks @Prusa Signed-off-by: salt.wei <salt.wei@bambulab.com> Change-Id: I2fa177e27ac53211952ea9b6c62e98182b8f05ce
903 lines
35 KiB
C++
903 lines
35 KiB
C++
// AABB tree built upon external data set, referencing the external data by integer indices.
|
||
// The AABB tree balancing and traversal (ray casting, closest triangle of an indexed triangle mesh)
|
||
// were adapted from libigl AABB.{cpp,hpp} Copyright (C) 2015 Alec Jacobson <alecjacobson@gmail.com>
|
||
// while the implicit balanced tree representation and memory optimizations are Vojtech's.
|
||
|
||
#ifndef slic3r_AABBTreeIndirect_hpp_
|
||
#define slic3r_AABBTreeIndirect_hpp_
|
||
|
||
#include <algorithm>
|
||
#include <limits>
|
||
#include <type_traits>
|
||
#include <vector>
|
||
|
||
#include <Eigen/Geometry>
|
||
|
||
#include "Utils.hpp" // for next_highest_power_of_2()
|
||
|
||
// Definition of the ray intersection hit structure.
|
||
#include <igl/Hit.h>
|
||
|
||
namespace Slic3r {
|
||
namespace AABBTreeIndirect {
|
||
|
||
// Static balanced AABB tree for raycasting and closest triangle search.
|
||
// The balanced tree is built over a single large std::vector of nodes, where the children of nodes
|
||
// are addressed implicitely using a power of two indexing rule.
|
||
// Memory for a full balanced tree is allocated, but not all nodes at the last level are used.
|
||
// This may seem like a waste of memory, but one saves memory for the node links and there is zero
|
||
// overhead of a memory allocator management (usually the memory allocator adds at least one pointer
|
||
// before the memory returned). However, allocating memory in a single vector is very fast even
|
||
// in multi-threaded environment and it is cache friendly.
|
||
//
|
||
// A balanced tree is built upon a vector of bounding boxes and their centroids, storing the reference
|
||
// to the source entity (a 3D triangle, a 2D segment etc, a 3D or 2D point etc).
|
||
// The source bounding boxes may have an epsilon applied to fight numeric rounding errors when
|
||
// traversing the AABB tree.
|
||
template<int ANumDimensions, typename ACoordType>
|
||
class Tree
|
||
{
|
||
public:
|
||
static constexpr int NumDimensions = ANumDimensions;
|
||
using CoordType = ACoordType;
|
||
using VectorType = Eigen::Matrix<CoordType, NumDimensions, 1, Eigen::DontAlign>;
|
||
using BoundingBox = Eigen::AlignedBox<CoordType, NumDimensions>;
|
||
// Following could be static constexpr size_t, but that would not link in C++11
|
||
enum : size_t {
|
||
// Node is not used.
|
||
npos = size_t(-1),
|
||
// Inner node (not leaf).
|
||
inner = size_t(-2)
|
||
};
|
||
|
||
// Single node of the implicit balanced AABB tree. There are no links to the children nodes,
|
||
// as these links are calculated implicitely using a power of two rule.
|
||
struct Node {
|
||
// Index of the external source entity, for which this AABB tree was built, npos for internal nodes.
|
||
size_t idx = npos;
|
||
// Bounding box around this entity, possibly with epsilons applied to fight numeric rounding errors
|
||
// when traversing the AABB tree.
|
||
BoundingBox bbox;
|
||
|
||
bool is_valid() const { return this->idx != npos; }
|
||
bool is_inner() const { return this->idx == inner; }
|
||
bool is_leaf() const { return ! this->is_inner(); }
|
||
|
||
template<typename SourceNode>
|
||
void set(const SourceNode &rhs) {
|
||
this->idx = rhs.idx();
|
||
this->bbox = rhs.bbox();
|
||
}
|
||
};
|
||
|
||
void clear() { m_nodes.clear(); }
|
||
|
||
// SourceNode shall implement
|
||
// size_t SourceNode::idx() const
|
||
// - Index to the outside entity (triangle, edge, point etc).
|
||
// const VectorType& SourceNode::centroid() const
|
||
// - Centroid of this node. The centroid is used for balancing the tree.
|
||
// const BoundingBox& SourceNode::bbox() const
|
||
// - Bounding box of this node, likely expanded with epsilon to account for numeric rounding during tree traversal.
|
||
// Union of bounding boxes at a single level of the AABB tree is used for deciding the longest axis aligned dimension
|
||
// to split around.
|
||
template<typename SourceNode>
|
||
void build(std::vector<SourceNode> &&input)
|
||
{
|
||
if (input.empty())
|
||
clear();
|
||
else {
|
||
// Allocate enough memory for a full binary tree.
|
||
m_nodes.assign(next_highest_power_of_2(input.size()) * 2 - 1, Node());
|
||
build_recursive(input, 0, 0, input.size() - 1);
|
||
}
|
||
input.clear();
|
||
}
|
||
|
||
const std::vector<Node>& nodes() const { return m_nodes; }
|
||
const Node& node(size_t idx) const { return m_nodes[idx]; }
|
||
bool empty() const { return m_nodes.empty(); }
|
||
|
||
// Addressing the child nodes using the power of two rule.
|
||
static size_t left_child_idx(size_t idx) { return idx * 2 + 1; }
|
||
static size_t right_child_idx(size_t idx) { return left_child_idx(idx) + 1; }
|
||
const Node& left_child(size_t idx) const { return m_nodes[left_child_idx(idx)]; }
|
||
const Node& right_child(size_t idx) const { return m_nodes[right_child_idx(idx)]; }
|
||
|
||
template<typename SourceNode>
|
||
void build(const std::vector<SourceNode> &input)
|
||
{
|
||
std::vector<SourceNode> copy(input);
|
||
this->build(std::move(copy));
|
||
}
|
||
|
||
private:
|
||
// Build a balanced tree by splitting the input sequence by an axis aligned plane at a dimension.
|
||
template<typename SourceNode>
|
||
void build_recursive(std::vector<SourceNode> &input, size_t node, const size_t left, const size_t right)
|
||
{
|
||
assert(node < m_nodes.size());
|
||
assert(left <= right);
|
||
|
||
if (left == right) {
|
||
// Insert a node into the balanced tree.
|
||
m_nodes[node].set(input[left]);
|
||
return;
|
||
}
|
||
|
||
// Calculate bounding box of the input.
|
||
BoundingBox bbox(input[left].bbox());
|
||
for (size_t i = left + 1; i <= right; ++ i)
|
||
bbox.extend(input[i].bbox());
|
||
int dimension = -1;
|
||
bbox.diagonal().maxCoeff(&dimension);
|
||
|
||
// Partition the input to left / right pieces of the same length to produce a balanced tree.
|
||
size_t center = (left + right) / 2;
|
||
partition_input(input, size_t(dimension), left, right, center);
|
||
// Insert an inner node into the tree. Inner node does not reference any input entity (triangle, line segment etc).
|
||
m_nodes[node].idx = inner;
|
||
m_nodes[node].bbox = bbox;
|
||
build_recursive(input, node * 2 + 1, left, center);
|
||
build_recursive(input, node * 2 + 2, center + 1, right);
|
||
}
|
||
|
||
// Partition the input m_nodes <left, right> at "k" and "dimension" using the QuickSelect method:
|
||
// https://en.wikipedia.org/wiki/Quickselect
|
||
// Items left of the k'th item are lower than the k'th item in the "dimension",
|
||
// items right of the k'th item are higher than the k'th item in the "dimension",
|
||
template<typename SourceNode>
|
||
void partition_input(std::vector<SourceNode> &input, const size_t dimension, size_t left, size_t right, const size_t k) const
|
||
{
|
||
while (left < right) {
|
||
size_t center = (left + right) / 2;
|
||
CoordType pivot;
|
||
{
|
||
// Bubble sort the input[left], input[center], input[right], so that a median of the three values
|
||
// will end up in input[center].
|
||
CoordType left_value = input[left ].centroid()(dimension);
|
||
CoordType center_value = input[center].centroid()(dimension);
|
||
CoordType right_value = input[right ].centroid()(dimension);
|
||
if (left_value > center_value) {
|
||
std::swap(input[left], input[center]);
|
||
std::swap(left_value, center_value);
|
||
}
|
||
if (left_value > right_value) {
|
||
std::swap(input[left], input[right]);
|
||
right_value = left_value;
|
||
}
|
||
if (center_value > right_value) {
|
||
std::swap(input[center], input[right]);
|
||
center_value = right_value;
|
||
}
|
||
pivot = center_value;
|
||
}
|
||
if (right <= left + 2)
|
||
// The <left, right> interval is already sorted.
|
||
break;
|
||
size_t i = left;
|
||
size_t j = right - 1;
|
||
std::swap(input[center], input[j]);
|
||
// Partition the set based on the pivot.
|
||
for (;;) {
|
||
// Skip left points that are already at correct positions.
|
||
// Search will certainly stop at position (right - 1), which stores the pivot.
|
||
while (input[++ i].centroid()(dimension) < pivot) ;
|
||
// Skip right points that are already at correct positions.
|
||
while (input[-- j].centroid()(dimension) > pivot && i < j) ;
|
||
if (i >= j)
|
||
break;
|
||
std::swap(input[i], input[j]);
|
||
}
|
||
// Restore pivot to the center of the sequence.
|
||
std::swap(input[i], input[right - 1]);
|
||
// Which side the kth element is in?
|
||
if (k < i)
|
||
right = i - 1;
|
||
else if (k == i)
|
||
// Sequence is partitioned, kth element is at its place.
|
||
break;
|
||
else
|
||
left = i + 1;
|
||
}
|
||
}
|
||
|
||
// The balanced tree storage.
|
||
std::vector<Node> m_nodes;
|
||
};
|
||
|
||
using Tree2f = Tree<2, float>;
|
||
using Tree3f = Tree<3, float>;
|
||
using Tree2d = Tree<2, double>;
|
||
using Tree3d = Tree<3, double>;
|
||
|
||
namespace detail {
|
||
template<typename AVertexType, typename AIndexedFaceType, typename ATreeType, typename AVectorType>
|
||
struct RayIntersector {
|
||
using VertexType = AVertexType;
|
||
using IndexedFaceType = AIndexedFaceType;
|
||
using TreeType = ATreeType;
|
||
using VectorType = AVectorType;
|
||
|
||
const std::vector<VertexType> &vertices;
|
||
const std::vector<IndexedFaceType> &faces;
|
||
const TreeType &tree;
|
||
|
||
const VectorType origin;
|
||
const VectorType dir;
|
||
const VectorType invdir;
|
||
|
||
// epsilon for ray-triangle intersection, see intersect_triangle1()
|
||
const double eps;
|
||
};
|
||
|
||
template<typename VertexType, typename IndexedFaceType, typename TreeType, typename VectorType>
|
||
struct RayIntersectorHits : RayIntersector<VertexType, IndexedFaceType, TreeType, VectorType> {
|
||
std::vector<igl::Hit> hits;
|
||
};
|
||
|
||
//FIXME implement SSE for float AABB trees with float ray queries.
|
||
// SSE/SSE2 is supported by any Intel/AMD x64 processor.
|
||
// SSE support requires 16 byte alignment of the AABB nodes, representing the bounding boxes with 4+4 floats,
|
||
// storing the node index as the 4th element of the bounding box min value etc.
|
||
// https://www.flipcode.com/archives/SSE_RayBox_Intersection_Test.shtml
|
||
template <typename Derivedsource, typename Deriveddir, typename Scalar>
|
||
inline bool ray_box_intersect_invdir(
|
||
const Eigen::MatrixBase<Derivedsource> &origin,
|
||
const Eigen::MatrixBase<Deriveddir> &inv_dir,
|
||
Eigen::AlignedBox<Scalar,3> box,
|
||
const Scalar &t0,
|
||
const Scalar &t1) {
|
||
// http://people.csail.mit.edu/amy/papers/box-jgt.pdf
|
||
// "An Efficient and Robust Ray–Box Intersection Algorithm"
|
||
if (inv_dir.x() < 0)
|
||
std::swap(box.min().x(), box.max().x());
|
||
if (inv_dir.y() < 0)
|
||
std::swap(box.min().y(), box.max().y());
|
||
Scalar tmin = (box.min().x() - origin.x()) * inv_dir.x();
|
||
Scalar tymax = (box.max().y() - origin.y()) * inv_dir.y();
|
||
if (tmin > tymax)
|
||
return false;
|
||
Scalar tmax = (box.max().x() - origin.x()) * inv_dir.x();
|
||
Scalar tymin = (box.min().y() - origin.y()) * inv_dir.y();
|
||
if (tymin > tmax)
|
||
return false;
|
||
if (tymin > tmin)
|
||
tmin = tymin;
|
||
if (tymax < tmax)
|
||
tmax = tymax;
|
||
if (inv_dir.z() < 0)
|
||
std::swap(box.min().z(), box.max().z());
|
||
Scalar tzmin = (box.min().z() - origin.z()) * inv_dir.z();
|
||
if (tzmin > tmax)
|
||
return false;
|
||
Scalar tzmax = (box.max().z() - origin.z()) * inv_dir.z();
|
||
if (tmin > tzmax)
|
||
return false;
|
||
if (tzmin > tmin)
|
||
tmin = tzmin;
|
||
if (tzmax < tmax)
|
||
tmax = tzmax;
|
||
return tmin < t1 && tmax > t0;
|
||
}
|
||
|
||
// The following intersect_triangle() is derived from raytri.c routine intersect_triangle1()
|
||
// Ray-Triangle Intersection Test Routines
|
||
// Different optimizations of my and Ben Trumbore's
|
||
// code from journals of graphics tools (JGT)
|
||
// http://www.acm.org/jgt/
|
||
// by Tomas Moller, May 2000
|
||
template<typename V, typename W>
|
||
std::enable_if_t<std::is_same<typename V::Scalar, double>::value&& std::is_same<typename W::Scalar, double>::value, bool>
|
||
intersect_triangle(const V &orig, const V &dir, const W &vert0, const W &vert1, const W &vert2, double &t, double &u, double &v, double eps)
|
||
{
|
||
// find vectors for two edges sharing vert0
|
||
const V edge1 = vert1 - vert0;
|
||
const V edge2 = vert2 - vert0;
|
||
// begin calculating determinant - also used to calculate U parameter
|
||
const V pvec = dir.cross(edge2);
|
||
// if determinant is near zero, ray lies in plane of triangle
|
||
const double det = edge1.dot(pvec);
|
||
V qvec;
|
||
|
||
if (det > eps) {
|
||
// calculate distance from vert0 to ray origin
|
||
V tvec = orig - vert0;
|
||
// calculate U parameter and test bounds
|
||
u = tvec.dot(pvec);
|
||
if (u < 0.0 || u > det)
|
||
return false;
|
||
// prepare to test V parameter
|
||
qvec = tvec.cross(edge1);
|
||
// calculate V parameter and test bounds
|
||
v = dir.dot(qvec);
|
||
if (v < 0.0 || u + v > det)
|
||
return false;
|
||
} else if (det < -eps) {
|
||
// calculate distance from vert0 to ray origin
|
||
V tvec = orig - vert0;
|
||
// calculate U parameter and test bounds
|
||
u = tvec.dot(pvec);
|
||
if (u > 0.0 || u < det)
|
||
return false;
|
||
// prepare to test V parameter
|
||
qvec = tvec.cross(edge1);
|
||
// calculate V parameter and test bounds
|
||
v = dir.dot(qvec);
|
||
if (v > 0.0 || u + v < det)
|
||
return false;
|
||
} else
|
||
// ray is parallel to the plane of the triangle
|
||
return false;
|
||
|
||
double inv_det = 1.0 / det;
|
||
// calculate t, ray intersects triangle
|
||
t = edge2.dot(qvec) * inv_det;
|
||
u *= inv_det;
|
||
v *= inv_det;
|
||
return true;
|
||
}
|
||
|
||
template<typename V, typename W>
|
||
std::enable_if_t<std::is_same<typename V::Scalar, double>::value && !std::is_same<typename W::Scalar, double>::value, bool>
|
||
intersect_triangle(const V &origin, const V &dir, const W &v0, const W &v1, const W &v2, double &t, double &u, double &v, double eps) {
|
||
return intersect_triangle(origin, dir, v0.template cast<double>(), v1.template cast<double>(), v2.template cast<double>(), t, u, v, eps);
|
||
}
|
||
|
||
template<typename V, typename W>
|
||
std::enable_if_t<! std::is_same<typename V::Scalar, double>::value && std::is_same<typename W::Scalar, double>::value, bool>
|
||
intersect_triangle(const V &origin, const V &dir, const W &v0, const W &v1, const W &v2, double &t, double &u, double &v, double eps) {
|
||
return intersect_triangle(origin.template cast<double>(), dir.template cast<double>(), v0, v1, v2, t, u, v, eps);
|
||
}
|
||
|
||
template<typename V, typename W>
|
||
std::enable_if_t<! std::is_same<typename V::Scalar, double>::value && ! std::is_same<typename W::Scalar, double>::value, bool>
|
||
intersect_triangle(const V &origin, const V &dir, const W &v0, const W &v1, const W &v2, double &t, double &u, double &v, double eps) {
|
||
return intersect_triangle(origin.template cast<double>(), dir.template cast<double>(), v0.template cast<double>(), v1.template cast<double>(), v2.template cast<double>(), t, u, v, eps);
|
||
}
|
||
|
||
template<typename Tree>
|
||
double intersect_triangle_epsilon(const Tree &tree) {
|
||
double eps = 0.000001;
|
||
if (! tree.empty()) {
|
||
const typename Tree::BoundingBox &bbox = tree.nodes().front().bbox;
|
||
double l = (bbox.max() - bbox.min()).cwiseMax();
|
||
if (l > 0)
|
||
eps /= (l * l);
|
||
}
|
||
return eps;
|
||
}
|
||
|
||
template<typename RayIntersectorType, typename Scalar>
|
||
static inline bool intersect_ray_recursive_first_hit(
|
||
RayIntersectorType &ray_intersector,
|
||
size_t node_idx,
|
||
Scalar min_t,
|
||
igl::Hit &hit)
|
||
{
|
||
const auto &node = ray_intersector.tree.node(node_idx);
|
||
assert(node.is_valid());
|
||
|
||
if (! ray_box_intersect_invdir(ray_intersector.origin, ray_intersector.invdir, node.bbox.template cast<Scalar>(), Scalar(0), min_t))
|
||
return false;
|
||
|
||
if (node.is_leaf()) {
|
||
// shoot ray, record hit
|
||
auto face = ray_intersector.faces[node.idx];
|
||
double t, u, v;
|
||
if (intersect_triangle(
|
||
ray_intersector.origin, ray_intersector.dir,
|
||
ray_intersector.vertices[face(0)], ray_intersector.vertices[face(1)], ray_intersector.vertices[face(2)],
|
||
t, u, v, ray_intersector.eps)
|
||
&& t > 0.) {
|
||
hit = igl::Hit { int(node.idx), -1, float(u), float(v), float(t) };
|
||
return true;
|
||
} else
|
||
return false;
|
||
} else {
|
||
// Left / right child node index.
|
||
size_t left = node_idx * 2 + 1;
|
||
size_t right = left + 1;
|
||
igl::Hit left_hit;
|
||
igl::Hit right_hit;
|
||
bool left_ret = intersect_ray_recursive_first_hit(ray_intersector, left, min_t, left_hit);
|
||
if (left_ret && left_hit.t < min_t) {
|
||
min_t = left_hit.t;
|
||
hit = left_hit;
|
||
} else
|
||
left_ret = false;
|
||
bool right_ret = intersect_ray_recursive_first_hit(ray_intersector, right, min_t, right_hit);
|
||
if (right_ret && right_hit.t < min_t)
|
||
hit = right_hit;
|
||
else
|
||
right_ret = false;
|
||
return left_ret || right_ret;
|
||
}
|
||
}
|
||
|
||
template<typename RayIntersectorType>
|
||
static inline void intersect_ray_recursive_all_hits(RayIntersectorType &ray_intersector, size_t node_idx)
|
||
{
|
||
using Scalar = typename RayIntersectorType::VectorType::Scalar;
|
||
|
||
const auto &node = ray_intersector.tree.node(node_idx);
|
||
assert(node.is_valid());
|
||
|
||
if (! ray_box_intersect_invdir(ray_intersector.origin, ray_intersector.invdir, node.bbox.template cast<Scalar>(),
|
||
Scalar(0), std::numeric_limits<Scalar>::infinity()))
|
||
return;
|
||
|
||
if (node.is_leaf()) {
|
||
auto face = ray_intersector.faces[node.idx];
|
||
double t, u, v;
|
||
if (intersect_triangle(
|
||
ray_intersector.origin, ray_intersector.dir,
|
||
ray_intersector.vertices[face(0)], ray_intersector.vertices[face(1)], ray_intersector.vertices[face(2)],
|
||
t, u, v, ray_intersector.eps)
|
||
&& t > 0.) {
|
||
ray_intersector.hits.emplace_back(igl::Hit{ int(node.idx), -1, float(u), float(v), float(t) });
|
||
}
|
||
} else {
|
||
// Left / right child node index.
|
||
size_t left = node_idx * 2 + 1;
|
||
size_t right = left + 1;
|
||
intersect_ray_recursive_all_hits(ray_intersector, left);
|
||
intersect_ray_recursive_all_hits(ray_intersector, right);
|
||
}
|
||
}
|
||
|
||
// Real-time collision detection, Ericson, Chapter 5
|
||
template<typename Vector>
|
||
static inline Vector closest_point_to_triangle(const Vector &p, const Vector &a, const Vector &b, const Vector &c)
|
||
{
|
||
using Scalar = typename Vector::Scalar;
|
||
// Check if P in vertex region outside A
|
||
Vector ab = b - a;
|
||
Vector ac = c - a;
|
||
Vector ap = p - a;
|
||
Scalar d1 = ab.dot(ap);
|
||
Scalar d2 = ac.dot(ap);
|
||
if (d1 <= 0 && d2 <= 0)
|
||
return a;
|
||
// Check if P in vertex region outside B
|
||
Vector bp = p - b;
|
||
Scalar d3 = ab.dot(bp);
|
||
Scalar d4 = ac.dot(bp);
|
||
if (d3 >= 0 && d4 <= d3)
|
||
return b;
|
||
// Check if P in edge region of AB, if so return projection of P onto AB
|
||
Scalar vc = d1*d4 - d3*d2;
|
||
if (a != b && vc <= 0 && d1 >= 0 && d3 <= 0) {
|
||
Scalar v = d1 / (d1 - d3);
|
||
return a + v * ab;
|
||
}
|
||
// Check if P in vertex region outside C
|
||
Vector cp = p - c;
|
||
Scalar d5 = ab.dot(cp);
|
||
Scalar d6 = ac.dot(cp);
|
||
if (d6 >= 0 && d5 <= d6)
|
||
return c;
|
||
// Check if P in edge region of AC, if so return projection of P onto AC
|
||
Scalar vb = d5*d2 - d1*d6;
|
||
if (vb <= 0 && d2 >= 0 && d6 <= 0) {
|
||
Scalar w = d2 / (d2 - d6);
|
||
return a + w * ac;
|
||
}
|
||
// Check if P in edge region of BC, if so return projection of P onto BC
|
||
Scalar va = d3*d6 - d5*d4;
|
||
if (va <= 0 && (d4 - d3) >= 0 && (d5 - d6) >= 0) {
|
||
Scalar w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
|
||
return b + w * (c - b);
|
||
}
|
||
// P inside face region. Compute Q through its barycentric coordinates (u,v,w)
|
||
Scalar denom = Scalar(1.0) / (va + vb + vc);
|
||
Scalar v = vb * denom;
|
||
Scalar w = vc * denom;
|
||
return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = 1.0-v-w
|
||
};
|
||
|
||
|
||
// Nothing to do with COVID-19 social distancing.
|
||
template<typename AVertexType, typename AIndexedFaceType, typename ATreeType, typename AVectorType>
|
||
struct IndexedTriangleSetDistancer {
|
||
using VertexType = AVertexType;
|
||
using IndexedFaceType = AIndexedFaceType;
|
||
using TreeType = ATreeType;
|
||
using VectorType = AVectorType;
|
||
using ScalarType = typename VectorType::Scalar;
|
||
|
||
const std::vector<VertexType> &vertices;
|
||
const std::vector<IndexedFaceType> &faces;
|
||
const TreeType &tree;
|
||
|
||
const VectorType origin;
|
||
|
||
inline VectorType closest_point_to_origin(size_t primitive_index,
|
||
ScalarType& squared_distance){
|
||
const auto &triangle = this->faces[primitive_index];
|
||
VectorType closest_point = closest_point_to_triangle<VectorType>(origin,
|
||
this->vertices[triangle(0)].template cast<ScalarType>(),
|
||
this->vertices[triangle(1)].template cast<ScalarType>(),
|
||
this->vertices[triangle(2)].template cast<ScalarType>());
|
||
squared_distance = (origin - closest_point).squaredNorm();
|
||
return closest_point;
|
||
}
|
||
};
|
||
|
||
template<typename IndexedPrimitivesDistancerType, typename Scalar>
|
||
static inline Scalar squared_distance_to_indexed_primitives_recursive(
|
||
IndexedPrimitivesDistancerType &distancer,
|
||
size_t node_idx,
|
||
Scalar low_sqr_d,
|
||
Scalar up_sqr_d,
|
||
size_t &i,
|
||
Eigen::PlainObjectBase<typename IndexedPrimitivesDistancerType::VectorType> &c)
|
||
{
|
||
using Vector = typename IndexedPrimitivesDistancerType::VectorType;
|
||
|
||
if (low_sqr_d > up_sqr_d)
|
||
return low_sqr_d;
|
||
|
||
// Save the best achieved hit.
|
||
auto set_min = [&i, &c, &up_sqr_d](const Scalar sqr_d_candidate, const size_t i_candidate, const Vector &c_candidate) {
|
||
if (sqr_d_candidate < up_sqr_d) {
|
||
i = i_candidate;
|
||
c = c_candidate;
|
||
up_sqr_d = sqr_d_candidate;
|
||
}
|
||
};
|
||
|
||
const auto &node = distancer.tree.node(node_idx);
|
||
assert(node.is_valid());
|
||
if (node.is_leaf())
|
||
{
|
||
Scalar sqr_dist;
|
||
Vector c_candidate = distancer.closest_point_to_origin(node.idx, sqr_dist);
|
||
set_min(sqr_dist, node.idx, c_candidate);
|
||
}
|
||
else
|
||
{
|
||
size_t left_node_idx = node_idx * 2 + 1;
|
||
size_t right_node_idx = left_node_idx + 1;
|
||
const auto &node_left = distancer.tree.node(left_node_idx);
|
||
const auto &node_right = distancer.tree.node(right_node_idx);
|
||
assert(node_left.is_valid());
|
||
assert(node_right.is_valid());
|
||
|
||
bool looked_left = false;
|
||
bool looked_right = false;
|
||
const auto &look_left = [&]()
|
||
{
|
||
size_t i_left;
|
||
Vector c_left = c;
|
||
Scalar sqr_d_left = squared_distance_to_indexed_primitives_recursive(distancer, left_node_idx, low_sqr_d, up_sqr_d, i_left, c_left);
|
||
set_min(sqr_d_left, i_left, c_left);
|
||
looked_left = true;
|
||
};
|
||
const auto &look_right = [&]()
|
||
{
|
||
size_t i_right;
|
||
Vector c_right = c;
|
||
Scalar sqr_d_right = squared_distance_to_indexed_primitives_recursive(distancer, right_node_idx, low_sqr_d, up_sqr_d, i_right, c_right);
|
||
set_min(sqr_d_right, i_right, c_right);
|
||
looked_right = true;
|
||
};
|
||
|
||
// must look left or right if in box
|
||
using BBoxScalar = typename IndexedPrimitivesDistancerType::TreeType::BoundingBox::Scalar;
|
||
if (node_left.bbox.contains(distancer.origin.template cast<BBoxScalar>()))
|
||
look_left();
|
||
if (node_right.bbox.contains(distancer.origin.template cast<BBoxScalar>()))
|
||
look_right();
|
||
// if haven't looked left and could be less than current min, then look
|
||
Scalar left_up_sqr_d = node_left.bbox.squaredExteriorDistance(distancer.origin);
|
||
Scalar right_up_sqr_d = node_right.bbox.squaredExteriorDistance(distancer.origin);
|
||
if (left_up_sqr_d < right_up_sqr_d) {
|
||
if (! looked_left && left_up_sqr_d < up_sqr_d)
|
||
look_left();
|
||
if (! looked_right && right_up_sqr_d < up_sqr_d)
|
||
look_right();
|
||
} else {
|
||
if (! looked_right && right_up_sqr_d < up_sqr_d)
|
||
look_right();
|
||
if (! looked_left && left_up_sqr_d < up_sqr_d)
|
||
look_left();
|
||
}
|
||
}
|
||
return up_sqr_d;
|
||
}
|
||
|
||
} // namespace detail
|
||
|
||
// Build a balanced AABB Tree over an indexed triangles set, balancing the tree
|
||
// on centroids of the triangles.
|
||
// Epsilon is applied to the bounding boxes of the AABB Tree to cope with numeric inaccuracies
|
||
// during tree traversal.
|
||
template<typename VertexType, typename IndexedFaceType>
|
||
inline Tree<3, typename VertexType::Scalar> build_aabb_tree_over_indexed_triangle_set(
|
||
// Indexed triangle set - 3D vertices.
|
||
const std::vector<VertexType> &vertices,
|
||
// Indexed triangle set - triangular faces, references to vertices.
|
||
const std::vector<IndexedFaceType> &faces,
|
||
//FIXME do we want to apply an epsilon?
|
||
const typename VertexType::Scalar eps = 0)
|
||
{
|
||
using TreeType = Tree<3, typename VertexType::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& centroid() const { return m_centroid; }
|
||
|
||
size_t m_idx;
|
||
BoundingBox m_bbox;
|
||
VectorType m_centroid;
|
||
};
|
||
|
||
std::vector<InputType> input;
|
||
input.reserve(faces.size());
|
||
const VectorType veps(eps, eps, eps);
|
||
for (size_t i = 0; i < faces.size(); ++ i) {
|
||
const IndexedFaceType &face = faces[i];
|
||
const VertexType &v1 = vertices[face(0)];
|
||
const VertexType &v2 = vertices[face(1)];
|
||
const VertexType &v3 = vertices[face(2)];
|
||
InputType n;
|
||
n.m_idx = i;
|
||
n.m_centroid = (1./3.) * (v1 + v2 + v3);
|
||
n.m_bbox = BoundingBox(v1, v1);
|
||
n.m_bbox.extend(v2);
|
||
n.m_bbox.extend(v3);
|
||
n.m_bbox.min() -= veps;
|
||
n.m_bbox.max() += veps;
|
||
input.emplace_back(n);
|
||
}
|
||
|
||
TreeType out;
|
||
out.build(std::move(input));
|
||
return out;
|
||
}
|
||
|
||
// Find a first intersection of a ray with indexed triangle set.
|
||
// Intersection test is calculated with the accuracy of VectorType::Scalar
|
||
// even if the triangle mesh and the AABB Tree are built with floats.
|
||
template<typename VertexType, typename IndexedFaceType, typename TreeType, typename VectorType>
|
||
inline bool intersect_ray_first_hit(
|
||
// Indexed triangle set - 3D vertices.
|
||
const std::vector<VertexType> &vertices,
|
||
// Indexed triangle set - triangular faces, references to vertices.
|
||
const std::vector<IndexedFaceType> &faces,
|
||
// AABBTreeIndirect::Tree over vertices & faces, bounding boxes built with the accuracy of vertices.
|
||
const TreeType &tree,
|
||
// Origin of the ray.
|
||
const VectorType &origin,
|
||
// Direction of the ray.
|
||
const VectorType &dir,
|
||
// First intersection of the ray with the indexed triangle set.
|
||
igl::Hit &hit,
|
||
// Epsilon for the ray-triangle intersection, it should be proportional to an average triangle edge length.
|
||
const double eps = 0.000001)
|
||
{
|
||
using Scalar = typename VectorType::Scalar;
|
||
auto ray_intersector = detail::RayIntersector<VertexType, IndexedFaceType, TreeType, VectorType> {
|
||
vertices, faces, tree,
|
||
origin, dir, VectorType(dir.cwiseInverse()),
|
||
eps
|
||
};
|
||
return ! tree.empty() && detail::intersect_ray_recursive_first_hit(
|
||
ray_intersector, size_t(0), std::numeric_limits<Scalar>::infinity(), hit);
|
||
}
|
||
|
||
// Find all intersections of a ray with indexed triangle set.
|
||
// Intersection test is calculated with the accuracy of VectorType::Scalar
|
||
// even if the triangle mesh and the AABB Tree are built with floats.
|
||
// The output hits are sorted by the ray parameter.
|
||
// If the ray intersects a shared edge of two triangles, hits for both triangles are returned.
|
||
template<typename VertexType, typename IndexedFaceType, typename TreeType, typename VectorType>
|
||
inline bool intersect_ray_all_hits(
|
||
// Indexed triangle set - 3D vertices.
|
||
const std::vector<VertexType> &vertices,
|
||
// Indexed triangle set - triangular faces, references to vertices.
|
||
const std::vector<IndexedFaceType> &faces,
|
||
// AABBTreeIndirect::Tree over vertices & faces, bounding boxes built with the accuracy of vertices.
|
||
const TreeType &tree,
|
||
// Origin of the ray.
|
||
const VectorType &origin,
|
||
// Direction of the ray.
|
||
const VectorType &dir,
|
||
// All intersections of the ray with the indexed triangle set, sorted by parameter t.
|
||
std::vector<igl::Hit> &hits,
|
||
// Epsilon for the ray-triangle intersection, it should be proportional to an average triangle edge length.
|
||
const double eps = 0.000001)
|
||
{
|
||
auto ray_intersector = detail::RayIntersectorHits<VertexType, IndexedFaceType, TreeType, VectorType> {
|
||
{ vertices, faces, {tree},
|
||
origin, dir, VectorType(dir.cwiseInverse()),
|
||
eps }
|
||
};
|
||
if (tree.empty()) {
|
||
hits.clear();
|
||
} else {
|
||
// Reusing the output memory if there is some memory already pre-allocated.
|
||
ray_intersector.hits = std::move(hits);
|
||
ray_intersector.hits.clear();
|
||
ray_intersector.hits.reserve(8);
|
||
detail::intersect_ray_recursive_all_hits(ray_intersector, 0);
|
||
hits = std::move(ray_intersector.hits);
|
||
std::sort(hits.begin(), hits.end(), [](const auto &l, const auto &r) { return l.t < r.t; });
|
||
}
|
||
return ! hits.empty();
|
||
}
|
||
|
||
// Finding a closest triangle, its closest point and squared distance to the closest point
|
||
// on a 3D indexed triangle set using a pre-built AABBTreeIndirect::Tree.
|
||
// Closest point to triangle test will be performed with the accuracy of VectorType::Scalar
|
||
// even if the triangle mesh and the AABB Tree are built with floats.
|
||
// Returns squared distance to the closest point or -1 if the input is empty.
|
||
template<typename VertexType, typename IndexedFaceType, typename TreeType, typename VectorType>
|
||
inline typename VectorType::Scalar squared_distance_to_indexed_triangle_set(
|
||
// Indexed triangle set - 3D vertices.
|
||
const std::vector<VertexType> &vertices,
|
||
// Indexed triangle set - triangular faces, references to vertices.
|
||
const std::vector<IndexedFaceType> &faces,
|
||
// AABBTreeIndirect::Tree over vertices & faces, bounding boxes built with the accuracy of vertices.
|
||
const TreeType &tree,
|
||
// Point to which the closest point on the indexed triangle set is searched for.
|
||
const VectorType &point,
|
||
// Index of the closest triangle in faces.
|
||
size_t &hit_idx_out,
|
||
// Position of the closest point on the indexed triangle set.
|
||
Eigen::PlainObjectBase<VectorType> &hit_point_out)
|
||
{
|
||
using Scalar = typename VectorType::Scalar;
|
||
auto distancer = detail::IndexedTriangleSetDistancer<VertexType, IndexedFaceType, TreeType, VectorType>
|
||
{ vertices, faces, tree, point };
|
||
return tree.empty() ? Scalar(-1) :
|
||
detail::squared_distance_to_indexed_primitives_recursive(distancer, size_t(0), Scalar(0), std::numeric_limits<Scalar>::infinity(), hit_idx_out, hit_point_out);
|
||
}
|
||
|
||
// Decides if exists some triangle in defined radius on a 3D indexed triangle set using a pre-built AABBTreeIndirect::Tree.
|
||
// Closest point to triangle test will be performed with the accuracy of VectorType::Scalar
|
||
// even if the triangle mesh and the AABB Tree are built with floats.
|
||
// Returns true if exists some triangle in defined radius, false otherwise.
|
||
template<typename VertexType, typename IndexedFaceType, typename TreeType, typename VectorType>
|
||
inline bool is_any_triangle_in_radius(
|
||
// Indexed triangle set - 3D vertices.
|
||
const std::vector<VertexType> &vertices,
|
||
// Indexed triangle set - triangular faces, references to vertices.
|
||
const std::vector<IndexedFaceType> &faces,
|
||
// AABBTreeIndirect::Tree over vertices & faces, bounding boxes built with the accuracy of vertices.
|
||
const TreeType &tree,
|
||
// Point to which the closest point on the indexed triangle set is searched for.
|
||
const VectorType &point,
|
||
//Square of maximum distance in which triangle is searched for
|
||
typename VectorType::Scalar &max_distance_squared)
|
||
{
|
||
using Scalar = typename VectorType::Scalar;
|
||
auto distancer = detail::IndexedTriangleSetDistancer<VertexType, IndexedFaceType, TreeType, VectorType>
|
||
{ vertices, faces, tree, point };
|
||
|
||
size_t hit_idx;
|
||
VectorType hit_point = VectorType::Ones() * (NaN<typename VectorType::Scalar>);
|
||
|
||
if(tree.empty())
|
||
{
|
||
return false;
|
||
}
|
||
|
||
detail::squared_distance_to_indexed_primitives_recursive(distancer, size_t(0), Scalar(0), max_distance_squared, hit_idx, hit_point);
|
||
|
||
return hit_point.allFinite();
|
||
}
|
||
|
||
|
||
// Traverse the tree and return the index of an entity whose bounding box
|
||
// contains a given point. Returns size_t(-1) when the point is outside.
|
||
template<typename TreeType, typename VectorType>
|
||
void get_candidate_idxs(const TreeType& tree, const VectorType& v, std::vector<size_t>& candidates, size_t node_idx = 0)
|
||
{
|
||
if (tree.empty() || ! tree.node(node_idx).bbox.contains(v))
|
||
return;
|
||
|
||
decltype(tree.node(node_idx)) node = tree.node(node_idx);
|
||
static_assert(std::is_reference<decltype(node)>::value,
|
||
"Nodes shall be addressed by reference.");
|
||
assert(node.is_valid());
|
||
assert(node.bbox.contains(v));
|
||
|
||
if (! node.is_leaf()) {
|
||
if (tree.left_child(node_idx).bbox.contains(v))
|
||
get_candidate_idxs(tree, v, candidates, tree.left_child_idx(node_idx));
|
||
if (tree.right_child(node_idx).bbox.contains(v))
|
||
get_candidate_idxs(tree, v, candidates, tree.right_child_idx(node_idx));
|
||
} else
|
||
candidates.push_back(node.idx);
|
||
|
||
return;
|
||
}
|
||
|
||
// Predicate: need to be specialized for intersections of different geomteries
|
||
template<class G> struct Intersecting {};
|
||
|
||
// Intersection predicate specialization for box-box intersections
|
||
template<class CoordType, int NumD>
|
||
struct Intersecting<Eigen::AlignedBox<CoordType, NumD>> {
|
||
Eigen::AlignedBox<CoordType, NumD> box;
|
||
|
||
Intersecting(const Eigen::AlignedBox<CoordType, NumD> &bb): box{bb} {}
|
||
|
||
bool operator() (const typename Tree<NumD, CoordType>::Node &node) const
|
||
{
|
||
return box.intersects(node.bbox);
|
||
}
|
||
};
|
||
|
||
template<class G> auto intersecting(const G &g) { return Intersecting<G>{g}; }
|
||
|
||
template<class G> struct Containing {};
|
||
|
||
// Intersection predicate specialization for box-box intersections
|
||
template<class CoordType, int NumD>
|
||
struct Containing<Eigen::AlignedBox<CoordType, NumD>> {
|
||
Eigen::AlignedBox<CoordType, NumD> box;
|
||
|
||
Containing(const Eigen::AlignedBox<CoordType, NumD> &bb): box{bb} {}
|
||
|
||
bool operator() (const typename Tree<NumD, CoordType>::Node &node) const
|
||
{
|
||
return box.contains(node.bbox);
|
||
}
|
||
};
|
||
|
||
template<class G> auto containing(const G &g) { return Containing<G>{g}; }
|
||
|
||
namespace detail {
|
||
|
||
template<int Dims, typename T, typename Pred, typename Fn>
|
||
void traverse_recurse(const Tree<Dims, T> &tree,
|
||
size_t idx,
|
||
Pred && pred,
|
||
Fn && callback)
|
||
{
|
||
assert(tree.node(idx).is_valid());
|
||
|
||
if (!pred(tree.node(idx))) return;
|
||
|
||
if (tree.node(idx).is_leaf()) {
|
||
callback(tree.node(idx).idx);
|
||
} else {
|
||
|
||
// call this with left and right node idx:
|
||
auto trv = [&](size_t idx) {
|
||
traverse_recurse(tree, idx, std::forward<Pred>(pred),
|
||
std::forward<Fn>(callback));
|
||
};
|
||
|
||
// Left / right child node index.
|
||
trv(Tree<Dims, T>::left_child_idx(idx));
|
||
trv(Tree<Dims, T>::right_child_idx(idx));
|
||
}
|
||
}
|
||
|
||
} // namespace detail
|
||
|
||
// Tree traversal with a predicate. Example usage:
|
||
// traverse(tree, intersecting(QueryBox), [](size_t face_idx) {
|
||
// /* ... */
|
||
// });
|
||
template<int Dims, typename T, typename Predicate, typename Fn>
|
||
void traverse(const Tree<Dims, T> &tree, Predicate &&pred, Fn &&callback)
|
||
{
|
||
if (tree.empty()) return;
|
||
|
||
detail::traverse_recurse(tree, size_t(0), std::forward<Predicate>(pred),
|
||
std::forward<Fn>(callback));
|
||
}
|
||
|
||
} // namespace AABBTreeIndirect
|
||
} // namespace Slic3r
|
||
|
||
#endif /* slic3r_AABBTreeIndirect_hpp_ */
|