AxioDL
/
metaforce
mirror of https://github.com/AxioDL/metaforce.git
2
0
Fork 0
Code Issues Projects Releases Wiki Activity

Finish pathfinding implementations

This commit is contained in:
Jack Andersen 2018-03-02 19:49:13 -10:00
parent 2018ef17d2
commit cb2988c9a4
6 changed files with 562 additions and 129 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

122
Runtime/World/CPathFindArea.cpp
View File

@ -48,6 +48,13 @@ int CPFAreaOctree::GetChildIndex(const zeus::CVector3f& point) const
return idx; return idx;
} }
rstl::prereserved_vector<CPFRegion*>* CPFAreaOctree::GetRegionList(const zeus::CVector3f& point)
{
if (x0_isLeaf)
return &x48_regions;
return x28_children[GetChildIndex(point)]->GetRegionList(point);
}
void CPFAreaOctree::GetRegionListList(rstl::reserved_vector<rstl::prereserved_vector<CPFRegion*>*, 32>& listOut, void CPFAreaOctree::GetRegionListList(rstl::reserved_vector<rstl::prereserved_vector<CPFRegion*>*, 32>& listOut,
const zeus::CVector3f& point, float padding) const zeus::CVector3f& point, float padding)
{ {
@ -78,14 +85,52 @@ bool CPFAreaOctree::IsPointInsidePaddedAABox(const zeus::CVector3f& point, float
point.z <= x4_aabb.max.z + padding; point.z <= x4_aabb.max.z + padding;
} }
CPFOpenList::CPFOpenList()
{
}
void CPFOpenList::Clear() void CPFOpenList::Clear()
{ {
x40_region.Data()->SetOpenMore(&x40_region);
x40_region.Data()->SetOpenLess(&x40_region);
x0_bitSet.Clear();
}
void CPFOpenList::Push(CPFRegion* reg)
{
x0_bitSet.Add(reg->GetIndex());
CPFRegion* other = x40_region.Data()->GetOpenMore();
while (other != &x40_region && reg->Data()->GetCost() > other->Data()->GetCost())
other = other->Data()->GetOpenMore();
other->Data()->GetOpenLess()->Data()->SetOpenMore(reg);
reg->Data()->SetOpenLess(other->Data()->GetOpenLess());
other->Data()->SetOpenLess(reg);
reg->Data()->SetOpenMore(other);
}
CPFRegion* CPFOpenList::Pop()
{
CPFRegion* reg = x40_region.Data()->GetOpenMore();
if (reg != &x40_region)
{
x0_bitSet.Rmv(reg->GetIndex());
reg->Data()->GetOpenMore()->Data()->SetOpenLess(reg->Data()->GetOpenLess());
reg->Data()->GetOpenLess()->Data()->SetOpenMore(reg->Data()->GetOpenMore());
reg->Data()->SetOpenMore(nullptr);
reg->Data()->SetOpenLess(nullptr);
return reg;
}
return nullptr;
}
void CPFOpenList::Pop(CPFRegion* reg)
{
x0_bitSet.Rmv(reg->GetIndex());
reg->Data()->GetOpenMore()->Data()->SetOpenLess(reg->Data()->GetOpenLess());
reg->Data()->GetOpenLess()->Data()->SetOpenMore(reg->Data()->GetOpenMore());
reg->Data()->SetOpenMore(nullptr);
reg->Data()->SetOpenLess(nullptr);
}
bool CPFOpenList::Test(CPFRegion* reg)
{
return x0_bitSet.Test(reg->GetIndex());
} }
CPFArea::CPFArea(std::unique_ptr<u8[]>&& buf, u32 len) CPFArea::CPFArea(std::unique_ptr<u8[]>&& buf, u32 len)
@ -143,16 +188,42 @@ CPFArea::CPFArea(std::unique_ptr<u8[]>&& buf, u32 len)
node.Fixup(*this); node.Fixup(*this);
} }
rstl::prereserved_vector<CPFRegion*>* CPFArea::GetOctreeRegionList(const zeus::CVector3f& point)
{
if (x30_hasCachedRegionList && zeus::close_enough(point, x24_cachedRegionListPoint))
return x20_cachedRegionList;
return x158_octree.back().GetRegionList(point);
}
u32 CPFArea::FindRegions(rstl::reserved_vector<CPFRegion*, 4>& regions, const zeus::CVector3f& point,
u32 flags, u32 indexMask)
{
bool isFlyer = (flags & 0x2) != 0;
bool isSwimmer = (flags & 0x4) != 0;
for (CPFRegion* region : *GetOctreeRegionList(point))
{
if (region->GetFlags() & 0xff & flags && (region->GetFlags() >> 16) & 0xff & indexMask &&
region->IsPointInside(point) && (isFlyer || isSwimmer || region->PointHeight(point) < 3.f))
{
regions.push_back(region);
if (regions.size() == regions.capacity())
break;
}
}
return u32(regions.size());
}
CPFRegion* CPFArea::FindClosestRegion(const zeus::CVector3f& point, u32 flags, u32 indexMask, float padding) CPFRegion* CPFArea::FindClosestRegion(const zeus::CVector3f& point, u32 flags, u32 indexMask, float padding)
{ {
rstl::reserved_vector<rstl::prereserved_vector<CPFRegion*>*, 32> regionListList; rstl::reserved_vector<rstl::prereserved_vector<CPFRegion*>*, 32> regionListList;
x158_octree.back().GetRegionListList(regionListList, point, padding); x158_octree.back().GetRegionListList(regionListList, point, padding);
bool isFlyer = (flags & 0x2) != 0; bool isFlyer = (flags & 0x2) != 0;
CPFRegion* ret = nullptr;
for (rstl::prereserved_vector<CPFRegion*>* list : regionListList) for (rstl::prereserved_vector<CPFRegion*>* list : regionListList)
{ {
for (CPFRegion* region : *list) for (CPFRegion* region : *list)
{ {
if (region->Data()->GetCookie() != x34_curRegionCookie) if (region->Data()->GetCookie() != x34_curCookie)
{ {
if (region->GetFlags() & 0xff & flags && if (region->GetFlags() & 0xff & flags &&
(region->GetFlags() >> 16) & 0xff & indexMask && (region->GetFlags() >> 16) & 0xff & indexMask &&
@ -161,19 +232,44 @@ CPFRegion* CPFArea::FindClosestRegion(const zeus::CVector3f& point, u32 flags, u
{ {
if (region->FindBestPoint(x10_tmpPolyPoints, point, flags, padding * padding)) if (region->FindBestPoint(x10_tmpPolyPoints, point, flags, padding * padding))
{ {
// TODO: Finish ret = region;
padding = region->Data()->GetBestPointDistanceSquared() == 0.0 ? 0.f :
std::sqrt(region->Data()->GetBestPointDistanceSquared());
x4_closestPoint = region->Data()->GetBestPoint();
} }
} }
region->Data()->SetCookie(x34_curRegionCookie); region->Data()->SetCookie(x34_curCookie);
} }
} }
} }
return nullptr; return ret;
} }
void CPFArea::FindClosestReachablePoint(rstl::reserved_vector<CPFRegion, 4>&, const zeus::CVector3f&, u32) zeus::CVector3f CPFArea::FindClosestReachablePoint(rstl::reserved_vector<CPFRegion*, 4>& regs,
const zeus::CVector3f& point, u32 flags, u32 indexMask)
{ {
zeus::CVector3f ret;
float closestDistSq = FLT_MAX;
for (CPFRegion& reg : x150_regions)
{
if (reg.GetFlags() & 0xff & flags && (reg.GetFlags() >> 16) & 0xff & indexMask)
{
for (CPFRegion* oreg : regs)
{
if (PathExists(oreg, &reg, flags))
{
float distSq = (reg.GetCentroid() - point).magSquared();
if (distSq < closestDistSq)
{
closestDistSq = distSq;
ret = reg.GetCentroid();
break;
}
}
}
}
}
return ret;
} }
bool CPFArea::PathExists(const CPFRegion* r1, const CPFRegion* r2, u32 flags) const bool CPFArea::PathExists(const CPFRegion* r1, const CPFRegion* r2, u32 flags) const
@ -193,9 +289,9 @@ bool CPFArea::PathExists(const CPFRegion* r1, const CPFRegion* r2, u32 flags) co
auto d = std::div(bit, 32); auto d = std::div(bit, 32);
if ((flags & 0x2) != 0) if ((flags & 0x2) != 0)
return ((x170_connectionsFlyers[d.quot] >> d.rem) & 0x1) == 0x1; return ((x170_connectionsFlyers[d.quot] >> d.rem) & 0x1) != 0;
else else
return ((x168_connectionsGround[d.quot] >> d.rem) & 0x1) == 0x1; return ((x168_connectionsGround[d.quot] >> d.rem) & 0x1) != 0;
} }
CFactoryFnReturn FPathFindAreaFactory(const urde::SObjectTag& tag, CFactoryFnReturn FPathFindAreaFactory(const urde::SObjectTag& tag,

83
Runtime/World/CPathFindArea.hpp
View File

@ -14,12 +14,12 @@ class CObjectReference;
class CPFBitSet class CPFBitSet
{ {
u32 x0_bitmap[16] = {};
public: public:
CPFBitSet() = default; void Clear() { for (u32& w : x0_bitmap) w = 0; }
void Clear(); void Add(s32 i) { x0_bitmap[i / 32] |= (1 << (i % 32)); }
void Add(s32); bool Test(s32 i) const { return (x0_bitmap[i / 32] & (1 << (i % 32))) != 0; }
bool Test(s32); void Rmv(s32 i) { x0_bitmap[i / 32] &= ~(1 << (i % 32)); }
void Rmv(s32);
}; };
class CPFAreaOctree class CPFAreaOctree
@ -33,7 +33,7 @@ public:
CPFAreaOctree(CMemoryInStream& in); CPFAreaOctree(CMemoryInStream& in);
void Fixup(CPFArea& area); void Fixup(CPFArea& area);
int GetChildIndex(const zeus::CVector3f& point) const; int GetChildIndex(const zeus::CVector3f& point) const;
void GetRegionList(const zeus::CVector3f&) const; rstl::prereserved_vector<CPFRegion*>* GetRegionList(const zeus::CVector3f& point);
void GetRegionListList(rstl::reserved_vector<rstl::prereserved_vector<CPFRegion*>*, 32>& listOut, void GetRegionListList(rstl::reserved_vector<rstl::prereserved_vector<CPFRegion*>*, 32>& listOut,
const zeus::CVector3f& point, float padding); const zeus::CVector3f& point, float padding);
bool IsPointInsidePaddedAABox(const zeus::CVector3f& point, float padding) const; bool IsPointInsidePaddedAABox(const zeus::CVector3f& point, float padding) const;
@ -43,33 +43,16 @@ public:
class CPFOpenList class CPFOpenList
{ {
friend class CPFArea; friend class CPFArea;
u32 x0_ = 0; CPFBitSet x0_bitSet;
u32 x4_ = 0;
u32 x8_ = 0;
u32 xc_ = 0;
u32 x10_ = 0;
u32 x14_ = 0;
u32 x18_ = 0;
u32 x1c_ = 0;
u32 x20_ = 0;
u32 x24_ = 0;
u32 x28_ = 0;
u32 x2c_ = 0;
u32 x30_ = 0;
u32 x34_ = 0;
u32 x38_ = 0;
u32 x3c_ = 0;
CPFRegion x40_region; CPFRegion x40_region;
CPFRegionData x90_regionData; CPFRegionData x90_regionData;
public: public:
CPFOpenList();
void Clear(); void Clear();
void Push(CPFRegion*); void Push(CPFRegion* reg);
void Pop(); CPFRegion* Pop();
void Pop(CPFRegion*); void Pop(CPFRegion* reg);
void Test(CPFRegion*); bool Test(CPFRegion* reg);
}; };
class CPFArea class CPFArea
@ -79,30 +62,15 @@ class CPFArea
friend class CPathFindSearch; friend class CPathFindSearch;
float x0_ = FLT_MAX; float x0_ = FLT_MAX;
zeus::CVector3f x4_; zeus::CVector3f x4_closestPoint;
std::vector<zeus::CVector3f> x10_tmpPolyPoints; std::vector<zeus::CVector3f> x10_tmpPolyPoints;
u32 x20_ = 0; rstl::prereserved_vector<CPFRegion*>* x20_cachedRegionList = nullptr;
zeus::CVector3f x24_; zeus::CVector3f x24_cachedRegionListPoint;
bool x30_ = false; bool x30_hasCachedRegionList = false;
s32 x34_curRegionCookie = 0; s32 x34_curCookie = 0;
u32 x38_ = 0; CPFBitSet x38_closedSet;
u32 x3c_ = 0; CPFOpenList x78_openList;
u32 x40_ = 0; //u32 x138_;
u32 x44_ = 0;
u32 x48_ = 0;
u32 x4c_ = 0;
u32 x50_ = 0;
u32 x54_ = 0;
u32 x58_ = 0;
u32 x5c_ = 0;
u32 x60_ = 0;
u32 x64_ = 0;
u32 x68_ = 0;
u32 x6c_ = 0;
u32 x70_ = 0;
u32 x74_ = 0;
CPFOpenList x78_;
u32 x138_;
//std::unique_ptr<u8[]> x13c_data; //std::unique_ptr<u8[]> x13c_data;
/* Used to be prereserved_vectors backed by x13c_data /* Used to be prereserved_vectors backed by x13c_data
* This has been changed to meet storage requirements of * This has been changed to meet storage requirements of
@ -122,19 +90,20 @@ public:
void SetTransform(const zeus::CTransform& xf) { x188_transform = xf; } void SetTransform(const zeus::CTransform& xf) { x188_transform = xf; }
const zeus::CTransform& GetTransform() const { return x188_transform; } const zeus::CTransform& GetTransform() const { return x188_transform; }
const CPFRegion& GetRegion(s32 i) const { return x150_regions[i]; } const CPFRegion& GetRegion(s32 i) const { return x150_regions[i]; }
void GetClosestPoint() const; const zeus::CVector3f& GetClosestPoint() const { return x4_closestPoint; }
void OpenList(); CPFOpenList& OpenList() { return x78_openList; }
void ClosedSet(); CPFBitSet& ClosedSet() { return x38_closedSet; }
const CPFRegionData& GetRegionData(s32 i) const { return x178_regionDatas[i]; } const CPFRegionData& GetRegionData(s32 i) const { return x178_regionDatas[i]; }
const CPFLink& GetLink(s32 i) const { return x148_links[i]; } const CPFLink& GetLink(s32 i) const { return x148_links[i]; }
const CPFNode& GetNode(s32 i) const { return x140_nodes[i]; } const CPFNode& GetNode(s32 i) const { return x140_nodes[i]; }
const CPFAreaOctree& GetOctree(s32 i) const { return x158_octree[i]; } const CPFAreaOctree& GetOctree(s32 i) const { return x158_octree[i]; }
const CPFRegion* GetOctreeRegionPtrs(s32 i) const { return x160_octreeRegionLookup[i]; } const CPFRegion* GetOctreeRegionPtrs(s32 i) const { return x160_octreeRegionLookup[i]; }
void GetOctreeRegionList(const zeus::CVector3f&); rstl::prereserved_vector<CPFRegion*>* GetOctreeRegionList(const zeus::CVector3f& point);
u32 FindRegions(rstl::reserved_vector<CPFRegion, 4>& regions, const zeus::CVector3f& point, u32 FindRegions(rstl::reserved_vector<CPFRegion*, 4>& regions, const zeus::CVector3f& point,
u32 flags, u32 indexMask); u32 flags, u32 indexMask);
CPFRegion* FindClosestRegion(const zeus::CVector3f& point, u32 flags, u32 indexMask, float padding); CPFRegion* FindClosestRegion(const zeus::CVector3f& point, u32 flags, u32 indexMask, float padding);
void FindClosestReachablePoint(rstl::reserved_vector<CPFRegion, 4>&, const zeus::CVector3f&, u32); zeus::CVector3f FindClosestReachablePoint(rstl::reserved_vector<CPFRegion*, 4>& regs,
const zeus::CVector3f& point, u32 flags, u32 indexMask);
bool PathExists(const CPFRegion* r1, const CPFRegion* r2, u32 flags) const; bool PathExists(const CPFRegion* r1, const CPFRegion* r2, u32 flags) const;
}; };

68
Runtime/World/CPathFindRegion.cpp
View File

@ -33,6 +33,11 @@ CPFRegion::CPFRegion(CMemoryInStream& in)
x4c_regionData = reinterpret_cast<CPFRegionData*>(in.readUint32Big()); x4c_regionData = reinterpret_cast<CPFRegionData*>(in.readUint32Big());
} }
const CPFLink* CPFRegion::GetPathLink() const
{
return &xc_startLink[x4c_regionData->GetPathLink()];
}
void CPFRegion::Fixup(CPFArea& area, u32& maxRegionNodes) void CPFRegion::Fixup(CPFArea& area, u32& maxRegionNodes)
{ {
if (x0_numNodes) if (x0_numNodes)
@ -48,6 +53,24 @@ void CPFRegion::Fixup(CPFArea& area, u32& maxRegionNodes)
maxRegionNodes = x0_numNodes; maxRegionNodes = x0_numNodes;
} }
bool CPFRegion::IsPointInside(const zeus::CVector3f& point) const
{
if (!x34_aabb.pointInside(point))
return false;
int i;
for (i=0 ; i<x0_numNodes ; ++i)
{
CPFNode& node = x4_startNode[i];
if ((point - node.GetPos()).dot(node.GetNormal()) < 0.f)
break;
}
zeus::CVector3f nodeToPoint = point - x4_startNode->GetPos();
if (i == x0_numNodes && nodeToPoint.dot(x18_normal) >= 0.f)
if ((nodeToPoint - x14_height * zeus::CVector3f::skUp).dot(x18_normal) <= 0.f)
return true;
return false;
}
float CPFRegion::PointHeight(const zeus::CVector3f& point) const float CPFRegion::PointHeight(const zeus::CVector3f& point) const
{ {
return (point - x4_startNode->GetPos()).dot(x18_normal); return (point - x4_startNode->GetPos()).dot(x18_normal);
@ -170,14 +193,38 @@ bool CPFRegion::FindBestPoint(std::vector<zeus::CVector3f>& polyPoints, const ze
return found; return found;
} }
void CPFRegion::SetLinkTo(s32 idx)
{
if (x8_numLinks <= 0)
return;
for (s32 i=0 ; i<x8_numLinks ; ++i)
if (xc_startLink[i].GetRegion() == idx)
{
Data()->SetPathLink(i);
return;
}
}
void CPFRegion::DropToGround(zeus::CVector3f& point) const
{
point.z -= (point - x4_startNode->GetPos()).dot(x18_normal) / x18_normal.z;
}
zeus::CVector3f CPFRegion::GetLinkMidPoint(const CPFLink& link) const
{
const CPFNode& node = x4_startNode[link.GetNode()];
const CPFNode& nextNode = x4_startNode[(link.GetNode() + 1) % x0_numNodes];
return (node.GetPos() + nextNode.GetPos()) * 0.5f;
}
zeus::CVector3f CPFRegion::FitThroughLink2d(const zeus::CVector3f& p1, const CPFLink& link, zeus::CVector3f CPFRegion::FitThroughLink2d(const zeus::CVector3f& p1, const CPFLink& link,
const zeus::CVector3f& p2, float f1) const const zeus::CVector3f& p2, float chRadius) const
{ {
CPFNode& node = x4_startNode[link.GetNode()]; CPFNode& node = x4_startNode[link.GetNode()];
CPFNode& nextNode = x4_startNode[(link.GetNode() + 1) % x0_numNodes]; CPFNode& nextNode = x4_startNode[(link.GetNode() + 1) % x0_numNodes];
zeus::CVector3f nodeDelta = nextNode.GetPos() - node.GetPos(); zeus::CVector3f nodeDelta = nextNode.GetPos() - node.GetPos();
float t = 0.5f; float t = 0.5f;
if (f1 < 0.5f * link.Get2dWidth()) if (chRadius < 0.5f * link.Get2dWidth())
{ {
zeus::CVector2f delta2d(nodeDelta.x, nodeDelta.y); zeus::CVector2f delta2d(nodeDelta.x, nodeDelta.y);
delta2d *= link.GetOO2dWidth(); delta2d *= link.GetOO2dWidth();
@ -189,13 +236,15 @@ zeus::CVector3f CPFRegion::FitThroughLink2d(const zeus::CVector3f& p1, const CPF
float f1b = delta2d.dot(zeus::CVector2f(nodeToP2.y, nodeToP2.y)); float f1b = delta2d.dot(zeus::CVector2f(nodeToP2.y, nodeToP2.y));
float f3 = f27 + f26; float f3 = f27 + f26;
if (f3 > FLT_EPSILON) if (f3 > FLT_EPSILON)
t = zeus::clamp(f1, 1.f / f3 * (f26 * f31 + f27 * f1b), link.Get2dWidth() - f1) * link.GetOO2dWidth(); t = zeus::clamp(chRadius, 1.f / f3 * (f26 * f31 + f27 * f1b), link.Get2dWidth() - chRadius) *
link.GetOO2dWidth();
} }
return nodeDelta * t + node.GetPos(); return nodeDelta * t + node.GetPos();
} }
zeus::CVector3f CPFRegion::FitThroughLink3d(const zeus::CVector3f& p1, const CPFLink& link, zeus::CVector3f CPFRegion::FitThroughLink3d(const zeus::CVector3f& p1, const CPFLink& link,
float f1, const zeus::CVector3f& p2, float f2, float f3) const float regionHeight, const zeus::CVector3f& p2,
float chRadius, float chHalfHeight) const
{ {
CPFNode& node = x4_startNode[link.GetNode()]; CPFNode& node = x4_startNode[link.GetNode()];
CPFNode& nextNode = x4_startNode[(link.GetNode() + 1) % x0_numNodes]; CPFNode& nextNode = x4_startNode[(link.GetNode() + 1) % x0_numNodes];
@ -204,7 +253,7 @@ zeus::CVector3f CPFRegion::FitThroughLink3d(const zeus::CVector3f& p1, const CPF
float f24 = (node.GetPos() - p2).dot(node.GetNormal()); float f24 = (node.GetPos() - p2).dot(node.GetNormal());
float f23 = f25 + f24; float f23 = f25 + f24;
# if 0 # if 0
if (f2 < 0.5f * link.Get2dWidth()) if (chRadius < 0.5f * link.Get2dWidth())
{ {
zeus::CVector2f delta2d(nodeDelta.x, nodeDelta.y); zeus::CVector2f delta2d(nodeDelta.x, nodeDelta.y);
delta2d *= link.GetOO2dWidth(); delta2d *= link.GetOO2dWidth();
@ -214,19 +263,20 @@ zeus::CVector3f CPFRegion::FitThroughLink3d(const zeus::CVector3f& p1, const CPF
float f1b = delta2d.dot(zeus::CVector2f(nodeToP2.y, nodeToP2.y)); float f1b = delta2d.dot(zeus::CVector2f(nodeToP2.y, nodeToP2.y));
if (f23 > FLT_EPSILON) if (f23 > FLT_EPSILON)
{ {
zeus::clamp(f2, 1.f / f23 * f24 * f29 + f25 * f1b, link.Get2dWidth() - f2) * link.GetOO2dWidth(); zeus::clamp(chRadius, 1.f / f23 * f24 * f29 + f25 * f1b, link.Get2dWidth() - chRadius) *
link.GetOO2dWidth();
} }
} }
#endif #endif
zeus::CVector3f midPoint = nodeDelta * 0.5f + node.GetPos(); zeus::CVector3f midPoint = nodeDelta * 0.5f + node.GetPos();
float z; float z;
if (f3 < 0.5f * f1) if (chHalfHeight < 0.5f * regionHeight)
{ {
float minZ = f3 + midPoint.z; float minZ = chHalfHeight + midPoint.z;
z = 0.5f * (p1.z + p2.z); z = 0.5f * (p1.z + p2.z);
if (f23 > FLT_EPSILON) if (f23 > FLT_EPSILON)
z = 1.f / f23 * (f24 * p1.z + f25 * p2.z); z = 1.f / f23 * (f24 * p1.z + f25 * p2.z);
z = zeus::clamp(minZ, z, f1 + midPoint.z - f3); z = zeus::clamp(minZ, z, regionHeight + midPoint.z - chHalfHeight);
} }
else else
{ {

45
Runtime/World/CPathFindRegion.hpp
View File

@ -54,14 +54,14 @@ public:
CPFRegionData* Data() const { return x4c_regionData; } CPFRegionData* Data() const { return x4c_regionData; }
u32 GetIndex() const { return x24_regionIdx; } u32 GetIndex() const { return x24_regionIdx; }
float GetHeight() const { return x14_height; } float GetHeight() const { return x14_height; }
void GetPathLink() const {} const CPFLink* GetPathLink() const;
u32 GetNumLinks() const { return x8_numLinks; } u32 GetNumLinks() const { return x8_numLinks; }
u32 GetFlags() const { return x10_flags; } u32 GetFlags() const { return x10_flags; }
const CPFLink* GetLink(u32 i) const { return xc_startLink + i; } const CPFLink* GetLink(u32 i) const { return xc_startLink + i; }
void SetCentroid(const zeus::CVector3f&); void SetCentroid(const zeus::CVector3f& c) { x28_centroid = c; }
const zeus::CVector3f& GetCentroid() const { return x28_centroid; } const zeus::CVector3f& GetCentroid() const { return x28_centroid; }
void Fixup(CPFArea& area, u32& maxRegionNodes); void Fixup(CPFArea& area, u32& maxRegionNodes);
bool IsPointInside(const zeus::CVector3f&); bool IsPointInside(const zeus::CVector3f& point) const;
const zeus::CVector3f& GetNormal() const { return x18_normal; } const zeus::CVector3f& GetNormal() const { return x18_normal; }
u32 GetNumNodes() const { return x0_numNodes; } u32 GetNumNodes() const { return x0_numNodes; }
const CPFNode* GetNode(u32 i) const { return x4_startNode + i; } const CPFNode* GetNode(u32 i) const { return x4_startNode + i; }
@ -69,11 +69,14 @@ public:
bool FindClosestPointOnPolygon(const std::vector<zeus::CVector3f>&, const zeus::CVector3f&, bool FindClosestPointOnPolygon(const std::vector<zeus::CVector3f>&, const zeus::CVector3f&,
const zeus::CVector3f&, bool); const zeus::CVector3f&, bool);
bool FindBestPoint(std::vector<zeus::CVector3f>& polyPoints, const zeus::CVector3f& point, u32 flags, float paddingSq); bool FindBestPoint(std::vector<zeus::CVector3f>& polyPoints, const zeus::CVector3f& point, u32 flags, float paddingSq);
void SetLinkTo(s32); void SetLinkTo(s32 idx);
void DropToGround(zeus::CVector3f&) const; void DropToGround(zeus::CVector3f& point) const;
void GetLinkMidPoint(const CPFLink&); zeus::CVector3f GetLinkMidPoint(const CPFLink& link) const;
zeus::CVector3f FitThroughLink2d(const zeus::CVector3f&, const CPFLink&, const zeus::CVector3f&, float) const; zeus::CVector3f FitThroughLink2d(const zeus::CVector3f& p1, const CPFLink& link,
zeus::CVector3f FitThroughLink3d(const zeus::CVector3f&, const CPFLink&, float, const zeus::CVector3f&, float, float) const; const zeus::CVector3f& p2, float chRadius) const;
zeus::CVector3f FitThroughLink3d(const zeus::CVector3f& p1, const CPFLink& link,
float regionHeight, const zeus::CVector3f& p2,
float chRadius, float chHalfHeight) const;
bool IsPointInsidePaddedAABox(const zeus::CVector3f& point, float padding) const; bool IsPointInsidePaddedAABox(const zeus::CVector3f& point, float padding) const;
}; };
@ -82,8 +85,10 @@ class CPFRegionData
float x0_bestPointDistSq = 0.f; float x0_bestPointDistSq = 0.f;
zeus::CVector3f x4_bestPoint; zeus::CVector3f x4_bestPoint;
s32 x10_cookie = -1; s32 x10_cookie = -1;
zeus::CVector3f x14_; float x14_cost = 0.f;
s32 x20_ = 0; float x18_g = 0.f;
float x1c_h = 0.f;
CPFRegion* x20_parent = nullptr;
CPFRegion* x24_openLess = nullptr; CPFRegion* x24_openLess = nullptr;
CPFRegion* x28_openMore = nullptr; CPFRegion* x28_openMore = nullptr;
s32 x2c_pathLink = 0; s32 x2c_pathLink = 0;
@ -94,17 +99,31 @@ public:
void SetOpenMore(CPFRegion* r) { x28_openMore = r; } void SetOpenMore(CPFRegion* r) { x28_openMore = r; }
CPFRegion* GetOpenLess() const { return x24_openLess; } CPFRegion* GetOpenLess() const { return x24_openLess; }
CPFRegion* GetOpenMore() const { return x28_openMore; } CPFRegion* GetOpenMore() const { return x28_openMore; }
void GetCost(); float GetCost() const { return x14_cost; }
float GetG() const { return x18_g; }
s32 GetPathLink() const { return x2c_pathLink; } s32 GetPathLink() const { return x2c_pathLink; }
void SetPathLink(s32 l) { x2c_pathLink = l; } void SetPathLink(s32 l) { x2c_pathLink = l; }
void GetParent() const; CPFRegion* GetParent() const { return x20_parent; }
void Setup(CPFRegion*, float, float);
void SetBestPoint(const zeus::CVector3f& bestPoint) { x4_bestPoint = bestPoint; } void SetBestPoint(const zeus::CVector3f& bestPoint) { x4_bestPoint = bestPoint; }
const zeus::CVector3f& GetBestPoint() const { return x4_bestPoint; } const zeus::CVector3f& GetBestPoint() const { return x4_bestPoint; }
void SetBestPointDistanceSquared(float distSq) { x0_bestPointDistSq = distSq; } void SetBestPointDistanceSquared(float distSq) { x0_bestPointDistSq = distSq; }
float GetBestPointDistanceSquared() const { return x0_bestPointDistSq; } float GetBestPointDistanceSquared() const { return x0_bestPointDistSq; }
void SetCookie(s32 c) { x10_cookie = c; } void SetCookie(s32 c) { x10_cookie = c; }
s32 GetCookie() const { return x10_cookie; } s32 GetCookie() const { return x10_cookie; }
void Setup(CPFRegion* parent, float g, float h)
{
x20_parent = parent;
x18_g = g;
x1c_h = h;
x14_cost = x18_g + x1c_h;
}
void Setup(CPFRegion* parent, float g)
{
x20_parent = parent;
x18_g = g;
x14_cost = x18_g + x1c_h;
}
}; };
} }

348
Runtime/World/CPathFindSearch.cpp
View File

@ -3,47 +3,343 @@
namespace urde namespace urde
{ {
CPathFindSearch::CPathFindSearch(CPFArea* area, u32 flags, u32 w2, float f1, float f2) CPathFindSearch::CPathFindSearch(CPFArea* area, u32 flags, u32 index, float chRadius, float chHeight)
: x0_area(area), xd0_f2(f2), xd4_f1(f1), xdc_flags(flags), xe0_indexMask(1u << w2) : x0_area(area), xd0_chHeight(chHeight), xd4_chRadius(chRadius), xdc_flags(flags), xe0_indexMask(1u << index)
{}
CPathFindSearch::EResult
CPathFindSearch::FindClosestReachablePoint(const zeus::CVector3f& p1, zeus::CVector3f& p2) const
{ {
if (!x0_area)
return EResult::InvalidArea;
} /* Work in local PFArea coordinates */
CPathFindSearch::EResult CPathFindSearch::Search(const zeus::CVector3f& p1, const zeus::CVector3f& p2)
{
x4_.clear();
xc8_ = 0;
if (!x0_area || x0_area->x150_regions.size() > 512)
{
xcc_result = EResult::One;
return xcc_result;
}
if (zeus::close_enough(p1, p2))
{
x4_.push_back(p1);
xcc_result = EResult::Zero;
return xcc_result;
}
zeus::CVector3f localP1 = x0_area->x188_transform.transposeRotate(p1 - x0_area->x188_transform.origin); zeus::CVector3f localP1 = x0_area->x188_transform.transposeRotate(p1 - x0_area->x188_transform.origin);
zeus::CVector3f localP2 = x0_area->x188_transform.transposeRotate(p2 - x0_area->x188_transform.origin); zeus::CVector3f localP2 = x0_area->x188_transform.transposeRotate(p2 - x0_area->x188_transform.origin);
/* Raise a bit above ground for step-up resolution */
if (!(xdc_flags & 0x2) && !(xdc_flags & 0x4)) if (!(xdc_flags & 0x2) && !(xdc_flags & 0x4))
{ {
localP2.z += 0.3f; localP2.z += 0.3f;
localP1.z += 0.3f; localP1.z += 0.3f;
} }
rstl::reserved_vector<CPFRegion, 4> regions; rstl::reserved_vector<CPFRegion*, 4> regions;
if (x0_area->FindRegions(regions, localP1, xdc_flags, xe0_indexMask) == 0) if (x0_area->FindRegions(regions, localP1, xdc_flags, xe0_indexMask) == 0)
{ {
x0_area->FindClosestRegion(localP1, xdc_flags, xe0_indexMask, xd8_); /* Point outside PATH; find nearest region point */
CPFRegion* region = x0_area->FindClosestRegion(localP1, xdc_flags, xe0_indexMask, xd8_padding);
if (!region)
return EResult::NoSourcePoint;
regions.push_back(region);
} }
// TODO: Finish /* Override dest point to be reachable */
return EResult::Zero; zeus::CVector3f closestPoint =
x0_area->FindClosestReachablePoint(regions, localP2, xdc_flags, xe0_indexMask) +
zeus::CVector3f(0.f, 0.f, 3.f);
p2 = x0_area->x188_transform * closestPoint;
return EResult::Success;
}
CPathFindSearch::EResult CPathFindSearch::Search(const zeus::CVector3f& p1, const zeus::CVector3f& p2)
{
u32 firstPoint = 0;
u32 flyToOutsidePoint = 0;
x4_waypoints.clear();
xc8_ = 0;
if (!x0_area || x0_area->x150_regions.size() > 512)
{
xcc_result = EResult::InvalidArea;
return xcc_result;
}
if (zeus::close_enough(p1, p2))
{
/* That was easy */
x4_waypoints.push_back(p1);
xcc_result = EResult::Success;
return xcc_result;
}
/* Work in local PFArea coordinates */
zeus::CVector3f localP1 = x0_area->x188_transform.transposeRotate(p1 - x0_area->x188_transform.origin);
zeus::CVector3f localP2 = x0_area->x188_transform.transposeRotate(p2 - x0_area->x188_transform.origin);
/* Raise a bit above ground for step-up resolution */
if (!(xdc_flags & 0x2) && !(xdc_flags & 0x4))
{
localP2.z += 0.3f;
localP1.z += 0.3f;
}
rstl::reserved_vector<CPFRegion*, 4> regions1;
rstl::reserved_vector<zeus::CVector3f, 16> points;
if (x0_area->FindRegions(regions1, localP1, xdc_flags, xe0_indexMask) == 0)
{
/* Point outside PATH; find nearest region point */
CPFRegion* region = x0_area->FindClosestRegion(localP1, xdc_flags, xe0_indexMask, xd8_padding);
if (!region)
{
xcc_result = EResult::NoSourcePoint;
return xcc_result;
}
if (xdc_flags & 0x2 || xdc_flags & 0x4)
{
points.push_back(localP1);
firstPoint = 1;
}
regions1.push_back(region);
localP1 = x0_area->GetClosestPoint();
}
zeus::CVector3f finalP2 = localP2;
rstl::reserved_vector<CPFRegion*, 4> regions2;
if (x0_area->FindRegions(regions2, localP2, xdc_flags, xe0_indexMask) == 0)
{
/* Point outside PATH; find nearest region point */
CPFRegion* region = x0_area->FindClosestRegion(localP2, xdc_flags, xe0_indexMask, xd8_padding);
if (!region)
{
xcc_result = EResult::NoDestPoint;
return xcc_result;
}
if (xdc_flags & 0x2 || xdc_flags & 0x4)
{
flyToOutsidePoint = 1;
}
regions2.push_back(region);
localP2 = x0_area->GetClosestPoint();
}
rstl::reserved_vector<CPFRegion*, 4> regions1Uniq;
rstl::reserved_vector<CPFRegion*, 4> regions2Uniq;
bool noPath = true;
for (CPFRegion* reg1 : regions1)
{
for (CPFRegion* reg2 : regions2)
{
if (reg1 == reg2)
{
/* Route within one region */
if (!(xdc_flags & 0x2) && !(xdc_flags & 0x4))
{
reg2->DropToGround(localP1);
reg2->DropToGround(localP2);
}
x4_waypoints.push_back(x0_area->x188_transform * localP1);
if (!zeus::close_enough(localP1, localP2))
x4_waypoints.push_back(x0_area->x188_transform * localP2);
if (flyToOutsidePoint && !zeus::close_enough(localP2, finalP2))
x4_waypoints.push_back(x0_area->x188_transform * finalP2);
xcc_result = EResult::Success;
return xcc_result;
}
if (x0_area->PathExists(reg1, reg2, xdc_flags))
{
/* Build unique source/dest region lists */
if (std::find(regions1Uniq.rbegin(), regions1Uniq.rend(), reg1) == regions1Uniq.rend())
regions1Uniq.push_back(reg1);
if (std::find(regions2Uniq.rbegin(), regions2Uniq.rend(), reg2) == regions2Uniq.rend())
regions2Uniq.push_back(reg2);
noPath = false;
}
}
}
/* Perform A* algorithm if path is known to exist */
if (noPath || !Search(regions1Uniq, localP1, regions2Uniq, localP2))
{
xcc_result = EResult::NoPath;
return xcc_result;
}
/* Set forward links with best path */
CPFRegion* reg = regions2Uniq[0];
u32 lastPoint = 0;
do {
reg->Data()->GetParent()->SetLinkTo(reg->GetIndex());
++lastPoint;
} while (reg->Data()->GetParent() != regions1Uniq[0]);
/* Setup point range */
bool includeP2 = true;
lastPoint -= 1;
firstPoint += 1;
lastPoint += firstPoint;
if (lastPoint > 15)
lastPoint = 15;
if (lastPoint + flyToOutsidePoint + 1 > 15)
includeP2 = false;
/* Ensure start and finish points are on ground */
if (!(xdc_flags & 0x2) && !(xdc_flags & 0x4))
{
regions1Uniq[0]->DropToGround(localP1);
regions2Uniq[0]->DropToGround(localP2);
}
/* Gather link points using midpoints */
float chHalfHeight = 0.5f * xd0_chHeight;
points.push_back(localP1);
reg = regions1Uniq[0];
for (u32 i=firstPoint ; i<=lastPoint ; ++i)
{
const CPFLink* link = reg->GetPathLink();
CPFRegion* linkReg = &x0_area->x150_regions[link->GetRegion()];
zeus::CVector3f midPoint = reg->GetLinkMidPoint(*link);
if (xdc_flags & 0x2 || xdc_flags & 0x4)
{
float minHeight = std::min(reg->GetHeight(), linkReg->GetHeight());
midPoint.z = zeus::clamp(chHalfHeight + midPoint.z, p2.z, minHeight + midPoint.z - chHalfHeight);
}
points.push_back(midPoint);
reg = linkReg;
}
/* Gather finish points */
if (includeP2)
{
points.push_back(localP2);
if (flyToOutsidePoint)
points.push_back(finalP2);
}
/* Optimize link points using character radius and height */
for (int i=0 ; i<2 ; ++i)
{
reg = regions1Uniq[0];
for (u32 j=firstPoint ; j<=(includeP2 ? lastPoint : lastPoint-1) ; ++j)
{
const CPFLink* link = reg->GetPathLink();
CPFRegion* linkReg = &x0_area->x150_regions[link->GetRegion()];
if (xdc_flags & 0x2 || xdc_flags & 0x4)
{
float minHeight = std::min(reg->GetHeight(), linkReg->GetHeight());
points[j] = reg->FitThroughLink3d(points[j-1], *link, minHeight, points[j+1],
xd4_chRadius, chHalfHeight);
}
else
{
points[j] = reg->FitThroughLink2d(points[j-1], *link, points[j+1], xd4_chRadius);
}
reg = linkReg;
}
}
/* Write out points */
for (u32 i=0 ; i<points.size() ; ++i)
if (i == points.size()-1 || !zeus::close_enough(points[i], points[i+1]))
x4_waypoints.push_back(x0_area->x188_transform * points[i]);
/* Done! */
xcc_result = EResult::Success;
return xcc_result;
}
/* A* search algorithm
* Reference: https://en.wikipedia.org/wiki/A*_search_algorithm
*/
bool CPathFindSearch::Search(rstl::reserved_vector<CPFRegion*, 4>& regs1, const zeus::CVector3f& p1,
rstl::reserved_vector<CPFRegion*, 4>& regs2, const zeus::CVector3f& p2)
{
/* Reset search sets */
x0_area->ClosedSet().Clear();
x0_area->OpenList().Clear();
/* Backup dest centroids */
rstl::reserved_vector<zeus::CVector3f, 4> centroidBackup2;
for (CPFRegion* reg2 : regs2)
{
centroidBackup2.push_back(reg2->GetCentroid());
reg2->SetCentroid(p2);
}
/* Initial heuristic */
float h = (p2 - p1).magnitude();
/* Backup source centroids and initialize heuristics */
rstl::reserved_vector<zeus::CVector3f, 4> centroidBackup1;
for (CPFRegion* reg1 : regs1)
{
centroidBackup1.push_back(reg1->GetCentroid());
reg1->SetCentroid(p1);
reg1->Data()->Setup(nullptr, 0.f, h);
x0_area->OpenList().Push(reg1);
}
/* Resolve path */
CPFRegion* reg;
while ((reg = x0_area->OpenList().Pop()))
{
/* Stop if we're at the destination */
if (std::find(regs2.begin(), regs2.end(), reg) != regs2.end())
break;
/* Exclude region from further resolves */
x0_area->ClosedSet().Add(reg->GetIndex());
for (u32 i=0 ; i<reg->GetNumLinks() ; ++i)
{
/* Potential link to follow */
CPFRegion* linkReg = &x0_area->x150_regions[reg->GetLink(i)->GetRegion()];
if (linkReg != reg->Data()->GetParent() && linkReg->GetFlags() & 0xff & xdc_flags &&
(linkReg->GetFlags() >> 16) & 0xff & xe0_indexMask)
{
/* Next G */
float g = (linkReg->GetCentroid() - reg->GetCentroid()).magnitude() + reg->Data()->GetG();
if ((!x0_area->ClosedSet().Test(linkReg->GetIndex()) && !x0_area->OpenList().Test(linkReg)) ||
linkReg->Data()->GetG() <= g)
{
if (x0_area->OpenList().Test(linkReg))
{
/* In rare cases, revisiting a region will yield a lower G (actual cost) */
x0_area->OpenList().Pop(linkReg);
linkReg->Data()->Setup(reg, g);
}
else
{
/* Compute next heuristic */
x0_area->ClosedSet().Rmv(linkReg->GetIndex());
float h = (p2 - linkReg->GetCentroid()).magnitude();
linkReg->Data()->Setup(reg, g, h);
}
/* Make next potential candidate */
x0_area->OpenList().Push(linkReg);
}
}
}
}
/* Restore source centroids */
auto p1It = centroidBackup1.begin();
for (CPFRegion* r : regs1)
r->SetCentroid(*p1It++);
/* Restore dest centroids */
auto p2It = centroidBackup2.begin();
for (CPFRegion* r : regs2)
r->SetCentroid(*p2It++);
/* Best destination region */
if (reg)
{
regs2.clear();
regs2.push_back(reg);
/* Retrace parents to find best source region */
while (CPFRegion* p = reg->Data()->GetParent())
reg = p;
regs1.clear();
regs1.push_back(reg);
}
return reg != nullptr;
} }
} }

