115 lines
3.5 KiB
C++
115 lines
3.5 KiB
C++
// Copyright Epic Games, Inc. All Rights Reserved.
|
|
|
|
#pragma once
|
|
|
|
#include "CoreTypes.h"
|
|
#include "Math/Quat.h"
|
|
#include "Math/UnrealMathUtility.h"
|
|
|
|
namespace UE::MassNavigation
|
|
{
|
|
// Calculates yaw angle from direction vector.
|
|
inline FQuat::FReal GetYawFromQuat(const FQuat Rotation)
|
|
{
|
|
const FQuat::FReal YawY = 2. * (Rotation.W * Rotation.Z + Rotation.X * Rotation.Y);
|
|
const FQuat::FReal YawX = (1. - 2. * (FMath::Square(Rotation.Y) + FMath::Square(Rotation.Z)));
|
|
return FMath::Atan2(YawY, YawX);
|
|
}
|
|
|
|
inline FVector::FReal GetYawFromDirection(const FVector Direction)
|
|
{
|
|
return FMath::Atan2(Direction.Y, Direction.X);
|
|
}
|
|
|
|
// Wraps and angle to range -PI..PI. Angle in radians.
|
|
inline FVector::FReal WrapAngle(const FVector::FReal Angle)
|
|
{
|
|
FVector::FReal WrappedAngle = FMath::Fmod(Angle, UE_DOUBLE_PI*2.);
|
|
WrappedAngle = (WrappedAngle > UE_DOUBLE_PI) ? WrappedAngle - UE_DOUBLE_PI * 2. : WrappedAngle;
|
|
WrappedAngle = (WrappedAngle < -UE_DOUBLE_PI) ? WrappedAngle + UE_DOUBLE_PI * 2. : WrappedAngle;
|
|
return WrappedAngle;
|
|
}
|
|
|
|
// Linearly interpolates between two angles (in Radians).
|
|
inline FVector::FReal LerpAngle(const FVector::FReal AngleA, const FVector::FReal AngleB, const FVector::FReal T)
|
|
{
|
|
const FVector::FReal DeltaAngle = WrapAngle(AngleB - AngleA);
|
|
return AngleA + DeltaAngle * T;
|
|
}
|
|
|
|
// Exponential smooth from current angle to target angle. Angles in radians.
|
|
inline FVector::FReal ExponentialSmoothingAngle(const FVector::FReal Angle, const FVector::FReal TargetAngle, const FVector::FReal DeltaTime, const FVector::FReal SmoothingTime)
|
|
{
|
|
// Note: based on FMath::ExponentialSmoothingApprox().
|
|
if (SmoothingTime < KINDA_SMALL_NUMBER)
|
|
{
|
|
return TargetAngle;
|
|
}
|
|
const FVector::FReal A = DeltaTime / SmoothingTime;
|
|
const FVector::FReal Exp = FMath::InvExpApprox(A);
|
|
return TargetAngle + WrapAngle(Angle - TargetAngle) * Exp;
|
|
}
|
|
|
|
// Clamps vectors magnitude to Mag.
|
|
inline FVector ClampVector(const FVector Vec, const FVector::FReal Mag)
|
|
{
|
|
const FVector::FReal Len = Vec.SizeSquared();
|
|
if (Len > FMath::Square(Mag)) {
|
|
return Vec * Mag / FMath::Sqrt(Len);
|
|
}
|
|
return Vec;
|
|
}
|
|
|
|
// Projects a point to segment and returns the time interpolation value.
|
|
inline FVector::FReal ProjectPtSeg(const FVector2D Point, const FVector2D Start, const FVector2D End)
|
|
{
|
|
const FVector2D Seg = End - Start;
|
|
const FVector2D Dir = Point - Start;
|
|
const FVector::FReal SegSizeSquared = Seg.SizeSquared();
|
|
const FVector::FReal SegDirDot = FVector2D::DotProduct(Seg, Dir);
|
|
|
|
if (SegDirDot < 0.)
|
|
{
|
|
return 0.;
|
|
}
|
|
|
|
if (SegDirDot > SegSizeSquared)
|
|
{
|
|
return 1.;
|
|
}
|
|
|
|
return SegSizeSquared > 0. ? (SegDirDot / SegSizeSquared) : 0.;
|
|
}
|
|
|
|
// Returns the SmoothStep curve for X in range [0..1].
|
|
inline float Smooth(const float X)
|
|
{
|
|
return X * X * (3.f - 2.f * X);
|
|
}
|
|
|
|
// Returns the SmoothStep curve for X in range [0..1].
|
|
inline double Smooth(const double X)
|
|
{
|
|
return X * X * (3. - 2. * X);
|
|
}
|
|
|
|
// Returns left direction from forward and up directions.
|
|
inline FVector GetLeftDirection(const FVector Forward, const FVector Up)
|
|
{
|
|
return FVector::CrossProduct(Forward, Up).GetSafeNormal();
|
|
}
|
|
|
|
// Computes miter normal in XY plane from two neighbour edge normals.
|
|
inline FVector ComputeMiterNormal(const FVector NormalA, const FVector NormalB)
|
|
{
|
|
FVector Mid = 0.5 * (NormalA + NormalB);
|
|
const FVector::FReal MidSquared = FVector::DotProduct(Mid, Mid);
|
|
if (MidSquared > KINDA_SMALL_NUMBER)
|
|
{
|
|
const FVector::FReal Scale = FMath::Min(1. / MidSquared, 20.);
|
|
Mid *= Scale;
|
|
}
|
|
return Mid;
|
|
}
|
|
} // UE::MassNavigation
|