39 #ifndef HPP_FCL_MORTON_INL_H 40 #define HPP_FCL_MORTON_INL_H 50 uint32_t quantize(S x, uint32_t n) {
51 return std::max(std::min((uint32_t)(x * (S)n), uint32_t(n - 1)), uint32_t(0));
56 morton_functor<S, uint32_t>::morton_functor(
const AABB& bbox)
58 inv(1.0 / (bbox.max_[0] - bbox.min_[0]),
59 1.0 / (bbox.max_[1] - bbox.min_[1]),
60 1.0 / (bbox.max_[2] - bbox.min_[2])) {
66 uint32_t morton_functor<S, uint32_t>::operator()(
const Vec3f& point)
const {
67 uint32_t x = detail::quantize((point[0] - base[0]) * inv[0], 1024u);
68 uint32_t
y = detail::quantize((point[1] - base[1]) * inv[1], 1024u);
69 uint32_t z = detail::quantize((point[2] - base[2]) * inv[2], 1024u);
71 return detail::morton_code(x, y, z);
76 morton_functor<S, uint64_t>::morton_functor(
const AABB& bbox)
78 inv(1.0 / (bbox.max_[0] - bbox.min_[0]),
79 1.0 / (bbox.max_[1] - bbox.min_[1]),
80 1.0 / (bbox.max_[2] - bbox.min_[2])) {
86 uint64_t morton_functor<S, uint64_t>::operator()(
const Vec3f& point)
const {
87 uint32_t x = detail::quantize((point[0] - base[0]) * inv[0], 1u << 20);
88 uint32_t y = detail::quantize((point[1] - base[1]) * inv[1], 1u << 20);
89 uint32_t z = detail::quantize((point[2] - base[2]) * inv[2], 1u << 20);
91 return detail::morton_code60(x, y, z);
96 constexpr
size_t morton_functor<S, uint64_t>::bits() {
101 template <
typename S>
102 constexpr
size_t morton_functor<S, uint32_t>::bits() {
107 template <
typename S,
size_t N>
108 morton_functor<S, std::bitset<N>>::morton_functor(
const AABB& bbox)
110 inv(1.0 / (bbox.max_[0] - bbox.min_[0]),
111 1.0 / (bbox.max_[1] - bbox.min_[1]),
112 1.0 / (bbox.max_[2] - bbox.min_[2])) {
117 template <
typename S,
size_t N>
118 std::bitset<N> morton_functor<S, std::bitset<N>>::operator()(
119 const Vec3f& point)
const {
120 S x = (point[0] -
base[0]) * inv[0];
121 S y = (point[1] -
base[1]) * inv[1];
122 S z = (point[2] -
base[2]) * inv[2];
123 int start_bit = bits() - 1;
130 for (
size_t i = 0; i < bits() / 3; ++i) {
131 bset[start_bit--] = ((z < 1) ? 0 : 1);
132 bset[start_bit--] = ((y < 1) ? 0 : 1);
133 bset[start_bit--] = ((x < 1) ? 0 : 1);
134 x = ((x >= 1) ? 2 * (x - 1) : 2 * x);
135 y = ((y >= 1) ? 2 * (y - 1) : 2 * y);
136 z = ((z >= 1) ? 2 * (z - 1) : 2 * z);
143 template <
typename S,
size_t N>
144 constexpr
size_t morton_functor<S, std::bitset<N>>::bits() {
MatrixDerived base(const Eigen::MatrixBase< MatrixDerived > &m)
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f