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

Work on pathfinding

This commit is contained in:
Jack Andersen 2018-02-28 20:17:16 -10:00
parent 0c95ee6665
commit 2018ef17d2
6 changed files with 260 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

65
Runtime/World/CPathFindArea.cpp
View File

@ -48,6 +48,36 @@ int CPFAreaOctree::GetChildIndex(const zeus::CVector3f& point) const
return idx;
}
void CPFAreaOctree::GetRegionListList(rstl::reserved_vector<rstl::prereserved_vector<CPFRegion*>*, 32>& listOut,
const zeus::CVector3f& point, float padding)
{
if (listOut.size() >= listOut.capacity())
return;
if (x0_isLeaf)
{
listOut.push_back(&x48_regions);
return;
}
for (int i=0 ; i<8 ; ++i)
{
CPFAreaOctree* ch = x28_children[i];
if (ch->IsPointInsidePaddedAABox(point, padding))
ch->GetRegionListList(listOut, point, padding);
}
}
bool CPFAreaOctree::IsPointInsidePaddedAABox(const zeus::CVector3f& point, float padding) const
{
return point.x >= x4_aabb.min.x - padding &&
point.x <= x4_aabb.max.x + padding &&
point.y >= x4_aabb.min.y - padding &&
point.y <= x4_aabb.max.y + padding &&
point.z >= x4_aabb.min.z - padding &&
point.z <= x4_aabb.max.z + padding;
}
CPFOpenList::CPFOpenList()
{
@ -84,7 +114,7 @@ CPFArea::CPFArea(std::unique_ptr<u8[]>&& buf, u32 len)
region.Fixup(*this, maxRegionNodes);
maxRegionNodes = std::max(maxRegionNodes, 4u);
x10_.reserve(maxRegionNodes);
x10_tmpPolyPoints.reserve(maxRegionNodes);
u32 numBitfieldWords = (numRegions * (numRegions - 1) / 2 + 31) / 32;
x168_connectionsGround.reserve(numBitfieldWords);
@ -113,6 +143,39 @@ CPFArea::CPFArea(std::unique_ptr<u8[]>&& buf, u32 len)
node.Fixup(*this);
}
CPFRegion* CPFArea::FindClosestRegion(const zeus::CVector3f& point, u32 flags, u32 indexMask, float padding)
{
rstl::reserved_vector<rstl::prereserved_vector<CPFRegion*>*, 32> regionListList;
x158_octree.back().GetRegionListList(regionListList, point, padding);
bool isFlyer = (flags & 0x2) != 0;
for (rstl::prereserved_vector<CPFRegion*>* list : regionListList)
{
for (CPFRegion* region : *list)
{
if (region->Data()->GetCookie() != x34_curRegionCookie)
{
if (region->GetFlags() & 0xff & flags &&
(region->GetFlags() >> 16) & 0xff & indexMask &&
region->IsPointInsidePaddedAABox(point, padding) &&
(isFlyer || region->PointHeight(point) < 3.f))
{
if (region->FindBestPoint(x10_tmpPolyPoints, point, flags, padding * padding))
{
// TODO: Finish
}
}
region->Data()->SetCookie(x34_curRegionCookie);
}
}
}
return nullptr;
}
void CPFArea::FindClosestReachablePoint(rstl::reserved_vector<CPFRegion, 4>&, const zeus::CVector3f&, u32)
{
}
bool CPFArea::PathExists(const CPFRegion* r1, const CPFRegion* r2, u32 flags) const
{
if (r1 == r2 || (flags & 0x4) != 0)

15
Runtime/World/CPathFindArea.hpp
View File

@ -34,8 +34,9 @@ public:
void Fixup(CPFArea& area);
int GetChildIndex(const zeus::CVector3f& point) const;
void GetRegionList(const zeus::CVector3f&) const;
void GetRegionListList(rstl::reserved_vector<rstl::prereserved_vector<CPFRegion*>*, 32>, const zeus::CVector3f&, float);
bool IsPointInPaddedAABox(const zeus::CVector3f&, float);
void GetRegionListList(rstl::reserved_vector<rstl::prereserved_vector<CPFRegion*>*, 32>& listOut,
const zeus::CVector3f& point, float padding);
bool IsPointInsidePaddedAABox(const zeus::CVector3f& point, float padding) const;
void Render();
};
@ -75,14 +76,15 @@ class CPFArea
{
friend class CPFRegion;
friend class CPFAreaOctree;
friend class CPathFindSearch;
float x0_ = FLT_MAX;
zeus::CVector3f x4_;
std::vector<zeus::CVector3f> x10_;
std::vector<zeus::CVector3f> x10_tmpPolyPoints;
u32 x20_ = 0;
zeus::CVector3f x24_;
bool x30_ = false;
u32 x34_ = 0;
s32 x34_curRegionCookie = 0;
u32 x38_ = 0;
u32 x3c_ = 0;
u32 x40_ = 0;
@ -129,8 +131,9 @@ public:
const CPFAreaOctree& GetOctree(s32 i) const { return x158_octree[i]; }
const CPFRegion* GetOctreeRegionPtrs(s32 i) const { return x160_octreeRegionLookup[i]; }
void GetOctreeRegionList(const zeus::CVector3f&);
void FindRegions(rstl::reserved_vector<CPFRegion, 4>&, const zeus::CVector3f&, u32);
void FindClosestRegion(const zeus::CVector3f&, u32, float);
u32 FindRegions(rstl::reserved_vector<CPFRegion, 4>& regions, const zeus::CVector3f& point,
u32 flags, u32 indexMask);
CPFRegion* FindClosestRegion(const zeus::CVector3f& point, u32 flags, u32 indexMask, float padding);
void FindClosestReachablePoint(rstl::reserved_vector<CPFRegion, 4>&, const zeus::CVector3f&, u32);
bool PathExists(const CPFRegion* r1, const CPFRegion* r2, u32 flags) const;
};

