CollisionDetection.h

Go to the documentation of this file.

 
#ifndef COLLISION_DETECTION_H
#define COLLISION_DETECTION_H
 
#include <queue>
#include <set>
#include <unordered_map>
#include <unordered_set>
#include "Context.h"
 
class CollisionDetection {
public:
    // -------- CONE COLLISION STRUCTURES --------
 
    struct Cone {
        helios::vec3 apex; 
        helios::vec3 axis; 
        float half_angle; 
        float height; 
    };
 
    struct AngularBins {
        int theta_divisions; 
        int phi_divisions; 
        float angular_resolution; 
 
        // Coverage data using packed bits for memory efficiency
        std::vector<uint8_t> coverage_bits; 
        std::vector<float> depth_values; 
 
        AngularBins(int theta_div, int phi_div) : theta_divisions(theta_div), phi_divisions(phi_div) {
            int total_bins = theta_div * phi_div;
            coverage_bits.resize((total_bins + 7) / 8, 0); // Packed bits
            depth_values.resize(total_bins, std::numeric_limits<float>::max());
            angular_resolution = (2.0f * M_PI) / float(theta_div * phi_div);
        }
 
        // Fast bit operations for coverage testing
        bool isCovered(int theta, int phi) const {
            int index = theta * phi_divisions + phi;
            return coverage_bits[index >> 3] & (1 << (index & 7));
        }
 
        void setCovered(int theta, int phi, float depth) {
            int index = theta * phi_divisions + phi;
            coverage_bits[index >> 3] |= (1 << (index & 7));
            depth_values[index] = std::min(depth_values[index], depth);
        }
 
        void clear() {
            std::fill(coverage_bits.begin(), coverage_bits.end(), 0);
            std::fill(depth_values.begin(), depth_values.end(), std::numeric_limits<float>::max());
        }
    };
 
    // -------- GENERIC RAY-TRACING STRUCTURES --------
 
    struct RayQuery {
        helios::vec3 origin; 
        helios::vec3 direction; 
        float max_distance; 
        std::vector<uint> target_UUIDs; 
 
        RayQuery() : origin(0, 0, 0), direction(0, 0, 1), max_distance(-1.0f) {
        }
        RayQuery(const helios::vec3 &ray_origin, const helios::vec3 &ray_direction, float max_dist = -1.0f, const std::vector<uint> &targets = {}) : origin(ray_origin), direction(ray_direction), max_distance(max_dist), target_UUIDs(targets) {
        }
    };
 
    struct HitResult {
        bool hit; 
        float distance; 
        uint primitive_UUID; 
        helios::vec3 intersection_point; 
        helios::vec3 normal; 
        float path_length; 
 
        HitResult() : hit(false), distance(-1.0f), primitive_UUID(0), intersection_point(0, 0, 0), normal(0, 0, 0), path_length(0.0f) {
        }
    };
 
    struct RayTracingStats {
        size_t total_rays_cast; 
        size_t total_hits; 
        size_t bvh_nodes_visited; 
        float average_ray_distance; 
 
        RayTracingStats() : total_rays_cast(0), total_hits(0), bvh_nodes_visited(0), average_ray_distance(0.0f) {
        }
    };
 
    static constexpr size_t WARP_SIZE = 32; 
    static constexpr size_t RAY_BATCH_SIZE = 1024; 
 
    struct RayPacket {
        // Ray data (Structure-of-Arrays layout)
        std::vector<helios::vec3> origins; 
        std::vector<helios::vec3> directions; 
        std::vector<float> max_distances; 
        std::vector<std::vector<uint>> target_UUIDs; 
 
        // Results (output)
        std::vector<HitResult> results; 
 
        size_t ray_count = 0; 
 
        RayPacket() = default;
 
        void reserve(size_t capacity) {
            origins.reserve(capacity);
            directions.reserve(capacity);
            max_distances.reserve(capacity);
            target_UUIDs.reserve(capacity);
            results.reserve(capacity);
        }
 
        void addRay(const RayQuery &query) {
            origins.push_back(query.origin);
            directions.push_back(query.direction);
            max_distances.push_back(query.max_distance);
            target_UUIDs.push_back(query.target_UUIDs);
            results.emplace_back(); // Initialize empty result
            ray_count++;
        }
 
        void clear() {
            origins.clear();
            directions.clear();
            max_distances.clear();
            target_UUIDs.clear();
            results.clear();
            ray_count = 0;
        }
 
