collision_node.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 
38 #include <../src/collision_node.h>
40 
41 namespace hpp {
42 namespace fcl {
43 
44 void collide(CollisionTraversalNodeBase* node, const CollisionRequest& request,
45  CollisionResult& result, BVHFrontList* front_list,
46  bool recursive) {
47  if (front_list && front_list->size() > 0) {
48  propagateBVHFrontListCollisionRecurse(node, request, result, front_list);
49  } else {
50  FCL_REAL sqrDistLowerBound = 0;
51  if (recursive)
52  collisionRecurse(node, 0, 0, front_list, sqrDistLowerBound);
53  else
54  collisionNonRecurse(node, front_list, sqrDistLowerBound);
55 
56  if (!std::isnan(sqrDistLowerBound)) {
57  if (sqrDistLowerBound == 0) {
58  assert(result.distance_lower_bound <= 0);
59  } else {
60  assert(result.distance_lower_bound * result.distance_lower_bound -
61  sqrDistLowerBound <
62  1e-8);
63  }
64  }
65  }
66 }
67 
68 void distance(DistanceTraversalNodeBase* node, BVHFrontList* front_list,
69  unsigned int qsize) {
70  node->preprocess();
71 
72  if (qsize <= 2)
73  distanceRecurse(node, 0, 0, front_list);
74  else
75  distanceQueueRecurse(node, 0, 0, front_list, qsize);
76 
77  node->postprocess();
78 }
79 
80 } // namespace fcl
81 
82 } // namespace hpp
hpp::fcl::BVHFrontList
std::list< BVHFrontNode > BVHFrontList
BVH front list is a list of front nodes.
Definition: BVH_front.h:67
hpp::fcl::distance
HPP_FCL_DLLAPI FCL_REAL distance(const Matrix3f &R0, const Vec3f &T0, const kIOS &b1, const kIOS &b2, Vec3f *P=NULL, Vec3f *Q=NULL)
Approximate distance between two kIOS bounding volumes.
Definition: kIOS.cpp:181
hpp::fcl::collide
HPP_FCL_DLLAPI std::size_t collide(const CollisionObject *o1, const CollisionObject *o2, const CollisionRequest &request, CollisionResult &result)
Main collision interface: given two collision objects, and the requirements for contacts,...
Definition: src/collision.cpp:63
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
Main namespace.
Definition: broadphase_bruteforce.h:44
hpp::fcl::CollisionRequest
request to the collision algorithm
Definition: collision_data.h:235
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
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::collisionNonRecurse
void collisionNonRecurse(CollisionTraversalNodeBase *node, BVHFrontList *front_list, FCL_REAL &sqrDistLowerBound)
Definition: traversal_recurse.cpp:87
hpp::fcl::CollisionResult::distance_lower_bound
FCL_REAL distance_lower_bound
Definition: collision_data.h:312


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