Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
96-bit 3D Morton code (Z-curve)
#pragma once
#include <limits>
#include <cassert>
#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])
{
assert(level <= MORTON_CODE_MAX_LEVEL);
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;
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)
{
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
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);
}
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;
}
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 false;
}
};
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;
}
}
};
};
#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 int level)
{
assert(level <= MORTON_CODE_MAX_LEVEL);
return (unsigned int) (((unsigned long long) 1 << level) - 1);
}
double getWidth(unsigned int level)
{
return maxIndex(level) + 1.0;
}
TEST(morton_code, simple_cases)
{
for (int level = 0; level < 9; ++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) {
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 code(center.x(), center.y(), center.z(), level);
EXPECT_EQ(code.center(), center);
}
}
}
}
}
}
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);
}
}
}
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
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.center(), center);
}
{
MortonCodeInsider code(center.x(), center.y(), center.z(), level);
EXPECT_EQ(code.center(), center);
}
}
}
TEST(morton_code, random_cases)
{
int n = 1000000;
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);
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);
if (code.center() != center) {
MortonCode tmp(x, y, z, level);
EXPECT_EQ(tmp.center(), center);
}
}
{
MortonCode code(center.x(), center.y(), center.z(), level);
EXPECT_EQ(code.center(), center);
}
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.