hpp-fcl  3.0.0
HPP fork of FCL -- The Flexible Collision Library
shape_shape_func.h
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2014, CNRS-LAAS
5  * Copyright (c) 2024, INRIA
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 Willow Garage, Inc. 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 HPP_FCL_INTERNAL_SHAPE_SHAPE_FUNC_H
39 #define HPP_FCL_INTERNAL_SHAPE_SHAPE_FUNC_H
40 
42 
43 #include <hpp/fcl/collision_data.h>
44 #include <hpp/fcl/collision_utility.h>
45 #include <hpp/fcl/narrowphase/narrowphase.h>
46 #include <hpp/fcl/shape/geometric_shapes_traits.h>
47 
48 namespace hpp {
49 namespace fcl {
50 
51 template <typename ShapeType1, typename ShapeType2>
52 struct ShapeShapeDistancer {
53  static FCL_REAL run(const CollisionGeometry* o1, const Transform3f& tf1,
54  const CollisionGeometry* o2, const Transform3f& tf2,
55  const GJKSolver* nsolver, const DistanceRequest& request,
56  DistanceResult& result) {
57  if (request.isSatisfied(result)) return result.min_distance;
58 
59  // Witness points on shape1 and shape2, normal pointing from shape1 to
60  // shape2.
61  Vec3f p1, p2, normal;
62  const FCL_REAL distance = ShapeShapeDistancer<ShapeType1, ShapeType2>::run(
63  o1, tf1, o2, tf2, nsolver, request.enable_signed_distance, p1, p2,
64  normal);
65 
66  result.update(distance, o1, o2, DistanceResult::NONE, DistanceResult::NONE,
67  p1, p2, normal);
68 
69  return distance;
70  }
71 
72  static FCL_REAL run(const CollisionGeometry* o1, const Transform3f& tf1,
73  const CollisionGeometry* o2, const Transform3f& tf2,
74  const GJKSolver* nsolver,
75  const bool compute_signed_distance, Vec3f& p1, Vec3f& p2,
76  Vec3f& normal) {
77  const ShapeType1* obj1 = static_cast<const ShapeType1*>(o1);
78  const ShapeType2* obj2 = static_cast<const ShapeType2*>(o2);
79  return nsolver->shapeDistance(*obj1, tf1, *obj2, tf2,
80  compute_signed_distance, p1, p2, normal);
81  }
82 };
83 
91 template <typename ShapeType1, typename ShapeType2>
92 FCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, const Transform3f& tf1,
93  const CollisionGeometry* o2, const Transform3f& tf2,
94  const GJKSolver* nsolver,
95  const DistanceRequest& request,
96  DistanceResult& result) {
97  return ShapeShapeDistancer<ShapeType1, ShapeType2>::run(
98  o1, tf1, o2, tf2, nsolver, request, result);
99 }
100 
101 namespace internal {
112 template <typename ShapeType1, typename ShapeType2>
113 FCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, const Transform3f& tf1,
114  const CollisionGeometry* o2, const Transform3f& tf2,
115  const GJKSolver* nsolver,
116  const bool compute_signed_distance, Vec3f& p1,
117  Vec3f& p2, Vec3f& normal) {
118  return ::hpp::fcl::ShapeShapeDistancer<ShapeType1, ShapeType2>::run(
119  o1, tf1, o2, tf2, nsolver, compute_signed_distance, p1, p2, normal);
120 }
121 } // namespace internal
122 
132 template <typename ShapeType1, typename ShapeType2>
133 struct ShapeShapeCollider {
134  static std::size_t run(const CollisionGeometry* o1, const Transform3f& tf1,
135  const CollisionGeometry* o2, const Transform3f& tf2,
136  const GJKSolver* nsolver,
137  const CollisionRequest& request,
138  CollisionResult& result) {
139  if (request.isSatisfied(result)) return result.numContacts();
140 
141  const bool compute_penetration =
142  request.enable_contact || (request.security_margin < 0);
143  Vec3f p1, p2, normal;
144  FCL_REAL distance = internal::ShapeShapeDistance<ShapeType1, ShapeType2>(
145  o1, tf1, o2, tf2, nsolver, compute_penetration, p1, p2, normal);
146 
147  size_t num_contacts = 0;
148  const FCL_REAL distToCollision = distance - request.security_margin;
149 
150  internal::updateDistanceLowerBoundFromLeaf(request, result, distToCollision,
151  p1, p2, normal);
152  if (distToCollision <= request.collision_distance_threshold &&
153  result.numContacts() < request.num_max_contacts) {
154  if (result.numContacts() < request.num_max_contacts) {
155  Contact contact(o1, o2, Contact::NONE, Contact::NONE, p1, p2, normal,
156  distance);
157  result.addContact(contact);
158  }
159  num_contacts = result.numContacts();
160  }
161 
162  return num_contacts;
163  }
164 };
165 
166 template <typename ShapeType1, typename ShapeType2>
167 std::size_t ShapeShapeCollide(const CollisionGeometry* o1,
168  const Transform3f& tf1,
169  const CollisionGeometry* o2,
170  const Transform3f& tf2, const GJKSolver* nsolver,
171  const CollisionRequest& request,
172  CollisionResult& result) {
173  return ShapeShapeCollider<ShapeType1, ShapeType2>::run(
174  o1, tf1, o2, tf2, nsolver, request, result);
175 }
176 
177 // clang-format off
178 // ==============================================================================================================
179 // ==============================================================================================================
180 // ==============================================================================================================
181 // Shape distance algorithms based on:
182 // - built-in function: 0
183 // - GJK: 1
184 //
185 // +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
186 // | | box | sphere | capsule | cone | cylinder | plane | half-space | triangle | ellipsoid | convex |
187 // +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
188 // | box | 1 | 0 | 1 | 1 | 1 | 0 | 0 | 1 | 1 | 1 |
189 // +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
190 // | sphere |/////| 0 | 0 | 1 | 0 | 0 | 0 | 0 | 1 | 1 |
191 // +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
192 // | capsule |/////|////////| 0 | 1 | 1 | 0 | 0 | 1 | 1 | 1 |
193 // +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
194 // | cone |/////|////////|/////////| 1 | 1 | 0 | 0 | 1 | 1 | 1 |
195 // +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
196 // | cylinder |/////|////////|/////////|//////| 1 | 0 | 0 | 1 | 1 | 1 |
197 // +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
198 // | plane |/////|////////|/////////|//////|//////////| ? | ? | 0 | 0 | 0 |
199 // +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
200 // | half-space |/////|////////|/////////|//////|//////////|///////| ? | 0 | 0 | 0 |
201 // +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
202 // | triangle |/////|////////|/////////|//////|//////////|///////|////////////| 0 | 1 | 1 |
203 // +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
204 // | ellipsoid |/////|////////|/////////|//////|//////////|///////|////////////|//////////| 1 | 1 |
205 // +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
206 // | convex |/////|////////|/////////|//////|//////////|///////|////////////|//////////|///////////| 1 |
207 // +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
208 //
209 // Number of pairs: 55
210 // - Specialized: 26
211 // - GJK: 29
212 // clang-format on
213 
214 #define SHAPE_SHAPE_DISTANCE_SPECIALIZATION(T1, T2) \
215  template <> \
216  HPP_FCL_DLLAPI FCL_REAL internal::ShapeShapeDistance<T1, T2>( \
217  const CollisionGeometry* o1, const Transform3f& tf1, \
218  const CollisionGeometry* o2, const Transform3f& tf2, \
219  const GJKSolver* nsolver, const bool compute_signed_distance, Vec3f& p1, \
220  Vec3f& p2, Vec3f& normal); \
221  template <> \
222  HPP_FCL_DLLAPI FCL_REAL internal::ShapeShapeDistance<T2, T1>( \
223  const CollisionGeometry* o1, const Transform3f& tf1, \
224  const CollisionGeometry* o2, const Transform3f& tf2, \
225  const GJKSolver* nsolver, const bool compute_signed_distance, Vec3f& p1, \
226  Vec3f& p2, Vec3f& normal); \
227  template <> \
228  inline HPP_FCL_DLLAPI FCL_REAL ShapeShapeDistance<T1, T2>( \
229  const CollisionGeometry* o1, const Transform3f& tf1, \
230  const CollisionGeometry* o2, const Transform3f& tf2, \
231  const GJKSolver* nsolver, const DistanceRequest& request, \
232  DistanceResult& result) { \
233  result.o1 = o1; \
234  result.o2 = o2; \
235  result.b1 = DistanceResult::NONE; \
236  result.b2 = DistanceResult::NONE; \
237  result.min_distance = internal::ShapeShapeDistance<T1, T2>( \
238  o1, tf1, o2, tf2, nsolver, request.enable_signed_distance, \
239  result.nearest_points[0], result.nearest_points[1], result.normal); \
240  return result.min_distance; \
241  } \
242  template <> \
243  inline HPP_FCL_DLLAPI FCL_REAL ShapeShapeDistance<T2, T1>( \
244  const CollisionGeometry* o1, const Transform3f& tf1, \
245  const CollisionGeometry* o2, const Transform3f& tf2, \
246  const GJKSolver* nsolver, const DistanceRequest& request, \
247  DistanceResult& result) { \
248  result.o1 = o1; \
249  result.o2 = o2; \
250  result.b1 = DistanceResult::NONE; \
251  result.b2 = DistanceResult::NONE; \
252  result.min_distance = internal::ShapeShapeDistance<T2, T1>( \
253  o1, tf1, o2, tf2, nsolver, request.enable_signed_distance, \
254  result.nearest_points[0], result.nearest_points[1], result.normal); \
255  return result.min_distance; \
256  }
257 
258 #define SHAPE_SELF_DISTANCE_SPECIALIZATION(T) \
259  template <> \
260  HPP_FCL_DLLAPI FCL_REAL internal::ShapeShapeDistance<T, T>( \
261  const CollisionGeometry* o1, const Transform3f& tf1, \
262  const CollisionGeometry* o2, const Transform3f& tf2, \
263  const GJKSolver* nsolver, const bool compute_signed_distance, Vec3f& p1, \
264  Vec3f& p2, Vec3f& normal); \
265  template <> \
266  inline HPP_FCL_DLLAPI FCL_REAL ShapeShapeDistance<T, T>( \
267  const CollisionGeometry* o1, const Transform3f& tf1, \
268  const CollisionGeometry* o2, const Transform3f& tf2, \
269  const GJKSolver* nsolver, const DistanceRequest& request, \
270  DistanceResult& result) { \
271  result.o1 = o1; \
272  result.o2 = o2; \
273  result.b1 = DistanceResult::NONE; \
274  result.b2 = DistanceResult::NONE; \
275  result.min_distance = internal::ShapeShapeDistance<T, T>( \
276  o1, tf1, o2, tf2, nsolver, request.enable_signed_distance, \
277  result.nearest_points[0], result.nearest_points[1], result.normal); \
278  return result.min_distance; \
279  }
280 
281 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Box, Halfspace)
282 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Box, Plane)
283 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Box, Sphere)
284 SHAPE_SELF_DISTANCE_SPECIALIZATION(Capsule)
285 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Capsule, Halfspace)
286 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Capsule, Plane)
287 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Cone, Halfspace)
288 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Cone, Plane)
289 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Cylinder, Halfspace)
290 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Cylinder, Plane)
291 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Sphere, Halfspace)
292 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Sphere, Plane)
293 SHAPE_SELF_DISTANCE_SPECIALIZATION(Sphere)
294 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Sphere, Cylinder)
295 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Sphere, Capsule)
296 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Ellipsoid, Halfspace)
297 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Ellipsoid, Plane)
298 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(ConvexBase, Halfspace)
299 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(ConvexBase, Plane)
300 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(TriangleP, Halfspace)
301 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(TriangleP, Plane)
302 SHAPE_SELF_DISTANCE_SPECIALIZATION(TriangleP)
303 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(TriangleP, Sphere)
304 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Plane, Halfspace)
305 SHAPE_SELF_DISTANCE_SPECIALIZATION(Plane)
306 SHAPE_SELF_DISTANCE_SPECIALIZATION(Halfspace)
307 
308 #undef SHAPE_SHAPE_DISTANCE_SPECIALIZATION
309 #undef SHAPE_SELF_DISTANCE_SPECIALIZATION
310 
311 } // namespace fcl
312 } // namespace hpp
313 
315 
316 #endif
collision_data.h
collision_utility.h
geometric_shapes_traits.h
hpp::fcl::distance
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.
hpp::fcl::internal::updateDistanceLowerBoundFromLeaf
void updateDistanceLowerBoundFromLeaf(const CollisionRequest &, CollisionResult &res, const FCL_REAL &distance, const Vec3f &p0, const Vec3f &p1, const Vec3f &normal)
Definition: collision_data.h:1186
hpp::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:67
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:66
hpp
Main namespace.
Definition: broadphase_bruteforce.h:44
hpp::fcl::Contact::NONE
static const int NONE
invalid contact primitive information
Definition: collision_data.h:109
hpp::fcl::DistanceResult::NONE
static const int NONE
invalid contact primitive information
Definition: collision_data.h:1088