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