mirror of
https://github.com/AxioDL/metaforce.git
synced 2025-12-09 06:27:43 +00:00
New code style refactor
This commit is contained in:
Jack Andersen
@@ -1,397 +1,351 @@
|
||||
#include "CPathFindSearch.hpp"
|
||||
|
||||
namespace urde
|
||||
{
|
||||
namespace urde {
|
||||
|
||||
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)
|
||||
{}
|
||||
: 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::FindClosestReachablePoint(const zeus::CVector3f& p1,
|
||||
zeus::CVector3f& p2) const {
|
||||
if (!x0_area)
|
||||
return EResult::InvalidArea;
|
||||
|
||||
/* 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);
|
||||
/* 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;
|
||||
}
|
||||
/* 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;
|
||||
if (x0_area->FindRegions(regions, 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)
|
||||
return EResult::NoSourcePoint;
|
||||
rstl::reserved_vector<CPFRegion*, 4> regions;
|
||||
if (x0_area->FindRegions(regions, 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)
|
||||
return EResult::NoSourcePoint;
|
||||
|
||||
regions.push_back(region);
|
||||
}
|
||||
regions.push_back(region);
|
||||
}
|
||||
|
||||
/* 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;
|
||||
/* 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;
|
||||
return EResult::Success;
|
||||
}
|
||||
|
||||
CPathFindSearch::EResult CPathFindSearch::PathExists(const zeus::CVector3f& p1, const zeus::CVector3f& p2) const
|
||||
{
|
||||
if (!x0_area)
|
||||
return EResult::InvalidArea;
|
||||
CPathFindSearch::EResult CPathFindSearch::PathExists(const zeus::CVector3f& p1, const zeus::CVector3f& p2) const {
|
||||
if (!x0_area)
|
||||
return EResult::InvalidArea;
|
||||
|
||||
/* 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);
|
||||
/* 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;
|
||||
}
|
||||
/* 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;
|
||||
if (x0_area->FindRegions(regions1, localP1, xdc_flags, xe0_indexMask) == 0)
|
||||
return EResult::NoSourcePoint;
|
||||
rstl::reserved_vector<CPFRegion*, 4> regions1;
|
||||
if (x0_area->FindRegions(regions1, localP1, xdc_flags, xe0_indexMask) == 0)
|
||||
return EResult::NoSourcePoint;
|
||||
|
||||
rstl::reserved_vector<CPFRegion*, 4> regions2;
|
||||
if (x0_area->FindRegions(regions2, localP2, xdc_flags, xe0_indexMask) == 0)
|
||||
return EResult::NoDestPoint;
|
||||
rstl::reserved_vector<CPFRegion*, 4> regions2;
|
||||
if (x0_area->FindRegions(regions2, localP2, xdc_flags, xe0_indexMask) == 0)
|
||||
return EResult::NoDestPoint;
|
||||
|
||||
for (CPFRegion* reg1 : regions1)
|
||||
for (CPFRegion* reg2 : regions2)
|
||||
if (reg1 == reg2 || x0_area->PathExists(reg1, reg2, xdc_flags))
|
||||
return EResult::Success;
|
||||
for (CPFRegion* reg1 : regions1)
|
||||
for (CPFRegion* reg2 : regions2)
|
||||
if (reg1 == reg2 || x0_area->PathExists(reg1, reg2, xdc_flags))
|
||||
return EResult::Success;
|
||||
|
||||
return EResult::NoPath;
|
||||
return EResult::NoPath;
|
||||
}
|
||||
|
||||
CPathFindSearch::EResult CPathFindSearch::OnPath(const zeus::CVector3f& p1) const
|
||||
{
|
||||
if (!x0_area)
|
||||
return EResult::InvalidArea;
|
||||
CPathFindSearch::EResult CPathFindSearch::OnPath(const zeus::CVector3f& p1) const {
|
||||
if (!x0_area)
|
||||
return EResult::InvalidArea;
|
||||
|
||||
/* Work in local PFArea coordinates */
|
||||
zeus::CVector3f localP1 = x0_area->x188_transform.transposeRotate(p1 - x0_area->x188_transform.origin);
|
||||
/* Work in local PFArea coordinates */
|
||||
zeus::CVector3f localP1 = x0_area->x188_transform.transposeRotate(p1 - x0_area->x188_transform.origin);
|
||||
|
||||
/* Raise a bit above ground for step-up resolution */
|
||||
if (!(xdc_flags & 0x2) && !(xdc_flags & 0x4))
|
||||
localP1.z() += 0.3f;
|
||||
/* Raise a bit above ground for step-up resolution */
|
||||
if (!(xdc_flags & 0x2) && !(xdc_flags & 0x4))
|
||||
localP1.z() += 0.3f;
|
||||
|
||||
rstl::reserved_vector<CPFRegion*, 4> regions1;
|
||||
if (x0_area->FindRegions(regions1, localP1, xdc_flags, xe0_indexMask) == 0)
|
||||
return EResult::NoSourcePoint;
|
||||
rstl::reserved_vector<CPFRegion*, 4> regions1;
|
||||
if (x0_area->FindRegions(regions1, localP1, xdc_flags, xe0_indexMask) == 0)
|
||||
return EResult::NoSourcePoint;
|
||||
|
||||
return EResult::Success;
|
||||
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_curWaypoint = 0;
|
||||
CPathFindSearch::EResult CPathFindSearch::Search(const zeus::CVector3f& p1, const zeus::CVector3f& p2) {
|
||||
u32 firstPoint = 0;
|
||||
u32 flyToOutsidePoint = 0;
|
||||
x4_waypoints.clear();
|
||||
xc8_curWaypoint = 0;
|
||||
|
||||
if (!x0_area || x0_area->x150_regions.size() > 512)
|
||||
{
|
||||
xcc_result = EResult::InvalidArea;
|
||||
return xcc_result;
|
||||
}
|
||||
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());
|
||||
reg = reg->Data()->GetParent();
|
||||
++lastPoint;
|
||||
} while (reg != 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! */
|
||||
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());
|
||||
reg = reg->Data()->GetParent();
|
||||
++lastPoint;
|
||||
} while (reg != 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();
|
||||
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);
|
||||
}
|
||||
/* 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();
|
||||
/* 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);
|
||||
}
|
||||
/* 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;
|
||||
/* 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);
|
||||
}
|
||||
/* 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);
|
||||
}
|
||||
}
|
||||
/* Make next potential candidate */
|
||||
x0_area->OpenList().Push(linkReg);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* Restore source centroids */
|
||||
auto p1It = centroidBackup1.begin();
|
||||
for (CPFRegion* r : regs1)
|
||||
r->SetCentroid(*p1It++);
|
||||
/* 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++);
|
||||
/* 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);
|
||||
}
|
||||
/* 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;
|
||||
return reg != nullptr;
|
||||
}
|
||||
|
||||
}
|
||||
} // namespace urde
|
||||
|
||||
Reference in New Issue
Block a user