Z3D/source/assets/file_parsers/generic/generic_3dtk_loader.ipp

#ifndef INCLUDE_GENERIC_3DTK_LOADER_IMPLEMENTATION
#	error Never include this file directly include 'generic_3dtk_loader.hpp'
#endif

#include <charconv>
#include <fstream>
#include "util/logger.hpp"
#include "glm/glm.hpp"
#include "glm/gtx/euler_angles.hpp"
#include <charconv>
#include <fstream>
#include <execution>
#include <queue>


template<bool Normal, bool Color, bool Reflectance>
std::error_code assets::detail::generic_3dtk_loader<Normal, Color, Reflectance>::prefetch(
	path_id_lookups& lookups
) {
	m_path_buffer.clear();
	lookups.point_clouds.by_extension(".3d", m_path_buffer);

	auto path_buffer = std::filesystem::path{};

	for (const auto entry : m_path_buffer)
	{
		path_buffer = entry->first;
		path_buffer.replace_extension(".pose");
		lookups.poses.try_emplace(path_buffer);
	}

	return {};
}

template<bool Normal, bool Color, bool Reflectance>
std::error_code assets::detail::generic_3dtk_loader<Normal, Color, Reflectance>::load(
	path_id_lookups& lookups,
	data_stores& stores,
	bool pedantic
) {
	m_path_buffer.clear();
	lookups.point_clouds.by_extension(".3d", m_path_buffer);

	std::for_each(
		std::execution::parallel_unsequenced_policy{},
		m_path_buffer.begin(),
		m_path_buffer.end(),
		parser_context{
			lookups,
			stores,
		}
	);

	return {};
}

template<bool Normal, bool Color, bool Reflectance>
assets::detail::generic_3dtk_loader<Normal, Color, Reflectance>::parser_context::parser_context(
	path_id_lookups& m_id_lookups,
	data_stores& m_stores
) :
	m_id_lookups{ &m_id_lookups },
	m_stores{ &m_stores }
{
	constexpr auto expected_vertex_count = 8192;
	m_point_cloud.positions().reserve(expected_vertex_count);
	m_point_cloud.normals().reserve(expected_vertex_count);
	m_point_cloud.colors().reserve(expected_vertex_count);
	m_point_cloud.reflectances().reserve(expected_vertex_count);
}

template<bool Normal, bool Color, bool Reflectance>
void assets::detail::generic_3dtk_loader<Normal, Color, Reflectance>::parser_context::reset()
{
	m_point_cloud.clear();
}

template<bool Normal, bool Color, bool Reflectance>
void assets::detail::generic_3dtk_loader<Normal, Color, Reflectance>::parser_context::operator()(lookup_type::const_pointer entry) noexcept
{
	const auto& [ filename, id ] = *entry;

	std::size_t pose_index;

	if (auto res = parse_scan_index(filename))
	{
		pose_index = *res;
	}
	else [[unlikely]]
	{
		const auto error = res.error();
		ztu::logger::error(
			"Error occurred while parsing scan index in filename %: [%] %",
			error.category().name(),
			error.message()
		);
		return;
	}

	auto pose_path = entry->first;
	pose_path.replace_extension(".pose");

	if (pose_path != m_last_pose_path)
	{
		if (const auto pose_list_id_it = m_id_lookups->pose_lists.find(pose_path); pose_list_id_it !=  m_id_lookups->pose_lists.end())
		{
			m_last_pose_path = pose_path;
			const auto pose_list_id = pose_list_id_it->second;
			// TODO What if the netry is not there we look it pup again and again
			// TODO Insert maybe a bool was_valid

			if (const auto [ pose_list_it, found ] = m_id_lookups->poses.find(pose_list_id); found)
			{
				m_last_pose_list = pose_list_it->second;
			}
			else
			{
				m_last_pose_list = {};
				ztu::logger::error("No matching pose found in store for %.", pose_path);
				return;
			}
		}
		else
		{
			ztu::logger::error("No matching pose registered in lookup %.", pose_path);
			return;
		}
	}
	else if (m_last_pose_list.data() == nullptr)
	{
		ztu::logger::error("No matching pose found in store for %.", pose_path);
		return;
	}

	reset();

	pose_data pose;

	if (pose_index < m_last_pose_list.size())
	{
		pose = m_last_pose_list[pose_index];
	}
	else
	{
		ztu::logger::error(
			"Pose index % of 3dtk file % is out of range for its pose file %. Proceeding with identity pose.",
			pose_index,
			filename,
			m_last_pose_path
		);
		pose = glm::identity<pose_data>();
	}


	if (const auto e = parse_file(filename, m_point_cloud))
	{
		ztu::logger::error("Could not parse 3dtk file %: %", filename, e.message());
		return;
	}

	transform_point_cloud(m_point_cloud.positions(), pose);

	m_stores->point_clouds.insert(id, m_point_cloud);
}

