OcTreeLUT.cpp
Go to the documentation of this file.
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 


octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Thu Jun 6 2019 17:31:45