132
Runtime/World/CPathFindRegion.cpp
View File

@ -48,6 +48,128 @@ void CPFRegion::Fixup(CPFArea& area, u32& maxRegionNodes)
maxRegionNodes = x0_numNodes;
}
float CPFRegion::PointHeight(const zeus::CVector3f& point) const
{
return (point - x4_startNode->GetPos()).dot(x18_normal);
}
bool CPFRegion::FindClosestPointOnPolygon(const std::vector<zeus::CVector3f>& polyPoints,
const zeus::CVector3f& normal,
const zeus::CVector3f& point, bool excludePolyPoints)
{
bool found = false;
int i;
for (i=0 ; i<polyPoints.size() ; ++i)
{
const zeus::CVector3f& p0 = polyPoints[i];
const zeus::CVector3f& p1 = polyPoints[(i + 1) % polyPoints.size()];
if ((p1 - p0).cross(normal).dot(point - p0) < 0.f)
break;
}
if (i == polyPoints.size())
{
float distToPoly = (polyPoints.front() - point).dot(normal);
float distToPolySq = distToPoly * distToPoly;
if (distToPolySq < x4c_regionData->GetBestPointDistanceSquared())
{
found = true;
x4c_regionData->SetBestPointDistanceSquared(distToPolySq);
x4c_regionData->SetBestPoint(normal * distToPoly + point);
}
}
else
{
bool projected = false;
for (i=0 ; i<polyPoints.size() ; ++i)
{
const zeus::CVector3f& p0 = polyPoints[i];
const zeus::CVector3f& p1 = polyPoints[(i + 1) % polyPoints.size()];
zeus::CVector3f p0ToP1 = p1 - p0;
zeus::CVector3f p1ToPoint = point - p1;
zeus::CVector3f sum = p1ToPoint + p0ToP1;
if (p0ToP1.cross(normal).dot(p1ToPoint) < 0.f &&
p0ToP1.dot(p1ToPoint) <= 0.f &&
sum.dot(p0ToP1) >= 0.f)
{
projected = true;
p0ToP1.normalize();
sum -= p0ToP1.dot(sum) * p0ToP1;
float distSq = sum.magSquared();
if (distSq < x4c_regionData->GetBestPointDistanceSquared())
{
found = true;
x4c_regionData->SetBestPointDistanceSquared(distSq);
x4c_regionData->SetBestPoint(point - sum);
}
break;
}
}
if (!projected && !excludePolyPoints)
{
for (i=0 ; i<polyPoints.size() ; ++i)
{
const zeus::CVector3f& p0 = polyPoints[i];
float distSq = (point - p0).magSquared();
if (distSq < x4c_regionData->GetBestPointDistanceSquared())
{
found = true;
x4c_regionData->SetBestPointDistanceSquared(distSq);
x4c_regionData->SetBestPoint(p0);
}
}
}
}
return found;
}
bool CPFRegion::FindBestPoint(std::vector<zeus::CVector3f>& polyPoints, const zeus::CVector3f& point,
u32 flags, float paddingSq)
{
bool found = false;
bool isFlyer = (flags & 0x2) != 0;
x4c_regionData->SetBestPointDistanceSquared(paddingSq);
if (!isFlyer)
{
for (int i=0 ; i<x0_numNodes ; ++i)
{
CPFNode& node = x4_startNode[i];
CPFNode& nextNode = x4_startNode[(i + 1) % x0_numNodes];
polyPoints.clear();
polyPoints.push_back(node.GetPos());
polyPoints.push_back(node.GetPos());
polyPoints.back().z += x14_height;
polyPoints.push_back(nextNode.GetPos());
polyPoints.back().z += x14_height;
polyPoints.push_back(nextNode.GetPos());
found |= FindClosestPointOnPolygon(polyPoints, node.GetNormal(), point, true);
}
}
polyPoints.clear();
for (int i=0 ; i<x0_numNodes ; ++i)
{
CPFNode& node = x4_startNode[i];
polyPoints.push_back(node.GetPos());
}
found |= FindClosestPointOnPolygon(polyPoints, x18_normal, point, false);
if (!isFlyer)
{
polyPoints.clear();
for (int i=x0_numNodes-1 ; i>=0 ; --i)
{
CPFNode& node = x4_startNode[i];
polyPoints.push_back(node.GetPos());
polyPoints.back().z += x14_height;
}
found |= FindClosestPointOnPolygon(polyPoints, -x18_normal, point, false);
}
return found;
}
zeus::CVector3f CPFRegion::FitThroughLink2d(const zeus::CVector3f& p1, const CPFLink& link,
const zeus::CVector3f& p2, float f1) const
{
@ -113,4 +235,14 @@ zeus::CVector3f CPFRegion::FitThroughLink3d(const zeus::CVector3f& p1, const CPF
return {midPoint.x, midPoint.y, z};
}
bool CPFRegion::IsPointInsidePaddedAABox(const zeus::CVector3f& point, float padding) const
{
return point.x >= x34_aabb.min.x - padding &&
point.x <= x34_aabb.max.x + padding &&
point.y >= x34_aabb.min.y - padding &&
point.y <= x34_aabb.max.y + padding &&
point.z >= x34_aabb.min.z - padding &&
point.z <= x34_aabb.max.z + padding;
}
}

