UFO 1.0.0
An Efficient Probabilistic 3D Mapping Framework That Embraces the Unknown
Loading...
Searching...
No Matches
occupancy_map.hpp
1
42#ifndef UFO_MAP_OCCUPANCY_PREDICATE_OCCUPANCY_MAP_HPP
43#define UFO_MAP_OCCUPANCY_PREDICATE_OCCUPANCY_MAP_HPP
44
45// UFO
46#include <ufo/container/tree/predicate/filter.hpp>
47
48namespace ufo::pred
49{
50template <bool Negated = false>
52 private:
53 bool enabled;
54
55 friend struct Init<OccupancyMap>;
56 friend struct InnerCheck<OccupancyMap>;
57 friend struct ValueCheck<OccupancyMap>;
58 friend struct ShaderInnerCheck<OccupancyMap>;
59 friend struct ShaderValueCheck<OccupancyMap>;
60};
61
62template <bool Negated>
64{
65 return {};
66}
67
68template <bool Negated>
69struct Init<OccupancyMap<Negated>> {
71
72 template <class Map>
73 static inline void apply(Pred& p, Map const& m)
74 {
75 p.enabled = m.isMapTypesEnabled(MapType::OCCUPANCY);
76 }
77};
78
79template <bool Negated>
80struct InnerCheck<OccupancyMap<Negated>> {
82
83 template <class Map, class Node>
84 static constexpr bool apply(Pred p, Map const&, Node const&)
85 {
86 if constexpr (!Map::enableDisableUtilityPresent()) {
87 if constexpr (Negated) {
88 return !is_occupancy_map_v<Map>;
89 } else {
90 return is_occupancy_map_v<Map>;
91 }
92 } else {
93 if constexpr (Negated) {
94 return !p.enabled;
95 } else {
96 return p.enabled;
97 }
98 }
99 }
100};
101
102template <bool Negated, class PredPost>
103struct InnerCheck<Then<OccupancyMap<Negated>, PredPost>> {
104 using Pred = Then<OccupancyMap<Negated>, PredPost>;
105
106 template <class Map, class Node>
107 static constexpr bool apply(Pred const& p, Map const& m, Node const& n)
108 {
109 if constexpr (!Map::enableDisableUtilityPresent()) {
110 if constexpr (InnerCheck<OccupancyMap<Negated>>::apply(p.pre, m, n)) {
111 return InnerCheck<PredPost>::apply(p.post, m, n);
112 } else {
113 return true;
114 }
115 } else {
116 return !InnerCheck<OccupancyMap<Negated>>::apply(p.pre, m, n) ||
117 InnerCheck<PredPost>::apply(p.post, m, n);
118 }
119 }
120};
121
122template <bool Negated>
123struct ValueCheck<OccupancyMap<Negated>> {
125
126 template <class Map, class Node>
127 static constexpr bool apply(Pred p, Map const&, Node const& n)
128 {
129 if constexpr (!Map::enableDisableUtilityPresent()) {
130 if constexpr (Negated) {
131 return !is_occupancy_map_v<Map>;
132 } else {
133 return is_occupancy_map_v<Map>;
134 }
135 } else {
136 if constexpr (Negated) {
137 return !p.enabled;
138 } else {
139 return p.enabled;
140 }
141 }
142 }
143};
144
145template <bool Negated, class PredPost>
146struct ValueCheck<Then<OccupancyMap<Negated>, PredPost>> {
147 using Pred = Then<OccupancyMap<Negated>, PredPost>;
148
149 template <class Map, class Node>
150 static constexpr bool apply(Pred const& p, Map const& m, Node const& n)
151 {
152 if constexpr (!Map::enableDisableUtilityPresent()) {
153 if constexpr (ValueCheck<OccupancyMap<Negated>>::apply(p.pre, m, n)) {
154 return ValueCheck<PredPost>::apply(p.post, m, n);
155 } else {
156 return true;
157 }
158 } else {
159 return !ValueCheck<OccupancyMap<Negated>>::apply(p.pre, m, n) ||
160 ValueCheck<PredPost>::apply(p.post, m, n);
161 }
162 }
163};
164} // namespace ufo::pred
165
166#endif // UFO_MAP_OCCUPANCY_PREDICATE_OCCUPANCY_MAP_HPP