template<bool Normal, bool Color, bool Reflectance>
ztu::result<std::size_t> assets::detail::generic_3dtk_loader<Normal, Color, Reflectance>::parse_scan_index(
	const std::string_view filename
) {
	static constexpr auto prefix = std::string_view{ "scan" };

	auto name_view = filename.substr(0, name_view.find('.'));

	if (name_view.length() <= prefix.length()) [[unlikely]]
	{
		return std::unexpected{ std::make_error_code(std::errc::invalid_argument) };
	}

	name_view = name_view.substr(prefix.length());

	std::size_t index;

	const auto res = std::from_chars(name_view.begin(), name_view.end(), index);

	if (res.ec != std::errc{}) [[unlikely]]
	{
		return std::unexpected{ std::make_error_code(res.ec) };
	}

	return index;
}

template<int L>
std::error_code read_vector(std::string_view& line, z3d::vec<L, float>& vec) {
	for (int i{}; i != L; ++i)
	{
		auto it = line.begin();

		const auto [minus, plus] = std::pair{ *it == '-', *it == '+' };
		it += plus or minus ? 3 : 2; // skip '[-+]?0x'

		const auto [ ptr, ec ] = std::from_chars(
			it,  line.end(),
			vec[i],
			std::chars_format::hex
		);

		if (ec != std::errc{})
		{
			return std::make_error_code(ec);
		}

		if (minus)
		{
			vec[i] *= -1.0;
		}

		line = { ptr + sizeof(' '), line.end() };
	}

	return {};
}

template<bool Normal, bool Color, bool Reflectance>
std::error_code assets::detail::generic_3dtk_loader<Normal, Color, Reflectance>::parse_file(
	const std::filesystem::path& filename,
	data_type& point_cloud
) {
	std::error_code error;

	auto in = std::ifstream(filename);

	if (not in.is_open())
	{
		return std::make_error_code(static_cast<std::errc>(errno));
	}

	std::string line;
	if (not std::getline(in, line))
	{
		return std::make_error_code(std::errc::invalid_seek);
	}

	constexpr auto expected_component_count = []()
	{
		auto count = std::tuple_size_v<point_cloud_vertex_components::position>;

		if (Normal)
		{
			count += std::tuple_size_v<point_cloud_vertex_components::normal>;
		}

		if (Color)
		{
			count += std::tuple_size_v<point_cloud_vertex_components::color>;
		}

		if (Reflectance)
		{
			count += std::tuple_size_v<point_cloud_vertex_components::reflectance>;
		}

		return count;
	}();


	auto& positions = point_cloud.positions();
	auto& normals = point_cloud.normals();
	auto& colors = point_cloud.colors();
	auto& reflectances = point_cloud.reflectances();

	do
	{
		auto line_view = std::string_view{ line };

		point_cloud_vertex_components::position position;
		if ((error = read_vector(line_view, position)))
		{
			return error;
		}
		positions.push_back(position);

		if constexpr (Normal)
		{
			point_cloud_vertex_components::normal normal;
			if ((error = read_vector(line_view, normal)))
			{
				return error;
			}
			normals.push_back(normal);
		}

		if constexpr (Color)
		{
			point_cloud_vertex_components::color color;
			if ((error = read_vector(line_view, color)))
			{
				return error;
			}
			colors.push_back(color);
		}

		if constexpr (Reflectance)
		{
			z3d::vec<1, point_cloud_vertex_components::reflectance> reflectance;
			if ((error = read_vector(line_view, reflectance)))
			{
				return error;
			}
			reflectances.push_back(reflectance[0]);
		}
	}
	while (std::getline(in, line));

	return {};
}

template<bool Normal, bool Color, bool Reflectance>
void assets::detail::generic_3dtk_loader<Normal, Color, Reflectance>::transform_point_cloud(
	std::span<point_cloud_vertex_components::position> points,
	const pose_data& pose
) {
	for (auto& point : points)
	{
		point = pose * glm::vec4{ point, 1.0f };
	}
}