UFO 1.0.0
An Efficient Probabilistic 3D Mapping Framework That Embraces the Unknown
Loading...
Searching...
No Matches
macros.hpp
1
41#ifndef UFO_UTILITY_MACROS
42#define UFO_UTILITY_MACROS
43
44#define UFO_REPEAT_2(M, N) M(N) M(N + 1)
45#define UFO_REPEAT_4(M, N) UFO_REPEAT_2(M, N) UFO_REPEAT_2(M, N + 2)
46#define UFO_REPEAT_8(M, N) UFO_REPEAT_4(M, N) UFO_REPEAT_4(M, N + 4)
47#define UFO_REPEAT_16(M, N) UFO_REPEAT_8(M, N) UFO_REPEAT_8(M, N + 8)
48#define UFO_REPEAT_32(M, N) UFO_REPEAT_16(M, N) UFO_REPEAT_16(M, N + 16)
49#define UFO_REPEAT_64(M, N) UFO_REPEAT_32(M, N) UFO_REPEAT_32(M, N + 32)
50#define UFO_REPEAT_128(M, N) UFO_REPEAT_64(M, N) UFO_REPEAT_64(M, N + 64)
51
52#define UFO_MIN(a, b) ((a) < (b) ? (a) : (b))
53#define UFO_MAX(a, b) ((a) < (b) ? (b) : (a))
54#define UFO_CLAMP(v, lo, hi) UFO_MAX(lo, UFO_MIN(hi, v))
55#define UFO_MIN_PAIR_FIRST(a, b) ((a).first < (b).first ? (a) : (b))
56#define UFO_MAX_PAIR_FIRST(a, b) ((a).first < (b).first ? (b) : (a))
57#define UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, i, j) \
58 { \
59 auto const a = UFO_MIN_PAIR_FIRST(c[i], c[j]); \
60 c[j] = UFO_MAX_PAIR_FIRST(c[i], c[j]); \
61 c[i] = a; \
62 }
63
64#define UFO_MIN_2(c) \
65 { \
66 c[0] = UFO_MIN(c[0], c[1]); \
67 }
68
69#define UFO_MIN_3(c) \
70 { \
71 c[0] = UFO_MIN(c[0], c[2]); \
72 UFO_MIN_2(c); \
73 }
74
75#define UFO_MIN_4(c) \
76 { \
77 c[0] = UFO_MIN(c[0], c[2]); \
78 c[1] = UFO_MIN(c[1], c[3]); \
79 UFO_MIN_2(c); \
80 }
81
82#define UFO_MIN_8(c) \
83 { \
84 c[0] = UFO_MIN(c[0], c[4]); \
85 c[1] = UFO_MIN(c[1], c[5]); \
86 c[2] = UFO_MIN(c[2], c[6]); \
87 c[3] = UFO_MIN(c[3], c[7]); \
88 UFO_MIN_4(c); \
89 }
90
91#define UFO_MIN_16(c) \
92 { \
93 c[0] = UFO_MIN(c[0], c[8]); \
94 c[1] = UFO_MIN(c[1], c[9]); \
95 c[2] = UFO_MIN(c[2], c[10]); \
96 c[3] = UFO_MIN(c[3], c[11]); \
97 c[4] = UFO_MIN(c[4], c[12]); \
98 c[5] = UFO_MIN(c[5], c[13]); \
99 c[6] = UFO_MIN(c[6], c[14]); \
100 c[7] = UFO_MIN(c[7], c[15]); \
101 UFO_MIN_8(c); \
102 }
103
104#define UFO_MIN_PAIR_FIRST_2(c) \
105 { \
106 c[0] = UFO_MIN_PAIR_FIRST(c[0], c[1]); \
107 }
108
109#define UFO_MIN_PAIR_FIRST_4(c) \
110 { \
111 c[0] = UFO_MIN_PAIR_FIRST(c[0], c[2]); \
112 c[1] = UFO_MIN_PAIR_FIRST(c[1], c[3]); \
113 UFO_MIN_PAIR_FIRST_2(c); \
114 }
115
116#define UFO_MIN_PAIR_FIRST_8(c) \
117 { \
118 c[0] = UFO_MIN_PAIR_FIRST(c[0], c[4]); \
119 c[1] = UFO_MIN_PAIR_FIRST(c[1], c[5]); \
120 c[2] = UFO_MIN_PAIR_FIRST(c[2], c[6]); \
121 c[3] = UFO_MIN_PAIR_FIRST(c[3], c[7]); \
122 UFO_MIN_PAIR_FIRST_4(c); \
123 }
124
125#define UFO_MIN_PAIR_FIRST_16(c) \
126 { \
127 c[0] = UFO_MIN_PAIR_FIRST(c[0], c[8]); \
128 c[1] = UFO_MIN_PAIR_FIRST(c[1], c[9]); \
129 c[2] = UFO_MIN_PAIR_FIRST(c[2], c[10]); \
130 c[3] = UFO_MIN_PAIR_FIRST(c[3], c[11]); \
131 c[4] = UFO_MIN_PAIR_FIRST(c[4], c[12]); \
132 c[5] = UFO_MIN_PAIR_FIRST(c[5], c[13]); \
133 c[6] = UFO_MIN_PAIR_FIRST(c[6], c[14]); \
134 c[7] = UFO_MIN_PAIR_FIRST(c[7], c[15]); \
135 UFO_MIN_PAIR_FIRST_8(c); \
136 }
137
138#define UFO_SORT_ASCENDING_PAIR_FIRST_2(c) \
139 { \
140 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 0, 1); \
141 }
142
143#define UFO_SORT_ASCENDING_PAIR_FIRST_4(c) \
144 { \
145 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 0, 2); \
146 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 1, 3); \
147 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 0, 1); \
148 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 2, 3); \
149 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 1, 2); \
150 }
151
152#define UFO_SORT_ASCENDING_PAIR_FIRST_8(c) \
153 { \
154 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 0, 2); \
155 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 1, 3); \
156 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 4, 6); \
157 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 5, 7); \
158 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 0, 4); \
159 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 1, 5); \
160 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 2, 6); \
161 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 3, 7); \
162 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 0, 1); \
163 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 2, 3); \
164 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 4, 5); \
165 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 6, 7); \
166 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 2, 4); \
167 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 3, 5); \
168 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 1, 4); \
169 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 3, 6); \
170 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 1, 2); \
171 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 3, 4); \
172 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 5, 6); \
173 }
174
175#define UFO_SORT_ASCENDING_PAIR_FIRST_16(c) \
176 { \
177 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 0, 13); \
178 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 1, 12); \
179 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 2, 15); \
180 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 3, 14); \
181 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 4, 8); \
182 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 5, 6); \
183 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 7, 11); \
184 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 9, 10); \
185 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 0, 5); \
186 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 1, 7); \
187 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 2, 9); \
188 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 3, 4); \
189 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 6, 13); \
190 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 8, 14); \
191 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 10, 15); \
192 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 11, 12); \
193 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 0, 1); \
194 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 2, 3); \
195 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 4, 5); \
196 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 6, 8); \
197 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 7, 9); \
198 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 10, 11); \
199 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 12, 13); \
200 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 14, 15); \
201 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 0, 2); \
202 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 1, 3); \
203 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 4, 10); \
204 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 5, 11); \
205 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 6, 7); \
206 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 8, 9); \
207 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 12, 14); \
208 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 13, 15); \
209 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 1, 2); \
210 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 3, 12); \
211 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 4, 6); \
212 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 5, 7); \
213 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 8, 10); \
214 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 9, 11); \
215 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 13, 14); \
216 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 1, 4); \
217 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 2, 6); \
218 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 5, 8); \
219 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 7, 10); \
220 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 9, 13); \
221 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 11, 14); \
222 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 2, 4); \
223 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 3, 6); \
224 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 9, 12); \
225 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 11, 13); \
226 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 3, 5); \
227 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 6, 8); \
228 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 7, 9); \
229 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 10, 12); \
230 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 3, 4); \
231 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 5, 6); \
232 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 7, 8); \
233 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 9, 10); \
234 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 11, 12); \
235 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 6, 7); \
236 UFO_SORT_ASCENDING_PAIR_FIRST_SWAP(c, 8, 9); \
237 }
238
239#endif // UFO_UTILITY_MACROS