Prototype vehicle controller for UE4/5, mostly written in 2022

VehicleController.cpp

// Dylan E. - 2022

#include "VehicleController.h"

#include "NavigationSystem.h"
#include "NavFilters/RecastFilter_UseDefaultArea.h"
#include "NavigationPath.h"

#include "Kismet/GameplayStatics.h"
#include "Kismet/KismetSystemLibrary.h"
#include "Kismet/KismetMathLibrary.h"

AVehicleController::AVehicleController()
{
	Collision = CreateDefaultSubobject<USphereComponent>(TEXT("Collision"));

	if (Collision)
	{
		Collision->SetupAttachment(GetTransformComponent());
		Collision->SetSphereRadius(CollisionRadius);
	}
}

void AVehicleController::FindPathToTarget(TEnumAsByte<StatusCode>& Status)
{
    auto NavSys = FNavigationSystem::GetCurrent<UNavigationSystemV1>(GetWorld());
    if (!NavSys || !GetPawn() || !TargetWaypoint)
        return;

    FNavLocation Result;
    FVector CurrentLocation = GetPawn()->GetActorLocation();
    bool bCanReachablePath = NavSys->GetRandomReachablePointInRadius(TargetWaypoint->GetActorLocation(), 300.0f, Result);
    if (!bCanReachablePath)
        bCanReachablePath = NavSys->GetRandomReachablePointInRadius(TargetWaypoint->GetActorLocation(), 150.0f, Result);

    UNavigationPath* NavPath = NavSys->FindPathToLocationSynchronously(GetWorld(), CurrentLocation, bCanReachablePath ? Result.Location : TargetWaypoint->GetActorLocation(), GetPawn(), URecastFilter_UseDefaultArea::StaticClass());
    if (NavPath && NavPath->PathPoints.Num())
    {
        TargetLoc = NavPath->PathPoints[(NavPath->PathPoints.Num() > 1) ? 1 : 0];
        NoPathFoundCounter = 0;
        Status = OK;

        #if ENABLE_DRAW_DEBUG
                auto TempStartLoc = CurrentLocation;
                auto NewTargetLoc = TargetLoc;

                TempStartLoc.Z += 3.0f;
                NewTargetLoc.Z += 3.0f;

                // Show the current path to the waypoint 
                DrawDebugLine(GetWorld(), TempStartLoc, NewTargetLoc, FColor::Yellow, false, 0.0f, 0, 1.0f);

                // Draw a sphere at the current path point we're headed to on the way to the waypoint
                DrawDebugSphere(GetWorld(), NewTargetLoc, 100.f, 12, FColor::Green);

                // Draw a line from current location to the waypoint
                auto tmp = TargetWaypoint->GetActorLocation();
                tmp.Z += 3.0f;
                DrawDebugLine(GetWorld(), TempStartLoc, tmp, FColor::Blue, false, 0.0f, 0, 1.0f);

                // Draw a sphere at the waypoint
                DrawDebugSphere(GetWorld(), TargetWaypoint->GetActorLocation(), 100.f, 12, FColor::Green);
        #endif
    }
}

void AVehicleController::PerformVehicleDetection()
{
    TArray<FVehicleInfo> TempDetectedVehicles;
    TSet<AActor*> TempVehicles;

    for (auto& VehicleClass : AvoidClasses)
    {
        TSet<AActor*> OverlappingActors;
        Collision->GetOverlappingActors(OverlappingActors, VehicleClass);
        TempVehicles.Append(OverlappingActors);
    }

    if (TempVehicles.Num())
    {
        for (const auto& Vehicle : TempVehicles)
        {
            auto VehicleDistance = Vehicle->GetActorLocation() - GetPawn()->GetActorLocation();
            FVector normalised = VehicleDistance.GetSafeNormal(0.0001f);

            auto LimitAngle = (180.0) / UE_DOUBLE_PI * FMath::Acos(FVector::DotProduct(normalised, GetPawn()->GetActorForwardVector()));
            if (LimitAngle < 90.0)
            {
                FVehicleInfo info;

                info.Vehicle = Vehicle;
                info.Distance = VehicleDistance.Length();
                info.Angle = LimitAngle * FVector::CrossProduct(GetPawn()->GetActorForwardVector().GetSafeNormal(0.0001f), normalised).Z;

                TempDetectedVehicles.Add(info);
            }
        }
    }

    if (TempDetectedVehicles.IsValidIndex(0))
    {
        // Get the closest vehicle..
        int32 TempIndex = 0;
        float TmpDistance = 9999999999.f;
        int32 TmpVehicleIdx = 0;
        for (const auto& Vehicle : TempDetectedVehicles)
        {
            if (Vehicle.Distance < TmpDistance)
            {
                TmpVehicleIdx = TempIndex;
                TmpDistance = Vehicle.Distance;
            }
            TempIndex++;
        }
        ClosestVehicleIdx = TmpVehicleIdx;

        // Now we'll get the vehicle that is in front of us
        TempIndex = 0;
        float MaxInFrontAngle = 0.f;
        int32 TmpInFrontVehicle = 0;
        for (const auto& Vehicle : TempDetectedVehicles)
        {
            float TempAngle = FMath::Abs(Vehicle.Angle);
            if (TempAngle < MaxInFrontAngle)
            {
                MaxInFrontAngle = TempAngle;
                TmpInFrontVehicle = TempIndex;
            }
            TempIndex++;
        }
        InFrontVehicle = TmpInFrontVehicle;
    }

    DetectedVehicles = TempDetectedVehicles;
}


