GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
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 <hpp/fcl/BVH/BVH_model.h> |
||
40 |
|||
41 |
#include <iostream> |
||
42 |
#include <string.h> |
||
43 |
|||
44 |
#include <hpp/fcl/BV/BV.h> |
||
45 |
#include <hpp/fcl/shape/convex.h> |
||
46 |
|||
47 |
#include <hpp/fcl/internal/BV_splitter.h> |
||
48 |
#include <hpp/fcl/internal/BV_fitter.h> |
||
49 |
|||
50 |
namespace hpp { |
||
51 |
namespace fcl { |
||
52 |
|||
53 |
3105 |
BVHModelBase::BVHModelBase() |
|
54 |
: vertices(NULL), |
||
55 |
tri_indices(NULL), |
||
56 |
prev_vertices(NULL), |
||
57 |
num_tris(0), |
||
58 |
num_vertices(0), |
||
59 |
build_state(BVH_BUILD_STATE_EMPTY), |
||
60 |
num_tris_allocated(0), |
||
61 |
num_vertices_allocated(0), |
||
62 |
3105 |
num_vertex_updated(0) {} |
|
63 |
|||
64 |
42 |
BVHModelBase::BVHModelBase(const BVHModelBase& other) |
|
65 |
: CollisionGeometry(other), |
||
66 |
42 |
num_tris(other.num_tris), |
|
67 |
42 |
num_vertices(other.num_vertices), |
|
68 |
42 |
build_state(other.build_state), |
|
69 |
42 |
num_tris_allocated(other.num_tris), |
|
70 |
42 |
num_vertices_allocated(other.num_vertices) { |
|
71 |
✓✗ | 42 |
if (other.vertices) { |
72 |
✓✗✓✗ ✓✓✓✗ |
46515 |
vertices = new Vec3f[num_vertices]; |
73 |
✓✗ | 42 |
std::copy(other.vertices, other.vertices + num_vertices, vertices); |
74 |
} else |
||
75 |
vertices = nullptr; |
||
76 |
|||
77 |
✓✗ | 42 |
if (other.tri_indices) { |
78 |
✓✗✓✗ ✓✓ |
50358 |
tri_indices = new Triangle[num_tris]; |
79 |
✓✗ | 42 |
std::copy(other.tri_indices, other.tri_indices + num_tris, tri_indices); |
80 |
} else |
||
81 |
tri_indices = nullptr; |
||
82 |
|||
83 |
✗✓ | 42 |
if (other.prev_vertices) { |
84 |
prev_vertices = new Vec3f[num_vertices]; |
||
85 |
std::copy(other.prev_vertices, other.prev_vertices + num_vertices, |
||
86 |
prev_vertices); |
||
87 |
} else |
||
88 |
42 |
prev_vertices = nullptr; |
|
89 |
42 |
} |
|
90 |
|||
91 |
11 |
bool BVHModelBase::isEqual(const CollisionGeometry& _other) const { |
|
92 |
✓✗ | 11 |
const BVHModelBase* other_ptr = dynamic_cast<const BVHModelBase*>(&_other); |
93 |
✗✓ | 11 |
if (other_ptr == nullptr) return false; |
94 |
11 |
const BVHModelBase& other = *other_ptr; |
|
95 |
|||
96 |
11 |
bool result = |
|
97 |
✓✓✓✗ |
11 |
num_tris == other.num_tris && num_vertices == other.num_vertices; |
98 |
|||
99 |
✓✓ | 11 |
if (!result) return false; |
100 |
|||
101 |
✓✓ | 19846 |
for (size_t k = 0; k < static_cast<size_t>(num_tris); ++k) |
102 |
✗✓ | 19836 |
if (tri_indices[k] != other.tri_indices[k]) return false; |
103 |
|||
104 |
✓✓ | 59518 |
for (size_t k = 0; k < static_cast<size_t>(num_vertices); ++k) |
105 |
✗✓ | 59508 |
if (vertices[k] != other.vertices[k]) return false; |
106 |
|||
107 |
✗✓✗✗ |
10 |
if (prev_vertices != nullptr && other.prev_vertices != nullptr) { |
108 |
for (size_t k = 0; k < static_cast<size_t>(num_vertices); ++k) { |
||
109 |
if (prev_vertices[k] != other.prev_vertices[k]) return false; |
||
110 |
} |
||
111 |
} |
||
112 |
|||
113 |
10 |
return true; |
|
114 |
} |
||
115 |
|||
116 |
2 |
void BVHModelBase::buildConvexRepresentation(bool share_memory) { |
|
117 |
✓✗ | 2 |
if (!convex) { |
118 |
2 |
Vec3f* points = vertices; |
|
119 |
2 |
Triangle* polygons = tri_indices; |
|
120 |
✓✗ | 2 |
if (!share_memory) { |
121 |
✓✗✓✓ ✓✗ |
18 |
points = new Vec3f[num_vertices]; |
122 |
2 |
std::copy(vertices, vertices + num_vertices, points); |
|
123 |
|||
124 |
✓✗✓✓ |
26 |
polygons = new Triangle[num_tris]; |
125 |
2 |
std::copy(tri_indices, tri_indices + num_tris, polygons); |
|
126 |
} |
||
127 |
2 |
convex.reset(new Convex<Triangle>(!share_memory, points, num_vertices, |
|
128 |
✓✗ | 2 |
polygons, num_tris)); |
129 |
} |
||
130 |
2 |
} |
|
131 |
|||
132 |
bool BVHModelBase::buildConvexHull(bool keepTriangle, |
||
133 |
const char* qhullCommand) { |
||
134 |
convex.reset(ConvexBase::convexHull(vertices, num_vertices, keepTriangle, |
||
135 |
qhullCommand)); |
||
136 |
return num_vertices == convex->num_points; |
||
137 |
} |
||
138 |
|||
139 |
template <typename BV> |
||
140 |
84 |
BVHModel<BV>::BVHModel(const BVHModel<BV>& other) |
|
141 |
: BVHModelBase(other), |
||
142 |
84 |
bv_splitter(other.bv_splitter), |
|
143 |
84 |
bv_fitter(other.bv_fitter) { |
|
144 |
✓✗ | 84 |
if (other.primitive_indices) { |
145 |
84 |
unsigned int num_primitives = 0; |
|
146 |
✓✗✗ | 84 |
switch (other.getModelType()) { |
147 |
84 |
case BVH_MODEL_TRIANGLES: |
|
148 |
84 |
num_primitives = num_tris; |
|
149 |
84 |
break; |
|
150 |
case BVH_MODEL_POINTCLOUD: |
||
151 |
num_primitives = num_vertices; |
||
152 |
break; |
||
153 |
84 |
default:; |
|
154 |
} |
||
155 |
|||
156 |
✓✗✓✗ |
84 |
primitive_indices = new unsigned int[num_primitives]; |
157 |
✓✗ | 84 |
std::copy(other.primitive_indices, other.primitive_indices + num_primitives, |
158 |
primitive_indices); |
||
159 |
} else |
||
160 |
primitive_indices = nullptr; |
||
161 |
|||
162 |
84 |
num_bvs = num_bvs_allocated = other.num_bvs; |
|
163 |
✓✗ | 84 |
if (other.bvs) { |
164 |
✓✗✓✗ ✓✓✓✗ |
201264 |
bvs = new BVNode<BV>[num_bvs]; |
165 |
✓✗ | 84 |
std::copy(other.bvs, other.bvs + num_bvs, bvs); |
166 |
} else |
||
167 |
bvs = nullptr; |
||
168 |
} |
||
169 |
|||
170 |
3102 |
int BVHModelBase::beginModel(unsigned int num_tris_, |
|
171 |
unsigned int num_vertices_) { |
||
172 |
✓✓ | 3102 |
if (build_state != BVH_BUILD_STATE_EMPTY) { |
173 |
✓✗ | 3 |
delete[] vertices; |
174 |
3 |
vertices = nullptr; |
|
175 |
✓✗ | 3 |
delete[] tri_indices; |
176 |
3 |
tri_indices = nullptr; |
|
177 |
✗✓ | 3 |
delete[] prev_vertices; |
178 |
3 |
prev_vertices = nullptr; |
|
179 |
|||
180 |
3 |
num_vertices_allocated = num_vertices = num_tris_allocated = num_tris = 0; |
|
181 |
3 |
deleteBVs(); |
|
182 |
} |
||
183 |
|||
184 |
✓✓ | 3102 |
if (num_tris_ <= 0) num_tris_ = 8; |
185 |
✓✓ | 3102 |
if (num_vertices_ <= 0) num_vertices_ = 8; |
186 |
|||
187 |
3102 |
num_vertices_allocated = num_vertices_; |
|
188 |
3102 |
num_tris_allocated = num_tris_; |
|
189 |
|||
190 |
✓✗✓✓ |
27918 |
tri_indices = new Triangle[num_tris_allocated]; |
191 |
|||
192 |
✗✓ | 3102 |
if (!tri_indices) { |
193 |
std::cerr << "BVH Error! Out of memory for tri_indices array on " |
||
194 |
"BeginModel() call!" |
||
195 |
<< std::endl; |
||
196 |
return BVH_ERR_MODEL_OUT_OF_MEMORY; |
||
197 |
} |
||
198 |
|||
199 |
✓✗✓✓ ✓✗ |
28430 |
vertices = new Vec3f[num_vertices_allocated]; |
200 |
✗✓ | 3102 |
if (!vertices) { |
201 |
std::cerr |
||
202 |
<< "BVH Error! Out of memory for vertices array on BeginModel() call!" |
||
203 |
<< std::endl; |
||
204 |
return BVH_ERR_MODEL_OUT_OF_MEMORY; |
||
205 |
} |
||
206 |
|||
207 |
✓✓ | 3102 |
if (build_state != BVH_BUILD_STATE_EMPTY) { |
208 |
std::cerr |
||
209 |
<< "BVH Warning! Call beginModel() on a BVHModel that is not empty. " |
||
210 |
3 |
"This model was cleared and previous triangles/vertices were lost." |
|
211 |
3 |
<< std::endl; |
|
212 |
3 |
build_state = BVH_BUILD_STATE_EMPTY; |
|
213 |
3 |
return BVH_ERR_BUILD_OUT_OF_SEQUENCE; |
|
214 |
} |
||
215 |
|||
216 |
3099 |
build_state = BVH_BUILD_STATE_BEGUN; |
|
217 |
|||
218 |
3099 |
return BVH_OK; |
|
219 |
} |
||
220 |
|||
221 |
32 |
int BVHModelBase::addVertex(const Vec3f& p) { |
|
222 |
✗✓ | 32 |
if (build_state != BVH_BUILD_STATE_BEGUN) { |
223 |
std::cerr << "BVH Warning! Call addVertex() in a wrong order. addVertex() " |
||
224 |
"was ignored. Must do a beginModel() to clear the model for " |
||
225 |
"addition of new vertices." |
||
226 |
<< std::endl; |
||
227 |
return BVH_ERR_BUILD_OUT_OF_SEQUENCE; |
||
228 |
} |
||
229 |
|||
230 |
✗✓ | 32 |
if (num_vertices >= num_vertices_allocated) { |
231 |
Vec3f* temp = new Vec3f[num_vertices_allocated * 2]; |
||
232 |
if (!temp) { |
||
233 |
std::cerr |
||
234 |
<< "BVH Error! Out of memory for vertices array on addVertex() call!" |
||
235 |
<< std::endl; |
||
236 |
return BVH_ERR_MODEL_OUT_OF_MEMORY; |
||
237 |
} |
||
238 |
|||
239 |
std::copy(vertices, vertices + num_vertices, temp); |
||
240 |
delete[] vertices; |
||
241 |
vertices = temp; |
||
242 |
num_vertices_allocated *= 2; |
||
243 |
} |
||
244 |
|||
245 |
32 |
vertices[num_vertices] = p; |
|
246 |
32 |
num_vertices += 1; |
|
247 |
|||
248 |
32 |
return BVH_OK; |
|
249 |
} |
||
250 |
|||
251 |
int BVHModelBase::addTriangles(const Matrixx3i& triangles) { |
||
252 |
if (build_state == BVH_BUILD_STATE_PROCESSED) { |
||
253 |
std::cerr << "BVH Warning! Call addSubModel() in a wrong order. " |
||
254 |
"addSubModel() was ignored. Must do a beginModel() to clear " |
||
255 |
"the model for addition of new vertices." |
||
256 |
<< std::endl; |
||
257 |
return BVH_ERR_BUILD_OUT_OF_SEQUENCE; |
||
258 |
} |
||
259 |
|||
260 |
const unsigned int num_tris_to_add = (unsigned int)triangles.rows(); |
||
261 |
|||
262 |
if (num_tris + num_tris_to_add > num_tris_allocated) { |
||
263 |
Triangle* temp = new Triangle[num_tris_allocated * 2 + num_tris_to_add]; |
||
264 |
if (!temp) { |
||
265 |
std::cerr << "BVH Error! Out of memory for tri_indices array on " |
||
266 |
"addSubModel() call!" |
||
267 |
<< std::endl; |
||
268 |
return BVH_ERR_MODEL_OUT_OF_MEMORY; |
||
269 |
} |
||
270 |
|||
271 |
std::copy(tri_indices, tri_indices + num_tris, temp); |
||
272 |
delete[] tri_indices; |
||
273 |
tri_indices = temp; |
||
274 |
num_tris_allocated = num_tris_allocated * 2 + num_tris_to_add; |
||
275 |
} |
||
276 |
|||
277 |
for (Eigen::DenseIndex i = 0; i < triangles.rows(); ++i) { |
||
278 |
const Matrixx3i::ConstRowXpr triangle = triangles.row(i); |
||
279 |
tri_indices[num_tris++].set(static_cast<Triangle::index_type>(triangle[0]), |
||
280 |
static_cast<Triangle::index_type>(triangle[1]), |
||
281 |
static_cast<Triangle::index_type>(triangle[2])); |
||
282 |
} |
||
283 |
|||
284 |
return BVH_OK; |
||
285 |
} |
||
286 |
|||
287 |
4 |
int BVHModelBase::addVertices(const Matrixx3f& points) { |
|
288 |
✗✓ | 4 |
if (build_state != BVH_BUILD_STATE_BEGUN) { |
289 |
std::cerr << "BVH Warning! Call addVertex() in a wrong order. " |
||
290 |
"addVertices() was ignored. Must do a beginModel() to clear " |
||
291 |
"the model for addition of new vertices." |
||
292 |
<< std::endl; |
||
293 |
return BVH_ERR_BUILD_OUT_OF_SEQUENCE; |
||
294 |
} |
||
295 |
|||
296 |
✗✓ | 4 |
if (num_vertices + points.rows() > num_vertices_allocated) { |
297 |
num_vertices_allocated = num_vertices + (unsigned int)points.rows(); |
||
298 |
Vec3f* temp = new Vec3f[num_vertices_allocated]; |
||
299 |
if (!temp) { |
||
300 |
std::cerr |
||
301 |
<< "BVH Error! Out of memory for vertices array on addVertex() call!" |
||
302 |
<< std::endl; |
||
303 |
return BVH_ERR_MODEL_OUT_OF_MEMORY; |
||
304 |
} |
||
305 |
|||
306 |
std::copy(vertices, vertices + num_vertices, temp); |
||
307 |
delete[] vertices; |
||
308 |
vertices = temp; |
||
309 |
} |
||
310 |
|||
311 |
✓✓ | 36 |
for (Eigen::DenseIndex id = 0; id < points.rows(); ++id) |
312 |
✓✗✓✗ |
32 |
vertices[num_vertices++] = points.row(id).transpose(); |
313 |
|||
314 |
4 |
return BVH_OK; |
|
315 |
} |
||
316 |
|||
317 |
96 |
int BVHModelBase::addTriangle(const Vec3f& p1, const Vec3f& p2, |
|
318 |
const Vec3f& p3) { |
||
319 |
✗✓ | 96 |
if (build_state == BVH_BUILD_STATE_PROCESSED) { |
320 |
std::cerr << "BVH Warning! Call addTriangle() in a wrong order. " |
||
321 |
"addTriangle() was ignored. Must do a beginModel() to clear " |
||
322 |
"the model for addition of new triangles." |
||
323 |
<< std::endl; |
||
324 |
return BVH_ERR_BUILD_OUT_OF_SEQUENCE; |
||
325 |
} |
||
326 |
|||
327 |
✓✓ | 96 |
if (num_vertices + 2 >= num_vertices_allocated) { |
328 |
✓✗✓✓ ✓✗ |
464 |
Vec3f* temp = new Vec3f[num_vertices_allocated * 2 + 2]; |
329 |
✗✓ | 16 |
if (!temp) { |
330 |
std::cerr << "BVH Error! Out of memory for vertices array on " |
||
331 |
"addTriangle() call!" |
||
332 |
<< std::endl; |
||
333 |
return BVH_ERR_MODEL_OUT_OF_MEMORY; |
||
334 |
} |
||
335 |
|||
336 |
16 |
std::copy(vertices, vertices + num_vertices, temp); |
|
337 |
✓✗ | 16 |
delete[] vertices; |
338 |
16 |
vertices = temp; |
|
339 |
16 |
num_vertices_allocated = num_vertices_allocated * 2 + 2; |
|
340 |
} |
||
341 |
|||
342 |
96 |
const unsigned int offset = num_vertices; |
|
343 |
|||
344 |
96 |
vertices[num_vertices] = p1; |
|
345 |
96 |
num_vertices++; |
|
346 |
96 |
vertices[num_vertices] = p2; |
|
347 |
96 |
num_vertices++; |
|
348 |
96 |
vertices[num_vertices] = p3; |
|
349 |
96 |
num_vertices++; |
|
350 |
|||
351 |
✓✓ | 96 |
if (num_tris >= num_tris_allocated) { |
352 |
✓✗✓✓ |
136 |
Triangle* temp = new Triangle[num_tris_allocated * 2]; |
353 |
✗✓ | 8 |
if (!temp) { |
354 |
std::cerr << "BVH Error! Out of memory for tri_indices array on " |
||
355 |
"addTriangle() call!" |
||
356 |
<< std::endl; |
||
357 |
return BVH_ERR_MODEL_OUT_OF_MEMORY; |
||
358 |
} |
||
359 |
|||
360 |
8 |
std::copy(tri_indices, tri_indices + num_tris, temp); |
|
361 |
✓✗ | 8 |
delete[] tri_indices; |
362 |
8 |
tri_indices = temp; |
|
363 |
8 |
num_tris_allocated *= 2; |
|
364 |
} |
||
365 |
|||
366 |
96 |
tri_indices[num_tris].set((Triangle::index_type)offset, |
|
367 |
96 |
(Triangle::index_type)(offset + 1), |
|
368 |
96 |
(Triangle::index_type)(offset + 2)); |
|
369 |
96 |
num_tris++; |
|
370 |
|||
371 |
96 |
return BVH_OK; |
|
372 |
} |
||
373 |
|||
374 |
int BVHModelBase::addSubModel(const std::vector<Vec3f>& ps) { |
||
375 |
if (build_state == BVH_BUILD_STATE_PROCESSED) { |
||
376 |
std::cerr << "BVH Warning! Call addSubModel() in a wrong order. " |
||
377 |
"addSubModel() was ignored. Must do a beginModel() to clear " |
||
378 |
"the model for addition of new vertices." |
||
379 |
<< std::endl; |
||
380 |
return BVH_ERR_BUILD_OUT_OF_SEQUENCE; |
||
381 |
} |
||
382 |
|||
383 |
const unsigned int num_vertices_to_add = (unsigned int)ps.size(); |
||
384 |
|||
385 |
if (num_vertices + num_vertices_to_add - 1 >= num_vertices_allocated) { |
||
386 |
Vec3f* temp = |
||
387 |
new Vec3f[num_vertices_allocated * 2 + num_vertices_to_add - 1]; |
||
388 |
if (!temp) { |
||
389 |
std::cerr << "BVH Error! Out of memory for vertices array on " |
||
390 |
"addSubModel() call!" |
||
391 |
<< std::endl; |
||
392 |
return BVH_ERR_MODEL_OUT_OF_MEMORY; |
||
393 |
} |
||
394 |
|||
395 |
std::copy(vertices, vertices + num_vertices, temp); |
||
396 |
delete[] vertices; |
||
397 |
vertices = temp; |
||
398 |
num_vertices_allocated = |
||
399 |
num_vertices_allocated * 2 + num_vertices_to_add - 1; |
||
400 |
} |
||
401 |
|||
402 |
for (size_t i = 0; i < (size_t)num_vertices_to_add; ++i) { |
||
403 |
vertices[num_vertices] = ps[i]; |
||
404 |
num_vertices++; |
||
405 |
} |
||
406 |
|||
407 |
return BVH_OK; |
||
408 |
} |
||
409 |
|||
410 |
3054 |
int BVHModelBase::addSubModel(const std::vector<Vec3f>& ps, |
|
411 |
const std::vector<Triangle>& ts) { |
||
412 |
✗✓ | 3054 |
if (build_state == BVH_BUILD_STATE_PROCESSED) { |
413 |
std::cerr << "BVH Warning! Call addSubModel() in a wrong order. " |
||
414 |
"addSubModel() was ignored. Must do a beginModel() to clear " |
||
415 |
"the model for addition of new vertices." |
||
416 |
<< std::endl; |
||
417 |
return BVH_ERR_BUILD_OUT_OF_SEQUENCE; |
||
418 |
} |
||
419 |
|||
420 |
3054 |
const unsigned int num_vertices_to_add = (unsigned int)ps.size(); |
|
421 |
|||
422 |
✓✓ | 3054 |
if (num_vertices + num_vertices_to_add - 1 >= num_vertices_allocated) { |
423 |
Vec3f* temp = |
||
424 |
✓✗✓✓ ✓✗ |
1431088 |
new Vec3f[num_vertices_allocated * 2 + num_vertices_to_add - 1]; |
425 |
✗✓ | 1181 |
if (!temp) { |
426 |
std::cerr << "BVH Error! Out of memory for vertices array on " |
||
427 |
"addSubModel() call!" |
||
428 |
<< std::endl; |
||
429 |
return BVH_ERR_MODEL_OUT_OF_MEMORY; |
||
430 |
} |
||
431 |
|||
432 |
1181 |
std::copy(vertices, vertices + num_vertices, temp); |
|
433 |
✓✗ | 1181 |
delete[] vertices; |
434 |
1181 |
vertices = temp; |
|
435 |
1181 |
num_vertices_allocated = |
|
436 |
1181 |
num_vertices_allocated * 2 + num_vertices_to_add - 1; |
|
437 |
} |
||
438 |
|||
439 |
3054 |
const unsigned int offset = num_vertices; |
|
440 |
|||
441 |
✓✓ | 1430230 |
for (size_t i = 0; i < (size_t)num_vertices_to_add; ++i) { |
442 |
1427176 |
vertices[num_vertices] = ps[i]; |
|
443 |
1427176 |
num_vertices++; |
|
444 |
} |
||
445 |
|||
446 |
3054 |
const unsigned int num_tris_to_add = (unsigned int)ts.size(); |
|
447 |
|||
448 |
✓✗ | 3054 |
if (num_tris + num_tris_to_add - 1 >= num_tris_allocated) { |
449 |
✓✗✓✓ |
1021109 |
Triangle* temp = new Triangle[num_tris_allocated * 2 + num_tris_to_add - 1]; |
450 |
✗✓ | 3054 |
if (!temp) { |
451 |
std::cerr << "BVH Error! Out of memory for tri_indices array on " |
||
452 |
"addSubModel() call!" |
||
453 |
<< std::endl; |
||
454 |
return BVH_ERR_MODEL_OUT_OF_MEMORY; |
||
455 |
} |
||
456 |
|||
457 |
3054 |
std::copy(tri_indices, tri_indices + num_tris, temp); |
|
458 |
✓✗ | 3054 |
delete[] tri_indices; |
459 |
3054 |
tri_indices = temp; |
|
460 |
3054 |
num_tris_allocated = num_tris_allocated * 2 + num_tris_to_add - 1; |
|
461 |
} |
||
462 |
|||
463 |
✓✓ | 975299 |
for (size_t i = 0; i < (size_t)num_tris_to_add; ++i) { |
464 |
972245 |
const Triangle& t = ts[i]; |
|
465 |
972245 |
tri_indices[num_tris].set(t[0] + (size_t)offset, t[1] + (size_t)offset, |
|
466 |
972245 |
t[2] + (size_t)offset); |
|
467 |
972245 |
num_tris++; |
|
468 |
} |
||
469 |
|||
470 |
3054 |
return BVH_OK; |
|
471 |
} |
||
472 |
|||
473 |
3102 |
int BVHModelBase::endModel() { |
|
474 |
✓✓ | 3102 |
if (build_state != BVH_BUILD_STATE_BEGUN) { |
475 |
std::cerr << "BVH Warning! Call endModel() in wrong order. endModel() was " |
||
476 |
3 |
"ignored." |
|
477 |
3 |
<< std::endl; |
|
478 |
3 |
return BVH_ERR_BUILD_OUT_OF_SEQUENCE; |
|
479 |
} |
||
480 |
|||
481 |
✓✓✗✓ |
3099 |
if (num_tris == 0 && num_vertices == 0) { |
482 |
std::cerr << "BVH Error! endModel() called on model with no triangles and " |
||
483 |
"vertices." |
||
484 |
<< std::endl; |
||
485 |
return BVH_ERR_BUILD_EMPTY_MODEL; |
||
486 |
} |
||
487 |
|||
488 |
✓✓ | 3099 |
if (num_tris_allocated > num_tris) { |
489 |
✓✓ | 3067 |
if (num_tris > 0) { |
490 |
✓✗✓✓ |
973650 |
Triangle* new_tris = new Triangle[num_tris]; |
491 |
✗✓ | 3059 |
if (!new_tris) { |
492 |
std::cerr << "BVH Error! Out of memory for tri_indices array in " |
||
493 |
"endModel() call!" |
||
494 |
<< std::endl; |
||
495 |
return BVH_ERR_MODEL_OUT_OF_MEMORY; |
||
496 |
} |
||
497 |
3059 |
std::copy(tri_indices, tri_indices + num_tris, new_tris); |
|
498 |
✓✗ | 3059 |
delete[] tri_indices; |
499 |
3059 |
tri_indices = new_tris; |
|
500 |
3059 |
num_tris_allocated = num_tris; |
|
501 |
} else { |
||
502 |
✓✗ | 8 |
delete[] tri_indices; |
503 |
8 |
tri_indices = NULL; |
|
504 |
8 |
num_tris = num_tris_allocated = 0; |
|
505 |
} |
||
506 |
} |
||
507 |
|||
508 |
✓✓ | 3099 |
if (num_vertices_allocated > num_vertices) { |
509 |
✓✗✓✓ ✓✗ |
1412785 |
Vec3f* new_vertices = new Vec3f[num_vertices]; |
510 |
✗✓ | 1186 |
if (!new_vertices) { |
511 |
std::cerr |
||
512 |
<< "BVH Error! Out of memory for vertices array in endModel() call!" |
||
513 |
<< std::endl; |
||
514 |
return BVH_ERR_MODEL_OUT_OF_MEMORY; |
||
515 |
} |
||
516 |
1186 |
std::copy(vertices, vertices + num_vertices, new_vertices); |
|
517 |
✓✗ | 1186 |
delete[] vertices; |
518 |
1186 |
vertices = new_vertices; |
|
519 |
1186 |
num_vertices_allocated = num_vertices; |
|
520 |
} |
||
521 |
|||
522 |
// construct BVH tree |
||
523 |
✗✓ | 3099 |
if (!allocateBVs()) return BVH_ERR_MODEL_OUT_OF_MEMORY; |
524 |
|||
525 |
3099 |
buildTree(); |
|
526 |
|||
527 |
// finish constructing |
||
528 |
3099 |
build_state = BVH_BUILD_STATE_PROCESSED; |
|
529 |
|||
530 |
3099 |
return BVH_OK; |
|
531 |
} |
||
532 |
|||
533 |
159 |
int BVHModelBase::beginReplaceModel() { |
|
534 |
✗✓ | 159 |
if (build_state != BVH_BUILD_STATE_PROCESSED) { |
535 |
std::cerr << "BVH Error! Call beginReplaceModel() on a BVHModel that has " |
||
536 |
"no previous frame." |
||
537 |
<< std::endl; |
||
538 |
return BVH_ERR_BUILD_EMPTY_PREVIOUS_FRAME; |
||
539 |
} |
||
540 |
|||
541 |
✗✓✗✗ |
159 |
if (prev_vertices) delete[] prev_vertices; |
542 |
159 |
prev_vertices = NULL; |
|
543 |
|||
544 |
159 |
num_vertex_updated = 0; |
|
545 |
|||
546 |
159 |
build_state = BVH_BUILD_STATE_REPLACE_BEGUN; |
|
547 |
|||
548 |
159 |
return BVH_OK; |
|
549 |
} |
||
550 |
|||
551 |
int BVHModelBase::replaceVertex(const Vec3f& p) { |
||
552 |
if (build_state != BVH_BUILD_STATE_REPLACE_BEGUN) { |
||
553 |
std::cerr << "BVH Warning! Call replaceVertex() in a wrong order. " |
||
554 |
"replaceVertex() was ignored. Must do a beginReplaceModel() " |
||
555 |
"for initialization." |
||
556 |
<< std::endl; |
||
557 |
return BVH_ERR_BUILD_OUT_OF_SEQUENCE; |
||
558 |
} |
||
559 |
|||
560 |
vertices[num_vertex_updated] = p; |
||
561 |
num_vertex_updated++; |
||
562 |
|||
563 |
return BVH_OK; |
||
564 |
} |
||
565 |
|||
566 |
int BVHModelBase::replaceTriangle(const Vec3f& p1, const Vec3f& p2, |
||
567 |
const Vec3f& p3) { |
||
568 |
if (build_state != BVH_BUILD_STATE_REPLACE_BEGUN) { |
||
569 |
std::cerr << "BVH Warning! Call replaceTriangle() in a wrong order. " |
||
570 |
"replaceTriangle() was ignored. Must do a beginReplaceModel() " |
||
571 |
"for initialization." |
||
572 |
<< std::endl; |
||
573 |
return BVH_ERR_BUILD_OUT_OF_SEQUENCE; |
||
574 |
} |
||
575 |
|||
576 |
vertices[num_vertex_updated] = p1; |
||
577 |
num_vertex_updated++; |
||
578 |
vertices[num_vertex_updated] = p2; |
||
579 |
num_vertex_updated++; |
||
580 |
vertices[num_vertex_updated] = p3; |
||
581 |
num_vertex_updated++; |
||
582 |
return BVH_OK; |
||
583 |
} |
||
584 |
|||
585 |
159 |
int BVHModelBase::replaceSubModel(const std::vector<Vec3f>& ps) { |
|
586 |
✗✓ | 159 |
if (build_state != BVH_BUILD_STATE_REPLACE_BEGUN) { |
587 |
std::cerr << "BVH Warning! Call replaceSubModel() in a wrong order. " |
||
588 |
"replaceSubModel() was ignored. Must do a beginReplaceModel() " |
||
589 |
"for initialization." |
||
590 |
<< std::endl; |
||
591 |
return BVH_ERR_BUILD_OUT_OF_SEQUENCE; |
||
592 |
} |
||
593 |
|||
594 |
✓✓ | 745863 |
for (unsigned int i = 0; i < ps.size(); ++i) { |
595 |
745704 |
vertices[num_vertex_updated] = ps[i]; |
|
596 |
745704 |
num_vertex_updated++; |
|
597 |
} |
||
598 |
159 |
return BVH_OK; |
|
599 |
} |
||
600 |
|||
601 |
159 |
int BVHModelBase::endReplaceModel(bool refit, bool bottomup) { |
|
602 |
✗✓ | 159 |
if (build_state != BVH_BUILD_STATE_REPLACE_BEGUN) { |
603 |
std::cerr << "BVH Warning! Call endReplaceModel() in a wrong order. " |
||
604 |
"endReplaceModel() was ignored. " |
||
605 |
<< std::endl; |
||
606 |
return BVH_ERR_BUILD_OUT_OF_SEQUENCE; |
||
607 |
} |
||
608 |
|||
609 |
✗✓ | 159 |
if (num_vertex_updated != num_vertices) { |
610 |
std::cerr << "BVH Error! The replaced model should have the same number of " |
||
611 |
"vertices as the old model." |
||
612 |
<< std::endl; |
||
613 |
return BVH_ERR_INCORRECT_DATA; |
||
614 |
} |
||
615 |
|||
616 |
✓✓ | 159 |
if (refit) // refit, do not change BVH structure |
617 |
{ |
||
618 |
89 |
refitTree(bottomup); |
|
619 |
} else // reconstruct bvh tree based on current frame data |
||
620 |
{ |
||
621 |
70 |
buildTree(); |
|
622 |
} |
||
623 |
|||
624 |
159 |
build_state = BVH_BUILD_STATE_PROCESSED; |
|
625 |
|||
626 |
159 |
return BVH_OK; |
|
627 |
} |
||
628 |
|||
629 |
int BVHModelBase::beginUpdateModel() { |
||
630 |
if (build_state != BVH_BUILD_STATE_PROCESSED && |
||
631 |
build_state != BVH_BUILD_STATE_UPDATED) { |
||
632 |
std::cerr << "BVH Error! Call beginUpdatemodel() on a BVHModel that has no " |
||
633 |
"previous frame." |
||
634 |
<< std::endl; |
||
635 |
return BVH_ERR_BUILD_EMPTY_PREVIOUS_FRAME; |
||
636 |
} |
||
637 |
|||
638 |
if (prev_vertices) { |
||
639 |
Vec3f* temp = prev_vertices; |
||
640 |
prev_vertices = vertices; |
||
641 |
vertices = temp; |
||
642 |
} else { |
||
643 |
prev_vertices = vertices; |
||
644 |
vertices = new Vec3f[num_vertices]; |
||
645 |
} |
||
646 |
|||
647 |
num_vertex_updated = 0; |
||
648 |
|||
649 |
build_state = BVH_BUILD_STATE_UPDATE_BEGUN; |
||
650 |
|||
651 |
return BVH_OK; |
||
652 |
} |
||
653 |
|||
654 |
int BVHModelBase::updateVertex(const Vec3f& p) { |
||
655 |
if (build_state != BVH_BUILD_STATE_UPDATE_BEGUN) { |
||
656 |
std::cerr |
||
657 |
<< "BVH Warning! Call updateVertex() in a wrong order. updateVertex() " |
||
658 |
"was ignored. Must do a beginUpdateModel() for initialization." |
||
659 |
<< std::endl; |
||
660 |
return BVH_ERR_BUILD_OUT_OF_SEQUENCE; |
||
661 |
} |
||
662 |
|||
663 |
vertices[num_vertex_updated] = p; |
||
664 |
num_vertex_updated++; |
||
665 |
|||
666 |
return BVH_OK; |
||
667 |
} |
||
668 |
|||
669 |
int BVHModelBase::updateTriangle(const Vec3f& p1, const Vec3f& p2, |
||
670 |
const Vec3f& p3) { |
||
671 |
if (build_state != BVH_BUILD_STATE_UPDATE_BEGUN) { |
||
672 |
std::cerr << "BVH Warning! Call updateTriangle() in a wrong order. " |
||
673 |
"updateTriangle() was ignored. Must do a beginUpdateModel() " |
||
674 |
"for initialization." |
||
675 |
<< std::endl; |
||
676 |
return BVH_ERR_BUILD_OUT_OF_SEQUENCE; |
||
677 |
} |
||
678 |
|||
679 |
vertices[num_vertex_updated] = p1; |
||
680 |
num_vertex_updated++; |
||
681 |
vertices[num_vertex_updated] = p2; |
||
682 |
num_vertex_updated++; |
||
683 |
vertices[num_vertex_updated] = p3; |
||
684 |
num_vertex_updated++; |
||
685 |
return BVH_OK; |
||
686 |
} |
||
687 |
|||
688 |
int BVHModelBase::updateSubModel(const std::vector<Vec3f>& ps) { |
||
689 |
if (build_state != BVH_BUILD_STATE_UPDATE_BEGUN) { |
||
690 |
std::cerr << "BVH Warning! Call updateSubModel() in a wrong order. " |
||
691 |
"updateSubModel() was ignored. Must do a beginUpdateModel() " |
||
692 |
"for initialization." |
||
693 |
<< std::endl; |
||
694 |
return BVH_ERR_BUILD_OUT_OF_SEQUENCE; |
||
695 |
} |
||
696 |
|||
697 |
for (unsigned int i = 0; i < ps.size(); ++i) { |
||
698 |
vertices[num_vertex_updated] = ps[i]; |
||
699 |
num_vertex_updated++; |
||
700 |
} |
||
701 |
return BVH_OK; |
||
702 |
} |
||
703 |
|||
704 |
int BVHModelBase::endUpdateModel(bool refit, bool bottomup) { |
||
705 |
if (build_state != BVH_BUILD_STATE_UPDATE_BEGUN) { |
||
706 |
std::cerr << "BVH Warning! Call endUpdateModel() in a wrong order. " |
||
707 |
"endUpdateModel() was ignored. " |
||
708 |
<< std::endl; |
||
709 |
return BVH_ERR_BUILD_OUT_OF_SEQUENCE; |
||
710 |
} |
||
711 |
|||
712 |
if (num_vertex_updated != num_vertices) { |
||
713 |
std::cerr << "BVH Error! The updated model should have the same number of " |
||
714 |
"vertices as the old model." |
||
715 |
<< std::endl; |
||
716 |
return BVH_ERR_INCORRECT_DATA; |
||
717 |
} |
||
718 |
|||
719 |
if (refit) // refit, do not change BVH structure |
||
720 |
{ |
||
721 |
refitTree(bottomup); |
||
722 |
} else // reconstruct bvh tree based on current frame data |
||
723 |
{ |
||
724 |
buildTree(); |
||
725 |
|||
726 |
// then refit |
||
727 |
|||
728 |
refitTree(bottomup); |
||
729 |
} |
||
730 |
|||
731 |
build_state = BVH_BUILD_STATE_UPDATED; |
||
732 |
|||
733 |
return BVH_OK; |
||
734 |
} |
||
735 |
|||
736 |
8626 |
void BVHModelBase::computeLocalAABB() { |
|
737 |
✓✗ | 8626 |
AABB aabb_; |
738 |
✓✓ | 4114727 |
for (unsigned int i = 0; i < num_vertices; ++i) { |
739 |
✓✗ | 4106101 |
aabb_ += vertices[i]; |
740 |
} |
||
741 |
|||
742 |
✓✗ | 8626 |
aabb_center = aabb_.center(); |
743 |
|||
744 |
8626 |
aabb_radius = 0; |
|
745 |
✓✓ | 4114727 |
for (unsigned int i = 0; i < num_vertices; ++i) { |
746 |
✓✗✓✗ |
4106101 |
FCL_REAL r = (aabb_center - vertices[i]).squaredNorm(); |
747 |
✓✓ | 4106101 |
if (r > aabb_radius) aabb_radius = r; |
748 |
} |
||
749 |
|||
750 |
8626 |
aabb_radius = sqrt(aabb_radius); |
|
751 |
|||
752 |
✓✗ | 8626 |
aabb_local = aabb_; |
753 |
8626 |
} |
|
754 |
|||
755 |
/// @brief Constructing an empty BVH |
||
756 |
template <typename BV> |
||
757 |
6210 |
BVHModel<BV>::BVHModel() |
|
758 |
: BVHModelBase(), |
||
759 |
✓✗ | 6210 |
bv_splitter(new BVSplitter<BV>(SPLIT_METHOD_MEAN)), |
760 |
6210 |
bv_fitter(new BVFitter<BV>()), |
|
761 |
num_bvs_allocated(0), |
||
762 |
primitive_indices(NULL), |
||
763 |
bvs(NULL), |
||
764 |
✓✗✓✗ ✓✗✓✗ |
18630 |
num_bvs(0) {} |
765 |
|||
766 |
template <typename BV> |
||
767 |
6 |
void BVHModel<BV>::deleteBVs() { |
|
768 |
✓✗ | 6 |
delete[] bvs; |
769 |
6 |
bvs = NULL; |
|
770 |
✓✗ | 6 |
delete[] primitive_indices; |
771 |
6 |
primitive_indices = NULL; |
|
772 |
6 |
num_bvs_allocated = num_bvs = 0; |
|
773 |
} |
||
774 |
|||
775 |
template <typename BV> |
||
776 |
6198 |
bool BVHModel<BV>::allocateBVs() { |
|
777 |
// construct BVH tree |
||
778 |
6198 |
unsigned int num_bvs_to_be_allocated = 0; |
|
779 |
✓✓ | 6198 |
if (num_tris == 0) |
780 |
16 |
num_bvs_to_be_allocated = 2 * num_vertices - 1; |
|
781 |
else |
||
782 |
6182 |
num_bvs_to_be_allocated = 2 * num_tris - 1; |
|
783 |
|||
784 |
✓✗✓✓ ✓✗ |
3883644 |
bvs = new BVNode<BV>[num_bvs_to_be_allocated]; |
785 |
✓✗ | 6198 |
primitive_indices = new unsigned int[num_bvs_to_be_allocated]; |
786 |
✓✗✗✓ |
6198 |
if (!bvs || !primitive_indices) { |
787 |
std::cerr << "BVH Error! Out of memory for BV array in endModel()!" |
||
788 |
<< std::endl; |
||
789 |
return false; |
||
790 |
} |
||
791 |
6198 |
num_bvs_allocated = num_bvs_to_be_allocated; |
|
792 |
6198 |
num_bvs = 0; |
|
793 |
6198 |
return true; |
|
794 |
} |
||
795 |
|||
796 |
template <typename BV> |
||
797 |
8 |
int BVHModel<BV>::memUsage(const bool msg) const { |
|
798 |
8 |
unsigned int mem_bv_list = (unsigned int)sizeof(BV) * num_bvs; |
|
799 |
8 |
unsigned int mem_tri_list = (unsigned int)sizeof(Triangle) * num_tris; |
|
800 |
8 |
unsigned int mem_vertex_list = (unsigned int)sizeof(Vec3f) * num_vertices; |
|
801 |
|||
802 |
8 |
unsigned int total_mem = mem_bv_list + mem_tri_list + mem_vertex_list + |
|
803 |
(unsigned int)sizeof(BVHModel<BV>); |
||
804 |
✗✓ | 8 |
if (msg) { |
805 |
std::cerr << "Total for model " << total_mem << " bytes." << std::endl; |
||
806 |
std::cerr << "BVs: " << num_bvs << " allocated." << std::endl; |
||
807 |
std::cerr << "Tris: " << num_tris << " allocated." << std::endl; |
||
808 |
std::cerr << "Vertices: " << num_vertices << " allocated." << std::endl; |
||
809 |
} |
||
810 |
|||
811 |
8 |
return static_cast<int>(total_mem); |
|
812 |
} |
||
813 |
|||
814 |
template <typename BV> |
||
815 |
6338 |
int BVHModel<BV>::buildTree() { |
|
816 |
// set BVFitter |
||
817 |
6338 |
bv_fitter->set(vertices, tri_indices, getModelType()); |
|
818 |
// set SplitRule |
||
819 |
6338 |
bv_splitter->set(vertices, tri_indices, getModelType()); |
|
820 |
|||
821 |
6338 |
num_bvs = 1; |
|
822 |
|||
823 |
6338 |
unsigned int num_primitives = 0; |
|
824 |
✓✓✗ | 6338 |
switch (getModelType()) { |
825 |
6322 |
case BVH_MODEL_TRIANGLES: |
|
826 |
6322 |
num_primitives = (unsigned int)num_tris; |
|
827 |
6322 |
break; |
|
828 |
16 |
case BVH_MODEL_POINTCLOUD: |
|
829 |
16 |
num_primitives = (unsigned int)num_vertices; |
|
830 |
16 |
break; |
|
831 |
default: |
||
832 |
std::cerr << "BVH Error: Model type not supported!" << std::endl; |
||
833 |
return BVH_ERR_UNSUPPORTED_FUNCTION; |
||
834 |
} |
||
835 |
|||
836 |
✓✓ | 2253360 |
for (unsigned int i = 0; i < num_primitives; ++i) primitive_indices[i] = i; |
837 |
6338 |
recursiveBuildTree(0, 0, num_primitives); |
|
838 |
|||
839 |
6338 |
bv_fitter->clear(); |
|
840 |
6338 |
bv_splitter->clear(); |
|
841 |
|||
842 |
6338 |
return BVH_OK; |
|
843 |
} |
||
844 |
|||
845 |
template <typename BV> |
||
846 |
4487706 |
int BVHModel<BV>::recursiveBuildTree(int bv_id, unsigned int first_primitive, |
|
847 |
unsigned int num_primitives) { |
||
848 |
4487706 |
BVHModelType type = getModelType(); |
|
849 |
4487706 |
BVNode<BV>* bvnode = bvs + bv_id; |
|
850 |
4487706 |
unsigned int* cur_primitive_indices = primitive_indices + first_primitive; |
|
851 |
|||
852 |
// constructing BV |
||
853 |
✓✗ | 4487706 |
BV bv = bv_fitter->fit(cur_primitive_indices, num_primitives); |
854 |
✓✗ | 4487706 |
bv_splitter->computeRule(bv, cur_primitive_indices, num_primitives); |
855 |
|||
856 |
✓✗ | 4487706 |
bvnode->bv = bv; |
857 |
4487706 |
bvnode->first_primitive = first_primitive; |
|
858 |
4487706 |
bvnode->num_primitives = num_primitives; |
|
859 |
|||
860 |
✓✓ | 4487706 |
if (num_primitives == 1) { |
861 |
2247022 |
bvnode->first_child = -((int)(*cur_primitive_indices) + 1); |
|
862 |
} else { |
||
863 |
2240684 |
bvnode->first_child = (int)num_bvs; |
|
864 |
2240684 |
num_bvs += 2; |
|
865 |
|||
866 |
2240684 |
unsigned int c1 = 0; |
|
867 |
✓✓ | 26214168 |
for (unsigned int i = 0; i < num_primitives; ++i) { |
868 |
✓✗ | 23973484 |
Vec3f p; |
869 |
✓✓ | 23973484 |
if (type == BVH_MODEL_POINTCLOUD) |
870 |
✓✗ | 384 |
p = vertices[cur_primitive_indices[i]]; |
871 |
✓✗ | 23973100 |
else if (type == BVH_MODEL_TRIANGLES) { |
872 |
23973100 |
const Triangle& t = tri_indices[cur_primitive_indices[i]]; |
|
873 |
23973100 |
const Vec3f& p1 = vertices[t[0]]; |
|
874 |
23973100 |
const Vec3f& p2 = vertices[t[1]]; |
|
875 |
23973100 |
const Vec3f& p3 = vertices[t[2]]; |
|
876 |
|||
877 |
✓✗✓✗ ✓✗✓✗ |
23973100 |
p = (p1 + p2 + p3) / 3.; |
878 |
} else { |
||
879 |
std::cerr << "BVH Error: Model type not supported!" << std::endl; |
||
880 |
return BVH_ERR_UNSUPPORTED_FUNCTION; |
||
881 |
} |
||
882 |
|||
883 |
// loop invariant: up to (but not including) index c1 in group 1, |
||
884 |
// then up to (but not including) index i in group 2 |
||
885 |
// |
||
886 |
// [1] [1] [1] [1] [2] [2] [2] [x] [x] ... [x] |
||
887 |
// c1 i |
||
888 |
// |
||
889 |
✓✗✓✓ |
23973484 |
if (bv_splitter->apply(p)) // in the right side |
890 |
{ |
||
891 |
// do nothing |
||
892 |
} else { |
||
893 |
11799394 |
unsigned int temp = cur_primitive_indices[i]; |
|
894 |
11799394 |
cur_primitive_indices[i] = cur_primitive_indices[c1]; |
|
895 |
11799394 |
cur_primitive_indices[c1] = temp; |
|
896 |
11799394 |
c1++; |
|
897 |
} |
||
898 |
} |
||
899 |
|||
900 |
✓✓✓✓ |
2240684 |
if ((c1 == 0) || (c1 == num_primitives)) c1 = num_primitives / 2; |
901 |
|||
902 |
2240684 |
const unsigned int num_first_half = c1; |
|
903 |
|||
904 |
✓✗ | 2240684 |
recursiveBuildTree(bvnode->leftChild(), first_primitive, num_first_half); |
905 |
✓✗ | 2240684 |
recursiveBuildTree(bvnode->rightChild(), first_primitive + num_first_half, |
906 |
num_primitives - num_first_half); |
||
907 |
} |
||
908 |
|||
909 |
4487706 |
return BVH_OK; |
|
910 |
} |
||
911 |
|||
912 |
template <typename BV> |
||
913 |
178 |
int BVHModel<BV>::refitTree(bool bottomup) { |
|
914 |
✓✓ | 178 |
if (bottomup) |
915 |
42 |
return refitTree_bottomup(); |
|
916 |
else |
||
917 |
136 |
return refitTree_topdown(); |
|
918 |
} |
||
919 |
|||
920 |
template <typename BV> |
||
921 |
42 |
int BVHModel<BV>::refitTree_bottomup() { |
|
922 |
// TODO the recomputation of the BV is done manually, without using |
||
923 |
// bv_fitter. The manual BV recomputation seems bugged. Using bv_fitter |
||
924 |
// seems to correct the bug. |
||
925 |
// bv_fitter->set(vertices, tri_indices, getModelType()); |
||
926 |
|||
927 |
42 |
int res = recursiveRefitTree_bottomup(0); |
|
928 |
|||
929 |
// bv_fitter->clear(); |
||
930 |
42 |
return res; |
|
931 |
} |
||
932 |
|||
933 |
template <typename BV> |
||
934 |
183078 |
int BVHModel<BV>::recursiveRefitTree_bottomup(int bv_id) { |
|
935 |
183078 |
BVNode<BV>* bvnode = bvs + bv_id; |
|
936 |
✓✓ | 183078 |
if (bvnode->isLeaf()) { |
937 |
91560 |
BVHModelType type = getModelType(); |
|
938 |
91560 |
int primitive_id = -(bvnode->first_child + 1); |
|
939 |
✗✓ | 91560 |
if (type == BVH_MODEL_POINTCLOUD) { |
940 |
BV bv; |
||
941 |
|||
942 |
if (prev_vertices) { |
||
943 |
Vec3f v[2]; |
||
944 |
v[0] = prev_vertices[primitive_id]; |
||
945 |
v[1] = vertices[primitive_id]; |
||
946 |
fit(v, 2, bv); |
||
947 |
} else |
||
948 |
fit(vertices + primitive_id, 1, bv); |
||
949 |
|||
950 |
bvnode->bv = bv; |
||
951 |
✓✗ | 91560 |
} else if (type == BVH_MODEL_TRIANGLES) { |
952 |
✓✗ | 91560 |
BV bv; |
953 |
91560 |
const Triangle& triangle = tri_indices[primitive_id]; |
|
954 |
|||
955 |
✗✓ | 91560 |
if (prev_vertices) { |
956 |
Vec3f v[6]; |
||
957 |
for (Triangle::index_type i = 0; i < 3; ++i) { |
||
958 |
v[i] = prev_vertices[triangle[i]]; |
||
959 |
v[i + 3] = vertices[triangle[i]]; |
||
960 |
} |
||
961 |
|||
962 |
fit(v, 6, bv); |
||
963 |
} else { |
||
964 |
// TODO use bv_fitter to build BV. See comment in refitTree_bottomup |
||
965 |
// unsigned int* cur_primitive_indices = primitive_indices + |
||
966 |
// bvnode->first_primitive; bv = bv_fitter->fit(cur_primitive_indices, |
||
967 |
// bvnode->num_primitives); |
||
968 |
✓✓✓✗ |
366240 |
Vec3f v[3]; |
969 |
✓✓ | 366240 |
for (int i = 0; i < 3; ++i) { |
970 |
✓✗ | 274680 |
v[i] = vertices[triangle[(Triangle::index_type)i]]; |
971 |
} |
||
972 |
|||
973 |
✓✗ | 91560 |
fit(v, 3, bv); |
974 |
} |
||
975 |
|||
976 |
✓✗ | 91560 |
bvnode->bv = bv; |
977 |
} else { |
||
978 |
std::cerr << "BVH Error: Model type not supported!" << std::endl; |
||
979 |
return BVH_ERR_UNSUPPORTED_FUNCTION; |
||
980 |
} |
||
981 |
} else { |
||
982 |
91518 |
recursiveRefitTree_bottomup(bvnode->leftChild()); |
|
983 |
91518 |
recursiveRefitTree_bottomup(bvnode->rightChild()); |
|
984 |
✗✗ | 91518 |
bvnode->bv = bvs[bvnode->leftChild()].bv + bvs[bvnode->rightChild()].bv; |
985 |
// TODO use bv_fitter to build BV. See comment in refitTree_bottomup |
||
986 |
// unsigned int* cur_primitive_indices = primitive_indices + |
||
987 |
// bvnode->first_primitive; bvnode->bv = |
||
988 |
// bv_fitter->fit(cur_primitive_indices, bvnode->num_primitives); |
||
989 |
} |
||
990 |
|||
991 |
183078 |
return BVH_OK; |
|
992 |
} |
||
993 |
|||
994 |
template <typename BV> |
||
995 |
136 |
int BVHModel<BV>::refitTree_topdown() { |
|
996 |
136 |
bv_fitter->set(vertices, prev_vertices, tri_indices, getModelType()); |
|
997 |
✓✓ | 325856 |
for (unsigned int i = 0; i < num_bvs; ++i) { |
998 |
325720 |
BV bv = bv_fitter->fit(primitive_indices + bvs[i].first_primitive, |
|
999 |
✓✗ | 325720 |
bvs[i].num_primitives); |
1000 |
✓✗ | 325720 |
bvs[i].bv = bv; |
1001 |
} |
||
1002 |
|||
1003 |
136 |
bv_fitter->clear(); |
|
1004 |
|||
1005 |
136 |
return BVH_OK; |
|
1006 |
} |
||
1007 |
|||
1008 |
template <> |
||
1009 |
void BVHModel<OBB>::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes, |
||
1010 |
const Vec3f& parent_c) { |
||
1011 |
OBB& obb = bvs[bv_id].bv; |
||
1012 |
if (!bvs[bv_id].isLeaf()) { |
||
1013 |
makeParentRelativeRecurse(bvs[bv_id].first_child, obb.axes, obb.To); |
||
1014 |
|||
1015 |
makeParentRelativeRecurse(bvs[bv_id].first_child + 1, obb.axes, obb.To); |
||
1016 |
} |
||
1017 |
|||
1018 |
// make self parent relative |
||
1019 |
// obb.axes = parent_axes.transpose() * obb.axes; |
||
1020 |
obb.axes.applyOnTheLeft(parent_axes.transpose()); |
||
1021 |
|||
1022 |
Vec3f t(obb.To - parent_c); |
||
1023 |
obb.To.noalias() = parent_axes.transpose() * t; |
||
1024 |
} |
||
1025 |
|||
1026 |
template <> |
||
1027 |
void BVHModel<RSS>::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes, |
||
1028 |
const Vec3f& parent_c) { |
||
1029 |
RSS& rss = bvs[bv_id].bv; |
||
1030 |
if (!bvs[bv_id].isLeaf()) { |
||
1031 |
makeParentRelativeRecurse(bvs[bv_id].first_child, rss.axes, rss.Tr); |
||
1032 |
|||
1033 |
makeParentRelativeRecurse(bvs[bv_id].first_child + 1, rss.axes, rss.Tr); |
||
1034 |
} |
||
1035 |
|||
1036 |
// make self parent relative |
||
1037 |
// rss.axes = parent_axes.transpose() * rss.axes; |
||
1038 |
rss.axes.applyOnTheLeft(parent_axes.transpose()); |
||
1039 |
|||
1040 |
Vec3f t(rss.Tr - parent_c); |
||
1041 |
rss.Tr.noalias() = parent_axes.transpose() * t; |
||
1042 |
} |
||
1043 |
|||
1044 |
template <> |
||
1045 |
void BVHModel<OBBRSS>::makeParentRelativeRecurse(int bv_id, |
||
1046 |
Matrix3f& parent_axes, |
||
1047 |
const Vec3f& parent_c) { |
||
1048 |
OBB& obb = bvs[bv_id].bv.obb; |
||
1049 |
RSS& rss = bvs[bv_id].bv.rss; |
||
1050 |
if (!bvs[bv_id].isLeaf()) { |
||
1051 |
makeParentRelativeRecurse(bvs[bv_id].first_child, obb.axes, obb.To); |
||
1052 |
|||
1053 |
makeParentRelativeRecurse(bvs[bv_id].first_child + 1, obb.axes, obb.To); |
||
1054 |
} |
||
1055 |
|||
1056 |
// make self parent relative |
||
1057 |
rss.axes.noalias() = parent_axes.transpose() * obb.axes; |
||
1058 |
obb.axes = rss.axes; |
||
1059 |
|||
1060 |
Vec3f t(obb.To - parent_c); |
||
1061 |
obb.To.noalias() = parent_axes.transpose() * t; |
||
1062 |
rss.Tr = obb.To; |
||
1063 |
} |
||
1064 |
|||
1065 |
template <> |
||
1066 |
3 |
NODE_TYPE BVHModel<AABB>::getNodeType() const { |
|
1067 |
3 |
return BV_AABB; |
|
1068 |
} |
||
1069 |
|||
1070 |
template <> |
||
1071 |
43 |
NODE_TYPE BVHModel<OBB>::getNodeType() const { |
|
1072 |
43 |
return BV_OBB; |
|
1073 |
} |
||
1074 |
|||
1075 |
template <> |
||
1076 |
7 |
NODE_TYPE BVHModel<RSS>::getNodeType() const { |
|
1077 |
7 |
return BV_RSS; |
|
1078 |
} |
||
1079 |
|||
1080 |
template <> |
||
1081 |
7 |
NODE_TYPE BVHModel<kIOS>::getNodeType() const { |
|
1082 |
7 |
return BV_kIOS; |
|
1083 |
} |
||
1084 |
|||
1085 |
template <> |
||
1086 |
34357 |
NODE_TYPE BVHModel<OBBRSS>::getNodeType() const { |
|
1087 |
34357 |
return BV_OBBRSS; |
|
1088 |
} |
||
1089 |
|||
1090 |
template <> |
||
1091 |
5 |
NODE_TYPE BVHModel<KDOP<16> >::getNodeType() const { |
|
1092 |
5 |
return BV_KDOP16; |
|
1093 |
} |
||
1094 |
|||
1095 |
template <> |
||
1096 |
7 |
NODE_TYPE BVHModel<KDOP<18> >::getNodeType() const { |
|
1097 |
7 |
return BV_KDOP18; |
|
1098 |
} |
||
1099 |
|||
1100 |
template <> |
||
1101 |
9 |
NODE_TYPE BVHModel<KDOP<24> >::getNodeType() const { |
|
1102 |
9 |
return BV_KDOP24; |
|
1103 |
} |
||
1104 |
|||
1105 |
template class BVHModel<KDOP<16> >; |
||
1106 |
template class BVHModel<KDOP<18> >; |
||
1107 |
template class BVHModel<KDOP<24> >; |
||
1108 |
template class BVHModel<OBB>; |
||
1109 |
template class BVHModel<AABB>; |
||
1110 |
template class BVHModel<RSS>; |
||
1111 |
template class BVHModel<kIOS>; |
||
1112 |
template class BVHModel<OBBRSS>; |
||
1113 |
|||
1114 |
} // namespace fcl |
||
1115 |
|||
1116 |
} // namespace hpp |
Generated by: GCOVR (Version 4.2) |