traversal_recurse.cpp
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-2015, 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 
39 
40 #include <vector>
41 
42 namespace hpp {
43 namespace fcl {
44 void collisionRecurse(CollisionTraversalNodeBase* node, unsigned int b1,
45  unsigned int b2, BVHFrontList* front_list,
46  FCL_REAL& sqrDistLowerBound) {
47  FCL_REAL sqrDistLowerBound1 = 0, sqrDistLowerBound2 = 0;
48  bool l1 = node->isFirstNodeLeaf(b1);
49  bool l2 = node->isSecondNodeLeaf(b2);
50  if (l1 && l2) {
51  updateFrontList(front_list, b1, b2);
52 
53  // if(node->BVDisjoints(b1, b2, sqrDistLowerBound)) return;
54  node->leafCollides(b1, b2, sqrDistLowerBound);
55  return;
56  }
57 
58  if (node->BVDisjoints(b1, b2, sqrDistLowerBound)) {
59  updateFrontList(front_list, b1, b2);
60  return;
61  }
62  if (node->firstOverSecond(b1, b2)) {
63  unsigned int c1 = (unsigned int)node->getFirstLeftChild(b1);
64  unsigned int c2 = (unsigned int)node->getFirstRightChild(b1);
65 
66  collisionRecurse(node, c1, b2, front_list, sqrDistLowerBound1);
67 
68  // early stop is disabled is front_list is used
69  if (node->canStop() && !front_list) return;
70 
71  collisionRecurse(node, c2, b2, front_list, sqrDistLowerBound2);
72  sqrDistLowerBound = std::min(sqrDistLowerBound1, sqrDistLowerBound2);
73  } else {
74  unsigned int c1 = (unsigned int)node->getSecondLeftChild(b2);
75  unsigned int c2 = (unsigned int)node->getSecondRightChild(b2);
76 
77  collisionRecurse(node, b1, c1, front_list, sqrDistLowerBound1);
78 
79  // early stop is disabled is front_list is used
80  if (node->canStop() && !front_list) return;
81 
82  collisionRecurse(node, b1, c2, front_list, sqrDistLowerBound2);
83  sqrDistLowerBound = std::min(sqrDistLowerBound1, sqrDistLowerBound2);
84  }
85 }
86 
87 void collisionNonRecurse(CollisionTraversalNodeBase* node,
88  BVHFrontList* front_list,
89  FCL_REAL& sqrDistLowerBound) {
90  typedef std::pair<unsigned int, unsigned int> BVPair_t;
91  // typedef std::stack<BVPair_t, std::vector<BVPair_t> > Stack_t;
92  typedef std::vector<BVPair_t> Stack_t;
93 
94  Stack_t pairs;
95  pairs.reserve(1000);
96  sqrDistLowerBound = std::numeric_limits<FCL_REAL>::infinity();
97  FCL_REAL sdlb = std::numeric_limits<FCL_REAL>::infinity();
98 
99  pairs.push_back(BVPair_t(0, 0));
100 
101  while (!pairs.empty()) {
102  unsigned int a = pairs.back().first, b = pairs.back().second;
103  pairs.pop_back();
104 
105  bool la = node->isFirstNodeLeaf(a);
106  bool lb = node->isSecondNodeLeaf(b);
107 
108  // Leaf / Leaf case
109  if (la && lb) {
110  updateFrontList(front_list, a, b);
111 
112  // TODO should we test the BVs ?
113  // if(node->BVDijsoints(a, b, sdlb)) {
114  // if (sdlb < sqrDistLowerBound) sqrDistLowerBound = sdlb;
115  // continue;
116  //}
117  node->leafCollides(a, b, sdlb);
118  if (sdlb < sqrDistLowerBound) sqrDistLowerBound = sdlb;
119  if (node->canStop() && !front_list) return;
120  continue;
121  }
122 
123  // TODO shouldn't we test the leaf triangle against BV is la != lb
124  // if (la && !lb) { // leaf triangle 1 against BV 2
125  // } else if (!la && lb) { // BV 1 against leaf triangle 2
126  // }
127 
128  // Check the BV
129  if (node->BVDisjoints(a, b, sdlb)) {
130  if (sdlb < sqrDistLowerBound) sqrDistLowerBound = sdlb;
131  updateFrontList(front_list, a, b);
132  continue;
133  }
134 
135  if (node->firstOverSecond(a, b)) {
136  unsigned int c1 = (unsigned int)node->getFirstLeftChild(a);
137  unsigned int c2 = (unsigned int)node->getFirstRightChild(a);
138  pairs.push_back(BVPair_t(c2, b));
139  pairs.push_back(BVPair_t(c1, b));
140  } else {
141  unsigned int c1 = (unsigned int)node->getSecondLeftChild(b);
142  unsigned int c2 = (unsigned int)node->getSecondRightChild(b);
143  pairs.push_back(BVPair_t(a, c2));
144  pairs.push_back(BVPair_t(a, c1));
145  }
146  }
147 }
148 
153 void distanceRecurse(DistanceTraversalNodeBase* node, unsigned int b1,
154  unsigned int b2, BVHFrontList* front_list) {
155  bool l1 = node->isFirstNodeLeaf(b1);
156  bool l2 = node->isSecondNodeLeaf(b2);
157 
158  if (l1 && l2) {
159  updateFrontList(front_list, b1, b2);
160 
161  node->leafComputeDistance(b1, b2);
162  return;
163  }
164 
165  unsigned int a1, a2, c1, c2;
166 
167  if (node->firstOverSecond(b1, b2)) {
168  a1 = (unsigned int)node->getFirstLeftChild(b1);
169  a2 = b2;
170  c1 = (unsigned int)node->getFirstRightChild(b1);
171  c2 = b2;
172  } else {
173  a1 = b1;
174  a2 = (unsigned int)node->getSecondLeftChild(b2);
175  c1 = b1;
176  c2 = (unsigned int)node->getSecondRightChild(b2);
177  }
178 
179  FCL_REAL d1 = node->BVDistanceLowerBound(a1, a2);
180  FCL_REAL d2 = node->BVDistanceLowerBound(c1, c2);
181 
182  if (d2 < d1) {
183  if (!node->canStop(d2))
184  distanceRecurse(node, c1, c2, front_list);
185  else
186  updateFrontList(front_list, c1, c2);
187 
188  if (!node->canStop(d1))
189  distanceRecurse(node, a1, a2, front_list);
190  else
191  updateFrontList(front_list, a1, a2);
192  } else {
193  if (!node->canStop(d1))
194  distanceRecurse(node, a1, a2, front_list);
195  else
196  updateFrontList(front_list, a1, a2);
197 
198  if (!node->canStop(d2))
199  distanceRecurse(node, c1, c2, front_list);
200  else
201  updateFrontList(front_list, c1, c2);
202  }
203 }
204 
206 struct HPP_FCL_LOCAL BVT {
209 
211  unsigned int b1, b2;
212 };
213 
215 struct HPP_FCL_LOCAL BVT_Comparer {
216  bool operator()(const BVT& lhs, const BVT& rhs) const {
217  return lhs.d > rhs.d;
218  }
219 };
220 
221 struct HPP_FCL_LOCAL BVTQ {
222  BVTQ() : qsize(2) {}
223 
224  bool empty() const { return pq.empty(); }
225 
226  size_t size() const { return pq.size(); }
227 
228  const BVT& top() const { return pq.top(); }
229 
230  void push(const BVT& x) { pq.push(x); }
231 
232  void pop() { pq.pop(); }
233 
234  bool full() const { return (pq.size() + 1 >= qsize); }
235 
236  std::priority_queue<BVT, std::vector<BVT>, BVT_Comparer> pq;
237 
239  unsigned int qsize;
240 };
241 
242 void distanceQueueRecurse(DistanceTraversalNodeBase* node, unsigned int b1,
243  unsigned int b2, BVHFrontList* front_list,
244  unsigned int qsize) {
245  BVTQ bvtq;
246  bvtq.qsize = qsize;
247 
248  BVT min_test;
249  min_test.b1 = b1;
250  min_test.b2 = b2;
251 
252  while (1) {
253  bool l1 = node->isFirstNodeLeaf(min_test.b1);
254  bool l2 = node->isSecondNodeLeaf(min_test.b2);
255 
256  if (l1 && l2) {
257  updateFrontList(front_list, min_test.b1, min_test.b2);
258 
259  node->leafComputeDistance(min_test.b1, min_test.b2);
260  } else if (bvtq.full()) {
261  // queue should not get two more tests, recur
262 
263  distanceQueueRecurse(node, min_test.b1, min_test.b2, front_list, qsize);
264  } else {
265  // queue capacity is not full yet
266  BVT bvt1, bvt2;
267 
268  if (node->firstOverSecond(min_test.b1, min_test.b2)) {
269  unsigned int c1 = (unsigned int)node->getFirstLeftChild(min_test.b1);
270  unsigned int c2 = (unsigned int)node->getFirstRightChild(min_test.b1);
271  bvt1.b1 = c1;
272  bvt1.b2 = min_test.b2;
273  bvt1.d = node->BVDistanceLowerBound(bvt1.b1, bvt1.b2);
274 
275  bvt2.b1 = c2;
276  bvt2.b2 = min_test.b2;
277  bvt2.d = node->BVDistanceLowerBound(bvt2.b1, bvt2.b2);
278  } else {
279  unsigned int c1 = (unsigned int)node->getSecondLeftChild(min_test.b2);
280  unsigned int c2 = (unsigned int)node->getSecondRightChild(min_test.b2);
281  bvt1.b1 = min_test.b1;
282  bvt1.b2 = c1;
283  bvt1.d = node->BVDistanceLowerBound(bvt1.b1, bvt1.b2);
284 
285  bvt2.b1 = min_test.b1;
286  bvt2.b2 = c2;
287  bvt2.d = node->BVDistanceLowerBound(bvt2.b1, bvt2.b2);
288  }
289 
290  bvtq.push(bvt1);
291  bvtq.push(bvt2);
292  }
293 
294  if (bvtq.empty())
295  break;
296  else {
297  min_test = bvtq.top();
298  bvtq.pop();
299 
300  if (node->canStop(min_test.d)) {
301  updateFrontList(front_list, min_test.b1, min_test.b2);
302  break;
303  }
304  }
305  }
306 }
307 
308 void propagateBVHFrontListCollisionRecurse(CollisionTraversalNodeBase* node,
309  const CollisionRequest& /*request*/,
310  CollisionResult& result,
311  BVHFrontList* front_list) {
312  FCL_REAL sqrDistLowerBound = -1, sqrDistLowerBound1 = 0,
313  sqrDistLowerBound2 = 0;
314  BVHFrontList::iterator front_iter;
315  BVHFrontList append;
316  for (front_iter = front_list->begin(); front_iter != front_list->end();
317  ++front_iter) {
318  unsigned int b1 = front_iter->left;
319  unsigned int b2 = front_iter->right;
320  bool l1 = node->isFirstNodeLeaf(b1);
321  bool l2 = node->isSecondNodeLeaf(b2);
322 
323  if (l1 & l2) {
324  front_iter->valid = false; // the front node is no longer valid, in
325  // collideRecurse will add again.
326  collisionRecurse(node, b1, b2, &append, sqrDistLowerBound);
327  } else {
328  if (!node->BVDisjoints(b1, b2, sqrDistLowerBound)) {
329  front_iter->valid = false;
330  if (node->firstOverSecond(b1, b2)) {
331  unsigned int c1 = (unsigned int)node->getFirstLeftChild(b1);
332  unsigned int c2 = (unsigned int)node->getFirstRightChild(b1);
333 
334  collisionRecurse(node, c1, b2, front_list, sqrDistLowerBound1);
335  collisionRecurse(node, c2, b2, front_list, sqrDistLowerBound2);
336  sqrDistLowerBound = std::min(sqrDistLowerBound1, sqrDistLowerBound2);
337  } else {
338  unsigned int c1 = (unsigned int)node->getSecondLeftChild(b2);
339  unsigned int c2 = (unsigned int)node->getSecondRightChild(b2);
340 
341  collisionRecurse(node, b1, c1, front_list, sqrDistLowerBound1);
342  collisionRecurse(node, b1, c2, front_list, sqrDistLowerBound2);
343  sqrDistLowerBound = std::min(sqrDistLowerBound1, sqrDistLowerBound2);
344  }
345  }
346  }
347  result.updateDistanceLowerBound(sqrt(sqrDistLowerBound));
348  }
349 
350  // clean the old front list (remove invalid node)
351  for (front_iter = front_list->begin(); front_iter != front_list->end();) {
352  if (!front_iter->valid)
353  front_iter = front_list->erase(front_iter);
354  else
355  ++front_iter;
356  }
357 
358  for (front_iter = append.begin(); front_iter != append.end(); ++front_iter) {
359  front_list->push_back(*front_iter);
360  }
361 }
362 
363 } // namespace fcl
364 
365 } // namespace hpp
hpp::fcl::BVT::b2
unsigned int b2
Definition: traversal_recurse.cpp:211
hpp::fcl::BVT::b1
unsigned int b1
bv indices for a pair of bvs in two models
Definition: traversal_recurse.cpp:211
hpp::fcl::BVHFrontList
std::list< BVHFrontNode > BVHFrontList
BVH front list is a list of front nodes.
Definition: BVH_front.h:67
geometric_shapes.d1
float d1
Definition: scripts/geometric_shapes.py:31
hpp::fcl::BVTQ::qsize
unsigned int qsize
Queue size.
Definition: traversal_recurse.cpp:239
hpp::fcl::BVT_Comparer
Comparer between two BVT.
Definition: traversal_recurse.cpp:215
hpp::fcl::BVT_Comparer::operator()
bool operator()(const BVT &lhs, const BVT &rhs) const
Definition: traversal_recurse.cpp:216
hpp::fcl::BVTQ::size
size_t size() const
Definition: traversal_recurse.cpp:226
a
list a
hpp::fcl::BVTQ::empty
bool empty() const
Definition: traversal_recurse.cpp:224
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:65
hpp::fcl::distanceRecurse
void distanceRecurse(DistanceTraversalNodeBase *node, unsigned int b1, unsigned int b2, BVHFrontList *front_list)
Definition: traversal_recurse.cpp:153
hpp::fcl::collisionRecurse
void collisionRecurse(CollisionTraversalNodeBase *node, unsigned int b1, unsigned int b2, BVHFrontList *front_list, FCL_REAL &sqrDistLowerBound)
Definition: traversal_recurse.cpp:44
hpp::fcl::BVTQ::pq
std::priority_queue< BVT, std::vector< BVT >, BVT_Comparer > pq
Definition: traversal_recurse.cpp:236
hpp::fcl::BVT
Bounding volume test structure.
Definition: traversal_recurse.cpp:206
hpp
Main namespace.
Definition: broadphase_bruteforce.h:44
hpp::fcl::CollisionRequest
request to the collision algorithm
Definition: collision_data.h:235
hpp::fcl::BVTQ::pop
void pop()
Definition: traversal_recurse.cpp:232
x
x
hpp::fcl::propagateBVHFrontListCollisionRecurse
void propagateBVHFrontListCollisionRecurse(CollisionTraversalNodeBase *node, const CollisionRequest &, CollisionResult &result, BVHFrontList *front_list)
Definition: traversal_recurse.cpp:308
hpp::fcl::CollisionResult
collision result
Definition: collision_data.h:302
l1
list l1
hpp::fcl::BVTQ
Definition: traversal_recurse.cpp:221
generate_distance_plot.b
float b
Definition: generate_distance_plot.py:7
hpp::fcl::BVTQ::full
bool full() const
Definition: traversal_recurse.cpp:234
hpp::fcl::BVT::d
FCL_REAL d
distance between bvs
Definition: traversal_recurse.cpp:208
hpp::fcl::BVTQ::push
void push(const BVT &x)
Definition: traversal_recurse.cpp:230
hpp::fcl::updateFrontList
void updateFrontList(BVHFrontList *front_list, unsigned int b1, unsigned int b2)
Add new front node into the front list.
Definition: BVH_front.h:70
traversal_recurse.h
hpp::fcl::distanceQueueRecurse
void distanceQueueRecurse(DistanceTraversalNodeBase *node, unsigned int b1, unsigned int b2, BVHFrontList *front_list, unsigned int qsize)
Definition: traversal_recurse.cpp:242
hpp::fcl::BVTQ::BVTQ
BVTQ()
Definition: traversal_recurse.cpp:222
hpp::fcl::collisionNonRecurse
void collisionNonRecurse(CollisionTraversalNodeBase *node, BVHFrontList *front_list, FCL_REAL &sqrDistLowerBound)
Definition: traversal_recurse.cpp:87
c2
c2
hpp::fcl::BVTQ::top
const BVT & top() const
Definition: traversal_recurse.cpp:228
l2
l2
hpp::fcl::CollisionResult::updateDistanceLowerBound
void updateDistanceLowerBound(const FCL_REAL &distance_lower_bound_)
Update the lower bound only if the distance is inferior.
Definition: collision_data.h:324


hpp-fcl
Author(s):
autogenerated on Fri Aug 2 2024 02:45:15