Directory: | ./ |
---|---|
File: | include/coal/shape/geometric_shapes.h |
Date: | 2025-04-01 09:23:31 |
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 | 2547595 | 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 | 61509718 | 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 | 2734874 | 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 | 793642 | 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 | 1482220 | 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 |