20
Runtime/World/CPathFindRegion.hpp
View File

@ -65,22 +65,22 @@ public:
const zeus::CVector3f& GetNormal() const { return x18_normal; }
u32 GetNumNodes() const { return x0_numNodes; }
const CPFNode* GetNode(u32 i) const { return x4_startNode + i; }
void PointHeight(const zeus::CVector3f&);
void FindClosestPointOnPolygon(const std::vector<zeus::CVector3f>&, const zeus::CVector3f&,
float PointHeight(const zeus::CVector3f& point) const;
bool FindClosestPointOnPolygon(const std::vector<zeus::CVector3f>&, const zeus::CVector3f&,
const zeus::CVector3f&, bool);
void FindBestPoint(std::vector<zeus::CVector3f>&, const zeus::CVector3f&, u32, float);
bool FindBestPoint(std::vector<zeus::CVector3f>& polyPoints, const zeus::CVector3f& point, u32 flags, float paddingSq);
void SetLinkTo(s32);
void DropToGround(zeus::CVector3f&) const;
void GetLinkMidPoint(const CPFLink&);
zeus::CVector3f FitThroughLink2d(const zeus::CVector3f&, const CPFLink&, const zeus::CVector3f&, float) const;
zeus::CVector3f FitThroughLink3d(const zeus::CVector3f&, const CPFLink&, float, const zeus::CVector3f&, float, float) const;
void IsPointInsidePaddedAABox(const zeus::CVector3f&, float) const;
bool IsPointInsidePaddedAABox(const zeus::CVector3f& point, float padding) const;
};
class CPFRegionData
{
float x0_ = 0.f;
zeus::CVector3f x4_;
float x0_bestPointDistSq = 0.f;
zeus::CVector3f x4_bestPoint;
s32 x10_cookie = -1;
zeus::CVector3f x14_;
s32 x20_ = 0;
@ -99,10 +99,10 @@ public:
void SetPathLink(s32 l) { x2c_pathLink = l; }
void GetParent() const;
void Setup(CPFRegion*, float, float);
void SetBestPoint(const zeus::CVector3f&);
void SetBestPointDistanceSquared(float);
float GetBestPointDistanceSquared() const;
zeus::CVector3f GetBestPoint() const;
void SetBestPoint(const zeus::CVector3f& bestPoint) { x4_bestPoint = bestPoint; }
const zeus::CVector3f& GetBestPoint() const { return x4_bestPoint; }
void SetBestPointDistanceSquared(float distSq) { x0_bestPointDistSq = distSq; }
float GetBestPointDistanceSquared() const { return x0_bestPointDistSq; }
void SetCookie(s32 c) { x10_cookie = c; }
s32 GetCookie() const { return x10_cookie; }
};

