contains_occupancy_states.hpp

 
#ifndef UFO_MAP_OCCUPANCY_PREDICATE_CONTAINS_OCCUPANCY_STATES_HPP
#define UFO_MAP_OCCUPANCY_PREDICATE_CONTAINS_OCCUPANCY_STATES_HPP
 
// UFO
#include <ufo/container/tree/index.hpp>
#include <ufo/container/tree/predicate/filter.hpp>
#include <ufo/map/occupancy/propagation_criteria.hpp>
 
namespace ufo::pred
{
struct ContainOccupancyStates {
    constexpr ContainOccupancyStates(bool unknown, bool free, bool occupied)
        : unknown(unknown), free(free), occupied(occupied)
    {
    }
 
    bool unknown;
    bool free;
    bool occupied;
};
 
template <>
struct Filter<ContainOccupancyStates> : public FilterBase<ContainOccupancyStates> {
    using Pred = ContainOccupancyStates;
 
    template <class Tree>
    static constexpr void init(Pred&, Tree const&)
    {
    }
 
    template <class Tree>
    [[nodiscard]] static constexpr bool returnable(Pred const& p, Tree const& t,
                                                   TreeIndex const& n)
    {
        return (p.unknown && t.containsUnknown(n)) || (p.free && t.containsFree(n)) ||
               (p.occupied && t.containsOccupied(n));
    }
 
    template <class Tree>
    [[nodiscard]] static constexpr bool traversable(Pred const& p, Tree const& t,
                                                    TreeIndex const& n)
    {
        if (OccupancyPropagationCriteria::NONE == t.occupancyPropagationCriteria()) {
            return true;
        }
 
        return (p.unknown && t.containsUnknown(n)) || (p.free && t.containsFree(n)) ||
               (p.occupied && t.containsOccupied(n));
    }
};
}  // namespace ufo::pred
 
#endif  // UFO_MAP_OCCUPANCY_PREDICATE_CONTAINS_OCCUPANCY_STATES_HPP