GCC Code Coverage Report

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