        [[nodiscard]] std::vector<RayQuery> toRayQueries() const {
            std::vector<RayQuery> queries;
            queries.reserve(ray_count);
            for (size_t i = 0; i < ray_count; ++i) {
                queries.emplace_back(origins[i], directions[i], max_distances[i], target_UUIDs[i]);
            }
            return queries;
        }
 
        [[nodiscard]] size_t getMemoryUsage() const {
            size_t base_memory = (origins.size() + directions.size()) * sizeof(helios::vec3) + max_distances.size() * sizeof(float) + results.size() * sizeof(HitResult);
 
            // Add memory for target UUIDs
            for (const auto &targets: target_UUIDs) {
                base_memory += targets.size() * sizeof(uint);
            }
 
            return base_memory;
        }
    };
 
    struct RayStream {
        std::vector<RayPacket> packets; 
        size_t current_packet = 0; 
        size_t total_rays = 0; 
 
        void addRays(const std::vector<RayQuery> &queries) {
            for (const auto &query: queries) {
                // Create new packet if current one is full
                if (packets.empty() || packets.back().ray_count >= RAY_BATCH_SIZE) {
                    packets.emplace_back();
                    packets.back().reserve(RAY_BATCH_SIZE);
                }
 
                packets.back().addRay(query);
                total_rays++;
            }
        }
 
        [[nodiscard]] std::vector<HitResult> getAllResults() const {
            std::vector<HitResult> all_results;
            all_results.reserve(total_rays);
 
            for (const auto &packet: packets) {
                all_results.insert(all_results.end(), packet.results.begin(), packet.results.end());
            }
 
            return all_results;
        }
 
        void clear() {
            packets.clear();
            current_packet = 0;
            total_rays = 0;
        }
 
        [[nodiscard]] size_t getMemoryUsage() const {
            size_t total_memory = 0;
            for (const auto &packet: packets) {
                total_memory += packet.getMemoryUsage();
            }
            return total_memory;
        }
    };
 
    struct OptimalPathResult {
        helios::vec3 direction; 
        int collisionCount; 
        float confidence; 
    };
 
    explicit CollisionDetection(helios::Context *context);
 
    ~CollisionDetection();
 
    // -------- PRIMITIVE/OBJECT COLLISION DETECTION --------
 
    std::vector<uint> findCollisions(uint UUID, bool allow_spatial_culling = true);
 
    std::vector<uint> findCollisions(const std::vector<uint> &UUIDs, bool allow_spatial_culling = true);
 
    std::vector<uint> findCollisions(const std::vector<uint> &primitive_UUIDs, const std::vector<uint> &object_IDs, bool allow_spatial_culling = true);
 
    std::vector<uint> findCollisions(const std::vector<uint> &query_UUIDs, const std::vector<uint> &query_object_IDs, const std::vector<uint> &target_UUIDs, const std::vector<uint> &target_object_IDs, bool allow_spatial_culling = true);
 
    // -------- GENERIC RAY-TRACING INTERFACE --------
 
    HitResult castRay(const RayQuery &ray_query);
 
    HitResult castRay(const helios::vec3 &origin, const helios::vec3 &direction, float max_distance = -1.0f, const std::vector<uint> &target_UUIDs = {});
 
    std::vector<HitResult> castRays(const std::vector<RayQuery> &ray_queries, RayTracingStats *stats = nullptr);
 
    // -------- OPTIMIZATION METHODS --------
 
    enum class BVHOptimizationMode {
        SOA_UNCOMPRESSED 
    };
 
    void setBVHOptimizationMode(BVHOptimizationMode mode);
 
    BVHOptimizationMode getBVHOptimizationMode() const;
 
    std::vector<HitResult> castRaysOptimized(const std::vector<RayQuery> &ray_queries, RayTracingStats *stats = nullptr);
 
#ifdef HELIOS_CUDA_AVAILABLE
    std::vector<HitResult> castRaysGPU(const std::vector<RayQuery> &ray_queries, RayTracingStats &stats);
#endif
 
    bool processRayStream(RayStream &ray_stream, RayTracingStats *stats = nullptr);
 
    struct MemoryUsageStats {
        size_t soa_memory_bytes = 0;
        size_t quantized_memory_bytes = 0;
        float quantized_reduction_percent = 0.0f; // Reduction vs SoA
    };
    MemoryUsageStats getBVHMemoryUsage() const;
 
    std::vector<std::vector<std::vector<std::vector<HitResult>>>> performGridRayIntersection(const helios::vec3 &grid_center, const helios::vec3 &grid_size, const helios::int3 &grid_divisions, const std::vector<RayQuery> &ray_queries);
 
