GCC Code Coverage Report

Directory: ./
File: include/hpp/constraints/convex-shape.hh
Date: 2025-05-05 12:19:30
Exec Total Coverage
Lines: 111 145 76.6%
Branches: 94 204 46.1%
Line Branch Exec Source
1 // Copyright (c) 2015, LAAS-CNRS
2 // Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
3 //
4
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are
7 // met:
8 //
9 // 1. Redistributions of source code must retain the above copyright
10 // notice, this list of conditions and the following disclaimer.
11 //
12 // 2. Redistributions in binary form must reproduce the above copyright
13 // notice, this list of conditions and the following disclaimer in the
14 // documentation and/or other materials provided with the distribution.
15 //
16 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
17 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
18 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
19 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
20 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
21 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
22 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
23 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
24 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
26 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
27 // DAMAGE.
28
29 #ifndef HPP_CONSTRAINTS_CONVEX_SHAPE_HH
30 #define HPP_CONSTRAINTS_CONVEX_SHAPE_HH
31
32 #include <coal/shape/geometric_shapes.h>
33
34 #include <vector>
35
36 // Only for specialization of vector3_t. This is a bad design of Pinocchio.
37 #include <hpp/constraints/config.hh>
38 #include <hpp/constraints/fwd.hh>
39 #include <hpp/pinocchio/joint.hh>
40 #include <pinocchio/multibody/model.hpp>
41
42 namespace hpp {
43 namespace constraints {
44 /// Return the closest point to point P, on a segment line \f$ A + t*v, t \in
45 /// [0,1] \f$. \param P PA where P is the point to \param A the origin the
46 /// segment line \param v vector presenting the segment line \param[out] B the
47 /// closest point
48 inline void closestPointToSegment(const vector3_t& P, const vector3_t& A,
49 const vector3_t& v, vector3_t& B) {
50 vector3_t w = A - P;
51 value_type c1, c2;
52 c1 = v.dot(w);
53 c2 = v.dot(v);
54 if (c1 <= 0)
55 B = A;
56 else if (c2 <= c1)
57 B = A + v;
58 else
59 B = A + c1 / c2 * v;
60 }
61
62 /// \param A, u point and vector defining the line \f$ A + t*u \f$
63 /// \param P, n point and normal vector defining the plane \f$ Q \in plane
64 /// \Rightleftarrow (P - Q) . n = 0 \f$ \return the intesection point. \warning
65 /// \c u and \c n are expected not to be orthogonal. \todo make this function
66 /// robust to orthogonal inputs.
67 152329 inline vector3_t linePlaneIntersection(const vector3_t& A, const vector3_t& u,
68 const vector3_t& P, const vector3_t& n) {
69
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 152334 times.
 152329 assert(std::abs(n.dot(u)) > 1e-8);
70
6/12
✓ Branch 2 taken 152339 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 152328 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 152337 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 152336 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 152339 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 152330 times.
✗ Branch 18 not taken.
 304664 return A + u * (n.dot(P - A)) / n.dot(u);
71 }
72
73 class HPP_CONSTRAINTS_DLLAPI ConvexShape {
74 public:
75 /// Represent a convex shape
76 /// \param pts a sequence of points lying in a plane. The convex shape is
77 /// obtained by connecting consecutive points (in a circular way)
78 ///
79 /// \note There is no convexity check yet. The order is important:
80 /// The normal is parallel to (pts[1] - pts[0]).cross (pts[2] - pts[1])
81 /// The normal to the segment in the plane are directed outward.
82 /// (pts[i+1] - pts[i]).cross (normalToConvexShape)
83 26 ConvexShape(const std::vector<vector3_t>& pts,
84 JointPtr_t joint = JointPtr_t())
85
4/8
✓ Branch 2 taken 26 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 26 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 26 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 26 times.
✗ Branch 14 not taken.
 26 : Pts_(pts), joint_(joint) {
86
1/2
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
 26 init();
87 26 }
88
89 ConvexShape(const coal::TriangleP& t, const JointPtr_t& joint = JointPtr_t())
90 : Pts_(triangleToPoints(t)), joint_(joint) {
91 init();
92 }
93
94 /// This constructor is required for compatibility with deprecated
95 /// Triangle constructor.
96 1 ConvexShape(const vector3_t& p0, const vector3_t& p1, const vector3_t& p2,
97 const JointPtr_t& joint = JointPtr_t())
98
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
 1 : Pts_(points(p0, p1, p2)), joint_(joint) {
99
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 init();
100 1 }
101
102 // Copy constructor
103
5/10
✓ Branch 2 taken 38601 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 38601 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 38601 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 38600 times.
✗ Branch 14 not taken.
✓ Branch 17 taken 38600 times.
✗ Branch 18 not taken.
 38594 ConvexShape(const ConvexShape& t) : Pts_(t.Pts_), joint_(t.joint_) { init(); }
104
105 14 void reverse() {
106 14 std::reverse(Pts_.begin(), Pts_.end());
107 14 init();
108 14 }
109
110 /// Intersection with a line defined by a point and a vector.
111 /// \param A, u point and vector expressed in the local frame.
112 inline vector3_t intersectionLocal(const vector3_t& A,
113 const vector3_t& u) const {
114 return linePlaneIntersection(A, u, C_, N_);
115 }
116
117 /// As isInside but consider A as expressed in joint frame.
118 23 inline bool isInsideLocal(const vector3_t& Ap) const {
119
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 23 times.
 23 assert(shapeDimension_ > 2);
120
2/2
✓ Branch 0 taken 60 times.
✓ Branch 1 taken 16 times.
 76 for (std::size_t i = 0; i < shapeDimension_; ++i) {
121
3/4
✓ Branch 4 taken 60 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 7 times.
✓ Branch 7 taken 53 times.
 60 if (Ns_[i].dot(Ap - Pts_[i]) > 0) return false;
122 }
123 16 return true;
124 }
125
126 /// Return the shortest distance from a point to the shape
127 /// A negative value means the point is inside the shape
128 /// \param a a point already in the plane containing the convex shape,
129 /// and expressed in the local frame.
130 152370 inline value_type distanceLocal(const vector3_t& a) const {
131
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 152370 times.
 152370 assert(shapeDimension_ > 1);
132 152370 const value_type inf = std::numeric_limits<value_type>::infinity();
133 152370 value_type minPosDist = inf, maxNegDist = -inf;
134 152370 bool outside = false;
135
2/2
✓ Branch 0 taken 610420 times.
✓ Branch 1 taken 152381 times.
 762801 for (std::size_t i = 0; i < shapeDimension_; ++i) {
136
2/4
✓ Branch 6 taken 610432 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 610431 times.
✗ Branch 10 not taken.
 610420 value_type d = dist(a - Pts_[i], Ls_[i], Us_[i], Ns_[i]);
137
2/2
✓ Branch 0 taken 97986 times.
✓ Branch 1 taken 512445 times.
 610431 if (d > 0) {
138 97986 outside = true;
139
2/2
✓ Branch 0 taken 87129 times.
✓ Branch 1 taken 10857 times.
 97986 if (d < minPosDist) minPosDist = d;
140 }
141
4/4
✓ Branch 0 taken 512443 times.
✓ Branch 1 taken 97988 times.
✓ Branch 2 taken 307399 times.
✓ Branch 3 taken 205044 times.
 610431 if (d <= 0 && d > maxNegDist) maxNegDist = d;
142 }
143
2/2
✓ Branch 0 taken 86415 times.
✓ Branch 1 taken 65966 times.
 152381 if (outside) return minPosDist;
144 65966 return maxNegDist;
145 }
146
147 /// Return the X axis of the plane in the joint frame
148 2 inline const vector3_t& planeXaxis() const {
149
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 2 times.
 2 assert(shapeDimension_ > 2);
150 2 return Ns_[0];
151 }
152 /// Return the Y axis of the plane in the joint frame
153 /// The Y axis is aligned with \f$ Pts_[1] - Pts_[0] \f$
154 2 inline const vector3_t& planeYaxis() const {
155
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 2 times.
 2 assert(shapeDimension_ > 2);
156 2 return Us_[0];
157 }
158
159 /// Transform of the shape in the joint frame
160 77112 inline const Transform3s& positionInJoint() const { return MinJoint_; }
161
162 ✗ bool operator==(ConvexShape const& other) const {
163 ✗ if (Pts_ != other.Pts_) return false;
164 ✗ if (shapeDimension_ != other.shapeDimension_) return false;
165 ✗ if (C_ != other.C_) return false;
166 ✗ if (N_ != other.N_) return false;
167 ✗ if (Ns_ != other.Ns_) return false;
168 ✗ if (Us_ != other.Us_) return false;
169 ✗ if (Ls_ != other.Ls_) return false;
170 ✗ if (MinJoint_ != other.MinJoint_) return false;
171 ✗ if (joint_ != other.joint_) return false;
172 ✗ return true;
173 }
174 ✗ bool operator!=(ConvexShape const& other) const {
175 ✗ return !(this->operator==(other));
176 }
177
178 /// The points in the joint frame. It is constant.
179 std::vector<vector3_t> Pts_;
180 size_t shapeDimension_;
181 /// the center in the joint frame. It is constant.
182 vector3_t C_;
183 /// the normal to the shape in the joint frame. It is constant.
184 vector3_t N_;
185 /// Ns_ and Us_ are unit vector, in the plane containing the shape,
186 /// expressed in the joint frame.
187 /// Ns_[i] is normal to edge i, pointing inside.
188 /// Ns_[i] is a vector director of edge i.
189 std::vector<vector3_t> Ns_, Us_;
190 vector_t Ls_;
191 Transform3s MinJoint_;
192 JointPtr_t joint_;
193
194 private:
195 /// Return the distance between the point A and the segment
196 /// [P, c2*v] oriented by u.
197 /// w = PA.
198 610427 inline value_type dist(const vector3_t& w, const value_type& c2,
199 const vector3_t& v, const vector3_t& u) const {
200 value_type c1;
201 610427 c1 = v.dot(w);
202
4/4
✓ Branch 0 taken 98146 times.
✓ Branch 1 taken 512288 times.
✓ Branch 3 taken 11569 times.
✓ Branch 4 taken 86576 times.
 610434 if (c1 <= 0) return (u.dot(w) > 0) ? (w.norm()) : (-w.norm());
203
2/2
✓ Branch 0 taken 98736 times.
✓ Branch 1 taken 413552 times.
 512288 if (c2 <= c1)
204 // TODO: (w - c2 * v).norm() == sqrt((u.dot(w)**2 + (c1 - c2)**2)
205 // second should be cheaper.
206
8/14
✓ Branch 1 taken 11569 times.
✓ Branch 2 taken 87169 times.
✓ Branch 4 taken 11569 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 11569 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 11569 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 87168 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 87168 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 87159 times.
✗ Branch 20 not taken.
 98736 return (u.dot(w) > 0) ? ((w - c2 * v).norm()) : (-(w - c2 * v).norm());
207 413552 return u.dot(w);
208 }
209
210 static std::vector<vector3_t> triangleToPoints(const coal::TriangleP& t) {
211 // TODO
212 // return points (t.a, t.b, t.c);
213 std::vector<vector3_t> ret(3);
214 ret[0] = t.a;
215 ret[1] = t.b;
216 ret[2] = t.c;
217 return ret;
218 }
219 1 static std::vector<vector3_t> points(const vector3_t& p0, const vector3_t& p1,
220 const vector3_t& p2) {
221
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 std::vector<vector3_t> ret(3);
222
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 ret[0] = p0;
223
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 ret[1] = p1;
224
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 ret[2] = p2;
225 1 return ret;
226 }
227
228 38642 void init() {
229 38642 shapeDimension_ = Pts_.size();
230
231
2/4
✗ Branch 0 not taken.
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 38638 times.
 38642 switch (shapeDimension_) {
232 ✗ case 0:
233 ✗ throw std::logic_error("Cannot represent an empty shape.");
234 break;
235 4 case 1:
236 4 C_ = Pts_[0];
237 // The transformation will be (N_, Ns_[0], Us_[0])
238 // Fill vectors so as to be consistent
239
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 N_ = vector3_t(1, 0, 0);
240
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 4 Ns_.push_back(vector3_t(0, 1, 0));
241
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 4 Us_.push_back(vector3_t(0, 0, 1));
242 4 break;
243 ✗ case 2:
244 ✗ Ls_ = vector_t(1);
245 ✗ C_ = (Pts_[0] + Pts_[1]) / 2;
246 // The transformation will be (N_, Ns_[0], Us_[0])
247 // Fill vectors so as to be consistent
248 ✗ Us_.push_back(Pts_[1] - Pts_[0]);
249 ✗ Ls_[0] = Us_[0].norm();
250 ✗ Us_[0].normalize();
251 ✗ if (Us_[0][0] != 0)
252 ✗ N_ = vector3_t(-Us_[0][1], Us_[0][0], 0);
253 else
254 ✗ N_ = vector3_t(0, -Us_[0][2], Us_[0][1]);
255 ✗ N_.normalize();
256 ✗ Ns_.push_back(Us_[0].cross(N_));
257 ✗ Ns_[0].normalize(); // Should be unnecessary
258 ✗ break;
259 38638 default:
260 38638 Ls_ = vector_t(shapeDimension_);
261 38638 C_.setZero();
262
2/2
✓ Branch 2 taken 155312 times.
✓ Branch 3 taken 38638 times.
 193950 for (std::size_t i = 0; i < shapeDimension_; ++i) C_ += Pts_[i];
263 // TODO This is very ugly. Why Eigen does not have the operator/=(int)
264 // ...
265
1/2
✓ Branch 2 taken 38638 times.
✗ Branch 3 not taken.
 38638 C_ /= (value_type)Pts_.size();
266
2/4
✓ Branch 6 taken 38638 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 38638 times.
✗ Branch 10 not taken.
 38638 N_ = (Pts_[1] - Pts_[0]).cross(Pts_[2] - Pts_[1]);
267
2/4
✓ Branch 2 taken 38638 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 38638 times.
 38638 assert(!N_.isZero());
268 38638 N_.normalize();
269
270 38638 Us_.resize(Pts_.size());
271 38638 Ns_.resize(Pts_.size());
272
2/2
✓ Branch 0 taken 155312 times.
✓ Branch 1 taken 38638 times.
 193950 for (std::size_t i = 0; i < shapeDimension_; ++i) {
273
1/2
✓ Branch 5 taken 155312 times.
✗ Branch 6 not taken.
 155312 Us_[i] = Pts_[(i + 1) % shapeDimension_] - Pts_[i];
274 155312 Ls_[i] = Us_[i].norm();
275 155312 Us_[i].normalize();
276 155312 Ns_[i] = Us_[i].cross(N_);
277 155312 Ns_[i].normalize();
278 }
279
2/2
✓ Branch 0 taken 155312 times.
✓ Branch 1 taken 38638 times.
 193950 for (std::size_t i = 0; i < shapeDimension_; ++i) {
280
1/2
✓ Branch 3 taken 155312 times.
✗ Branch 4 not taken.
 155312 assert(Us_[(i + 1) % shapeDimension_].dot(Ns_[i]) < 0 &&
281 "The sequence does not define a convex surface");
282 }
283 38638 break;
284 }
285
286 38642 MinJoint_.translation() = C_;
287
1/2
✓ Branch 3 taken 38642 times.
✗ Branch 4 not taken.
 38642 MinJoint_.rotation().col(0) = N_;
288
1/2
✓ Branch 4 taken 38642 times.
✗ Branch 5 not taken.
 38642 MinJoint_.rotation().col(1) = Ns_[0];
289
1/2
✓ Branch 4 taken 38642 times.
✗ Branch 5 not taken.
 38642 MinJoint_.rotation().col(2) = Us_[0];
290 38642 }
291 };
292
293 struct HPP_CONSTRAINTS_DLLAPI ConvexShapeData {
294 // normal in the world frame
295 vector3_t normal_;
296 // center in the world frame
297 vector3_t center_;
298 // Current joint position
299 Transform3s oMj_;
300
301 /// Compute center and normal in world frame
302 6 inline void updateToCurrentTransform(const ConvexShape& cs) {
303
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 if (cs.joint_ == NULL) {
304 6 oMj_.setIdentity();
305 6 _recompute<true>(cs);
306 } else {
307 ✗ oMj_ = cs.joint_->currentTransformation();
308 ✗ _recompute<false>(cs);
309 }
310 6 }
311
312 /// Compute center and normal in world frame
313 /// Thread safe version.
314 229432 inline void updateToCurrentTransform(const ConvexShape& cs,
315 const pinocchio::DeviceData& d) {
316
2/2
✓ Branch 1 taken 1879 times.
✓ Branch 2 taken 227553 times.
 229432 if (cs.joint_ == NULL) {
317 1879 oMj_.setIdentity();
318 1885 _recompute<true>(cs);
319 } else {
320 227553 oMj_ = cs.joint_->currentTransformation(d);
321 227561 _recompute<false>(cs);
322 }
323 229439 }
324
325 template <bool WorldFrame>
326 458884 inline void _recompute(const ConvexShape& cs) {
327 if (WorldFrame) {
328 3768 center_ = cs.C_;
329 3788 normal_ = cs.N_;
330 } else {
331 455116 center_ = oMj_.act(cs.C_);
332
1/2
✓ Branch 3 taken 227556 times.
✗ Branch 4 not taken.
 455124 normal_ = oMj_.rotation() * cs.N_;
333 }
334 458900 }
335
336 /// Intersection with a line defined by a point and a vector.
337 /// \param A, u point and vector expressed in the world frame.
338 152327 inline vector3_t intersection(const vector3_t& A, const vector3_t& u) const {
339 152327 return linePlaneIntersection(A, u, center_, normal_);
340 }
341
342 /// Check whether the intersection of the line defined by A and u
343 /// onto the plane containing the triangle is inside the triangle.
344 /// \param A, u point and vector in world frame defining the line \f$ A + t*u
345 /// \f$
346 ✗ inline bool isInside(const ConvexShape& cs, const vector3_t& A,
347 const vector3_t& u) const {
348 ✗ return isInside(cs, intersection(A, u));
349 }
350 /// Check whether the point As in world frame is inside the triangle.
351 18 inline bool isInside(const ConvexShape& cs, const vector3_t& Ap) const {
352
2/4
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
 18 if (cs.joint_ == NULL) return cs.isInsideLocal(Ap);
353 ✗ vector3_t Ap_loc = oMj_.actInv(Ap);
354 ✗ return cs.isInsideLocal(Ap_loc);
355 }
356
357 /// \param yaxis vector in world frame to which we should try to align
358 inline Transform3s alignedPositionInJoint(const ConvexShape& cs,
359 vector3_t yaxis) const {
360 assert(cs.shapeDimension_ > 2);
361 // Project vector onto the plane
362 yaxis = oMj_.actInv(yaxis);
363 vector3_t yproj = yaxis - yaxis.dot(cs.N_) * cs.N_;
364 if (yproj.isZero())
365 return cs.MinJoint_;
366 else {
367 Transform3s M;
368 M.translation() = cs.C_;
369 M.rotation().col(0) = cs.N_;
370 M.rotation().col(1) = yaxis;
371 M.rotation().col(2) = cs.N_.cross(yaxis);
372 return M;
373 }
374 }
375
376 /// See ConvexShape::distanceLocal
377 /// \param a a point already in the plane containing the convex shape,
378 /// and expressed in the global frame.
379 152368 inline value_type distance(const ConvexShape& cs, vector3_t a) const {
380
2/2
✓ Branch 1 taken 150448 times.
✓ Branch 2 taken 1917 times.
 152368 if (cs.joint_ != NULL) a = oMj_.actInv(a);
381 152365 return cs.distanceLocal(a);
382 }
383 };
384 } // namespace constraints
385 } // namespace hpp
386
387 #endif // HPP_CONSTRAINTS_CONVEX_SHAPE_HH
388