coal  3.0.1
Coal, The Collision Detection Library. Previously known as HPP-FCL, fork of FCL -- The Flexible Collision Library
support_functions.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-2015, Open Source Robotics Foundation
6  * Copyright (c) 2021-2024, INRIA
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of Open Source Robotics Foundation nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  */
36 
39 #ifndef COAL_SUPPORT_FUNCTIONS_H
40 #define COAL_SUPPORT_FUNCTIONS_H
41 
42 #include "coal/shape/geometric_shapes.h"
43 #include "coal/math/transform.h"
44 #include "coal/collision_data.h"
45 #include "coal/narrowphase/support_data.h"
46 
47 namespace coal {
48 namespace details {
49 
50 // ============================================================================
51 // ============================ SUPPORT FUNCTIONS =============================
52 // ============================================================================
63 template <int _SupportOptions = SupportOptions::NoSweptSphere>
64 Vec3s getSupport(const ShapeBase* shape, const Vec3s& dir, int& hint);
65 
67 template <int _SupportOptions = SupportOptions::NoSweptSphere>
68 void getShapeSupport(const TriangleP* triangle, const Vec3s& dir,
69  Vec3s& support, int& /*unused*/,
70  ShapeSupportData& /*unused*/);
71 
73 template <int _SupportOptions = SupportOptions::NoSweptSphere>
74 void getShapeSupport(const Box* box, const Vec3s& dir, Vec3s& support,
75  int& /*unused*/, ShapeSupportData& /*unused*/);
76 
78 template <int _SupportOptions = SupportOptions::NoSweptSphere>
79 void getShapeSupport(const Sphere* sphere, const Vec3s& dir, Vec3s& support,
80  int& /*unused*/, ShapeSupportData& /*unused*/);
81 
83 template <int _SupportOptions = SupportOptions::NoSweptSphere>
84 void getShapeSupport(const Ellipsoid* ellipsoid, const Vec3s& dir,
85  Vec3s& support, int& /*unused*/,
86  ShapeSupportData& /*unused*/);
87 
89 template <int _SupportOptions = SupportOptions::NoSweptSphere>
90 void getShapeSupport(const Capsule* capsule, const Vec3s& dir, Vec3s& support,
91  int& /*unused*/, ShapeSupportData& /*unused*/);
92 
94 template <int _SupportOptions = SupportOptions::NoSweptSphere>
95 void getShapeSupport(const Cone* cone, const Vec3s& dir, Vec3s& support,
96  int& /*unused*/, ShapeSupportData& /*unused*/);
97 
99 template <int _SupportOptions = SupportOptions::NoSweptSphere>
100 void getShapeSupport(const Cylinder* cylinder, const Vec3s& dir, Vec3s& support,
101  int& /*unused*/, ShapeSupportData& /*unused*/);
102 
106 template <int _SupportOptions = SupportOptions::NoSweptSphere,
107  typename IndexType>
108 void getShapeSupport(const ConvexBaseTpl<IndexType>* convex, const Vec3s& dir,
109  Vec3s& support, int& hint, ShapeSupportData& /*unused*/);
110 
116 template <typename IndexType>
117 struct LargeConvex : ConvexBaseTpl<IndexType> {};
118 typedef LargeConvex<Triangle16::IndexType> LargeConvex16;
119 typedef LargeConvex<Triangle32::IndexType> LargeConvex32;
120 
122 template <typename IndexType>
123 struct SmallConvex : ConvexBaseTpl<IndexType> {};
124 typedef SmallConvex<Triangle16::IndexType> SmallConvex16;
125 typedef SmallConvex<Triangle32::IndexType> SmallConvex32;
126 
128 template <int _SupportOptions = SupportOptions::NoSweptSphere,
129  typename IndexType>
130 void getShapeSupport(const SmallConvex<IndexType>* convex, const Vec3s& dir,
131  Vec3s& support, int& hint, ShapeSupportData& data);
132 
134 template <int _SupportOptions = SupportOptions::NoSweptSphere,
135  typename IndexType>
136 void getShapeSupport(const LargeConvex<IndexType>* convex, const Vec3s& dir,
137  Vec3s& support, int& hint, ShapeSupportData& support_data);
138 
139 // ============================================================================
140 // ========================== SUPPORT SET FUNCTIONS ===========================
141 // ============================================================================
177 template <int _SupportOptions = SupportOptions::NoSweptSphere>
178 void getSupportSet(const ShapeBase* shape, SupportSet& support_set, int& hint,
179  size_t num_sampled_supports = 6, Scalar tol = Scalar(1e-3));
180 
188 template <int _SupportOptions = SupportOptions::NoSweptSphere>
189 void getSupportSet(const ShapeBase* shape, const Vec3s& dir,
190  SupportSet& support_set, int& hint,
191  size_t num_sampled_supports = 6, Scalar tol = Scalar(1e-3)) {
192  support_set.tf.rotation() = constructOrthonormalBasisFromVector(dir);
193  const Vec3s& support_dir = support_set.getNormal();
194  const Vec3s support = getSupport<_SupportOptions>(shape, support_dir, hint);
195  getSupportSet<_SupportOptions>(shape, support_set, hint, num_sampled_supports,
196  tol);
197 }
198 
201 template <int _SupportOptions = SupportOptions::NoSweptSphere>
202 void getShapeSupportSet(const TriangleP* triangle, SupportSet& support_set,
203  int& /*unused*/, ShapeSupportData& /*unused*/,
204  size_t /*unused*/ num_sampled_supports = 6,
205  Scalar tol = Scalar(1e-3));
206 
209 template <int _SupportOptions = SupportOptions::NoSweptSphere>
210 void getShapeSupportSet(const Box* box, SupportSet& support_set,
211  int& /*unused*/, ShapeSupportData& support_data,
212  size_t /*unused*/ num_sampled_supports = 6,
213  Scalar tol = Scalar(1e-3));
214 
217 template <int _SupportOptions = SupportOptions::NoSweptSphere>
218 void getShapeSupportSet(const Sphere* sphere, SupportSet& support_set,
219  int& /*unused*/, ShapeSupportData& /*unused*/,
220  size_t /*unused*/ num_sampled_supports = 6,
221  Scalar /*unused*/ tol = Scalar(1e-3));
222 
225 template <int _SupportOptions = SupportOptions::NoSweptSphere>
226 void getShapeSupportSet(const Ellipsoid* ellipsoid, SupportSet& support_set,
227  int& /*unused*/, ShapeSupportData& /*unused*/,
228  size_t /*unused*/ num_sampled_supports = 6,
229  Scalar /*unused*/ tol = Scalar(1e-3));
230 
233 template <int _SupportOptions = SupportOptions::NoSweptSphere>
234 void getShapeSupportSet(const Capsule* capsule, SupportSet& support_set,
235  int& /*unused*/, ShapeSupportData& /*unused*/,
236  size_t /*unused*/ num_sampled_supports = 6,
237  Scalar tol = Scalar(1e-3));
238 
241 template <int _SupportOptions = SupportOptions::NoSweptSphere>
242 void getShapeSupportSet(const Cone* cone, SupportSet& support_set,
243  int& /*unused*/, ShapeSupportData& /*unused*/,
244  size_t num_sampled_supports = 6,
245  Scalar tol = Scalar(1e-3));
246 
249 template <int _SupportOptions = SupportOptions::NoSweptSphere>
250 void getShapeSupportSet(const Cylinder* cylinder, SupportSet& support_set,
251  int& /*unused*/, ShapeSupportData& /*unused*/,
252  size_t num_sampled_supports = 6,
253  Scalar tol = Scalar(1e-3));
254 
259 template <int _SupportOptions = SupportOptions::NoSweptSphere,
260  typename IndexType>
261 void getShapeSupportSet(const ConvexBaseTpl<IndexType>* convex,
262  SupportSet& support_set, int& hint,
263  ShapeSupportData& support_data,
264  size_t /*unused*/ num_sampled_supports = 6,
265  Scalar tol = Scalar(1e-3));
266 
269 template <int _SupportOptions = SupportOptions::NoSweptSphere,
270  typename IndexType>
271 void getShapeSupportSet(const SmallConvex<IndexType>* convex,
272  SupportSet& support_set, int& /*unused*/,
273  ShapeSupportData& /*unused*/,
274  size_t /*unused*/ num_sampled_supports = 6,
275  Scalar tol = Scalar(1e-3));
276 
279 template <int _SupportOptions = SupportOptions::NoSweptSphere,
280  typename IndexType>
281 void getShapeSupportSet(const LargeConvex<IndexType>* convex,
282  SupportSet& support_set, int& hint,
283  ShapeSupportData& support_data,
284  size_t /*unused*/ num_sampled_supports = 6,
285  Scalar tol = Scalar(1e-3));
286 
292 COAL_DLLAPI void computeSupportSetConvexHull(SupportSet::Polygon& cloud,
293  SupportSet::Polygon& cvx_hull);
294 
295 } // namespace details
296 } // namespace coal
297 
298 #endif // COAL_SUPPORT_FUNCTIONS_H
coal::Box
Center at zero point, axis aligned box.
Definition: geometric_shapes.h:164
coal::Capsule
Capsule It is where is the distance between the point x and the capsule segment AB,...
Definition: geometric_shapes.h:381
coal::Cone
Cone The base of the cone is at and the top is at .
Definition: geometric_shapes.h:466
coal::Cylinder
Cylinder along Z axis. The cylinder is defined at its centroid.
Definition: geometric_shapes.h:556
coal::Ellipsoid
Ellipsoid centered at point zero.
Definition: geometric_shapes.h:303
coal::ShapeBase
Base class for all basic geometric shapes.
Definition: geometric_shapes.h:58
coal::Sphere
Center at zero point sphere.
Definition: geometric_shapes.h:238
coal::Transform3s::rotation
const Matrix3s & rotation() const
get rotation
Definition: transform.h:113
coal::TriangleP
Triangle stores the points instead of only indices of points.
Definition: geometric_shapes.h:108
collision_data.h
COAL_DLLAPI
#define COAL_DLLAPI
Definition: config.hh:88
coal::details::SmallConvex16
SmallConvex< Triangle16::IndexType > SmallConvex16
Definition: support_functions.h:124
coal::details::NoSweptSphere
@ NoSweptSphere
Definition: support_data.h:57
coal::details::getShapeSupportSet
void getShapeSupportSet(const TriangleP *triangle, SupportSet &support_set, int &, ShapeSupportData &, size_t num_sampled_supports=6, Scalar tol=Scalar(1e-3))
Triangle support set function. Assumes the support set frame has already been computed.
coal::details::LargeConvex16
LargeConvex< Triangle16::IndexType > LargeConvex16
Definition: support_functions.h:118
coal::details::getShapeSupport
void getShapeSupport(const TriangleP *triangle, const Vec3s &dir, Vec3s &support, int &, ShapeSupportData &)
Triangle support function.
coal::details::SmallConvex32
SmallConvex< Triangle32::IndexType > SmallConvex32
Definition: support_functions.h:125
coal::details::LargeConvex32
LargeConvex< Triangle32::IndexType > LargeConvex32
Definition: support_functions.h:119
coal::details::getSupport
Vec3s getSupport(const ShapeBase *shape, const Vec3s &dir, int &hint)
the support function for shape. The output support point is expressed in the local frame of the shape...
coal::details::getSupportSet
void getSupportSet(const ShapeBase *shape, SupportSet &support_set, int &hint, size_t num_sampled_supports=6, Scalar tol=Scalar(1e-3))
Computes the support set for shape. This function assumes the frame of the support set has already be...
coal::details::computeSupportSetConvexHull
void computeSupportSetConvexHull(SupportSet::Polygon &cloud, SupportSet::Polygon &cvx_hull)
Computes the convex-hull of support_set. For now, this function is only needed for Box and ConvexBase...
coal
Main namespace.
Definition: broadphase_bruteforce.h:44
coal::constructOrthonormalBasisFromVector
Matrix3s constructOrthonormalBasisFromVector(const Vec3s &vec)
Construct othonormal basis from vector. The z-axis is the normalized input vector.
Definition: transform.h:261
coal::Vec3s
Eigen::Matrix< Scalar, 3, 1 > Vec3s
Definition: data_types.h:70
coal::Scalar
double Scalar
Definition: data_types.h:68
geometric_shapes.h
coal::ContactPatch
This structure allows to encode contact patches. A contact patch is defined by a set of points belong...
Definition: collision_data.h:511
coal::ContactPatch::getNormal
Vec3s getNormal() const
Normal of the contact patch, expressed in the WORLD frame.
Definition: collision_data.h:568
coal::ContactPatch::tf
Transform3s tf
Frame of the set, expressed in the world coordinates. The z-axis of the frame's rotation is the conta...
Definition: collision_data.h:517
coal::ContactPatch::Polygon
std::vector< Vec2s > Polygon
Definition: collision_data.h:513
coal::details::LargeConvex
Cast a ConvexBase to a LargeConvex to use the log version of getShapeSupport. This is much faster tha...
Definition: support_functions.h:117
coal::details::ShapeSupportData
Stores temporary data for the computation of support points.
Definition: support_data.h:62
coal::details::SmallConvex
See LargeConvex.
Definition: support_functions.h:123
support_data.h