    std::vector<std::vector<HitResult>> calculateVoxelPathLengths(const helios::vec3 &scan_origin, const std::vector<helios::vec3> &ray_directions, const std::vector<helios::vec3> &voxel_centers, const std::vector<helios::vec3> &voxel_sizes);
 
    void calculateRayPathLengthsDetailed(const helios::vec3 &grid_center, const helios::vec3 &grid_size, const helios::int3 &grid_divisions, const std::vector<helios::vec3> &ray_origins, const std::vector<helios::vec3> &ray_directions,
                                         std::vector<HitResult> &hit_results);
 
    // -------- CONE INTERSECTION QUERIES --------
 
    OptimalPathResult findOptimalConePath(const helios::vec3 &apex, const helios::vec3 &centralAxis, float half_angle, float height = 0.0f, int initialSamples = 256);
 
 
    // -------- GRID-BASED INTERSECTION --------
 
    void calculateGridIntersection(const helios::vec3 &grid_center, const helios::vec3 &grid_size, const helios::int3 &grid_divisions, const std::vector<uint> &UUIDs = {});
 
    std::vector<std::vector<std::vector<std::vector<uint>>>> getGridCells();
 
    std::vector<uint> getGridIntersections(int i, int j, int k);
 
    // -------- PRIMITIVE SLICING OPERATIONS --------
 
    std::vector<uint> slicePrimitive(uint UUID, const std::vector<helios::vec3> &voxel_face_vertices, helios::WarningAggregator &warnings);
 
    std::vector<uint> slicePrimitivesUsingGrid(const std::vector<uint> &UUIDs, const helios::vec3 &grid_center, const helios::vec3 &grid_size, const helios::int3 &grid_divisions);
 
    void calculatePrimitiveVoxelIntersection(const std::vector<uint> &UUIDs = {});
 
    // -------- GEOMETRIC UTILITY FUNCTIONS --------
 
    helios::vec3 linesIntersection(const helios::vec3 &line1_point, const helios::vec3 &line1_direction, const helios::vec3 &line2_point, const helios::vec3 &line2_direction) const;
 
    bool approxSame(float a, float b, float absTol, float relTol) const;
 
    bool approxSame(const helios::vec3 &a, const helios::vec3 &b, float absTol) const;
 
    helios::vec2 interpolate_texture_UV_to_slice_point(const helios::vec3 &p1, const helios::vec2 &uv1, const helios::vec3 &p2, const helios::vec2 &uv2, const helios::vec3 &ps) const;
 
    // -------- VOXEL RAY PATH LENGTH CALCULATIONS --------
 
    void calculateVoxelRayPathLengths(const helios::vec3 &grid_center, const helios::vec3 &grid_size, const helios::int3 &grid_divisions, const std::vector<helios::vec3> &ray_origins, const std::vector<helios::vec3> &ray_directions);
 
    void setVoxelTransmissionProbability(int P_denom, int P_trans, const helios::int3 &ijk);
 
    void getVoxelTransmissionProbability(const helios::int3 &ijk, int &P_denom, int &P_trans) const;
 
    void setVoxelRbar(float r_bar, const helios::int3 &ijk);
 
    float getVoxelRbar(const helios::int3 &ijk) const;
 
    void getVoxelRayHitCounts(const helios::int3 &ijk, int &hit_before, int &hit_after, int &hit_inside) const;
 
    std::vector<float> getVoxelRayPathLengths(const helios::int3 &ijk) const;
 
    void clearVoxelData();
 
    // -------- FEATURE 4: COLLISION MINIMIZATION --------
 
    int optimizeLayout(const std::vector<uint> &UUIDs, float learning_rate = 0.01f, int max_iterations = 1000);
 
    // -------- SPATIAL OPTIMIZATION --------
 
    std::vector<std::pair<uint, uint>> findCollisionsWithinDistance(const std::vector<uint> &query_UUIDs, const std::vector<uint> &target_UUIDs, float max_distance);
 
    void setMaxCollisionDistance(float distance);
 
    [[nodiscard]] float getMaxCollisionDistance() const;
 
    std::vector<uint> filterGeometryByDistance(const helios::vec3 &query_center, float max_radius, const std::vector<uint> &candidate_UUIDs = {});
 
    bool findNearestPrimitiveDistance(const helios::vec3 &origin, const helios::vec3 &direction, const std::vector<uint> &candidate_UUIDs, float &distance, helios::vec3 &obstacle_direction);
 
