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_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