GCC Code Coverage Report

Directory: ./
File: src/BVH/BVH_model.cpp
Date: 2025-05-02 10:16:21
Exec Total Coverage
Lines: 372 660 56.4%
Branches: 258 709 36.4%
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<Triangle32>(*(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<Triangle32>& tri_indices_ = *(tri_indices);
99 20 const std::vector<Triangle32>& 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<Triangle32>> 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<Triangle32>(*(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 ConvexTpl<Triangle32>(points, num_vertices, polygons, num_tris));
151 3 }
152 }
153
154 ✗ bool BVHModelBase::buildConvexHull(bool keepTriangle,
155 const char* qhullCommand) {
156 ✗ convex.reset(BVHModelBase::ConvexType::convexHull(
157 ✗ vertices, num_vertices, keepTriangle, 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<Triangle32>(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<Triangle32>> temp(
280 ✗ new std::vector<Triangle32>(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<Triangle32>& 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<Triangle32::IndexType>(triangle[0]),
300 ✗ static_cast<Triangle32::IndexType>(triangle[1]),
301 ✗ static_cast<Triangle32::IndexType>(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<Triangle32>> 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<Triangle32>(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
1/2
✓ Branch 5 taken 64 times.
✗ Branch 6 not taken.
 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(Triangle32::IndexType(offset),
394 96 Triangle32::IndexType((offset + 1)),
395 96 Triangle32::IndexType((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<Triangle32>& 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<Triangle32>> temp(new std::vector<Triangle32>(
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<Triangle32>& 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 Triangle32& t = ts[i];
499 1956566 tri_indices_[num_tris].set(t[0] + Triangle32::IndexType(offset),
500 978283 t[1] + Triangle32::IndexType(offset),
501 978283 t[2] + Triangle32::IndexType(offset));
502 978283 num_tris++;
503 }
504
505 3060 return BVH_OK;
506 }
507
508 3108 int BVHModelBase::endModel() {
509
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 3105 times.
 3108 if (build_state != BVH_BUILD_STATE_BEGUN) {
510 std::cerr << "BVH Warning! Call endModel() in wrong order. endModel() was "
511 3 "ignored."
512 3 << std::endl;
513 3 return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
514 }
515
516
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) {
517 std::cerr << "BVH Error! endModel() called on model with no triangles and "
518 ✗ "vertices."
519 ✗ << std::endl;
520 ✗ return BVH_ERR_BUILD_EMPTY_MODEL;
521 }
522
523
2/2
✓ Branch 0 taken 3073 times.
✓ Branch 1 taken 32 times.
 3105 if (num_tris_allocated > num_tris) {
524
2/2
✓ Branch 0 taken 3065 times.
✓ Branch 1 taken 8 times.
 3073 if (num_tris > 0) {
525 std::shared_ptr<std::vector<Triangle32>> new_tris(
526
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<Triangle32>(num_tris));
527
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 3065 times.
 3065 if (!(new_tris.get())) {
528 std::cerr << "BVH Error! Out of memory for tri_indices array in "
529 ✗ "endModel() call!"
530 ✗ << std::endl;
531 ✗ return BVH_ERR_MODEL_OUT_OF_MEMORY;
532 }
533
534
2/2
✓ Branch 0 taken 976629 times.
✓ Branch 1 taken 3065 times.
 979694 for (size_t i = 0; i < num_tris; ++i) {
535
1/2
✓ Branch 5 taken 976629 times.
✗ Branch 6 not taken.
 976629 (*new_tris)[i] = (*tri_indices)[i];
536 }
537 3065 tri_indices = new_tris;
538 3065 num_tris_allocated = num_tris;
539
1/2
✓ Branch 1 taken 3065 times.
✗ Branch 2 not taken.
 3065 } else {
540 8 tri_indices.reset();
541 8 num_tris = num_tris_allocated = 0;
542 }
543 }
544
545
2/2
✓ Branch 0 taken 1191 times.
✓ Branch 1 taken 1914 times.
 3105 if (num_vertices_allocated > num_vertices) {
546
1/2
✓ Branch 0 taken 1191 times.
✗ Branch 1 not taken.
 1191 if (num_vertices > 0) {
547 std::shared_ptr<std::vector<Vec3s>> new_vertices(
548
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));
549
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1191 times.
 1191 if (!(new_vertices.get())) {
550 std::cerr
551 ✗ << "BVH Error! Out of memory for vertices array in endModel() call!"
552 ✗ << std::endl;
553 ✗ return BVH_ERR_MODEL_OUT_OF_MEMORY;
554 }
555
556
2/2
✓ Branch 0 taken 1420193 times.
✓ Branch 1 taken 1191 times.
 1421384 for (size_t i = 0; i < num_vertices; ++i) {
557
1/2
✓ Branch 5 taken 1420193 times.
✗ Branch 6 not taken.
 1420193 (*new_vertices)[i] = (*vertices)[i];
558 }
559 1191 vertices = new_vertices;
560 1191 num_vertices_allocated = num_vertices;
561
1/2
✓ Branch 1 taken 1191 times.
✗ Branch 2 not taken.
 1191 } else {
562 ✗ vertices.reset();
563 ✗ num_vertices = num_vertices_allocated = 0;
564 }
565 }
566
567 // construct BVH tree
568
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 3105 times.
 3105 if (!allocateBVs()) return BVH_ERR_MODEL_OUT_OF_MEMORY;
569
570 3105 buildTree();
571
572 // finish constructing
573 3105 build_state = BVH_BUILD_STATE_PROCESSED;
574
575 3105 return BVH_OK;
576 }
577
578 159 int BVHModelBase::beginReplaceModel() {
579
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 159 times.
 159 if (build_state != BVH_BUILD_STATE_PROCESSED) {
580 std::cerr << "BVH Error! Call beginReplaceModel() on a BVHModel that has "
581 ✗ "no previous frame."
582 ✗ << std::endl;
583 ✗ return BVH_ERR_BUILD_EMPTY_PREVIOUS_FRAME;
584 }
585
586
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 159 times.
 159 if (prev_vertices.get()) prev_vertices.reset();
587
588 159 num_vertex_updated = 0;
589
590 159 build_state = BVH_BUILD_STATE_REPLACE_BEGUN;
591
592 159 return BVH_OK;
593 }
594
595 ✗ int BVHModelBase::replaceVertex(const Vec3s& p) {
596 ✗ if (build_state != BVH_BUILD_STATE_REPLACE_BEGUN) {
597 std::cerr << "BVH Warning! Call replaceVertex() in a wrong order. "
598 "replaceVertex() was ignored. Must do a beginReplaceModel() "
599 ✗ "for initialization."
600 ✗ << std::endl;
601 ✗ return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
602 }
603
604 ✗ (*vertices)[num_vertex_updated] = p;
605 ✗ num_vertex_updated++;
606
607 ✗ return BVH_OK;
608 }
609
610 ✗ int BVHModelBase::replaceTriangle(const Vec3s& p1, const Vec3s& p2,
611 const Vec3s& p3) {
612 ✗ if (build_state != BVH_BUILD_STATE_REPLACE_BEGUN) {
613 std::cerr << "BVH Warning! Call replaceTriangle() in a wrong order. "
614 "replaceTriangle() was ignored. Must do a beginReplaceModel() "
615 ✗ "for initialization."
616 ✗ << std::endl;
617 ✗ return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
618 }
619
620 ✗ (*vertices)[num_vertex_updated] = p1;
621 ✗ num_vertex_updated++;
622 ✗ (*vertices)[num_vertex_updated] = p2;
623 ✗ num_vertex_updated++;
624 ✗ (*vertices)[num_vertex_updated] = p3;
625 ✗ num_vertex_updated++;
626 ✗ return BVH_OK;
627 }
628
629 159 int BVHModelBase::replaceSubModel(const std::vector<Vec3s>& ps) {
630
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 159 times.
 159 if (build_state != BVH_BUILD_STATE_REPLACE_BEGUN) {
631 std::cerr << "BVH Warning! Call replaceSubModel() in a wrong order. "
632 "replaceSubModel() was ignored. Must do a beginReplaceModel() "
633 ✗ "for initialization."
634 ✗ << std::endl;
635 ✗ return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
636 }
637
638 159 std::vector<Vec3s>& vertices_ = *vertices;
639
2/2
✓ Branch 1 taken 745704 times.
✓ Branch 2 taken 159 times.
 745863 for (unsigned int i = 0; i < ps.size(); ++i) {
640 745704 vertices_[num_vertex_updated] = ps[i];
641 745704 num_vertex_updated++;
642 }
643 159 return BVH_OK;
644 }
645
646 159 int BVHModelBase::endReplaceModel(bool refit, bool bottomup) {
647
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 159 times.
 159 if (build_state != BVH_BUILD_STATE_REPLACE_BEGUN) {
648 std::cerr << "BVH Warning! Call endReplaceModel() in a wrong order. "
649 ✗ "endReplaceModel() was ignored. "
650 ✗ << std::endl;
651 ✗ return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
652 }
653
654
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 159 times.
 159 if (num_vertex_updated != num_vertices) {
655 std::cerr << "BVH Error! The replaced model should have the same number of "
656 ✗ "vertices as the old model."
657 ✗ << std::endl;
658 ✗ return BVH_ERR_INCORRECT_DATA;
659 }
660
661
2/2
✓ Branch 0 taken 89 times.
✓ Branch 1 taken 70 times.
 159 if (refit) // refit, do not change BVH structure
662 {
663 89 refitTree(bottomup);
664 } else // reconstruct bvh tree based on current frame data
665 {
666 70 buildTree();
667 }
668
669 159 build_state = BVH_BUILD_STATE_PROCESSED;
670
671 159 return BVH_OK;
672 }
673
674 ✗ int BVHModelBase::beginUpdateModel() {
675 ✗ if (build_state != BVH_BUILD_STATE_PROCESSED &&
676 ✗ build_state != BVH_BUILD_STATE_UPDATED) {
677 std::cerr << "BVH Error! Call beginUpdatemodel() on a BVHModel that has no "
678 ✗ "previous frame."
679 ✗ << std::endl;
680 ✗ return BVH_ERR_BUILD_EMPTY_PREVIOUS_FRAME;
681 }
682
683 ✗ if (prev_vertices.get()) {
684 ✗ std::shared_ptr<std::vector<Vec3s>> temp = prev_vertices;
685 ✗ prev_vertices = vertices;
686 ✗ vertices = temp;
687 ✗ } else {
688 ✗ prev_vertices = vertices;
689 ✗ vertices.reset(new std::vector<Vec3s>(num_vertices));
690 }
691
692 ✗ num_vertex_updated = 0;
693
694 ✗ build_state = BVH_BUILD_STATE_UPDATE_BEGUN;
695
696 ✗ return BVH_OK;
697 }
698
699 ✗ int BVHModelBase::updateVertex(const Vec3s& p) {
700 ✗ if (build_state != BVH_BUILD_STATE_UPDATE_BEGUN) {
701 std::cerr
702 << "BVH Warning! Call updateVertex() in a wrong order. updateVertex() "
703 ✗ "was ignored. Must do a beginUpdateModel() for initialization."
704 ✗ << std::endl;
705 ✗ return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
706 }
707
708 ✗ (*vertices)[num_vertex_updated] = p;
709 ✗ num_vertex_updated++;
710
711 ✗ return BVH_OK;
712 }
713
714 ✗ int BVHModelBase::updateTriangle(const Vec3s& p1, const Vec3s& p2,
715 const Vec3s& p3) {
716 ✗ if (build_state != BVH_BUILD_STATE_UPDATE_BEGUN) {
717 std::cerr << "BVH Warning! Call updateTriangle() in a wrong order. "
718 "updateTriangle() was ignored. Must do a beginUpdateModel() "
719 ✗ "for initialization."
720 ✗ << std::endl;
721 ✗ return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
722 }
723
724 ✗ (*vertices)[num_vertex_updated] = p1;
725 ✗ num_vertex_updated++;
726 ✗ (*vertices)[num_vertex_updated] = p2;
727 ✗ num_vertex_updated++;
728 ✗ (*vertices)[num_vertex_updated] = p3;
729 ✗ num_vertex_updated++;
730 ✗ return BVH_OK;
731 }
732
733 ✗ int BVHModelBase::updateSubModel(const std::vector<Vec3s>& ps) {
734 ✗ if (build_state != BVH_BUILD_STATE_UPDATE_BEGUN) {
735 std::cerr << "BVH Warning! Call updateSubModel() in a wrong order. "
736 "updateSubModel() was ignored. Must do a beginUpdateModel() "
737 ✗ "for initialization."
738 ✗ << std::endl;
739 ✗ return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
740 }
741
742 ✗ std::vector<Vec3s>& vertices_ = *vertices;
743 ✗ for (unsigned int i = 0; i < ps.size(); ++i) {
744 ✗ vertices_[num_vertex_updated] = ps[i];
745 ✗ num_vertex_updated++;
746 }
747 ✗ return BVH_OK;
748 }
749
750 ✗ int BVHModelBase::endUpdateModel(bool refit, bool bottomup) {
751 ✗ if (build_state != BVH_BUILD_STATE_UPDATE_BEGUN) {
752 std::cerr << "BVH Warning! Call endUpdateModel() in a wrong order. "
753 ✗ "endUpdateModel() was ignored. "
754 ✗ << std::endl;
755 ✗ return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
756 }
757
758 ✗ if (num_vertex_updated != num_vertices) {
759 std::cerr << "BVH Error! The updated model should have the same number of "
760 ✗ "vertices as the old model."
761 ✗ << std::endl;
762 ✗ return BVH_ERR_INCORRECT_DATA;
763 }
764
765 ✗ if (refit) // refit, do not change BVH structure
766 {
767 ✗ refitTree(bottomup);
768 } else // reconstruct bvh tree based on current frame data
769 {
770 ✗ buildTree();
771
772 // then refit
773
774 ✗ refitTree(bottomup);
775 }
776
777 ✗ build_state = BVH_BUILD_STATE_UPDATED;
778
779 ✗ return BVH_OK;
780 }
781
782 8628 void BVHModelBase::computeLocalAABB() {
783
1/2
✓ Branch 1 taken 8628 times.
✗ Branch 2 not taken.
 8628 AABB aabb_;
784 8628 const std::vector<Vec3s>& vertices_ = *vertices;
785
2/2
✓ Branch 0 taken 4112649 times.
✓ Branch 1 taken 8628 times.
 4121277 for (unsigned int i = 0; i < num_vertices; ++i) {
786
1/2
✓ Branch 2 taken 4112649 times.
✗ Branch 3 not taken.
 4112649 aabb_ += vertices_[i];
787 }
788
789
1/2
✓ Branch 1 taken 8628 times.
✗ Branch 2 not taken.
 8628 aabb_center = aabb_.center();
790
791 8628 aabb_radius = 0;
792
2/2
✓ Branch 0 taken 4112649 times.
✓ Branch 1 taken 8628 times.
 4121277 for (unsigned int i = 0; i < num_vertices; ++i) {
793
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();
794
2/2
✓ Branch 0 taken 10757 times.
✓ Branch 1 taken 4101892 times.
 4112649 if (r > aabb_radius) aabb_radius = r;
795 }
796
797 8628 aabb_radius = sqrt(aabb_radius);
798
799
1/2
✓ Branch 1 taken 8628 times.
✗ Branch 2 not taken.
 8628 aabb_local = aabb_;
800 8628 }
801
802 /// @brief Constructing an empty BVH
803 template <typename BV>
804 6228 BVHModel<BV>::BVHModel()
805 : BVHModelBase(),
806
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)),
807
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>()),
808 6228 num_bvs_allocated(0),
809
1/2
✓ Branch 2 taken 3114 times.
✗ Branch 3 not taken.
 12456 num_bvs(0) {}
810
811 template <typename BV>
812 6 void BVHModel<BV>::deleteBVs() {
813 6 bvs.reset();
814 6 primitive_indices.reset();
815 6 num_bvs_allocated = num_bvs = 0;
816 }
817
818 template <typename BV>
819 6210 bool BVHModel<BV>::allocateBVs() {
820 // construct BVH tree
821 6210 unsigned int num_bvs_to_be_allocated = 0;
822
2/2
✓ Branch 0 taken 8 times.
✓ Branch 1 taken 3097 times.
 6210 if (num_tris == 0)
823 16 num_bvs_to_be_allocated = 2 * num_vertices - 1;
824 else
825 6194 num_bvs_to_be_allocated = 2 * num_tris - 1;
826
827
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));
828
1/2
✓ Branch 2 taken 3105 times.
✗ Branch 3 not taken.
 12420 primitive_indices.reset(
829
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));
830
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())) {
831 ✗ std::cerr << "BVH Error! Out of memory for BV array in endModel()!"
832 ✗ << std::endl;
833 ✗ return false;
834 }
835 6210 num_bvs_allocated = num_bvs_to_be_allocated;
836 6210 num_bvs = 0;
837 6210 return true;
838 }
839
840 template <typename BV>
841 8 int BVHModel<BV>::memUsage(const bool msg) const {
842 8 unsigned int mem_bv_list = (unsigned int)sizeof(BV) * num_bvs;
843 8 unsigned int mem_tri_list = (unsigned int)sizeof(Triangle32) * num_tris;
844 8 unsigned int mem_vertex_list = (unsigned int)sizeof(Vec3s) * num_vertices;
845
846 8 unsigned int total_mem = mem_bv_list + mem_tri_list + mem_vertex_list +
847 (unsigned int)sizeof(BVHModel<BV>);
848
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 4 times.
 8 if (msg) {
849 ✗ std::cerr << "Total for model " << total_mem << " bytes." << std::endl;
850 ✗ std::cerr << "BVs: " << num_bvs << " allocated." << std::endl;
851 ✗ std::cerr << "Tris: " << num_tris << " allocated." << std::endl;
852 ✗ std::cerr << "Vertices: " << num_vertices << " allocated." << std::endl;
853 }
854
855 8 return static_cast<int>(total_mem);
856 }
857
858 template <typename BV>
859 6350 int BVHModel<BV>::buildTree() {
860 // set BVFitter
861
1/2
✓ Branch 1 taken 3175 times.
✗ Branch 2 not taken.
 6350 Vec3s* vertices_ = vertices.get() ? vertices->data() : NULL;
862
2/2
✓ Branch 1 taken 3167 times.
✓ Branch 2 taken 8 times.
 6350 Triangle32* tri_indices_ = tri_indices.get() ? tri_indices->data() : NULL;
863 6350 bv_fitter->set(vertices_, tri_indices_, getModelType());
864 // set SplitRule
865 6350 bv_splitter->set(vertices_, tri_indices_, getModelType());
866
867 6350 num_bvs = 1;
868
869 6350 unsigned int num_primitives = 0;
870
2/3
✓ Branch 1 taken 3167 times.
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
 6350 switch (getModelType()) {
871 6334 case BVH_MODEL_TRIANGLES:
872 6334 num_primitives = (unsigned int)num_tris;
873 6334 break;
874 16 case BVH_MODEL_POINTCLOUD:
875 16 num_primitives = (unsigned int)num_vertices;
876 16 break;
877 ✗ default:
878 ✗ std::cerr << "BVH Error: Model type not supported!" << std::endl;
879 ✗ return BVH_ERR_UNSUPPORTED_FUNCTION;
880 }
881
882 6350 std::vector<unsigned int>& primitive_indices_ = *primitive_indices;
883
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;
884 6350 recursiveBuildTree(0, 0, num_primitives);
885
886 6350 bv_fitter->clear();
887 6350 bv_splitter->clear();
888
889 6350 return BVH_OK;
890 }
891
892 template <typename BV>
893 4511846 int BVHModel<BV>::recursiveBuildTree(int bv_id, unsigned int first_primitive,
894 unsigned int num_primitives) {
895 4511846 BVHModelType type = getModelType();
896 4511846 BVNode<BV>* bvnode = bvs->data() + bv_id;
897 4511846 unsigned int* cur_primitive_indices =
898 4511846 primitive_indices->data() + first_primitive;
899
900 // constructing BV
901
1/2
✓ Branch 2 taken 2255923 times.
✗ Branch 3 not taken.
 4511846 BV bv = bv_fitter->fit(cur_primitive_indices, num_primitives);
902
1/2
✓ Branch 2 taken 2255923 times.
✗ Branch 3 not taken.
 4511846 bv_splitter->computeRule(bv, cur_primitive_indices, num_primitives);
903
904
1/2
✓ Branch 1 taken 2255923 times.
✗ Branch 2 not taken.
 4511846 bvnode->bv = bv;
905 4511846 bvnode->first_primitive = first_primitive;
906 4511846 bvnode->num_primitives = num_primitives;
907
908
2/2
✓ Branch 0 taken 1129549 times.
✓ Branch 1 taken 1126374 times.
 4511846 if (num_primitives == 1) {
909 2259098 bvnode->first_child = -((int)(*cur_primitive_indices) + 1);
910 } else {
911 2252748 bvnode->first_child = (int)num_bvs;
912 2252748 num_bvs += 2;
913
914 2252748 unsigned int c1 = 0;
915 2252748 const std::vector<Vec3s>& vertices_ = *vertices;
916 2252748 const std::vector<Triangle32>& tri_indices_ = *tri_indices;
917
2/2
✓ Branch 0 taken 12055007 times.
✓ Branch 1 taken 1126374 times.
 26362762 for (unsigned int i = 0; i < num_primitives; ++i) {
918
1/2
✓ Branch 1 taken 12055007 times.
✗ Branch 2 not taken.
 24110014 Vec3s p;
919
2/2
✓ Branch 0 taken 192 times.
✓ Branch 1 taken 12054815 times.
 24110014 if (type == BVH_MODEL_POINTCLOUD)
920
1/2
✓ Branch 2 taken 192 times.
✗ Branch 3 not taken.
 384 p = vertices_[cur_primitive_indices[i]];
921
1/2
✓ Branch 0 taken 12054815 times.
✗ Branch 1 not taken.
 24109630 else if (type == BVH_MODEL_TRIANGLES) {
922 24109630 const Triangle32& t = tri_indices_[cur_primitive_indices[i]];
923 24109630 const Vec3s& p1 = vertices_[t[0]];
924 24109630 const Vec3s& p2 = vertices_[t[1]];
925 24109630 const Vec3s& p3 = vertices_[t[2]];
926
927
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.;
928 } else {
929 ✗ std::cerr << "BVH Error: Model type not supported!" << std::endl;
930 ✗ return BVH_ERR_UNSUPPORTED_FUNCTION;
931 }
932
933 // loop invariant: up to (but not including) index c1 in group 1,
934 // then up to (but not including) index i in group 2
935 //
936 // [1] [1] [1] [1] [2] [2] [2] [x] [x] ... [x]
937 // c1 i
938 //
939
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
940 {
941 // do nothing
942 } else {
943 11868432 unsigned int temp = cur_primitive_indices[i];
944 11868432 cur_primitive_indices[i] = cur_primitive_indices[c1];
945 11868432 cur_primitive_indices[c1] = temp;
946 11868432 c1++;
947 }
948 }
949
950
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;
951
952 2252748 const unsigned int num_first_half = c1;
953
954
1/2
✓ Branch 2 taken 1126374 times.
✗ Branch 3 not taken.
 2252748 recursiveBuildTree(bvnode->leftChild(), first_primitive, num_first_half);
955
1/2
✓ Branch 2 taken 1126374 times.
✗ Branch 3 not taken.
 2252748 recursiveBuildTree(bvnode->rightChild(), first_primitive + num_first_half,
956 num_primitives - num_first_half);
957 }
958
959 4511846 return BVH_OK;
960 }
961
962 template <typename BV>
963 178 int BVHModel<BV>::refitTree(bool bottomup) {
964
2/2
✓ Branch 0 taken 21 times.
✓ Branch 1 taken 68 times.
 178 if (bottomup)
965 42 return refitTree_bottomup();
966 else
967 136 return refitTree_topdown();
968 }
969
970 template <typename BV>
971 42 int BVHModel<BV>::refitTree_bottomup() {
972 // TODO the recomputation of the BV is done manually, without using
973 // bv_fitter. The manual BV recomputation seems bugged. Using bv_fitter
974 // seems to correct the bug.
975 // bv_fitter->set(vertices, tri_indices, getModelType());
976
977 42 int res = recursiveRefitTree_bottomup(0);
978
979 // bv_fitter->clear();
980 42 return res;
981 }
982
983 template <typename BV>
984 183078 int BVHModel<BV>::recursiveRefitTree_bottomup(int bv_id) {
985 183078 BVNode<BV>* bvnode = bvs->data() + bv_id;
986
2/2
✓ Branch 1 taken 45780 times.
✓ Branch 2 taken 45759 times.
 183078 if (bvnode->isLeaf()) {
987 91560 BVHModelType type = getModelType();
988 91560 int primitive_id = -(bvnode->first_child + 1);
989
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 45780 times.
 91560 if (type == BVH_MODEL_POINTCLOUD) {
990 ✗ BV bv;
991
992 ✗ if (prev_vertices.get()) {
993 ✗ Vec3s v[2];
994 ✗ v[0] = (*prev_vertices)[static_cast<size_t>(primitive_id)];
995 ✗ v[1] = (*vertices)[static_cast<size_t>(primitive_id)];
996 ✗ fit(v, 2, bv);
997 } else
998 ✗ fit(vertices->data() + primitive_id, 1, bv);
999
1000 ✗ bvnode->bv = bv;
1001
1/2
✓ Branch 0 taken 45780 times.
✗ Branch 1 not taken.
 91560 } else if (type == BVH_MODEL_TRIANGLES) {
1002
1/2
✓ Branch 1 taken 45780 times.
✗ Branch 2 not taken.
 91560 BV bv;
1003 91560 const Triangle32& triangle =
1004 91560 (*tri_indices)[static_cast<size_t>(primitive_id)];
1005
1006
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 45780 times.
 91560 if (prev_vertices.get()) {
1007 ✗ Vec3s v[6];
1008 ✗ for (Triangle32::IndexType i = 0; i < 3; ++i) {
1009 ✗ v[i] = (*prev_vertices)[triangle[i]];
1010 ✗ v[i + 3] = (*vertices)[triangle[i]];
1011 }
1012
1013 ✗ fit(v, 6, bv);
1014 } else {
1015 // TODO use bv_fitter to build BV. See comment in refitTree_bottomup
1016 // unsigned int* cur_primitive_indices = primitive_indices +
1017 // bvnode->first_primitive; bv = bv_fitter->fit(cur_primitive_indices,
1018 // bvnode->num_primitives);
1019
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];
1020
2/2
✓ Branch 0 taken 137340 times.
✓ Branch 1 taken 45780 times.
 366240 for (int i = 0; i < 3; ++i) {
1021
1/2
✓ Branch 4 taken 137340 times.
✗ Branch 5 not taken.
 274680 v[i] = (*vertices)[triangle[(Triangle32::IndexType)i]];
1022 }
1023
1024
1/2
✓ Branch 1 taken 45780 times.
✗ Branch 2 not taken.
 91560 fit(v, 3, bv);
1025 }
1026
1027
1/2
✓ Branch 1 taken 45780 times.
✗ Branch 2 not taken.
 91560 bvnode->bv = bv;
1028 } else {
1029 ✗ std::cerr << "BVH Error: Model type not supported!" << std::endl;
1030 ✗ return BVH_ERR_UNSUPPORTED_FUNCTION;
1031 }
1032 } else {
1033 91518 recursiveRefitTree_bottomup(bvnode->leftChild());
1034 91518 recursiveRefitTree_bottomup(bvnode->rightChild());
1035
0/2
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 91518 bvnode->bv = (*bvs)[static_cast<size_t>(bvnode->leftChild())].bv +
1036 91518 (*bvs)[static_cast<size_t>(bvnode->rightChild())].bv;
1037 // TODO use bv_fitter to build BV. See comment in refitTree_bottomup
1038 // unsigned int* cur_primitive_indices = primitive_indices +
1039 // bvnode->first_primitive; bvnode->bv =
1040 // bv_fitter->fit(cur_primitive_indices, bvnode->num_primitives);
1041 }
1042
1043 183078 return BVH_OK;
1044 }
1045
1046 template <typename BV>
1047 136 int BVHModel<BV>::refitTree_topdown() {
1048
1/2
✓ Branch 1 taken 68 times.
✗ Branch 2 not taken.
 136 Vec3s* vertices_ = vertices.get() ? vertices->data() : NULL;
1049
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 68 times.
 136 Vec3s* prev_vertices_ = prev_vertices.get() ? prev_vertices->data() : NULL;
1050
1/2
✓ Branch 1 taken 68 times.
✗ Branch 2 not taken.
 136 Triangle32* tri_indices_ = tri_indices.get() ? tri_indices->data() : NULL;
1051 136 bv_fitter->set(vertices_, prev_vertices_, tri_indices_, getModelType());
1052 136 BVNode<BV>* bvs_ = bvs->data();
1053 136 unsigned int* primitive_indices_ = primitive_indices->data();
1054
2/2
✓ Branch 0 taken 162860 times.
✓ Branch 1 taken 68 times.
 325856 for (unsigned int i = 0; i < num_bvs; ++i) {
1055 325720 BV bv = bv_fitter->fit(primitive_indices_ + bvs_[i].first_primitive,
1056
1/2
✓ Branch 1 taken 162860 times.
✗ Branch 2 not taken.
 325720 bvs_[i].num_primitives);
1057
1/2
✓ Branch 1 taken 162860 times.
✗ Branch 2 not taken.
 325720 bvs_[i].bv = bv;
1058 }
1059
1060 136 bv_fitter->clear();
1061
1062 136 return BVH_OK;
1063 }
1064
1065 template <>
1066 ✗ void BVHModel<OBB>::makeParentRelativeRecurse(int bv_id, Matrix3s& parent_axes,
1067 const Vec3s& parent_c) {
1068 ✗ bv_node_vector_t& bvs_ = *bvs;
1069 ✗ OBB& obb = bvs_[static_cast<size_t>(bv_id)].bv;
1070 ✗ if (!bvs_[static_cast<size_t>(bv_id)].isLeaf()) {
1071 ✗ makeParentRelativeRecurse(bvs_[static_cast<size_t>(bv_id)].first_child,
1072 ✗ obb.axes, obb.To);
1073
1074 ✗ makeParentRelativeRecurse(bvs_[static_cast<size_t>(bv_id)].first_child + 1,
1075 ✗ obb.axes, obb.To);
1076 }
1077
1078 // make self parent relative
1079 // obb.axes = parent_axes.transpose() * obb.axes;
1080 ✗ obb.axes.applyOnTheLeft(parent_axes.transpose());
1081
1082 ✗ Vec3s t(obb.To - parent_c);
1083 ✗ obb.To.noalias() = parent_axes.transpose() * t;
1084 }
1085
1086 template <>
1087 ✗ void BVHModel<RSS>::makeParentRelativeRecurse(int bv_id, Matrix3s& parent_axes,
1088 const Vec3s& parent_c) {
1089 ✗ bv_node_vector_t& bvs_ = *bvs;
1090 ✗ RSS& rss = bvs_[static_cast<size_t>(bv_id)].bv;
1091 ✗ if (!bvs_[static_cast<size_t>(bv_id)].isLeaf()) {
1092 ✗ makeParentRelativeRecurse(bvs_[static_cast<size_t>(bv_id)].first_child,
1093 ✗ rss.axes, rss.Tr);
1094
1095 ✗ makeParentRelativeRecurse(bvs_[static_cast<size_t>(bv_id)].first_child + 1,
1096 ✗ rss.axes, rss.Tr);
1097 }
1098
1099 // make self parent relative
1100 // rss.axes = parent_axes.transpose() * rss.axes;
1101 ✗ rss.axes.applyOnTheLeft(parent_axes.transpose());
1102
1103 ✗ Vec3s t(rss.Tr - parent_c);
1104 ✗ rss.Tr.noalias() = parent_axes.transpose() * t;
1105 }
1106
1107 template <>
1108 ✗ void BVHModel<OBBRSS>::makeParentRelativeRecurse(int bv_id,
1109 Matrix3s& parent_axes,
1110 const Vec3s& parent_c) {
1111 ✗ bv_node_vector_t& bvs_ = *bvs;
1112 ✗ OBB& obb = bvs_[static_cast<size_t>(bv_id)].bv.obb;
1113 ✗ RSS& rss = bvs_[static_cast<size_t>(bv_id)].bv.rss;
1114 ✗ if (!bvs_[static_cast<size_t>(bv_id)].isLeaf()) {
1115 ✗ makeParentRelativeRecurse(bvs_[static_cast<size_t>(bv_id)].first_child,
1116 ✗ obb.axes, obb.To);
1117
1118 ✗ makeParentRelativeRecurse(bvs_[static_cast<size_t>(bv_id)].first_child + 1,
1119 ✗ obb.axes, obb.To);
1120 }
1121
1122 // make self parent relative
1123 ✗ rss.axes.noalias() = parent_axes.transpose() * obb.axes;
1124 ✗ obb.axes = rss.axes;
1125
1126 ✗ Vec3s t(obb.To - parent_c);
1127 ✗ obb.To.noalias() = parent_axes.transpose() * t;
1128 ✗ rss.Tr = obb.To;
1129 }
1130
1131 template <>
1132 3 NODE_TYPE BVHModel<AABB>::getNodeType() const {
1133 3 return BV_AABB;
1134 }
1135
1136 template <>
1137 43 NODE_TYPE BVHModel<OBB>::getNodeType() const {
1138 43 return BV_OBB;
1139 }
1140
1141 template <>
1142 7 NODE_TYPE BVHModel<RSS>::getNodeType() const {
1143 7 return BV_RSS;
1144 }
1145
1146 template <>
1147 7 NODE_TYPE BVHModel<kIOS>::getNodeType() const {
1148 7 return BV_kIOS;
1149 }
1150
1151 template <>
1152 39069 NODE_TYPE BVHModel<OBBRSS>::getNodeType() const {
1153 39069 return BV_OBBRSS;
1154 }
1155
1156 template <>
1157 5 NODE_TYPE BVHModel<KDOP<16>>::getNodeType() const {
1158 5 return BV_KDOP16;
1159 }
1160
1161 template <>
1162 7 NODE_TYPE BVHModel<KDOP<18>>::getNodeType() const {
1163 7 return BV_KDOP18;
1164 }
1165
1166 template <>
1167 9 NODE_TYPE BVHModel<KDOP<24>>::getNodeType() const {
1168 9 return BV_KDOP24;
1169 }
1170
1171 template class BVHModel<KDOP<16>>;
1172 template class BVHModel<KDOP<18>>;
1173 template class BVHModel<KDOP<24>>;
1174 template class BVHModel<OBB>;
1175 template class BVHModel<AABB>;
1176 template class BVHModel<RSS>;
1177 template class BVHModel<kIOS>;
1178 template class BVHModel<OBBRSS>;
1179
1180 } // namespace coal
1181