<commit_before>#include <filesystem>
#include <fstream>

#include "EntityManager.hpp"

#include "RecastSystem.hpp"
#include "RecastComponent.hpp"
#include "RecastDebugShader.hpp"

#include "data/AdjustableComponent.hpp"
#include "data/ModelComponent.hpp"
#include "data/ModelDataComponent.hpp"
#include "data/NavMeshComponent.hpp"
#include "data/ShaderComponent.hpp"
#include "functions/Execute.hpp"

#include "helpers/assertHelper.hpp"
#include "systems/opengl/shaders/shaderHelper.hpp"

#include "angle.hpp"
#include "with.hpp"

namespace Flags {
	enum {
		Walk = 1,
	};
}

namespace kengine {
	static EntityManager * g_em;

	// declarations
	static void execute(float deltaTime);
	//
	EntityCreator * RecastSystem(EntityManager & em) {
		g_em = &em;

		return [](Entity & e) {
			e += functions::Execute{ execute };
			e += makeGBufferShaderComponent<RecastDebugShader>(*g_em);
		};
	}

	// declarations
	static void buildNavMeshes();
	//
	static void execute(float deltaTime) {
		buildNavMeshes();
	}

	// declarations
	static void createRecastMesh(const char * file, Entity & model, NavMeshComponent & navMesh, const ModelDataComponent & modelData);
	//
	static void buildNavMeshes() {
		static const auto buildRecastComponent = [](auto && entities) {
			for (auto & [e, model, modelData, navMesh, _] : entities) {
				g_em->runTask([&] {
					kengine_assert(*g_em, navMesh.vertsPerPoly <= DT_VERTS_PER_POLYGON);
					createRecastMesh(model.file, e, navMesh, modelData);
					if constexpr (std::is_same<RebuildNavMeshComponent, putils_typeof(_)>())
						e.detach<RebuildNavMeshComponent>();
				});
			}
			g_em->completeTasks();
		};

		buildRecastComponent(g_em->getEntities<ModelComponent, ModelDataComponent, NavMeshComponent, no<RecastComponent>>());
		buildRecastComponent(g_em->getEntities<ModelComponent, ModelDataComponent, NavMeshComponent, RebuildNavMeshComponent>());
	}

	// declarations
	using HeightfieldPtr = UniquePtr<rcHeightfield, rcFreeHeightField>;
	using CompactHeightfieldPtr = UniquePtr<rcCompactHeightfield, rcFreeCompactHeightfield>;
	using ContourSetPtr = UniquePtr<rcContourSet, rcFreeContourSet>;
	using PolyMeshPtr = UniquePtr<rcPolyMesh, rcFreePolyMesh>;
	using PolyMeshDetailPtr = UniquePtr<rcPolyMeshDetail, rcFreePolyMeshDetail>;

	struct NavMeshData {
		unsigned char * data = nullptr;
		int size = 0;
	};

	static NavMeshData loadBinaryFile(const char * binaryFile, const NavMeshComponent & navMesh);
	static void saveBinaryFile(const char * binaryFile, const NavMeshData & data, const NavMeshComponent & navMesh);
	static NavMeshData createNavMeshData(const NavMeshComponent & navMesh, const ModelDataComponent & modelData, const ModelDataComponent::Mesh & meshData);
	static NavMeshPtr createNavMesh(const NavMeshData & data);
	static NavMeshQueryPtr createNavMeshQuery(const NavMeshComponent & params, const dtNavMesh & navMesh);
	//
	static void createRecastMesh(const char * file, Entity & e, NavMeshComponent & navMesh, const ModelDataComponent & modelData) {
		NavMeshData data;

		const putils::string<4096> binaryFile("%s.nav", file);
		bool mustSave = false;
		data = loadBinaryFile(binaryFile, navMesh);
		if (data.data == nullptr) {
			data = createNavMeshData(navMesh, modelData, modelData.meshes[navMesh.concernedMesh]);
			if (data.data == nullptr)
				return;
			mustSave = true;
		}

		auto & recast = e.attach<RecastComponent>();
		recast.navMesh = createNavMesh(data);
		if (recast.navMesh == nullptr) {
			dtFree(data.data);
			return;
		}

		recast.navMeshQuery = createNavMeshQuery(navMesh, *recast.navMesh);
		if (recast.navMeshQuery == nullptr) {
			dtFree(data.data);
			return;
		}

		if (mustSave)
			saveBinaryFile(binaryFile, data, navMesh);

		const auto & model = e.get<ModelComponent>();
		navMesh.getPath = [&](const Entity & e, const putils::Point3f & startWorldSpace, const putils::Point3f & endWorldSpace) {
			const auto mat = shaderHelper::getModelMatrix(model, e.get<TransformComponent>());

			float start[3];
			{
				auto tmp = glm::vec4(shaderHelper::toVec(startWorldSpace), 1.f);
				tmp = glm::inverse(mat) * tmp;
				start[0] = tmp.x;
				start[1] = tmp.y;
				start[2] = tmp.z;
			}

			float end[3];
			{
				auto tmp = glm::vec4(shaderHelper::toVec(endWorldSpace), 1.f);
				tmp = glm::inverse(mat) * tmp;
				end[0] = tmp.x;
				end[1] = tmp.y;
				end[2] = tmp.z;
			}


			NavMeshComponent::Path ret;

			const float extents[3] = { navMesh.characterRadius * 2.f, navMesh.characterHeight, navMesh.characterRadius * 2.f };

			dtPolyRef startRef;
			float startPt[3];
			auto status = recast.navMeshQuery->findNearestPoly(start, extents, nullptr, &startRef, startPt);
			if (dtStatusFailed(status) || startRef == 0) {
				kengine_assert_failed(*g_em, "[Recast] Failed to find nearest poly");
				return ret;
			}

			dtPolyRef endRef;
			float endPt[3];
			status = recast.navMeshQuery->findNearestPoly(end, extents, nullptr, &endRef, endPt);
			if (dtStatusFailed(status) || endRef == 0) {
				kengine_assert_failed(*g_em, "[Recast] Failed to find nearest poly");
				return ret;
			}

			dtPolyRef path[KENGINE_NAVMESH_MAX_PATH_LENGTH];
			int pathCount = 0;
			status = recast.navMeshQuery->findPath(startRef, endRef, startPt, endPt, nullptr, path, &pathCount, lengthof(path));
			if (dtStatusFailed(status)) {
				kengine_assert_failed(*g_em, "[Recast] Failed to find path");
				return ret;
			}

			static_assert(sizeof(putils::Point3f) == sizeof(float[3]));
			ret.resize(ret.capacity());

			int straightPathCount = 0;
			status = recast.navMeshQuery->findStraightPath(startPt, endPt, path, pathCount, ret[0].raw, nullptr, nullptr, &straightPathCount, ret.capacity());
			if (dtStatusFailed(status)) {
				kengine_assert_failed(*g_em, "[Recast] Failed to find straight path");
				return ret;
			}
			ret.resize(straightPathCount);

			return ret;
		};
	}

