Skip to content

Instantly share code, notes, and snippets.

@PolarNick239

PolarNick239/morton_code.h

Last active March 20, 2023 16:38
Star
Embed
What would you like to do?
Learn more about clone URLs
Download ZIP
96-bit 3D Morton code (Z-curve)
Raw
morton_code.h
#pragma once
#include <limits>
#include <cassert>
#include <stdexcept>
#define MORTON_CODE_MAX_LEVEL 32
#define UINT8 unsigned char
#define UINT32 unsigned int
#define UINT64 unsigned long long
#define UINT32_NBITS (sizeof(UINT32) * 8)
#define MORTON_CODE_UINT32_N ((3 * MORTON_CODE_MAX_LEVEL + UINT32_NBITS - 1) / UINT32_NBITS)
#pragma pack(push, 1)
class MortonCode {
protected:
static UINT32 expandBits(UINT32 v)
{
assert(v < ((UINT32) 1 << 11));
// _______32 bits_______
// |000.............vvv| - 11-v significant bits
//
// convert to:
//
// _______32 bits_______
// |0v00v........00v00v|
// See https://stackoverflow.com/a/1024889
v = (v | (v << 16)) & 0x070000FF; // 0000 0111 0000 0000 0000 0000 1111 1111
v = (v | (v << 8)) & 0x0700F00F; // 0000 0111 0000 0000 1111 0000 0000 1111
v = (v | (v << 4)) & 0x430C30C3; // 0100 0011 0000 1100 0011 0000 1100 0011
v = (v | (v << 2)) & 0x49249249; // 0100 1001 0010 0100 1001 0010 0100 1001
assert(v < ((UINT32) 1 << 31));
return v;
}
static UINT64 expandBits(UINT64 v)
{
assert(v < ((UINT64) 1 << 22));
// _______32 bits_____________32 bits_______
// |000.............000|000.............vvv| - 22-v significant bits
//
// convert to:
//
// _______32 bits_____________32 bits_______
// |v00............00v0|0v...........00v00v|
v = (v | (v << 16)) & 0x0000003F0000FFFFull; // 0000 0000 0000 0000 0000 0000 0011 1111 0000 0000 0000 0000 1111 1111 1111 1111
v = (v | (v << 16)) & 0x003F0000FF0000FFull; // 0000 0000 0011 1111 0000 0000 0000 0000 1111 1111 0000 0000 0000 0000 1111 1111
v = (v | (v << 8)) & 0x300F00F00F00F00Full; // 0011 0000 0000 1111 0000 0000 1111 0000 0000 1111 0000 0000 1111 0000 0000 1111
v = (v | (v << 4)) & 0x30C30C30C30C30C3ull; // 0011 0000 1100 0011 0000 1100 0011 0000 1100 0011 0000 1100 0011 0000 1100 0011
v = (v | (v << 2)) & 0x9249249249249249ull; // 1001 0010 0100 1001 0010 0100 1001 0010 0100 1001 0010 0100 1001 0010 0100 1001
return v;
}
static UINT32 shrinkBits(UINT32 v)
{
assert((v & (~0x49249249)) == 0);
// _______32 bits_______
// |0v00v........00v00v|
//
// convert to:
//
// _______32 bits_______
// |000.............vvv| - 11-v significant bits
v = v & 0x49249249; // 0100 1001 0010 0100 1001 0010 0100 1001
v = (v | (v >> 2)) & 0x430C30C3; // 0100 0011 0000 1100 0011 0000 1100 0011
v = (v | (v >> 4)) & 0x0700F00F; // 0000 0111 0000 0000 1111 0000 0000 1111
v = (v | (v >> 8)) & 0x070000FF; // 0000 0111 0000 0000 0000 0000 1111 1111
v = (v | (v >> 16)) & 0x000007FF; // 0000 0000 0000 0000 0000 0111 1111 1111
assert(v < ((UINT32) 1 << 11));
return v;
}
static UINT32 shrinkBits(UINT64 v)
{
assert((v & (~0x9249249249249249ull)) == 0);
// _______32 bits_____________32 bits_______
// |v00............00v0|0v...........00v00v|
//
// convert to:
//
// _______32 bits_____________32 bits_______
// |000.............000|000.............vvv| - 22-v significant bits
v = v & 0x9249249249249249ull; // 1001 0010 0100 1001 0010 0100 1001 0010 0100 1001 0010 0100 1001 0010 0100 1001
v = (v | (v >> 2)) & 0x30C30C30C30C30C3ull; // 0011 0000 1100 0011 0000 1100 0011 0000 1100 0011 0000 1100 0011 0000 1100 0011
v = (v | (v >> 4)) & 0x300F00F00F00F00Full; // 0011 0000 0000 1111 0000 0000 1111 0000 0000 1111 0000 0000 1111 0000 0000 1111
v = (v | (v >> 8)) & 0x003F0000FF0000FFull; // 0000 0000 0011 1111 0000 0000 0000 0000 1111 1111 0000 0000 0000 0000 1111 1111
v = (v | (v >> 16)) & 0x0000003F0000FFFFull; // 0000 0000 0000 0000 0000 0000 0011 1111 0000 0000 0000 0000 1111 1111 1111 1111
v = (v | (v >> 16)) & 0x00000000003FFFFFull; // 0000 0000 0000 0000 0000 0000 0000 0000 0000 0000 0011 1111 1111 1111 1111 1111
assert(v < ((UINT64) 1 << 22));
return (UINT32) v;
}
static void encodeKey(unsigned int x_index, unsigned int y_index, unsigned z_index, unsigned int level, UINT32 key[MORTON_CODE_UINT32_N])
{
if (level == 0) {
assert(x_index == 0 && y_index == 0 && z_index == 0);
key[0] = 0;
key[1] = 0;
key[2] = 0;
return;
}
assert(x_index < ((UINT64) 1 << level));
assert(y_index < ((UINT64) 1 << level));
assert(z_index < ((UINT64) 1 << level));
static_assert(MORTON_CODE_UINT32_N == 3, "Unexpected MortonCode key size!");
// _______32 bits_____________32 bits_____________32 bits_______
// |xyz..............xy|z.................x|yz..............xyz|
// | 11-x 11-y 10-z | 21-x 21-y 22-z |
const UINT32 LAST_21_BITS = ((1 << 21) - 1);
const UINT32 LAST_22_BITS = ((1 << 22) - 1);
const UINT32 FULL_32_BITS = 0xFFFFFFFF;
const UINT32 FRST_11_BITS = FULL_32_BITS - LAST_21_BITS;
const UINT32 FRST_10_BITS = FULL_32_BITS - LAST_22_BITS;
assert(MORTON_CODE_MAX_LEVEL - level < 32); // due to https://stackoverflow.com/a/9860578
x_index = x_index << (MORTON_CODE_MAX_LEVEL - level);
y_index = y_index << (MORTON_CODE_MAX_LEVEL - level);
z_index = z_index << (MORTON_CODE_MAX_LEVEL - level);
UINT64 x12 = expandBits((UINT64) (x_index & LAST_21_BITS));
UINT64 y12 = expandBits((UINT64) (y_index & LAST_21_BITS));
UINT64 z12 = expandBits((UINT64) (z_index & LAST_22_BITS));
UINT32 x0 = expandBits((x_index & FRST_11_BITS) >> 21);
UINT32 y0 = expandBits((y_index & FRST_11_BITS) >> 21);
UINT32 z0 = expandBits((z_index & FRST_10_BITS) >> 22);
UINT64 key12 = (x12 << 2) | (y12 << 1) | (z12 << 0);
UINT32 key0 = (z0 << 2) | (x0 << 1) | (y0 << 0);
key[2] = (UINT32) (key12 & 0x00000000FFFFFFFFull);
key[1] = (UINT32) ((key12 & 0xFFFFFFFF00000000ull) >> 32);
key[0] = key0;
}
static void decodeKey(const UINT32 key[MORTON_CODE_UINT32_N], unsigned int level, unsigned int &x_index, unsigned int &y_index, unsigned int &z_index)
{
if (level == 0) {
assert(key[0] == 0 && key[1] == 0 && key[2] == 0);
x_index = 0;
y_index = 0;
z_index = 0;
return;
}
static_assert(MORTON_CODE_UINT32_N == 3, "Unexpected MortonCode key size!");
UINT64 key12 = ((UINT64) key[1] << 32) | key[2];
UINT32 key0 = key[0];
// _______32 bits_____________32 bits_____________32 bits_______
// |xyz..............xy|z.................x|yz..............xyz|
// | 11-x 11-y 10-z | 21-x 21-y 22-z |
const UINT32 LAST_21_BITS = ((1 << 21) - 1);
const UINT32 LAST_22_BITS = ((1 << 22) - 1);
const UINT32 FULL_32_BITS = 0xFFFFFFFF;
const UINT32 FRST_11_BITS = FULL_32_BITS - LAST_21_BITS;
const UINT32 FRST_10_BITS = FULL_32_BITS - LAST_22_BITS;
UINT32 x12 = shrinkBits((key12 >> 2) & 0x9249249249249249ull); // 1001 0010 0100 1001 0010 0100 1001 0010 0100 1001 0010 0100 1001 0010 0100 1001
UINT32 y12 = shrinkBits((key12 >> 1) & 0x9249249249249249ull); // 1001 0010 0100 1001 0010 0100 1001 0010 0100 1001 0010 0100 1001 0010 0100 1001
UINT32 z12 = shrinkBits((key12 >> 0) & 0x9249249249249249ull); // 1001 0010 0100 1001 0010 0100 1001 0010 0100 1001 0010 0100 1001 0010 0100 1001
UINT32 x0 = shrinkBits((key0 >> 1) & 0x49249249); // 0100 1001 0010 0100 1001 0010 0100 1001
UINT32 y0 = shrinkBits((key0 >> 0) & 0x49249249); // 0100 1001 0010 0100 1001 0010 0100 1001
UINT32 z0 = shrinkBits((key0 >> 2) & 0x49249249); // 0100 1001 0010 0100 1001 0010 0100 1001
assert(MORTON_CODE_MAX_LEVEL - level < 32); // due to https://stackoverflow.com/a/9860578
x_index = ((x0 << 21) | x12) >> (MORTON_CODE_MAX_LEVEL - level);
y_index = ((y0 << 21) | y12) >> (MORTON_CODE_MAX_LEVEL - level);
z_index = ((z0 << 22) | z12) >> (MORTON_CODE_MAX_LEVEL - level);
assert(x_index < ((UINT64) 1 << level));
assert(y_index < ((UINT64) 1 << level));
assert(z_index < ((UINT64) 1 << level));
}
UINT32 key[MORTON_CODE_UINT32_N];
UINT8 level;
public:
MortonCode()
{
key[0] = std::numeric_limits<UINT32>::max();
key[1] = std::numeric_limits<UINT32>::max();
key[2] = std::numeric_limits<UINT32>::max();
level = std::numeric_limits<UINT8>::max();
}
MortonCode(unsigned int x_index, unsigned int y_index, unsigned z_index, unsigned char lvl)
{
level = lvl;
encodeKey(x_index, y_index, z_index, lvl, key);
}
/**
* x, y, z are in unit cube
*/
MortonCode(double x, double y, double z, unsigned char lvl)
{
level = lvl;
if (level == 0) {
key[0] = 0;
key[1] = 0;
key[2] = 0;
return;
}
const double cube_width = ((UINT64) 1 << MORTON_CODE_MAX_LEVEL);
x = std::min(std::max(x * cube_width, 0.0), cube_width - 1.0);
y = std::min(std::max(y * cube_width, 0.0), cube_width - 1.0);
z = std::min(std::max(z * cube_width, 0.0), cube_width - 1.0);
assert((UINT64) x < ((UINT64) 1 << MORTON_CODE_MAX_LEVEL));
assert((UINT64) y < ((UINT64) 1 << MORTON_CODE_MAX_LEVEL));
assert((UINT64) z < ((UINT64) 1 << MORTON_CODE_MAX_LEVEL));
UINT32 x_index = (UINT32) x;
UINT32 y_index = (UINT32) y;
UINT32 z_index = (UINT32) z;
assert(level > 0);
assert(level <= MORTON_CODE_MAX_LEVEL);
assert(MORTON_CODE_MAX_LEVEL - level < 32);
x_index /= 1 << (MORTON_CODE_MAX_LEVEL - level);
y_index /= 1 << (MORTON_CODE_MAX_LEVEL - level);
z_index /= 1 << (MORTON_CODE_MAX_LEVEL - level);
encodeKey(x_index, y_index, z_index, level, key);
}
void decodeIndices(unsigned int &x_index, unsigned int &y_index, unsigned int &z_index) const
{
decodeKey(key, level, x_index, y_index, z_index);
}
void decodeIndices(unsigned long long &x_index, unsigned long long &y_index, unsigned long long &z_index) const
{
unsigned int x, y, z;
decodeKey(key, level, x, y, z);
x_index = x;
y_index = y;
z_index = z;
}
vector3d center() const
{
const double cube_width = ((UINT64) 1 << level);
unsigned int x_index, y_index, z_index;
decodeKey(key, level, x_index, y_index, z_index);
return vector3d(x_index + 0.5, y_index + 0.5, z_index + 0.5) / cube_width;
}
unsigned char getLevel() const
{
return level;
}
MortonCode copy() const
{
MortonCode that;
that.key[0] = key[0];
that.key[1] = key[1];
that.key[2] = key[2];
that.level = level;
return that;
}
MortonCode createParent() const
{
if (level == 0)
throw std::runtime_error("Root node at zero level has no parent!");
MortonCode parent = copy();
parent.level--;
// (1 lvl) key[0] (11 lvl) key[1] (22 lvl) key[2]
// _vvv___32 bits____vv_v_____32 bits_____v_vv____32 bits_______
// |xyz..............xy|z.................x|yz..............xyz|
// | 11-x 11-y 10-z | 11-x 10-y 11-z | 10-x 11-y 11-z |
if (level == 1) {
parent.key[0] = 0;
} else if (level <= 11) {
const UINT32 FULL_32_BITS = 0xFFFFFFFF;
const UINT32 LAST_BITS = (((UINT32) 1 << (2 + 3 * (11 - level))) - 1);
parent.key[0] = parent.key[0] & (FULL_32_BITS - LAST_BITS);
parent.key[1] = 0;
} else if (level <= 22) {
const UINT32 FULL_32_BITS = 0xFFFFFFFF;
const UINT32 LAST_BITS = (((UINT32) 1 << (1 + 3 * (22 - level))) - 1);
parent.key[1] = parent.key[1] & (FULL_32_BITS - LAST_BITS);
parent.key[2] = 0;
} else {
const UINT32 FULL_32_BITS = 0xFFFFFFFF;
const UINT32 LAST_BITS = (((UINT32) 1 << (0 + 3 * (33 - level))) - 1);
parent.key[2] = parent.key[2] & (FULL_32_BITS - LAST_BITS);
}
return parent;
}
MortonCode createAncestor(unsigned char lvl) const
{
if (lvl >= level)
throw std::runtime_error("Ancestor level should be smaller!");
// Very similar to createParent()
MortonCode ancestor = copy();
ancestor.level = lvl;
// (1 lvl) key[0] (11 lvl) key[1] (22 lvl) key[2]
// _vvv___32 bits____vv_v_____32 bits_____v_vv____32 bits_______
// |xyz..............xy|z.................x|yz..............xyz|
// | 11-x 11-y 10-z | 11-x 10-y 11-z | 10-x 11-y 11-z |
if (lvl == 0) {
ancestor.key[0] = 0;
ancestor.key[1] = 0;
ancestor.key[2] = 0;
} else if ((lvl + 1) <= 11) {
const UINT32 FULL_32_BITS = 0xFFFFFFFF;
const UINT32 LAST_BITS = (((UINT32) 1 << (2 + 3 * (11 - (lvl + 1)))) - 1);
ancestor.key[0] = ancestor.key[0] & (FULL_32_BITS - LAST_BITS);
ancestor.key[1] = 0;
ancestor.key[2] = 0;
} else if ((lvl + 1) <= 22) {
const UINT32 FULL_32_BITS = 0xFFFFFFFF;
const UINT32 LAST_BITS = (((UINT32) 1 << (1 + 3 * (22 - (lvl + 1)))) - 1);
ancestor.key[1] = ancestor.key[1] & (FULL_32_BITS - LAST_BITS);
ancestor.key[2] = 0;
} else {
const UINT32 FULL_32_BITS = 0xFFFFFFFF;
const UINT32 LAST_BITS = (((UINT32) 1 << (0 + 3 * (33 - (lvl + 1)))) - 1);
ancestor.key[2] = ancestor.key[2] & (FULL_32_BITS - LAST_BITS);
}
return ancestor;
}
void getSubindexFromParent(unsigned int &dx, unsigned int &dy, unsigned int &dz) const
{
if (level == 0)
throw std::runtime_error("Root node at zero level has no parent!");
// (1 lvl) key[0] (11 lvl) key[1] (22 lvl) key[2]
// _vvv___32 bits____vv_v_____32 bits_____v_vv____32 bits_______
// |xyz..............xy|z.................x|yz..............xyz|
// | 11-x 11-y 10-z | 11-x 10-y 11-z | 10-x 11-y 11-z |
if (level < 11) {
unsigned int xyz = (key[0] >> (32 - 3 * level)) & 0x7;
dx = (xyz & 0x4) > 0;
dy = (xyz & 0x2) > 0;
dz = (xyz & 0x1) > 0;
} else if (level == 11) {
const UINT32 FIRST_BIT = 0x80000000;
dx = (key[0] & 0x2) > 0;
dy = (key[0] & 0x1) > 0;
dz = (key[1] & FIRST_BIT) > 0;
} else if (level < 22) {
unsigned int xyz = (key[1] >> (64 - 3 * level)) & 0x7;
dx = (xyz & 0x4) > 0;
dy = (xyz & 0x2) > 0;
dz = (xyz & 0x1) > 0;
} else if (level == 22) {
const UINT32 FIRST_BIT = 0x80000000;
const UINT32 SECND_BIT = 0x40000000;
dx = (key[1] & 0x1) > 0;
dy = (key[2] & FIRST_BIT) > 0;
dz = (key[2] & SECND_BIT) > 0;
} else {
unsigned int xyz = (key[2] >> (96 - 3 * level)) & 0x7;
dx = (xyz & 0x4) > 0;
dy = (xyz & 0x2) > 0;
dz = (xyz & 0x1) > 0;
}
assert(dx < 2 && dy < 2 && dz < 2);
}
MortonCode createChild(unsigned int dx, unsigned int dy, unsigned int dz) const
{
assert(dx < 2 && dy < 2 && dz < 2);
MortonCode child = copy();
child.level += 1;
// (1 lvl) key[0] (11 lvl) key[1] (22 lvl) key[2]
// _vvv___32 bits____vv_v_____32 bits_____v_vv____32 bits_______
// |xyz..............xy|z.................x|yz..............xyz|
// | 11-x 11-y 10-z | 11-x 10-y 11-z | 10-x 11-y 11-z |
unsigned int xyz = dx * 4 + dy * 2 + dz;
if (child.level < 11) {
child.key[0] |= (xyz << (3 * (11 - child.level) - 1));
} else if (child.level == 11) {
child.key[0] |= (xyz >> 1);
child.key[1] = (dz << 31);
} else if (child.level < 22) {
child.key[1] |= (xyz << (3 * (22 - child.level) - 2));
} else if (child.level == 22) {
child.key[1] |= (xyz >> 2);
child.key[2] = (xyz << 30);
} else {
child.key[2] |= (xyz << (3 * (33 - child.level) - 3));
}
return child;
}
MortonCode createSibling(unsigned int dx, unsigned int dy, unsigned int dz) const
{
assert(dx < 2 && dy < 2 && dz < 2);
assert(level != 0);
MortonCode parent = createParent();
return parent.createChild(dx, dy, dz);
}
MortonCode createNeighbour(unsigned int dx, unsigned int dy, unsigned int dz, bool &neighbour_out_of_cube) const
{
assert(dx < 3 && dy < 3 && dz < 3);
if (dx == 1 && dy == 1 && dz == 1) {
neighbour_out_of_cube = false;
return copy();
}
if (level == 0) {
neighbour_out_of_cube = true;
return copy();
}
// TODO: speedup (re-implement without decodeKey+encodeKey)
unsigned int x_index, y_index, z_index;
decodeKey(key, level, x_index, y_index, z_index);
neighbour_out_of_cube = false;
UINT64 MAX_VALUE = ((UINT64) 1 << level) - 1;
if (dx == 0 && x_index == 0) {
neighbour_out_of_cube = true;
}
if (dx == 2 && x_index == MAX_VALUE) {
neighbour_out_of_cube = true;
}
if (dy == 0 && y_index == 0) {
neighbour_out_of_cube = true;
}
if (dy == 2 && y_index == MAX_VALUE) {
neighbour_out_of_cube = true;
}
if (dz == 0 && z_index == 0) {
neighbour_out_of_cube = true;
}
if (dz == 2 && z_index == MAX_VALUE) {
neighbour_out_of_cube = true;
}
if (neighbour_out_of_cube) {
return copy();
} else {
MortonCode neighbour(x_index + dx - 1, y_index + dy - 1, z_index + dz - 1, level);
return neighbour;
}
}
bool isAncestorFor(const MortonCode &child) const
{
if (level >= child.level)
return false;
MortonCode real_ancestor = child.createAncestor(level);
return (*this) == real_ancestor;
}
class Comparator {
public:
bool operator()(const MortonCode &a, const MortonCode &b) const
{
assert(a.level <= MORTON_CODE_MAX_LEVEL);
assert(b.level <= MORTON_CODE_MAX_LEVEL);
for (int i = 0; i < MORTON_CODE_UINT32_N; ++i) {
if (a.key[i] < b.key[i]) {
return true;
} else if (a.key[i] > b.key[i]) {
return false;
}
}
return a.level < b.level;
}
};
class ComparatorCoarseToFine {
public:
bool operator()(const MortonCode &a, const MortonCode &b) const
{
assert(a.level <= MORTON_CODE_MAX_LEVEL);
assert(b.level <= MORTON_CODE_MAX_LEVEL);
if (a.level < b.level) {
return true;
} else if (b.level < a.level) {
return false;
} else {
assert(a.level == b.level);
for (int i = 0; i < MORTON_CODE_UINT32_N; ++i) {
if (a.key[i] < b.key[i]) {
return true;
} else if (a.key[i] > b.key[i]) {
return false;
}
}
return false;
}
}
};
static unsigned char levelByRadius(float root_width, float radius)
{
unsigned char lvl = 0;
float lvl_radius = root_width / 2;
while (lvl + 1 <= MORTON_CODE_MAX_LEVEL && radius < lvl_radius) {
lvl_radius /= 2.0f;
++lvl;
}
assert(radius >= lvl_radius);
assert(2.0f * lvl_radius > radius);
return lvl;
}
static float radiusByLevel(float root_width, unsigned char lvl)
{
assert(lvl <= MORTON_CODE_MAX_LEVEL);
float radius = root_width / 2.0f;
float lvl_radius = radius / (1 << lvl);
assert(levelByRadius(root_width, lvl_radius) == lvl);
return lvl_radius;
}
static unsigned int maxIndex(unsigned char level)
{
assert(level <= MORTON_CODE_MAX_LEVEL);
return (unsigned int) (((unsigned long long) 1 << level) - 1);
}
bool operator==(const MortonCode &that) const
{
return level == that.level
&& key[2] == that.key[2]
&& key[1] == that.key[1]
&& key[0] == that.key[0];
}
};
std::ostream &operator<<(std::ostream &os, MortonCode const &key) {
os << "MortonCode {lvl=" << (int) key.getLevel() << " ";
if (key.getLevel() == 0) {
os << "root x=0 y=0 z=0 from [0; 1)}";
return os;
}
MortonCode cur = key;
std::vector<unsigned int> xyzs;
while (true) {
unsigned int dx, dy, dz;
cur.getSubindexFromParent(dx, dy, dz);
xyzs.push_back(dx);
xyzs.push_back(dy);
xyzs.push_back(dz);
MortonCode par = cur.createParent();
if (par.getLevel() == 0) {
break;
}
cur = par;
}
assert(xyzs.size() == 3 * key.getLevel());
unsigned long x = 0;
unsigned long y = 0;
unsigned long z = 0;
for (int lvl = key.getLevel() - 1; lvl >= 0; --lvl) {
x = 2 * x + xyzs[3*lvl+0];
y = 2 * y + xyzs[3*lvl+1];
z = 2 * z + xyzs[3*lvl+2];
}
unsigned long long index_limit = ((unsigned long long) 1 << key.getLevel());
os << "x=" << x << " y=" << y << " z=" << z << " from [0; " << index_limit << ")";
os << "\txyz=";
for (int lvl = key.getLevel() - 1; lvl >= 0; --lvl) {
x = 2 * x + xyzs[3*lvl+0];
y = 2 * y + xyzs[3*lvl+1];
z = 2 * z + xyzs[3*lvl+2];
os << xyzs[3*lvl+0] << xyzs[3*lvl+1] << xyzs[3*lvl+2];
if (lvl > 0)
os << ".";
}
os << "}";
return os;
}
#pragma pack(pop)
static_assert(sizeof(UINT8) == 1, "Unexpected uint8 size!");
static_assert(sizeof(UINT32) == 4, "Unexpected uint32 size!");
static_assert(sizeof(UINT64) == 8, "Unexpected uint64 size!");
static_assert(MORTON_CODE_UINT32_N == 3, "Unexpected MortonCode key size!");
static_assert(sizeof(MortonCode) == 13, "Unexpected MortonCode size!");
#undef UINT8
#undef UINT32
#undef UINT64
#undef UINT32_NBITS
#undef MORTON_CODE_UINT32_N
Raw
morton_code_test.cpp
#include "morton_code.h"
#include <gtest/gtest.h>
#include <stdio.h>
#include <math.h>
#include <random>
#if defined(__GNUC__) && defined(__GNUC_MINOR__) && (__GNUC__ == 4) && (__GNUC_MINOR__ <= 4)
#define default_random_engine mt19937
#define uniform_int_distribution uniform_int
#endif
class MortonCodeInsider : public MortonCode
{
public:
MortonCodeInsider()
: MortonCode() {}
MortonCodeInsider(unsigned int xIndex, unsigned int yIndex, unsigned int zIndex, unsigned char lvl)
: MortonCode(xIndex, yIndex, zIndex, lvl) {}
MortonCodeInsider(double x, double y, double z, unsigned char lvl)
: MortonCode(x, y, z, lvl) {}
unsigned int getKey(int index)
{
return key[index];
}
};
unsigned int maxIndex(unsigned char level)
{
assert(level <= MORTON_CODE_MAX_LEVEL);
return (unsigned int) (((unsigned long long) 1 << level) - 1);
}
double getWidth(unsigned char level)
{
return maxIndex(level) + 1.0;
}
TEST(morton_code, corner_fixed_cases)
{
{
unsigned int level = MORTON_CODE_MAX_LEVEL;
unsigned int x = maxIndex(level);
unsigned int y = maxIndex(level);
unsigned int z = maxIndex(level);
double cube_width = getWidth(level);
vector3d xyz(x, y, z);
vector3d center = (xyz + vector3d(0.5, 0.5, 0.5)) / cube_width;
{
MortonCodeInsider code(x, y, z, level);
EXPECT_EQ(code.getKey(0), 0xFFFFFFFF);
EXPECT_EQ(code.getKey(1), 0xFFFFFFFF);
EXPECT_EQ(code.getKey(2), 0xFFFFFFFF);
EXPECT_EQ(code.center(), center);
}
{
MortonCodeInsider code(center.x(), center.y(), center.z(), level);
EXPECT_EQ(code.getKey(0), 0xFFFFFFFF);
EXPECT_EQ(code.getKey(1), 0xFFFFFFFF);
EXPECT_EQ(code.getKey(2), 0xFFFFFFFF);
EXPECT_EQ(code.center(), center);
}
}
//__________________________________________________________________________________________________________________
{
unsigned int level = MORTON_CODE_MAX_LEVEL;
unsigned int x = maxIndex(level) - 1; // ...110
unsigned int y = maxIndex(level) - 2; // ...101 => .........110101011
unsigned int z = maxIndex(level) - 4; // ...011
double cube_width = getWidth(level);
vector3d xyz(x, y, z);
vector3d center = (xyz + vector3d(0.5, 0.5, 0.5)) / cube_width;
{
MortonCodeInsider code(x, y, z, level);
EXPECT_EQ(code.getKey(0), 0xFFFFFFFF);
EXPECT_EQ(code.getKey(1), 0xFFFFFFFF);
EXPECT_EQ(code.getKey(2), 0xFFFFFFAB); // .........110101011
EXPECT_EQ(code.center(), center);
}
{
MortonCodeInsider code(center.x(), center.y(), center.z(), level);
EXPECT_EQ(code.getKey(0), 0xFFFFFFFF);
EXPECT_EQ(code.getKey(1), 0xFFFFFFFF);
EXPECT_EQ(code.getKey(2), 0xFFFFFFAB); // .........110101011
EXPECT_EQ(code.center(), center);
}
}
//__________________________________________________________________________________________________________________
{
unsigned int level = MORTON_CODE_MAX_LEVEL;
unsigned int x = maxIndex(level) - 4; // 11...011
unsigned int y = maxIndex(level) - 1; // 11...110 => 111111.........011110101
unsigned int z = maxIndex(level) - 2; // 11...101
double cube_width = getWidth(level);
vector3d xyz(x, y, z);
vector3d center = (xyz + vector3d(0.5, 0.5, 0.5)) / cube_width;
{
MortonCodeInsider code(x, y, z, level);
EXPECT_EQ(code.getKey(0), 0xFFFFFFFF);
EXPECT_EQ(code.getKey(1), 0xFFFFFFFF);
EXPECT_EQ(code.getKey(2), 0xFFFFFEF5); // 111111.........011110101
EXPECT_EQ(code.center(), center);
}
{
MortonCodeInsider code(center.x(), center.y(), center.z(), level);
EXPECT_EQ(code.getKey(0), 0xFFFFFFFF);
EXPECT_EQ(code.getKey(1), 0xFFFFFFFF);
EXPECT_EQ(code.getKey(2), 0xFFFFFEF5); // 111111.........011110101
EXPECT_EQ(code.center(), center);
}
}
//__________________________________________________________________________________________________________________
{
unsigned int level = MORTON_CODE_MAX_LEVEL;
unsigned int x = maxIndex(level) - 4 - (1 << 31); // 01...011
unsigned int y = maxIndex(level) - 1; // 11...110 => 011111.........011110101
unsigned int z = maxIndex(level) - 2; // 11...101
double cube_width = getWidth(level);
vector3d xyz(x, y, z);
vector3d center = (xyz + vector3d(0.5, 0.5, 0.5)) / cube_width;
{
MortonCodeInsider code(x, y, z, level);
EXPECT_EQ(code.getKey(0), 0x7FFFFFFF);
EXPECT_EQ(code.getKey(1), 0xFFFFFFFF);
EXPECT_EQ(code.getKey(2), 0xFFFFFEF5); // 011111.........011110101
EXPECT_EQ(code.center(), center);
}
{
MortonCodeInsider code(center.x(), center.y(), center.z(), level);
EXPECT_EQ(code.getKey(0), 0x7FFFFFFF);
EXPECT_EQ(code.getKey(1), 0xFFFFFFFF);
EXPECT_EQ(code.getKey(2), 0xFFFFFEF5); // 011111.........011110101
EXPECT_EQ(code.center(), center);
}
}
//__________________________________________________________________________________________________________________
{
unsigned int level = MORTON_CODE_MAX_LEVEL;
unsigned int x = maxIndex(level) - 4; // 11...011
unsigned int y = maxIndex(level) - 1 - (1 << 31); // 01...110 => 101111.........011110101
unsigned int z = maxIndex(level) - 2; // 11...101
double cube_width = getWidth(level);
vector3d xyz(x, y, z);
vector3d center = (xyz + vector3d(0.5, 0.5, 0.5)) / cube_width;
{
MortonCodeInsider code(x, y, z, level);
EXPECT_EQ(code.getKey(0), 0xBFFFFFFF);
EXPECT_EQ(code.getKey(1), 0xFFFFFFFF);
EXPECT_EQ(code.getKey(2), 0xFFFFFEF5); // 101111.........011110101
EXPECT_EQ(code.center(), center);
}
{
MortonCodeInsider code(center.x(), center.y(), center.z(), level);
EXPECT_EQ(code.getKey(0), 0xBFFFFFFF);
EXPECT_EQ(code.getKey(1), 0xFFFFFFFF);
EXPECT_EQ(code.getKey(2), 0xFFFFFEF5); // 101111.........011110101
EXPECT_EQ(code.center(), center);
}
}
//__________________________________________________________________________________________________________________
{
unsigned int level = MORTON_CODE_MAX_LEVEL;
unsigned int x = maxIndex(level) - 4; // 11...011
unsigned int y = maxIndex(level) - 1; // 11...110 => 110111.........011110101
unsigned int z = maxIndex(level) - 2 - (1 << 31); // 01...101
double cube_width = getWidth(level);
vector3d xyz(x, y, z);
vector3d center = (xyz + vector3d(0.5, 0.5, 0.5)) / cube_width;
{
MortonCodeInsider code(x, y, z, level);
EXPECT_EQ(code.getKey(0), 0xDFFFFFFF);
EXPECT_EQ(code.getKey(1), 0xFFFFFFFF);
EXPECT_EQ(code.getKey(2), 0xFFFFFEF5); // 110111.........011110101
EXPECT_EQ(code.center(), center);
}
{
MortonCodeInsider code(center.x(), center.y(), center.z(), level);
EXPECT_EQ(code.getKey(0), 0xDFFFFFFF);
EXPECT_EQ(code.getKey(1), 0xFFFFFFFF);
EXPECT_EQ(code.getKey(2), 0xFFFFFEF5); // 110111.........011110101
EXPECT_EQ(code.center(), center);
}
}
}
void testCube(unsigned int x, unsigned int y, unsigned int z, unsigned char level)
{
double cube_width = getWidth(level);
vector3d xyz(x, y, z);
vector3d center = (xyz + vector3d(0.5, 0.5, 0.5)) / cube_width;
MortonCode code(x, y, z, level);
EXPECT_EQ(code.center(), center);
{
MortonCode code2(center.x(), center.y(), center.z(), level);
EXPECT_EQ(code, code2);
}
if (level > 0) {
MortonCode parent0(x / 2, y / 2, z / 2, level - 1);
MortonCode parent1 = code.createParent();
EXPECT_EQ(parent0, parent1);
for (unsigned int dx = 0; dx < 2; ++dx) {
for (unsigned int dy = 0; dy < 2; ++dy) {
for (unsigned int dz = 0; dz < 2; ++dz) {
unsigned int x0 = x / 2 * 2;
unsigned int y0 = y / 2 * 2;
unsigned int z0 = z / 2 * 2;
MortonCode sibling0(x0 + dx, y0 + dy, z0 + dz, level);
MortonCode sibling1 = code.createSibling(dx, dy, dz);
MortonCode sibling2 = parent1.createChild(dx, dy, dz);
EXPECT_EQ(sibling0, sibling1);
EXPECT_EQ(sibling1, sibling2);
unsigned int dx0 = std::numeric_limits<unsigned int>::max();
unsigned int dy0 = std::numeric_limits<unsigned int>::max();
unsigned int dz0 = std::numeric_limits<unsigned int>::max();
sibling0.getSubindexFromParent(dx0, dy0, dz0);
EXPECT_EQ(dx0, dx);
EXPECT_EQ(dy0, dy);
EXPECT_EQ(dz0, dz);
}
}
}
for (int dz = 0; dz <= 2; ++dz) {
for (int dy = 0; dy <= 2; ++dy) {
for (int dx = 0; dx <= 2; ++dx) {
bool neighbour_out_of_cube;
MortonCode neighbour1 = code.createNeighbour(dx, dy, dz, neighbour_out_of_cube);
EXPECT_EQ(neighbour_out_of_cube,
(ptrdiff_t) x + dx - 1 < 0 || (ptrdiff_t) x + dx - 1 > maxIndex(level) ||
(ptrdiff_t) y + dy - 1 < 0 || (ptrdiff_t) y + dy - 1 > maxIndex(level) ||
(ptrdiff_t) z + dz - 1 < 0 || (ptrdiff_t) z + dz - 1 > maxIndex(level));
if (neighbour_out_of_cube)
continue;
vector3d expected_center = (vector3d(x + dx - 1, y + dy - 1, z + dz - 1) + vector3d(0.5, 0.5, 0.5)) / cube_width;
vector3d found_center = neighbour1.center();
EXPECT_EQ(found_center, expected_center);
// double voxel_radius = 0.5 / cube_width;
// EXPECT_LT((found_center - expected_center).norm(), 0.1 * voxel_radius);
MortonCode neighbour0(x + dx - 1, y + dy - 1, z + dz - 1, level);
EXPECT_EQ(neighbour0, neighbour1);
}
}
}
}
}
TEST(morton_code, fixed_cases)
{
unsigned int level = 23;
unsigned int x = 262144; // 0100 0000 0000 0000 0000
unsigned int y = 131072; // 0010 0000 0000 0000 0000
unsigned int z = 65536; // 0001 0000 0000 0000 0000
testCube(x, y, z, level);
}
TEST(morton_code, simple_cases)
{
for (int level = 0; level < 8; ++level) {
for (unsigned int x = 0; x <= maxIndex(level); ++x) {
for (unsigned int y = 0; y <= maxIndex(level); ++y) {
for (unsigned int z = 0; z <= maxIndex(level); ++z) {
testCube(x, y, z, level);
}
}
}
}
for (int level = 8; level <= MORTON_CODE_MAX_LEVEL; ++level) {
unsigned long long step = maxIndex(level);
step = (step + 7) / 8;
for (unsigned int xi = 0; xi <= 8; ++xi) {
for (unsigned int yi = 0; yi <= 8; ++yi) {
for (unsigned int zi = 0; zi <= 8; ++zi) {
unsigned int x = std::min(xi * step, (unsigned long long) maxIndex(level));
unsigned int y = std::min(yi * step, (unsigned long long) maxIndex(level));
unsigned int z = std::min(zi * step, (unsigned long long) maxIndex(level));
testCube(x, y, z, level);
}
}
}
}
}
TEST(morton_code, random_cases)
{
int n = 2390000;
std::default_random_engine r(239);
std::uniform_int_distribution<int> level_distribution(9, MORTON_CODE_MAX_LEVEL);
for (int i = 0; i < n; ++i) {
int level = level_distribution(r);
std::uniform_int_distribution<unsigned int> xyz_distribution(0, maxIndex(level));
unsigned int x = xyz_distribution(r);
unsigned int y = xyz_distribution(r);
unsigned int z = xyz_distribution(r);
testCube(x, y, z, level);
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment