GCC Code Coverage Report

Directory: ./
File: include/coal/shape/geometric_shapes.h
Date: 2025-04-13 14:25:19
Exec Total Coverage
Lines: 294 343 85.7%
Branches: 329 776 42.4%
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 116923186 ShapeBase() {}
61
62 ///  \brief Copy constructor
63 200 ShapeBase(const ShapeBase& other)
64 200 : CollisionGeometry(other),
65 200 m_swept_sphere_radius(other.m_swept_sphere_radius) {}
66
67 19 ShapeBase& operator=(const ShapeBase& other) = default;
68
69 233846640 virtual ~ShapeBase() {};
70
71 /// @brief Get object type: a geometric shape
72 2547601 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 61509724 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 2734865 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.
 4 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
1/2
✓ Branch 2 taken 5 times.
✗ Branch 3 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 793645 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.
 4 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
2/4
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
 12 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
1/2
✓ Branch 2 taken 5 times.
✗ Branch 3 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.
 4 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
1/2
✓ Branch 2 taken 5 times.
✗ Branch 3 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.
 4 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 1482232 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.
 4 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
1/2
✓ Branch 2 taken 5 times.
✗ Branch 3 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 /// @brief Base for convex polytope.
638 /// @note Inherited classes are responsible for filling ConvexBase::neighbors;
639 class COAL_DLLAPI ConvexBase : public ShapeBase {
640 public:
641 /// @brief Build a convex hull based on Qhull library
642 /// and store the vertices and optionally the triangles
643 /// \param points, num_points the points whose convex hull should be computed.
644 /// \param keepTriangles if \c true, returns a Convex<Triangle> object which
645 /// contains the triangle of the shape.
646 /// \param qhullCommand the command sent to qhull.
647 /// - if \c keepTriangles is \c true, this parameter should include
648 /// "Qt". If \c NULL, "Qt" is passed to Qhull.
649 /// - if \c keepTriangles is \c false, an empty string is passed to
650 /// Qhull.
651 /// \note Coal must have been compiled with option \c COAL_HAS_QHULL set
652 /// to \c ON.
653 static ConvexBase* convexHull(std::shared_ptr<std::vector<Vec3s>>& points,
654 unsigned int num_points, bool keepTriangles,
655 const char* qhullCommand = NULL);
656
657 // TODO(louis): put this method in private sometime in the future.
658 COAL_DEPRECATED static ConvexBase* convexHull(
659 const Vec3s* points, unsigned int num_points, bool keepTriangles,
660 const char* qhullCommand = NULL);
661
662 virtual ~ConvexBase();
663
664 /// @brief Clone (deep copy).
665 /// This method is consistent with BVHModel `clone` method.
666 /// The copy constructor is called, which duplicates the data.
667 ✗ virtual ConvexBase* clone() const { return new ConvexBase(*this); }
668
669 /// @brief Compute AABB
670 void computeLocalAABB();
671
672 /// @brief Get node type: a convex polytope
673 168873 NODE_TYPE getNodeType() const { return GEOM_CONVEX; }
674
675 #ifdef COAL_HAS_QHULL
676 /// @brief Builds the double description of the convex polytope, i.e. the set
677 /// of hyperplanes which intersection form the polytope.
678 void buildDoubleDescription();
679 #endif
680
681 struct COAL_DLLAPI Neighbors {
682 unsigned char count_;
683 unsigned int* n_;
684
685 8 unsigned char const& count() const { return count_; }
686 24 unsigned int& operator[](int i) {
687
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 24 times.
 24 assert(i < count_);
688 24 return n_[i];
689 }
690 ✗ unsigned int const& operator[](int i) const {
691 ✗ assert(i < count_);
692 ✗ return n_[i];
693 }
694
695 12 bool operator==(const Neighbors& other) const {
696
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 12 times.
 12 if (count_ != other.count_) return false;
697
698
2/2
✓ Branch 0 taken 48 times.
✓ Branch 1 taken 12 times.
 60 for (int i = 0; i < count_; ++i) {
699
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 48 times.
 48 if (n_[i] != other.n_[i]) return false;
700 }
701
702 12 return true;
703 }
704
705 12 bool operator!=(const Neighbors& other) const { return !(*this == other); }
706 };
707
708 /// @brief Above this threshold, the convex polytope is considered large.
709 /// This influcences the way the support function is computed.
710 static constexpr size_t num_vertices_large_convex_threshold = 32;
711
712 /// @brief An array of the points of the polygon.
713 std::shared_ptr<std::vector<Vec3s>> points;
714 unsigned int num_points;
715
716 /// @brief An array of the normals of the polygon.
717 std::shared_ptr<std::vector<Vec3s>> normals;
718 /// @brief An array of the offsets to the normals of the polygon.
719 /// Note: there are as many offsets as normals.
720 std::shared_ptr<std::vector<Scalar>> offsets;
721 unsigned int num_normals_and_offsets;
722
723 /// @brief Neighbors of each vertex.
724 /// It is an array of size num_points. For each vertex, it contains the number
725 /// of neighbors and a list of indices pointing to them.
726 std::shared_ptr<std::vector<Neighbors>> neighbors;
727
728 /// @brief center of the convex polytope, this is used for collision: center
729 /// is guaranteed in the internal of the polytope (as it is convex)
730 Vec3s center;
731
732 /// @brief The support warm start polytope contains certain points of `this`
733 /// which are support points in specific directions of space.
734 /// This struct is used to warm start the support function computation for
735 /// large meshes (`num_points` > 32).
736 struct SupportWarmStartPolytope {
737 /// @brief Array of support points to warm start the support function
738 /// computation.
739 std::vector<Vec3s> points;
740
741 /// @brief Indices of the support points warm starts.
742 /// These are the indices of the real convex, not the indices of points in
743 /// the warm start polytope.
744 std::vector<int> indices;
745 };
746
747 /// @brief Number of support warm starts.
748 static constexpr size_t num_support_warm_starts = 14;
749
750 /// @brief Support warm start polytopes.
751 SupportWarmStartPolytope support_warm_starts;
752
753 protected:
754 /// @brief Construct an uninitialized convex object
755 /// Initialization is done with ConvexBase::initialize.
756 61319 ConvexBase()
757 61319 : ShapeBase(),
758 61319 num_points(0),
759 61319 num_normals_and_offsets(0),
760
2/4
✓ Branch 6 taken 61319 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 61319 times.
✗ Branch 10 not taken.
 61319 center(Vec3s::Zero()) {}
761
762 /// @brief Initialize the points of the convex shape
763 /// This also initializes the ConvexBase::center.
764 ///
765 /// \param ownStorage weither the ConvexBase owns the data.
766 /// \param points_ list of 3D points ///
767 /// \param num_points_ number of 3D points
768 void initialize(std::shared_ptr<std::vector<Vec3s>> points_,
769 unsigned int num_points_);
770
771 /// @brief Set the points of the convex shape.
772 ///
773 /// \param ownStorage weither the ConvexBase owns the data.
774 /// \param points_ list of 3D points ///
775 /// \param num_points_ number of 3D points
776 void set(std::shared_ptr<std::vector<Vec3s>> points_,
777 unsigned int num_points_);
778
779 /// @brief Copy constructor
780 /// Only the list of neighbors is copied.
781 ConvexBase(const ConvexBase& other);
782
783 #ifdef COAL_HAS_QHULL
784 void buildDoubleDescriptionFromQHullResult(const orgQhull::Qhull& qh);
785 #endif
786
787 /// @brief Build the support points warm starts.
788 void buildSupportWarmStart();
789
790 /// @brief Array of indices of the neighbors of each vertex.
791 /// Since we don't know a priori the number of neighbors of each vertex, we
792 /// store the indices of the neighbors in a single array.
793 /// The `neighbors` attribute, an array of `Neighbors`, is used to point each
794 /// vertex to the right indices in the `nneighbors_` array.
795 std::shared_ptr<std::vector<unsigned int>> nneighbors_;
796
797 private:
798 void computeCenter();
799
800 2 virtual bool isEqual(const CollisionGeometry& _other) const {
801
1/2
✓ Branch 0 taken 2 times.
✗ Branch 1 not taken.
 2 const ConvexBase* other_ptr = dynamic_cast<const ConvexBase*>(&_other);
802
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 2 times.
 2 if (other_ptr == nullptr) return false;
803 2 const ConvexBase& other = *other_ptr;
804
805
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 2 times.
 2 if (num_points != other.num_points) return false;
806
807
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.
 6 if ((!(points.get()) && other.points.get()) ||
808
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 2 times.
 4 (points.get() && !(other.points.get())))
809 ✗ return false;
810
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.
 2 if (points.get() && other.points.get()) {
811 2 const std::vector<Vec3s>& points_ = *points;
812 2 const std::vector<Vec3s>& other_points_ = *(other.points);
813
2/2
✓ Branch 0 taken 12 times.
✓ Branch 1 taken 2 times.
 14 for (unsigned int i = 0; i < num_points; ++i) {
814
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 12 times.
 12 if (points_[i] != (other_points_)[i]) return false;
815 }
816 }
817
818
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.
 6 if ((!(neighbors.get()) && other.neighbors.get()) ||
819
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 2 times.
 4 (neighbors.get() && !(other.neighbors.get())))
820 ✗ return false;
821
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.
 2 if (neighbors.get() && other.neighbors.get()) {
822 2 const std::vector<Neighbors>& neighbors_ = *neighbors;
823 2 const std::vector<Neighbors>& other_neighbors_ = *(other.neighbors);
824
2/2
✓ Branch 0 taken 12 times.
✓ Branch 1 taken 2 times.
 14 for (unsigned int i = 0; i < num_points; ++i) {
825
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 12 times.
 12 if (neighbors_[i] != other_neighbors_[i]) return false;
826 }
827 }
828
829
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.
 4 if ((!(normals.get()) && other.normals.get()) ||
830
0/2
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 2 (normals.get() && !(other.normals.get())))
831 ✗ return false;
832
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.
 2 if (normals.get() && other.normals.get()) {
833 ✗ const std::vector<Vec3s>& normals_ = *normals;
834 ✗ const std::vector<Vec3s>& other_normals_ = *(other.normals);
835 ✗ for (unsigned int i = 0; i < num_normals_and_offsets; ++i) {
836 ✗ if (normals_[i] != other_normals_[i]) return false;
837 }
838 }
839
840
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.
 4 if ((!(offsets.get()) && other.offsets.get()) ||
841
0/2
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 2 (offsets.get() && !(other.offsets.get())))
842 ✗ return false;
843
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.
 2 if (offsets.get() && other.offsets.get()) {
844 ✗ const std::vector<Scalar>& offsets_ = *offsets;
845 ✗ const std::vector<Scalar>& other_offsets_ = *(other.offsets);
846 ✗ for (unsigned int i = 0; i < num_normals_and_offsets; ++i) {
847 ✗ if (offsets_[i] != other_offsets_[i]) return false;
848 }
849 }
850
851 2 if (this->support_warm_starts.points.size() !=
852
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.
 4 other.support_warm_starts.points.size() ||
853 2 this->support_warm_starts.indices.size() !=
854 2 other.support_warm_starts.indices.size()) {
855 ✗ return false;
856 }
857
858
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
 2 for (size_t i = 0; i < this->support_warm_starts.points.size(); ++i) {
859 ✗ if (this->support_warm_starts.points[i] !=
860 ✗ other.support_warm_starts.points[i] ||
861 ✗ this->support_warm_starts.indices[i] !=
862 ✗ other.support_warm_starts.indices[i]) {
863 ✗ return false;
864 }
865 }
866
867
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
 4 return center == other.center &&
868 4 getSweptSphereRadius() == other.getSweptSphereRadius();
869 }
870
871 public:
872 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
873 };
874
875 template <typename PolygonT>
876 class Convex;
877
878 /// @brief Half Space: this is equivalent to the Plane in ODE.
879 /// A Half space has a priviledged direction: the direction of the normal.
880 /// The separation plane is defined as n * x = d; Points in the negative side of
881 /// the separation plane (i.e. {x | n * x < d}) are inside the half space and
882 /// points in the positive side of the separation plane (i.e. {x | n * x > d})
883 /// are outside the half space.
884 /// Note: prefer using a Halfspace instead of a Plane if possible, it has better
885 /// behavior w.r.t. collision detection algorithms.
886 class COAL_DLLAPI Halfspace : public ShapeBase {
887 public:
888 /// @brief Construct a half space with normal direction and offset
889
1/2
✓ Branch 2 taken 17635 times.
✗ Branch 3 not taken.
 17635 Halfspace(const Vec3s& n_, Scalar d_) : ShapeBase(), n(n_), d(d_) {
890
1/2
✓ Branch 1 taken 17635 times.
✗ Branch 2 not taken.
 17635 unitNormalTest();
891 17635 }
892
893 /// @brief Construct a plane with normal direction and offset
894 5 Halfspace(Scalar a, Scalar b, Scalar c, Scalar d_)
895
1/2
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
 5 : ShapeBase(), n(a, b, c), d(d_) {
896
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 unitNormalTest();
897 5 }
898
899
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 Halfspace() : ShapeBase(), n(1, 0, 0), d(0) {}
900
901 20 Halfspace(const Halfspace& other)
902
1/2
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
 20 : ShapeBase(other), n(other.n), d(other.d) {}
903
904 /// @brief operator =
905 6 Halfspace& operator=(const Halfspace& other) {
906 6 n = other.n;
907 6 d = other.d;
908 6 return *this;
909 }
910
911 /// @brief Clone *this into a new Halfspace
912 ✗ virtual Halfspace* clone() const { return new Halfspace(*this); };
913
914 29376 Scalar signedDistance(const Vec3s& p) const {
915 29376 return n.dot(p) - (d + this->getSweptSphereRadius());
916 }
917
918 11925 Scalar distance(const Vec3s& p) const {
919 11925 return std::abs(this->signedDistance(p));
920 }
921
922 /// @brief Compute AABB
923 void computeLocalAABB();
924
925 /// @brief Get node type: a half space
926 6647 NODE_TYPE getNodeType() const { return GEOM_HALFSPACE; }
927
928 5 Scalar minInflationValue() const {
929 5 return std::numeric_limits<Scalar>::lowest();
930 }
931
932 /// \brief Inflate the halfspace by an amount given by `value`.
933 /// This value can be positive or negative but must always >=
934 /// `minInflationValue()`.
935 ///
936 /// \param[in] value of the shape inflation.
937 ///
938 /// \returns a new inflated halfspace and the related transform to account for
939 /// the change of shape frame
940 5 std::pair<Halfspace, Transform3s> inflated(const Scalar value) const {
941
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 5 times.
 5 if (value <= minInflationValue())
942 ✗ COAL_THROW_PRETTY("value (" << value
943 << ") is two small. It should be at least: "
944 << minInflationValue(),
945 std::invalid_argument);
946
2/4
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
 10 return std::make_pair(Halfspace(n, d + value), Transform3s());
947 }
948
949 /// @brief Plane normal
950 Vec3s n;
951
952 /// @brief Plane offset
953 Scalar d;
954
955 protected:
956 /// @brief Turn non-unit normal into unit
957 void unitNormalTest();
958
959 private:
960 15 virtual bool isEqual(const CollisionGeometry& _other) const {
961
1/2
✓ Branch 0 taken 15 times.
✗ Branch 1 not taken.
 15 const Halfspace* other_ptr = dynamic_cast<const Halfspace*>(&_other);
962
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 15 times.
 15 if (other_ptr == nullptr) return false;
963 15 const Halfspace& other = *other_ptr;
964
965
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 &&
966 30 getSweptSphereRadius() == other.getSweptSphereRadius();
967 }
968
969 public:
970 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
971 };
972
973 /// @brief Infinite plane.
974 /// A plane can be viewed as two half spaces; it has no priviledged direction.
975 /// Note: prefer using a Halfspace instead of a Plane if possible, it has better
976 /// behavior w.r.t. collision detection algorithms.
977 class COAL_DLLAPI Plane : public ShapeBase {
978 public:
979 /// @brief Construct a plane with normal direction and offset
980
1/2
✓ Branch 2 taken 255 times.
✗ Branch 3 not taken.
 255 Plane(const Vec3s& n_, Scalar d_) : ShapeBase(), n(n_), d(d_) {
981
1/2
✓ Branch 1 taken 255 times.
✗ Branch 2 not taken.
 255 unitNormalTest();
982 255 }
983
984 /// @brief Construct a plane with normal direction and offset
985 ✗ Plane(Scalar a, Scalar b, Scalar c, Scalar d_)
986 ✗ : ShapeBase(), n(a, b, c), d(d_) {
987 ✗ unitNormalTest();
988 }
989
990
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 Plane() : ShapeBase(), n(1, 0, 0), d(0) {}
991
992
1/2
✓ Branch 2 taken 15 times.
✗ Branch 3 not taken.
 15 Plane(const Plane& other) : ShapeBase(other), n(other.n), d(other.d) {}
993
994 /// @brief operator =
995 6 Plane& operator=(const Plane& other) {
996 6 n = other.n;
997 6 d = other.d;
998 6 return *this;
999 }
1000
1001 /// @brief Clone *this into a new Plane
1002 ✗ virtual Plane* clone() const { return new Plane(*this); };
1003
1004 87 Scalar signedDistance(const Vec3s& p) const {
1005 87 const Scalar dist = n.dot(p) - d;
1006 87 Scalar signed_dist = std::abs(n.dot(p) - d) - this->getSweptSphereRadius();
1007
2/2
✓ Branch 0 taken 62 times.
✓ Branch 1 taken 25 times.
 87 if (dist >= 0) {
1008 62 return signed_dist;
1009 }
1010
1/2
✓ Branch 0 taken 25 times.
✗ Branch 1 not taken.
 25 if (signed_dist >= 0) {
1011 25 return -signed_dist;
1012 }
1013 ✗ return signed_dist;
1014 }
1015
1016 36 Scalar distance(const Vec3s& p) const {
1017 36 return std::abs(std::abs(n.dot(p) - d) - this->getSweptSphereRadius());
1018 }
1019
1020 /// @brief Compute AABB
1021 void computeLocalAABB();
1022
1023 /// @brief Get node type: a plane
1024 5844 NODE_TYPE getNodeType() const { return GEOM_PLANE; }
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 Plane* other_ptr = dynamic_cast<const Plane*>(&_other);
1039
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 15 times.
 15 if (other_ptr == nullptr) return false;
1040 15 const Plane& 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 } // namespace coal
1051
1052 #endif
1053