00001 /* 00002 * OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees 00003 * http://octomap.github.com/ 00004 * 00005 * Copyright (c) 2009-2013, R. Schmitt, K.M. Wurm and A. Hornung, 00006 * University of Freiburg. All rights reserved. 00007 * License: New BSD 00008 * 00009 * Redistribution and use in source and binary forms, with or without 00010 * modification, are permitted provided that the following conditions are met: 00011 * 00012 * * Redistributions of source code must retain the above copyright 00013 * notice, this list of conditions and the following disclaimer. 00014 * * Redistributions in binary form must reproduce the above copyright 00015 * notice, this list of conditions and the following disclaimer in the 00016 * documentation and/or other materials provided with the distribution. 00017 * * Neither the name of the University of Freiburg nor the names of its 00018 * contributors may be used to endorse or promote products derived from 00019 * this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00022 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00023 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00024 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00025 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00026 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00027 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00028 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00029 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00030 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00031 * POSSIBILITY OF SUCH DAMAGE. 00032 */ 00033 00034 00035 #include <list> 00036 #include <queue> 00037 #include <octomap/OcTreeLUT.h> 00038 00039 namespace octomap { 00040 00041 00042 OcTreeLUT::OcTreeLUT(unsigned int _max_depth) : 00043 max_depth(_max_depth) { 00044 initLUT(); 00045 } 00046 00047 OcTreeLUT::~OcTreeLUT() { 00048 } 00049 00050 void OcTreeLUT::initLUT() { 00051 00052 // begin Face-Neighbors 00053 // LUT_N 00054 nf_values[0][LUT_S] = nf_values[0][LUT_N] = 2; 00055 nf_values[1][LUT_S] = nf_values[1][LUT_N] = 3; 00056 nf_values[2][LUT_S] = nf_values[2][LUT_N] = 0; 00057 nf_values[3][LUT_S] = nf_values[3][LUT_N] = 1; 00058 nf_values[4][LUT_S] = nf_values[4][LUT_N] = 6; 00059 nf_values[5][LUT_S] = nf_values[5][LUT_N] = 7; 00060 nf_values[6][LUT_S] = nf_values[6][LUT_N] = 4; 00061 nf_values[7][LUT_S] = nf_values[7][LUT_N] = 5; 00062 00063 nf_rec_values[0][LUT_N] = LUT_NO_REC; 00064 nf_rec_values[1][LUT_N] = LUT_NO_REC; 00065 nf_rec_values[2][LUT_N] = LUT_SELF; 00066 nf_rec_values[3][LUT_N] = LUT_SELF; 00067 nf_rec_values[4][LUT_N] = LUT_NO_REC; 00068 nf_rec_values[5][LUT_N] = LUT_NO_REC; 00069 nf_rec_values[6][LUT_N] = LUT_SELF; 00070 nf_rec_values[7][LUT_N] = LUT_SELF; 00071 00072 // LUT_S 00073 nf_rec_values[0][LUT_S] = LUT_SELF; 00074 nf_rec_values[1][LUT_S] = LUT_SELF; 00075 nf_rec_values[2][LUT_S] = LUT_NO_REC; 00076 nf_rec_values[3][LUT_S] = LUT_NO_REC; 00077 nf_rec_values[4][LUT_S] = LUT_SELF; 00078 nf_rec_values[5][LUT_S] = LUT_SELF; 00079 nf_rec_values[6][LUT_S] = LUT_NO_REC; 00080 nf_rec_values[7][LUT_S] = LUT_NO_REC; 00081 00082 // LUT_E 00083 nf_values[0][LUT_W] = nf_values[0][LUT_E] = 1; 00084 nf_values[1][LUT_W] = nf_values[1][LUT_E] = 0; 00085 nf_values[2][LUT_W] = nf_values[2][LUT_E] = 3; 00086 nf_values[3][LUT_W] = nf_values[3][LUT_E] = 2; 00087 nf_values[4][LUT_W] = nf_values[4][LUT_E] = 5; 00088 nf_values[5][LUT_W] = nf_values[5][LUT_E] = 4; 00089 nf_values[6][LUT_W] = nf_values[6][LUT_E] = 7; 00090 nf_values[7][LUT_W] = nf_values[7][LUT_E] = 6; 00091 00092 nf_rec_values[0][LUT_E] = LUT_NO_REC; 00093 nf_rec_values[1][LUT_E] = LUT_SELF; 00094 nf_rec_values[2][LUT_E] = LUT_NO_REC; 00095 nf_rec_values[3][LUT_E] = LUT_SELF; 00096 nf_rec_values[4][LUT_E] = LUT_NO_REC; 00097 nf_rec_values[5][LUT_E] = LUT_SELF; 00098 nf_rec_values[6][LUT_E] = LUT_NO_REC; 00099 nf_rec_values[7][LUT_E] = LUT_SELF; 00100 00101 // LUT_W 00102 nf_rec_values[0][LUT_W] = LUT_SELF; 00103 nf_rec_values[1][LUT_W] = LUT_NO_REC; 00104 nf_rec_values[2][LUT_W] = LUT_SELF; 00105 nf_rec_values[3][LUT_W] = LUT_NO_REC; 00106 nf_rec_values[4][LUT_W] = LUT_SELF; 00107 nf_rec_values[5][LUT_W] = LUT_NO_REC; 00108 nf_rec_values[6][LUT_W] = LUT_SELF; 00109 nf_rec_values[7][LUT_W] = LUT_NO_REC; 00110 00111 // LUT_F 00112 nf_values[0][LUT_R] = nf_values[0][LUT_F] = 4; 00113 nf_values[1][LUT_R] = nf_values[1][LUT_F] = 5; 00114 nf_values[2][LUT_R] = nf_values[2][LUT_F] = 6; 00115 nf_values[3][LUT_R] = nf_values[3][LUT_F] = 7; 00116 nf_values[4][LUT_R] = nf_values[4][LUT_F] = 0; 00117 nf_values[5][LUT_R] = nf_values[5][LUT_F] = 1; 00118 nf_values[6][LUT_R] = nf_values[6][LUT_F] = 2; 00119 nf_values[7][LUT_R] = nf_values[7][LUT_F] = 3; 00120 00121 nf_rec_values[0][LUT_F] = LUT_NO_REC; 00122 nf_rec_values[1][LUT_F] = LUT_NO_REC; 00123 nf_rec_values[2][LUT_F] = LUT_NO_REC; 00124 nf_rec_values[3][LUT_F] = LUT_NO_REC; 00125 nf_rec_values[4][LUT_F] = LUT_SELF; 00126 nf_rec_values[5][LUT_F] = LUT_SELF; 00127 nf_rec_values[6][LUT_F] = LUT_SELF; 00128 nf_rec_values[7][LUT_F] = LUT_SELF; 00129 00130 // LUT_R 00131 nf_rec_values[0][LUT_R] = LUT_SELF; 00132 nf_rec_values[1][LUT_R] = LUT_SELF; 00133 nf_rec_values[2][LUT_R] = LUT_SELF; 00134 nf_rec_values[3][LUT_R] = LUT_SELF; 00135 nf_rec_values[4][LUT_R] = LUT_NO_REC; 00136 nf_rec_values[5][LUT_R] = LUT_NO_REC; 00137 nf_rec_values[6][LUT_R] = LUT_NO_REC; 00138 nf_rec_values[7][LUT_R] = LUT_NO_REC; 00139 // end Face-Neighbors 00140 00141 00142 // begin Edge-Neighbors 00143 // LUT_NW 00144 for (int i = LUT_NW; i < LUT_SE+1; ++i) { 00145 nf_values[0][i] = 3; 00146 nf_values[1][i] = 2; 00147 nf_values[2][i] = 1; 00148 nf_values[3][i] = 0; 00149 nf_values[4][i] = 7; 00150 nf_values[5][i] = 6; 00151 nf_values[6][i] = 5; 00152 nf_values[7][i] = 4; 00153 } 00154 00155 nf_rec_values[0][LUT_NW] = LUT_NW_TO_W; 00156 nf_rec_values[1][LUT_NW] = LUT_NO_REC; 00157 nf_rec_values[2][LUT_NW] = LUT_SELF; 00158 nf_rec_values[3][LUT_NW] = LUT_NW_TO_N; 00159 nf_rec_values[4][LUT_NW] = LUT_NW_TO_W; 00160 nf_rec_values[5][LUT_NW] = LUT_NO_REC; 00161 nf_rec_values[6][LUT_NW] = LUT_SELF; 00162 nf_rec_values[7][LUT_NW] = LUT_NW_TO_N; 00163 00164 // LUT_NE 00165 nf_rec_values[0][LUT_NE] = LUT_NO_REC; 00166 nf_rec_values[1][LUT_NE] = LUT_NE_TO_E; 00167 nf_rec_values[2][LUT_NE] = LUT_NE_TO_N; 00168 nf_rec_values[3][LUT_NE] = LUT_SELF; 00169 nf_rec_values[4][LUT_NE] = LUT_NO_REC; 00170 nf_rec_values[5][LUT_NE] = LUT_NE_TO_E; 00171 nf_rec_values[6][LUT_NE] = LUT_NE_TO_N; 00172 nf_rec_values[7][LUT_NE] = LUT_SELF; 00173 00174 // LUT_SW 00175 nf_rec_values[0][LUT_SW] = LUT_SELF; 00176 nf_rec_values[1][LUT_SW] = LUT_SW_TO_S; 00177 nf_rec_values[2][LUT_SW] = LUT_SW_TO_W; 00178 nf_rec_values[3][LUT_SW] = LUT_NO_REC; 00179 nf_rec_values[4][LUT_SW] = LUT_SELF; 00180 nf_rec_values[5][LUT_SW] = LUT_SW_TO_S; 00181 nf_rec_values[6][LUT_SW] = LUT_SW_TO_W; 00182 nf_rec_values[7][LUT_SW] = LUT_NO_REC; 00183 00184 // LUT_SE 00185 nf_rec_values[0][LUT_SE] = LUT_SE_TO_S; 00186 nf_rec_values[1][LUT_SE] = LUT_SELF; 00187 nf_rec_values[2][LUT_SE] = LUT_NO_REC; 00188 nf_rec_values[3][LUT_SE] = LUT_SE_TO_E; 00189 nf_rec_values[4][LUT_SE] = LUT_SE_TO_S; 00190 nf_rec_values[5][LUT_SE] = LUT_SELF; 00191 nf_rec_values[6][LUT_SE] = LUT_NO_REC; 00192 nf_rec_values[7][LUT_SE] = LUT_SE_TO_E; 00193 00194 // LUT_FN 00195 for (int i = LUT_FN; i < LUT_RS+1; ++i) { 00196 nf_values[0][i] = 6; 00197 nf_values[1][i] = 7; 00198 nf_values[2][i] = 4; 00199 nf_values[3][i] = 5; 00200 nf_values[4][i] = 2; 00201 nf_values[5][i] = 3; 00202 nf_values[6][i] = 0; 00203 nf_values[7][i] = 1; 00204 } 00205 00206 nf_rec_values[0][LUT_FN] = LUT_NO_REC; 00207 nf_rec_values[1][LUT_FN] = LUT_NO_REC; 00208 nf_rec_values[2][LUT_FN] = LUT_FN_TO_N; 00209 nf_rec_values[3][LUT_FN] = LUT_FN_TO_N; 00210 nf_rec_values[4][LUT_FN] = LUT_FN_TO_F; 00211 nf_rec_values[5][LUT_FN] = LUT_FN_TO_F; 00212 nf_rec_values[6][LUT_FN] = LUT_SELF; 00213 nf_rec_values[7][LUT_FN] = LUT_SELF; 00214 00215 // LUT_RN 00216 nf_rec_values[0][LUT_RN] = LUT_RN_TO_R; 00217 nf_rec_values[1][LUT_RN] = LUT_RN_TO_R; 00218 nf_rec_values[2][LUT_RN] = LUT_SELF; 00219 nf_rec_values[3][LUT_RN] = LUT_SELF; 00220 nf_rec_values[4][LUT_RN] = LUT_NO_REC; 00221 nf_rec_values[5][LUT_RN] = LUT_NO_REC; 00222 nf_rec_values[6][LUT_RN] = LUT_RN_TO_N; 00223 nf_rec_values[7][LUT_RN] = LUT_RN_TO_N; 00224 00225 // LUT_FS 00226 nf_rec_values[0][LUT_FS] = LUT_FS_TO_S; 00227 nf_rec_values[1][LUT_FS] = LUT_FS_TO_S; 00228 nf_rec_values[2][LUT_FS] = LUT_NO_REC; 00229 nf_rec_values[3][LUT_FS] = LUT_NO_REC; 00230 nf_rec_values[4][LUT_FS] = LUT_SELF; 00231 nf_rec_values[5][LUT_FS] = LUT_SELF; 00232 nf_rec_values[6][LUT_FS] = LUT_FS_TO_F; 00233 nf_rec_values[7][LUT_FS] = LUT_FS_TO_F; 00234 00235 // LUT_RS 00236 nf_rec_values[0][LUT_RS] = LUT_SELF; 00237 nf_rec_values[1][LUT_RS] = LUT_SELF; 00238 nf_rec_values[2][LUT_RS] = LUT_RS_TO_R; 00239 nf_rec_values[3][LUT_RS] = LUT_RS_TO_R; 00240 nf_rec_values[4][LUT_RS] = LUT_RS_TO_S; 00241 nf_rec_values[5][LUT_RS] = LUT_RS_TO_S; 00242 nf_rec_values[6][LUT_RS] = LUT_NO_REC; 00243 nf_rec_values[7][LUT_RS] = LUT_NO_REC; 00244 00245 // LUT_FE 00246 for (int i = LUT_FE; i < LUT_RW+1; ++i) { 00247 nf_values[0][i] = 5; 00248 nf_values[1][i] = 4; 00249 nf_values[2][i] = 7; 00250 nf_values[3][i] = 6; 00251 nf_values[4][i] = 1; 00252 nf_values[5][i] = 0; 00253 nf_values[6][i] = 3; 00254 nf_values[7][i] = 2; 00255 } 00256 00257 nf_rec_values[0][LUT_FE] = LUT_NO_REC; 00258 nf_rec_values[1][LUT_FE] = LUT_FE_TO_E; 00259 nf_rec_values[2][LUT_FE] = LUT_NO_REC; 00260 nf_rec_values[3][LUT_FE] = LUT_FE_TO_E; 00261 nf_rec_values[4][LUT_FE] = LUT_FE_TO_F; 00262 nf_rec_values[5][LUT_FE] = LUT_SELF; 00263 nf_rec_values[6][LUT_FE] = LUT_FE_TO_F; 00264 nf_rec_values[7][LUT_FE] = LUT_SELF; 00265 00266 // LUT_FW 00267 nf_rec_values[0][LUT_FW] = LUT_FW_TO_W; 00268 nf_rec_values[1][LUT_FW] = LUT_NO_REC; 00269 nf_rec_values[2][LUT_FW] = LUT_FW_TO_W; 00270 nf_rec_values[3][LUT_FW] = LUT_NO_REC; 00271 nf_rec_values[4][LUT_FW] = LUT_SELF; 00272 nf_rec_values[5][LUT_FW] = LUT_FW_TO_F; 00273 nf_rec_values[6][LUT_FW] = LUT_SELF; 00274 nf_rec_values[7][LUT_FW] = LUT_FW_TO_F; 00275 00276 // LUT_RE 00277 nf_rec_values[0][LUT_RE] = LUT_RE_TO_R; 00278 nf_rec_values[1][LUT_RE] = LUT_SELF; 00279 nf_rec_values[2][LUT_RE] = LUT_RE_TO_R; 00280 nf_rec_values[3][LUT_RE] = LUT_SELF; 00281 nf_rec_values[4][LUT_RE] = LUT_NO_REC; 00282 nf_rec_values[5][LUT_RE] = LUT_RE_TO_E; 00283 nf_rec_values[6][LUT_RE] = LUT_NO_REC; 00284 nf_rec_values[7][LUT_RE] = LUT_RE_TO_E; 00285 00286 // LUT_RW 00287 nf_rec_values[0][LUT_RW] = LUT_SELF; 00288 nf_rec_values[1][LUT_RW] = LUT_RW_TO_R; 00289 nf_rec_values[2][LUT_RW] = LUT_SELF; 00290 nf_rec_values[3][LUT_RW] = LUT_RW_TO_R; 00291 nf_rec_values[4][LUT_RW] = LUT_RW_TO_W; 00292 nf_rec_values[5][LUT_RW] = LUT_NO_REC; 00293 nf_rec_values[6][LUT_RW] = LUT_RW_TO_W; 00294 nf_rec_values[7][LUT_RW] = LUT_NO_REC; 00295 00296 // end Edge-Neighbors 00297 00298 00299 // begin Vertex-Neighbors 00300 // LUT_FNE 00301 for (int i = LUT_FNE; i < LUT_RSW+1; ++i) { 00302 nf_values[0][i] = 7; 00303 nf_values[1][i] = 6; 00304 nf_values[2][i] = 5; 00305 nf_values[3][i] = 4; 00306 nf_values[4][i] = 3; 00307 nf_values[5][i] = 2; 00308 nf_values[6][i] = 1; 00309 nf_values[7][i] = 0; 00310 } 00311 00312 nf_rec_values[0][LUT_FNE] = LUT_NO_REC; 00313 nf_rec_values[1][LUT_FNE] = LUT_FNE_TO_E; 00314 nf_rec_values[2][LUT_FNE] = LUT_FNE_TO_N; 00315 nf_rec_values[3][LUT_FNE] = LUT_FNE_TO_NE; 00316 nf_rec_values[4][LUT_FNE] = LUT_FNE_TO_F; 00317 nf_rec_values[5][LUT_FNE] = LUT_FNE_TO_FE; 00318 nf_rec_values[6][LUT_FNE] = LUT_FNE_TO_FN; 00319 nf_rec_values[7][LUT_FNE] = LUT_SELF; 00320 00321 // LUT_FNW 00322 nf_rec_values[0][LUT_FNW] = LUT_FNW_TO_W; 00323 nf_rec_values[1][LUT_FNW] = LUT_NO_REC; 00324 nf_rec_values[2][LUT_FNW] = LUT_FNW_TO_NW; 00325 nf_rec_values[3][LUT_FNW] = LUT_FNW_TO_N; 00326 nf_rec_values[4][LUT_FNW] = LUT_FNW_TO_FW; 00327 nf_rec_values[5][LUT_FNW] = LUT_FNW_TO_F; 00328 nf_rec_values[6][LUT_FNW] = LUT_SELF; 00329 nf_rec_values[7][LUT_FNW] = LUT_FNW_TO_FN; 00330 00331 // LUT_FSE 00332 nf_rec_values[0][LUT_FSE] = LUT_FSE_TO_S; 00333 nf_rec_values[1][LUT_FSE] = LUT_FSE_TO_SE; 00334 nf_rec_values[2][LUT_FSE] = LUT_NO_REC; 00335 nf_rec_values[3][LUT_FSE] = LUT_FSE_TO_E; 00336 nf_rec_values[4][LUT_FSE] = LUT_FSE_TO_FS; 00337 nf_rec_values[5][LUT_FSE] = LUT_SELF; 00338 nf_rec_values[6][LUT_FSE] = LUT_FSE_TO_F; 00339 nf_rec_values[7][LUT_FSE] = LUT_FSE_TO_FE; 00340 00341 // LUT_FSW 00342 nf_rec_values[0][LUT_FSW] = LUT_FSW_TO_SW; 00343 nf_rec_values[1][LUT_FSW] = LUT_FSW_TO_S; 00344 nf_rec_values[2][LUT_FSW] = LUT_FSW_TO_W; 00345 nf_rec_values[3][LUT_FSW] = LUT_NO_REC; 00346 nf_rec_values[4][LUT_FSW] = LUT_SELF; 00347 nf_rec_values[5][LUT_FSW] = LUT_FSW_TO_FS; 00348 nf_rec_values[6][LUT_FSW] = LUT_FSW_TO_FW; 00349 nf_rec_values[7][LUT_FSW] = LUT_FSW_TO_F; 00350 00351 // LUT_RNE 00352 nf_rec_values[0][LUT_RNE] = LUT_RNE_TO_R; 00353 nf_rec_values[1][LUT_RNE] = LUT_RNE_TO_RE; 00354 nf_rec_values[2][LUT_RNE] = LUT_RNE_TO_RN; 00355 nf_rec_values[3][LUT_RNE] = LUT_SELF; 00356 nf_rec_values[4][LUT_RNE] = LUT_NO_REC; 00357 nf_rec_values[5][LUT_RNE] = LUT_RNE_TO_E; 00358 nf_rec_values[6][LUT_RNE] = LUT_RNE_TO_N; 00359 nf_rec_values[7][LUT_RNE] = LUT_RNE_TO_NE; 00360 00361 // LUT_RNW 00362 nf_rec_values[0][LUT_RNW] = LUT_RNW_TO_RW; 00363 nf_rec_values[1][LUT_RNW] = LUT_RNW_TO_R; 00364 nf_rec_values[2][LUT_RNW] = LUT_SELF; 00365 nf_rec_values[3][LUT_RNW] = LUT_RNW_TO_RN; 00366 nf_rec_values[4][LUT_RNW] = LUT_RNW_TO_W; 00367 nf_rec_values[5][LUT_RNW] = LUT_NO_REC; 00368 nf_rec_values[6][LUT_RNW] = LUT_RNW_TO_NW; 00369 nf_rec_values[7][LUT_RNW] = LUT_RNW_TO_N; 00370 00371 // LUT_RSE 00372 nf_rec_values[0][LUT_RSE] = LUT_RSE_TO_RS; 00373 nf_rec_values[1][LUT_RSE] = LUT_SELF; 00374 nf_rec_values[2][LUT_RSE] = LUT_RSE_TO_R; 00375 nf_rec_values[3][LUT_RSE] = LUT_RSE_TO_RE; 00376 nf_rec_values[4][LUT_RSE] = LUT_RSE_TO_S; 00377 nf_rec_values[5][LUT_RSE] = LUT_RSE_TO_SE; 00378 nf_rec_values[6][LUT_RSE] = LUT_NO_REC; 00379 nf_rec_values[7][LUT_RSE] = LUT_RSE_TO_E; 00380 00381 // LUT_RSW 00382 nf_rec_values[0][LUT_RSW] = LUT_SELF; 00383 nf_rec_values[1][LUT_RSW] = LUT_RSW_TO_RS; 00384 nf_rec_values[2][LUT_RSW] = LUT_RSW_TO_RW; 00385 nf_rec_values[3][LUT_RSW] = LUT_RSW_TO_R; 00386 nf_rec_values[4][LUT_RSW] = LUT_RSW_TO_SW; 00387 nf_rec_values[5][LUT_RSW] = LUT_RSW_TO_S; 00388 nf_rec_values[6][LUT_RSW] = LUT_RSW_TO_W; 00389 nf_rec_values[7][LUT_RSW] = LUT_NO_REC; 00390 00391 00392 nf_multiple_values[LUT_N][0] = 0; 00393 nf_multiple_values[LUT_N][1] = 1; 00394 nf_multiple_values[LUT_N][2] = 4; 00395 nf_multiple_values[LUT_N][3] = 5; 00396 00397 nf_multiple_values[LUT_S][0] = 2; 00398 nf_multiple_values[LUT_S][1] = 3; 00399 nf_multiple_values[LUT_S][2] = 6; 00400 nf_multiple_values[LUT_S][3] = 7; 00401 00402 nf_multiple_values[LUT_E][0] = 0; 00403 nf_multiple_values[LUT_E][1] = 2; 00404 nf_multiple_values[LUT_E][2] = 4; 00405 nf_multiple_values[LUT_E][3] = 6; 00406 00407 nf_multiple_values[LUT_W][0] = 1; 00408 nf_multiple_values[LUT_W][1] = 3; 00409 nf_multiple_values[LUT_W][2] = 5; 00410 nf_multiple_values[LUT_W][3] = 7; 00411 00412 nf_multiple_values[LUT_F][0] = 0; 00413 nf_multiple_values[LUT_F][1] = 1; 00414 nf_multiple_values[LUT_F][2] = 2; 00415 nf_multiple_values[LUT_F][3] = 3; 00416 00417 nf_multiple_values[LUT_R][0] = 4; 00418 nf_multiple_values[LUT_R][1] = 5; 00419 nf_multiple_values[LUT_R][2] = 6; 00420 nf_multiple_values[LUT_R][3] = 7; 00421 00422 nf_multiple_values[LUT_NW][0] = 1; 00423 nf_multiple_values[LUT_NW][1] = 5; 00424 nf_multiple_values[LUT_NW][2] = LUT_NO_REC; 00425 nf_multiple_values[LUT_NW][3] = LUT_NO_REC; 00426 00427 nf_multiple_values[LUT_NE][0] = 0; 00428 nf_multiple_values[LUT_NE][1] = 4; 00429 nf_multiple_values[LUT_NE][2] = LUT_NO_REC; 00430 nf_multiple_values[LUT_NE][3] = LUT_NO_REC; 00431 00432 nf_multiple_values[LUT_SW][0] = 3; 00433 nf_multiple_values[LUT_SW][1] = 7; 00434 nf_multiple_values[LUT_SW][2] = LUT_NO_REC; 00435 nf_multiple_values[LUT_SW][3] = LUT_NO_REC; 00436 00437 nf_multiple_values[LUT_SE][0] = 2; 00438 nf_multiple_values[LUT_SE][1] = 6; 00439 nf_multiple_values[LUT_SE][2] = LUT_NO_REC; 00440 nf_multiple_values[LUT_SE][3] = LUT_NO_REC; 00441 00442 nf_multiple_values[LUT_FN][0] = 1; 00443 nf_multiple_values[LUT_FN][1] = 3; 00444 nf_multiple_values[LUT_FN][2] = LUT_NO_REC; 00445 nf_multiple_values[LUT_FN][3] = LUT_NO_REC; 00446 00447 nf_multiple_values[LUT_RN][0] = 4; 00448 nf_multiple_values[LUT_RN][1] = 5; 00449 nf_multiple_values[LUT_RN][2] = LUT_NO_REC; 00450 nf_multiple_values[LUT_RN][3] = LUT_NO_REC; 00451 00452 nf_multiple_values[LUT_FS][0] = 2; 00453 nf_multiple_values[LUT_FS][1] = 3; 00454 nf_multiple_values[LUT_FS][2] = LUT_NO_REC; 00455 nf_multiple_values[LUT_FS][3] = LUT_NO_REC; 00456 00457 nf_multiple_values[LUT_RS][0] = 6; 00458 nf_multiple_values[LUT_RS][1] = 7; 00459 nf_multiple_values[LUT_RS][2] = LUT_NO_REC; 00460 nf_multiple_values[LUT_RS][3] = LUT_NO_REC; 00461 00462 nf_multiple_values[LUT_FE][0] = 0; 00463 nf_multiple_values[LUT_FE][1] = 2; 00464 nf_multiple_values[LUT_FE][2] = LUT_NO_REC; 00465 nf_multiple_values[LUT_FE][3] = LUT_NO_REC; 00466 00467 nf_multiple_values[LUT_FW][0] = 1; 00468 nf_multiple_values[LUT_FW][1] = 3; 00469 nf_multiple_values[LUT_FW][2] = LUT_NO_REC; 00470 nf_multiple_values[LUT_FW][3] = LUT_NO_REC; 00471 00472 nf_multiple_values[LUT_RE][0] = 4; 00473 nf_multiple_values[LUT_RE][1] = 6; 00474 nf_multiple_values[LUT_RE][2] = LUT_NO_REC; 00475 nf_multiple_values[LUT_RE][3] = LUT_NO_REC; 00476 00477 nf_multiple_values[LUT_RW][0] = 5; 00478 nf_multiple_values[LUT_RW][1] = 7; 00479 nf_multiple_values[LUT_RW][2] = LUT_NO_REC; 00480 nf_multiple_values[LUT_RW][3] = LUT_NO_REC; 00481 00482 nf_multiple_values[LUT_FNE][0] = 0; 00483 nf_multiple_values[LUT_FNE][1] = LUT_NO_REC; 00484 nf_multiple_values[LUT_FNE][2] = LUT_NO_REC; 00485 nf_multiple_values[LUT_FNE][3] = LUT_NO_REC; 00486 00487 nf_multiple_values[LUT_FNW][0] = 1; 00488 nf_multiple_values[LUT_FNW][1] = LUT_NO_REC; 00489 nf_multiple_values[LUT_FNW][2] = LUT_NO_REC; 00490 nf_multiple_values[LUT_FNW][3] = LUT_NO_REC; 00491 00492 nf_multiple_values[LUT_FSE][0] = 2; 00493 nf_multiple_values[LUT_FSE][1] = LUT_NO_REC; 00494 nf_multiple_values[LUT_FSE][2] = LUT_NO_REC; 00495 nf_multiple_values[LUT_FSE][3] = LUT_NO_REC; 00496 00497 nf_multiple_values[LUT_FSW][0] = 3; 00498 nf_multiple_values[LUT_FSW][1] = LUT_NO_REC; 00499 nf_multiple_values[LUT_FSW][2] = LUT_NO_REC; 00500 nf_multiple_values[LUT_FSW][3] = LUT_NO_REC; 00501 00502 nf_multiple_values[LUT_RNE][0] = 4; 00503 nf_multiple_values[LUT_RNE][1] = LUT_NO_REC; 00504 nf_multiple_values[LUT_RNE][2] = LUT_NO_REC; 00505 nf_multiple_values[LUT_RNE][3] = LUT_NO_REC; 00506 00507 nf_multiple_values[LUT_RNW][0] = 5; 00508 nf_multiple_values[LUT_RNW][1] = LUT_NO_REC; 00509 nf_multiple_values[LUT_RNW][2] = LUT_NO_REC; 00510 nf_multiple_values[LUT_RNW][3] = LUT_NO_REC; 00511 00512 nf_multiple_values[LUT_RSE][0] = 6; 00513 nf_multiple_values[LUT_RSE][1] = LUT_NO_REC; 00514 nf_multiple_values[LUT_RSE][2] = LUT_NO_REC; 00515 nf_multiple_values[LUT_RSE][3] = LUT_NO_REC; 00516 00517 nf_multiple_values[LUT_RSW][0] = 7; 00518 nf_multiple_values[LUT_RSW][1] = LUT_NO_REC; 00519 nf_multiple_values[LUT_RSW][2] = LUT_NO_REC; 00520 nf_multiple_values[LUT_RSW][3] = LUT_NO_REC; 00521 00522 }; 00523 00524 00525 unsigned int OcTreeLUT::genPos(const OcTreeKey& key, const int& i) const { 00526 unsigned int retval = 0; 00527 if (key.k[0] & (1 << i)) retval += 1; 00528 if (key.k[1] & (1 << i)) retval += 2; 00529 if (key.k[2] & (1 << i)) retval += 4; 00530 return retval; 00531 } 00532 00533 00534 /* 00535 * used internally to generate neighbor key from a given key 00536 */ 00537 void OcTreeLUT::changeKey(const int& val, OcTreeKey& key, const unsigned short int& i) const { 00538 switch (val) { 00539 case 0: 00540 key.k[0] &= ~(1 << i); 00541 key.k[1] &= ~(1 << i); 00542 key.k[2] &= ~(1 << i); 00543 break; 00544 case 1: 00545 key.k[0] |= (1 << i); 00546 key.k[1] &= ~(1 << i); 00547 key.k[2] &= ~(1 << i); 00548 break; 00549 case 2: 00550 key.k[0] &= ~(1 << i); 00551 key.k[1] |= (1 << i); 00552 key.k[2] &= ~(1 << i); 00553 break; 00554 case 3: 00555 key.k[0] |= (1 << i); 00556 key.k[1] |= (1 << i); 00557 key.k[2] &= ~(1 << i); 00558 break; 00559 case 4: 00560 key.k[0] &= ~(1 << i); 00561 key.k[1] &= ~(1 << i); 00562 key.k[2] |= (1 << i); 00563 break; 00564 case 5: 00565 key.k[0] |= (1 << i); 00566 key.k[1] &= ~(1 << i); 00567 key.k[2] |= (1 << i); 00568 break; 00569 case 6: 00570 key.k[0] &= ~(1 << i); 00571 key.k[1] |= (1 << i); 00572 key.k[2] |= (1 << i); 00573 break; 00574 case 7: 00575 key.k[0] |= (1 << i); 00576 key.k[1] |= (1 << i); 00577 key.k[2] |= (1 << i); 00578 break; 00579 } 00580 } 00581 00582 00583 bool OcTreeLUT::genNeighborKey(const OcTreeKey& node_key, const signed char& dir, 00584 OcTreeKey& neighbor_key) const { 00585 00586 neighbor_key.k[0] = node_key.k[0]; 00587 neighbor_key.k[1] = node_key.k[1]; 00588 neighbor_key.k[2] = node_key.k[2]; 00589 00590 unsigned int depth = 0; 00591 signed char curDir = dir; 00592 00593 signed char pos; 00594 while (depth < max_depth) { 00595 pos = static_cast<signed char>(genPos(neighbor_key, depth)); 00596 changeKey(nf_values[pos][curDir], neighbor_key, depth); 00597 00598 if (nf_rec_values[pos][curDir] != LUT_NO_REC) { // recurs 00599 curDir -= nf_rec_values[pos][curDir]; 00600 depth++; 00601 } 00602 else { 00603 return true; 00604 } 00605 } 00606 00607 return false; 00608 }; 00609 00610 00611 00612 } // namespace 00613