key_util.hpp

Go to the documentation of this file.

/*
 * SPDX-FileCopyrightText: 2016-2019 Emanuele Vespa
 * SPDX-FileCopyrightText: 2021-2023 Smart Robotics Lab, Imperial College London, Technical University of Munich
 * SPDX-FileCopyrightText: 2021-2023 Nils Funk
 * SPDX-FileCopyrightText: 2021-2023 Sotiris Papatheodorou
 * SPDX-License-Identifier: BSD-3-Clause
 */

#ifndef SE_KEY_UTIL_HPP
#define SE_KEY_UTIL_HPP

#include <algorithm>

#include "se/map/utils/setup_util.hpp"
#include "se/map/utils/type_util.hpp"



namespace se {

#define NUM_DIM 3
#define SCALE_OFFSET 5
#define KEY_SCALE_LIMIT 19

constexpr uint64_t CODE_MASK[] = { 
    0x1ffffffffffffff,
    0x1fffffffffffff8,
    0x1ffffffffffffc0,
    0x1fffffffffffe00,
    0x1fffffffffff000,
    0x1ffffffffff8000,
    0x1fffffffffc0000,
    0x1ffffffffe00000,
    0x1ffffffff000000,
    0x1fffffff8000000,
    0x1ffffffc0000000,
    0x1fffffe00000000,
    0x1fffff000000000,
    0x1ffff8000000000,
    0x1fffc0000000000,
    0x1ffe00000000000,
    0x1ff000000000000,
    0x1f8000000000000,
    0x1c0000000000000};

constexpr uint64_t SCALE_MASK = 0x1F; 

namespace keyops {

inline bool is_valid(const se::key_t key, const se::scale_t limit = KEY_SCALE_LIMIT);

inline bool is_valid(const Eigen::Vector3i& coord);

inline se::code_t expand(unsigned long long value);

inline se::key_t compact(uint64_t value);

inline bool encode_key(const Eigen::Vector3i& coord, const se::scale_t scale, se::key_t& key);

inline se::key_t encode_key(const Eigen::Vector3i& coord, const se::scale_t scale);

inline bool encode_key(const se::key_t code, const se::scale_t scale, se::key_t& key);

inline se::key_t encode_key(const se::key_t code, const se::scale_t scale);

inline void decode_key(const se::key_t key, Eigen::Vector3i& coord, scale_t& scale);

inline void encode_code(const Eigen::Vector3i& coord, se::code_t& code);

inline se::code_t encode_code(const Eigen::Vector3i& coord);

inline void decode_code(const se::code_t code, Eigen::Vector3i& coord);



inline idx_t code_to_child_idx(const se::code_t code, const scale_t scale);

inline se::code_t key_to_code(const se::key_t key);

inline Eigen::Vector3i key_to_coord(const se::key_t key);

inline scale_t key_to_scale(const se::key_t key);

inline bool key_at_scale(const se::key_t key, const se::scale_t scale, se::key_t& key_at_scale);

inline bool code_at_scale(const se::key_t key, const se::scale_t scale, se::code_t& code_at_scale);

inline void parent_key(const se::key_t key, se::key_t& parent_key);

inline se::key_t block_key(const se::key_t key, const se::scale_t max_block_scale);

inline se::code_t block_code(const se::key_t key, const se::scale_t max_block_scale);

inline void parent_to_child_key(const se::key_t parent_key,
                                const se::code_t code_at_scale,
                                se::key_t& child_key);

inline bool is_child(const se::key_t parent_key, const se::key_t child_key);

inline bool is_siblings(const se::key_t sibling_1_key, const se::key_t sibling_2_key);

template<Sort = Sort::SmallToLarge>
inline void sort_keys(std::vector<se::key_t>& keys);

template<>
inline void sort_keys<Sort::SmallToLarge>(std::vector<se::key_t>& keys);

template<>
inline void sort_keys<Sort::LargeToSmall>(std::vector<se::key_t>& keys);

template<se::Safe SafeB>
inline void unique_keys(const std::vector<se::key_t>& keys, std::vector<se::key_t>& unique_keys);

template<se::Safe SafeB>
inline void unique_codes(const std::vector<se::key_t>& keys, std::vector<se::key_t>& unique_keys);

template<se::Safe SafeB>
inline void unique_allocation(const std::vector<se::key_t>& keys,
                              const scale_t max_block_scale,
                              std::vector<se::key_t>& unique_keys);

template<se::Safe SafeB>
inline void unique_at_scale(const std::vector<se::key_t>& keys,
                            const se::scale_t scale,
                            std::vector<se::key_t>& unique_keys);

inline void face_neighbours(const se::key_t key, std::array<se::key_t, 6> face_neighbour_keys);

inline void neighbours(const se::key_t key, std::array<se::key_t, 26> neighbour_keys);

inline void siblings(const se::key_t key, std::array<se::key_t, 8> sibling_keys);

} // namespace keyops
} // namespace se

#include "impl/key_util_impl.hpp"

#endif // SE_KEY_UTIL_HPP