285 lines
14 KiB
C++
285 lines
14 KiB
C++
// Copyright Epic Games, Inc. All Rights Reserved.
|
|
|
|
#include "MassNavMeshNavigationProcessors.h"
|
|
#include "MassCommonFragments.h"
|
|
#include "Avoidance/MassAvoidanceFragments.h"
|
|
#include "MassCommonTypes.h"
|
|
#include "MassDebugger.h"
|
|
#include "MassExecutionContext.h"
|
|
#include "MassNavMeshNavigationFragments.h"
|
|
#include "MassNavMeshNavigationTypes.h"
|
|
#include "MassNavigationFragments.h"
|
|
#include "MassSignalSubsystem.h"
|
|
#include "MassSimulationLOD.h"
|
|
|
|
|
|
UMassNavMeshPathFollowProcessor::UMassNavMeshPathFollowProcessor()
|
|
: EntityQuery_Conditional(*this)
|
|
{
|
|
ExecutionFlags = (int32)EProcessorExecutionFlags::AllNetModes;
|
|
ExecutionOrder.ExecuteInGroup = UE::Mass::ProcessorGroupNames::Tasks;
|
|
ExecutionOrder.ExecuteBefore.Add(UE::Mass::ProcessorGroupNames::Avoidance);
|
|
}
|
|
|
|
void UMassNavMeshPathFollowProcessor::InitializeInternal(UObject& Owner, const TSharedRef<FMassEntityManager>& EntityManager)
|
|
{
|
|
Super::InitializeInternal(Owner, EntityManager);
|
|
SignalSubsystem = UWorld::GetSubsystem<UMassSignalSubsystem>(Owner.GetWorld());
|
|
}
|
|
|
|
void UMassNavMeshPathFollowProcessor::ConfigureQueries(const TSharedRef<FMassEntityManager>& EntityManager)
|
|
{
|
|
EntityQuery_Conditional.AddRequirement<FMassNavMeshShortPathFragment>(EMassFragmentAccess::ReadWrite);
|
|
EntityQuery_Conditional.AddRequirement<FMassMoveTargetFragment>(EMassFragmentAccess::ReadWrite);
|
|
EntityQuery_Conditional.AddRequirement<FTransformFragment>(EMassFragmentAccess::ReadOnly);
|
|
|
|
// @todo: validate LOD and variable ticking
|
|
EntityQuery_Conditional.AddRequirement<FMassSimulationLODFragment>(EMassFragmentAccess::ReadOnly, EMassFragmentPresence::Optional);
|
|
EntityQuery_Conditional.AddRequirement<FMassSimulationVariableTickFragment>(EMassFragmentAccess::ReadOnly, EMassFragmentPresence::Optional);
|
|
}
|
|
|
|
void UMassNavMeshPathFollowProcessor::Execute(FMassEntityManager& EntityManager, FMassExecutionContext& Context)
|
|
{
|
|
if (!SignalSubsystem)
|
|
{
|
|
return;
|
|
}
|
|
|
|
TArray<FMassEntityHandle> EntitiesToSignalPathDone;
|
|
|
|
EntityQuery_Conditional.ForEachEntityChunk(Context, [this, &EntitiesToSignalPathDone](FMassExecutionContext& Context)
|
|
{
|
|
const TArrayView<FMassNavMeshShortPathFragment> ShortPathList = Context.GetMutableFragmentView<FMassNavMeshShortPathFragment>();
|
|
const TArrayView<FMassMoveTargetFragment> MoveTargetList = Context.GetMutableFragmentView<FMassMoveTargetFragment>();
|
|
const TConstArrayView<FTransformFragment> TransformList = Context.GetFragmentView<FTransformFragment>();
|
|
|
|
// @todo: validate LOD and variable ticking
|
|
const TConstArrayView<FMassSimulationLODFragment> SimLODList = Context.GetFragmentView<FMassSimulationLODFragment>();
|
|
const bool bHasLOD = (SimLODList.Num() > 0);
|
|
|
|
const TConstArrayView<FMassSimulationVariableTickFragment> SimVariableTickList = Context.GetFragmentView<FMassSimulationVariableTickFragment>();
|
|
const float WorldDeltaTime = Context.GetDeltaTimeSeconds();
|
|
|
|
for (FMassExecutionContext::FEntityIterator EntityIt = Context.CreateEntityIterator(); EntityIt; ++EntityIt)
|
|
{
|
|
FMassNavMeshShortPathFragment& ShortPath = ShortPathList[EntityIt];
|
|
FMassMoveTargetFragment& MoveTarget = MoveTargetList[EntityIt];
|
|
const FMassEntityHandle Entity = Context.GetEntity(EntityIt);
|
|
|
|
bool bDisplayDebug = false;
|
|
#if WITH_MASSGAMEPLAY_DEBUG // this will result in bDisplayDebug == false and disabling of all the vlogs below
|
|
bDisplayDebug = UE::Mass::Debug::IsDebuggingEntity(Entity);
|
|
#endif // WITH_MASSGAMEPLAY_DEBUG
|
|
|
|
// Must have at least two points to interpolate.
|
|
if (MoveTarget.GetCurrentAction() == EMassMovementAction::Move && ShortPath.NumPoints >= 2)
|
|
{
|
|
UE_CVLOG_UELOG(bDisplayDebug, this, LogMassNavMeshNavigation, Verbose, TEXT("Entity [%s] Updating navmesh path following"), *Entity.DebugGetDescription());
|
|
|
|
const bool bWasDone = ShortPath.IsDone();
|
|
|
|
// Note: this should be in sync with the logic in apply velocity.
|
|
const bool bHasSteering = (bHasLOD == false) || (SimLODList[EntityIt].LOD != EMassLOD::Off);
|
|
|
|
if (!bHasSteering || !MoveTarget.bSteeringFallingBehind)
|
|
{
|
|
// Update progress
|
|
ShortPath.MoveTargetProgressDistance += MoveTarget.DesiredSpeed.Get() * WorldDeltaTime;
|
|
}
|
|
|
|
if (!bWasDone)
|
|
{
|
|
const FTransformFragment& TransformFragment = TransformList[EntityIt];
|
|
const FVector EntityLocation = TransformFragment.GetTransform().GetLocation();
|
|
|
|
const uint8 LastIndex = ShortPath.NumPoints - 1;
|
|
|
|
// Point index on the short path where we should update the path.
|
|
const uint8 UpdatePointIndex = ShortPath.bPartialResult ? (ShortPath.NumPoints - ShortPath.NumPointsBeyondUpdate) : LastIndex;
|
|
|
|
// If the shortpath is partial, it's expected to be full, meaning MaxPoints-NumPointsBeyondUpdate >= 1 so the check below must be valid.
|
|
check(UpdatePointIndex >= 1);
|
|
|
|
// Update entity progress on path (EntityDistanceToGoal)
|
|
uint8 ClosestSegmentIndex = 0;
|
|
float ClosestDistanceSquare = FLT_MAX;
|
|
for (uint8 PointIndex = 0; PointIndex < UpdatePointIndex && PointIndex+1 <= LastIndex; PointIndex++)
|
|
{
|
|
// Project on closest segment
|
|
const float DistanceSquared = FMath::PointDistToSegmentSquared(EntityLocation, ShortPath.Points[PointIndex].Position, ShortPath.Points[PointIndex+1].Position);
|
|
if (DistanceSquared < ClosestDistanceSquare)
|
|
{
|
|
ClosestDistanceSquare = DistanceSquared;
|
|
ClosestSegmentIndex = PointIndex;
|
|
}
|
|
}
|
|
|
|
const FMassNavMeshPathPoint& PointA = ShortPath.Points[ClosestSegmentIndex];
|
|
const FMassNavMeshPathPoint& PointB = ShortPath.Points[ClosestSegmentIndex+1];
|
|
const float DistanceOnClosestSegment = (EntityLocation - PointA.Position).Dot((PointB.Position - PointA.Position).GetSafeNormal());
|
|
const float EntityProgress = ShortPath.Points[ClosestSegmentIndex].Distance.Get() + DistanceOnClosestSegment;
|
|
MoveTarget.EntityDistanceToGoal = FMath::Max(0.f, ShortPath.Points[UpdatePointIndex].Distance.Get() - EntityProgress);
|
|
|
|
#if WITH_MASSGAMEPLAY_DEBUG
|
|
UE_CVLOG_UELOG(bDisplayDebug, this, LogMassNavMeshNavigation, Verbose, TEXT("Entity [%s] ProgressDistance: %.2f, UpdatePointIndex: %i, EntityDistanceToGoal: %.2f"),
|
|
*Entity.DebugGetDescription(), ShortPath.MoveTargetProgressDistance, UpdatePointIndex, MoveTarget.EntityDistanceToGoal);
|
|
|
|
if (bDisplayDebug)
|
|
{
|
|
// Display update point
|
|
const FVector ZOffset(0,0,20);
|
|
UE_VLOG_CIRCLE(this, LogMassNavMeshNavigation, Display, ShortPath.Points[UpdatePointIndex].Position + ZOffset,
|
|
FVector(0,0,1), /*Radius*/10.f, FColor::Green, TEXT("%i"), UpdatePointIndex);
|
|
|
|
// Display entity progress
|
|
const float T = (EntityProgress - ShortPath.Points[ClosestSegmentIndex].Distance.Get()) / (PointB.Distance.Get() - PointA.Distance.Get());
|
|
const FVector ProjectedEntityPosition = FMath::Lerp(PointA.Position, PointB.Position, T);
|
|
UE_VLOG_SEGMENT(this, LogMassNavMeshNavigation, Display, EntityLocation + ZOffset, ProjectedEntityPosition + ZOffset, FColor::Silver, TEXT(""));
|
|
UE_VLOG_CIRCLE(this, LogMassNavMeshNavigation, Display, ProjectedEntityPosition + ZOffset,
|
|
FVector(0,0,1), /*Radius*/10.f, FColor::Silver, TEXT(""));
|
|
}
|
|
#endif // WITH_MASSGAMEPLAY_DEBUG
|
|
|
|
if (ShortPath.MoveTargetProgressDistance <= 0.0f)
|
|
{
|
|
// Before the start of the path
|
|
MoveTarget.Center = ShortPath.Points[0].Position;
|
|
MoveTarget.Forward = ShortPath.Points[0].Tangent.GetVector();
|
|
MoveTarget.DistanceToGoal = ShortPath.Points[UpdatePointIndex].Distance.Get();
|
|
|
|
UE_CVLOG_UELOG(bDisplayDebug, this, LogMassNavMeshNavigation, Log, TEXT("Entity [%s] before start of path. EntityDistanceToGoal: %.1f, DistanceToGoal: %.1f."),
|
|
*Entity.DebugGetDescription(),
|
|
MoveTarget.EntityDistanceToGoal,
|
|
MoveTarget.DistanceToGoal);
|
|
}
|
|
else if (ShortPath.MoveTargetProgressDistance <= ShortPath.Points[UpdatePointIndex].Distance.Get())
|
|
{
|
|
// Along the path, interpolate.
|
|
uint8 PointIndex = 0;
|
|
while (PointIndex < (ShortPath.NumPoints - 2))
|
|
{
|
|
const FMassNavMeshPathPoint& NextPoint = ShortPath.Points[PointIndex + 1];
|
|
if (ShortPath.MoveTargetProgressDistance <= NextPoint.Distance.Get())
|
|
{
|
|
break;
|
|
}
|
|
PointIndex++;
|
|
}
|
|
const FMassNavMeshPathPoint& CurrPoint = ShortPath.Points[PointIndex];
|
|
const FMassNavMeshPathPoint& NextPoint = ShortPath.Points[PointIndex + 1];
|
|
|
|
const float T = (ShortPath.MoveTargetProgressDistance - CurrPoint.Distance.Get()) / (NextPoint.Distance.Get() - CurrPoint.Distance.Get());
|
|
|
|
// Set move target location using the new progress distance.
|
|
MoveTarget.Center = FMath::Lerp(CurrPoint.Position, NextPoint.Position, T);
|
|
MoveTarget.Forward = FMath::Lerp(CurrPoint.Tangent.GetVector(), NextPoint.Tangent.GetVector(), T).GetSafeNormal();
|
|
MoveTarget.DistanceToGoal = ShortPath.Points[UpdatePointIndex].Distance.Get() - FMath::Lerp(CurrPoint.Distance.Get(), NextPoint.Distance.Get(), T);
|
|
|
|
UE_CVLOG_UELOG(bDisplayDebug, this, LogMassNavMeshNavigation, Verbose, TEXT("Entity [%s] Distance to goal on short path: %.1f."),
|
|
*Entity.DebugGetDescription(),
|
|
MoveTarget.DistanceToGoal);
|
|
}
|
|
else
|
|
{
|
|
// Update point reached.
|
|
MoveTarget.Center = ShortPath.Points[UpdatePointIndex].Position;
|
|
MoveTarget.Forward = ShortPath.Points[UpdatePointIndex].Tangent.GetVector();
|
|
MoveTarget.DistanceToGoal = 0.f;
|
|
|
|
if (ShortPath.bPartialResult)
|
|
{
|
|
UE_CVLOG_UELOG(bDisplayDebug, this, LogMassNavMeshNavigation, Log, TEXT("Entity [%s] Finished path follow on short path."), *Entity.DebugGetDescription());
|
|
ShortPath.bDone = true;
|
|
}
|
|
else
|
|
{
|
|
// Last section of the path, wait for the steering to complete the movement.
|
|
UE_CVLOG_UELOG(bDisplayDebug, this, LogMassNavMeshNavigation, Log, TEXT("Entity [%s] Waiting to reach the end of path."), *Entity.DebugGetDescription());
|
|
|
|
const float SegmentSize = ShortPath.Points[UpdatePointIndex].Distance.Get() - ShortPath.Points[UpdatePointIndex-1].Distance.Get();
|
|
const FVector PreviousPointLocation = ShortPath.Points[UpdatePointIndex-1].Position;
|
|
const FVector PathDir = (ShortPath.Points[UpdatePointIndex].Position - PreviousPointLocation).GetSafeNormal();
|
|
|
|
const FVector RelativeLocation = EntityLocation - PreviousPointLocation;
|
|
const float ProjectionOnSegment = RelativeLocation.Dot(PathDir);
|
|
if (ProjectionOnSegment > (SegmentSize - ShortPath.EndReachedDistance))
|
|
{
|
|
UE_CVLOG_UELOG(bDisplayDebug, this, LogMassNavMeshNavigation, Log, TEXT("Entity [%s] Finished path follow on short path, end of path."), *Entity.DebugGetDescription());
|
|
ShortPath.bDone = true;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
const bool bIsDone = ShortPath.IsDone();
|
|
|
|
// Signal path done.
|
|
if (!bWasDone && bIsDone)
|
|
{
|
|
EntitiesToSignalPathDone.Add(Entity);
|
|
}
|
|
|
|
#if WITH_MASSGAMEPLAY_DEBUG
|
|
if (bDisplayDebug)
|
|
{
|
|
const FColor EntityColor = UE::Mass::Debug::GetEntityDebugColor(Entity);
|
|
|
|
const FVector ZOffset(0,0,10);
|
|
FLinearColor MixColor(EntityColor);
|
|
MixColor += FColor::White;
|
|
MixColor /= 2;
|
|
const FColor LightEntityColor = MixColor.ToFColorSRGB();
|
|
|
|
FLinearColor BorderMixColor(EntityColor);
|
|
BorderMixColor += FColor::Black;
|
|
BorderMixColor /= 2;
|
|
const FColor DarkEntityColor = BorderMixColor.ToFColorSRGB();
|
|
|
|
// Draw path
|
|
for (uint8 PointIndex = 0; PointIndex < ShortPath.NumPoints - 1; PointIndex++)
|
|
{
|
|
const FMassNavMeshPathPoint& CurrPoint = ShortPath.Points[PointIndex];
|
|
const FMassNavMeshPathPoint& NextPoint = ShortPath.Points[PointIndex + 1];
|
|
UE_VLOG_SEGMENT_THICK(this, LogMassNavMeshNavigation, Display, CurrPoint.Position + ZOffset, NextPoint.Position + ZOffset, EntityColor, /*Thickness*/3, TEXT(""));
|
|
UE_VLOG_SEGMENT_THICK(this, LogMassNavMeshNavigation, Display, CurrPoint.Left + ZOffset, NextPoint.Left + ZOffset, DarkEntityColor, /*Thickness*/2, TEXT(""));
|
|
UE_VLOG_SEGMENT_THICK(this, LogMassNavMeshNavigation, Display, CurrPoint.Right + ZOffset, NextPoint.Right + ZOffset, DarkEntityColor, /*Thickness*/2, TEXT(""));
|
|
}
|
|
|
|
// Draw point indices
|
|
for (uint8 PointIndex = 0; PointIndex < ShortPath.NumPoints; PointIndex++)
|
|
{
|
|
const FMassNavMeshPathPoint& CurrPoint = ShortPath.Points[PointIndex];
|
|
UE_VLOG_CIRCLE(this, LogMassNavMeshNavigation, Display, CurrPoint.Position + ZOffset, FVector(0,0,1), /*Radius*/4.f, EntityColor, TEXT("%i"), PointIndex);
|
|
}
|
|
|
|
// Draw tangents
|
|
for (uint8 PointIndex = 0; PointIndex < ShortPath.NumPoints; PointIndex++)
|
|
{
|
|
const FMassNavMeshPathPoint& CurrPoint = ShortPath.Points[PointIndex];
|
|
const FVector CurrBase = CurrPoint.Position + ZOffset + FVector(0,0,1);
|
|
UE_VLOG_SEGMENT_THICK(this, LogMassNavMeshNavigation, Verbose, CurrBase, CurrBase + CurrPoint.Tangent.GetVector() * 100.0f, LightEntityColor, /*Thickness*/1, TEXT(""));
|
|
}
|
|
|
|
// Draw MoveTarget
|
|
constexpr float Radius = 20.f;
|
|
UE_VLOG_CIRCLE(this, LogMassNavMeshNavigation, Display, MoveTarget.Center + ZOffset, FVector(0,0,1), Radius, LightEntityColor, TEXT(""));
|
|
|
|
// Draw tolerance distance at end (shown as a circle instead of an infinite perpendicular line)
|
|
if (!ShortPath.bPartialResult)
|
|
{
|
|
const float EndRadius = ShortPath.EndReachedDistance;
|
|
UE_VLOG_WIRECIRCLE(this, LogMassNavMeshNavigation, Display, ShortPath.Points[ShortPath.NumPoints-1].Position + ZOffset,
|
|
FVector(0,0,1), EndRadius, FColor::Black, TEXT("End"));
|
|
}
|
|
}
|
|
#endif // WITH_MASSGAMEPLAY_DEBUG
|
|
}
|
|
}
|
|
});
|
|
|
|
if (EntitiesToSignalPathDone.Num())
|
|
{
|
|
SignalSubsystem->SignalEntities(UE::Mass::Signals::FollowPointPathDone, EntitiesToSignalPathDone);
|
|
}
|
|
}
|