GCC Code Coverage Report

Directory: ./
File: src/BVH/BVH_model.cpp
Date: 2025-04-13 14:25:19
Exec Total Coverage
Lines: 371 658 56.4%
Branches: 256 701 36.5%
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 * Copyright (c) 2020, INRIA
7 * All rights reserved.
8 *
9 * Redistribution and use in source and binary forms, with or without
10 * modification, are permitted provided that the following conditions
11 * are met:
12 *
13 * * Redistributions of source code must retain the above copyright
14 * notice, this list of conditions and the following disclaimer.
15 * * Redistributions in binary form must reproduce the above
16 * copyright notice, this list of conditions and the following
17 * disclaimer in the documentation and/or other materials provided
18 * with the distribution.
19 * * Neither the name of Open Source Robotics Foundation nor the names of its
20 * contributors may be used to endorse or promote products derived
21 * from this software without specific prior written permission.
22 *
23 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 * POSSIBILITY OF SUCH DAMAGE.
35 */
36
37 /** \author Jia Pan */
38
39 #include "coal/BV/BV_node.h"
40 #include "coal/BVH/BVH_model.h"
41
42 #include "coal/BV/BV.h"
43 #include "coal/shape/convex.h"
44
45 #include "coal/internal/BV_splitter.h"
46 #include "coal/internal/BV_fitter.h"
47
48 #include <iostream>
49 #include <string.h>
50
51 namespace coal {
52
53 3114 BVHModelBase::BVHModelBase()
54 3114 : num_tris(0),
55 3114 num_vertices(0),
56 3114 build_state(BVH_BUILD_STATE_EMPTY),
57 3114 num_tris_allocated(0),
58 3114 num_vertices_allocated(0),
59 3114 num_vertex_updated(0) {}
60
61 44 BVHModelBase::BVHModelBase(const BVHModelBase& other)
62 : CollisionGeometry(other),
63 44 num_tris(other.num_tris),
64 44 num_vertices(other.num_vertices),
65 44 build_state(other.build_state),
66 44 num_tris_allocated(other.num_tris),
67 44 num_vertices_allocated(other.num_vertices) {
68
3/6
✓ Branch 1 taken 44 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 44 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 44 times.
✗ Branch 8 not taken.
 44 if (other.vertices.get() && other.vertices->size() > 0) {
69
3/6
✓ Branch 2 taken 44 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 44 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 44 times.
✗ Branch 9 not taken.
 44 vertices.reset(new std::vector<Vec3s>(*(other.vertices)));
70 } else
71 ✗ vertices.reset();
72
73
3/6
✓ Branch 1 taken 44 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 44 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 44 times.
✗ Branch 8 not taken.
 44 if (other.tri_indices.get() && other.tri_indices->size() > 0) {
74
3/6
✓ Branch 2 taken 44 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 44 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 44 times.
✗ Branch 9 not taken.
 44 tri_indices.reset(new std::vector<Triangle>(*(other.tri_indices)));
75 } else
76 ✗ tri_indices.reset();
77
78
2/6
✗ Branch 1 not taken.
✓ Branch 2 taken 44 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 44 times.
 44 if (other.prev_vertices.get() && other.prev_vertices->size() > 0) {
79 ✗ prev_vertices.reset(new std::vector<Vec3s>(*(other.prev_vertices)));
80 } else
81 44 prev_vertices.reset();
82 44 }
83
84 21 bool BVHModelBase::isEqual(const CollisionGeometry& _other) const {
85
1/2
✓ Branch 0 taken 21 times.
✗ Branch 1 not taken.
 21 const BVHModelBase* other_ptr = dynamic_cast<const BVHModelBase*>(&_other);
86
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 21 times.
 21 if (other_ptr == nullptr) return false;
87 21 const BVHModelBase& other = *other_ptr;
88
89 21 bool result =
90
3/4
✓ Branch 0 taken 20 times.
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
 21 num_tris == other.num_tris && num_vertices == other.num_vertices;
91
92
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 20 times.
 21 if (!result) return false;
93
94
3/8
✗ Branch 1 not taken.
✓ Branch 2 taken 20 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 20 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 20 times.
 60 if ((!(tri_indices.get()) && other.tri_indices.get()) ||
95
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 20 times.
 40 (tri_indices.get() && !(other.tri_indices.get())))
96 ✗ return false;
97
3/6
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 20 times.
✗ Branch 7 not taken.
 20 if (tri_indices.get() && other.tri_indices.get()) {
98 20 const std::vector<Triangle>& tri_indices_ = *(tri_indices);
99 20 const std::vector<Triangle>& other_tri_indices_ = *(other.tri_indices);
100
2/2
✓ Branch 0 taken 41636 times.
✓ Branch 1 taken 20 times.
 41656 for (size_t k = 0; k < static_cast<size_t>(num_tris); ++k)
101
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 41636 times.
 41636 if (tri_indices_[k] != other_tri_indices_[k]) return false;
102 }
103
104
3/8
✗ Branch 1 not taken.
✓ Branch 2 taken 20 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 20 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 20 times.
 60 if ((!(vertices.get()) && other.vertices.get()) ||
105
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 20 times.
 40 (vertices.get() && !(other.vertices.get())))
106 ✗ return false;
107
3/6
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 20 times.
✗ Branch 7 not taken.
 20 if (vertices.get() && other.vertices.get()) {
108 20 const std::vector<Vec3s>& vertices_ = *(vertices);
109 20 const std::vector<Vec3s>& other_vertices_ = *(other.vertices);
110
2/2
✓ Branch 0 taken 124908 times.
✓ Branch 1 taken 20 times.
 124928 for (size_t k = 0; k < static_cast<size_t>(num_vertices); ++k)
111
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 124908 times.
 124908 if (vertices_[k] != other_vertices_[k]) return false;
112 }
113
114
4/8
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 20 times.
 40 if ((!(prev_vertices.get()) && other.prev_vertices.get()) ||
115
0/2
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 20 (prev_vertices.get() && !(other.prev_vertices.get())))
116 ✗ return false;
117
2/6
✗ Branch 1 not taken.
✓ Branch 2 taken 20 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 20 times.
 20 if (prev_vertices.get() && other.prev_vertices.get()) {
118 ✗ const std::vector<Vec3s>& prev_vertices_ = *(prev_vertices);
119 ✗ const std::vector<Vec3s>& other_prev_vertices_ = *(other.prev_vertices);
120 ✗ for (size_t k = 0; k < static_cast<size_t>(num_vertices); ++k) {
121 ✗ if (prev_vertices_[k] != other_prev_vertices_[k]) return false;
122 }
123 }
124
125 20 return true;
126 }
127
128 3 void BVHModelBase::buildConvexRepresentation(bool share_memory) {
129
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 3 times.
 3 if (!(vertices.get())) {
130 std::cerr << "BVH Error in `buildConvexRepresentation`! The BVHModel has "
131 ✗ "no vertices."
132 ✗ << std::endl;
133 ✗ return;
134 }
135
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 3 times.
 3 if (!(tri_indices.get())) {
136 std::cerr << "BVH Error in `buildConvexRepresentation`! The BVHModel has "
137 ✗ "no triangles."
138 ✗ << std::endl;
139 ✗ return;
140 }
141
142
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 if (!convex) {
143 3 std::shared_ptr<std::vector<Vec3s>> points = vertices;
144 3 std::shared_ptr<std::vector<Triangle>> polygons = tri_indices;
145
1/2
✓ Branch 0 taken 3 times.
✗ Branch 1 not taken.
 3 if (!share_memory) {
146
3/6
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
 3 points.reset(new std::vector<Vec3s>(*(vertices)));
147
3/6
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
 3 polygons.reset(new std::vector<Triangle>(*(tri_indices)));
148 }
149
1/2
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
 6 convex.reset(
150
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 3 new Convex<Triangle>(points, num_vertices, polygons, num_tris));
151 3 }
152 }
153
154 ✗ bool BVHModelBase::buildConvexHull(bool keepTriangle,
155 const char* qhullCommand) {
156 ✗ convex.reset(ConvexBase::convexHull(vertices, num_vertices, keepTriangle,
157 qhullCommand));
158 ✗ return num_vertices == convex->num_points;
159 }
160
161 template <typename BV>
162 88 BVHModel<BV>::BVHModel(const BVHModel<BV>& other)
163 : BVHModelBase(other),
164 88 bv_splitter(other.bv_splitter),
165 88 bv_fitter(other.bv_fitter) {
166
1/2
✓ Branch 1 taken 44 times.
✗ Branch 2 not taken.
 88 if (other.primitive_indices.get()) {
167
1/2
✓ Branch 1 taken 44 times.
✗ Branch 2 not taken.
 88 primitive_indices.reset(
168
2/4
✓ Branch 2 taken 44 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 44 times.
✗ Branch 6 not taken.
 88 new std::vector<unsigned int>(*(other.primitive_indices)));
169 } else
170 ✗ primitive_indices.reset();
171
172 88 num_bvs = num_bvs_allocated = other.num_bvs;
173
1/2
✓ Branch 1 taken 44 times.
✗ Branch 2 not taken.
 88 if (other.bvs.get()) {
174
3/6
✓ Branch 2 taken 44 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 44 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 44 times.
✗ Branch 9 not taken.
 88 bvs.reset(new bv_node_vector_t(*(other.bvs)));
175 } else
176 ✗ bvs.reset();
177 }
178
179 3108 int BVHModelBase::beginModel(unsigned int num_tris_,
180 unsigned int num_vertices_) {
181
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 3105 times.
 3108 if (build_state != BVH_BUILD_STATE_EMPTY) {
182 3 vertices.reset();
183 3 tri_indices.reset();
184 3 tri_indices.reset();
185 3 prev_vertices.reset();
186
187 3 num_vertices_allocated = num_vertices = num_tris_allocated = num_tris = 0;
188 3 deleteBVs();
189 }
190
191
2/2
✓ Branch 0 taken 3076 times.
✓ Branch 1 taken 32 times.
 3108 if (num_tris_ <= 0) num_tris_ = 8;
192
2/2
✓ Branch 0 taken 3076 times.
✓ Branch 1 taken 32 times.
 3108 if (num_vertices_ <= 0) num_vertices_ = 8;
193
194 3108 num_vertices_allocated = num_vertices_;
195 3108 num_tris_allocated = num_tris_;
196
197
1/2
✓ Branch 0 taken 3108 times.
✗ Branch 1 not taken.
 3108 if (num_tris_allocated > 0) {
198
3/6
✓ Branch 2 taken 3108 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3108 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3108 times.
✗ Branch 9 not taken.
 3108 tri_indices.reset(new std::vector<Triangle>(num_tris_allocated));
199
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 3108 times.
 3108 if (!(tri_indices.get())) {
200 std::cerr << "BVH Error! Out of memory for tri_indices array on "
201 ✗ "BeginModel() call!"
202 ✗ << std::endl;
203 ✗ return BVH_ERR_MODEL_OUT_OF_MEMORY;
204 }
205 } else
206 ✗ tri_indices.reset();
207
208
1/2
✓ Branch 0 taken 3108 times.
✗ Branch 1 not taken.
 3108 if (num_vertices_allocated > 0) {
209
3/6
✓ Branch 2 taken 3108 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3108 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3108 times.
✗ Branch 9 not taken.
 3108 vertices.reset(new std::vector<Vec3s>(num_vertices_allocated));
210
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 3108 times.
 3108 if (!(vertices.get())) {
211 std::cerr
212 ✗ << "BVH Error! Out of memory for vertices array on BeginModel() call!"
213 ✗ << std::endl;
214 ✗ return BVH_ERR_MODEL_OUT_OF_MEMORY;
215 }
216 } else {
217 ✗ vertices.reset();
218 ✗ prev_vertices.reset();
219 };
220
221
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 3105 times.
 3108 if (build_state != BVH_BUILD_STATE_EMPTY) {
222 std::cerr
223 << "BVH Warning! Calling beginModel() on a BVHModel that is not empty. "
224 3 "This model was cleared and previous triangles/vertices were lost."
225 3 << std::endl;
226 3 build_state = BVH_BUILD_STATE_EMPTY;
227 3 return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
228 }
229
230 3105 build_state = BVH_BUILD_STATE_BEGUN;
231
232 3105 return BVH_OK;
233 }
234
235 32 int BVHModelBase::addVertex(const Vec3s& p) {
236
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 32 times.
 32 if (build_state != BVH_BUILD_STATE_BEGUN) {
237 std::cerr << "BVH Warning! Call addVertex() in a wrong order. addVertex() "
238 "was ignored. Must do a beginModel() to clear the model for "
239 ✗ "addition of new vertices."
240 ✗ << std::endl;
241 ✗ return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
242 }
243
244
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 32 times.
 32 if (num_vertices >= num_vertices_allocated) {
245 std::shared_ptr<std::vector<Vec3s>> temp(
246 ✗ new std::vector<Vec3s>(num_vertices_allocated * 2));
247 ✗ if (!(temp.get())) {
248 std::cerr
249 ✗ << "BVH Error! Out of memory for vertices array on addVertex() call!"
250 ✗ << std::endl;
251 ✗ return BVH_ERR_MODEL_OUT_OF_MEMORY;
252 }
253
254 ✗ for (size_t i = 0; i < num_vertices; ++i) {
255 ✗ (*temp)[i] = (*vertices)[i];
256 }
257 ✗ vertices = temp;
258 ✗ num_vertices_allocated *= 2;
259 }
260
261 32 (*vertices)[num_vertices] = p;
262 32 num_vertices += 1;
263
264 32 return BVH_OK;
265 }
266
267 ✗ int BVHModelBase::addTriangles(const Matrixx3i& triangles) {
268 ✗ if (build_state == BVH_BUILD_STATE_PROCESSED) {
269 std::cerr << "BVH Warning! Call addSubModel() in a wrong order. "
270 "addSubModel() was ignored. Must do a beginModel() to clear "
271 ✗ "the model for addition of new vertices."
272 ✗ << std::endl;
273 ✗ return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
274 }
275
276 ✗ const unsigned int num_tris_to_add = (unsigned int)triangles.rows();
277
278 ✗ if (num_tris + num_tris_to_add > num_tris_allocated) {
279 std::shared_ptr<std::vector<Triangle>> temp(
280 ✗ new std::vector<Triangle>(num_tris_allocated * 2 + num_tris_to_add));
281 ✗ if (!(temp.get())) {
282 std::cerr << "BVH Error! Out of memory for tri_indices array on "
283 ✗ "addSubModel() call!"
284 ✗ << std::endl;
285 ✗ return BVH_ERR_MODEL_OUT_OF_MEMORY;
286 }
287
288 ✗ for (size_t i = 0; i < num_tris; ++i) {
289 ✗ (*temp)[i] = (*tri_indices)[i];
290 }
291 ✗ tri_indices = temp;
292 ✗ num_tris_allocated = num_tris_allocated * 2 + num_tris_to_add;
293 }
294
295 ✗ std::vector<Triangle>& tri_indices_ = *tri_indices;
296 ✗ for (Eigen::DenseIndex i = 0; i < triangles.rows(); ++i) {
297 ✗ const Matrixx3i::ConstRowXpr triangle = triangles.row(i);
298 ✗ tri_indices_[num_tris++].set(
299 ✗ static_cast<Triangle::index_type>(triangle[0]),
300 ✗ static_cast<Triangle::index_type>(triangle[1]),
301 ✗ static_cast<Triangle::index_type>(triangle[2]));
302 }
303
304 ✗ return BVH_OK;
305 }
306
307 4 int BVHModelBase::addVertices(const MatrixX3s& points) {
308
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 4 times.
 4 if (build_state != BVH_BUILD_STATE_BEGUN) {
309 std::cerr << "BVH Warning! Call addVertex() in a wrong order. "
310 "addVertices() was ignored. Must do a beginModel() to clear "
311 ✗ "the model for addition of new vertices."
312 ✗ << std::endl;
313 ✗ return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
314 }
315
316
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 4 times.
 4 if (num_vertices + points.rows() > num_vertices_allocated) {
317 ✗ num_vertices_allocated = num_vertices + (unsigned int)points.rows();
318 std::shared_ptr<std::vector<Vec3s>> temp(
319 ✗ new std::vector<Vec3s>(num_vertices_allocated));
320 ✗ if (!(temp.get())) {
321 std::cerr
322 ✗ << "BVH Error! Out of memory for vertices array on addVertex() call!"
323 ✗ << std::endl;
324 ✗ return BVH_ERR_MODEL_OUT_OF_MEMORY;
325 }
326
327 ✗ for (size_t i = 0; i < num_vertices; ++i) {
328 ✗ (*temp)[i] = (*vertices)[i];
329 }
330 ✗ vertices = temp;
331 }
332
333 4 std::vector<Vec3s>& vertices_ = *vertices;
334
2/2
✓ Branch 1 taken 32 times.
✓ Branch 2 taken 4 times.
 36 for (Eigen::DenseIndex id = 0; id < points.rows(); ++id)
335
2/4
✓ Branch 2 taken 32 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 32 times.
✗ Branch 7 not taken.
 32 vertices_[num_vertices++] = points.row(id).transpose();
336
337 4 return BVH_OK;
338 }
339
340 96 int BVHModelBase::addTriangle(const Vec3s& p1, const Vec3s& p2,
341 const Vec3s& p3) {
342
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 96 times.
 96 if (build_state == BVH_BUILD_STATE_PROCESSED) {
343 std::cerr << "BVH Warning! Call addTriangle() in a wrong order. "
344 "addTriangle() was ignored. Must do a beginModel() to clear "
345 ✗ "the model for addition of new triangles."
346 ✗ << std::endl;
347 ✗ return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
348 }
349
350
2/2
✓ Branch 0 taken 16 times.
✓ Branch 1 taken 80 times.
 96 if (num_vertices + 2 >= num_vertices_allocated) {
351 std::shared_ptr<std::vector<Vec3s>> temp(
352
3/6
✓ Branch 2 taken 16 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 16 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 16 times.
✗ Branch 9 not taken.
 16 new std::vector<Vec3s>(num_vertices_allocated * 2 + 2));
353
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 16 times.
 16 if (!(temp.get())) {
354 std::cerr << "BVH Error! Out of memory for vertices array on "
355 ✗ "addTriangle() call!"
356 ✗ << std::endl;
357 ✗ return BVH_ERR_MODEL_OUT_OF_MEMORY;
358 }
359
360
2/2
✓ Branch 0 taken 192 times.
✓ Branch 1 taken 16 times.
 208 for (size_t i = 0; i < num_vertices; ++i) {
361
1/2
✓ Branch 5 taken 192 times.
✗ Branch 6 not taken.
 192 (*temp)[i] = (*vertices)[i];
362 }
363 16 vertices = temp;
364 16 num_vertices_allocated = num_vertices_allocated * 2 + 2;
365
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 16 }
366
367 96 const unsigned int offset = num_vertices;
368
369 96 (*vertices)[num_vertices] = p1;
370 96 num_vertices++;
371 96 (*vertices)[num_vertices] = p2;
372 96 num_vertices++;
373 96 (*vertices)[num_vertices] = p3;
374 96 num_vertices++;
375
376
2/2
✓ Branch 0 taken 8 times.
✓ Branch 1 taken 88 times.
 96 if (num_tris >= num_tris_allocated) {
377 std::shared_ptr<std::vector<Triangle>> temp(
378
3/6
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 8 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 8 times.
✗ Branch 9 not taken.
 8 new std::vector<Triangle>(num_tris_allocated * 2));
379
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 8 times.
 8 if (!(temp.get())) {
380 std::cerr << "BVH Error! Out of memory for tri_indices array on "
381 ✗ "addTriangle() call!"
382 ✗ << std::endl;
383 ✗ return BVH_ERR_MODEL_OUT_OF_MEMORY;
384 }
385
386
2/2
✓ Branch 0 taken 64 times.
✓ Branch 1 taken 8 times.
 72 for (size_t i = 0; i < num_tris; ++i) {
387 64 (*temp)[i] = (*tri_indices)[i];
388 }
389 8 tri_indices = temp;
390 8 num_tris_allocated *= 2;
391
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 }
392
393 96 (*tri_indices)[num_tris].set((Triangle::index_type)offset,
394 96 (Triangle::index_type)(offset + 1),
395 96 (Triangle::index_type)(offset + 2));
396 96 num_tris++;
397
398 96 return BVH_OK;
399 }
400
401 ✗ int BVHModelBase::addSubModel(const std::vector<Vec3s>& ps) {
402 ✗ if (build_state == BVH_BUILD_STATE_PROCESSED) {
403 std::cerr << "BVH Warning! Calling addSubModel() in a wrong order. "
404 "addSubModel() was ignored. Must do a beginModel() to clear "
405 ✗ "the model for addition of new vertices."
406 ✗ << std::endl;
407 ✗ return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
408 }
409
410 ✗ const unsigned int num_vertices_to_add = (unsigned int)ps.size();
411
412 ✗ if (num_vertices + num_vertices_to_add - 1 >= num_vertices_allocated) {
413 std::shared_ptr<std::vector<Vec3s>> temp(new std::vector<Vec3s>(
414 ✗ num_vertices_allocated * 2 + num_vertices_to_add - 1));
415 ✗ if (!(temp.get())) {
416 std::cerr << "BVH Error! Out of memory for vertices array on "
417 ✗ "addSubModel() call!"
418 ✗ << std::endl;
419 ✗ return BVH_ERR_MODEL_OUT_OF_MEMORY;
420 }
421
422 ✗ for (size_t i = 0; i < num_vertices; ++i) {
423 ✗ (*temp)[i] = (*vertices)[i];
424 }
425 ✗ vertices = temp;
426 ✗ num_vertices_allocated =
427 ✗ num_vertices_allocated * 2 + num_vertices_to_add - 1;
428 }
429
430 ✗ std::vector<Vec3s>& vertices_ = *vertices;
431 ✗ for (size_t i = 0; i < (size_t)num_vertices_to_add; ++i) {
432 ✗ vertices_[num_vertices] = ps[i];
433 ✗ num_vertices++;
434 }
435
436 ✗ return BVH_OK;
437 }
438
439 3060 int BVHModelBase::addSubModel(const std::vector<Vec3s>& ps,
440 const std::vector<Triangle>& ts) {
441
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 3060 times.
 3060 if (build_state == BVH_BUILD_STATE_PROCESSED) {
442 std::cerr << "BVH Warning! Calling addSubModel() in a wrong order. "
443 "addSubModel() was ignored. Must do a beginModel() to clear "
444 ✗ "the model for addition of new vertices."
445 ✗ << std::endl;
446 ✗ return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
447 }
448
449 3060 const unsigned int num_vertices_to_add = (unsigned int)ps.size();
450
451
2/2
✓ Branch 0 taken 1184 times.
✓ Branch 1 taken 1876 times.
 3060 if (num_vertices + num_vertices_to_add - 1 >= num_vertices_allocated) {
452 std::shared_ptr<std::vector<Vec3s>> temp(new std::vector<Vec3s>(
453
3/6
✓ Branch 2 taken 1184 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1184 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1184 times.
✗ Branch 9 not taken.
 1184 num_vertices_allocated * 2 + num_vertices_to_add - 1));
454
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1184 times.
 1184 if (!(temp.get())) {
455 std::cerr << "BVH Error! Out of memory for vertices array on "
456 ✗ "addSubModel() call!"
457 ✗ << std::endl;
458 ✗ return BVH_ERR_MODEL_OUT_OF_MEMORY;
459 }
460
461
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1184 times.
 1184 for (size_t i = 0; i < num_vertices; ++i) {
462 ✗ (*temp)[i] = (*vertices)[i];
463 }
464 1184 vertices = temp;
465 1184 num_vertices_allocated =
466 1184 num_vertices_allocated * 2 + num_vertices_to_add - 1;
467
1/2
✓ Branch 1 taken 1184 times.
✗ Branch 2 not taken.
 1184 }
468
469 3060 const unsigned int offset = num_vertices;
470
471 3060 std::vector<Vec3s>& vertices_ = *vertices;
472
2/2
✓ Branch 0 taken 1435778 times.
✓ Branch 1 taken 3060 times.
 1438838 for (size_t i = 0; i < (size_t)num_vertices_to_add; ++i) {
473 1435778 vertices_[num_vertices] = ps[i];
474 1435778 num_vertices++;
475 }
476
477 3060 const unsigned int num_tris_to_add = (unsigned int)ts.size();
478
479
2/2
✓ Branch 0 taken 3058 times.
✓ Branch 1 taken 2 times.
 3060 if (num_tris + num_tris_to_add - 1 >= num_tris_allocated) {
480 std::shared_ptr<std::vector<Triangle>> temp(new std::vector<Triangle>(
481
3/6
✓ Branch 2 taken 3058 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3058 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3058 times.
✗ Branch 9 not taken.
 3058 num_tris_allocated * 2 + num_tris_to_add - 1));
482
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 3058 times.
 3058 if (!(temp.get())) {
483 std::cerr << "BVH Error! Out of memory for tri_indices array on "
484 ✗ "addSubModel() call!"
485 ✗ << std::endl;
486 ✗ return BVH_ERR_MODEL_OUT_OF_MEMORY;
487 }
488
489
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 3058 times.
 3058 for (size_t i = 0; i < num_tris; ++i) {
490 ✗ (*temp)[i] = (*tri_indices)[i];
491 }
492 3058 tri_indices = temp;
493 3058 num_tris_allocated = num_tris_allocated * 2 + num_tris_to_add - 1;
494
1/2
✓ Branch 1 taken 3058 times.
✗ Branch 2 not taken.
 3058 }
495
496 3060 std::vector<Triangle>& tri_indices_ = *tri_indices;
497
2/2
✓ Branch 0 taken 978283 times.
✓ Branch 1 taken 3060 times.
 981343 for (size_t i = 0; i < (size_t)num_tris_to_add; ++i) {
498 978283 const Triangle& t = ts[i];
499 1956566 tri_indices_[num_tris].set(t[0] + (size_t)offset, t[1] + (size_t)offset,
500 978283 t[2] + (size_t)offset);
501 978283 num_tris++;
502 }
503
504 3060 return BVH_OK;
505 }
506
507 3108 int BVHModelBase::endModel() {
508
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 3105 times.
 3108 if (build_state != BVH_BUILD_STATE_BEGUN) {
509 std::cerr << "BVH Warning! Call endModel() in wrong order. endModel() was "
510 3 "ignored."
511 3 << std::endl;
512 3 return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
513 }
514
515
3/4
✓ Branch 0 taken 8 times.
✓ Branch 1 taken 3097 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 8 times.
 3105 if (num_tris == 0 && num_vertices == 0) {
516 std::cerr << "BVH Error! endModel() called on model with no triangles and "
517 ✗ "vertices."
518 ✗ << std::endl;
519 ✗ return BVH_ERR_BUILD_EMPTY_MODEL;
520 }
521
522
2/2
✓ Branch 0 taken 3073 times.
✓ Branch 1 taken 32 times.
 3105 if (num_tris_allocated > num_tris) {
523
2/2
✓ Branch 0 taken 3065 times.
✓ Branch 1 taken 8 times.
 3073 if (num_tris > 0) {
524 std::shared_ptr<std::vector<Triangle>> new_tris(
525
3/6
✓ Branch 2 taken 3065 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3065 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3065 times.
✗ Branch 9 not taken.
 3065 new std::vector<Triangle>(num_tris));
526
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 3065 times.
 3065 if (!(new_tris.get())) {
527 std::cerr << "BVH Error! Out of memory for tri_indices array in "
528 ✗ "endModel() call!"
529 ✗ << std::endl;
530 ✗ return BVH_ERR_MODEL_OUT_OF_MEMORY;
531 }
532
533
2/2
✓ Branch 0 taken 976629 times.
✓ Branch 1 taken 3065 times.
 979694 for (size_t i = 0; i < num_tris; ++i) {
534 976629 (*new_tris)[i] = (*tri_indices)[i];
535 }
536 3065 tri_indices = new_tris;
537 3065 num_tris_allocated = num_tris;
538
1/2
✓ Branch 1 taken 3065 times.
✗ Branch 2 not taken.
 3065 } else {
539 8 tri_indices.reset();
540 8 num_tris = num_tris_allocated = 0;
541 }
542 }
543
544
2/2
✓ Branch 0 taken 1191 times.
✓ Branch 1 taken 1914 times.
 3105 if (num_vertices_allocated > num_vertices) {
545
1/2
✓ Branch 0 taken 1191 times.
✗ Branch 1 not taken.
 1191 if (num_vertices > 0) {
546 std::shared_ptr<std::vector<Vec3s>> new_vertices(
547
3/6
✓ Branch 2 taken 1191 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1191 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1191 times.
✗ Branch 9 not taken.
 1191 new std::vector<Vec3s>(num_vertices));
548
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1191 times.
 1191 if (!(new_vertices.get())) {
549 std::cerr
550 ✗ << "BVH Error! Out of memory for vertices array in endModel() call!"
551 ✗ << std::endl;
552 ✗ return BVH_ERR_MODEL_OUT_OF_MEMORY;
553 }
554
555
2/2
✓ Branch 0 taken 1420193 times.
✓ Branch 1 taken 1191 times.
 1421384 for (size_t i = 0; i < num_vertices; ++i) {
556
1/2
✓ Branch 5 taken 1420193 times.
✗ Branch 6 not taken.
 1420193 (*new_vertices)[i] = (*vertices)[i];
557 }
558 1191 vertices = new_vertices;
559 1191 num_vertices_allocated = num_vertices;
560
1/2
✓ Branch 1 taken 1191 times.
✗ Branch 2 not taken.
 1191 } else {
561 ✗ vertices.reset();
562 ✗ num_vertices = num_vertices_allocated = 0;
563 }
564 }
565
566 // construct BVH tree
567
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 3105 times.
 3105 if (!allocateBVs()) return BVH_ERR_MODEL_OUT_OF_MEMORY;
568
569 3105 buildTree();
570
571 // finish constructing
572 3105 build_state = BVH_BUILD_STATE_PROCESSED;
573
574 3105 return BVH_OK;
575 }
576
577 159 int BVHModelBase::beginReplaceModel() {
578
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 159 times.
 159 if (build_state != BVH_BUILD_STATE_PROCESSED) {
579 std::cerr << "BVH Error! Call beginReplaceModel() on a BVHModel that has "
580 ✗ "no previous frame."
581 ✗ << std::endl;
582 ✗ return BVH_ERR_BUILD_EMPTY_PREVIOUS_FRAME;
583 }
584
585
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 159 times.
 159 if (prev_vertices.get()) prev_vertices.reset();
586
587 159 num_vertex_updated = 0;
588
589 159 build_state = BVH_BUILD_STATE_REPLACE_BEGUN;
590
591 159 return BVH_OK;
592 }
593
594 ✗ int BVHModelBase::replaceVertex(const Vec3s& p) {
595 ✗ if (build_state != BVH_BUILD_STATE_REPLACE_BEGUN) {
596 std::cerr << "BVH Warning! Call replaceVertex() in a wrong order. "
597 "replaceVertex() was ignored. Must do a beginReplaceModel() "
598 ✗ "for initialization."
599 ✗ << std::endl;
600 ✗ return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
601 }
602
603 ✗ (*vertices)[num_vertex_updated] = p;
604 ✗ num_vertex_updated++;
605
606 ✗ return BVH_OK;
607 }
608
609 ✗ int BVHModelBase::replaceTriangle(const Vec3s& p1, const Vec3s& p2,
610 const Vec3s& p3) {
611 ✗ if (build_state != BVH_BUILD_STATE_REPLACE_BEGUN) {
612 std::cerr << "BVH Warning! Call replaceTriangle() in a wrong order. "
613 "replaceTriangle() was ignored. Must do a beginReplaceModel() "
614 ✗ "for initialization."
615 ✗ << std::endl;
616 ✗ return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
617 }
618
619 ✗ (*vertices)[num_vertex_updated] = p1;
620 ✗ num_vertex_updated++;
621 ✗ (*vertices)[num_vertex_updated] = p2;
622 ✗ num_vertex_updated++;
623 ✗ (*vertices)[num_vertex_updated] = p3;
624 ✗ num_vertex_updated++;
625 ✗ return BVH_OK;
626 }
627
628 159 int BVHModelBase::replaceSubModel(const std::vector<Vec3s>& ps) {
629
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 159 times.
 159 if (build_state != BVH_BUILD_STATE_REPLACE_BEGUN) {
630 std::cerr << "BVH Warning! Call replaceSubModel() in a wrong order. "
631 "replaceSubModel() was ignored. Must do a beginReplaceModel() "
632 ✗ "for initialization."
633 ✗ << std::endl;
634 ✗ return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
635 }
636
637 159 std::vector<Vec3s>& vertices_ = *vertices;
638
2/2
✓ Branch 1 taken 745704 times.
✓ Branch 2 taken 159 times.
 745863 for (unsigned int i = 0; i < ps.size(); ++i) {
639 745704 vertices_[num_vertex_updated] = ps[i];
640 745704 num_vertex_updated++;
641 }
642 159 return BVH_OK;
643 }
644
645 159 int BVHModelBase::endReplaceModel(bool refit, bool bottomup) {
646
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 159 times.
 159 if (build_state != BVH_BUILD_STATE_REPLACE_BEGUN) {
647 std::cerr << "BVH Warning! Call endReplaceModel() in a wrong order. "
648 ✗ "endReplaceModel() was ignored. "
649 ✗ << std::endl;
650 ✗ return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
651 }
652
653
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 159 times.
 159 if (num_vertex_updated != num_vertices) {
654 std::cerr << "BVH Error! The replaced model should have the same number of "
655 ✗ "vertices as the old model."
656 ✗ << std::endl;
657 ✗ return BVH_ERR_INCORRECT_DATA;
658 }
659
660
2/2
✓ Branch 0 taken 89 times.
✓ Branch 1 taken 70 times.
 159 if (refit) // refit, do not change BVH structure
661 {
662 89 refitTree(bottomup);
663 } else // reconstruct bvh tree based on current frame data
664 {
665 70 buildTree();
666 }
667
668 159 build_state = BVH_BUILD_STATE_PROCESSED;
669
670 159 return BVH_OK;
671 }
672
673 ✗ int BVHModelBase::beginUpdateModel() {
674 ✗ if (build_state != BVH_BUILD_STATE_PROCESSED &&
675 ✗ build_state != BVH_BUILD_STATE_UPDATED) {
676 std::cerr << "BVH Error! Call beginUpdatemodel() on a BVHModel that has no "
677 ✗ "previous frame."
678 ✗ << std::endl;
679 ✗ return BVH_ERR_BUILD_EMPTY_PREVIOUS_FRAME;
680 }
681
682 ✗ if (prev_vertices.get()) {
683 ✗ std::shared_ptr<std::vector<Vec3s>> temp = prev_vertices;
684 ✗ prev_vertices = vertices;
685 ✗ vertices = temp;
686 ✗ } else {
687 ✗ prev_vertices = vertices;
688 ✗ vertices.reset(new std::vector<Vec3s>(num_vertices));
689 }
690
691 ✗ num_vertex_updated = 0;
692
693 ✗ build_state = BVH_BUILD_STATE_UPDATE_BEGUN;
694
695 ✗ return BVH_OK;
696 }
697
698 ✗ int BVHModelBase::updateVertex(const Vec3s& p) {
699 ✗ if (build_state != BVH_BUILD_STATE_UPDATE_BEGUN) {
700 std::cerr
701 << "BVH Warning! Call updateVertex() in a wrong order. updateVertex() "
702 ✗ "was ignored. Must do a beginUpdateModel() for initialization."
703 ✗ << std::endl;
704 ✗ return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
705 }
706
707 ✗ (*vertices)[num_vertex_updated] = p;
708 ✗ num_vertex_updated++;
709
710 ✗ return BVH_OK;
711 }
712
713 ✗ int BVHModelBase::updateTriangle(const Vec3s& p1, const Vec3s& p2,
714 const Vec3s& p3) {
715 ✗ if (build_state != BVH_BUILD_STATE_UPDATE_BEGUN) {
716 std::cerr << "BVH Warning! Call updateTriangle() in a wrong order. "
717 "updateTriangle() was ignored. Must do a beginUpdateModel() "
718 ✗ "for initialization."
719 ✗ << std::endl;
720 ✗ return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
721 }
722
723 ✗ (*vertices)[num_vertex_updated] = p1;
724 ✗ num_vertex_updated++;
725 ✗ (*vertices)[num_vertex_updated] = p2;
726 ✗ num_vertex_updated++;
727 ✗ (*vertices)[num_vertex_updated] = p3;
728 ✗ num_vertex_updated++;
729 ✗ return BVH_OK;
730 }
731
732 ✗ int BVHModelBase::updateSubModel(const std::vector<Vec3s>& ps) {
733 ✗ if (build_state != BVH_BUILD_STATE_UPDATE_BEGUN) {
734 std::cerr << "BVH Warning! Call updateSubModel() in a wrong order. "
735 "updateSubModel() was ignored. Must do a beginUpdateModel() "
736 ✗ "for initialization."
737 ✗ << std::endl;
738 ✗ return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
739 }
740
741 ✗ std::vector<Vec3s>& vertices_ = *vertices;
742 ✗ for (unsigned int i = 0; i < ps.size(); ++i) {
743 ✗ vertices_[num_vertex_updated] = ps[i];
744 ✗ num_vertex_updated++;
745 }
746 ✗ return BVH_OK;
747 }
748
749 ✗ int BVHModelBase::endUpdateModel(bool refit, bool bottomup) {
750 ✗ if (build_state != BVH_BUILD_STATE_UPDATE_BEGUN) {
751 std::cerr << "BVH Warning! Call endUpdateModel() in a wrong order. "
752 ✗ "endUpdateModel() was ignored. "
753 ✗ << std::endl;
754 ✗ return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
755 }
756
757 ✗ if (num_vertex_updated != num_vertices) {
758 std::cerr << "BVH Error! The updated model should have the same number of "
759 ✗ "vertices as the old model."
760 ✗ << std::endl;
761 ✗ return BVH_ERR_INCORRECT_DATA;
762 }
763
764 ✗ if (refit) // refit, do not change BVH structure
765 {
766 ✗ refitTree(bottomup);
767 } else // reconstruct bvh tree based on current frame data
768 {
769 ✗ buildTree();
770
771 // then refit
772
773 ✗ refitTree(bottomup);
774 }
775
776 ✗ build_state = BVH_BUILD_STATE_UPDATED;
777
778 ✗ return BVH_OK;
779 }
780
781 8628 void BVHModelBase::computeLocalAABB() {
782
1/2
✓ Branch 1 taken 8628 times.
✗ Branch 2 not taken.
 8628 AABB aabb_;
783 8628 const std::vector<Vec3s>& vertices_ = *vertices;
784
2/2
✓ Branch 0 taken 4112649 times.
✓ Branch 1 taken 8628 times.
 4121277 for (unsigned int i = 0; i < num_vertices; ++i) {
785
1/2
✓ Branch 2 taken 4112649 times.
✗ Branch 3 not taken.
 4112649 aabb_ += vertices_[i];
786 }
787
788
1/2
✓ Branch 1 taken 8628 times.
✗ Branch 2 not taken.
 8628 aabb_center = aabb_.center();
789
790 8628 aabb_radius = 0;
791
2/2
✓ Branch 0 taken 4112649 times.
✓ Branch 1 taken 8628 times.
 4121277 for (unsigned int i = 0; i < num_vertices; ++i) {
792
2/4
✓ Branch 2 taken 4112649 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4112649 times.
✗ Branch 6 not taken.
 4112649 Scalar r = (aabb_center - vertices_[i]).squaredNorm();
793
2/2
✓ Branch 0 taken 10757 times.
✓ Branch 1 taken 4101892 times.
 4112649 if (r > aabb_radius) aabb_radius = r;
794 }
795
796 8628 aabb_radius = sqrt(aabb_radius);
797
798
1/2
✓ Branch 1 taken 8628 times.
✗ Branch 2 not taken.
 8628 aabb_local = aabb_;
799 8628 }
800
801 /// @brief Constructing an empty BVH
802 template <typename BV>
803 6228 BVHModel<BV>::BVHModel()
804 : BVHModelBase(),
805
2/4
✓ Branch 1 taken 3114 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3114 times.
✗ Branch 5 not taken.
 6228 bv_splitter(new BVSplitter<BV>(SPLIT_METHOD_MEAN)),
806
2/4
✓ Branch 1 taken 3114 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3114 times.
✗ Branch 6 not taken.
 6228 bv_fitter(new BVFitter<BV>()),
807 6228 num_bvs_allocated(0),
808
1/2
✓ Branch 2 taken 3114 times.
✗ Branch 3 not taken.
 12456 num_bvs(0) {}
809
810 template <typename BV>
811 6 void BVHModel<BV>::deleteBVs() {
812 6 bvs.reset();
813 6 primitive_indices.reset();
814 6 num_bvs_allocated = num_bvs = 0;
815 }
816
817 template <typename BV>
818 6210 bool BVHModel<BV>::allocateBVs() {
819 // construct BVH tree
820 6210 unsigned int num_bvs_to_be_allocated = 0;
821
2/2
✓ Branch 0 taken 8 times.
✓ Branch 1 taken 3097 times.
 6210 if (num_tris == 0)
822 16 num_bvs_to_be_allocated = 2 * num_vertices - 1;
823 else
824 6194 num_bvs_to_be_allocated = 2 * num_tris - 1;
825
826
3/6
✓ Branch 2 taken 3105 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3105 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3105 times.
✗ Branch 9 not taken.
 6210 bvs.reset(new bv_node_vector_t(num_bvs_to_be_allocated));
827
1/2
✓ Branch 2 taken 3105 times.
✗ Branch 3 not taken.
 12420 primitive_indices.reset(
828
2/4
✓ Branch 1 taken 3105 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3105 times.
✗ Branch 5 not taken.
 6210 new std::vector<unsigned int>(num_bvs_to_be_allocated));
829
3/6
✓ Branch 1 taken 3105 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 3105 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 3105 times.
 6210 if (!(bvs.get()) || !(primitive_indices.get())) {
830 ✗ std::cerr << "BVH Error! Out of memory for BV array in endModel()!"
831 ✗ << std::endl;
832 ✗ return false;
833 }
834 6210 num_bvs_allocated = num_bvs_to_be_allocated;
835 6210 num_bvs = 0;
836 6210 return true;
837 }
838
839 template <typename BV>
840 8 int BVHModel<BV>::memUsage(const bool msg) const {
841 8 unsigned int mem_bv_list = (unsigned int)sizeof(BV) * num_bvs;
842 8 unsigned int mem_tri_list = (unsigned int)sizeof(Triangle) * num_tris;
843 8 unsigned int mem_vertex_list = (unsigned int)sizeof(Vec3s) * num_vertices;
844
845 8 unsigned int total_mem = mem_bv_list + mem_tri_list + mem_vertex_list +
846 (unsigned int)sizeof(BVHModel<BV>);
847
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 4 times.
 8 if (msg) {
848 ✗ std::cerr << "Total for model " << total_mem << " bytes." << std::endl;
849 ✗ std::cerr << "BVs: " << num_bvs << " allocated." << std::endl;
850 ✗ std::cerr << "Tris: " << num_tris << " allocated." << std::endl;
851 ✗ std::cerr << "Vertices: " << num_vertices << " allocated." << std::endl;
852 }
853
854 8 return static_cast<int>(total_mem);
855 }
856
857 template <typename BV>
858 6350 int BVHModel<BV>::buildTree() {
859 // set BVFitter
860
1/2
✓ Branch 1 taken 3175 times.
✗ Branch 2 not taken.
 6350 Vec3s* vertices_ = vertices.get() ? vertices->data() : NULL;
861
2/2
✓ Branch 1 taken 3167 times.
✓ Branch 2 taken 8 times.
 6350 Triangle* tri_indices_ = tri_indices.get() ? tri_indices->data() : NULL;
862 6350 bv_fitter->set(vertices_, tri_indices_, getModelType());
863 // set SplitRule
864 6350 bv_splitter->set(vertices_, tri_indices_, getModelType());
865
866 6350 num_bvs = 1;
867
868 6350 unsigned int num_primitives = 0;
869
2/3
✓ Branch 1 taken 3167 times.
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
 6350 switch (getModelType()) {
870 6334 case BVH_MODEL_TRIANGLES:
871 6334 num_primitives = (unsigned int)num_tris;
872 6334 break;
873 16 case BVH_MODEL_POINTCLOUD:
874 16 num_primitives = (unsigned int)num_vertices;
875 16 break;
876 ✗ default:
877 ✗ std::cerr << "BVH Error: Model type not supported!" << std::endl;
878 ✗ return BVH_ERR_UNSUPPORTED_FUNCTION;
879 }
880
881 6350 std::vector<unsigned int>& primitive_indices_ = *primitive_indices;
882
2/2
✓ Branch 1 taken 1129549 times.
✓ Branch 2 taken 3175 times.
 2265448 for (unsigned int i = 0; i < num_primitives; ++i) primitive_indices_[i] = i;
883 6350 recursiveBuildTree(0, 0, num_primitives);
884
885 6350 bv_fitter->clear();
886 6350 bv_splitter->clear();
887
888 6350 return BVH_OK;
889 }
890
891 template <typename BV>
892 4511846 int BVHModel<BV>::recursiveBuildTree(int bv_id, unsigned int first_primitive,
893 unsigned int num_primitives) {
894 4511846 BVHModelType type = getModelType();
895 4511846 BVNode<BV>* bvnode = bvs->data() + bv_id;
896 4511846 unsigned int* cur_primitive_indices =
897 4511846 primitive_indices->data() + first_primitive;
898
899 // constructing BV
900
1/2
✓ Branch 2 taken 2255923 times.
✗ Branch 3 not taken.
 4511846 BV bv = bv_fitter->fit(cur_primitive_indices, num_primitives);
901
1/2
✓ Branch 2 taken 2255923 times.
✗ Branch 3 not taken.
 4511846 bv_splitter->computeRule(bv, cur_primitive_indices, num_primitives);
902
903
1/2
✓ Branch 1 taken 2255923 times.
✗ Branch 2 not taken.
 4511846 bvnode->bv = bv;
904 4511846 bvnode->first_primitive = first_primitive;
905 4511846 bvnode->num_primitives = num_primitives;
906
907
2/2
✓ Branch 0 taken 1129549 times.
✓ Branch 1 taken 1126374 times.
 4511846 if (num_primitives == 1) {
908 2259098 bvnode->first_child = -((int)(*cur_primitive_indices) + 1);
909 } else {
910 2252748 bvnode->first_child = (int)num_bvs;
911 2252748 num_bvs += 2;
912
913 2252748 unsigned int c1 = 0;
914 2252748 const std::vector<Vec3s>& vertices_ = *vertices;
915 2252748 const std::vector<Triangle>& tri_indices_ = *tri_indices;
916
2/2
✓ Branch 0 taken 12055007 times.
✓ Branch 1 taken 1126374 times.
 26362762 for (unsigned int i = 0; i < num_primitives; ++i) {
917
1/2
✓ Branch 1 taken 12055007 times.
✗ Branch 2 not taken.
 24110014 Vec3s p;
918
2/2
✓ Branch 0 taken 192 times.
✓ Branch 1 taken 12054815 times.
 24110014 if (type == BVH_MODEL_POINTCLOUD)
919
1/2
✓ Branch 2 taken 192 times.
✗ Branch 3 not taken.
 384 p = vertices_[cur_primitive_indices[i]];
920
1/2
✓ Branch 0 taken 12054815 times.
✗ Branch 1 not taken.
 24109630 else if (type == BVH_MODEL_TRIANGLES) {
921 24109630 const Triangle& t = tri_indices_[cur_primitive_indices[i]];
922 24109630 const Vec3s& p1 = vertices_[t[0]];
923 24109630 const Vec3s& p2 = vertices_[t[1]];
924 24109630 const Vec3s& p3 = vertices_[t[2]];
925
926
4/8
✓ Branch 1 taken 12054815 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12054815 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12054815 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 12054815 times.
✗ Branch 11 not taken.
 24109630 p = (p1 + p2 + p3) / 3.;
927 } else {
928 ✗ std::cerr << "BVH Error: Model type not supported!" << std::endl;
929 ✗ return BVH_ERR_UNSUPPORTED_FUNCTION;
930 }
931
932 // loop invariant: up to (but not including) index c1 in group 1,
933 // then up to (but not including) index i in group 2
934 //
935 // [1] [1] [1] [1] [2] [2] [2] [x] [x] ... [x]
936 // c1 i
937 //
938
3/4
✓ Branch 2 taken 12055007 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 5934216 times.
✓ Branch 5 taken 6120791 times.
 24110014 if (bv_splitter->apply(p)) // in the right side
939 {
940 // do nothing
941 } else {
942 11868432 unsigned int temp = cur_primitive_indices[i];
943 11868432 cur_primitive_indices[i] = cur_primitive_indices[c1];
944 11868432 cur_primitive_indices[c1] = temp;
945 11868432 c1++;
946 }
947 }
948
949
4/4
✓ Branch 0 taken 993687 times.
✓ Branch 1 taken 132687 times.
✓ Branch 2 taken 167940 times.
✓ Branch 3 taken 825747 times.
 2252748 if ((c1 == 0) || (c1 == num_primitives)) c1 = num_primitives / 2;
950
951 2252748 const unsigned int num_first_half = c1;
952
953
1/2
✓ Branch 2 taken 1126374 times.
✗ Branch 3 not taken.
 2252748 recursiveBuildTree(bvnode->leftChild(), first_primitive, num_first_half);
954
1/2
✓ Branch 2 taken 1126374 times.
✗ Branch 3 not taken.
 2252748 recursiveBuildTree(bvnode->rightChild(), first_primitive + num_first_half,
955 num_primitives - num_first_half);
956 }
957
958 4511846 return BVH_OK;
959 }
960
961 template <typename BV>
962 178 int BVHModel<BV>::refitTree(bool bottomup) {
963
2/2
✓ Branch 0 taken 21 times.
✓ Branch 1 taken 68 times.
 178 if (bottomup)
964 42 return refitTree_bottomup();
965 else
966 136 return refitTree_topdown();
967 }
968
969 template <typename BV>
970 42 int BVHModel<BV>::refitTree_bottomup() {
971 // TODO the recomputation of the BV is done manually, without using
972 // bv_fitter. The manual BV recomputation seems bugged. Using bv_fitter
973 // seems to correct the bug.
974 // bv_fitter->set(vertices, tri_indices, getModelType());
975
976 42 int res = recursiveRefitTree_bottomup(0);
977
978 // bv_fitter->clear();
979 42 return res;
980 }
981
982 template <typename BV>
983 183078 int BVHModel<BV>::recursiveRefitTree_bottomup(int bv_id) {
984 183078 BVNode<BV>* bvnode = bvs->data() + bv_id;
985
2/2
✓ Branch 1 taken 45780 times.
✓ Branch 2 taken 45759 times.
 183078 if (bvnode->isLeaf()) {
986 91560 BVHModelType type = getModelType();
987 91560 int primitive_id = -(bvnode->first_child + 1);
988
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 45780 times.
 91560 if (type == BVH_MODEL_POINTCLOUD) {
989 ✗ BV bv;
990
991 ✗ if (prev_vertices.get()) {
992 ✗ Vec3s v[2];
993 ✗ v[0] = (*prev_vertices)[static_cast<size_t>(primitive_id)];
994 ✗ v[1] = (*vertices)[static_cast<size_t>(primitive_id)];
995 ✗ fit(v, 2, bv);
996 } else
997 ✗ fit(vertices->data() + primitive_id, 1, bv);
998
999 ✗ bvnode->bv = bv;
1000
1/2
✓ Branch 0 taken 45780 times.
✗ Branch 1 not taken.
 91560 } else if (type == BVH_MODEL_TRIANGLES) {
1001
1/2
✓ Branch 1 taken 45780 times.
✗ Branch 2 not taken.
 91560 BV bv;
1002 91560 const Triangle& triangle =
1003 91560 (*tri_indices)[static_cast<size_t>(primitive_id)];
1004
1005
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 45780 times.
 91560 if (prev_vertices.get()) {
1006 ✗ Vec3s v[6];
1007 ✗ for (Triangle::index_type i = 0; i < 3; ++i) {
1008 ✗ v[i] = (*prev_vertices)[triangle[i]];
1009 ✗ v[i + 3] = (*vertices)[triangle[i]];
1010 }
1011
1012 ✗ fit(v, 6, bv);
1013 } else {
1014 // TODO use bv_fitter to build BV. See comment in refitTree_bottomup
1015 // unsigned int* cur_primitive_indices = primitive_indices +
1016 // bvnode->first_primitive; bv = bv_fitter->fit(cur_primitive_indices,
1017 // bvnode->num_primitives);
1018
3/4
✓ Branch 1 taken 137340 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 137340 times.
✓ Branch 4 taken 45780 times.
 366240 Vec3s v[3];
1019
2/2
✓ Branch 0 taken 137340 times.
✓ Branch 1 taken 45780 times.
 366240 for (int i = 0; i < 3; ++i) {
1020
1/2
✓ Branch 4 taken 137340 times.
✗ Branch 5 not taken.
 274680 v[i] = (*vertices)[triangle[(Triangle::index_type)i]];
1021 }
1022
1023
1/2
✓ Branch 1 taken 45780 times.
✗ Branch 2 not taken.
 91560 fit(v, 3, bv);
1024 }
1025
1026
1/2
✓ Branch 1 taken 45780 times.
✗ Branch 2 not taken.
 91560 bvnode->bv = bv;
1027 } else {
1028 ✗ std::cerr << "BVH Error: Model type not supported!" << std::endl;
1029 ✗ return BVH_ERR_UNSUPPORTED_FUNCTION;
1030 }
1031 } else {
1032 91518 recursiveRefitTree_bottomup(bvnode->leftChild());
1033 91518 recursiveRefitTree_bottomup(bvnode->rightChild());
1034
0/2
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 91518 bvnode->bv = (*bvs)[static_cast<size_t>(bvnode->leftChild())].bv +
1035 91518 (*bvs)[static_cast<size_t>(bvnode->rightChild())].bv;
1036 // TODO use bv_fitter to build BV. See comment in refitTree_bottomup
1037 // unsigned int* cur_primitive_indices = primitive_indices +
1038 // bvnode->first_primitive; bvnode->bv =
1039 // bv_fitter->fit(cur_primitive_indices, bvnode->num_primitives);
1040 }
1041
1042 183078 return BVH_OK;
1043 }
1044
1045 template <typename BV>
1046 136 int BVHModel<BV>::refitTree_topdown() {
1047
1/2
✓ Branch 1 taken 68 times.
✗ Branch 2 not taken.
 136 Vec3s* vertices_ = vertices.get() ? vertices->data() : NULL;
1048
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 68 times.
 136 Vec3s* prev_vertices_ = prev_vertices.get() ? prev_vertices->data() : NULL;
1049
1/2
✓ Branch 1 taken 68 times.
✗ Branch 2 not taken.
 136 Triangle* tri_indices_ = tri_indices.get() ? tri_indices->data() : NULL;
1050 136 bv_fitter->set(vertices_, prev_vertices_, tri_indices_, getModelType());
1051 136 BVNode<BV>* bvs_ = bvs->data();
1052 136 unsigned int* primitive_indices_ = primitive_indices->data();
1053
2/2
✓ Branch 0 taken 162860 times.
✓ Branch 1 taken 68 times.
 325856 for (unsigned int i = 0; i < num_bvs; ++i) {
1054 325720 BV bv = bv_fitter->fit(primitive_indices_ + bvs_[i].first_primitive,
1055
1/2
✓ Branch 1 taken 162860 times.
✗ Branch 2 not taken.
 325720 bvs_[i].num_primitives);
1056
1/2
✓ Branch 1 taken 162860 times.
✗ Branch 2 not taken.
 325720 bvs_[i].bv = bv;
1057 }
1058
1059 136 bv_fitter->clear();
1060
1061 136 return BVH_OK;
1062 }
1063
1064 template <>
1065 ✗ void BVHModel<OBB>::makeParentRelativeRecurse(int bv_id, Matrix3s& parent_axes,
1066 const Vec3s& parent_c) {
1067 ✗ bv_node_vector_t& bvs_ = *bvs;
1068 ✗ OBB& obb = bvs_[static_cast<size_t>(bv_id)].bv;
1069 ✗ if (!bvs_[static_cast<size_t>(bv_id)].isLeaf()) {
1070 ✗ makeParentRelativeRecurse(bvs_[static_cast<size_t>(bv_id)].first_child,
1071 ✗ obb.axes, obb.To);
1072
1073 ✗ makeParentRelativeRecurse(bvs_[static_cast<size_t>(bv_id)].first_child + 1,
1074 ✗ obb.axes, obb.To);
1075 }
1076
1077 // make self parent relative
1078 // obb.axes = parent_axes.transpose() * obb.axes;
1079 ✗ obb.axes.applyOnTheLeft(parent_axes.transpose());
1080
1081 ✗ Vec3s t(obb.To - parent_c);
1082 ✗ obb.To.noalias() = parent_axes.transpose() * t;
1083 }
1084
1085 template <>
1086 ✗ void BVHModel<RSS>::makeParentRelativeRecurse(int bv_id, Matrix3s& parent_axes,
1087 const Vec3s& parent_c) {
1088 ✗ bv_node_vector_t& bvs_ = *bvs;
1089 ✗ RSS& rss = bvs_[static_cast<size_t>(bv_id)].bv;
1090 ✗ if (!bvs_[static_cast<size_t>(bv_id)].isLeaf()) {
1091 ✗ makeParentRelativeRecurse(bvs_[static_cast<size_t>(bv_id)].first_child,
1092 ✗ rss.axes, rss.Tr);
1093
1094 ✗ makeParentRelativeRecurse(bvs_[static_cast<size_t>(bv_id)].first_child + 1,
1095 ✗ rss.axes, rss.Tr);
1096 }
1097
1098 // make self parent relative
1099 // rss.axes = parent_axes.transpose() * rss.axes;
1100 ✗ rss.axes.applyOnTheLeft(parent_axes.transpose());
1101
1102 ✗ Vec3s t(rss.Tr - parent_c);
1103 ✗ rss.Tr.noalias() = parent_axes.transpose() * t;
1104 }
1105
1106 template <>
1107 ✗ void BVHModel<OBBRSS>::makeParentRelativeRecurse(int bv_id,
1108 Matrix3s& parent_axes,
1109 const Vec3s& parent_c) {
1110 ✗ bv_node_vector_t& bvs_ = *bvs;
1111 ✗ OBB& obb = bvs_[static_cast<size_t>(bv_id)].bv.obb;
1112 ✗ RSS& rss = bvs_[static_cast<size_t>(bv_id)].bv.rss;
1113 ✗ if (!bvs_[static_cast<size_t>(bv_id)].isLeaf()) {
1114 ✗ makeParentRelativeRecurse(bvs_[static_cast<size_t>(bv_id)].first_child,
1115 ✗ obb.axes, obb.To);
1116
1117 ✗ makeParentRelativeRecurse(bvs_[static_cast<size_t>(bv_id)].first_child + 1,
1118 ✗ obb.axes, obb.To);
1119 }
1120
1121 // make self parent relative
1122 ✗ rss.axes.noalias() = parent_axes.transpose() * obb.axes;
1123 ✗ obb.axes = rss.axes;
1124
1125 ✗ Vec3s t(obb.To - parent_c);
1126 ✗ obb.To.noalias() = parent_axes.transpose() * t;
1127 ✗ rss.Tr = obb.To;
1128 }
1129
1130 template <>
1131 3 NODE_TYPE BVHModel<AABB>::getNodeType() const {
1132 3 return BV_AABB;
1133 }
1134
1135 template <>
1136 43 NODE_TYPE BVHModel<OBB>::getNodeType() const {
1137 43 return BV_OBB;
1138 }
1139
1140 template <>
1141 7 NODE_TYPE BVHModel<RSS>::getNodeType() const {
1142 7 return BV_RSS;
1143 }
1144
1145 template <>
1146 7 NODE_TYPE BVHModel<kIOS>::getNodeType() const {
1147 7 return BV_kIOS;
1148 }
1149
1150 template <>
1151 39069 NODE_TYPE BVHModel<OBBRSS>::getNodeType() const {
1152 39069 return BV_OBBRSS;
1153 }
1154
1155 template <>
1156 5 NODE_TYPE BVHModel<KDOP<16>>::getNodeType() const {
1157 5 return BV_KDOP16;
1158 }
1159
1160 template <>
1161 7 NODE_TYPE BVHModel<KDOP<18>>::getNodeType() const {
1162 7 return BV_KDOP18;
1163 }
1164
1165 template <>
1166 9 NODE_TYPE BVHModel<KDOP<24>>::getNodeType() const {
1167 9 return BV_KDOP24;
1168 }
1169
1170 template class BVHModel<KDOP<16>>;
1171 template class BVHModel<KDOP<18>>;
1172 template class BVHModel<KDOP<24>>;
1173 template class BVHModel<OBB>;
1174 template class BVHModel<AABB>;
1175 template class BVHModel<RSS>;
1176 template class BVHModel<kIOS>;
1177 template class BVHModel<OBBRSS>;
1178
1179 } // namespace coal
1180