414 lines
16 KiB
C
414 lines
16 KiB
C
|
// This file is part of libigl, a simple c++ geometry processing library.
|
||
|
|
//
|
||
|
|
// Copyright (C) 2015 Alec Jacobson <alecjacobson@gmail.com>
|
||
|
|
//
|
||
|
|
// This Source Code Form is subject to the terms of the Mozilla Public License
|
||
|
|
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
|
||
|
|
// obtain one at http://mozilla.org/MPL/2.0/.
|
||
|
|
#ifndef IGL_AABB_H
|
||
|
|
#define IGL_AABB_H
|
||
|
|
|
||
|
|
#include "Hit.h"
|
||
|
|
#include "igl_inline.h"
|
||
|
|
#include <Eigen/Core>
|
||
|
|
#include <Eigen/Geometry>
|
||
|
|
#include <vector>
|
||
|
|
namespace igl
|
||
|
|
{
|
||
|
|
// Implementation of semi-general purpose axis-aligned bounding box hierarchy.
|
||
|
|
// The mesh (V,Ele) is stored and managed by the caller and each routine here
|
||
|
|
// simply takes it as references (it better not change between calls).
|
||
|
|
//
|
||
|
|
// It's a little annoying that the Dimension is a template parameter and not
|
||
|
|
// picked up at run time from V. This leads to duplicated code for 2d/3d (up to
|
||
|
|
// dim).
|
||
|
|
template <typename DerivedV, int DIM>
|
||
|
|
class AABB
|
||
|
|
{
|
||
|
|
public:
|
||
|
|
typedef typename DerivedV::Scalar Scalar;
|
||
|
|
typedef Eigen::Matrix<Scalar,1,DIM> RowVectorDIMS;
|
||
|
|
typedef Eigen::Matrix<Scalar,DIM,1> VectorDIMS;
|
||
|
|
typedef Eigen::Matrix<Scalar,Eigen::Dynamic,DIM> MatrixXDIMS;
|
||
|
|
// Shared pointers are slower...
|
||
|
|
AABB * m_left;
|
||
|
|
AABB * m_right;
|
||
|
|
Eigen::AlignedBox<Scalar,DIM> m_box;
|
||
|
|
// -1 non-leaf
|
||
|
|
int m_primitive;
|
||
|
|
//Scalar m_low_sqr_d;
|
||
|
|
//int m_depth;
|
||
|
|
AABB():
|
||
|
|
m_left(NULL), m_right(NULL),
|
||
|
|
m_box(), m_primitive(-1)
|
||
|
|
//m_low_sqr_d(std::numeric_limits<double>::infinity()),
|
||
|
|
//m_depth(0)
|
||
|
|
{}
|
||
|
|
// http://stackoverflow.com/a/3279550/148668
|
||
|
|
AABB(const AABB& other):
|
||
|
|
m_left(other.m_left ? new AABB(*other.m_left) : NULL),
|
||
|
|
m_right(other.m_right ? new AABB(*other.m_right) : NULL),
|
||
|
|
m_box(other.m_box),
|
||
|
|
m_primitive(other.m_primitive)
|
||
|
|
//m_low_sqr_d(other.m_low_sqr_d),
|
||
|
|
//m_depth(std::max(
|
||
|
|
// m_left ? m_left->m_depth + 1 : 0,
|
||
|
|
// m_right ? m_right->m_depth + 1 : 0))
|
||
|
|
{
|
||
|
|
}
|
||
|
|
// copy-swap idiom
|
||
|
|
friend void swap(AABB& first, AABB& second)
|
||
|
|
{
|
||
|
|
// Enable ADL
|
||
|
|
using std::swap;
|
||
|
|
swap(first.m_left,second.m_left);
|
||
|
|
swap(first.m_right,second.m_right);
|
||
|
|
swap(first.m_box,second.m_box);
|
||
|
|
swap(first.m_primitive,second.m_primitive);
|
||
|
|
//swap(first.m_low_sqr_d,second.m_low_sqr_d);
|
||
|
|
//swap(first.m_depth,second.m_depth);
|
||
|
|
}
|
||
|
|
// Pass-by-value (aka copy)
|
||
|
|
AABB& operator=(AABB other)
|
||
|
|
{
|
||
|
|
swap(*this,other);
|
||
|
|
return *this;
|
||
|
|
}
|
||
|
|
AABB(AABB&& other):
|
||
|
|
// initialize via default constructor
|
||
|
|
AABB()
|
||
|
|
{
|
||
|
|
swap(*this,other);
|
||
|
|
}
|
||
|
|
// Seems like there should have been an elegant solution to this using
|
||
|
|
// the copy-swap idiom above:
|
||
|
|
IGL_INLINE void deinit()
|
||
|
|
{
|
||
|
|
m_primitive = -1;
|
||
|
|
m_box = Eigen::AlignedBox<Scalar,DIM>();
|
||
|
|
delete m_left;
|
||
|
|
m_left = NULL;
|
||
|
|
delete m_right;
|
||
|
|
m_right = NULL;
|
||
|
|
}
|
||
|
|
~AABB()
|
||
|
|
{
|
||
|
|
deinit();
|
||
|
|
}
|
||
|
|
// Build an Axis-Aligned Bounding Box tree for a given mesh and given
|
||
|
|
// serialization of a previous AABB tree.
|
||
|
|
//
|
||
|
|
// Inputs:
|
||
|
|
// V #V by dim list of mesh vertex positions.
|
||
|
|
// Ele #Ele by dim+1 list of mesh indices into #V.
|
||
|
|
// bb_mins max_tree by dim list of bounding box min corner positions
|
||
|
|
// bb_maxs max_tree by dim list of bounding box max corner positions
|
||
|
|
// elements max_tree list of element or (not leaf id) indices into Ele
|
||
|
|
// i recursive call index {0}
|
||
|
|
template <
|
||
|
|
typename DerivedEle,
|
||
|
|
typename Derivedbb_mins,
|
||
|
|
typename Derivedbb_maxs,
|
||
|
|
typename Derivedelements>
|
||
|
|
IGL_INLINE void init(
|
||
|
|
const Eigen::MatrixBase<DerivedV> & V,
|
||
|
|
const Eigen::MatrixBase<DerivedEle> & Ele,
|
||
|
|
const Eigen::MatrixBase<Derivedbb_mins> & bb_mins,
|
||
|
|
const Eigen::MatrixBase<Derivedbb_maxs> & bb_maxs,
|
||
|
|
const Eigen::MatrixBase<Derivedelements> & elements,
|
||
|
|
const int i = 0);
|
||
|
|
// Wrapper for root with empty serialization
|
||
|
|
template <typename DerivedEle>
|
||
|
|
IGL_INLINE void init(
|
||
|
|
const Eigen::MatrixBase<DerivedV> & V,
|
||
|
|
const Eigen::MatrixBase<DerivedEle> & Ele);
|
||
|
|
// Build an Axis-Aligned Bounding Box tree for a given mesh.
|
||
|
|
//
|
||
|
|
// Inputs:
|
||
|
|
// V #V by dim list of mesh vertex positions.
|
||
|
|
// Ele #Ele by dim+1 list of mesh indices into #V.
|
||
|
|
// SI #Ele by dim list revealing for each coordinate where Ele's
|
||
|
|
// barycenters would be sorted: SI(e,d) = i --> the dth coordinate of
|
||
|
|
// the barycenter of the eth element would be placed at position i in a
|
||
|
|
// sorted list.
|
||
|
|
// I #I list of indices into Ele of elements to include (for recursive
|
||
|
|
// calls)
|
||
|
|
//
|
||
|
|
template <typename DerivedEle, typename DerivedSI, typename DerivedI>
|
||
|
|
IGL_INLINE void init(
|
||
|
|
const Eigen::MatrixBase<DerivedV> & V,
|
||
|
|
const Eigen::MatrixBase<DerivedEle> & Ele,
|
||
|
|
const Eigen::MatrixBase<DerivedSI> & SI,
|
||
|
|
const Eigen::MatrixBase<DerivedI>& I);
|
||
|
|
// Return whether at leaf node
|
||
|
|
IGL_INLINE bool is_leaf() const;
|
||
|
|
// Find the indices of elements containing given point: this makes sense
|
||
|
|
// when Ele is a co-dimension 0 simplex (tets in 3D, triangles in 2D).
|
||
|
|
//
|
||
|
|
// Inputs:
|
||
|
|
// V #V by dim list of mesh vertex positions. **Should be same as used to
|
||
|
|
// construct mesh.**
|
||
|
|
// Ele #Ele by dim+1 list of mesh indices into #V. **Should be same as used to
|
||
|
|
// construct mesh.**
|
||
|
|
// q dim row-vector query position
|
||
|
|
// first whether to only return first element containing q
|
||
|
|
// Returns:
|
||
|
|
// list of indices of elements containing q
|
||
|
|
template <typename DerivedEle, typename Derivedq>
|
||
|
|
IGL_INLINE std::vector<int> find(
|
||
|
|
const Eigen::MatrixBase<DerivedV> & V,
|
||
|
|
const Eigen::MatrixBase<DerivedEle> & Ele,
|
||
|
|
const Eigen::MatrixBase<Derivedq> & q,
|
||
|
|
const bool first=false) const;
|
||
|
|
|
||
|
|
// If number of elements m then total tree size should be 2*h where h is
|
||
|
|
// the deepest depth 2^ceil(log(#Ele*2-1))
|
||
|
|
IGL_INLINE int subtree_size() const;
|
||
|
|
|
||
|
|
// Serialize this class into 3 arrays (so we can pass it pack to matlab)
|
||
|
|
//
|
||
|
|
// Outputs:
|
||
|
|
// bb_mins max_tree by dim list of bounding box min corner positions
|
||
|
|
// bb_maxs max_tree by dim list of bounding box max corner positions
|
||
|
|
// elements max_tree list of element or (not leaf id) indices into Ele
|
||
|
|
// i recursive call index into these arrays {0}
|
||
|
|
template <
|
||
|
|
typename Derivedbb_mins,
|
||
|
|
typename Derivedbb_maxs,
|
||
|
|
typename Derivedelements>
|
||
|
|
IGL_INLINE void serialize(
|
||
|
|
Eigen::PlainObjectBase<Derivedbb_mins> & bb_mins,
|
||
|
|
Eigen::PlainObjectBase<Derivedbb_maxs> & bb_maxs,
|
||
|
|
Eigen::PlainObjectBase<Derivedelements> & elements,
|
||
|
|
const int i = 0) const;
|
||
|
|
// Compute squared distance to a query point
|
||
|
|
//
|
||
|
|
// Inputs:
|
||
|
|
// V #V by dim list of vertex positions
|
||
|
|
// Ele #Ele by dim list of simplex indices
|
||
|
|
// p dim-long query point
|
||
|
|
// Outputs:
|
||
|
|
// i facet index corresponding to smallest distances
|
||
|
|
// c closest point
|
||
|
|
// Returns squared distance
|
||
|
|
//
|
||
|
|
// Known bugs: currently assumes Elements are triangles regardless of
|
||
|
|
// dimension.
|
||
|
|
template <typename DerivedEle>
|
||
|
|
IGL_INLINE Scalar squared_distance(
|
||
|
|
const Eigen::MatrixBase<DerivedV> & V,
|
||
|
|
const Eigen::MatrixBase<DerivedEle> & Ele,
|
||
|
|
const RowVectorDIMS & p,
|
||
|
|
int & i,
|
||
|
|
Eigen::PlainObjectBase<RowVectorDIMS> & c) const;
|
||
|
|
//private:
|
||
|
|
// Compute squared distance to a query point
|
||
|
|
//
|
||
|
|
// Inputs:
|
||
|
|
// V #V by dim list of vertex positions
|
||
|
|
// Ele #Ele by dim list of simplex indices
|
||
|
|
// p dim-long query point
|
||
|
|
// low_sqr_d lower bound on squared distance, specified maximum squared
|
||
|
|
// distance
|
||
|
|
// up_sqr_d current upper bounded on squared distance, current minimum
|
||
|
|
// squared distance (only consider distances less than this), see
|
||
|
|
// output.
|
||
|
|
// Outputs:
|
||
|
|
// up_sqr_d updated current minimum squared distance
|
||
|
|
// i facet index corresponding to smallest distances
|
||
|
|
// c closest point
|
||
|
|
// Returns squared distance
|
||
|
|
//
|
||
|
|
// Known bugs: currently assumes Elements are triangles regardless of
|
||
|
|
// dimension.
|
||
|
|
template <typename DerivedEle>
|
||
|
|
IGL_INLINE Scalar squared_distance(
|
||
|
|
const Eigen::MatrixBase<DerivedV> & V,
|
||
|
|
const Eigen::MatrixBase<DerivedEle> & Ele,
|
||
|
|
const RowVectorDIMS & p,
|
||
|
|
const Scalar low_sqr_d,
|
||
|
|
const Scalar up_sqr_d,
|
||
|
|
int & i,
|
||
|
|
Eigen::PlainObjectBase<RowVectorDIMS> & c) const;
|
||
|
|
// Default low_sqr_d
|
||
|
|
template <typename DerivedEle>
|
||
|
|
IGL_INLINE Scalar squared_distance(
|
||
|
|
const Eigen::MatrixBase<DerivedV> & V,
|
||
|
|
const Eigen::MatrixBase<DerivedEle> & Ele,
|
||
|
|
const RowVectorDIMS & p,
|
||
|
|
const Scalar up_sqr_d,
|
||
|
|
int & i,
|
||
|
|
Eigen::PlainObjectBase<RowVectorDIMS> & c) const;
|
||
|
|
// All hits
|
||
|
|
template <typename DerivedEle>
|
||
|
|
IGL_INLINE bool intersect_ray(
|
||
|
|
const Eigen::MatrixBase<DerivedV> & V,
|
||
|
|
const Eigen::MatrixBase<DerivedEle> & Ele,
|
||
|
|
const RowVectorDIMS & origin,
|
||
|
|
const RowVectorDIMS & dir,
|
||
|
|
std::vector<igl::Hit> & hits) const;
|
||
|
|
// First hit
|
||
|
|
template <typename DerivedEle>
|
||
|
|
IGL_INLINE bool intersect_ray(
|
||
|
|
const Eigen::MatrixBase<DerivedV> & V,
|
||
|
|
const Eigen::MatrixBase<DerivedEle> & Ele,
|
||
|
|
const RowVectorDIMS & origin,
|
||
|
|
const RowVectorDIMS & dir,
|
||
|
|
igl::Hit & hit) const;
|
||
|
|
//private:
|
||
|
|
template <typename DerivedEle>
|
||
|
|
IGL_INLINE bool intersect_ray(
|
||
|
|
const Eigen::MatrixBase<DerivedV> & V,
|
||
|
|
const Eigen::MatrixBase<DerivedEle> & Ele,
|
||
|
|
const RowVectorDIMS & origin,
|
||
|
|
const RowVectorDIMS & dir,
|
||
|
|
const Scalar min_t,
|
||
|
|
igl::Hit & hit) const;
|
||
|
|
|
||
|
|
|
||
|
|
public:
|
||
|
|
// Compute the squared distance from all query points in P to the
|
||
|
|
// _closest_ points on the primitives stored in the AABB hierarchy for
|
||
|
|
// the mesh (V,Ele).
|
||
|
|
//
|
||
|
|
// Inputs:
|
||
|
|
// V #V by dim list of vertex positions
|
||
|
|
// Ele #Ele by dim list of simplex indices
|
||
|
|
// P #P by dim list of query points
|
||
|
|
// Outputs:
|
||
|
|
// sqrD #P list of squared distances
|
||
|
|
// I #P list of indices into Ele of closest primitives
|
||
|
|
// C #P by dim list of closest points
|
||
|
|
template <
|
||
|
|
typename DerivedEle,
|
||
|
|
typename DerivedP,
|
||
|
|
typename DerivedsqrD,
|
||
|
|
typename DerivedI,
|
||
|
|
typename DerivedC>
|
||
|
|
IGL_INLINE void squared_distance(
|
||
|
|
const Eigen::MatrixBase<DerivedV> & V,
|
||
|
|
const Eigen::MatrixBase<DerivedEle> & Ele,
|
||
|
|
const Eigen::MatrixBase<DerivedP> & P,
|
||
|
|
Eigen::PlainObjectBase<DerivedsqrD> & sqrD,
|
||
|
|
Eigen::PlainObjectBase<DerivedI> & I,
|
||
|
|
Eigen::PlainObjectBase<DerivedC> & C) const;
|
||
|
|
|
||
|
|
// Compute the squared distance from all query points in P already stored
|
||
|
|
// in its own AABB hierarchy to the _closest_ points on the primitives
|
||
|
|
// stored in the AABB hierarchy for the mesh (V,Ele).
|
||
|
|
//
|
||
|
|
// Inputs:
|
||
|
|
// V #V by dim list of vertex positions
|
||
|
|
// Ele #Ele by dim list of simplex indices
|
||
|
|
// other AABB hierarchy of another set of primitives (must be points)
|
||
|
|
// other_V #other_V by dim list of query points
|
||
|
|
// other_Ele #other_Ele by ss list of simplex indices into other_V
|
||
|
|
// (must be simple list of points: ss == 1)
|
||
|
|
// Outputs:
|
||
|
|
// sqrD #P list of squared distances
|
||
|
|
// I #P list of indices into Ele of closest primitives
|
||
|
|
// C #P by dim list of closest points
|
||
|
|
template <
|
||
|
|
typename DerivedEle,
|
||
|
|
typename Derivedother_V,
|
||
|
|
typename Derivedother_Ele,
|
||
|
|
typename DerivedsqrD,
|
||
|
|
typename DerivedI,
|
||
|
|
typename DerivedC>
|
||
|
|
IGL_INLINE void squared_distance(
|
||
|
|
const Eigen::MatrixBase<DerivedV> & V,
|
||
|
|
const Eigen::MatrixBase<DerivedEle> & Ele,
|
||
|
|
const AABB<Derivedother_V,DIM> & other,
|
||
|
|
const Eigen::MatrixBase<Derivedother_V> & other_V,
|
||
|
|
const Eigen::MatrixBase<Derivedother_Ele> & other_Ele,
|
||
|
|
Eigen::PlainObjectBase<DerivedsqrD> & sqrD,
|
||
|
|
Eigen::PlainObjectBase<DerivedI> & I,
|
||
|
|
Eigen::PlainObjectBase<DerivedC> & C) const;
|
||
|
|
private:
|
||
|
|
template <
|
||
|
|
typename DerivedEle,
|
||
|
|
typename Derivedother_V,
|
||
|
|
typename Derivedother_Ele,
|
||
|
|
typename DerivedsqrD,
|
||
|
|
typename DerivedI,
|
||
|
|
typename DerivedC>
|
||
|
|
IGL_INLINE Scalar squared_distance_helper(
|
||
|
|
const Eigen::MatrixBase<DerivedV> & V,
|
||
|
|
const Eigen::MatrixBase<DerivedEle> & Ele,
|
||
|
|
const AABB<Derivedother_V,DIM> * other,
|
||
|
|
const Eigen::MatrixBase<Derivedother_V> & other_V,
|
||
|
|
const Eigen::MatrixBase<Derivedother_Ele>& other_Ele,
|
||
|
|
const Scalar up_sqr_d,
|
||
|
|
Eigen::PlainObjectBase<DerivedsqrD> & sqrD,
|
||
|
|
Eigen::PlainObjectBase<DerivedI> & I,
|
||
|
|
Eigen::PlainObjectBase<DerivedC> & C) const;
|
||
|
|
// Compute the squared distance to the primitive in this node: assumes
|
||
|
|
// that this is indeed a leaf node.
|
||
|
|
//
|
||
|
|
// Inputs:
|
||
|
|
// V #V by dim list of vertex positions
|
||
|
|
// Ele #Ele by dim list of simplex indices
|
||
|
|
// p dim-long query point
|
||
|
|
// sqr_d current minimum distance for this query, see output
|
||
|
|
// i current index into Ele of closest point, see output
|
||
|
|
// c dim-long current closest point, see output
|
||
|
|
// Outputs:
|
||
|
|
// sqr_d minimum of initial value and squared distance to this
|
||
|
|
// primitive
|
||
|
|
// i possibly updated index into Ele of closest point
|
||
|
|
// c dim-long possibly updated closest point
|
||
|
|
template <typename DerivedEle>
|
||
|
|
IGL_INLINE void leaf_squared_distance(
|
||
|
|
const Eigen::MatrixBase<DerivedV> & V,
|
||
|
|
const Eigen::MatrixBase<DerivedEle> & Ele,
|
||
|
|
const RowVectorDIMS & p,
|
||
|
|
const Scalar low_sqr_d,
|
||
|
|
Scalar & sqr_d,
|
||
|
|
int & i,
|
||
|
|
Eigen::PlainObjectBase<RowVectorDIMS> & c) const;
|
||
|
|
// Default low_sqr_d
|
||
|
|
template <typename DerivedEle>
|
||
|
|
IGL_INLINE void leaf_squared_distance(
|
||
|
|
const Eigen::MatrixBase<DerivedV> & V,
|
||
|
|
const Eigen::MatrixBase<DerivedEle> & Ele,
|
||
|
|
const RowVectorDIMS & p,
|
||
|
|
Scalar & sqr_d,
|
||
|
|
int & i,
|
||
|
|
Eigen::PlainObjectBase<RowVectorDIMS> & c) const;
|
||
|
|
// If new distance (sqr_d_candidate) is less than current distance
|
||
|
|
// (sqr_d), then update this distance and its associated values
|
||
|
|
// _in-place_:
|
||
|
|
//
|
||
|
|
// Inputs:
|
||
|
|
// p dim-long query point (only used in DEBUG mode)
|
||
|
|
// sqr_d candidate minimum distance for this query, see output
|
||
|
|
// i candidate index into Ele of closest point, see output
|
||
|
|
// c dim-long candidate closest point, see output
|
||
|
|
// sqr_d current minimum distance for this query, see output
|
||
|
|
// i current index into Ele of closest point, see output
|
||
|
|
// c dim-long current closest point, see output
|
||
|
|
// Outputs:
|
||
|
|
// sqr_d minimum of initial value and squared distance to this
|
||
|
|
// primitive
|
||
|
|
// i possibly updated index into Ele of closest point
|
||
|
|
// c dim-long possibly updated closest point
|
||
|
|
IGL_INLINE void set_min(
|
||
|
|
const RowVectorDIMS & p,
|
||
|
|
const Scalar sqr_d_candidate,
|
||
|
|
const int i_candidate,
|
||
|
|
const RowVectorDIMS & c_candidate,
|
||
|
|
Scalar & sqr_d,
|
||
|
|
int & i,
|
||
|
|
Eigen::PlainObjectBase<RowVectorDIMS> & c) const;
|
||
|
|
public:
|
||
|
|
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||
|
|
};
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
#ifndef IGL_STATIC_LIBRARY
|
||
|
|
# include "AABB.cpp"
|
||
|
|
#endif
|
||
|
|
|
||
|
|
#endif
|