	static NavMeshData loadBinaryFile(const char * binaryFile, const NavMeshComponent & navMesh) {
		NavMeshData data;

		std::ifstream f(binaryFile, std::ifstream::binary);
		if (!f)
			return data;<commit_msg>fix crash in RecastSystem::getPath + transform its results to worldspace<commit_after>#include <filesystem>
#include <fstream>

#include "EntityManager.hpp"

#include "RecastSystem.hpp"
#include "RecastComponent.hpp"
#include "RecastDebugShader.hpp"

#include "data/AdjustableComponent.hpp"
#include "data/ModelComponent.hpp"
#include "data/ModelDataComponent.hpp"
#include "data/NavMeshComponent.hpp"
#include "data/ShaderComponent.hpp"
#include "functions/Execute.hpp"

#include "helpers/assertHelper.hpp"
#include "systems/opengl/shaders/shaderHelper.hpp"

#include "angle.hpp"
#include "with.hpp"

namespace Flags {
	enum {
		Walk = 1,
	};
}

namespace kengine {
	static EntityManager * g_em;

	// declarations
	static void execute(float deltaTime);
	//
	EntityCreator * RecastSystem(EntityManager & em) {
		g_em = &em;

		return [](Entity & e) {
			e += functions::Execute{ execute };
			e += makeGBufferShaderComponent<RecastDebugShader>(*g_em);
		};
	}

	// declarations
	static void buildNavMeshes();
	//
	static void execute(float deltaTime) {
		buildNavMeshes();
	}

	// declarations
	static void createRecastMesh(const char * file, Entity & model, NavMeshComponent & navMesh, const ModelDataComponent & modelData);
	//
	static void buildNavMeshes() {
		static const auto buildRecastComponent = [](auto && entities) {
			for (auto & [e, model, modelData, navMesh, _] : entities) {
				g_em->runTask([&] {
					kengine_assert(*g_em, navMesh.vertsPerPoly <= DT_VERTS_PER_POLYGON);
					createRecastMesh(model.file, e, navMesh, modelData);
					if constexpr (std::is_same<RebuildNavMeshComponent, putils_typeof(_)>())
						e.detach<RebuildNavMeshComponent>();
				});
			}
			g_em->completeTasks();
		};

		buildRecastComponent(g_em->getEntities<ModelComponent, ModelDataComponent, NavMeshComponent, no<RecastComponent>>());
		buildRecastComponent(g_em->getEntities<ModelComponent, ModelDataComponent, NavMeshComponent, RebuildNavMeshComponent>());
	}

	// declarations
	using HeightfieldPtr = UniquePtr<rcHeightfield, rcFreeHeightField>;
	using CompactHeightfieldPtr = UniquePtr<rcCompactHeightfield, rcFreeCompactHeightfield>;
	using ContourSetPtr = UniquePtr<rcContourSet, rcFreeContourSet>;
	using PolyMeshPtr = UniquePtr<rcPolyMesh, rcFreePolyMesh>;
	using PolyMeshDetailPtr = UniquePtr<rcPolyMeshDetail, rcFreePolyMeshDetail>;

	struct NavMeshData {
		unsigned char * data = nullptr;
		int size = 0;
	};

	static NavMeshData loadBinaryFile(const char * binaryFile, const NavMeshComponent & navMesh);
	static void saveBinaryFile(const char * binaryFile, const NavMeshData & data, const NavMeshComponent & navMesh);
	static NavMeshData createNavMeshData(const NavMeshComponent & navMesh, const ModelDataComponent & modelData, const ModelDataComponent::Mesh & meshData);
	static NavMeshPtr createNavMesh(const NavMeshData & data);
	static NavMeshQueryPtr createNavMeshQuery(const NavMeshComponent & params, const dtNavMesh & navMesh);
	//
	static void createRecastMesh(const char * file, Entity & e, NavMeshComponent & navMesh, const ModelDataComponent & modelData) {
		NavMeshData data;

		const putils::string<4096> binaryFile("%s.nav", file);
		bool mustSave = false;
		data = loadBinaryFile(binaryFile, navMesh);
		if (data.data == nullptr) {
			data = createNavMeshData(navMesh, modelData, modelData.meshes[navMesh.concernedMesh]);
			if (data.data == nullptr)
				return;
			mustSave = true;
		}

		auto & recast = e.attach<RecastComponent>();
		recast.navMesh = createNavMesh(data);
		if (recast.navMesh == nullptr) {
			dtFree(data.data);
			return;
		}

		recast.navMeshQuery = createNavMeshQuery(navMesh, *recast.navMesh);
		if (recast.navMeshQuery == nullptr) {
			dtFree(data.data);
			return;
		}

		if (mustSave)
			saveBinaryFile(binaryFile, data, navMesh);

		const auto & model = e.get<ModelComponent>();
		navMesh.getPath = [&](const Entity & e, const putils::Point3f & startWorldSpace, const putils::Point3f & endWorldSpace) {
			static const dtQueryFilter filter;

			const auto mat = shaderHelper::getModelMatrix(model, e.get<TransformComponent>());

			float start[3];
			{
				auto tmp = glm::vec4(shaderHelper::toVec(startWorldSpace), 1.f);
				tmp = glm::inverse(mat) * tmp;
				start[0] = tmp.x;
				start[1] = tmp.y;
				start[2] = tmp.z;
			}

			float end[3];
			{
				auto tmp = glm::vec4(shaderHelper::toVec(endWorldSpace), 1.f);
				tmp = glm::inverse(mat) * tmp;
				end[0] = tmp.x;
				end[1] = tmp.y;
				end[2] = tmp.z;
			}


			NavMeshComponent::Path ret;

			const float extents[3] = { navMesh.characterRadius * 2.f, navMesh.characterHeight, navMesh.characterRadius * 2.f };

			dtPolyRef startRef;
			float startPt[3];
			auto status = recast.navMeshQuery->findNearestPoly(start, extents, &filter, &startRef, startPt);
			if (dtStatusFailed(status) || startRef == 0) {
				kengine_assert_failed(*g_em, "[Recast] Failed to find nearest poly");
				return ret;
			}

			dtPolyRef endRef;
			float endPt[3];
			status = recast.navMeshQuery->findNearestPoly(end, extents, &filter, &endRef, endPt);
			if (dtStatusFailed(status) || endRef == 0) {
				kengine_assert_failed(*g_em, "[Recast] Failed to find nearest poly");
				return ret;
			}

			dtPolyRef path[KENGINE_NAVMESH_MAX_PATH_LENGTH];
			int pathCount = 0;
			status = recast.navMeshQuery->findPath(startRef, endRef, startPt, endPt, &filter, path, &pathCount, lengthof(path));
			if (dtStatusFailed(status)) {
				kengine_assert_failed(*g_em, "[Recast] Failed to find path");
				return ret;
			}

			static_assert(sizeof(putils::Point3f) == sizeof(float[3]));
			ret.resize(ret.capacity());

			int straightPathCount = 0;
			status = recast.navMeshQuery->findStraightPath(startPt, endPt, path, pathCount, ret[0].raw, nullptr, nullptr, &straightPathCount, ret.capacity());
			if (dtStatusFailed(status)) {
				kengine_assert_failed(*g_em, "[Recast] Failed to find straight path");
				return ret;
			}
			ret.resize(straightPathCount);

			for (auto & step : ret) {
				glm::vec4 v(shaderHelper::toVec(step), 1.f);
				v = mat * v;
				step = { v.x, v.y, v.z };
			}

			return ret;
		};
	}

	static NavMeshData loadBinaryFile(const char * binaryFile, const NavMeshComponent & navMesh) {
		NavMeshData data;

		std::ifstream f(binaryFile, std::ifstream::binary);
		if (!f)
			return data;
{'fraction_non_alphanumeric': 0.12606197912452463, 'fraction_numerical': 0.0053402378833238935, 'mean_word_length': 4.797373358348968, 'pattern_counts': {'":': 0, '<': 39, '<?xml version=': 0, '>': 53, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '5193713', 'n_tokens_mistral': 4582, 'n_tokens_neox': 4051, 'n_words': 885}