OcTreeLUT.cpp
Go to the documentation of this file.
1 /*
2  * OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees
3  * http://octomap.github.com/
4  *
5  * Copyright (c) 2009-2013, R. Schmitt, K.M. Wurm and A. Hornung,
6  * University of Freiburg. All rights reserved.
7  * License: New BSD
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above copyright
15  * notice, this list of conditions and the following disclaimer in the
16  * documentation and/or other materials provided with the distribution.
17  * * Neither the name of the University of Freiburg nor the names of its
18  * contributors may be used to endorse or promote products derived from
19  * this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
22  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
23  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
24  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
25  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
26  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
27  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
28  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
29  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
30  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31  * POSSIBILITY OF SUCH DAMAGE.
32  */
33 
34 
35 #include <list>
36 #include <queue>
37 #include <octomap/OcTreeLUT.h>
38 
39 namespace octomap {
40 
41 
42  OcTreeLUT::OcTreeLUT(unsigned int _max_depth) :
43  max_depth(_max_depth) {
44  initLUT();
45  }
46 
47  OcTreeLUT::~OcTreeLUT() {
48  }
49 
50  void OcTreeLUT::initLUT() {
51 
52  // begin Face-Neighbors
53  // LUT_N
54  nf_values[0][LUT_S] = nf_values[0][LUT_N] = 2;
55  nf_values[1][LUT_S] = nf_values[1][LUT_N] = 3;
56  nf_values[2][LUT_S] = nf_values[2][LUT_N] = 0;
57  nf_values[3][LUT_S] = nf_values[3][LUT_N] = 1;
58  nf_values[4][LUT_S] = nf_values[4][LUT_N] = 6;
59  nf_values[5][LUT_S] = nf_values[5][LUT_N] = 7;
60  nf_values[6][LUT_S] = nf_values[6][LUT_N] = 4;
61  nf_values[7][LUT_S] = nf_values[7][LUT_N] = 5;
62 
63  nf_rec_values[0][LUT_N] = LUT_NO_REC;
64  nf_rec_values[1][LUT_N] = LUT_NO_REC;
65  nf_rec_values[2][LUT_N] = LUT_SELF;
66  nf_rec_values[3][LUT_N] = LUT_SELF;
67  nf_rec_values[4][LUT_N] = LUT_NO_REC;
68  nf_rec_values[5][LUT_N] = LUT_NO_REC;
69  nf_rec_values[6][LUT_N] = LUT_SELF;
70  nf_rec_values[7][LUT_N] = LUT_SELF;
71 
72  // LUT_S
73  nf_rec_values[0][LUT_S] = LUT_SELF;
74  nf_rec_values[1][LUT_S] = LUT_SELF;
75  nf_rec_values[2][LUT_S] = LUT_NO_REC;
76  nf_rec_values[3][LUT_S] = LUT_NO_REC;
77  nf_rec_values[4][LUT_S] = LUT_SELF;
78  nf_rec_values[5][LUT_S] = LUT_SELF;
79  nf_rec_values[6][LUT_S] = LUT_NO_REC;
80  nf_rec_values[7][LUT_S] = LUT_NO_REC;
81 
82  // LUT_E
83  nf_values[0][LUT_W] = nf_values[0][LUT_E] = 1;
84  nf_values[1][LUT_W] = nf_values[1][LUT_E] = 0;
85  nf_values[2][LUT_W] = nf_values[2][LUT_E] = 3;
86  nf_values[3][LUT_W] = nf_values[3][LUT_E] = 2;
87  nf_values[4][LUT_W] = nf_values[4][LUT_E] = 5;
88  nf_values[5][LUT_W] = nf_values[5][LUT_E] = 4;
89  nf_values[6][LUT_W] = nf_values[6][LUT_E] = 7;
90  nf_values[7][LUT_W] = nf_values[7][LUT_E] = 6;
91 
92  nf_rec_values[0][LUT_E] = LUT_NO_REC;
93  nf_rec_values[1][LUT_E] = LUT_SELF;
94  nf_rec_values[2][LUT_E] = LUT_NO_REC;
95  nf_rec_values[3][LUT_E] = LUT_SELF;
96  nf_rec_values[4][LUT_E] = LUT_NO_REC;
97  nf_rec_values[5][LUT_E] = LUT_SELF;
98  nf_rec_values[6][LUT_E] = LUT_NO_REC;
99  nf_rec_values[7][LUT_E] = LUT_SELF;
100 
101  // LUT_W
102  nf_rec_values[0][LUT_W] = LUT_SELF;
103  nf_rec_values[1][LUT_W] = LUT_NO_REC;
104  nf_rec_values[2][LUT_W] = LUT_SELF;
105  nf_rec_values[3][LUT_W] = LUT_NO_REC;
106  nf_rec_values[4][LUT_W] = LUT_SELF;
107  nf_rec_values[5][LUT_W] = LUT_NO_REC;
108  nf_rec_values[6][LUT_W] = LUT_SELF;
109  nf_rec_values[7][LUT_W] = LUT_NO_REC;
110 
111  // LUT_F
112  nf_values[0][LUT_R] = nf_values[0][LUT_F] = 4;
113  nf_values[1][LUT_R] = nf_values[1][LUT_F] = 5;
114  nf_values[2][LUT_R] = nf_values[2][LUT_F] = 6;
115  nf_values[3][LUT_R] = nf_values[3][LUT_F] = 7;
116  nf_values[4][LUT_R] = nf_values[4][LUT_F] = 0;
117  nf_values[5][LUT_R] = nf_values[5][LUT_F] = 1;
118  nf_values[6][LUT_R] = nf_values[6][LUT_F] = 2;
119  nf_values[7][LUT_R] = nf_values[7][LUT_F] = 3;
120 
121  nf_rec_values[0][LUT_F] = LUT_NO_REC;
122  nf_rec_values[1][LUT_F] = LUT_NO_REC;
123  nf_rec_values[2][LUT_F] = LUT_NO_REC;
124  nf_rec_values[3][LUT_F] = LUT_NO_REC;
125  nf_rec_values[4][LUT_F] = LUT_SELF;
126  nf_rec_values[5][LUT_F] = LUT_SELF;
127  nf_rec_values[6][LUT_F] = LUT_SELF;
128  nf_rec_values[7][LUT_F] = LUT_SELF;
129 
130  // LUT_R
131  nf_rec_values[0][LUT_R] = LUT_SELF;
132  nf_rec_values[1][LUT_R] = LUT_SELF;
133  nf_rec_values[2][LUT_R] = LUT_SELF;
134  nf_rec_values[3][LUT_R] = LUT_SELF;
135  nf_rec_values[4][LUT_R] = LUT_NO_REC;
136  nf_rec_values[5][LUT_R] = LUT_NO_REC;
137  nf_rec_values[6][LUT_R] = LUT_NO_REC;
138  nf_rec_values[7][LUT_R] = LUT_NO_REC;
139  // end Face-Neighbors
140 
141 
142  // begin Edge-Neighbors
143  // LUT_NW
144  for (int i = LUT_NW; i < LUT_SE+1; ++i) {
145  nf_values[0][i] = 3;
146  nf_values[1][i] = 2;
147  nf_values[2][i] = 1;
148  nf_values[3][i] = 0;
149  nf_values[4][i] = 7;
150  nf_values[5][i] = 6;
151  nf_values[6][i] = 5;
152  nf_values[7][i] = 4;
153  }
154 
155  nf_rec_values[0][LUT_NW] = LUT_NW_TO_W;
156  nf_rec_values[1][LUT_NW] = LUT_NO_REC;
157  nf_rec_values[2][LUT_NW] = LUT_SELF;
158  nf_rec_values[3][LUT_NW] = LUT_NW_TO_N;
159  nf_rec_values[4][LUT_NW] = LUT_NW_TO_W;
160  nf_rec_values[5][LUT_NW] = LUT_NO_REC;
161  nf_rec_values[6][LUT_NW] = LUT_SELF;
162  nf_rec_values[7][LUT_NW] = LUT_NW_TO_N;
163 
164  // LUT_NE
165  nf_rec_values[0][LUT_NE] = LUT_NO_REC;
166  nf_rec_values[1][LUT_NE] = LUT_NE_TO_E;
167  nf_rec_values[2][LUT_NE] = LUT_NE_TO_N;
168  nf_rec_values[3][LUT_NE] = LUT_SELF;
169  nf_rec_values[4][LUT_NE] = LUT_NO_REC;
170  nf_rec_values[5][LUT_NE] = LUT_NE_TO_E;
171  nf_rec_values[6][LUT_NE] = LUT_NE_TO_N;
172  nf_rec_values[7][LUT_NE] = LUT_SELF;
173 
174  // LUT_SW
175  nf_rec_values[0][LUT_SW] = LUT_SELF;
176  nf_rec_values[1][LUT_SW] = LUT_SW_TO_S;
177  nf_rec_values[2][LUT_SW] = LUT_SW_TO_W;
178  nf_rec_values[3][LUT_SW] = LUT_NO_REC;
179  nf_rec_values[4][LUT_SW] = LUT_SELF;
180  nf_rec_values[5][LUT_SW] = LUT_SW_TO_S;
181  nf_rec_values[6][LUT_SW] = LUT_SW_TO_W;
182  nf_rec_values[7][LUT_SW] = LUT_NO_REC;
183 
184  // LUT_SE
185  nf_rec_values[0][LUT_SE] = LUT_SE_TO_S;
186  nf_rec_values[1][LUT_SE] = LUT_SELF;
187  nf_rec_values[2][LUT_SE] = LUT_NO_REC;
188  nf_rec_values[3][LUT_SE] = LUT_SE_TO_E;
189  nf_rec_values[4][LUT_SE] = LUT_SE_TO_S;
190  nf_rec_values[5][LUT_SE] = LUT_SELF;
191  nf_rec_values[6][LUT_SE] = LUT_NO_REC;
192  nf_rec_values[7][LUT_SE] = LUT_SE_TO_E;
193 
194  // LUT_FN
195  for (int i = LUT_FN; i < LUT_RS+1; ++i) {
196  nf_values[0][i] = 6;
197  nf_values[1][i] = 7;
198  nf_values[2][i] = 4;
199  nf_values[3][i] = 5;
200  nf_values[4][i] = 2;
201  nf_values[5][i] = 3;
202  nf_values[6][i] = 0;
203  nf_values[7][i] = 1;
204  }
205 
206  nf_rec_values[0][LUT_FN] = LUT_NO_REC;
207  nf_rec_values[1][LUT_FN] = LUT_NO_REC;
208  nf_rec_values[2][LUT_FN] = LUT_FN_TO_N;
209  nf_rec_values[3][LUT_FN] = LUT_FN_TO_N;
210  nf_rec_values[4][LUT_FN] = LUT_FN_TO_F;
211  nf_rec_values[5][LUT_FN] = LUT_FN_TO_F;
212  nf_rec_values[6][LUT_FN] = LUT_SELF;
213  nf_rec_values[7][LUT_FN] = LUT_SELF;
214 
215  // LUT_RN
216  nf_rec_values[0][LUT_RN] = LUT_RN_TO_R;
217  nf_rec_values[1][LUT_RN] = LUT_RN_TO_R;
218  nf_rec_values[2][LUT_RN] = LUT_SELF;
219  nf_rec_values[3][LUT_RN] = LUT_SELF;
220  nf_rec_values[4][LUT_RN] = LUT_NO_REC;
221  nf_rec_values[5][LUT_RN] = LUT_NO_REC;
222  nf_rec_values[6][LUT_RN] = LUT_RN_TO_N;
223  nf_rec_values[7][LUT_RN] = LUT_RN_TO_N;
224 
225  // LUT_FS
226  nf_rec_values[0][LUT_FS] = LUT_FS_TO_S;
227  nf_rec_values[1][LUT_FS] = LUT_FS_TO_S;
228  nf_rec_values[2][LUT_FS] = LUT_NO_REC;
229  nf_rec_values[3][LUT_FS] = LUT_NO_REC;
230  nf_rec_values[4][LUT_FS] = LUT_SELF;
231  nf_rec_values[5][LUT_FS] = LUT_SELF;
232  nf_rec_values[6][LUT_FS] = LUT_FS_TO_F;
233  nf_rec_values[7][LUT_FS] = LUT_FS_TO_F;
234 
235  // LUT_RS
236  nf_rec_values[0][LUT_RS] = LUT_SELF;
237  nf_rec_values[1][LUT_RS] = LUT_SELF;
238  nf_rec_values[2][LUT_RS] = LUT_RS_TO_R;
239  nf_rec_values[3][LUT_RS] = LUT_RS_TO_R;
240  nf_rec_values[4][LUT_RS] = LUT_RS_TO_S;
241  nf_rec_values[5][LUT_RS] = LUT_RS_TO_S;
242  nf_rec_values[6][LUT_RS] = LUT_NO_REC;
243  nf_rec_values[7][LUT_RS] = LUT_NO_REC;
244 
245  // LUT_FE
246  for (int i = LUT_FE; i < LUT_RW+1; ++i) {
247  nf_values[0][i] = 5;
248  nf_values[1][i] = 4;
249  nf_values[2][i] = 7;
250  nf_values[3][i] = 6;
251  nf_values[4][i] = 1;
252  nf_values[5][i] = 0;
253  nf_values[6][i] = 3;
254  nf_values[7][i] = 2;
255  }
256 
257  nf_rec_values[0][LUT_FE] = LUT_NO_REC;
258  nf_rec_values[1][LUT_FE] = LUT_FE_TO_E;
259  nf_rec_values[2][LUT_FE] = LUT_NO_REC;
260  nf_rec_values[3][LUT_FE] = LUT_FE_TO_E;
261  nf_rec_values[4][LUT_FE] = LUT_FE_TO_F;
262  nf_rec_values[5][LUT_FE] = LUT_SELF;
263  nf_rec_values[6][LUT_FE] = LUT_FE_TO_F;
264  nf_rec_values[7][LUT_FE] = LUT_SELF;
265 
266  // LUT_FW
267  nf_rec_values[0][LUT_FW] = LUT_FW_TO_W;
268  nf_rec_values[1][LUT_FW] = LUT_NO_REC;
269  nf_rec_values[2][LUT_FW] = LUT_FW_TO_W;
270  nf_rec_values[3][LUT_FW] = LUT_NO_REC;
271  nf_rec_values[4][LUT_FW] = LUT_SELF;
272  nf_rec_values[5][LUT_FW] = LUT_FW_TO_F;
273  nf_rec_values[6][LUT_FW] = LUT_SELF;
274  nf_rec_values[7][LUT_FW] = LUT_FW_TO_F;
275 
276  // LUT_RE
277  nf_rec_values[0][LUT_RE] = LUT_RE_TO_R;
278  nf_rec_values[1][LUT_RE] = LUT_SELF;
279  nf_rec_values[2][LUT_RE] = LUT_RE_TO_R;
280  nf_rec_values[3][LUT_RE] = LUT_SELF;
281  nf_rec_values[4][LUT_RE] = LUT_NO_REC;
282  nf_rec_values[5][LUT_RE] = LUT_RE_TO_E;
283  nf_rec_values[6][LUT_RE] = LUT_NO_REC;
284  nf_rec_values[7][LUT_RE] = LUT_RE_TO_E;
285 
286  // LUT_RW
287  nf_rec_values[0][LUT_RW] = LUT_SELF;
288  nf_rec_values[1][LUT_RW] = LUT_RW_TO_R;
289  nf_rec_values[2][LUT_RW] = LUT_SELF;
290  nf_rec_values[3][LUT_RW] = LUT_RW_TO_R;
291  nf_rec_values[4][LUT_RW] = LUT_RW_TO_W;
292  nf_rec_values[5][LUT_RW] = LUT_NO_REC;
293  nf_rec_values[6][LUT_RW] = LUT_RW_TO_W;
294  nf_rec_values[7][LUT_RW] = LUT_NO_REC;
295 
296  // end Edge-Neighbors
297 
298 
299  // begin Vertex-Neighbors
300  // LUT_FNE
301  for (int i = LUT_FNE; i < LUT_RSW+1; ++i) {
302  nf_values[0][i] = 7;
303  nf_values[1][i] = 6;
304  nf_values[2][i] = 5;
305  nf_values[3][i] = 4;
306  nf_values[4][i] = 3;
307  nf_values[5][i] = 2;
308  nf_values[6][i] = 1;
309  nf_values[7][i] = 0;
310  }
311 
312  nf_rec_values[0][LUT_FNE] = LUT_NO_REC;
313  nf_rec_values[1][LUT_FNE] = LUT_FNE_TO_E;
314  nf_rec_values[2][LUT_FNE] = LUT_FNE_TO_N;
315  nf_rec_values[3][LUT_FNE] = LUT_FNE_TO_NE;
316  nf_rec_values[4][LUT_FNE] = LUT_FNE_TO_F;
317  nf_rec_values[5][LUT_FNE] = LUT_FNE_TO_FE;
318  nf_rec_values[6][LUT_FNE] = LUT_FNE_TO_FN;
319  nf_rec_values[7][LUT_FNE] = LUT_SELF;
320 
321  // LUT_FNW
322  nf_rec_values[0][LUT_FNW] = LUT_FNW_TO_W;
323  nf_rec_values[1][LUT_FNW] = LUT_NO_REC;
324  nf_rec_values[2][LUT_FNW] = LUT_FNW_TO_NW;
325  nf_rec_values[3][LUT_FNW] = LUT_FNW_TO_N;
326  nf_rec_values[4][LUT_FNW] = LUT_FNW_TO_FW;
327  nf_rec_values[5][LUT_FNW] = LUT_FNW_TO_F;
328  nf_rec_values[6][LUT_FNW] = LUT_SELF;
329  nf_rec_values[7][LUT_FNW] = LUT_FNW_TO_FN;
330 
331  // LUT_FSE
332  nf_rec_values[0][LUT_FSE] = LUT_FSE_TO_S;
333  nf_rec_values[1][LUT_FSE] = LUT_FSE_TO_SE;
334  nf_rec_values[2][LUT_FSE] = LUT_NO_REC;
335  nf_rec_values[3][LUT_FSE] = LUT_FSE_TO_E;
336  nf_rec_values[4][LUT_FSE] = LUT_FSE_TO_FS;
337  nf_rec_values[5][LUT_FSE] = LUT_SELF;
338  nf_rec_values[6][LUT_FSE] = LUT_FSE_TO_F;
339  nf_rec_values[7][LUT_FSE] = LUT_FSE_TO_FE;
340 
341  // LUT_FSW
342  nf_rec_values[0][LUT_FSW] = LUT_FSW_TO_SW;
343  nf_rec_values[1][LUT_FSW] = LUT_FSW_TO_S;
344  nf_rec_values[2][LUT_FSW] = LUT_FSW_TO_W;
345  nf_rec_values[3][LUT_FSW] = LUT_NO_REC;
346  nf_rec_values[4][LUT_FSW] = LUT_SELF;
347  nf_rec_values[5][LUT_FSW] = LUT_FSW_TO_FS;
348  nf_rec_values[6][LUT_FSW] = LUT_FSW_TO_FW;
349  nf_rec_values[7][LUT_FSW] = LUT_FSW_TO_F;
350 
351  // LUT_RNE
352  nf_rec_values[0][LUT_RNE] = LUT_RNE_TO_R;
353  nf_rec_values[1][LUT_RNE] = LUT_RNE_TO_RE;
354  nf_rec_values[2][LUT_RNE] = LUT_RNE_TO_RN;
355  nf_rec_values[3][LUT_RNE] = LUT_SELF;
356  nf_rec_values[4][LUT_RNE] = LUT_NO_REC;
357  nf_rec_values[5][LUT_RNE] = LUT_RNE_TO_E;
358  nf_rec_values[6][LUT_RNE] = LUT_RNE_TO_N;
359  nf_rec_values[7][LUT_RNE] = LUT_RNE_TO_NE;
360 
361  // LUT_RNW
362  nf_rec_values[0][LUT_RNW] = LUT_RNW_TO_RW;
363  nf_rec_values[1][LUT_RNW] = LUT_RNW_TO_R;
364  nf_rec_values[2][LUT_RNW] = LUT_SELF;
365  nf_rec_values[3][LUT_RNW] = LUT_RNW_TO_RN;
366  nf_rec_values[4][LUT_RNW] = LUT_RNW_TO_W;
367  nf_rec_values[5][LUT_RNW] = LUT_NO_REC;
368  nf_rec_values[6][LUT_RNW] = LUT_RNW_TO_NW;
369  nf_rec_values[7][LUT_RNW] = LUT_RNW_TO_N;
370 
371  // LUT_RSE
372  nf_rec_values[0][LUT_RSE] = LUT_RSE_TO_RS;
373  nf_rec_values[1][LUT_RSE] = LUT_SELF;
374  nf_rec_values[2][LUT_RSE] = LUT_RSE_TO_R;
375  nf_rec_values[3][LUT_RSE] = LUT_RSE_TO_RE;
376  nf_rec_values[4][LUT_RSE] = LUT_RSE_TO_S;
377  nf_rec_values[5][LUT_RSE] = LUT_RSE_TO_SE;
378  nf_rec_values[6][LUT_RSE] = LUT_NO_REC;
379  nf_rec_values[7][LUT_RSE] = LUT_RSE_TO_E;
380 
381  // LUT_RSW
382  nf_rec_values[0][LUT_RSW] = LUT_SELF;
383  nf_rec_values[1][LUT_RSW] = LUT_RSW_TO_RS;
384  nf_rec_values[2][LUT_RSW] = LUT_RSW_TO_RW;
385  nf_rec_values[3][LUT_RSW] = LUT_RSW_TO_R;
386  nf_rec_values[4][LUT_RSW] = LUT_RSW_TO_SW;
387  nf_rec_values[5][LUT_RSW] = LUT_RSW_TO_S;
388  nf_rec_values[6][LUT_RSW] = LUT_RSW_TO_W;
389  nf_rec_values[7][LUT_RSW] = LUT_NO_REC;
390 
391 
392  nf_multiple_values[LUT_N][0] = 0;
393  nf_multiple_values[LUT_N][1] = 1;
394  nf_multiple_values[LUT_N][2] = 4;
395  nf_multiple_values[LUT_N][3] = 5;
396 
397  nf_multiple_values[LUT_S][0] = 2;
398  nf_multiple_values[LUT_S][1] = 3;
399  nf_multiple_values[LUT_S][2] = 6;
400  nf_multiple_values[LUT_S][3] = 7;
401 
402  nf_multiple_values[LUT_E][0] = 0;
403  nf_multiple_values[LUT_E][1] = 2;
404  nf_multiple_values[LUT_E][2] = 4;
405  nf_multiple_values[LUT_E][3] = 6;
406 
407  nf_multiple_values[LUT_W][0] = 1;
408  nf_multiple_values[LUT_W][1] = 3;
409  nf_multiple_values[LUT_W][2] = 5;
410  nf_multiple_values[LUT_W][3] = 7;
411 
412  nf_multiple_values[LUT_F][0] = 0;
413  nf_multiple_values[LUT_F][1] = 1;
414  nf_multiple_values[LUT_F][2] = 2;
415  nf_multiple_values[LUT_F][3] = 3;
416 
417  nf_multiple_values[LUT_R][0] = 4;
418  nf_multiple_values[LUT_R][1] = 5;
419  nf_multiple_values[LUT_R][2] = 6;
420  nf_multiple_values[LUT_R][3] = 7;
421 
422  nf_multiple_values[LUT_NW][0] = 1;
423  nf_multiple_values[LUT_NW][1] = 5;
424  nf_multiple_values[LUT_NW][2] = LUT_NO_REC;
425  nf_multiple_values[LUT_NW][3] = LUT_NO_REC;
426 
427  nf_multiple_values[LUT_NE][0] = 0;
428  nf_multiple_values[LUT_NE][1] = 4;
429  nf_multiple_values[LUT_NE][2] = LUT_NO_REC;
430  nf_multiple_values[LUT_NE][3] = LUT_NO_REC;
431 
432  nf_multiple_values[LUT_SW][0] = 3;
433  nf_multiple_values[LUT_SW][1] = 7;
434  nf_multiple_values[LUT_SW][2] = LUT_NO_REC;
435  nf_multiple_values[LUT_SW][3] = LUT_NO_REC;
436 
437  nf_multiple_values[LUT_SE][0] = 2;
438  nf_multiple_values[LUT_SE][1] = 6;
439  nf_multiple_values[LUT_SE][2] = LUT_NO_REC;
440  nf_multiple_values[LUT_SE][3] = LUT_NO_REC;
441 
442  nf_multiple_values[LUT_FN][0] = 1;
443  nf_multiple_values[LUT_FN][1] = 3;
444  nf_multiple_values[LUT_FN][2] = LUT_NO_REC;
445  nf_multiple_values[LUT_FN][3] = LUT_NO_REC;
446 
447  nf_multiple_values[LUT_RN][0] = 4;
448  nf_multiple_values[LUT_RN][1] = 5;
449  nf_multiple_values[LUT_RN][2] = LUT_NO_REC;
450  nf_multiple_values[LUT_RN][3] = LUT_NO_REC;
451 
452  nf_multiple_values[LUT_FS][0] = 2;
453  nf_multiple_values[LUT_FS][1] = 3;
454  nf_multiple_values[LUT_FS][2] = LUT_NO_REC;
455  nf_multiple_values[LUT_FS][3] = LUT_NO_REC;
456 
457  nf_multiple_values[LUT_RS][0] = 6;
458  nf_multiple_values[LUT_RS][1] = 7;
459  nf_multiple_values[LUT_RS][2] = LUT_NO_REC;
460  nf_multiple_values[LUT_RS][3] = LUT_NO_REC;
461 
462  nf_multiple_values[LUT_FE][0] = 0;
463  nf_multiple_values[LUT_FE][1] = 2;
464  nf_multiple_values[LUT_FE][2] = LUT_NO_REC;
465  nf_multiple_values[LUT_FE][3] = LUT_NO_REC;
466 
467  nf_multiple_values[LUT_FW][0] = 1;
468  nf_multiple_values[LUT_FW][1] = 3;
469  nf_multiple_values[LUT_FW][2] = LUT_NO_REC;
470  nf_multiple_values[LUT_FW][3] = LUT_NO_REC;
471 
472  nf_multiple_values[LUT_RE][0] = 4;
473  nf_multiple_values[LUT_RE][1] = 6;
474  nf_multiple_values[LUT_RE][2] = LUT_NO_REC;
475  nf_multiple_values[LUT_RE][3] = LUT_NO_REC;
476 
477  nf_multiple_values[LUT_RW][0] = 5;
478  nf_multiple_values[LUT_RW][1] = 7;
479  nf_multiple_values[LUT_RW][2] = LUT_NO_REC;
480  nf_multiple_values[LUT_RW][3] = LUT_NO_REC;
481 
482  nf_multiple_values[LUT_FNE][0] = 0;
483  nf_multiple_values[LUT_FNE][1] = LUT_NO_REC;
484  nf_multiple_values[LUT_FNE][2] = LUT_NO_REC;
485  nf_multiple_values[LUT_FNE][3] = LUT_NO_REC;
486 
487  nf_multiple_values[LUT_FNW][0] = 1;
488  nf_multiple_values[LUT_FNW][1] = LUT_NO_REC;
489  nf_multiple_values[LUT_FNW][2] = LUT_NO_REC;
490  nf_multiple_values[LUT_FNW][3] = LUT_NO_REC;
491 
492  nf_multiple_values[LUT_FSE][0] = 2;
493  nf_multiple_values[LUT_FSE][1] = LUT_NO_REC;
494  nf_multiple_values[LUT_FSE][2] = LUT_NO_REC;
495  nf_multiple_values[LUT_FSE][3] = LUT_NO_REC;
496 
497  nf_multiple_values[LUT_FSW][0] = 3;
498  nf_multiple_values[LUT_FSW][1] = LUT_NO_REC;
499  nf_multiple_values[LUT_FSW][2] = LUT_NO_REC;
500  nf_multiple_values[LUT_FSW][3] = LUT_NO_REC;
501 
502  nf_multiple_values[LUT_RNE][0] = 4;
503  nf_multiple_values[LUT_RNE][1] = LUT_NO_REC;
504  nf_multiple_values[LUT_RNE][2] = LUT_NO_REC;
505  nf_multiple_values[LUT_RNE][3] = LUT_NO_REC;
506 
507  nf_multiple_values[LUT_RNW][0] = 5;
508  nf_multiple_values[LUT_RNW][1] = LUT_NO_REC;
509  nf_multiple_values[LUT_RNW][2] = LUT_NO_REC;
510  nf_multiple_values[LUT_RNW][3] = LUT_NO_REC;
511 
512  nf_multiple_values[LUT_RSE][0] = 6;
513  nf_multiple_values[LUT_RSE][1] = LUT_NO_REC;
514  nf_multiple_values[LUT_RSE][2] = LUT_NO_REC;
515  nf_multiple_values[LUT_RSE][3] = LUT_NO_REC;
516 
517  nf_multiple_values[LUT_RSW][0] = 7;
518  nf_multiple_values[LUT_RSW][1] = LUT_NO_REC;
519  nf_multiple_values[LUT_RSW][2] = LUT_NO_REC;
520  nf_multiple_values[LUT_RSW][3] = LUT_NO_REC;
521 
522  };
523 
524 
525  unsigned int OcTreeLUT::genPos(const OcTreeKey& key, const int& i) const {
526  unsigned int retval = 0;
527  if (key.k[0] & (1 << i)) retval += 1;
528  if (key.k[1] & (1 << i)) retval += 2;
529  if (key.k[2] & (1 << i)) retval += 4;
530  return retval;
531  }
532 
533 
534  /*
535  * used internally to generate neighbor key from a given key
536  */
537  void OcTreeLUT::changeKey(const int& val, OcTreeKey& key, const unsigned short int& i) const {
538  switch (val) {
539  case 0:
540  key.k[0] &= ~(1 << i);
541  key.k[1] &= ~(1 << i);
542  key.k[2] &= ~(1 << i);
543  break;
544  case 1:
545  key.k[0] |= (1 << i);
546  key.k[1] &= ~(1 << i);
547  key.k[2] &= ~(1 << i);
548  break;
549  case 2:
550  key.k[0] &= ~(1 << i);
551  key.k[1] |= (1 << i);
552  key.k[2] &= ~(1 << i);
553  break;
554  case 3:
555  key.k[0] |= (1 << i);
556  key.k[1] |= (1 << i);
557  key.k[2] &= ~(1 << i);
558  break;
559  case 4:
560  key.k[0] &= ~(1 << i);
561  key.k[1] &= ~(1 << i);
562  key.k[2] |= (1 << i);
563  break;
564  case 5:
565  key.k[0] |= (1 << i);
566  key.k[1] &= ~(1 << i);
567  key.k[2] |= (1 << i);
568  break;
569  case 6:
570  key.k[0] &= ~(1 << i);
571  key.k[1] |= (1 << i);
572  key.k[2] |= (1 << i);
573  break;
574  case 7:
575  key.k[0] |= (1 << i);
576  key.k[1] |= (1 << i);
577  key.k[2] |= (1 << i);
578  break;
579  }
580  }
581 
582 
583  bool OcTreeLUT::genNeighborKey(const OcTreeKey& node_key, const signed char& dir,
584  OcTreeKey& neighbor_key) const {
585 
586  neighbor_key.k[0] = node_key.k[0];
587  neighbor_key.k[1] = node_key.k[1];
588  neighbor_key.k[2] = node_key.k[2];
589 
590  unsigned int depth = 0;
591  signed char curDir = dir;
592 
593  signed char pos;
594  while (depth < max_depth) {
595  pos = static_cast<signed char>(genPos(neighbor_key, depth));
596  changeKey(nf_values[pos][curDir], neighbor_key, depth);
597 
598  if (nf_rec_values[pos][curDir] != LUT_NO_REC) { // recurs
599  curDir -= nf_rec_values[pos][curDir];
600  depth++;
601  }
602  else {
603  return true;
604  }
605  }
606 
607  return false;
608  };
609 
610 
611 
612 } // namespace
613 
LUT_FN_TO_F
#define LUT_FN_TO_F
Definition: OcTreeLUTdefs.h:79
LUT_FSE
#define LUT_FSE
Definition: OcTreeLUTdefs.h:63
LUT_FNW_TO_W
#define LUT_FNW_TO_W
Definition: OcTreeLUTdefs.h:104
LUT_FNW_TO_FW
#define LUT_FNW_TO_FW
Definition: OcTreeLUTdefs.h:107
LUT_FNE
#define LUT_FNE
Definition: OcTreeLUTdefs.h:61
LUT_RS_TO_R
#define LUT_RS_TO_R
Definition: OcTreeLUTdefs.h:85
LUT_NW_TO_N
#define LUT_NW_TO_N
Definition: OcTreeLUTdefs.h:72
LUT_FE_TO_E
#define LUT_FE_TO_E
Definition: OcTreeLUTdefs.h:88
LUT_FN_TO_N
#define LUT_FN_TO_N
Definition: OcTreeLUTdefs.h:80
octomap::OcTreeLUT::genNeighborKey
bool genNeighborKey(const OcTreeKey &node_key, const signed char &dir, OcTreeKey &neighbor_key) const
Definition: OcTreeLUT.cpp:583
LUT_FSE_TO_FE
#define LUT_FSE_TO_FE
Definition: OcTreeLUTdefs.h:116
LUT_FNE_TO_FN
#define LUT_FNE_TO_FN
Definition: OcTreeLUTdefs.h:101
LUT_W
#define LUT_W
Definition: OcTreeLUTdefs.h:42
LUT_RSE
#define LUT_RSE
Definition: OcTreeLUTdefs.h:67
LUT_SW
#define LUT_SW
Definition: OcTreeLUTdefs.h:49
LUT_SW_TO_W
#define LUT_SW_TO_W
Definition: OcTreeLUTdefs.h:76
LUT_RNW_TO_N
#define LUT_RNW_TO_N
Definition: OcTreeLUTdefs.h:137
LUT_FSW_TO_FS
#define LUT_FSW_TO_FS
Definition: OcTreeLUTdefs.h:121
LUT_NE_TO_E
#define LUT_NE_TO_E
Definition: OcTreeLUTdefs.h:73
LUT_RN_TO_N
#define LUT_RN_TO_N
Definition: OcTreeLUTdefs.h:81
LUT_FNW_TO_N
#define LUT_FNW_TO_N
Definition: OcTreeLUTdefs.h:106
LUT_RNE_TO_RE
#define LUT_RNE_TO_RE
Definition: OcTreeLUTdefs.h:126
LUT_FS_TO_S
#define LUT_FS_TO_S
Definition: OcTreeLUTdefs.h:84
octomap::OcTreeLUT::OcTreeLUT
OcTreeLUT(unsigned int _max_depth)
Definition: OcTreeLUT.cpp:42
LUT_FS
#define LUT_FS
Definition: OcTreeLUTdefs.h:53
LUT_FNE_TO_NE
#define LUT_FNE_TO_NE
Definition: OcTreeLUTdefs.h:99
LUT_NO_REC
#define LUT_NO_REC
Definition: OcTreeLUTdefs.h:155
LUT_FSW_TO_S
#define LUT_FSW_TO_S
Definition: OcTreeLUTdefs.h:119
LUT_RSW_TO_S
#define LUT_RSW_TO_S
Definition: OcTreeLUTdefs.h:150
LUT_FW_TO_W
#define LUT_FW_TO_W
Definition: OcTreeLUTdefs.h:90
octomap::OcTreeLUT::nf_multiple_values
signed char nf_multiple_values[26][4]
Definition: OcTreeLUT.h:102
LUT_S
#define LUT_S
Definition: OcTreeLUTdefs.h:40
LUT_FN
#define LUT_FN
Definition: OcTreeLUTdefs.h:51
LUT_FSE_TO_F
#define LUT_FSE_TO_F
Definition: OcTreeLUTdefs.h:115
LUT_FS_TO_F
#define LUT_FS_TO_F
Definition: OcTreeLUTdefs.h:83
LUT_FSW_TO_SW
#define LUT_FSW_TO_SW
Definition: OcTreeLUTdefs.h:118
LUT_FE
#define LUT_FE
Definition: OcTreeLUTdefs.h:55
octomap::OcTreeLUT::changeKey
void changeKey(const int &val, OcTreeKey &key, const unsigned short int &i) const
Definition: OcTreeLUT.cpp:537
LUT_RSE_TO_RS
#define LUT_RSE_TO_RS
Definition: OcTreeLUTdefs.h:139
LUT_FSE_TO_FS
#define LUT_FSE_TO_FS
Definition: OcTreeLUTdefs.h:114
LUT_RNW_TO_R
#define LUT_RNW_TO_R
Definition: OcTreeLUTdefs.h:133
LUT_RNE_TO_R
#define LUT_RNE_TO_R
Definition: OcTreeLUTdefs.h:125
LUT_RW_TO_R
#define LUT_RW_TO_R
Definition: OcTreeLUTdefs.h:93
LUT_RNW
#define LUT_RNW
Definition: OcTreeLUTdefs.h:66
LUT_RW
#define LUT_RW
Definition: OcTreeLUTdefs.h:58
LUT_RNW_TO_RW
#define LUT_RNW_TO_RW
Definition: OcTreeLUTdefs.h:132
LUT_RSE_TO_RE
#define LUT_RSE_TO_RE
Definition: OcTreeLUTdefs.h:141
LUT_SE_TO_S
#define LUT_SE_TO_S
Definition: OcTreeLUTdefs.h:78
LUT_RSW_TO_R
#define LUT_RSW_TO_R
Definition: OcTreeLUTdefs.h:148
LUT_RS
#define LUT_RS
Definition: OcTreeLUTdefs.h:54
LUT_N
#define LUT_N
Definition: OcTreeLUTdefs.h:39
LUT_NW
#define LUT_NW
Definition: OcTreeLUTdefs.h:47
LUT_SE
#define LUT_SE
Definition: OcTreeLUTdefs.h:50
LUT_NE_TO_N
#define LUT_NE_TO_N
Definition: OcTreeLUTdefs.h:74
octomap::OcTreeKey::k
unsigned short int k[3]
Definition: OcTreeKey.h:94
LUT_SW_TO_S
#define LUT_SW_TO_S
Definition: OcTreeLUTdefs.h:75
LUT_FSE_TO_SE
#define LUT_FSE_TO_SE
Definition: OcTreeLUTdefs.h:112
LUT_RSE_TO_E
#define LUT_RSE_TO_E
Definition: OcTreeLUTdefs.h:144
octomap::OcTreeLUT::genPos
unsigned int genPos(const OcTreeKey &key, const int &i) const
Definition: OcTreeLUT.cpp:525
LUT_E
#define LUT_E
Definition: OcTreeLUTdefs.h:41
LUT_RSW_TO_RS
#define LUT_RSW_TO_RS
Definition: OcTreeLUTdefs.h:146
LUT_RNW_TO_W
#define LUT_RNW_TO_W
Definition: OcTreeLUTdefs.h:135
LUT_FNW
#define LUT_FNW
Definition: OcTreeLUTdefs.h:62
LUT_FSW_TO_F
#define LUT_FSW_TO_F
Definition: OcTreeLUTdefs.h:123
LUT_FNE_TO_FE
#define LUT_FNE_TO_FE
Definition: OcTreeLUTdefs.h:102
LUT_FSE_TO_S
#define LUT_FSE_TO_S
Definition: OcTreeLUTdefs.h:111
LUT_RSE_TO_SE
#define LUT_RSE_TO_SE
Definition: OcTreeLUTdefs.h:143
LUT_FW_TO_F
#define LUT_FW_TO_F
Definition: OcTreeLUTdefs.h:89
LUT_RE_TO_R
#define LUT_RE_TO_R
Definition: OcTreeLUTdefs.h:91
LUT_NE
#define LUT_NE
Definition: OcTreeLUTdefs.h:48
LUT_R
#define LUT_R
Definition: OcTreeLUTdefs.h:44
LUT_FNE_TO_N
#define LUT_FNE_TO_N
Definition: OcTreeLUTdefs.h:98
LUT_FNE_TO_F
#define LUT_FNE_TO_F
Definition: OcTreeLUTdefs.h:100
LUT_RSW_TO_W
#define LUT_RSW_TO_W
Definition: OcTreeLUTdefs.h:151
LUT_RE
#define LUT_RE
Definition: OcTreeLUTdefs.h:57
LUT_RNE
#define LUT_RNE
Definition: OcTreeLUTdefs.h:65
LUT_FNW_TO_NW
#define LUT_FNW_TO_NW
Definition: OcTreeLUTdefs.h:105
LUT_FNE_TO_E
#define LUT_FNE_TO_E
Definition: OcTreeLUTdefs.h:97
LUT_FSW_TO_W
#define LUT_FSW_TO_W
Definition: OcTreeLUTdefs.h:120
octomap::OcTreeLUT::nf_values
signed char nf_values[8][26]
Definition: OcTreeLUT.h:100
LUT_FSW_TO_FW
#define LUT_FSW_TO_FW
Definition: OcTreeLUTdefs.h:122
LUT_FNW_TO_FN
#define LUT_FNW_TO_FN
Definition: OcTreeLUTdefs.h:109
octomap::OcTreeLUT::max_depth
unsigned int max_depth
Definition: OcTreeLUT.h:98
LUT_RSW_TO_RW
#define LUT_RSW_TO_RW
Definition: OcTreeLUTdefs.h:147
octomap::OcTreeLUT::nf_rec_values
signed char nf_rec_values[8][26]
Definition: OcTreeLUT.h:101
LUT_RNW_TO_RN
#define LUT_RNW_TO_RN
Definition: OcTreeLUTdefs.h:134
LUT_RNE_TO_N
#define LUT_RNE_TO_N
Definition: OcTreeLUTdefs.h:129
LUT_FSE_TO_E
#define LUT_FSE_TO_E
Definition: OcTreeLUTdefs.h:113
LUT_SELF
#define LUT_SELF
Definition: OcTreeLUTdefs.h:153
LUT_RN_TO_R
#define LUT_RN_TO_R
Definition: OcTreeLUTdefs.h:82
LUT_SE_TO_E
#define LUT_SE_TO_E
Definition: OcTreeLUTdefs.h:77
LUT_RSE_TO_R
#define LUT_RSE_TO_R
Definition: OcTreeLUTdefs.h:140
LUT_FW
#define LUT_FW
Definition: OcTreeLUTdefs.h:56
LUT_RNE_TO_E
#define LUT_RNE_TO_E
Definition: OcTreeLUTdefs.h:128
LUT_RSE_TO_S
#define LUT_RSE_TO_S
Definition: OcTreeLUTdefs.h:142
LUT_RNE_TO_NE
#define LUT_RNE_TO_NE
Definition: OcTreeLUTdefs.h:130
LUT_RW_TO_W
#define LUT_RW_TO_W
Definition: OcTreeLUTdefs.h:94
LUT_RS_TO_S
#define LUT_RS_TO_S
Definition: OcTreeLUTdefs.h:86
LUT_RSW
#define LUT_RSW
Definition: OcTreeLUTdefs.h:68
LUT_RN
#define LUT_RN
Definition: OcTreeLUTdefs.h:52
LUT_F
#define LUT_F
Definition: OcTreeLUTdefs.h:43
LUT_RSW_TO_SW
#define LUT_RSW_TO_SW
Definition: OcTreeLUTdefs.h:149
LUT_FNW_TO_F
#define LUT_FNW_TO_F
Definition: OcTreeLUTdefs.h:108
LUT_FSW
#define LUT_FSW
Definition: OcTreeLUTdefs.h:64
LUT_RNE_TO_RN
#define LUT_RNE_TO_RN
Definition: OcTreeLUTdefs.h:127
LUT_RNW_TO_NW
#define LUT_RNW_TO_NW
Definition: OcTreeLUTdefs.h:136
LUT_RE_TO_E
#define LUT_RE_TO_E
Definition: OcTreeLUTdefs.h:92
LUT_FE_TO_F
#define LUT_FE_TO_F
Definition: OcTreeLUTdefs.h:87
LUT_NW_TO_W
#define LUT_NW_TO_W
Definition: OcTreeLUTdefs.h:71

octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Mon Jun 10 2019 14:00:13