GCC Code Coverage Report

Directory: ./
File: src/BVH/BVH_utility.cpp
Date: 2025-06-03 17:15:40
Exec Total Coverage
Lines: 332 387 85.8%
Functions: 25 25 100.0%
Branches: 403 932 43.2%
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/BVH/BVH_utility.h"
39 #include "coal/narrowphase/narrowphase.h"
40 #include "coal/shape/geometric_shapes_utility.h"
41 #include "coal/internal/shape_shape_func.h"
42
43 namespace coal {
44
45 namespace details {
46 template <typename BV>
47 80 BVHModel<BV>* BVHExtract(const BVHModel<BV>& model, const Transform3s& pose,
48 const AABB& _aabb) {
49
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 40 times.
 80 assert(model.getModelType() == BVH_MODEL_TRIANGLES);
50 80 const Matrix3s& q = pose.getRotation();
51
3/6
✓ Branch 2 taken 40 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 40 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 40 times.
✗ Branch 9 not taken.
 80 AABB aabb = translate(_aabb, -pose.getTranslation());
52
53
1/2
✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
 80 Transform3s box_pose;
54
1/2
✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
 80 Box box;
55
1/2
✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
 80 constructBox(_aabb, box, box_pose);
56
2/4
✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 40 times.
✗ Branch 5 not taken.
 80 box_pose = pose.inverseTimes(box_pose);
57
58
1/2
✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
 80 GJKSolver gjk;
59 // Early-stop GJK as soon as a separating plane is found
60 80 gjk.gjk.setDistanceEarlyBreak(Scalar(1e-3));
61
62 // Check what triangles should be kept.
63 // TODO use the BV hierarchy
64
1/2
✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
 80 std::vector<bool> keep_vertex(model.num_vertices, false);
65
1/2
✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
 80 std::vector<bool> keep_tri(model.num_tris, false);
66 80 unsigned int ntri = 0;
67 80 const std::vector<Vec3s>& model_vertices_ = *(model.vertices);
68 80 const std::vector<Triangle32>& model_tri_indices_ = *(model.tri_indices);
69
2/2
✓ Branch 0 taken 480 times.
✓ Branch 1 taken 40 times.
 1040 for (unsigned int i = 0; i < model.num_tris; ++i) {
70 960 const Triangle32& t = model_tri_indices_[i];
71
72 960 bool keep_this_tri =
73
6/12
✓ Branch 2 taken 480 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 480 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 480 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 480 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 480 times.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✓ Branch 20 taken 480 times.
 960 keep_vertex[t[0]] || keep_vertex[t[1]] || keep_vertex[t[2]];
74
75
1/2
✓ Branch 0 taken 480 times.
✗ Branch 1 not taken.
 960 if (!keep_this_tri) {
76
2/2
✓ Branch 0 taken 1120 times.
✓ Branch 1 taken 240 times.
 2720 for (unsigned int j = 0; j < 3; ++j) {
77
5/8
✓ Branch 3 taken 1120 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1120 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1120 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 240 times.
✓ Branch 12 taken 880 times.
 2240 if (aabb.contain(q * model_vertices_[t[j]])) {
78 480 keep_this_tri = true;
79 480 break;
80 }
81 }
82
83
1/2
✓ Branch 7 taken 480 times.
✗ Branch 8 not taken.
 960 const TriangleP tri(model_vertices_[t[0]], model_vertices_[t[1]],
84 model_vertices_[t[2]]);
85 960 const bool enable_nearest_points = false;
86 960 const bool compute_penetration = false; // we only need to know if there
87 // is a collision or not
88
1/2
✓ Branch 1 taken 480 times.
✗ Branch 2 not taken.
 960 const DistanceRequest distanceRequest(enable_nearest_points,
89 compute_penetration);
90
1/2
✓ Branch 2 taken 480 times.
✗ Branch 3 not taken.
 960 DistanceResult distanceResult;
91
1/2
✓ Branch 1 taken 480 times.
✗ Branch 2 not taken.
 960 const Scalar distance = ShapeShapeDistance<Box, TriangleP>(
92
1/2
✓ Branch 1 taken 480 times.
✗ Branch 2 not taken.
 960 &box, box_pose, &tri, Transform3s(), &gjk, distanceRequest,
93 distanceResult);
94 960 bool is_collision =
95
1/2
✓ Branch 1 taken 480 times.
✗ Branch 2 not taken.
 960 (distance <= gjk.getDistancePrecision(compute_penetration));
96
97
4/4
✓ Branch 0 taken 240 times.
✓ Branch 1 taken 240 times.
✓ Branch 2 taken 16 times.
✓ Branch 3 taken 224 times.
 960 if (!keep_this_tri && is_collision) {
98 32 keep_this_tri = true;
99 }
100 960 }
101
2/2
✓ Branch 0 taken 256 times.
✓ Branch 1 taken 224 times.
 960 if (keep_this_tri) {
102
3/6
✓ Branch 2 taken 256 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 256 times.
✗ Branch 8 not taken.
✓ Branch 12 taken 256 times.
✗ Branch 13 not taken.
 512 keep_vertex[t[0]] = keep_vertex[t[1]] = keep_vertex[t[2]] = true;
103
1/2
✓ Branch 1 taken 256 times.
✗ Branch 2 not taken.
 512 keep_tri[i] = true;
104 512 ntri++;
105 }
106 }
107
108
2/2
✓ Branch 0 taken 8 times.
✓ Branch 1 taken 32 times.
 80 if (ntri == 0) return NULL;
109
110
2/6
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 64 BVHModel<BV>* new_model(new BVHModel<BV>());
111
1/2
✓ Branch 2 taken 32 times.
✗ Branch 3 not taken.
 64 new_model->beginModel(ntri, std::min(ntri * 3, model.num_vertices));
112
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 std::vector<unsigned int> idxConversion(model.num_vertices);
113
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 32 times.
 64 assert(new_model->num_vertices == 0);
114 64 std::vector<Vec3s>& new_model_vertices_ = *(new_model->vertices);
115
2/2
✓ Branch 1 taken 1152 times.
✓ Branch 2 taken 32 times.
 2368 for (unsigned int i = 0; i < keep_vertex.size(); ++i) {
116
3/4
✓ Branch 1 taken 1152 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 768 times.
✓ Branch 5 taken 384 times.
 2304 if (keep_vertex[i]) {
117 1536 idxConversion[i] = new_model->num_vertices;
118
1/2
✓ Branch 3 taken 768 times.
✗ Branch 4 not taken.
 1536 new_model_vertices_[new_model->num_vertices] = model_vertices_[i];
119 1536 new_model->num_vertices++;
120 }
121 }
122
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 32 times.
 64 assert(new_model->num_tris == 0);
123 64 std::vector<Triangle32>& new_model_tri_indices_ = *(new_model->tri_indices);
124
2/2
✓ Branch 1 taken 384 times.
✓ Branch 2 taken 32 times.
 832 for (unsigned int i = 0; i < keep_tri.size(); ++i) {
125
3/4
✓ Branch 1 taken 384 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 256 times.
✓ Branch 5 taken 128 times.
 768 if (keep_tri[i]) {
126 512 new_model_tri_indices_[new_model->num_tris].set(
127 idxConversion[model_tri_indices_[i][0]],
128 idxConversion[model_tri_indices_[i][1]],
129 idxConversion[model_tri_indices_[i][2]]);
130 512 new_model->num_tris++;
131 }
132 }
133
2/4
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 32 times.
 64 if (new_model->endModel() != BVH_OK) {
134 ✗ delete new_model;
135 ✗ return NULL;
136 }
137 64 return new_model;
138 80 }
139 } // namespace details
140
141 template <>
142 5 BVHModel<OBB>* BVHExtract(const BVHModel<OBB>& model, const Transform3s& pose,
143 const AABB& aabb) {
144 5 return details::BVHExtract(model, pose, aabb);
145 }
146 template <>
147 5 BVHModel<AABB>* BVHExtract(const BVHModel<AABB>& model, const Transform3s& pose,
148 const AABB& aabb) {
149 5 return details::BVHExtract(model, pose, aabb);
150 }
151 template <>
152 5 BVHModel<RSS>* BVHExtract(const BVHModel<RSS>& model, const Transform3s& pose,
153 const AABB& aabb) {
154 5 return details::BVHExtract(model, pose, aabb);
155 }
156 template <>
157 5 BVHModel<kIOS>* BVHExtract(const BVHModel<kIOS>& model, const Transform3s& pose,
158 const AABB& aabb) {
159 5 return details::BVHExtract(model, pose, aabb);
160 }
161 template <>
162 5 BVHModel<OBBRSS>* BVHExtract(const BVHModel<OBBRSS>& model,
163 const Transform3s& pose, const AABB& aabb) {
164 5 return details::BVHExtract(model, pose, aabb);
165 }
166 template <>
167 5 BVHModel<KDOP<16> >* BVHExtract(const BVHModel<KDOP<16> >& model,
168 const Transform3s& pose, const AABB& aabb) {
169 5 return details::BVHExtract(model, pose, aabb);
170 }
171 template <>
172 5 BVHModel<KDOP<18> >* BVHExtract(const BVHModel<KDOP<18> >& model,
173 const Transform3s& pose, const AABB& aabb) {
174 5 return details::BVHExtract(model, pose, aabb);
175 }
176 template <>
177 5 BVHModel<KDOP<24> >* BVHExtract(const BVHModel<KDOP<24> >& model,
178 const Transform3s& pose, const AABB& aabb) {
179 5 return details::BVHExtract(model, pose, aabb);
180 }
181
182 1789350 void getCovariance(Vec3s* ps, Vec3s* ps2, Triangle32* ts, unsigned int* indices,
183 unsigned int n, Matrix3s& M) {
184
2/4
✓ Branch 1 taken 1789350 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1789350 times.
✗ Branch 5 not taken.
 1789350 Vec3s S1(Vec3s::Zero());
185
6/12
✓ Branch 1 taken 1789350 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1789350 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1789350 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1789350 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1789350 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1789350 times.
✗ Branch 17 not taken.
 1789350 Vec3s S2[3] = {Vec3s::Zero(), Vec3s::Zero(), Vec3s::Zero()};
186
187
2/2
✓ Branch 0 taken 1775801 times.
✓ Branch 1 taken 13549 times.
 1789350 if (ts) {
188
2/2
✓ Branch 0 taken 10154655 times.
✓ Branch 1 taken 1775801 times.
 11930456 for (unsigned int i = 0; i < n; ++i) {
189
1/2
✓ Branch 0 taken 10154655 times.
✗ Branch 1 not taken.
 10154655 const Triangle32& t = (indices) ? ts[indices[i]] : ts[i];
190
191 10154655 const Vec3s& p1 = ps[t[0]];
192 10154655 const Vec3s& p2 = ps[t[1]];
193 10154655 const Vec3s& p3 = ps[t[2]];
194
195
4/8
✓ Branch 1 taken 10154655 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10154655 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10154655 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10154655 times.
✗ Branch 11 not taken.
 10154655 S1[0] += (p1[0] + p2[0] + p3[0]);
196
4/8
✓ Branch 1 taken 10154655 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10154655 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10154655 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10154655 times.
✗ Branch 11 not taken.
 10154655 S1[1] += (p1[1] + p2[1] + p3[1]);
197
4/8
✓ Branch 1 taken 10154655 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10154655 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10154655 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10154655 times.
✗ Branch 11 not taken.
 10154655 S1[2] += (p1[2] + p2[2] + p3[2]);
198
7/14
✓ Branch 1 taken 10154655 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10154655 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10154655 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10154655 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10154655 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 10154655 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 10154655 times.
✗ Branch 20 not taken.
 10154655 S2[0][0] += (p1[0] * p1[0] + p2[0] * p2[0] + p3[0] * p3[0]);
199
7/14
✓ Branch 1 taken 10154655 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10154655 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10154655 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10154655 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10154655 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 10154655 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 10154655 times.
✗ Branch 20 not taken.
 10154655 S2[1][1] += (p1[1] * p1[1] + p2[1] * p2[1] + p3[1] * p3[1]);
200
7/14
✓ Branch 1 taken 10154655 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10154655 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10154655 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10154655 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10154655 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 10154655 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 10154655 times.
✗ Branch 20 not taken.
 10154655 S2[2][2] += (p1[2] * p1[2] + p2[2] * p2[2] + p3[2] * p3[2]);
201
7/14
✓ Branch 1 taken 10154655 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10154655 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10154655 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10154655 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10154655 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 10154655 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 10154655 times.
✗ Branch 20 not taken.
 10154655 S2[0][1] += (p1[0] * p1[1] + p2[0] * p2[1] + p3[0] * p3[1]);
202
7/14
✓ Branch 1 taken 10154655 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10154655 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10154655 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10154655 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10154655 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 10154655 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 10154655 times.
✗ Branch 20 not taken.
 10154655 S2[0][2] += (p1[0] * p1[2] + p2[0] * p2[2] + p3[0] * p3[2]);
203
7/14
✓ Branch 1 taken 10154655 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10154655 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10154655 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10154655 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10154655 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 10154655 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 10154655 times.
✗ Branch 20 not taken.
 10154655 S2[1][2] += (p1[1] * p1[2] + p2[1] * p2[2] + p3[1] * p3[2]);
204
205
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 10154655 times.
 10154655 if (ps2) {
206 ✗ const Vec3s& p1 = ps2[t[0]];
207 ✗ const Vec3s& p2 = ps2[t[1]];
208 ✗ const Vec3s& p3 = ps2[t[2]];
209
210 ✗ S1[0] += (p1[0] + p2[0] + p3[0]);
211 ✗ S1[1] += (p1[1] + p2[1] + p3[1]);
212 ✗ S1[2] += (p1[2] + p2[2] + p3[2]);
213
214 ✗ S2[0][0] += (p1[0] * p1[0] + p2[0] * p2[0] + p3[0] * p3[0]);
215 ✗ S2[1][1] += (p1[1] * p1[1] + p2[1] * p2[1] + p3[1] * p3[1]);
216 ✗ S2[2][2] += (p1[2] * p1[2] + p2[2] * p2[2] + p3[2] * p3[2]);
217 ✗ S2[0][1] += (p1[0] * p1[1] + p2[0] * p2[1] + p3[0] * p3[1]);
218 ✗ S2[0][2] += (p1[0] * p1[2] + p2[0] * p2[2] + p3[0] * p3[2]);
219 ✗ S2[1][2] += (p1[1] * p1[2] + p2[1] * p2[2] + p3[1] * p3[2]);
220 }
221 }
222 } else {
223
2/2
✓ Branch 0 taken 213428 times.
✓ Branch 1 taken 13549 times.
 226977 for (unsigned int i = 0; i < n; ++i) {
224
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 213428 times.
 213428 const Vec3s& p = (indices) ? ps[indices[i]] : ps[i];
225
1/2
✓ Branch 1 taken 213428 times.
✗ Branch 2 not taken.
 213428 S1 += p;
226
3/6
✓ Branch 1 taken 213428 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 213428 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 213428 times.
✗ Branch 8 not taken.
 213428 S2[0][0] += (p[0] * p[0]);
227
3/6
✓ Branch 1 taken 213428 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 213428 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 213428 times.
✗ Branch 8 not taken.
 213428 S2[1][1] += (p[1] * p[1]);
228
3/6
✓ Branch 1 taken 213428 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 213428 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 213428 times.
✗ Branch 8 not taken.
 213428 S2[2][2] += (p[2] * p[2]);
229
3/6
✓ Branch 1 taken 213428 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 213428 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 213428 times.
✗ Branch 8 not taken.
 213428 S2[0][1] += (p[0] * p[1]);
230
3/6
✓ Branch 1 taken 213428 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 213428 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 213428 times.
✗ Branch 8 not taken.
 213428 S2[0][2] += (p[0] * p[2]);
231
3/6
✓ Branch 1 taken 213428 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 213428 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 213428 times.
✗ Branch 8 not taken.
 213428 S2[1][2] += (p[1] * p[2]);
232
233
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 213428 times.
 213428 if (ps2) // another frame
234 {
235 ✗ const Vec3s& p = (indices) ? ps2[indices[i]] : ps2[i];
236 ✗ S1 += p;
237 ✗ S2[0][0] += (p[0] * p[0]);
238 ✗ S2[1][1] += (p[1] * p[1]);
239 ✗ S2[2][2] += (p[2] * p[2]);
240 ✗ S2[0][1] += (p[0] * p[1]);
241 ✗ S2[0][2] += (p[0] * p[2]);
242 ✗ S2[1][2] += (p[1] * p[2]);
243 }
244 }
245 }
246
247
3/4
✗ Branch 0 not taken.
✓ Branch 1 taken 1789350 times.
✓ Branch 2 taken 1775801 times.
✓ Branch 3 taken 13549 times.
 1789350 unsigned int n_points = ((ps2) ? 2 : 1) * ((ts) ? 3 : 1) * n;
248
249
4/8
✓ Branch 1 taken 1789350 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1789350 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1789350 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1789350 times.
✗ Branch 11 not taken.
 1789350 M(0, 0) = S2[0][0] - S1[0] * S1[0] / Scalar(n_points);
250
4/8
✓ Branch 1 taken 1789350 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1789350 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1789350 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1789350 times.
✗ Branch 11 not taken.
 1789350 M(1, 1) = S2[1][1] - S1[1] * S1[1] / Scalar(n_points);
251
4/8
✓ Branch 1 taken 1789350 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1789350 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1789350 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1789350 times.
✗ Branch 11 not taken.
 1789350 M(2, 2) = S2[2][2] - S1[2] * S1[2] / Scalar(n_points);
252
4/8
✓ Branch 1 taken 1789350 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1789350 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1789350 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1789350 times.
✗ Branch 11 not taken.
 1789350 M(0, 1) = S2[0][1] - S1[0] * S1[1] / Scalar(n_points);
253
4/8
✓ Branch 1 taken 1789350 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1789350 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1789350 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1789350 times.
✗ Branch 11 not taken.
 1789350 M(1, 2) = S2[1][2] - S1[1] * S1[2] / Scalar(n_points);
254
4/8
✓ Branch 1 taken 1789350 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1789350 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1789350 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1789350 times.
✗ Branch 11 not taken.
 1789350 M(0, 2) = S2[0][2] - S1[0] * S1[2] / Scalar(n_points);
255
2/4
✓ Branch 1 taken 1789350 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1789350 times.
✗ Branch 5 not taken.
 1789350 M(1, 0) = M(0, 1);
256
2/4
✓ Branch 1 taken 1789350 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1789350 times.
✗ Branch 5 not taken.
 1789350 M(2, 0) = M(0, 2);
257
2/4
✓ Branch 1 taken 1789350 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1789350 times.
✗ Branch 5 not taken.
 1789350 M(2, 1) = M(1, 2);
258 1789350 }
259
260 /** @brief Compute the RSS bounding volume parameters: radius, rectangle size
261 * and the origin. The bounding volume axes are known.
262 */
263 1347836 void getRadiusAndOriginAndRectangleSize(Vec3s* ps, Vec3s* ps2, Triangle32* ts,
264 unsigned int* indices, unsigned int n,
265 const Matrix3s& axes, Vec3s& origin,
266 Scalar l[2], Scalar& r) {
267 1347836 bool indirect_index = true;
268
2/2
✓ Branch 0 taken 26381 times.
✓ Branch 1 taken 1321455 times.
 1347836 if (!indices) indirect_index = false;
269
270
3/4
✗ Branch 0 not taken.
✓ Branch 1 taken 1347836 times.
✓ Branch 2 taken 1321455 times.
✓ Branch 3 taken 26381 times.
 1347836 unsigned int size_P = ((ps2) ? 2 : 1) * ((ts) ? 3 : 1) * n;
271
272
2/4
✓ Branch 0 taken 1347836 times.
✗ Branch 1 not taken.
✓ Branch 4 taken 1347836 times.
✗ Branch 5 not taken.
 1347836 Scalar(*P)[3] = new Scalar[size_P][3];
273
274 1347836 int P_id = 0;
275
276
2/2
✓ Branch 0 taken 1321455 times.
✓ Branch 1 taken 26381 times.
 1347836 if (ts) {
277
2/2
✓ Branch 0 taken 7265973 times.
✓ Branch 1 taken 1321455 times.
 8587428 for (unsigned int i = 0; i < n; ++i) {
278
1/2
✓ Branch 0 taken 7265973 times.
✗ Branch 1 not taken.
 7265973 unsigned int index = indirect_index ? indices[i] : i;
279 7265973 const Triangle32& t = ts[index];
280
281
2/2
✓ Branch 0 taken 21797919 times.
✓ Branch 1 taken 7265973 times.
 29063892 for (Triangle32::IndexType j = 0; j < 3; ++j) {
282 21797919 Triangle32::IndexType point_id = t[j];
283 21797919 const Vec3s& p = ps[point_id];
284
4/8
✓ Branch 1 taken 21797919 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 21797919 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 21797919 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 21797919 times.
✗ Branch 11 not taken.
 21797919 Vec3s v(p[0], p[1], p[2]);
285
2/4
✓ Branch 1 taken 21797919 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 21797919 times.
✗ Branch 5 not taken.
 21797919 P[P_id][0] = axes.col(0).dot(v);
286
2/4
✓ Branch 1 taken 21797919 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 21797919 times.
✗ Branch 5 not taken.
 21797919 P[P_id][1] = axes.col(1).dot(v);
287
2/4
✓ Branch 1 taken 21797919 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 21797919 times.
✗ Branch 5 not taken.
 21797919 P[P_id][2] = axes.col(2).dot(v);
288 21797919 P_id++;
289 }
290
291
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 7265973 times.
 7265973 if (ps2) {
292 ✗ for (Triangle32::IndexType j = 0; j < 3; ++j) {
293 ✗ Triangle32::IndexType point_id = t[j];
294 ✗ const Vec3s& p = ps2[point_id];
295 // FIXME Is this right ?????
296 ✗ Vec3s v(p[0], p[1], p[2]);
297 ✗ P[P_id][0] = axes.col(0).dot(v);
298 ✗ P[P_id][1] = axes.col(0).dot(v);
299 ✗ P[P_id][2] = axes.col(1).dot(v);
300 ✗ P_id++;
301 }
302 }
303 }
304 } else {
305
2/2
✓ Branch 0 taken 250378 times.
✓ Branch 1 taken 26381 times.
 276759 for (unsigned int i = 0; i < n; ++i) {
306
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 250378 times.
 250378 unsigned int index = indirect_index ? indices[i] : i;
307
308 250378 const Vec3s& p = ps[index];
309
4/8
✓ Branch 1 taken 250378 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 250378 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 250378 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 250378 times.
✗ Branch 11 not taken.
 250378 Vec3s v(p[0], p[1], p[2]);
310
2/4
✓ Branch 1 taken 250378 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 250378 times.
✗ Branch 5 not taken.
 250378 P[P_id][0] = axes.col(0).dot(v);
311
2/4
✓ Branch 1 taken 250378 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 250378 times.
✗ Branch 5 not taken.
 250378 P[P_id][1] = axes.col(1).dot(v);
312
2/4
✓ Branch 1 taken 250378 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 250378 times.
✗ Branch 5 not taken.
 250378 P[P_id][2] = axes.col(2).dot(v);
313 250378 P_id++;
314
315
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 250378 times.
 250378 if (ps2) {
316 ✗ const Vec3s& v = ps2[index];
317 ✗ P[P_id][0] = axes.col(0).dot(v);
318 ✗ P[P_id][1] = axes.col(1).dot(v);
319 ✗ P[P_id][2] = axes.col(2).dot(v);
320 ✗ P_id++;
321 }
322 }
323 }
324
325 Scalar minx, maxx, miny, maxy, minz, maxz;
326
327 Scalar cz, radsqr;
328
329 1347836 minz = maxz = P[0][2];
330
331
2/2
✓ Branch 0 taken 20700461 times.
✓ Branch 1 taken 1347836 times.
 22048297 for (unsigned int i = 1; i < size_P; ++i) {
332 20700461 Scalar z_value = P[i][2];
333
2/2
✓ Branch 0 taken 1683918 times.
✓ Branch 1 taken 19016543 times.
 20700461 if (z_value < minz)
334 1683918 minz = z_value;
335
2/2
✓ Branch 0 taken 1665672 times.
✓ Branch 1 taken 17350871 times.
 19016543 else if (z_value > maxz)
336 1665672 maxz = z_value;
337 }
338
339 1347836 r = (Scalar)0.5 * (maxz - minz);
340 1347836 radsqr = r * r;
341 1347836 cz = (Scalar)0.5 * (maxz + minz);
342
343 // compute an initial norm of rectangle along x direction
344
345 // find minx and maxx as starting points
346
347 1347836 unsigned int minindex = 0, maxindex = 0;
348 Scalar mintmp, maxtmp;
349 1347836 mintmp = maxtmp = P[0][0];
350
351
2/2
✓ Branch 0 taken 20700461 times.
✓ Branch 1 taken 1347836 times.
 22048297 for (unsigned int i = 1; i < size_P; ++i) {
352 20700461 Scalar x_value = P[i][0];
353
2/2
✓ Branch 0 taken 1736049 times.
✓ Branch 1 taken 18964412 times.
 20700461 if (x_value < mintmp) {
354 1736049 minindex = i;
355 1736049 mintmp = x_value;
356
2/2
✓ Branch 0 taken 1776736 times.
✓ Branch 1 taken 17187676 times.
 18964412 } else if (x_value > maxtmp) {
357 1776736 maxindex = i;
358 1776736 maxtmp = x_value;
359 }
360 }
361
362 Scalar x, dz;
363 1347836 dz = P[minindex][2] - cz;
364 1347836 minx = P[minindex][0] + std::sqrt(std::max<Scalar>(radsqr - dz * dz, 0));
365 1347836 dz = P[maxindex][2] - cz;
366 1347836 maxx = P[maxindex][0] - std::sqrt(std::max<Scalar>(radsqr - dz * dz, 0));
367
368 // grow minx/maxx
369
370
2/2
✓ Branch 0 taken 22048297 times.
✓ Branch 1 taken 1347836 times.
 23396133 for (unsigned int i = 0; i < size_P; ++i) {
371
2/2
✓ Branch 0 taken 3160131 times.
✓ Branch 1 taken 18888166 times.
 22048297 if (P[i][0] < minx) {
372 3160131 dz = P[i][2] - cz;
373 3160131 x = P[i][0] + std::sqrt(std::max<Scalar>(radsqr - dz * dz, 0));
374
2/2
✓ Branch 0 taken 211139 times.
✓ Branch 1 taken 2948992 times.
 3160131 if (x < minx) minx = x;
375
2/2
✓ Branch 0 taken 3126881 times.
✓ Branch 1 taken 15761285 times.
 18888166 } else if (P[i][0] > maxx) {
376 3126881 dz = P[i][2] - cz;
377 3126881 x = P[i][0] - std::sqrt(std::max<Scalar>(radsqr - dz * dz, 0));
378
2/2
✓ Branch 0 taken 217396 times.
✓ Branch 1 taken 2909485 times.
 3126881 if (x > maxx) maxx = x;
379 }
380 }
381
382 // compute an initial norm of rectangle along y direction
383
384 // find miny and maxy as starting points
385
386 1347836 minindex = maxindex = 0;
387 1347836 mintmp = maxtmp = P[0][1];
388
2/2
✓ Branch 0 taken 20700461 times.
✓ Branch 1 taken 1347836 times.
 22048297 for (unsigned int i = 1; i < size_P; ++i) {
389 20700461 Scalar y_value = P[i][1];
390
2/2
✓ Branch 0 taken 2386779 times.
✓ Branch 1 taken 18313682 times.
 20700461 if (y_value < mintmp) {
391 2386779 minindex = i;
392 2386779 mintmp = y_value;
393
2/2
✓ Branch 0 taken 1729269 times.
✓ Branch 1 taken 16584413 times.
 18313682 } else if (y_value > maxtmp) {
394 1729269 maxindex = i;
395 1729269 maxtmp = y_value;
396 }
397 }
398
399 Scalar y;
400 1347836 dz = P[minindex][2] - cz;
401 1347836 miny = P[minindex][1] + std::sqrt(std::max<Scalar>(radsqr - dz * dz, 0));
402 1347836 dz = P[maxindex][2] - cz;
403 1347836 maxy = P[maxindex][1] - std::sqrt(std::max<Scalar>(radsqr - dz * dz, 0));
404
405 // grow miny/maxy
406
407
2/2
✓ Branch 0 taken 22048297 times.
✓ Branch 1 taken 1347836 times.
 23396133 for (unsigned int i = 0; i < size_P; ++i) {
408
2/2
✓ Branch 0 taken 3064489 times.
✓ Branch 1 taken 18983808 times.
 22048297 if (P[i][1] < miny) {
409 3064489 dz = P[i][2] - cz;
410 3064489 y = P[i][1] + std::sqrt(std::max<Scalar>(radsqr - dz * dz, 0));
411
2/2
✓ Branch 0 taken 117534 times.
✓ Branch 1 taken 2946955 times.
 3064489 if (y < miny) miny = y;
412
2/2
✓ Branch 0 taken 3045839 times.
✓ Branch 1 taken 15937969 times.
 18983808 } else if (P[i][1] > maxy) {
413 3045839 dz = P[i][2] - cz;
414 3045839 y = P[i][1] - std::sqrt(std::max<Scalar>(radsqr - dz * dz, 0));
415
2/2
✓ Branch 0 taken 113644 times.
✓ Branch 1 taken 2932195 times.
 3045839 if (y > maxy) maxy = y;
416 }
417 }
418
419 // corners may have some points which are not covered - grow lengths if
420 // necessary quite conservative (can be improved)
421 Scalar dx, dy, u, t;
422 1347836 Scalar a = std::sqrt((Scalar)0.5);
423
2/2
✓ Branch 0 taken 22048297 times.
✓ Branch 1 taken 1347836 times.
 23396133 for (unsigned int i = 0; i < size_P; ++i) {
424
2/2
✓ Branch 0 taken 2837914 times.
✓ Branch 1 taken 19210383 times.
 22048297 if (P[i][0] > maxx) {
425
2/2
✓ Branch 0 taken 456658 times.
✓ Branch 1 taken 2381256 times.
 2837914 if (P[i][1] > maxy) {
426 456658 dx = P[i][0] - maxx;
427 456658 dy = P[i][1] - maxy;
428 456658 u = dx * a + dy * a;
429 456658 t = (a * u - dx) * (a * u - dx) + (a * u - dy) * (a * u - dy) +
430 456658 (cz - P[i][2]) * (cz - P[i][2]);
431 456658 u = u - std::sqrt(std::max<Scalar>(radsqr - t, 0));
432
2/2
✓ Branch 0 taken 95062 times.
✓ Branch 1 taken 361596 times.
 456658 if (u > 0) {
433 95062 maxx += u * a;
434 95062 maxy += u * a;
435 }
436
2/2
✓ Branch 0 taken 458630 times.
✓ Branch 1 taken 1922626 times.
 2381256 } else if (P[i][1] < miny) {
437 458630 dx = P[i][0] - maxx;
438 458630 dy = P[i][1] - miny;
439 458630 u = dx * a - dy * a;
440 458630 t = (a * u - dx) * (a * u - dx) + (-a * u - dy) * (-a * u - dy) +
441 458630 (cz - P[i][2]) * (cz - P[i][2]);
442 458630 u = u - std::sqrt(std::max<Scalar>(radsqr - t, 0));
443
2/2
✓ Branch 0 taken 100484 times.
✓ Branch 1 taken 358146 times.
 458630 if (u > 0) {
444 100484 maxx += u * a;
445 100484 miny -= u * a;
446 }
447 }
448
2/2
✓ Branch 0 taken 2842349 times.
✓ Branch 1 taken 16368034 times.
 19210383 } else if (P[i][0] < minx) {
449
2/2
✓ Branch 0 taken 439663 times.
✓ Branch 1 taken 2402686 times.
 2842349 if (P[i][1] > maxy) {
450 439663 dx = P[i][0] - minx;
451 439663 dy = P[i][1] - maxy;
452 439663 u = dy * a - dx * a;
453 439663 t = (-a * u - dx) * (-a * u - dx) + (a * u - dy) * (a * u - dy) +
454 439663 (cz - P[i][2]) * (cz - P[i][2]);
455 439663 u = u - std::sqrt(std::max<Scalar>(radsqr - t, 0));
456
2/2
✓ Branch 0 taken 91716 times.
✓ Branch 1 taken 347947 times.
 439663 if (u > 0) {
457 91716 minx -= u * a;
458 91716 maxy += u * a;
459 }
460
2/2
✓ Branch 0 taken 444583 times.
✓ Branch 1 taken 1958103 times.
 2402686 } else if (P[i][1] < miny) {
461 444583 dx = P[i][0] - minx;
462 444583 dy = P[i][1] - miny;
463 444583 u = -dx * a - dy * a;
464 444583 t = (-a * u - dx) * (-a * u - dx) + (-a * u - dy) * (-a * u - dy) +
465 444583 (cz - P[i][2]) * (cz - P[i][2]);
466 444583 u = u - std::sqrt(std::max<Scalar>(radsqr - t, 0));
467
2/2
✓ Branch 0 taken 110241 times.
✓ Branch 1 taken 334342 times.
 444583 if (u > 0) {
468 110241 minx -= u * a;
469 110241 miny -= u * a;
470 }
471 }
472 }
473 }
474
475
4/8
✓ Branch 1 taken 1347836 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1347836 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1347836 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1347836 times.
✗ Branch 11 not taken.
 1347836 origin.noalias() = axes * Vec3s(minx, miny, cz);
476
477 1347836 l[0] = std::max<Scalar>(maxx - minx, 0);
478 1347836 l[1] = std::max<Scalar>(maxy - miny, 0);
479
480
1/2
✓ Branch 0 taken 1347836 times.
✗ Branch 1 not taken.
 1347836 delete[] P;
481 1347836 }
482
483 /** @brief Compute the bounding volume extent and center for a set or subset of
484 * points. The bounding volume axes are known.
485 */
486 19868 static inline void getExtentAndCenter_pointcloud(Vec3s* ps, Vec3s* ps2,
487 unsigned int* indices,
488 unsigned int n, Matrix3s& axes,
489 Vec3s& center, Vec3s& extent) {
490 19868 bool indirect_index = true;
491
1/2
✓ Branch 0 taken 19868 times.
✗ Branch 1 not taken.
 19868 if (!indices) indirect_index = false;
492
493 19868 Scalar real_max = (std::numeric_limits<Scalar>::max)();
494
495
1/2
✓ Branch 1 taken 19868 times.
✗ Branch 2 not taken.
 19868 Vec3s min_coord(real_max, real_max, real_max);
496
1/2
✓ Branch 1 taken 19868 times.
✗ Branch 2 not taken.
 19868 Vec3s max_coord(-real_max, -real_max, -real_max);
497
498
2/2
✓ Branch 0 taken 61150 times.
✓ Branch 1 taken 19868 times.
 81018 for (unsigned int i = 0; i < n; ++i) {
499
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 61150 times.
 61150 unsigned int index = indirect_index ? indices[i] : i;
500
501 61150 const Vec3s& p = ps[index];
502
3/6
✓ Branch 1 taken 61150 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 61150 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 61150 times.
✗ Branch 8 not taken.
 61150 Vec3s proj(axes.transpose() * p);
503
504
2/2
✓ Branch 0 taken 183450 times.
✓ Branch 1 taken 61150 times.
 244600 for (int j = 0; j < 3; ++j) {
505
6/10
✓ Branch 1 taken 183450 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 183450 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 89341 times.
✓ Branch 7 taken 94109 times.
✓ Branch 9 taken 89341 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 89341 times.
✗ Branch 13 not taken.
 183450 if (proj[j] > max_coord[j]) max_coord[j] = proj[j];
506
6/10
✓ Branch 1 taken 183450 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 183450 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 98136 times.
✓ Branch 7 taken 85314 times.
✓ Branch 9 taken 98136 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 98136 times.
✗ Branch 13 not taken.
 183450 if (proj[j] < min_coord[j]) min_coord[j] = proj[j];
507 }
508
509
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 61150 times.
 61150 if (ps2) {
510 ✗ const Vec3s& v = ps2[index];
511 ✗ proj.noalias() = axes.transpose() * v;
512
513 ✗ for (int j = 0; j < 3; ++j) {
514 ✗ if (proj[j] > max_coord[j]) max_coord[j] = proj[j];
515 ✗ if (proj[j] < min_coord[j]) min_coord[j] = proj[j];
516 }
517 }
518 }
519
520
5/10
✓ Branch 1 taken 19868 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 19868 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 19868 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 19868 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 19868 times.
✗ Branch 14 not taken.
 19868 center.noalias() = axes * (max_coord + min_coord) / 2;
521
522
4/8
✓ Branch 1 taken 19868 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 19868 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 19868 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 19868 times.
✗ Branch 11 not taken.
 19868 extent.noalias() = (max_coord - min_coord) / 2;
523 19868 }
524
525 /** @brief Compute the bounding volume extent and center for a set or subset of
526 * points. The bounding volume axes are known.
527 */
528 1479132 static inline void getExtentAndCenter_mesh(Vec3s* ps, Vec3s* ps2,
529 Triangle32* ts,
530 unsigned int* indices,
531 unsigned int n, Matrix3s& axes,
532 Vec3s& center, Vec3s& extent) {
533 1479132 bool indirect_index = true;
534
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1479132 times.
 1479132 if (!indices) indirect_index = false;
535
536 1479132 Scalar real_max = (std::numeric_limits<Scalar>::max)();
537
538
1/2
✓ Branch 1 taken 1479132 times.
✗ Branch 2 not taken.
 1479132 Vec3s min_coord(real_max, real_max, real_max);
539
1/2
✓ Branch 1 taken 1479132 times.
✗ Branch 2 not taken.
 1479132 Vec3s max_coord(-real_max, -real_max, -real_max);
540
541
2/2
✓ Branch 0 taken 8273139 times.
✓ Branch 1 taken 1479132 times.
 9752271 for (unsigned int i = 0; i < n; ++i) {
542
1/2
✓ Branch 0 taken 8273139 times.
✗ Branch 1 not taken.
 8273139 unsigned int index = indirect_index ? indices[i] : i;
543 8273139 const Triangle32& t = ts[index];
544
545
2/2
✓ Branch 0 taken 24819417 times.
✓ Branch 1 taken 8273139 times.
 33092556 for (Triangle32::IndexType j = 0; j < 3; ++j) {
546 24819417 Triangle32::IndexType point_id = t[j];
547 24819417 const Vec3s& p = ps[point_id];
548
3/6
✓ Branch 1 taken 24819417 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24819417 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 24819417 times.
✗ Branch 8 not taken.
 24819417 Vec3s proj(axes.transpose() * p);
549
550
2/2
✓ Branch 0 taken 74458251 times.
✓ Branch 1 taken 24819417 times.
 99277668 for (int k = 0; k < 3; ++k) {
551
6/10
✓ Branch 1 taken 74458251 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 74458251 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 10224501 times.
✓ Branch 7 taken 64233750 times.
✓ Branch 9 taken 10224501 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 10224501 times.
✗ Branch 13 not taken.
 74458251 if (proj[k] > max_coord[k]) max_coord[k] = proj[k];
552
6/10
✓ Branch 1 taken 74458251 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 74458251 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 10760103 times.
✓ Branch 7 taken 63698148 times.
✓ Branch 9 taken 10760103 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 10760103 times.
✗ Branch 13 not taken.
 74458251 if (proj[k] < min_coord[k]) min_coord[k] = proj[k];
553 }
554 }
555
556
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 8273139 times.
 8273139 if (ps2) {
557 ✗ for (Triangle32::IndexType j = 0; j < 3; ++j) {
558 ✗ Triangle32::IndexType point_id = t[j];
559 ✗ const Vec3s& p = ps2[point_id];
560 ✗ Vec3s proj(axes.transpose() * p);
561
562 ✗ for (int k = 0; k < 3; ++k) {
563 ✗ if (proj[k] > max_coord[k]) max_coord[k] = proj[k];
564 ✗ if (proj[k] < min_coord[k]) min_coord[k] = proj[k];
565 }
566 }
567 }
568 }
569
570
3/6
✓ Branch 1 taken 1479132 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1479132 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1479132 times.
✗ Branch 8 not taken.
 1479132 Vec3s o((max_coord + min_coord) / 2);
571
572
3/6
✓ Branch 1 taken 1479132 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1479132 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1479132 times.
✗ Branch 8 not taken.
 1479132 center.noalias() = axes * o;
573
574
4/8
✓ Branch 1 taken 1479132 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1479132 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1479132 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1479132 times.
✗ Branch 11 not taken.
 1479132 extent.noalias() = (max_coord - min_coord) / 2;
575 1479132 }
576
577 1499000 void getExtentAndCenter(Vec3s* ps, Vec3s* ps2, Triangle32* ts,
578 unsigned int* indices, unsigned int n, Matrix3s& axes,
579 Vec3s& center, Vec3s& extent) {
580
2/2
✓ Branch 0 taken 1479132 times.
✓ Branch 1 taken 19868 times.
 1499000 if (ts)
581 1479132 getExtentAndCenter_mesh(ps, ps2, ts, indices, n, axes, center, extent);
582 else
583 19868 getExtentAndCenter_pointcloud(ps, ps2, indices, n, axes, center, extent);
584 1499000 }
585
586 6540 void circumCircleComputation(const Vec3s& a, const Vec3s& b, const Vec3s& c,
587 Vec3s& center, Scalar& radius) {
588
2/4
✓ Branch 1 taken 6540 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6540 times.
✗ Branch 5 not taken.
 6540 Vec3s e1 = a - c;
589
2/4
✓ Branch 1 taken 6540 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6540 times.
✗ Branch 5 not taken.
 6540 Vec3s e2 = b - c;
590
1/2
✓ Branch 1 taken 6540 times.
✗ Branch 2 not taken.
 6540 Scalar e1_len2 = e1.squaredNorm();
591
1/2
✓ Branch 1 taken 6540 times.
✗ Branch 2 not taken.
 6540 Scalar e2_len2 = e2.squaredNorm();
592
1/2
✓ Branch 1 taken 6540 times.
✗ Branch 2 not taken.
 6540 Vec3s e3 = e1.cross(e2);
593
1/2
✓ Branch 1 taken 6540 times.
✗ Branch 2 not taken.
 6540 Scalar e3_len2 = e3.squaredNorm();
594
2/4
✓ Branch 1 taken 6540 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6540 times.
✗ Branch 5 not taken.
 6540 radius = e1_len2 * e2_len2 * (e1 - e2).squaredNorm() / e3_len2;
595 6540 radius = std::sqrt(radius) * Scalar(0.5);
596
597
7/14
✓ Branch 1 taken 6540 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6540 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6540 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6540 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 6540 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 6540 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 6540 times.
✗ Branch 20 not taken.
 6540 center = (e2 * e1_len2 - e1 * e2_len2).cross(e3) * (0.5 * 1 / e3_len2) + c;
598 6540 }
599
600 667935 static inline Scalar maximumDistance_mesh(Vec3s* ps, Vec3s* ps2, Triangle32* ts,
601 unsigned int* indices, unsigned int n,
602 const Vec3s& query) {
603 667935 bool indirect_index = true;
604
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 667935 times.
 667935 if (!indices) indirect_index = false;
605
606 667935 Scalar maxD = 0;
607
2/2
✓ Branch 0 taken 3340620 times.
✓ Branch 1 taken 667935 times.
 4008555 for (unsigned int i = 0; i < n; ++i) {
608
1/2
✓ Branch 0 taken 3340620 times.
✗ Branch 1 not taken.
 3340620 unsigned int index = indirect_index ? indices[i] : i;
609 3340620 const Triangle32& t = ts[index];
610
611
2/2
✓ Branch 0 taken 10021860 times.
✓ Branch 1 taken 3340620 times.
 13362480 for (Triangle32::IndexType j = 0; j < 3; ++j) {
612 10021860 Triangle32::IndexType point_id = t[j];
613 10021860 const Vec3s& p = ps[point_id];
614
615
2/4
✓ Branch 1 taken 10021860 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10021860 times.
✗ Branch 5 not taken.
 10021860 Scalar d = (p - query).squaredNorm();
616
2/2
✓ Branch 0 taken 1566115 times.
✓ Branch 1 taken 8455745 times.
 10021860 if (d > maxD) maxD = d;
617 }
618
619
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 3340620 times.
 3340620 if (ps2) {
620 ✗ for (Triangle32::IndexType j = 0; j < 3; ++j) {
621 ✗ Triangle32::IndexType point_id = t[j];
622 ✗ const Vec3s& p = ps2[point_id];
623
624 ✗ Scalar d = (p - query).squaredNorm();
625 ✗ if (d > maxD) maxD = d;
626 }
627 }
628 }
629
630 667935 return std::sqrt(maxD);
631 }
632
633 3 static inline Scalar maximumDistance_pointcloud(Vec3s* ps, Vec3s* ps2,
634 unsigned int* indices,
635 unsigned int n,
636 const Vec3s& query) {
637 3 bool indirect_index = true;
638
1/2
✓ Branch 0 taken 3 times.
✗ Branch 1 not taken.
 3 if (!indices) indirect_index = false;
639
640 3 Scalar maxD = 0;
641
2/2
✓ Branch 0 taken 36 times.
✓ Branch 1 taken 3 times.
 39 for (unsigned int i = 0; i < n; ++i) {
642
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 36 times.
 36 unsigned int index = indirect_index ? indices[i] : i;
643
644 36 const Vec3s& p = ps[index];
645
2/4
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 36 times.
✗ Branch 5 not taken.
 36 Scalar d = (p - query).squaredNorm();
646
2/2
✓ Branch 0 taken 7 times.
✓ Branch 1 taken 29 times.
 36 if (d > maxD) maxD = d;
647
648
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 36 times.
 36 if (ps2) {
649 ✗ const Vec3s& v = ps2[index];
650 ✗ Scalar d = (v - query).squaredNorm();
651 ✗ if (d > maxD) maxD = d;
652 }
653 }
654
655 3 return std::sqrt(maxD);
656 }
657
658 667938 Scalar maximumDistance(Vec3s* ps, Vec3s* ps2, Triangle32* ts,
659 unsigned int* indices, unsigned int n,
660 const Vec3s& query) {
661
2/2
✓ Branch 0 taken 667935 times.
✓ Branch 1 taken 3 times.
 667938 if (ts)
662 667935 return maximumDistance_mesh(ps, ps2, ts, indices, n, query);
663 else
664 3 return maximumDistance_pointcloud(ps, ps2, indices, n, query);
665 }
666
667 } // namespace coal
668