GCC Code Coverage Report

Directory: ./
File: include/coal/shape/geometric_shapes.hxx
Date: 2025-05-02 10:16:21
Exec Total Coverage
Lines: 74 86 86.0%
Branches: 34 106 32.1%
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_HXX
39 #define COAL_GEOMETRIC_SHAPES_HXX
40
41 #include "coal/shape/convex.h"
42 #include "coal/shape/geometric_shapes.h"
43 #include "coal/narrowphase/support_data.h"
44
45 #include <iostream>
46
47 namespace coal {
48
49 template <typename IndexType>
50 NODE_TYPE ConvexBaseTpl<IndexType>::getNodeType() const {
51 COAL_THROW_PRETTY("Uknown vertex index type for ConvexBase.",
52 std::runtime_error);
53 return NODE_COUNT;
54 }
55
56 template <>
57 ✗ inline NODE_TYPE ConvexBaseTpl<std::uint16_t>::getNodeType() const {
58 ✗ return GEOM_CONVEX16;
59 }
60
61 template <>
62 168873 inline NODE_TYPE ConvexBaseTpl<std::uint32_t>::getNodeType() const {
63 168873 return GEOM_CONVEX32;
64 }
65
66 template <typename IndexType>
67 61317 void ConvexBaseTpl<IndexType>::initialize(
68 std::shared_ptr<std::vector<Vec3s>> points_, unsigned int num_points_) {
69 61317 this->points = points_;
70 61317 this->num_points = num_points_;
71
1/2
✓ Branch 2 taken 61317 times.
✗ Branch 3 not taken.
 61317 COAL_ASSERT(this->points->size() == this->num_points,
72 "The number of points is not consistent with the size of the "
73 "points vector",
74 std::logic_error);
75 61317 this->num_normals_and_offsets = 0;
76 61317 this->normals.reset();
77 61317 this->offsets.reset();
78 61317 this->computeCenter();
79 61317 }
80
81 template <typename IndexType>
82 61182 void ConvexBaseTpl<IndexType>::set(std::shared_ptr<std::vector<Vec3s>> points_,
83 unsigned int num_points_) {
84
1/2
✓ Branch 2 taken 61182 times.
✗ Branch 3 not taken.
 61182 initialize(points_, num_points_);
85 61182 }
86
87 template <typename IndexType>
88 18 ConvexBaseTpl<IndexType>& ConvexBaseTpl<IndexType>::operator=(
89 const ConvexBaseTpl& other) {
90
1/2
✓ Branch 0 taken 18 times.
✗ Branch 1 not taken.
 18 if (this != &other) {
91 // Copy the base
92 18 this->base() = other.base();
93
94 // Shallow copy the rest of the data
95 18 this->points = other.points;
96 18 this->num_points = other.num_points;
97 18 this->normals = other.normals;
98 18 this->offsets = other.offsets;
99 18 this->num_normals_and_offsets = other.num_normals_and_offsets;
100 18 this->neighbors = other.neighbors;
101 18 this->nneighbors_ = other.nneighbors_;
102 18 this->center = other.center;
103 18 this->support_warm_starts = other.support_warm_starts;
104 }
105
106 18 return *this;
107 }
108
109 template <typename IndexType>
110 template <typename OtherIndexType>
111 3 void ConvexBaseTpl<IndexType>::deepcopy(const ConvexBaseTpl<IndexType>* source,
112 ConvexBaseTpl<OtherIndexType>* copy) {
113
2/4
✓ Branch 0 taken 2 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 2 times.
 3 if (source == nullptr || copy == nullptr) {
114 ✗ return;
115 }
116
117 // Copy the base
118 3 copy->base() = source->base();
119
120 // Copy the non-templated data
121
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 3 if (source->points != nullptr) {
122
1/2
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
 3 copy->points.reset(new std::vector<Vec3s>(*source->points));
123 } else {
124 ✗ copy->points.reset();
125 ✗ assert(source->num_points == 0);
126 }
127 3 copy->num_points = source->num_points;
128
129
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
 3 if (source->normals != nullptr) {
130 ✗ copy->normals.reset(new std::vector<Vec3s>(*source->normals));
131 } else {
132 3 copy->normals.reset();
133
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 2 times.
 3 assert(source->num_normals_and_offsets == 0);
134 }
135
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
 3 if (source->offsets != nullptr) {
136 ✗ copy->offsets.reset(new std::vector<Scalar>(*source->offsets));
137 } else {
138 3 copy->offsets.reset();
139
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 2 times.
 3 assert(source->num_normals_and_offsets == 0);
140 }
141 3 copy->num_normals_and_offsets = source->num_normals_and_offsets;
142
143 3 copy->center = source->center;
144 3 copy->support_warm_starts =
145 source->support_warm_starts.template cast<OtherIndexType>();
146
147 // Convert neighbors to new type
148 3 if (source->points->size() >=
149
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
 3 (std::size_t)(std::numeric_limits<OtherIndexType>::max())) {
150 ✗ COAL_THROW_PRETTY(
151 "The source has more points than the max of OtherIndexType.",
152 std::runtime_error);
153 }
154
155
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 3 if (source->nneighbors_ != nullptr) {
156 3 const std::vector<IndexType>& source_nneighbors = *(source->nneighbors_);
157
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 6 copy->nneighbors_.reset(
158
2/4
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
 6 new std::vector<OtherIndexType>(source_nneighbors.size()));
159 3 std::vector<OtherIndexType>& copy_nneighbors = *(copy->nneighbors_);
160
2/2
✓ Branch 1 taken 60 times.
✓ Branch 2 taken 2 times.
 87 for (std::size_t i = 0; i < source_nneighbors.size(); ++i) {
161 84 copy_nneighbors[i] = OtherIndexType(source_nneighbors[i]);
162 }
163 } else {
164 ✗ copy->nneighbors_.reset();
165 }
166
167
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 3 if (source->neighbors != nullptr) {
168 typedef typename ConvexBaseTpl<OtherIndexType>::Neighbors OtherNeighbors;
169
1/2
✗ Branch 4 not taken.
✓ Branch 5 taken 2 times.
 3 assert(source->neighbors->size() == source->points->size());
170 3 const std::vector<Neighbors>& source_neighbors = *(source->neighbors);
171
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 6 copy->neighbors.reset(
172
2/4
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
 6 new std::vector<OtherNeighbors>(source_neighbors.size()));
173 3 std::vector<OtherNeighbors>& copy_neighbors = *(copy->neighbors);
174
2/2
✓ Branch 1 taken 16 times.
✓ Branch 2 taken 2 times.
 27 for (std::size_t i = 0; i < source_neighbors.size(); ++i) {
175 24 copy_neighbors[i].count = source_neighbors[i].count;
176 24 copy_neighbors[i].begin_id = OtherIndexType(source_neighbors[i].begin_id);
177 }
178 } else {
179 ✗ copy->neighbors.reset();
180 }
181 }
182
183 template <typename IndexType>
184 61317 void ConvexBaseTpl<IndexType>::computeCenter() {
185 61317 center.setZero();
186 61317 const std::vector<Vec3s>& points_ = *points;
187
2/2
✓ Branch 0 taken 368547 times.
✓ Branch 1 taken 61317 times.
 429864 for (std::size_t i = 0; i < num_points; ++i)
188 368547 center += points_[i]; // TODO(jcarpent): vectorization
189
1/2
✓ Branch 1 taken 61317 times.
✗ Branch 2 not taken.
 61317 center /= Scalar(num_points);
190 61317 }
191
192 // forward declaration for ConvexBase
193 template <typename BV, typename S>
194 void computeBV(const S& s, const Transform3s& tf, BV& bv);
195
196 template <typename IndexType>
197 1 void ConvexBaseTpl<IndexType>::computeLocalAABB() {
198
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 computeBV<AABB, ConvexBaseTpl<IndexType>>(*this, Transform3s(), aabb_local);
199 1 const Scalar ssr = this->getSweptSphereRadius();
200
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 if (ssr > 0) {
201 ✗ aabb_local.min_ -= Vec3s::Constant(ssr);
202 ✗ aabb_local.max_ += Vec3s::Constant(ssr);
203 }
204
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 aabb_center = aabb_local.center();
205
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();
206 1 }
207
208 // Reorders `tri` such that the dot product between the normal of triangle and
209 // the vector `triangle barycentre - convex_tri.center` is positive.
210 template <typename IndexType>
211 void reorderTriangle(const ConvexTpl<TriangleTpl<IndexType>>* convex_tri,
212 TriangleTpl<IndexType>& tri) {
213 Vec3s p0, p1, p2;
214 p0 = (*(convex_tri->points))[tri[0]];
215 p1 = (*(convex_tri->points))[tri[1]];
216 p2 = (*(convex_tri->points))[tri[2]];
217
218 Vec3s barycentre_tri, center_barycenter;
219 barycentre_tri = (p0 + p1 + p2) / 3;
220 center_barycenter = barycentre_tri - convex_tri->center;
221
222 Vec3s edge_tri1, edge_tri2, n_tri;
223 edge_tri1 = p1 - p0;
224 edge_tri2 = p2 - p1;
225 n_tri = edge_tri1.cross(edge_tri2);
226
227 if (center_barycenter.dot(n_tri) < 0) {
228 tri.set(tri[1], tri[0], tri[2]);
229 }
230 }
231
232 } // namespace coal
233
234 #endif // COAL_GEOMETRIC_SHAPES_HXX
235