GCC Code Coverage Report

Directory: ./
File: src/shape/geometric_shapes.cpp
Date: 2025-04-01 09:23:31
Exec Total Coverage
Lines: 152 159 95.6%
Branches: 143 294 48.6%
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 #include "coal/shape/geometric_shapes.h"
39 #include "coal/shape/geometric_shapes_utility.h"
40
41 namespace coal {
42
43 61317 void ConvexBase::initialize(std::shared_ptr<std::vector<Vec3s>> points_,
44 unsigned int num_points_) {
45 61317 this->points = points_;
46 61317 this->num_points = num_points_;
47
1/2
✓ Branch 4 taken 61317 times.
✗ Branch 5 not taken.
 61317 COAL_ASSERT(this->points->size() == this->num_points,
48 "The number of points is not consistent with the size of the "
49 "points vector",
50 std::logic_error);
51 61317 this->num_normals_and_offsets = 0;
52 61317 this->normals.reset();
53 61317 this->offsets.reset();
54 61317 this->computeCenter();
55 61317 }
56
57 61182 void ConvexBase::set(std::shared_ptr<std::vector<Vec3s>> points_,
58 unsigned int num_points_) {
59
1/2
✓ Branch 2 taken 61182 times.
✗ Branch 3 not taken.
 61182 initialize(points_, num_points_);
60 61182 }
61
62 19 ConvexBase::ConvexBase(const ConvexBase& other)
63 : ShapeBase(other),
64 19 num_points(other.num_points),
65 19 num_normals_and_offsets(other.num_normals_and_offsets),
66
1/2
✓ Branch 6 taken 19 times.
✗ Branch 7 not taken.
 19 center(other.center) {
67
3/6
✓ Branch 1 taken 19 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 19 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 19 times.
✗ Branch 8 not taken.
 19 if (other.points.get() && other.points->size() > 0) {
68 // Deep copy of other points
69
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.
 19 points.reset(new std::vector<Vec3s>(*other.points));
70 } else
71 ✗ points.reset();
72
73
3/6
✓ Branch 1 taken 19 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 19 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 19 times.
✗ Branch 8 not taken.
 19 if (other.nneighbors_.get() && other.nneighbors_->size() > 0) {
74 // Deep copy the list of all the neighbors of all the points
75
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.
 19 nneighbors_.reset(new std::vector<unsigned int>(*(other.nneighbors_)));
76
3/6
✓ Branch 1 taken 19 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 19 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 19 times.
✗ Branch 8 not taken.
 19 if (other.neighbors.get() && other.neighbors->size() > 0) {
77 // Fill each neighbors for each point in the Convex object.
78
3/6
✓ Branch 4 taken 19 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 19 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 19 times.
✗ Branch 11 not taken.
 19 neighbors.reset(new std::vector<Neighbors>(other.neighbors->size()));
79
1/2
✗ Branch 4 not taken.
✓ Branch 5 taken 19 times.
 19 assert(neighbors->size() == points->size());
80 19 unsigned int* p_nneighbors = nneighbors_->data();
81
82 19 std::vector<Neighbors>& neighbors_ = *neighbors;
83 19 const std::vector<Neighbors>& other_neighbors_ = *(other.neighbors);
84
2/2
✓ Branch 2 taken 224 times.
✓ Branch 3 taken 19 times.
 243 for (size_t i = 0; i < neighbors->size(); ++i) {
85 224 Neighbors& n = neighbors_[i];
86 224 n.count_ = other_neighbors_[i].count_;
87 224 n.n_ = p_nneighbors;
88 224 p_nneighbors += n.count_;
89 }
90 } else
91 ✗ neighbors.reset();
92 } else
93 ✗ nneighbors_.reset();
94
95
2/6
✗ Branch 1 not taken.
✓ Branch 2 taken 19 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 19 times.
 19 if (other.normals.get() && other.normals->size() > 0) {
96 ✗ normals.reset(new std::vector<Vec3s>(*(other.normals)));
97 } else
98 19 normals.reset();
99
100
2/6
✗ Branch 1 not taken.
✓ Branch 2 taken 19 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 19 times.
 19 if (other.offsets.get() && other.offsets->size() > 0) {
101 ✗ offsets.reset(new std::vector<Scalar>(*(other.offsets)));
102 } else
103 19 offsets.reset();
104
105
1/2
✓ Branch 1 taken 19 times.
✗ Branch 2 not taken.
 19 support_warm_starts = other.support_warm_starts;
106
107
1/2
✓ Branch 1 taken 19 times.
✗ Branch 2 not taken.
 19 ShapeBase::operator=(*this);
108 19 }
109
110 122676 ConvexBase::~ConvexBase() {}
111
112 61317 void ConvexBase::computeCenter() {
113 61317 center.setZero();
114 61317 const std::vector<Vec3s>& points_ = *points;
115
2/2
✓ Branch 0 taken 368547 times.
✓ Branch 1 taken 61317 times.
 429864 for (std::size_t i = 0; i < num_points; ++i)
116 368547 center += points_[i]; // TODO(jcarpent): vectorization
117
1/2
✓ Branch 1 taken 61317 times.
✗ Branch 2 not taken.
 61317 center /= Scalar(num_points);
118 61317 }
119
120 17640 void Halfspace::unitNormalTest() {
121 17640 Scalar l = n.norm();
122
2/2
✓ Branch 0 taken 17638 times.
✓ Branch 1 taken 2 times.
 17640 if (l > 0) {
123 17638 Scalar inv_l = Scalar(1) / l;
124
1/2
✓ Branch 1 taken 17638 times.
✗ Branch 2 not taken.
 17638 n *= inv_l;
125 17638 d *= inv_l;
126 } else {
127
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 n << 1, 0, 0;
128 2 d = 0;
129 }
130 17640 }
131
132 255 void Plane::unitNormalTest() {
133 255 Scalar l = n.norm();
134
2/2
✓ Branch 0 taken 253 times.
✓ Branch 1 taken 2 times.
 255 if (l > 0) {
135 253 Scalar inv_l = Scalar(1) / l;
136
1/2
✓ Branch 1 taken 253 times.
✗ Branch 2 not taken.
 253 n *= inv_l;
137 253 d *= inv_l;
138 } else {
139
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 n << 1, 0, 0;
140 2 d = 0;
141 }
142 255 }
143
144 14112 void Box::computeLocalAABB() {
145
2/4
✓ Branch 1 taken 14112 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14112 times.
✗ Branch 5 not taken.
 14112 computeBV<AABB>(*this, Transform3s(), aabb_local);
146 14112 const Scalar ssr = this->getSweptSphereRadius();
147
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 14111 times.
 14112 if (ssr > 0) {
148
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 aabb_local.min_ -= Vec3s::Constant(ssr);
149
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 aabb_local.max_ += Vec3s::Constant(ssr);
150 }
151
1/2
✓ Branch 1 taken 14112 times.
✗ Branch 2 not taken.
 14112 aabb_center = aabb_local.center();
152
2/4
✓ Branch 1 taken 14112 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14112 times.
✗ Branch 5 not taken.
 14112 aabb_radius = (aabb_local.min_ - aabb_center).norm();
153 14112 }
154
155 16918 void Sphere::computeLocalAABB() {
156
2/4
✓ Branch 1 taken 16918 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16918 times.
✗ Branch 5 not taken.
 16918 computeBV<AABB>(*this, Transform3s(), aabb_local);
157 16918 const Scalar ssr = this->getSweptSphereRadius();
158
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 16917 times.
 16918 if (ssr > 0) {
159
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 aabb_local.min_ -= Vec3s::Constant(ssr);
160
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 aabb_local.max_ += Vec3s::Constant(ssr);
161 }
162
1/2
✓ Branch 1 taken 16918 times.
✗ Branch 2 not taken.
 16918 aabb_center = aabb_local.center();
163 16918 aabb_radius = radius;
164 16918 }
165
166 1 void Ellipsoid::computeLocalAABB() {
167
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 computeBV<AABB>(*this, Transform3s(), aabb_local);
168 1 const Scalar ssr = this->getSweptSphereRadius();
169
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 1 if (ssr > 0) {
170
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 aabb_local.min_ -= Vec3s::Constant(ssr);
171
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 aabb_local.max_ += Vec3s::Constant(ssr);
172 }
173
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 aabb_center = aabb_local.center();
174
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 aabb_radius = (aabb_local.min_ - aabb_center).norm();
175 1 }
176
177 10011 void Capsule::computeLocalAABB() {
178
2/4
✓ Branch 1 taken 10011 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10011 times.
✗ Branch 5 not taken.
 10011 computeBV<AABB>(*this, Transform3s(), aabb_local);
179 10011 const Scalar ssr = this->getSweptSphereRadius();
180
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 10010 times.
 10011 if (ssr > 0) {
181
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 aabb_local.min_ -= Vec3s::Constant(ssr);
182
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 aabb_local.max_ += Vec3s::Constant(ssr);
183 }
184
1/2
✓ Branch 1 taken 10011 times.
✗ Branch 2 not taken.
 10011 aabb_center = aabb_local.center();
185
2/4
✓ Branch 1 taken 10011 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10011 times.
✗ Branch 5 not taken.
 10011 aabb_radius = (aabb_local.min_ - aabb_center).norm();
186 10011 }
187
188 1 void Cone::computeLocalAABB() {
189
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 computeBV<AABB>(*this, Transform3s(), aabb_local);
190 1 const Scalar ssr = this->getSweptSphereRadius();
191
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 1 if (ssr > 0) {
192
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 aabb_local.min_ -= Vec3s::Constant(ssr);
193
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 aabb_local.max_ += Vec3s::Constant(ssr);
194 }
195
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 aabb_center = aabb_local.center();
196
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 aabb_radius = (aabb_local.min_ - aabb_center).norm();
197 1 }
198
199 8913 void Cylinder::computeLocalAABB() {
200
2/4
✓ Branch 1 taken 8913 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8913 times.
✗ Branch 5 not taken.
 8913 computeBV<AABB>(*this, Transform3s(), aabb_local);
201 8913 const Scalar ssr = this->getSweptSphereRadius();
202
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 8912 times.
 8913 if (ssr > 0) {
203
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 aabb_local.min_ -= Vec3s::Constant(ssr);
204
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 aabb_local.max_ += Vec3s::Constant(ssr);
205 }
206
1/2
✓ Branch 1 taken 8913 times.
✗ Branch 2 not taken.
 8913 aabb_center = aabb_local.center();
207
2/4
✓ Branch 1 taken 8913 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8913 times.
✗ Branch 5 not taken.
 8913 aabb_radius = (aabb_local.min_ - aabb_center).norm();
208 8913 }
209
210 1 void ConvexBase::computeLocalAABB() {
211
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 computeBV<AABB>(*this, Transform3s(), aabb_local);
212 1 const Scalar ssr = this->getSweptSphereRadius();
213
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 if (ssr > 0) {
214 ✗ aabb_local.min_ -= Vec3s::Constant(ssr);
215 ✗ aabb_local.max_ += Vec3s::Constant(ssr);
216 }
217
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 aabb_center = aabb_local.center();
218
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 aabb_radius = (aabb_local.min_ - aabb_center).norm();
219 1 }
220
221 1 void Halfspace::computeLocalAABB() {
222
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 computeBV<AABB>(*this, Transform3s(), aabb_local);
223 1 const Scalar ssr = this->getSweptSphereRadius();
224
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 1 if (ssr > 0) {
225
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 aabb_local.min_ -= Vec3s::Constant(ssr);
226
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 aabb_local.max_ += Vec3s::Constant(ssr);
227 }
228
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 aabb_center = aabb_local.center();
229
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 aabb_radius = (aabb_local.min_ - aabb_center).norm();
230 1 }
231
232 1 void Plane::computeLocalAABB() {
233
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 computeBV<AABB>(*this, Transform3s(), aabb_local);
234 1 const Scalar ssr = this->getSweptSphereRadius();
235
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 1 if (ssr > 0) {
236
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 aabb_local.min_ -= Vec3s::Constant(ssr);
237
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 aabb_local.max_ += Vec3s::Constant(ssr);
238 }
239
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 aabb_center = aabb_local.center();
240
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 aabb_radius = (aabb_local.min_ - aabb_center).norm();
241 1 }
242
243 1 void TriangleP::computeLocalAABB() {
244
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 computeBV<AABB>(*this, Transform3s(), aabb_local);
245 1 const Scalar ssr = this->getSweptSphereRadius();
246
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 1 if (ssr > 0) {
247
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 aabb_local.min_ -= Vec3s::Constant(ssr);
248
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 aabb_local.max_ += Vec3s::Constant(ssr);
249 }
250
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 aabb_center = aabb_local.center();
251
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 aabb_radius = (aabb_local.min_ - aabb_center).norm();
252 1 }
253
254 } // namespace coal
255