EXPERIMENTAL B-Spline path fitting functionality for the Flying Navigation System.

FlyingSplineExtension.h

// Copyright Ben Sutherland 2021. All rights reserved.
// EXPERIMENTAL B-Spline path fitting functionality for the Flying Navigation System.

#pragma once

#include "CoreMinimal.h"
#include "Kismet/BlueprintFunctionLibrary.h"
#include "FlyingNavFunctionLibrary.h"
#include "NavigationPath.h"
#include "FlyingSplineExtension.generated.h"

UCLASS()
class FLYINGNAVSYSTEM_API UFlyingSplineExtension : public UBlueprintFunctionLibrary
{
	GENERATED_BODY()

public:
	/*
	* EXPERIMENTAL: Smooths navigation path by constructing a clamped B-Spline and sampling points.
	* @param SampleRate: samples in new path per cm along path (approx)
	*/
	UFUNCTION(BlueprintCallable, Category = "AI|Navigation", meta = (WorldContext="WorldContextObject"))
	static UNavigationPath* SmoothPath(UObject* WorldContextObject, UNavigationPath* Path, const float SampleRate = 100.f, const float Tightness = 10.f);
};

// Implementation based on b-spline.js (pretty much just a straight UE4 port)
// https://github.com/thibauts/b-spline/blob/master/index.js
static FVector EvalBSpline(float t, const float Degree, const TArray<FVector>& Points, const TArray<float>& Knots, const TArray<float>& Weights)
{
	const int32 n = Points.Num();	// points count
	constexpr int32 d = 3;			// point dimensionality

	checkf(Degree >= 1, TEXT("Degree must be at least 1 (linear)"));
	checkf(Degree <= n-1, TEXT("Degree must be less than the number of points."));
	checkf(Knots.Num() == n+Degree+1, TEXT("Bad knot vector length"));
	checkf(Weights.Num() == n, TEXT("Bad weights length"));

	// Range to iterate over knots
	const int32 Domain[2] = {Degree, Knots.Num()-Degree-1};

	// remap t to the domain where the spline is defined
	const float Low  = Knots[Domain[0]];
	const float High = Knots[Domain[1]];
	t = t * (High - Low) + Low;

	checkf(Low <= t && t <= High, TEXT("t out of bounds"));

	// find s (the spline segment) for the [t] value provided
	int32 s;
	for (s = Domain[0]; s < Domain[1]; s++) {
		if(t >= Knots[s] && t <= Knots[s+1]) {
			break;
		}
	}

	// convert points to homogeneous coordinates
	TArray<FVector4> v; v.SetNumUninitialized(n);
	for (int32 i = 0; i < n; i++) {
		for (int32 j = 0; j < d; j++) {
			v[i][j] = Points[i][j] * Weights[i];
		}
		v[i][d] = Weights[i];
	}

	// l (level) goes from 1 to the curve degree + 1
	for (int32 l = 1; l <= Degree+1; l++) {
		// build level l of the pyramid
		for (int32 i = s; i > s-Degree-1+l; i--) {
			const float Alpha = (t - Knots[i]) / (Knots[i + Degree + 1 - l] - Knots[i]);

			// interpolate each component
			for (int32 j = 0; j < d+1; j++) {
				v[i][j] = (1 - Alpha) * v[i-1][j] + Alpha * v[i][j];
			}
		}
	}

	// convert back to cartesian and return
	FVector Result;
	for (int32 i = 0; i < d; i++) {
		Result[i] = v[s][i] / v[s][d];
	}
	
	return Result;
}

static TArray<FVector> BSplineSmooth(const TArray<FVector>& PathPoints, const float PathLength, const float SampleRate = 100.f, const float AngleWeight = 10.f)
{
	const int32 NumPathPoints = PathPoints.Num();
	if (NumPathPoints <= 2 || FMath::IsNearlyZero(PathLength))
	{
		return PathPoints;
	}
	
	// Cubic spline
	const int32 Degree = FMath::Min(3, NumPathPoints-1);
	
	// B-splines with clamped knot vectors pass through 
	// the two end control points.
	//
	// A clamped knot vector must have `degree + 1` equal knots 
	// at both its beginning and end.

	const int32 NumKnots = NumPathPoints + Degree + 1;
	TArray<float> KnotVector; KnotVector.SetNumZeroed(NumKnots);
	for (int32 i = Degree+1; i < NumKnots; i++)
	{
		KnotVector[i] = FMath::Min(i, NumPathPoints)-Degree;
	}

	// Weights:
	TArray<float> Weights;
	// Uniform weights
	Weights.Init(1, NumPathPoints);
	// Angle weights
	for (int32 i = 1; i < NumPathPoints-1; i++)
	{
		const FVector InDirection = (PathPoints[i] - PathPoints[i-1]).GetSafeNormal();
		const FVector OutDirection = (PathPoints[i] - PathPoints[i+1]).GetSafeNormal();
		// const float Angle = FMath::Acos(InDirection | OutDirection);
		Weights[i] += AngleWeight*(1.f-INV_PI*FMath::Acos(InDirection | OutDirection));
	}
	
	
	// Approximate equally spaced samples
	const float NumSamples = PathLength/SampleRate;
	const float SampleInterval = SampleRate/PathLength;
	TArray<FVector> CurvePoints; CurvePoints.Reserve(NumSamples + 1);
	for (float t = 0; t < 1; t += SampleInterval)
	{
		CurvePoints.Add(EvalBSpline(t, Degree, PathPoints, KnotVector, Weights));
	}
	CurvePoints.Add(PathPoints.Last());
	
	return CurvePoints;
}

inline UNavigationPath* UFlyingSplineExtension::SmoothPath(UObject* WorldContextObject, UNavigationPath* Path, const float SampleRate, const float Tightness)
{
	const TArray<FNavPathPoint>& NavPathPoints = Path->GetPath()->GetPathPoints();

	TArray<FVector> PathPoints; PathPoints.Reserve(NavPathPoints.Num());
	for (const FNavPathPoint& NavPathPoint : NavPathPoints)
	{
		PathPoints.Add(NavPathPoint);
	}

	const TArray<FVector> CurvePoints = BSplineSmooth(PathPoints, Path->GetPathLength(), SampleRate, Tightness);

	return UFlyingNavFunctionLibrary::SetNavigationPathPoints(WorldContextObject, Path, CurvePoints);
}