From 0acded7adb6e04d5b6d0faa97b47ee066d338286 Mon Sep 17 00:00:00 2001 From: modawan Date: Wed, 29 Jul 2026 17:19:14 +0900 Subject: [PATCH 1/2] [nfc][graphics] Refactor walkmesh to SoA and separate vertex indices BWM format stores vertices, faces, edges as separare arrays, effectively a structure of arrays. Before this patch we used to transform this into an array of structures (struct Face) and keep only one array in the Walkmesh. This approach had several problems: 1. Vertices are duplicated. Each vertex is a vec3, and there are 1.5x more faces than vertices, because most faces have adjecent faces. 2. It is more difficult to find adjecent faces, without keeping original vector indices. We have to check distance between vertices to find if it is the same vertex. With indices it is enough to check if indices are the same. The patch also updates return value of raycast function. They used to return a nullable pointer to the face and distance as an output parameter. Now they return a struct with an index of the face that the ray intersects, distance from the origin point to the intersection, and an error code if there is no intersection. --- include/reone/graphics/format/bwmreader.h | 9 +- include/reone/graphics/walkmesh.h | 65 +++++-- src/libs/game/object/door.cpp | 6 +- src/libs/graphics/format/bwmreader.cpp | 65 ++++--- src/libs/graphics/walkmesh.cpp | 105 +++++++----- src/libs/scene/graph.cpp | 47 ++--- src/libs/scene/node/walkmesh.cpp | 16 +- test/game/object.cpp | 16 +- test/graphics/walkmesh.cpp | 198 ++++++++++++++++------ 9 files changed, 332 insertions(+), 195 deletions(-) diff --git a/include/reone/graphics/format/bwmreader.h b/include/reone/graphics/format/bwmreader.h index b370922fc..d2ef5e811 100644 --- a/include/reone/graphics/format/bwmreader.h +++ b/include/reone/graphics/format/bwmreader.h @@ -50,7 +50,7 @@ class BwmReader : boost::noncopyable { uint32_t _numVertices {0}; uint32_t _offVertices {0}; uint32_t _numFaces {0}; - uint32_t _offIndices {0}; + uint32_t _offFaces {0}; uint32_t _offMaterials {0}; uint32_t _offNormals {0}; uint32_t _offPlanarDistances {0}; @@ -63,15 +63,10 @@ class BwmReader : boost::noncopyable { uint32_t _numPerimeters {0}; uint32_t _offPerimeters {0}; - std::vector _vertices; - std::vector _indices; - std::vector _materials; - std::vector _normals; - std::shared_ptr _walkmesh; void loadVertices(); - void loadIndices(); + void loadFaces(); void loadMaterials(); void loadNormals(); void loadAABB(); diff --git a/include/reone/graphics/walkmesh.h b/include/reone/graphics/walkmesh.h index ef3f9e4cb..ae426379d 100644 --- a/include/reone/graphics/walkmesh.h +++ b/include/reone/graphics/walkmesh.h @@ -24,15 +24,32 @@ namespace reone { namespace graphics { +enum RaycastFail { + RAYCAST_OK = 0, + RAYCAST_NO_INTERSECTION, + RAYCAST_NO_MATERIAL, + RAYCAST_FLIPPED_NORMAL, +}; + +struct Raycast { + uint32_t face; + float distance; + RaycastFail fail; +}; + class Walkmesh : boost::noncopyable { public: struct Face { - int index {0}; + uint32_t index {0}; uint32_t material {0}; - std::vector vertices; + glm::vec3 vertices[3]; glm::vec3 normal {0.0f}; }; + struct FaceVertices { + uint32_t indices[3]; + }; + struct AABB { graphics::AABB value; int faceIdx {-1}; @@ -41,52 +58,64 @@ class Walkmesh : boost::noncopyable { }; /** - * @return pointer to intersected face or nullptr when no intersection + * @return index of the face that the ray intersects, distance from the + * origin point to the intersection, and an error code if there is no + * intersection. */ - const Walkmesh::Face *raycast( + Raycast raycast( std::set walkcheckSurfaces, const glm::vec3 &origin, const glm::vec3 &dir, float maxDistance, - bool ignoreBackface, - float &outDistance) const; + bool ignoreBackface) const; bool contains(const glm::vec2 &point) const; bool isAreaWalkmesh() const { return _area; } - const std::vector &faces() const { return _faces; } - - void add(Face &&face) { - _faces.push_back(face); + Face getFace(uint32_t index) const { + FaceVertices face = faces[index]; + return Face { + index, + materials[index], + { + vertices[face.indices[0]], + vertices[face.indices[1]], + vertices[face.indices[2]], + }, + normals[index], + }; } void setRootAABB(std::shared_ptr aabb) { _rootAabb = std::move(aabb); } + void verify() const; + + std::vector vertices; + std::vector faces; + std::vector normals; + std::vector materials; + private: - std::vector _faces; std::shared_ptr _rootAabb; - bool _area {false}; - const Walkmesh::Face *raycastAABB( + Raycast raycastAABB( std::set surfaces, const glm::vec3 &origin, const glm::vec3 &dir, float maxDistance, - bool ignoreBackface, - float &outDistance) const; + bool ignoreBackface) const; - bool raycastFace( + Raycast raycastFace( std::set surfaces, const Walkmesh::Face &face, const glm::vec3 &origin, const glm::vec3 &dir, float maxDistance, - bool ignoreBackface, - float &outDistance) const; + bool ignoreBackface) const; friend class BwmReader; }; diff --git a/src/libs/game/object/door.cpp b/src/libs/game/object/door.cpp index a83ba4daf..9e40d6456 100644 --- a/src/libs/game/object/door.cpp +++ b/src/libs/game/object/door.cpp @@ -249,9 +249,9 @@ void Door::applyRestingPose() { void Door::loadLinkedTransitionGeometry(const Walkmesh &walkmesh) { AABB bounds; - for (const auto &face : walkmesh.faces()) { - for (const auto &vertex : face.vertices) { - bounds.expand(vertex); + for (const auto &face : walkmesh.faces) { + for (const auto &vertex : face.indices) { + bounds.expand(walkmesh.vertices[vertex]); } } if (bounds.isDegenerate() || bounds.min().x == bounds.max().x || bounds.min().y == bounds.max().y) { diff --git a/src/libs/graphics/format/bwmreader.cpp b/src/libs/graphics/format/bwmreader.cpp index 604e7a34a..acdd69927 100644 --- a/src/libs/graphics/format/bwmreader.cpp +++ b/src/libs/graphics/format/bwmreader.cpp @@ -44,7 +44,7 @@ void BwmReader::load() { _offVertices = _bwm.readUint32(); _numFaces = _bwm.readUint32(); - _offIndices = _bwm.readUint32(); + _offFaces = _bwm.readUint32(); _offMaterials = _bwm.readUint32(); _offNormals = _bwm.readUint32(); _offPlanarDistances = _bwm.readUint32(); @@ -67,24 +67,13 @@ void BwmReader::load() { _walkmesh->_area = _type == WalkmeshType::WOK; loadVertices(); - loadIndices(); + loadFaces(); loadMaterials(); loadNormals(); - for (uint32_t i = 0; i < _numFaces; ++i) { - uint32_t material = _materials[i]; - uint32_t *indices = &_indices[3 * i + 0]; - - Walkmesh::Face face; - face.index = i; - face.material = material; - face.vertices.push_back(glm::make_vec3(&_vertices[3 * indices[0]])); - face.vertices.push_back(glm::make_vec3(&_vertices[3 * indices[1]])); - face.vertices.push_back(glm::make_vec3(&_vertices[3 * indices[2]])); - face.normal = glm::make_vec3(&_normals[3 * i]); - - _walkmesh->_faces.push_back(std::move(face)); - } +#ifndef _NDEBUG + _walkmesh->verify(); +#endif if (_type == WalkmeshType::WOK) { loadAABB(); @@ -93,43 +82,47 @@ void BwmReader::load() { void BwmReader::loadVertices() { _bwm.seek(_offVertices); - _vertices.reserve(3 * _numVertices); - + auto &array = _walkmesh->vertices; + array.reserve(_numVertices); for (uint32_t i = 0; i < _numVertices; ++i) { - _vertices.push_back(_bwm.readFloat()); - _vertices.push_back(_bwm.readFloat()); - _vertices.push_back(_bwm.readFloat()); + float x = _bwm.readFloat(); + float y = _bwm.readFloat(); + float z = _bwm.readFloat(); + array.emplace_back(x, y, z); } } -void BwmReader::loadIndices() { - _bwm.seek(_offIndices); - _indices.reserve(3 * _numFaces); - +void BwmReader::loadFaces() { + _bwm.seek(_offFaces); + auto &array = _walkmesh->faces; + array.reserve(_numFaces); for (uint32_t i = 0; i < _numFaces; ++i) { - _indices.push_back(_bwm.readUint32()); - _indices.push_back(_bwm.readUint32()); - _indices.push_back(_bwm.readUint32()); + uint32_t v0 = _bwm.readUint32(); + uint32_t v1 = _bwm.readUint32(); + uint32_t v2 = _bwm.readUint32(); + Walkmesh::FaceVertices face = {{v0, v1, v2}}; + array.emplace_back(face); } } void BwmReader::loadMaterials() { _bwm.seek(_offMaterials); - _materials.reserve(_numFaces); - + auto &array = _walkmesh->materials; + array.reserve(_numFaces); for (uint32_t i = 0; i < _numFaces; ++i) { - _materials.push_back(_bwm.readUint32()); + array.emplace_back(_bwm.readUint32()); } } void BwmReader::loadNormals() { _bwm.seek(_offNormals); - _normals.reserve(3 * _numFaces); - + auto &array = _walkmesh->normals; + array.reserve(_numFaces); for (uint32_t i = 0; i < _numFaces; ++i) { - _normals.push_back(_bwm.readFloat()); - _normals.push_back(_bwm.readFloat()); - _normals.push_back(_bwm.readFloat()); + float x = _bwm.readFloat(); + float y = _bwm.readFloat(); + float z = _bwm.readFloat(); + array.emplace_back(x, y, z); } } diff --git a/src/libs/graphics/walkmesh.cpp b/src/libs/graphics/walkmesh.cpp index 9af35a27d..7f1369c9c 100644 --- a/src/libs/graphics/walkmesh.cpp +++ b/src/libs/graphics/walkmesh.cpp @@ -21,75 +21,79 @@ namespace reone { namespace graphics { -const Walkmesh::Face *Walkmesh::raycast( +Raycast Walkmesh::raycast( std::set surfaces, const glm::vec3 &origin, const glm::vec3 &dir, float maxDistance, - bool ignoreBackface, - float &outDistance) const { + bool ignoreBackface) const { // For area walkmeshes, find intersection via AABB tree if (_rootAabb) { - return raycastAABB(surfaces, origin, dir, maxDistance, ignoreBackface, outDistance); + return raycastAABB(surfaces, origin, dir, maxDistance, ignoreBackface); + } + + Raycast minResult = {0}; + minResult.distance = FLT_MAX; + + if (faces.empty()) { + minResult.fail = RAYCAST_NO_INTERSECTION; + return minResult; } // For placeable and door walkmeshes, test all faces for intersection - float distance = 0.0f; - float minDistance = std::numeric_limits::max(); - std::optional> intersected; - for (auto &face : _faces) { - if (!raycastFace(surfaces, face, origin, dir, maxDistance, ignoreBackface, distance)) { + for (uint32_t i = 0; i < faces.size(); ++i) { + Raycast result = raycastFace(surfaces, getFace(i), origin, dir, maxDistance, ignoreBackface); + if (result.fail) { + if (!minResult.fail && minResult.distance == FLT_MAX) { + minResult.fail = result.fail; + } continue; } - - if (distance < minDistance) { - minDistance = distance; - intersected = face; + if (result.distance < minResult.distance) { + minResult = result; } } - if (intersected) { - outDistance = minDistance; - return &intersected->get(); - } - return nullptr; + return minResult; } -const Walkmesh::Face *Walkmesh::raycastAABB( +Raycast Walkmesh::raycastAABB( std::set surfaces, const glm::vec3 &origin, const glm::vec3 &dir, float maxDistance, - bool ignoreBackface, - float &outDistance) const { - - float distance = 0.0f; + bool ignoreBackface) const { std::stack aabbs; aabbs.push(_rootAabb.get()); - outDistance = std::numeric_limits::max(); - const Face *result = nullptr; + glm::vec3 invDir = 1.0f / dir; + Raycast minResult = {0}; + minResult.distance = FLT_MAX; - auto invDir = 1.0f / dir; + bool foundFace = false; while (!aabbs.empty()) { auto aabb = aabbs.top(); aabbs.pop(); // Test ray/face intersection for tree leafs if (aabb->faceIdx != -1) { - const Face &face = _faces[aabb->faceIdx]; - if (raycastFace(surfaces, face, origin, dir, maxDistance, ignoreBackface, distance)) { - if (distance < outDistance) { - result = &face; - outDistance = distance; + foundFace = true; + Raycast result = raycastFace(surfaces, getFace(aabb->faceIdx), origin, dir, maxDistance, ignoreBackface); + if (result.fail) { + if (!minResult.fail && minResult.distance == FLT_MAX) { + minResult.fail = result.fail; } + continue; + } + if (result.distance < minResult.distance) { + minResult = result; } - continue; } // Test ray/AABB intersection + float distance = 0.0f; if (!aabb->value.raycast(origin, invDir, maxDistance, distance)) { continue; } @@ -103,20 +107,26 @@ const Walkmesh::Face *Walkmesh::raycastAABB( } } - return result; + if (!foundFace) { + minResult.fail = RAYCAST_NO_INTERSECTION; + } + + return minResult; } -bool Walkmesh::raycastFace( +Raycast Walkmesh::raycastFace( std::set surfaces, const Face &face, const glm::vec3 &origin, const glm::vec3 &dir, float maxDistance, - bool ignoreBackface, - float &outDistance) const { + bool ignoreBackface) const { + + Raycast result = {0}; if (surfaces.count(face.material) == 0) { - return false; + result.fail = RAYCAST_NO_MATERIAL; + return result; } const glm::vec3 &p0 = face.vertices[0]; @@ -127,14 +137,15 @@ bool Walkmesh::raycastFace( float distance = 0.0f; if (glm::intersectRayTriangle(origin, dir, p0, p1, p2, baryPosition, distance) && distance > 0.0f && distance < maxDistance) { + result.face = face.index; + result.distance = distance; if (ignoreBackface && glm::dot(face.normal, dir) > 0) { - return false; + result.fail = RAYCAST_FLIPPED_NORMAL; } - outDistance = distance; - return true; + } else { + result.fail = RAYCAST_NO_INTERSECTION; } - - return false; + return result; } bool Walkmesh::contains(const glm::vec2 &point) const { @@ -144,6 +155,16 @@ bool Walkmesh::contains(const glm::vec2 &point) const { return _rootAabb->value.contains(point); } +void Walkmesh::verify() const { + assert(faces.size() == normals.size()); + assert(faces.size() == materials.size()); + for (const FaceVertices &face : faces) { + for (uint32_t index : face.indices) { + assert(vertices.size() > index); + } + } +} + } // namespace graphics } // namespace reone diff --git a/src/libs/scene/graph.cpp b/src/libs/scene/graph.cpp index 370dcb1be..26417cfdd 100644 --- a/src/libs/scene/graph.cpp +++ b/src/libs/scene/graph.cpp @@ -791,19 +791,20 @@ bool SceneGraph::testElevation(const glm::vec3 &position, Collision &outCollisio } } auto objSpaceOrigin = glm::vec3(root->absoluteTransformInverse() * glm::vec4(origin, 1.0f)); - float distance = 0.0f; - auto face = root->walkmesh().raycast(_walkcheckSurfaces, objSpaceOrigin, down, 2.0f * kElevationTestZ, /*ignoreBackface=*/true, distance); - if (!face || distance >= minDistance) { + auto raycast = root->walkmesh().raycast(_walkcheckSurfaces, objSpaceOrigin, down, 2.0f * kElevationTestZ, /*ignoreBackface=*/true); + if (raycast.fail || raycast.distance >= minDistance) { continue; } - walkable = _walkableSurfaces.count(face->material) > 0; + uint32_t material = root->walkmesh().materials[raycast.face]; + glm::vec3 normal = root->walkmesh().normals[raycast.face]; + walkable = _walkableSurfaces.count(material) > 0; if (walkable) { outCollision.user = root->user(); - outCollision.intersection = origin + distance * down; - outCollision.normal = root->absoluteTransform() * glm::vec4 {face->normal, 0.0f}; - outCollision.material = face->material; + outCollision.intersection = origin + raycast.distance * down; + outCollision.normal = root->absoluteTransform() * glm::vec4 {normal, 0.0f}; + outCollision.material = material; } - minDistance = distance; + minDistance = raycast.distance; } return walkable; @@ -835,16 +836,17 @@ bool SceneGraph::testLineOfSight(const glm::vec3 &origin, const glm::vec3 &dest, originLocal = root->absoluteTransformInverse() * glm::vec4 {origin, 1.0f}; dirLocal = root->absoluteTransformInverse() * glm::vec4 {dir, 0.0f}; } - float distance = 0.0f; - auto face = root->walkmesh().raycast(_lineOfSightSurfaces, originLocal, dirLocal, maxDistance, /*ignoreBackface=*/false, distance); - if (!face || distance > minDistance) { + auto raycast = root->walkmesh().raycast(_lineOfSightSurfaces, originLocal, dirLocal, maxDistance, /*ignoreBackface=*/false); + if (raycast.fail || raycast.distance > minDistance) { continue; } + uint32_t material = root->walkmesh().materials[raycast.face]; + glm::vec3 normal = root->walkmesh().normals[raycast.face]; outCollision.user = root->user(); - outCollision.intersection = origin + distance * dir; - outCollision.normal = root->absoluteTransform() * glm::vec4(face->normal, 0.0f); - outCollision.material = face->material; - minDistance = distance; + outCollision.intersection = origin + raycast.distance * dir; + outCollision.normal = root->absoluteTransform() * glm::vec4(normal, 0.0f); + outCollision.material = material; + minDistance = raycast.distance; } return minDistance != std::numeric_limits::max(); @@ -868,16 +870,17 @@ bool SceneGraph::testWalk(const glm::vec3 &origin, const glm::vec3 &dest, const } glm::vec3 objSpaceOrigin(root->absoluteTransformInverse() * glm::vec4(origin, 1.0f)); glm::vec3 objSpaceDir(root->absoluteTransformInverse() * glm::vec4(dir, 0.0f)); - float distance = 0.0f; - auto face = root->walkmesh().raycast(_walkcheckSurfaces, objSpaceOrigin, objSpaceDir, kMaxCollisionDistanceWalk, /*ignoreBackface=*/false, distance); - if (!face || distance > maxDistance || distance > minDistance) { + auto raycast = root->walkmesh().raycast(_walkcheckSurfaces, objSpaceOrigin, objSpaceDir, kMaxCollisionDistanceWalk, /*ignoreBackface=*/false); + if (raycast.fail || raycast.distance > maxDistance || raycast.distance > minDistance) { continue; } + uint32_t material = root->walkmesh().materials[raycast.face]; + glm::vec3 normal = root->walkmesh().normals[raycast.face]; outCollision.user = root->user(); - outCollision.intersection = origin + distance * dir; - outCollision.normal = root->absoluteTransform() * glm::vec4(face->normal, 0.0f); - outCollision.material = face->material; - minDistance = distance; + outCollision.intersection = origin + raycast.distance * dir; + outCollision.normal = root->absoluteTransform() * glm::vec4(normal, 0.0f); + outCollision.material = material; + minDistance = raycast.distance; } return minDistance != std::numeric_limits::max(); diff --git a/src/libs/scene/node/walkmesh.cpp b/src/libs/scene/node/walkmesh.cpp index 0248dc289..ef9533bbb 100644 --- a/src/libs/scene/node/walkmesh.cpp +++ b/src/libs/scene/node/walkmesh.cpp @@ -36,18 +36,22 @@ void WalkmeshSceneNode::init() { std::vector vertices; std::vector faces; - for (auto &wface : _walkmesh.faces()) { + for (size_t i = 0; i < _walkmesh.faces.size(); ++i) { size_t vertIdxStart = vertices.size() / 7; - for (int i = 0; i < 3; ++i) { - vertices.push_back(wface.vertices[i].x); - vertices.push_back(wface.vertices[i].y); - vertices.push_back(wface.vertices[i].z); + + Walkmesh::Face wface = _walkmesh.getFace(i); + float material = glm::min(1.0f, static_cast(wface.material) / static_cast(kMaxWalkmeshMaterials - 1)); + + for (glm::vec3 v : wface.vertices) { + vertices.push_back(v.x); + vertices.push_back(v.y); + vertices.push_back(v.z); vertices.push_back(wface.normal.x); vertices.push_back(wface.normal.y); vertices.push_back(wface.normal.z); - float material = glm::min(1.0f, static_cast(wface.material) / static_cast(kMaxWalkmeshMaterials - 1)); vertices.push_back(material); } + Mesh::Face face; face.vertices[0] = vertIdxStart + 0; face.vertices[1] = vertIdxStart + 1; diff --git a/test/game/object.cpp b/test/game/object.cpp index 42fd635a2..5c3161204 100644 --- a/test/game/object.cpp +++ b/test/game/object.cpp @@ -399,13 +399,15 @@ scene::MockSceneGraph &testSceneGraph(TestEngine &engine) { std::shared_ptr makeDoorWalkmesh() { auto walkmesh = std::make_shared(); - walkmesh->add(graphics::Walkmesh::Face { - 0, - 0, - {glm::vec3(-2.0f, -0.5f, 0.0f), - glm::vec3(2.0f, -0.5f, 3.0f), - glm::vec3(2.0f, 0.5f, 0.0f)}, - glm::vec3(0.0f, 0.0f, 1.0f)}); + walkmesh->vertices = { + glm::vec3(-2.0f, -0.5f, 0.0f), + glm::vec3(2.0f, -0.5f, 3.0f), + glm::vec3(2.0f, 0.5f, 0.0f)}; + walkmesh->normals = { + glm::vec3(0.0f, 0.0f, 1.0f), + }; + walkmesh->faces = {{0, 1, 2}}; + walkmesh->materials = {0}; return walkmesh; } diff --git a/test/graphics/walkmesh.cpp b/test/graphics/walkmesh.cpp index 79c99d44b..11ddc4a83 100644 --- a/test/graphics/walkmesh.cpp +++ b/test/graphics/walkmesh.cpp @@ -25,10 +25,34 @@ using namespace reone::graphics; TEST(Walkmesh, should_find_ray_walkmesh_intersection__intersection_from_close) { // given auto walkmesh = Walkmesh(); - walkmesh.add(Walkmesh::Face {0, 0, std::vector {glm::vec3(-1.0f, 0.0f, 0.0f), glm::vec3(-1.0f, 1.0f, 0.0f), glm::vec3(0.0f, 0.0f, 0.0f)}, glm::vec3(1.0f, 0.0f, 0.0f)}); - walkmesh.add(Walkmesh::Face {1, 0, std::vector {glm::vec3(0.0f, 1.0f, 0.0f), glm::vec3(-1.0f, 1.0f, 0.0f), glm::vec3(0.0f, 0.0f, 0.0f)}, glm::vec3(1.0f, 0.0f, 0.0f)}); - walkmesh.add(Walkmesh::Face {2, 0, std::vector {glm::vec3(0.0f, 0.0f, 0.0f), glm::vec3(1.0f, 0.0f, 0.0f), glm::vec3(1.0f, -1.0f, 0.0f)}, glm::vec3(1.0f, 0.0f, 0.0f)}); - walkmesh.add(Walkmesh::Face {3, 0, std::vector {glm::vec3(0.0f, 0.0f, 0.0f), glm::vec3(1.0f, -1.0f, 0.0f), glm::vec3(0.0f, -1.0f, 0.0f)}, glm::vec3(1.0f, 0.0f, 0.0f)}); + walkmesh.vertices = { + glm::vec3(-1.0f, 0.0f, 0.0f), + glm::vec3(-1.0f, 1.0f, 0.0f), + glm::vec3(0.0f, 0.0f, 0.0f), + glm::vec3(0.0f, 1.0f, 0.0f), + glm::vec3(-1.0f, 1.0f, 0.0f), + glm::vec3(0.0f, 0.0f, 0.0f), + glm::vec3(0.0f, 0.0f, 0.0f), + glm::vec3(1.0f, 0.0f, 0.0f), + glm::vec3(1.0f, -1.0f, 0.0f), + glm::vec3(0.0f, 0.0f, 0.0f), + glm::vec3(1.0f, -1.0f, 0.0f), + glm::vec3(0.0f, -1.0f, 0.0f), + }; + walkmesh.normals = { + glm::vec3(1.0f, 0.0f, 0.0f), + glm::vec3(1.0f, 0.0f, 0.0f), + glm::vec3(1.0f, 0.0f, 0.0f), + glm::vec3(1.0f, 0.0f, 0.0f), + }; + walkmesh.faces = { + {0, 1, 2}, + {3, 4, 5}, + {6, 7, 8}, + {9, 10, 11}, + }; + walkmesh.materials = {0, 0, 0, 0}; + auto rootAabb = std::make_shared(); rootAabb->value = AABB(glm::vec3(-1.0f, -1.0f, 0.0f), glm::vec3(1.0f, 1.0f, 0.0f)); rootAabb->left = std::make_shared(); @@ -46,22 +70,45 @@ TEST(Walkmesh, should_find_ray_walkmesh_intersection__intersection_from_close) { walkmesh.setRootAABB(rootAabb); // when - float distance = -1.0f; - auto face = walkmesh.raycast(std::set {0}, glm::vec3(-0.5f, 0.25, 1.0f), glm::vec3(0.0f, 0.0f, -1.0f), 10.0f, /*ignoreBackface=*/false, distance); + auto raycast = walkmesh.raycast(std::set {0}, glm::vec3(-0.5f, 0.25, 1.0f), glm::vec3(0.0f, 0.0f, -1.0f), 10.0f, /*ignoreBackface=*/false); // then - EXPECT_TRUE(static_cast(face)); - EXPECT_EQ(0, face->index); - EXPECT_NEAR(1.0f, distance, 1e-5); + EXPECT_EQ(RAYCAST_OK, raycast.fail); + EXPECT_EQ(0, raycast.face); + EXPECT_NEAR(1.0f, raycast.distance, 1e-5); } TEST(Walkmesh, should_find_ray_walkmesh_intersection__intersection_from_far) { // given auto walkmesh = Walkmesh(); - walkmesh.add(Walkmesh::Face {0, 0, std::vector {glm::vec3(-1.0f, 0.0f, 0.0f), glm::vec3(-1.0f, 1.0f, 0.0f), glm::vec3(0.0f, 0.0f, 0.0f)}, glm::vec3(1.0f, 0.0f, 0.0f)}); - walkmesh.add(Walkmesh::Face {1, 0, std::vector {glm::vec3(0.0f, 1.0f, 0.0f), glm::vec3(-1.0f, 1.0f, 0.0f), glm::vec3(0.0f, 0.0f, 0.0f)}, glm::vec3(1.0f, 0.0f, 0.0f)}); - walkmesh.add(Walkmesh::Face {2, 0, std::vector {glm::vec3(0.0f, 0.0f, 0.0f), glm::vec3(1.0f, 0.0f, 0.0f), glm::vec3(1.0f, -1.0f, 0.0f)}, glm::vec3(1.0f, 0.0f, 0.0f)}); - walkmesh.add(Walkmesh::Face {3, 0, std::vector {glm::vec3(0.0f, 0.0f, 0.0f), glm::vec3(1.0f, -1.0f, 0.0f), glm::vec3(0.0f, -1.0f, 0.0f)}, glm::vec3(1.0f, 0.0f, 0.0f)}); + walkmesh.vertices = { + glm::vec3(-1.0f, 0.0f, 0.0f), + glm::vec3(-1.0f, 1.0f, 0.0f), + glm::vec3(0.0f, 0.0f, 0.0f), + glm::vec3(0.0f, 1.0f, 0.0f), + glm::vec3(-1.0f, 1.0f, 0.0f), + glm::vec3(0.0f, 0.0f, 0.0f), + glm::vec3(0.0f, 0.0f, 0.0f), + glm::vec3(1.0f, 0.0f, 0.0f), + glm::vec3(1.0f, -1.0f, 0.0f), + glm::vec3(0.0f, 0.0f, 0.0f), + glm::vec3(1.0f, -1.0f, 0.0f), + glm::vec3(0.0f, -1.0f, 0.0f), + }; + walkmesh.normals = { + glm::vec3(1.0f, 0.0f, 0.0f), + glm::vec3(1.0f, 0.0f, 0.0f), + glm::vec3(1.0f, 0.0f, 0.0f), + glm::vec3(1.0f, 0.0f, 0.0f), + }; + walkmesh.faces = { + {0, 1, 2}, + {3, 4, 5}, + {6, 7, 8}, + {9, 10, 11}, + }; + walkmesh.materials = {0, 0, 0, 0}; + auto rootAabb = std::make_shared(); rootAabb->value = AABB(glm::vec3(-1.0f, -1.0f, 0.0f), glm::vec3(1.0f, 1.0f, 0.0f)); rootAabb->left = std::make_shared(); @@ -79,20 +126,43 @@ TEST(Walkmesh, should_find_ray_walkmesh_intersection__intersection_from_far) { walkmesh.setRootAABB(rootAabb); // when - float distance = -1.0f; - auto face = walkmesh.raycast(std::set {0}, glm::vec3(-0.5f, 0.25, 20.0f), glm::vec3(0.0f, 0.0f, -1.0f), 10.0f, /*ignoreBackface=*/false, distance); + auto raycast = walkmesh.raycast(std::set {0}, glm::vec3(-0.5f, 0.25, 20.0f), glm::vec3(0.0f, 0.0f, -1.0f), 10.0f, /*ignoreBackface=*/false); // then - EXPECT_TRUE(!static_cast(face)); + EXPECT_EQ(RAYCAST_NO_INTERSECTION, raycast.fail); } TEST(Walkmesh, should_find_ray_walkmesh_intersection__no_intersection) { // given auto walkmesh = Walkmesh(); - walkmesh.add(Walkmesh::Face {0, 0, std::vector {glm::vec3(-1.0f, 0.0f, 0.0f), glm::vec3(-1.0f, 1.0f, 0.0f), glm::vec3(0.0f, 0.0f, 0.0f)}, glm::vec3(1.0f, 0.0f, 0.0f)}); - walkmesh.add(Walkmesh::Face {1, 0, std::vector {glm::vec3(0.0f, 1.0f, 0.0f), glm::vec3(-1.0f, 1.0f, 0.0f), glm::vec3(0.0f, 0.0f, 0.0f)}, glm::vec3(1.0f, 0.0f, 0.0f)}); - walkmesh.add(Walkmesh::Face {2, 0, std::vector {glm::vec3(0.0f, 0.0f, 0.0f), glm::vec3(1.0f, 0.0f, 0.0f), glm::vec3(1.0f, -1.0f, 0.0f)}, glm::vec3(1.0f, 0.0f, 0.0f)}); - walkmesh.add(Walkmesh::Face {3, 0, std::vector {glm::vec3(0.0f, 0.0f, 0.0f), glm::vec3(1.0f, -1.0f, 0.0f), glm::vec3(0.0f, -1.0f, 0.0f)}, glm::vec3(1.0f, 0.0f, 0.0f)}); + walkmesh.vertices = { + glm::vec3(-1.0f, 0.0f, 0.0f), + glm::vec3(-1.0f, 1.0f, 0.0f), + glm::vec3(0.0f, 0.0f, 0.0f), + glm::vec3(0.0f, 1.0f, 0.0f), + glm::vec3(-1.0f, 1.0f, 0.0f), + glm::vec3(0.0f, 0.0f, 0.0f), + glm::vec3(0.0f, 0.0f, 0.0f), + glm::vec3(1.0f, 0.0f, 0.0f), + glm::vec3(1.0f, -1.0f, 0.0f), + glm::vec3(0.0f, 0.0f, 0.0f), + glm::vec3(1.0f, -1.0f, 0.0f), + glm::vec3(0.0f, -1.0f, 0.0f), + }; + walkmesh.normals = { + glm::vec3(1.0f, 0.0f, 0.0f), + glm::vec3(1.0f, 0.0f, 0.0f), + glm::vec3(1.0f, 0.0f, 0.0f), + glm::vec3(1.0f, 0.0f, 0.0f), + }; + walkmesh.faces = { + {0, 1, 2}, + {3, 4, 5}, + {6, 7, 8}, + {9, 10, 11}, + }; + walkmesh.materials = {0, 0, 0, 0}; + auto rootAabb = std::make_shared(); rootAabb->value = AABB(glm::vec3(-1.0f, -1.0f, 0.0f), glm::vec3(1.0f, 1.0f, 0.0f)); rootAabb->left = std::make_shared(); @@ -110,22 +180,33 @@ TEST(Walkmesh, should_find_ray_walkmesh_intersection__no_intersection) { walkmesh.setRootAABB(rootAabb); // when - float distance = -1.0f; - auto face = walkmesh.raycast(std::set {0}, glm::vec3(-0.5f, 0.25, 1.0f), glm::vec3(1.0f, 0.0f, 0.0f), 10.0f, /*ignoreBackface=*/false, distance); + auto raycast = walkmesh.raycast(std::set {0}, glm::vec3(-0.5f, 0.25, 1.0f), glm::vec3(1.0f, 0.0f, 0.0f), 10.0f, /*ignoreBackface=*/false); // then - EXPECT_TRUE(!static_cast(face)); + EXPECT_EQ(RAYCAST_NO_INTERSECTION, raycast.fail); } TEST(Walkmesh, should_find_ray_walkmesh_intersection__ignore_backface) { // given auto walkmesh = Walkmesh(); - - // Bottom triangle (facing up) - walkmesh.add(Walkmesh::Face {0, 0, std::vector {glm::vec3(-1.0f, -1.0f, 0.0f), glm::vec3(1.0f, -1.0f, 0.0f), glm::vec3(0.0f, 1.0f, 0.0f)}, glm::vec3(0.0f, 0.0f, 1.0f)}); - - // Top triangle (facing down) - walkmesh.add(Walkmesh::Face {1, 0, std::vector {glm::vec3(-1.0f, -1.0f, 1.0f), glm::vec3(1.0f, -1.0f, 1.0f), glm::vec3(0.0f, 1.0f, 1.0f)}, glm::vec3(0.0f, 0.0f, -1.0f)}); + walkmesh.vertices = { + // Bottom triangle (facing up) + glm::vec3(-1.0f, -1.0f, 0.0f), + glm::vec3(1.0f, -1.0f, 0.0f), + glm::vec3(0.0f, 1.0f, 0.0f), + // Top triangle (facing down) + glm::vec3(-1.0f, -1.0f, 1.0f), + glm::vec3(1.0f, -1.0f, 1.0f), + glm::vec3(0.0f, 1.0f, 1.0f), + }; + walkmesh.normals = { + glm::vec3(0.0f, 0.0f, 1.0f), + glm::vec3(0.0f, 0.0f, -1.0f)}; + walkmesh.faces = { + {0, 1, 2}, + {3, 4, 5}, + }; + walkmesh.materials = {0, 0}; // Root AABB covers both triangles. auto rootAabb = std::make_shared(); @@ -145,30 +226,41 @@ TEST(Walkmesh, should_find_ray_walkmesh_intersection__ignore_backface) { // When ignoreBackface is false, we should hit the top face first // regardless of its orientation (normal is up or down). - float distance = -1.0f; - auto face = walkmesh.raycast(std::set {0}, glm::vec3(0.0f, 0.0f, 2.0f), glm::vec3(0.0f, 0.0f, -1.0f), 10.0f, /*ignoreBackface=*/false, distance); - EXPECT_TRUE(face); - EXPECT_EQ(1, face->index); - EXPECT_NEAR(1.0f, distance, 1e-5); + auto raycast = walkmesh.raycast(std::set {0}, glm::vec3(0.0f, 0.0f, 2.0f), glm::vec3(0.0f, 0.0f, -1.0f), 10.0f, /*ignoreBackface=*/false); + EXPECT_EQ(RAYCAST_OK, raycast.fail); + EXPECT_EQ(1, raycast.face); + EXPECT_NEAR(1.0f, raycast.distance, 1e-5); // When ignoreBackface is true, we should ignore the top face and // hit the bottom face. - distance = -1.0f; - face = walkmesh.raycast(std::set {0}, glm::vec3(0.0f, 0.0f, 2.0f), glm::vec3(0.0f, 0.0f, -1.0f), 10.0f, /*ignoreBackface=*/true, distance); - EXPECT_TRUE(face); - EXPECT_EQ(0, face->index); - EXPECT_NEAR(2.0f, distance, 1e-5); + raycast = walkmesh.raycast(std::set {0}, glm::vec3(0.0f, 0.0f, 2.0f), glm::vec3(0.0f, 0.0f, -1.0f), 10.0f, /*ignoreBackface=*/true); + EXPECT_EQ(RAYCAST_OK, raycast.fail); + EXPECT_EQ(0, raycast.face); + EXPECT_NEAR(2.0f, raycast.distance, 1e-5); } TEST(Walkmesh, should_find_ray_walkmesh_intersection__multi_level) { // given auto walkmesh = Walkmesh(); - // Bottom triangle (facing up) - walkmesh.add(Walkmesh::Face {0, 0, std::vector {glm::vec3(-1.0f, -1.0f, 0.0f), glm::vec3(1.0f, -1.0f, 0.0f), glm::vec3(0.0f, 1.0f, 0.0f)}, glm::vec3(0.0f, 0.0f, 1.0f)}); - - // Top triangle (facing up) - walkmesh.add(Walkmesh::Face {1, 0, std::vector {glm::vec3(-1.0f, -1.0f, 1.0f), glm::vec3(1.0f, -1.0f, 1.0f), glm::vec3(0.0f, 1.0f, 1.0f)}, glm::vec3(0.0f, 0.0f, 1.0f)}); + walkmesh.vertices = { + // Bottom triangle (facing up) + glm::vec3(-1.0f, -1.0f, 0.0f), + glm::vec3(1.0f, -1.0f, 0.0f), + glm::vec3(0.0f, 1.0f, 0.0f), + // Top triangle (facing up) + glm::vec3(-1.0f, -1.0f, 1.0f), + glm::vec3(1.0f, -1.0f, 1.0f), + glm::vec3(0.0f, 1.0f, 1.0f), + }; + walkmesh.normals = { + glm::vec3(0.0f, 0.0f, 1.0f), + glm::vec3(0.0f, 0.0f, 1.0f)}; + walkmesh.faces = { + {0, 1, 2}, + {3, 4, 5}, + }; + walkmesh.materials = {0, 0}; auto rootAabb = std::make_shared(); rootAabb->value = AABB(glm::vec3(-1.0f, -1.0f, -0.2f), glm::vec3(1.0f, 1.0f, 1.2f)); @@ -187,17 +279,15 @@ TEST(Walkmesh, should_find_ray_walkmesh_intersection__multi_level) { // With both triangles are facing up, we should pick the one // closer to the origin. - float distance = -1.0f; - auto face = walkmesh.raycast(std::set {0}, glm::vec3(0.0f, 0.0f, 1.1f), glm::vec3(0.0f, 0.0f, -1.0f), 10.0f, /*ignoreBackface=*/true, distance); - EXPECT_TRUE(face); - EXPECT_EQ(1, face->index); - EXPECT_NEAR(0.1f, distance, 1e-5); + auto raycast = walkmesh.raycast(std::set {0}, glm::vec3(0.0f, 0.0f, 1.1f), glm::vec3(0.0f, 0.0f, -1.0f), 10.0f, /*ignoreBackface=*/true); + EXPECT_EQ(RAYCAST_OK, raycast.fail); + EXPECT_EQ(1, raycast.face); + EXPECT_NEAR(0.1f, raycast.distance, 1e-5); // Now move the origin closer to the bottom triangle. Raycast // should pick it instead of the top triangle. - distance = -1.0f; - face = walkmesh.raycast(std::set {0}, glm::vec3(0.0f, 0.0f, 0.2f), glm::vec3(0.0f, 0.0f, -1.0f), 10.0f, /*ignoreBackface=*/true, distance); - EXPECT_TRUE(face); - EXPECT_EQ(0, face->index); - EXPECT_NEAR(0.2f, distance, 1e-5); + raycast = walkmesh.raycast(std::set {0}, glm::vec3(0.0f, 0.0f, 0.2f), glm::vec3(0.0f, 0.0f, -1.0f), 10.0f, /*ignoreBackface=*/true); + EXPECT_EQ(RAYCAST_OK, raycast.fail); + EXPECT_EQ(0, raycast.face); + EXPECT_NEAR(0.2f, raycast.distance, 1e-5); } From 33f44dc2a90ca35302ef258380d669e4ce89b819 Mon Sep 17 00:00:00 2001 From: modawan Date: Sun, 26 Jul 2026 16:52:01 +0900 Subject: [PATCH 2/2] [game] Reimplement pathfinding Pathfinding is implemented as a collection of algorithms: 1. Global pathfinding algorithm builds a coarse-grained path from A to B as a sequence of walkmesh faces. It ensures that a path exists, but makes no effort to make it "natural" from the player perspective. 2. Funnel algorithm takes a sequence of faces from the global pathfinding, and attempts to make a straight-line path through it. 3. Steering behaviour takes a direction from the funnel algorithm, and turns it into a "driving" force. It then calculates and combines other forces: - "keepout" force to steer away from borders and corners - "stuck" force to recover from pathfinding failures. The combined force is then integrated to smooth changes of direction and make the path more natural. Omissions: 1. Dynamic objects (creatures) are not handled. Each creature has a "personal space" radius that must be taken into account for pathfinding. 2. Static objects that do not have a carve-out for them in the room walkmesh are not reflected in the Uniwalk yet. Each object has it is own "non-walkable" mesh, which must be subtracted from the room mesh. --- include/reone/game/debug.h | 2 + include/reone/game/game.h | 1 + include/reone/game/object/area.h | 8 +- include/reone/game/object/creature.h | 31 +- include/reone/game/pathfinder.h | 108 +++- src/libs/game/debug.cpp | 9 + src/libs/game/game.cpp | 8 + src/libs/game/object/area.cpp | 42 +- src/libs/game/object/creature.cpp | 202 +++---- src/libs/game/pathfinder.cpp | 801 +++++++++++++++++++++++---- src/libs/game/player.cpp | 3 +- test/game/object.cpp | 4 +- test/game/pathfinder.cpp | 119 ++-- 13 files changed, 1003 insertions(+), 335 deletions(-) diff --git a/include/reone/game/debug.h b/include/reone/game/debug.h index 6a3645d91..eb47a5af0 100644 --- a/include/reone/game/debug.h +++ b/include/reone/game/debug.h @@ -24,10 +24,12 @@ namespace game { bool isShowAABBEnabled(); bool isShowWalkmeshEnabled(); bool isShowTriggersEnabled(); +bool isShowPathEnabled(); void setShowAABB(bool show); void setShowWalkmesh(bool show); void setShowTriggers(bool show); +void setShowPath(bool show); } // namespace game diff --git a/include/reone/game/game.h b/include/reone/game/game.h index 85a697d37..9d1245077 100644 --- a/include/reone/game/game.h +++ b/include/reone/game/game.h @@ -795,6 +795,7 @@ class Game : boost::noncopyable { void consoleTurretState(const ConsoleArgs &tokens); void consoleStartTurretGame(const ConsoleArgs &tokens); void consoleShowImGui(const ConsoleArgs &tokens); + void consoleShowPath(const ConsoleArgs &tokens); // END Console commands }; diff --git a/include/reone/game/object/area.h b/include/reone/game/object/area.h index 765f5e5e4..ce8da015e 100644 --- a/include/reone/game/object/area.h +++ b/include/reone/game/object/area.h @@ -94,8 +94,8 @@ class Area : public Object { bool landObject(Object &object); void add(const std::shared_ptr &object); - bool moveCreature(const std::shared_ptr &creature, const glm::vec2 &dir, bool run, float dt); - bool moveCreatureTowards(const std::shared_ptr &creature, const glm::vec2 &dest, bool run, float dt); + bool moveCreature(const std::shared_ptr &creature, const glm::vec2 &dir, bool run, float dt, + float maxDistance = FLT_MAX); void determineObjectRoom(Object &object); bool isUnescapable() const { return _unescapable; } @@ -113,12 +113,13 @@ class Area : public Object { const CameraStyle &camStyleDefault() const { return _camStyleDefault; } const std::string &music() const { return _music; } const ObjectList &objects() const { return _objects; } - const Pathfinder &pathfinder() const { return _pathfinder; } const std::string &localizedName() const { return _localizedName; } const RoomMap &rooms() const { return _rooms; } const Grass &grass() const { return _grass; } const glm::vec3 &ambientColor() const { return _ambientColor; } + Pathfinder &pathfinder() { return _pathfinder; } + void setUnescapable(bool value); // Objects @@ -313,7 +314,6 @@ class Area : public Object { void loadLYT(); void loadVIS(); - void loadPTH(); void applySceneProperties(); void attachRoomToSceneGraph(Room &room); void attachObjectToSceneGraph(const std::shared_ptr &object); diff --git a/include/reone/game/object/creature.h b/include/reone/game/object/creature.h index 138fe53ee..9c8a4b847 100644 --- a/include/reone/game/object/creature.h +++ b/include/reone/game/object/creature.h @@ -33,6 +33,7 @@ #include "../d20/attributes.h" #include "../d20/itemattributes.h" #include "../object.h" +#include "../pathfinder.h" #include "item.h" @@ -63,15 +64,6 @@ class Creature : public Object, public scene::IAnimationEventListener { Run }; - struct Path { - glm::vec3 destination {0.0f}; - std::vector points; - uint32_t timeFound {0}; - int pointIdx {0}; - - void selectNextPoint(); - }; - struct BodyBag { std::string name; int appearance {0}; /**< index into placeables.2da */ @@ -202,16 +194,10 @@ class Creature : public Object, public scene::IAnimationEventListener { // END Equipment // Pathfinding - bool navigateTo(const glm::vec3 &dest, bool run, float distance, float dt); - void advanceOnPath(bool run, float dt); - void updatePath(const glm::vec3 &dest); - void clearPath(); - void setPath(const glm::vec3 &dest, std::vector &&points, uint32_t timeFound); - - std::shared_ptr &path() { return _path; } - + void advanceOnPath(const glm::vec3 &dest, const glm::vec3 &dir, bool run, float distance, float dt); + glm::vec3 computeSteeringForce(const Uniwalk &uni, const glm::vec3 &next, float dt); // END Pathfinding // Blocking doors @@ -421,7 +407,16 @@ class Creature : public Object, public scene::IAnimationEventListener { ModelType _modelType {ModelType::Creature}; std::shared_ptr _portrait; - std::shared_ptr _path; + // Current path that the creature is following, its velocity and position at + // the previous frame. + std::optional _path; + glm::vec3 _pathVelocity; + glm::vec3 _previousPosition; + // When there is no progress on the path, apply _stuckForce to steer the + // creature in a random direction until the timer runs out. + Timer _stuckTimer; + glm::vec3 _stuckForce; + float _walkSpeed {0.0f}; float _runSpeed {0.0f}; float _creaturePersonalSpace {0.6f}; diff --git a/include/reone/game/pathfinder.h b/include/reone/game/pathfinder.h index 829faf544..06d8aeb41 100644 --- a/include/reone/game/pathfinder.h +++ b/include/reone/game/pathfinder.h @@ -17,45 +17,99 @@ #pragma once -#include "reone/resource/path.h" +#include "reone/graphics/aabb.h" +#include "reone/graphics/walkmesh.h" namespace reone { namespace game { -/** - * A* pathfinding. - */ -class Pathfinder : boost::noncopyable { -public: - void load(const std::vector &points, const std::unordered_map &pointZ); - - const std::vector findPath(const glm::vec3 &from, const glm::vec3 &to) const; - -private: - struct ContextVertex { - uint16_t index {0}; - uint16_t parentIndex {0xffff}; - float distance {0.0f}; - float heuristic {0.0f}; - float totalCost {0.0f}; - }; +/// Walkable face that is used for pathfinding. +struct Uniface { + /// Indices into Uniwalk::vertices array. + uint32_t vertices[3]; + + /// Indices of adjecent faces. Edges [0, 1], [1, 2], [2, 0] are + // adjecent when they are common with any other face. + uint32_t adjecent[3]; - struct Context { - std::unordered_map vertices; - std::set open; - std::set closed; + /// Center of a face. Used to estimate distance between two faces. + glm::vec3 centroid; +}; + +/// Subdivision of a walkmesh. Uniroom associates a range of faces [begin, end) +/// to AABB. +struct Uniroom { + uint32_t begin; + uint32_t end; + glm::vec3 min; + glm::vec3 max; +}; - const ContextVertex &getVertexWithLeastTotalCostFromOpen() const; +/// Unified walkmesh, assembled from walkmeshes of all rooms in the area. +struct Uniwalk { + std::vector vertices; + std::vector faces; + std::vector rooms; +}; + +/// State of a face for A* algorithm. +struct AStarFace { + enum Flag { + Open = 1, + Closed = 2, }; - std::vector _vertices; - std::unordered_map> _adjacentVertices; + Flag flag; + uint32_t parent; +}; + +/// Element of a list of faces to consider next for A* algorithm. +struct AStarOpenFace { + uint32_t index; + float cost; +}; + +struct AStarContext { + std::vector state; + std::vector open; +}; + +/// Fully calculated path. +struct AStarPath { + std::vector faces; + glm::vec3 from; + glm::vec3 to; + uint32_t next; + glm::vec3 nextPoint; - uint16_t getNearestVertex(const glm::vec3 &point) const; - uint16_t getNearestVertexBetweenPoints(const glm::vec3 &point, const glm::vec3 &ref) const; + int32_t index; + bool active; }; +/// Handle to a calculated path. +struct Path { + int32_t index; +}; + +struct Pathfinder : public boost::noncopyable { + Uniwalk uni; + AStarContext astar; + std::vector paths; +}; + +void uniwalkLoadRoom(struct Uniwalk &wm, graphics::Walkmesh &data, std::set &walkableMaterial); +void uniwalkFinalize(struct Uniwalk &uni); + +std::optional createPath(Pathfinder &pf, const glm::vec3 &from, const glm::vec3 &to); +bool updatePath(Pathfinder &pf, Path p, const glm::vec3 ¤t); +void releasePath(Pathfinder &pf, Path path); + +glm::vec3 getNextPathPoint(Pathfinder &pf, Path path); +glm::vec3 getLastPathPoint(Pathfinder &pf, Path path); + +glm::vec3 computeKeepoutForce(const Uniwalk &uni, const glm::vec3 &position); + } // namespace game } // namespace reone diff --git a/src/libs/game/debug.cpp b/src/libs/game/debug.cpp index a5f868130..c0af24be9 100644 --- a/src/libs/game/debug.cpp +++ b/src/libs/game/debug.cpp @@ -24,6 +24,7 @@ namespace game { static bool g_showAABB = false; static bool g_showWalkmesh = false; static bool g_showTriggers = false; +static bool g_showPath = false; bool isShowAABBEnabled() { return g_showAABB; @@ -37,6 +38,10 @@ bool isShowTriggersEnabled() { return g_showTriggers; } +bool isShowPathEnabled() { + return g_showPath; +} + void setShowAABB(bool show) { g_showAABB = show; } @@ -49,6 +54,10 @@ void setShowTriggers(bool show) { g_showTriggers = show; } +void setShowPath(bool show) { + g_showPath = show; +} + } // namespace game } // namespace reone diff --git a/src/libs/game/game.cpp b/src/libs/game/game.cpp index 6c06a2674..2058acff1 100644 --- a/src/libs/game/game.cpp +++ b/src/libs/game/game.cpp @@ -464,6 +464,8 @@ void Game::initConsole() { registerConsoleCommand("listgames", "list savegames", &Game::consoleListGames); registerConsoleCommand("loadgame", "load a savegame", &Game::consoleLoadGame); registerConsoleCommand("startpazaak", "start a development Pazaak match", &Game::consoleStartPazaak); + registerConsoleCommand("showpath", "show debug overlay for pathfinding", &Game::consoleShowPath); + if (_options.game.developer) { registerConsoleCommand("minigameinfo", "print minigame metadata for current area", &Game::consoleMiniGameInfo); registerConsoleCommand("startswoop", "enter the developer swoop race mode for the current area", &Game::consoleStartSwoop); @@ -4337,6 +4339,12 @@ void Game::consoleShowImGui(const ConsoleArgs &args) { _showImGui = show; } +void Game::consoleShowPath(const ConsoleArgs &args) { + consoleCheckUsage(args, 1, 1, "1|0"); + bool show = args.get(1).value(); + setShowPath(show); +} + } // namespace game } // namespace reone diff --git a/src/libs/game/object/area.cpp b/src/libs/game/object/area.cpp index 29384940a..bde199002 100644 --- a/src/libs/game/object/area.cpp +++ b/src/libs/game/object/area.cpp @@ -181,7 +181,6 @@ void Area::load(std::string name, const Gff &are, const Gff &git, bool fromSave) loadLYT(); loadGIT(gitParsed, git); loadVIS(); - loadPTH(); } void Area::activate() { @@ -392,6 +391,7 @@ void Area::loadLYT() { throw ResourceNotFoundException("Area LYT not found: " + _name); } auto &sceneGraph = _services.scene.graphs.get(_sceneName); + auto walkableSurfaces = _services.game.surfaces.getWalkableSurfaces(); for (auto &lytRoom : layout->rooms) { auto model = _services.resource.models.get(lytRoom.name); if (!model) { @@ -434,6 +434,7 @@ void Area::loadLYT() { if (walkmesh) { walkmeshSceneNode = sceneGraph.newWalkmesh(*walkmesh); sceneGraph.addRoot(walkmeshSceneNode); + uniwalkLoadRoom(_pathfinder.uni, *walkmesh, walkableSurfaces); } // Grass @@ -457,6 +458,10 @@ void Area::loadLYT() { } _rooms.insert(std::make_pair(room->name(), std::move(room))); } + + uniwalkFinalize(_pathfinder.uni); + // Allow up to 64 concurrent paths. + _pathfinder.paths.resize(64); } void Area::loadVIS() { @@ -476,28 +481,6 @@ Visibility Area::fixVisibility(const Visibility &visibility) { return result; } -void Area::loadPTH() { - std::shared_ptr path(_services.resource.paths.get(_name)); - if (!path) { - return; - } - std::unordered_map pointZ; - - auto &sceneGraph = _services.scene.graphs.get(_sceneName); - - for (size_t i = 0; i < path->points.size(); ++i) { - const Path::Point &point = path->points[i]; - Collision collision; - if (!sceneGraph.testElevation(glm::vec3(point.x, point.y, scene::kElevationTestZ), collision)) { - warn(str(boost::format("Point %d elevation not found") % i)); - continue; - } - pointZ.insert(std::make_pair(static_cast(i), collision.intersection.z)); - } - - _pathfinder.load(path->points, pointZ); -} - void Area::initCameras(const glm::vec3 &entryPosition, float entryFacing) { glm::vec3 position(entryPosition); position.z += 1.7f; @@ -877,7 +860,8 @@ void Area::update(float dt) { updateHeartbeat(dt); } -bool Area::moveCreature(const std::shared_ptr &creature, const glm::vec2 &dir, bool run, float dt) { +bool Area::moveCreature(const std::shared_ptr &creature, const glm::vec2 &dir, bool run, float dt, + float maxDistance) { static glm::vec3 up {0.0f, 0.0f, 1.0f}; static glm::vec3 zOffset {0.0f, 0.0f, 0.1f}; @@ -897,6 +881,10 @@ bool Area::moveCreature(const std::shared_ptr &creature, const glm::ve float speed = run ? creature->runSpeed() : creature->walkSpeed(); float speedDt = speed * dt; + if (speedDt > maxDistance) { + speedDt = maxDistance; + } + glm::vec3 dest(origin); dest.x += dir.x * speedDt; dest.y += dir.y * speedDt; @@ -1009,12 +997,6 @@ bool Area::findCreatureCollision( return found; } -bool Area::moveCreatureTowards(const std::shared_ptr &creature, const glm::vec2 &dest, bool run, float dt) { - glm::vec2 delta(dest - glm::vec2(creature->position())); - glm::vec2 dir(glm::normalize(delta)); - return moveCreature(creature, dir, run, dt); -} - bool Area::isObjectSeen(const Creature &subject, const Object &object) const { if (!object.visible()) { return false; diff --git a/src/libs/game/object/creature.cpp b/src/libs/game/object/creature.cpp index d0eaca355..4b3430919 100644 --- a/src/libs/game/object/creature.cpp +++ b/src/libs/game/object/creature.cpp @@ -26,6 +26,7 @@ #include "reone/game/animationutil.h" #include "reone/game/attack.h" #include "reone/game/d20/classes.h" +#include "reone/game/debug.h" #include "reone/game/di/services.h" #include "reone/game/effect/acdecrease.h" #include "reone/game/effect/acincrease.h" @@ -58,6 +59,7 @@ #include "reone/resource/resources.h" #include "reone/resource/strings.h" #include "reone/scene/di/services.h" +#include "reone/scene/drawdebug.h" #include "reone/scene/graphs.h" #include "reone/scene/types.h" #include "reone/script/types.h" @@ -86,6 +88,7 @@ static constexpr int kSituationalAttackBonus = 10; static constexpr float kCloseRangeAttackDistance2 = 25.0f; static constexpr size_t kACBonusTypeCount = static_cast(ACBonus::Deflection) + 1; static constexpr float kKeepPathDuration = 1000.0f; +static constexpr float kPathPointTolerance = 0.5f; static constexpr char kItemPropertyCostTable[] = "iprp_costtable"; static constexpr char kBonusCostTable[] = "iprp_bonuscost"; @@ -527,13 +530,6 @@ Creature::Creature( _perception.hearingRange = 20.0f; } -void Creature::Path::selectNextPoint() { - size_t pointCount = points.size(); - if (pointIdx < pointCount) { - pointIdx++; - } -} - void Creature::loadFromBlueprint(const std::string &resRef) { auto utc = _services.resource.gffs.get(resRef, ResType::Utc); if (!utc) { @@ -995,42 +991,6 @@ void Creature::setMovementType(MovementType type) { _animFireForget = false; } -void Creature::setPath(const glm::vec3 &dest, std::vector &&points, uint32_t timeFound) { - int pointIdx = 0; - if (_path) { - bool lastPointReached = _path->pointIdx == _path->points.size(); - if (lastPointReached) { - float nearestDist = INFINITY; - for (int i = 0; i < points.size(); ++i) { - float dist = glm::distance2(_path->destination, points[i]); - if (dist < nearestDist) { - nearestDist = dist; - pointIdx = i; - } - } - } else { - const glm::vec3 &nextPoint = _path->points[_path->pointIdx]; - for (int i = 0; i < points.size(); ++i) { - if (points[i] == nextPoint) { - pointIdx = i; - break; - } - } - } - } - auto path = std::make_unique(); - path->destination = dest; - path->points = points; - path->timeFound = timeFound; - path->pointIdx = pointIdx; - - _path = std::move(path); -} - -void Creature::clearPath() { - _path.reset(); -} - glm::vec3 Creature::getSelectablePosition() const { auto model = std::static_pointer_cast(_sceneNode); if (!model) { @@ -2254,10 +2214,65 @@ void Creature::takeGold(int amount) { _gold -= amount; } +glm::vec3 Creature::computeSteeringForce(const Uniwalk &uni, const glm::vec3 &next, float dt) { + glm::vec3 desiredForce = glm::normalize(next - _position); + glm::vec3 keepoutForce = computeKeepoutForce(uni, _position); + + // If we're not making progress - move in a random direction and + // hope. If we wander off too far, the path will be recalculated. + if (!_stuckTimer.elapsed() || glm::length2(_position - _previousPosition) < 0.0001) { + // Try to unstuck for some time even if we're moving + // again. Otherwise desiredForce kicks in again next frame. + if (_stuckTimer.elapsed()) { + _stuckTimer.reset(1.0f); + _stuckForce = + glm::normalize(glm::vec3 { + randomFloat(-1.0f, 1.0f), + randomFloat(-1.0f, 1.0f), + randomFloat(-1.0f, 1.0f), + }); + } else { + _stuckTimer.update(dt); + } + desiredForce = glm::vec3 {0.0f, 0.0f, 0.0f}; + } else { + _stuckForce = glm::vec3 {0.0f, 0.0f, 0.0f}; + } + _previousPosition = _position; + + glm::vec3 combinedForce = desiredForce + 0.1f * keepoutForce + _stuckForce; + + drawdebug::pushId("computeSteeringForce"); + drawdebug::pushId(_id); + drawdebug::clear(); + + if (isShowPathEnabled()) { + drawdebug::line(_position, _position + desiredForce, 0x00BFFFFF, 0.02); + drawdebug::line(_position, _position + keepoutForce, 0xDDA0DDFF, 0.02); + drawdebug::line(_position, _position + _stuckForce, 0xF08080FF, 0.02); + drawdebug::line(_position, _position + combinedForce, 0xADFF2FFF, 0.02); + } + + drawdebug::popId(); + drawdebug::popId(); + + return combinedForce; +} + bool Creature::navigateTo(const glm::vec3 &dest, bool run, float distance, float dt) { if (_movementRestricted) return false; + auto module = _game.module(); + if (!module || !module->area()) { + // Navigation without a module does not make sense. This is only useful + // for unit tests. + return true; + } + + Pathfinder &pf = module->area()->pathfinder(); + + // Stop if we reached the destination. float distToDest2 = getSquareDistanceTo(glm::vec2(dest)); if (distToDest2 <= distance * distance) { setMovementType(Creature::MovementType::None); @@ -2265,62 +2280,65 @@ bool Creature::navigateTo(const glm::vec3 &dest, bool run, float distance, float return true; } - bool updPath = true; - if (_path) { - uint32_t now = _services.system.clock.millis(); - if (_path->destination == dest || now - _path->timeFound <= kKeepPathDuration) { - advanceOnPath(run, dt); - updPath = false; - } + float eps2 = std::min(0.5f * 0.5f, distance * distance); + if (_path && getSquareDistanceTo(getLastPathPoint(pf, *_path)) < eps2) { + // Reached the last point, but not reached the destination. Find another + // path. + releasePath(pf, *_path); + _path = std::nullopt; } - if (updPath) { - updatePath(dest); + + if (_path && !updatePath(pf, *_path, position())) { + // Lost the path and cannot recalculate. + releasePath(pf, *_path); + _path = std::nullopt; + return false; } - return false; -} + // Advance on path. + if (_path) { + glm::vec3 steeringForce = computeSteeringForce(pf.uni, getNextPathPoint(pf, *_path), dt); + _pathVelocity += steeringForce * dt; -void Creature::advanceOnPath(bool run, float dt) { - const glm::vec3 &origin = _position; - size_t pointCount = _path->points.size(); - glm::vec3 dest; - float distToDest; + float maxSpeed = 0.5f; + float speed = glm::min(glm::length(_pathVelocity), maxSpeed); + _pathVelocity = glm::normalize(_pathVelocity) * speed; - if (_path->pointIdx == pointCount) { - dest = _path->destination; - distToDest = glm::distance2(origin, dest); + glm::vec3 dir = glm::normalize(_pathVelocity); + advanceOnPath(dest, dir, run, distance, dt); + return false; + } - } else { - const glm::vec3 &nextPoint = _path->points[_path->pointIdx]; - float distToNextPoint = glm::distance2(origin, nextPoint); - float distToPathDest = glm::distance2(origin, _path->destination); + // Find a path and start following it. + _path = createPath(pf, position(), dest); + if (!_path) { + return false; + } + _pathVelocity = {0.0f, 0.0f, 0.0f}; - if (distToPathDest < distToNextPoint) { - dest = _path->destination; - distToDest = distToPathDest; - _path->pointIdx = static_cast(pointCount); + return navigateTo(dest, run, distance, dt); +} - } else { - dest = nextPoint; - distToDest = distToNextPoint; - } - } +void Creature::advanceOnPath(const glm::vec3 &dest, const glm::vec3 &dir, bool run, float distance, float dt) { + setMovementType(run ? Creature::MovementType::Run : Creature::MovementType::Walk); + _game.module()->area()->moveCreature( + _game.getObjectById(_id), dir, run, dt, getDistanceTo(dest)); - if (distToDest <= 1.0f) { - _path->selectNextPoint(); - } else { - std::shared_ptr creature(_game.getObjectById(_id)); - if (_game.module()->area()->moveCreatureTowards(creature, dest, run, dt)) { - setMovementType(run ? Creature::MovementType::Run : Creature::MovementType::Walk); - } else { - setMovementType(Creature::MovementType::None); - } - // Report a door that obstructed this step. A door can stop the creature - // from making progress while the slide in moveCreature still produces - // some sideways motion, so this is keyed on the recorded obstruction - // rather than on whether the step moved the creature at all. - dispatchBlockedEvent(); + // Report a door that obstructed this step. A door can stop the creature + // from making progress while the slide in moveCreature still produces + // some sideways motion, so this is keyed on the recorded obstruction + // rather than on whether the step moved the creature at all. + dispatchBlockedEvent(); +} + +void Creature::clearPath() { + if (!_path) { + return; } + + Pathfinder &pf = _game.module()->area()->pathfinder(); + releasePath(pf, *_path); + _path = std::nullopt; } void Creature::dispatchBlockedEvent() { @@ -2338,12 +2356,6 @@ void Creature::dispatchBlockedEvent() { runBlockedScript(_blockingDoorId); } -void Creature::updatePath(const glm::vec3 &dest) { - std::vector points(_game.module()->area()->pathfinder().findPath(_position, dest)); - uint32_t now = _services.system.clock.millis(); - setPath(dest, std::move(points), now); -} - std::string Creature::getAnimationName(AnimationType anim) const { std::string result; switch (anim) { diff --git a/src/libs/game/pathfinder.cpp b/src/libs/game/pathfinder.cpp index 987e986bb..5db21d688 100644 --- a/src/libs/game/pathfinder.cpp +++ b/src/libs/game/pathfinder.cpp @@ -15,168 +15,749 @@ * along with this program. If not, see . */ -#include "reone/game/pathfinder.h" +/** + == Summary + + Pathfinding is implemented as a collection of algorithms: + + 1. Global pathfinding algorithm builds a coarse-grained path from A to B as a + sequence of walkmesh faces. It ensures that a path exists, but makes no + effort to make it "natural" from the player perspective. + + 2. Funnel algorithm takes a sequence of faces from the global pathfinding, + and attempts to make a straight-line path through it. + + 3. Steering behaviour takes a direction from the funnel algorithm, and turns + it into a "driving" force. It then calculates and combines other forces: + + - "keepout" force to steer away from borders and corners + - "stuck" force to recover from pathfinding failures. + + The combined force is then integrated to smooth changes of direction and make + the path more natural. + + == Data structures + + The primary input for these algorithms is a Uniwalk. It is a combined mesh of + all room walkmeshes of an area. Walkmeshes are loaded into an area Uniwalk, + and then combined using uniwalkFinalize. Uniwalk still tracks what faces + belong to what room, and computes room AABBs to make spatial queries faster. + + Pathfinder struct keeps the Uniwalk, global pathfinding context, and a list + of active paths. Global pathfinding is expensive, so the implementation puts + an arbitrary limit on the maximum number of active paths. AStarPath objects + are re-used to avoid frequent re-allocation. + + == Omissions -using namespace reone::resource; + 1. Dynamic objects (creatures) are not handled. Each creature has a "personal + space" radius that must be taken into account for pathfinding. + + 2. Static objects that do not have a carve-out for them in the room walkmesh + are not reflected in the Uniwalk yet. Each object has it is own + "non-walkable" mesh, which must be subtracted from the room mesh. + */ + +#include "reone/game/pathfinder.h" +#include "reone/game/debug.h" +#include "reone/scene/drawdebug.h" namespace reone { namespace game { -const Pathfinder::ContextVertex &Pathfinder::Context::getVertexWithLeastTotalCostFromOpen() const { - uint16_t bestIdx = 0xffff; - float bestTotalCost = std::numeric_limits().max(); +static const uint32_t kTriangleEdges[3][2] = {{0, 1}, {1, 2}, {2, 0}}; + +// Import a walkmesh of a single room into Uniwalk structure. Once all rooms are +// imported, call uniwalkFinalize to establish connections between rooms. +void uniwalkLoadRoom(struct Uniwalk &uni, graphics::Walkmesh &data, std::set &walkableMaterial) { + // Base of the vertex array for this room. + uint32_t beginVertex = uni.vertices.size(); + + // Copy all vertices without de-duplicating them with the previously loaded + // meshes. Calculate AABB. + glm::vec3 min = {FLT_MAX, FLT_MAX, FLT_MAX}; + glm::vec3 max = {FLT_MIN, FLT_MIN, FLT_MIN}; + uni.vertices.reserve(uni.vertices.size() + data.vertices.size()); + for (glm::vec3 v : data.vertices) { + uni.vertices.push_back(v); + min = glm::min(min, v); + max = glm::max(max, v); + } + + // Expand AABB a bit to avoid rounding errors. + glm::vec3 expand = {0.1f, 0.1f, 0.1f}; + min -= expand; + max += expand; + + // Base of the face array for this room. + uint32_t beginFace = uni.faces.size(); + + // Copy all walkable faces. + uni.faces.reserve(uni.faces.size() + data.faces.size()); + for (uint32_t i = 0; i < data.faces.size(); ++i) { + uint32_t material = data.materials[i]; + if (!walkableMaterial.count(material)) { + continue; + } - for (uint16_t idx : open) { - const ContextVertex &vert = vertices.find(idx)->second; - if (bestIdx == 0xffff || vert.totalCost < bestTotalCost) { - bestIdx = idx; - bestTotalCost = vert.totalCost; + const graphics::Walkmesh::FaceVertices inputFace = data.faces[i]; + Uniface face = {0}; + face.centroid = {0.0f, 0.0f, 0.0f}; + for (uint32_t i = 0; i < 3; ++i) { + uint32_t index = inputFace.indices[i] + beginVertex; + face.vertices[i] = index; + face.adjecent[i] = UINT32_MAX; + face.centroid += uni.vertices[index]; } + face.centroid *= 0.333333f; + uni.faces.push_back(face); } - return vertices.find(bestIdx)->second; + uint32_t endFace = uni.faces.size(); + Uniroom room = { + beginFace, + endFace, + min, max}; + + uni.rooms.push_back(room); +} + +static void drawDebugFace(const Uniwalk &uni, uint32_t index) { + uint32_t colorAdj = 0x0000FFFF; + uint32_t colorBorder = 0x9400D3FF; + float thickness = 0.03f; + + const Uniface &face = uni.faces[index]; + bool isBorderFace = false; + for (uint32_t i = 0; i < 3; ++i) { + uint32_t v0 = face.vertices[kTriangleEdges[i][0]]; + uint32_t v1 = face.vertices[kTriangleEdges[i][1]]; + uint32_t color = colorAdj; + if (face.adjecent[i] == UINT32_MAX) { + isBorderFace = true; + color = colorBorder; + } + drawdebug::line(uni.vertices[v0], uni.vertices[v1], color, thickness); + } + + glm::vec3 textOffset(0.0f, 0.0f, 0.5f); + uint32_t color = isBorderFace ? colorBorder : colorAdj; + drawdebug::text(std::to_string(index), face.centroid + textOffset, color); } -void Pathfinder::load(const std::vector &points, const std::unordered_map &pointZ) { - for (uint16_t i = 0; i < points.size(); ++i) { - float z = pointZ.count(i) > 0 ? pointZ.at(i) : 0.0f; +static void drawDebugUniwalk(const Uniwalk &uni) { + drawdebug::pushId("uniwalk"); + drawdebug::clear(); + + if (!isShowPathEnabled()) { + drawdebug::popId(); + return; + } - const auto &point = points[i]; - glm::vec3 pointVec(point.x, point.y, z); - _vertices.push_back(pointVec); + for (uint32_t i = 0; i < uni.faces.size(); ++i) { + // Draw contour and index of each face. + drawDebugFace(uni, i); - glm::vec3 adjPointVec; - for (auto &adjPointIdx : point.adjPoints) { - const Path::Point &adjPoint = points[adjPointIdx]; - _adjacentVertices[i].push_back(static_cast(adjPointIdx)); + // Draw connections to adjecent faces. + for (uint32_t j = 0; j < 3; ++j) { + + uint32_t adj = uni.faces[i].adjecent[j]; + if (adj == UINT32_MAX) { + continue; + } + glm::vec3 centroidAdj = uni.faces[adj].centroid; + drawdebug::line(uni.faces[i].centroid, centroidAdj, 0x00ff00ff, 0.02f); } } + + drawdebug::popId(); } -const std::vector Pathfinder::findPath(const glm::vec3 &from, const glm::vec3 &to) const { - // When there are no vertices, return a path of start and end points - if (_vertices.empty()) { - return std::vector {from, to}; +/// Mark edges that are shared between two faces. +static void markAdjecent(Uniwalk &uni, uint32_t faceA, uint32_t faceB) { + Uniface &a = uni.faces[faceA]; + Uniface &b = uni.faces[faceB]; + + for (uint32_t i = 0; i < 3; ++i) { + for (uint32_t j = 0; j < 3; ++j) { + + uint32_t i0 = a.vertices[kTriangleEdges[i][0]]; + uint32_t i1 = a.vertices[kTriangleEdges[i][1]]; + + uint32_t j0 = b.vertices[kTriangleEdges[j][0]]; + uint32_t j1 = b.vertices[kTriangleEdges[j][1]]; + + if (i0 != j1 || i1 != j0) { + continue; + } + + // i-th edge of face A is the same as j-th edge of face B. + a.adjecent[i] = faceB; + b.adjecent[j] = faceA; + } } +} - // Find vertices nearest to start and end points - uint16_t fromIdx = getNearestVertexBetweenPoints(from, to); - uint16_t toIdx = getNearestVertexBetweenPoints(to, from); +void uniwalkFinalize(struct Uniwalk &uni) { + // Deduplicate vertices, so that the same vertex gets the same index in all + // faces. This way we can compare indices of two vertices instead of + // calculating distance between them every time. + const float eps2 = 0.1f * 0.1f; + std::vector dedup(uni.vertices.size(), UINT32_MAX); + for (uint32_t i = 0; i < uni.vertices.size(); ++i) { + if (dedup[i] != UINT32_MAX) { + // This vertex, and all following vertices that are close to it are + // already deduplicated. + continue; + } + for (uint32_t j = i + 1; j < uni.vertices.size(); ++j) { + if (glm::distance2(uni.vertices[i], uni.vertices[j]) < eps2) { + dedup[j] = i; + } + } + } + for (Uniface &face : uni.faces) { + for (uint32_t &v : face.vertices) { + if (dedup[v] != UINT32_MAX) { + v = dedup[v]; + } + } + } - // When start and end point have a common nearest vertex, return a path of start and end point - if (fromIdx == toIdx) { - return std::vector {from, to}; + // For each faces, find faces that are adjecent to it. + for (uint32_t i = 0; i < uni.faces.size(); ++i) { + for (uint32_t j = i + 1; j < uni.faces.size(); ++j) { + markAdjecent(uni, i, j); + } } - Context ctx; + drawDebugUniwalk(uni); +} - // Add vertex, nearest to start point, to open list - ContextVertex fromVert; - fromVert.index = fromIdx; - ctx.vertices.insert(std::make_pair(fromIdx, fromVert)); - ctx.open.insert(fromIdx); +struct CastResult { + glm::vec3 intersection; + bool intersects; +}; + +static CastResult castFace(const Uniwalk &uni, uint32_t face, glm::vec3 position) { + // Offset position slightly to prevent rounding errors. + glm::vec3 castPosition = position + glm::vec3(0.0f, 0.0f, 0.1f); + float maxDistance = 100.0f; + float distance = 0.0f; + glm::vec2 baryPosition(0.0f); + glm::vec3 down(0.0f, 0.0f, -1.0f); + + const uint32_t *indices = uni.faces[face].vertices; + glm::vec3 v0 = uni.vertices[indices[0]]; + glm::vec3 v1 = uni.vertices[indices[1]]; + glm::vec3 v2 = uni.vertices[indices[2]]; + + bool intersects = glm::intersectRayTriangle(castPosition, down, v0, v1, v2, baryPosition, distance); + glm::vec3 intersection = castPosition + down * distance; + return {intersection, intersects}; +} - while (!ctx.open.empty()) { - // Extract vertex with least total cost from open list - const ContextVertex ¤t = ctx.getVertexWithLeastTotalCostFromOpen(); - ctx.open.erase(current.index); - - // Add current vertex to closed list - ctx.closed.insert(current.index); - - // Reconstruct path if current vertex is nearest to end point - if (current.index == toIdx) { - std::vector path; - uint16_t idx = current.index; - do { - const ContextVertex &vert = ctx.vertices.find(idx)->second; - path.push_back(_vertices[vert.index]); - idx = vert.parentIndex; - } while (idx != 0xffff); - reverse(path.begin(), path.end()); - return path; - } - - // Skip current vertex if it has no adjacent vertices - auto maybeAdjVerts = _adjacentVertices.find(current.index); - if (maybeAdjVerts == _adjacentVertices.end()) +static uint32_t findFaceAt(const Uniwalk &uni, glm::vec3 position) { + for (const Uniroom &room : uni.rooms) { + // Skip the room if the if the position is not in its AABB. + if (glm::any(glm::greaterThan(position, room.max)) || glm::any(glm::lessThan(position, room.min))) { continue; + } + + // Raycast to each face in the room. + for (uint32_t i = room.begin; i < room.end; ++i) { + CastResult res = castFace(uni, i, position); + if (res.intersects) { + return i; + } + } + } + + return UINT32_MAX; +} - for (auto &adjVertIdx : maybeAdjVerts->second) { - // Skip adjacent vertex if it is present in closed list - if (ctx.closed.count(adjVertIdx) > 0) +/// Find a face with the minimum cost and remove it from the open list. +static AStarOpenFace popOpenFace(std::vector &open) { + assert(open.size() >= 1); + if (open.size() == 1) { + AStarOpenFace face = open[0]; + open.resize(0); + return face; + } + + float minCost = FLT_MAX; + uint32_t minIndex = UINT32_MAX; + for (uint32_t i = 0; i < open.size(); ++i) { + if (open[i].cost < minCost) { + minCost = open[i].cost; + minIndex = i; + } + } + + AStarOpenFace face = open[minIndex]; + + // Remove the face from the list. + uint32_t lastIndex = open.size() - 1; + if (lastIndex != minIndex) { + std::swap(open[minIndex], open[lastIndex]); + } + open.resize(open.size() - 1); + + return face; +} + +static bool findPathAStar(AStarPath &path, AStarContext &ctx, const Uniwalk &uni, + uint32_t fromFace, uint32_t toFace) { + // Reset the context. + ctx.state.resize(uni.faces.size()); + memset(&ctx.state[0], 0xFF, sizeof(ctx.state[0]) * ctx.state.size()); + ctx.open.resize(0); + + // Initialize the algorithm. Find a reverse path - a path from toFace to + // fromFace. When the algorithm reaches the fromFace, it backtracks and + // reverses the path, so it transforms to natural order [fromFace, toFace]. + ctx.state[toFace].flag = AStarFace::Open; + ctx.open.push_back({toFace, 0.0f}); + + while (!ctx.open.empty()) { + // Extract face with least total cost from open list and close it. + AStarOpenFace current = popOpenFace(ctx.open); + ctx.state[current.index].flag = AStarFace::Closed; + + if (current.index == fromFace) { + // Reached the destination face. Now reconstruct the path by + // following the parents. + uint32_t index = fromFace; + path.faces.resize(0); + while (index != UINT32_MAX) { + path.faces.push_back(index); + index = ctx.state[index].parent; + } + return true; + } + + for (uint32_t adj : uni.faces[current.index].adjecent) { + if (adj == UINT32_MAX) { continue; + } - ContextVertex child; - child.index = adjVertIdx; - child.parentIndex = current.index; - child.distance = current.distance + glm::distance2(_vertices[current.index], _vertices[adjVertIdx]); - child.heuristic = glm::distance2(_vertices[child.index], _vertices[toIdx]); - child.totalCost = child.distance + child.heuristic; - - // Do nothing if adjacent vertex is present in open list and computed distance is greater - auto maybeOpenAdjVert = ctx.open.find(adjVertIdx); - if (maybeOpenAdjVert != ctx.open.end()) { - const ContextVertex &openAdjVert = ctx.vertices.find(*maybeOpenAdjVert)->second; - if (child.distance > openAdjVert.distance) - continue; + // Skip adjacent vertex if it is closed. + if (ctx.state[adj].flag == AStarFace::Closed) { + continue; } - // Insert or update adjacent vertex in open list - ctx.vertices.insert(std::make_pair(adjVertIdx, child)); - ctx.open.insert(adjVertIdx); + glm::vec3 currentCentroid = uni.faces[current.index].centroid; + glm::vec3 adjCentroid = uni.faces[adj].centroid; + float distance = glm::distance2(currentCentroid, adjCentroid); + float heuristic = glm::distance2(adjCentroid, uni.faces[toFace].centroid); + float cost = current.cost + distance + heuristic; + + if (ctx.state[adj].flag == AStarFace::Open) { + bool foundOpen = false; + for (AStarOpenFace &open : ctx.open) { + if (open.index != adj) { + continue; + } + foundOpen = true; + + if (cost < open.cost) { + // Adjecent face is "best" reachable from the current face. + open.cost = cost; + ctx.state[adj].parent = current.index; + } + break; + } + assert(foundOpen); + } else { + // Open the adjecent face. + ctx.open.push_back({adj, cost}); + ctx.state[adj] = {AStarFace::Open, current.index}; + } } } - // Return a path of start and end points by default - return std::vector {from, to}; + // Path not found. + return false; } -uint16_t Pathfinder::getNearestVertex(const glm::vec3 &point) const { - uint16_t index = 0xffff; - float minDist = 0.0f; +/// Portal is an edge between two consecutive faces on a path. +struct Portal { + uint32_t left; + uint32_t right; +}; - for (int i = 0; i < _vertices.size(); ++i) { - float dist = glm::distance2(point, _vertices[i]); +/// Find an edge between \p face and \p nextFace. +static Portal findPortal(const Uniwalk &uni, uint32_t face, uint32_t nextFace) { + uint32_t found[2]; + uint32_t numFound = 0; - if (index == 0xffff || dist < minDist) { - index = i; - minDist = dist; + for (uint32_t vi : uni.faces[face].vertices) { + for (uint32_t vj : uni.faces[nextFace].vertices) { + if (vi != vj) { + continue; + } + found[numFound] = vi; + ++numFound; + if (numFound == 2) { + return Portal {found[0], found[1]}; + } + } + } + assert(0 && "faces are not adjecent"); + return Portal {0, 0}; +} + +/// Funnel is a segment of a path where all portals are reachable in a straight +/// line from the base. +struct Funnel { + // Apex of the funnel + glm::vec3 base; + + // Left and right edges of the funnel. + uint32_t left; + uint32_t right; + + // Last processed portal + Portal portal; +}; + +/// FunnelUpdate describes how a funnel changes with the next portal. + +/// A funnel can only narrow, so attempts to expand a funnel should be +/// ignored. A funnel collapses when it has to go around the corner (i.e. when +/// the right edge goes over the left edge and vise versa). +struct FunnelUpdate { + enum Change { + Expand, + Narrow, + Corner, + }; + + // Direction of Expand or Corner. + enum Direction { + Left, + Right, + }; + + Change change; + Direction direction; +}; + +static FunnelUpdate checkFunnelUpdate(Funnel funnel, Portal portal, const Uniwalk &uni) { + glm::vec3 left = uni.vertices[funnel.left] - funnel.base; + glm::vec3 right = uni.vertices[funnel.right] - funnel.base; + + glm::vec3 portLeft = uni.vertices[portal.left] - funnel.base; + glm::vec3 portRight = uni.vertices[portal.right] - funnel.base; + + if (funnel.portal.left != portal.left) { + float leftToPort = glm::cross(left, portLeft).z; + float leftToRight = glm::cross(left, right).z; + if ((leftToPort * leftToRight) >= 0.0f) { + float rightToPort = glm::cross(right, portLeft).z; + float portToLeft = glm::cross(portLeft, left).z; + if ((rightToPort * portToLeft) >= 0) { + // Portal left narrows the funnel. + return {FunnelUpdate::Narrow, FunnelUpdate::Left}; + } + // Portal left went over the right edge - mark the right point as + // the corner. + return {FunnelUpdate::Corner, FunnelUpdate::Right}; + } + // Funnel tries to expand to the left. + return {FunnelUpdate::Expand, FunnelUpdate::Left}; + } + + float rightToPort = glm::cross(right, portRight).z; + float rightToLeft = glm::cross(right, left).z; + if ((rightToPort * rightToLeft) >= 0.0f) { + float leftToPort = glm::cross(left, portRight).z; + float portToRight = glm::cross(portRight, right).z; + if ((leftToPort * portToRight) >= 0) { + // Portal right narrows the funnel. + return {FunnelUpdate::Narrow, FunnelUpdate::Right}; + } + // Portal right went over the right edge - mark left point as the + // corner and restart the algorithm. + return {FunnelUpdate::Corner, FunnelUpdate::Left}; + } + // Funnel tries to expand to the right. + return {FunnelUpdate::Expand, FunnelUpdate::Right}; +} + +static void drawDebugFunnel(const Funnel &funnel, const Uniwalk &uni, int32_t id) { + drawdebug::pushId(id); + drawdebug::pushId("funnel"); + drawdebug::clear(); + if (isShowPathEnabled()) { + drawdebug::line(funnel.base, uni.vertices[funnel.left], 0xC0FF3EFF, 0.1f); + drawdebug::line(funnel.base, uni.vertices[funnel.right], 0xFF7F24FF, 0.1f); + } + drawdebug::popId(); + drawdebug::popId(); +} + +/// Find a straight line path from the current point through a complete path. +/// If there is a corner that makes a straight path imposible, return a midpoint +/// of the farthest edge on the path that we can reach with a straight line. +static glm::vec3 funnelPath(AStarPath &path, const Uniwalk &uni, const glm::vec3 ¤t) { + Funnel funnel; + funnel.base = current; + + bool initialized = false; + for (; path.next + 1 < path.faces.size(); ++path.next) { + Portal portal = findPortal(uni, path.faces[path.next], path.faces[path.next + 1]); + if (!initialized) { + funnel.left = portal.left; + funnel.right = portal.right; + funnel.portal = portal; + initialized = true; + continue; + } + + // Keep left/right consistent. + if ((funnel.portal.left == portal.right) || funnel.portal.right == portal.left) { + std::swap(portal.left, portal.right); + } + + FunnelUpdate upd = checkFunnelUpdate(funnel, portal, uni); + funnel.portal = portal; + switch (upd.change) { + case FunnelUpdate::Expand: { + // Never expand the funnel. + break; + } + case FunnelUpdate::Narrow: { + switch (upd.direction) { + case FunnelUpdate::Left: { + funnel.left = portal.left; + break; + } + case FunnelUpdate::Right: { + funnel.right = portal.right; + break; + } + } + break; + } + case FunnelUpdate::Corner: { + // Path goes around a corner. Pick a midpoint between funnel edges + // to avoid getting stuck at a corner. + glm::vec3 left = uni.vertices[funnel.left]; + glm::vec3 right = uni.vertices[funnel.right]; + drawDebugFunnel(funnel, uni, path.index); + return (left + right) * 0.5f; + } + } + } + + if (!initialized) { + // Trivial path between two points. + return path.to; + } + + // Check that the destination is within the funnel. Otherwise move to the + // funnel edge, same as when we hit a corner. + glm::vec3 left = uni.vertices[funnel.left] - funnel.base; + glm::vec3 right = uni.vertices[funnel.right] - funnel.base; + glm::vec3 last = path.to - funnel.base; + + float leftToLast = glm::cross(left, last).z; + float leftToRight = glm::cross(left, right).z; + float rightToLast = glm::cross(right, last).z; + float rightToLeft = glm::cross(right, left).z; + + drawDebugFunnel(funnel, uni, path.index); + + if ((leftToLast * leftToRight) < 0.0f || (rightToLast * rightToLeft) < 0.0f) { + // Hit a corner. + return (uni.vertices[funnel.left] + uni.vertices[funnel.right]) * 0.5f; + } + + return path.to; +} + +static bool findPath(AStarPath &path, AStarContext &astar, const Uniwalk &uni, + const glm::vec3 &from, const glm::vec3 &to) { + uint32_t fromFace = findFaceAt(uni, from); + uint32_t toFace = findFaceAt(uni, to); + + if (fromFace == UINT32_MAX || toFace == UINT32_MAX) { + return false; + } + + bool found = findPathAStar(path, astar, uni, fromFace, toFace); + if (!found) { + return false; + } + + drawdebug::pushId(path.index); + drawdebug::pushId("findPath"); + drawdebug::clear(); + if (isShowPathEnabled()) { + for (uint32_t i = 1; i < path.faces.size(); ++i) { + glm::vec3 from = uni.faces[path.faces[i - 1]].centroid; + glm::vec3 to = uni.faces[path.faces[i]].centroid; + drawdebug::line(from, to, 0xCD2626FF, 0.1); + } + } + drawdebug::popId(); + drawdebug::popId(); + + path.from = from; + path.to = to; + path.next = 0; + path.active = true; + path.nextPoint = funnelPath(path, uni, from); + return true; +} + +std::optional createPath(Pathfinder &pf, const glm::vec3 &from, const glm::vec3 &to) { + for (int32_t i = 0; i < pf.paths.size(); ++i) { + AStarPath &path = pf.paths[i]; + if (path.active) { + continue; + } + + bool found = findPath(path, pf.astar, pf.uni, from, to); + if (!found) { + return std::nullopt; } + path.index = i; + return Path {i}; } - return index; + return std::nullopt; } -// Return the nearest vertex that is somewhere between the two points. If there -// is no suitable vertex, returns getNearestVertex(point). -uint16_t Pathfinder::getNearestVertexBetweenPoints(const glm::vec3 &point, const glm::vec3 &ref) const { - uint16_t index = 0xffff; - float minDistToPoint = 0.0f; +void releasePath(Pathfinder &pf, Path path) { + pf.paths[path.index].active = false; + + if (isShowPathEnabled()) { + drawdebug::pushId(path.index); + + drawdebug::pushId("findPath"); + drawdebug::clear(); + drawdebug::popId(); + + drawdebug::pushId("funnel"); + drawdebug::clear(); + drawdebug::popId(); + + drawdebug::pushId("updatePath"); + drawdebug::clear(); + drawdebug::popId(); + + drawdebug::popId(); + } +} - float maxDist = glm::distance2(point, ref); +bool updatePath(Pathfinder &pf, Path p, const glm::vec3 ¤t) { + AStarPath &path = pf.paths[p.index]; - for (int i = 0; i < _vertices.size(); ++i) { - float distToPoint = glm::distance2(point, _vertices[i]); - float distToRef = glm::distance2(ref, _vertices[i]); + uint32_t checkFrom = (path.next == 0) ? path.next : path.next - 1; + uint32_t checkTo = path.next + 1; - bool init = index == 0xffff; - bool foundNearest = distToPoint < minDistToPoint; - bool isBetweenPoints = distToRef <= maxDist; + bool isOnPath = false; + uint32_t currentFace = UINT32_MAX; + for (uint32_t i = checkFrom; i < checkTo; ++i) { + CastResult res = castFace(pf.uni, path.faces[i], current); + if (res.intersects) { + isOnPath = true; + break; + } + } + if (!isOnPath) { + // Wandered off the path, recalculate. + bool foundNewPath = findPath(path, pf.astar, pf.uni, current, path.to); + if (!foundNewPath) { + return false; + } + } - if (init || (foundNearest && isBetweenPoints)) { - index = i; - minDistToPoint = distToPoint; + float eps2 = 0.01f; + if (path.next < path.faces.size()) { + if (glm::distance2(path.nextPoint, current) < eps2) { + // Reached an intermediate point, move to the next. + path.nextPoint = funnelPath(path, pf.uni, current); } } - if (index == 0xffff) { - // Fallback to a simple comparison if there is no vertex between points. - return getNearestVertex(point); + drawdebug::pushId(p.index); + drawdebug::pushId("updatePath"); + drawdebug::clear(); + + if (isShowPathEnabled()) { + drawdebug::line(current, path.nextPoint, 0xF0E68CFF, 0.05); + } + + drawdebug::popId(); + drawdebug::popId(); + + return true; +} + +glm::vec3 getNextPathPoint(Pathfinder &pf, Path p) { + AStarPath &path = pf.paths[p.index]; + return path.nextPoint; +} + +glm::vec3 getLastPathPoint(Pathfinder &pf, Path path) { + return pf.paths[path.index].to; +} + +// Keepout vector points away from a border edge or a corner vertex. +glm::vec3 computeKeepoutForce(const Uniwalk &uni, const glm::vec3 &position) { + float keepoutDistance2 = 4.0f; + float keepoutCornerDistance2 = 2.0f; + glm::vec3 keepout = {0.0f, 0.0f, 0.0f}; + + for (const Uniroom &room : uni.rooms) { + if (glm::any(glm::greaterThan(position, room.max)) || glm::any(glm::lessThan(position, room.min))) { + // Not in this room. + continue; + } + for (uint32_t i = room.begin; i < room.end; ++i) { + const Uniface &face = uni.faces[i]; + for (uint32_t j = 0; j < 3; ++j) { + if (face.adjecent[j] != UINT32_MAX) { + // Edges to other walkable faces do not contribute to + // keepout. + continue; + } + glm::vec3 v0 = uni.vertices[face.vertices[kTriangleEdges[j][0]]]; + glm::vec3 v1 = uni.vertices[face.vertices[kTriangleEdges[j][1]]]; + glm::vec3 edge = v0 - v1; + float edgeLen2 = glm::length2(edge); + float projRatio = glm::dot(edge, position - v1) / edgeLen2; + if (projRatio > 0.0f && projRatio < 1.0f) { + // Current position projects to the edge. + glm::vec3 proj = v1 + edge * projRatio; + glm::vec3 fromEdge = position - proj; + float fromEdgeLength2 = glm::length2(fromEdge); + if (fromEdgeLength2 < keepoutDistance2) { + keepout += fromEdge / fromEdgeLength2; + } + } + + // Corner points push outside. + glm::vec3 fromCorner0 = position - v0; + float fromCorner0Len2 = glm::length2(fromCorner0); + if (fromCorner0Len2 < keepoutCornerDistance2) { + keepout += fromCorner0 / fromCorner0Len2; + } + + glm::vec3 fromCorner1 = position - v1; + float fromCorner1Len2 = glm::length2(fromCorner1); + if (fromCorner1Len2 < keepoutCornerDistance2) { + keepout += fromCorner1 / fromCorner1Len2; + } + } + } } - return index; + return keepout; } } // namespace game diff --git a/src/libs/game/player.cpp b/src/libs/game/player.cpp index e57aa285d..651cd3812 100644 --- a/src/libs/game/player.cpp +++ b/src/libs/game/player.cpp @@ -147,8 +147,9 @@ void Player::update(float dt) { if (movement) { partyLeader->clearAllActions(); + partyLeader->clearPath(); glm::vec2 dir(glm::normalize(glm::vec2(-glm::sin(facing), glm::cos(facing)))); - _area.moveCreature(partyLeader, dir, !_walk, dt); + _area.moveCreature(partyLeader, dir, !_walk, dt, FLT_MAX); partyLeader->setMovementType(_walk ? Creature::MovementType::Walk : Creature::MovementType::Run); } else if (partyLeader->actions().empty()) { partyLeader->setMovementType(Creature::MovementType::None); diff --git a/test/game/object.cpp b/test/game/object.cpp index 5c3161204..3581621cd 100644 --- a/test/game/object.cpp +++ b/test/game/object.cpp @@ -515,8 +515,8 @@ std::shared_ptr makeMovingCreature( // it exercises the same path AI, scripts and actions take, unlike direct player // locomotion which calls Area::moveCreature. void navigationStep(Creature &creature, const glm::vec3 &dest, float dt = 1.0f) { - creature.setPath(dest, std::vector {dest}, 0); - creature.advanceOnPath(false, dt); + glm::vec3 dir = glm::normalize(dest - creature.position()); + creature.advanceOnPath(dest, dir, /*run=*/false, /*distance=*/0.1f, dt); } std::shared_ptr makeTransitionTriggerGff( diff --git a/test/game/pathfinder.cpp b/test/game/pathfinder.cpp index 5321cbafb..2840561ef 100644 --- a/test/game/pathfinder.cpp +++ b/test/game/pathfinder.cpp @@ -1,5 +1,5 @@ /* - * Copyright (c) 2020-2023 The reone project contributors + * Copyright (c) 2026 The reone project contributors * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -15,63 +15,86 @@ * along with this program. If not, see . */ +#include #include #include "reone/game/pathfinder.h" using namespace reone; using namespace reone::game; -using namespace reone::resource; + +void checkAdjecent(const Uniface &face, std::array ref) { + for (uint32_t i = 0; i < 3; ++i) { + EXPECT_EQ(face.adjecent[i], ref[i]); + } +} TEST(Pathfinder, should_find_shortest_path) { - // given - std::vector points {{0.0f, 0.0f, {1, 4, 5}}, - {1.0f, 0.0f, {0, 2, 4, 5}}, - {2.0f, 0.0f, {1, 3, 6}}, - {3.0f, 0.0f, {2, 7}}, - {0.0f, 1.0f, {0, 1, 5, 8}}, - {1.0f, 1.0f, {0, 1, 4}}, - {2.0f, 1.0f, {2, 10}}, - {3.0f, 1.0f, {3, 11}}, - {0.0f, 2.0f, {4, 12}}, - {1.0f, 2.0f, {10, 13, 14}}, - {2.0f, 2.0f, {6, 9, 13, 14}}, - {3.0f, 2.0f, {7}}, - {0.0f, 3.0f, {8, 13}}, - {1.0f, 3.0f, {9, 10, 12, 14}}, - {2.0f, 3.0f, {9, 10, 13, 15}}, - {3.0f, 3.0f, {14}}}; - std::unordered_map pointToZ {{0, 0.0f}, - {1, 0.0f}, - {2, 0.0f}, - {3, 0.0f}, - {4, 0.0f}, - {5, 0.0f}, - {6, 0.0f}, - {7, 0.0f}, - {8, 0.0f}, - {9, 0.0f}, - {10, 0.0f}, - {12, 0.0f}, - {12, 0.0f}, - {13, 0.0f}, - {14, 0.0f}, - {15, 0.0f}}; + // Build a square subdivided into 8 triangles. + Pathfinder pf; + + pf.uni.vertices = { + // Center. + {1.0f, 1.0f, 0.0f}, // 0 + // Vertices in clockwise order starting from the bottom left. + {0.0f, 0.0f, 0.0f}, // 1 + {0.0f, 1.0f, 0.0f}, // 2 + {0.0f, 2.0f, 0.0f}, // 3 + {1.0f, 2.0f, 0.0f}, // 4 + {2.0f, 2.0f, 0.0f}, // 5 + {2.0f, 1.0f, 0.0f}, // 6 + {2.0f, 0.0f, 0.0f}, // 7 + {1.0f, 0.0f, 0.0f}, // 8 + }; + + // Faces in clockwise order starting from the bottom left. + pf.uni.faces = { + {{0, 1, 2}}, // 0 + {{0, 2, 3}}, // 1 + {{0, 3, 4}}, // 2 + {{0, 4, 5}}, // 3 + {{0, 5, 6}}, // 4 + {{0, 6, 7}}, // 5 + {{0, 7, 8}}, // 6 + {{0, 8, 1}}, // 7 + }; + + // Single room for all faces. + pf.uni.rooms = {{0, 8, {0.0f, 0.0f, 0.0f}, {2.0f, 2.0f, 0.0f}}}; + + // Build adjecency lists. + for (Uniface &face : pf.uni.faces) { + for (uint32_t i = 0; i < 3; ++i) { + face.adjecent[i] = UINT32_MAX; + } + } + uniwalkFinalize(pf.uni); + + checkAdjecent(pf.uni.faces[0], {7, UINT32_MAX, 1}); + checkAdjecent(pf.uni.faces[1], {0, UINT32_MAX, 2}); + checkAdjecent(pf.uni.faces[2], {1, UINT32_MAX, 3}); + checkAdjecent(pf.uni.faces[3], {2, UINT32_MAX, 4}); + checkAdjecent(pf.uni.faces[4], {3, UINT32_MAX, 5}); + checkAdjecent(pf.uni.faces[5], {4, UINT32_MAX, 6}); + checkAdjecent(pf.uni.faces[6], {5, UINT32_MAX, 7}); + checkAdjecent(pf.uni.faces[7], {6, UINT32_MAX, 0}); - Pathfinder pathfinder; - pathfinder.load(points, pointToZ); + pf.paths.resize(1); - glm::vec3 from {1.0f, 1.0f, 0.0f}; - glm::vec3 to {1.0f, 3.0f, 0.0f}; + // Find a path from face 0 to face 5. + glm::vec3 current = {0.2f, 0.8f, 0.0f}; + glm::vec3 dest = {1.8f, 0.8f, 0.0f}; + std::optional path = createPath(pf, current, dest); + EXPECT_TRUE(path); - // when - auto path = pathfinder.findPath(from, to); + // The funnel algorithm should determine that there is a straight line from + // current to dest. + glm::vec3 v0 = getNextPathPoint(pf, *path); + EXPECT_EQ(v0, dest); - // then - EXPECT_EQ(path.size(), 5); - EXPECT_EQ(path.at(0), (glm::vec3 {1.0f, 1.0f, 0.0f})); - EXPECT_EQ(path.at(1), (glm::vec3 {0.0f, 1.0f, 0.0f})); - EXPECT_EQ(path.at(2), (glm::vec3 {0.0f, 2.0f, 0.0f})); - EXPECT_EQ(path.at(3), (glm::vec3 {0.0f, 3.0f, 0.0f})); - EXPECT_EQ(path.at(4), (glm::vec3 {1.0f, 3.0f, 0.0f})); + // The actual path is a sequence of faces. + AStarPath &astarPath = pf.paths[path->index]; + std::vector expectedPath = { + 0, 7, 6, 5}; + EXPECT_EQ(astarPath.faces, expectedPath); }