hpp-fcl 2.3.0
HPP fork of FCL -- The Flexible Collision Library
Loading...
Searching...
No Matches
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 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of Willow Garage, Inc. nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 */
34
37#ifndef HPP_FCL_INTERNAL_SHAPE_SHAPE_FUNC_H
38#define HPP_FCL_INTERNAL_SHAPE_SHAPE_FUNC_H
39
41
46
47namespace hpp {
48namespace fcl {
49
50template <typename T_SH1, typename T_SH2>
51HPP_FCL_DLLAPI FCL_REAL ShapeShapeDistance(const CollisionGeometry* o1,
52 const Transform3f& tf1,
53 const CollisionGeometry* o2,
54 const Transform3f& tf2,
55 const GJKSolver* nsolver,
56 const DistanceRequest& request,
57 DistanceResult& result);
58
59template <typename T_SH1, typename T_SH2>
60struct ShapeShapeCollider {
61 static std::size_t run(const CollisionGeometry* o1, const Transform3f& tf1,
62 const CollisionGeometry* o2, const Transform3f& tf2,
63 const GJKSolver* nsolver,
64 const CollisionRequest& request,
65 CollisionResult& result) {
66 if (request.isSatisfied(result)) return result.numContacts();
67
68 DistanceResult distanceResult;
69 DistanceRequest distanceRequest(request.enable_contact);
70 FCL_REAL distance = ShapeShapeDistance<T_SH1, T_SH2>(
71 o1, tf1, o2, tf2, nsolver, distanceRequest, distanceResult);
72
73 size_t num_contacts = 0;
74 const Vec3f& p1 = distanceResult.nearest_points[0];
75 const Vec3f& p2 = distanceResult.nearest_points[1];
76 FCL_REAL distToCollision = distance - request.security_margin;
77
78 internal::updateDistanceLowerBoundFromLeaf(request, result, distToCollision,
79 p1, p2);
80 if (distToCollision <= request.collision_distance_threshold &&
81 result.numContacts() < request.num_max_contacts) {
82 if (result.numContacts() < request.num_max_contacts) {
83 const Vec3f& p1 = distanceResult.nearest_points[0];
84 const Vec3f& p2 = distanceResult.nearest_points[1];
85
86 Contact contact(
87 o1, o2, distanceResult.b1, distanceResult.b2, (p1 + p2) / 2,
88 (distance <= 0 ? distanceResult.normal : (p2 - p1).normalized()),
89 -distance);
90
91 result.addContact(contact);
92 }
93 num_contacts = result.numContacts();
94 }
95
96 return num_contacts;
97 }
98};
99
100template <typename ShapeType1, typename ShapeType2>
101std::size_t ShapeShapeCollide(const CollisionGeometry* o1,
102 const Transform3f& tf1,
103 const CollisionGeometry* o2,
104 const Transform3f& tf2, const GJKSolver* nsolver,
105 const CollisionRequest& request,
106 CollisionResult& result) {
107 return ShapeShapeCollider<ShapeType1, ShapeType2>::run(
108 o1, tf1, o2, tf2, nsolver, request, result);
109}
110
111#define SHAPE_SHAPE_DISTANCE_SPECIALIZATION(T1, T2) \
112 template <> \
113 HPP_FCL_DLLAPI FCL_REAL ShapeShapeDistance<T1, T2>( \
114 const CollisionGeometry* o1, const Transform3f& tf1, \
115 const CollisionGeometry* o2, const Transform3f& tf2, \
116 const GJKSolver* nsolver, const DistanceRequest& request, \
117 DistanceResult& result); \
118 template <> \
119 HPP_FCL_DLLAPI FCL_REAL ShapeShapeDistance<T2, T1>( \
120 const CollisionGeometry* o1, const Transform3f& tf1, \
121 const CollisionGeometry* o2, const Transform3f& tf2, \
122 const GJKSolver* nsolver, const DistanceRequest& request, \
123 DistanceResult& result)
124
125SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Box, Halfspace);
126SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Box, Plane);
127SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Box, Sphere);
128SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Capsule, Capsule);
129SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Capsule, Halfspace);
130SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Capsule, Plane);
131SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Cone, Halfspace);
132SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Cone, Plane);
133SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Cylinder, Halfspace);
134SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Cylinder, Plane);
135SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Sphere, Halfspace);
136SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Sphere, Plane);
137SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Sphere, Sphere);
138SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Sphere, Cylinder);
139
140SHAPE_SHAPE_DISTANCE_SPECIALIZATION(ConvexBase, Halfspace);
141SHAPE_SHAPE_DISTANCE_SPECIALIZATION(TriangleP, Halfspace);
142
143#undef SHAPE_SHAPE_DISTANCE_SPECIALIZATION
144
145#define SHAPE_SHAPE_COLLIDE_SPECIALIZATION(T1, T2) \
146 template <> \
147 struct ShapeShapeCollider<T1, T2> { \
148 static HPP_FCL_DLLAPI std::size_t run(const CollisionGeometry* o1, \
149 const Transform3f& tf1, \
150 const CollisionGeometry* o2, \
151 const Transform3f& tf2, \
152 const GJKSolver* nsolver, \
153 const CollisionRequest& request, \
154 CollisionResult& result); \
155 }
156
157SHAPE_SHAPE_COLLIDE_SPECIALIZATION(Sphere, Sphere);
158
159#undef SHAPE_SHAPE_COLLIDE_SPECIALIZATION
160} // namespace fcl
161
162} // namespace hpp
163
165
166#endif
#define HPP_FCL_DLLAPI
Definition: config.hh:64
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.
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
double FCL_REAL
Definition: data_types.h:65
Main namespace.
Definition: broadphase_bruteforce.h:44