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