broadphase_dynamic_AABB_tree-inl.h
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011-2014, Willow Garage, Inc.
5  * Copyright (c) 2014-2016, Open Source Robotics Foundation
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of Open Source Robotics Foundation nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  */
35 
38 #ifndef HPP_FCL_BROAD_PHASE_DYNAMIC_AABB_TREE_INL_H
39 #define HPP_FCL_BROAD_PHASE_DYNAMIC_AABB_TREE_INL_H
40 
42 
43 #include <limits>
44 
45 #if HPP_FCL_HAVE_OCTOMAP
46 #include "hpp/fcl/octree.h"
47 #endif
48 
49 #include "hpp/fcl/BV/BV.h"
51 
52 namespace hpp {
53 namespace fcl {
54 namespace detail {
55 
56 namespace dynamic_AABB_tree {
57 
58 #if HPP_FCL_HAVE_OCTOMAP
59 
60 //==============================================================================
61 template <typename Derived>
62 bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
63  const OcTree* tree2, const OcTree::OcTreeNode* root2,
64  const AABB& root2_bv,
65  const Eigen::MatrixBase<Derived>& translation2,
67  if (!root2) {
68  if (root1->isLeaf()) {
69  CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data);
70 
71  if (!obj1->collisionGeometry()->isFree()) {
72  const AABB& root2_bv_t = translate(root2_bv, translation2);
73  if (root1->bv.overlap(root2_bv_t)) {
74  Box* box = new Box();
75  Transform3f box_tf;
77  tf2.translation() = translation2;
78  constructBox(root2_bv, tf2, *box, box_tf);
79 
80  box->cost_density =
81  tree2->getOccupancyThres(); // thresholds are 0, 1, so uncertain
82 
83  CollisionObject obj2(shared_ptr<CollisionGeometry>(box), box_tf);
84  return (*callback)(obj1, &obj2);
85  }
86  }
87  } else {
88  if (collisionRecurse_(root1->children[0], tree2, nullptr, root2_bv,
89  translation2, callback))
90  return true;
91  if (collisionRecurse_(root1->children[1], tree2, nullptr, root2_bv,
92  translation2, callback))
93  return true;
94  }
95 
96  return false;
97  } else if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) {
98  CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data);
99 
100  if (!tree2->isNodeFree(root2) && !obj1->collisionGeometry()->isFree()) {
101  const AABB& root2_bv_t = translate(root2_bv, translation2);
102  if (root1->bv.overlap(root2_bv_t)) {
103  Box* box = new Box();
104  Transform3f box_tf;
106  tf2.translation() = translation2;
107  constructBox(root2_bv, tf2, *box, box_tf);
108 
109  box->cost_density = root2->getOccupancy();
110  box->threshold_occupied = tree2->getOccupancyThres();
111 
112  CollisionObject obj2(shared_ptr<CollisionGeometry>(box), box_tf);
113  return (*callback)(obj1, &obj2);
114  } else
115  return false;
116  } else
117  return false;
118  }
119 
120  const AABB& root2_bv_t = translate(root2_bv, translation2);
121  if (tree2->isNodeFree(root2) || !root1->bv.overlap(root2_bv_t)) return false;
122 
123  if (!tree2->nodeHasChildren(root2) ||
124  (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) {
125  if (collisionRecurse_(root1->children[0], tree2, root2, root2_bv,
126  translation2, callback))
127  return true;
128  if (collisionRecurse_(root1->children[1], tree2, root2, root2_bv,
129  translation2, callback))
130  return true;
131  } else {
132  for (unsigned int i = 0; i < 8; ++i) {
133  if (tree2->nodeChildExists(root2, i)) {
134  const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i);
135  AABB child_bv;
136  computeChildBV(root2_bv, i, child_bv);
137 
138  if (collisionRecurse_(root1, tree2, child, child_bv, translation2,
139  callback))
140  return true;
141  } else {
142  AABB child_bv;
143  computeChildBV(root2_bv, i, child_bv);
144  if (collisionRecurse_(root1, tree2, nullptr, child_bv, translation2,
145  callback))
146  return true;
147  }
148  }
149  }
150  return false;
151 }
152 
153 //==============================================================================
154 template <typename Derived>
155 bool distanceRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
156  const OcTree* tree2, const OcTree::OcTreeNode* root2,
157  const AABB& root2_bv,
158  const Eigen::MatrixBase<Derived>& translation2,
159  DistanceCallBackBase* callback, FCL_REAL& min_dist) {
160  if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) {
161  if (tree2->isNodeOccupied(root2)) {
162  Box* box = new Box();
163  Transform3f box_tf;
165  tf2.translation() = translation2;
166  constructBox(root2_bv, tf2, *box, box_tf);
167  CollisionObject obj(shared_ptr<CollisionGeometry>(box), box_tf);
168  return (*callback)(static_cast<CollisionObject*>(root1->data), &obj,
169  min_dist);
170  } else
171  return false;
172  }
173 
174  if (!tree2->isNodeOccupied(root2)) return false;
175 
176  if (!tree2->nodeHasChildren(root2) ||
177  (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) {
178  const AABB& aabb2 = translate(root2_bv, translation2);
179  FCL_REAL d1 = aabb2.distance(root1->children[0]->bv);
180  FCL_REAL d2 = aabb2.distance(root1->children[1]->bv);
181 
182  if (d2 < d1) {
183  if (d2 < min_dist) {
184  if (distanceRecurse_(root1->children[1], tree2, root2, root2_bv,
185  translation2, callback, min_dist))
186  return true;
187  }
188 
189  if (d1 < min_dist) {
190  if (distanceRecurse_(root1->children[0], tree2, root2, root2_bv,
191  translation2, callback, min_dist))
192  return true;
193  }
194  } else {
195  if (d1 < min_dist) {
196  if (distanceRecurse_(root1->children[0], tree2, root2, root2_bv,
197  translation2, callback, min_dist))
198  return true;
199  }
200 
201  if (d2 < min_dist) {
202  if (distanceRecurse_(root1->children[1], tree2, root2, root2_bv,
203  translation2, callback, min_dist))
204  return true;
205  }
206  }
207  } else {
208  for (unsigned int i = 0; i < 8; ++i) {
209  if (tree2->nodeChildExists(root2, i)) {
210  const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i);
211  AABB child_bv;
212  computeChildBV(root2_bv, i, child_bv);
213  const AABB& aabb2 = translate(child_bv, translation2);
214 
215  FCL_REAL d = root1->bv.distance(aabb2);
216 
217  if (d < min_dist) {
218  if (distanceRecurse_(root1, tree2, child, child_bv, translation2,
219  callback, min_dist))
220  return true;
221  }
222  }
223  }
224  }
225 
226  return false;
227 }
228 
229 #endif
230 
231 } // namespace dynamic_AABB_tree
232 } // namespace detail
233 } // namespace fcl
234 } // namespace hpp
235 
236 #endif
hpp::fcl::detail::NodeBase::data
void * data
Definition: node_base.h:66
geometric_shapes.d1
float d1
Definition: scripts/geometric_shapes.py:31
hpp::fcl::OcTree::nodeHasChildren
bool nodeHasChildren(const OcTreeNode *node) const
return true if node has at least one child
Definition: octree.h:208
collision_manager.callback
callback
Definition: collision_manager.py:27
collision_manager.box
box
Definition: collision_manager.py:11
hpp::fcl::detail::NodeBase::isLeaf
bool isLeaf() const
whether is a leaf
Definition: node_base-inl.h:55
hpp::fcl::OcTree::getOccupancyThres
FCL_REAL getOccupancyThres() const
the threshold used to decide whether one node is occupied, this is NOT the octree occupied_thresold
Definition: octree.h:165
geometric_shapes_utility.h
hpp::fcl::OcTree::OcTreeNode
octomap::OcTreeNode OcTreeNode
Definition: octree.h:63
hpp::fcl::detail::NodeBase::bv
BV bv
the bounding volume for the node
Definition: node_base.h:52
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:65
hpp::fcl::DistanceCallBackBase
Base callback class for distance queries. This class can be supersed by child classes to provide desi...
Definition: broadphase_callbacks.h:73
hpp::fcl::computeChildBV
static void computeChildBV(const AABB &root_bv, unsigned int i, AABB &child_bv)
compute the bounding volume of an octree node's i-th child
Definition: octree.h:239
hpp::fcl::constructBox
HPP_FCL_DLLAPI void constructBox(const AABB &bv, Box &box, Transform3f &tf)
construct a box shape (with a configuration) from a given bounding volume
Definition: geometric_shapes_utility.cpp:911
hpp::fcl::OcTree::getNodeChild
OcTreeNode * getNodeChild(OcTreeNode *node, unsigned int childIdx)
Definition: octree.h:180
BV.h
hpp
Main namespace.
Definition: broadphase_bruteforce.h:44
hpp::fcl::CollisionObject
the object for collision or distance computation, contains the geometry and the transform information
Definition: collision_object.h:215
hpp::fcl::AABB::distance
FCL_REAL distance(const AABB &other) const
Distance between two AABBs.
Definition: AABB.cpp:114
gjk.tf2
tuple tf2
Definition: test/scripts/gjk.py:36
hpp::fcl::AABB
A class describing the AABB collision structure, which is a box in 3D space determined by two diagona...
Definition: BV/AABB.h:54
hpp::fcl::OcTree::nodeChildExists
bool nodeChildExists(const OcTreeNode *node, unsigned int childIdx) const
return true if the child at childIdx exists
Definition: octree.h:199
hpp::fcl::OcTree::isNodeOccupied
bool isNodeOccupied(const OcTreeNode *node) const
whether one node is completely occupied
Definition: octree.h:121
hpp::fcl::detail::NodeBase
dynamic AABB tree node
Definition: node_base.h:50
hpp::fcl::CollisionObject::collisionGeometry
const shared_ptr< const CollisionGeometry > collisionGeometry() const
get shared pointer to collision geometry of the object instance
Definition: collision_object.h:304
hpp::fcl::Transform3f::Identity
static Transform3f Identity()
Definition: transform.h:67
hpp::fcl::OcTree::isNodeFree
bool isNodeFree(const OcTreeNode *node) const
whether one node is completely free
Definition: octree.h:127
hpp::fcl::AABB::size
FCL_REAL size() const
Size of the AABB (used in BV_Splitter to order two AABBs)
Definition: BV/AABB.h:154
hpp::fcl::Transform3f
Simple transform class used locally by InterpMotion.
Definition: transform.h:54
broadphase_dynamic_AABB_tree.h
hpp::fcl::detail::NodeBase::children
NodeBase< BV > * children[2]
for leaf node, children nodes
Definition: node_base.h:65
hpp::fcl::CollisionCallBackBase
Base callback class for collision queries. This class can be supersed by child classes to provide des...
Definition: broadphase_callbacks.h:50
hpp::fcl::OcTree
Octree is one type of collision geometry which can encode uncertainty information in the sensor data.
Definition: octree.h:53
octree.h
hpp::fcl::translate
static AABB translate(const AABB &aabb, const Vec3f &t)
translate the center of AABB by t
Definition: BV/AABB.h:226
hpp::fcl::Box
Center at zero point, axis aligned box.
Definition: shape/geometric_shapes.h:125


hpp-fcl
Author(s):
autogenerated on Fri Aug 2 2024 02:45:12