Branch data Line data Source code
1 : : /*
2 : : * Copyright (C) 2026 by Thun Lu. All rights reserved.
3 : : * Author: Thun Lu <thun.lu@zohomail.cn>
4 : : * Repo: https://github.com/thun-res/vlink
5 : : * _ __ __ _ __
6 : : * | | / / / / (_) ____ / /__
7 : : * | | / / / / / / / __ \ / //_/
8 : : * | |/ / / /___ / / / / / / / ,<
9 : : * |___/ /_____/ /_/ /_/ /_/ /_/|_|
10 : : *
11 : : * Licensed under the Apache License, Version 2.0 (the "License");
12 : : * you may not use this file except in compliance with the License.
13 : : * You may obtain a copy of the License at
14 : : *
15 : : * http://www.apache.org/licenses/LICENSE-2.0
16 : : *
17 : : * Unless required by applicable law or agreed to in writing, software
18 : : * distributed under the License is distributed on an "AS IS" BASIS,
19 : : * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
20 : : * See the License for the specific language governing permissions and
21 : : * limitations under the License.
22 : : */
23 : :
24 : : #include "./zerocopy/point_cloud.h"
25 : :
26 : : #include <tsl/robin_set.h>
27 : :
28 : : #include <array>
29 : : #include <limits>
30 : : #include <sstream>
31 : : #include <string>
32 : : #include <utility>
33 : : #include <vector>
34 : :
35 : : namespace vlink {
36 : :
37 : : namespace zerocopy {
38 : :
39 : 4 : static void pc_pack_to_vertical(uint8_t* dst, const uint8_t* src, size_t count, uint16_t pack, const uint16_t* offsets,
40 : : const uint8_t* sizes, size_t field_count) noexcept {
41 : 4 : size_t out_pos = 0;
42 : :
43 [ + + ]: 20 : for (size_t f = 0; f < field_count; ++f) {
44 : 16 : uint16_t field_offset = offsets[f];
45 : 16 : uint8_t field_size = sizes[f];
46 : :
47 [ + + ]: 120 : for (size_t p = 0; p < count; ++p) {
48 : 104 : std::memcpy(dst + out_pos, src + (p * pack) + field_offset, field_size);
49 : :
50 : 104 : out_pos += field_size;
51 : : }
52 : : }
53 : 4 : }
54 : :
55 : 4 : static void pc_unpack_from_vertical(uint8_t* dst, const uint8_t* src, size_t count, uint16_t pack,
56 : : const uint16_t* offsets, const uint8_t* sizes, size_t field_count) noexcept {
57 : 4 : size_t in_pos = 0;
58 : :
59 [ + + ]: 20 : for (size_t f = 0; f < field_count; ++f) {
60 : 16 : uint16_t field_offset = offsets[f];
61 : 16 : uint8_t field_size = sizes[f];
62 : :
63 [ + + ]: 120 : for (size_t p = 0; p < count; ++p) {
64 : 104 : std::memcpy(dst + (p * pack) + field_offset, src + in_pos, field_size);
65 : :
66 : 104 : in_pos += field_size;
67 : : }
68 : : }
69 : 4 : }
70 : :
71 : 131136 : static int32_t pc_floor_div(int32_t value, int32_t divisor) noexcept {
72 : 131136 : int32_t q = value / divisor;
73 : 131136 : int32_t r = value % divisor;
74 : :
75 [ + + + + ]: 131136 : if (r != 0 && r < 0) {
76 : 32515 : --q;
77 : : }
78 : :
79 : 131136 : return q;
80 : : }
81 : :
82 : 131090 : static uint64_t pc_voxel_key(int32_t fx, int32_t fy, int32_t fz) noexcept {
83 : : static constexpr uint64_t kBias = static_cast<uint64_t>(1) << 20;
84 : : static constexpr uint64_t kMask = (static_cast<uint64_t>(1) << 21) - 1;
85 : :
86 : 131090 : uint64_t kx = (static_cast<uint64_t>(static_cast<int64_t>(fx) + static_cast<int64_t>(kBias))) & kMask;
87 : 131090 : uint64_t ky = (static_cast<uint64_t>(static_cast<int64_t>(fy) + static_cast<int64_t>(kBias))) & kMask;
88 : 131090 : uint64_t kz = (static_cast<uint64_t>(static_cast<int64_t>(fz) + static_cast<int64_t>(kBias))) & kMask;
89 : :
90 : 131090 : return (kx << 42) | (ky << 21) | kz;
91 : : }
92 : :
93 : : struct PcVoxelKeyHash {
94 : 131090 : size_t operator()(uint64_t key) const noexcept {
95 : 131090 : key ^= key >> 33;
96 : 131090 : key *= 0xFF51AFD7ED558CCDULL;
97 : 131090 : key ^= key >> 33;
98 : 131090 : key *= 0xC4CEB9FE1A85EC53ULL;
99 : 131090 : key ^= key >> 33;
100 : :
101 : 131090 : return static_cast<size_t>(key);
102 : : }
103 : : };
104 : :
105 : : static constexpr size_t kVoxelLutSize = 65536;
106 : :
107 : : // PointCloud::Vector3f
108 : 23 : PointCloud::Vector3f::Vector3f() noexcept {
109 : : #if defined(__arm__) || defined(__x86__) || defined(__i386__)
110 : : #ifndef __ANDROID__
111 : : #warning "[PointCloud::Vector3f] No support for 32-bit architecture."
112 : : #endif
113 : : #else
114 : : static_assert(sizeof(Vector3f) == 12, "Sizeof must be 12 bytes.");
115 : : #endif
116 : 23 : }
117 : :
118 : 3 : PointCloud::Vector3f::Vector3f(float _x, float _y, float _z) noexcept : x(_x), y(_y), z(_z) {}
119 : :
120 : 0 : std::ostream& operator<<(std::ostream& ostream, const PointCloud::Vector3f& v3f) noexcept {
121 : 0 : ostream << "(" << v3f.x << ", " << v3f.y << ", " << v3f.z << ")";
122 : :
123 : 0 : return ostream;
124 : : }
125 : :
126 : 3 : PointCloud::Vector3d::Vector3d(double _x, double _y, double _z) noexcept : x(_x), y(_y), z(_z) {}
127 : :
128 : : // PointCloud::Vector3d
129 : 4 : PointCloud::Vector3d::Vector3d() noexcept {
130 : : #if defined(__arm__) || defined(__x86__) || defined(__i386__)
131 : : #ifndef __ANDROID__
132 : : #warning "[PointCloud::Vector3d] No support for 32-bit architecture."
133 : : #endif
134 : : #else
135 : : static_assert(sizeof(Vector3d) == 24, "Sizeof must be 24 bytes.");
136 : : #endif
137 : 4 : }
138 : :
139 : 0 : std::ostream& operator<<(std::ostream& ostream, const PointCloud::Vector3d& v3d) noexcept {
140 : 0 : ostream << "(" << v3d.x << ", " << v3d.y << ", " << v3d.z << ")";
141 : :
142 : 0 : return ostream;
143 : : }
144 : :
145 : : // PointCloud
146 : 206 : PointCloud::PointCloud() noexcept {
147 : : #if defined(__arm__) || defined(__x86__) || defined(__i386__)
148 : : #ifndef __ANDROID__
149 : : #warning "[PointCloud] No support for 32-bit architecture."
150 : : #endif
151 : : #else
152 : : static_assert(sizeof(PointCloud) == 256, "Sizeof must be 256 bytes.");
153 : : #endif
154 : 206 : }
155 : :
156 : 211 : PointCloud::~PointCloud() noexcept {
157 [ + + + - : 211 : if (is_owner_ && data_ && capacity_ != 0) {
+ - ]
158 : 142 : Bytes::bytes_free(data_, capacity_);
159 : : }
160 : 211 : }
161 : :
162 : 2 : PointCloud::PointCloud(const PointCloud& target) noexcept { deep_copy(target); }
163 : :
164 : 3 : PointCloud::PointCloud(PointCloud&& target) noexcept { move_copy(target); }
165 : :
166 : 2 : PointCloud& PointCloud::operator=(const PointCloud& target) noexcept {
167 [ + + ]: 2 : if VUNLIKELY (this == &target) {
168 : 1 : return *this;
169 : : }
170 : :
171 : 1 : deep_copy(target);
172 : :
173 : 1 : return *this;
174 : : }
175 : :
176 : 2 : PointCloud& PointCloud::operator=(PointCloud&& target) noexcept {
177 [ + + ]: 2 : if VUNLIKELY (this == &target) {
178 : 1 : return *this;
179 : : }
180 : :
181 : 1 : move_copy(target);
182 : :
183 : 1 : return *this;
184 : : }
185 : :
186 : 30 : bool PointCloud::operator<<(const Bytes& bytes) noexcept {
187 : : static constexpr size_t kMagicNumberBeginSize = sizeof(kMagicNumberBegin);
188 : : static constexpr size_t kVersionSize = sizeof(kWireVersion);
189 : : // static constexpr size_t kMagicNumberEndSize = sizeof(kMagicNumberEnd);
190 : :
191 [ + + ]: 30 : if VUNLIKELY (bytes.empty()) {
192 : 2 : return false;
193 : : }
194 : :
195 [ + + ]: 28 : if VUNLIKELY (!check_valid(bytes)) {
196 : 5 : return false;
197 : : }
198 : :
199 : 23 : uint32_t wire_version = 0;
200 : 23 : std::memcpy(&wire_version, bytes.data() + kMagicNumberBeginSize, kVersionSize);
201 : :
202 [ - + ]: 23 : if VUNLIKELY (version_major(wire_version) != version_major(kWireVersion)) {
203 : : return false; // LCOV_EXCL_LINE GCOVR_EXCL_LINE
204 : : }
205 : :
206 [ + + + - : 23 : if (is_owner_ && data_ && capacity_ != 0) {
+ - ]
207 : 1 : Bytes::bytes_free(data_, capacity_);
208 : : }
209 : :
210 : : #if defined(__GNUC__) && !defined(__clang__)
211 : : #pragma GCC diagnostic push
212 : : #pragma GCC diagnostic ignored "-Wclass-memaccess"
213 : : #if __GNUC__ >= 11
214 : : #pragma GCC diagnostic ignored "-Wstringop-overread"
215 : : #endif
216 : : #endif
217 : :
218 : 23 : auto* target_ptr = reinterpret_cast<uint8_t*>(this);
219 : :
220 : 23 : std::memcpy(target_ptr, bytes.data() + kMagicNumberBeginSize + kVersionSize, sizeof(PointCloud));
221 : :
222 : : #if defined(__GNUC__) && !defined(__clang__)
223 : : #pragma GCC diagnostic pop
224 : : #endif
225 : :
226 : 23 : is_owner_ = false;
227 : 23 : index_ = 0;
228 : 23 : data_ = const_cast<uint8_t*>(bytes.data() + kMagicNumberBeginSize + kVersionSize + sizeof(PointCloud));
229 : 23 : capacity_ = 0;
230 : :
231 : 23 : uint8_t vertical_raw = 0;
232 : 23 : std::memcpy(&vertical_raw, &vertical_, sizeof(vertical_raw));
233 : :
234 [ + + ]: 23 : if VUNLIKELY (vertical_raw > 1) {
235 : 1 : clear(true);
236 : :
237 : 1 : return false;
238 : : }
239 : :
240 : 22 : vertical_ = (vertical_raw != 0);
241 : :
242 [ + + ]: 22 : if (extent_ == 0) {
243 : 18 : downsample_ = 0;
244 : : }
245 : :
246 : : static constexpr size_t kSerializedOverhead =
247 : : kMagicNumberBeginSize + kVersionSize + sizeof(PointCloud) + sizeof(kMagicNumberEnd);
248 : :
249 [ + - + + : 22 : if VUNLIKELY (pack_size_ != 0 && size_ > (std::numeric_limits<size_t>::max() - kSerializedOverhead) / pack_size_) {
+ + ]
250 : 1 : clear(true);
251 : :
252 : 1 : return false;
253 : : }
254 : :
255 [ + + ]: 21 : if VUNLIKELY (bytes.size() != get_serialized_size()) {
256 : 2 : clear(true);
257 : :
258 : 2 : return false;
259 : : }
260 : :
261 : 19 : std::array<uint16_t, 16> field_offsets{};
262 : 19 : std::array<uint8_t, 16> field_sizes{};
263 : 19 : size_t field_count = 0;
264 : :
265 [ + - ]: 19 : if VLIKELY (pack_size_ != 0) {
266 : 19 : uint16_t field_offset = 0;
267 : 19 : bool leading_zero = true;
268 : :
269 [ + + ]: 323 : for (int i = 15; i >= 0; --i) {
270 : 304 : uint8_t field_size = (protocol_.size_num >> (i * 4)) & 0xF;
271 : :
272 [ + + + + ]: 304 : if (leading_zero && field_size == 0) {
273 : 236 : continue;
274 : : }
275 : :
276 : 68 : leading_zero = false;
277 : 68 : field_offsets[field_count] = field_offset;
278 : 68 : field_sizes[field_count] = field_size;
279 : 68 : ++field_count;
280 : 68 : field_offset += field_size;
281 : : }
282 : :
283 [ - + ]: 19 : if VUNLIKELY (field_offset != pack_size_) {
284 : : clear(true); // LCOV_EXCL_LINE GCOVR_EXCL_LINE
285 : :
286 : : return false; // LCOV_EXCL_LINE GCOVR_EXCL_LINE
287 : : }
288 : : }
289 : :
290 [ + + + - : 19 : if (vertical_ && size_ != 0 && pack_size_ != 0) {
+ - ]
291 : 4 : const uint8_t* payload = data_;
292 : :
293 : 4 : capacity_ = size_ * pack_size_;
294 : 4 : data_ = Bytes::bytes_malloc(capacity_);
295 : :
296 [ - + ]: 4 : if VUNLIKELY (!data_) {
297 : : clear(true); // LCOV_EXCL_LINE GCOVR_EXCL_LINE
298 : :
299 : : return false; // LCOV_EXCL_LINE GCOVR_EXCL_LINE
300 : : }
301 : :
302 : 4 : is_owner_ = true;
303 : 4 : index_ = capacity_;
304 : :
305 : 4 : pc_unpack_from_vertical(data_, payload, size_, pack_size_, field_offsets.data(), field_sizes.data(), field_count);
306 : : }
307 : :
308 : 19 : return true;
309 : : }
310 : :
311 : 33 : bool PointCloud::operator>>(Bytes& bytes) const noexcept {
312 : : static constexpr size_t kMagicNumberBeginSize = sizeof(kMagicNumberBegin);
313 : : static constexpr size_t kVersionSize = sizeof(kWireVersion);
314 : : static constexpr size_t kMagicNumberEndSize = sizeof(kMagicNumberEnd);
315 : :
316 [ + + + + : 33 : if (bytes.empty() || bytes.size() != get_serialized_size()) {
+ + ]
317 : 31 : bytes = Bytes::create(get_serialized_size());
318 : :
319 [ - + ]: 31 : if VUNLIKELY (bytes.empty()) {
320 : 0 : return false;
321 : : }
322 : : }
323 : :
324 : 33 : std::memcpy(bytes.data(), &kMagicNumberBegin, kMagicNumberBeginSize);
325 : :
326 : 33 : std::memcpy(bytes.data() + kMagicNumberBeginSize, &kWireVersion, kVersionSize);
327 : :
328 : : // NOLINTNEXTLINE(bugprone-undefined-memory-manipulation)
329 : 33 : std::memcpy(bytes.data() + kMagicNumberBeginSize + kVersionSize, this, sizeof(PointCloud));
330 : :
331 : 33 : const auto data_offset = reinterpret_cast<const uint8_t*>(&data_) - reinterpret_cast<const uint8_t*>(this);
332 : 33 : const size_t data_pointer_size = sizeof(data_);
333 : 33 : std::memset(bytes.data() + kMagicNumberBeginSize + kVersionSize + data_offset, 0, data_pointer_size);
334 : :
335 [ + - + + : 33 : if VLIKELY (data_ != nullptr && size_ != 0 && pack_size_ != 0) {
+ + + - +
+ ]
336 : 32 : uint8_t* payload = bytes.data() + kMagicNumberBeginSize + kVersionSize + sizeof(PointCloud);
337 : :
338 [ + + ]: 32 : if (vertical_) {
339 : 4 : std::array<uint16_t, 16> field_offsets{};
340 : 4 : std::array<uint8_t, 16> field_sizes{};
341 : 4 : size_t field_count = 0;
342 : 4 : uint16_t field_offset = 0;
343 : 4 : bool leading_zero = true;
344 : :
345 [ + + ]: 68 : for (int i = 15; i >= 0; --i) {
346 : 64 : uint8_t field_size = (protocol_.size_num >> (i * 4)) & 0xF;
347 : :
348 [ + + + + ]: 64 : if (leading_zero && field_size == 0) {
349 : 48 : continue;
350 : : }
351 : :
352 : 16 : leading_zero = false;
353 : 16 : field_offsets[field_count] = field_offset;
354 : 16 : field_sizes[field_count] = field_size;
355 : 16 : ++field_count;
356 : :
357 : 16 : field_offset += field_size;
358 : : }
359 : :
360 : 4 : pc_pack_to_vertical(payload, data_, size_, pack_size_, field_offsets.data(), field_sizes.data(), field_count);
361 : : } else {
362 : 28 : std::memcpy(payload, data_, size_ * pack_size_);
363 : : }
364 : : }
365 : :
366 : 33 : std::memcpy(bytes.data() + kMagicNumberBeginSize + kVersionSize + sizeof(PointCloud) + (size_ * pack_size_),
367 : : &kMagicNumberEnd, kMagicNumberEndSize);
368 : :
369 : 33 : return true;
370 : : }
371 : :
372 : 38 : bool PointCloud::check_valid(const Bytes& bytes) noexcept {
373 : : static constexpr size_t kMagicNumberBeginSize = sizeof(kMagicNumberBegin);
374 : : static constexpr size_t kVersionSize = sizeof(kWireVersion);
375 : : static constexpr size_t kMagicNumberEndSize = sizeof(kMagicNumberEnd);
376 : :
377 [ + + ]: 38 : if VUNLIKELY (bytes.size() < kMagicNumberBeginSize + kVersionSize + sizeof(PointCloud) + kMagicNumberEndSize) {
378 : 1 : return false;
379 : : }
380 : :
381 : 37 : uint32_t check_magic = 0;
382 : :
383 : 37 : std::memcpy(&check_magic, bytes.begin(), kMagicNumberBeginSize);
384 : :
385 [ + + ]: 37 : if VUNLIKELY (check_magic != kMagicNumberBegin) {
386 : 2 : return false;
387 : : }
388 : :
389 : 35 : uint32_t wire_version = 0;
390 : 35 : std::memcpy(&wire_version, bytes.data() + kMagicNumberBeginSize, kVersionSize);
391 : :
392 [ + + ]: 35 : if VUNLIKELY (version_major(wire_version) != version_major(kWireVersion)) {
393 : 2 : return false;
394 : : }
395 : :
396 : 33 : std::memcpy(&check_magic, bytes.end() - kMagicNumberEndSize, kMagicNumberEndSize);
397 : :
398 [ + + ]: 33 : if VUNLIKELY (check_magic != kMagicNumberEnd) {
399 : 4 : return false;
400 : : }
401 : :
402 : 29 : return true;
403 : : }
404 : :
405 [ + + + - : 14 : bool PointCloud::is_valid() const noexcept { return data_ != nullptr && size_ != 0 && pack_size_ != 0; }
+ - ]
406 : :
407 : 20 : bool PointCloud::shallow_copy(const PointCloud& target) noexcept {
408 [ + + ]: 20 : if VUNLIKELY (this == &target) {
409 : 3 : return false;
410 : : }
411 : :
412 [ + + + - : 17 : if (is_owner_ && data_ && capacity_ != 0) {
+ - ]
413 : 4 : const auto current = reinterpret_cast<uintptr_t>(data_);
414 : 4 : const auto source = reinterpret_cast<uintptr_t>(target.data_);
415 : :
416 [ + + + + : 4 : if VUNLIKELY (source >= current && source - current < capacity_) {
+ + ]
417 : 2 : return false;
418 : : }
419 : :
420 : 2 : Bytes::bytes_free(data_, capacity_);
421 : : }
422 : :
423 : 15 : header = target.header;
424 : 15 : capacity_ = 0;
425 : 15 : size_ = target.size_;
426 : 15 : reserved_buf_ = target.reserved_buf_;
427 : 15 : reserved_buf2_ = target.reserved_buf2_;
428 : 15 : reserved_buf3_ = target.reserved_buf3_;
429 : 15 : downsample_ = target.downsample_;
430 : 15 : extent_ = target.extent_;
431 : 15 : vertical_ = target.vertical_;
432 : 15 : pack_size_ = target.pack_size_;
433 : 15 : is_owner_ = false;
434 : 15 : index_ = target.index_;
435 : 15 : data_ = target.data_;
436 : :
437 : 15 : protocol_.size_num = target.protocol_.size_num;
438 : 15 : protocol_.type_num = target.protocol_.type_num;
439 : 15 : std::memcpy(protocol_.names, target.protocol_.names, sizeof(protocol_.names));
440 : :
441 : 15 : return true;
442 : : }
443 : :
444 : 9 : bool PointCloud::deep_copy(const PointCloud& target) noexcept {
445 [ + - - + : 9 : if VUNLIKELY (target.pack_size_ != 0 && target.size_ > std::numeric_limits<size_t>::max() / target.pack_size_) {
- + ]
446 : 0 : return false;
447 : : }
448 : :
449 : 9 : const size_t target_size = target.size_ * target.pack_size_;
450 : :
451 [ + + + - : 9 : if VLIKELY (data_ && is_owner_ && target.data_ && capacity_ != 0 && capacity_ == target_size) {
+ + + - +
+ + - + +
+ + + + ]
452 : 1 : const auto current = reinterpret_cast<uintptr_t>(data_);
453 : 1 : const auto source = reinterpret_cast<uintptr_t>(target.data_);
454 : :
455 [ + - - + : 1 : if VUNLIKELY (source >= current && source - current < capacity_) {
- + ]
456 : 0 : return false;
457 : : }
458 : :
459 : 1 : header = target.header;
460 : 1 : size_ = target.size_;
461 : 1 : reserved_buf_ = target.reserved_buf_;
462 : 1 : reserved_buf2_ = target.reserved_buf2_;
463 : 1 : reserved_buf3_ = target.reserved_buf3_;
464 : 1 : downsample_ = target.downsample_;
465 : 1 : extent_ = target.extent_;
466 : 1 : vertical_ = target.vertical_;
467 : 1 : pack_size_ = target.pack_size_;
468 : 1 : index_ = target.index_;
469 : :
470 : 1 : protocol_.size_num = target.protocol_.size_num;
471 : 1 : protocol_.type_num = target.protocol_.type_num;
472 : 1 : std::memcpy(protocol_.names, target.protocol_.names, sizeof(protocol_.names));
473 : :
474 : 1 : std::memcpy(data_, target.data_, capacity_);
475 : :
476 : 1 : return true;
477 : : }
478 : :
479 [ + + ]: 8 : if VUNLIKELY (!shallow_copy(target)) {
480 : 2 : return false;
481 : : }
482 : :
483 [ + - + - : 6 : if VLIKELY (data_ != nullptr && target_size != 0) {
+ - ]
484 : 6 : auto* target_data = data_;
485 : 6 : capacity_ = target_size;
486 : 6 : data_ = Bytes::bytes_malloc(capacity_);
487 : :
488 [ - + ]: 6 : if VUNLIKELY (!data_) {
489 : 0 : capacity_ = 0;
490 : 0 : size_ = 0;
491 : 0 : index_ = 0;
492 : 0 : return false;
493 : : }
494 : :
495 : 6 : std::memcpy(data_, target_data, capacity_);
496 : 6 : is_owner_ = true;
497 : : }
498 : :
499 : 6 : return true;
500 : : }
501 : :
502 : 6 : bool PointCloud::move_copy(PointCloud& target) noexcept {
503 [ + + ]: 6 : if VUNLIKELY (!shallow_copy(target)) {
504 : 1 : return false;
505 : : }
506 : :
507 : 5 : is_owner_ = target.is_owner_;
508 : 5 : capacity_ = target.capacity_;
509 : :
510 : 5 : target.capacity_ = 0;
511 : 5 : target.size_ = 0;
512 : 5 : target.reserved_buf_ = 0;
513 : 5 : target.reserved_buf2_ = 0;
514 : 5 : target.reserved_buf3_ = 0;
515 : 5 : target.downsample_ = 0;
516 : 5 : target.extent_ = 0;
517 : 5 : target.vertical_ = false;
518 : 5 : target.pack_size_ = 0;
519 : 5 : target.is_owner_ = false;
520 : 5 : target.index_ = 0;
521 : 5 : target.data_ = nullptr;
522 : :
523 : 5 : target.protocol_.size_num = 0;
524 : 5 : target.protocol_.type_num = 0;
525 : 5 : std::memset(&target.protocol_.names, 0, sizeof(protocol_.names));
526 : :
527 : : #if defined(__GNUC__) && !defined(__clang__)
528 : : #pragma GCC diagnostic push
529 : : #pragma GCC diagnostic ignored "-Wclass-memaccess"
530 : : #if __GNUC__ >= 11
531 : : #pragma GCC diagnostic ignored "-Wstringop-overread"
532 : : #endif
533 : : #endif
534 : :
535 : 5 : std::memset(&target.header, 0, sizeof(header));
536 : :
537 : : #if defined(__GNUC__) && !defined(__clang__)
538 : : #pragma GCC diagnostic pop
539 : : #endif
540 : :
541 : 5 : return true;
542 : : }
543 : :
544 : 65 : size_t PointCloud::get_serialized_size() const noexcept {
545 : : static constexpr size_t kMagicNumberBeginSize = sizeof(kMagicNumberBegin);
546 : : static constexpr size_t kVersionSize = sizeof(kWireVersion);
547 : : static constexpr size_t kMagicNumberEndSize = sizeof(kMagicNumberEnd);
548 : :
549 : 65 : return kMagicNumberBeginSize + kVersionSize + sizeof(PointCloud) + (size_ * pack_size_) + kMagicNumberEndSize;
550 : : }
551 : :
552 : 22 : PointCloud::KeyMap PointCloud::get_key_map(KeyList* key_list) const noexcept {
553 : 22 : KeyMap map;
554 : :
555 : 22 : auto target_key_list = protocol_.get_key_list();
556 : :
557 : 22 : uint16_t index = 0;
558 : :
559 [ + + ]: 120 : for (const auto& key : target_key_list) {
560 : 98 : map.try_emplace(key.name, index);
561 : :
562 : 98 : index += key.size;
563 : : }
564 : :
565 [ + + ]: 22 : if (key_list) {
566 [ - + ]: 4 : if (extent_ != 0) {
567 [ # # # # : 0 : for (size_t i = 0; i < target_key_list.size() && i < 3; ++i) {
# # ]
568 [ # # # # : 0 : if (target_key_list[i].type == kInt16Type && target_key_list[i].size == sizeof(int16_t)) {
# # ]
569 : 0 : target_key_list[i].type = kFloatType;
570 : : }
571 : : }
572 : : }
573 : :
574 : 4 : *key_list = std::move(target_key_list);
575 : : }
576 : :
577 : 22 : return map;
578 : 22 : }
579 : :
580 : 6 : PointCloud::KeyList PointCloud::get_key_list() const noexcept {
581 : 6 : auto key_list = protocol_.get_key_list();
582 : :
583 [ + + ]: 6 : if (extent_ != 0) {
584 [ + + + - : 4 : for (size_t i = 0; i < key_list.size() && i < 3; ++i) {
+ + ]
585 [ + - + - : 3 : if (key_list[i].type == kInt16Type && key_list[i].size == sizeof(int16_t)) {
+ - ]
586 : 3 : key_list[i].type = kFloatType;
587 : : }
588 : : }
589 : : }
590 : :
591 : 6 : return key_list;
592 : : }
593 : :
594 : 68 : size_t PointCloud::size() const noexcept { return size_; }
595 : :
596 : 21 : size_t PointCloud::pack_size() const noexcept { return pack_size_; }
597 : :
598 : 33 : bool PointCloud::is_owner() const noexcept { return is_owner_; }
599 : :
600 : 24 : uint16_t PointCloud::get_extent() const noexcept { return extent_; }
601 : :
602 : 16 : bool PointCloud::get_vertical() const noexcept { return vertical_; }
603 : :
604 : 2 : void PointCloud::set_vertical(bool vertical) noexcept { vertical_ = vertical; }
605 : :
606 : 10 : uint8_t PointCloud::get_downsample() const noexcept { return downsample_; }
607 : :
608 : 9 : bool PointCloud::downsample(uint8_t level) noexcept {
609 [ + + ]: 9 : if (level == 0) {
610 : 1 : downsample_ = 0;
611 : :
612 : 1 : return true;
613 : : }
614 : :
615 [ + + ]: 8 : if VUNLIKELY (extent_ == 0) {
616 : 1 : return false;
617 : : }
618 : :
619 [ + + - + : 7 : if VUNLIKELY (!is_owner_ || !data_ || pack_size_ < (sizeof(int16_t) * 3)) {
+ + - + +
+ ]
620 : 1 : return false;
621 : : }
622 : :
623 : 6 : downsample_ = level;
624 : :
625 [ + + ]: 6 : if VUNLIKELY (size_ == 0) {
626 : 1 : return true;
627 : : }
628 : :
629 : 5 : auto v_q = static_cast<int32_t>((static_cast<uint32_t>(level) * 128U + 127U) / 255U);
630 : :
631 : 5 : const int32_t cell_lo = pc_floor_div(-32768, v_q);
632 : 5 : const int32_t cell_hi = pc_floor_div(32767, v_q);
633 : 5 : const int32_t cells_span = cell_hi - cell_lo;
634 : 5 : const uint64_t cells_per_axis = static_cast<uint64_t>(cells_span) + 1;
635 : 5 : const uint64_t cell_count = cells_per_axis * cells_per_axis * cells_per_axis;
636 [ + - ]: 5 : const size_t reserve_n = (cell_count < static_cast<uint64_t>(size_)) ? static_cast<size_t>(cell_count)
637 : : : size_; // LCOV_EXCL_LINE GCOVR_EXCL_LINE
638 : :
639 : 5 : const uint16_t ps = pack_size_;
640 : :
641 : 5 : tsl::robin_set<uint64_t, PcVoxelKeyHash> seen;
642 : 5 : seen.reserve(reserve_n);
643 : :
644 : 5 : std::vector<int16_t> cell_lut;
645 : 5 : const int16_t* lut = nullptr;
646 : :
647 [ + + ]: 5 : if (size_ >= kVoxelLutSize) {
648 : 2 : cell_lut.resize(kVoxelLutSize);
649 : :
650 [ + + ]: 131074 : for (size_t idx = 0; idx < kVoxelLutSize; ++idx) {
651 : 131072 : const int32_t q = static_cast<int32_t>(idx) - 32768;
652 : 131072 : cell_lut[idx] = static_cast<int16_t>(pc_floor_div(q, v_q));
653 : : }
654 : :
655 : 2 : lut = cell_lut.data();
656 : : }
657 : :
658 : 5 : size_t w = 0;
659 : :
660 [ + + ]: 131095 : for (size_t r = 0; r < size_; ++r) {
661 : 131090 : const uint8_t* src = data_ + (r * ps);
662 : :
663 : 131090 : int16_t qx = 0;
664 : 131090 : int16_t qy = 0;
665 : 131090 : int16_t qz = 0;
666 : :
667 : 131090 : std::memcpy(&qx, src, sizeof(int16_t));
668 : 131090 : std::memcpy(&qy, src + sizeof(int16_t), sizeof(int16_t));
669 : 131090 : std::memcpy(&qz, src + (sizeof(int16_t) * 2), sizeof(int16_t));
670 : :
671 : 131090 : int32_t fx = 0;
672 : 131090 : int32_t fy = 0;
673 : 131090 : int32_t fz = 0;
674 : :
675 [ + + ]: 131090 : if (lut != nullptr) {
676 : 131072 : fx = lut[static_cast<int32_t>(qx) + 32768];
677 : 131072 : fy = lut[static_cast<int32_t>(qy) + 32768];
678 : 131072 : fz = lut[static_cast<int32_t>(qz) + 32768];
679 : : } else {
680 : 18 : fx = pc_floor_div(qx, v_q);
681 : 18 : fy = pc_floor_div(qy, v_q);
682 : 18 : fz = pc_floor_div(qz, v_q);
683 : : }
684 : :
685 [ + + ]: 131090 : if (seen.insert(pc_voxel_key(fx, fy, fz)).second) {
686 [ + + ]: 15 : if (w != r) {
687 : 5 : std::memcpy(data_ + (w * ps), src, ps);
688 : : }
689 : :
690 : 15 : ++w;
691 : : }
692 : : }
693 : :
694 : 5 : size_ = w;
695 : 5 : index_ = w * ps;
696 : :
697 : 5 : return true;
698 : 5 : }
699 : :
700 : 11 : uint64_t PointCloud::get_protocol_size_num() const noexcept { return protocol_.size_num; }
701 : :
702 : 10 : uint64_t PointCloud::get_protocol_type_num() const noexcept { return protocol_.type_num; }
703 : :
704 : 1 : std::string PointCloud::get_protocol_size_str() const noexcept { return protocol_.get_size_for_print(); }
705 : :
706 : 3 : std::string PointCloud::get_protocol_name_str() const noexcept {
707 : 3 : return std::string(protocol_.names, ::strnlen(protocol_.names, sizeof(protocol_.names)));
708 : : }
709 : :
710 : 4 : std::string PointCloud::get_protocol_type_str() const noexcept {
711 [ + + ]: 4 : if (extent_ == 0) {
712 : 3 : return protocol_.get_type_for_print();
713 : : }
714 : :
715 : 1 : auto key_list = protocol_.get_key_list();
716 : :
717 : 1 : std::string print_str;
718 : :
719 [ + + ]: 5 : for (size_t i = 0; i < key_list.size(); ++i) {
720 : 4 : const auto& key = key_list[i];
721 : 4 : uint8_t type = key.type;
722 : :
723 [ + + + - : 4 : if (i < 3 && key.type == kInt16Type && key.size == sizeof(int16_t)) {
+ - ]
724 : 3 : type = kFloatType;
725 : : }
726 : :
727 [ + + ]: 4 : if (!print_str.empty()) {
728 : 3 : print_str += ",";
729 : : }
730 : :
731 [ - - - - : 4 : switch (type) {
- - - - -
+ - - ]
732 : 0 : case kBoolType:
733 : 0 : print_str += "bool";
734 : 0 : break;
735 : 0 : case kInt8Type:
736 : 0 : print_str += "int8";
737 : 0 : break;
738 : 0 : case kUint8Type:
739 : 0 : print_str += "uint8";
740 : 0 : break;
741 : 0 : case kInt16Type:
742 : 0 : print_str += "int16";
743 : 0 : break;
744 : 0 : case kUint16Type:
745 : 0 : print_str += "uint16";
746 : 0 : break;
747 : 0 : case kInt32Type:
748 : 0 : print_str += "int32";
749 : 0 : break;
750 : 0 : case kUint32Type:
751 : 0 : print_str += "uint32";
752 : 0 : break;
753 : 0 : case kInt64Type:
754 : 0 : print_str += "int64";
755 : 0 : break;
756 : 0 : case kUint64Type:
757 : 0 : print_str += "uint64";
758 : 0 : break;
759 : 4 : case kFloatType:
760 : 4 : print_str += "float";
761 : 4 : break;
762 : 0 : case kDoubleType:
763 : 0 : print_str += "double";
764 : 0 : break;
765 : 0 : default:
766 : 0 : break;
767 : : }
768 : : }
769 : :
770 : 1 : return print_str;
771 : 1 : }
772 : :
773 : 27 : const uint8_t* PointCloud::get_internal_data() const noexcept { return data_; }
774 : :
775 : 13 : size_t PointCloud::get_reserved_size() const noexcept {
776 [ + + ]: 13 : if (pack_size_ == 0) {
777 : 2 : return 0;
778 : : }
779 : :
780 : 11 : return capacity_ / pack_size_;
781 : : }
782 : :
783 : 38 : bool PointCloud::get_value_v3f(float& x, float& y, float& z, size_t loop_index) const noexcept {
784 [ + + ]: 38 : if (extent_ != 0) {
785 [ + - + + : 31 : if VUNLIKELY (!data_ || loop_index >= size_ || pack_size_ < sizeof(int16_t) * 3) {
+ + - + +
+ ]
786 : 1 : return false;
787 : : }
788 : :
789 : 30 : const uint8_t* base = data_ + (loop_index * pack_size_);
790 : :
791 : 30 : int16_t qx = 0;
792 : 30 : int16_t qy = 0;
793 : 30 : int16_t qz = 0;
794 : :
795 : 30 : std::memcpy(&qx, base, sizeof(int16_t));
796 : 30 : std::memcpy(&qy, base + sizeof(int16_t), sizeof(int16_t));
797 : 30 : std::memcpy(&qz, base + (sizeof(int16_t) * 2), sizeof(int16_t));
798 : :
799 : 30 : x = Quantize::decode<float>(extent_, qx);
800 : 30 : y = Quantize::decode<float>(extent_, qy);
801 : 30 : z = Quantize::decode<float>(extent_, qz);
802 : :
803 : 30 : return true;
804 : : }
805 : :
806 [ + - + + : 7 : if VUNLIKELY (!data_ || loop_index >= size_ || pack_size_ < sizeof(float) * 3) {
+ + - + +
+ ]
807 : 2 : return false;
808 : : }
809 : :
810 : 5 : std::memcpy(&x, data_ + (loop_index * pack_size_), sizeof(float));
811 : 5 : std::memcpy(&y, data_ + (loop_index * pack_size_) + sizeof(float), sizeof(float));
812 : 5 : std::memcpy(&z, data_ + (loop_index * pack_size_) + sizeof(float) + sizeof(float), sizeof(float));
813 : :
814 : 5 : return true;
815 : : }
816 : :
817 : 23 : bool PointCloud::get_value_v3f(Vector3f& v3f, size_t loop_index) const noexcept {
818 [ + + ]: 23 : if (extent_ != 0) {
819 : 16 : return get_value_v3f(v3f.x, v3f.y, v3f.z, loop_index);
820 : : }
821 : :
822 [ + - + + : 7 : if VUNLIKELY (!data_ || loop_index >= size_ || pack_size_ < sizeof(float) * 3) {
+ + - + +
+ ]
823 : 2 : return false;
824 : : }
825 : :
826 : 5 : std::memcpy(&v3f, data_ + (loop_index * pack_size_), sizeof(float) * 3);
827 : :
828 : 5 : return true;
829 : : }
830 : :
831 : 20 : PointCloud::Vector3f PointCloud::get_value_v3f(size_t loop_index) const noexcept {
832 : 20 : PointCloud::Vector3f v3f;
833 : :
834 : 20 : get_value_v3f(v3f, loop_index);
835 : :
836 : 20 : return v3f;
837 : : }
838 : :
839 : 8 : bool PointCloud::get_value_v3d(double& x, double& y, double& z, size_t loop_index) const noexcept {
840 [ + + ]: 8 : if (extent_ != 0) {
841 [ + - + + : 4 : if VUNLIKELY (!data_ || loop_index >= size_ || pack_size_ < sizeof(int16_t) * 3) {
+ + - + +
+ ]
842 : 2 : return false;
843 : : }
844 : :
845 : 2 : const uint8_t* base = data_ + (loop_index * pack_size_);
846 : :
847 : 2 : int16_t qx = 0;
848 : 2 : int16_t qy = 0;
849 : 2 : int16_t qz = 0;
850 : :
851 : 2 : std::memcpy(&qx, base, sizeof(int16_t));
852 : 2 : std::memcpy(&qy, base + sizeof(int16_t), sizeof(int16_t));
853 : 2 : std::memcpy(&qz, base + (sizeof(int16_t) * 2), sizeof(int16_t));
854 : :
855 : 2 : x = Quantize::decode<double>(extent_, qx);
856 : 2 : y = Quantize::decode<double>(extent_, qy);
857 : 2 : z = Quantize::decode<double>(extent_, qz);
858 : :
859 : 2 : return true;
860 : : }
861 : :
862 [ + - + + : 4 : if VUNLIKELY (!data_ || loop_index >= size_ || pack_size_ < sizeof(double) * 3) {
+ + - + +
+ ]
863 : 2 : return false;
864 : : }
865 : :
866 : 2 : std::memcpy(&x, data_ + (loop_index * pack_size_), sizeof(double));
867 : 2 : std::memcpy(&y, data_ + (loop_index * pack_size_) + sizeof(double), sizeof(double));
868 : 2 : std::memcpy(&z, data_ + (loop_index * pack_size_) + sizeof(double) + sizeof(double), sizeof(double));
869 : :
870 : 2 : return true;
871 : : }
872 : :
873 : 5 : bool PointCloud::get_value_v3d(Vector3d& v3d, size_t loop_index) const noexcept {
874 [ + + ]: 5 : if (extent_ != 0) {
875 : 1 : return get_value_v3d(v3d.x, v3d.y, v3d.z, loop_index);
876 : : }
877 : :
878 [ + - + + : 4 : if VUNLIKELY (!data_ || loop_index >= size_ || pack_size_ < sizeof(double) * 3) {
+ + - + +
+ ]
879 : 2 : return false;
880 : : }
881 : :
882 : 2 : std::memcpy(&v3d, data_ + (loop_index * pack_size_), sizeof(double) * 3);
883 : :
884 : 2 : return true;
885 : : }
886 : :
887 : 1 : PointCloud::Vector3d PointCloud::get_value_v3d(size_t loop_index) const noexcept {
888 : 1 : PointCloud::Vector3d v3d;
889 : :
890 : 1 : get_value_v3d(v3d, loop_index);
891 : :
892 : 1 : return v3d;
893 : : }
894 : :
895 : 16 : double PointCloud::get_value_for_double_float(size_t loop_index, uint16_t offset, uint8_t type) const noexcept {
896 [ + + + + : 16 : switch (type) {
+ + + + +
+ + + ]
897 : 1 : case kBoolType:
898 : 1 : return get_value<uint8_t>(loop_index, offset);
899 : 1 : case kInt8Type:
900 : 1 : return get_value<int8_t>(loop_index, offset);
901 : 2 : case kUint8Type:
902 : 2 : return get_value<uint8_t>(loop_index, offset);
903 : 2 : case kInt16Type:
904 : 2 : return get_value<int16_t>(loop_index, offset);
905 : 1 : case kUint16Type:
906 : 1 : return get_value<uint16_t>(loop_index, offset);
907 : 2 : case kInt32Type:
908 : 2 : return get_value<int32_t>(loop_index, offset);
909 : 1 : case kUint32Type:
910 : 1 : return get_value<uint32_t>(loop_index, offset);
911 : 1 : case kInt64Type:
912 : 1 : return get_value<int64_t>(loop_index, offset);
913 : 1 : case kUint64Type:
914 : 1 : return get_value<uint64_t>(loop_index, offset);
915 : 1 : case kFloatType:
916 : 1 : return get_value<float>(loop_index, offset);
917 : 2 : case kDoubleType:
918 : 2 : return get_value<double>(loop_index, offset);
919 : 1 : default:
920 : 1 : return 0;
921 : : }
922 : : }
923 : :
924 : 13 : double PointCloud::get_value_for_double_float(size_t loop_index, KeyMap& key_map, std::string_view key,
925 : : uint8_t type) const noexcept {
926 [ + + + + : 13 : switch (type) {
+ + + + +
+ + + ]
927 : 1 : case kBoolType:
928 : 1 : return get_value<uint8_t>(loop_index, key_map, key);
929 : 1 : case kInt8Type:
930 : 1 : return get_value<int8_t>(loop_index, key_map, key);
931 : 1 : case kUint8Type:
932 : 1 : return get_value<uint8_t>(loop_index, key_map, key);
933 : 1 : case kInt16Type:
934 : 1 : return get_value<int16_t>(loop_index, key_map, key);
935 : 1 : case kUint16Type:
936 : 1 : return get_value<uint16_t>(loop_index, key_map, key);
937 : 1 : case kInt32Type:
938 : 1 : return get_value<int32_t>(loop_index, key_map, key);
939 : 1 : case kUint32Type:
940 : 1 : return get_value<uint32_t>(loop_index, key_map, key);
941 : 1 : case kInt64Type:
942 : 1 : return get_value<int64_t>(loop_index, key_map, key);
943 : 1 : case kUint64Type:
944 : 1 : return get_value<uint64_t>(loop_index, key_map, key);
945 : 2 : case kFloatType:
946 : 2 : return get_value<float>(loop_index, key_map, key);
947 : 1 : case kDoubleType:
948 : 1 : return get_value<double>(loop_index, key_map, key);
949 : 1 : default:
950 : 1 : return 0;
951 : : }
952 : : }
953 : :
954 : 17 : std::string PointCloud::get_value_for_print(size_t loop_index, uint16_t offset, uint8_t type) const noexcept {
955 [ + + + + : 17 : switch (type) {
+ + + + +
+ + + ]
956 : 2 : case kBoolType:
957 [ + + ]: 2 : return get_value<bool>(loop_index, offset) ? "true" : "false";
958 : 1 : case kInt8Type:
959 : 1 : return std::to_string(get_value<int8_t>(loop_index, offset));
960 : 2 : case kUint8Type:
961 : 2 : return std::to_string(get_value<uint8_t>(loop_index, offset));
962 : 2 : case kInt16Type:
963 : 2 : return std::to_string(get_value<int16_t>(loop_index, offset));
964 : 1 : case kUint16Type:
965 : 1 : return std::to_string(get_value<uint16_t>(loop_index, offset));
966 : 2 : case kInt32Type:
967 : 2 : return std::to_string(get_value<int32_t>(loop_index, offset));
968 : 1 : case kUint32Type:
969 : 1 : return std::to_string(get_value<uint32_t>(loop_index, offset));
970 : 1 : case kInt64Type:
971 : 1 : return std::to_string(get_value<int64_t>(loop_index, offset));
972 : 1 : case kUint64Type:
973 : 1 : return std::to_string(get_value<uint64_t>(loop_index, offset));
974 : 1 : case kFloatType:
975 : 1 : return std::to_string(get_value<float>(loop_index, offset));
976 : 2 : case kDoubleType:
977 : 2 : return std::to_string(get_value<double>(loop_index, offset));
978 : 1 : default:
979 : 1 : return std::string();
980 : : }
981 : : }
982 : :
983 : 14 : std::string PointCloud::get_value_for_print(size_t loop_index, KeyMap& key_map, std::string_view key,
984 : : uint8_t type) const noexcept {
985 [ + + + + : 14 : switch (type) {
+ + + + +
+ + + ]
986 : 2 : case kBoolType:
987 [ + + ]: 2 : return get_value<bool>(loop_index, key_map, key) ? "true" : "false";
988 : 1 : case kInt8Type:
989 : 1 : return std::to_string(get_value<int8_t>(loop_index, key_map, key));
990 : 1 : case kUint8Type:
991 : 1 : return std::to_string(get_value<uint8_t>(loop_index, key_map, key));
992 : 1 : case kInt16Type:
993 : 1 : return std::to_string(get_value<int16_t>(loop_index, key_map, key));
994 : 1 : case kUint16Type:
995 : 1 : return std::to_string(get_value<uint16_t>(loop_index, key_map, key));
996 : 1 : case kInt32Type:
997 : 1 : return std::to_string(get_value<int32_t>(loop_index, key_map, key));
998 : 1 : case kUint32Type:
999 : 1 : return std::to_string(get_value<uint32_t>(loop_index, key_map, key));
1000 : 1 : case kInt64Type:
1001 : 1 : return std::to_string(get_value<int64_t>(loop_index, key_map, key));
1002 : 1 : case kUint64Type:
1003 : 1 : return std::to_string(get_value<uint64_t>(loop_index, key_map, key));
1004 : 2 : case kFloatType:
1005 : 2 : return std::to_string(get_value<float>(loop_index, key_map, key));
1006 : 1 : case kDoubleType:
1007 : 1 : return std::to_string(get_value<double>(loop_index, key_map, key));
1008 : 1 : default:
1009 : 1 : return std::string();
1010 : : }
1011 : : }
1012 : :
1013 : 28 : bool PointCloud::compress_protocol_xyz() noexcept {
1014 : 28 : uint16_t num_fields = 0;
1015 : 28 : uint64_t temp_size_num = protocol_.size_num;
1016 : :
1017 : : do {
1018 : 90 : ++num_fields;
1019 : 90 : temp_size_num >>= 4;
1020 [ + + ]: 90 : } while (temp_size_num != 0);
1021 : :
1022 [ - + ]: 28 : if VUNLIKELY (num_fields < 3) {
1023 : : return false; // LCOV_EXCL_LINE GCOVR_EXCL_LINE
1024 : : }
1025 : :
1026 [ + + ]: 109 : for (uint16_t i = 0; i < 3; ++i) {
1027 : 82 : uint64_t shift = static_cast<uint64_t>(num_fields - 1 - i) * 4;
1028 : 82 : uint8_t cur_type = (protocol_.type_num >> shift) & 0xF;
1029 : :
1030 [ + + + + : 82 : if VUNLIKELY (cur_type != kFloatType && cur_type != kDoubleType && cur_type != kInt16Type) {
+ + + - +
+ ]
1031 : 1 : return false;
1032 : : }
1033 : : }
1034 : :
1035 [ + + ]: 108 : for (uint16_t i = 0; i < 3; ++i) {
1036 : 81 : uint64_t shift = static_cast<uint64_t>(num_fields - 1 - i) * 4;
1037 : :
1038 : 81 : protocol_.size_num &= ~(static_cast<uint64_t>(0xF) << shift);
1039 : 81 : protocol_.size_num |= (static_cast<uint64_t>(sizeof(int16_t)) << shift);
1040 : 81 : protocol_.type_num &= ~(static_cast<uint64_t>(0xF) << shift);
1041 : 81 : protocol_.type_num |= (static_cast<uint64_t>(kInt16Type) << shift);
1042 : : }
1043 : :
1044 : 27 : return true;
1045 : : }
1046 : :
1047 : 12 : bool PointCloud::create(size_t size, uint64_t size_num, uint64_t type_num, std::string_view key_str, uint16_t extent,
1048 : : bool vertical) noexcept {
1049 [ + + ]: 12 : if VUNLIKELY (!Protocol::check_valid(size_num, key_str)) {
1050 : 4 : return false;
1051 : : }
1052 : :
1053 : 8 : Protocol new_protocol{};
1054 : 8 : new_protocol.size_num = size_num;
1055 : 8 : std::memset(new_protocol.names, 0, sizeof(new_protocol.names));
1056 : 8 : std::memcpy(new_protocol.names, key_str.data(), key_str.size());
1057 : 8 : new_protocol.type_num = type_num;
1058 : :
1059 [ + + ]: 8 : if (extent != 0) {
1060 : 1 : PointCloud protocol_probe;
1061 : 1 : protocol_probe.protocol_ = new_protocol;
1062 : :
1063 [ - + ]: 1 : if VUNLIKELY (!protocol_probe.compress_protocol_xyz()) {
1064 : : return false; // LCOV_EXCL_LINE GCOVR_EXCL_LINE
1065 : : }
1066 : :
1067 : 1 : new_protocol = protocol_probe.protocol_;
1068 [ + - ]: 1 : }
1069 : :
1070 : 8 : const size_t new_pack_size = new_protocol.get_pack_size();
1071 : :
1072 [ + - - + : 8 : if VUNLIKELY (new_pack_size != 0 && size > std::numeric_limits<size_t>::max() / new_pack_size) {
- + ]
1073 : 0 : return false;
1074 : : }
1075 : :
1076 [ - + - - : 8 : if (is_owner_ && data_ && capacity_ != 0) {
- - ]
1077 : 0 : Bytes::bytes_free(data_, capacity_);
1078 : : }
1079 : :
1080 : 8 : data_ = nullptr;
1081 : 8 : is_owner_ = false;
1082 : 8 : capacity_ = 0;
1083 : 8 : size_ = 0;
1084 : 8 : index_ = 0;
1085 : :
1086 : 8 : protocol_ = new_protocol;
1087 : 8 : extent_ = extent;
1088 : 8 : vertical_ = vertical;
1089 : 8 : downsample_ = 0;
1090 : :
1091 : 8 : pack_size_ = new_pack_size;
1092 : 8 : capacity_ = size * pack_size_;
1093 : :
1094 [ + + ]: 8 : if VLIKELY (capacity_ != 0) {
1095 : 7 : data_ = Bytes::bytes_malloc(capacity_);
1096 : :
1097 [ - + ]: 7 : if VUNLIKELY (!data_) {
1098 : 0 : capacity_ = 0;
1099 : 0 : return false;
1100 : : }
1101 : :
1102 : 7 : is_owner_ = true;
1103 : : }
1104 : :
1105 : 8 : return true;
1106 : : }
1107 : :
1108 : 6 : void PointCloud::clear(bool force) noexcept {
1109 [ + + ]: 6 : if (force) {
1110 [ + + + - : 5 : if (is_owner_ && data_ && capacity_ != 0) {
+ - ]
1111 : 1 : Bytes::bytes_free(data_, capacity_);
1112 : : }
1113 : :
1114 : 5 : pack_size_ = 0;
1115 : 5 : capacity_ = 0;
1116 : 5 : data_ = nullptr;
1117 : 5 : reserved_buf_ = 0;
1118 : 5 : reserved_buf2_ = 0;
1119 : 5 : reserved_buf3_ = 0;
1120 : 5 : extent_ = 0;
1121 : 5 : vertical_ = false;
1122 : 5 : is_owner_ = false;
1123 : :
1124 : 5 : protocol_.size_num = 0;
1125 : 5 : std::memset(protocol_.names, 0, sizeof(protocol_.names));
1126 : 5 : protocol_.type_num = 0;
1127 : :
1128 : : #if defined(__GNUC__) && !defined(__clang__)
1129 : : #pragma GCC diagnostic push
1130 : : #pragma GCC diagnostic ignored "-Wclass-memaccess"
1131 : : #if __GNUC__ >= 11
1132 : : #pragma GCC diagnostic ignored "-Wstringop-overread"
1133 : : #endif
1134 : : #endif
1135 : :
1136 : 5 : std::memset(&header, 0, sizeof(header));
1137 : :
1138 : : #if defined(__GNUC__) && !defined(__clang__)
1139 : : #pragma GCC diagnostic pop
1140 : : #endif
1141 : : }
1142 : :
1143 : 6 : size_ = 0;
1144 : 6 : index_ = 0;
1145 : 6 : downsample_ = 0;
1146 : 6 : }
1147 : :
1148 : : // PointCloud::Protocol
1149 : 12 : bool PointCloud::Protocol::check_valid(uint64_t _size_num, std::string_view _names) noexcept {
1150 [ + + - + : 12 : if VUNLIKELY (_size_num == 0 || _names.empty() || _names.size() > sizeof(names)) {
+ + + + +
+ ]
1151 : 2 : return false;
1152 : : }
1153 : :
1154 : 10 : uint16_t num_count = 0;
1155 : 10 : uint16_t key_count = 0;
1156 : :
1157 : : do {
1158 : 45 : num_count++;
1159 : 45 : _size_num >>= 4;
1160 [ + + ]: 45 : } while (_size_num != 0);
1161 : :
1162 [ + + ]: 263 : for (auto c : _names) {
1163 [ + + ]: 253 : if (c == ',') {
1164 : 35 : ++key_count;
1165 : : }
1166 : : }
1167 : :
1168 [ + - ]: 10 : if (!_names.empty()) {
1169 : 10 : ++key_count;
1170 : : }
1171 : :
1172 [ + + - + : 10 : if VUNLIKELY (key_count != num_count || key_count < 3 || key_count > 16) {
+ + - + +
+ ]
1173 : 2 : return false;
1174 : : }
1175 : :
1176 : 8 : return true;
1177 : : }
1178 : :
1179 : 131 : std::string PointCloud::Protocol::get_names(const std::vector<std::string>& keys) noexcept {
1180 : 131 : std::string key_str;
1181 : :
1182 [ + + ]: 581 : for (const auto& key : keys) {
1183 [ + + ]: 450 : if (!key_str.empty()) {
1184 : 319 : key_str += ',';
1185 : : }
1186 : :
1187 : 450 : key_str += key;
1188 : : }
1189 : :
1190 [ - + ]: 131 : if VUNLIKELY (key_str.size() > sizeof(names)) {
1191 : 0 : return std::string();
1192 : : }
1193 : :
1194 : 131 : return key_str;
1195 : 131 : }
1196 : :
1197 : 29 : PointCloud::KeyList PointCloud::Protocol::get_key_list() const noexcept {
1198 : 29 : KeyList key_list;
1199 : 29 : key_list.reserve(16);
1200 : :
1201 : 29 : uint64_t temp_size_num = size_num;
1202 : 29 : uint64_t temp_type_num = type_num;
1203 : :
1204 : 58 : std::istringstream iss(std::string(names, ::strnlen(names, sizeof(names))));
1205 : 29 : std::string token;
1206 : :
1207 : 29 : bool leading_zero = true;
1208 : :
1209 [ + + ]: 491 : for (int i = 15; i >= 0; --i) {
1210 : 463 : uint8_t size_nibble = (temp_size_num >> (i * 4)) & 0xF;
1211 : 463 : uint8_t type_nibble = (temp_type_num >> (i * 4)) & 0xF;
1212 : :
1213 [ + + + + ]: 463 : if (leading_zero && size_nibble == 0) {
1214 : 339 : continue;
1215 : : }
1216 : :
1217 : 124 : leading_zero = false;
1218 : :
1219 [ + + ]: 124 : if VUNLIKELY (!std::getline(iss, token, ',')) {
1220 : 1 : break;
1221 : : }
1222 : :
1223 : 123 : Key key;
1224 : 123 : key.size = size_nibble;
1225 : 123 : key.type = type_nibble;
1226 : 123 : key.name = token;
1227 : :
1228 : 123 : key_list.emplace_back(std::move(key));
1229 : 123 : }
1230 : :
1231 : 29 : return key_list;
1232 : 29 : }
1233 : :
1234 : 1 : std::string PointCloud::Protocol::get_size_for_print() const noexcept {
1235 : 1 : bool leading_zero = true;
1236 : :
1237 : 1 : uint64_t temp_size_num = size_num;
1238 : :
1239 : 1 : std::string print_str;
1240 : :
1241 [ + + ]: 17 : for (int i = 15; i >= 0; --i) {
1242 : 16 : uint8_t size_nibble = (temp_size_num >> (i * 4)) & 0xF;
1243 : :
1244 [ + + + + ]: 16 : if (leading_zero && size_nibble == 0) {
1245 : 12 : continue;
1246 : : }
1247 : :
1248 : 4 : leading_zero = false;
1249 : :
1250 [ + + ]: 4 : if (!print_str.empty()) {
1251 : 3 : print_str += ",";
1252 : : }
1253 : :
1254 : 4 : print_str += std::to_string(size_nibble);
1255 : : }
1256 : :
1257 : 1 : return print_str;
1258 : : }
1259 : :
1260 : 3 : std::string PointCloud::Protocol::get_type_for_print() const noexcept {
1261 : 3 : bool leading_zero = true;
1262 : :
1263 : 3 : uint64_t temp_type_num = type_num;
1264 : :
1265 : 3 : std::string print_str;
1266 : :
1267 [ + + ]: 51 : for (int i = 15; i >= 0; --i) {
1268 : 48 : uint8_t type_nibble = (temp_type_num >> (i * 4)) & 0xF;
1269 : :
1270 [ + + + + ]: 48 : if (leading_zero && type_nibble == 0) {
1271 : 30 : continue;
1272 : : }
1273 : :
1274 : 18 : leading_zero = false;
1275 : :
1276 [ + + ]: 18 : if (!print_str.empty()) {
1277 : 13 : print_str += ",";
1278 : : }
1279 : :
1280 [ + + + + : 18 : switch (type_nibble) {
+ + + + +
+ + + ]
1281 : 1 : case kBoolType:
1282 : 1 : print_str += "bool";
1283 : 1 : break;
1284 : 1 : case kInt8Type:
1285 : 1 : print_str += "int8";
1286 : 1 : break;
1287 : 1 : case kUint8Type:
1288 : 1 : print_str += "uint8";
1289 : 1 : break;
1290 : 1 : case kInt16Type:
1291 : 1 : print_str += "int16";
1292 : 1 : break;
1293 : 1 : case kUint16Type:
1294 : 1 : print_str += "uint16";
1295 : 1 : break;
1296 : 1 : case kInt32Type:
1297 : 1 : print_str += "int32";
1298 : 1 : break;
1299 : 1 : case kUint32Type:
1300 : 1 : print_str += "uint32";
1301 : 1 : break;
1302 : 1 : case kInt64Type:
1303 : 1 : print_str += "int64";
1304 : 1 : break;
1305 : 1 : case kUint64Type:
1306 : 1 : print_str += "uint64";
1307 : 1 : break;
1308 : 5 : case kFloatType:
1309 : 5 : print_str += "float";
1310 : 5 : break;
1311 : 1 : case kDoubleType:
1312 : 1 : print_str += "double";
1313 : 1 : break;
1314 : 3 : default:
1315 : 3 : break;
1316 : : }
1317 : : }
1318 : :
1319 : 3 : return print_str;
1320 : : }
1321 : :
1322 : : } // namespace zerocopy
1323 : :
1324 : : } // namespace vlink
|
