GCC Code Coverage Report


Directory: ./
File: include/coal/broadphase/broadphase_dynamic_AABB_tree-inl.h
Date: 2025-04-01 09:23:31
Exec Total Coverage
Lines: 0 101 0.0%
Branches: 0 230 0.0%

Line Branch Exec Source
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
36 /** @author Jia Pan */
37
38 #ifndef COAL_BROAD_PHASE_DYNAMIC_AABB_TREE_INL_H
39 #define COAL_BROAD_PHASE_DYNAMIC_AABB_TREE_INL_H
40
41 #include "coal/broadphase/broadphase_dynamic_AABB_tree.h"
42
43 #include <limits>
44
45 #if COAL_HAVE_OCTOMAP
46 #include "coal/octree.h"
47 #endif
48
49 #include "coal/BV/BV.h"
50 #include "coal/shape/geometric_shapes_utility.h"
51
52 namespace coal {
53 namespace detail {
54
55 namespace dynamic_AABB_tree {
56
57 #if COAL_HAVE_OCTOMAP
58
59 //==============================================================================
60 template <typename Derived>
61 bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
62 const OcTree* tree2, const OcTree::OcTreeNode* root2,
63 const AABB& root2_bv,
64 const Eigen::MatrixBase<Derived>& translation2,
65 CollisionCallBackBase* callback) {
66 if (!root2) {
67 if (root1->isLeaf()) {
68 CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data);
69
70 if (!obj1->collisionGeometry()->isFree()) {
71 const AABB& root2_bv_t = translate(root2_bv, translation2);
72 if (root1->bv.overlap(root2_bv_t)) {
73 Box* box = new Box();
74 Transform3s box_tf;
75 Transform3s tf2 = Transform3s::Identity();
76 tf2.translation() = translation2;
77 constructBox(root2_bv, tf2, *box, box_tf);
78
79 box->cost_density =
80 tree2->getOccupancyThres(); // thresholds are 0, 1, so uncertain
81
82 CollisionObject obj2(shared_ptr<CollisionGeometry>(box), box_tf);
83 return (*callback)(obj1, &obj2);
84 }
85 }
86 } else {
87 if (collisionRecurse_(root1->children[0], tree2, nullptr, root2_bv,
88 translation2, callback))
89 return true;
90 if (collisionRecurse_(root1->children[1], tree2, nullptr, root2_bv,
91 translation2, callback))
92 return true;
93 }
94
95 return false;
96 } else if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) {
97 CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data);
98
99 if (!tree2->isNodeFree(root2) && !obj1->collisionGeometry()->isFree()) {
100 const AABB& root2_bv_t = translate(root2_bv, translation2);
101 if (root1->bv.overlap(root2_bv_t)) {
102 Box* box = new Box();
103 Transform3s box_tf;
104 Transform3s tf2 = Transform3s::Identity();
105 tf2.translation() = translation2;
106 constructBox(root2_bv, tf2, *box, box_tf);
107
108 box->cost_density = Scalar(root2->getOccupancy());
109 box->threshold_occupied = tree2->getOccupancyThres();
110
111 CollisionObject obj2(shared_ptr<CollisionGeometry>(box), box_tf);
112 return (*callback)(obj1, &obj2);
113 } else
114 return false;
115 } else
116 return false;
117 }
118
119 const AABB& root2_bv_t = translate(root2_bv, translation2);
120 if (tree2->isNodeFree(root2) || !root1->bv.overlap(root2_bv_t)) return false;
121
122 if (!tree2->nodeHasChildren(root2) ||
123 (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) {
124 if (collisionRecurse_(root1->children[0], tree2, root2, root2_bv,
125 translation2, callback))
126 return true;
127 if (collisionRecurse_(root1->children[1], tree2, root2, root2_bv,
128 translation2, callback))
129 return true;
130 } else {
131 for (unsigned int i = 0; i < 8; ++i) {
132 if (tree2->nodeChildExists(root2, i)) {
133 const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i);
134 AABB child_bv;
135 computeChildBV(root2_bv, i, child_bv);
136
137 if (collisionRecurse_(root1, tree2, child, child_bv, translation2,
138 callback))
139 return true;
140 } else {
141 AABB child_bv;
142 computeChildBV(root2_bv, i, child_bv);
143 if (collisionRecurse_(root1, tree2, nullptr, child_bv, translation2,
144 callback))
145 return true;
146 }
147 }
148 }
149 return false;
150 }
151
152 //==============================================================================
153 template <typename Derived>
154 bool distanceRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
155 const OcTree* tree2, const OcTree::OcTreeNode* root2,
156 const AABB& root2_bv,
157 const Eigen::MatrixBase<Derived>& translation2,
158 DistanceCallBackBase* callback, Scalar& min_dist) {
159 if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) {
160 if (tree2->isNodeOccupied(root2)) {
161 Box* box = new Box();
162 Transform3s box_tf;
163 Transform3s tf2 = Transform3s::Identity();
164 tf2.translation() = translation2;
165 constructBox(root2_bv, tf2, *box, box_tf);
166 CollisionObject obj(shared_ptr<CollisionGeometry>(box), box_tf);
167 return (*callback)(static_cast<CollisionObject*>(root1->data), &obj,
168 min_dist);
169 } else
170 return false;
171 }
172
173 if (!tree2->isNodeOccupied(root2)) return false;
174
175 if (!tree2->nodeHasChildren(root2) ||
176 (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) {
177 const AABB& aabb2 = translate(root2_bv, translation2);
178 Scalar d1 = aabb2.distance(root1->children[0]->bv);
179 Scalar d2 = aabb2.distance(root1->children[1]->bv);
180
181 if (d2 < d1) {
182 if (d2 < min_dist) {
183 if (distanceRecurse_(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_(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_(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_(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 const AABB& aabb2 = translate(child_bv, translation2);
213
214 Scalar d = root1->bv.distance(aabb2);
215
216 if (d < min_dist) {
217 if (distanceRecurse_(root1, tree2, child, child_bv, translation2,
218 callback, min_dist))
219 return true;
220 }
221 }
222 }
223 }
224
225 return false;
226 }
227
228 #endif
229
230 } // namespace dynamic_AABB_tree
231 } // namespace detail
232 } // namespace coal
233
234 #endif
235