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