GCC Code Coverage Report


Directory: ./
File: include/coal/broadphase/broadphase_dynamic_AABB_tree_array-inl.h
Date: 2025-04-01 09:23:31
Exec Total Coverage
Lines: 0 104 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_ARRAY_INL_H
39 #define COAL_BROAD_PHASE_DYNAMIC_AABB_TREE_ARRAY_INL_H
40
41 #include "coal/broadphase/broadphase_dynamic_AABB_tree_array.h"
42 #include "coal/shape/geometric_shapes_utility.h"
43
44 #if COAL_HAVE_OCTOMAP
45 #include "coal/octree.h"
46 #endif
47
48 namespace coal {
49 namespace detail {
50 namespace dynamic_AABB_tree_array {
51
52 #if COAL_HAVE_OCTOMAP
53
54 //==============================================================================
55 template <typename Derived>
56 bool collisionRecurse_(
57 DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes1,
58 size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2,
59 const AABB& root2_bv, const Eigen::MatrixBase<Derived>& translation2,
60 CollisionCallBackBase* callback) {
61 DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root1 =
62 nodes1 + root1_id;
63 if (!root2) {
64 if (root1->isLeaf()) {
65 CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data);
66
67 if (!obj1->collisionGeometry()->isFree()) {
68 const AABB& root_bv_t = translate(root2_bv, translation2);
69 if (root1->bv.overlap(root_bv_t)) {
70 Box* box = new Box();
71 Transform3s box_tf;
72 Transform3s tf2 = Transform3s::Identity();
73 tf2.translation() = translation2;
74 constructBox(root2_bv, tf2, *box, box_tf);
75
76 box->cost_density = tree2->getDefaultOccupancy();
77
78 CollisionObject obj2(shared_ptr<CollisionGeometry>(box), box_tf);
79 return (*callback)(obj1, &obj2);
80 }
81 }
82 } else {
83 if (collisionRecurse_(nodes1, root1->children[0], tree2, nullptr,
84 root2_bv, translation2, callback))
85 return true;
86 if (collisionRecurse_(nodes1, root1->children[1], tree2, nullptr,
87 root2_bv, translation2, callback))
88 return true;
89 }
90
91 return false;
92 } else if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) {
93 CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data);
94 if (!tree2->isNodeFree(root2) && !obj1->collisionGeometry()->isFree()) {
95 const AABB& root_bv_t = translate(root2_bv, translation2);
96 if (root1->bv.overlap(root_bv_t)) {
97 Box* box = new Box();
98 Transform3s box_tf;
99 Transform3s tf2 = Transform3s::Identity();
100 tf2.translation() = translation2;
101 constructBox(root2_bv, tf2, *box, box_tf);
102
103 box->cost_density = Scalar(root2->getOccupancy());
104 box->threshold_occupied = tree2->getOccupancyThres();
105
106 CollisionObject obj2(shared_ptr<CollisionGeometry>(box), box_tf);
107 return (*callback)(obj1, &obj2);
108 } else
109 return false;
110 } else
111 return false;
112 }
113
114 const AABB& root_bv_t = translate(root2_bv, translation2);
115 if (tree2->isNodeFree(root2) || !root1->bv.overlap(root_bv_t)) return false;
116
117 if (!tree2->nodeHasChildren(root2) ||
118 (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) {
119 if (collisionRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv,
120 translation2, callback))
121 return true;
122 if (collisionRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv,
123 translation2, callback))
124 return true;
125 } else {
126 for (unsigned int i = 0; i < 8; ++i) {
127 if (tree2->nodeChildExists(root2, i)) {
128 const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i);
129 AABB child_bv;
130 computeChildBV(root2_bv, i, child_bv);
131
132 if (collisionRecurse_(nodes1, root1_id, tree2, child, child_bv,
133 translation2, callback))
134 return true;
135 } else {
136 AABB child_bv;
137 computeChildBV(root2_bv, i, child_bv);
138 if (collisionRecurse_(nodes1, root1_id, tree2, nullptr, child_bv,
139 translation2, callback))
140 return true;
141 }
142 }
143 }
144
145 return false;
146 }
147
148 //==============================================================================
149 template <typename Derived>
150 bool distanceRecurse_(
151 DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes1,
152 size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2,
153 const AABB& root2_bv, const Eigen::MatrixBase<Derived>& translation2,
154 DistanceCallBackBase* callback, Scalar& min_dist) {
155 DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root1 =
156 nodes1 + root1_id;
157 if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) {
158 if (tree2->isNodeOccupied(root2)) {
159 Box* box = new Box();
160 Transform3s box_tf;
161 Transform3s tf2 = Transform3s::Identity();
162 tf2.translation() = translation2;
163 constructBox(root2_bv, tf2, *box, box_tf);
164 CollisionObject obj(shared_ptr<CollisionGeometry>(box), box_tf);
165 return (*callback)(static_cast<CollisionObject*>(root1->data), &obj,
166 min_dist);
167 } else
168 return false;
169 }
170
171 if (!tree2->isNodeOccupied(root2)) return false;
172
173 if (!tree2->nodeHasChildren(root2) ||
174 (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) {
175 const AABB& aabb2 = translate(root2_bv, translation2);
176
177 Scalar d1 = aabb2.distance((nodes1 + root1->children[0])->bv);
178 Scalar d2 = aabb2.distance((nodes1 + root1->children[1])->bv);
179
180 if (d2 < d1) {
181 if (d2 < min_dist) {
182 if (distanceRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv,
183 translation2, callback, min_dist))
184 return true;
185 }
186
187 if (d1 < min_dist) {
188 if (distanceRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv,
189 translation2, callback, min_dist))
190 return true;
191 }
192 } else {
193 if (d1 < min_dist) {
194 if (distanceRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv,
195 translation2, callback, min_dist))
196 return true;
197 }
198
199 if (d2 < min_dist) {
200 if (distanceRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv,
201 translation2, callback, min_dist))
202 return true;
203 }
204 }
205 } else {
206 for (unsigned int i = 0; i < 8; ++i) {
207 if (tree2->nodeChildExists(root2, i)) {
208 const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i);
209 AABB child_bv;
210 computeChildBV(root2_bv, i, child_bv);
211
212 const AABB& aabb2 = translate(child_bv, translation2);
213 Scalar d = root1->bv.distance(aabb2);
214
215 if (d < min_dist) {
216 if (distanceRecurse_(nodes1, root1_id, tree2, child, child_bv,
217 translation2, callback, min_dist))
218 return true;
219 }
220 }
221 }
222 }
223
224 return false;
225 }
226
227 #endif
228
229 } // namespace dynamic_AABB_tree_array
230 } // namespace detail
231 } // namespace coal
232
233 #endif
234