96-bit 3D Morton code (Z-curve)
#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 |
#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