occupancy_interval.hpp

 
#ifndef UFO_MAP_OCCUPANCY_PREDICATE_OCCUPANCY_INTERVAL_HPP
#define UFO_MAP_OCCUPANCY_PREDICATE_OCCUPANCY_INTERVAL_HPP
 
// UFO
#include <ufo/container/tree/index.hpp>
#include <ufo/container/tree/predicate/filter.hpp>
#include <ufo/map/occupancy/predicate/occupancy.hpp>
 
namespace ufo::pred
{
template <bool Negated = false>
struct OccupancyInterval {
    using occupancy_t = float;
 
    constexpr OccupancyInterval(occupancy_t min, occupancy_t max) : min(min), max(max) {}
 
    OccupancyMin min;
    OccupancyMax max;
};
 
template <bool Negated>
OccupancyInterval<!Negated> operator!(OccupancyInterval<Negated> p)
{
    return {p.min, p.max};
}
 
template <bool Negated>
struct Filter<OccupancyInterval<Negated>>
    : public FilterBase<OccupancyInterval<Negated>> {
    using Pred = OccupancyInterval<Negated>;
 
    template <class Tree>
    static constexpr void init(Pred& p, Tree const& t)
    {
        init(p.min, t);
        init(p.max, t);
    }
 
    template <class Tree>
    [[nodiscard]] static constexpr bool returnable(Pred const& p, Tree const& t,
                                                   TreeIndex const& n)
    {
        if constexpr (Negated) {
            return !valueCheck(p.min, t, n) && !valueCheck(p.max, t, n);
        } else {
            return valueCheck(p.min, t, n) && valueCheck(p.max, t, n);
        }
    }
 
    template <class Tree>
    [[nodiscard]] static constexpr bool traversable(Pred const& p, Tree const& t,
                                                    TreeIndex const& n)
    {
        if constexpr (Negated) {
            return true;
        } else {
            return innerCheck(p.min, t, n) && innerCheck(p.max, t, n);
        }
    }
};
}  // namespace ufo::pred
 
#endif  // UFO_MAP_OCCUPANCY_PREDICATE_OCCUPANCY_INTERVAL_HPP