-
-
Save yateam/7ce5f43515978f7f172b53801205296d to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
// 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); | |
} |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
// 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