38#ifndef COAL_INTERNAL_SHAPE_SHAPE_FUNC_H
39#define COAL_INTERNAL_SHAPE_SHAPE_FUNC_H
52template <
typename ShapeType1,
typename ShapeType2>
53struct ShapeShapeDistancer {
54 static Scalar run(
const CollisionGeometry* o1,
const Transform3s& tf1,
55 const CollisionGeometry* o2,
const Transform3s& tf2,
56 const GJKSolver* nsolver,
const DistanceRequest& request,
57 DistanceResult& result) {
58 if (request.isSatisfied(result))
return result.min_distance;
63 const Scalar distance = ShapeShapeDistancer<ShapeType1, ShapeType2>::run(
64 o1, tf1, o2, tf2, nsolver, request.enable_signed_distance, p1, p2,
67 result.update(distance, o1, o2, DistanceResult::NONE, DistanceResult::NONE,
73 static Scalar 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, Vec3s& p2,
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);
92template <
typename ShapeType1,
typename ShapeType2>
93Scalar ShapeShapeDistance(
const CollisionGeometry* o1,
const Transform3s& tf1,
94 const CollisionGeometry* o2,
const Transform3s& tf2,
95 const GJKSolver* nsolver,
96 const DistanceRequest& request,
97 DistanceResult& result) {
99 return ShapeShapeDistancer<ShapeType1, ShapeType2>::run(
100 o1, tf1, o2, tf2, nsolver, request, result);
114template <
typename ShapeType1,
typename ShapeType2>
115Scalar ShapeShapeDistance(
const CollisionGeometry* o1,
const Transform3s& tf1,
116 const CollisionGeometry* o2,
const Transform3s& tf2,
117 const GJKSolver* nsolver,
118 const bool compute_signed_distance,
Vec3s& p1,
120 return ::coal::ShapeShapeDistancer<ShapeType1, ShapeType2>::run(
121 o1, tf1, o2, tf2, nsolver, compute_signed_distance, p1, p2, normal);
134template <
typename ShapeType1,
typename ShapeType2>
135struct ShapeShapeCollider {
136 static std::size_t run(
const CollisionGeometry* o1,
const Transform3s& tf1,
137 const CollisionGeometry* o2,
const Transform3s& tf2,
138 const GJKSolver* nsolver,
139 const CollisionRequest& request,
140 CollisionResult& result) {
141 if (request.isSatisfied(result))
return result.numContacts();
143 const bool compute_penetration =
144 request.enable_contact || (request.security_margin < 0);
145 Vec3s p1, p2, normal;
146 Scalar distance = internal::ShapeShapeDistance<ShapeType1, ShapeType2>(
147 o1, tf1, o2, tf2, nsolver, compute_penetration, p1, p2, normal);
149 size_t num_contacts = 0;
150 const Scalar distToCollision =
distance - request.security_margin;
152 internal::updateDistanceLowerBoundFromLeaf(request, result, distToCollision,
154 if (distToCollision <= request.collision_distance_threshold &&
155 result.numContacts() < request.num_max_contacts) {
156 if (result.numContacts() < request.num_max_contacts) {
157 Contact contact(o1, o2, Contact::NONE, Contact::NONE, p1, p2, normal,
159 result.addContact(contact);
161 num_contacts = result.numContacts();
168template <
typename ShapeType1,
typename ShapeType2>
169std::size_t ShapeShapeCollide(
const CollisionGeometry* o1,
170 const Transform3s& tf1,
171 const CollisionGeometry* o2,
172 const Transform3s& tf2,
const GJKSolver* nsolver,
173 const CollisionRequest& request,
174 CollisionResult& result) {
176 return ShapeShapeCollider<ShapeType1, ShapeType2>::run(
177 o1, tf1, o2, tf2, nsolver, request, result);
217#define SHAPE_SHAPE_DISTANCE_SPECIALIZATION(T1, T2) \
219 COAL_DLLAPI Scalar 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); \
225 COAL_DLLAPI Scalar 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); \
231 inline COAL_DLLAPI Scalar 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) { \
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; \
246 inline COAL_DLLAPI Scalar 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) { \
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; \
261#define SHAPE_SELF_DISTANCE_SPECIALIZATION(T) \
263 COAL_DLLAPI Scalar 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); \
269 inline COAL_DLLAPI Scalar 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) { \
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; \
284SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Box, Halfspace)
285SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Box, Plane)
286SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Box, Sphere)
287SHAPE_SELF_DISTANCE_SPECIALIZATION(Capsule)
288SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Capsule, Halfspace)
289SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Capsule, Plane)
290SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Cone, Halfspace)
291SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Cone, Plane)
292SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Cylinder, Halfspace)
293SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Cylinder, Plane)
294SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Sphere, Halfspace)
295SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Sphere, Plane)
296SHAPE_SELF_DISTANCE_SPECIALIZATION(Sphere)
297SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Sphere, Cylinder)
298SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Sphere, Capsule)
299SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Ellipsoid, Halfspace)
300SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Ellipsoid, Plane)
301SHAPE_SHAPE_DISTANCE_SPECIALIZATION(
ConvexBase16, Halfspace)
302SHAPE_SHAPE_DISTANCE_SPECIALIZATION(
ConvexBase32, Halfspace)
305SHAPE_SHAPE_DISTANCE_SPECIALIZATION(TriangleP, Halfspace)
306SHAPE_SHAPE_DISTANCE_SPECIALIZATION(TriangleP, Plane)
307SHAPE_SELF_DISTANCE_SPECIALIZATION(TriangleP)
308SHAPE_SHAPE_DISTANCE_SPECIALIZATION(TriangleP, Sphere)
309SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Plane, Halfspace)
310SHAPE_SELF_DISTANCE_SPECIALIZATION(Plane)
311SHAPE_SELF_DISTANCE_SPECIALIZATION(Halfspace)
313#undef SHAPE_SHAPE_DISTANCE_SPECIALIZATION
314#undef SHAPE_SELF_DISTANCE_SPECIALIZATION
ConvexBaseTpl< Triangle32::IndexType > ConvexBase32
Definition geometric_shapes.h:949
ConvexBaseTpl< Triangle16::IndexType > ConvexBase16
Definition geometric_shapes.h:948
Main namespace.
Definition broadphase_bruteforce.h:44
Scalar 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.
Eigen::Matrix< Scalar, 3, 1 > Vec3s
Definition data_types.h:70
double Scalar
Definition data_types.h:68
#define COAL_TRACY_ZONE_SCOPED_N(x)
Definition tracy.hh:26