GCC Code Coverage Report

Directory: ./
File: include/coal/shape/geometric_shapes.h
Date: 2025-06-03 17:15:40
Exec Total Coverage
Lines: 306 359 85.2%
Functions: 102 123 82.9%
Branches: 336 814 41.3%
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 * 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 Open Source Robotics Foundation 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
36 /** \author Jia Pan */
37
38 #ifndef COAL_GEOMETRIC_SHAPES_H
39 #define COAL_GEOMETRIC_SHAPES_H
40
41 #include <vector>
42 #include <memory>
43
44 #include <boost/math/constants/constants.hpp>
45
46 #include "coal/collision_object.h"
47 #include "coal/data_types.h"
48
49 #ifdef COAL_HAS_QHULL
50 namespace orgQhull {
51 class Qhull;
52 }
53 #endif
54
55 namespace coal {
56
57 /// @brief Base class for all basic geometric shapes
58 class COAL_DLLAPI ShapeBase : public CollisionGeometry {
59 public:
60 116923206 ShapeBase() {}
61
62 ///  \brief Copy constructor
63 181 ShapeBase(const ShapeBase& other)
64 181 : CollisionGeometry(other),
65 181 m_swept_sphere_radius(other.m_swept_sphere_radius) {}
66
67 20 ShapeBase& operator=(const ShapeBase& other) = default;
68
69 233846642 virtual ~ShapeBase() {};
70
71 /// @brief Get object type: a geometric shape
72 2547629 OBJECT_TYPE getObjectType() const { return OT_GEOM; }
73
74 /// @brief Set radius of sphere swept around the shape.
75 /// Must be >= 0.
76 24401 void setSweptSphereRadius(Scalar radius) {
77
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 24401 times.
 24401 if (radius < 0) {
78 ✗ COAL_THROW_PRETTY("Swept-sphere radius must be positive.",
79 std::invalid_argument);
80 }
81 24401 this->m_swept_sphere_radius = radius;
82 24401 }
83
84 /// @brief Get radius of sphere swept around the shape.
85 /// This radius is always >= 0.
86 61509752 Scalar getSweptSphereRadius() const { return this->m_swept_sphere_radius; }
87
88 protected:
89 /// \brief Radius of the sphere swept around the shape.
90 /// Default value is 0.
91 /// Note: this property differs from `inflated` method of certain
92 /// derived classes (e.g. Box, Sphere, Ellipsoid, Capsule, Cone, Cylinder)
93 /// in the sense that inflated returns a new shape which can be inflated but
94 /// also deflated.
95 /// Also, an inflated shape is not rounded. It simply has a different size.
96 /// Sweeping a shape with a sphere is a different operation (a Minkowski sum),
97 /// which rounds the sharp corners of a shape.
98 /// The swept sphere radius is a property of the shape itself and can be
99 /// manually updated between collision checks.
100 Scalar m_swept_sphere_radius{0};
101 };
102
103 /// @defgroup Geometric_Shapes Geometric shapes
104 /// Classes of different types of geometric shapes.
105 /// @{
106
107 /// @brief Triangle stores the points instead of only indices of points
108 class COAL_DLLAPI TriangleP : public ShapeBase {
109 public:
110
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 1 TriangleP() {};
111
112 116816979 TriangleP(const Vec3s& a_, const Vec3s& b_, const Vec3s& c_)
113
3/6
✓ Branch 2 taken 116816979 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 116816979 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 116816979 times.
✗ Branch 9 not taken.
 116816979 : ShapeBase(), a(a_), b(b_), c(c_) {}
114
115 1 TriangleP(const TriangleP& other)
116
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 1 : ShapeBase(other), a(other.a), b(other.b), c(other.c) {}
117
118 /// @brief Clone *this into a new TriangleP
119 ✗ virtual TriangleP* clone() const { return new TriangleP(*this); };
120
121 /// @brief virtual function of compute AABB in local coordinate
122 void computeLocalAABB();
123
124 87620051 NODE_TYPE getNodeType() const { return GEOM_TRIANGLE; }
125
126 // std::pair<ShapeBase*, Transform3s> inflated(const Scalar value) const
127 // {
128 // if (value == 0) return std::make_pair(new TriangleP(*this),
129 // Transform3s()); Vec3s AB(b - a), BC(c - b), CA(a - c); AB.normalize();
130 // BC.normalize();
131 // CA.normalize();
132 //
133 // Vec3s new_a(a + value * Vec3s(-AB + CA).normalized());
134 // Vec3s new_b(b + value * Vec3s(-BC + AB).normalized());
135 // Vec3s new_c(c + value * Vec3s(-CA + BC).normalized());
136 //
137 // return std::make_pair(new TriangleP(new_a, new_b, new_c),
138 // Transform3s());
139 // }
140 //
141 // Scalar minInflationValue() const
142 // {
143 // return (std::numeric_limits<Scalar>::max)(); // TODO(jcarpent):
144 // implement
145 // }
146
147 Vec3s a, b, c;
148
149 private:
150 14 virtual bool isEqual(const CollisionGeometry& _other) const {
151
1/2
✓ Branch 0 taken 14 times.
✗ Branch 1 not taken.
 14 const TriangleP* other_ptr = dynamic_cast<const TriangleP*>(&_other);
152
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 14 times.
 14 if (other_ptr == nullptr) return false;
153 14 const TriangleP& other = *other_ptr;
154
155
4/8
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 14 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 14 times.
✗ Branch 10 not taken.
 28 return a == other.a && b == other.b && c == other.c &&
156 28 getSweptSphereRadius() == other.getSweptSphereRadius();
157 }
158
159 public:
160 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
161 };
162
163 /// @brief Center at zero point, axis aligned box
164 class COAL_DLLAPI Box : public ShapeBase {
165 public:
166 7754 Box(Scalar x, Scalar y, Scalar z)
167
1/2
✓ Branch 2 taken 7754 times.
✗ Branch 3 not taken.
 7754 : ShapeBase(), halfSide(x / 2, y / 2, z / 2) {}
168
169
2/4
✓ Branch 2 taken 2921 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2921 times.
✗ Branch 6 not taken.
 2921 Box(const Vec3s& side_) : ShapeBase(), halfSide(side_ / 2) {}
170
171
1/2
✓ Branch 2 taken 24 times.
✗ Branch 3 not taken.
 24 Box(const Box& other) : ShapeBase(other), halfSide(other.halfSide) {}
172
173 2629 Box& operator=(const Box& other) {
174
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 2629 times.
 2629 if (this == &other) return *this;
175
176 2629 this->halfSide = other.halfSide;
177 2629 return *this;
178 }
179
180 /// @brief Clone *this into a new Box
181 ✗ virtual Box* clone() const { return new Box(*this); };
182
183 /// @brief Default constructor
184
1/2
✓ Branch 2 taken 2631 times.
✗ Branch 3 not taken.
 2631 Box() {}
185
186 /// @brief box side half-length
187 Vec3s halfSide;
188
189 /// @brief Compute AABB
190 void computeLocalAABB();
191
192 /// @brief Get node type: a box
193 2734876 NODE_TYPE getNodeType() const { return GEOM_BOX; }
194
195 4 Scalar computeVolume() const { return 8 * halfSide.prod(); }
196
197 2 Matrix3s computeMomentofInertia() const {
198
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 Scalar V = computeVolume();
199
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
 2 Vec3s s(halfSide.cwiseAbs2() * V);
200
10/20
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 2 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 2 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 2 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 2 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 2 times.
✗ Branch 29 not taken.
 2 return (Vec3s(s[1] + s[2], s[0] + s[2], s[0] + s[1]) / 3).asDiagonal();
201 }
202
203 7 Scalar minInflationValue() const { return -halfSide.minCoeff(); }
204
205 /// \brief Inflate the box by an amount given by `value`.
206 /// This value can be positive or negative but must always >=
207 /// `minInflationValue()`.
208 ///
209 /// \param[in] value of the shape inflation.
210 ///
211 /// \returns a new inflated box and the related transform to account for the
212 /// change of shape frame
213 6 std::pair<Box, Transform3s> inflated(const Scalar value) const {
214
2/2
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 5 times.
 6 if (value <= minInflationValue())
215
20/40
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 1 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 1 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 1 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 1 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 1 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 1 times.
✗ Branch 41 not taken.
✓ Branch 43 taken 1 times.
✗ Branch 44 not taken.
✓ Branch 46 taken 1 times.
✗ Branch 47 not taken.
✓ Branch 49 taken 1 times.
✗ Branch 50 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 56 taken 1 times.
✗ Branch 57 not taken.
✓ Branch 59 taken 1 times.
✗ Branch 60 not taken.
 1 COAL_THROW_PRETTY("value (" << value << ") "
216 << "is two small. It should be at least: "
217 << minInflationValue(),
218 std::invalid_argument);
219
5/10
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 5 times.
✗ Branch 14 not taken.
 10 return std::make_pair(Box(2 * (halfSide + Vec3s::Constant(value))),
220
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 15 Transform3s());
221 }
222
223 private:
224 15 virtual bool isEqual(const CollisionGeometry& _other) const {
225
1/2
✓ Branch 0 taken 15 times.
✗ Branch 1 not taken.
 15 const Box* other_ptr = dynamic_cast<const Box*>(&_other);
226
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 15 times.
 15 if (other_ptr == nullptr) return false;
227 15 const Box& other = *other_ptr;
228
229
2/4
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 15 times.
✗ Branch 4 not taken.
 30 return halfSide == other.halfSide &&
230 30 getSweptSphereRadius() == other.getSweptSphereRadius();
231 }
232
233 public:
234 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
235 };
236
237 /// @brief Center at zero point sphere
238 class COAL_DLLAPI Sphere : public ShapeBase {
239 public:
240 /// @brief Default constructor
241 2 Sphere() {}
242
243 5717 explicit Sphere(Scalar radius_) : ShapeBase(), radius(radius_) {}
244
245 25 Sphere(const Sphere& other) : ShapeBase(other), radius(other.radius) {}
246
247 /// @brief Clone *this into a new Sphere
248 ✗ virtual Sphere* clone() const { return new Sphere(*this); };
249
250 /// @brief Radius of the sphere
251 Scalar radius;
252
253 /// @brief Compute AABB
254 void computeLocalAABB();
255
256 /// @brief Get node type: a sphere
257 793660 NODE_TYPE getNodeType() const { return GEOM_SPHERE; }
258
259 2 Matrix3s computeMomentofInertia() const {
260
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 Scalar I = Scalar(0.4) * radius * radius * computeVolume();
261
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
 2 return I * Matrix3s::Identity();
262 }
263
264 4 Scalar computeVolume() const {
265 4 return 4 * boost::math::constants::pi<Scalar>() * radius * radius * radius /
266 4 3;
267 }
268
269 8 Scalar minInflationValue() const { return -radius; }
270
271 /// \brief Inflate the sphere by an amount given by `value`.
272 /// This value can be positive or negative but must always >=
273 /// `minInflationValue()`.
274 ///
275 /// \param[in] value of the shape inflation.
276 ///
277 /// \returns a new inflated sphere and the related transform to account for
278 /// the change of shape frame
279 7 std::pair<Sphere, Transform3s> inflated(const Scalar value) const {
280
2/2
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 6 times.
 7 if (value <= minInflationValue())
281
18/36
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 1 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 1 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 1 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 1 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 1 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 1 times.
✗ Branch 41 not taken.
✓ Branch 44 taken 1 times.
✗ Branch 45 not taken.
✓ Branch 47 taken 1 times.
✗ Branch 48 not taken.
✓ Branch 51 taken 1 times.
✗ Branch 52 not taken.
✓ Branch 54 taken 1 times.
✗ Branch 55 not taken.
 1 COAL_THROW_PRETTY("value (" << value
282 << ") is two small. It should be at least: "
283 << minInflationValue(),
284 std::invalid_argument);
285
3/6
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
 6 return std::make_pair(Sphere(radius + value), Transform3s());
286 }
287
288 private:
289 15 virtual bool isEqual(const CollisionGeometry& _other) const {
290
1/2
✓ Branch 0 taken 15 times.
✗ Branch 1 not taken.
 15 const Sphere* other_ptr = dynamic_cast<const Sphere*>(&_other);
291
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 15 times.
 15 if (other_ptr == nullptr) return false;
292 15 const Sphere& other = *other_ptr;
293
294
2/4
✓ Branch 0 taken 15 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 15 times.
✗ Branch 3 not taken.
 30 return radius == other.radius &&
295 30 getSweptSphereRadius() == other.getSweptSphereRadius();
296 }
297
298 public:
299 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
300 };
301
302 /// @brief Ellipsoid centered at point zero
303 class COAL_DLLAPI Ellipsoid : public ShapeBase {
304 public:
305 /// @brief Default constructor
306
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 Ellipsoid() {}
307
308
1/2
✓ Branch 2 taken 19 times.
✗ Branch 3 not taken.
 19 Ellipsoid(Scalar rx, Scalar ry, Scalar rz) : ShapeBase(), radii(rx, ry, rz) {}
309
310
1/2
✓ Branch 2 taken 150 times.
✗ Branch 3 not taken.
 150 explicit Ellipsoid(const Vec3s& radii) : radii(radii) {}
311
312
1/2
✓ Branch 2 taken 24 times.
✗ Branch 3 not taken.
 24 Ellipsoid(const Ellipsoid& other) : ShapeBase(other), radii(other.radii) {}
313
314 /// @brief Clone *this into a new Ellipsoid
315 ✗ virtual Ellipsoid* clone() const { return new Ellipsoid(*this); };
316
317 /// @brief Radii of the Ellipsoid (such that on boundary: x^2/rx^2 + y^2/ry^2
318 /// + z^2/rz^2 = 1)
319 Vec3s radii;
320
321 /// @brief Compute AABB
322 void computeLocalAABB();
323
324 /// @brief Get node type: an ellipsoid
325 45543 NODE_TYPE getNodeType() const { return GEOM_ELLIPSOID; }
326
327 ✗ Matrix3s computeMomentofInertia() const {
328 ✗ Scalar V = computeVolume();
329 ✗ Scalar a2 = V * radii[0] * radii[0];
330 ✗ Scalar b2 = V * radii[1] * radii[1];
331 ✗ Scalar c2 = V * radii[2] * radii[2];
332 ✗ Scalar alpha = Scalar(0.2);
333 ✗ return (Matrix3s() << alpha * (b2 + c2), 0, 0, 0, alpha * (a2 + c2), 0, 0,
334 ✗ 0, alpha * (a2 + b2))
335 ✗ .finished();
336 }
337
338 ✗ Scalar computeVolume() const {
339 ✗ return 4 * boost::math::constants::pi<Scalar>() * radii[0] * radii[1] *
340 ✗ radii[2] / 3;
341 }
342
343 7 Scalar minInflationValue() const { return -radii.minCoeff(); }
344
345 /// \brief Inflate the ellipsoid by an amount given by `value`.
346 /// This value can be positive or negative but must always >=
347 /// `minInflationValue()`.
348 ///
349 /// \param[in] value of the shape inflation.
350 ///
351 /// \returns a new inflated ellipsoid and the related transform to account for
352 /// the change of shape frame
353 6 std::pair<Ellipsoid, Transform3s> inflated(const Scalar value) const {
354
2/2
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 5 times.
 6 if (value <= minInflationValue())
355
19/38
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 1 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 1 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 1 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 1 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 1 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 1 times.
✗ Branch 41 not taken.
✓ Branch 43 taken 1 times.
✗ Branch 44 not taken.
✓ Branch 46 taken 1 times.
✗ Branch 47 not taken.
✓ Branch 49 taken 1 times.
✗ Branch 50 not taken.
✓ Branch 53 taken 1 times.
✗ Branch 54 not taken.
✓ Branch 56 taken 1 times.
✗ Branch 57 not taken.
 1 COAL_THROW_PRETTY("value (" << value
356 << ") is two small. It should be at least: "
357 << minInflationValue(),
358 std::invalid_argument);
359
4/8
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
 10 return std::make_pair(Ellipsoid(radii + Vec3s::Constant(value)),
360
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 15 Transform3s());
361 }
362
363 private:
364 15 virtual bool isEqual(const CollisionGeometry& _other) const {
365
1/2
✓ Branch 0 taken 15 times.
✗ Branch 1 not taken.
 15 const Ellipsoid* other_ptr = dynamic_cast<const Ellipsoid*>(&_other);
366
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 15 times.
 15 if (other_ptr == nullptr) return false;
367 15 const Ellipsoid& other = *other_ptr;
368
369
2/4
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 15 times.
✗ Branch 4 not taken.
 30 return radii == other.radii &&
370 30 getSweptSphereRadius() == other.getSweptSphereRadius();
371 }
372
373 public:
374 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
375 };
376
377 /// @brief Capsule
378 /// It is \f$ { x~\in~\mathbb{R}^3, d(x, AB) \leq radius } \f$
379 /// where \f$ d(x, AB) \f$ is the distance between the point x and the capsule
380 /// segment AB, with \f$ A = (0,0,-halfLength), B = (0,0,halfLength) \f$.
381 class COAL_DLLAPI Capsule : public ShapeBase {
382 public:
383 /// @brief Default constructor
384 2 Capsule() {}
385
386 1117 Capsule(Scalar radius_, Scalar lz_) : ShapeBase(), radius(radius_) {
387 1117 halfLength = lz_ / 2;
388 1117 }
389
390 24 Capsule(const Capsule& other)
391 24 : ShapeBase(other), radius(other.radius), halfLength(other.halfLength) {}
392
393 /// @brief Clone *this into a new Capsule
394 ✗ virtual Capsule* clone() const { return new Capsule(*this); };
395
396 /// @brief Radius of capsule
397 Scalar radius;
398
399 /// @brief Half Length along z axis
400 Scalar halfLength;
401
402 /// @brief Compute AABB
403 void computeLocalAABB();
404
405 /// @brief Get node type: a capsule
406 42513 NODE_TYPE getNodeType() const { return GEOM_CAPSULE; }
407
408 2 Scalar computeVolume() const {
409 2 return boost::math::constants::pi<Scalar>() * radius * radius *
410 2 ((halfLength * 2) + radius * 4 / Scalar(3));
411 }
412
413 2 Matrix3s computeMomentofInertia() const {
414 2 Scalar v_cyl = radius * radius * (halfLength * 2) *
415 2 boost::math::constants::pi<Scalar>();
416 4 Scalar v_sph = radius * radius * radius *
417 2 boost::math::constants::pi<Scalar>() * 4 / Scalar(3);
418
419 2 Scalar h2 = halfLength * halfLength;
420 2 Scalar r2 = radius * radius;
421 2 Scalar ix =
422 2 v_cyl * (h2 / Scalar(3) + r2 / Scalar(4)) +
423 2 v_sph * (Scalar(0.4) * r2 + h2 + Scalar(0.75) * radius * halfLength);
424 2 Scalar iz = (Scalar(0.5) * v_cyl + Scalar(0.4) * v_sph) * radius * radius;
425
426
11/22
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 2 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 2 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 2 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 2 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 2 times.
✗ Branch 29 not taken.
✓ Branch 32 taken 2 times.
✗ Branch 33 not taken.
 2 return (Matrix3s() << ix, 0, 0, 0, ix, 0, 0, 0, iz).finished();
427 }
428
429 7 Scalar minInflationValue() const { return -radius; }
430
431 /// \brief Inflate the capsule by an amount given by `value`.
432 /// This value can be positive or negative but must always >=
433 /// `minInflationValue()`.
434 ///
435 /// \param[in] value of the shape inflation.
436 ///
437 /// \returns a new inflated capsule and the related transform to account for
438 /// the change of shape frame
439 6 std::pair<Capsule, Transform3s> inflated(const Scalar value) const {
440
2/2
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 5 times.
 6 if (value <= minInflationValue())
441
18/36
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 1 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 1 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 1 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 1 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 1 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 1 times.
✗ Branch 41 not taken.
✓ Branch 44 taken 1 times.
✗ Branch 45 not taken.
✓ Branch 47 taken 1 times.
✗ Branch 48 not taken.
✓ Branch 51 taken 1 times.
✗ Branch 52 not taken.
✓ Branch 54 taken 1 times.
✗ Branch 55 not taken.
 1 COAL_THROW_PRETTY("value (" << value
442 << ") is two small. It should be at least: "
443 << minInflationValue(),
444 std::invalid_argument);
445
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 10 return std::make_pair(Capsule(radius + value, 2 * halfLength),
446
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 15 Transform3s());
447 }
448
449 private:
450 15 virtual bool isEqual(const CollisionGeometry& _other) const {
451
1/2
✓ Branch 0 taken 15 times.
✗ Branch 1 not taken.
 15 const Capsule* other_ptr = dynamic_cast<const Capsule*>(&_other);
452
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 15 times.
 15 if (other_ptr == nullptr) return false;
453 15 const Capsule& other = *other_ptr;
454
455
3/6
✓ Branch 0 taken 15 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 15 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
 30 return radius == other.radius && halfLength == other.halfLength &&
456 30 getSweptSphereRadius() == other.getSweptSphereRadius();
457 }
458
459 public:
460 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
461 };
462
463 /// @brief Cone
464 /// The base of the cone is at \f$ z = - halfLength \f$ and the top is at
465 /// \f$ z = halfLength \f$.
466 class COAL_DLLAPI Cone : public ShapeBase {
467 public:
468 /// @brief Default constructor
469 1 Cone() {}
470
471 1073 Cone(Scalar radius_, Scalar lz_) : ShapeBase(), radius(radius_) {
472 1073 halfLength = lz_ / 2;
473 1073 }
474
475 24 Cone(const Cone& other)
476 24 : ShapeBase(other), radius(other.radius), halfLength(other.halfLength) {}
477
478 /// @brief Clone *this into a new Cone
479 ✗ virtual Cone* clone() const { return new Cone(*this); };
480
481 /// @brief Radius of the cone
482 Scalar radius;
483
484 /// @brief Half Length along z axis
485 Scalar halfLength;
486
487 /// @brief Compute AABB
488 void computeLocalAABB();
489
490 /// @brief Get node type: a cone
491 11952 NODE_TYPE getNodeType() const { return GEOM_CONE; }
492
493 4 Scalar computeVolume() const {
494 4 return boost::math::constants::pi<Scalar>() * radius * radius *
495 4 (halfLength * 2) / 3;
496 }
497
498 2 Matrix3s computeMomentofInertia() const {
499
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 Scalar V = computeVolume();
500 2 Scalar ix =
501 2 V * (Scalar(0.4) * halfLength * halfLength + 3 * radius * radius / 20);
502 2 Scalar iz = Scalar(0.3) * V * radius * radius;
503
504
11/22
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 2 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 2 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 2 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 2 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 2 times.
✗ Branch 29 not taken.
✓ Branch 32 taken 2 times.
✗ Branch 33 not taken.
 2 return (Matrix3s() << ix, 0, 0, 0, ix, 0, 0, 0, iz).finished();
505 }
506
507
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 Vec3s computeCOM() const { return Vec3s(0, 0, -Scalar(0.5) * halfLength); }
508
509 7 Scalar minInflationValue() const { return -(std::min)(radius, halfLength); }
510
511 /// \brief Inflate the cone by an amount given by `value`.
512 /// This value can be positive or negative but must always >=
513 /// `minInflationValue()`.
514 ///
515 /// \param[in] value of the shape inflation.
516 ///
517 /// \returns a new inflated cone and the related transform to account for the
518 /// change of shape frame
519 6 std::pair<Cone, Transform3s> inflated(const Scalar value) const {
520
3/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
✓ Branch 4 taken 5 times.
 6 if (value <= minInflationValue())
521
19/38
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 1 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 1 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 1 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 1 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 1 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 1 times.
✗ Branch 41 not taken.
✓ Branch 43 taken 1 times.
✗ Branch 44 not taken.
✓ Branch 46 taken 1 times.
✗ Branch 47 not taken.
✓ Branch 49 taken 1 times.
✗ Branch 50 not taken.
✓ Branch 53 taken 1 times.
✗ Branch 54 not taken.
✓ Branch 56 taken 1 times.
✗ Branch 57 not taken.
 1 COAL_THROW_PRETTY("value (" << value
522 << ") is two small. It should be at least: "
523 << minInflationValue(),
524 std::invalid_argument);
525
526 // tan(alpha) = 2*halfLength/radius;
527 5 const Scalar tan_alpha = 2 * halfLength / radius;
528 5 const Scalar sin_alpha = tan_alpha / std::sqrt(1 + tan_alpha * tan_alpha);
529 5 const Scalar top_inflation = value / sin_alpha;
530 5 const Scalar bottom_inflation = value;
531
532 5 const Scalar new_lz = 2 * halfLength + top_inflation + bottom_inflation;
533 5 const Scalar new_cz = (top_inflation + bottom_inflation) / Scalar(2);
534 5 const Scalar new_radius = new_lz / tan_alpha;
535
536
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 10 return std::make_pair(Cone(new_radius, new_lz),
537
3/6
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
 15 Transform3s(Vec3s(0., 0., new_cz)));
538 }
539
540 private:
541 14 virtual bool isEqual(const CollisionGeometry& _other) const {
542
1/2
✓ Branch 0 taken 14 times.
✗ Branch 1 not taken.
 14 const Cone* other_ptr = dynamic_cast<const Cone*>(&_other);
543
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 14 times.
 14 if (other_ptr == nullptr) return false;
544 14 const Cone& other = *other_ptr;
545
546
3/6
✓ Branch 0 taken 14 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 14 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 14 times.
✗ Branch 5 not taken.
 28 return radius == other.radius && halfLength == other.halfLength &&
547 28 getSweptSphereRadius() == other.getSweptSphereRadius();
548 }
549
550 public:
551 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
552 };
553
554 /// @brief Cylinder along Z axis.
555 /// The cylinder is defined at its centroid.
556 class COAL_DLLAPI Cylinder : public ShapeBase {
557 public:
558 /// @brief Default constructor
559 2 Cylinder() {}
560
561 5597 Cylinder(Scalar radius_, Scalar lz_) : ShapeBase(), radius(radius_) {
562 5597 halfLength = lz_ / 2;
563 5597 }
564
565 24 Cylinder(const Cylinder& other)
566 24 : ShapeBase(other), radius(other.radius), halfLength(other.halfLength) {}
567
568 1 Cylinder& operator=(const Cylinder& other) {
569
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 if (this == &other) return *this;
570
571 1 this->radius = other.radius;
572 1 this->halfLength = other.halfLength;
573 1 return *this;
574 }
575
576 /// @brief Clone *this into a new Cylinder
577 ✗ virtual Cylinder* clone() const { return new Cylinder(*this); };
578
579 /// @brief Radius of the cylinder
580 Scalar radius;
581
582 /// @brief Half Length along z axis
583 Scalar halfLength;
584
585 /// @brief Compute AABB
586 void computeLocalAABB();
587
588 /// @brief Get node type: a cylinder
589 1482240 NODE_TYPE getNodeType() const { return GEOM_CYLINDER; }
590
591 4 Scalar computeVolume() const {
592 4 return boost::math::constants::pi<Scalar>() * radius * radius *
593 4 (halfLength * 2);
594 }
595
596 2 Matrix3s computeMomentofInertia() const {
597
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 Scalar V = computeVolume();
598 2 Scalar ix = V * (radius * radius / 4 + halfLength * halfLength / 3);
599 2 Scalar iz = V * radius * radius / 2;
600
11/22
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 2 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 2 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 2 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 2 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 2 times.
✗ Branch 29 not taken.
✓ Branch 32 taken 2 times.
✗ Branch 33 not taken.
 2 return (Matrix3s() << ix, 0, 0, 0, ix, 0, 0, 0, iz).finished();
601 }
602
603 7 Scalar minInflationValue() const { return -(std::min)(radius, halfLength); }
604
605 /// \brief Inflate the cylinder by an amount given by `value`.
606 /// This value can be positive or negative but must always >=
607 /// `minInflationValue()`.
608 ///
609 /// \param[in] value of the shape inflation.
610 ///
611 /// \returns a new inflated cylinder and the related transform to account for
612 /// the change of shape frame
613 6 std::pair<Cylinder, Transform3s> inflated(const Scalar value) const {
614
2/2
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 5 times.
 6 if (value <= minInflationValue())
615
19/38
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 1 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 1 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 1 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 1 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 1 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 1 times.
✗ Branch 41 not taken.
✓ Branch 43 taken 1 times.
✗ Branch 44 not taken.
✓ Branch 46 taken 1 times.
✗ Branch 47 not taken.
✓ Branch 49 taken 1 times.
✗ Branch 50 not taken.
✓ Branch 53 taken 1 times.
✗ Branch 54 not taken.
✓ Branch 56 taken 1 times.
✗ Branch 57 not taken.
 1 COAL_THROW_PRETTY("value (" << value
616 << ") is two small. It should be at least: "
617 << minInflationValue(),
618 std::invalid_argument);
619
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 10 return std::make_pair(Cylinder(radius + value, 2 * (halfLength + value)),
620
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 15 Transform3s());
621 }
622
623 private:
624 15 virtual bool isEqual(const CollisionGeometry& _other) const {
625
1/2
✓ Branch 0 taken 15 times.
✗ Branch 1 not taken.
 15 const Cylinder* other_ptr = dynamic_cast<const Cylinder*>(&_other);
626
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 15 times.
 15 if (other_ptr == nullptr) return false;
627 15 const Cylinder& other = *other_ptr;
628
629
3/6
✓ Branch 0 taken 15 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 15 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
 30 return radius == other.radius && halfLength == other.halfLength &&
630 30 getSweptSphereRadius() == other.getSweptSphereRadius();
631 }
632
633 public:
634 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
635 };
636
637 template <typename _IndexType>
638 struct ConvexBaseTplNeighbors {
639 typedef _IndexType IndexType;
640
641 unsigned char count;
642 IndexType begin_id;
643
644 16 bool operator==(const ConvexBaseTplNeighbors& other) const {
645
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 12 times.
 16 if (count != other.count) return false;
646
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 12 times.
 16 if (begin_id != other.begin_id) return false;
647
648 16 return true;
649 }
650
651 16 bool operator!=(const ConvexBaseTplNeighbors& other) const {
652 16 return !(*this == other);
653 }
654 };
655
656 // The support warm start polytope contains certain points of `this`
657 // which are support points in specific directions of space.
658 // This struct is used to warm start the support function computation for
659 // large meshes (`num_points` > 32).
660 template <typename _IndexType>
661 struct ConvexBaseTplSupportWarmStartPolytope {
662 typedef _IndexType IndexType;
663
664 // Array of support points to warm start the support function
665 // computation.
666 std::vector<Vec3s> points;
667
668 // Indices of the support points warm starts.
669 // These are the indices of the real convex, not the indices of points in
670 // the warm start polytope.
671 std::vector<IndexType> indices;
672
673 // Cast to a different index type.
674 template <typename OtherIndexType>
675 3 ConvexBaseTplSupportWarmStartPolytope<OtherIndexType> cast() const {
676 typedef ConvexBaseTplSupportWarmStartPolytope<OtherIndexType> ResType;
677 3 ResType res;
678
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 3 res.points = this->points;
679 3 res.indices.clear();
680
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
 3 for (size_t i = 0; i < this->indices.size(); ++i) {
681 ✗ res.indices.push_back(OtherIndexType(this->indices[i]));
682 }
683 3 return res;
684 ✗ }
685 };
686
687 /// @brief Base for convex polytope.
688 /// @tparam _IndexType type of vertices indexes.
689 /// @note Inherited classes are responsible for filling ConvexBase::neighbors;
690 template <typename _IndexType>
691 class ConvexBaseTpl : public ShapeBase {
692 public:
693 // clang-format off
694 COAL_DEPRECATED_MESSAGE(Use IndexType) typedef _IndexType index_type;
695 // clang-format on
696 typedef _IndexType IndexType;
697 typedef ShapeBase Base;
698
699 template <typename OtherIndexType>
700 friend class ConvexBaseTpl;
701
702 /// @brief Build a convex hull based on Qhull library
703 /// and store the vertices and optionally the triangles
704 /// \param points, num_points the points whose convex hull should be computed.
705 /// \param keepTriangles if \c true, returns a Convex<Triangle> object which
706 /// contains the triangle of the shape.
707 /// \param qhullCommand the command sent to qhull.
708 /// - if \c keepTriangles is \c true, this parameter should include
709 /// "Qt". If \c NULL, "Qt" is passed to Qhull.
710 /// - if \c keepTriangles is \c false, an empty string is passed to
711 /// Qhull.
712 /// \note Coal must have been compiled with option \c COAL_HAS_QHULL set
713 /// to \c ON.
714 static COAL_DLLAPI ConvexBaseTpl* convexHull(
715 std::shared_ptr<std::vector<Vec3s>>& points, unsigned int num_points,
716 bool keepTriangles, const char* qhullCommand = NULL);
717
718 // TODO(louis): put this method in private sometime in the future.
719 COAL_DEPRECATED static COAL_DLLAPI ConvexBaseTpl* convexHull(
720 const Vec3s* points, unsigned int num_points, bool keepTriangles,
721 const char* qhullCommand = NULL);
722
723 122678 virtual ~ConvexBaseTpl() {}
724
725 /// @brief Cast ConvexBaseTpl to ShapeBase.
726 /// This method should never be marked as virtual
727 21 Base& base() { return static_cast<Base&>(*this); }
728
729 /// @brief Const cast ConvexBaseTpl to ShapeBase.
730 /// This method should never be marked as virtual
731 21 const Base& base() const { return static_cast<const Base&>(*this); }
732
733 /// @brief Copy constructor.
734 /// The copy constructor only shallow copies the data (it copies the shared
735 /// pointers but does not deep clones the data).
736 ConvexBaseTpl(const ConvexBaseTpl& other) { *this = other; }
737
738 /// @brief Copy assignment operator.
739 /// The copy assignment operator shallow copies the data, just as the copy
740 /// constructor.
741 ConvexBaseTpl& operator=(const ConvexBaseTpl& other);
742
743 /// @brief Clone (deep copy).
744 COAL_DEPRECATED_MESSAGE(Use deepcopy instead.)
745 ✗ virtual ConvexBaseTpl* clone() const { return this->deepcopy(); }
746
747 /// @brief Deep copy of the ConvexBaseTpl.
748 /// This method deep copies every field of the class.
749 ✗ virtual ConvexBaseTpl* deepcopy() const {
750 ✗ ConvexBaseTpl* copy = new ConvexBaseTpl();
751 ✗ deepcopy(this, copy);
752 ✗ return copy;
753 }
754
755 /// @brief Cast this ConvexBase vertex indices to OtherIndexType.
756 /// This effectively deep copies this ConvexBaseTpl into a new one.
757 template <typename OtherIndexType>
758 ConvexBaseTpl<OtherIndexType> cast() const {
759 ConvexBaseTpl<OtherIndexType> res;
760 deepcopy(this, &res);
761 return res;
762 }
763
764 /// @brief Compute AABB
765 void computeLocalAABB();
766
767 /// @brief Get node type: a convex polytope
768 NODE_TYPE getNodeType() const;
769
770 #ifdef COAL_HAS_QHULL
771 /// @brief Builds the double description of the convex polytope, i.e. the set
772 /// of hyperplanes which intersection form the polytope.
773 void COAL_DLLAPI buildDoubleDescription();
774 #endif
775
776 using Neighbors = coal::ConvexBaseTplNeighbors<IndexType>;
777
778 /// @brief Get the index of the j-th neighbor of the i-th vertex.
779 96 IndexType neighbor(IndexType i, IndexType j) const {
780
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 48 times.
 96 assert(i < IndexType(num_points));
781 96 const std::vector<Neighbors>& nns = *neighbors;
782 96 IndexType begin_id = nns[i].begin_id;
783 #ifndef NDEBUG
784 96 unsigned char count = nns[i].count;
785
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 48 times.
 96 assert(j < count);
786 #endif
787 96 const std::vector<IndexType>& nns_vec = *nneighbors_;
788 96 return nns_vec[begin_id + j];
789 }
790
791 /// @brief Above this threshold, the convex polytope is considered large.
792 /// This influcences the way the support function is computed.
793 static constexpr size_t num_vertices_large_convex_threshold = 32;
794
795 /// @brief An array of the points of the polygon.
796 std::shared_ptr<std::vector<Vec3s>> points;
797 unsigned int num_points;
798
799 /// @brief An array of the normals of the polygon.
800 std::shared_ptr<std::vector<Vec3s>> normals;
801 /// @brief An array of the offsets to the normals of the polygon.
802 /// Note: there are as many offsets as normals.
803 std::shared_ptr<std::vector<Scalar>> offsets;
804 unsigned int num_normals_and_offsets;
805
806 /// @brief Neighbors of each vertex.
807 /// It is an array of size num_points. For each vertex, it contains the number
808 /// of neighbors and a list of indices pointing to them.
809 std::shared_ptr<std::vector<Neighbors>> neighbors;
810
811 /// @brief center of the convex polytope, this is used for collision: center
812 /// is guaranteed in the internal of the polytope (as it is convex)
813 Vec3s center;
814
815 using SupportWarmStartPolytope =
816 ConvexBaseTplSupportWarmStartPolytope<IndexType>;
817
818 /// @brief Number of support warm starts.
819 static constexpr size_t num_support_warm_starts = 14;
820
821 /// @brief Support warm start polytopes.
822 SupportWarmStartPolytope support_warm_starts;
823
824 protected:
825 /// @brief Construct an uninitialized convex object
826 /// Initialization is done with ConvexBase::initialize.
827 61346 ConvexBaseTpl()
828 : ShapeBase(),
829 61346 num_points(0),
830 61346 num_normals_and_offsets(0),
831
2/4
✓ Branch 6 taken 61339 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 61339 times.
✗ Branch 10 not taken.
 61346 center(Vec3s::Zero()) {}
832
833 /// @brief Initialize the points of the convex shape
834 /// This also initializes the ConvexBase::center.
835 ///
836 /// \param points_ list of 3D points ///
837 /// \param num_points_ number of 3D points
838 void initialize(std::shared_ptr<std::vector<Vec3s>> points_,
839 unsigned int num_points_);
840
841 /// @brief Set the points of the convex shape.
842 ///
843 /// \param points_ list of 3D points ///
844 /// \param num_points_ number of 3D points
845 void set(std::shared_ptr<std::vector<Vec3s>> points_,
846 unsigned int num_points_);
847
848 #ifdef COAL_HAS_QHULL
849 void COAL_DLLAPI
850 buildDoubleDescriptionFromQHullResult(const orgQhull::Qhull& qh);
851 #endif
852
853 /// @brief Build the support points warm starts.
854 void COAL_DLLAPI buildSupportWarmStart();
855
856 /// @brief Array of indices of the neighbors of each vertex.
857 /// Since we don't know a priori the number of neighbors of each vertex, we
858 /// store the indices of the neighbors in a single array.
859 /// The `neighbors` attribute, an array of `Neighbors`, is used to point each
860 /// vertex to the right indices in the `nneighbors_` array.
861 std::shared_ptr<std::vector<IndexType>> nneighbors_;
862
863 protected:
864 /// @brief Deep copy of a ConvexBaseTpl.
865 /// This method deep copies every field of the class.
866 template <typename OtherIndexType>
867 static void deepcopy(const ConvexBaseTpl<IndexType>* source,
868 ConvexBaseTpl<OtherIndexType>* copy);
869
870 void computeCenter();
871
872 3 virtual bool isEqual(const CollisionGeometry& _other) const {
873 3 const ConvexBaseTpl* other_ptr =
874
1/2
✓ Branch 0 taken 2 times.
✗ Branch 1 not taken.
 3 dynamic_cast<const ConvexBaseTpl*>(&_other);
875
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 2 times.
 3 if (other_ptr == nullptr) return false;
876 3 const ConvexBaseTpl& other = *other_ptr;
877
878
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 2 times.
 3 if (num_points != other.num_points) return false;
879
880
3/8
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 2 times.
 9 if ((!(points.get()) && other.points.get()) ||
881
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 2 times.
 6 (points.get() && !(other.points.get())))
882 ✗ return false;
883
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
 3 if (points.get() && other.points.get()) {
884 3 const std::vector<Vec3s>& points_ = *points;
885 3 const std::vector<Vec3s>& other_points_ = *(other.points);
886
2/2
✓ Branch 0 taken 12 times.
✓ Branch 1 taken 2 times.
 19 for (unsigned int i = 0; i < num_points; ++i) {
887
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 12 times.
 16 if (points_[i] != (other_points_)[i]) return false;
888 }
889 }
890
891
3/8
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 2 times.
 9 if ((!(neighbors.get()) && other.neighbors.get()) ||
892
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 2 times.
 6 (neighbors.get() && !(other.neighbors.get())))
893 ✗ return false;
894
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
 3 if (neighbors.get() && other.neighbors.get()) {
895 3 const std::vector<Neighbors>& neighbors_ = *neighbors;
896 3 const std::vector<Neighbors>& other_neighbors_ = *(other.neighbors);
897
2/2
✓ Branch 0 taken 12 times.
✓ Branch 1 taken 2 times.
 19 for (unsigned int i = 0; i < num_points; ++i) {
898
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 12 times.
 16 if (neighbors_[i] != other_neighbors_[i]) return false;
899 }
900 }
901
902
4/8
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 2 times.
 6 if ((!(normals.get()) && other.normals.get()) ||
903
0/2
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 3 (normals.get() && !(other.normals.get())))
904 ✗ return false;
905
2/6
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 2 times.
 3 if (normals.get() && other.normals.get()) {
906 ✗ const std::vector<Vec3s>& normals_ = *normals;
907 ✗ const std::vector<Vec3s>& other_normals_ = *(other.normals);
908 ✗ for (unsigned int i = 0; i < num_normals_and_offsets; ++i) {
909 ✗ if (normals_[i] != other_normals_[i]) return false;
910 }
911 }
912
913
4/8
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 2 times.
 6 if ((!(offsets.get()) && other.offsets.get()) ||
914
0/2
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 3 (offsets.get() && !(other.offsets.get())))
915 ✗ return false;
916
2/6
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 2 times.
 3 if (offsets.get() && other.offsets.get()) {
917 ✗ const std::vector<Scalar>& offsets_ = *offsets;
918 ✗ const std::vector<Scalar>& other_offsets_ = *(other.offsets);
919 ✗ for (unsigned int i = 0; i < num_normals_and_offsets; ++i) {
920 ✗ if (offsets_[i] != other_offsets_[i]) return false;
921 }
922 }
923
924 3 if (this->support_warm_starts.points.size() !=
925
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 2 times.
 6 other.support_warm_starts.points.size() ||
926 3 this->support_warm_starts.indices.size() !=
927 3 other.support_warm_starts.indices.size()) {
928 ✗ return false;
929 }
930
931
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
 3 for (size_t i = 0; i < this->support_warm_starts.points.size(); ++i) {
932 ✗ if (this->support_warm_starts.points[i] !=
933 ✗ other.support_warm_starts.points[i] ||
934 ✗ this->support_warm_starts.indices[i] !=
935 ✗ other.support_warm_starts.indices[i]) {
936 ✗ return false;
937 }
938 }
939
940
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
 6 return center == other.center &&
941 6 getSweptSphereRadius() == other.getSweptSphereRadius();
942 }
943
944 public:
945 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
946 };
947
948 typedef ConvexBaseTpl<Triangle16::IndexType> ConvexBase16;
949 typedef ConvexBaseTpl<Triangle32::IndexType> ConvexBase32;
950 // typedef ConvexBase32 ConvexBase;
951
952 template <typename PolygonT>
953 class ConvexTpl;
954
955 /// @brief Half Space: this is equivalent to the Plane in ODE.
956 /// A Half space has a priviledged direction: the direction of the normal.
957 /// The separation plane is defined as n * x = d; Points in the negative side of
958 /// the separation plane (i.e. {x | n * x < d}) are inside the half space and
959 /// points in the positive side of the separation plane (i.e. {x | n * x > d})
960 /// are outside the half space.
961 /// Note: prefer using a Halfspace instead of a Plane if possible, it has better
962 /// behavior w.r.t. collision detection algorithms.
963 class COAL_DLLAPI Halfspace : public ShapeBase {
964 public:
965 /// @brief Construct a half space with normal direction and offset
966
1/2
✓ Branch 2 taken 17635 times.
✗ Branch 3 not taken.
 17635 Halfspace(const Vec3s& n_, Scalar d_) : ShapeBase(), n(n_), d(d_) {
967
1/2
✓ Branch 1 taken 17635 times.
✗ Branch 2 not taken.
 17635 unitNormalTest();
968 17635 }
969
970 /// @brief Construct a plane with normal direction and offset
971 5 Halfspace(Scalar a, Scalar b, Scalar c, Scalar d_)
972
1/2
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
 5 : ShapeBase(), n(a, b, c), d(d_) {
973
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 unitNormalTest();
974 5 }
975
976
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 Halfspace() : ShapeBase(), n(1, 0, 0), d(0) {}
977
978 20 Halfspace(const Halfspace& other)
979
1/2
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
 20 : ShapeBase(other), n(other.n), d(other.d) {}
980
981 /// @brief operator =
982 6 Halfspace& operator=(const Halfspace& other) {
983 6 n = other.n;
984 6 d = other.d;
985 6 return *this;
986 }
987
988 /// @brief Clone *this into a new Halfspace
989 ✗ virtual Halfspace* clone() const { return new Halfspace(*this); };
990
991 29376 Scalar signedDistance(const Vec3s& p) const {
992 29376 return n.dot(p) - (d + this->getSweptSphereRadius());
993 }
994
995 11925 Scalar distance(const Vec3s& p) const {
996 11925 return std::abs(this->signedDistance(p));
997 }
998
999 /// @brief Compute AABB
1000 void computeLocalAABB();
1001
1002 /// @brief Get node type: a half space
1003 6647 NODE_TYPE getNodeType() const { return GEOM_HALFSPACE; }
1004
1005 5 Scalar minInflationValue() const {
1006 5 return std::numeric_limits<Scalar>::lowest();
1007 }
1008
1009 /// \brief Inflate the halfspace by an amount given by `value`.
1010 /// This value can be positive or negative but must always >=
1011 /// `minInflationValue()`.
1012 ///
1013 /// \param[in] value of the shape inflation.
1014 ///
1015 /// \returns a new inflated halfspace and the related transform to account for
1016 /// the change of shape frame
1017 5 std::pair<Halfspace, Transform3s> inflated(const Scalar value) const {
1018
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 5 times.
 5 if (value <= minInflationValue())
1019 ✗ COAL_THROW_PRETTY("value (" << value
1020 << ") is two small. It should be at least: "
1021 << minInflationValue(),
1022 std::invalid_argument);
1023
3/6
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
 5 return std::make_pair(Halfspace(n, d + value), Transform3s());
1024 }
1025
1026 /// @brief Plane normal
1027 Vec3s n;
1028
1029 /// @brief Plane offset
1030 Scalar d;
1031
1032 protected:
1033 /// @brief Turn non-unit normal into unit
1034 void unitNormalTest();
1035
1036 private:
1037 15 virtual bool isEqual(const CollisionGeometry& _other) const {
1038
1/2
✓ Branch 0 taken 15 times.
✗ Branch 1 not taken.
 15 const Halfspace* other_ptr = dynamic_cast<const Halfspace*>(&_other);
1039
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 15 times.
 15 if (other_ptr == nullptr) return false;
1040 15 const Halfspace& other = *other_ptr;
1041
1042
3/6
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 15 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 15 times.
✗ Branch 6 not taken.
 30 return n == other.n && d == other.d &&
1043 30 getSweptSphereRadius() == other.getSweptSphereRadius();
1044 }
1045
1046 public:
1047 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
1048 };
1049
1050 /// @brief Infinite plane.
1051 /// A plane can be viewed as two half spaces; it has no priviledged direction.
1052 /// Note: prefer using a Halfspace instead of a Plane if possible, it has better
1053 /// behavior w.r.t. collision detection algorithms.
1054 class COAL_DLLAPI Plane : public ShapeBase {
1055 public:
1056 /// @brief Construct a plane with normal direction and offset
1057
1/2
✓ Branch 2 taken 255 times.
✗ Branch 3 not taken.
 255 Plane(const Vec3s& n_, Scalar d_) : ShapeBase(), n(n_), d(d_) {
1058
1/2
✓ Branch 1 taken 255 times.
✗ Branch 2 not taken.
 255 unitNormalTest();
1059 255 }
1060
1061 /// @brief Construct a plane with normal direction and offset
1062 ✗ Plane(Scalar a, Scalar b, Scalar c, Scalar d_)
1063 ✗ : ShapeBase(), n(a, b, c), d(d_) {
1064 ✗ unitNormalTest();
1065 ✗ }
1066
1067
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 Plane() : ShapeBase(), n(1, 0, 0), d(0) {}
1068
1069
1/2
✓ Branch 2 taken 15 times.
✗ Branch 3 not taken.
 15 Plane(const Plane& other) : ShapeBase(other), n(other.n), d(other.d) {}
1070
1071 /// @brief operator =
1072 6 Plane& operator=(const Plane& other) {
1073 6 n = other.n;
1074 6 d = other.d;
1075 6 return *this;
1076 }
1077
1078 /// @brief Clone *this into a new Plane
1079 ✗ virtual Plane* clone() const { return new Plane(*this); };
1080
1081 87 Scalar signedDistance(const Vec3s& p) const {
1082 87 const Scalar dist = n.dot(p) - d;
1083 87 Scalar signed_dist = std::abs(n.dot(p) - d) - this->getSweptSphereRadius();
1084
2/2
✓ Branch 0 taken 62 times.
✓ Branch 1 taken 25 times.
 87 if (dist >= 0) {
1085 62 return signed_dist;
1086 }
1087
1/2
✓ Branch 0 taken 25 times.
✗ Branch 1 not taken.
 25 if (signed_dist >= 0) {
1088 25 return -signed_dist;
1089 }
1090 ✗ return signed_dist;
1091 }
1092
1093 36 Scalar distance(const Vec3s& p) const {
1094 36 return std::abs(std::abs(n.dot(p) - d) - this->getSweptSphereRadius());
1095 }
1096
1097 /// @brief Compute AABB
1098 void computeLocalAABB();
1099
1100 /// @brief Get node type: a plane
1101 5844 NODE_TYPE getNodeType() const { return GEOM_PLANE; }
1102
1103 /// @brief Plane normal
1104 Vec3s n;
1105
1106 /// @brief Plane offset
1107 Scalar d;
1108
1109 protected:
1110 /// @brief Turn non-unit normal into unit
1111 void unitNormalTest();
1112
1113 private:
1114 15 virtual bool isEqual(const CollisionGeometry& _other) const {
1115
1/2
✓ Branch 0 taken 15 times.
✗ Branch 1 not taken.
 15 const Plane* other_ptr = dynamic_cast<const Plane*>(&_other);
1116
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 15 times.
 15 if (other_ptr == nullptr) return false;
1117 15 const Plane& other = *other_ptr;
1118
1119
3/6
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 15 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 15 times.
✗ Branch 6 not taken.
 30 return n == other.n && d == other.d &&
1120 30 getSweptSphereRadius() == other.getSweptSphereRadius();
1121 }
1122
1123 public:
1124 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
1125 };
1126
1127 /** @} */ // end of Geometric_Shapes
1128
1129 } // namespace coal
1130
1131 #include "coal/shape/geometric_shapes.hxx"
1132
1133 #endif
1134