Program Listing for File OcTreeKey.h
↰ Return to documentation for file (/tmp/ws/src/octomap/octomap/include/octomap/OcTreeKey.h)
/*
* OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees
* https://octomap.github.io/
*
* Copyright (c) 2009-2013, K.M. Wurm and A. Hornung, University of Freiburg
* All rights reserved.
* License: New BSD
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the University of Freiburg nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef OCTOMAP_OCTREE_KEY_H
#define OCTOMAP_OCTREE_KEY_H
/* According to c++ standard including this header has no practical effect
* but it can be used to determine the c++ standard library implementation.
*/
#include <ciso646>
#include <assert.h>
/* Libc++ does not implement the TR1 namespace, all c++11 related functionality
* is instead implemented in the std namespace.
*/
#if defined(__GNUC__) && ! defined(_LIBCPP_VERSION)
#include <tr1/unordered_set>
#include <tr1/unordered_map>
namespace octomap {
namespace unordered_ns = std::tr1;
}
#else
#include <unordered_set>
#include <unordered_map>
namespace octomap {
namespace unordered_ns = std;
}
#endif
namespace octomap {
typedef uint16_t key_type;
class OcTreeKey {
public:
OcTreeKey () {}
OcTreeKey (key_type a, key_type b, key_type c){
k[0] = a;
k[1] = b;
k[2] = c;
}
OcTreeKey(const OcTreeKey& other){
k[0] = other.k[0];
k[1] = other.k[1];
k[2] = other.k[2];
}
bool operator== (const OcTreeKey &other) const {
return ((k[0] == other[0]) && (k[1] == other[1]) && (k[2] == other[2]));
}
bool operator!= (const OcTreeKey& other) const {
return( (k[0] != other[0]) || (k[1] != other[1]) || (k[2] != other[2]) );
}
OcTreeKey& operator=(const OcTreeKey& other){
k[0] = other.k[0]; k[1] = other.k[1]; k[2] = other.k[2];
return *this;
}
const key_type& operator[] (unsigned int i) const {
return k[i];
}
key_type& operator[] (unsigned int i) {
return k[i];
}
key_type k[3];
struct KeyHash{
size_t operator()(const OcTreeKey& key) const{
// a simple hashing function
// explicit casts to size_t to operate on the complete range
// constanst will be promoted according to C++ standard
return static_cast<size_t>(key.k[0])
+ 1447*static_cast<size_t>(key.k[1])
+ 345637*static_cast<size_t>(key.k[2]);
}
};
};
typedef unordered_ns::unordered_set<OcTreeKey, OcTreeKey::KeyHash> KeySet;
typedef unordered_ns::unordered_map<OcTreeKey, bool, OcTreeKey::KeyHash> KeyBoolMap;
class KeyRay {
public:
KeyRay () {
ray.resize(maxSize);
reset();
}
KeyRay(const KeyRay& other){
ray = other.ray;
size_t dSize = other.end() - other.begin();
end_of_ray = ray.begin() + dSize;
}
void reset() {
end_of_ray = begin();
}
void addKey(const OcTreeKey& k) {
assert(end_of_ray != ray.end());
*end_of_ray = k;
++end_of_ray;
}
size_t size() const { return end_of_ray - ray.begin(); }
size_t sizeMax() const { return maxSize; }
typedef std::vector<OcTreeKey>::iterator iterator;
typedef std::vector<OcTreeKey>::const_iterator const_iterator;
typedef std::vector<OcTreeKey>::reverse_iterator reverse_iterator;
iterator begin() { return ray.begin(); }
iterator end() { return end_of_ray; }
const_iterator begin() const { return ray.begin(); }
const_iterator end() const { return end_of_ray; }
reverse_iterator rbegin() { return (reverse_iterator) end_of_ray; }
reverse_iterator rend() { return ray.rend(); }
private:
std::vector<OcTreeKey> ray;
std::vector<OcTreeKey>::iterator end_of_ray;
const static size_t maxSize = 100000;
};
inline void computeChildKey (unsigned int pos, key_type center_offset_key,
const OcTreeKey& parent_key, OcTreeKey& child_key) {
// x-axis
if (pos & 1) child_key[0] = parent_key[0] + center_offset_key;
else child_key[0] = parent_key[0] - center_offset_key - (center_offset_key ? 0 : 1);
// y-axis
if (pos & 2) child_key[1] = parent_key[1] + center_offset_key;
else child_key[1] = parent_key[1] - center_offset_key - (center_offset_key ? 0 : 1);
// z-axis
if (pos & 4) child_key[2] = parent_key[2] + center_offset_key;
else child_key[2] = parent_key[2] - center_offset_key - (center_offset_key ? 0 : 1);
}
inline uint8_t computeChildIdx(const OcTreeKey& key, int depth){
uint8_t pos = 0;
if (key.k[0] & (1 << depth))
pos += 1;
if (key.k[1] & (1 << depth))
pos += 2;
if (key.k[2] & (1 << depth))
pos += 4;
return pos;
}
inline OcTreeKey computeIndexKey(key_type level, const OcTreeKey& key) {
if (level == 0)
return key;
else {
key_type mask = 65535 << level;
OcTreeKey result = key;
result[0] &= mask;
result[1] &= mask;
result[2] &= mask;
return result;
}
}
} // namespace
#endif