    bool findNearestSolidObstacleInCone(const helios::vec3 &apex, const helios::vec3 &axis, float half_angle, float height, const std::vector<uint> &candidate_UUIDs, float &distance, helios::vec3 &obstacle_direction, int num_rays = 64);
 
 
    bool findNearestSolidObstacleInCone(const helios::vec3 &apex, const helios::vec3 &axis, float half_angle, float height, const std::vector<uint> &candidate_UUIDs, const std::vector<uint> &plant_primitives, float &distance,
                                        helios::vec3 &obstacle_direction, int num_rays = 64);
 
 
    // -------- BVH MANAGEMENT --------
 
    void buildBVH(const std::vector<uint> &UUIDs = {});
 
    void updateBVH(const std::vector<uint> &UUIDs, bool force_rebuild = false);
 
    void setStaticGeometry(const std::vector<uint> &UUIDs);
 
    void rebuildBVH();
 
    void disableAutomaticBVHRebuilds();
 
    void enableAutomaticBVHRebuilds();
 
    void enableHierarchicalBVH();
 
    void disableHierarchicalBVH();
 
    void buildStaticBVH();
 
    void enableTreeBasedBVH(float isolation_distance = 5.0f);
 
    void disableTreeBasedBVH();
 
    [[nodiscard]] bool isTreeBasedBVHEnabled() const;
 
    void initializeObstacleSpatialGrid();
 
    void registerTree(uint tree_object_id, const std::vector<uint> &tree_primitives);
 
    void setStaticObstacles(const std::vector<uint> &obstacle_primitives);
 
    std::vector<uint> getRelevantGeometryForTree(const helios::vec3 &query_position, const std::vector<uint> &query_primitives = {}, float max_distance = 15.0f);
 
    [[nodiscard]] bool isBVHValid() const;
 
    // -------- GPU ACCELERATION CONTROL --------
 
    void enableGPUAcceleration();
 
    void disableGPUAcceleration();
 
    [[nodiscard]] bool isGPUAccelerationEnabled() const;
 
    // -------- UTILITY METHODS --------
 
    void disableMessages();
 
    void enableMessages();
 
    [[nodiscard]] size_t getPrimitiveCount() const;
 
    void getBVHStatistics(size_t &node_count, size_t &leaf_count, size_t &max_depth) const;
 
    static int selfTest(int argc, char **argv);
 
private:
    helios::Context *context;
 
    bool gpu_acceleration_enabled;
 
    bool printmessages;
 
    // -------- THREAD-SAFE PRIMITIVE CACHE --------
 
    struct CachedPrimitive {
        helios::PrimitiveType type;
        std::vector<helios::vec3> vertices;
 
        CachedPrimitive() : type(helios::PRIMITIVE_TYPE_TRIANGLE) {
        }
        CachedPrimitive(helios::PrimitiveType t, const std::vector<helios::vec3> &v) : type(t), vertices(v) {
        }
    };
 
    std::unordered_map<uint, CachedPrimitive> primitive_cache;
 
    void buildPrimitiveCache();
 
    HitResult intersectPrimitiveThreadSafe(const helios::vec3 &origin, const helios::vec3 &direction, uint primitive_id, float max_distance);
 
    bool triangleIntersect(const helios::vec3 &origin, const helios::vec3 &direction, const helios::vec3 &v0, const helios::vec3 &v1, const helios::vec3 &v2, float &distance);
 
    bool patchIntersect(const helios::vec3 &origin, const helios::vec3 &direction, const helios::vec3 &v0, const helios::vec3 &v1, const helios::vec3 &v2, const helios::vec3 &v3, float &distance);
 
    // -------- BVH DATA STRUCTURES --------
 
    struct BVHNode {
        helios::vec3 aabb_min; 
        helios::vec3 aabb_max; 
        uint left_child; 
        uint right_child; 
        uint primitive_start; 
        uint primitive_count; 
        bool is_leaf; 
 
        BVHNode() : aabb_min(0, 0, 0), aabb_max(0, 0, 0), left_child(0xFFFFFFFF), right_child(0xFFFFFFFF), primitive_start(0), primitive_count(0), is_leaf(false) {
        }
    };
 
    struct BVHNodesSoA {
        // Hot data: frequently accessed during traversal (cache-friendly grouping)
        std::vector<helios::vec3> aabb_mins; 
        std::vector<helios::vec3> aabb_maxs; 
        std::vector<uint32_t> left_children; 
        std::vector<uint32_t> right_children; 
 
