graph_generator.h

Go to the documentation of this file.

 
 
#pragma once
#define _USE_MATH_DEFINES
 
#include <cmath>
#include <set>
#include <vector>
#include <array>
#include <Node.h>
#include <cassert>
#include <variant>
#include <MultiRT.h>
#include <unordered_map>
 
// Forward declares for embree raytracer.
namespace HF::RayTracer {
    class EmbreeRayTracer;
    class NanoRTRayTracer;
}
namespace HF::SpatialStructures {
    class Graph;
    struct Edge;
    enum STEP;
}
 
namespace HF::GraphGenerator {
 
    /* For C++ 2020.
    template <typename T>
    concept Point = requires(T pt) {
            {pt[0]}->std::convertible_to<float>;
            {pt[1]}->std::convertible_to<float>;
            {pt[2]}->std::convertible_to<float>;
    };
    */
 
    class UniqueQueue;
    struct optional_real3;
 
    using real_t = double;                            
    using real3 = std::array<real_t, 3>;              
    using graph_edge = HF::SpatialStructures::Edge;   
 
    constexpr real_t default_z_precision = 0.0001;
    constexpr real_t default_ground_offset = 0.01;
    constexpr real_t default_spacing_precision = 0.00001;
 
    using RayTracer = HF::RayTracer::MultiRT; 
    using pair = std::pair<int, int>; 
 
    template<typename real_type>
    inline real_t CastToReal(real_type t) { return static_cast<real_t>(t); }
    
    template<typename real_3_type>
    inline real3 CastToReal3(real_3_type t) {
        return real3{
            CastToReal(t[0]), CastToReal(t[1]), CastToReal(t[2])
        };
    };
 
    struct Precision {
        real_t node_z;              
        real_t node_spacing;        
        real_t ground_offset;       
    };
    enum HIT_FLAG {
        NO_FLAG = 0,
        FLOORS = 1,
        OBSTACLES = 2,
        BOTH = 3
    };
 
    using hashmap = std::unordered_map<int, HIT_FLAG>; 
    
    enum class GeometryFilterMode {
        ALL_INTERSECTIONS = 0, 
        OBSTACLES_ONLY = 1, 
        OBSTACLES_AND_FLOORS 
    };
 
    struct GeometryFlagMap {
 
    private:
        hashmap internal_dictionary;
 
        inline void DetermineFilterMode(
            const std::vector<int>& walkable,
            const std::vector<int>& obstacle)
        {
            bool has_walkable = !(walkable.empty());
            bool has_obstacle = !(obstacle.empty());
 
            if (has_walkable && has_obstacle) this->Mode = GeometryFilterMode::OBSTACLES_AND_FLOORS;
            else if (has_obstacle && !has_walkable) this->Mode = GeometryFilterMode::OBSTACLES_ONLY;
            else this->Mode = GeometryFilterMode::ALL_INTERSECTIONS;
        }
 
    public:
        GeometryFilterMode Mode = GeometryFilterMode::ALL_INTERSECTIONS; 
 
        inline void SetGeometryIds(
            const std::vector<int> & obstacle_geometry, 
            const std::vector<int> & walkable_geometry)
        {
            for (auto id : obstacle_geometry)
                this->Set(id, HIT_FLAG::OBSTACLES);
            for (auto id : walkable_geometry)
                this->Set(id, HIT_FLAG::FLOORS);
 
            DetermineFilterMode(walkable_geometry, obstacle_geometry);
        }
 
        inline bool HasKey(int id) const {
            return internal_dictionary.count(id) >= 1;
        }
 
        inline HIT_FLAG operator[](int id) const {
            if (!HasKey(id)) return HIT_FLAG::NO_FLAG;
            return internal_dictionary.at(id);
        }
 
        inline void Set(int id, HIT_FLAG flag) {
            internal_dictionary[id] = flag;
        }
    };
 
    struct GraphParams {
        real_t up_step;  
        real_t up_slope; 
        real_t down_step; 
        real_t down_slope; 
        Precision precision; 
        GeometryFlagMap geom_ids; 
    };
 
    struct optional_real3 {
        real3 pt{ NAN, NAN, NAN }; 
 
        inline optional_real3() {};
 
        inline optional_real3(real_t x, real_t y, real_t z) : pt(real3{ x,y,z }) {};
    
        inline optional_real3(const real3 & in_real3) : pt(in_real3) {};
 
        inline real3 & operator *() { return pt; }
        
        inline explicit operator bool() const {
            return !(std::isnan(pt[0]) && std::isnan(pt[1]) && std::isnan(pt[2]));
        }
    };
 
 
 
    std::set<std::pair<int, int>> permutations(int limit);
 
