thick raycast + iterator testing

.github/workflows/ubuntu.yaml

@@ -20,13 +20,23 @@ jobs:
         build_type: [ Debug, Release ]
 
     steps:
+    - name: Install core dependencies
+      uses: awalsh128/cache-apt-pkgs-action@latest
+      with:
+        packages: ninja-build cmake libtbb-dev libeigen3-dev libglm-dev libfmt-dev
+        version: 1.0
 
-    - name: Install dependencies
-      run: sudo apt update && sudo apt install -y ninja-build cmake libtbb-dev libeigen3-dev libglm-dev libfmt-dev
-    - name: Build and install LVR2 dependency
+    # - name: Install lvr2 dependencies
+    #   uses: awalsh128/cache-apt-pkgs-action@latest
+    #   with:
+    #     packages: libflann-dev libopenmpi-dev libtiff-dev libboost-all-dev freeglut3-dev libyaml-cpp-dev libopencv-dev libcgal-dev libgdal-dev libgsl-dev libomp-dev
+    #     version: 1.0
+
+    - name: Build and install LVR2
       run: |
-        sudo apt update && sudo apt install -y libflann-dev libopenmpi-dev libtiff-dev libboost-all-dev freeglut3-dev libyaml-cpp-dev libopencv-dev libcgal-dev libgdal-dev libgsl-dev libomp-dev
-        git clone https://github.com/uos/lvr2.git && cd lvr2
+        sudo apt update && sudo apt install libflann-dev libopenmpi-dev libtiff-dev libboost-all-dev \
+          freeglut3-dev libyaml-cpp-dev libopencv-dev libcgal-dev libgdal-dev libgsl-dev libomp-dev
+        git clone https://github.com/uos/lvr2.git --depth 1 && cd lvr2
         cmake -B build -G Ninja                     \
           -DWITH_CUDA=OFF                           \
           -DCMAKE_CXX_COMPILER=${{matrix.compiler}} \

include/chad/cluster.hpp

