diff --git a/Runtime/World/CPathFindRegion.cpp b/Runtime/World/CPathFindRegion.cpp index f6ec10d2c..56f00621c 100644 --- a/Runtime/World/CPathFindRegion.cpp +++ b/Runtime/World/CPathFindRegion.cpp @@ -33,33 +33,43 @@ CPFRegion::CPFRegion(CMemoryInStream& in) { const CPFLink* CPFRegion::GetPathLink() const { return &xc_startLink[x4c_regionData->GetPathLink()]; } void CPFRegion::Fixup(CPFArea& area, u32& maxRegionNodes) { - if (x0_numNodes) + if (x0_numNodes != 0) { x4_startNode = &area.x140_nodes[reinterpret_cast(x4_startNode)]; - else + } else { x4_startNode = nullptr; - if (x8_numLinks) + } + + if (x8_numLinks != 0) { xc_startLink = &area.x148_links[reinterpret_cast(xc_startLink)]; - else + } else { xc_startLink = nullptr; + } + x4c_regionData = &area.x178_regionDatas[x24_regionIdx]; - if (x0_numNodes > maxRegionNodes) + if (x0_numNodes > maxRegionNodes) { maxRegionNodes = x0_numNodes; + } } bool CPFRegion::IsPointInside(const zeus::CVector3f& point) const { - if (!x34_aabb.pointInside(point)) + if (!x34_aabb.pointInside(point)) { return false; + } + u32 i; for (i = 0; i < x0_numNodes; ++i) { - CPFNode& node = x4_startNode[i]; - if ((point - node.GetPos()).dot(node.GetNormal()) < 0.f) + const CPFNode& node = x4_startNode[i]; + if ((point - node.GetPos()).dot(node.GetNormal()) < 0.f) { break; + } } - if (i != x0_numNodes) + + if (i != x0_numNodes) { return false; - zeus::CVector3f nodeToPoint = point - x4_startNode->GetPos(); - return nodeToPoint.dot(x18_normal) >= 0.f && - (nodeToPoint - x14_height * zeus::skUp).dot(x18_normal) <= 0.f; + } + + const zeus::CVector3f nodeToPoint = point - x4_startNode->GetPos(); + return nodeToPoint.dot(x18_normal) >= 0.f && (nodeToPoint - x14_height * zeus::skUp).dot(x18_normal) <= 0.f; } float CPFRegion::PointHeight(const zeus::CVector3f& point) const { @@ -73,13 +83,14 @@ bool CPFRegion::FindClosestPointOnPolygon(const std::vector& po 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) + 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; + const float distToPoly = (polyPoints.front() - point).dot(normal); + const float distToPolySq = distToPoly * distToPoly; if (distToPolySq < x4c_regionData->GetBestPointDistanceSquared()) { found = true; x4c_regionData->SetBestPointDistanceSquared(distToPolySq); @@ -93,11 +104,12 @@ bool CPFRegion::FindClosestPointOnPolygon(const std::vector& po 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(); + const float distSq = sum.magSquared(); if (distSq < x4c_regionData->GetBestPointDistanceSquared()) { found = true; x4c_regionData->SetBestPointDistanceSquared(distSq); @@ -110,7 +122,7 @@ bool CPFRegion::FindClosestPointOnPolygon(const std::vector& po if (!projected && !excludePolyPoints) { for (i = 0; i < polyPoints.size(); ++i) { const zeus::CVector3f& p0 = polyPoints[i]; - float distSq = (point - p0).magSquared(); + const float distSq = (point - p0).magSquared(); if (distSq < x4c_regionData->GetBestPointDistanceSquared()) { found = true; x4c_regionData->SetBestPointDistanceSquared(distSq); @@ -125,12 +137,14 @@ bool CPFRegion::FindClosestPointOnPolygon(const std::vector& po bool CPFRegion::FindBestPoint(std::vector& polyPoints, const zeus::CVector3f& point, u32 flags, float paddingSq) const { bool found = false; - bool isFlyer = (flags & 0x2) != 0; + const bool isFlyer = (flags & 0x2) != 0; + x4c_regionData->SetBestPointDistanceSquared(paddingSq); + if (!isFlyer) { for (u32 i = 0; i < x0_numNodes; ++i) { - CPFNode& node = x4_startNode[i]; - CPFNode& nextNode = x4_startNode[(i + 1) % x0_numNodes]; + const CPFNode& node = x4_startNode[i]; + const CPFNode& nextNode = x4_startNode[(i + 1) % x0_numNodes]; polyPoints.clear(); polyPoints.push_back(node.GetPos()); polyPoints.push_back(node.GetPos()); @@ -144,7 +158,7 @@ bool CPFRegion::FindBestPoint(std::vector& polyPoints, const ze polyPoints.clear(); for (u32 i = 0; i < x0_numNodes; ++i) { - CPFNode& node = x4_startNode[i]; + const CPFNode& node = x4_startNode[i]; polyPoints.push_back(node.GetPos()); } found |= FindClosestPointOnPolygon(polyPoints, x18_normal, point, false); @@ -152,7 +166,7 @@ bool CPFRegion::FindBestPoint(std::vector& polyPoints, const ze if (!isFlyer) { polyPoints.clear(); for (int i = int(x0_numNodes) - 1; i >= 0; --i) { - CPFNode& node = x4_startNode[i]; + const CPFNode& node = x4_startNode[i]; polyPoints.push_back(node.GetPos()); polyPoints.back().z() += x14_height; } @@ -187,55 +201,59 @@ zeus::CVector3f CPFRegion::GetLinkMidPoint(const CPFLink& link) const { zeus::CVector3f CPFRegion::FitThroughLink2d(const zeus::CVector3f& p1, const CPFLink& link, 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(); + const CPFNode& node = x4_startNode[link.GetNode()]; + const CPFNode& nextNode = x4_startNode[(link.GetNode() + 1) % x0_numNodes]; + const zeus::CVector3f nodeDelta = nextNode.GetPos() - node.GetPos(); float t = 0.5f; + if (chRadius < 0.5f * link.Get2dWidth()) { zeus::CVector2f delta2d = nodeDelta.toVec2f(); delta2d *= link.GetOO2dWidth(); - zeus::CVector3f nodeToP1 = p1 - node.GetPos(); - float f27 = nodeToP1.dot(node.GetNormal()); - float f31 = delta2d.dot(nodeToP1.toVec2f()); - zeus::CVector3f nodeToP2 = p2 - node.GetPos(); - float f26 = -nodeToP2.dot(node.GetNormal()); - float f1b = delta2d.dot(nodeToP2.toVec2f()); - float f3 = f27 + f26; - if (f3 > FLT_EPSILON) + const zeus::CVector3f nodeToP1 = p1 - node.GetPos(); + const float f27 = nodeToP1.dot(node.GetNormal()); + const float f31 = delta2d.dot(nodeToP1.toVec2f()); + const zeus::CVector3f nodeToP2 = p2 - node.GetPos(); + const float f26 = -nodeToP2.dot(node.GetNormal()); + const float f1b = delta2d.dot(nodeToP2.toVec2f()); + const float f3 = f27 + f26; + if (f3 > FLT_EPSILON) { 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 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]; - zeus::CVector3f nodeDelta = nextNode.GetPos() - node.GetPos(); - float f25 = (p1 - node.GetPos()).dot(node.GetNormal()); - float f24 = (node.GetPos() - p2).dot(node.GetNormal()); - float f23 = f25 + f24; + const CPFNode& node = x4_startNode[link.GetNode()]; + const CPFNode& nextNode = x4_startNode[(link.GetNode() + 1) % x0_numNodes]; + const zeus::CVector3f nodeDelta = nextNode.GetPos() - node.GetPos(); + const float f25 = (p1 - node.GetPos()).dot(node.GetNormal()); + const float f24 = (node.GetPos() - p2).dot(node.GetNormal()); + const float f23 = f25 + f24; + #if 0 if (chRadius < 0.5f * link.Get2dWidth()) { zeus::CVector2f delta2d(nodeDelta.x, nodeDelta.y); delta2d *= link.GetOO2dWidth(); - zeus::CVector3f nodeToP1 = p1 - node.GetPos(); - float f29 = delta2d.dot(zeus::CVector2f(nodeToP1.y, nodeToP1.y)); - zeus::CVector3f nodeToP2 = p2 - node.GetPos(); - float f1b = delta2d.dot(zeus::CVector2f(nodeToP2.y, nodeToP2.y)); + const zeus::CVector3f nodeToP1 = p1 - node.GetPos(); + const float f29 = delta2d.dot(zeus::CVector2f(nodeToP1.y, nodeToP1.y)); + const zeus::CVector3f nodeToP2 = p2 - node.GetPos(); + const float f1b = delta2d.dot(zeus::CVector2f(nodeToP2.y, nodeToP2.y)); if (f23 > FLT_EPSILON) { - zeus::clamp(chRadius, 1.f / f23 * f24 * f29 + f25 * f1b, link.Get2dWidth() - chRadius) * - 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(); + + const zeus::CVector3f midPoint = nodeDelta * 0.5f + node.GetPos(); float z; if (chHalfHeight < 0.5f * regionHeight) { - float minZ = chHalfHeight + midPoint.z(); + const float minZ = chHalfHeight + midPoint.z(); z = 0.5f * (p1.z() + p2.z()); - if (f23 > FLT_EPSILON) + if (f23 > FLT_EPSILON) { z = (f24 * p1.z() + f25 * p2.z()) / f23; + } z = zeus::clamp(minZ, z, regionHeight + midPoint.z() - chHalfHeight); } else { z = (p1.z() + p2.z()) * 0.5f;