489 lines
20 KiB
C++
489 lines
20 KiB
C++
// Copyright Epic Games, Inc. All Rights Reserved.
|
|
|
|
#include "MassZoneGraphNavigationFragments.h"
|
|
#include "ZoneGraphTypes.h"
|
|
#include "ZoneGraphQuery.h"
|
|
|
|
|
|
namespace UE::MassNavigation::ZoneGraphPath
|
|
{
|
|
struct FCachedLaneSegmentIterator
|
|
{
|
|
FCachedLaneSegmentIterator(const FMassZoneGraphCachedLaneFragment& InCachedLane, const float DistanceAlongPath, const bool bInMoveReverse)
|
|
: CachedLane(InCachedLane)
|
|
, SegmentInc(bInMoveReverse ? -1 : 1)
|
|
, bMoveReverse(bInMoveReverse)
|
|
{
|
|
check(CachedLane.NumPoints >= 2);
|
|
CurrentSegment = CachedLane.FindSegmentIndexAtDistance(DistanceAlongPath);
|
|
LastSegment = bInMoveReverse ? 0 : ((int32)CachedLane.NumPoints - 2);
|
|
}
|
|
|
|
bool HasReachedDistance(const float Distance) const
|
|
{
|
|
if (CurrentSegment == LastSegment)
|
|
{
|
|
return true;
|
|
}
|
|
|
|
if (bMoveReverse)
|
|
{
|
|
const float SegStartDistance = CachedLane.LanePointProgressions[CurrentSegment].Get();
|
|
if (Distance > SegStartDistance)
|
|
{
|
|
return true;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
const float SegEndDistance = CachedLane.LanePointProgressions[CurrentSegment + 1].Get();
|
|
if (Distance < SegEndDistance)
|
|
{
|
|
return true;
|
|
}
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
void Next()
|
|
{
|
|
if (CurrentSegment != LastSegment)
|
|
{
|
|
CurrentSegment += SegmentInc;
|
|
}
|
|
}
|
|
|
|
const FMassZoneGraphCachedLaneFragment& CachedLane;
|
|
int32 CurrentSegment = 0;
|
|
int32 LastSegment = 0;
|
|
int32 SegmentInc = 0;
|
|
bool bMoveReverse = false;
|
|
};
|
|
|
|
} // UE::MassMovement::ZoneGraphPath
|
|
|
|
void FMassZoneGraphCachedLaneFragment::CacheLaneData(const FZoneGraphStorage& ZoneGraphStorage, const FZoneGraphLaneHandle CurrentLaneHandle,
|
|
const float CurrentDistanceAlongLane, const float TargetDistanceAlongLane, const float InflateDistance)
|
|
{
|
|
const FZoneLaneData& Lane = ZoneGraphStorage.Lanes[CurrentLaneHandle.Index];
|
|
|
|
const float StartDistance = FMath::Min(CurrentDistanceAlongLane, TargetDistanceAlongLane);
|
|
const float EndDistance = FMath::Max(CurrentDistanceAlongLane, TargetDistanceAlongLane);
|
|
const float CurrentLaneLength = ZoneGraphStorage.LanePointProgressions[Lane.PointsEnd - 1];
|
|
|
|
// If cached data contains the request part of the lane, early out.
|
|
const float InflatedStartDistance = FMath::Max(0.0f, StartDistance - InflateDistance);
|
|
const float InflatedEndDistance = FMath::Min(EndDistance + InflateDistance, CurrentLaneLength);
|
|
if (LaneHandle == CurrentLaneHandle
|
|
&& NumPoints > 0
|
|
&& InflatedStartDistance >= LanePointProgressions[0].Get()
|
|
&& InflatedEndDistance <= LanePointProgressions[NumPoints - 1].Get())
|
|
{
|
|
return;
|
|
}
|
|
|
|
Reset();
|
|
CacheID++;
|
|
|
|
LaneHandle = CurrentLaneHandle;
|
|
LaneWidth = FMassInt16Real(Lane.Width);
|
|
LaneLength = CurrentLaneLength;
|
|
|
|
const int32 LaneNumPoints = Lane.PointsEnd - Lane.PointsBegin;
|
|
if (LaneNumPoints <= (int32)MaxPoints)
|
|
{
|
|
// If we can fit all the lane's points, just do a copy.
|
|
NumPoints = (uint8)LaneNumPoints;
|
|
for (int32 Index = 0; Index < (int32)NumPoints; Index++)
|
|
{
|
|
LanePoints[Index] = ZoneGraphStorage.LanePoints[Lane.PointsBegin + Index];
|
|
LaneTangentVectors[Index] = FMassSnorm8Vector2D(FVector2D(ZoneGraphStorage.LaneTangentVectors[Lane.PointsBegin + Index]));
|
|
LanePointProgressions[Index] = FMassInt16Real10(ZoneGraphStorage.LanePointProgressions[Lane.PointsBegin + Index]);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// Find the segment of the lane that is important and copy that.
|
|
int32 StartSegmentIndex = 0;
|
|
int32 EndSegmentIndex = 0;
|
|
UE::ZoneGraph::Query::CalculateLaneSegmentIndexAtDistance(ZoneGraphStorage, CurrentLaneHandle, StartDistance, StartSegmentIndex);
|
|
UE::ZoneGraph::Query::CalculateLaneSegmentIndexAtDistance(ZoneGraphStorage, CurrentLaneHandle, EndDistance, EndSegmentIndex);
|
|
|
|
// Expand if close to start of a segment start.
|
|
if ((StartSegmentIndex - 1) >= Lane.PointsBegin && (StartDistance - InflateDistance) < ZoneGraphStorage.LanePointProgressions[StartSegmentIndex])
|
|
{
|
|
StartSegmentIndex--;
|
|
}
|
|
// Expand if close to end segment end.
|
|
if ((EndSegmentIndex + 1) < (Lane.PointsEnd - 2) && (EndDistance + InflateDistance) > ZoneGraphStorage.LanePointProgressions[EndSegmentIndex + 1])
|
|
{
|
|
EndSegmentIndex++;
|
|
}
|
|
|
|
NumPoints = (uint8)FMath::Min((EndSegmentIndex - StartSegmentIndex) + 2, (int32)MaxPoints);
|
|
|
|
for (int32 Index = 0; Index < (int32)NumPoints; Index++)
|
|
{
|
|
check((StartSegmentIndex + Index) >= Lane.PointsBegin && (StartSegmentIndex + Index) < Lane.PointsEnd);
|
|
LanePoints[Index] = ZoneGraphStorage.LanePoints[StartSegmentIndex + Index];
|
|
LaneTangentVectors[Index] = FMassSnorm8Vector2D(FVector2D(ZoneGraphStorage.LaneTangentVectors[StartSegmentIndex + Index]));
|
|
LanePointProgressions[Index] = FMassInt16Real10(ZoneGraphStorage.LanePointProgressions[StartSegmentIndex + Index]);
|
|
}
|
|
}
|
|
|
|
// Calculate extra space around the lane on adjacent lanes.
|
|
TArray<FZoneGraphLinkedLane> LinkedLanes;
|
|
UE::ZoneGraph::Query::GetLinkedLanes(ZoneGraphStorage, CurrentLaneHandle, EZoneLaneLinkType::Adjacent, EZoneLaneLinkFlags::Left|EZoneLaneLinkFlags::Right, EZoneLaneLinkFlags::None, LinkedLanes);
|
|
|
|
float AdjacentLeftWidth = 0.0f;
|
|
float AdjacentRightWidth = 0.0f;
|
|
for (const FZoneGraphLinkedLane& LinkedLane : LinkedLanes)
|
|
{
|
|
if (LinkedLane.HasFlags(EZoneLaneLinkFlags::Left))
|
|
{
|
|
const FZoneLaneData& AdjacentLane = ZoneGraphStorage.Lanes[LinkedLane.DestLane.Index];
|
|
AdjacentLeftWidth += AdjacentLane.Width;
|
|
}
|
|
else if (LinkedLane.HasFlags(EZoneLaneLinkFlags::Right))
|
|
{
|
|
const FZoneLaneData& AdjacentLane = ZoneGraphStorage.Lanes[LinkedLane.DestLane.Index];
|
|
AdjacentRightWidth += AdjacentLane.Width;
|
|
}
|
|
}
|
|
LaneLeftSpace = FMassInt16Real(AdjacentLeftWidth);
|
|
LaneRightSpace = FMassInt16Real(AdjacentRightWidth);
|
|
}
|
|
|
|
// Fills Points from CachedLane
|
|
bool FMassZoneGraphShortPathFragment::RequestPath(const FMassZoneGraphCachedLaneFragment& CachedLane, const FZoneGraphShortPathRequest& Request, const float InCurrentDistanceAlongLane, const float AgentRadius)
|
|
{
|
|
Reset();
|
|
|
|
if (CachedLane.NumPoints < 2)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
|
|
// The current distance can come from a quantized lane distance. Check against quantized bounds, but clamp it to the actual path length when calculating the path.
|
|
static_assert(std::is_same_v<decltype(FMassZoneGraphPathPoint::Distance), FMassInt16Real>, "Assuming FMassZoneGraphPathPoint::Distance is quantized to 10 units.");
|
|
const float LaneLengthQuantized = FMath::CeilToFloat(CachedLane.LaneLength / 10.0f) * 10.0f;
|
|
|
|
static constexpr float Epsilon = 0.1f;
|
|
ensureMsgf(InCurrentDistanceAlongLane >= -Epsilon && InCurrentDistanceAlongLane <= (LaneLengthQuantized + Epsilon), TEXT("Current distance %f should be within the lane bounds 0.0 - %f"), InCurrentDistanceAlongLane, LaneLengthQuantized);
|
|
|
|
const float CurrentDistanceAlongLane = FMath::Min(InCurrentDistanceAlongLane, CachedLane.LaneLength);
|
|
|
|
// Set common lane parameters
|
|
#if WITH_MASSGAMEPLAY_DEBUG
|
|
DebugLaneHandle = CachedLane.LaneHandle;
|
|
#endif
|
|
|
|
bMoveReverse = Request.bMoveReverse;
|
|
EndOfPathIntent = Request.EndOfPathIntent;
|
|
bPartialResult = false;
|
|
|
|
const float DeflatedLaneHalfWidth = FMath::Max(0.0f, CachedLane.LaneWidth.Get() - AgentRadius) * 0.5f;
|
|
const float DeflatedLaneLeft = DeflatedLaneHalfWidth + CachedLane.LaneLeftSpace.Get();
|
|
const float DeflatedLaneRight = DeflatedLaneHalfWidth + CachedLane.LaneRightSpace.Get();
|
|
|
|
const float TargetDistanceAlongLane = FMath::Clamp(Request.TargetDistance, 0.0f, CachedLane.LaneLength);
|
|
const float MinDistanceAlongLane = FMath::Min(CurrentDistanceAlongLane, TargetDistanceAlongLane);
|
|
const float MaxDistanceAlongLane = FMath::Max(CurrentDistanceAlongLane, TargetDistanceAlongLane);
|
|
|
|
const float TangentSign = Request.bMoveReverse ? -1.0f : 1.0f;
|
|
|
|
// Slop factors used when testing if a point is conservatively inside the lane.
|
|
constexpr float OffLaneCapSlop = 10.0f;
|
|
constexpr float OffLaneEdgeSlop = 1.0f;
|
|
|
|
// Calculate how the start point relates to the corresponding location on lane.
|
|
FVector StartLanePosition;
|
|
FVector StartLaneTangent;
|
|
CachedLane.GetPointAndTangentAtDistance(CurrentDistanceAlongLane, StartLanePosition, StartLaneTangent);
|
|
float StartDistanceAlongPath = CurrentDistanceAlongLane;
|
|
|
|
FVector StartPosition = Request.StartPosition;
|
|
// Calculate start point's relation to the start point location on lane.
|
|
const FVector StartDelta = StartPosition - StartLanePosition;
|
|
const FVector StartLeftDir = FVector::CrossProduct(StartLaneTangent, FVector::UpVector);
|
|
float StartLaneOffset = FloatCastChecked<float>(FVector::DotProduct(StartLeftDir, StartDelta), UE::LWC::DefaultFloatPrecision);
|
|
float StartLaneForwardOffset = FloatCastChecked<float>(FVector::DotProduct(StartLaneTangent, StartDelta) * TangentSign, UE::LWC::DefaultFloatPrecision);
|
|
// The point is off-lane if behind the start, or beyond the boundary.
|
|
const bool bStartOffLane = StartLaneForwardOffset < -OffLaneCapSlop
|
|
|| StartLaneOffset < -(DeflatedLaneRight + OffLaneEdgeSlop)
|
|
|| StartLaneOffset > (DeflatedLaneLeft + OffLaneEdgeSlop);
|
|
StartLaneOffset = FMath::Clamp(StartLaneOffset, -DeflatedLaneRight, DeflatedLaneLeft);
|
|
|
|
if (bStartOffLane)
|
|
{
|
|
// The start point was off-lane, move the start location along the lane a bit further to have smoother connection.
|
|
const float StartForwardOffset = FMath::Clamp(Request.AnticipationDistance.Get() + StartLaneForwardOffset, 0.0f, Request.AnticipationDistance.Get());
|
|
StartDistanceAlongPath += StartForwardOffset * TangentSign; // Not clamping this distance intentionally so that the halfway point and clamping later works correctly.
|
|
}
|
|
|
|
// Calculate how the end point relates to the corresponding location on lane.
|
|
const bool bHasEndOfPathPoint = Request.bIsEndOfPathPositionSet;
|
|
float EndDistanceAlongPath = TargetDistanceAlongLane;
|
|
FVector EndLanePosition = FVector::ZeroVector;
|
|
FVector EndLaneTangent = FVector::ZeroVector;
|
|
bool bEndOffLane = false;
|
|
float EndLaneOffset = StartLaneOffset;
|
|
|
|
if (bHasEndOfPathPoint)
|
|
{
|
|
// Calculate end point's relation to the end point location on lane.
|
|
CachedLane.GetPointAndTangentAtDistance(TargetDistanceAlongLane, EndLanePosition, EndLaneTangent);
|
|
const FVector EndPosition = Request.EndOfPathPosition;
|
|
const FVector EndDelta = EndPosition - EndLanePosition;
|
|
const FVector LeftDir = FVector::CrossProduct(EndLaneTangent, FVector::UpVector);
|
|
EndLaneOffset = FloatCastChecked<float>(FVector::DotProduct(LeftDir, EndDelta), UE::LWC::DefaultFloatPrecision);
|
|
const float EndLaneForwardOffset = FloatCastChecked<float>(FVector::DotProduct(EndLaneTangent, EndDelta) * TangentSign, UE::LWC::DefaultFloatPrecision);
|
|
// The point is off-lane if further than the, or beyond the boundary.
|
|
bEndOffLane = EndLaneForwardOffset > OffLaneCapSlop
|
|
|| EndLaneOffset < -(DeflatedLaneRight + OffLaneEdgeSlop)
|
|
|| EndLaneOffset > (DeflatedLaneLeft + OffLaneEdgeSlop);
|
|
EndLaneOffset = FMath::Clamp(EndLaneOffset, -DeflatedLaneRight, DeflatedLaneLeft);
|
|
|
|
// Move the end location along the lane a bit back to have smoother connection.
|
|
const float EndForwardOffset = FMath::Clamp(Request.AnticipationDistance.Get() - EndLaneForwardOffset, 0.0f, Request.AnticipationDistance.Get());
|
|
EndDistanceAlongPath -= EndForwardOffset * TangentSign; // Not clamping this distance intentionally so that the halfway point and clamping later works correctly.
|
|
}
|
|
|
|
// Clamp the path move distances to current lane. We use halfway point to split the anticipation in case it gets truncated.
|
|
const float HalfwayDistanceAlongLane = FMath::Clamp((StartDistanceAlongPath + EndDistanceAlongPath) * 0.5f, MinDistanceAlongLane, MaxDistanceAlongLane);
|
|
|
|
if (Request.bMoveReverse)
|
|
{
|
|
StartDistanceAlongPath = FMath::Clamp(StartDistanceAlongPath, HalfwayDistanceAlongLane, MaxDistanceAlongLane);
|
|
EndDistanceAlongPath = FMath::Clamp(EndDistanceAlongPath, MinDistanceAlongLane, HalfwayDistanceAlongLane);
|
|
}
|
|
else
|
|
{
|
|
StartDistanceAlongPath = FMath::Clamp(StartDistanceAlongPath, MinDistanceAlongLane, HalfwayDistanceAlongLane);
|
|
EndDistanceAlongPath = FMath::Clamp(EndDistanceAlongPath, HalfwayDistanceAlongLane, MaxDistanceAlongLane);
|
|
}
|
|
|
|
// Check if the mid path got clamped away. This can happen if start of end or both are off-mesh, or just a short path.
|
|
const float MidPathMoveDistance = FMath::Abs(EndDistanceAlongPath - StartDistanceAlongPath);
|
|
const bool bHasMidPath = MidPathMoveDistance > KINDA_SMALL_NUMBER;
|
|
|
|
// If end position is not set to a specific location, use proposed offset
|
|
if (!bHasEndOfPathPoint)
|
|
{
|
|
// Slope defines how much the offset can change over the course of the path.
|
|
constexpr float MaxLaneOffsetSlope = 1.0f / 10.0f;
|
|
const float MaxOffset = MidPathMoveDistance * MaxLaneOffsetSlope;
|
|
const float LaneOffset = FMath::Clamp(Request.EndOfPathOffset.Get(), -MaxOffset, MaxOffset);
|
|
EndLaneOffset = FMath::Clamp(EndLaneOffset + LaneOffset, -DeflatedLaneRight, DeflatedLaneLeft);
|
|
}
|
|
|
|
// Always add off-lane start point.
|
|
if (bStartOffLane)
|
|
{
|
|
FMassZoneGraphPathPoint& StartPoint = Points[NumPoints++];
|
|
StartPoint.DistanceAlongLane = FMassInt16Real10(CurrentDistanceAlongLane);
|
|
StartPoint.Position = Request.StartPosition;
|
|
StartPoint.Tangent = FMassSnorm8Vector2D(StartLaneTangent * TangentSign);
|
|
StartPoint.bOffLane = true;
|
|
StartPoint.bIsLaneExtrema = false;
|
|
|
|
// Update start point to be inside the lane.
|
|
CachedLane.GetPointAndTangentAtDistance(StartDistanceAlongPath, StartLanePosition, StartLaneTangent);
|
|
const FVector LeftDir = FVector::CrossProduct(StartLaneTangent, FVector::UpVector);
|
|
StartPosition = StartLanePosition + LeftDir * StartLaneOffset;
|
|
|
|
// Adjust the start point to point towards the first on-lane point.
|
|
const FVector DirToClampedPoint = StartPosition - StartPoint.Position;
|
|
StartPoint.Tangent = FMassSnorm8Vector2D(DirToClampedPoint.GetSafeNormal());
|
|
}
|
|
|
|
// The second point is added if there was no off-lane start point, or we have mid path.
|
|
// This ensures that there's always at least one start point, and that no excess points are added if both start & end are off-lane close to each other.
|
|
if (!bStartOffLane || bHasMidPath)
|
|
{
|
|
// Add first on-lane point.
|
|
FMassZoneGraphPathPoint& Point = Points[NumPoints++];
|
|
Point.DistanceAlongLane = FMassInt16Real10(StartDistanceAlongPath);
|
|
Point.Position = StartPosition;
|
|
Point.Tangent = FMassSnorm8Vector2D(StartLaneTangent * TangentSign);
|
|
Point.bOffLane = false;
|
|
Point.bIsLaneExtrema = false;
|
|
}
|
|
|
|
// Add in between points.
|
|
const float InvDistanceRange = 1.0f / (EndDistanceAlongPath - StartDistanceAlongPath); // Used for lane offset interpolation.
|
|
float PrevDistanceAlongLane = StartDistanceAlongPath;
|
|
|
|
UE::MassNavigation::ZoneGraphPath::FCachedLaneSegmentIterator SegmentIterator(CachedLane, StartDistanceAlongPath, Request.bMoveReverse);
|
|
while (!SegmentIterator.HasReachedDistance(EndDistanceAlongPath))
|
|
{
|
|
// The segment endpoint is start when moving backwards (i.e. the segment index), and end when moving forwards.
|
|
const int32 CurrentSegmentEndPointIndex = SegmentIterator.CurrentSegment + (SegmentIterator.bMoveReverse ? 0 : 1);
|
|
const float DistanceAlongLane = CachedLane.LanePointProgressions[CurrentSegmentEndPointIndex].Get();
|
|
|
|
if (FMath::IsNearlyEqual(PrevDistanceAlongLane, DistanceAlongLane) == false)
|
|
{
|
|
if (NumPoints < MaxPoints)
|
|
{
|
|
const FVector& LanePosition = CachedLane.LanePoints[CurrentSegmentEndPointIndex];
|
|
const FVector LaneTangent = CachedLane.LaneTangentVectors[CurrentSegmentEndPointIndex].GetVector();
|
|
|
|
const float LaneOffsetT = (DistanceAlongLane - StartDistanceAlongPath) * InvDistanceRange;
|
|
const float LaneOffset = FMath::Lerp(StartLaneOffset, EndLaneOffset, LaneOffsetT);
|
|
const FVector LeftDir = FVector::CrossProduct(LaneTangent, FVector::UpVector);
|
|
|
|
FMassZoneGraphPathPoint& Point = Points[NumPoints++];
|
|
Point.DistanceAlongLane = FMassInt16Real10(DistanceAlongLane);
|
|
Point.Position = LanePosition + LeftDir * LaneOffset;
|
|
Point.Tangent = FMassSnorm8Vector2D(LaneTangent * TangentSign);
|
|
Point.bOffLane = false;
|
|
Point.bIsLaneExtrema = false;
|
|
|
|
PrevDistanceAlongLane = DistanceAlongLane;
|
|
}
|
|
else
|
|
{
|
|
bPartialResult = true;
|
|
break;
|
|
}
|
|
}
|
|
|
|
SegmentIterator.Next();
|
|
}
|
|
|
|
// The second last point is added if there is no end point, or we have mid path.
|
|
// This ensures that there's always at least one end point, and that no excess points are added if both start & end are off-lane close to each other.
|
|
if (!bHasEndOfPathPoint || bHasMidPath)
|
|
{
|
|
if (NumPoints < MaxPoints)
|
|
{
|
|
// Interpolate last point on mid path.
|
|
FVector LanePosition;
|
|
FVector LaneTangent;
|
|
CachedLane.InterpolatePointAndTangentOnSegment(SegmentIterator.CurrentSegment, EndDistanceAlongPath, LanePosition, LaneTangent);
|
|
|
|
const float LaneOffset = EndLaneOffset;
|
|
const FVector LeftDir = FVector::CrossProduct(LaneTangent, FVector::UpVector);
|
|
|
|
FMassZoneGraphPathPoint& Point = Points[NumPoints++];
|
|
Point.DistanceAlongLane = FMassInt16Real10(EndDistanceAlongPath);
|
|
Point.Position = LanePosition + LeftDir * LaneOffset;
|
|
Point.Tangent = FMassSnorm8Vector2D(LaneTangent * TangentSign);
|
|
Point.bOffLane = false;
|
|
Point.bIsLaneExtrema = !Request.bIsEndOfPathPositionSet && CachedLane.IsDistanceAtLaneExtrema(EndDistanceAlongPath);
|
|
}
|
|
else
|
|
{
|
|
bPartialResult = true;
|
|
}
|
|
}
|
|
|
|
checkf(NumPoints >= 1, TEXT("Path should have at least 1 point at this stage but has none."));
|
|
|
|
// Add end of path point if set.
|
|
if (bHasEndOfPathPoint)
|
|
{
|
|
if (NumPoints < MaxPoints)
|
|
{
|
|
const FVector EndPosition = Request.EndOfPathPosition;
|
|
|
|
// Use provided direction if set, otherwise use direction from last point on lane to end of path point
|
|
const FVector EndDirection = (Request.bIsEndOfPathDirectionSet) ?
|
|
Request.EndOfPathDirection.Get() :
|
|
(EndPosition - Points[NumPoints-1].Position).GetSafeNormal();
|
|
|
|
FMassZoneGraphPathPoint& Point = Points[NumPoints++];
|
|
Point.DistanceAlongLane = FMassInt16Real10(TargetDistanceAlongLane);
|
|
Point.Position = EndPosition;
|
|
Point.Tangent = FMassSnorm8Vector2D(EndDirection);
|
|
Point.bOffLane = bEndOffLane;
|
|
Point.bIsLaneExtrema = false;
|
|
}
|
|
else
|
|
{
|
|
bPartialResult = true;
|
|
}
|
|
}
|
|
|
|
checkf(NumPoints >= 2, TEXT("Path should have at least 2 points at this stage, has %d."), NumPoints);
|
|
|
|
// Calculate movement distance at each point.
|
|
float PathDistance = 0.0f;
|
|
Points[0].Distance.Set(PathDistance);
|
|
for (uint8 PointIndex = 1; PointIndex < NumPoints; PointIndex++)
|
|
{
|
|
FMassZoneGraphPathPoint& PrevPoint = Points[PointIndex - 1];
|
|
FMassZoneGraphPathPoint& Point = Points[PointIndex];
|
|
const FVector PrevPosition = PrevPoint.Position;
|
|
const FVector Position = Point.Position;
|
|
const float DeltaDistance = FloatCastChecked<float>(FVector::Dist(PrevPosition, Position), UE::LWC::DefaultFloatPrecision);
|
|
PathDistance += DeltaDistance;
|
|
Point.Distance.Set(PathDistance);
|
|
}
|
|
|
|
// If the last point on path reaches end of the lane, set the next handle to the next lane. It will be update when path finishes.
|
|
if (!bPartialResult && Request.NextLaneHandle.IsValid())
|
|
{
|
|
const FMassZoneGraphPathPoint& LastPoint = Points[NumPoints - 1];
|
|
|
|
if (Request.NextExitLinkType == EZoneLaneLinkType::Adjacent || LastPoint.bIsLaneExtrema)
|
|
{
|
|
NextLaneHandle = Request.NextLaneHandle;
|
|
NextExitLinkType = Request.NextExitLinkType;
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
bool FMassZoneGraphShortPathFragment::RequestStand(const FMassZoneGraphCachedLaneFragment& CachedLane, const float CurrentDistanceAlongLane, const FVector& CurrentPosition)
|
|
{
|
|
Reset();
|
|
|
|
if (CachedLane.NumPoints < 2)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
static constexpr float Epsilon = 0.1f;
|
|
check(CurrentDistanceAlongLane >= -Epsilon && CurrentDistanceAlongLane <= (CachedLane.LaneLength + Epsilon));
|
|
|
|
// Get current location
|
|
FVector CurrentLanePosition;
|
|
FVector CurrentLaneTangent;
|
|
CachedLane.GetPointAndTangentAtDistance(CurrentDistanceAlongLane, CurrentLanePosition, CurrentLaneTangent);
|
|
|
|
// Set common lane parameters
|
|
#if WITH_MASSGAMEPLAY_DEBUG
|
|
DebugLaneHandle = CachedLane.LaneHandle;
|
|
#endif
|
|
bMoveReverse = false;
|
|
EndOfPathIntent = EMassMovementAction::Stand;
|
|
bPartialResult = false;
|
|
|
|
// Add start point, if the start is outside the lane, add another point to get back to lane.
|
|
const FVector StartMoveOffset = CurrentPosition - CurrentLanePosition;
|
|
|
|
FMassZoneGraphPathPoint& StartPoint = Points[NumPoints];
|
|
StartPoint.DistanceAlongLane = FMassInt16Real10(CurrentDistanceAlongLane);
|
|
StartPoint.Position = CurrentPosition;
|
|
StartPoint.Tangent = FMassSnorm8Vector2D(CurrentLaneTangent);
|
|
StartPoint.bOffLane = false;
|
|
StartPoint.bIsLaneExtrema = false;
|
|
StartPoint.Distance = FMassInt16Real(0.0f);
|
|
NumPoints++;
|
|
|
|
FMassZoneGraphPathPoint& EndPoint = Points[NumPoints];
|
|
EndPoint.DistanceAlongLane = FMassInt16Real10(CurrentDistanceAlongLane);
|
|
EndPoint.Position = CurrentPosition;
|
|
EndPoint.Tangent = FMassSnorm8Vector2D(CurrentLaneTangent);
|
|
EndPoint.bOffLane = false;
|
|
EndPoint.bIsLaneExtrema = false;
|
|
EndPoint.Distance = FMassInt16Real(0.0f);
|
|
NumPoints++;
|
|
|
|
return true;
|
|
}
|