PeriDyno 1.0.0
Loading...
Searching...
No Matches
VoxelOctree.cu
Go to the documentation of this file.
1#include "VoxelOctree.h"
2#include "Algorithm/Reduction.h"
3
4#include <fstream>
5#include <iostream>
6#include <sstream>
7#include <limits>
8#include <thrust/sort.h>
9
10namespace dyno
11{
12 IMPLEMENT_TCLASS(VoxelOctree, TDataType)
13
14 template<typename TDataType>
15 VoxelOctree<TDataType>::VoxelOctree()
16 : TopologyModule()
17 {
18 }
19
20 template<typename TDataType>
21 VoxelOctree<TDataType>::~VoxelOctree()
22 {
23 }
24
25 template<typename TDataType>
26 void VoxelOctree<TDataType>::setVoxelOctree(DArray<VoxelOctreeNode<Coord>>& oct)
27 {
28 m_octree.resize(oct.size());
29 m_octree.assign(oct);
30
31 tagAsChanged();
32 }
33
34 template <typename Coord>
35 __global__ void SO_CountLeafs(
36 DArray<int> leafs,
37 DArray<VoxelOctreeNode<Coord>> octree)
38 {
39 int tId = threadIdx.x + (blockIdx.x * blockDim.x);
40
41 if (tId >= leafs.size()) return;
42
43 if (!octree[tId].midside())
44 leafs[tId] = 1;
45 }
46
47 template <typename Coord>
48 __global__ void SO_ComputeLeafs(
49 DArray<Coord> leafs_pos,
50 DArray<int> leafs_sdf,
51 DArray<int> leafs,
52 DArray<VoxelOctreeNode<Coord>> octree)
53 {
54 int tId = threadIdx.x + (blockIdx.x * blockDim.x);
55
56 if (tId >= leafs.size()) return;
57
58 if (!octree[tId].midside())
59 {
60 leafs_pos[leafs[tId]] = octree[tId].position();
61 //leafs_sdf[leafs[tId]] = octree[tId].value();
62 leafs_sdf[leafs[tId]] = tId;
63 }
64 //printf("voxelOctree: tId pos %d %f %f %f \n",tId, leafs_pos[leafs[tId]][0], leafs_pos[leafs[tId]][1], leafs_pos[leafs[tId]][2]);
65 }
66
67 template<typename TDataType>
68 void VoxelOctree<TDataType>::getLeafs(DArray<Coord>& pos, DArray<int>& pos_pos)
69 {
70 DArray<int> points_count;
71 points_count.resize(m_octree.size());
72 points_count.reset();
73 //数叶子节点的个数
74 cuExecute(points_count.size(),
75 SO_CountLeafs,
76 points_count,
77 m_octree);
78
79 int leafs_num = thrust::reduce(thrust::device, points_count.begin(), points_count.begin() + points_count.size(), (int)0, thrust::plus<int>());
80 thrust::exclusive_scan(thrust::device, points_count.begin(), points_count.begin() + points_count.size(), points_count.begin());
81 std::printf("GetLeafs: the number of leafs is: %d \n", leafs_num);
82
83 DArray<Coord> points_pos;
84 points_pos.resize(leafs_num);
85 DArray<int> points_sdf;
86 points_sdf.resize(leafs_num);
87 //取叶节点的坐标
88 cuExecute(points_count.size(),
89 SO_ComputeLeafs,
90 points_pos,
91 points_sdf,
92 points_count,
93 m_octree);
94
95 pos.assign(points_pos);
96 pos_pos.assign(points_sdf);
97
98 points_count.clear();
99 points_pos.clear();
100 points_sdf.clear();
101 }
102
103 template <typename Real, typename Coord>
104 __global__ void VO_ComputeVertices(
105 DArray<Coord> vertices,
106 DArray<Coord> centers,
107 DArray<int> leaves,
108 DArray<VoxelOctreeNode<Coord>> octree,
109 Real dx)
110 {
111 int tId = threadIdx.x + (blockIdx.x * blockDim.x);
112 if (tId >= centers.size()) return;
113 Real coef0 = Real(pow(Real(2), int(octree[leaves[tId]].level())));
114 Real h = coef0 * dx;
115
116 Coord p = centers[tId] - 0.5 * h;
117 vertices[8*tId] = p;
118 vertices[8*tId + 1] = p + Coord(h, 0, 0);
119 vertices[8*tId + 2] = p + Coord(h, h, 0);
120 vertices[8*tId + 3] = p + Coord(0, h, 0);
121 vertices[8*tId + 4] = p + Coord(0, 0, h);
122 vertices[8*tId + 5] = p + Coord(h, 0, h);
123 vertices[8*tId + 6] = p + Coord(h, h, h);
124 vertices[8*tId + 7] = p + Coord(0, h, h);
125 }
126
127 template<typename TDataType>
128 void VoxelOctree<TDataType>::getCellVertices(DArray<Coord>& pos)
129 {
130 DArray<Coord> centers;
131 DArray<int> leaves;
132 this->getLeafs(centers, leaves);
133 uint cellSize = centers.size();
134 pos.resize(8 * cellSize);
135
136 cuExecute(cellSize,
137 VO_ComputeVertices,
138 pos,
139 centers,
140 leaves,
141 m_octree,
142 m_dx);
143 centers.clear();
144 leaves.clear();
145 }
146
147 template <typename Real, typename Coord>
148 __global__ void VO_ComputeVertices0(
149 DArray<Coord> vertices,
150 DArray<VoxelOctreeNode<Coord>> octree,
151 Real dx,
152 int bottom_size)
153 {
154 int tId = threadIdx.x + (blockIdx.x * blockDim.x);
155
156 if (tId >= bottom_size) return;
157
158 Coord p = octree[tId].position() - 0.5 * dx;
159
160 vertices[8 * tId] = p;
161 vertices[8 * tId + 1] = p + Coord(dx, 0, 0);
162 vertices[8 * tId + 2] = p + Coord(dx, dx, 0);
163 vertices[8 * tId + 3] = p + Coord(0, dx, 0);
164 vertices[8 * tId + 4] = p + Coord(0, 0, dx);
165 vertices[8 * tId + 5] = p + Coord(dx, 0, dx);
166 vertices[8 * tId + 6] = p + Coord(dx, dx, dx);
167 vertices[8 * tId + 7] = p + Coord(0, dx, dx);
168 }
169
170 template<typename TDataType>
171 void VoxelOctree<TDataType>::getCellVertices0(DArray<Coord>& pos)
172 {
173 pos.resize(8 * m_level0);
174
175 cuExecute(m_level0,
176 VO_ComputeVertices0,
177 pos,
178 m_octree,
179 m_dx,
180 m_level0);
181 }
182
183 template <typename Real, typename Coord>
184 __global__ void VO_ComputeVertices1(
185 DArray<Coord> vertices,
186 int nx,
187 int ny,
188 int nz,
189 Real dx,
190 Coord origin)
191 {
192 int tId = threadIdx.x + (blockIdx.x * blockDim.x);
193
194 if (tId >= vertices.size()) return;
195
196 int i = tId % nx;
197 int j = ((tId - i) / nx) % ny;
198 int k = (tId - i - j * nx) / (nx*ny);
199
200 Coord p = origin + Coord(i*dx, j*dx, k*dx);
201
202 vertices[8 * tId] = p;
203 vertices[8 * tId + 1] = p + Coord(dx, 0, 0);
204 vertices[8 * tId + 2] = p + Coord(dx, dx, 0);
205 vertices[8 * tId + 3] = p + Coord(0, dx, 0);
206 vertices[8 * tId + 4] = p + Coord(0, 0, dx);
207 vertices[8 * tId + 5] = p + Coord(dx, 0, dx);
208 vertices[8 * tId + 6] = p + Coord(dx, dx, dx);
209 vertices[8 * tId + 7] = p + Coord(0, dx, dx);
210
211 //printf("point: %d %d %d, %d, %d %d %d, %f %f %f \n", nx, ny, nz, tId, i, j, k, p[0], p[1], p[2]);
212 }
213
214 template<typename TDataType>
215 void VoxelOctree<TDataType>::getCellVertices1(DArray<Coord>& pos)
216 {
217 int num = m_nx * m_ny*m_nz;
218 pos.resize(8 * num);
219
220 cuExecute(num,
221 VO_ComputeVertices1,
222 pos,
223 m_nx,
224 m_ny,
225 m_nz,
226 m_dx,
227 m_origin);
228 }
229
230 template <typename Coord>
231 __global__ void VO_CountVertices2(
232 DArray<int> count,
233 DArray<VoxelOctreeNode<Coord>> octree,
234 int bottom_size)
235 {
236 int tId = threadIdx.x + (blockIdx.x * blockDim.x);
237
238 if (tId >= bottom_size) return;
239
240 int x1y0z0 = octree[tId].m_neighbor[1];
241 if (x1y0z0 == EMPTY) return;
242
243 int x0y1z0 = octree[tId].m_neighbor[3];
244 if (x0y1z0 == EMPTY) return;
245
246 int x0y0z1 = octree[tId].m_neighbor[5];
247 if (x0y0z1 == EMPTY) return;
248
249 int x1y1z0=octree[x1y0z0].m_neighbor[3];
250 if (x1y1z0 == EMPTY) return;
251
252 int x1y0z1 = octree[x1y0z0].m_neighbor[5];
253 if (x1y0z1 == EMPTY) return;
254
255 int x1y1z1 = octree[x1y1z0].m_neighbor[5];
256 if (x1y1z1 == EMPTY) return;
257
258 int x0y1z1 = octree[x0y0z1].m_neighbor[3];
259 if (x0y1z1 == EMPTY) return;
260
261 count[tId] = 1;
262 }
263
264 template <typename Coord>
265 __global__ void VO_ComputeVertices2(
266 DArray<Coord> vertices,
267 DArray<VoxelOctreeNode<Coord>> octree,
268 DArray<int> count)
269 {
270 int tId = threadIdx.x + (blockIdx.x * blockDim.x);
271
272 if (tId >= count.size()) return;
273
274 if (count[tId] == 0 || count[tId] != count[tId - 1])
275 {
276 int x1y0z0 = octree[tId].m_neighbor[1];
277 int x0y1z0 = octree[tId].m_neighbor[3];
278 int x0y0z1 = octree[tId].m_neighbor[5];
279 int x1y1z0 = octree[x1y0z0].m_neighbor[3];
280 int x1y0z1 = octree[x1y0z0].m_neighbor[5];
281 int x1y1z1 = octree[x1y1z0].m_neighbor[5];
282 int x0y1z1 = octree[x0y0z1].m_neighbor[3];
283
284 vertices[8 * count[tId]] = octree[tId].position();
285 vertices[8 * count[tId] + 1] = octree[x1y0z0].position();
286 vertices[8 * count[tId] + 2] = octree[x1y1z0].position();
287 vertices[8 * count[tId] + 3] = octree[x0y1z0].position();
288 vertices[8 * count[tId] + 4] = octree[x0y0z1].position();
289 vertices[8 * count[tId] + 5] = octree[x1y0z1].position();
290 vertices[8 * count[tId] + 6] = octree[x1y1z1].position();
291 vertices[8 * count[tId] + 7] = octree[x0y1z1].position();
292 }
293 }
294
295 template<typename TDataType>
296 void VoxelOctree<TDataType>::getCellVertices2(DArray<Coord>& pos)
297 {
298 DArray<int> count;
299 count.resize(m_level0);
300 cuExecute(m_level0,
301 VO_CountVertices2,
302 count,
303 m_octree,
304 m_level0);
305 int grid_num = thrust::reduce(thrust::device, count.begin(), count.begin() + count.size(), (int)0, thrust::plus<int>());
306 thrust::exclusive_scan(thrust::device, count.begin(), count.begin() + count.size(), count.begin());
307
308 printf("the grid number is: %d \n", grid_num);
309
310 pos.resize(8 * grid_num);
311 cuExecute(m_level0,
312 VO_ComputeVertices2,
313 pos,
314 m_octree,
315 count);
316 }
317
318 //axis_index: 0(x-1),1(x+1),2(y-1),3(y+1),4(z-1),5(z+1)
319 template <typename Coord>
320 GPU_FUNC void VO_NeighborIteration(
321 DArray<uint>& count,
322 DArray<VoxelOctreeNode<Coord>>& octree,
323 int& num,
324 int index,
325 int axis_index)
326 {
327 if (octree[index].midside() == true)
328 {
329 num += 4;
330 int child_index = octree[index].child();
331 if (axis_index == 0)
332 {
333 VO_NeighborIteration(count, octree, num, child_index + 6, 0);
334 VO_NeighborIteration(count, octree, num, child_index + 4, 0);
335 VO_NeighborIteration(count, octree, num, child_index + 2, 0);
336 VO_NeighborIteration(count, octree, num, child_index + 0, 0);
337 }
338 else if (axis_index == 1)
339 {
340 VO_NeighborIteration(count, octree, num, child_index + 7, 1);
341 VO_NeighborIteration(count, octree, num, child_index + 5, 1);
342 VO_NeighborIteration(count, octree, num, child_index + 3, 1);
343 VO_NeighborIteration(count, octree, num, child_index + 1, 1);
344 }
345 else if (axis_index == 2)
346 {
347 VO_NeighborIteration(count, octree, num, child_index + 5, 2);
348 VO_NeighborIteration(count, octree, num, child_index + 4, 2);
349 VO_NeighborIteration(count, octree, num, child_index + 1, 2);
350 VO_NeighborIteration(count, octree, num, child_index + 0, 2);
351 }
352 else if (axis_index == 3)
353 {
354 VO_NeighborIteration(count, octree, num, child_index + 7, 3);
355 VO_NeighborIteration(count, octree, num, child_index + 6, 3);
356 VO_NeighborIteration(count, octree, num, child_index + 3, 3);
357 VO_NeighborIteration(count, octree, num, child_index + 2, 3);
358 }
359 else if (axis_index == 4)
360 {
361 VO_NeighborIteration(count, octree, num, child_index + 3, 4);
362 VO_NeighborIteration(count, octree, num, child_index + 2, 4);
363 VO_NeighborIteration(count, octree, num, child_index + 1, 4);
364 VO_NeighborIteration(count, octree, num, child_index + 0, 4);
365 }
366 else if (axis_index == 5)
367 {
368 VO_NeighborIteration(count, octree, num, child_index + 7, 5);
369 VO_NeighborIteration(count, octree, num, child_index + 6, 5);
370 VO_NeighborIteration(count, octree, num, child_index + 5, 5);
371 VO_NeighborIteration(count, octree, num, child_index + 4, 5);
372 }
373 }
374 else
375 {
376 atomicAdd(&(count[index]), 1);
377 }
378 }
379
380 template <typename Coord>
381 __global__ void VO_CountNeighbors(
382 DArray<uint> count,
383 DArray<int> leafs,
384 DArray<VoxelOctreeNode<Coord>> octree)
385 {
386 int tId = threadIdx.x + (blockIdx.x * blockDim.x);
387
388 if (tId >= leafs.size()) return;
389
390 VoxelOctreeNode<Coord> node0 = octree[leafs[tId]];
391 int neighbor_num = 0;
392
393 for (int i = 0; i < 6; i++)
394 {
395 if (node0.m_neighbor[i] != EMPTY)
396 VO_NeighborIteration(count, octree, neighbor_num, node0.m_neighbor[i], i);
397 }
398
399 atomicAdd(&(count[leafs[tId]]), neighbor_num);
400 }
401
402 //axis_index: 0(x-1),1(x+1),2(y-1),3(y+1),4(z-1),5(z+1)
403 template <typename Coord>
404 GPU_FUNC void VO_GetNeighborIteration(
405 DArrayList<int>& neighbors,
406 DArray<VoxelOctreeNode<Coord>>& octree,
407 int index0,
408 int index,
409 int axis_index,
410 bool is_same_level)
411 {
412 if (octree[index].midside() == true)
413 {
414 int child_index = octree[index].child();
415 if (axis_index == 0)
416 {
417 VO_GetNeighborIteration(neighbors, octree, index0, child_index + 6, 0, false);
418 VO_GetNeighborIteration(neighbors, octree, index0, child_index + 4, 0, false);
419 VO_GetNeighborIteration(neighbors, octree, index0, child_index + 2, 0, false);
420 VO_GetNeighborIteration(neighbors, octree, index0, child_index + 0, 0, false);
421 }
422 else if (axis_index == 1)
423 {
424 VO_GetNeighborIteration(neighbors, octree, index0, child_index + 7, 1, false);
425 VO_GetNeighborIteration(neighbors, octree, index0, child_index + 5, 1, false);
426 VO_GetNeighborIteration(neighbors, octree, index0, child_index + 3, 1, false);
427 VO_GetNeighborIteration(neighbors, octree, index0, child_index + 1, 1, false);
428 }
429 else if (axis_index == 2)
430 {
431 VO_GetNeighborIteration(neighbors, octree, index0, child_index + 5, 2, false);
432 VO_GetNeighborIteration(neighbors, octree, index0, child_index + 4, 2, false);
433 VO_GetNeighborIteration(neighbors, octree, index0, child_index + 1, 2, false);
434 VO_GetNeighborIteration(neighbors, octree, index0, child_index + 0, 2, false);
435 }
436 else if (axis_index == 3)
437 {
438 VO_GetNeighborIteration(neighbors, octree, index0, child_index + 7, 3, false);
439 VO_GetNeighborIteration(neighbors, octree, index0, child_index + 6, 3, false);
440 VO_GetNeighborIteration(neighbors, octree, index0, child_index + 3, 3, false);
441 VO_GetNeighborIteration(neighbors, octree, index0, child_index + 2, 3, false);
442 }
443 else if (axis_index == 4)
444 {
445 VO_GetNeighborIteration(neighbors, octree, index0, child_index + 3, 4, false);
446 VO_GetNeighborIteration(neighbors, octree, index0, child_index + 2, 4, false);
447 VO_GetNeighborIteration(neighbors, octree, index0, child_index + 1, 4, false);
448 VO_GetNeighborIteration(neighbors, octree, index0, child_index + 0, 4, false);
449 }
450 else if (axis_index == 5)
451 {
452 VO_GetNeighborIteration(neighbors, octree, index0, child_index + 7, 5, false);
453 VO_GetNeighborIteration(neighbors, octree, index0, child_index + 6, 5, false);
454 VO_GetNeighborIteration(neighbors, octree, index0, child_index + 5, 5, false);
455 VO_GetNeighborIteration(neighbors, octree, index0, child_index + 4, 5, false);
456 }
457 }
458 else
459 {
460 if (is_same_level == false)
461 {
462 List<int>& list1 = neighbors[index0];
463 list1.atomicInsert(index);
464 }
465
466 List<int>& list2 = neighbors[index];
467 list2.atomicInsert(index0);
468 }
469 }
470
471 template <typename Coord>
472 __global__ void VO_GetNeighbors(
473 DArrayList<int> neighbors,
474 DArray<int> leafs,
475 DArray<VoxelOctreeNode<Coord>> octree)
476 {
477 int tId = threadIdx.x + (blockIdx.x * blockDim.x);
478
479 if (tId >= leafs.size()) return;
480
481 VoxelOctreeNode<Coord> node0 = octree[leafs[tId]];
482
483 for (int i = 0; i < 6; i++)
484 {
485 if (node0.m_neighbor[i] != EMPTY)
486 VO_GetNeighborIteration(neighbors, octree, leafs[tId], node0.m_neighbor[i], i, true);
487 }
488 }
489
490 template<typename TDataType>
491 void VoxelOctree<TDataType>::updateNeighbors()
492 {
493 //printf("Update neighbors!!!~~~ \n");
494
495 DArray<Coord> leafs;
496 DArray<int> leafs_index;
497
498 this->getLeafs(leafs, leafs_index);
499
500 DArray<uint> count;
501 count.resize(m_octree.size());
502 count.reset();
503
504 cuExecute(leafs.size(),
505 VO_CountNeighbors,
506 count,
507 leafs_index,
508 m_octree);
509
510 m_neighbors.resize(count);
511 cuExecute(leafs.size(),
512 VO_GetNeighbors,
513 m_neighbors,
514 leafs_index,
515 m_octree);
516
517 leafs.clear();
518 leafs_index.clear();
519 count.clear();
520 }
521
522 template <typename Real>
523 __global__ void SO_GetLeafsValue(
524 DArray<Real> leafs_sdf,
525 DArray<int> leafs_count,
526 DArray<Real> octree_value)
527 {
528 int tId = threadIdx.x + (blockIdx.x * blockDim.x);
529 if (tId >= leafs_count.size()) return;
530
531 leafs_sdf[tId] = octree_value[leafs_count[tId]];
532 }
533
534 template<typename TDataType>
535 void VoxelOctree<TDataType>::setSdfValues(DArray<Real>& vals)
536 {
537 sdfValues.assign(vals);
538 }
539
540 template<typename TDataType>
541 void VoxelOctree<TDataType>::getLeafsValue(DArray<Coord>& pos, DArray<Real>& val)
542 {
543 DArray<int> leafs_count;
544 this->getLeafs(pos, leafs_count);
545
546 val.resize(leafs_count.size());
547
548
549 cuExecute(leafs_count.size(),
550 SO_GetLeafsValue,
551 val,
552 leafs_count,
553 sdfValues);
554
555 leafs_count.clear();
556 }
557
558 //template <typename Coord>
559 //__global__ void SO_BWInitial(
560 // DArray<int> bw,
561 // DArray<int> bw_count,
562 // DArray<VoxelOctreeNode<Coord>> octree)
563 //{
564 // int tId = threadIdx.x + (blockIdx.x * blockDim.x);
565 // if (tId >= bw.size()) return;
566 // if (tId == 0 || (octree[tId].level() != octree[tId - 1].level()))
567 // {
568 // bw[tId] = 1;
569 // bw_count[tId] = 1;
570 // }
571 //}
572
573 //template <typename Coord>
574 //__global__ void SO_BWIteration(
575 // DArray<int> bw,
576 // DArray<int> bw_count,
577 // DArrayList<int> neighbor,
578 // DArray<VoxelOctreeNode<Coord>> octree)
579 //{
580 // int tId = threadIdx.x + (blockIdx.x * blockDim.x);
581 // if (tId >= bw.size()) return;
582 // if (bw_count[tId] == 1)
583 // {
584 // for (int i = 0; i < 6; i++)
585 // {
586 // //printf("iteration: %d %d %d,%d %d \n", tId, i, neighbor.size(), neighbor[i], bw_count[neighbor[i]]);
587 // int id = octree[tId].m_neighbor[i];
588 // if (id > 0 && bw_count[id] == 0)
589 // {
590 // if (bw[tId] == 0)
591 // bw[id] = 1;
592 // bw_count[id] = 1;
593 // }
594 // }
595 // }
596 //}
597
598 //template<typename TDataType>
599 //void VoxelOctree<TDataType>::getOctreeBW(DArray<int>& nodes_bw)
600 //{
601 // DArrayList<int>& leafs_neighbors = this->getNeighbors();
602 // uint num = size();
603 // DArray<int> nodes_count;
604 // nodes_bw.resize(num);
605 // nodes_count.resize(num);
606 // nodes_bw.reset();
607 // nodes_count.reset();
608 // int tnum = 0;
609 // Reduction<int> reduce;
610 // cuExecute(num,
611 // SO_BWInitial,
612 // nodes_bw,
613 // nodes_count,
614 // this->getVoxelOctree());
615 // tnum = reduce.accumulate(nodes_count.begin(), nodes_count.size());
616 // while (tnum < num)
617 // {
618 // cuExecute(num,
619 // SO_BWIteration,
620 // nodes_bw,
621 // nodes_count,
622 // leafs_neighbors,
623 // this->getVoxelOctree());
624 // tnum = reduce.accumulate(nodes_count.begin(), nodes_count.size());
625 // }
626 // nodes_count.clear();
627 //}
628
629
630 template <typename Real>
631 GPU_FUNC Real lerp(Real p, Real p0, Real p1, Real v0, Real v1)
632 {
633 if ((p1 - p0) < REAL_EPSILON)
634 return v0;
635 else
636 return ((p1 - p) * v0 + (p - p0) * v1) / (p1 - p0);
637 }
638
639 template <typename Real, typename Coord, typename TDataType>
640 __global__ void SO_GetSignDistance(
641 DArray<Real> point_sdf,
642 DArray<Coord> point_normal,
643 DArray<Coord> point_pos,
644 VoxelOctree<TDataType> oct_topology,
645 DArray<Real> oct_value,
646 Coord origin,
647 Real dx,
648 int nx,
649 int ny,
650 int nz,
651 bool inverted)
652 {
653 int tId = threadIdx.x + (blockIdx.x * blockDim.x);
654
655 if (tId >= point_pos.size()) return;
656
657 Coord po = (point_pos[tId] - origin) / dx;
658
659 int i = (int)std::floor(po[0]);
660 int j = (int)std::floor(po[1]);
661 int k = (int)std::floor(po[2]);
662
663 //TODO: check the correctness
664 if (i < 0 || i >= nx - 1 || j < 0 || j >= ny - 1 || k < 0 || k >= nz - 1)
665 {
666 if (inverted == true)
667 point_sdf[tId] = -100000.0f;
668 else
669 point_sdf[tId] = 100000.0f;
670 point_normal[tId] = Coord(0);
671 return;
672 }
673 int node_id[8];
674 VoxelOctreeNode<Coord> node[8];
675 oct_topology.getNode(i, j, k, node[0], node_id[0]);
676 oct_topology.getNode(i + 1, j, k, node[1], node_id[1]);
677 oct_topology.getNode(i, j + 1, k, node[2], node_id[2]);
678 oct_topology.getNode(i + 1, j + 1, k, node[3], node_id[3]);
679 oct_topology.getNode(i, j, k + 1, node[4], node_id[4]);
680 oct_topology.getNode(i + 1, j, k + 1, node[5], node_id[5]);
681 oct_topology.getNode(i, j + 1, k + 1, node[6], node_id[6]);
682 oct_topology.getNode(i + 1, j + 1, k + 1, node[7], node_id[7]);
683
684 Real dx00 = lerp(point_pos[tId][0], node[0].position()[0], node[1].position()[0], oct_value[node[0].value()], oct_value[node[1].value()]);
685 Real dx10 = lerp(point_pos[tId][0], node[2].position()[0], node[3].position()[0], oct_value[node[2].value()], oct_value[node[3].value()]);
686 Real dxy0 = lerp(point_pos[tId][1], node[0].position()[1], node[2].position()[1], dx00, dx10);
687
688 Real dx01 = lerp(point_pos[tId][0], node[4].position()[0], node[5].position()[0], oct_value[node[4].value()], oct_value[node[5].value()]);
689 Real dx11 = lerp(point_pos[tId][0], node[6].position()[0], node[7].position()[0], oct_value[node[6].value()], oct_value[node[7].value()]);
690 Real dxy1 = lerp(point_pos[tId][1], node[0].position()[1], node[2].position()[1], dx01, dx11);
691
692 Real d0y0 = lerp(point_pos[tId][1], node[0].position()[1], node[2].position()[1], oct_value[node[0].value()], oct_value[node[2].value()]);
693 Real d0y1 = lerp(point_pos[tId][1], node[4].position()[1], node[6].position()[1], oct_value[node[4].value()], oct_value[node[6].value()]);
694 Real d0yz = lerp(point_pos[tId][2], node[0].position()[2], node[4].position()[2], d0y0, d0y1);
695
696 Real d1y0 = lerp(point_pos[tId][1], node[1].position()[1], node[3].position()[1], oct_value[node[1].value()], oct_value[node[3].value()]);
697 Real d1y1 = lerp(point_pos[tId][1], node[5].position()[1], node[7].position()[1], oct_value[node[5].value()], oct_value[node[7].value()]);
698 Real d1yz = lerp(point_pos[tId][2], node[0].position()[2], node[4].position()[2], d1y0, d1y1);
699
700 Real dx0z = lerp(point_pos[tId][2], node[0].position()[2], node[4].position()[2], dx00, dx01);
701 Real dx1z = lerp(point_pos[tId][2], node[0].position()[2], node[4].position()[2], dx10, dx11);
702
703 point_normal[tId][0] = d0yz - d1yz;
704 point_normal[tId][1] = dx0z - dx1z;
705 point_normal[tId][2] = dxy0 - dxy1;
706
707 Real l = point_normal[tId].norm();
708 if (l < 0.0001f) point_normal[tId] = Coord(0);
709 else point_normal[tId] = point_normal[tId].normalize();
710
711 if (inverted == true)
712 {
713 point_sdf[tId] = -lerp(point_pos[tId][2], node[0].position()[2], node[4].position()[2], dxy0, dxy1);
714 point_normal[tId] = -point_normal[tId];
715 }
716 else
717 point_sdf[tId] = lerp(point_pos[tId][2], node[0].position()[2], node[4].position()[2], dxy0, dxy1);
718 }
719
720
721 template<typename TDataType>
722 void VoxelOctree<TDataType>::getSignDistance(
723 DArray<Coord> point_pos,
724 DArray<Real>& point_sdf,
725 DArray<Coord>& point_normal,
726 bool inverted)
727 {
728 point_sdf.resize(point_pos.size());
729 point_normal.resize(point_pos.size());
730
731 int nx, ny, nz;
732 //auto oct = this->stateSDFTopology()->getDataPtr();
733 getGrid(nx, ny, nz);
734
735 cuExecute(point_pos.size(),
736 SO_GetSignDistance,
737 point_sdf,
738 point_normal,
739 point_pos,
740 *this,
741 this->getSdfValues(),
742 this->getOrigin(),
743 this->getDx(),
744 nx,
745 ny,
746 nz,
747 inverted);
748 }
749
750 DYN_FUNC static void kernel1(Real& val, Real val_x)
751 {
752 if (std::abs(val_x) < 1)
753 val = (1 - std::abs(val_x));
754 else
755 val = 0;
756 }
757 DYN_FUNC static void kernel2(Real& val, Real val_x)
758 {
759 if (std::abs(val_x) < 0.5)
760 val = (0.75 - (std::abs(val_x) * std::abs(val_x)));
761 else if (std::abs(val_x) < 1.5)
762 val = 0.5 * (1.5 - (std::abs(val_x))) * (1.5 - (std::abs(val_x)));
763 else
764 val = 0;
765 }
766 DYN_FUNC static void kernel3(Real& val, Real val_x)
767 {
768 if (std::abs(val_x) < 1)
769 val = ((0.5 * abs(val_x) * abs(val_x) * abs(val_x)) - (abs(val_x) * abs(val_x)) + (2.0f / 3.0f));
770 else if (std::abs(val_x) < 2)
771 val = (1.0f / 6.0f) * (2.0f - (std::abs(val_x))) * (2.0f - (std::abs(val_x))) * (2.0f - (std::abs(val_x)));
772 else
773 val = 0.0f;
774 }
775
776
777 template <typename Real, typename Coord>
778 DYN_FUNC void SO_InterpolationFunction(
779 Real& weight,
780 Coord point_pos,
781 Coord grid_pos,
782 Real grid_spacing)
783 {
784 Real w1, w2, w3;
785 kernel2(w1, (point_pos[0] - grid_pos[0]) / grid_spacing);
786 kernel2(w2, (point_pos[1] - grid_pos[1]) / grid_spacing);
787 kernel2(w3, (point_pos[2] - grid_pos[2]) / grid_spacing);
788
789 weight = w1 * w2 * w3;
790 }
791
792 template <typename Real, typename Coord, typename TDataType>
793 __global__ void SO_GetSignDistanceKernel(
794 DArray<Real> point_sdf,
795 DArray<Coord> point_pos,
796 VoxelOctree<TDataType> octree_node,
797 DArray<Real> octree_value,
798 Coord origin_,
799 Real dx_,
800 int nx_,
801 int ny_,
802 int nz_)
803 {
804 int tId = threadIdx.x + (blockIdx.x * blockDim.x);
805 if (tId >= point_pos.size()) return;
806 Coord poi = point_pos[tId] - origin_;
807 int nx_pos = clamp(int(std::floor(poi[0] / dx_)), 0, nx_ - 1);
808 int ny_pos = clamp(int(std::floor(poi[1] / dx_)), 0, ny_ - 1);
809 int nz_pos = clamp(int(std::floor(poi[2] / dx_)), 0, nz_ - 1);
810 Real coef = Real(pow(Real(2), int(octree_node.getLevelNum())));
811 Real dx_top = dx_ * coef;
812 int node0_id;
813 VoxelOctreeNode<Coord> node0;
814 octree_node.getNode(nx_pos, ny_pos, nz_pos, node0, node0_id);
815 if ((node0.position() - point_pos[tId]).norm() < REAL_EPSILON)
816 {
817 point_sdf[tId] = octree_value[node0.value()];
818 return;
819 }
820 bool mask[6] = { 0,0,0,0,0,0 };
821 int node_id[6];
822 VoxelOctreeNode<Coord> node[6];
823 if (nx_pos > 0)
824 {
825 octree_node.getNode(nx_pos - 1, ny_pos, nz_pos, node[0], node_id[0]);
826 mask[0] = 1;
827 }
828 if (nx_pos < nx_ - 1)
829 {
830 octree_node.getNode(nx_pos + 1, ny_pos, nz_pos, node[1], node_id[1]);
831 mask[1] = 1;
832 }
833 if (ny_pos > 0)
834 {
835 octree_node.getNode(nx_pos, ny_pos - 1, nz_pos, node[2], node_id[2]);
836 mask[2] = 1;
837 }
838 if (ny_pos < ny_ - 1)
839 {
840 octree_node.getNode(nx_pos, ny_pos + 1, nz_pos, node[3], node_id[3]);
841 mask[3] = 1;
842 }
843 if (nz_pos > 0)
844 {
845 octree_node.getNode(nx_pos, ny_pos, nz_pos - 1, node[4], node_id[4]);
846 mask[4] = 1;
847 }
848 if (nz_pos < nz_ - 1)
849 {
850 octree_node.getNode(nx_pos, ny_pos, nz_pos + 1, node[5], node_id[5]);
851 mask[5] = 1;
852 }
853 Real weight[7] = { 0,0,0,0,0,0,0 };
854 SO_InterpolationFunction(weight[0], point_pos[tId], node0.position(), dx_top);
855 for (int i = 1; i < 7; i++)
856 {
857 if (mask[i] == true)
858 SO_InterpolationFunction(weight[i], point_pos[tId], node[i - 1].position(), dx_top);
859 }
860 Real weight_sum = weight[0] + weight[1] + weight[2] + weight[3] + weight[4] + weight[5] + weight[6];
861 //printf("the weight:%d %f %f %f %f %f %f %f %f \n", tId, weight[0], weight[1], weight[2], weight[3], weight[4], weight[5], weight[6], weight_sum);
862 if (weight_sum < REAL_EPSILON)
863 {
864 point_sdf[tId] = octree_value[node0.value()];
865 return;
866 }
867 else
868 {
869 Real sdf_value = octree_value[node0.value()] * weight[0] / weight_sum;;
870 for (int i = 1; i < 7; i++)
871 {
872 if (mask[i - 1] == true)
873 sdf_value += octree_value[node[i - 1].value()] * weight[i] / weight_sum;
874 }
875 point_sdf[tId] = sdf_value;
876 return;
877 }
878 }
879
880 template<typename TDataType>
881 void VoxelOctree<TDataType>::getSignDistanceKernel(
882 DArray<Coord> point_pos,
883 DArray<Real>& point_sdf)
884 {
885 point_sdf.resize(point_pos.size());
886
887 int nx, ny, nz;
888 this->getGrid(nx, ny, nz);
889
890 cuExecute(point_pos.size(),
891 SO_GetSignDistanceKernel,
892 point_sdf,
893 point_pos,
894 *this,
895 this->getSdfValues(),
896 this->getOrigin(),
897 this->getDx(),
898 nx,
899 ny,
900 nz);
901 }
902
903 template <typename Real, typename Coord, typename TDataType>
904 __global__ void SO_GetSignDistanceMLS(
905 DArray<Real> point_sdf,
906 DArray<Coord> point_normal,
907 DArray<Coord> point_pos,
908 VoxelOctree<TDataType> octree_node,
909 DArrayList<int> octree_neighbors,
910 DArray<Real> octree_value,
911 Coord origin_,
912 Real dx_,
913 int nx_,
914 int ny_,
915 int nz_,
916 bool inverted)
917 {
918 int tId = threadIdx.x + (blockIdx.x * blockDim.x);
919 if (tId >= point_pos.size()) return;
920
921 Coord pos = point_pos[tId];
922 Coord po = (pos - origin_) / dx_;
923
924 int pi = (int)std::floor(po[0]);
925 int pj = (int)std::floor(po[1]);
926 int pk = (int)std::floor(po[2]);
927
928 //TODO: check the correctness
929 if (pi < 0 || pi > nx_ - 1 || pj < 0 || pj > ny_ - 1 || pk < 0 || pk > nz_ - 1)
930 {
931 if (inverted == true)
932 point_sdf[tId] = -100000.0f;
933 else
934 point_sdf[tId] = 100000.0f;
935 point_normal[tId] = Coord(0);
936 return;
937 }
938
939 int id_0 = 0;
940 VoxelOctreeNode<Coord> node_0;
941 octree_node.getNode(pi, pj, pk, node_0, id_0);
942 Coord pos_0 = node_0.position();
943
944 Real coef = Real(pow(Real(2), int((node_0.level()))));
945 Real maxdx_ = dx_ * coef;
946
947 List<int>& node_list = octree_neighbors[id_0];
948 int list_size = node_list.size();
949 if (list_size == 0)
950 {
951 printf("~~~~~Error: The is no neighbor of that leaf node!~~~~~ %f %f %f %d %d %lld %d \n", origin_[0], origin_[1], origin_[2], id_0, node_0.value(), node_0.level(), node_0.midside());
952 }
953
954 Real dist = (pos_0 - pos).norm();
955 Real weight;
956 kernel3(weight, dist / maxdx_);
957 //SO_InterpolationFunction(weight, pos, pos_0, maxdx_);
958 Real weight_b = weight * octree_value[id_0];
959
960 Vec4d b(1, pos_0[0], pos_0[1], pos_0[2]);
961 b *= weight_b;
962
963 Mat4d M(1.0f, pos_0[0], pos_0[1], pos_0[2],
964 pos_0[0], pos_0[0] * pos_0[0], pos_0[0] * pos_0[1], pos_0[0] * pos_0[2],
965 pos_0[1], pos_0[1] * pos_0[0], pos_0[1] * pos_0[1], pos_0[1] * pos_0[2],
966 pos_0[2], pos_0[2] * pos_0[0], pos_0[2] * pos_0[1], pos_0[2] * pos_0[2]);
967 M *= weight;
968
969 for (int i = 0; i < list_size; i++)
970 {
971 Coord pos_i = octree_node[node_list[i]].position();
972
973 dist = (pos_i - pos).norm();
974 kernel3(weight, dist / maxdx_);
975 //SO_InterpolationFunction(weight, pos, pos_i, maxdx_);
976 weight_b = weight * octree_value[node_list[i]];
977
978 Vec4d b_i(1, pos_i[0], pos_i[1], pos_i[2]);
979 b += (b_i * weight_b);
980
981 M(0, 0) += weight; M(0, 1) += weight * pos_i[0]; M(0, 2) += weight * pos_i[1]; M(0, 3) += weight * pos_i[2];
982 M(1, 0) += weight * pos_i[0]; M(1, 1) += weight * pos_i[0] * pos_i[0]; M(1, 2) += weight * pos_i[0] * pos_i[1]; M(1, 3) += weight * pos_i[0] * pos_i[2];
983 M(2, 0) += weight * pos_i[1]; M(2, 1) += weight * pos_i[1] * pos_i[0]; M(2, 2) += weight * pos_i[1] * pos_i[1]; M(2, 3) += weight * pos_i[1] * pos_i[2];
984 M(3, 0) += weight * pos_i[2]; M(3, 1) += weight * pos_i[2] * pos_i[0]; M(3, 2) += weight * pos_i[2] * pos_i[1]; M(3, 3) += weight * pos_i[2] * pos_i[2];
985 }
986 Mat4d M_inv = M.inverse();
987
988 Vec4d x = M_inv * b;
989 Vec4d p(1.0f, pos[0], pos[1], pos[2]);
990
991 Real sdf_value = x.dot(p);
992 if (abs(sdf_value) < 0.01*maxdx_) sdf_value = 0.0;
993
994 Coord norm = Coord(x[1], x[2], x[3]);
995 norm.normalize();
996 if (inverted == true)
997 {
998 point_sdf[tId] = Real(-(sdf_value));
999 point_normal[tId] = norm;
1000 }
1001 else
1002 {
1003 point_sdf[tId] = Real(sdf_value);
1004 point_normal[tId] = -norm;
1005 }
1006 }
1007
1008 template<typename TDataType>
1009 void VoxelOctree<TDataType>::getSignDistanceMLS(
1010 DArray<Coord> point_pos,
1011 DArray<Real>& point_sdf,
1012 DArray<Coord>& point_normal,
1013 bool inverted)
1014 {
1015 point_sdf.resize(point_pos.size());
1016 point_normal.resize(point_pos.size());
1017
1018 auto& neighbors = this->getNeighbors();
1019
1020 int nx, ny, nz;
1021 this->getGrid(nx, ny, nz);
1022
1023 cuExecute(point_pos.size(),
1024 SO_GetSignDistanceMLS,
1025 point_sdf,
1026 point_normal,
1027 point_pos,
1028 *this,
1029 neighbors,
1030 this->getSdfValues(),
1031 this->getOrigin(),
1032 this->getDx(),
1033 nx,
1034 ny,
1035 nz,
1036 inverted);
1037 }
1038
1039
1040 DEFINE_CLASS(VoxelOctree);
1041}
Generated by doxygen 1.13.2