GCC Code Coverage Report

Line	Branch	Exec	Source
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
37			/** \authors Jia Pan, Florent Lamiraux, Josef Mirabel, Louis Montaut */
38
39			#ifndef COAL_MINKOWSKI_DIFFERENCE_H
40			#define COAL_MINKOWSKI_DIFFERENCE_H
41
42			#include "coal/shape/geometric_shapes.h"
43			#include "coal/math/transform.h"
44			#include "coal/narrowphase/support_functions.h"
45
46			namespace coal {
47
48			namespace details {
49
50			/// @brief Minkowski difference class of two shapes
51			///
52			/// @note The Minkowski difference is expressed in the frame of the first shape.
53			struct COAL_DLLAPI MinkowskiDiff {
54			typedef Eigen::Array<Scalar, 1, 2> Array2;
55
56			/// @brief points to two shapes
57			const ShapeBase* shapes[2];
58
59			/// @brief Store temporary data for the computation of the support point for
60			/// each shape.
61			ShapeSupportData data[2];
62
63			/// @brief rotation from shape1 to shape0
64			/// such that @f$ p_in_0 = oR1 * p_in_1 + ot1 @f$.
65			Matrix3s oR1;
66
67			/// @brief translation from shape1 to shape0
68			/// such that @f$ p_in_0 = oR1 * p_in_1 + ot1 @f$.
69			Vec3s ot1;
70
71			/// @brief The radii of the sphere swepted around each shape of the Minkowski
72			/// difference. The 2 values correspond to the swept-sphere radius of shape 0
73			/// and shape 1.
74			Array2 swept_sphere_radius;
75
76			/// @brief Wether or not to use the normalize heuristic in the GJK Nesterov
77			/// acceleration. This setting is only applied if the Nesterov acceleration in
78			/// the GJK class is active.
79			bool normalize_support_direction;
80
81			typedef void (*GetSupportFunction)(const MinkowskiDiff& minkowskiDiff,
82			const Vec3s& dir, Vec3s& support0,
83			Vec3s& support1,
84			support_func_guess_t& hint,
85			ShapeSupportData data[2]);
86			GetSupportFunction getSupportFunc;
87
88	6/14 ✓ Branch 1 taken 60952074 times. ✗ Branch 2 not taken. ✓ Branch 3 taken 60952074 times. ✓ Branch 4 taken 30476037 times. ✓ Branch 6 taken 30476037 times. ✗ Branch 7 not taken. ✓ Branch 9 taken 30476037 times. ✗ Branch 10 not taken. ✓ Branch 12 taken 30476037 times. ✗ Branch 13 not taken. ✗ Branch 15 not taken. ✗ Branch 16 not taken. ✗ Branch 17 not taken. ✗ Branch 18 not taken.	91428111	MinkowskiDiff() : normalize_support_direction(false), getSupportFunc(NULL) {}
89
90			/// @brief Set the two shapes, assuming the relative transformation between
91			/// them is identity.
92			/// Consequently, all support points computed with the MinkowskiDiff
93			/// will be expressed in the world frame.
94			/// @param shape0 the first shape.
95			/// @param shape1 the second shape.
96			/// @tparam _SupportOptions is a value of the SupportOptions enum. If set to
97			/// `WithSweptSphere`, the support computation will take into account the
98			/// swept sphere radius of the shapes. If set to `NoSweptSphere`, where
99			/// this information is simply stored in the Minkowski's difference
100			/// `swept_sphere_radius` array. This array is then used to correct the
101			/// solution found when GJK or EPA have converged.
102			///
103			/// @note In practice, there is no need to take into account the swept-sphere
104			/// radius in the iterations of GJK/EPA. It is in fact detrimental to the
105			/// convergence of both algos. This is because it makes corners and edges of
106			/// shapes look strictly convex to the algorithms, which leads to poor
107			/// convergence. This swept sphere template parameter is only here for
108			/// debugging purposes and for specific uses cases where the swept sphere
109			/// radius is needed in the support function. The rule is simple. When
110			/// interacting with GJK/EPA, the `SupportOptions` template
111			/// parameter should always be set to `NoSweptSphere` (except for
112			/// debugging or testing purposes). When working directly with the shapes
113			/// involved in the collision, and not relying on GJK/EPA, the
114			/// `SupportOptions` template parameter should be set to `WithSweptSphere`.
115			/// This is for example the case for specialized collision/distance functions.
116			template <int _SupportOptions = SupportOptions::NoSweptSphere>
117			void set(const ShapeBase* shape0, const ShapeBase* shape1);
118
119			/// @brief Set the two shapes, with a relative transformation from shape0 to
120			/// shape1. Consequently, all support points computed with the MinkowskiDiff
121			/// will be expressed in the frame of shape0.
122			/// @param shape0 the first shape.
123			/// @param shape1 the second shape.
124			/// @param tf0 the transformation of the first shape.
125			/// @param tf1 the transformation of the second shape.
126			/// @tparam _SupportOptions see `set(const ShapeBase*, const
127			/// ShapeBase*)` for more details.
128			template <int _SupportOptions = SupportOptions::NoSweptSphere>
129			void set(const ShapeBase* shape0, const ShapeBase* shape1,
130			const Transform3s& tf0, const Transform3s& tf1);
131
132			/// @brief support function for shape0.
133			/// The output vector is expressed in the local frame of shape0.
134			/// @return a point which belongs to the set {argmax_{v in shape0}
135			/// v.dot(dir)}, i.e a support of shape0 in direction dir.
136			/// @param dir support direction.
137			/// @param hint used to initialize the search when shape is a ConvexBase
138			/// object.
139			/// @tparam _SupportOptions see `set(const ShapeBase*, const
140			/// ShapeBase*)` for more details.
141			template <int _SupportOptions = SupportOptions::NoSweptSphere>
142		✗	inline Vec3s support0(const Vec3s& dir, int& hint) const {
143		✗	return getSupport<_SupportOptions>(shapes[0], dir, hint);
144			}
145
146			/// @brief support function for shape1.
147			/// The output vector is expressed in the local frame of shape0.
148			/// This is mandatory because in the end we are interested in support points
149			/// of the Minkowski difference; hence the supports of shape0 and shape1 must
150			/// live in the same frame.
151			/// @return a point which belongs to the set {tf * argmax_{v in shape1}
152			/// v.dot(R^T * dir)}, i.e. the support of shape1 in direction dir (tf is the
153			/// tranform from shape1 to shape0).
154			/// @param dir support direction.
155			/// @param hint used to initialize the search when shape is a ConvexBase.
156			/// @tparam _SupportOptions see `set(const ShapeBase*, const
157			/// ShapeBase*)` for more details.
158			template <int _SupportOptions = SupportOptions::NoSweptSphere>
159		✗	inline Vec3s support1(const Vec3s& dir, int& hint) const {
160			// clang-format off
161		✗	return oR1 * getSupport<_SupportOptions>(shapes[1], oR1.transpose() * dir, hint) + ot1;
162			// clang-format on
163			}
164
165			/// @brief Support function for the pair of shapes. This method assumes `set`
166			/// has already been called.
167			/// @param[in] dir the support direction.
168			/// @param[out] supp0 support of shape0 in direction dir, expressed in the
169			/// frame of shape0.
170			/// @param[out] supp1 support of shape1 in direction -dir, expressed in the
171			/// frame of shape0.
172			/// @param[in/out] hint used to initialize the search when shape is a
173			/// ConvexBase object.
174		72451398	inline void support(const Vec3s& dir, Vec3s& supp0, Vec3s& supp1,
175			support_func_guess_t& hint) const {
176	1/2 ✗ Branch 0 not taken. ✓ Branch 1 taken 72451398 times.	72451398	assert(getSupportFunc != NULL);
177		72451398	getSupportFunc(*this, dir, supp0, supp1, hint,
178		72451398	const_cast<ShapeSupportData*>(data));
179		72451398	}
180			};
181
182			} // namespace details
183
184			} // namespace coal
185
186			#endif // COAL_MINKOWSKI_DIFFERENCE_H
187