diff --git a/src/common/objects/dobjgc.h b/src/common/objects/dobjgc.h index f5514255db..280c86273f 100644 --- a/src/common/objects/dobjgc.h +++ b/src/common/objects/dobjgc.h @@ -215,6 +215,9 @@ class TObjPtr mutable DObject *o; }; public: + TObjPtr() = default; + + TObjPtr(T t) : pp(t) {} constexpr TObjPtr& operator=(T q) noexcept { diff --git a/src/g_level.cpp b/src/g_level.cpp index 919e1071d8..467ab6d324 100644 --- a/src/g_level.cpp +++ b/src/g_level.cpp @@ -2473,100 +2473,169 @@ DEFINE_ACTION_FUNCTION(FLevelLocals, GetEpisodeName) //---------------------------------------------------------------------------- // Code by RicardoLuis0 -TArray * GetNeighbors(AActor * self) +static TArray>& GetPathNodeNeighbors(AActor * self) { - PClass * cls = PClass::FindClass("PathNode"); - if(!cls->IsAncestorOf(self->GetClass())) - { - ThrowAbortException(X_BAD_SELF, "Invalid class passed to GetNeighbors (must be PathNodeInfo)"); - } + static PClass * nodeCls = PClass::FindClass(NAME_PathNode); - PField *var = dyn_cast(cls->FindSymbol("neighbors", true)); +#ifndef NDEBUG + if(!nodeCls->IsAncestorOf(self->GetClass())) + { + ThrowAbortException(X_BAD_SELF, "Invalid class passed to GetNeighbors (must be PathNode)"); + } +#endif + + static PField *var = dyn_cast(nodeCls->FindSymbol("neighbors", true)); assert(var); assert(var->Type->isDynArray()); - assert(static_cast(var->Type)->ElementType == cls); + assert(static_cast(var->Type)->ElementType == nodeCls->VMType); - return reinterpret_cast*>(reinterpret_cast(self) + var->Offset); + return *reinterpret_cast>*>(reinterpret_cast(self) + var->Offset); } -int AS_BinarySearch(TMap* fScore, bool exact = false) +static void ReconstructPath(TMap &cameFrom, AActor* current, TArray> &path) { - return 0; -} + path.Clear(); + path.Push(current); + AActor ** tmp = cameFrom.CheckKey(current); -void AS_ReconstructPath(TMap* cameFrom, AActor* chaser) -{ - -} - -bool FLevelLocals::AStar(AActor* chaser, AActor* target, AActor* startnode, AActor* goalnode) -{ - if (!chaser || !target || PathNodes.Size() < 1) - return false; - - // If supplying nodes, skip the search. - const bool getstart = (!startnode || !startnode->IsKindOf(NAME_PathNode)); - const bool getgoal = (!goalnode || !goalnode->IsKindOf(NAME_PathNode)); - - if (getstart || getgoal) + if(tmp) do { - double dist[2]; - dist[0] = dist[1] = 100000000.0; + path.Insert(0, *tmp); + } + while(tmp = cameFrom.CheckKey(*tmp)); +} +static AActor* FindClosestNode(AActor* from, double maxSearch) +{ + static PClass * nodeCls = PClass::FindClass(NAME_PathNode); - for (int i = 0; i < PathNodes.Size(); i++) + FPortalGroupArray check(FPortalGroupArray::PGA_Full3d); + FMultiBlockThingsIterator it(check, from->Level, from->Pos().X, from->Pos().Y, from->Pos().Z - ((from->Height + maxSearch) / 2), from->Height + maxSearch, from->radius + maxSearch, false, from->Sector); + FMultiBlockThingsIterator::CheckResult res; + + AActor * closest = nullptr; + double closestDist = DBL_MAX; + + while(it.Next(&res)) + { + if(nodeCls->IsAncestorOf(res.thing->GetClass())) { - AActor *node = PathNodes[i]; - if (!node) continue; - - double dis; - if (getstart) + double dst = res.thing->Distance3D(from); + if(dst < closestDist && P_CheckSight(res.thing, from)) { - dis = node->Distance2DSquared(chaser); - if (dis < dist[0] && P_CheckSight(node, chaser)) // TO DO: Make 3D aware, so 3D floors can work better. - { - startnode = node; - dist[0] = dis; - } + closestDist = dst; + closest = res.thing; } - - - dis = node->Distance2DSquared(target); - if (dis < dist[1] && P_CheckSight(node, target)) - { - goalnode = node; - dist[1] = dis; - } - } - - // Incomplete graph. - if (!startnode || !goalnode) - return false; - - if (startnode == goalnode) - { - chaser->ClearPath(); - chaser->Path.Push(MakeObjPtr(startnode)); - return true; } } - - // Begin A* here. + + return closest; +} + +template +static V GetOr(TMap map, const K &key, V alt) +{ + V *k = map.CheckKey(key); + return k ? *k : alt; +} + +static bool FindPathAStar(AActor* start, AActor* goal, TArray> &path) +{ + TArray openSet; TMap cameFrom; TMap gScore; TMap fScore; + openSet.Push(start); + gScore.Insert(start, 0); + fScore.Insert(start, start->Distance3D(goal)); + + auto lt_fScore = [&fScore](AActor* lhs, AActor* rhs) + { + return GetOr(fScore, lhs, DBL_MAX) < GetOr(fScore, rhs, DBL_MAX); + }; + + while(openSet.Size() > 0) + { + AActor * current = openSet[0]; + openSet.Delete(0); + if(current == goal) + { + ReconstructPath(cameFrom, current, path); + return true; + } + + double current_gScore = GetOr(gScore, current, DBL_MAX); + + for(AActor * neighbor : GetPathNodeNeighbors(current)) + { + + double tentative_gScore = current_gScore + current->Distance3D(neighbor); + + double neighbor_gScore = GetOr(gScore, neighbor, DBL_MAX); + + if(tentative_gScore < neighbor_gScore) + { + openSet.SortedDelete(neighbor, lt_fScore); + cameFrom.Insert(neighbor, current); + gScore.Insert(neighbor, tentative_gScore); + fScore.Insert(neighbor, tentative_gScore + neighbor->Distance3D(goal)); + openSet.SortedInsert(neighbor, lt_fScore); + } + } + } return false; } -DEFINE_ACTION_FUNCTION(FLevelLocals, AStar) +bool FLevelLocals::FindPath(AActor* start, AActor* goal, AActor* startNode, AActor* goalNode, double maxSearch) +{ + static PClass * nodeCls = PClass::FindClass(NAME_PathNode); + + if (!start || !goal) + { + return false; + } + + assert(startNode == nullptr || nodeCls->IsAncestorOf(startNode->GetClass())); + assert(goalNode == nullptr || nodeCls->IsAncestorOf(goalNode->GetClass())); + + if(startNode == nullptr) startNode = FindClosestNode(start, maxSearch); + if(goalNode == nullptr) goalNode = FindClosestNode(goal, maxSearch); + + // Incomplete graph. + if (!startNode || !goalNode) + { + return false; + } + + if (startNode == goalNode) + { + start->ClearPath(); + start->Path.Push(MakeObjPtr(startNode)); + return true; + } + + if(FindPathAStar(startNode, goalNode, start->Path)) + { + if (start->goal && nodeCls->IsAncestorOf(start->goal->GetClass())) + { + start->goal = nullptr; + } + return true; + } + + return false; +} + +DEFINE_ACTION_FUNCTION(FLevelLocals, FindPath) { PARAM_SELF_STRUCT_PROLOGUE(FLevelLocals); PARAM_OBJECT(chaser, AActor); PARAM_OBJECT(target, AActor); PARAM_OBJECT(startnode, AActor); PARAM_OBJECT(goalnode, AActor); - return self->AStar(chaser, target, startnode, goalnode); + PARAM_FLOAT(maxSearch); + return self->FindPath(chaser, target, startnode, goalnode, maxSearch); } diff --git a/src/g_levellocals.h b/src/g_levellocals.h index b75e3ef0d2..e7475bedb2 100644 --- a/src/g_levellocals.h +++ b/src/g_levellocals.h @@ -444,9 +444,8 @@ public: void SetMusic(); - bool AStar(AActor *chaser, AActor *target, AActor *startnode = nullptr, AActor *goalnode = nullptr); + bool FindPath(AActor *chaser, AActor *target, AActor *startnode = nullptr, AActor *goalnode = nullptr, double maxSearch = 256.0); - TArray PathNodes; TArray vertexes; TArray sectors; TArray extsectors; // container for non-trivial sector information. sector_t must be trivially copyable for *_fakeflat to work as intended. diff --git a/src/p_setup.cpp b/src/p_setup.cpp index 051fac4375..02f89c860d 100644 --- a/src/p_setup.cpp +++ b/src/p_setup.cpp @@ -611,8 +611,6 @@ void P_SetupLevel(FLevelLocals *Level, int position, bool newGame) while ((ac = it.Next())) { ac->SetDynamicLights(); - if (ac->IsKindOf(NAME_PathNode)) - Level->PathNodes.Push(ac); } } diff --git a/src/playsim/p_enemy.cpp b/src/playsim/p_enemy.cpp index 242680b962..4a11e5a13e 100644 --- a/src/playsim/p_enemy.cpp +++ b/src/playsim/p_enemy.cpp @@ -2532,7 +2532,7 @@ void A_DoChase (AActor *actor, bool fastchase, FState *meleestate, FState *missi } if (!actor->goal) { - if (actor->Path.Size() < 1 && actor->Level->AStar(actor, actor->target)) + if (actor->Path.Size() < 1 && actor->Level->FindPath(actor, actor->target)) actor->goal = actor->Path[0]; } diff --git a/wadsrc/static/zscript/actors/actor.zs b/wadsrc/static/zscript/actors/actor.zs index ff6c49f1d3..d725ff129b 100644 --- a/wadsrc/static/zscript/actors/actor.zs +++ b/wadsrc/static/zscript/actors/actor.zs @@ -261,7 +261,7 @@ class Actor : Thinker native private native int InventoryID; // internal counter. native uint freezetics; native Vector2 AutomapOffsets; - native Array Path; // Cannot be cast to PathNode, unfortunately. + native Array Path; meta String Obituary; // Player was killed by this actor meta String HitObituary; // Player was killed by this actor in melee diff --git a/wadsrc/static/zscript/actors/shared/sharedmisc.zs b/wadsrc/static/zscript/actors/shared/sharedmisc.zs index e297651156..be43c01476 100644 --- a/wadsrc/static/zscript/actors/shared/sharedmisc.zs +++ b/wadsrc/static/zscript/actors/shared/sharedmisc.zs @@ -255,7 +255,7 @@ class PathNode : Actor // For non-connected paths. Stamina will be used to set this. Necessary for tele/portals. private int group; - Array neighbors; + Array neighbors; Default { diff --git a/wadsrc/static/zscript/doombase.zs b/wadsrc/static/zscript/doombase.zs index a953b4c5b8..93a556db8d 100644 --- a/wadsrc/static/zscript/doombase.zs +++ b/wadsrc/static/zscript/doombase.zs @@ -554,7 +554,7 @@ struct LevelLocals native native void SpawnParticle(FSpawnParticleParams p); native VisualThinker SpawnVisualThinker(Class type); - native bool AStar(Actor chaser, Actor target, Actor startnode = null, Actor goalnode = null); + native bool FindPath(Actor chaser, Actor target, Actor startnode = null, Actor goalnode = null, double maxSearch = 256.0); } // a few values of this need to be readable by the play code.