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/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/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/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/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/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..3581621cd 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; } @@ -513,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); } 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); }