shape_collision_traversal_node-inl.h
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-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 
38 #ifndef FCL_TRAVERSAL_SHAPECOLLISIONTRAVERSALNODE_INL_H
39 #define FCL_TRAVERSAL_SHAPECOLLISIONTRAVERSALNODE_INL_H
40 
42 
43 namespace fcl
44 {
45 
46 namespace detail
47 {
48 
49 //==============================================================================
50 template <typename Shape1, typename Shape2, typename NarrowPhaseSolver>
53  : CollisionTraversalNodeBase<typename Shape1::S>()
54 {
55  model1 = nullptr;
56  model2 = nullptr;
57 
58  nsolver = nullptr;
59 }
60 
61 //==============================================================================
62 template <typename Shape1, typename Shape2, typename NarrowPhaseSolver>
64 BVTesting(int, int) const
65 {
66  return false;
67 }
68 
69 //==============================================================================
70 template <typename Shape1, typename Shape2, typename NarrowPhaseSolver>
72 leafTesting(int, int) const
73 {
74  if(model1->isOccupied() && model2->isOccupied())
75  {
76  bool is_collision = false;
77  if(this->request.enable_contact)
78  {
79  std::vector<ContactPoint<S>> contacts;
80  if(nsolver->shapeIntersect(*model1, this->tf1, *model2, this->tf2, &contacts))
81  {
82  is_collision = true;
83  if(this->request.num_max_contacts > this->result->numContacts())
84  {
85  const size_t free_space = this->request.num_max_contacts - this->result->numContacts();
86  size_t num_adding_contacts;
87 
88  // If the free space is not enough to add all the new contacts, we add contacts in descent order of penetration depth.
89  if (free_space < contacts.size())
90  {
91  std::partial_sort(contacts.begin(), contacts.begin() + free_space, contacts.end(), std::bind(comparePenDepth<S>, std::placeholders::_2, std::placeholders::_1));
92  num_adding_contacts = free_space;
93  }
94  else
95  {
96  num_adding_contacts = contacts.size();
97  }
98 
99  for(size_t i = 0; i < num_adding_contacts; ++i)
100  this->result->addContact(Contact<S>(model1, model2, Contact<S>::NONE, Contact<S>::NONE, contacts[i].pos, contacts[i].normal, contacts[i].penetration_depth));
101  }
102  }
103  }
104  else
105  {
106  if(nsolver->shapeIntersect(*model1, this->tf1, *model2, this->tf2, nullptr))
107  {
108  is_collision = true;
109  if(this->request.num_max_contacts > this->result->numContacts())
110  this->result->addContact(Contact<S>(model1, model2, Contact<S>::NONE, Contact<S>::NONE));
111  }
112  }
113 
114  if(is_collision && this->request.enable_cost)
115  {
116  AABB<S> aabb1, aabb2;
117  computeBV(*model1, this->tf1, aabb1);
118  computeBV(*model2, this->tf2, aabb2);
119  AABB<S> overlap_part;
120  aabb1.overlap(aabb2, overlap_part);
121  this->result->addCostSource(CostSource<S>(overlap_part, cost_density), this->request.num_max_cost_sources);
122  }
123  }
124  else if((!model1->isFree() && !model2->isFree()) && this->request.enable_cost)
125  {
126  if(nsolver->shapeIntersect(*model1, this->tf1, *model2, this->tf2, nullptr))
127  {
128  AABB<S> aabb1, aabb2;
129  computeBV(*model1, this->tf1, aabb1);
130  computeBV(*model2, this->tf2, aabb2);
131  AABB<S> overlap_part;
132  aabb1.overlap(aabb2, overlap_part);
133  this->result->addCostSource(CostSource<S>(overlap_part, cost_density), this->request.num_max_cost_sources);
134  }
135  }
136 }
137 
138 //==============================================================================
139 template <typename Shape1, typename Shape2, typename NarrowPhaseSolver>
142  const Shape1& shape1,
144  const Shape2& shape2,
146  const NarrowPhaseSolver* nsolver,
149 {
150  node.model1 = &shape1;
151  node.tf1 = tf1;
152  node.model2 = &shape2;
153  node.tf2 = tf2;
154  node.nsolver = nsolver;
155 
156  node.request = request;
157  node.result = &result;
158 
159  node.cost_density = shape1.cost_density * shape2.cost_density;
160 
161  return true;
162 }
163 
164 } // namespace detail
165 } // namespace fcl
166 
167 #endif
fcl::detail::ShapeCollisionTraversalNode::S
typename Shape1::S S
Definition: shape_collision_traversal_node.h:58
fcl::Transform3
Eigen::Transform< S, 3, Eigen::Isometry > Transform3
Definition: types.h:91
fcl::detail::ShapeCollisionTraversalNode::model2
const Shape2 * model2
Definition: shape_collision_traversal_node.h:69
fcl::detail::ShapeCollisionTraversalNode::BVTesting
bool BVTesting(int, int) const
BV culling test in one BVTT node.
Definition: shape_collision_traversal_node-inl.h:64
fcl::detail::ShapeCollisionTraversalNode
Traversal node for collision between two shapes.
Definition: shape_collision_traversal_node.h:53
fcl::detail::ShapeCollisionTraversalNode::nsolver
const NarrowPhaseSolver * nsolver
Definition: shape_collision_traversal_node.h:73
fcl::Contact
Contact information returned by collision.
Definition: contact.h:48
fcl::detail::CollisionTraversalNodeBase< Shape1::S >::request
CollisionRequest< Shape1::S > request
request setting for collision
Definition: collision_traversal_node_base.h:72
fcl::CostSource
Cost source describes an area with a cost. The area is described by an AABB region.
Definition: cost_source.h:49
fcl::detail::TraversalNodeBase< Shape1::S >::tf1
Transform3< Shape1::S > tf1
configuation of first object
Definition: traversal_node_base.h:85
fcl::computeBV
FCL_EXPORT void computeBV(const Shape &s, const Transform3< typename BV::S > &tf, BV &bv)
calculate a bounding volume for a shape in a specific configuration
Definition: geometry/shape/utility-inl.h:1056
fcl::AABB< S >
fcl::CollisionResult
collision result
Definition: collision_request.h:48
fcl::detail::ShapeCollisionTraversalNode::model1
const Shape1 * model1
Definition: shape_collision_traversal_node.h:68
fcl::Contact::pos
Vector3< S > pos
contact position, in world space
Definition: contact.h:74
fcl::CollisionRequest
Parameters for performing collision request.
Definition: collision_request.h:52
fcl::detail::initialize
template bool initialize(MeshCollisionTraversalNodeOBB< double > &node, const BVHModel< OBB< double >> &model1, const Transform3< double > &tf1, const BVHModel< OBB< double >> &model2, const Transform3< double > &tf2, const CollisionRequest< double > &request, CollisionResult< double > &result)
fcl::detail::CollisionTraversalNodeBase
Node structure encoding the information required for collision traversal.
Definition: collision_traversal_node_base.h:52
fcl::detail::CollisionTraversalNodeBase< Shape1::S >::result
CollisionResult< Shape1::S > * result
collision result kept during the traversal iteration
Definition: collision_traversal_node_base.h:75
fcl::detail::ShapeCollisionTraversalNode::ShapeCollisionTraversalNode
ShapeCollisionTraversalNode()
Definition: shape_collision_traversal_node-inl.h:52
shape_collision_traversal_node.h
fcl::AABB::overlap
bool overlap(const AABB< S > &other) const
Check whether two AABB are overlap.
Definition: AABB-inl.h:98
fcl::detail::TraversalNodeBase< Shape1::S >::tf2
Transform3< Shape1::S > tf2
configuration of second object
Definition: traversal_node_base.h:88
fcl::detail::ShapeCollisionTraversalNode::cost_density
S cost_density
Definition: shape_collision_traversal_node.h:71
fcl::detail::ShapeCollisionTraversalNode::leafTesting
void leafTesting(int, int) const
Intersection testing between leaves (two shapes)
Definition: shape_collision_traversal_node-inl.h:72
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45


fcl
Author(s):
autogenerated on Tue Dec 5 2023 03:40:48