34
Runtime/World/CPathFindSearch.cpp
View File

@ -4,16 +4,46 @@ namespace urde
{
CPathFindSearch::CPathFindSearch(CPFArea* area, u32 flags, u32 w2, float f1, float f2)
: x0_area(area), xd0_f2(f2), xd4_f1(f1), xdc_flags(flags), xe0_w2(1u << w2)
: x0_area(area), xd0_f2(f2), xd4_f1(f1), xdc_flags(flags), xe0_indexMask(1u << w2)
{
}
void CPathFindSearch::Search(const zeus::CVector3f& p1, const zeus::CVector3f& p2)
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 localP2 = x0_area->x188_transform.transposeRotate(p2 - x0_area->x188_transform.origin);
if (!(xdc_flags & 0x2) && !(xdc_flags & 0x4))
{
localP2.z += 0.3f;
localP1.z += 0.3f;
}
rstl::reserved_vector<CPFRegion, 4> regions;
if (x0_area->FindRegions(regions, localP1, xdc_flags, xe0_indexMask) == 0)
{
x0_area->FindClosestRegion(localP1, xdc_flags, xe0_indexMask, xd8_);
}
// TODO: Finish
return EResult::Zero;
}
}

15
Runtime/World/CPathFindSearch.hpp
View File

@ -9,17 +9,28 @@ namespace urde
class CPathFindSearch
{
public:
enum class EResult
{
Zero,
One,
Two,
Three,
Four
};
private:
CPFArea* x0_area;
rstl::reserved_vector<zeus::CVector3f, 16> x4_;
u32 xc8_ = 0;
EResult xcc_result;
float xd0_f2;
float xd4_f1;
float xd8_ = 10.f;
u32 xdc_flags; // 0x2: flyer, 0x4: path-always-exists
u32 xe0_w2;
u32 xe0_indexMask;
public:
CPathFindSearch(CPFArea* area, u32 flags, u32 w2, float f1, float f2);
void Search(const zeus::CVector3f& p1, const zeus::CVector3f& p2);
EResult Search(const zeus::CVector3f& p1, const zeus::CVector3f& p2);
};
}