Directory: | ./ |
---|---|
File: | include/coal/broadphase/detail/morton-inl.h |
Date: | 2025-04-01 09:23:31 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 15 | 15 | 100.0% |
Branches: | 5 | 10 | 50.0% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | /* | ||
2 | * Software License Agreement (BSD License) | ||
3 | * | ||
4 | * Copyright (c) 2011-2014, Willow Garage, Inc. | ||
5 | * Copyright (c) 2014-2016, Open Source Robotics Foundation | ||
6 | * Copyright (c) 2016, Toyota Research Institute | ||
7 | * All rights reserved. | ||
8 | * | ||
9 | * Redistribution and use in source and binary forms, with or without | ||
10 | * modification, are permitted provided that the following conditions | ||
11 | * are met: | ||
12 | * | ||
13 | * * Redistributions of source code must retain the above copyright | ||
14 | * notice, this list of conditions and the following disclaimer. | ||
15 | * * Redistributions in binary form must reproduce the above | ||
16 | * copyright notice, this list of conditions and the following | ||
17 | * disclaimer in the documentation and/or other materials provided | ||
18 | * with the distribution. | ||
19 | * * Neither the name of Open Source Robotics Foundation nor the names of its | ||
20 | * contributors may be used to endorse or promote products derived | ||
21 | * from this software without specific prior written permission. | ||
22 | * | ||
23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS | ||
24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT | ||
25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS | ||
26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE | ||
27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, | ||
28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, | ||
29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; | ||
30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER | ||
31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT | ||
32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN | ||
33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE | ||
34 | * POSSIBILITY OF SUCH DAMAGE. | ||
35 | */ | ||
36 | |||
37 | /** @author Jia Pan */ | ||
38 | |||
39 | #ifndef COAL_MORTON_INL_H | ||
40 | #define COAL_MORTON_INL_H | ||
41 | |||
42 | #include "coal/broadphase/detail/morton.h" | ||
43 | |||
44 | namespace coal { /// @cond IGNORE | ||
45 | namespace detail { | ||
46 | |||
47 | //============================================================================== | ||
48 | template <typename S> | ||
49 | 76026 | uint32_t quantize(S x, uint32_t n) { | |
50 | 76026 | return std::max(std::min((uint32_t)(x * (S)n), uint32_t(n - 1)), uint32_t(0)); | |
51 | } | ||
52 | |||
53 | //============================================================================== | ||
54 | template <typename S> | ||
55 | 86 | morton_functor<S, uint32_t>::morton_functor(const AABB& bbox) | |
56 | 86 | : base(bbox.min_), | |
57 |
2/4✓ Branch 1 taken 86 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 86 times.
✗ Branch 5 not taken.
|
86 | inv(Scalar(1) / (bbox.max_[0] - bbox.min_[0]), |
58 |
2/4✓ Branch 1 taken 86 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 86 times.
✗ Branch 5 not taken.
|
86 | Scalar(1) / (bbox.max_[1] - bbox.min_[1]), |
59 |
1/2✓ Branch 3 taken 86 times.
✗ Branch 4 not taken.
|
86 | Scalar(1) / (bbox.max_[2] - bbox.min_[2])) { |
60 | // Do nothing | ||
61 | 86 | } | |
62 | |||
63 | //============================================================================== | ||
64 | template <typename S> | ||
65 | 25342 | uint32_t morton_functor<S, uint32_t>::operator()(const Vec3s& point) const { | |
66 | 25342 | uint32_t x = detail::quantize((point[0] - base[0]) * inv[0], 1024u); | |
67 | 25342 | uint32_t y = detail::quantize((point[1] - base[1]) * inv[1], 1024u); | |
68 | 25342 | uint32_t z = detail::quantize((point[2] - base[2]) * inv[2], 1024u); | |
69 | |||
70 | 25342 | return detail::morton_code(x, y, z); | |
71 | } | ||
72 | |||
73 | //============================================================================== | ||
74 | template <typename S> | ||
75 | morton_functor<S, uint64_t>::morton_functor(const AABB& bbox) | ||
76 | : base(bbox.min_), | ||
77 | inv(Scalar(1) / (bbox.max_[0] - bbox.min_[0]), | ||
78 | Scalar(1) / (bbox.max_[1] - bbox.min_[1]), | ||
79 | Scalar(1) / (bbox.max_[2] - bbox.min_[2])) { | ||
80 | // Do nothing | ||
81 | } | ||
82 | |||
83 | //============================================================================== | ||
84 | template <typename S> | ||
85 | uint64_t morton_functor<S, uint64_t>::operator()(const Vec3s& point) const { | ||
86 | uint32_t x = detail::quantize((point[0] - base[0]) * inv[0], 1u << 20); | ||
87 | uint32_t y = detail::quantize((point[1] - base[1]) * inv[1], 1u << 20); | ||
88 | uint32_t z = detail::quantize((point[2] - base[2]) * inv[2], 1u << 20); | ||
89 | |||
90 | return detail::morton_code60(x, y, z); | ||
91 | } | ||
92 | |||
93 | //============================================================================== | ||
94 | template <typename S> | ||
95 | constexpr size_t morton_functor<S, uint64_t>::bits() { | ||
96 | return 60; | ||
97 | } | ||
98 | |||
99 | //============================================================================== | ||
100 | template <typename S> | ||
101 | 172 | constexpr size_t morton_functor<S, uint32_t>::bits() { | |
102 | 172 | return 30; | |
103 | } | ||
104 | |||
105 | //============================================================================== | ||
106 | template <typename S, size_t N> | ||
107 | morton_functor<S, std::bitset<N>>::morton_functor(const AABB& bbox) | ||
108 | : base(bbox.min_), | ||
109 | inv(Scalar(1) / (bbox.max_[0] - bbox.min_[0]), | ||
110 | Scalar(1) / (bbox.max_[1] - bbox.min_[1]), | ||
111 | Scalar(1) / (bbox.max_[2] - bbox.min_[2])) { | ||
112 | // Do nothing | ||
113 | } | ||
114 | |||
115 | //============================================================================== | ||
116 | template <typename S, size_t N> | ||
117 | std::bitset<N> morton_functor<S, std::bitset<N>>::operator()( | ||
118 | const Vec3s& point) const { | ||
119 | S x = (point[0] - base[0]) * inv[0]; | ||
120 | S y = (point[1] - base[1]) * inv[1]; | ||
121 | S z = (point[2] - base[2]) * inv[2]; | ||
122 | int start_bit = bits() - 1; | ||
123 | std::bitset<N> bset; | ||
124 | |||
125 | x *= 2; | ||
126 | y *= 2; | ||
127 | z *= 2; | ||
128 | |||
129 | for (size_t i = 0; i < bits() / 3; ++i) { | ||
130 | bset[start_bit--] = ((z < 1) ? 0 : 1); | ||
131 | bset[start_bit--] = ((y < 1) ? 0 : 1); | ||
132 | bset[start_bit--] = ((x < 1) ? 0 : 1); | ||
133 | x = ((x >= 1) ? 2 * (x - 1) : 2 * x); | ||
134 | y = ((y >= 1) ? 2 * (y - 1) : 2 * y); | ||
135 | z = ((z >= 1) ? 2 * (z - 1) : 2 * z); | ||
136 | } | ||
137 | |||
138 | return bset; | ||
139 | } | ||
140 | |||
141 | //============================================================================== | ||
142 | template <typename S, size_t N> | ||
143 | constexpr size_t morton_functor<S, std::bitset<N>>::bits() { | ||
144 | return N; | ||
145 | } | ||
146 | |||
147 | } // namespace detail | ||
148 | /// @endcond | ||
149 | } // namespace coal | ||
150 | |||
151 | #endif | ||
152 |