hpp-fcl 2.4.1
HPP fork of FCL -- The Flexible Collision Library
Loading...
Searching...
No Matches
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
52namespace hpp {
53namespace fcl {
54namespace detail {
55
56namespace dynamic_AABB_tree {
57
58#if HPP_FCL_HAVE_OCTOMAP
59
60//==============================================================================
61template <typename Derived>
62bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
63 const OcTree* tree2, const OcTree::OcTreeNode* root2,
64 const AABB& root2_bv,
65 const Eigen::MatrixBase<Derived>& translation2,
66 CollisionCallBackBase* callback) {
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();
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//==============================================================================
154template <typename Derived>
155bool 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
A class describing the AABB collision structure, which is a box in 3D space determined by two diagona...
Definition: AABB.h:54
Center at zero point, axis aligned box.
Definition: geometric_shapes.h:125
the object for collision or distance computation, contains the geometry and the transform information
Definition: collision_object.h:215
Octree is one type of collision geometry which can encode uncertainty information in the sensor data.
Definition: octree.h:53
bool isNodeOccupied(const OcTreeNode *node) const
whether one node is completely occupied
Definition: octree.h:121
OcTreeNode * getNodeChild(OcTreeNode *node, unsigned int childIdx)
Definition: octree.h:180
octomap::OcTreeNode OcTreeNode
Definition: octree.h:63
bool nodeChildExists(const OcTreeNode *node, unsigned int childIdx) const
return true if the child at childIdx exists
Definition: octree.h:199
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
bool nodeHasChildren(const OcTreeNode *node) const
return true if node has at least one child
Definition: octree.h:208
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
static Transform3f Identity()
Definition: transform.h:67
const Vec3f & translation() const
get translation
Definition: transform.h:103
FCL_REAL distance(const AABB &other) const
Distance between two AABBs.
KDOP< N > translate(const KDOP< N > &bv, const Vec3f &t)
translate the KDOP BV
FCL_REAL size() const
Size of the AABB (used in BV_Splitter to order two AABBs)
Definition: AABB.h:154
const shared_ptr< const CollisionGeometry > collisionGeometry() const
get shared pointer to collision geometry of the object instance
Definition: collision_object.h:304
FCL_REAL cost_density
collision cost for unit volume
Definition: collision_object.h:165
FCL_REAL threshold_occupied
threshold for occupied ( >= is occupied)
Definition: collision_object.h:168
void constructBox(const AABB &bv, Box &box, Transform3f &tf)
construct a box shape (with a configuration) from a given bounding volume
double FCL_REAL
Definition: data_types.h:65
Main namespace.
Definition: broadphase_bruteforce.h:44
Base callback class for collision queries. This class can be supersed by child classes to provide des...
Definition: broadphase_callbacks.h:50
Base callback class for distance queries. This class can be supersed by child classes to provide desi...
Definition: broadphase_callbacks.h:73
dynamic AABB tree node
Definition: node_base.h:50
NodeBase< BV > * children[2]
for leaf node, children nodes
Definition: node_base.h:65
void * data
Definition: node_base.h:66
bool isLeaf() const
whether is a leaf
Definition: node_base-inl.h:55
BV bv
the bounding volume for the node
Definition: node_base.h:52