occupancy_state.hpp

 
#ifndef UFO_MAP_OCCUPANCY_PREDICATE_OCCUPANCY_STATE_HPP
#define UFO_MAP_OCCUPANCY_PREDICATE_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, bool Negated = false>
struct OccupancyState {
};
 
using Unknown  = OccupancyState<ufo::OccupancyState::UNKNOWN>;
using Free     = OccupancyState<ufo::OccupancyState::FREE>;
using Occupied = OccupancyState<ufo::OccupancyState::OCCUPIED>;
 
template <ufo::OccupancyState State, bool Negated>
constexpr OccupancyState<State, !Negated> operator!(OccupancyState<State, Negated> const&)
{
    return OccupancyState<State, !Negated>();
}
 
template <ufo::OccupancyState State, bool Negated>
struct Filter<OccupancyState<State, Negated>>
    : public FilterBase<OccupancyState<State, Negated>> {
    using Pred = OccupancyState<State, Negated>;
 
    template <class Tree>
    static constexpr void init(Pred&, Tree const&)
    {
    }
 
    template <class Tree, class Node>
    [[nodiscard]] static constexpr bool returnable(Pred const& p, Tree const& t,
                                                   Node const& n)
    {
        bool v = [&]() -> bool {
            if constexpr (ufo::OccupancyState::UNKNOWN == State) {
                return t.occupancyUnknown(n.index);
            } else if constexpr (ufo::OccupancyState::FREE == State) {
                return t.occupancyFree(n.index);
            } else if constexpr (ufo::OccupancyState::OCCUPIED == State) {
                return t.occupancyOccupied(n.index);
            }
        }();
 
        if constexpr (Negated) {
            v = !v;
        }
 
        return v;
    }
 
    template <class Tree, class Node>
    [[nodiscard]] static constexpr bool traversable(Pred const& p, Tree const& t,
                                                    Node const& n)
    {
        if constexpr (Negated) {
            if constexpr (ufo::OccupancyState::UNKNOWN == State) {
                return t.occupancyContainsFree(n.index) || t.occupancyContainsOccupied(n.index);
            } else if constexpr (ufo::OccupancyState::FREE == State) {
                return t.occupancyContainsUnknown(n.index) ||
                       t.occupancyContainsOccupied(n.index);
            } else if constexpr (ufo::OccupancyState::OCCUPIED == State) {
                return t.occupancyContainsUnknown(n.index) || t.occupancyContainsFree(n.index);
            }
        } else {
            if constexpr (ufo::OccupancyState::UNKNOWN == State) {
                return t.occupancyContainsUnknown(n.index);
            } else if constexpr (ufo::OccupancyState::FREE == State) {
                return t.occupancyContainsFree(n.index);
            } else if constexpr (ufo::OccupancyState::OCCUPIED == State) {
                return t.occupancyContainsOccupied(n.index);
            }
        }
    }
};
}  // namespace ufo::pred
 
#endif  // UFO_MAP_OCCUPANCY_PREDICATE_OCCUPANCY_STATE_HPP