        // Cold data: accessed less frequently (separate for better cache utilization)
        std::vector<uint32_t> primitive_starts; 
        std::vector<uint32_t> primitive_counts; 
        std::vector<uint8_t> is_leaf_flags; 
 
        // Metadata
        size_t node_count = 0; 
 
        BVHNodesSoA() = default;
 
        void reserve(size_t capacity) {
            aabb_mins.reserve(capacity);
            aabb_maxs.reserve(capacity);
            left_children.reserve(capacity);
            right_children.reserve(capacity);
            primitive_starts.reserve(capacity);
            primitive_counts.reserve(capacity);
            is_leaf_flags.reserve(capacity);
        }
 
        void clear() {
            aabb_mins.clear();
            aabb_maxs.clear();
            left_children.clear();
            right_children.clear();
            primitive_starts.clear();
            primitive_counts.clear();
            is_leaf_flags.clear();
            node_count = 0;
        }
 
        [[nodiscard]] size_t getMemoryUsage() const {
            return (aabb_mins.size() + aabb_maxs.size()) * sizeof(helios::vec3) + (left_children.size() + right_children.size() + primitive_starts.size() + primitive_counts.size()) * sizeof(uint32_t) + is_leaf_flags.size() * sizeof(uint8_t);
        }
    };
 
 
    std::vector<BVHNode> bvh_nodes;
 
    size_t next_available_node_index;
 
    BVHNodesSoA bvh_nodes_soa;
 
    BVHOptimizationMode bvh_optimization_mode = BVHOptimizationMode::SOA_UNCOMPRESSED;
 
    std::vector<uint> primitive_indices;
 
    std::unordered_map<uint, std::pair<helios::vec3, helios::vec3>> primitive_aabbs_cache;
 
    std::unordered_set<uint> dirty_primitive_cache;
 
    std::vector<std::vector<std::vector<std::vector<uint>>>> grid_cells;
 
    helios::vec3 grid_center;
    helios::vec3 grid_size;
    helios::int3 grid_divisions;
 
    // -------- VOXEL RAY STATISTICS DATA --------
 
    std::vector<std::vector<std::vector<int>>> voxel_ray_counts;
 
    std::vector<std::vector<std::vector<int>>> voxel_transmitted;
 
    std::vector<std::vector<std::vector<float>>> voxel_path_lengths;
 
    std::vector<std::vector<std::vector<int>>> voxel_hit_before;
 
    std::vector<std::vector<std::vector<int>>> voxel_hit_after;
 
    std::vector<std::vector<std::vector<int>>> voxel_hit_inside;
 
    std::vector<std::vector<std::vector<std::vector<float>>>> voxel_individual_path_lengths;
 
    // -------- OPTIMIZED FLAT ARRAY DATA (Structure-of-Arrays) --------
 
    std::vector<int> voxel_ray_counts_flat;
    std::vector<int> voxel_transmitted_flat;
    std::vector<float> voxel_path_lengths_flat;
    std::vector<int> voxel_hit_before_flat;
    std::vector<int> voxel_hit_after_flat;
    std::vector<int> voxel_hit_inside_flat;
 
    std::vector<float> voxel_individual_path_lengths_flat;
    std::vector<size_t> voxel_individual_path_offsets; // Start offset for each voxel
    std::vector<size_t> voxel_individual_path_counts; // Count for each voxel
 
    bool use_flat_arrays;
 
    bool voxel_data_initialized;
 
    helios::vec3 voxel_grid_center;
    helios::vec3 voxel_grid_size;
    helios::int3 voxel_grid_divisions;
 
    // -------- SPATIAL OPTIMIZATION DATA --------
 
    float max_collision_distance;
 
    std::unordered_map<uint, helios::vec3> primitive_centroids_cache;
 
    // -------- GPU MEMORY POINTERS --------
 
    void *d_bvh_nodes;
 
    uint *d_primitive_indices;
 
    bool gpu_memory_allocated;
 
    std::set<uint> last_processed_uuids;
 
    std::set<uint> last_processed_deleted_uuids;
 
    std::set<uint> static_geometry_cache;
 
    std::set<uint> last_bvh_geometry;
 
    bool bvh_dirty;
 
    bool soa_dirty;
 
    bool automatic_bvh_rebuilds;
 
    mutable bool batch_mode_skip_bvh_check;
 
    // -------- HIERARCHICAL BVH DATA STRUCTURES --------
 
    bool hierarchical_bvh_enabled;
 
    std::vector<BVHNode> static_bvh_nodes;
 
    std::vector<uint> static_bvh_primitives;
 
    bool static_bvh_valid;
 
