AIVehicleMovement.cpp

// Fill out your copyright notice in the Description page of Project Settings.


#include "AIVehicleMovement.h"
#include "DrawDebugHelpers.h"
#include "Engine/World.h"

float FPIDController::CalcNewInput(float Error, float Position)
{
	ErrorSum = FMath::Clamp(Error + ErrorSum, ErrorMin, ErrorMax);
	float Input = Error * Proportional + ErrorSum * Integral + Derivative * (LastPosition - Position);
	LastPosition = Position;
	return Input;
}

void UAIVehicleMovement::RequestDirectMove(const FVector& MoveVelocity, bool bForceMaxSpeed)
{
	Super::RequestDirectMove(MoveVelocity, bForceMaxSpeed);
	//UE_LOG(LogTemp, Log, TEXT("Vel: %s"), *MoveVelocity.ToCompactString());

	FVector VehicleLocation = GetOwner()->GetActorLocation();
	FVector Destination = VehicleLocation + MoveVelocity * GetWorld()->GetDeltaSeconds();
	//DrawDebugLine(GetWorld(), GetOwner()->GetActorLocation(), Destination, FColor::Red, false, 1.f, 0, 3.f);

	FVector Distance = Destination - VehicleLocation;
	FVector InitialDistance = Destination - InitialLocation;
	FVector VehicleDirection = GetOwner()->GetActorForwardVector();

	// Throttle controller
	float ForwardFactor = FVector::DotProduct(VehicleDirection, Distance.GetSafeNormal());
	
	float Error = Distance.Size() / InitialDistance.Size() * FMath::Sign(ForwardFactor);
	float Position = 1 - Error;

	float Input = ThrottleController.CalcNewInput(Error, Position);

	const float TURN_AROUND_FACTOR = bTurningAround ? 0.3f : 0.1f;
	if (ForwardFactor < TURN_AROUND_FACTOR && (bForceMaxSpeed || Distance.Size() > 300.f))
	{
		bTurningAround = true;
		SetThrottleInput(-1.f);
	}
	else
	{
		bTurningAround = false;
		SetThrottleInput(bForceMaxSpeed ? ForwardFactor : Input);
	}

	// Steering controller
	float InitialYaw = InitialDistance.Rotation().Yaw - InitialDirection.Rotation().Yaw;
	if (InitialYaw < -180)
	{
		InitialYaw += 360;
	}
	else if (InitialYaw > 180)
	{
		InitialYaw -= 360;
	}
	float CurrentYaw = Distance.Rotation().Yaw - VehicleDirection.Rotation().Yaw;
	if (CurrentYaw < -180)
	{
		CurrentYaw += 360;
	}
	else if (CurrentYaw > 180)
	{
		CurrentYaw -= 360;
	}

	float SteeringPosition = (-CurrentYaw + 180) / 180;
	float SteeringError = 1 - SteeringPosition;

	float SteeringInput = SteeringController.CalcNewInput(SteeringError, SteeringPosition);
	if (bTurningAround)
	{
		SetSteeringInput(SteeringError > 0 ? -1.f : 1.f);
	}
	else
	{
		SetSteeringInput(SteeringInput);
	}
	SetHandbrakeInput(false);
}

void UAIVehicleMovement::StopActiveMovement()
{
	Super::StopActiveMovement();
	InitialLocation = GetOwner()->GetActorLocation();
	InitialDirection = GetOwner()->GetActorForwardVector();
	SetHandbrakeInput(true);
	SetThrottleInput(0.f);
}

AIVehicleMovement.h

// Fill out your copyright notice in the Description page of Project Settings.

#pragma once

#include "CoreMinimal.h"
#include "WheeledVehicleMovementComponent4W.h"
#include "AIVehicleMovement.generated.h"

USTRUCT()
struct FPIDController
{
	GENERATED_BODY()

	UPROPERTY(EditAnywhere, Category = "PID")
	float Proportional;

	UPROPERTY(EditAnywhere, Category = "PID")
	float Integral;

	UPROPERTY(EditAnywhere, Category = "PID")
	float Derivative;

	UPROPERTY(EditAnywhere, Category = "PID")
	float ErrorMin;

	UPROPERTY(EditAnywhere, Category = "PID")
	float ErrorMax;

	float ErrorSum;
	float LastPosition;

	FPIDController() {}

	FPIDController(float P, float I, float D, float ErrorMin, float ErrorMax)
	{
		Proportional = P;
		Integral = I;
		Derivative = D;
		this->ErrorMin = ErrorMin;
		this->ErrorMax = ErrorMax;
	}

	float CalcNewInput(float Error, float Position);
};

/**
 * 
 */
UCLASS()
class VEHICLEAI_API UAIVehicleMovement : public UWheeledVehicleMovementComponent4W
{
	GENERATED_BODY()

	UPROPERTY(EditAnywhere, Category = "PID")
	FPIDController ThrottleController = FPIDController(0.f, 0.f, 0.f, 0.f, 0.f);

	UPROPERTY(EditAnywhere, Category = "PID")
	FPIDController SteeringController = FPIDController(0, 0, 0, 0, 0);

	FVector InitialLocation;
	FVector InitialDirection;
	bool bTurningAround = false;

public:
	virtual void RequestDirectMove(const FVector& MoveVelocity, bool bForceMaxSpeed) override;
	virtual void StopActiveMovement() override;
};