contains_occupancy_state.hpp

 
#ifndef UFO_MAP_OCCUPANCY_PREDICATE_CONTAINS_OCCUPANCY_STATE_HPP
#define UFO_MAP_OCCUPANCY_PREDICATE_CONTAINS_OCCUPANCY_STATE_HPP
 
// UFO
#include <ufo/container/tree/predicate/filter.hpp>
#include <ufo/map/occupancy/propagation_criteria.hpp>
#include <ufo/map/occupancy/state.hpp>
 
namespace ufo::pred
{
template <ufo::OccupancyState State>
struct ContainOccupancyState {
};
 
using ContainUnknown  = ContainOccupancyState<ufo::OccupancyState::UNKNOWN>;
using ContainFree     = ContainOccupancyState<ufo::OccupancyState::FREE>;
using ContainOccupied = ContainOccupancyState<ufo::OccupancyState::OCCUPIED>;
 
template <ufo::OccupancyState State>
struct Filter<ContainOccupancyState<State>>
    : public FilterBase<ContainOccupancyState<State>> {
    using Pred = ContainOccupancyState<State>;
 
    template <class Tree>
    static constexpr void init(Pred&, Tree const&)
    {
    }
 
    template <class Tree, class Node>
    [[nodiscard]] static constexpr bool returnable(Pred const&, Tree const& t,
                                                   Node const& n)
    {
        if constexpr (ufo::OccupancyState::UNKNOWN == State) {
            return t.containsUnknown(n.index);
        } else if constexpr (ufo::OccupancyState::FREE == State) {
            return t.containsFree(n.index);
        } else if constexpr (ufo::OccupancyState::OCCUPIED == State) {
            return t.containsOccupied(n.index);
        }
    }
 
    template <class Tree, class Node>
    [[nodiscard]] static constexpr bool traversable(Pred const& p, Tree const& t,
                                                    Node const& n)
    {
        if (OccupancyPropagationCriteria::NONE == t.occupancyPropagationCriteria()) {
            return true;
        }
 
        if constexpr (ufo::OccupancyState::UNKNOWN == State) {
            return t.containsUnknown(n.index);
        } else if constexpr (ufo::OccupancyState::FREE == State) {
            return t.containsFree(n.index);
        } else if constexpr (ufo::OccupancyState::OCCUPIED == State) {
            return t.containsOccupied(n.index);
        }
    }
};
}  // namespace ufo::pred
 
#endif  // UFO_MAP_OCCUPANCY_PREDICATE_CONTAINS_OCCUPANCY_STATE_HPP