    std::set<uint> last_static_bvh_geometry;
 
    void updateHierarchicalBVH(const std::set<uint> &requested_geometry, bool force_rebuild);
 
    // -------- TREE-BASED BVH DATA STRUCTURES --------
 
    // Forward declaration
    struct TreeBVH;
 
    bool tree_based_bvh_enabled;
 
    float tree_isolation_distance;
 
    std::unordered_map<uint, uint> object_to_tree_map;
 
    std::vector<uint> static_obstacle_primitives;
 
    struct ObstacleSpatialGrid {
        float cell_size;
        std::unordered_map<int64_t, std::vector<uint>> grid_cells;
 
        [[nodiscard]] int64_t getGridKey(float x, float y) const {
            auto grid_x = static_cast<int32_t>(std::floor(x / cell_size));
            auto grid_y = static_cast<int32_t>(std::floor(y / cell_size));
            return (static_cast<int64_t>(grid_x) << 32) | static_cast<uint32_t>(grid_y);
        }
 
        [[nodiscard]] std::vector<uint> getRelevantObstacles(const helios::vec3 &position, float radius) const;
    };
 
    mutable ObstacleSpatialGrid obstacle_spatial_grid;
    bool obstacle_spatial_grid_initialized;
 
    // -------- GAP DETECTION DATA STRUCTURES --------
 
    struct RaySample {
        helios::vec3 direction; 
        float distance; 
        bool is_free; 
    };
 
    struct Gap {
        helios::vec3 center_direction; 
        float angular_size; 
        float angular_distance; 
        float score; 
        std::vector<int> sample_indices; 
    };
 
    struct SpatialHashGrid {
        struct Cell {
            std::vector<size_t> sample_indices; 
        };
 
        std::vector<std::vector<Cell>> grid; 
        int theta_resolution; 
        int phi_resolution; 
        float theta_step; 
        float phi_step; 
 
        explicit SpatialHashGrid(int theta_res = 32, int phi_res = 16) : theta_resolution(theta_res), phi_resolution(phi_res) {
            grid.resize(theta_resolution);
            for (auto &row: grid) {
                row.resize(phi_resolution);
            }
            theta_step = M_PI / theta_resolution;
            phi_step = 2.0f * M_PI / phi_resolution;
        }
 
        void clear() {
            for (auto &row: grid) {
                for (auto &cell: row) {
                    cell.sample_indices.clear();
                }
            }
        }
 
        [[nodiscard]] std::pair<int, int> getGridIndex(const helios::vec3 &direction) const {
            // Convert direction to spherical coordinates
            float theta = acosf(std::max(-1.0f, std::min(1.0f, direction.z)));
            float phi = atan2f(direction.y, direction.x);
            if (phi < 0)
                phi += 2.0f * M_PI;
 
            int theta_idx = std::min((int) (theta / theta_step), theta_resolution - 1);
            int phi_idx = std::min((int) (phi / phi_step), phi_resolution - 1);
 
            return {theta_idx, phi_idx};
        }
 
        void addSample(size_t sample_idx, const helios::vec3 &direction) {
            auto [theta_idx, phi_idx] = getGridIndex(direction);
            grid[theta_idx][phi_idx].sample_indices.push_back(sample_idx);
        }
 
        [[nodiscard]] std::vector<size_t> getNearbyIndices(const helios::vec3 &direction, int radius = 1) const {
            auto [center_theta, center_phi] = getGridIndex(direction);
            std::vector<size_t> nearby_indices;
 
            for (int dt = -radius; dt <= radius; dt++) {
                for (int dp = -radius; dp <= radius; dp++) {
                    int theta_idx = center_theta + dt;
                    int phi_idx = (center_phi + dp + phi_resolution) % phi_resolution;
 
                    if (theta_idx >= 0 && theta_idx < theta_resolution) {
                        const auto &cell = grid[theta_idx][phi_idx];
                        nearby_indices.insert(nearby_indices.end(), cell.sample_indices.begin(), cell.sample_indices.end());
                    }
                }
            }
 
            return nearby_indices;
        }
    };
 
    struct TreeBVH {
        uint tree_object_id; 
        helios::vec3 tree_center; 
        float tree_radius; 
        std::vector<BVHNode> nodes; 
        std::vector<uint> primitive_indices; 
        BVHNodesSoA soa_structure; 
        bool soa_dirty; 
 
        TreeBVH() : tree_object_id(0), tree_center(0, 0, 0), tree_radius(0), soa_dirty(true) {
        }
 