void AVehicleController::PerformAvoidanceTraces()
{
    if (UseTraces)
    {
        FVector start;

        FHitResult outHit;

        TArray<AActor*> ignores;
        ignores.Add(GetPawn());

        // Trace Ahead   
        FVector end = GetPawn()->GetActorForwardVector() * TraceLength + start;
        bool Ahead = UKismetSystemLibrary::LineTraceSingle(GetWorld(), start, end, UEngineTypes::ConvertToTraceType(ECollisionChannel::ECC_Visibility), false, ignores, EDrawDebugTrace::ForOneFrame, outHit, true);

        // Trace Behind
        end = (GetPawn()->GetActorForwardVector() * -1.0) * TraceLength + start;
        bool Behind = UKismetSystemLibrary::LineTraceSingle(GetWorld(), start, end, UEngineTypes::ConvertToTraceType(ECollisionChannel::ECC_Visibility), false, ignores, EDrawDebugTrace::ForOneFrame, outHit, true);

        // Trace Right
        end = (GetPawn()->GetActorRightVector()) * TraceLength + start;
        bool Right = UKismetSystemLibrary::LineTraceSingle(GetWorld(), start, end, UEngineTypes::ConvertToTraceType(ECollisionChannel::ECC_Visibility), false, ignores, EDrawDebugTrace::ForOneFrame, outHit, true);

        // Trace Left
        end = (GetPawn()->GetActorRightVector() * -1.0) * TraceLength + start;
        bool Left = UKismetSystemLibrary::LineTraceSingle(GetWorld(), start, end, UEngineTypes::ConvertToTraceType(ECollisionChannel::ECC_Visibility), false, ignores, EDrawDebugTrace::ForOneFrame, outHit, true);


        // Now we can calculate the need for avoidance
        if ((Left || Right))
        {
            AdjustSteeringForAvoidance = true;
            if ((!Ahead) || (Right && Left))
                SteeringAdjustment = 0.0f;
            else
                SteeringAdjustment = Right ? -1.0f : 1.0f;
        }

        if ((Ahead || Behind))
        {
            AdjustThrottleForAvoidance = true;
            if (Ahead && Behind)
                ThrottleAdjustment = 0.50;
            else
                ThrottleAdjustment = Behind ? 1.0 : 0.0;
        }
    }
}

float AVehicleController::CalculateSteeringAvoidance()
{
    if (DetectedVehicles.IsValidIndex(ClosestVehicleIdx))
    {
        auto AbsoluteAngle = FMath::Abs(DetectedVehicles[ClosestVehicleIdx].Angle);
        auto AngleSign = FMath::Sign<double>(DetectedVehicles[ClosestVehicleIdx].Angle);

        float AngleFactor = FMath::GetMappedRangeValueClamped(FVector2D(5.0f, 15.0f), FVector2D(0.0f, 1.0f), AbsoluteAngle);

        float SteeringMin = FMath::GetMappedRangeValueClamped(FVector2D(800.0f + AngleFactor * -400.0f, 1000.0f), FVector2D(55.0f, 15.0f), DetectedVehicles[ClosestVehicleIdx].Distance);
        float SteeringMax = FMath::GetMappedRangeValueClamped(FVector2D(300.0f, 800.0f), FVector2D(5.0f, 0.0f), DetectedVehicles[ClosestVehicleIdx].Distance);

        float ClampedAngle = FMath::GetMappedRangeValueClamped(FVector2D(5.0f, 15.0f), FVector2D(SteeringMin, SteeringMax), AbsoluteAngle);
        float CalculatedSteeringAngle = (ClampedAngle * AngleSign) * -1.0f;

        return FMath::GetMappedRangeValueClamped(FVector2D(0.0f, 45.0f), FVector2D(0.0f, 1.0f), FMath::Abs(CalculatedSteeringAngle)) * FMath::Sign<double>(CalculatedSteeringAngle);
    }

    return 0.0f;
}


