Files
Point-LIO_ROS2/include/ivox/hilbert.hpp
2024-01-18 08:54:06 -05:00

831 lines
28 KiB
C++

// 2021-09-18, https://github.com/spectral3d/hilbert_hpp is under MIT license.
//Copyright (c) 2019 David Beynon
//
//Permission is hereby granted, free of charge, to any person obtaining a copy
//of this software and associated documentation files (the "Software"), to deal
//in the Software without restriction, including without limitation the rights
//to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//copies of the Software, and to permit persons to whom the Software is
//furnished to do so, subject to the following conditions:
//
//The above copyright notice and this permission notice shall be included in all
//copies or substantial portions of the Software.
//
//THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
//SOFTWARE.
#ifndef INCLUDED_HILBERT_HPP
#define INCLUDED_HILBERT_HPP
#pragma once
#include <algorithm>
#include <array>
#include <cstdint>
#include <limits>
#include <type_traits>
// Version 1.1 - 29 August 2019
//
// N dimensional hilbert curve encoding & decoding based on the paper
// "Programming the Hilbert Curve" by John Skilling.
//
// The interface assumes an std::array of some unsigned integer type. This
// contains an index in lexographic order, or a set of coordinates on the
// hilbert curve.
//
// Two implementations are included.
//
// hilbert::v1 contains a fairly straightforward implementation of the paper,
// with standard looping constructs.
//
// hilbert::v2 performs uses template metaprogramming to unroll loops and
// theoretically improve performance on some systems.
//
// v1 produces smaller code, which may be more performant on machines with
// small caches. v2 produces relatively large code that seems more efficient
// on modern systems for dimension up to about 100.
//
// v2 should be built with -O3 for best results. -O0 is extremely slow
// on most systems.
//
// Interface is as follows:
//
// Find the position of a point on an N dimensional Hilbert Curve.
//
// Based on the paper "Programming the Hilbert Curve" by John Skilling.
//
// Index is encoded with most significant objects first. Lexographic
// sort order.
//template<typename T, size_t N>
//std::array<T, N>
//IndexToPosition(std::array<T, N> const &in);
// Find the index of a point on an N dimensional Hilbert Curve.
//
// Based on the paper "Programming the Hilbert Curve" by John Skilling.
//
// Index is encoded with most significant objects first. Lexographic
// sort order.
//template<typename T, size_t N>
//std::array<T, N>
//PositionToIndex(std::array<T, N> const &in);
//
namespace hilbert
{
// Fairly straightforward implementation. Loops are loops and code mostly
// does what one would expect.
namespace v1
{
namespace internal
{
// Extract bits from transposed form.
//
// e.g.
//
// a d g j a b c d
// b e h k -> e f g h
// c f i l i j k l
//
template<typename T, size_t N>
std::array<T, N>
UntransposeBits(std::array<T, N> const &in)
{
const size_t bits = std::numeric_limits<T>::digits;
const T high_bit(T(1) << (bits - 1));
const size_t bit_count(bits * N);
std::array<T, N> out;
std::fill(out.begin(), out.end(), 0);
// go through all bits in input, msb first. Shift distances are
// from msb.
for(size_t b=0;b<bit_count;b++)
{
size_t src_bit, dst_bit, src, dst;
src = b % N;
dst = b / bits;
src_bit = b / N;
dst_bit = b % bits;
out[dst] |= (((in[src] << src_bit) & high_bit) >> dst_bit);
}
return out;
}
// Pack bits into transposed form.
//
// e.g.
//
// a b c d a d g j
// e f g h -> b e h k
// i j k l c f i l
//
template<typename T, size_t N>
std::array<T, N>
TransposeBits(std::array<T, N> const &in)
{
const size_t bits = std::numeric_limits<T>::digits;
const T high_bit(T(1) << (bits - 1));
const size_t bit_count(bits * N);
std::array<T, N> out;
std::fill(out.begin(), out.end(), 0);
// go through all bits in input, msb first. Shift distances
// are from msb.
for(size_t b=0;b<bit_count;b++)
{
size_t src_bit, dst_bit, src, dst;
src = b / bits;
dst = b % N;
src_bit = b % bits;
dst_bit = b / N;
out[dst] |= (((in[src] << src_bit) & high_bit) >> dst_bit);
}
return out;
}
}
//
// Public interfaces.
//
// Find the position of a point on an N dimensional Hilbert Curve.
//
// Based on the paper "Programming the Hilbert Curve" by John Skilling.
//
// Index is encoded with most significant objects first. Lexographic
// sort order.
template<typename T, size_t N>
std::array<T, N>
IndexToPosition(
std::array<T, N> const &in)
{
// First convert index to transpose.
std::array<T, N> out(internal::TransposeBits(in));
// Initial gray encoding of transposed vector.
{
T tmp = out[N-1] >> 1;
for(size_t n=N-1;n;n--)
{
out[n]^= out[n-1];
}
out[0]^= tmp;
}
// Apply transforms to gray code.
{
T cur_bit(2),
low_bits;
while(cur_bit)
{
low_bits = cur_bit - 1;
size_t n(N);
do
{
n--;
if(out[n] & cur_bit)
{
// flip low bits of X
out[0]^= low_bits;
}
else
{
// swap low bits with X
T t((out[n] ^ out[0]) & low_bits);
out[n]^= t;
out[0]^= t;
}
}
while(n);
cur_bit<<= 1;
}
}
return out;
}
// Find the index of a point on an N dimensional Hilbert Curve.
//
// Based on the paper "Programming the Hilbert Curve" by John Skilling.
//
// Index is encoded with most significant objects first. Lexographic
// sort order.
template<typename T, size_t N>
std::array<T, N>
PositionToIndex(std::array<T, N> const &in)
{
const size_t bits = std::numeric_limits<T>::digits;
std::array<T, N> out(in);
// reverse transforms to convert into transposed gray code.
{
T cur_bit(T(1) << (bits - 1)),
low_bits;
do
{
low_bits = cur_bit - 1;
for(size_t n=0;n<N;n++)
{
if(out[n] & cur_bit)
{
// flip low bits of X
out[0]^= low_bits;
}
else
{
// swap low bits with X
T t((out[n] ^ out[0]) & low_bits);
out[n]^= t;
out[0]^= t;
}
}
cur_bit>>= 1;
} while(low_bits > 1);
}
// Remove gray code from transposed vector.
{
T cur_bit(T(1) << (bits - 1)),
t(0);
for(size_t n=1;n<N;n++)
{
out[n]^= out[n-1];
}
do
{
if(out[N-1] & cur_bit)
{
t^= (cur_bit - 1);
}
cur_bit>>= 1;
} while(cur_bit > 1);
for(auto &v : out)
{
v^= t;
}
}
return internal::UntransposeBits(out);
}
} // namespace v1
// Implementation using metaprogramming to unroll most loops.
// Optimised performance should be superior to v1 provided all code remains
// in cache etc.
//
// At some value of N v1 should overtake v2.
namespace v2
{
namespace internal
{
// Metaprogramming guts. Unrolled loops, abandon all hope etc.
namespace tmp
{
template<typename T, size_t N, size_t D>
T
TransposeBits2(
std::array<T, N> const &,
std::integral_constant<size_t, D>,
std::integral_constant<size_t, 0>)
{
return T(0);
}
template<typename T, size_t N, size_t D, size_t B>
T
TransposeBits2(
std::array<T, N> const &in,
std::integral_constant<size_t, D>,
std::integral_constant<size_t, B>)
{
const size_t size = std::numeric_limits<T>::digits;
const size_t src = ((D + ((size - B) * N)) / size);
const size_t src_bit =
size - (1+((D + ((size - B) * N)) % size));
const T src_bit_val = T(1) << src_bit;
const size_t dst_bit = (B - 1);
const T dst_bit_val = T(1) << dst_bit;
// Multiply rather than shift to avoid clang implicit
// conversion warning.
T bit = ((in[src] & src_bit_val) >> src_bit) * dst_bit_val;
return bit + TransposeBits2(
in,
std::integral_constant<size_t, D>(),
std::integral_constant<size_t, B-1>());
}
template<typename T, size_t N>
void
TransposeBits(
std::array<T, N> const &,
std::array<T, N> &,
std::integral_constant<size_t, 0>)
{
}
template<typename T, size_t N, size_t D>
void
TransposeBits(
std::array<T, N> const &in,
std::array<T, N> &out,
std::integral_constant<size_t, D>)
{
out[D-1] = TransposeBits2(
in,
std::integral_constant<size_t, D-1>(),
std::integral_constant<
size_t,
std::numeric_limits<T>::digits>());
TransposeBits(
in,
out,
std::integral_constant<size_t, D-1>());
}
template<typename T, size_t N, size_t D>
T
UntransposeBits2(
std::array<T, N> const &,
std::integral_constant<size_t, D>,
std::integral_constant<size_t, 0>)
{
return T(0);
}
template<typename T, size_t N, size_t D, size_t B>
T
UntransposeBits2(
std::array<T, N> const &in,
std::integral_constant<size_t, D>,
std::integral_constant<size_t, B>)
{
const size_t size = std::numeric_limits<T>::digits;
const size_t src = ((D * size) + (size - B)) % N;
const size_t src_bit =
size - (((((D * size) + (size - B))) / N) + 1);
const T src_bit_val = T(1) << src_bit;
const size_t dst_bit(B - 1);
const T dst_bit_val = T(1) << dst_bit;
// Multiply rather than shift to avoid clang implicit
// conversion warning.
T bit = ((in[src] & src_bit_val) >> src_bit) * dst_bit_val;
return bit + UntransposeBits2(
in,
std::integral_constant<size_t, D>(),
std::integral_constant<size_t, size_t(B-1)>());
}
template<typename T, size_t N>
void
UntransposeBits(
std::array<T, N> const &,
std::array<T, N> &,
std::integral_constant<size_t, 0>)
{
}
template<typename T, size_t N, size_t D>
void
UntransposeBits(
std::array<T, N> const &in,
std::array<T, N> &out,
std::integral_constant<size_t, D>)
{
out[D-1] = UntransposeBits2(
in,
std::integral_constant<size_t, D-1>(),
std::integral_constant<
size_t,
std::numeric_limits<T>::digits>());
UntransposeBits(
in,
out,
std::integral_constant<size_t, D-1>());
}
template<typename T, size_t N>
void
ApplyGrayCode1(
std::array<T, N> const &in,
std::array<T, N> &out,
std::integral_constant<size_t, 0>)
{
out[0]^= in[N-1] >> 1;
}
template<typename T, size_t N, size_t I>
void
ApplyGrayCode1(
std::array<T, N> const &in,
std::array<T, N> &out,
std::integral_constant<size_t, I>)
{
out[I]^= out[I-1];
ApplyGrayCode1(
in,
out,
std::integral_constant<size_t, I-1>());
}
// Remove a gray code from a transposed vector
template<typename T, size_t N>
void
RemoveGrayCode1(
std::array<T, N> &,
std::integral_constant<size_t, 0>)
{
}
// xor array values with previous values.
template<typename T, size_t N, size_t D>
void
RemoveGrayCode1(
std::array<T, N> &in,
std::integral_constant<size_t, D>)
{
const size_t src_idx = N - (D + 1);
const size_t dst_idx = N - D;
in[dst_idx]^= in[src_idx];
RemoveGrayCode1(in, std::integral_constant<size_t, D-1>());
}
template<typename T>
T RemoveGrayCode2(T, std::integral_constant<size_t, 1>)
{
return T(0);
}
template<typename T, size_t B>
T RemoveGrayCode2(T v, std::integral_constant<size_t, B>)
{
const T cur_bit(T(1) << (B-1));
const T low_bits(cur_bit - 1);
if(v & cur_bit)
{
return low_bits ^ RemoveGrayCode2(
v,
std::integral_constant<size_t, B-1>());
}
else
{
return RemoveGrayCode2(
v,
std::integral_constant<size_t, B-1>());
}
}
template<typename T, size_t N, size_t B>
void
GrayToHilbert2(
std::array<T, N> &,
std::integral_constant<size_t, B>,
std::integral_constant<size_t, 0>)
{
}
template<typename T, size_t N, size_t B, size_t I>
void
GrayToHilbert2(
std::array<T, N> &out,
std::integral_constant<size_t, B>,
std::integral_constant<size_t, I>)
{
const size_t n(I-1);
const T cur_bit(T(1) << (std::numeric_limits<T>::digits - B));
const T low_bits(cur_bit - 1);
if(out[n] & cur_bit)
{
// flip low bits of X
out[0]^= low_bits;
}
else
{
// swap low bits with X
T t((out[n] ^ out[0]) & low_bits);
out[n]^= t;
out[0]^= t;
}
GrayToHilbert2(
out,
std::integral_constant<size_t, B>(),
std::integral_constant<size_t, I-1>());
}
template<typename T, size_t N>
void
GrayToHilbert(
std::array<T, N> &,
std::integral_constant<size_t, 0>)
{
}
template<typename T, size_t N, size_t B>
void
GrayToHilbert(
std::array<T, N> &out,
std::integral_constant<size_t, B>)
{
GrayToHilbert2(
out,
std::integral_constant<size_t, B>(),
std::integral_constant<size_t, N>());
GrayToHilbert(out, std::integral_constant<size_t, B-1>());
}
template<typename T, size_t N, size_t B>
void
HilbertToGray2(
std::array<T, N> &,
std::integral_constant<size_t, B>,
std::integral_constant<size_t, 0>)
{
}
template<typename T, size_t N, size_t B, size_t I>
void
HilbertToGray2(
std::array<T, N> &out,
std::integral_constant<size_t, B>,
std::integral_constant<size_t, I>)
{
const size_t cur_bit(T(1) << B);
const size_t low_bits(cur_bit-1);
const size_t n(N-I);
if(out[n] & cur_bit)
{
// flip low bits of X
out[0]^= low_bits;
}
else
{
// swap low bits with X
T t((out[n] ^ out[0]) & low_bits);
out[n]^= t;
out[0]^= t;
}
HilbertToGray2(
out,
std::integral_constant<size_t, B>(),
std::integral_constant<size_t, I-1>());
}
template<typename T, size_t N>
void
HilbertToGray(
std::array<T, N> &,
std::integral_constant<size_t, 0>)
{
}
template<typename T, size_t N, size_t B>
void
HilbertToGray(
std::array<T, N> &out,
std::integral_constant<size_t, B>)
{
HilbertToGray2(
out,
std::integral_constant<size_t, B>(),
std::integral_constant<size_t, N>());
HilbertToGray(out, std::integral_constant<size_t, B-1>());
}
template<typename T, size_t N>
void
ApplyMaskToArray(
std::array<T, N> &,
T,
std::integral_constant<size_t, 0>)
{
}
template<typename T, size_t N, size_t I>
void
ApplyMaskToArray(
std::array<T, N> &a,
T mask, std::integral_constant<size_t, I>)
{
a[I-1]^= mask;
ApplyMaskToArray(
a,
mask,
std::integral_constant<size_t, I-1>());
}
} // namespace tmp
// Pack bits into transposed form.
//
// e.g.
//
// a b c d a d g j
// e f g h -> b e h k
// i j k l c f i l
//
template<typename T, size_t N>
std::array<T, N>
TransposeBits(std::array<T, N> const &in)
{
std::array<T, N> out;
std::fill(out.begin(), out.end(), 0);
tmp::TransposeBits(
in,
out,
std::integral_constant<size_t, N>());
return out;
}
// Extract bits from transposed form.
// e.g.
//
// a d g j a b c d
// b e h k -> e f g h
// c f i l i j k l
//
template<typename T, size_t N>
std::array<T, N>
UntransposeBits(std::array<T, N> const &in)
{
std::array<T, N> out;
std::fill(out.begin(), out.end(), 0);
tmp::UntransposeBits(
in,
out,
std::integral_constant<size_t, N>());
return out;
}
// Apply a gray code to a transformed vector.
template<typename T, size_t N>
std::array<T, N>
ApplyGrayCode(std::array<T, N> const &in)
{
std::array<T, N> out(in);
tmp::ApplyGrayCode1(
in,
out,
std::integral_constant<size_t, N-1>());
return out;
}
template<typename T, size_t N>
std::array<T, N>
RemoveGrayCode(std::array<T, N> const &in)
{
const size_t bits = std::numeric_limits<T>::digits;
std::array<T, N> out(in);
// Remove gray code from transposed vector.
{
// xor values with prev values.
tmp::RemoveGrayCode1(
out,
std::integral_constant<size_t, N-1>());
// create a mask.
T t = tmp::RemoveGrayCode2(
out[N-1],
std::integral_constant<size_t, bits>());
// Apply mask to output.
tmp::ApplyMaskToArray(
out,
t,
std::integral_constant<size_t, N>());
}
return out;
}
// Generate code to convert from a transposed gray code to a hilbert
// code.
template<typename T, size_t N>
std::array<T, N>
GrayToHilbert(std::array<T, N> const &in)
{
std::array<T, N> out(in);
tmp::GrayToHilbert(
out,
std::integral_constant<
size_t,
std::numeric_limits<T>::digits - 1>());
return out;
}
// Generate code to convert from a hilbert code to a transposed gray
// code.
template<typename T, size_t N>
std::array<T, N>
HilbertToGray(std::array<T, N> const &in)
{
std::array<T, N> out(in);
tmp::HilbertToGray(
out,
std::integral_constant<
size_t,
std::numeric_limits<T>::digits-1>());
return out;
}
}
//
// Public interfaces.
//
// Find the position of a point on an N dimensional Hilbert Curve.
//
// Based on the paper "Programming the Hilbert Curve" by John Skilling.
//
// Index is encoded with most significant objects first. Lexographic
// sort order.
template<typename T, size_t N>
std::array<T, N>
IndexToPosition(std::array<T, N> const &in)
{
// First convert index to transpose.
return internal::GrayToHilbert(
internal::ApplyGrayCode(
internal::TransposeBits(in)));
}
// Find the index of a point on an N dimensional Hilbert Curve.
//
// Based on the paper "Programming the Hilbert Curve" by John Skilling.
//
// Index is encoded with most significant objects first. Lexographic
// sort order.
template<typename T, size_t N>
std::array<T, N>
PositionToIndex(std::array<T, N> const &in)
{
return internal::UntransposeBits(
internal::RemoveGrayCode(
internal::HilbertToGray(in)));
}
} // namespace v2
} // namespace hilbert
#endif