1060 lines
39 KiB
C++
1060 lines
39 KiB
C++
// Copyright Epic Games, Inc. All Rights Reserved.
|
|
|
|
#include "Meshing/LidarPointCloudMeshing.h"
|
|
#include "LidarPointCloud.h"
|
|
#include "LidarPointCloudOctree.h"
|
|
#include "LidarPointCloudOctreeMacros.h"
|
|
#include "Async/Async.h"
|
|
#include "Async/Future.h"
|
|
|
|
namespace
|
|
{
|
|
FORCEINLINE uint64 CalculateNumPermutations(int32 NumPoints)
|
|
{
|
|
return NumPoints > 4000000 ? UINT64_MAX : NumPoints * (NumPoints - 1) * (NumPoints - 2) / 6;
|
|
}
|
|
|
|
namespace MarchingCubes
|
|
{
|
|
// Stores triangle indices
|
|
constexpr int8 TriangleTable[256][16] =
|
|
{
|
|
{-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{0, 8, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{0, 1, 9, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{1, 8, 3, 9, 8, 1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{1, 2, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{0, 8, 3, 1, 2, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{9, 2, 10, 0, 2, 9, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{2, 8, 3, 2, 10, 8, 10, 9, 8, -1, -1, -1, -1, -1, -1, -1},
|
|
{3, 11, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{0, 11, 2, 8, 11, 0, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{1, 9, 0, 2, 3, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{1, 11, 2, 1, 9, 11, 9, 8, 11, -1, -1, -1, -1, -1, -1, -1},
|
|
{3, 10, 1, 11, 10, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{0, 10, 1, 0, 8, 10, 8, 11, 10, -1, -1, -1, -1, -1, -1, -1},
|
|
{3, 9, 0, 3, 11, 9, 11, 10, 9, -1, -1, -1, -1, -1, -1, -1},
|
|
{9, 8, 10, 10, 8, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{4, 7, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{4, 3, 0, 7, 3, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{0, 1, 9, 8, 4, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{4, 1, 9, 4, 7, 1, 7, 3, 1, -1, -1, -1, -1, -1, -1, -1},
|
|
{1, 2, 10, 8, 4, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{3, 4, 7, 3, 0, 4, 1, 2, 10, -1, -1, -1, -1, -1, -1, -1},
|
|
{9, 2, 10, 9, 0, 2, 8, 4, 7, -1, -1, -1, -1, -1, -1, -1},
|
|
{2, 10, 9, 2, 9, 7, 2, 7, 3, 7, 9, 4, -1, -1, -1, -1},
|
|
{8, 4, 7, 3, 11, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{11, 4, 7, 11, 2, 4, 2, 0, 4, -1, -1, -1, -1, -1, -1, -1},
|
|
{9, 0, 1, 8, 4, 7, 2, 3, 11, -1, -1, -1, -1, -1, -1, -1},
|
|
{4, 7, 11, 9, 4, 11, 9, 11, 2, 9, 2, 1, -1, -1, -1, -1},
|
|
{3, 10, 1, 3, 11, 10, 7, 8, 4, -1, -1, -1, -1, -1, -1, -1},
|
|
{1, 11, 10, 1, 4, 11, 1, 0, 4, 7, 11, 4, -1, -1, -1, -1},
|
|
{4, 7, 8, 9, 0, 11, 9, 11, 10, 11, 0, 3, -1, -1, -1, -1},
|
|
{4, 7, 11, 4, 11, 9, 9, 11, 10, -1, -1, -1, -1, -1, -1, -1},
|
|
{9, 5, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{9, 5, 4, 0, 8, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{0, 5, 4, 1, 5, 0, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{8, 5, 4, 8, 3, 5, 3, 1, 5, -1, -1, -1, -1, -1, -1, -1},
|
|
{1, 2, 10, 9, 5, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{3, 0, 8, 1, 2, 10, 4, 9, 5, -1, -1, -1, -1, -1, -1, -1},
|
|
{5, 2, 10, 5, 4, 2, 4, 0, 2, -1, -1, -1, -1, -1, -1, -1},
|
|
{2, 10, 5, 3, 2, 5, 3, 5, 4, 3, 4, 8, -1, -1, -1, -1},
|
|
{9, 5, 4, 2, 3, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{0, 11, 2, 0, 8, 11, 4, 9, 5, -1, -1, -1, -1, -1, -1, -1},
|
|
{0, 5, 4, 0, 1, 5, 2, 3, 11, -1, -1, -1, -1, -1, -1, -1},
|
|
{2, 1, 5, 2, 5, 8, 2, 8, 11, 4, 8, 5, -1, -1, -1, -1},
|
|
{10, 3, 11, 10, 1, 3, 9, 5, 4, -1, -1, -1, -1, -1, -1, -1},
|
|
{4, 9, 5, 0, 8, 1, 8, 10, 1, 8, 11, 10, -1, -1, -1, -1},
|
|
{5, 4, 0, 5, 0, 11, 5, 11, 10, 11, 0, 3, -1, -1, -1, -1},
|
|
{5, 4, 8, 5, 8, 10, 10, 8, 11, -1, -1, -1, -1, -1, -1, -1},
|
|
{9, 7, 8, 5, 7, 9, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{9, 3, 0, 9, 5, 3, 5, 7, 3, -1, -1, -1, -1, -1, -1, -1},
|
|
{0, 7, 8, 0, 1, 7, 1, 5, 7, -1, -1, -1, -1, -1, -1, -1},
|
|
{1, 5, 3, 3, 5, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{9, 7, 8, 9, 5, 7, 10, 1, 2, -1, -1, -1, -1, -1, -1, -1},
|
|
{10, 1, 2, 9, 5, 0, 5, 3, 0, 5, 7, 3, -1, -1, -1, -1},
|
|
{8, 0, 2, 8, 2, 5, 8, 5, 7, 10, 5, 2, -1, -1, -1, -1},
|
|
{2, 10, 5, 2, 5, 3, 3, 5, 7, -1, -1, -1, -1, -1, -1, -1},
|
|
{7, 9, 5, 7, 8, 9, 3, 11, 2, -1, -1, -1, -1, -1, -1, -1},
|
|
{9, 5, 7, 9, 7, 2, 9, 2, 0, 2, 7, 11, -1, -1, -1, -1},
|
|
{2, 3, 11, 0, 1, 8, 1, 7, 8, 1, 5, 7, -1, -1, -1, -1},
|
|
{11, 2, 1, 11, 1, 7, 7, 1, 5, -1, -1, -1, -1, -1, -1, -1},
|
|
{9, 5, 8, 8, 5, 7, 10, 1, 3, 10, 3, 11, -1, -1, -1, -1},
|
|
{5, 7, 0, 5, 0, 9, 7, 11, 0, 1, 0, 10, 11, 10, 0, -1},
|
|
{11, 10, 0, 11, 0, 3, 10, 5, 0, 8, 0, 7, 5, 7, 0, -1},
|
|
{11, 10, 5, 7, 11, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{10, 6, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{0, 8, 3, 5, 10, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{9, 0, 1, 5, 10, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{1, 8, 3, 1, 9, 8, 5, 10, 6, -1, -1, -1, -1, -1, -1, -1},
|
|
{1, 6, 5, 2, 6, 1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{1, 6, 5, 1, 2, 6, 3, 0, 8, -1, -1, -1, -1, -1, -1, -1},
|
|
{9, 6, 5, 9, 0, 6, 0, 2, 6, -1, -1, -1, -1, -1, -1, -1},
|
|
{5, 9, 8, 5, 8, 2, 5, 2, 6, 3, 2, 8, -1, -1, -1, -1},
|
|
{2, 3, 11, 10, 6, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{11, 0, 8, 11, 2, 0, 10, 6, 5, -1, -1, -1, -1, -1, -1, -1},
|
|
{0, 1, 9, 2, 3, 11, 5, 10, 6, -1, -1, -1, -1, -1, -1, -1},
|
|
{5, 10, 6, 1, 9, 2, 9, 11, 2, 9, 8, 11, -1, -1, -1, -1},
|
|
{6, 3, 11, 6, 5, 3, 5, 1, 3, -1, -1, -1, -1, -1, -1, -1},
|
|
{0, 8, 11, 0, 11, 5, 0, 5, 1, 5, 11, 6, -1, -1, -1, -1},
|
|
{3, 11, 6, 0, 3, 6, 0, 6, 5, 0, 5, 9, -1, -1, -1, -1},
|
|
{6, 5, 9, 6, 9, 11, 11, 9, 8, -1, -1, -1, -1, -1, -1, -1},
|
|
{5, 10, 6, 4, 7, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{4, 3, 0, 4, 7, 3, 6, 5, 10, -1, -1, -1, -1, -1, -1, -1},
|
|
{1, 9, 0, 5, 10, 6, 8, 4, 7, -1, -1, -1, -1, -1, -1, -1},
|
|
{10, 6, 5, 1, 9, 7, 1, 7, 3, 7, 9, 4, -1, -1, -1, -1},
|
|
{6, 1, 2, 6, 5, 1, 4, 7, 8, -1, -1, -1, -1, -1, -1, -1},
|
|
{1, 2, 5, 5, 2, 6, 3, 0, 4, 3, 4, 7, -1, -1, -1, -1},
|
|
{8, 4, 7, 9, 0, 5, 0, 6, 5, 0, 2, 6, -1, -1, -1, -1},
|
|
{7, 3, 9, 7, 9, 4, 3, 2, 9, 5, 9, 6, 2, 6, 9, -1},
|
|
{3, 11, 2, 7, 8, 4, 10, 6, 5, -1, -1, -1, -1, -1, -1, -1},
|
|
{5, 10, 6, 4, 7, 2, 4, 2, 0, 2, 7, 11, -1, -1, -1, -1},
|
|
{0, 1, 9, 4, 7, 8, 2, 3, 11, 5, 10, 6, -1, -1, -1, -1},
|
|
{9, 2, 1, 9, 11, 2, 9, 4, 11, 7, 11, 4, 5, 10, 6, -1},
|
|
{8, 4, 7, 3, 11, 5, 3, 5, 1, 5, 11, 6, -1, -1, -1, -1},
|
|
{5, 1, 11, 5, 11, 6, 1, 0, 11, 7, 11, 4, 0, 4, 11, -1},
|
|
{0, 5, 9, 0, 6, 5, 0, 3, 6, 11, 6, 3, 8, 4, 7, -1},
|
|
{6, 5, 9, 6, 9, 11, 4, 7, 9, 7, 11, 9, -1, -1, -1, -1},
|
|
{10, 4, 9, 6, 4, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{4, 10, 6, 4, 9, 10, 0, 8, 3, -1, -1, -1, -1, -1, -1, -1},
|
|
{10, 0, 1, 10, 6, 0, 6, 4, 0, -1, -1, -1, -1, -1, -1, -1},
|
|
{8, 3, 1, 8, 1, 6, 8, 6, 4, 6, 1, 10, -1, -1, -1, -1},
|
|
{1, 4, 9, 1, 2, 4, 2, 6, 4, -1, -1, -1, -1, -1, -1, -1},
|
|
{3, 0, 8, 1, 2, 9, 2, 4, 9, 2, 6, 4, -1, -1, -1, -1},
|
|
{0, 2, 4, 4, 2, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{8, 3, 2, 8, 2, 4, 4, 2, 6, -1, -1, -1, -1, -1, -1, -1},
|
|
{10, 4, 9, 10, 6, 4, 11, 2, 3, -1, -1, -1, -1, -1, -1, -1},
|
|
{0, 8, 2, 2, 8, 11, 4, 9, 10, 4, 10, 6, -1, -1, -1, -1},
|
|
{3, 11, 2, 0, 1, 6, 0, 6, 4, 6, 1, 10, -1, -1, -1, -1},
|
|
{6, 4, 1, 6, 1, 10, 4, 8, 1, 2, 1, 11, 8, 11, 1, -1},
|
|
{9, 6, 4, 9, 3, 6, 9, 1, 3, 11, 6, 3, -1, -1, -1, -1},
|
|
{8, 11, 1, 8, 1, 0, 11, 6, 1, 9, 1, 4, 6, 4, 1, -1},
|
|
{3, 11, 6, 3, 6, 0, 0, 6, 4, -1, -1, -1, -1, -1, -1, -1},
|
|
{6, 4, 8, 11, 6, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{7, 10, 6, 7, 8, 10, 8, 9, 10, -1, -1, -1, -1, -1, -1, -1},
|
|
{0, 7, 3, 0, 10, 7, 0, 9, 10, 6, 7, 10, -1, -1, -1, -1},
|
|
{10, 6, 7, 1, 10, 7, 1, 7, 8, 1, 8, 0, -1, -1, -1, -1},
|
|
{10, 6, 7, 10, 7, 1, 1, 7, 3, -1, -1, -1, -1, -1, -1, -1},
|
|
{1, 2, 6, 1, 6, 8, 1, 8, 9, 8, 6, 7, -1, -1, -1, -1},
|
|
{2, 6, 9, 2, 9, 1, 6, 7, 9, 0, 9, 3, 7, 3, 9, -1},
|
|
{7, 8, 0, 7, 0, 6, 6, 0, 2, -1, -1, -1, -1, -1, -1, -1},
|
|
{7, 3, 2, 6, 7, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{2, 3, 11, 10, 6, 8, 10, 8, 9, 8, 6, 7, -1, -1, -1, -1},
|
|
{2, 0, 7, 2, 7, 11, 0, 9, 7, 6, 7, 10, 9, 10, 7, -1},
|
|
{1, 8, 0, 1, 7, 8, 1, 10, 7, 6, 7, 10, 2, 3, 11, -1},
|
|
{11, 2, 1, 11, 1, 7, 10, 6, 1, 6, 7, 1, -1, -1, -1, -1},
|
|
{8, 9, 6, 8, 6, 7, 9, 1, 6, 11, 6, 3, 1, 3, 6, -1},
|
|
{0, 9, 1, 11, 6, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{7, 8, 0, 7, 0, 6, 3, 11, 0, 11, 6, 0, -1, -1, -1, -1},
|
|
{7, 11, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{7, 6, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{3, 0, 8, 11, 7, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{0, 1, 9, 11, 7, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{8, 1, 9, 8, 3, 1, 11, 7, 6, -1, -1, -1, -1, -1, -1, -1},
|
|
{10, 1, 2, 6, 11, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{1, 2, 10, 3, 0, 8, 6, 11, 7, -1, -1, -1, -1, -1, -1, -1},
|
|
{2, 9, 0, 2, 10, 9, 6, 11, 7, -1, -1, -1, -1, -1, -1, -1},
|
|
{6, 11, 7, 2, 10, 3, 10, 8, 3, 10, 9, 8, -1, -1, -1, -1},
|
|
{7, 2, 3, 6, 2, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{7, 0, 8, 7, 6, 0, 6, 2, 0, -1, -1, -1, -1, -1, -1, -1},
|
|
{2, 7, 6, 2, 3, 7, 0, 1, 9, -1, -1, -1, -1, -1, -1, -1},
|
|
{1, 6, 2, 1, 8, 6, 1, 9, 8, 8, 7, 6, -1, -1, -1, -1},
|
|
{10, 7, 6, 10, 1, 7, 1, 3, 7, -1, -1, -1, -1, -1, -1, -1},
|
|
{10, 7, 6, 1, 7, 10, 1, 8, 7, 1, 0, 8, -1, -1, -1, -1},
|
|
{0, 3, 7, 0, 7, 10, 0, 10, 9, 6, 10, 7, -1, -1, -1, -1},
|
|
{7, 6, 10, 7, 10, 8, 8, 10, 9, -1, -1, -1, -1, -1, -1, -1},
|
|
{6, 8, 4, 11, 8, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{3, 6, 11, 3, 0, 6, 0, 4, 6, -1, -1, -1, -1, -1, -1, -1},
|
|
{8, 6, 11, 8, 4, 6, 9, 0, 1, -1, -1, -1, -1, -1, -1, -1},
|
|
{9, 4, 6, 9, 6, 3, 9, 3, 1, 11, 3, 6, -1, -1, -1, -1},
|
|
{6, 8, 4, 6, 11, 8, 2, 10, 1, -1, -1, -1, -1, -1, -1, -1},
|
|
{1, 2, 10, 3, 0, 11, 0, 6, 11, 0, 4, 6, -1, -1, -1, -1},
|
|
{4, 11, 8, 4, 6, 11, 0, 2, 9, 2, 10, 9, -1, -1, -1, -1},
|
|
{10, 9, 3, 10, 3, 2, 9, 4, 3, 11, 3, 6, 4, 6, 3, -1},
|
|
{8, 2, 3, 8, 4, 2, 4, 6, 2, -1, -1, -1, -1, -1, -1, -1},
|
|
{0, 4, 2, 4, 6, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{1, 9, 0, 2, 3, 4, 2, 4, 6, 4, 3, 8, -1, -1, -1, -1},
|
|
{1, 9, 4, 1, 4, 2, 2, 4, 6, -1, -1, -1, -1, -1, -1, -1},
|
|
{8, 1, 3, 8, 6, 1, 8, 4, 6, 6, 10, 1, -1, -1, -1, -1},
|
|
{10, 1, 0, 10, 0, 6, 6, 0, 4, -1, -1, -1, -1, -1, -1, -1},
|
|
{4, 6, 3, 4, 3, 8, 6, 10, 3, 0, 3, 9, 10, 9, 3, -1},
|
|
{10, 9, 4, 6, 10, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{4, 9, 5, 7, 6, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{0, 8, 3, 4, 9, 5, 11, 7, 6, -1, -1, -1, -1, -1, -1, -1},
|
|
{5, 0, 1, 5, 4, 0, 7, 6, 11, -1, -1, -1, -1, -1, -1, -1},
|
|
{11, 7, 6, 8, 3, 4, 3, 5, 4, 3, 1, 5, -1, -1, -1, -1},
|
|
{9, 5, 4, 10, 1, 2, 7, 6, 11, -1, -1, -1, -1, -1, -1, -1},
|
|
{6, 11, 7, 1, 2, 10, 0, 8, 3, 4, 9, 5, -1, -1, -1, -1},
|
|
{7, 6, 11, 5, 4, 10, 4, 2, 10, 4, 0, 2, -1, -1, -1, -1},
|
|
{3, 4, 8, 3, 5, 4, 3, 2, 5, 10, 5, 2, 11, 7, 6, -1},
|
|
{7, 2, 3, 7, 6, 2, 5, 4, 9, -1, -1, -1, -1, -1, -1, -1},
|
|
{9, 5, 4, 0, 8, 6, 0, 6, 2, 6, 8, 7, -1, -1, -1, -1},
|
|
{3, 6, 2, 3, 7, 6, 1, 5, 0, 5, 4, 0, -1, -1, -1, -1},
|
|
{6, 2, 8, 6, 8, 7, 2, 1, 8, 4, 8, 5, 1, 5, 8, -1},
|
|
{9, 5, 4, 10, 1, 6, 1, 7, 6, 1, 3, 7, -1, -1, -1, -1},
|
|
{1, 6, 10, 1, 7, 6, 1, 0, 7, 8, 7, 0, 9, 5, 4, -1},
|
|
{4, 0, 10, 4, 10, 5, 0, 3, 10, 6, 10, 7, 3, 7, 10, -1},
|
|
{7, 6, 10, 7, 10, 8, 5, 4, 10, 4, 8, 10, -1, -1, -1, -1},
|
|
{6, 9, 5, 6, 11, 9, 11, 8, 9, -1, -1, -1, -1, -1, -1, -1},
|
|
{3, 6, 11, 0, 6, 3, 0, 5, 6, 0, 9, 5, -1, -1, -1, -1},
|
|
{0, 11, 8, 0, 5, 11, 0, 1, 5, 5, 6, 11, -1, -1, -1, -1},
|
|
{6, 11, 3, 6, 3, 5, 5, 3, 1, -1, -1, -1, -1, -1, -1, -1},
|
|
{1, 2, 10, 9, 5, 11, 9, 11, 8, 11, 5, 6, -1, -1, -1, -1},
|
|
{0, 11, 3, 0, 6, 11, 0, 9, 6, 5, 6, 9, 1, 2, 10, -1},
|
|
{11, 8, 5, 11, 5, 6, 8, 0, 5, 10, 5, 2, 0, 2, 5, -1},
|
|
{6, 11, 3, 6, 3, 5, 2, 10, 3, 10, 5, 3, -1, -1, -1, -1},
|
|
{5, 8, 9, 5, 2, 8, 5, 6, 2, 3, 8, 2, -1, -1, -1, -1},
|
|
{9, 5, 6, 9, 6, 0, 0, 6, 2, -1, -1, -1, -1, -1, -1, -1},
|
|
{1, 5, 8, 1, 8, 0, 5, 6, 8, 3, 8, 2, 6, 2, 8, -1},
|
|
{1, 5, 6, 2, 1, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{1, 3, 6, 1, 6, 10, 3, 8, 6, 5, 6, 9, 8, 9, 6, -1},
|
|
{10, 1, 0, 10, 0, 6, 9, 5, 0, 5, 6, 0, -1, -1, -1, -1},
|
|
{0, 3, 8, 5, 6, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{10, 5, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{11, 5, 10, 7, 5, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{11, 5, 10, 11, 7, 5, 8, 3, 0, -1, -1, -1, -1, -1, -1, -1},
|
|
{5, 11, 7, 5, 10, 11, 1, 9, 0, -1, -1, -1, -1, -1, -1, -1},
|
|
{10, 7, 5, 10, 11, 7, 9, 8, 1, 8, 3, 1, -1, -1, -1, -1},
|
|
{11, 1, 2, 11, 7, 1, 7, 5, 1, -1, -1, -1, -1, -1, -1, -1},
|
|
{0, 8, 3, 1, 2, 7, 1, 7, 5, 7, 2, 11, -1, -1, -1, -1},
|
|
{9, 7, 5, 9, 2, 7, 9, 0, 2, 2, 11, 7, -1, -1, -1, -1},
|
|
{7, 5, 2, 7, 2, 11, 5, 9, 2, 3, 2, 8, 9, 8, 2, -1},
|
|
{2, 5, 10, 2, 3, 5, 3, 7, 5, -1, -1, -1, -1, -1, -1, -1},
|
|
{8, 2, 0, 8, 5, 2, 8, 7, 5, 10, 2, 5, -1, -1, -1, -1},
|
|
{9, 0, 1, 5, 10, 3, 5, 3, 7, 3, 10, 2, -1, -1, -1, -1},
|
|
{9, 8, 2, 9, 2, 1, 8, 7, 2, 10, 2, 5, 7, 5, 2, -1},
|
|
{1, 3, 5, 3, 7, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{0, 8, 7, 0, 7, 1, 1, 7, 5, -1, -1, -1, -1, -1, -1, -1},
|
|
{9, 0, 3, 9, 3, 5, 5, 3, 7, -1, -1, -1, -1, -1, -1, -1},
|
|
{9, 8, 7, 5, 9, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{5, 8, 4, 5, 10, 8, 10, 11, 8, -1, -1, -1, -1, -1, -1, -1},
|
|
{5, 0, 4, 5, 11, 0, 5, 10, 11, 11, 3, 0, -1, -1, -1, -1},
|
|
{0, 1, 9, 8, 4, 10, 8, 10, 11, 10, 4, 5, -1, -1, -1, -1},
|
|
{10, 11, 4, 10, 4, 5, 11, 3, 4, 9, 4, 1, 3, 1, 4, -1},
|
|
{2, 5, 1, 2, 8, 5, 2, 11, 8, 4, 5, 8, -1, -1, -1, -1},
|
|
{0, 4, 11, 0, 11, 3, 4, 5, 11, 2, 11, 1, 5, 1, 11, -1},
|
|
{0, 2, 5, 0, 5, 9, 2, 11, 5, 4, 5, 8, 11, 8, 5, -1},
|
|
{9, 4, 5, 2, 11, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{2, 5, 10, 3, 5, 2, 3, 4, 5, 3, 8, 4, -1, -1, -1, -1},
|
|
{5, 10, 2, 5, 2, 4, 4, 2, 0, -1, -1, -1, -1, -1, -1, -1},
|
|
{3, 10, 2, 3, 5, 10, 3, 8, 5, 4, 5, 8, 0, 1, 9, -1},
|
|
{5, 10, 2, 5, 2, 4, 1, 9, 2, 9, 4, 2, -1, -1, -1, -1},
|
|
{8, 4, 5, 8, 5, 3, 3, 5, 1, -1, -1, -1, -1, -1, -1, -1},
|
|
{0, 4, 5, 1, 0, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{8, 4, 5, 8, 5, 3, 9, 0, 5, 0, 3, 5, -1, -1, -1, -1},
|
|
{9, 4, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{4, 11, 7, 4, 9, 11, 9, 10, 11, -1, -1, -1, -1, -1, -1, -1},
|
|
{0, 8, 3, 4, 9, 7, 9, 11, 7, 9, 10, 11, -1, -1, -1, -1},
|
|
{1, 10, 11, 1, 11, 4, 1, 4, 0, 7, 4, 11, -1, -1, -1, -1},
|
|
{3, 1, 4, 3, 4, 8, 1, 10, 4, 7, 4, 11, 10, 11, 4, -1},
|
|
{4, 11, 7, 9, 11, 4, 9, 2, 11, 9, 1, 2, -1, -1, -1, -1},
|
|
{9, 7, 4, 9, 11, 7, 9, 1, 11, 2, 11, 1, 0, 8, 3, -1},
|
|
{11, 7, 4, 11, 4, 2, 2, 4, 0, -1, -1, -1, -1, -1, -1, -1},
|
|
{11, 7, 4, 11, 4, 2, 8, 3, 4, 3, 2, 4, -1, -1, -1, -1},
|
|
{2, 9, 10, 2, 7, 9, 2, 3, 7, 7, 4, 9, -1, -1, -1, -1},
|
|
{9, 10, 7, 9, 7, 4, 10, 2, 7, 8, 7, 0, 2, 0, 7, -1},
|
|
{3, 7, 10, 3, 10, 2, 7, 4, 10, 1, 10, 0, 4, 0, 10, -1},
|
|
{1, 10, 2, 8, 7, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{4, 9, 1, 4, 1, 7, 7, 1, 3, -1, -1, -1, -1, -1, -1, -1},
|
|
{4, 9, 1, 4, 1, 7, 0, 8, 1, 8, 7, 1, -1, -1, -1, -1},
|
|
{4, 0, 3, 7, 4, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{4, 8, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{9, 10, 8, 10, 11, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{3, 0, 9, 3, 9, 11, 11, 9, 10, -1, -1, -1, -1, -1, -1, -1},
|
|
{0, 1, 10, 0, 10, 8, 8, 10, 11, -1, -1, -1, -1, -1, -1, -1},
|
|
{3, 1, 10, 11, 3, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{1, 2, 11, 1, 11, 9, 9, 11, 8, -1, -1, -1, -1, -1, -1, -1},
|
|
{3, 0, 9, 3, 9, 11, 1, 2, 9, 2, 11, 9, -1, -1, -1, -1},
|
|
{0, 2, 11, 8, 0, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{3, 2, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{2, 3, 8, 2, 8, 10, 10, 8, 9, -1, -1, -1, -1, -1, -1, -1},
|
|
{9, 10, 2, 0, 9, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{2, 3, 8, 2, 8, 10, 0, 1, 8, 1, 10, 8, -1, -1, -1, -1},
|
|
{1, 10, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{1, 3, 8, 9, 1, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{0, 9, 1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{0, 3, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
|
|
{-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}
|
|
};
|
|
|
|
const FVector3f VertexOffsetTable[12] =
|
|
{
|
|
FVector3f(0.5, 0, 0),
|
|
FVector3f(1, 0.5, 0),
|
|
FVector3f(0.5, 1, 0),
|
|
FVector3f(0, 0.5, 0),
|
|
FVector3f(0.5, 0, 1),
|
|
FVector3f(1, 0.5, 1),
|
|
FVector3f(0.5, 1, 1),
|
|
FVector3f(0, 0.5, 1),
|
|
FVector3f(0, 0, 0.5),
|
|
FVector3f(1, 0, 0.5),
|
|
FVector3f(1, 1, 0.5),
|
|
FVector3f(0, 1, 0.5)
|
|
};
|
|
|
|
// 256 possible marching cubes combinations
|
|
constexpr int32 NumCombinations = 256;
|
|
|
|
// Max 5 triangles x 3 vertices x 12 bytes each + 1 byte for NumVertices
|
|
constexpr int32 MaxCombinationSize = 181;
|
|
|
|
/** Calculate pre-computed data, ready for processing. */
|
|
TArray<uint8> BuildScaledVertexTable(float CellSize)
|
|
{
|
|
TArray<uint8> VertexData;
|
|
VertexData.AddUninitialized(NumCombinations * MaxCombinationSize);
|
|
|
|
TArray<FVector3f> TempArray;
|
|
uint8* VertexDataPtr = VertexData.GetData();
|
|
for (int32 i = 0; i < NumCombinations; ++i)
|
|
{
|
|
const int8* VertexList = TriangleTable[i];
|
|
TempArray.Reset();
|
|
|
|
for (int32 t = 0; t < 16; t += 3)
|
|
{
|
|
if (VertexList[t] == -1)
|
|
{
|
|
break;
|
|
}
|
|
|
|
TempArray.Add(VertexOffsetTable[VertexList[t + 2]] * CellSize);
|
|
TempArray.Add(VertexOffsetTable[VertexList[t + 1]] * CellSize);
|
|
TempArray.Add(VertexOffsetTable[VertexList[t]] * CellSize);
|
|
}
|
|
|
|
*VertexDataPtr++ = TempArray.Num();
|
|
FMemory::Memcpy(VertexDataPtr, TempArray.GetData(), TempArray.Num() * 12);
|
|
VertexDataPtr += MaxCombinationSize - 1;
|
|
}
|
|
|
|
return VertexData;
|
|
}
|
|
|
|
/** Constructs the normals table for vertices */
|
|
TArray<TArray<FVector3f>>* GetNormalsTable()
|
|
{
|
|
static TArray<TArray<FVector3f>> NormalsTable;
|
|
|
|
if(NormalsTable.Num() == 0)
|
|
{
|
|
NormalsTable.AddDefaulted(NumCombinations);
|
|
for(int32 i = 0; i < NumCombinations; ++i)
|
|
{
|
|
TArray<FVector3f>& Normals = NormalsTable[i];
|
|
Normals.Reserve(15);
|
|
|
|
for(int32 j = 0; j < 15; j += 3)
|
|
{
|
|
const int8 A = TriangleTable[i][j];
|
|
const int8 B = TriangleTable[i][j+1];
|
|
const int8 C = TriangleTable[i][j+2];
|
|
|
|
if(A > -1)
|
|
{
|
|
const FVector3f P1 = VertexOffsetTable[A];
|
|
const FVector3f P2 = VertexOffsetTable[B];
|
|
const FVector3f P3 = VertexOffsetTable[C];
|
|
|
|
const FVector3f V1 = (P3 - P2).GetSafeNormal();
|
|
const FVector3f V2 = (P1 - P2).GetSafeNormal();
|
|
|
|
const FVector3f Normal = FVector3f::CrossProduct(V1, V2).GetSafeNormal();
|
|
Normals.Append({Normal, Normal, Normal});
|
|
}
|
|
else
|
|
{
|
|
static TArray<FVector3f> EmptyNormal({ FVector3f::ZeroVector, FVector3f::ZeroVector, FVector3f::ZeroVector });
|
|
Normals.Append(EmptyNormal);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return &NormalsTable;
|
|
}
|
|
|
|
/** Calculates the color lookup table for vertices */
|
|
void CalculateColorLookupTable(int32 GridDimension, TArray<TArray<uint32>>& OutColorTable)
|
|
{
|
|
static TArray<TArray<FIntVector>> MasterColorOffsetTable;
|
|
|
|
if(MasterColorOffsetTable.Num() == 0)
|
|
{
|
|
TArray<TArray<FVector3f>>* NormalsTable = GetNormalsTable();
|
|
|
|
MasterColorOffsetTable.AddDefaulted(NumCombinations);
|
|
for(int32 i = 0; i < NumCombinations; ++i)
|
|
{
|
|
TArray<FIntVector>& ColorOffsets = MasterColorOffsetTable[i];
|
|
ColorOffsets.Reserve(15);
|
|
|
|
for(int32 j = 0; j < 15; ++j)
|
|
{
|
|
const int8 Idx = TriangleTable[i][j];
|
|
if(Idx > -1)
|
|
{
|
|
const static FVector3f Corners[8] =
|
|
{
|
|
FVector3f(0, 0, 0),
|
|
FVector3f(1, 0, 0),
|
|
FVector3f(0, 1, 0),
|
|
FVector3f(1, 1, 0),
|
|
FVector3f(0, 0, 1),
|
|
FVector3f(1, 0, 1),
|
|
FVector3f(0, 1, 1),
|
|
FVector3f(1, 1, 1)
|
|
};
|
|
|
|
const FVector3f P = VertexOffsetTable[Idx];
|
|
const FVector3f N = -(*NormalsTable)[i][j];
|
|
|
|
float MinDist = FLT_MAX;
|
|
int32 MinC = 0;
|
|
|
|
for(int32 c = 0; c < 8; c++)
|
|
{
|
|
const FVector3f V = Corners[c] - P;
|
|
if(FVector3f::DotProduct(N, V) > 0)
|
|
{
|
|
const float Dist = V.SizeSquared();
|
|
|
|
if(Dist < MinDist)
|
|
{
|
|
MinDist = Dist;
|
|
MinC = c;
|
|
}
|
|
}
|
|
}
|
|
|
|
ColorOffsets.Emplace((FVector)Corners[MinC]);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
const int32 GridDimensionSq = GridDimension * GridDimension;
|
|
|
|
OutColorTable.SetNum(NumCombinations);
|
|
for(int32 i = 0; i < NumCombinations; ++i)
|
|
{
|
|
TArray<FIntVector>& MasterOffsets = MasterColorOffsetTable[i];
|
|
TArray<uint32>& ColorOffsets = OutColorTable[i];
|
|
ColorOffsets.SetNumZeroed(MasterOffsets.Num());
|
|
|
|
for(int32 j = 0; j < ColorOffsets.Num(); ++j)
|
|
{
|
|
const FIntVector& MasterOffset = MasterOffsets[j];
|
|
ColorOffsets[j] = MasterOffset.Z * GridDimensionSq + MasterOffset.Y * GridDimension + MasterOffset.X;
|
|
}
|
|
}
|
|
}
|
|
|
|
/** Applies Marching Cubes algorithm to extract mesh information from the given voxelized grid */
|
|
void ProcessGrid(uint8* VoxelizedGrid, uint8* VertexData, FColor* ColorGrid, TArray<TArray<uint32>>& ColorLookupTable, int32 GridDimension, float CellSize, FVector3f VertexOffset,
|
|
TArray<FVector3f>& OutVertices, TArray<FVector3f>* OutNormals, TArray<FColor>* OutColors)
|
|
{
|
|
TArray<TArray<FVector3f>>* NormalsTable = GetNormalsTable();
|
|
|
|
FVector3f Coords;
|
|
uint8* Vertices = nullptr;
|
|
|
|
const int32 GridDimensionSq = GridDimension * GridDimension;
|
|
|
|
uint8* Grid = VoxelizedGrid;
|
|
uint8* GridUp = Grid + GridDimensionSq;
|
|
|
|
for (int32 sZ = 0; sZ < GridDimension - 1; ++sZ)
|
|
{
|
|
Coords.Z = VertexOffset.Z + (sZ + 0.5f) * CellSize;
|
|
|
|
Grid = VoxelizedGrid + sZ * GridDimensionSq;
|
|
GridUp = Grid + GridDimensionSq;
|
|
int32 BaseIndex = 0;
|
|
|
|
for (int32 sY = 0; sY < GridDimension - 1; ++sY)
|
|
{
|
|
Coords.Y = VertexOffset.Y + (sY + 0.5f) * CellSize;
|
|
|
|
for (int32 sX = 0; sX < GridDimension - 1; ++sX, ++BaseIndex)
|
|
{
|
|
Coords.X = VertexOffset.X + (sX + 0.5f) * CellSize;
|
|
|
|
// Calculate the variation of the cube
|
|
uint8 CubeVariation = 0;
|
|
if (Grid[BaseIndex] == 0) { CubeVariation |= 1; }
|
|
if (Grid[BaseIndex + 1] == 0) { CubeVariation |= 2; }
|
|
if (Grid[BaseIndex + GridDimension + 1] == 0) { CubeVariation |= 4; }
|
|
if (Grid[BaseIndex + GridDimension] == 0) { CubeVariation |= 8; }
|
|
if (GridUp[BaseIndex] == 0) { CubeVariation |= 16; }
|
|
if (GridUp[BaseIndex + 1] == 0) { CubeVariation |= 32; }
|
|
if (GridUp[BaseIndex + GridDimension + 1] == 0) { CubeVariation |= 64; }
|
|
if (GridUp[BaseIndex + GridDimension] == 0) { CubeVariation |= 128; }
|
|
|
|
// No triangles to add
|
|
if (CubeVariation == 0 || CubeVariation == 255)
|
|
{
|
|
continue;
|
|
}
|
|
|
|
Vertices = &VertexData[CubeVariation * MaxCombinationSize];
|
|
const uint8 NumVertices = Vertices[0];
|
|
|
|
if (NumVertices == 0)
|
|
{
|
|
continue;
|
|
}
|
|
|
|
// Fill vertex positions
|
|
{
|
|
const int32 Offset = OutVertices.AddUninitialized(NumVertices);
|
|
for (FVector3f* Vertex = OutVertices.GetData() + Offset, *SourceData = (FVector3f*)(Vertices + 1), *DataEnd = Vertex + NumVertices; Vertex != DataEnd; ++Vertex, ++SourceData)
|
|
{
|
|
*Vertex = Coords + *SourceData;
|
|
}
|
|
}
|
|
|
|
// Fill normals if requested
|
|
if(OutNormals)
|
|
{
|
|
const int32 Offset = OutNormals->AddUninitialized(NumVertices);
|
|
FMemory::Memcpy(OutNormals->GetData() + Offset, (*NormalsTable)[CubeVariation].GetData(), NumVertices * sizeof(FVector3f));
|
|
}
|
|
|
|
// Fill colors if requested
|
|
if(OutColors)
|
|
{
|
|
const uint32 BaseColorIndex = sZ * GridDimensionSq + BaseIndex;
|
|
const uint32* LookupTablePtr = ColorLookupTable[CubeVariation].GetData();
|
|
const int32 Offset = OutColors->AddUninitialized(NumVertices);
|
|
FColor* ColorsPtr = OutColors->GetData() + Offset;
|
|
for(int32 v = 0; v < NumVertices; ++v)
|
|
{
|
|
*ColorsPtr++ = ColorGrid[BaseColorIndex + *LookupTablePtr++];
|
|
}
|
|
}
|
|
}
|
|
|
|
++BaseIndex;
|
|
}
|
|
}
|
|
}
|
|
|
|
void Run(FLidarPointCloudOctree* Octree, const float& CellSize, bool bUseSelection, TArray<FVector3f>& OutVertices, TArray<FVector3f>* OutNormals, TArray<FColor>* OutColors)
|
|
{
|
|
FScopeBenchmarkTimer Timer("Marching Cubes");
|
|
|
|
// Expand original bounds to make sure the mesh is closed at the edges
|
|
const FBox Bounds = Octree->GetBounds().ExpandBy(FVector::OneVector * CellSize, FVector::OneVector * CellSize);
|
|
const FVector3f BoundsSize = (FVector3f)Bounds.GetSize();
|
|
const FVector3f LocationOffset = (FVector3f)Octree->GetOwner()->LocationOffset;
|
|
|
|
// Number of cells in each axis
|
|
const int32 BatchSize = GetDefault<ULidarPointCloudSettings>()->MeshingBatchSize;
|
|
|
|
// Determines the number of samples to perform
|
|
const FIntVector NumSamples((FVector)BoundsSize / ((BatchSize - 1) * CellSize) + 1);
|
|
const int32 TotalNumSamples = NumSamples.X * NumSamples.Y * NumSamples.Z;
|
|
|
|
// Precalculated for performance reasons
|
|
const float InversedCellSize = 1 / CellSize;
|
|
const FBox BaseSamplingBounds(Bounds.Min, Bounds.Min + BatchSize * CellSize);
|
|
TArray<TArray<uint32>> ColorLookupTable;
|
|
CalculateColorLookupTable(BatchSize, ColorLookupTable);
|
|
|
|
// Multithreading
|
|
const int32 MaxNumThreads = FMath::Min(FPlatformMisc::NumberOfCoresIncludingHyperthreads() - 1, TotalNumSamples);
|
|
TArray<TFuture<void>> ThreadResults;
|
|
FThreadSafeCounter SampleIndex = 0;
|
|
|
|
// Data storage
|
|
FCriticalSection VertexLock;
|
|
OutVertices.Empty();
|
|
|
|
TArray<uint8> VertexData = BuildScaledVertexTable(CellSize);
|
|
uint8* VertexDataPtr = VertexData.GetData();
|
|
|
|
// Make sure to acquire data release lock before loading nodes
|
|
FScopeLock ReleaseLock(&Octree->DataReleaseLock);
|
|
|
|
Octree->LoadAllNodes(false);
|
|
|
|
const FLidarPointCloudOctree* OctreeConst = Octree;
|
|
|
|
// Fire threads
|
|
for (int32 t = 0; t < MaxNumThreads && SampleIndex.GetValue() < TotalNumSamples; t++)
|
|
{
|
|
ThreadResults.Add(Async(EAsyncExecution::TaskGraph, [&SampleIndex, bUseSelection, &OutVertices, &VertexLock, VertexDataPtr, BatchSize,
|
|
OutNormals, OutColors, &ColorLookupTable, TotalNumSamples, NumSamples, BaseSamplingBounds, CellSize, OctreeConst, InversedCellSize, LocationOffset]
|
|
{
|
|
// Local caching arrays to reduce number of syncs required
|
|
TArray<FVector3f> _Vertices;
|
|
TArray<FVector3f> _Normals;
|
|
TArray<FVector3f>* _NormalsPtr = OutNormals ? &_Normals : nullptr;
|
|
TArray<FColor> _Colors;
|
|
TArray<FColor>* _ColorsPtr = OutColors ? &_Colors : nullptr;
|
|
|
|
const int32 NumCellsSq = BatchSize * BatchSize;
|
|
const int32 NumCellsCu = NumCellsSq * BatchSize;
|
|
|
|
uint8* VoxelizedGrid = new uint8[NumCellsCu];
|
|
FColor* ColorGrid = nullptr;
|
|
if(OutColors)
|
|
{
|
|
ColorGrid = new FColor[NumCellsCu];
|
|
}
|
|
|
|
const float OffsetMultiplier = (BatchSize - 1) * CellSize;
|
|
|
|
TArray64<const FLidarPointCloudPoint*> Selection;
|
|
|
|
int32 Index;
|
|
while ((Index = SampleIndex.Add(1)) < TotalNumSamples)
|
|
{
|
|
const int32 W = Index % (NumSamples.X * NumSamples.Y);
|
|
const FBox SamplingBounds = BaseSamplingBounds.ShiftBy(FVector(W % NumSamples.X, W / NumSamples.X, Index / (NumSamples.X * NumSamples.Y)) * OffsetMultiplier);
|
|
|
|
// Sample the data using the calculated bounds
|
|
#if WITH_EDITOR
|
|
if(bUseSelection)
|
|
{
|
|
OctreeConst->GetSelectedPointsInBox(Selection, SamplingBounds);
|
|
}
|
|
else
|
|
#endif
|
|
{
|
|
OctreeConst->GetPointsInBox(Selection, SamplingBounds, true);
|
|
}
|
|
|
|
// Skip if no points in selection
|
|
if (Selection.Num() == 0)
|
|
{
|
|
continue;
|
|
}
|
|
|
|
// Clear existing grids
|
|
FMemory::Memzero(VoxelizedGrid, NumCellsCu);
|
|
if(ColorGrid)
|
|
{
|
|
FMemory::Memzero(ColorGrid, NumCellsCu * sizeof(FColor));
|
|
}
|
|
|
|
// Calculate voxelized grids
|
|
for (const FLidarPointCloudPoint** Point = Selection.GetData(), **DataEnd = Selection.GetData() + Selection.Num(); Point != DataEnd; ++Point)
|
|
{
|
|
// Calculate location relative to sampling bounds
|
|
const FVector3f Location = (*Point)->Location - (FVector3f)SamplingBounds.Min;
|
|
|
|
// Calculate grid coordinates
|
|
const FIntVector Grid(FMath::Min(BatchSize - 1, (int32)(Location.X * InversedCellSize)), FMath::Min(BatchSize - 1, (int32)(Location.Y * InversedCellSize)), FMath::Min(BatchSize - 1, (int32)(Location.Z * InversedCellSize)));
|
|
|
|
const int32 GridIndex = Grid.Z * NumCellsSq + Grid.Y * BatchSize + Grid.X;
|
|
|
|
VoxelizedGrid[GridIndex] = 1;
|
|
|
|
if(ColorGrid)
|
|
{
|
|
ColorGrid[GridIndex] = (*Point)->Color;
|
|
}
|
|
}
|
|
|
|
ProcessGrid(VoxelizedGrid, VertexDataPtr, ColorGrid, ColorLookupTable, BatchSize, CellSize, (FVector3f)SamplingBounds.Min + LocationOffset, _Vertices, _NormalsPtr, _ColorsPtr);
|
|
}
|
|
|
|
// Clean up
|
|
delete[] VoxelizedGrid;
|
|
if(ColorGrid)
|
|
{
|
|
delete[] ColorGrid;
|
|
}
|
|
|
|
// Sync
|
|
FScopeLock Lock(&VertexLock);
|
|
OutVertices.Append(_Vertices);
|
|
if(OutNormals)
|
|
{
|
|
OutNormals->Append(_Normals);
|
|
}
|
|
if(OutColors)
|
|
{
|
|
OutColors->Append(_Colors);
|
|
}
|
|
}));
|
|
}
|
|
|
|
// Sync threads
|
|
for (const TFuture<void>& ThreadResult : ThreadResults)
|
|
{
|
|
ThreadResult.Get();
|
|
}
|
|
|
|
Octree->ReleaseAllNodes(false);
|
|
}
|
|
}
|
|
}
|
|
|
|
void LidarPointCloudMeshing::FMeshBuffers::Init(uint32 Capacity, bool bExpandIfNotEmpty, uint32& OutIndexOffset, uint32& OutVertexOffset)
|
|
{
|
|
if((Indices.IsEmpty() && Vertices.IsEmpty()) || !bExpandIfNotEmpty)
|
|
{
|
|
OutIndexOffset = OutVertexOffset = 0;
|
|
Indices.SetNumUninitialized(Capacity);
|
|
Vertices.Reserve(Capacity);
|
|
Bounds = FBoxSphereBounds(EForceInit::ForceInit);
|
|
}
|
|
else
|
|
{
|
|
OutIndexOffset = Indices.AddUninitialized(Capacity);
|
|
OutVertexOffset = Vertices.Num();
|
|
Vertices.Reserve(Vertices.Num() + Capacity);
|
|
}
|
|
}
|
|
|
|
void LidarPointCloudMeshing::FMeshBuffers::ExpandBounds(const FBox& NewBounds)
|
|
{
|
|
const FBoxSphereBounds ConvertedNewBounds(NewBounds);
|
|
Bounds = Bounds.BoxExtent == FVector::ZeroVector ? ConvertedNewBounds : Bounds + ConvertedNewBounds;
|
|
}
|
|
|
|
void LidarPointCloudMeshing::FMeshBuffers::NormalizeNormals()
|
|
{
|
|
for (FVertexData *Vertex = Vertices.GetData(), *DataEnd = Vertex + Vertices.Num(); Vertex != DataEnd; ++Vertex)
|
|
{
|
|
Vertex->Normal.Normalize();
|
|
}
|
|
}
|
|
|
|
void LidarPointCloudMeshing::CalculateNormals(FLidarPointCloudOctree* Octree, FThreadSafeBool* bCancelled, int32 Quality, float Tolerance, TArray64<FLidarPointCloudPoint*>& InPointSelection)
|
|
{
|
|
/** Groups sampling information together for readability */
|
|
struct FSamplingUnit
|
|
{
|
|
FVector3f Center;
|
|
FVector3f Extent;
|
|
TArray64<FLidarPointCloudPoint*> Points;
|
|
FLidarPointCloudOctreeNode* Node;
|
|
|
|
FSamplingUnit(const FVector3f& Center, const FVector3f& Extent, FLidarPointCloudOctreeNode* Node)
|
|
: Center(Center)
|
|
, Extent(Extent)
|
|
, Node(Node)
|
|
{
|
|
}
|
|
|
|
FSamplingUnit* ConstructChildAtLocation(int32 i)
|
|
{
|
|
return new FSamplingUnit(Center + Extent * (FVector3f(-0.5f) + FVector3f((i & 4) == 4, (i & 2) == 2, (i & 1) == 1)), Extent / 2, Node ? Node->GetChildNodeAtLocation(i) : nullptr);
|
|
}
|
|
};
|
|
|
|
int32 DesiredNumIterations = Quality;
|
|
|
|
const FLidarPointCloudNormal UpNormal = FVector3f::UpVector;
|
|
|
|
TArray64<int32> Indices;
|
|
|
|
TQueue<FSamplingUnit*> Q;
|
|
{
|
|
FSamplingUnit* Root = new FSamplingUnit(FVector3f::ZeroVector, Octree->SharedData[0].Extent, Octree->Root);
|
|
if (InPointSelection.Num() == 0)
|
|
{
|
|
Octree->GetPoints(Root->Points);
|
|
}
|
|
else
|
|
{
|
|
Root->Points = InPointSelection;
|
|
}
|
|
Q.Enqueue(Root);
|
|
}
|
|
FSamplingUnit* SamplingUnit;
|
|
while ((!bCancelled || !*bCancelled) && Q.Dequeue(SamplingUnit))
|
|
{
|
|
while (SamplingUnit->Points.Num() >= 3)
|
|
{
|
|
// Find Most Probable Plane
|
|
FPlane BestPlane(EForceInit::ForceInit);
|
|
{
|
|
TArray64<FLidarPointCloudPoint*>* Points;
|
|
bool bDestroyArray = false;
|
|
|
|
// If the sampling unit is attached to an existing node, use its grid-allocated points to pick random models from - much more accurate and faster
|
|
if (SamplingUnit->Node)
|
|
{
|
|
Points = new TArray64<FLidarPointCloudPoint*>();
|
|
Points->Reserve(SamplingUnit->Node->GetNumPoints());
|
|
FOR(Point, SamplingUnit->Node)
|
|
{
|
|
if (!Point->Normal.IsValid())
|
|
{
|
|
Points->Add(Point);
|
|
}
|
|
}
|
|
|
|
// This is a temporary array - need to destroy it after it's used
|
|
bDestroyArray = true;
|
|
}
|
|
// ... otherwise, just use whatever points are left
|
|
else
|
|
{
|
|
Points = &SamplingUnit->Points;
|
|
}
|
|
|
|
TMap<FIntVector, uint32> PlaneModels;
|
|
FIntVector CurrentModel;
|
|
|
|
TArray64<FLidarPointCloudPoint*>& SelectedPoints = *Points;
|
|
|
|
const int32 NumPoints = SelectedPoints.Num();
|
|
const int32 MaxPointIndex = NumPoints - 1;
|
|
const uint32 ConfidenceThreshold = NumPoints * 0.8f; // We are confident at 80% consensus
|
|
const uint32 ValidThreshold = NumPoints / 2; // We need at least 50% for consensus
|
|
|
|
Indices.Reset(Points->Num());
|
|
for (int32 i = 0; i < NumPoints; ++i)
|
|
{
|
|
Indices.Add(i);
|
|
}
|
|
|
|
const int32 NumIterations = FMath::Min((uint64)DesiredNumIterations, CalculateNumPermutations(NumPoints));
|
|
for (int32 i = 0; i < NumIterations; ++i)
|
|
{
|
|
// Find Random Model
|
|
do
|
|
{
|
|
int32 A = FMath::RandRange(0, MaxPointIndex);
|
|
int32 B = FMath::RandRange(0, MaxPointIndex - 1);
|
|
int32 C = FMath::RandRange(0, MaxPointIndex - 2);
|
|
int32 X = Indices[A];
|
|
Indices.RemoveAtSwap(A, EAllowShrinking::No);
|
|
int32 Y = Indices[B];
|
|
Indices.RemoveAtSwap(B, EAllowShrinking::No);
|
|
int32 Z = Indices[C];
|
|
Indices.Add(X);
|
|
Indices.Add(Y);
|
|
|
|
if (X > Y)
|
|
{
|
|
if (X > Z)
|
|
{
|
|
CurrentModel.X = X;
|
|
CurrentModel.Y = Y > Z ? Y : Z;
|
|
CurrentModel.Z = Y > Z ? Z : Y;
|
|
}
|
|
else
|
|
{
|
|
CurrentModel.X = Z;
|
|
CurrentModel.Y = X;
|
|
CurrentModel.Z = Y;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (Y > Z)
|
|
{
|
|
CurrentModel.X = Y;
|
|
CurrentModel.Y = X > Z ? X : Z;
|
|
CurrentModel.Z = X > Z ? Z : X;
|
|
}
|
|
else
|
|
{
|
|
CurrentModel.X = Z;
|
|
CurrentModel.Y = Y;
|
|
CurrentModel.Z = X;
|
|
}
|
|
}
|
|
} while (PlaneModels.Find(CurrentModel));
|
|
|
|
const FPlane Plane((FVector)SelectedPoints[CurrentModel.X]->Location, (FVector)SelectedPoints[CurrentModel.Y]->Location, (FVector)SelectedPoints[CurrentModel.Z]->Location);
|
|
|
|
// Count Inner Points
|
|
uint32 NumInnerPoints = 0;
|
|
for (FLidarPointCloudPoint** Point = SelectedPoints.GetData(), **DataEnd = Point + NumPoints; Point != DataEnd; ++Point)
|
|
{
|
|
if (FMath::Abs(Plane.PlaneDot((FVector)(*Point)->Location)) <= Tolerance)
|
|
{
|
|
++NumInnerPoints;
|
|
}
|
|
}
|
|
|
|
// Confidence is high enough, we can stop here
|
|
if (NumInnerPoints >= ConfidenceThreshold)
|
|
{
|
|
BestPlane = Plane;
|
|
break;
|
|
}
|
|
|
|
PlaneModels.Emplace(CurrentModel, NumInnerPoints);
|
|
}
|
|
|
|
// If the best plane has not been found yet, pick the highest scoring one
|
|
if (BestPlane.W == 0)
|
|
{
|
|
CurrentModel = FIntVector::NoneValue;
|
|
|
|
// Anything with less points then the Valid Threshold will not be considered
|
|
uint32 NumInnerPoints = ValidThreshold;
|
|
for (TPair<FIntVector, uint32>& Model : PlaneModels)
|
|
{
|
|
if (Model.Value > NumInnerPoints)
|
|
{
|
|
CurrentModel = Model.Key;
|
|
NumInnerPoints = Model.Value;
|
|
}
|
|
}
|
|
|
|
BestPlane = CurrentModel != FIntVector::NoneValue ? FPlane((FVector)SelectedPoints[CurrentModel.X]->Location, (FVector)SelectedPoints[CurrentModel.Y]->Location, (FVector)SelectedPoints[CurrentModel.Z]->Location) : FPlane(EForceInit::ForceInit);
|
|
}
|
|
|
|
// If the points array was created temporarily, destroy it
|
|
if (bDestroyArray)
|
|
{
|
|
delete Points;
|
|
Points = nullptr;
|
|
}
|
|
}
|
|
|
|
bool bSuccess = false;
|
|
|
|
if (BestPlane.W != 0)
|
|
{
|
|
const FLidarPointCloudNormal Normal = BestPlane;
|
|
|
|
int32 PointCount = SamplingUnit->Points.Num();
|
|
|
|
// Apply Normals
|
|
for (FLidarPointCloudPoint** DataStart = SamplingUnit->Points.GetData(), **Point = DataStart, **DataEnd = Point + SamplingUnit->Points.Num(); Point != DataEnd; ++Point)
|
|
{
|
|
if (FMath::Abs(BestPlane.PlaneDot((FVector)(*Point)->Location)) <= Tolerance)
|
|
{
|
|
(*Point)->Normal = Normal;
|
|
--DataEnd;
|
|
SamplingUnit->Points.RemoveAtSwap(Point-- - DataStart, EAllowShrinking::No);
|
|
}
|
|
}
|
|
|
|
bSuccess = PointCount - SamplingUnit->Points.Num() > 0;
|
|
}
|
|
|
|
if(!bSuccess)
|
|
{
|
|
FSamplingUnit* Sublevels[8];
|
|
for (int32 i = 0; i < 8; ++i)
|
|
{
|
|
Sublevels[i] = SamplingUnit->ConstructChildAtLocation(i);
|
|
}
|
|
|
|
// Build and enqueue sub-levels
|
|
for (FLidarPointCloudPoint** Point = SamplingUnit->Points.GetData(), **DataEnd = Point + SamplingUnit->Points.Num(); Point != DataEnd; ++Point)
|
|
{
|
|
Sublevels[((*Point)->Location.X > SamplingUnit->Center.X ? 4 : 0) + ((*Point)->Location.Y > SamplingUnit->Center.Y ? 2 : 0) + ((*Point)->Location.Z > SamplingUnit->Center.Z)]->Points.Add(*Point);
|
|
}
|
|
|
|
SamplingUnit->Points.Empty();
|
|
|
|
// Enqueue
|
|
for (int32 i = 0; i < 8; ++i)
|
|
{
|
|
if (Sublevels[i]->Points.Num() > 0)
|
|
{
|
|
Q.Enqueue(Sublevels[i]);
|
|
}
|
|
else
|
|
{
|
|
delete Sublevels[i];
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// To any stray points left simply apply Up Vector
|
|
for (FLidarPointCloudPoint* P : SamplingUnit->Points)
|
|
{
|
|
P->Normal = UpNormal;
|
|
}
|
|
|
|
// Delete when finished processing
|
|
delete SamplingUnit;
|
|
SamplingUnit = nullptr;
|
|
}
|
|
}
|
|
|
|
void LidarPointCloudMeshing::BuildCollisionMesh(FLidarPointCloudOctree* Octree, const float& CellSize, FTriMeshCollisionData* CollisionMesh)
|
|
{
|
|
if (!CollisionMesh)
|
|
{
|
|
return;
|
|
}
|
|
|
|
TArray<FVector3f> Vertices;
|
|
MarchingCubes::Run(Octree, CellSize, false, Vertices, nullptr, nullptr);
|
|
|
|
FScopeBenchmarkTimer Timer("Exporting Collision Data");
|
|
|
|
CollisionMesh->Vertices.Empty(Vertices.Num());
|
|
CollisionMesh->Indices.Empty(Vertices.Num() / 3);
|
|
CollisionMesh->Indices.AddUninitialized(Vertices.Num() / 3);
|
|
|
|
bool bInSet = false;
|
|
TSet<FVector3f> TmpVertexData;
|
|
TmpVertexData.Reserve(Vertices.Num());
|
|
uint32* IndicesPtr = (uint32*)CollisionMesh->Indices.GetData();
|
|
for (FVector3f *Vertex = Vertices.GetData(), *DataEnd = Vertex + Vertices.Num(); Vertex != DataEnd; ++Vertex, ++IndicesPtr)
|
|
{
|
|
*IndicesPtr = TmpVertexData.Add(*Vertex, &bInSet).AsInteger();
|
|
if (!bInSet)
|
|
{
|
|
CollisionMesh->Vertices.Add(*Vertex);
|
|
}
|
|
}
|
|
|
|
CollisionMesh->Vertices.Shrink();
|
|
}
|
|
|
|
void LidarPointCloudMeshing::BuildStaticMeshBuffers(FLidarPointCloudOctree* Octree, const float& CellSize, bool bUseSelection, FMeshBuffers* OutMeshBuffers, const FTransform& Transform)
|
|
{
|
|
if(OutMeshBuffers == nullptr)
|
|
{
|
|
return;
|
|
}
|
|
|
|
TArray<FVector3f> Vertices;
|
|
TArray<FVector3f> Normals;
|
|
TArray<FColor> Colors;
|
|
MarchingCubes::Run(Octree, CellSize, bUseSelection, Vertices, &Normals, &Colors);
|
|
|
|
FScopeBenchmarkTimer Timer("Building Static Mesh Buffers");
|
|
|
|
FBox Bounds(EForceInit::ForceInit);
|
|
uint32 IndexOffset;
|
|
uint32 VertexOffset;
|
|
OutMeshBuffers->Init(Vertices.Num(), true, IndexOffset, VertexOffset);
|
|
|
|
// We do not support doubles yet
|
|
const FTransform3f Transform3f = (FTransform3f)Transform;
|
|
|
|
bool bInSet = false;
|
|
TSet<FIntVector3> TmpVertexData;
|
|
TmpVertexData.Reserve(Vertices.Num());
|
|
uint32* IndicesPtr = OutMeshBuffers->Indices.GetData() + IndexOffset;
|
|
FVector3f* NormalsPtr = Normals.GetData();
|
|
FColor* ColorsPtr = Colors.GetData();
|
|
for (FVector3f *Vertex = Vertices.GetData(), *DataEnd = Vertex + Vertices.Num(); Vertex != DataEnd; ++Vertex, ++IndicesPtr, ++NormalsPtr, ++ColorsPtr)
|
|
{
|
|
*IndicesPtr = TmpVertexData.Add(FIntVector3((FVector)(*Vertex * 100)), &bInSet).AsInteger() + VertexOffset;
|
|
if (bInSet)
|
|
{
|
|
OutMeshBuffers->Vertices[*IndicesPtr].Normal += *NormalsPtr;
|
|
}
|
|
else
|
|
{
|
|
const FVector3f Location = Transform3f.TransformPosition(*Vertex);
|
|
OutMeshBuffers->Vertices.Emplace(Location, *NormalsPtr, *ColorsPtr);
|
|
Bounds += (FVector)Location;
|
|
}
|
|
};
|
|
|
|
OutMeshBuffers->ExpandBounds(Bounds);
|
|
OutMeshBuffers->NormalizeNormals();
|
|
}
|