diff --git a/Runtime/World/CPathFindArea.cpp b/Runtime/World/CPathFindArea.cpp
index fe3df8c87..9d071d011 100644
--- a/Runtime/World/CPathFindArea.cpp
+++ b/Runtime/World/CPathFindArea.cpp
@@ -48,6 +48,13 @@ int CPFAreaOctree::GetChildIndex(const zeus::CVector3f& point) const
     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,
                                       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;
 }
 
-CPFOpenList::CPFOpenList()
-{
-
-}
-
 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)
@@ -143,16 +188,42 @@ CPFArea::CPFArea(std::unique_ptr<u8[]>&& buf, u32 len)
         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)
 {
     rstl::reserved_vector<rstl::prereserved_vector<CPFRegion*>*, 32> regionListList;
     x158_octree.back().GetRegionListList(regionListList, point, padding);
     bool isFlyer = (flags & 0x2) != 0;
+    CPFRegion* ret = nullptr;
     for (rstl::prereserved_vector<CPFRegion*>* list : regionListList)
     {
         for (CPFRegion* region : *list)
         {
-            if (region->Data()->GetCookie() != x34_curRegionCookie)
+            if (region->Data()->GetCookie() != x34_curCookie)
             {
                 if (region->GetFlags() & 0xff & flags &&
                     (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))
                     {
-                        // 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
@@ -193,9 +289,9 @@ bool CPFArea::PathExists(const CPFRegion* r1, const CPFRegion* r2, u32 flags) co
 
     auto d = std::div(bit, 32);
     if ((flags & 0x2) != 0)
-        return ((x170_connectionsFlyers[d.quot] >> d.rem) & 0x1) == 0x1;
+        return ((x170_connectionsFlyers[d.quot] >> d.rem) & 0x1) != 0;
     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,
diff --git a/Runtime/World/CPathFindArea.hpp b/Runtime/World/CPathFindArea.hpp
index 062600116..ccaa16614 100644
--- a/Runtime/World/CPathFindArea.hpp
+++ b/Runtime/World/CPathFindArea.hpp
@@ -14,12 +14,12 @@ class CObjectReference;
 
 class CPFBitSet
 {
+    u32 x0_bitmap[16] = {};
 public:
-    CPFBitSet() = default;
-    void Clear();
-    void Add(s32);
-    bool Test(s32);
-    void Rmv(s32);
+    void Clear() { for (u32& w : x0_bitmap) w = 0; }
+    void Add(s32 i) { x0_bitmap[i / 32] |= (1 << (i % 32)); }
+    bool Test(s32 i) const { return (x0_bitmap[i / 32] & (1 << (i % 32))) != 0; }
+    void Rmv(s32 i) { x0_bitmap[i / 32] &= ~(1 << (i % 32)); }
 };
 
 class CPFAreaOctree
@@ -33,7 +33,7 @@ public:
     CPFAreaOctree(CMemoryInStream& in);
     void Fixup(CPFArea& area);
     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,
                            const zeus::CVector3f& point, float padding);
     bool IsPointInsidePaddedAABox(const zeus::CVector3f& point, float padding) const;
@@ -43,33 +43,16 @@ public:
 class CPFOpenList
 {
     friend class CPFArea;
-    u32 x0_ = 0;
-    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;
+    CPFBitSet x0_bitSet;
     CPFRegion x40_region;
     CPFRegionData x90_regionData;
 
 public:
-    CPFOpenList();
-
     void Clear();
-    void Push(CPFRegion*);
-    void Pop();
-    void Pop(CPFRegion*);
-    void Test(CPFRegion*);
+    void Push(CPFRegion* reg);
+    CPFRegion* Pop();
+    void Pop(CPFRegion* reg);
+    bool Test(CPFRegion* reg);
 };
 
 class CPFArea
@@ -79,30 +62,15 @@ class CPFArea
     friend class CPathFindSearch;
 
     float x0_ = FLT_MAX;
-    zeus::CVector3f x4_;
+    zeus::CVector3f x4_closestPoint;
     std::vector<zeus::CVector3f> x10_tmpPolyPoints;
-    u32 x20_ = 0;
-    zeus::CVector3f x24_;
-    bool x30_ = false;
-    s32 x34_curRegionCookie = 0;
-    u32 x38_ = 0;
-    u32 x3c_ = 0;
-    u32 x40_ = 0;
-    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_;
+    rstl::prereserved_vector<CPFRegion*>* x20_cachedRegionList = nullptr;
+    zeus::CVector3f x24_cachedRegionListPoint;
+    bool x30_hasCachedRegionList = false;
+    s32 x34_curCookie = 0;
+    CPFBitSet x38_closedSet;
+    CPFOpenList x78_openList;
+    //u32 x138_;
     //std::unique_ptr<u8[]> x13c_data;
     /* Used to be prereserved_vectors backed by x13c_data
      * This has been changed to meet storage requirements of
@@ -122,19 +90,20 @@ public:
     void SetTransform(const zeus::CTransform& xf) { x188_transform = xf; }
     const zeus::CTransform& GetTransform() const { return x188_transform; }
     const CPFRegion& GetRegion(s32 i) const { return x150_regions[i]; }
-    void GetClosestPoint() const;
-    void OpenList();
-    void ClosedSet();
+    const zeus::CVector3f& GetClosestPoint() const { return x4_closestPoint; }
+    CPFOpenList& OpenList() { return x78_openList; }
+    CPFBitSet& ClosedSet() { return x38_closedSet; }
     const CPFRegionData& GetRegionData(s32 i) const { return x178_regionDatas[i]; }
     const CPFLink& GetLink(s32 i) const { return x148_links[i]; }
     const CPFNode& GetNode(s32 i) const { return x140_nodes[i]; }
     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&);
-    u32 FindRegions(rstl::reserved_vector<CPFRegion, 4>& regions, const zeus::CVector3f& point,
+    rstl::prereserved_vector<CPFRegion*>* GetOctreeRegionList(const zeus::CVector3f& point);
+    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);
+    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;
 };
 
diff --git a/Runtime/World/CPathFindRegion.cpp b/Runtime/World/CPathFindRegion.cpp
index 37fa8bca2..92d7758df 100644
--- a/Runtime/World/CPathFindRegion.cpp
+++ b/Runtime/World/CPathFindRegion.cpp
@@ -33,6 +33,11 @@ CPFRegion::CPFRegion(CMemoryInStream& in)
     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)
 {
     if (x0_numNodes)
@@ -48,6 +53,24 @@ void CPFRegion::Fixup(CPFArea& area, u32& maxRegionNodes)
         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
 {
     return (point - x4_startNode->GetPos()).dot(x18_normal);
@@ -170,14 +193,38 @@ bool CPFRegion::FindBestPoint(std::vector<zeus::CVector3f>& polyPoints, const ze
     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,
-                                            const zeus::CVector3f& p2, float f1) const
+                                            const zeus::CVector3f& p2, float chRadius) const
 {
     CPFNode& node = x4_startNode[link.GetNode()];
     CPFNode& nextNode = x4_startNode[(link.GetNode() + 1) % x0_numNodes];
     zeus::CVector3f nodeDelta = nextNode.GetPos() - node.GetPos();
     float t = 0.5f;
-    if (f1 < 0.5f * link.Get2dWidth())
+    if (chRadius < 0.5f * link.Get2dWidth())
     {
         zeus::CVector2f delta2d(nodeDelta.x, nodeDelta.y);
         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 f3 = f27 + f26;
         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();
 }
 
 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& 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 f23 = f25 + f24;
 # if 0
-    if (f2 < 0.5f * link.Get2dWidth())
+    if (chRadius < 0.5f * link.Get2dWidth())
     {
         zeus::CVector2f delta2d(nodeDelta.x, nodeDelta.y);
         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));
         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
     zeus::CVector3f midPoint = nodeDelta * 0.5f + node.GetPos();
     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);
         if (f23 > FLT_EPSILON)
             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
     {
diff --git a/Runtime/World/CPathFindRegion.hpp b/Runtime/World/CPathFindRegion.hpp
index c97a5449a..aad06166b 100644
--- a/Runtime/World/CPathFindRegion.hpp
+++ b/Runtime/World/CPathFindRegion.hpp
@@ -54,14 +54,14 @@ public:
     CPFRegionData* Data() const { return x4c_regionData; }
     u32 GetIndex() const { return x24_regionIdx; }
     float GetHeight() const { return x14_height; }
-    void GetPathLink() const {}
+    const CPFLink* GetPathLink() const;
     u32 GetNumLinks() const { return x8_numLinks; }
     u32 GetFlags() const { return x10_flags; }
     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; }
     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; }
     u32 GetNumNodes() const { return x0_numNodes; }
     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&,
                                    const zeus::CVector3f&, bool);
     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 SetLinkTo(s32 idx);
+    void DropToGround(zeus::CVector3f& point) const;
+    zeus::CVector3f GetLinkMidPoint(const CPFLink& link) const;
+    zeus::CVector3f FitThroughLink2d(const zeus::CVector3f& p1, const CPFLink& link,
+                                     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;
 };
 
@@ -82,8 +85,10 @@ class CPFRegionData
     float x0_bestPointDistSq = 0.f;
     zeus::CVector3f x4_bestPoint;
     s32 x10_cookie = -1;
-    zeus::CVector3f x14_;
-    s32 x20_ = 0;
+    float x14_cost = 0.f;
+    float x18_g = 0.f;
+    float x1c_h = 0.f;
+    CPFRegion* x20_parent = nullptr;
     CPFRegion* x24_openLess = nullptr;
     CPFRegion* x28_openMore = nullptr;
     s32 x2c_pathLink = 0;
@@ -94,17 +99,31 @@ public:
     void SetOpenMore(CPFRegion* r) { x28_openMore = r; }
     CPFRegion* GetOpenLess() const { return x24_openLess; }
     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; }
     void SetPathLink(s32 l) { x2c_pathLink = l; }
-    void GetParent() const;
-    void Setup(CPFRegion*, float, float);
+    CPFRegion* GetParent() const { return x20_parent; }
     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; }
+
+    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;
+    }
 };
 }
 
diff --git a/Runtime/World/CPathFindSearch.cpp b/Runtime/World/CPathFindSearch.cpp
index b65e328bd..a35c07011 100644
--- a/Runtime/World/CPathFindSearch.cpp
+++ b/Runtime/World/CPathFindSearch.cpp
@@ -3,47 +3,343 @@
 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_indexMask(1u << w2)
+CPathFindSearch::CPathFindSearch(CPFArea* area, u32 flags, u32 index, float chRadius, float chHeight)
+: 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;
 
-}
-
-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;
-    }
-
+    /* 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> regions;
+    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_);
+        /* 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
-    return EResult::Zero;
+    /* Override dest point to be reachable */
+    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;
 }
 
 }
diff --git a/Runtime/World/CPathFindSearch.hpp b/Runtime/World/CPathFindSearch.hpp
index 84d72f71e..599c6b95a 100644
--- a/Runtime/World/CPathFindSearch.hpp
+++ b/Runtime/World/CPathFindSearch.hpp
@@ -12,25 +12,28 @@ class CPathFindSearch
 public:
     enum class EResult
     {
-        Zero,
-        One,
-        Two,
-        Three,
-        Four
+        Success,
+        InvalidArea,
+        NoSourcePoint,
+        NoDestPoint,
+        NoPath
     };
 private:
     CPFArea* x0_area;
-    rstl::reserved_vector<zeus::CVector3f, 16> x4_;
+    rstl::reserved_vector<zeus::CVector3f, 16> x4_waypoints;
     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
+    float xd0_chHeight;
+    float xd4_chRadius;
+    float xd8_padding = 10.f;
+    u32 xdc_flags; // 0x2: flyer, 0x4: path-always-exists (swimmers)
     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:
-    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 FindClosestReachablePoint(const zeus::CVector3f& p1, zeus::CVector3f& p2) const;
 };
 
 }