void AVehicleController::PerformSteeringUpdate()
{
    if (TargetLoc.IsZero())
        return;

    auto a = (TargetLoc - GetPawn()->GetActorLocation()).GetSafeNormal(0.0001f);
    bool Forwards = (180.0) / UE_DOUBLE_PI * FMath::Acos(FVector::DotProduct(a, GetPawn()->GetActorForwardVector())) < 10.0;
    float RightAmt = (180.0) / UE_DOUBLE_PI * FMath::Acos(FVector::DotProduct(a, GetPawn()->GetActorRightVector()));
    float LeftAmt = (180.0) / UE_DOUBLE_PI * FMath::Acos(FVector::DotProduct(a, (GetPawn()->GetActorRightVector() * -1)));

    float SteeringInput = AdjustSteeringForAvoidance ? SteeringAdjustment :
        (Forwards ? 0.0 :
            RightAmt < LeftAmt ? 1.0 : -1.0) + CalculateSteeringAvoidance();

    WheeledMovementComponent->SetSteeringInput(SteeringInput);
}

void AVehicleController::UpdateCurrentTarget()
{
    if (!TargetWaypoint || !GetPawn())
        return;

    auto CurrentPawnLoc = GetPawn()->GetActorLocation();
    auto WaypointLoc = TargetWaypoint->GetActorLocation();

    if ((CurrentPawnLoc - WaypointLoc).Length() > 450.f)     // @todo: use defined radius
        return;
    else
    {
        // Set the new waypoint..
        int32 MaxIndex = Waypoints.Num();
        if (!Loop && CurrentWaypointIdx == MaxIndex)
            return;

        CurrentWaypointIdx = (CurrentWaypointIdx + 1) % MaxIndex;
        if (!Waypoints.IsValidIndex(CurrentWaypointIdx))
            TargetWaypoint = Waypoints[0];
        else
            TargetWaypoint = Waypoints[CurrentWaypointIdx];
    }
}


float AVehicleController::GetSteeringAngleAllWheels()
{
    TArray<float> Angles;
    for (auto& Wheel : WheeledMovementComponent->Wheels) {
        if (Wheel->GetSteerAngle() != 0)
            Angles.Add(Wheel->GetSteerAngle());
    }

    float Result = 0.f;
    if (Angles.IsValidIndex(0))
    {
        float Total = 0.0f;
        for (const auto& Angle : Angles)
            Total += Angle;

        Result = Total / static_cast<float>(Angles.Num());
    }
    return Result;
}

float AVehicleController::CalculateCurveAnticipation()
{
    int32 NextIndex = (CurrentWaypointIdx + 1) % Waypoints.Num();

    FQuat WheelRot = FQuat(UKismetMathLibrary::MakeRotator(0.0, 0.0, GetSteeringAngleAllWheels() * 1.0));
    FQuat CurrRot = FQuat(GetPawn()->GetActorRotation());

    FQuat CurrQuat = FQuat(FRotator(CurrRot * WheelRot).GetInverse());
    FQuat TargetQuat = FQuat(UKismetMathLibrary::FindLookAtRotation(TargetLoc, Waypoints[NextIndex]->GetActorLocation()));

    return FRotator(TargetQuat * CurrQuat).Yaw;
}

VehicleController.h

// Dylan E. - 2022

#pragma once
#include "CoreMinimal.h"
#include "AIController.h"

#include "Components/SphereComponent.h"

#include "WheeledVehiclePawn.h"
#include "ChaosVehicleMovementComponent.h"
#include "ChaosWheeledVehicleMovementComponent.h"

#include "VehicleController.generated.h"


/** Metadata for detected vehicles */
USTRUCT(BlueprintType)
struct FVehicleInfo
{
	GENERATED_BODY()
public:
	
	UPROPERTY(BlueprintReadWrite, EditAnywhere, meta = (DisplayName = "Distance"))
		float Distance = 0;

	UPROPERTY(BlueprintReadWrite, EditAnywhere, meta = (DisplayName = "Angle"))
		float Angle = 0;

	UPROPERTY(BlueprintReadWrite, EditInstanceOnly, meta = (DisplayName = "Vehicle"))
		TObjectPtr<AActor> Vehicle;
};

UENUM(BlueprintType)
enum StatusCode : uint8
{
	OK,
	MustReset,
	NoPathPossible,
	NoPathFound,
	GS_Invalid
};

UCLASS(Blueprintable, BlueprintType)
class AVehicleController : public AAIController
{
	GENERATED_BODY()
public:

	AVehicleController();


	UPROPERTY(BlueprintReadWrite, EditAnywhere, Category = "Vehicle Controller", meta = (MultiLine = "true"))
		USphereComponent* Collision;
		

	UPROPERTY(BlueprintReadWrite, EditAnywhere, Category = "Vehicle Controller", meta = (MultiLine = "true"))
		float CollisionRadius;
		
