GCC Code Coverage Report

Directory: ./
File: include/coal/shape/convex.hxx
Date: 2025-06-03 17:15:40
Exec Total Coverage
Lines: 65 165 39.4%
Functions: 8 26 30.8%
Branches: 43 256 16.8%
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 #include "coal/shape/convex.h"
45
46 namespace coal {
47
48 template <typename PolygonT>
49 169 ConvexTpl<PolygonT>::ConvexTpl(std::shared_ptr<std::vector<Vec3s>> points_,
50 unsigned int num_points_,
51 std::shared_ptr<std::vector<PolygonT>> polygons_,
52 unsigned int num_polygons_)
53 169 : Base(), polygons(polygons_), num_polygons(num_polygons_) {
54
1/2
✓ Branch 2 taken 135 times.
✗ Branch 3 not taken.
 169 this->initialize(points_, num_points_);
55
1/2
✓ Branch 1 taken 135 times.
✗ Branch 2 not taken.
 169 this->fillNeighbors();
56
1/2
✓ Branch 1 taken 135 times.
✗ Branch 2 not taken.
 169 this->buildSupportWarmStart();
57 169 }
58
59 template <typename PolygonT>
60 18 ConvexTpl<PolygonT>& ConvexTpl<PolygonT>::operator=(const ConvexTpl& other) {
61
1/2
✓ Branch 0 taken 18 times.
✗ Branch 1 not taken.
 18 if (this != &other) {
62 // Copy the base
63 18 this->base() = other.base();
64
65 // Shallow copy the polygons
66 18 this->num_polygons = other.num_polygons;
67 18 this->polygons = other.polygons;
68 }
69
70 18 return *this;
71 }
72
73 template <typename PolygonT>
74 template <typename OtherPolygonT>
75 4 void ConvexTpl<PolygonT>::deepcopy(const ConvexTpl<PolygonT>* source,
76 ConvexTpl<OtherPolygonT>* copy) {
77
2/4
✓ Branch 0 taken 2 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 2 times.
 4 if (source == nullptr || copy == nullptr) {
78 ✗ return;
79 }
80
81 // Deep copy the base
82 4 Base::deepcopy(source, copy);
83
84 // Deep copy the polygons
85 typedef typename OtherPolygonT::IndexType OtherIndexType;
86 4 copy->num_polygons = source->num_polygons;
87
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 if (source->polygons != nullptr) {
88 4 const std::vector<PolygonT>& source_polygons = *(source->polygons);
89
1/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
 4 copy->polygons.reset(
90
1/2
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
 8 new std::vector<OtherPolygonT>(source_polygons.size()));
91 4 std::vector<OtherPolygonT>& copy_polygons = *(copy->polygons);
92
2/2
✓ Branch 1 taken 18 times.
✓ Branch 2 taken 2 times.
 40 for (std::size_t i = 0; i < source_polygons.size(); ++i) {
93
2/4
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 6 times.
✗ Branch 7 not taken.
 36 copy_polygons[i] = source_polygons[i].template cast<OtherIndexType>();
94 }
95 } else {
96 ✗ copy->polygons.reset();
97 }
98 }
99
100 template <typename PolygonT>
101 61182 void ConvexTpl<PolygonT>::set(std::shared_ptr<std::vector<Vec3s>> points_,
102 unsigned int num_points_,
103 std::shared_ptr<std::vector<PolygonT>> polygons_,
104 unsigned int num_polygons_) {
105
1/2
✓ Branch 2 taken 61182 times.
✗ Branch 3 not taken.
 61182 Base::set(points_, num_points_);
106
107 61182 this->num_polygons = num_polygons_;
108 61182 this->polygons = polygons_;
109
110 61182 this->fillNeighbors();
111 61182 this->buildSupportWarmStart();
112 61182 }
113
114 template <typename PolygonT>
115 ✗ Matrix3s ConvexTpl<PolygonT>::computeMomentofInertia() const {
116 typedef typename PolygonT::size_type size_type;
117 typedef typename PolygonT::IndexType IndexType;
118
119 ✗ Matrix3s C = Matrix3s::Zero();
120
121 ✗ Matrix3s C_canonical;
122 ✗ C_canonical << Scalar(1 / 60.0), //
123 ✗ Scalar(1 / 120.0), //
124 ✗ Scalar(1 / 120.0), //
125 ✗ Scalar(1 / 120.0), //
126 ✗ Scalar(1 / 60.0), //
127 ✗ Scalar(1 / 120.0), //
128 ✗ Scalar(1 / 120.0), //
129 ✗ Scalar(1 / 120.0), //
130 ✗ Scalar(1 / 60.0);
131
132 ✗ if (!(points.get())) {
133 ✗ std::cerr << "Error in `ConvexTpl::computeMomentofInertia`! ConvexTpl has "
134 "no vertices."
135 ✗ << std::endl;
136 ✗ return C;
137 }
138 ✗ const std::vector<Vec3s>& points_ = *points;
139 ✗ if (!(polygons.get())) {
140 ✗ std::cerr << "Error in `ConvexTpl::computeMomentofInertia`! ConvexTpl has "
141 "no polygons."
142 ✗ << std::endl;
143 ✗ return C;
144 }
145 ✗ const std::vector<PolygonT>& polygons_ = *polygons;
146 ✗ for (unsigned int i = 0; i < num_polygons; ++i) {
147 ✗ const PolygonT& polygon = polygons_[i];
148
149 // compute the center of the polygon
150 ✗ Vec3s plane_center(0, 0, 0);
151 ✗ for (size_type j = 0; j < polygon.size(); ++j)
152 ✗ plane_center += points_[polygon[(IndexType)j]];
153 ✗ plane_center /= Scalar(polygon.size());
154
155 // compute the volume of tetrahedron making by neighboring two points, the
156 // plane center and the reference point (zero) of the convex shape
157 ✗ const Vec3s& v3 = plane_center;
158 ✗ for (size_type j = 0; j < polygon.size(); ++j) {
159 ✗ IndexType e_first = polygon[static_cast<IndexType>(j)];
160 IndexType e_second =
161 ✗ polygon[static_cast<IndexType>((j + 1) % polygon.size())];
162 ✗ const Vec3s& v1 = points_[e_first];
163 ✗ const Vec3s& v2 = points_[e_second];
164 ✗ Matrix3s A;
165 ✗ A << v1.transpose(), v2.transpose(),
166 ✗ v3.transpose(); // this is A' in the original document
167 ✗ C += A.transpose() * C_canonical * A * (v1.cross(v2)).dot(v3);
168 }
169 }
170
171 ✗ return C.trace() * Matrix3s::Identity() - C;
172 }
173
174 template <typename PolygonT>
175 ✗ Vec3s ConvexTpl<PolygonT>::computeCOM() const {
176 typedef typename PolygonT::size_type size_type;
177 typedef typename PolygonT::IndexType IndexType;
178
179 ✗ Vec3s com(0, 0, 0);
180 ✗ Scalar vol = 0;
181 ✗ if (!(points.get())) {
182 ✗ std::cerr << "Error in `ConvexTpl::computeCOM`! ConvexTpl has no vertices."
183 ✗ << std::endl;
184 ✗ return com;
185 }
186 ✗ const std::vector<Vec3s>& points_ = *points;
187 ✗ if (!(polygons.get())) {
188 ✗ std::cerr << "Error in `ConvexTpl::computeCOM`! ConvexTpl has no polygons."
189 ✗ << std::endl;
190 ✗ return com;
191 }
192 ✗ const std::vector<PolygonT>& polygons_ = *polygons;
193 ✗ for (unsigned int i = 0; i < num_polygons; ++i) {
194 ✗ const PolygonT& polygon = polygons_[i];
195 // compute the center of the polygon
196 ✗ Vec3s plane_center(0, 0, 0);
197 ✗ for (size_type j = 0; j < polygon.size(); ++j)
198 ✗ plane_center += points_[polygon[(IndexType)j]];
199 ✗ plane_center /= Scalar(polygon.size());
200
201 // compute the volume of tetrahedron making by neighboring two points, the
202 // plane center and the reference point (zero) of the convex shape
203 ✗ const Vec3s& v3 = plane_center;
204 ✗ for (size_type j = 0; j < polygon.size(); ++j) {
205 ✗ IndexType e_first = polygon[static_cast<IndexType>(j)];
206 IndexType e_second =
207 ✗ polygon[static_cast<IndexType>((j + 1) % polygon.size())];
208 ✗ const Vec3s& v1 = points_[e_first];
209 ✗ const Vec3s& v2 = points_[e_second];
210 ✗ Scalar d_six_vol = (v1.cross(v2)).dot(v3);
211 ✗ vol += d_six_vol;
212 ✗ com += (points_[e_first] + points_[e_second] + plane_center) * d_six_vol;
213 }
214 }
215
216 ✗ return com / (vol * 4); // here we choose zero as the reference
217 }
218
219 template <typename PolygonT>
220 ✗ Scalar ConvexTpl<PolygonT>::computeVolume() const {
221 typedef typename PolygonT::size_type size_type;
222 typedef typename PolygonT::IndexType IndexType;
223
224 ✗ Scalar vol = 0;
225 ✗ if (!(points.get())) {
226 std::cerr
227 ✗ << "Error in `ConvexTpl::computeVolume`! ConvexTpl has no vertices."
228 ✗ << std::endl;
229 ✗ return vol;
230 }
231 ✗ const std::vector<Vec3s>& points_ = *points;
232 ✗ if (!(polygons.get())) {
233 std::cerr
234 ✗ << "Error in `ConvexTpl::computeVolume`! ConvexTpl has no polygons."
235 ✗ << std::endl;
236 ✗ return vol;
237 }
238 ✗ const std::vector<PolygonT>& polygons_ = *polygons;
239 ✗ for (unsigned int i = 0; i < num_polygons; ++i) {
240 ✗ const PolygonT& polygon = polygons_[i];
241
242 // compute the center of the polygon
243 ✗ Vec3s plane_center(0, 0, 0);
244 ✗ for (size_type j = 0; j < polygon.size(); ++j)
245 ✗ plane_center += points_[polygon[(IndexType)j]];
246 ✗ plane_center /= Scalar(polygon.size());
247
248 // compute the volume of tetrahedron making by neighboring two points, the
249 // plane center and the reference point (zero point) of the convex shape
250 ✗ const Vec3s& v3 = plane_center;
251 ✗ for (size_type j = 0; j < polygon.size(); ++j) {
252 ✗ IndexType e_first = polygon[static_cast<IndexType>(j)];
253 IndexType e_second =
254 ✗ polygon[static_cast<IndexType>((j + 1) % polygon.size())];
255 ✗ const Vec3s& v1 = points_[e_first];
256 ✗ const Vec3s& v2 = points_[e_second];
257 ✗ Scalar d_six_vol = (v1.cross(v2)).dot(v3);
258 ✗ vol += d_six_vol;
259 }
260 }
261
262 ✗ return vol / 6;
263 }
264
265 template <typename PolygonT>
266 61420 void ConvexTpl<PolygonT>::fillNeighbors() {
267
3/8
✓ Branch 1 taken 61318 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 61318 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 61318 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
 184260 neighbors.reset(new std::vector<Neighbors>(num_points));
268
269 typedef typename PolygonT::size_type size_type;
270 typedef typename PolygonT::IndexType IndexType;
271
272
1/2
✓ Branch 1 taken 61318 times.
✗ Branch 2 not taken.
 61420 std::vector<std::set<IndexType>> nneighbors(num_points);
273 61420 unsigned int c_nneighbors = 0;
274
275
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 61318 times.
 61420 if (!(polygons.get())) {
276 std::cerr
277 ✗ << "Error in `ConvexTpl::fillNeighbors`! ConvexTpl has no polygons."
278 ✗ << std::endl;
279 }
280 61420 const std::vector<PolygonT>& polygons_ = *polygons;
281
2/2
✓ Branch 0 taken 491805 times.
✓ Branch 1 taken 61318 times.
 554366 for (unsigned int l = 0; l < num_polygons; ++l) {
282 492946 const PolygonT& polygon = polygons_[l];
283 492946 const size_type n = polygon.size();
284
285
2/2
✓ Branch 1 taken 1475439 times.
✓ Branch 2 taken 491805 times.
 1971832 for (size_type j = 0; j < polygon.size(); ++j) {
286
2/2
✓ Branch 0 taken 491805 times.
✓ Branch 1 taken 983634 times.
 1478886 size_type i = (j == 0) ? n - 1 : j - 1;
287
2/2
✓ Branch 0 taken 983634 times.
✓ Branch 1 taken 491805 times.
 1478886 size_type k = (j == n - 1) ? 0 : j + 1;
288 1478886 IndexType pi = polygon[(IndexType)i], pj = polygon[(IndexType)j],
289 1478886 pk = polygon[(IndexType)k];
290 // Update neighbors of pj;
291
3/4
✓ Branch 2 taken 1475439 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 768312 times.
✓ Branch 5 taken 707127 times.
 1478886 if (nneighbors[pj].count(pi) == 0) {
292 770069 c_nneighbors++;
293
1/2
✓ Branch 2 taken 768312 times.
✗ Branch 3 not taken.
 770069 nneighbors[pj].insert(pi);
294 }
295
3/4
✓ Branch 2 taken 1475439 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 768312 times.
✓ Branch 5 taken 707127 times.
 1478886 if (nneighbors[pj].count(pk) == 0) {
296 770069 c_nneighbors++;
297
1/2
✓ Branch 2 taken 768312 times.
✗ Branch 3 not taken.
 770069 nneighbors[pj].insert(pk);
298 }
299 }
300 }
301
302
3/8
✓ Branch 1 taken 61318 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 61318 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 61318 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
 122840 nneighbors_.reset(new std::vector<IndexType>(c_nneighbors));
303
304 61420 std::vector<Neighbors>& neighbors_ = *neighbors;
305 61420 std::vector<IndexType>& nneighbors__ = *(nneighbors_);
306 61420 IndexType begin_id = 0;
307
2/2
✓ Branch 0 taken 368551 times.
✓ Branch 1 taken 61318 times.
 430758 for (unsigned int i = 0; i < num_points; ++i) {
308 369338 Neighbors& n = neighbors_[i];
309
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 368551 times.
 369338 if (nneighbors[i].size() >= (std::numeric_limits<unsigned char>::max)())
310 ✗ COAL_THROW_PRETTY("Too many neighbors.", std::logic_error);
311 369338 n.count = (unsigned char)nneighbors[i].size();
312 369338 n.begin_id = begin_id;
313 369338 IndexType j = 0;
314
2/2
✓ Branch 5 taken 1536624 times.
✓ Branch 6 taken 368551 times.
 1909476 for (IndexType idx : nneighbors[i]) {
315 1540138 nneighbors__[n.begin_id + j] = idx;
316 1540138 j++;
317 }
318 369338 begin_id += n.count;
319 }
320 61420 }
321
322 } // namespace coal
323
324 #endif
325