.. _program_listing_file__tmp_ws_src_hpp-fcl_include_hpp_fcl_broadphase_detail_hierarchy_tree.h: Program Listing for File hierarchy_tree.h ========================================= |exhale_lsh| :ref:`Return to documentation for file ` (``/tmp/ws/src/hpp-fcl/include/hpp/fcl/broadphase/detail/hierarchy_tree.h``) .. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS .. code-block:: cpp /* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Open Source Robotics Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #ifndef HPP_FCL_HIERARCHY_TREE_H #define HPP_FCL_HIERARCHY_TREE_H #include #include #include #include #include "hpp/fcl/warning.hh" #include "hpp/fcl/BV/AABB.h" #include "hpp/fcl/broadphase/detail/morton.h" #include "hpp/fcl/broadphase/detail/node_base.h" namespace hpp { namespace fcl { namespace detail { template class HierarchyTree { public: typedef NodeBase Node; HierarchyTree(int bu_threshold_ = 16, int topdown_level_ = 0); ~HierarchyTree(); void init(std::vector& leaves, int level = 0); Node* insert(const BV& bv, void* data); void remove(Node* leaf); void clear(); bool empty() const; void update(Node* leaf, int lookahead_level = -1); bool update(Node* leaf, const BV& bv); bool update(Node* leaf, const BV& bv, const Vec3f& vel, FCL_REAL margin); bool update(Node* leaf, const BV& bv, const Vec3f& vel); size_t getMaxHeight() const; size_t getMaxDepth() const; void balanceBottomup(); void balanceTopdown(); void balanceIncremental(int iterations); void refit(); void extractLeaves(const Node* root, std::vector& leaves) const; size_t size() const; Node* getRoot() const; Node*& getRoot(); void print(Node* root, int depth); private: typedef typename std::vector*>::iterator NodeVecIterator; typedef typename std::vector*>::const_iterator NodeVecConstIterator; struct SortByMorton { bool operator()(const Node* a, const Node* b) const { return a->code < b->code; } }; void bottomup(const NodeVecIterator lbeg, const NodeVecIterator lend); Node* topdown(const NodeVecIterator lbeg, const NodeVecIterator lend); size_t getMaxHeight(Node* node) const; void getMaxDepth(Node* node, size_t depth, size_t& max_depth) const; Node* topdown_0(const NodeVecIterator lbeg, const NodeVecIterator lend); Node* topdown_1(const NodeVecIterator lbeg, const NodeVecIterator lend); void init_0(std::vector& leaves); void init_1(std::vector& leaves); void init_2(std::vector& leaves); void init_3(std::vector& leaves); Node* mortonRecurse_0(const NodeVecIterator lbeg, const NodeVecIterator lend, const uint32_t& split, int bits); Node* mortonRecurse_1(const NodeVecIterator lbeg, const NodeVecIterator lend, const uint32_t& split, int bits); Node* mortonRecurse_2(const NodeVecIterator lbeg, const NodeVecIterator lend); void update_(Node* leaf, const BV& bv); Node* sort(Node* n, Node*& r); // leaf node. void insertLeaf(Node* const sub_root, Node* const leaf); // adjusted. Node* removeLeaf(Node* const leaf); void fetchLeaves(Node* root, std::vector& leaves, int depth = -1); static size_t indexOf(Node* node); Node* createNode(Node* parent, const BV& bv, void* data); Node* createNode(Node* parent, const BV& bv1, const BV& bv2, void* data); Node* createNode(Node* parent, void* data); void deleteNode(Node* node); void recurseDeleteNode(Node* node); void recurseRefit(Node* node); protected: Node* root_node; size_t n_leaves; unsigned int opath; Node* free_node; int max_lookahead_level; public: int topdown_level; int bu_threshold; }; template bool nodeBaseLess(NodeBase* a, NodeBase* b, int d); template size_t select(const NodeBase& query, const NodeBase& node1, const NodeBase& node2); template size_t select(const BV& query, const NodeBase& node1, const NodeBase& node2); } // namespace detail } // namespace fcl } // namespace hpp #include "hpp/fcl/broadphase/detail/hierarchy_tree-inl.h" #endif