25
Runtime/World/CPathFindSearch.hpp
View File

@ -12,25 +12,28 @@ class CPathFindSearch
public: public:
enum class EResult enum class EResult
{ {
Zero, Success,
One, InvalidArea,
Two, NoSourcePoint,
Three, NoDestPoint,
Four NoPath
}; };
private: private:
CPFArea* x0_area; CPFArea* x0_area;
rstl::reserved_vector<zeus::CVector3f, 16> x4_; rstl::reserved_vector<zeus::CVector3f, 16> x4_waypoints;
u32 xc8_ = 0; u32 xc8_ = 0;
EResult xcc_result; EResult xcc_result;
float xd0_f2; float xd0_chHeight;
float xd4_f1; float xd4_chRadius;
float xd8_ = 10.f; float xd8_padding = 10.f;
u32 xdc_flags; // 0x2: flyer, 0x4: path-always-exists u32 xdc_flags; // 0x2: flyer, 0x4: path-always-exists (swimmers)
u32 xe0_indexMask; u32 xe0_indexMask;
bool Search(rstl::reserved_vector<CPFRegion*, 4>& regs1, const zeus::CVector3f& p1,
rstl::reserved_vector<CPFRegion*, 4>& regs2, const zeus::CVector3f& p2);
public: public:
CPathFindSearch(CPFArea* area, u32 flags, u32 w2, float f1, float f2); CPathFindSearch(CPFArea* area, u32 flags, u32 index, float chRadius, float chHeight);
EResult Search(const zeus::CVector3f& p1, const zeus::CVector3f& p2); EResult Search(const zeus::CVector3f& p1, const zeus::CVector3f& p2);
EResult FindClosestReachablePoint(const zeus::CVector3f& p1, zeus::CVector3f& p2) const;
}; };
} }