broadphase_dynamic_AABB_tree_array-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_ARRAY_INL_H
39 #define HPP_FCL_BROAD_PHASE_DYNAMIC_AABB_TREE_ARRAY_INL_H
40 
43 
44 #if HPP_FCL_HAVE_OCTOMAP
45 #include "hpp/fcl/octree.h"
46 #endif
47 
48 namespace hpp {
49 namespace fcl {
50 namespace detail {
51 namespace dynamic_AABB_tree_array {
52 
53 #if HPP_FCL_HAVE_OCTOMAP
54 
55 //==============================================================================
56 template <typename Derived>
57 bool collisionRecurse_(
59  size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2,
60  const AABB& root2_bv, const Eigen::MatrixBase<Derived>& translation2,
63  nodes1 + root1_id;
64  if (!root2) {
65  if (root1->isLeaf()) {
66  CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data);
67 
68  if (!obj1->collisionGeometry()->isFree()) {
69  const AABB& root_bv_t = translate(root2_bv, translation2);
70  if (root1->bv.overlap(root_bv_t)) {
71  Box* box = new Box();
72  Transform3f box_tf;
74  tf2.translation() = translation2;
75  constructBox(root2_bv, tf2, *box, box_tf);
76 
77  box->cost_density = tree2->getDefaultOccupancy();
78 
79  CollisionObject obj2(shared_ptr<CollisionGeometry>(box), box_tf);
80  return (*callback)(obj1, &obj2);
81  }
82  }
83  } else {
84  if (collisionRecurse_(nodes1, root1->children[0], tree2, nullptr,
85  root2_bv, translation2, callback))
86  return true;
87  if (collisionRecurse_(nodes1, root1->children[1], tree2, nullptr,
88  root2_bv, translation2, callback))
89  return true;
90  }
91 
92  return false;
93  } else if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) {
94  CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data);
95  if (!tree2->isNodeFree(root2) && !obj1->collisionGeometry()->isFree()) {
96  const AABB& root_bv_t = translate(root2_bv, translation2);
97  if (root1->bv.overlap(root_bv_t)) {
98  Box* box = new Box();
99  Transform3f box_tf;
101  tf2.translation() = translation2;
102  constructBox(root2_bv, tf2, *box, box_tf);
103 
104  box->cost_density = root2->getOccupancy();
105  box->threshold_occupied = tree2->getOccupancyThres();
106 
107  CollisionObject obj2(shared_ptr<CollisionGeometry>(box), box_tf);
108  return (*callback)(obj1, &obj2);
109  } else
110  return false;
111  } else
112  return false;
113  }
114 
115  const AABB& root_bv_t = translate(root2_bv, translation2);
116  if (tree2->isNodeFree(root2) || !root1->bv.overlap(root_bv_t)) return false;
117 
118  if (!tree2->nodeHasChildren(root2) ||
119  (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) {
120  if (collisionRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv,
121  translation2, callback))
122  return true;
123  if (collisionRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv,
124  translation2, callback))
125  return true;
126  } else {
127  for (unsigned int i = 0; i < 8; ++i) {
128  if (tree2->nodeChildExists(root2, i)) {
129  const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i);
130  AABB child_bv;
131  computeChildBV(root2_bv, i, child_bv);
132 
133  if (collisionRecurse_(nodes1, root1_id, tree2, child, child_bv,
134  translation2, callback))
135  return true;
136  } else {
137  AABB child_bv;
138  computeChildBV(root2_bv, i, child_bv);
139  if (collisionRecurse_(nodes1, root1_id, tree2, nullptr, child_bv,
140  translation2, callback))
141  return true;
142  }
143  }
144  }
145 
146  return false;
147 }
148 
149 //==============================================================================
150 template <typename Derived>
151 bool distanceRecurse_(
153  size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2,
154  const AABB& root2_bv, const Eigen::MatrixBase<Derived>& translation2,
155  DistanceCallBackBase* callback, FCL_REAL& min_dist) {
157  nodes1 + root1_id;
158  if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) {
159  if (tree2->isNodeOccupied(root2)) {
160  Box* box = new Box();
161  Transform3f box_tf;
163  tf2.translation() = translation2;
164  constructBox(root2_bv, tf2, *box, box_tf);
165  CollisionObject obj(shared_ptr<CollisionGeometry>(box), box_tf);
166  return (*callback)(static_cast<CollisionObject*>(root1->data), &obj,
167  min_dist);
168  } else
169  return false;
170  }
171 
172  if (!tree2->isNodeOccupied(root2)) return false;
173 
174  if (!tree2->nodeHasChildren(root2) ||
175  (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) {
176  const AABB& aabb2 = translate(root2_bv, translation2);
177 
178  FCL_REAL d1 = aabb2.distance((nodes1 + root1->children[0])->bv);
179  FCL_REAL d2 = aabb2.distance((nodes1 + root1->children[1])->bv);
180 
181  if (d2 < d1) {
182  if (d2 < min_dist) {
183  if (distanceRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv,
184  translation2, callback, min_dist))
185  return true;
186  }
187 
188  if (d1 < min_dist) {
189  if (distanceRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv,
190  translation2, callback, min_dist))
191  return true;
192  }
193  } else {
194  if (d1 < min_dist) {
195  if (distanceRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv,
196  translation2, callback, min_dist))
197  return true;
198  }
199 
200  if (d2 < min_dist) {
201  if (distanceRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv,
202  translation2, callback, min_dist))
203  return true;
204  }
205  }
206  } else {
207  for (unsigned int i = 0; i < 8; ++i) {
208  if (tree2->nodeChildExists(root2, i)) {
209  const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i);
210  AABB child_bv;
211  computeChildBV(root2_bv, i, child_bv);
212 
213  const AABB& aabb2 = translate(child_bv, translation2);
214  FCL_REAL d = root1->bv.distance(aabb2);
215 
216  if (d < min_dist) {
217  if (distanceRecurse_(nodes1, root1_id, tree2, child, child_bv,
218  translation2, callback, min_dist))
219  return true;
220  }
221  }
222  }
223  }
224 
225  return false;
226 }
227 
228 #endif
229 
230 } // namespace dynamic_AABB_tree_array
231 } // namespace detail
232 } // namespace fcl
233 } // namespace hpp
234 
235 #endif
bool isNodeFree(const OcTreeNode *node) const
whether one node is completely free
Definition: octree.h:127
Simple transform class used locally by InterpMotion.
Definition: transform.h:54
FCL_REAL cost_density
collision cost for unit volume
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
Main namespace.
double getOccupancy() const
FCL_REAL getDefaultOccupancy() const
Definition: octree.h:171
Base callback class for collision queries. This class can be supersed by child classes to provide des...
tuple tf2
static void computeChildBV(const AABB &root_bv, unsigned int i, AABB &child_bv)
compute the bounding volume of an octree node&#39;s i-th child
Definition: octree.h:239
FCL_REAL threshold_occupied
threshold for occupied ( >= is occupied)
const Vec3f & translation() const
get translation
Definition: transform.h:103
FCL_REAL size() const
Size of the AABB (used in BV_Splitter to order two AABBs)
Definition: BV/AABB.h:154
Octree is one type of collision geometry which can encode uncertainty information in the sensor data...
Definition: octree.h:53
double FCL_REAL
Definition: data_types.h:65
Center at zero point, axis aligned box.
OcTreeNode * getNodeChild(OcTreeNode *node, unsigned int childIdx)
Definition: octree.h:180
A class describing the AABB collision structure, which is a box in 3D space determined by two diagona...
Definition: BV/AABB.h:54
static AABB translate(const AABB &aabb, const Vec3f &t)
translate the center of AABB by t
Definition: BV/AABB.h:226
const shared_ptr< const CollisionGeometry > collisionGeometry() const
get geometry from the object instance
FCL_REAL distance(const AABB &other) const
Distance between two AABBs.
Definition: AABB.cpp:114
static Transform3f Identity()
Definition: transform.h:67
the object for collision or distance computation, contains the geometry and the transform information...
HPP_FCL_DLLAPI void constructBox(const AABB &bv, Box &box, Transform3f &tf)
construct a box shape (with a configuration) from a given bounding volume
bool isNodeOccupied(const OcTreeNode *node) const
whether one node is completely occupied
Definition: octree.h:121
Base callback class for distance queries. This class can be supersed by child classes to provide desi...
bool nodeChildExists(const OcTreeNode *node, unsigned int childIdx) const
return true if the child at childIdx exists
Definition: octree.h:199
bool nodeHasChildren(const OcTreeNode *node) const
return true if node has at least one child
Definition: octree.h:208


hpp-fcl
Author(s):
autogenerated on Fri Jun 2 2023 02:39:00