@@ -5,17 +5,18 @@
 #include <algorithm>
 
 namespace chad {
-    namespace detail {
+    // cluster of 8 separate 8-bit leaves
+    struct LeafCluster {
         // wrapper for signed distance cluster
         struct TSDFs {
             // set 8 bits to represent signed distance, normalized within truncation distance
-            void inline set(uint8_t leaf_i, float signed_distance, float truncation_distance_recip) noexcept {
+            void inline set(uint8_t leaf_i, float signed_distance, float sdf_trunc_recip) noexcept {
                 // absolute value range for signed distances stored as integers
                 static constexpr uint64_t sd_range_abs = std::numeric_limits<uint8_t>::max() / 2;
 
                 float sd = signed_distance;
                 // scale signed distance to be normalized within truncation distance
-                sd = std::clamp(sd * truncation_distance_recip, -1.0f, 1.0f);
+                sd = std::clamp(sd * sdf_trunc_recip, -1.0f, 1.0f);
                 // scale up to fit 8-bit integer, add offset to fit within unsigned
                 sd = sd * float(sd_range_abs) + float(sd_range_abs); // range should be [-127, 128]
 
@@ -30,7 +31,7 @@ namespace chad {
                 _value |= uint64_t(0xff) << uint64_t(leaf_i * 8);
             }
             // retrieve signed distance from single leaf if it is not empty
-            auto inline try_get(uint8_t leaf_i, float truncation_distance) const -> std::pair<float, bool> {
+            auto inline try_get(uint8_t leaf_i, float sdf_trunc) const -> std::pair<float, bool> {
                 // absolute value range for signed distances stored as integers
                 static constexpr uint64_t sd_range_abs = std::numeric_limits<uint8_t>::max() / 2;
 
@@ -46,7 +47,7 @@ namespace chad {
                 // scale signed distance back up to normalized [-1, 1]
                 signed_distance *= float(1.0 / float(sd_range_abs));
                 // scale back to denormalized signed distance
-                signed_distance *= truncation_distance;
+                signed_distance *= sdf_trunc;
                 return { signed_distance, true };
             }
             uint64_t _value;
@@ -66,20 +67,40 @@ namespace chad {
             // }
             uint64_t _value;
         };
-    }
+        // wrapper for cluster of 8 unsigned floats (+0.0f to +1.0f)
+        struct Ufloats {
+            // TODO
+        };
+        // wrapper for cluster of 8 signed floats (-1.0f to +1.0f)
+        struct Sfloats {
+            // TODO
+        };
+        // wrapper for cluster of 8 uint8_t values (0xff is reserved)
+        struct Uints {
+            // TODO
+        };
+        // wrapper for cluster of 8 int8_t values (0xff is reserved)
+        struct Sints {
+            // TODO
+        };
 
-    // cluster of 8 separate 8-bit leaves
-    struct LeafCluster {
-        LeafCluster(): _value(0) {}
-
-        bool is_empty() {
+        LeafCluster(): _value(0) {
+        }
+        bool inline is_empty() {
             return _value == std::numeric_limits<uint64_t>::max();
         }
+        bool inline operator==(const LeafCluster& other) const noexcept {
+            return _value == other._value;
+        }
 
         union {
-            uint64_t        _value;
-            detail::TSDFs   _tsdfs;
-            detail::Weights _weigh;
+            uint64_t _value;
+            TSDFs    _tsdfs;
+            Weights  _weigh;
+            Ufloats  _ufloats;
+            Sfloats  _sfloats;
+            Uints    _uints;
+            Sints    _sints;
         };
     };
 }

include/chad/detail/levels.hpp

@@ -55,9 +55,8 @@ namespace chad::detail {
         }
         // add a single node with the given children and return its address
         auto add(const std::array<uint32_t, 8>& children) -> uint32_t {
-
             // check if theres enough space for a placeholder node
-            if (_occupied_n + 9 < _raw_data.size()) {
+            if (_occupied_n + 9 >= _raw_data.size()) {
                 _raw_data.resize(_raw_data.size() + 9);
             }
 
@@ -161,7 +160,22 @@ namespace chad::detail {
             else return 0;
         }
 
-        auto try_get_leaf_cluster(uint32_t parent_addr, uint8_t child_i) const -> std::pair<LeafCluster, bool> {
+        auto try_get_lc_addr(uint32_t parent_addr, uint8_t child_i) const -> std::pair<uint32_t, bool> {
+            uint32_t child_mask = _nodes[MAX_DEPTH - 1]._raw_data[parent_addr];
+            uint32_t child_bit = uint32_t(1) << child_i;
+
+            // check if the leaf cluster exists
+            if (child_mask & child_bit) {
+                // count the number of children that are stored before this one
+                uint8_t masked = uint8_t(child_mask & (child_bit - 1));
+                uint8_t child_count = std::popcount(masked);
+                // child count will correspond to the requested child's index
+                uint32_t child_addr = _nodes.back()._raw_data[parent_addr + uint32_t(child_count + 1)];
+                return { child_addr, true };
+            }
+            else return { {}, false };
+        }
+        auto try_get_lc(uint32_t parent_addr, uint8_t child_i) const -> std::pair<LeafCluster, bool> {
             uint32_t child_mask = _nodes[MAX_DEPTH - 1]._raw_data[parent_addr];
             uint32_t child_bit = uint32_t(1) << child_i;
 

include/chad/detail/morton.hpp

@@ -1,4 +1,6 @@
 #pragma once
+#include <array>
+#include <vector>
 #include <functional>
 #if !defined(__BMI2__)
 #   error "Requires BMI2 instruction set"
@@ -9,7 +11,11 @@
 namespace chad::detail {
     struct MortonCode {
         MortonCode(uint64_t value): _value(value) {}
-        MortonCode(glm::ivec3 vox_pos) noexcept {
+        MortonCode(const glm::ivec3& vox_pos) noexcept {
+            encode(vox_pos);
+        }
+
+        void inline encode(const glm::ivec3& vox_pos) noexcept {
             // truncate from 32-bit int to 21-bit int
             uint32_t x, y, z;
             x = (1 << 20) + uint32_t(vox_pos.x);
@@ -26,18 +32,26 @@ namespace chad::detail {
             z -= 1 << 20;
             return { int32_t(x), int32_t(y), int32_t(z) };
         }
+
         bool inline operator==(const MortonCode& other) const noexcept {
             return _value == other._value;
         }
+        bool inline operator<(const MortonCode& other) const noexcept {
+            return _value < other._value;
+        }
+        bool inline operator>(const MortonCode& other) const noexcept {
+            return _value > other._value;
+        }
 
         uint64_t _value;
     };
 
     using MortonVector = std::vector<std::pair<glm::vec3, MortonCode>>;
-    auto calc_morton_vector(const std::vector<std::array<float, 3>>& points, const float voxel_resolution) -> MortonVector;
+    auto calc_morton_vector(const std::vector<std::array<float, 3>>& points, const float sdf_res) -> MortonVector;
     auto sort_morton_vector(MortonVector& points_mc) -> std::vector<glm::vec3>;
 }
 
+// overload the standard hashing operator for MortonCode
 namespace std {
     template<> struct hash<chad::detail::MortonCode> {
         size_t inline operator()(const chad::detail::MortonCode& mc) const noexcept {

include/chad/detail/octree.hpp

@@ -1,7 +1,10 @@
 #pragma once
 #include <array>
 #include <cstdint>
+#include <fmt/base.h>
 #include <gtl/phmap.hpp>
+#include <glm/glm.hpp>
+#include <glm/gtc/type_aligned.hpp>
 #include "chad/detail/morton.hpp"
 #include "chad/detail/virtual_array.hpp"
 
@@ -15,6 +18,7 @@ namespace chad::detail {
             _nodes.push_back({ 0, 0, 0, 0, 0, 0, 0, 0 }); // root node
             _leaves.push_back({}); // dummy leaf node
         }
+
         void clear() {
             _node_lookup.clear();
             _nodes.clear();
@@ -23,6 +27,7 @@ namespace chad::detail {
             _nodes.push_back({ 0, 0, 0, 0, 0, 0, 0, 0 }); // root node
             _leaves.push_back({}); // dummy leaf node
         }
+        // insert single leaf
         auto inline insert(MortonCode mc) -> Leaf& {
             // see if node at given level has been created already
             static constexpr uint32_t lookup_depth = 18;
@@ -71,6 +76,116 @@ namespace chad::detail {
 
             return _leaves[leaf_addr];
         }
+        // insert TSDFs via points and normals
+        void insert(const std::vector<glm::vec3>& points, const std::vector<glm::vec3>& normals, const glm::vec3 position, float sdf_res, float sdf_trunc) {
+            auto beg = std::chrono::high_resolution_clock::now();
+            const float sdf_res_recip = float(1.0 / double(sdf_res));
+            const glm::aligned_vec3 position_aligned = position;
+
+            gtl::flat_hash_set<MortonCode> traversed_voxels;
+            for (size_t i = 0; i < points.size(); i++) {
+                const glm::aligned_vec3 point = points[i];
+                const glm::aligned_vec3 normal = normals[i];
+
+                // get all voxels along ray within truncation distance via variant of DDA line algorithm (-> "A fast voxel traversal algorithm for ray tracing")
+                // as Bresehnham's line algorithm misses some voxels
+                const glm::aligned_vec3 direction = glm::normalize(point - position_aligned);
+                const glm::aligned_vec3 direction_recip = 1.0f / direction;
+                const glm::aligned_vec3 start = point - direction * (sdf_trunc - 0.5f*sdf_res); // add flooring bias
+                const glm::aligned_vec3 final = point + direction * (sdf_trunc + 0.5f*sdf_res); // add flooring bias
+                const glm::aligned_ivec3 voxel_start = glm::aligned_ivec3(glm::floor(start * sdf_res_recip));
+                const glm::aligned_ivec3 voxel_final = glm::aligned_ivec3(glm::floor(final * sdf_res_recip));
+
+                // stepN: direction of increment for each dimension
+                const glm::aligned_ivec3 voxel_step_direction = glm::sign(voxel_final - voxel_start);
+                // tDeltaN: portion of "direction" needed to traverse full voxel
+                const glm::aligned_vec3 voxel_step_delta = glm::abs(sdf_res * direction_recip);
+                // tMaxN: portion of "direction" needed to traverse current voxel
+                glm::aligned_vec3 voxel_step_max;
+                // for x
+                if      (voxel_step_direction.x < 0) voxel_step_max.x = sdf_res * std::floor(start.x * sdf_res_recip);
+                else if (voxel_step_direction.x > 0) voxel_step_max.x = sdf_res * std::ceil (start.x * sdf_res_recip);
+                else /*voxel_step_direction.x == 0*/ voxel_step_max.x = std::numeric_limits<float>::max();
+                // for y
+                if      (voxel_step_direction.y < 0) voxel_step_max.y = sdf_res * std::floor(start.y * sdf_res_recip);
+                else if (voxel_step_direction.y > 0) voxel_step_max.y = sdf_res * std::ceil (start.y * sdf_res_recip);
+                else /*voxel_step_direction.x == 0*/ voxel_step_max.y = std::numeric_limits<float>::max();
+                // for z
+                if      (voxel_step_direction.z < 0) voxel_step_max.z = sdf_res * std::floor(start.z * sdf_res_recip);
+                else if (voxel_step_direction.z > 0) voxel_step_max.z = sdf_res * std::ceil (start.z * sdf_res_recip);
+                else /*voxel_step_direction.x == 0*/ voxel_step_max.z = std::numeric_limits<float>::max();
+                voxel_step_max = voxel_step_max - start; // distance to voxel boundaries
+                voxel_step_max = glm::abs(voxel_step_max * direction_recip); // portion of "direction" needed to cross voxel boundaries
+
+                // current voxel during traversal
+                glm::ivec3 voxel_current = voxel_start;
+
+                // add all 8 corner voxels at start voxel
+                for (uint8_t x = 0; x < 2; x++) {
+                    for (uint8_t y = 0; y < 2; y++) {
+                        for (uint8_t z = 0; z < 2; z++) {
+                            MortonCode mc{ voxel_current + glm::ivec3{ x, y, z } };
+                            traversed_voxels.emplace(mc);
+                        }
+                    }
+                }
+
+                // traverse ray within truncation distance
+                while (true) {
+                    if (voxel_step_max.x < voxel_step_max.y) {
+                        if (voxel_step_max.x < voxel_step_max.z) {
+                            voxel_current.x += voxel_step_direction.x; // step in x direction
+                            voxel_step_max.x += voxel_step_delta.x; // update for next voxel boundary
+                            if (voxel_current.x == voxel_final.x + voxel_step_direction.x) break;
+                        }
+                        else {
+                            voxel_current.z += voxel_step_direction.z; // step in z direction
+                            voxel_step_max.z += voxel_step_delta.z; // update for next voxel boundary
+                            if (voxel_current.z == voxel_final.z + voxel_step_direction.z) break;
+                        }
+                    }
+                    else {
+                        if (voxel_step_max.y < voxel_step_max.z) {
+
+                            voxel_current.y += voxel_step_direction.y; // step in y direction
+                            voxel_step_max.y += voxel_step_delta.y; // update for next voxel boundary
+                            if (voxel_current.y == voxel_final.y + voxel_step_direction.y) break;
+                        }
+                        else {
+                            voxel_current.z += voxel_step_direction.z; // step in z direction
+                            voxel_step_max.z += voxel_step_delta.z; // update for next voxel boundary
+                            if (voxel_current.z == voxel_final.z + voxel_step_direction.z) break;
+                        }
+                    }
+
+                    // add all 8 corner voxels
+                    for (uint8_t x = 0; x < 2; x++) {
+                        for (uint8_t y = 0; y < 2; y++) {
+                            for (uint8_t z = 0; z < 2; z++) {
+                                MortonCode mc{ voxel_current + glm::ivec3{ x, y, z } };
+                                traversed_voxels.emplace(mc);
+                            }
+                        }
+                    }
+                }
+
+                for (const MortonCode& voxel: traversed_voxels) {
+                    auto& leaf = insert(voxel);
+
+                    // compute signed distance
+                    glm::aligned_vec3 point_to_voxel = glm::aligned_vec3(voxel.decode()) * sdf_res - point;
+                    float signed_distance = glm::dot(normal, point_to_voxel);
+                    // weighted average with incremented weight
+                    leaf._signed_distance = leaf._signed_distance * leaf._weight + signed_distance;
+                    leaf._weight++;
+                    leaf._signed_distance = leaf._signed_distance / leaf._weight;
+                }
+                traversed_voxels.clear();
+            }
+            auto end = std::chrono::high_resolution_clock::now();
+            auto dur = std::chrono::duration<double, std::milli> (end - beg).count();
+            fmt::println("oct  upd {:.2f}", dur);
+        }
 
         auto static get_root() -> uint32_t {
             return 0;