    class GraphGenerator
    {
    public:
        int max_nodes;          
        int core_count;         
 
        int min_connections;  
        int max_step_connection; 
        real3 spacing;          
 
        GraphParams params; 
 
        RayTracer ray_tracer; 
    public:
        
        GraphGenerator(HF::RayTracer::EmbreeRayTracer& ray_tracer, const std::vector<int> & obstacle_ids = std::vector<int>(0), const std::vector<int> & walkable_ids = std::vector<int>(0));
    
        GraphGenerator(HF::RayTracer::NanoRTRayTracer& ray_tracer, const std::vector<int> & obstacle_ids = std::vector<int>(0), const std::vector<int>& walkable_ids = std::vector<int>(0));
        
        GraphGenerator(HF::RayTracer::MultiRT& ray_tracer, const std::vector<int> & obstacle_ids = std::vector<int>(0), const std::vector<int>& walkable_ids = std::vector<int>(0));
 
 
        template <
            typename node_type,
            typename node2_type,
            typename up_step_type,
            typename up_slope_type,
            typename down_step_type,
            typename down_slope_type,
            typename z_precision_type =  real_t,
            typename connect_offset_type = real_t,
            typename spacing_precision_type = real_t
        >
        inline SpatialStructures::Graph BuildNetwork(
            const node_type& start_point,
            const node2_type& Spacing,
            int MaxNodes,
            up_step_type UpStep,
            up_slope_type UpSlope,
            down_step_type DownStep,
            down_slope_type DownSlope,
            int max_step_connections,
            int min_connections = 1,
            int cores = -1,
            z_precision_type node_z_precision = default_z_precision,
            connect_offset_type  node_spacing_precision = default_spacing_precision,
            spacing_precision_type ground_offset = default_ground_offset
        ) {
            assert(node_z_precision != 0);
            return IMPL_BuildNetwork(
                CastToReal3(start_point),
                CastToReal3(Spacing),
                MaxNodes,
                CastToReal(UpStep),
                CastToReal(UpSlope),
                CastToReal(DownStep),
                CastToReal(DownSlope),
                max_step_connections,
                min_connections,
                cores,
                CastToReal(node_z_precision),
                CastToReal(node_spacing_precision),
                CastToReal(ground_offset)
            );
        }
 
 
        SpatialStructures::Graph IMPL_BuildNetwork(
            const real3& start_point,
            const real3& Spacing,
            int MaxNodes,
            real_t UpStep,
            real_t UpSlope,
            real_t DownStep,
            real_t DownSlope,
            int max_step_connections,
            int min_connections,
            int cores = -1,
            real_t node_z_precision = default_z_precision,
            real_t node_spacing_precision = default_spacing_precision,
            real_t ground_offset = default_ground_offset
        );
 
 
        SpatialStructures::Graph CrawlGeom(UniqueQueue& todo);
        
        SpatialStructures::Graph CrawlGeomParallel(UniqueQueue& todo);
    };
 
    optional_real3 ValidateStartPoint(RayTracer& RT, const real3& start_point, const GraphParams & Params);
    
    optional_real3 CheckRay(
        RayTracer& RT,
        const real3& origin,
        const real3& direction,
        real_t node_z_tolerance,
        HIT_FLAG flag = BOTH,
        const GeometryFlagMap& geometry_dict = GeometryFlagMap()
    );
 
 
    std::vector<pair> CreateDirecs(int max_step_connections);
 
    std::vector<graph_edge> GetChildren(
        const real3 & parent,
        const std::vector<real3>& possible_children,
        RayTracer  & rt,
        const GraphParams & GP
    );
 
    std::vector<real3> GeneratePotentialChildren(
        const real3& parent,
        const std::vector<pair>& direcs,
        const real3& spacing,
        const GraphParams & gp
    );
 
    template<typename A, typename D, typename N>
    inline void MoveNode(const A& dist, const D& direction, N& node) {
        node[0] = std::fma(direction[0], dist, node[0]);
        node[1] = std::fma(direction[1], dist, node[1]);
        node[2] = std::fma(direction[2], dist, node[2]);
    }
 
    std::vector<real3> CheckChildren(
        const real3& parent,
        const std::vector<real3>& possible_children,
        RayTracer& rt,
        const GraphParams & params
    );
 
    HF::SpatialStructures::STEP CheckConnection(
        const real3 & parent,
        const real3 & child,
        RayTracer& rt,
        const GraphParams & params
    );
 
    bool OcclusionCheck(const real3 & parent, const real3 & child, RayTracer& RT);
 
    bool CheckSlope(
        const real3 & parent,
        const real3 & child,
        const GraphParams & gp
    );
 
}