hpp-fcl 2.4.1
HPP fork of FCL -- The Flexible Collision Library
Loading...
Searching...
No Matches
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
48namespace hpp {
49namespace fcl {
50namespace detail {
51namespace dynamic_AABB_tree_array {
52
53#if HPP_FCL_HAVE_OCTOMAP
54
55//==============================================================================
56template <typename Derived>
57bool collisionRecurse_(
59 size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2,
60 const AABB& root2_bv, const Eigen::MatrixBase<Derived>& translation2,
61 CollisionCallBackBase* callback) {
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();
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//==============================================================================
150template <typename Derived>
151bool 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
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
FCL_REAL getDefaultOccupancy() const
Definition: octree.h:171
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
Definition: node_base_array.h:51
void * data
Definition: node_base_array.h:61
size_t children[2]
Definition: node_base_array.h:60
BV bv
Definition: node_base_array.h:52
bool isLeaf() const
Definition: node_base_array-inl.h:52