GCC Code Coverage Report


Directory: ./
File: include/coal/shape/details/convex.hxx
Date: 2025-04-01 09:23:31
Exec Total Coverage
Lines: 52 151 34.4%
Branches: 38 244 15.6%

Line Branch Exec Source
1 /*
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2019, CNRS - LAAS
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of Open Source Robotics Foundation nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 */
34
35 /** \author Joseph Mirabel */
36
37 #ifndef COAL_SHAPE_CONVEX_HXX
38 #define COAL_SHAPE_CONVEX_HXX
39
40 #include <set>
41 #include <vector>
42 #include <iostream>
43
44 namespace coal {
45
46 template <typename PolygonT>
47 169 Convex<PolygonT>::Convex(std::shared_ptr<std::vector<Vec3s>> points_,
48 unsigned int num_points_,
49 std::shared_ptr<std::vector<PolygonT>> polygons_,
50 unsigned int num_polygons_)
51 169 : ConvexBase(), polygons(polygons_), num_polygons(num_polygons_) {
52
1/2
✓ Branch 2 taken 135 times.
✗ Branch 3 not taken.
169 this->initialize(points_, num_points_);
53
1/2
✓ Branch 1 taken 135 times.
✗ Branch 2 not taken.
169 this->fillNeighbors();
54
1/2
✓ Branch 1 taken 135 times.
✗ Branch 2 not taken.
169 this->buildSupportWarmStart();
55 169 }
56
57 template <typename PolygonT>
58 20 Convex<PolygonT>::Convex(const Convex<PolygonT>& other)
59 20 : ConvexBase(other), num_polygons(other.num_polygons) {
60
1/2
✓ Branch 1 taken 19 times.
✗ Branch 2 not taken.
20 if (other.polygons.get()) {
61
3/6
✓ Branch 2 taken 19 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 19 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 19 times.
✗ Branch 9 not taken.
20 polygons.reset(new std::vector<PolygonT>(*(other.polygons)));
62 } else
63 polygons.reset();
64 20 }
65
66 template <typename PolygonT>
67 122774 Convex<PolygonT>::~Convex() {}
68
69 template <typename PolygonT>
70 61182 void Convex<PolygonT>::set(std::shared_ptr<std::vector<Vec3s>> points_,
71 unsigned int num_points_,
72 std::shared_ptr<std::vector<PolygonT>> polygons_,
73 unsigned int num_polygons_) {
74
1/2
✓ Branch 2 taken 61182 times.
✗ Branch 3 not taken.
61182 ConvexBase::set(points_, num_points_);
75
76 61182 this->num_polygons = num_polygons_;
77 61182 this->polygons = polygons_;
78
79 61182 this->fillNeighbors();
80 61182 this->buildSupportWarmStart();
81 61182 }
82
83 template <typename PolygonT>
84 2 Convex<PolygonT>* Convex<PolygonT>::clone() const {
85
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
2 return new Convex(*this);
86 }
87
88 template <typename PolygonT>
89 Matrix3s Convex<PolygonT>::computeMomentofInertia() const {
90 typedef typename PolygonT::size_type size_type;
91 typedef typename PolygonT::index_type index_type;
92
93 Matrix3s C = Matrix3s::Zero();
94
95 Matrix3s C_canonical;
96 C_canonical << Scalar(1 / 60), //
97 Scalar(1 / 120), //
98 Scalar(1 / 120), //
99 Scalar(1 / 120), //
100 Scalar(1 / 60), //
101 Scalar(1 / 120), //
102 Scalar(1 / 120), //
103 Scalar(1 / 120), //
104 Scalar(1 / 60);
105
106 if (!(points.get())) {
107 std::cerr
108 << "Error in `Convex::computeMomentofInertia`! Convex has no vertices."
109 << std::endl;
110 return C;
111 }
112 const std::vector<Vec3s>& points_ = *points;
113 if (!(polygons.get())) {
114 std::cerr
115 << "Error in `Convex::computeMomentofInertia`! Convex has no polygons."
116 << std::endl;
117 return C;
118 }
119 const std::vector<PolygonT>& polygons_ = *polygons;
120 for (unsigned int i = 0; i < num_polygons; ++i) {
121 const PolygonT& polygon = polygons_[i];
122
123 // compute the center of the polygon
124 Vec3s plane_center(0, 0, 0);
125 for (size_type j = 0; j < polygon.size(); ++j)
126 plane_center += points_[polygon[(index_type)j]];
127 plane_center /= Scalar(polygon.size());
128
129 // compute the volume of tetrahedron making by neighboring two points, the
130 // plane center and the reference point (zero) of the convex shape
131 const Vec3s& v3 = plane_center;
132 for (size_type j = 0; j < polygon.size(); ++j) {
133 index_type e_first = polygon[static_cast<index_type>(j)];
134 index_type e_second =
135 polygon[static_cast<index_type>((j + 1) % polygon.size())];
136 const Vec3s& v1 = points_[e_first];
137 const Vec3s& v2 = points_[e_second];
138 Matrix3s A;
139 A << v1.transpose(), v2.transpose(),
140 v3.transpose(); // this is A' in the original document
141 C += A.transpose() * C_canonical * A * (v1.cross(v2)).dot(v3);
142 }
143 }
144
145 return C.trace() * Matrix3s::Identity() - C;
146 }
147
148 template <typename PolygonT>
149 Vec3s Convex<PolygonT>::computeCOM() const {
150 typedef typename PolygonT::size_type size_type;
151 typedef typename PolygonT::index_type index_type;
152
153 Vec3s com(0, 0, 0);
154 Scalar vol = 0;
155 if (!(points.get())) {
156 std::cerr << "Error in `Convex::computeCOM`! Convex has no vertices."
157 << std::endl;
158 return com;
159 }
160 const std::vector<Vec3s>& points_ = *points;
161 if (!(polygons.get())) {
162 std::cerr << "Error in `Convex::computeCOM`! Convex has no polygons."
163 << std::endl;
164 return com;
165 }
166 const std::vector<PolygonT>& polygons_ = *polygons;
167 for (unsigned int i = 0; i < num_polygons; ++i) {
168 const PolygonT& polygon = polygons_[i];
169 // compute the center of the polygon
170 Vec3s plane_center(0, 0, 0);
171 for (size_type j = 0; j < polygon.size(); ++j)
172 plane_center += points_[polygon[(index_type)j]];
173 plane_center /= Scalar(polygon.size());
174
175 // compute the volume of tetrahedron making by neighboring two points, the
176 // plane center and the reference point (zero) of the convex shape
177 const Vec3s& v3 = plane_center;
178 for (size_type j = 0; j < polygon.size(); ++j) {
179 index_type e_first = polygon[static_cast<index_type>(j)];
180 index_type e_second =
181 polygon[static_cast<index_type>((j + 1) % polygon.size())];
182 const Vec3s& v1 = points_[e_first];
183 const Vec3s& v2 = points_[e_second];
184 Scalar d_six_vol = (v1.cross(v2)).dot(v3);
185 vol += d_six_vol;
186 com += (points_[e_first] + points_[e_second] + plane_center) * d_six_vol;
187 }
188 }
189
190 return com / (vol * 4); // here we choose zero as the reference
191 }
192
193 template <typename PolygonT>
194 Scalar Convex<PolygonT>::computeVolume() const {
195 typedef typename PolygonT::size_type size_type;
196 typedef typename PolygonT::index_type index_type;
197
198 Scalar vol = 0;
199 if (!(points.get())) {
200 std::cerr << "Error in `Convex::computeVolume`! Convex has no vertices."
201 << std::endl;
202 return vol;
203 }
204 const std::vector<Vec3s>& points_ = *points;
205 if (!(polygons.get())) {
206 std::cerr << "Error in `Convex::computeVolume`! Convex has no polygons."
207 << std::endl;
208 return vol;
209 }
210 const std::vector<PolygonT>& polygons_ = *polygons;
211 for (unsigned int i = 0; i < num_polygons; ++i) {
212 const PolygonT& polygon = polygons_[i];
213
214 // compute the center of the polygon
215 Vec3s plane_center(0, 0, 0);
216 for (size_type j = 0; j < polygon.size(); ++j)
217 plane_center += points_[polygon[(index_type)j]];
218 plane_center /= Scalar(polygon.size());
219
220 // compute the volume of tetrahedron making by neighboring two points, the
221 // plane center and the reference point (zero point) of the convex shape
222 const Vec3s& v3 = plane_center;
223 for (size_type j = 0; j < polygon.size(); ++j) {
224 index_type e_first = polygon[static_cast<index_type>(j)];
225 index_type e_second =
226 polygon[static_cast<index_type>((j + 1) % polygon.size())];
227 const Vec3s& v1 = points_[e_first];
228 const Vec3s& v2 = points_[e_second];
229 Scalar d_six_vol = (v1.cross(v2)).dot(v3);
230 vol += d_six_vol;
231 }
232 }
233
234 return vol / 6;
235 }
236
237 template <typename PolygonT>
238 61416 void Convex<PolygonT>::fillNeighbors() {
239
3/6
✓ Branch 2 taken 61318 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 61318 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 61318 times.
✗ Branch 9 not taken.
61416 neighbors.reset(new std::vector<Neighbors>(num_points));
240
241 typedef typename PolygonT::size_type size_type;
242 typedef typename PolygonT::index_type index_type;
243
1/2
✓ Branch 2 taken 61318 times.
✗ Branch 3 not taken.
61416 std::vector<std::set<index_type>> nneighbors(num_points);
244 61416 unsigned int c_nneighbors = 0;
245
246
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 61318 times.
61416 if (!(polygons.get())) {
247 std::cerr << "Error in `Convex::fillNeighbors`! Convex has no polygons."
248 << std::endl;
249 }
250 61416 const std::vector<PolygonT>& polygons_ = *polygons;
251
2/2
✓ Branch 0 taken 491805 times.
✓ Branch 1 taken 61318 times.
554349 for (unsigned int l = 0; l < num_polygons; ++l) {
252 492933 const PolygonT& polygon = polygons_[l];
253 492933 const size_type n = polygon.size();
254
255
2/2
✓ Branch 1 taken 1475439 times.
✓ Branch 2 taken 491805 times.
1971780 for (size_type j = 0; j < polygon.size(); ++j) {
256
2/2
✓ Branch 0 taken 491805 times.
✓ Branch 1 taken 983634 times.
1478847 size_type i = (j == 0) ? n - 1 : j - 1;
257
2/2
✓ Branch 0 taken 983634 times.
✓ Branch 1 taken 491805 times.
1478847 size_type k = (j == n - 1) ? 0 : j + 1;
258 1478847 index_type pi = polygon[(index_type)i], pj = polygon[(index_type)j],
259 1478847 pk = polygon[(index_type)k];
260 // Update neighbors of pj;
261
3/4
✓ Branch 2 taken 1475439 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 768312 times.
✓ Branch 5 taken 707127 times.
1478847 if (nneighbors[pj].count(pi) == 0) {
262 770048 c_nneighbors++;
263
1/2
✓ Branch 2 taken 768312 times.
✗ Branch 3 not taken.
770048 nneighbors[pj].insert(pi);
264 }
265
3/4
✓ Branch 2 taken 1475439 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 768312 times.
✓ Branch 5 taken 707127 times.
1478847 if (nneighbors[pj].count(pk) == 0) {
266 770048 c_nneighbors++;
267
1/2
✓ Branch 2 taken 768312 times.
✗ Branch 3 not taken.
770048 nneighbors[pj].insert(pk);
268 }
269 }
270 }
271
272
3/6
✓ Branch 2 taken 61318 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 61318 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 61318 times.
✗ Branch 9 not taken.
61416 nneighbors_.reset(new std::vector<unsigned int>(c_nneighbors));
273
274 61416 unsigned int* p_nneighbors = nneighbors_->data();
275 61416 std::vector<Neighbors>& neighbors_ = *neighbors;
276
2/2
✓ Branch 0 taken 368551 times.
✓ Branch 1 taken 61318 times.
430739 for (unsigned int i = 0; i < num_points; ++i) {
277 369323 Neighbors& n = neighbors_[i];
278
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 368551 times.
369323 if (nneighbors[i].size() >= (std::numeric_limits<unsigned char>::max)())
279 COAL_THROW_PRETTY("Too many neighbors.", std::logic_error);
280 369323 n.count_ = (unsigned char)nneighbors[i].size();
281 369323 n.n_ = p_nneighbors;
282 p_nneighbors =
283
1/2
✓ Branch 5 taken 368551 times.
✗ Branch 6 not taken.
369323 std::copy(nneighbors[i].begin(), nneighbors[i].end(), p_nneighbors);
284 }
285
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 61318 times.
61416 assert(p_nneighbors == nneighbors_->data() + c_nneighbors);
286 61416 }
287
288 } // namespace coal
289
290 #endif
291