Work on pathfinding

Runtime/World/CPathFindArea.cpp

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

Runtime/World/CPathFindArea.hpp

@@ -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);
+    void GetRegionListList(rstl::reserved_vector<rstl::prereserved_vector<CPFRegion*>*, 32>& listOut,
-    bool IsPointInPaddedAABox(const zeus::CVector3f&, float);
+                           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);
+    u32 FindRegions(rstl::reserved_vector<CPFRegion, 4>& regions, const zeus::CVector3f& point,
-    void FindClosestRegion(const zeus::CVector3f&, u32, float);
+                    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;
 };

Runtime/World/CPathFindRegion.cpp

@@ -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;
+}
+
 }

Runtime/World/CPathFindRegion.hpp

@@ -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&);
+    float PointHeight(const zeus::CVector3f& point) const;
-    void FindClosestPointOnPolygon(const std::vector<zeus::CVector3f>&, const zeus::CVector3f&,
+    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;
+    float x0_bestPointDistSq = 0.f;
-    zeus::CVector3f x4_;
+    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 SetBestPoint(const zeus::CVector3f& bestPoint) { x4_bestPoint = bestPoint; }
-    void SetBestPointDistanceSquared(float);
+    const zeus::CVector3f& GetBestPoint() const { return x4_bestPoint; }
-    float GetBestPointDistanceSquared() const;
+    void SetBestPointDistanceSquared(float distSq) { x0_bestPointDistSq = distSq; }
-    zeus::CVector3f GetBestPoint() const;
+    float GetBestPointDistanceSquared() const { return x0_bestPointDistSq; }
     void SetCookie(s32 c) { x10_cookie = c; }
     s32 GetCookie() const { return x10_cookie; }
 };

Runtime/World/CPathFindSearch.cpp

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

Runtime/World/CPathFindSearch.hpp

@@ -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);
 };
 
 }