// Copyright (c) 2008,2011 INRIA Sophia-Antipolis (France). // All rights reserved. // // This file is part of CGAL (www.cgal.org). // You can redistribute it and/or modify it under the terms of the GNU // General Public License as published by the Free Software Foundation, // either version 3 of the License, or (at your option) any later version. // // Licensees holding a valid commercial license may use this file in // accordance with the commercial license agreement provided with the software. // // This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE // WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. // // $URL$ // $Id$ // // // Author(s) : Camille Wormser, Pierre Alliez, Stephane Tayeb #ifndef CGAL_AABB_TREE_H #define CGAL_AABB_TREE_H #include #include #include #include #include #include #include #include #ifdef CGAL_HAS_THREADS #include #endif namespace CGAL { /** * @class AABB_tree * * */ template class AABB_tree { public: /// types typedef AABBTraits AABB_traits; typedef typename AABBTraits::FT FT; typedef typename AABBTraits::Point Point; typedef typename AABBTraits::Primitive Primitive; typedef typename AABBTraits::Bounding_box Bounding_box; typedef typename AABBTraits::Primitive::Id Primitive_id; typedef typename AABBTraits::Point_and_primitive_id Point_and_primitive_id; typedef typename AABBTraits::Object_and_primitive_id Object_and_primitive_id; private: // internal KD-tree used to accelerate the distance queries typedef AABB_search_tree Search_tree; // type of the primitives container typedef std::vector Primitives; public: // size type is the size_type of the primitive container typedef typename Primitives::size_type size_type; public: /** * @brief Default Constructor * * Builds an empty tree datastructure. */ AABB_tree(const AABBTraits& traits); /** * @brief Constructor * @param first iterator over first primitive to insert * @param beyond past-the-end iterator * * Builds the datastructure. Type ConstPrimitiveIterator can be any const * iterator on a container of Primitive::id_type such that Primitive has * a constructor taking a ConstPrimitiveIterator as argument. */ #ifndef CGAL_CFG_NO_CPP0X_VARIADIC_TEMPLATES template AABB_tree(ConstPrimitiveIterator first, ConstPrimitiveIterator beyond,T...); #else template AABB_tree(ConstPrimitiveIterator first, ConstPrimitiveIterator beyond); template AABB_tree(ConstPrimitiveIterator first, ConstPrimitiveIterator beyond,T1); template AABB_tree(ConstPrimitiveIterator first, ConstPrimitiveIterator beyond,T1,T2); template AABB_tree(ConstPrimitiveIterator first, ConstPrimitiveIterator beyond,T1,T2,T3); template AABB_tree(ConstPrimitiveIterator first, ConstPrimitiveIterator beyond,T1,T2,T3,T4); template AABB_tree(ConstPrimitiveIterator first, ConstPrimitiveIterator beyond,T1,T2,T3,T4,T5); #endif /// Clears the current tree and rebuilds the datastructure. /// Type ConstPrimitiveIterator can be any const iterator on /// a container of Primitive::id_type such that Primitive has /// a constructor taking a ConstPrimitiveIterator as argument. /// Returns true if the memory allocation was successful. #ifndef CGAL_CFG_NO_CPP0X_VARIADIC_TEMPLATES template void rebuild(ConstPrimitiveIterator first, ConstPrimitiveIterator beyond,T...); #else template void rebuild(ConstPrimitiveIterator first, ConstPrimitiveIterator beyond); template void rebuild(ConstPrimitiveIterator first, ConstPrimitiveIterator beyond,T1); template void rebuild(ConstPrimitiveIterator first, ConstPrimitiveIterator beyond,T1,T2); template void rebuild(ConstPrimitiveIterator first, ConstPrimitiveIterator beyond,T1,T2,T3); template void rebuild(ConstPrimitiveIterator first, ConstPrimitiveIterator beyond,T1,T2,T3,T4); template void rebuild(ConstPrimitiveIterator first, ConstPrimitiveIterator beyond,T1,T2,T3,T4,T5); #endif /// Add primitives in the structure. build() must be called before any /// request. #ifndef CGAL_CFG_NO_CPP0X_VARIADIC_TEMPLATES template void insert(ConstPrimitiveIterator first, ConstPrimitiveIterator beyond,T...); #else template void insert(ConstPrimitiveIterator first, ConstPrimitiveIterator beyond); template void insert(ConstPrimitiveIterator first, ConstPrimitiveIterator beyond,T1); template void insert(ConstPrimitiveIterator first, ConstPrimitiveIterator beyond,T1,T2); template void insert(ConstPrimitiveIterator first, ConstPrimitiveIterator beyond,T1,T2,T3); template void insert(ConstPrimitiveIterator first, ConstPrimitiveIterator beyond,T1,T2,T3,T4); template void insert(ConstPrimitiveIterator first, ConstPrimitiveIterator beyond,T1,T2,T3,T4,T5); #endif inline void insert(const Primitive& p); /// Build the data structure, after calls to insert() void build(); /// Non virtual destructor ~AABB_tree() { clear(); } const AABBTraits& traits() const{ return m_traits; } /// Clears the tree void clear() { // clear AABB tree m_primitives.clear(); clear_nodes(); clear_search_tree(); } // bbox and size const Bounding_box& bbox() const { return root_node()->bbox(); } size_type size() const { return m_primitives.size(); } bool empty() const { return m_primitives.empty(); } private: #ifndef CGAL_CFG_NO_CPP0X_VARIADIC_TEMPLATES template void set_primitive_data_impl(CGAL::Boolean_tag,T ... ){} template void set_primitive_data_impl(CGAL::Boolean_tag,T ... t) {m_traits.set_shared_data(t...);} template void set_shared_data(T...t){ set_primitive_data_impl(CGAL::Boolean_tag::value>(),t...); } #else template void set_primitive_data_impl(CGAL::Boolean_tag){} template void set_primitive_data_impl(CGAL::Boolean_tag) {m_traits.set_shared_data();} template void set_shared_data(){ set_primitive_data_impl(CGAL::Boolean_tag::value>()); } template void set_primitive_data_impl(CGAL::Boolean_tag,T1){} template void set_primitive_data_impl(CGAL::Boolean_tag,T1 t1) {m_traits.set_shared_data(t1);} template void set_shared_data(T1 t1){ set_primitive_data_impl(Boolean_tag::value>(),t1); } template void set_primitive_data_impl(CGAL::Boolean_tag,T1,T2){} template void set_primitive_data_impl(CGAL::Boolean_tag,T1 t1,T2 t2) {m_traits.set_shared_data(t1,t2);} template void set_shared_data(T1 t1,T2 t2){ set_primitive_data_impl(Boolean_tag::value>(),t1,t2); } template void set_primitive_data_impl(CGAL::Boolean_tag,T1,T2,T3){} template void set_primitive_data_impl(CGAL::Boolean_tag,T1 t1,T2 t2,T3 t3) {m_traits.set_shared_data(t1,t2,t3);} template void set_shared_data(T1 t1,T2 t2,T3 t3){ set_primitive_data_impl(Boolean_tag::value>(),t1,t2,t3); } template void set_primitive_data_impl(CGAL::Boolean_tag,T1,T2,T3,T4){} template void set_primitive_data_impl(CGAL::Boolean_tag,T1 t1,T2 t2,T3 t3,T4 t4) {m_traits.set_shared_data(t1,t2,t3,t4);} template void set_shared_data(T1 t1,T2 t2,T3 t3,T4 t4){ set_primitive_data_impl(Boolean_tag::value>(),t1,t2,t3,t4); } template void set_primitive_data_impl(CGAL::Boolean_tag,T1,T2,T3,T4,T5){} template void set_primitive_data_impl(CGAL::Boolean_tag,T1 t1,T2 t2,T3 t3,T4 t4,T5 t5) {m_traits.set_shared_data(t1,t2,t3,t4,t5);} template void set_shared_data(T1 t1,T2 t2,T3 t3,T4 t4,T5 t5){ set_primitive_data_impl(Boolean_tag::value>(),t1,t2,t3,t4,t5); } #endif template bool accelerate_distance_queries_impl(ConstPointIterator first, ConstPointIterator beyond) const; public: /// Constructs internal search tree with a given point set // returns true iff successful memory allocation // Note: this function simply forward the call to the _impl. // The need of the _impl version comes from the fact that // we guarantee this class to be read-only thread-safe and // that accelerate_distance_queries() also calls the _impl // version that has no mutex locking strategy. template bool accelerate_distance_queries(ConstPointIterator first, ConstPointIterator beyond) const { #ifdef CGAL_HAS_THREADS //this ensures that this is done once at a time boost::mutex::scoped_lock scoped_lock(kd_tree_mutex); #endif clear_search_tree(); return accelerate_distance_queries_impl(first,beyond); } /// Constructs internal search tree from /// a point set taken on the internal primitives // returns true iff successful memory allocation bool accelerate_distance_queries() const; // intersection tests template bool do_intersect(const Query& query) const; // #intersections template size_type number_of_intersected_primitives(const Query& query) const; // all intersections template OutputIterator all_intersected_primitives(const Query& query, OutputIterator out) const; template OutputIterator all_intersections(const Query& query, OutputIterator out) const; // any intersection template boost::optional any_intersected_primitive(const Query& query) const; template boost::optional any_intersection(const Query& query) const; // distance queries FT squared_distance(const Point& query) const; FT squared_distance(const Point& query, const Point& hint) const; Point closest_point(const Point& query) const; Point closest_point(const Point& query, const Point& hint) const; Point_and_primitive_id closest_point_and_primitive(const Point& query) const; Point_and_primitive_id closest_point_and_primitive(const Point& query, const Point_and_primitive_id& hint) const; private: // clear nodes void clear_nodes() { delete [] m_p_root_node; m_p_root_node = NULL; } // clears internal KD tree void clear_search_tree() const { delete m_p_search_tree; m_p_search_tree = NULL; m_search_tree_constructed = false; m_default_search_tree_constructed = false; } public: // made public for advanced use by the polyhedron demo /// generic traversal of the tree template void traversal(const Query& query, Traversal_traits& traits) const { if(!empty()) root_node()->template traversal(query, traits, m_primitives.size()); else std::cerr << "AABB tree traversal with empty tree" << std::endl; } private: typedef AABB_node Node; public: // returns a point which must be on one primitive Point_and_primitive_id any_reference_point_and_id() const { CGAL_assertion(!empty()); return Point_and_primitive_id( internal::Primitive_helper::get_reference_point(m_primitives[0],m_traits), m_primitives[0].id() ); } public: Point_and_primitive_id best_hint(const Point& query) const { if(m_search_tree_constructed) return m_p_search_tree->closest_point(query); else return this->any_reference_point_and_id(); } private: //Traits class AABBTraits m_traits; // set of input primitives Primitives m_primitives; // single root node Node* m_p_root_node; #ifdef CGAL_HAS_THREADS mutable boost::mutex internal_tree_mutex;//mutex used to protect const calls inducing build() mutable boost::mutex kd_tree_mutex;//mutex used to protect calls to accelerate_distance_queries #endif const Node* root_node() const { if(m_need_build){ #ifdef CGAL_HAS_THREADS //this ensures that build() will be called once boost::mutex::scoped_lock scoped_lock(internal_tree_mutex); if(m_need_build) #endif const_cast< AABB_tree* >(this)->build(); } return m_p_root_node; } // search KD-tree mutable const Search_tree* m_p_search_tree; mutable bool m_search_tree_constructed; mutable bool m_default_search_tree_constructed; bool m_need_build; private: // Disabled copy constructor & assignment operator typedef AABB_tree Self; AABB_tree(const Self& src); Self& operator=(const Self& src); }; // end class AABB_tree template AABB_tree::AABB_tree(const Tr& traits) : m_traits(traits) , m_primitives() , m_p_root_node(NULL) , m_p_search_tree(NULL) , m_search_tree_constructed(false) , m_default_search_tree_constructed(false) , m_need_build(false) {} #ifndef CGAL_CFG_NO_CPP0X_VARIADIC_TEMPLATES template template AABB_tree::AABB_tree(ConstPrimitiveIterator first, ConstPrimitiveIterator beyond, T ... t) : m_traits() , m_primitives() , m_p_root_node(NULL) , m_p_search_tree(NULL) , m_search_tree_constructed(false) , m_default_search_tree_constructed(false) , m_need_build(false) { // Insert each primitive into tree insert(first, beyond,t...); } template template void AABB_tree::insert(ConstPrimitiveIterator first, ConstPrimitiveIterator beyond, T ... t) { set_shared_data(t...); while(first != beyond) { m_primitives.push_back(Primitive(first,t...)); ++first; } m_need_build = true; } // Clears tree and insert a set of primitives template template void AABB_tree::rebuild(ConstPrimitiveIterator first, ConstPrimitiveIterator beyond, T ... t) { // cleanup current tree and internal KD tree clear(); // inserts primitives insert(first, beyond,t...); build(); } #else //=============constructor====================== template template AABB_tree::AABB_tree(ConstPrimitiveIterator first, ConstPrimitiveIterator beyond) : m_traits() , m_primitives() , m_p_root_node(NULL) , m_p_search_tree(NULL) , m_search_tree_constructed(false) , m_default_search_tree_constructed(false) , m_need_build(false) { // Insert each primitive into tree insert(first, beyond); } template template AABB_tree::AABB_tree(ConstPrimitiveIterator first, ConstPrimitiveIterator beyond, T1 t1) : m_traits() , m_primitives() , m_p_root_node(NULL) , m_p_search_tree(NULL) , m_search_tree_constructed(false) , m_default_search_tree_constructed(false) , m_need_build(false) { // Insert each primitive into tree insert(first, beyond,t1); } template template AABB_tree::AABB_tree(ConstPrimitiveIterator first, ConstPrimitiveIterator beyond, T1 t1,T2 t2) : m_traits() , m_primitives() , m_p_root_node(NULL) , m_p_search_tree(NULL) , m_search_tree_constructed(false) , m_default_search_tree_constructed(false) , m_need_build(false) { // Insert each primitive into tree insert(first, beyond,t1,t2); } template template AABB_tree::AABB_tree(ConstPrimitiveIterator first, ConstPrimitiveIterator beyond, T1 t1,T2 t2,T3 t3) : m_traits() , m_primitives() , m_p_root_node(NULL) , m_p_search_tree(NULL) , m_search_tree_constructed(false) , m_default_search_tree_constructed(false) , m_need_build(false) { // Insert each primitive into tree insert(first, beyond,t1,t2,t3); } template template AABB_tree::AABB_tree(ConstPrimitiveIterator first, ConstPrimitiveIterator beyond, T1 t1,T2 t2,T3 t3,T4 t4) : m_traits() , m_primitives() , m_p_root_node(NULL) , m_p_search_tree(NULL) , m_search_tree_constructed(false) , m_default_search_tree_constructed(false) , m_need_build(false) { // Insert each primitive into tree insert(first, beyond,t1,t2,t3,t4); } template template AABB_tree::AABB_tree(ConstPrimitiveIterator first, ConstPrimitiveIterator beyond, T1 t1,T2 t2,T3 t3,T4 t4,T5 t5) : m_traits() , m_primitives() , m_p_root_node(NULL) , m_p_search_tree(NULL) , m_search_tree_constructed(false) , m_default_search_tree_constructed(false) , m_need_build(false) { // Insert each primitive into tree insert(first, beyond,t1,t2,t3,t4,t5); } //=============insert====================== template template void AABB_tree::insert(ConstPrimitiveIterator first, ConstPrimitiveIterator beyond) { set_shared_data(); while(first != beyond) { m_primitives.push_back(Primitive(first)); ++first; } m_need_build = true; } template template void AABB_tree::insert(ConstPrimitiveIterator first, ConstPrimitiveIterator beyond, T1 t1) { set_shared_data(t1); while(first != beyond) { m_primitives.push_back(Primitive(first,t1)); ++first; } m_need_build = true; } template template void AABB_tree::insert(ConstPrimitiveIterator first, ConstPrimitiveIterator beyond, T1 t1,T2 t2) { set_shared_data(t1,t2); while(first != beyond) { m_primitives.push_back(Primitive(first,t1,t2)); ++first; } m_need_build = true; } template template void AABB_tree::insert(ConstPrimitiveIterator first, ConstPrimitiveIterator beyond, T1 t1,T2 t2,T3 t3) { set_shared_data(t1,t2,t3); while(first != beyond) { m_primitives.push_back(Primitive(first,t1,t2,t3)); ++first; } m_need_build = true; } template template void AABB_tree::insert(ConstPrimitiveIterator first, ConstPrimitiveIterator beyond, T1 t1,T2 t2,T3 t3,T4 t4) { set_shared_data(t1,t2,t3,t4); while(first != beyond) { m_primitives.push_back(Primitive(first,t1,t2,t3,t4)); ++first; } m_need_build = true; } template template void AABB_tree::insert(ConstPrimitiveIterator first, ConstPrimitiveIterator beyond, T1 t1,T2 t2,T3 t3,T4 t4,T5 t5) { set_shared_data(t1,t2,t3,t4,t5); while(first != beyond) { m_primitives.push_back(Primitive(first,t1,t2,t3,t4,t5)); ++first; } m_need_build = true; } //=============insert====================== template template void AABB_tree::rebuild(ConstPrimitiveIterator first, ConstPrimitiveIterator beyond) { // cleanup current tree and internal KD tree clear(); // inserts primitives insert(first, beyond); build(); } template template void AABB_tree::rebuild(ConstPrimitiveIterator first, ConstPrimitiveIterator beyond, T1 t1) { // cleanup current tree and internal KD tree clear(); // inserts primitives insert(first, beyond,t1); build(); } template template void AABB_tree::rebuild(ConstPrimitiveIterator first, ConstPrimitiveIterator beyond, T1 t1,T2 t2) { // cleanup current tree and internal KD tree clear(); // inserts primitives insert(first, beyond,t1,t2); build(); } template template void AABB_tree::rebuild(ConstPrimitiveIterator first, ConstPrimitiveIterator beyond, T1 t1,T2 t2,T3 t3) { // cleanup current tree and internal KD tree clear(); // inserts primitives insert(first, beyond,t1,t2,t3); build(); } template template void AABB_tree::rebuild(ConstPrimitiveIterator first, ConstPrimitiveIterator beyond, T1 t1,T2 t2,T3 t3,T4 t4) { // cleanup current tree and internal KD tree clear(); // inserts primitives insert(first, beyond,t1,t2,t3,t4); build(); } template template void AABB_tree::rebuild(ConstPrimitiveIterator first, ConstPrimitiveIterator beyond, T1 t1,T2 t2,T3 t3,T4 t4,T5 t5) { // cleanup current tree and internal KD tree clear(); // inserts primitives insert(first, beyond,t1,t2,t3,t4,t5); build(); } #endif template void AABB_tree::insert(const Primitive& p) { m_primitives.push_back(p); m_need_build = true; } // Build the data structure, after calls to insert(..) template void AABB_tree::build() { clear_nodes(); CGAL_assertion(m_primitives.size() > 1); // allocates tree nodes m_p_root_node = new Node[m_primitives.size()-1](); if(m_p_root_node == NULL) { std::cerr << "Unable to allocate memory for AABB tree" << std::endl; CGAL_assertion(m_p_root_node != NULL); m_primitives.clear(); clear(); } // constructs the tree m_p_root_node->expand(m_primitives.begin(), m_primitives.end(), m_primitives.size(),m_traits); // In case the users has switched on the acceletated distance query // data structure with the default arguments, then it has to be // rebuilt. if(m_default_search_tree_constructed) accelerate_distance_queries(); m_need_build = false; } // constructs the search KD tree from given points // to accelerate the distance queries template template bool AABB_tree::accelerate_distance_queries_impl(ConstPointIterator first, ConstPointIterator beyond) const { m_p_search_tree = new Search_tree(first, beyond); if(m_p_search_tree != NULL) { m_search_tree_constructed = true; return true; } else { std::cerr << "Unable to allocate memory for accelerating distance queries" << std::endl; return false; } } // constructs the search KD tree from internal primitives template bool AABB_tree::accelerate_distance_queries() const { CGAL_assertion(!m_primitives.empty()); #ifdef CGAL_HAS_THREADS //this ensures that this function will be done once boost::mutex::scoped_lock scoped_lock(kd_tree_mutex); #endif //we only redo computation only if needed if (!m_need_build && m_default_search_tree_constructed) return m_search_tree_constructed; // iterate over primitives to get reference points on them std::vector points; points.reserve(m_primitives.size()); typename Primitives::const_iterator it; for(it = m_primitives.begin(); it != m_primitives.end(); ++it) points.push_back( Point_and_primitive_id( internal::Primitive_helper::get_reference_point(*it,m_traits), it->id() ) ); // clears current KD tree clear_search_tree(); m_default_search_tree_constructed = true; return accelerate_distance_queries_impl(points.begin(), points.end()); } template template bool AABB_tree::do_intersect(const Query& query) const { using namespace CGAL::internal::AABB_tree; typedef typename AABB_tree::AABB_traits AABBTraits; Do_intersect_traits traversal_traits(m_traits); this->traversal(query, traversal_traits); return traversal_traits.is_intersection_found(); } template template typename AABB_tree::size_type AABB_tree::number_of_intersected_primitives(const Query& query) const { using namespace CGAL::internal::AABB_tree; using CGAL::internal::AABB_tree::Counting_output_iterator; typedef typename AABB_tree::AABB_traits AABBTraits; typedef Counting_output_iterator Counting_iterator; size_type counter = 0; Counting_iterator out(&counter); Listing_primitive_traits traversal_traits(out,m_traits); this->traversal(query, traversal_traits); return counter; } template template OutputIterator AABB_tree::all_intersected_primitives(const Query& query, OutputIterator out) const { using namespace CGAL::internal::AABB_tree; typedef typename AABB_tree::AABB_traits AABBTraits; Listing_primitive_traits traversal_traits(out,m_traits); this->traversal(query, traversal_traits); return out; } template template OutputIterator AABB_tree::all_intersections(const Query& query, OutputIterator out) const { using namespace CGAL::internal::AABB_tree; typedef typename AABB_tree::AABB_traits AABBTraits; Listing_intersection_traits traversal_traits(out,m_traits); this->traversal(query, traversal_traits); return out; } template template boost::optional::Object_and_primitive_id> AABB_tree::any_intersection(const Query& query) const { using namespace CGAL::internal::AABB_tree; typedef typename AABB_tree::AABB_traits AABBTraits; First_intersection_traits traversal_traits(m_traits); this->traversal(query, traversal_traits); return traversal_traits.result(); } template template boost::optional::Primitive_id> AABB_tree::any_intersected_primitive(const Query& query) const { using namespace CGAL::internal::AABB_tree; typedef typename AABB_tree::AABB_traits AABBTraits; First_primitive_traits traversal_traits(m_traits); this->traversal(query, traversal_traits); return traversal_traits.result(); } // closest point with user-specified hint template typename AABB_tree::Point AABB_tree::closest_point(const Point& query, const Point& hint) const { typename Primitive::Id hint_primitive = m_primitives[0].id(); using namespace CGAL::internal::AABB_tree; typedef typename AABB_tree::AABB_traits AABBTraits; Projection_traits projection_traits(hint,hint_primitive,m_traits); this->traversal(query, projection_traits); return projection_traits.closest_point(); } // closest point without hint, the search KD-tree is queried for the // first closest neighbor point to get a hint template typename AABB_tree::Point AABB_tree::closest_point(const Point& query) const { const Point_and_primitive_id hint = best_hint(query); return closest_point(query,hint.first); } // squared distance with user-specified hint template typename AABB_tree::FT AABB_tree::squared_distance(const Point& query, const Point& hint) const { const Point closest = this->closest_point(query, hint); return typename Tr::Compute_squared_distance_3()(query, closest); } // squared distance without user-specified hint template typename AABB_tree::FT AABB_tree::squared_distance(const Point& query) const { const Point closest = this->closest_point(query); return typename Tr::Compute_squared_distance_3()(query, closest); } // closest point with user-specified hint template typename AABB_tree::Point_and_primitive_id AABB_tree::closest_point_and_primitive(const Point& query) const { return closest_point_and_primitive(query,best_hint(query)); } // closest point with user-specified hint template typename AABB_tree::Point_and_primitive_id AABB_tree::closest_point_and_primitive(const Point& query, const Point_and_primitive_id& hint) const { using namespace CGAL::internal::AABB_tree; typedef typename AABB_tree::AABB_traits AABBTraits; Projection_traits projection_traits(hint.first,hint.second,m_traits); this->traversal(query, projection_traits); return projection_traits.closest_point_and_primitive(); } } // end namespace CGAL #endif // CGAL_AABB_TREE_H /***EMACS SETTINGS***/ /* Local Variables: */ /* tab-width: 2 */ /* End: */