        void clear() {
            nodes.clear();
            primitive_indices.clear();
            soa_structure = BVHNodesSoA();
            soa_dirty = true;
        }
    };
 
    std::unordered_map<uint, TreeBVH> tree_bvh_map;
 
    // -------- PRIVATE HELPER METHODS --------
 
    void ensureBVHCurrent();
 
    void calculateAABB(const std::vector<uint> &primitives, helios::vec3 &aabb_min, helios::vec3 &aabb_max) const;
 
    void buildBVHRecursive(uint node_index, size_t primitive_start, size_t primitive_count, int depth);
 
    std::vector<uint> traverseBVH_CPU(const helios::vec3 &query_aabb_min, const helios::vec3 &query_aabb_max);
 
#ifdef HELIOS_CUDA_AVAILABLE
    std::vector<uint> traverseBVH_GPU(const helios::vec3 &query_aabb_min, const helios::vec3 &query_aabb_max);
#endif
 
    bool aabbIntersect(const helios::vec3 &min1, const helios::vec3 &max1, const helios::vec3 &min2, const helios::vec3 &max2);
 
    bool rayAABBIntersect(const helios::vec3 &origin, const helios::vec3 &direction, const helios::vec3 &aabb_min, const helios::vec3 &aabb_max, float &t_min, float &t_max) const;
 
    uint32_t rayAABBIntersectSIMD(const helios::vec3 *ray_origins, const helios::vec3 *ray_directions, const helios::vec3 *aabb_mins, const helios::vec3 *aabb_maxs, float *t_mins, float *t_maxs, int count);
 
    void traverseBVHSIMD(const helios::vec3 *ray_origins, const helios::vec3 *ray_directions, int count, HitResult *results);
 
    void traverseBVHSIMDImpl(const helios::vec3 *ray_origins, const helios::vec3 *ray_directions, int count, HitResult *results);
 
    bool coneAABBIntersect(const Cone &cone, const helios::vec3 &aabb_min, const helios::vec3 &aabb_max);
 
    bool coneAABBIntersectFast(const Cone &cone, const helios::vec3 &aabb_min, const helios::vec3 &aabb_max);
 
    bool coneAABBIntersect(const helios::vec3 &cone_origin, const helios::vec3 &cone_direction, float cone_angle, float max_distance, const helios::vec3 &aabb_min, const helios::vec3 &aabb_max);
 
    int countRayIntersections(const helios::vec3 &origin, const helios::vec3 &direction, float max_distance = -1.0f);
 
    bool findNearestRayIntersection(const helios::vec3 &origin, const helios::vec3 &direction, const std::set<uint> &candidate_UUIDs, float &nearest_distance, float max_distance = -1.0f);
 
    std::vector<helios::vec3> sampleDirectionsInCone(const helios::vec3 &apex, const helios::vec3 &central_axis, float half_angle, int num_samples);
 
    // -------- GAP DETECTION HELPER METHODS --------
 
    std::vector<Gap> detectGapsInCone(const helios::vec3 &apex, const helios::vec3 &central_axis, float half_angle, float height, int num_samples);
 
 
    std::vector<uint> getCandidatePrimitivesInCone(const helios::vec3 &apex, const helios::vec3 &central_axis, float half_angle, float height);
 
    std::vector<uint> getCandidatesUsingSpatialGrid(const Cone &cone, const helios::vec3 &apex, const helios::vec3 &central_axis, float half_angle, float height);
 
    float calculateGapAngularSize(const std::vector<RaySample> &gap_samples, const helios::vec3 &central_axis);
 
    void scoreGapsByFishEyeMetric(std::vector<Gap> &gaps, const helios::vec3 &central_axis);
 
    helios::vec3 findOptimalGapDirection(const std::vector<Gap> &gaps, const helios::vec3 &central_axis);
 
    // -------- VOXEL RAY PATH LENGTH HELPER METHODS --------
 
    void initializeVoxelData(const helios::vec3 &grid_center, const helios::vec3 &grid_size, const helios::int3 &grid_divisions);
 
    void calculateVoxelRayPathLengths_CPU(const std::vector<helios::vec3> &ray_origins, const std::vector<helios::vec3> &ray_directions);
 
    bool calculateVoxelRayPathLengths_GPU(const std::vector<helios::vec3> &ray_origins, const std::vector<helios::vec3> &ray_directions);
 
    bool validateVoxelIndices(const helios::int3 &ijk) const;
 
    void calculateVoxelAABB(const helios::int3 &ijk, helios::vec3 &voxel_min, helios::vec3 &voxel_max) const;
 
