73 lines
2.7 KiB
C++
73 lines
2.7 KiB
C++
// Copyright Epic Games, Inc. All Rights Reserved.
|
|
|
|
#include "MassLookAtFragments.h"
|
|
#include "ZoneGraphSubsystem.h"
|
|
#include "ZoneGraphTypes.h"
|
|
#include "Curves/BezierUtilities.h"
|
|
|
|
FVector FMassLookAtTrajectoryFragment::GetPointAtDistanceExtrapolated(const float DistanceAlongPath) const
|
|
{
|
|
if (NumPoints == 0)
|
|
{
|
|
return FVector::ZeroVector;
|
|
}
|
|
|
|
if (NumPoints == 1)
|
|
{
|
|
const float ExtrapolatedDistance = DistanceAlongPath - Points[0].DistanceAlongLane.Get();
|
|
const FMassLookAtTrajectoryPoint& Point = Points[0];
|
|
return Point.Position + FVector(Point.Tangent.Get() * ExtrapolatedDistance, 0.0f);
|
|
}
|
|
|
|
// Extrapolate along tangents if out of bounds.
|
|
const float StartDistance = Points[0].DistanceAlongLane.Get();
|
|
if (DistanceAlongPath < StartDistance)
|
|
{
|
|
const float ExtrapolatedDistance = DistanceAlongPath - StartDistance;
|
|
const FMassLookAtTrajectoryPoint& Point = Points[0];
|
|
return Point.Position + FVector(Point.Tangent.Get() * ExtrapolatedDistance, 0.0f);
|
|
}
|
|
|
|
const int32 LastPointIndex = NumPoints - 1;
|
|
const float EndDistance = Points[LastPointIndex].DistanceAlongLane.Get();
|
|
if (DistanceAlongPath > EndDistance)
|
|
{
|
|
const float ExtrapolatedDistance = DistanceAlongPath - EndDistance;
|
|
const FMassLookAtTrajectoryPoint& Point = Points[LastPointIndex];
|
|
return Point.Position + FVector(Point.Tangent.Get() * ExtrapolatedDistance, 0.0f);
|
|
}
|
|
|
|
check(NumPoints >= 2);
|
|
|
|
// Find segment
|
|
int32 SegmentIndex = 0;
|
|
while (SegmentIndex < ((int32)NumPoints - 2))
|
|
{
|
|
const float SegmentEndDistance = Points[SegmentIndex + 1].DistanceAlongLane.Get();
|
|
if (DistanceAlongPath < SegmentEndDistance)
|
|
{
|
|
break;
|
|
}
|
|
SegmentIndex++;
|
|
}
|
|
|
|
check(SegmentIndex >= 0 && SegmentIndex <= (int32)NumPoints - 2);
|
|
|
|
// Interpolate
|
|
const FMassLookAtTrajectoryPoint& StartPoint = Points[SegmentIndex];
|
|
const FMassLookAtTrajectoryPoint& EndPoint = Points[SegmentIndex + 1];
|
|
|
|
const float SegStartDistance = StartPoint.DistanceAlongLane.Get();
|
|
const float SegEndDistance = EndPoint.DistanceAlongLane.Get();
|
|
const float SegLength = SegEndDistance - SegStartDistance;
|
|
const float InvSegLength = SegLength > KINDA_SMALL_NUMBER ? 1.0f / SegLength : 0.0f;
|
|
const float T = FMath::Clamp((DistanceAlongPath - SegStartDistance) * InvSegLength, 0.0f, 1.0f);
|
|
|
|
// 1/3 third is used to create smooth bezier curve. On linear segments 1/3 will result linear interpolation.
|
|
const FVector::FReal TangentDistance = FVector::Dist(StartPoint.Position, EndPoint.Position) / 3.;
|
|
const FVector StartControlPoint = StartPoint.Position + StartPoint.Tangent.GetVector() * TangentDistance;
|
|
const FVector EndControlPoint = EndPoint.Position - EndPoint.Tangent.GetVector() * TangentDistance;
|
|
|
|
return UE::CubicBezier::Eval(StartPoint.Position, StartControlPoint, EndControlPoint, EndPoint.Position, T);
|
|
}
|