salmon-engine-projects/space-game001
14
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Merge branch 'witcher001-path' into witcher001

Browse Source
This commit is contained in:
Vladislav Khorev 2026-05-01 16:53:25 +03:00
commit 5231b50aab
8 changed files with 1669 additions and 109 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

433
resources/config2/navigation.json
View File

@ -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
]
]
}
]

666
resources/config2/navigation2.json
View File

@ -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
]
]
}
]

37
src/Character.cpp
View File

@ -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<ZL::Texture> texture)
{
for (auto& animEntry : animations) {

18
src/Character.h
View File

@ -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.

223
src/Location.cpp
View File

@ -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)
@ -180,38 +197,51 @@ namespace ZL
void Location::setupNavigation(const std::string& navigationJsonPath)
{
std::vector<PathFinder::ObstacleMesh> 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();
#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<PathFinder::DynamicObstacle> 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 +670,156 @@ namespace ZL
return navigation.setAreaAvailable(areaName, available);
}
void Location::resolveCharacterCollisions()
{
std::vector<Character*> 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<Character*> 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<Character*> 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 +831,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();

9
src/Location.h
View File

@ -13,6 +13,8 @@
#include "dialogue/DialogueSystem.h"
#include "SparkEmitter.h"
#include <functional>
#include <cstdint>
#include <unordered_map>
namespace ZL
{
@ -112,6 +114,13 @@ namespace ZL
protected:
Renderer& renderer;
Inventory& inventory;
private:
void resolveCharacterCollisions();
void updateDynamicReplans(int64_t deltaMs);
std::unordered_map<Character*, Eigen::Vector3f> lastCharacterPositions;
std::unordered_map<Character*, int64_t> replanCooldownRemainingMs;
};
} // namespace ZL

366
src/navigation/PathFinder.cpp
View File

@ -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<Eigen::Vector2f>& polygon)
{
if (polygon.size() < 2) {
return (std::numeric_limits<float>::max)();
}
float best = (std::numeric_limits<float>::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<ObstacleMesh>& obstacleMeshes,
@ -74,8 +120,8 @@ std::vector<Eigen::Vector3f> 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 +167,7 @@ std::vector<Eigen::Vector3f> 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 +175,7 @@ std::vector<Eigen::Vector3f> 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 +206,161 @@ std::vector<Eigen::Vector3f> PathFinder::findPath(const Eigen::Vector3f& start,
}
}
std::reverse(cells.begin(), cells.end());
cells = smoothCells(cells);
cells = smoothCells(cells, walkable);
std::vector<Eigen::Vector3f> 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<Eigen::Vector3f> PathFinder::findPath(const Eigen::Vector3f& start,
const Eigen::Vector3f& end,
const std::vector<DynamicObstacle>& dynamicObstacles) const
{
if (!ready || walkable.empty()) {
return {};
}
std::vector<unsigned char> 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<int>(std::floor((minWorldX - minX) / cellSize)));
const int maxCellX = (std::min)(gridWidth - 1, static_cast<int>(std::floor((maxWorldX - minX) / cellSize)));
const int minCellZ = (std::max)(0, static_cast<int>(std::floor((minWorldZ - minZ) / cellSize)));
const int maxCellZ = (std::min)(gridDepth - 1, static_cast<int>(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<int>(walkableGrid.size())) {
walkableGrid[static_cast<size_t>(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<float> cost(static_cast<size_t>(cellCount), INF_COST);
std::vector<int> cameFrom(static_cast<size_t>(cellCount), -1);
std::priority_queue<QueueNode> open;
const int startIndex = indexOf(startCell);
const int endIndex = indexOf(endCell);
cost[static_cast<size_t>(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<size_t>(current.index)] + stepCost;
if (newCost >= cost[static_cast<size_t>(nextIndex)]) {
continue;
}
cost[static_cast<size_t>(nextIndex)] = newCost;
cameFrom[static_cast<size_t>(nextIndex)] = current.index;
const float priority = newCost + distanceCells(next, endCell);
open.push({ nextIndex, priority });
}
}
if (cameFrom[static_cast<size_t>(endIndex)] == -1) {
return {};
}
std::vector<Cell> cells;
for (int current = endIndex; current != -1; current = cameFrom[static_cast<size_t>(current)]) {
cells.push_back({ current % gridWidth, current / gridWidth });
if (current == startIndex) {
break;
}
}
std::reverse(cells.begin(), cells.end());
cells = smoothCells(cells, walkableGrid);
std::vector<Eigen::Vector3f> path;
path.reserve(cells.size());
@ -206,7 +406,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)
@ -215,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;
@ -236,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";
@ -259,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";
}
@ -266,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()
@ -304,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;
@ -319,20 +541,75 @@ void PathFinder::rebuildWalkableGrid()
walkable.assign(static_cast<size_t>(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<float>::max)();
}
const float sampleOffset = (std::max)(cellSize * 0.25f, 0.05f);
float best = (std::numeric_limits<float>::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<size_t>(indexOf(cell))] = 1;
break;
}
@ -341,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<int>(walkable.size())) {
continue;
}
if (!walkable[static_cast<size_t>(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<size_t>(idx)] = 0;
break;
}
if (padding > 0.0f &&
distancePointToPolygonEdges(center, obstacle.polygon) <= padding) {
walkable[static_cast<size_t>(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) {
@ -406,7 +726,16 @@ bool PathFinder::isInsideGrid(const Cell& cell) const
bool PathFinder::isCellWalkable(const Cell& cell) const
{
return isInsideGrid(cell) && walkable[static_cast<size_t>(indexOf(cell))] != 0;
return isCellWalkable(cell, walkable);
}
bool PathFinder::isCellWalkable(const Cell& cell, const std::vector<unsigned char>& walkableGrid) const
{
const int idx = indexOf(cell);
return isInsideGrid(cell) &&
idx >= 0 &&
idx < static_cast<int>(walkableGrid.size()) &&
walkableGrid[static_cast<size_t>(idx)] != 0;
}
int PathFinder::indexOf(const Cell& cell) const
@ -415,6 +744,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<unsigned char>& walkableGrid) const
{
Cell origin;
if (!worldToCell(point, origin)) {
@ -424,7 +758,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 +772,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 +784,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<unsigned char>& walkableGrid) const
{
const Eigen::Vector3f a = cellCenter(from);
const Eigen::Vector3f b = cellCenter(to);
@ -460,7 +799,7 @@ bool PathFinder::hasLineOfSight(const Cell& from, const Cell& to) const
const float t = static_cast<float>(i) / static_cast<float>(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 +808,11 @@ bool PathFinder::hasLineOfSight(const Cell& from, const Cell& to) const
}
std::vector<PathFinder::Cell> PathFinder::smoothCells(const std::vector<Cell>& cells) const
{
return smoothCells(cells, walkable);
}
std::vector<PathFinder::Cell> PathFinder::smoothCells(const std::vector<Cell>& cells, const std::vector<unsigned char>& walkableGrid) const
{
if (cells.size() <= 2) {
return cells;
@ -480,7 +824,7 @@ std::vector<PathFinder::Cell> PathFinder::smoothCells(const std::vector<Cell>& 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;
}

22
src/navigation/PathFinder.h
View File

@ -14,6 +14,16 @@ public:
Eigen::Vector3f offset = Eigen::Vector3f::Zero();
};
struct ObstaclePolygon {
std::string name;
std::vector<Eigen::Vector2f> polygon;
};
struct DynamicObstacle {
Eigen::Vector3f position = Eigen::Vector3f::Zero();
float radius = 0.0f;
};
struct Cell {
int x = 0;
int z = 0;
@ -32,6 +42,10 @@ public:
std::vector<Eigen::Vector3f> findPath(const Eigen::Vector3f& start,
const Eigen::Vector3f& end) const;
std::vector<Eigen::Vector3f> findPath(const Eigen::Vector3f& start,
const Eigen::Vector3f& end,
const std::vector<DynamicObstacle>& dynamicObstacles) const;
bool setAreaAvailable(const std::string& areaName, bool available);
bool isReady() const { return ready; }
bool isWalkable(const Eigen::Vector3f& point) const;
@ -44,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;
@ -54,6 +69,7 @@ private:
std::string loadedConfigPath;
std::string loadedZipPath;
std::vector<ObstacleMesh> obstacles;
std::vector<ObstaclePolygon> obstaclePolygons;
std::vector<unsigned char> walkable;
std::vector<NavigationArea> areas;
@ -62,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;
@ -74,6 +91,11 @@ private:
std::vector<Cell> smoothCells(const std::vector<Cell>& cells) const;
static bool pointInPolygon(float x, float z, const std::vector<Eigen::Vector2f>& polygon);
bool isCellWalkable(const Cell& cell, const std::vector<unsigned char>& walkableGrid) const;
bool findNearestWalkableCell(const Eigen::Vector3f& point, Cell& out, const std::vector<unsigned char>& walkableGrid) const;
bool hasLineOfSight(const Cell& from, const Cell& to, const std::vector<unsigned char>& walkableGrid) const;
std::vector<Cell> smoothCells(const std::vector<Cell>& cells, const std::vector<unsigned char>& walkableGrid) const;
};
} // namespace ZL