Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
// 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);
}
// 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;
};
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment