coal/internal/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 COAL_INTERNAL_SHAPE_SHAPE_FUNC_H
39 #define COAL_INTERNAL_SHAPE_SHAPE_FUNC_H
40 
42 
43 #include "coal/collision_data.h"
44 #include "coal/collision_utility.h"
45 #include "coal/narrowphase/narrowphase.h"
46 #include "coal/shape/geometric_shapes_traits.h"
47 
48 namespace coal {
49 
50 template <typename ShapeType1, typename ShapeType2>
51 struct ShapeShapeDistancer {
52  static CoalScalar run(const CollisionGeometry* o1, const Transform3s& tf1,
53  const CollisionGeometry* o2, const Transform3s& tf2,
54  const GJKSolver* nsolver,
55  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  Vec3s p1, p2, normal;
62  const CoalScalar distance =
63  ShapeShapeDistancer<ShapeType1, ShapeType2>::run(
64  o1, tf1, o2, tf2, nsolver, request.enable_signed_distance, p1, p2,
65  normal);
66 
67  result.update(distance, o1, o2, DistanceResult::NONE, DistanceResult::NONE,
68  p1, p2, normal);
69 
70  return distance;
71  }
72 
73  static CoalScalar run(const CollisionGeometry* o1, const Transform3s& tf1,
74  const CollisionGeometry* o2, const Transform3s& tf2,
75  const GJKSolver* nsolver,
76  const bool compute_signed_distance, Vec3s& p1,
77  Vec3s& p2, Vec3s& normal) {
78  const ShapeType1* obj1 = static_cast<const ShapeType1*>(o1);
79  const ShapeType2* obj2 = static_cast<const ShapeType2*>(o2);
80  return nsolver->shapeDistance(*obj1, tf1, *obj2, tf2,
81  compute_signed_distance, p1, p2, normal);
82  }
83 };
84 
92 template <typename ShapeType1, typename ShapeType2>
93 CoalScalar ShapeShapeDistance(const CollisionGeometry* o1,
94  const Transform3s& tf1,
95  const CollisionGeometry* o2,
96  const Transform3s& tf2, const GJKSolver* nsolver,
97  const DistanceRequest& request,
98  DistanceResult& result) {
99  return ShapeShapeDistancer<ShapeType1, ShapeType2>::run(
100  o1, tf1, o2, tf2, nsolver, request, result);
101 }
102 
103 namespace internal {
114 template <typename ShapeType1, typename ShapeType2>
115 CoalScalar ShapeShapeDistance(const CollisionGeometry* o1,
116  const Transform3s& tf1,
117  const CollisionGeometry* o2,
118  const Transform3s& tf2, const GJKSolver* nsolver,
119  const bool compute_signed_distance, Vec3s& p1,
120  Vec3s& p2, Vec3s& normal) {
121  return ::coal::ShapeShapeDistancer<ShapeType1, ShapeType2>::run(
122  o1, tf1, o2, tf2, nsolver, compute_signed_distance, p1, p2, normal);
123 }
124 } // namespace internal
125 
135 template <typename ShapeType1, typename ShapeType2>
136 struct ShapeShapeCollider {
137  static std::size_t run(const CollisionGeometry* o1, const Transform3s& tf1,
138  const CollisionGeometry* o2, const Transform3s& tf2,
139  const GJKSolver* nsolver,
140  const CollisionRequest& request,
141  CollisionResult& result) {
142  if (request.isSatisfied(result)) return result.numContacts();
143 
144  const bool compute_penetration =
145  request.enable_contact || (request.security_margin < 0);
146  Vec3s p1, p2, normal;
147  CoalScalar distance = internal::ShapeShapeDistance<ShapeType1, ShapeType2>(
148  o1, tf1, o2, tf2, nsolver, compute_penetration, p1, p2, normal);
149 
150  size_t num_contacts = 0;
151  const CoalScalar distToCollision = distance - request.security_margin;
152 
153  internal::updateDistanceLowerBoundFromLeaf(request, result, distToCollision,
154  p1, p2, normal);
155  if (distToCollision <= request.collision_distance_threshold &&
156  result.numContacts() < request.num_max_contacts) {
157  if (result.numContacts() < request.num_max_contacts) {
158  Contact contact(o1, o2, Contact::NONE, Contact::NONE, p1, p2, normal,
159  distance);
160  result.addContact(contact);
161  }
162  num_contacts = result.numContacts();
163  }
164 
165  return num_contacts;
166  }
167 };
168 
169 template <typename ShapeType1, typename ShapeType2>
170 std::size_t ShapeShapeCollide(const CollisionGeometry* o1,
171  const Transform3s& tf1,
172  const CollisionGeometry* o2,
173  const Transform3s& tf2, const GJKSolver* nsolver,
174  const CollisionRequest& request,
175  CollisionResult& result) {
176  return ShapeShapeCollider<ShapeType1, ShapeType2>::run(
177  o1, tf1, o2, tf2, nsolver, request, result);
178 }
179 
180 // clang-format off
181 // ==============================================================================================================
182 // ==============================================================================================================
183 // ==============================================================================================================
184 // Shape distance algorithms based on:
185 // - built-in function: 0
186 // - GJK: 1
187 //
188 // +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
189 // | | box | sphere | capsule | cone | cylinder | plane | half-space | triangle | ellipsoid | convex |
190 // +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
191 // | box | 1 | 0 | 1 | 1 | 1 | 0 | 0 | 1 | 1 | 1 |
192 // +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
193 // | sphere |/////| 0 | 0 | 1 | 0 | 0 | 0 | 0 | 1 | 1 |
194 // +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
195 // | capsule |/////|////////| 0 | 1 | 1 | 0 | 0 | 1 | 1 | 1 |
196 // +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
197 // | cone |/////|////////|/////////| 1 | 1 | 0 | 0 | 1 | 1 | 1 |
198 // +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
199 // | cylinder |/////|////////|/////////|//////| 1 | 0 | 0 | 1 | 1 | 1 |
200 // +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
201 // | plane |/////|////////|/////////|//////|//////////| ? | ? | 0 | 0 | 0 |
202 // +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
203 // | half-space |/////|////////|/////////|//////|//////////|///////| ? | 0 | 0 | 0 |
204 // +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
205 // | triangle |/////|////////|/////////|//////|//////////|///////|////////////| 0 | 1 | 1 |
206 // +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
207 // | ellipsoid |/////|////////|/////////|//////|//////////|///////|////////////|//////////| 1 | 1 |
208 // +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
209 // | convex |/////|////////|/////////|//////|//////////|///////|////////////|//////////|///////////| 1 |
210 // +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
211 //
212 // Number of pairs: 55
213 // - Specialized: 26
214 // - GJK: 29
215 // clang-format on
216 
217 #define SHAPE_SHAPE_DISTANCE_SPECIALIZATION(T1, T2) \
218  template <> \
219  COAL_DLLAPI CoalScalar internal::ShapeShapeDistance<T1, T2>( \
220  const CollisionGeometry* o1, const Transform3s& tf1, \
221  const CollisionGeometry* o2, const Transform3s& tf2, \
222  const GJKSolver* nsolver, const bool compute_signed_distance, Vec3s& p1, \
223  Vec3s& p2, Vec3s& normal); \
224  template <> \
225  COAL_DLLAPI CoalScalar internal::ShapeShapeDistance<T2, T1>( \
226  const CollisionGeometry* o1, const Transform3s& tf1, \
227  const CollisionGeometry* o2, const Transform3s& tf2, \
228  const GJKSolver* nsolver, const bool compute_signed_distance, Vec3s& p1, \
229  Vec3s& p2, Vec3s& normal); \
230  template <> \
231  inline COAL_DLLAPI CoalScalar ShapeShapeDistance<T1, T2>( \
232  const CollisionGeometry* o1, const Transform3s& tf1, \
233  const CollisionGeometry* o2, const Transform3s& tf2, \
234  const GJKSolver* nsolver, const DistanceRequest& request, \
235  DistanceResult& result) { \
236  result.o1 = o1; \
237  result.o2 = o2; \
238  result.b1 = DistanceResult::NONE; \
239  result.b2 = DistanceResult::NONE; \
240  result.min_distance = internal::ShapeShapeDistance<T1, T2>( \
241  o1, tf1, o2, tf2, nsolver, request.enable_signed_distance, \
242  result.nearest_points[0], result.nearest_points[1], result.normal); \
243  return result.min_distance; \
244  } \
245  template <> \
246  inline COAL_DLLAPI CoalScalar ShapeShapeDistance<T2, T1>( \
247  const CollisionGeometry* o1, const Transform3s& tf1, \
248  const CollisionGeometry* o2, const Transform3s& tf2, \
249  const GJKSolver* nsolver, const DistanceRequest& request, \
250  DistanceResult& result) { \
251  result.o1 = o1; \
252  result.o2 = o2; \
253  result.b1 = DistanceResult::NONE; \
254  result.b2 = DistanceResult::NONE; \
255  result.min_distance = internal::ShapeShapeDistance<T2, T1>( \
256  o1, tf1, o2, tf2, nsolver, request.enable_signed_distance, \
257  result.nearest_points[0], result.nearest_points[1], result.normal); \
258  return result.min_distance; \
259  }
260 
261 #define SHAPE_SELF_DISTANCE_SPECIALIZATION(T) \
262  template <> \
263  COAL_DLLAPI CoalScalar internal::ShapeShapeDistance<T, T>( \
264  const CollisionGeometry* o1, const Transform3s& tf1, \
265  const CollisionGeometry* o2, const Transform3s& tf2, \
266  const GJKSolver* nsolver, const bool compute_signed_distance, Vec3s& p1, \
267  Vec3s& p2, Vec3s& normal); \
268  template <> \
269  inline COAL_DLLAPI CoalScalar ShapeShapeDistance<T, T>( \
270  const CollisionGeometry* o1, const Transform3s& tf1, \
271  const CollisionGeometry* o2, const Transform3s& tf2, \
272  const GJKSolver* nsolver, const DistanceRequest& request, \
273  DistanceResult& result) { \
274  result.o1 = o1; \
275  result.o2 = o2; \
276  result.b1 = DistanceResult::NONE; \
277  result.b2 = DistanceResult::NONE; \
278  result.min_distance = internal::ShapeShapeDistance<T, T>( \
279  o1, tf1, o2, tf2, nsolver, request.enable_signed_distance, \
280  result.nearest_points[0], result.nearest_points[1], result.normal); \
281  return result.min_distance; \
282  }
283 
284 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Box, Halfspace)
285 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Box, Plane)
286 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Box, Sphere)
287 SHAPE_SELF_DISTANCE_SPECIALIZATION(Capsule)
288 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Capsule, Halfspace)
289 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Capsule, Plane)
290 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Cone, Halfspace)
291 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Cone, Plane)
292 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Cylinder, Halfspace)
293 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Cylinder, Plane)
294 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Sphere, Halfspace)
295 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Sphere, Plane)
296 SHAPE_SELF_DISTANCE_SPECIALIZATION(Sphere)
297 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Sphere, Cylinder)
298 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Sphere, Capsule)
299 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Ellipsoid, Halfspace)
300 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Ellipsoid, Plane)
301 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(ConvexBase, Halfspace)
302 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(ConvexBase, Plane)
303 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(TriangleP, Halfspace)
304 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(TriangleP, Plane)
305 SHAPE_SELF_DISTANCE_SPECIALIZATION(TriangleP)
306 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(TriangleP, Sphere)
307 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Plane, Halfspace)
308 SHAPE_SELF_DISTANCE_SPECIALIZATION(Plane)
309 SHAPE_SELF_DISTANCE_SPECIALIZATION(Halfspace)
310 
311 #undef SHAPE_SHAPE_DISTANCE_SPECIALIZATION
312 #undef SHAPE_SELF_DISTANCE_SPECIALIZATION
313 
314 } // namespace coal
315 
317 
318 #endif
coal::Vec3s
Eigen::Matrix< CoalScalar, 3, 1 > Vec3s
Definition: coal/data_types.h:77
coal::internal::updateDistanceLowerBoundFromLeaf
void updateDistanceLowerBoundFromLeaf(const CollisionRequest &, CollisionResult &res, const CoalScalar &distance, const Vec3s &p0, const Vec3s &p1, const Vec3s &normal)
Definition: coal/collision_data.h:1184
gjk.tf1
tuple tf1
Definition: test/scripts/gjk.py:27
collision_data.h
coal
Main namespace.
Definition: coal/broadphase/broadphase_bruteforce.h:44
narrowphase.h
coal::distance
COAL_DLLAPI CoalScalar distance(const Matrix3s &R0, const Vec3s &T0, const kIOS &b1, const kIOS &b2, Vec3s *P=NULL, Vec3s *Q=NULL)
Approximate distance between two kIOS bounding volumes.
Definition: kIOS.cpp:180
coal::DistanceResult::NONE
static const int NONE
invalid contact primitive information
Definition: coal/collision_data.h:1086
geometric_shapes_traits.h
gjk.tf2
tuple tf2
Definition: test/scripts/gjk.py:36
omniidl_be_python_with_docstring.run
def run(tree, args)
Definition: omniidl_be_python_with_docstring.py:140
octree.p1
tuple p1
Definition: octree.py:54
collision_utility.h
coal::CoalScalar
double CoalScalar
Definition: coal/data_types.h:76
coal::Contact::NONE
static const int NONE
invalid contact primitive information
Definition: coal/collision_data.h:108

hpp-fcl
Author(s):
autogenerated on Sat Nov 23 2024 03:44:59