    std::vector<std::pair<helios::int3, float>> traverseVoxelGrid(const helios::vec3 &ray_origin, const helios::vec3 &ray_direction) const;
 
    inline size_t flatIndex(int i, int j, int k) const {
        return static_cast<size_t>(i) * static_cast<size_t>(voxel_grid_divisions.y) * static_cast<size_t>(voxel_grid_divisions.z) + static_cast<size_t>(j) * static_cast<size_t>(voxel_grid_divisions.z) + static_cast<size_t>(k);
    }
 
    inline size_t flatIndex(const helios::int3 &ijk) const {
        return flatIndex(ijk.x, ijk.y, ijk.z);
    }
 
#ifdef HELIOS_CUDA_AVAILABLE
    void allocateGPUMemory();
 
    void freeGPUMemory();
 
    void transferBVHToGPU();
#endif
 
    void markBVHDirty();
 
    void incrementalUpdateBVH(const std::set<uint> &added_geometry, const std::set<uint> &removed_geometry, const std::set<uint> &final_geometry);
 
    void updatePrimitiveAABBCache(uint uuid);
 
    void optimizedRebuildBVH(const std::set<uint> &final_geometry);
 
    bool validateUUIDs(const std::vector<uint> &UUIDs) const;
 
    bool rayPrimitiveIntersection(const helios::vec3 &origin, const helios::vec3 &direction, uint primitive_UUID, float &distance) const;
 
    void castRaysCPU(const std::vector<RayQuery> &ray_queries, std::vector<HitResult> &results, RayTracingStats &stats);
 
#ifdef HELIOS_CUDA_AVAILABLE
    void castRaysGPU(const std::vector<RayQuery> &ray_queries, std::vector<HitResult> &results, RayTracingStats &stats);
#endif
 
    // -------- OPTIMIZED RAY TRACING PRIVATE METHODS --------
 
    void convertBVHLayout(BVHOptimizationMode from_mode, BVHOptimizationMode to_mode);
    void ensureOptimizedBVH(); // Populate optimized BVH structures on demand
 
    std::vector<HitResult> castRaysSoA(const std::vector<RayQuery> &ray_queries, RayTracingStats &stats);
 
    HitResult castRaySoATraversal(const RayQuery &query, RayTracingStats &stats);
 
    HitResult castRayBVHTraversal(const RayQuery &query);
 
    inline bool aabbIntersectSoA(const helios::vec3 &ray_origin, const helios::vec3 &ray_direction, float max_distance, size_t node_index) const;
 
    bool rayAABBIntersect(const helios::vec3 &ray_origin, const helios::vec3 &ray_direction, const helios::vec3 &aabb_min, const helios::vec3 &aabb_max) const;
 
    HitResult intersectPrimitive(const RayQuery &query, uint primitive_id);
 
    bool rayAABBIntersectPrimitive(const helios::vec3 &origin, const helios::vec3 &direction, const helios::vec3 &aabb_min, const helios::vec3 &aabb_max, float &distance);
 
    // -------- RASTERIZATION-BASED COLLISION DETECTION --------
 
    int calculateOptimalBinCount(float cone_half_angle, int geometry_count);
 
    std::vector<uint> filterPrimitivesParallel(const Cone &cone, const std::vector<uint> &primitive_uuids);
 
    void projectGeometryToBins(const Cone &cone, const std::vector<uint> &filtered_uuids, AngularBins &bins);
 
    std::vector<Gap> findGapsInCoverageMap(const AngularBins &bins, const Cone &cone);
 
    bool sphericalCoordsToBinIndices(float theta, float phi, const AngularBins &bins, int &theta_bin, int &phi_bin);
 
    float cartesianToSphericalCone(const helios::vec3 &vector, const helios::vec3 &cone_axis, float &theta, float &phi);
 
    void projectGeometryToBinsSerial(const Cone &cone, const std::vector<uint> &filtered_uuids, AngularBins &bins);
 
    Gap floodFillGap(const AngularBins &bins, int start_theta, int start_phi, std::vector<std::vector<bool>> &visited, const Cone &cone);
 
    float calculateBinSolidAngle(int theta_bin, int phi_bin, const AngularBins &bins, float cone_half_angle);
 
    helios::vec3 binIndicesToCartesian(int theta_bin, int phi_bin, const AngularBins &bins, const Cone &cone);
 
    void addUnoccupiedNeighbors(int theta, int phi, const AngularBins &bins, std::vector<std::vector<bool>> &visited, std::queue<std::pair<int, int>> &queue);
};
 
#endif