	UPROPERTY(BlueprintReadWrite, EditDefaultsOnly, Category = "Vehicle Controller", meta = (MultiLine = "true"))
		UChaosWheeledVehicleMovementComponent* WheeledMovementComponent;


	UPROPERTY(BlueprintReadWrite, EditDefaultsOnly, Category = "Vehicle Controller", meta = (MultiLine = "true"))
		AActor* TargetWaypoint;

	UPROPERTY(BlueprintReadWrite, EditDefaultsOnly, Category = "Vehicle Controller", meta = (MultiLine = "true"))
		FVector TargetLoc;

	UPROPERTY(BlueprintReadWrite, EditAnywhere, Category = "Vehicle Controller", meta = (MultiLine = "true", ExposeOnSpawn = "true"))
		TArray<TSubclassOf<AActor>> AvoidClasses;


	UPROPERTY(BlueprintReadWrite, EditInstanceOnly, Category = "Vehicle Controller", meta = (MultiLine = "true", ExposeOnSpawn = "true"))
		TArray<AActor*> Waypoints;

	UPROPERTY(BlueprintReadWrite, EditAnywhere, Category = "Vehicle Controller", meta = (MultiLine = "true"))
		TArray<FVehicleInfo> DetectedVehicles;

	UPROPERTY(BlueprintReadWrite, EditAnywhere, Category = "Vehicle Controller", meta = (MultiLine = "true"))
		int32 ClosestVehicleIdx;

	UPROPERTY(BlueprintReadWrite, EditAnywhere, Category = "Vehicle Controller", meta = (MultiLine = "true"))
		int32 InFrontVehicle;

	UPROPERTY(BlueprintReadWrite, EditAnywhere, Category = "Vehicle Controller", meta = (MultiLine = "true"))
		bool Loop = true;

	UPROPERTY(BlueprintReadWrite, EditAnywhere, Category = "Vehicle Controller", meta = (MultiLine = "true"))
		bool UseTraces = true;

	UPROPERTY(BlueprintReadWrite, EditAnywhere, Category = "Vehicle Controller", meta = (MultiLine = "true"))
		float TraceLength = 0.0f;

	UPROPERTY(BlueprintReadWrite, EditAnywhere, Category = "Vehicle Controller", meta = (MultiLine = "true"))
		bool AdjustSteeringForAvoidance = false;

	UPROPERTY(BlueprintReadWrite, EditAnywhere, Category = "Vehicle Controller", meta = (MultiLine = "true"))
		bool AdjustThrottleForAvoidance = false;

	UPROPERTY(BlueprintReadWrite, EditAnywhere, Category = "Vehicle Controller", meta = (MultiLine = "true"))
		float ThrottleAdjustment = 0.0f;

	UPROPERTY(BlueprintReadWrite, EditAnywhere, Category = "Vehicle Controller", meta = (MultiLine = "true"))
		float SteeringAdjustment = 0.0f;

	UPROPERTY(BlueprintReadWrite, EditAnywhere, Category = "Vehicle Controller", meta = (MultiLine = "true"))
		int32 CurrentWaypointIdx;

	UFUNCTION(BlueprintCallable, Category = "Vehicle Controller")
		void FindPathToTarget(UPARAM(ref) TEnumAsByte<StatusCode>& Status);

	UFUNCTION(BlueprintCallable, Category = "Vehicle Controller")
		void PerformVehicleDetection();

	UFUNCTION(BlueprintCallable, Category = "Vehicle Controller")
		void PerformAvoidanceTraces();

	UFUNCTION(BlueprintCallable, Category = "Vehicle Controller")
		void PerformSteeringUpdate();

	UFUNCTION(BlueprintCallable, Category = "Vehicle Controller")
		void UpdateCurrentTarget();

	UFUNCTION(BlueprintCallable, Category = "Vehicle Controller")
		float GetSteeringAngleAllWheels();

	UFUNCTION(BlueprintCallable, Category = "Vehicle Controller")
		float CalculateSteeringAvoidance();	

	UFUNCTION(BlueprintCallable, Category = "Vehicle Controller")
		float CalculateCurveAnticipation();


	UPROPERTY(BlueprintReadWrite, EditAnywhere, Category = "Vehicle Controller|Error Handling")
		int32 NoPathFoundCounter;
	
	UPROPERTY(BlueprintReadWrite, EditAnywhere, Category = "Vehicle Controller|Error Handling")
		int32 NoPathPossibleCounter;
	
	UPROPERTY(BlueprintReadWrite, EditAnywhere, Category = "Vehicle Controller|Error Handling")
		int32 LowVelocityCounter;
	
	UPROPERTY(BlueprintReadWrite, EditAnywhere, Category = "Vehicle Controller|Error Handling")
		int32 MaxNoPathPossible;
};