diff --git a/util/grid/grid33.hpp b/util/grid/grid33.hpp index 680c621..523d857 100644 --- a/util/grid/grid33.hpp +++ b/util/grid/grid33.hpp @@ -222,16 +222,15 @@ public: mWords[n >> 6] &= ~(uint64_t(1) << (n & 63)); } - template void set(uint32_t n, bool On) { - if constexpr (UseBranchless) { +#if 1 auto &word = mWords[n >> 6]; n &= 63; word &= ~(uint64_t(1) << n); word |= uint64_t(On) << n; - } else { +#else On ? this->setOn(n) : this->setOff(n); - } +#endif } void setOn() { @@ -392,7 +391,7 @@ public: const uint32_t inner_index = getInnerIndex(coord); auto& inner_data = inner_ptr->data[inner_index]; - if (inner_ptr->mask.setOn(inner_index)) { + if (inner_ptr->mask.setOn(inner_index) == false) { inner_data = std::make_shared(); } diff --git a/util/grid/treexy/treexy_serialization.hpp b/util/grid/treexy/treexy_serialization.hpp new file mode 100644 index 0000000..cfd915b --- /dev/null +++ b/util/grid/treexy/treexy_serialization.hpp @@ -0,0 +1,229 @@ +#ifndef SERIALIZATION_HPP +#define SERIALIZATION_HPP + +#include +#include +#include +#include +#include +#include +#include +#include "../grid33.hpp" + +#ifdef __GNUG__ +#include +#include +#include +#endif + +/** + * Serialize a grid to ostream. Easy :) + */ +template +inline void Serialize(std::ostream &out, const VoxelGrid &grid); + +struct HeaderInfo +{ + std::string type_name; + int inner_bits = 0; + int leaf_bits = 0; + double resolution = 0; +}; + +/** + * @brief GetHeaderInfo is used to recover informations from the header of a + * file/stream + * + * @param header first line of the file + */ +inline HeaderInfo GetHeaderInfo(std::string header); + +/** + * @brief Deserialize create a grid. Note that template arguments need to be + * consistent with HeaderInfo + */ +template +inline VoxelGrid Deserialize(std::istream &input, + HeaderInfo info); + +//--------------------------------------------------------- +namespace details +{ +#ifdef __GNUG__ + std::string demangle(const char *name) + { + int status = -4; // some arbitrary value to eliminate the compiler warning + + std::unique_ptr res{ + abi::__cxa_demangle(name, NULL, NULL, &status), std::free}; + return (status == 0) ? res.get() : name; + } + +#else + + // does nothing if not g++ + std::string demangle(const char *name) + { + return name; + } +#endif + +} // namespace details + +template +inline void Write(std::ostream &out, const T &val) +{ + static_assert(std::is_trivially_copyable_v, "Must be trivially copyable"); + out.write(reinterpret_cast(&val), sizeof(T)); +} + +template +inline void Serialize(std::ostream &out, const VoxelGrid &grid) +{ + static_assert(std::is_trivially_copyable_v, + "DataT must ne trivially copyable"); + + char header[256]; + std::string type_name = details::demangle(typeid(DataT).name()); + + sprintf(header, + "Treexy::VoxelGrid<%s,%d,%d>(%lf)\n", + type_name.c_str(), + IBITS, + LBITS, + grid.resolution); + + out.write(header, std::strlen(header)); + + //------------ + Write(out, uint32_t(grid.root_map.size())); + + for (const auto &it : grid.root_map) + { + const CoordT &root_coord = it.first; + Write(out, root_coord.x); + Write(out, root_coord.y); + Write(out, root_coord.z); + + const auto &inner_grid = it.second; + for (size_t w = 0; w < inner_grid.mask.wordCount(); w++) + { + Write(out, inner_grid.mask.getWord(w)); + } + for (auto inner = inner_grid.mask.beginOn(); inner; ++inner) + { + const uint32_t inner_index = *inner; + const auto &leaf_grid = *(inner_grid.data[inner_index]); + + for (size_t w = 0; w < leaf_grid.mask.wordCount(); w++) + { + Write(out, leaf_grid.mask.getWord(w)); + } + for (auto leaf = leaf_grid.mask.beginOn(); leaf; ++leaf) + { + const uint32_t leaf_index = *leaf; + Write(out, leaf_grid.data[leaf_index]); + } + } + } +} + +template +inline T Read(std::istream &input) +{ + T out; + static_assert(std::is_trivially_copyable_v, "Must be trivially copyable"); + input.read(reinterpret_cast(&out), sizeof(T)); + return out; +} + +inline HeaderInfo GetHeaderInfo(std::string header) +{ + const std::string expected_prefix = "Treexy::VoxelGrid<"; + if (header.rfind(expected_prefix, 0) != 0) + { + throw std::runtime_error("Header wasn't recognized"); + } + int p1 = header.find(",", 18) + 1; + auto part_type = header.substr(18, p1 - 18 - 1); + + int p2 = header.find(",", p1 + 1) + 1; + auto part_ibits = header.substr(p1, p2 - p1 - 1); + + int p3 = header.find(">", p2) + 1; + auto part_lbits = header.substr(p2, p3 - p2 - 1); + + int p4 = header.find("(", p3) + 1; + int p5 = header.find(")", p4); + auto part_res = header.substr(p4, p5 - p4); + + HeaderInfo info; + info.type_name = part_type; + info.inner_bits = std::stoi(part_ibits); + info.leaf_bits = std::stoi(part_lbits); + info.resolution = std::stod(part_res); + + return info; +} + +template +inline VoxelGrid Deserialize(std::istream &input, + HeaderInfo info) +{ + std::string type_name = details::demangle(typeid(DataT).name()); + if (type_name != info.type_name) + { + throw std::runtime_error("DataT does not match"); + } + if (info.inner_bits != IBITS) + { + throw std::runtime_error("INNER_BITS does not match"); + } + if (info.leaf_bits != LBITS) + { + throw std::runtime_error("LEAF_BITS does not match"); + } + + //------------ + + VoxelGrid grid(info.resolution); + + uint32_t root_count = Read(input); + + for (size_t root_index = 0; root_index < root_count; root_index++) + { + CoordT root_coord; + root_coord.x = Read(input); + root_coord.y = Read(input); + root_coord.z = Read(input); + + auto &inner_grid = grid.root_map[root_coord]; + + for (size_t w = 0; w < inner_grid.mask.wordCount(); w++) + { + uint64_t word = Read(input); + inner_grid.mask.setWord(w, word); + } + for (auto inner = inner_grid.mask.beginOn(); inner; ++inner) + { + const uint32_t inner_index = *inner; + using LeafGridT = typename VoxelGrid::LeafGrid; + inner_grid.data[inner_index] = std::make_shared(); + auto &leaf_grid = inner_grid.data[inner_index]; + + for (size_t w = 0; w < leaf_grid->mask.wordCount(); w++) + { + uint64_t word = Read(input); + leaf_grid->mask.setWord(w, word); + } + for (auto leaf = leaf_grid->mask.beginOn(); leaf; ++leaf) + { + const uint32_t leaf_index = *leaf; + leaf_grid->data[leaf_index] = Read(input); + } + } + } + return grid; +} + +#endif \ No newline at end of file