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 
48  }
49 
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 
71 
72  // LUT_S
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 
100 
101  // LUT_W
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 
129 
130  // LUT_R
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 
163 
164  // LUT_NE
173 
174  // LUT_SW
183 
184  // LUT_SE
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 
214 
215  // LUT_RN
224 
225  // LUT_FS
234 
235  // LUT_RS
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 
265 
266  // LUT_FW
275 
276  // LUT_RE
285 
286  // LUT_RW
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 
320 
321  // LUT_FNW
330 
331  // LUT_FSE
340 
341  // LUT_FSW
350 
351  // LUT_RNE
360 
361  // LUT_RNW
370 
371  // LUT_RSE
380 
381  // LUT_RSW
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;
426 
427  nf_multiple_values[LUT_NE][0] = 0;
428  nf_multiple_values[LUT_NE][1] = 4;
431 
432  nf_multiple_values[LUT_SW][0] = 3;
433  nf_multiple_values[LUT_SW][1] = 7;
436 
437  nf_multiple_values[LUT_SE][0] = 2;
438  nf_multiple_values[LUT_SE][1] = 6;
441 
442  nf_multiple_values[LUT_FN][0] = 1;
443  nf_multiple_values[LUT_FN][1] = 3;
446 
447  nf_multiple_values[LUT_RN][0] = 4;
448  nf_multiple_values[LUT_RN][1] = 5;
451 
452  nf_multiple_values[LUT_FS][0] = 2;
453  nf_multiple_values[LUT_FS][1] = 3;
456 
457  nf_multiple_values[LUT_RS][0] = 6;
458  nf_multiple_values[LUT_RS][1] = 7;
461 
462  nf_multiple_values[LUT_FE][0] = 0;
463  nf_multiple_values[LUT_FE][1] = 2;
466 
467  nf_multiple_values[LUT_FW][0] = 1;
468  nf_multiple_values[LUT_FW][1] = 3;
471 
472  nf_multiple_values[LUT_RE][0] = 4;
473  nf_multiple_values[LUT_RE][1] = 6;
476 
477  nf_multiple_values[LUT_RW][0] = 5;
478  nf_multiple_values[LUT_RW][1] = 7;
481 
482  nf_multiple_values[LUT_FNE][0] = 0;
486 
487  nf_multiple_values[LUT_FNW][0] = 1;
491 
492  nf_multiple_values[LUT_FSE][0] = 2;
496 
497  nf_multiple_values[LUT_FSW][0] = 3;
501 
502  nf_multiple_values[LUT_RNE][0] = 4;
506 
507  nf_multiple_values[LUT_RNW][0] = 5;
511 
512  nf_multiple_values[LUT_RSE][0] = 6;
516 
517  nf_multiple_values[LUT_RSW][0] = 7;
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 
#define LUT_FN_TO_F
Definition: OcTreeLUTdefs.h:79
#define LUT_FSE
Definition: OcTreeLUTdefs.h:63
#define LUT_FNW_TO_W
#define LUT_FNW_TO_FW
#define LUT_FNE
Definition: OcTreeLUTdefs.h:61
#define LUT_RS_TO_R
Definition: OcTreeLUTdefs.h:85
#define LUT_NW_TO_N
Definition: OcTreeLUTdefs.h:72
#define LUT_FE_TO_E
Definition: OcTreeLUTdefs.h:88
#define LUT_FN_TO_N
Definition: OcTreeLUTdefs.h:80
bool genNeighborKey(const OcTreeKey &node_key, const signed char &dir, OcTreeKey &neighbor_key) const
Definition: OcTreeLUT.cpp:583
#define LUT_FSE_TO_FE
#define LUT_FNE_TO_FN
#define LUT_W
Definition: OcTreeLUTdefs.h:42
#define LUT_RSE
Definition: OcTreeLUTdefs.h:67
#define LUT_SW
Definition: OcTreeLUTdefs.h:49
#define LUT_SW_TO_W
Definition: OcTreeLUTdefs.h:76
#define LUT_RNW_TO_N
#define LUT_FSW_TO_FS
#define LUT_NE_TO_E
Definition: OcTreeLUTdefs.h:73
#define LUT_RN_TO_N
Definition: OcTreeLUTdefs.h:81
#define LUT_FNW_TO_N
#define LUT_RNE_TO_RE
#define LUT_FS_TO_S
Definition: OcTreeLUTdefs.h:84
OcTreeLUT(unsigned int _max_depth)
Definition: OcTreeLUT.cpp:42
#define LUT_FS
Definition: OcTreeLUTdefs.h:53
#define LUT_FNE_TO_NE
Definition: OcTreeLUTdefs.h:99
#define LUT_NO_REC
#define LUT_FSW_TO_S
#define LUT_RSW_TO_S
#define LUT_FW_TO_W
Definition: OcTreeLUTdefs.h:90
signed char nf_multiple_values[26][4]
Definition: OcTreeLUT.h:102
#define LUT_S
Definition: OcTreeLUTdefs.h:40
#define LUT_FN
Definition: OcTreeLUTdefs.h:51
#define LUT_FSE_TO_F
#define LUT_FS_TO_F
Definition: OcTreeLUTdefs.h:83
#define LUT_FSW_TO_SW
#define LUT_FE
Definition: OcTreeLUTdefs.h:55
void changeKey(const int &val, OcTreeKey &key, const unsigned short int &i) const
Definition: OcTreeLUT.cpp:537
#define LUT_RSE_TO_RS
#define LUT_FSE_TO_FS
#define LUT_RNW_TO_R
#define LUT_RNE_TO_R
#define LUT_RW_TO_R
Definition: OcTreeLUTdefs.h:93
#define LUT_RNW
Definition: OcTreeLUTdefs.h:66
#define LUT_RW
Definition: OcTreeLUTdefs.h:58
#define LUT_RNW_TO_RW
#define LUT_RSE_TO_RE
#define LUT_SE_TO_S
Definition: OcTreeLUTdefs.h:78
#define LUT_RSW_TO_R
#define LUT_RS
Definition: OcTreeLUTdefs.h:54
#define LUT_N
Definition: OcTreeLUTdefs.h:39
#define LUT_NW
Definition: OcTreeLUTdefs.h:47
#define LUT_SE
Definition: OcTreeLUTdefs.h:50
#define LUT_NE_TO_N
Definition: OcTreeLUTdefs.h:74
unsigned short int k[3]
Definition: OcTreeKey.h:94
#define LUT_SW_TO_S
Definition: OcTreeLUTdefs.h:75
#define LUT_FSE_TO_SE
#define LUT_RSE_TO_E
unsigned int genPos(const OcTreeKey &key, const int &i) const
Definition: OcTreeLUT.cpp:525
#define LUT_E
Definition: OcTreeLUTdefs.h:41
#define LUT_RSW_TO_RS
#define LUT_RNW_TO_W
#define LUT_FNW
Definition: OcTreeLUTdefs.h:62
#define LUT_FSW_TO_F
#define LUT_FNE_TO_FE
#define LUT_FSE_TO_S
#define LUT_RSE_TO_SE
#define LUT_FW_TO_F
Definition: OcTreeLUTdefs.h:89
#define LUT_RE_TO_R
Definition: OcTreeLUTdefs.h:91
#define LUT_NE
Definition: OcTreeLUTdefs.h:48
#define LUT_R
Definition: OcTreeLUTdefs.h:44
#define LUT_FNE_TO_N
Definition: OcTreeLUTdefs.h:98
#define LUT_FNE_TO_F
#define LUT_RSW_TO_W
#define LUT_RE
Definition: OcTreeLUTdefs.h:57
#define LUT_RNE
Definition: OcTreeLUTdefs.h:65
#define LUT_FNW_TO_NW
#define LUT_FNE_TO_E
Definition: OcTreeLUTdefs.h:97
#define LUT_FSW_TO_W
signed char nf_values[8][26]
Definition: OcTreeLUT.h:100
#define LUT_FSW_TO_FW
#define LUT_FNW_TO_FN
unsigned int max_depth
Definition: OcTreeLUT.h:98
#define LUT_RSW_TO_RW
signed char nf_rec_values[8][26]
Definition: OcTreeLUT.h:101
#define LUT_RNW_TO_RN
#define LUT_RNE_TO_N
#define LUT_FSE_TO_E
#define LUT_SELF
#define LUT_RN_TO_R
Definition: OcTreeLUTdefs.h:82
#define LUT_SE_TO_E
Definition: OcTreeLUTdefs.h:77
#define LUT_RSE_TO_R
#define LUT_FW
Definition: OcTreeLUTdefs.h:56
#define LUT_RNE_TO_E
#define LUT_RSE_TO_S
#define LUT_RNE_TO_NE
#define LUT_RW_TO_W
Definition: OcTreeLUTdefs.h:94
#define LUT_RS_TO_S
Definition: OcTreeLUTdefs.h:86
#define LUT_RSW
Definition: OcTreeLUTdefs.h:68
#define LUT_RN
Definition: OcTreeLUTdefs.h:52
#define LUT_F
Definition: OcTreeLUTdefs.h:43
#define LUT_RSW_TO_SW
#define LUT_FNW_TO_F
#define LUT_FSW
Definition: OcTreeLUTdefs.h:64
#define LUT_RNE_TO_RN
#define LUT_RNW_TO_NW
#define LUT_RE_TO_E
Definition: OcTreeLUTdefs.h:92
#define LUT_FE_TO_F
Definition: OcTreeLUTdefs.h:87
#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