From cebbb8bb5d457c65a9064532d4ce58ed56560dae Mon Sep 17 00:00:00 2001 From: Ariari04 Date: Sun, 26 Apr 2026 22:18:42 +0600 Subject: [PATCH 1/2] Added collision --- src/Character.cpp | 37 ++++++ src/Character.h | 18 ++- src/Location.cpp | 206 +++++++++++++++++++++++++++++++++- src/Location.h | 11 +- src/navigation/PathFinder.cpp | 200 +++++++++++++++++++++++++++++++-- src/navigation/PathFinder.h | 14 +++ 6 files changed, 465 insertions(+), 21 deletions(-) diff --git a/src/Character.cpp b/src/Character.cpp index c374e5f..f4e94d5 100644 --- a/src/Character.cpp +++ b/src/Character.cpp @@ -78,6 +78,43 @@ void Character::setPathPlanner(PathPlanner planner) { pathPlanner = std::move(planner); } +bool Character::isMoving() const +{ + Eigen::Vector3f toTarget = walkTarget - position; + toTarget.y() = 0.f; + return !pathWaypoints.empty() || toTarget.norm() > WALK_THRESHOLD; +} + +Eigen::Vector3f Character::getCurrentNavigationTarget() const +{ + if (!pathWaypoints.empty() && currentWaypointIndex < pathWaypoints.size()) { + return pathWaypoints[currentWaypointIndex]; + } + return walkTarget; +} + +void Character::forceReplan() +{ + if (!pathPlanner) { + return; + } + + const Eigen::Vector3f normalizedTarget(requestedWalkTarget.x(), 0.f, requestedWalkTarget.z()); + pathWaypoints = pathPlanner(position, normalizedTarget); + currentWaypointIndex = 0; + + if (!pathWaypoints.empty()) { + for (Eigen::Vector3f& waypoint : pathWaypoints) { + waypoint.y() = 0.f; + } + walkTarget = pathWaypoints.back(); + return; + } + + walkTarget = Eigen::Vector3f(position.x(), 0.f, position.z()); + onArrivedCallback = nullptr; +} + void Character::setTexture(std::shared_ptr texture) { for (auto& animEntry : animations) { diff --git a/src/Character.h b/src/Character.h index d2e77f5..974729f 100644 --- a/src/Character.h +++ b/src/Character.h @@ -53,11 +53,21 @@ public: void drawShadowDepth(Renderer& renderer); void drawWithShadow(Renderer& renderer, const Eigen::Matrix4f& lightFromCamera, GLuint shadowMapTex, const Eigen::Vector3f& lightDirCamera); + // Character-to-character collision (XZ-plane). Used by Location to keep + // player/NPCs from walking through each other. + float collisionRadius = 0.45f; - // attackDirection is a world-space horizontal vector pointing from the - // attacker toward this character — i.e. the direction the hit pushes - // them. A zero vector disables directional sparks (isotropic burst). - void applyDamage(float damageAmount, const Eigen::Vector3f& attackDirection = Eigen::Vector3f::Zero()); + // Movement/path introspection used for dynamic replanning. + bool isMoving() const; + const Eigen::Vector3f& getRequestedWalkTarget() const { return requestedWalkTarget; } + Eigen::Vector3f getCurrentNavigationTarget() const; + void forceReplan(); + + + // attackDirection is a world-space horizontal vector pointing from the + // attacker toward this character — i.e. the direction the hit pushes + // them. A zero vector disables directional sparks (isotropic burst). + void applyDamage(float damageAmount, const Eigen::Vector3f& attackDirection = Eigen::Vector3f::Zero()); // Configures the per-character hit-spark emitter with the shared spark // texture. Safe to call once per character after construction. diff --git a/src/Location.cpp b/src/Location.cpp index b8a5691..0b4ed07 100644 --- a/src/Location.cpp +++ b/src/Location.cpp @@ -23,6 +23,23 @@ namespace ZL // on_npc_interact callback fires and the conversation begins. static constexpr float NPC_TALK_DISTANCE = 1.25f; + static float distancePointToSegmentXZ(const Eigen::Vector3f& p, + const Eigen::Vector3f& a, + const Eigen::Vector3f& b) + { + const Eigen::Vector2f p2(p.x(), p.z()); + const Eigen::Vector2f a2(a.x(), a.z()); + const Eigen::Vector2f b2(b.x(), b.z()); + const Eigen::Vector2f ab = b2 - a2; + const float abLenSq = ab.squaredNorm(); + if (abLenSq <= 1e-8f) { + return (p2 - a2).norm(); + } + const float t = std::clamp((p2 - a2).dot(ab) / abLenSq, 0.0f, 1.0f); + const Eigen::Vector2f closest = a2 + ab * t; + return (p2 - closest).norm(); + } + Location::Location(Renderer& iRenderer, Inventory& iInventory) : renderer(iRenderer) , inventory(iInventory) @@ -201,17 +218,43 @@ namespace ZL buildDebugNavMeshes(); #endif - auto planner = [this](const Eigen::Vector3f& start, const Eigen::Vector3f& end) { - return navigation.findPath(start, end); + static constexpr float kDynamicObstacleInfluenceDist = 6.0f; + + auto makePlanner = [this](const Character* self) { + return [this, self](const Eigen::Vector3f& start, const Eigen::Vector3f& end) { + std::vector dynamicObstacles; + dynamicObstacles.reserve(npcs.size() + 1); + + const auto addCharacter = [&](const Character* other) { + if (!other || other == self) return; + if (other->hp <= 0.f) return; + + if (distancePointToSegmentXZ(other->position, start, end) > kDynamicObstacleInfluenceDist) { + return; + } + + PathFinder::DynamicObstacle obs; + obs.position = Eigen::Vector3f(other->position.x(), navigation.getFloorY(), other->position.z()); + obs.radius = (std::max)(0.0f, other->collisionRadius); + dynamicObstacles.push_back(obs); + }; + + addCharacter(player.get()); + for (const auto& npc : npcs) { + addCharacter(npc.get()); + } + + return navigation.findPath(start, end, dynamicObstacles); }; + }; if (player) { - player->setPathPlanner(planner); + player->setPathPlanner(makePlanner(player.get())); } for (auto& npc : npcs) { if (npc) { - npc->setPathPlanner(planner); + npc->setPathPlanner(makePlanner(npc.get())); } } } @@ -640,6 +683,156 @@ namespace ZL return navigation.setAreaAvailable(areaName, available); } + void Location::resolveCharacterCollisions() + { + std::vector characters; + characters.reserve(npcs.size() + 1); + + if (player) { + characters.push_back(player.get()); + } + for (auto& npc : npcs) { + if (npc) { + characters.push_back(npc.get()); + } + } + + if (characters.size() < 2) { + return; + } + + static constexpr int kIterations = 3; + static constexpr float kMinSeparationEps = 1e-4f; + + for (int iter = 0; iter < kIterations; ++iter) { + for (size_t i = 0; i < characters.size(); ++i) { + for (size_t j = i + 1; j < characters.size(); ++j) { + Character* a = characters[i]; + Character* b = characters[j]; + if (!a || !b) continue; + if (a->hp <= 0.f || b->hp <= 0.f) continue; + + const float minDist = a->collisionRadius + b->collisionRadius; + if (minDist <= 0.f) continue; + + const Eigen::Vector2f pa(a->position.x(), a->position.z()); + const Eigen::Vector2f pb(b->position.x(), b->position.z()); + const Eigen::Vector2f delta = pb - pa; + const float dist = delta.norm(); + + if (dist >= minDist) { + continue; + } + + Eigen::Vector2f normal(1.f, 0.f); + if (dist > kMinSeparationEps) { + normal = delta / dist; + } + + const float penetration = (minDist - dist); + const float push = penetration * 0.5f; + + Eigen::Vector3f newA = a->position; + Eigen::Vector3f newB = b->position; + newA.x() -= normal.x() * push; + newA.z() -= normal.y() * push; + newA.y() = 0.f; + newB.x() += normal.x() * push; + newB.z() += normal.y() * push; + newB.y() = 0.f; + + if (navigation.isReady()) { + const bool aOk = navigation.isWalkable(newA); + const bool bOk = navigation.isWalkable(newB); + + if (aOk && bOk) { + a->position = newA; + b->position = newB; + } + else if (aOk && !bOk) { + a->position = newA; + } + else if (!aOk && bOk) { + b->position = newB; + } + } + else { + a->position = newA; + b->position = newB; + } + } + } + } + } + + void Location::updateDynamicReplans(int64_t deltaMs) + { + static constexpr float kMovedEps = 0.05f; + static constexpr float kReplanTriggerDist = 1.1f; + static constexpr int64_t kReplanCooldownMs = 300; + + for (auto it = replanCooldownRemainingMs.begin(); it != replanCooldownRemainingMs.end();) { + it->second -= deltaMs; + if (it->second <= 0) { + it = replanCooldownRemainingMs.erase(it); + } + else { + ++it; + } + } + + std::vector characters; + characters.reserve(npcs.size() + 1); + if (player) characters.push_back(player.get()); + for (auto& npc : npcs) if (npc) characters.push_back(npc.get()); + + std::vector movers; + movers.reserve(characters.size()); + for (Character* c : characters) { + if (!c) continue; + auto it = lastCharacterPositions.find(c); + if (it == lastCharacterPositions.end()) { + lastCharacterPositions[c] = c->position; + continue; + } + + const Eigen::Vector3f prev = it->second; + const float moved = (c->position - prev).norm(); + it->second = c->position; + + if (moved > kMovedEps) { + movers.push_back(c); + } + } + + if (movers.empty()) { + return; + } + + for (Character* mover : movers) { + if (!mover) continue; + + for (Character* walker : characters) { + if (!walker || walker == mover) continue; + if (walker->hp <= 0.f) continue; + if (!walker->isMoving()) continue; + + if (replanCooldownRemainingMs.find(walker) != replanCooldownRemainingMs.end()) { + continue; + } + + const Eigen::Vector3f nextTarget = walker->getCurrentNavigationTarget(); + const float distToSegment = distancePointToSegmentXZ(mover->position, walker->position, nextTarget); + if (distToSegment > kReplanTriggerDist) { + continue; + } + + walker->forceReplan(); + replanCooldownRemainingMs[walker] = kReplanCooldownMs; + } + } + } + void Location::update(int64_t delta) { if (player) { @@ -651,6 +844,9 @@ namespace ZL npc->update(delta); } + resolveCharacterCollisions(); + updateDynamicReplans(delta); + // Check if player reached target interactive object if (targetInteractiveObject && player) { float distToObject = (player->position - targetInteractiveObject->position).norm(); @@ -883,4 +1079,4 @@ namespace ZL } -} // namespace ZL \ No newline at end of file +} // namespace ZL diff --git a/src/Location.h b/src/Location.h index ec993cd..63676ba 100644 --- a/src/Location.h +++ b/src/Location.h @@ -13,6 +13,8 @@ #include "dialogue/DialogueSystem.h" #include "SparkEmitter.h" #include +#include +#include namespace ZL { @@ -112,6 +114,13 @@ namespace ZL protected: Renderer& renderer; Inventory& inventory; + + private: + void resolveCharacterCollisions(); + void updateDynamicReplans(int64_t deltaMs); + + std::unordered_map lastCharacterPositions; + std::unordered_map replanCooldownRemainingMs; }; -} // namespace ZL \ No newline at end of file +} // namespace ZL diff --git a/src/navigation/PathFinder.cpp b/src/navigation/PathFinder.cpp index 3131435..20ab1db 100644 --- a/src/navigation/PathFinder.cpp +++ b/src/navigation/PathFinder.cpp @@ -74,8 +74,8 @@ std::vector PathFinder::findPath(const Eigen::Vector3f& start, Cell startCell; Cell endCell; - if (!findNearestWalkableCell(start, startCell) || - !findNearestWalkableCell(end, endCell)) { + if (!findNearestWalkableCell(start, startCell, walkable) || + !findNearestWalkableCell(end, endCell, walkable)) { return {}; } @@ -121,7 +121,7 @@ std::vector PathFinder::findPath(const Eigen::Vector3f& start, for (const auto& offset : offsets) { Cell next{ currentCell.x + offset[0], currentCell.z + offset[1] }; - if (!isCellWalkable(next)) { + if (!isCellWalkable(next, walkable)) { continue; } @@ -129,7 +129,7 @@ std::vector PathFinder::findPath(const Eigen::Vector3f& start, if (diagonal) { Cell horizontal{ currentCell.x + offset[0], currentCell.z }; Cell vertical{ currentCell.x, currentCell.z + offset[1] }; - if (!isCellWalkable(horizontal) || !isCellWalkable(vertical)) { + if (!isCellWalkable(horizontal, walkable) || !isCellWalkable(vertical, walkable)) { continue; } } @@ -160,7 +160,161 @@ std::vector PathFinder::findPath(const Eigen::Vector3f& start, } } std::reverse(cells.begin(), cells.end()); - cells = smoothCells(cells); + cells = smoothCells(cells, walkable); + + std::vector path; + path.reserve(cells.size()); + for (const Cell& cell : cells) { + path.push_back(cellCenter(cell)); + } + + if (!path.empty() && (path.front() - Eigen::Vector3f(start.x(), floorY, start.z())).norm() < cellSize * 0.75f) { + path.erase(path.begin()); + } + + if (!path.empty()) { + Cell requestedEndCell; + if (worldToCell(end, requestedEndCell) && + requestedEndCell.x == endCell.x && + requestedEndCell.z == endCell.z) { + path.back() = Eigen::Vector3f(end.x(), floorY, end.z()); + } + } + + return path; +} + +std::vector PathFinder::findPath(const Eigen::Vector3f& start, + const Eigen::Vector3f& end, + const std::vector& dynamicObstacles) const +{ + if (!ready || walkable.empty()) { + return {}; + } + + std::vector walkableGrid = walkable; + if (!dynamicObstacles.empty()) { + for (const DynamicObstacle& obstacle : dynamicObstacles) { + const float radius = obstacle.radius + agentRadius; + if (radius <= 0.0f) continue; + + const float minWorldX = obstacle.position.x() - radius; + const float maxWorldX = obstacle.position.x() + radius; + const float minWorldZ = obstacle.position.z() - radius; + const float maxWorldZ = obstacle.position.z() + radius; + + const int minCellX = (std::max)(0, static_cast(std::floor((minWorldX - minX) / cellSize))); + const int maxCellX = (std::min)(gridWidth - 1, static_cast(std::floor((maxWorldX - minX) / cellSize))); + const int minCellZ = (std::max)(0, static_cast(std::floor((minWorldZ - minZ) / cellSize))); + const int maxCellZ = (std::min)(gridDepth - 1, static_cast(std::floor((maxWorldZ - minZ) / cellSize))); + + const float radiusSq = radius * radius; + for (int z = minCellZ; z <= maxCellZ; ++z) { + for (int x = minCellX; x <= maxCellX; ++x) { + const Cell cell{ x, z }; + const Eigen::Vector3f center = cellCenter(cell); + const float dx = center.x() - obstacle.position.x(); + const float dz = center.z() - obstacle.position.z(); + if (dx * dx + dz * dz <= radiusSq) { + const int idx = indexOf(cell); + if (idx >= 0 && idx < static_cast(walkableGrid.size())) { + walkableGrid[static_cast(idx)] = 0; + } + } + } + } + } + } + + Cell startCell; + Cell endCell; + if (!findNearestWalkableCell(start, startCell, walkableGrid) || + !findNearestWalkableCell(end, endCell, walkableGrid)) { + return {}; + } + + if (startCell.x == endCell.x && startCell.z == endCell.z) { + return { cellCenter(endCell) }; + } + + struct QueueNode { + int index = 0; + float priority = 0.0f; + + bool operator<(const QueueNode& other) const + { + return priority > other.priority; + } + }; + + const int cellCount = gridWidth * gridDepth; + std::vector cost(static_cast(cellCount), INF_COST); + std::vector cameFrom(static_cast(cellCount), -1); + std::priority_queue open; + + const int startIndex = indexOf(startCell); + const int endIndex = indexOf(endCell); + + cost[static_cast(startIndex)] = 0.0f; + open.push({ startIndex, 0.0f }); + + static const int offsets[8][2] = { + { 1, 0 }, { -1, 0 }, { 0, 1 }, { 0, -1 }, + { 1, 1 }, { 1, -1 }, { -1, 1 }, { -1, -1 } + }; + + while (!open.empty()) { + const QueueNode current = open.top(); + open.pop(); + + if (current.index == endIndex) { + break; + } + + const Cell currentCell{ current.index % gridWidth, current.index / gridWidth }; + + for (const auto& offset : offsets) { + Cell next{ currentCell.x + offset[0], currentCell.z + offset[1] }; + if (!isCellWalkable(next, walkableGrid)) { + continue; + } + + const bool diagonal = offset[0] != 0 && offset[1] != 0; + if (diagonal) { + Cell horizontal{ currentCell.x + offset[0], currentCell.z }; + Cell vertical{ currentCell.x, currentCell.z + offset[1] }; + if (!isCellWalkable(horizontal, walkableGrid) || !isCellWalkable(vertical, walkableGrid)) { + continue; + } + } + + const int nextIndex = indexOf(next); + const float stepCost = diagonal ? 1.41421356f : 1.0f; + const float newCost = cost[static_cast(current.index)] + stepCost; + if (newCost >= cost[static_cast(nextIndex)]) { + continue; + } + + cost[static_cast(nextIndex)] = newCost; + cameFrom[static_cast(nextIndex)] = current.index; + const float priority = newCost + distanceCells(next, endCell); + open.push({ nextIndex, priority }); + } + } + + if (cameFrom[static_cast(endIndex)] == -1) { + return {}; + } + + std::vector cells; + for (int current = endIndex; current != -1; current = cameFrom[static_cast(current)]) { + cells.push_back({ current % gridWidth, current / gridWidth }); + if (current == startIndex) { + break; + } + } + std::reverse(cells.begin(), cells.end()); + cells = smoothCells(cells, walkableGrid); std::vector path; path.reserve(cells.size()); @@ -206,7 +360,7 @@ bool PathFinder::setAreaAvailable(const std::string& areaName, bool available) bool PathFinder::isWalkable(const Eigen::Vector3f& point) const { Cell cell; - return worldToCell(point, cell) && isCellWalkable(cell); + return worldToCell(point, cell) && isCellWalkable(cell, walkable); } void PathFinder::loadConfig(const std::string& configPath, const std::string& zipPath) @@ -406,7 +560,16 @@ bool PathFinder::isInsideGrid(const Cell& cell) const bool PathFinder::isCellWalkable(const Cell& cell) const { - return isInsideGrid(cell) && walkable[static_cast(indexOf(cell))] != 0; + return isCellWalkable(cell, walkable); +} + +bool PathFinder::isCellWalkable(const Cell& cell, const std::vector& walkableGrid) const +{ + const int idx = indexOf(cell); + return isInsideGrid(cell) && + idx >= 0 && + idx < static_cast(walkableGrid.size()) && + walkableGrid[static_cast(idx)] != 0; } int PathFinder::indexOf(const Cell& cell) const @@ -415,6 +578,11 @@ int PathFinder::indexOf(const Cell& cell) const } bool PathFinder::findNearestWalkableCell(const Eigen::Vector3f& point, Cell& out) const +{ + return findNearestWalkableCell(point, out, walkable); +} + +bool PathFinder::findNearestWalkableCell(const Eigen::Vector3f& point, Cell& out, const std::vector& walkableGrid) const { Cell origin; if (!worldToCell(point, origin)) { @@ -424,7 +592,7 @@ bool PathFinder::findNearestWalkableCell(const Eigen::Vector3f& point, Cell& out origin.z = (std::min)((std::max)(z, 0), gridDepth - 1); } - if (isCellWalkable(origin)) { + if (isCellWalkable(origin, walkableGrid)) { out = origin; return true; } @@ -438,7 +606,7 @@ bool PathFinder::findNearestWalkableCell(const Eigen::Vector3f& point, Cell& out } Cell candidate{ origin.x + dx, origin.z + dz }; - if (isCellWalkable(candidate)) { + if (isCellWalkable(candidate, walkableGrid)) { out = candidate; return true; } @@ -450,6 +618,11 @@ bool PathFinder::findNearestWalkableCell(const Eigen::Vector3f& point, Cell& out } bool PathFinder::hasLineOfSight(const Cell& from, const Cell& to) const +{ + return hasLineOfSight(from, to, walkable); +} + +bool PathFinder::hasLineOfSight(const Cell& from, const Cell& to, const std::vector& walkableGrid) const { const Eigen::Vector3f a = cellCenter(from); const Eigen::Vector3f b = cellCenter(to); @@ -460,7 +633,7 @@ bool PathFinder::hasLineOfSight(const Cell& from, const Cell& to) const const float t = static_cast(i) / static_cast(steps); const Eigen::Vector3f p = a * (1.0f - t) + b * t; Cell cell; - if (!worldToCell(p, cell) || !isCellWalkable(cell)) { + if (!worldToCell(p, cell) || !isCellWalkable(cell, walkableGrid)) { return false; } } @@ -469,6 +642,11 @@ bool PathFinder::hasLineOfSight(const Cell& from, const Cell& to) const } std::vector PathFinder::smoothCells(const std::vector& cells) const +{ + return smoothCells(cells, walkable); +} + +std::vector PathFinder::smoothCells(const std::vector& cells, const std::vector& walkableGrid) const { if (cells.size() <= 2) { return cells; @@ -480,7 +658,7 @@ std::vector PathFinder::smoothCells(const std::vector& c while (anchor < cells.size() - 1) { size_t next = cells.size() - 1; - while (next > anchor + 1 && !hasLineOfSight(cells[anchor], cells[next])) { + while (next > anchor + 1 && !hasLineOfSight(cells[anchor], cells[next], walkableGrid)) { --next; } diff --git a/src/navigation/PathFinder.h b/src/navigation/PathFinder.h index 1ac9b9a..8cb11e7 100644 --- a/src/navigation/PathFinder.h +++ b/src/navigation/PathFinder.h @@ -14,6 +14,11 @@ public: Eigen::Vector3f offset = Eigen::Vector3f::Zero(); }; + struct DynamicObstacle { + Eigen::Vector3f position = Eigen::Vector3f::Zero(); + float radius = 0.0f; + }; + struct Cell { int x = 0; int z = 0; @@ -32,6 +37,10 @@ public: std::vector findPath(const Eigen::Vector3f& start, const Eigen::Vector3f& end) const; + std::vector findPath(const Eigen::Vector3f& start, + const Eigen::Vector3f& end, + const std::vector& dynamicObstacles) const; + bool setAreaAvailable(const std::string& areaName, bool available); bool isReady() const { return ready; } bool isWalkable(const Eigen::Vector3f& point) const; @@ -74,6 +83,11 @@ private: std::vector smoothCells(const std::vector& cells) const; static bool pointInPolygon(float x, float z, const std::vector& polygon); + + bool isCellWalkable(const Cell& cell, const std::vector& walkableGrid) const; + bool findNearestWalkableCell(const Eigen::Vector3f& point, Cell& out, const std::vector& walkableGrid) const; + bool hasLineOfSight(const Cell& from, const Cell& to, const std::vector& walkableGrid) const; + std::vector smoothCells(const std::vector& cells, const std::vector& walkableGrid) const; }; } // namespace ZL From 8b0a18a6cfbb7980bbbda8defdcef2675ff07a5f Mon Sep 17 00:00:00 2001 From: Ariari04 Date: Fri, 1 May 2026 16:01:32 +0600 Subject: [PATCH 2/2] corrected collision of tree --- resources/config2/navigation.json | 433 ++++++++++++++++--- resources/config2/navigation2.json | 666 ++++++++++++++++++++++++++++- src/Location.cpp | 19 +- src/navigation/PathFinder.cpp | 166 +++++++ src/navigation/PathFinder.h | 8 + 5 files changed, 1204 insertions(+), 88 deletions(-) diff --git a/resources/config2/navigation.json b/resources/config2/navigation.json index 985774f..877a74a 100644 --- a/resources/config2/navigation.json +++ b/resources/config2/navigation.json @@ -3,175 +3,472 @@ "agentRadius": 0.45, "floorY": 0.0, "objectPadding": 0.25, + "boundaryPadding": 0.35, "areas": [ { "name": "main_corridor", "available": true, "polygon": [ - [-2.2, 0.8], - [2.2, 0.8], - [2.2, -40.8], - [-2.2, -40.8] + [ + -2.2, + 0.8 + ], + [ + 2.2, + 0.8 + ], + [ + 2.2, + -40.8 + ], + [ + -2.2, + -40.8 + ] ] }, { "name": "left_door_05", "available": true, "polygon": [ - [-3.4, -4.2], - [-2.0, -4.2], - [-2.0, -5.8], - [-3.4, -5.8] + [ + -3.4, + -4.2 + ], + [ + -2.0, + -4.2 + ], + [ + -2.0, + -5.8 + ], + [ + -3.4, + -5.8 + ] ] }, { "name": "right_door_05", "available": true, "polygon": [ - [2.0, -4.2], - [3.4, -4.2], - [3.4, -5.8], - [2.0, -5.8] + [ + 2.0, + -4.2 + ], + [ + 3.4, + -4.2 + ], + [ + 3.4, + -5.8 + ], + [ + 2.0, + -5.8 + ] ] }, { "name": "left_room_05", "available": true, "polygon": [ - [-8.8, -1.0], - [-3.0, -1.0], - [-3.0, -9.0], - [-8.8, -9.0] + [ + -8.8, + -1.0 + ], + [ + -3.0, + -1.0 + ], + [ + -3.0, + -9.0 + ], + [ + -8.8, + -9.0 + ] ] }, { "name": "right_room_05", "available": true, "polygon": [ - [3.0, -1.0], - [8.8, -1.0], - [8.8, -9.0], - [3.0, -9.0] + [ + 3.0, + -1.0 + ], + [ + 8.8, + -1.0 + ], + [ + 8.8, + -9.0 + ], + [ + 3.0, + -9.0 + ] ] }, { "name": "left_door_15", "available": true, "polygon": [ - [-3.4, -14.2], - [-2.0, -14.2], - [-2.0, -15.8], - [-3.4, -15.8] + [ + -3.4, + -14.2 + ], + [ + -2.0, + -14.2 + ], + [ + -2.0, + -15.8 + ], + [ + -3.4, + -15.8 + ] ] }, { "name": "right_door_15", "available": true, "polygon": [ - [2.0, -14.2], - [3.4, -14.2], - [3.4, -15.8], - [2.0, -15.8] + [ + 2.0, + -14.2 + ], + [ + 3.4, + -14.2 + ], + [ + 3.4, + -15.8 + ], + [ + 2.0, + -15.8 + ] ] }, { "name": "left_room_15", "available": true, "polygon": [ - [-8.8, -11.0], - [-3.0, -11.0], - [-3.0, -19.0], - [-8.8, -19.0] + [ + -8.8, + -11.0 + ], + [ + -3.0, + -11.0 + ], + [ + -3.0, + -19.0 + ], + [ + -8.8, + -19.0 + ] ] }, { "name": "right_room_15", "available": true, "polygon": [ - [3.0, -11.0], - [8.8, -11.0], - [8.8, -19.0], - [3.0, -19.0] + [ + 3.0, + -11.0 + ], + [ + 8.8, + -11.0 + ], + [ + 8.8, + -19.0 + ], + [ + 3.0, + -19.0 + ] ] }, { "name": "left_door_25", "available": true, "polygon": [ - [-3.4, -24.2], - [-2.0, -24.2], - [-2.0, -25.8], - [-3.4, -25.8] + [ + -3.4, + -24.2 + ], + [ + -2.0, + -24.2 + ], + [ + -2.0, + -25.8 + ], + [ + -3.4, + -25.8 + ] ] }, { "name": "right_door_25", "available": true, "polygon": [ - [2.0, -24.2], - [3.4, -24.2], - [3.4, -25.8], - [2.0, -25.8] + [ + 2.0, + -24.2 + ], + [ + 3.4, + -24.2 + ], + [ + 3.4, + -25.8 + ], + [ + 2.0, + -25.8 + ] ] }, { "name": "left_room_25", "available": true, "polygon": [ - [-8.8, -21.0], - [-3.0, -21.0], - [-3.0, -29.0], - [-8.8, -29.0] + [ + -8.8, + -21.0 + ], + [ + -3.0, + -21.0 + ], + [ + -3.0, + -29.0 + ], + [ + -8.8, + -29.0 + ] ] }, { "name": "right_room_25", "available": true, "polygon": [ - [3.0, -21.0], - [8.8, -21.0], - [8.8, -29.0], - [3.0, -29.0] + [ + 3.0, + -21.0 + ], + [ + 8.8, + -21.0 + ], + [ + 8.8, + -29.0 + ], + [ + 3.0, + -29.0 + ] ] }, { "name": "left_door_35", "available": true, "polygon": [ - [-3.4, -34.2], - [-2.0, -34.2], - [-2.0, -35.8], - [-3.4, -35.8] + [ + -3.4, + -34.2 + ], + [ + -2.0, + -34.2 + ], + [ + -2.0, + -35.8 + ], + [ + -3.4, + -35.8 + ] ] }, { "name": "right_door_35", "available": true, "polygon": [ - [2.0, -34.2], - [3.4, -34.2], - [3.4, -35.8], - [2.0, -35.8] + [ + 2.0, + -34.2 + ], + [ + 3.4, + -34.2 + ], + [ + 3.4, + -35.8 + ], + [ + 2.0, + -35.8 + ] ] }, { "name": "left_room_35", "available": true, "polygon": [ - [-8.8, -31.0], - [-3.0, -31.0], - [-3.0, -39.0], - [-8.8, -39.0] + [ + -8.8, + -31.0 + ], + [ + -3.0, + -31.0 + ], + [ + -3.0, + -39.0 + ], + [ + -8.8, + -39.0 + ] ] }, { "name": "right_room_35", "available": true, "polygon": [ - [3.0, -31.0], - [8.8, -31.0], - [8.8, -39.0], - [3.0, -39.0] + [ + 3.0, + -31.0 + ], + [ + 8.8, + -31.0 + ], + [ + 8.8, + -39.0 + ], + [ + 3.0, + -39.0 + ] + ] + } + ], + "obstacles": [ + { + "name": "firebox", + "polygon": [ + [ + -2.49468, + -32.87888 + ], + [ + -2.05776, + -32.87888 + ], + [ + -2.05776, + -31.51618 + ], + [ + -2.49468, + -31.51618 + ] + ] + }, + { + "name": "bench", + "polygon": [ + [ + -2.54485, + -9.31999 + ], + [ + -2.51786, + -9.3557 + ], + [ + -2.48369, + -9.37177 + ], + [ + -2.45574, + -9.38357 + ], + [ + -2.17907, + -9.39566 + ], + [ + -1.80829, + -9.40749 + ], + [ + -1.7521, + -9.36413 + ], + [ + -1.66462, + -9.28728 + ], + [ + -1.65801, + -9.14512 + ], + [ + -1.65114, + -7.22169 + ], + [ + -1.65535, + -6.51944 + ], + [ + -1.72626, + -6.42306 + ], + [ + -1.76837, + -6.39498 + ], + [ + -2.47166, + -6.39746 + ], + [ + -2.5309, + -6.44135 + ], + [ + -2.53691, + -7.18464 + ] ] } ] diff --git a/resources/config2/navigation2.json b/resources/config2/navigation2.json index f503138..d0cefc6 100644 --- a/resources/config2/navigation2.json +++ b/resources/config2/navigation2.json @@ -8,10 +8,668 @@ "name": "main_corridor", "available": true, "polygon": [ - [-200, 200], - [200, 200], - [200, -200], - [-200, -200] + [ + -200, + 200 + ], + [ + 200, + 200 + ], + [ + 200, + -200 + ], + [ + -200, + -200 + ] + ] + } + ], + "obstacles": [ + { + "name": "door", + "polygon": [ + [ + -5.2, + 7.08001 + ], + [ + -5.19904, + 7.07025 + ], + [ + -5.19619, + 7.06087 + ], + [ + -5.19157, + 7.05223 + ], + [ + -5.18536, + 7.04465 + ], + [ + -5.17778, + 7.03843 + ], + [ + -5.16913, + 7.03381 + ], + [ + -5.15975, + 7.03097 + ], + [ + -5.15, + 7.03001 + ], + [ + -3.75, + 7.13001 + ], + [ + -3.625, + 7.43001 + ], + [ + -3.625, + 8.83001 + ], + [ + -3.75, + 9.13001 + ], + [ + -5.15, + 9.23001 + ], + [ + -5.15975, + 9.22905 + ], + [ + -5.16913, + 9.2262 + ], + [ + -5.17778, + 9.22158 + ], + [ + -5.18536, + 9.21536 + ], + [ + -5.19157, + 9.20778 + ], + [ + -5.19619, + 9.19914 + ], + [ + -5.19904, + 9.18976 + ], + [ + -5.2, + 9.18001 + ] + ] + }, + { + "name": "inai", + "polygon": [ + [ + -3.75, + -15.0 + ], + [ + 3.75, + -15.0 + ], + [ + 3.75, + 15.0 + ], + [ + -3.75, + 15.0 + ] + ] + }, + { + "name": "tree001", + "polygon": [ + [ + 10.45, + 12.0 + ], + [ + 10.38971, + 12.225 + ], + [ + 10.225, + 12.38971 + ], + [ + 10.0, + 12.45 + ], + [ + 9.775, + 12.38971 + ], + [ + 9.61029, + 12.225 + ], + [ + 9.55, + 12.0 + ], + [ + 9.61029, + 11.775 + ], + [ + 9.775, + 11.61029 + ], + [ + 10.0, + 11.55 + ], + [ + 10.225, + 11.61029 + ], + [ + 10.38971, + 11.775 + ] + ] + }, + { + "name": "tree002", + "polygon": [ + [ + -11.55, + 19.0 + ], + [ + -11.61029, + 19.225 + ], + [ + -11.775, + 19.38971 + ], + [ + -12.0, + 19.45 + ], + [ + -12.225, + 19.38971 + ], + [ + -12.38971, + 19.225 + ], + [ + -12.45, + 19.0 + ], + [ + -12.38971, + 18.775 + ], + [ + -12.225, + 18.61029 + ], + [ + -12.0, + 18.55 + ], + [ + -11.775, + 18.61029 + ], + [ + -11.61029, + 18.775 + ] + ] + }, + { + "name": "tree003", + "polygon": [ + [ + -11.55, + 8.0 + ], + [ + -11.61029, + 8.225 + ], + [ + -11.775, + 8.38971 + ], + [ + -12.0, + 8.45 + ], + [ + -12.225, + 8.38971 + ], + [ + -12.38971, + 8.225 + ], + [ + -12.45, + 8.0 + ], + [ + -12.38971, + 7.775 + ], + [ + -12.225, + 7.61029 + ], + [ + -12.0, + 7.55 + ], + [ + -11.775, + 7.61029 + ], + [ + -11.61029, + 7.775 + ] + ] + }, + { + "name": "tree004", + "polygon": [ + [ + -11.55, + 0.0 + ], + [ + -11.61029, + 0.225 + ], + [ + -11.775, + 0.38971 + ], + [ + -12.0, + 0.45 + ], + [ + -12.225, + 0.38971 + ], + [ + -12.38971, + 0.225 + ], + [ + -12.45, + 0.0 + ], + [ + -12.38971, + -0.225 + ], + [ + -12.225, + -0.38971 + ], + [ + -12.0, + -0.45 + ], + [ + -11.775, + -0.38971 + ], + [ + -11.61029, + -0.225 + ] + ] + }, + { + "name": "tree005", + "polygon": [ + [ + -11.55, + -8.0 + ], + [ + -11.61029, + -7.775 + ], + [ + -11.775, + -7.61029 + ], + [ + -12.0, + -7.55 + ], + [ + -12.225, + -7.61029 + ], + [ + -12.38971, + -7.775 + ], + [ + -12.45, + -8.0 + ], + [ + -12.38971, + -8.225 + ], + [ + -12.225, + -8.38971 + ], + [ + -12.0, + -8.45 + ], + [ + -11.775, + -8.38971 + ], + [ + -11.61029, + -8.225 + ] + ] + }, + { + "name": "tree006", + "polygon": [ + [ + 8.94915, + -2.59884 + ], + [ + 8.88886, + -2.37384 + ], + [ + 8.72415, + -2.20913 + ], + [ + 8.49915, + -2.14884 + ], + [ + 8.27415, + -2.20913 + ], + [ + 8.10944, + -2.37384 + ], + [ + 8.04915, + -2.59884 + ], + [ + 8.10944, + -2.82384 + ], + [ + 8.27415, + -2.98855 + ], + [ + 8.49915, + -3.04884 + ], + [ + 8.72415, + -2.98855 + ], + [ + 8.88886, + -2.82384 + ] + ] + }, + { + "name": "tree007", + "polygon": [ + [ + 15.0436, + 5.3401 + ], + [ + 14.98331, + 5.5651 + ], + [ + 14.8186, + 5.72981 + ], + [ + 14.5936, + 5.7901 + ], + [ + 14.3686, + 5.72981 + ], + [ + 14.20389, + 5.5651 + ], + [ + 14.1436, + 5.3401 + ], + [ + 14.20389, + 5.1151 + ], + [ + 14.3686, + 4.95039 + ], + [ + 14.5936, + 4.8901 + ], + [ + 14.8186, + 4.95039 + ], + [ + 14.98331, + 5.1151 + ] + ] + }, + { + "name": "tree008", + "polygon": [ + [ + 24.3795, + 9.00583 + ], + [ + 24.31921, + 9.23083 + ], + [ + 24.1545, + 9.39554 + ], + [ + 23.9295, + 9.45583 + ], + [ + 23.7045, + 9.39554 + ], + [ + 23.53979, + 9.23083 + ], + [ + 23.4795, + 9.00583 + ], + [ + 23.53979, + 8.78083 + ], + [ + 23.7045, + 8.61612 + ], + [ + 23.9295, + 8.55583 + ], + [ + 24.1545, + 8.61612 + ], + [ + 24.31921, + 8.78083 + ] + ] + }, + { + "name": "tree009", + "polygon": [ + [ + 30.2628, + -1.45278 + ], + [ + 30.20251, + -1.22778 + ], + [ + 30.0378, + -1.06307 + ], + [ + 29.8128, + -1.00278 + ], + [ + 29.5878, + -1.06307 + ], + [ + 29.42309, + -1.22778 + ], + [ + 29.3628, + -1.45278 + ], + [ + 29.42309, + -1.67778 + ], + [ + 29.5878, + -1.84249 + ], + [ + 29.8128, + -1.90278 + ], + [ + 30.0378, + -1.84249 + ], + [ + 30.20251, + -1.67778 + ] + ] + }, + { + "name": "tree010", + "polygon": [ + [ + 33.6271, + 14.609 + ], + [ + 33.56681, + 14.834 + ], + [ + 33.4021, + 14.99871 + ], + [ + 33.1771, + 15.059 + ], + [ + 32.9521, + 14.99871 + ], + [ + 32.78739, + 14.834 + ], + [ + 32.7271, + 14.609 + ], + [ + 32.78739, + 14.384 + ], + [ + 32.9521, + 14.21929 + ], + [ + 33.1771, + 14.159 + ], + [ + 33.4021, + 14.21929 + ], + [ + 33.56681, + 14.384 + ] ] } ] diff --git a/src/Location.cpp b/src/Location.cpp index 0b4ed07..af04760 100644 --- a/src/Location.cpp +++ b/src/Location.cpp @@ -197,22 +197,9 @@ namespace ZL void Location::setupNavigation(const std::string& navigationJsonPath) { - std::vector obstacles; - obstacles.reserve(gameObjects.size() + interactiveObjects.size()); - - for (const auto& item : gameObjects) { - const LoadedGameObject& gameObj = item.second; - obstacles.push_back({ &gameObj.mesh.data, Eigen::Vector3f::Zero() }); - } - - for (const InteractiveObject& intObj : interactiveObjects) { - if (!intObj.isActive) { - continue; - } - obstacles.push_back({ &intObj.mesh.data, intObj.position }); - } - - navigation.build(obstacles, navigationJsonPath, CONST_ZIP_FILE); + // Static navigation blockers are defined in the navigation JSON as polygons. + // NPC + player are handled as dynamic obstacles, so they are intentionally NOT in JSON. + navigation.build({}, navigationJsonPath, CONST_ZIP_FILE); #ifdef SHOW_PATH buildDebugNavMeshes(); diff --git a/src/navigation/PathFinder.cpp b/src/navigation/PathFinder.cpp index 20ab1db..51c6e9a 100644 --- a/src/navigation/PathFinder.cpp +++ b/src/navigation/PathFinder.cpp @@ -38,6 +38,52 @@ namespace { return polygon; } + + float distancePointToSegment2D(const Eigen::Vector2f& p, + const Eigen::Vector2f& a, + const Eigen::Vector2f& b) + { + const Eigen::Vector2f ab = b - a; + const float abLenSq = ab.squaredNorm(); + if (abLenSq <= 1e-8f) { + return (p - a).norm(); + } + float t = (p - a).dot(ab) / abLenSq; + t = (std::max)(0.0f, (std::min)(1.0f, t)); + const Eigen::Vector2f proj = a + t * ab; + return (p - proj).norm(); + } + + Eigen::Vector2f closestPointOnSegment2D(const Eigen::Vector2f& p, + const Eigen::Vector2f& a, + const Eigen::Vector2f& b) + { + const Eigen::Vector2f ab = b - a; + const float abLenSq = ab.squaredNorm(); + if (abLenSq <= 1e-8f) { + return a; + } + + float t = (p - a).dot(ab) / abLenSq; + t = (std::max)(0.0f, (std::min)(1.0f, t)); + return a + t * ab; + } + + float distancePointToPolygonEdges(const Eigen::Vector2f& p, + const std::vector& polygon) + { + if (polygon.size() < 2) { + return (std::numeric_limits::max)(); + } + + float best = (std::numeric_limits::max)(); + for (size_t i = 0; i < polygon.size(); ++i) { + const Eigen::Vector2f& a = polygon[i]; + const Eigen::Vector2f& b = polygon[(i + 1) % polygon.size()]; + best = (std::min)(best, distancePointToSegment2D(p, a, b)); + } + return best; + } } void PathFinder::build(const std::vector& obstacleMeshes, @@ -369,7 +415,9 @@ void PathFinder::loadConfig(const std::string& configPath, const std::string& zi agentRadius = 0.45f; floorY = 0.0f; objectPadding = 0.25f; + boundaryPadding = 0.0f; areas.clear(); + obstaclePolygons.clear(); try { std::string content; @@ -390,6 +438,7 @@ void PathFinder::loadConfig(const std::string& configPath, const std::string& zi agentRadius = root.value("agentRadius", agentRadius); floorY = root.value("floorY", floorY); objectPadding = root.value("objectPadding", objectPadding); + boundaryPadding = root.value("boundaryPadding", boundaryPadding); if (!root.contains("areas") || !root["areas"].is_array()) { std::cerr << "[nav] Navigation config has no 'areas' array: " << configPath << "\n"; @@ -413,6 +462,18 @@ void PathFinder::loadConfig(const std::string& configPath, const std::string& zi areas.push_back(area); } + + if (root.contains("obstacles") && root["obstacles"].is_array()) { + for (const auto& item : root["obstacles"]) { + ObstaclePolygon obstacle; + obstacle.name = item.value("name", ""); + obstacle.polygon = readPolygon(item); + if (obstacle.polygon.size() < 3) { + continue; + } + obstaclePolygons.push_back(std::move(obstacle)); + } + } } catch (const std::exception& e) { std::cerr << "[nav] Failed to load config '" << configPath << "': " << e.what() << "\n"; } @@ -420,6 +481,7 @@ void PathFinder::loadConfig(const std::string& configPath, const std::string& zi cellSize = (std::max)(cellSize, 0.1f); agentRadius = (std::max)(agentRadius, 0.0f); objectPadding = (std::max)(objectPadding, 0.0f); + boundaryPadding = (std::max)(boundaryPadding, 0.0f); } void PathFinder::resetGridBounds() @@ -458,6 +520,12 @@ void PathFinder::resetGridBounds() } } + for (const ObstaclePolygon& obstacle : obstaclePolygons) { + for (const Eigen::Vector2f& point : obstacle.polygon) { + includePoint(point.x(), point.y()); + } + } + const float padding = cellSize * 2.0f + agentRadius + objectPadding; minX -= padding; minZ -= padding; @@ -473,20 +541,75 @@ void PathFinder::rebuildWalkableGrid() walkable.assign(static_cast(gridWidth * gridDepth), 0); markAvailableAreasWalkable(); markObstacleMeshesBlocked(); + markObstaclePolygonsBlocked(); } void PathFinder::markAvailableAreasWalkable() { + const auto insideAvailableArea = [this](const Eigen::Vector2f& point) { + for (const NavigationArea& area : areas) { + if (!area.available) { + continue; + } + if (pointInPolygon(point.x(), point.y(), area.polygon)) { + return true; + } + } + return false; + }; + + const auto externalBoundaryDistance = [this, &insideAvailableArea](const Eigen::Vector2f& point) { + if (boundaryPadding <= 0.0f) { + return (std::numeric_limits::max)(); + } + + const float sampleOffset = (std::max)(cellSize * 0.25f, 0.05f); + float best = (std::numeric_limits::max)(); + + for (const NavigationArea& area : areas) { + if (!area.available || area.polygon.size() < 2) { + continue; + } + + for (size_t i = 0; i < area.polygon.size(); ++i) { + const Eigen::Vector2f& a = area.polygon[i]; + const Eigen::Vector2f& b = area.polygon[(i + 1) % area.polygon.size()]; + const Eigen::Vector2f edge = b - a; + if (edge.squaredNorm() <= 1e-8f) { + continue; + } + + const Eigen::Vector2f closest = closestPointOnSegment2D(point, a, b); + Eigen::Vector2f normal(-edge.y(), edge.x()); + normal.normalize(); + + const bool sideAInside = insideAvailableArea(closest + normal * sampleOffset); + const bool sideBInside = insideAvailableArea(closest - normal * sampleOffset); + if (sideAInside && sideBInside) { + continue; + } + + best = (std::min)(best, distancePointToSegment2D(point, a, b)); + } + } + + return best; + }; + for (int z = 0; z < gridDepth; ++z) { for (int x = 0; x < gridWidth; ++x) { const Cell cell{ x, z }; const Eigen::Vector3f center = cellCenter(cell); + const Eigen::Vector2f point(center.x(), center.z()); for (const NavigationArea& area : areas) { if (!area.available) { continue; } if (pointInPolygon(center.x(), center.z(), area.polygon)) { + if (boundaryPadding > 0.0f && externalBoundaryDistance(point) < boundaryPadding) { + continue; + } walkable[static_cast(indexOf(cell))] = 1; break; } @@ -495,8 +618,51 @@ void PathFinder::markAvailableAreasWalkable() } } +void PathFinder::markObstaclePolygonsBlocked() +{ + if (obstaclePolygons.empty()) { + return; + } + + const float padding = agentRadius + objectPadding; + + for (int z = 0; z < gridDepth; ++z) { + for (int x = 0; x < gridWidth; ++x) { + const Cell cell{ x, z }; + const int idx = indexOf(cell); + if (idx < 0 || idx >= static_cast(walkable.size())) { + continue; + } + if (!walkable[static_cast(idx)]) { + continue; + } + + const Eigen::Vector3f center3 = cellCenter(cell); + const Eigen::Vector2f center(center3.x(), center3.z()); + + for (const ObstaclePolygon& obstacle : obstaclePolygons) { + if (pointInPolygon(center.x(), center.y(), obstacle.polygon)) { + walkable[static_cast(idx)] = 0; + break; + } + if (padding > 0.0f && + distancePointToPolygonEdges(center, obstacle.polygon) <= padding) { + walkable[static_cast(idx)] = 0; + break; + } + } + } + } +} + void PathFinder::markObstacleMeshesBlocked() { + if (!obstaclePolygons.empty()) { + // Prefer JSON-defined obstacle polygons over mesh-derived blockers. + // Mesh blockers tend to over-block (e.g. tree crowns) and are harder to author/tune. + return; + } + const float padding = agentRadius + objectPadding; for (const ObstacleMesh& obstacle : obstacles) { diff --git a/src/navigation/PathFinder.h b/src/navigation/PathFinder.h index 8cb11e7..60134f0 100644 --- a/src/navigation/PathFinder.h +++ b/src/navigation/PathFinder.h @@ -14,6 +14,11 @@ public: Eigen::Vector3f offset = Eigen::Vector3f::Zero(); }; + struct ObstaclePolygon { + std::string name; + std::vector polygon; + }; + struct DynamicObstacle { Eigen::Vector3f position = Eigen::Vector3f::Zero(); float radius = 0.0f; @@ -53,6 +58,7 @@ private: float agentRadius = 0.45f; float floorY = 0.0f; float objectPadding = 0.25f; + float boundaryPadding = 0.0f; float minX = 0.0f; float minZ = 0.0f; @@ -63,6 +69,7 @@ private: std::string loadedConfigPath; std::string loadedZipPath; std::vector obstacles; + std::vector obstaclePolygons; std::vector walkable; std::vector areas; @@ -71,6 +78,7 @@ private: void rebuildWalkableGrid(); void markAvailableAreasWalkable(); void markObstacleMeshesBlocked(); + void markObstaclePolygonsBlocked(); bool worldToCell(const Eigen::Vector3f& point, Cell& out) const; Eigen::Vector3f cellCenter(const Cell& cell) const;