mirror of
https://github.com/litruv/TobiiEyetracking.git
synced 2026-07-25 11:16:10 +10:00
Added Tobii from 4.23 + updated
This commit is contained in:
@@ -0,0 +1,261 @@
|
||||
/******************************************************************************
|
||||
* Copyright 2017- Tobii Technology AB. All rights reserved.
|
||||
*
|
||||
* @author Temaran | Fredrik Lindh | fredrik.lindh@tobii.com | https://github.com/Temaran
|
||||
******************************************************************************/
|
||||
|
||||
#include "TobiiProjectileComponent.h"
|
||||
#include "TobiiGTOMBlueprintLibrary.h"
|
||||
#include "TobiiInteractionsBlueprintLibrary.h"
|
||||
|
||||
#include "Components/PrimitiveComponent.h"
|
||||
#include "Engine/World.h"
|
||||
#include "GameFramework/Actor.h"
|
||||
|
||||
UTobiiProjectileComponent::UTobiiProjectileComponent()
|
||||
: InitialVelocity(1.0f, 0.0f, 0.0f)
|
||||
, bAffectedByGravity(true)
|
||||
, WorldSpaceGravity(0.0f, 0.0f, 0.0f)
|
||||
, InitialFuelSecs(0.0f)
|
||||
, MaxLifeTimeSecs(0.0f)
|
||||
|
||||
, HomingBehavior(ETobiiProjectileHomingBehavior::Acceleration)
|
||||
, SteeringMaxTurnSpeedDegPerSec(10.0f)
|
||||
|
||||
, GuidanceSystem(ETobiiProjectileGuidanceSystem::ComplexPrediction)
|
||||
, GuidanceSystemUpdateFreq(0.0f)
|
||||
, GuidanceSystemMaximumTargetAngleDeg(0.0f)
|
||||
|
||||
, GuidanceSystemTarget()
|
||||
, AccelerationVectorTowardsTarget()
|
||||
, bTargetOffCourse(false)
|
||||
{
|
||||
bCanThrust = true;
|
||||
bUsesFuel = InitialFuelSecs > 0.0f;
|
||||
CurrentFuelSecs = InitialFuelSecs;
|
||||
CurrentLifeTime = 0.0f;
|
||||
GuidanceSystemUpdateTimerSecsLeft = 0.0f;
|
||||
LastTargetVelocity = FVector::ZeroVector;
|
||||
TargetAcceleration = FVector::ZeroVector;
|
||||
|
||||
HomingAccelerationMagnitude = 10000.0f;
|
||||
PrimaryComponentTick.bCanEverTick = true;
|
||||
}
|
||||
|
||||
void UTobiiProjectileComponent::BeginPlay()
|
||||
{
|
||||
Super::BeginPlay();
|
||||
|
||||
bCanThrust = true;
|
||||
bUsesFuel = InitialFuelSecs > 0.0f;
|
||||
CurrentFuelSecs = InitialFuelSecs;
|
||||
CurrentLifeTime = 0.0f;
|
||||
GuidanceSystemUpdateTimerSecsLeft = 0.0f;
|
||||
LastTargetVelocity = FVector::ZeroVector;
|
||||
TargetAcceleration = FVector::ZeroVector;
|
||||
if (WorldSpaceGravity.IsZero())
|
||||
{
|
||||
WorldSpaceGravity.Set(0.0f, 0.0f, GetGravityZ());
|
||||
}
|
||||
|
||||
//This is a copy from the base class InitializeComponent function. This should really be done at beginplay to let any instigator configure initial speed etc.
|
||||
if (InitialVelocity.SizeSquared() > 0.f)
|
||||
{
|
||||
FVector NewVelocity;
|
||||
if (InitialSpeed > 0.f)
|
||||
{
|
||||
NewVelocity = InitialVelocity.GetSafeNormal() * InitialSpeed;
|
||||
}
|
||||
else
|
||||
{
|
||||
NewVelocity = InitialVelocity;
|
||||
}
|
||||
|
||||
if (bInitialVelocityInLocalSpace)
|
||||
{
|
||||
SetVelocityInLocalSpace(NewVelocity);
|
||||
}
|
||||
|
||||
if (bRotationFollowsVelocity)
|
||||
{
|
||||
if (UpdatedComponent)
|
||||
{
|
||||
UpdatedComponent->SetWorldRotation(NewVelocity.Rotation());
|
||||
}
|
||||
}
|
||||
|
||||
UpdateComponentVelocity();
|
||||
|
||||
if (UpdatedPrimitive && UpdatedPrimitive->IsSimulatingPhysics())
|
||||
{
|
||||
UpdatedPrimitive->SetPhysicsLinearVelocity(NewVelocity);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void UTobiiProjectileComponent::TickComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction)
|
||||
{
|
||||
CurrentLifeTime += DeltaTime;
|
||||
|
||||
if (bUsesFuel && bCanThrust)
|
||||
{
|
||||
CurrentFuelSecs -= DeltaTime;
|
||||
if (CurrentFuelSecs <= 0.0f)
|
||||
{
|
||||
CurrentFuelSecs = 0.0f;
|
||||
bCanThrust = false;
|
||||
}
|
||||
}
|
||||
|
||||
bTargetOffCourse = false;
|
||||
if(GuidanceSystemMaximumTargetAngleDeg > FLT_EPSILON && HomingTargetComponent.IsValid())
|
||||
{
|
||||
FVector TargetFocusPosition;
|
||||
UTobiiGTOMBlueprintLibrary::GetPrimitiveComponentFocusLocation(HomingTargetComponent.Get(), TargetFocusPosition);
|
||||
|
||||
//If our target is too far off course, disable thrust
|
||||
FVector DeltaVector = TargetFocusPosition - GetOwner()->GetActorLocation();
|
||||
FVector MyDirection = Velocity;
|
||||
|
||||
bool bHasValidVectors = DeltaVector.Normalize();
|
||||
bHasValidVectors = bHasValidVectors && MyDirection.Normalize();
|
||||
|
||||
if (bHasValidVectors)
|
||||
{
|
||||
float SeparationAngle = FMath::RadiansToDegrees(FMath::Acos(FVector::DotProduct(DeltaVector, MyDirection)));
|
||||
if (SeparationAngle > GuidanceSystemMaximumTargetAngleDeg)
|
||||
{
|
||||
bTargetOffCourse = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (HomingTargetComponent.IsValid() && !bTargetOffCourse)
|
||||
{
|
||||
TargetAcceleration = HomingTargetComponent->GetComponentVelocity() - LastTargetVelocity;
|
||||
LastTargetVelocity = HomingTargetComponent->GetComponentVelocity();
|
||||
|
||||
if (GuidanceSystemUpdateFreq == 0.0f)
|
||||
{
|
||||
SimulateGuidanceSystem();
|
||||
}
|
||||
else
|
||||
{
|
||||
GuidanceSystemUpdateTimerSecsLeft -= DeltaTime;
|
||||
if (GuidanceSystemUpdateTimerSecsLeft <= 0.0f)
|
||||
{
|
||||
GuidanceSystemUpdateTimerSecsLeft = 1.0f / GuidanceSystemUpdateFreq;
|
||||
SimulateGuidanceSystem();
|
||||
}
|
||||
}
|
||||
|
||||
//Steering behavior
|
||||
if (bIsHomingProjectile && bCanThrust && HomingBehavior == ETobiiProjectileHomingBehavior::Steering)
|
||||
{
|
||||
FVector ForwardDir = Velocity;
|
||||
FVector TowardsTarget = GuidanceSystemTarget - GetOwner()->GetActorLocation();
|
||||
bool bHasValidVectors = ForwardDir.Normalize();
|
||||
bHasValidVectors = bHasValidVectors && TowardsTarget.Normalize();
|
||||
|
||||
if (bHasValidVectors)
|
||||
{
|
||||
float AngularDifferenceRad = FMath::Acos(FVector::DotProduct(ForwardDir, TowardsTarget));
|
||||
float AlphaStep = FMath::Clamp(FMath::DegreesToRadians(SteeringMaxTurnSpeedDegPerSec) * DeltaTime / AngularDifferenceRad, 0.0f, 1.0f);
|
||||
FVector NewVelocityDirection = FQuat::Slerp(ForwardDir.ToOrientationQuat(), TowardsTarget.ToOrientationQuat(), AlphaStep).GetForwardVector();
|
||||
Velocity = NewVelocityDirection * Velocity.Size();
|
||||
UpdateComponentVelocity();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Super::TickComponent(DeltaTime, TickType, ThisTickFunction);
|
||||
|
||||
if (MaxLifeTimeSecs > 0.0f && CurrentLifeTime > MaxLifeTimeSecs)
|
||||
{
|
||||
GetOwner()->Destroy();
|
||||
}
|
||||
}
|
||||
|
||||
FVector UTobiiProjectileComponent::ComputeAcceleration(const FVector& InVelocity, float DeltaTime) const
|
||||
{
|
||||
FVector Acceleration(FVector::ZeroVector);
|
||||
|
||||
if (bAffectedByGravity)
|
||||
{
|
||||
Acceleration += WorldSpaceGravity;
|
||||
}
|
||||
|
||||
if (bCanThrust && !bTargetOffCourse && bIsHomingProjectile && HomingTargetComponent.IsValid() && HomingBehavior == ETobiiProjectileHomingBehavior::Acceleration)
|
||||
{
|
||||
Acceleration += AccelerationVectorTowardsTarget;
|
||||
}
|
||||
|
||||
return Acceleration;
|
||||
}
|
||||
|
||||
void UTobiiProjectileComponent::SimulateGuidanceSystem()
|
||||
{
|
||||
FVector TargetFocusPosition;
|
||||
UTobiiGTOMBlueprintLibrary::GetPrimitiveComponentFocusLocation(HomingTargetComponent.Get(), TargetFocusPosition);
|
||||
|
||||
switch (GuidanceSystem)
|
||||
{
|
||||
case ETobiiProjectileGuidanceSystem::Prediction:
|
||||
case ETobiiProjectileGuidanceSystem::ComplexPrediction:
|
||||
{
|
||||
FTobiiAccelerationBasedHomingData InterceptData;
|
||||
InterceptData.ProjectilePosition = GetOwner()->GetActorLocation();
|
||||
InterceptData.ProjectileVelocity = Velocity;
|
||||
InterceptData.ProjectileAccelerationMagnitude = HomingAccelerationMagnitude;
|
||||
InterceptData.TargetPosition = TargetFocusPosition;
|
||||
InterceptData.TargetVelocity = HomingTargetComponent->GetComponentVelocity();
|
||||
InterceptData.TargetAcceleration = TargetAcceleration;
|
||||
InterceptData.bAttemptClosestApproachSolution = GuidanceSystem == ETobiiProjectileGuidanceSystem::ComplexPrediction;
|
||||
|
||||
FTobiiAccelerationBasedHomingResult HomingResult;
|
||||
if (UTobiiInteractionsBlueprintLibrary::FindNeededAccelerationForAccelerationBasedHomingProjectile(InterceptData, HomingResult))
|
||||
{
|
||||
GuidanceSystemTarget = HomingResult.ExpectedInterceptLocation;
|
||||
AccelerationVectorTowardsTarget = HomingResult.SuggestedAcceleration;
|
||||
}
|
||||
else
|
||||
{
|
||||
GuidanceSystemTarget = TargetFocusPosition;
|
||||
AccelerationVectorTowardsTarget = (GuidanceSystemTarget - UpdatedComponent->GetComponentLocation()).GetSafeNormal() * HomingAccelerationMagnitude;
|
||||
}
|
||||
break;
|
||||
}
|
||||
case ETobiiProjectileGuidanceSystem::Simple:
|
||||
default:
|
||||
{
|
||||
GuidanceSystemTarget = TargetFocusPosition;
|
||||
AccelerationVectorTowardsTarget = (GuidanceSystemTarget - UpdatedComponent->GetComponentLocation()).GetSafeNormal() * HomingAccelerationMagnitude;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void UTobiiProjectileComponent::StartHoming(USceneComponent* NewHomingTargetComponent)
|
||||
{
|
||||
HomingTargetComponent = NewHomingTargetComponent;
|
||||
if (HomingTargetComponent != nullptr)
|
||||
{
|
||||
bIsHomingProjectile = true;
|
||||
}
|
||||
|
||||
bSimulationEnabled = true;
|
||||
if (UpdatedComponent == nullptr)
|
||||
{
|
||||
UpdatedComponent = GetOwner()->GetRootComponent();
|
||||
}
|
||||
if (Cast<UPrimitiveComponent>(UpdatedComponent) != nullptr)
|
||||
{
|
||||
Cast<UPrimitiveComponent>(UpdatedComponent)->SetSimulatePhysics(false);
|
||||
}
|
||||
PrimaryComponentTick.SetTickFunctionEnable(true);
|
||||
}
|
||||
|
||||
void UTobiiProjectileComponent::StopHoming()
|
||||
{
|
||||
bIsHomingProjectile = false;
|
||||
HomingTargetComponent = nullptr;
|
||||
}
|
||||
@@ -0,0 +1,466 @@
|
||||
/******************************************************************************
|
||||
* Copyright 2017- Tobii Technology AB. All rights reserved.
|
||||
*
|
||||
* @author Temaran | Fredrik Lindh | fredrik.lindh@tobii.com | https://github.com/Temaran
|
||||
******************************************************************************/
|
||||
|
||||
#include "TobiiThrowAtGazeComponent.h"
|
||||
#include "TobiiInteractionsBlueprintLibrary.h"
|
||||
#include "../TobiiInteractionsInternalTypes.h"
|
||||
|
||||
#include "IEyeTracker.h"
|
||||
#include "DrawDebugHelpers.h"
|
||||
#include "Camera/CameraComponent.h"
|
||||
#include "Engine/Engine.h"
|
||||
#include "Engine/World.h"
|
||||
#include "HAL/IConsoleManager.h"
|
||||
|
||||
static TAutoConsoleVariable<int32> CVarThrowAtGazeEnabled(TEXT("tobii.interaction.ThrowAtGazeEnabled"), 1, TEXT("Throw where your are looking."));
|
||||
static TAutoConsoleVariable<int32> CVarThrowAtGazeDebug(TEXT("tobii.debug.ThrowAtGaze"), 1, TEXT("Draw throw at gaze debug data."));
|
||||
|
||||
UTobiiThrowAtGazeComponent::UTobiiThrowAtGazeComponent()
|
||||
: ThrowTarget(nullptr)
|
||||
, MaxThrowSpeedCmPerSecs(1000.0f)
|
||||
|
||||
, MaxIterations(5)
|
||||
, bAlwaysPreferLowApex(false)
|
||||
, bPreferLowApexForTargetsBelowThrower(true)
|
||||
, bShouldTraceResult(true)
|
||||
, ThrowApexOffsetMinimumCm(10.0f)
|
||||
, ThrowApexOffsetMaximumCm(500.0f)
|
||||
, MinimumApexDeltaCm(10.0f)
|
||||
, TraceRadiusCm(1.0f)
|
||||
, MaxNrTraceSteps(50)
|
||||
, TraceStepSizeSecs(0.1f)
|
||||
, MaxTraceDistance(10000.0f)
|
||||
, NoTargetAcceptanceThreshold(10.0f)
|
||||
, TraceChannel(ECC_Visibility)
|
||||
, TraceIgnoreActors()
|
||||
, bShouldTraceIgnoreOwner(true)
|
||||
|
||||
, bUseCustomGravity(false)
|
||||
, CustomProjectileGravity(FVector::ZeroVector)
|
||||
|
||||
, LastTargetVelocity(FVector::ZeroVector)
|
||||
, CalculatedTargetAcceleration(FVector::ZeroVector)
|
||||
{
|
||||
}
|
||||
|
||||
FVector UTobiiThrowAtGazeComponent::CalculateProjectileGravityVector()
|
||||
{
|
||||
return bUseCustomGravity ? CustomProjectileGravity : FVector(0.0f, 0.0f, GetWorld()->GetGravityZ());
|
||||
}
|
||||
|
||||
bool UTobiiThrowAtGazeComponent::ThrowAtGazeAvailable()
|
||||
{
|
||||
return UTobiiInteractionsBlueprintLibrary::IsThrowAtGazeEnabled();
|
||||
}
|
||||
|
||||
bool UTobiiThrowAtGazeComponent::TraceBallisticProjectilePath(FVector ProjectileOrigin, FVector ProjectileInitialVelocity, TArray<FVector>& OutTracedPath)
|
||||
{
|
||||
TArray<AActor*> IgnoredActors = TraceIgnoreActors;
|
||||
if (bShouldTraceIgnoreOwner)
|
||||
{
|
||||
IgnoredActors.Add(GetOwner());
|
||||
}
|
||||
|
||||
FTobiiProjectileTraceData TraceData;
|
||||
TraceData.TraceChannel = TraceChannel;
|
||||
TraceData.IgnoredActors = IgnoredActors;
|
||||
TraceData.MaxNrSteps = MaxNrTraceSteps;
|
||||
TraceData.StepSizeSecs = TraceStepSizeSecs;
|
||||
TraceData.TraceRadiusCm = TraceRadiusCm;
|
||||
TraceData.ProjectileInitialPosition = ProjectileOrigin;
|
||||
TraceData.ProjectileVelocity = ProjectileInitialVelocity;
|
||||
TraceData.ProjectileAcceleration = CalculateProjectileGravityVector();
|
||||
|
||||
FHitResult HitResult;
|
||||
return UTobiiInteractionsBlueprintLibrary::TraceBallisticProjectilePath(GetWorld(), TraceData, OutTracedPath, HitResult);
|
||||
}
|
||||
|
||||
void UTobiiThrowAtGazeComponent::TickComponent(float DeltaTime, enum ELevelTick TickType, FActorComponentTickFunction *ThisTickFunction)
|
||||
{
|
||||
if (ThrowTarget.IsValid())
|
||||
{
|
||||
const float AccelerationLerpSpeed = 0.7f;
|
||||
FVector CurrentTargetVelocity = ThrowTarget->GetOwner()->GetVelocity();
|
||||
CalculatedTargetAcceleration = FMath::Lerp(CalculatedTargetAcceleration, CurrentTargetVelocity - LastTargetVelocity, AccelerationLerpSpeed);
|
||||
LastTargetVelocity = CurrentTargetVelocity;
|
||||
}
|
||||
}
|
||||
|
||||
FVector UTobiiThrowAtGazeComponent::CorrectThrow(const FVector& ThrowOrigin, const FVector& OriginalThrowVector, float ThrowAngleThresholdDeg, float ThrowSpeedThreshold)
|
||||
{
|
||||
FTobiiBallisticResult BallisticResult;
|
||||
TArray<FVector> TracedPath;
|
||||
ETobiiThrowAtGazeResult Result = CalculateThrowAtGazeVector(ThrowOrigin, BallisticResult, TracedPath);
|
||||
|
||||
switch (Result)
|
||||
{
|
||||
case ETobiiThrowAtGazeResult::OutOfRange:
|
||||
case ETobiiThrowAtGazeResult::DirectHit:
|
||||
{
|
||||
if (ThrowAngleThresholdDeg > FLT_EPSILON)
|
||||
{
|
||||
FVector SuggestedDirection = BallisticResult.SuggestedInitialVelocity;
|
||||
FVector OriginalDirection = OriginalThrowVector;
|
||||
bool bHasValidVectors = SuggestedDirection.Normalize();
|
||||
bHasValidVectors = bHasValidVectors && OriginalDirection.Normalize();
|
||||
|
||||
if (bHasValidVectors)
|
||||
{
|
||||
float SeparationDot = FVector::DotProduct(SuggestedDirection, OriginalDirection);
|
||||
float SeparationAngleDeg = FMath::RadiansToDegrees(FMath::Acos(SeparationDot));
|
||||
if (SeparationAngleDeg > ThrowAngleThresholdDeg)
|
||||
{
|
||||
static const auto DrawDebugCVar = IConsoleManager::Get().FindConsoleVariable(TEXT("tobii.debug"));
|
||||
if (DrawDebugCVar->GetInt() && CVarThrowAtGazeDebug.GetValueOnGameThread())
|
||||
{
|
||||
UE_LOG(LogTemp, Warning, TEXT("Throw diverged too much. Separation angle was: %f"), SeparationAngleDeg);
|
||||
}
|
||||
|
||||
return OriginalThrowVector;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (ThrowSpeedThreshold > FLT_EPSILON)
|
||||
{
|
||||
float SuggestedSpeedSq = BallisticResult.SuggestedInitialVelocity.SizeSquared();
|
||||
float OriginalSpeedSq = OriginalThrowVector.SizeSquared();
|
||||
float ThresholdSq = ThrowSpeedThreshold * ThrowSpeedThreshold;
|
||||
|
||||
//Only invalidate too weak throws
|
||||
if (OriginalSpeedSq + ThresholdSq < SuggestedSpeedSq)
|
||||
{
|
||||
static const auto DrawDebugCVar = IConsoleManager::Get().FindConsoleVariable(TEXT("tobii.debug"));
|
||||
if (DrawDebugCVar->GetInt() && CVarThrowAtGazeDebug.GetValueOnGameThread())
|
||||
{
|
||||
UE_LOG(LogTemp, Warning, TEXT("Throw was too weak. Throw speed was: %f. Suggested speed was: %f"), OriginalThrowVector.Size(), BallisticResult.SuggestedInitialVelocity.Size());
|
||||
}
|
||||
|
||||
return OriginalThrowVector;
|
||||
}
|
||||
}
|
||||
|
||||
return BallisticResult.SuggestedInitialVelocity;
|
||||
}
|
||||
|
||||
default:
|
||||
{
|
||||
return OriginalThrowVector;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
ETobiiThrowAtGazeResult UTobiiThrowAtGazeComponent::CalculateThrowAtGazeVector(const FVector& ThrowOrigin, FTobiiBallisticResult& OutBallisticResult, TArray<FVector>& OutTracedPath)
|
||||
{
|
||||
UWorld* World = GetWorld();
|
||||
if (World == nullptr || GEngine == nullptr || !GEngine->EyeTrackingDevice.IsValid())
|
||||
{
|
||||
return ETobiiThrowAtGazeResult::UnknownError;
|
||||
}
|
||||
|
||||
if (ThrowTarget.IsValid())
|
||||
{
|
||||
return CalculateThrowArc(ThrowOrigin, ThrowTarget->GetCenterOfMass(), ThrowTarget->GetOwner()->GetVelocity(), CalculatedTargetAcceleration, OutBallisticResult, OutTracedPath);
|
||||
}
|
||||
else if (World->GetFirstPlayerController() != nullptr && World->GetFirstPlayerController()->PlayerCameraManager != nullptr)
|
||||
{
|
||||
FVector GazeTargetPosition;
|
||||
FVector GazeRayOrigin = FVector::ZeroVector;
|
||||
FVector GazeRayDirection = FVector::ZeroVector;
|
||||
|
||||
FEyeTrackerGazeData CombinedGazeData;
|
||||
GEngine->EyeTrackingDevice->GetEyeTrackerGazeData(CombinedGazeData);
|
||||
if (CombinedGazeData.ConfidenceValue > 0.5f)
|
||||
{
|
||||
GazeRayOrigin = CombinedGazeData.GazeOrigin;
|
||||
GazeRayDirection = CombinedGazeData.GazeDirection;
|
||||
}
|
||||
else
|
||||
{
|
||||
// If eyetracking data is not available this frame, just use the center of the screen
|
||||
GazeRayOrigin = World->GetFirstPlayerController()->PlayerCameraManager->GetCameraLocation();
|
||||
GazeRayDirection = World->GetFirstPlayerController()->PlayerCameraManager->GetCameraRotation().Vector();
|
||||
}
|
||||
|
||||
FVector EndPoint = GazeRayOrigin + GazeRayDirection * MaxTraceDistance;
|
||||
FHitResult HitResult;
|
||||
if (World->LineTraceSingleByChannel(HitResult, GazeRayOrigin, EndPoint, TraceChannel))
|
||||
{
|
||||
GazeTargetPosition = HitResult.Location;
|
||||
}
|
||||
else
|
||||
{
|
||||
GazeTargetPosition = EndPoint;
|
||||
}
|
||||
|
||||
return CalculateThrowArc(ThrowOrigin, GazeTargetPosition, FVector::ZeroVector, FVector::ZeroVector, OutBallisticResult, OutTracedPath);
|
||||
}
|
||||
|
||||
return ETobiiThrowAtGazeResult::NoEyetrackingInput;
|
||||
}
|
||||
|
||||
ETobiiThrowAtGazeResult UTobiiThrowAtGazeComponent::CalculateThrowArc(const FVector& ThrowOrigin, const FVector& TargetLocation, const FVector& TargetVelocity, const FVector& TargetAcceleration, FTobiiBallisticResult& OutBallisticResult, TArray<FVector>& OutTracedPath)
|
||||
{
|
||||
OutBallisticResult = FTobiiBallisticResult();
|
||||
OutTracedPath = TArray<FVector>();
|
||||
if (GetWorld() == nullptr)
|
||||
{
|
||||
return ETobiiThrowAtGazeResult::UnknownError;
|
||||
}
|
||||
|
||||
static const auto DrawDebugCVar = IConsoleManager::Get().FindConsoleVariable(TEXT("tobii.debug"));
|
||||
const FVector ProjectileAcceleration = CalculateProjectileGravityVector();
|
||||
const float BaseAcceptanceThreshold = TraceRadiusCm + NoTargetAcceptanceThreshold;
|
||||
const float TraceAcceptanceThresholdSq = BaseAcceptanceThreshold * BaseAcceptanceThreshold;
|
||||
|
||||
bool bShouldDrawDebug = DrawDebugCVar->GetInt() && CVarThrowAtGazeDebug.GetValueOnGameThread();
|
||||
if (bShouldDrawDebug)
|
||||
{
|
||||
DrawDebugSphere(GetWorld(), TargetLocation, 20.0f, 16, FColor::Yellow, false, 5.0f);
|
||||
}
|
||||
|
||||
FTobiiBallisticData BallisticData;
|
||||
BallisticData.ProjectileInitialPosition = ThrowOrigin;
|
||||
BallisticData.ProjectileAcceleration = ProjectileAcceleration;
|
||||
BallisticData.TargetPosition = TargetLocation;
|
||||
BallisticData.TargetVelocity = TargetVelocity;
|
||||
BallisticData.TargetAcceleration = TargetAcceleration;
|
||||
|
||||
TArray<AActor*> IgnoredActors = TraceIgnoreActors;
|
||||
if (bShouldTraceIgnoreOwner)
|
||||
{
|
||||
IgnoredActors.Add(GetOwner());
|
||||
}
|
||||
|
||||
FTobiiProjectileTraceData TraceData;
|
||||
TraceData.TraceChannel = TraceChannel;
|
||||
TraceData.IgnoredActors = IgnoredActors;
|
||||
TraceData.MaxNrSteps = MaxNrTraceSteps;
|
||||
TraceData.StepSizeSecs = TraceStepSizeSecs;
|
||||
TraceData.TraceRadiusCm = TraceRadiusCm;
|
||||
TraceData.ProjectileInitialPosition = ThrowOrigin;
|
||||
TraceData.ProjectileAcceleration = ProjectileAcceleration;
|
||||
|
||||
TArray<float> ApexesToTest;
|
||||
if (MaxIterations > 0)
|
||||
{
|
||||
//We want to first decide which apexes we should test, and then we can use logic to suss out which one ends up being the best.
|
||||
int32 Iterations = FMath::Max(0, MaxIterations);
|
||||
float ApexStep = (ThrowApexOffsetMaximumCm - ThrowApexOffsetMinimumCm) / Iterations;
|
||||
for (int32 ApexIdx = 0; ApexIdx <= Iterations; ApexIdx++)
|
||||
{
|
||||
float CurrentApex = ThrowApexOffsetMinimumCm + ApexIdx * ApexStep;
|
||||
ApexesToTest.Add(CurrentApex);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
ApexesToTest.Add((ThrowApexOffsetMaximumCm + ThrowApexOffsetMinimumCm) / 2);
|
||||
}
|
||||
|
||||
// Loop variables
|
||||
float BestResultDistanceToTargetSq = FLT_MAX;
|
||||
ETobiiThrowAtGazeResult BestResultStatus = ETobiiThrowAtGazeResult::NoPath;
|
||||
while (ApexesToTest.Num() > 0)
|
||||
{
|
||||
// Pick the next apex
|
||||
int32 SelectedApexIndex;
|
||||
if (bAlwaysPreferLowApex || (bPreferLowApexForTargetsBelowThrower && TargetLocation.Z < ThrowOrigin.Z))
|
||||
{
|
||||
SelectedApexIndex = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
SelectedApexIndex = (int32)(ApexesToTest.Num() / 2.0f);
|
||||
}
|
||||
SelectedApexIndex = FMath::Clamp(SelectedApexIndex, 0, ApexesToTest.Num() - 1);
|
||||
BallisticData.ProjectileApexOffsetCm = ApexesToTest[SelectedApexIndex];
|
||||
ApexesToTest.RemoveAt(SelectedApexIndex);
|
||||
|
||||
// Find initial velocity for given apex
|
||||
TArray<FTobiiBallisticResult> Results;
|
||||
if (UTobiiInteractionsBlueprintLibrary::FindNeededInitialVelocityForBallisticProjectile(BallisticData, Results))
|
||||
{
|
||||
for (FTobiiBallisticResult& Result : Results)
|
||||
{
|
||||
float SuggestedInitialSpeed = Result.SuggestedInitialVelocity.Size();
|
||||
if (SuggestedInitialSpeed < FLT_EPSILON)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
bool bIsInRange = SuggestedInitialSpeed < MaxThrowSpeedCmPerSecs;
|
||||
if (!bIsInRange)
|
||||
{
|
||||
Result.SuggestedInitialVelocity.Normalize();
|
||||
Result.SuggestedInitialVelocity *= MaxThrowSpeedCmPerSecs;
|
||||
}
|
||||
|
||||
TArray<FVector> TracedPath;
|
||||
bool bWayIsClear = true;
|
||||
float DistanceToTargetSq = FLT_MAX;
|
||||
if (bShouldTraceResult)
|
||||
{
|
||||
TraceData.ProjectileVelocity = Result.SuggestedInitialVelocity;
|
||||
|
||||
FHitResult HitResult;
|
||||
if (UTobiiInteractionsBlueprintLibrary::TraceBallisticProjectilePath(GetWorld(), TraceData, TracedPath, HitResult))
|
||||
{
|
||||
FVector HitToTarget = TargetLocation - HitResult.Location;
|
||||
float ToTargetDistSq = HitToTarget.SizeSquared();
|
||||
|
||||
if (ThrowTarget.IsValid())
|
||||
{
|
||||
if (HitResult.Actor != ThrowTarget->GetOwner())
|
||||
{
|
||||
bWayIsClear = false; // If we have a target, and hit something, that's okay as long as it's our target.
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
bWayIsClear = ToTargetDistSq < TraceAcceptanceThresholdSq;
|
||||
}
|
||||
|
||||
DistanceToTargetSq = ToTargetDistSq;
|
||||
}
|
||||
|
||||
if (bShouldDrawDebug)
|
||||
{
|
||||
if (TracedPath.Num() > 1)
|
||||
{
|
||||
FColor DrawColor = bWayIsClear ? FColor::Green : FColor::Red;
|
||||
FVector PrevPoint = TracedPath[0];
|
||||
int32 MiddlePointIdx = (int32)(TracedPath.Num() / 2.0f);
|
||||
if (MiddlePointIdx >= 0 && MiddlePointIdx < TracedPath.Num())
|
||||
{
|
||||
DrawDebugString(GetWorld(), TracedPath[MiddlePointIdx] + FVector(0.0f, 0.0f, 10.0f), FString::Printf(TEXT("%f"), BallisticData.ProjectileApexOffsetCm), nullptr, DrawColor, 5.0f);
|
||||
}
|
||||
for (auto& TracePoint : TracedPath)
|
||||
{
|
||||
DrawDebugLine(GetWorld(), PrevPoint, TracePoint, DrawColor, false, 5.0f, 0, 1.0f);
|
||||
PrevPoint = TracePoint;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (bWayIsClear)
|
||||
{
|
||||
if (bIsInRange)
|
||||
{
|
||||
//We're done! Best outcome.
|
||||
OutBallisticResult = Result;
|
||||
OutTracedPath = TracedPath;
|
||||
BestResultDistanceToTargetSq = DistanceToTargetSq;
|
||||
BestResultStatus = ETobiiThrowAtGazeResult::DirectHit;
|
||||
break;
|
||||
}
|
||||
else if (BestResultStatus <= ETobiiThrowAtGazeResult::OutOfRange)
|
||||
{
|
||||
bool bIsFaster = Result.ExpectedInterceptTimeSecs < OutBallisticResult.ExpectedInterceptTimeSecs;
|
||||
if (bIsFaster)
|
||||
{
|
||||
OutBallisticResult = Result;
|
||||
OutTracedPath = TracedPath;
|
||||
BestResultDistanceToTargetSq = DistanceToTargetSq;
|
||||
BestResultStatus = ETobiiThrowAtGazeResult::OutOfRange;
|
||||
}
|
||||
}
|
||||
}
|
||||
else if (TracedPath.Num() > 0)
|
||||
{
|
||||
if (bIsInRange)
|
||||
{
|
||||
if (BestResultStatus <= ETobiiThrowAtGazeResult::BlockedByWorldInRange)
|
||||
{
|
||||
bool bIsCloser = DistanceToTargetSq < BestResultDistanceToTargetSq;
|
||||
if (bIsCloser)
|
||||
{
|
||||
OutBallisticResult = Result;
|
||||
OutTracedPath = TracedPath;
|
||||
BestResultDistanceToTargetSq = DistanceToTargetSq;
|
||||
BestResultStatus = ETobiiThrowAtGazeResult::BlockedByWorldInRange;
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (BestResultStatus <= ETobiiThrowAtGazeResult::BlockedByWorldAndOutOfRange)
|
||||
{
|
||||
bool bIsCloser = DistanceToTargetSq < BestResultDistanceToTargetSq;
|
||||
if (bIsCloser)
|
||||
{
|
||||
OutBallisticResult = Result;
|
||||
OutTracedPath = TracedPath;
|
||||
BestResultDistanceToTargetSq = DistanceToTargetSq;
|
||||
BestResultStatus = ETobiiThrowAtGazeResult::BlockedByWorldAndOutOfRange;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (BestResultStatus == ETobiiThrowAtGazeResult::DirectHit)
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
//Use what we have learned to prune apexes for future loops
|
||||
if (Results.Num() == 0)
|
||||
{
|
||||
//If we didn't have any solutions or we are out of range, it means the apex is too low. Clear all apexes that are smaller than our current.
|
||||
for (int32 CurrentApexIdx = 0; CurrentApexIdx < ApexesToTest.Num(); CurrentApexIdx++)
|
||||
{
|
||||
if (ApexesToTest[CurrentApexIdx] <= BallisticData.ProjectileApexOffsetCm)
|
||||
{
|
||||
ApexesToTest.RemoveAt(CurrentApexIdx);
|
||||
CurrentApexIdx--;
|
||||
}
|
||||
}
|
||||
}
|
||||
else if (BestResultStatus == ETobiiThrowAtGazeResult::OutOfRange)
|
||||
{
|
||||
//If we are out of range, we need to select an apex closer to a 45 degree throw angle.
|
||||
//@TODO: Should we maybe do a solve for throw force given 45 degree throw angle here to optimize?
|
||||
FVector VelNorm = OutBallisticResult.SuggestedInitialVelocity;
|
||||
FVector ProjectedToXY = FVector(VelNorm.X, VelNorm.Y, 0.0f);
|
||||
bool bHasValidVectors = VelNorm.Normalize();
|
||||
bHasValidVectors = bHasValidVectors && ProjectedToXY.Normalize();
|
||||
if (bHasValidVectors)
|
||||
{
|
||||
float ThrowAngleCos = FVector::DotProduct(VelNorm, ProjectedToXY);
|
||||
|
||||
//1 means it is along the ground, 0 is straight up. So if the value is between 0.5 and 1 we need to aim higher and if it's between 0 and 0.5 we need to go lower
|
||||
if (ThrowAngleCos < 0.5f)
|
||||
{
|
||||
for (int32 CurrentApexIdx = 0; CurrentApexIdx < ApexesToTest.Num(); CurrentApexIdx++)
|
||||
{
|
||||
if (ApexesToTest[CurrentApexIdx] >= BallisticData.ProjectileApexOffsetCm)
|
||||
{
|
||||
ApexesToTest.RemoveAt(CurrentApexIdx);
|
||||
CurrentApexIdx--;
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
for (int32 CurrentApexIdx = 0; CurrentApexIdx < ApexesToTest.Num(); CurrentApexIdx++)
|
||||
{
|
||||
if (ApexesToTest[CurrentApexIdx] <= BallisticData.ProjectileApexOffsetCm)
|
||||
{
|
||||
ApexesToTest.RemoveAt(CurrentApexIdx);
|
||||
CurrentApexIdx--;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Output results
|
||||
return BestResultStatus;
|
||||
}
|
||||
@@ -0,0 +1,141 @@
|
||||
/******************************************************************************
|
||||
* Copyright 2017- Tobii Technology AB. All rights reserved.
|
||||
*
|
||||
* @author Temaran | Fredrik Lindh | fredrik.lindh@tobii.com | https://github.com/Temaran
|
||||
******************************************************************************/
|
||||
|
||||
#include "TobiiAimAtGazeComponent.h"
|
||||
#include "TobiiInteractionsBlueprintLibrary.h"
|
||||
#include "TobiiInteractionsInternalTypes.h"
|
||||
#include "TobiiGTOMBlueprintLibrary.h"
|
||||
|
||||
#include "DrawDebugHelpers.h"
|
||||
#include "Engine/World.h"
|
||||
#include "GameFramework/Pawn.h"
|
||||
#include "GameFramework/PlayerController.h"
|
||||
#include "HAL/IConsoleManager.h"
|
||||
|
||||
static TAutoConsoleVariable<int32> CVarAimAtGazeEnabled(TEXT("tobii.interaction.AimAtGazeEnabled"), 1, TEXT("Aim@gaze will rotate the view to align with the gaze point when the user triggers it, usually by entering ADS (Aim Down Sights). 0 - Aim@gaze is off. 1 - Aim@gaze is on."));
|
||||
static TAutoConsoleVariable<float> CVarAimAtGazeDoneThreshold(TEXT("tobii.interaction.AimAtGazeDoneThreshold"), 0.0001, TEXT("If the dot product between the current and target direction quats is smaller than this, we are done rotating."));
|
||||
|
||||
static TAutoConsoleVariable<int32> CVarAimAtGazeDebug(TEXT("tobii.debug.AimAtGaze"), 1, TEXT("0 - Will not visualize aim@gaze debug data. 1 - Will visualize aim@gaze debug data."));
|
||||
|
||||
UTobiiAimAtGazeComponent::UTobiiAimAtGazeComponent()
|
||||
: bAllowRetarget(false)
|
||||
, AimSpeed(0.2f)
|
||||
|
||||
, CurrentFocusComponent(nullptr)
|
||||
, CurrentAimTarget(0.0f, 0.0f, 0.0f)
|
||||
, bIsGazeAiming(false)
|
||||
{
|
||||
PrimaryComponentTick.bCanEverTick = true;
|
||||
}
|
||||
|
||||
bool UTobiiAimAtGazeComponent::AimAtGazeAvailable()
|
||||
{
|
||||
return UTobiiInteractionsBlueprintLibrary::IsAimAtGazeEnabled();
|
||||
}
|
||||
|
||||
void UTobiiAimAtGazeComponent::AimAtGaze()
|
||||
{
|
||||
if (!AimAtGazeAvailable())
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
CurrentFocusComponent.Reset();
|
||||
|
||||
FTobiiGazeFocusData FocusData;
|
||||
UTobiiGTOMBlueprintLibrary::GetFilteredGazeFocusData(FocusLayerFilters, bIsWhiteList, true, false, FocusData);
|
||||
if (FocusData.FocusedActor.IsValid())
|
||||
{
|
||||
CurrentFocusComponent = FocusData.FocusedPrimitiveComponent;
|
||||
CurrentAimTarget = FocusData.LastVisibleWorldLocation;
|
||||
bIsGazeAiming = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
const FHitResult& GazeHitData = UTobiiGTOMBlueprintLibrary::GetNaiveGazeHit();
|
||||
CurrentAimTarget = GazeHitData.Location;
|
||||
bIsGazeAiming = true;
|
||||
}
|
||||
}
|
||||
|
||||
void UTobiiAimAtGazeComponent::ContinuousAimAtGaze()
|
||||
{
|
||||
if (!AimAtGazeAvailable())
|
||||
{
|
||||
CurrentFocusComponent = nullptr;
|
||||
return;
|
||||
}
|
||||
|
||||
if (bAllowRetarget)
|
||||
{
|
||||
FTobiiGazeFocusData FocusData;
|
||||
UTobiiGTOMBlueprintLibrary::GetFilteredGazeFocusData(FocusLayerFilters, bIsWhiteList, true, false, FocusData);
|
||||
if (FocusData.FocusedPrimitiveComponent.IsValid())
|
||||
{
|
||||
CurrentFocusComponent = FocusData.FocusedPrimitiveComponent;
|
||||
}
|
||||
}
|
||||
|
||||
if (CurrentFocusComponent.IsValid())
|
||||
{
|
||||
FVector TargetFocusPosition;
|
||||
UTobiiGTOMBlueprintLibrary::GetPrimitiveComponentFocusLocation(CurrentFocusComponent.Get(), TargetFocusPosition);
|
||||
|
||||
CurrentAimTarget = TargetFocusPosition;
|
||||
bIsGazeAiming = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
CurrentFocusComponent.Reset();
|
||||
}
|
||||
}
|
||||
|
||||
void UTobiiAimAtGazeComponent::TickComponent(float DeltaTime, enum ELevelTick TickType, FActorComponentTickFunction *ThisTickFunction)
|
||||
{
|
||||
Super::TickComponent(DeltaTime, TickType, ThisTickFunction);
|
||||
|
||||
static const auto DrawDebugCVar = IConsoleManager::Get().FindConsoleVariable(TEXT("tobii.debug"));
|
||||
if (CurrentFocusComponent != nullptr && DrawDebugCVar->GetInt() && CVarAimAtGazeDebug.GetValueOnGameThread())
|
||||
{
|
||||
DrawDebugBox(GetWorld(), CurrentAimTarget, FVector(50.0f), FColor::Cyan, false, 0.0f);
|
||||
}
|
||||
|
||||
APlayerController* PlayerController = nullptr;
|
||||
APawn* OwnerPawn = Cast<APawn>(GetOwner());
|
||||
|
||||
// We make it possible to attach AimAtGaze component to either Pawn or PlayerController
|
||||
if (OwnerPawn != nullptr)
|
||||
{
|
||||
PlayerController = Cast<APlayerController>(OwnerPawn->GetController());
|
||||
}
|
||||
else
|
||||
{
|
||||
PlayerController = Cast<APlayerController>(GetOwner());
|
||||
}
|
||||
|
||||
if (PlayerController != nullptr)
|
||||
{
|
||||
if (AimAtGazeAvailable() && bIsGazeAiming && PlayerController->PlayerCameraManager != nullptr)
|
||||
{
|
||||
FVector CameraLocation = PlayerController->PlayerCameraManager->GetCameraLocation();
|
||||
FVector AimTargetDirection = CurrentAimTarget - CameraLocation;
|
||||
|
||||
FQuat TargetQuat = AimTargetDirection.ToOrientationQuat();
|
||||
FQuat CurrentControlQuat = FQuat(PlayerController->GetControlRotation());
|
||||
|
||||
CurrentControlQuat = FQuat::Slerp(CurrentControlQuat, TargetQuat, AimSpeed);
|
||||
float DotDistance = (CurrentControlQuat | TargetQuat);
|
||||
|
||||
if ((1.0f - DotDistance) < CVarAimAtGazeDoneThreshold.GetValueOnGameThread())
|
||||
{
|
||||
bIsGazeAiming = false;
|
||||
CurrentControlQuat = TargetQuat;
|
||||
}
|
||||
|
||||
PlayerController->SetControlRotation(CurrentControlQuat.Rotator());
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,114 @@
|
||||
/******************************************************************************
|
||||
* Copyright 2017- Tobii Technology AB. All rights reserved.
|
||||
*
|
||||
* @author Temaran | Fredrik Lindh | fredrik.lindh@tobii.com | https://github.com/Temaran
|
||||
******************************************************************************/
|
||||
|
||||
#include "TobiiFireAtGazeComponent.h"
|
||||
#include "TobiiInteractionsBlueprintLibrary.h"
|
||||
#include "TobiiGTOMBlueprintLibrary.h"
|
||||
#include "../TobiiInteractionsInternalTypes.h"
|
||||
|
||||
#include "Engine/Engine.h"
|
||||
#include "IEyeTracker.h"
|
||||
#include "Camera/CameraComponent.h"
|
||||
#include "Engine/World.h"
|
||||
#include "HAL/IConsoleManager.h"
|
||||
|
||||
static TAutoConsoleVariable<int32> CVarFireAtGazeEnabled(TEXT("tobii.interaction.FireAtGazeEnabled"), 1, TEXT("Fire at gaze lets you fire where you look when not in ADS (Aim Down Sights) mode. 0 - Fire at gaze is disabled. 1 - Fire at gaze is enabled."));
|
||||
|
||||
UTobiiFireAtGazeComponent::UTobiiFireAtGazeComponent()
|
||||
: CameraComponent(nullptr)
|
||||
|
||||
, FireAtGazeTargetActor(nullptr)
|
||||
, FireAtGazeTargetComponent(nullptr)
|
||||
, FireAtGazeTargetLocation(0.0f, 0.0f, 0.0f)
|
||||
|
||||
, MaxDistance(10000.0f)
|
||||
, NoTargetBehavior(ETobiiFireAtGazeNoTargetBehavior::PointGunToGaze)
|
||||
, TraceChannel(ECC_Visibility)
|
||||
{
|
||||
PrimaryComponentTick.bCanEverTick = true;
|
||||
}
|
||||
|
||||
bool UTobiiFireAtGazeComponent::FireAtGazeAvailable()
|
||||
{
|
||||
return CameraComponent != nullptr
|
||||
&& UTobiiInteractionsBlueprintLibrary::IsFireAtGazeEnabled();
|
||||
}
|
||||
|
||||
bool UTobiiFireAtGazeComponent::WantsCrosshair()
|
||||
{
|
||||
if (!FireAtGazeAvailable())
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
switch (NoTargetBehavior)
|
||||
{
|
||||
case ETobiiFireAtGazeNoTargetBehavior::PointGunForward:
|
||||
return !FireAtGazeTargetActor.IsValid();
|
||||
case ETobiiFireAtGazeNoTargetBehavior::PointGunToGaze:
|
||||
return true;
|
||||
default:
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
void UTobiiFireAtGazeComponent::TickComponent(float DeltaTime, enum ELevelTick TickType, FActorComponentTickFunction *ThisTickFunction)
|
||||
{
|
||||
Super::TickComponent(DeltaTime, TickType, ThisTickFunction);
|
||||
|
||||
if (!FireAtGazeAvailable() || GEngine == nullptr || !GEngine->EyeTrackingDevice.IsValid())
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
FTobiiGazeFocusData FocusData;
|
||||
UTobiiGTOMBlueprintLibrary::GetFilteredGazeFocusData(FocusLayerFilters, bIsWhiteList, true, false, FocusData);
|
||||
if (FocusData.FocusedPrimitiveComponent.IsValid())
|
||||
{
|
||||
FireAtGazeTargetActor = FocusData.FocusedActor;
|
||||
FireAtGazeTargetComponent = FocusData.FocusedPrimitiveComponent;
|
||||
FireAtGazeTargetLocation = FocusData.LastVisibleWorldLocation;
|
||||
}
|
||||
else
|
||||
{
|
||||
FVector GazeRayOrigin, GazeRayDirection;
|
||||
switch (NoTargetBehavior)
|
||||
{
|
||||
case ETobiiFireAtGazeNoTargetBehavior::PointGunForward:
|
||||
{
|
||||
GazeRayOrigin = CameraComponent->GetComponentLocation();
|
||||
GazeRayDirection = CameraComponent->GetForwardVector();
|
||||
break;
|
||||
}
|
||||
|
||||
case ETobiiFireAtGazeNoTargetBehavior::PointGunToGaze:
|
||||
default:
|
||||
{
|
||||
FEyeTrackerGazeData CombinedGazeData;
|
||||
GEngine->EyeTrackingDevice->GetEyeTrackerGazeData(CombinedGazeData);
|
||||
GazeRayOrigin = CombinedGazeData.GazeOrigin;
|
||||
GazeRayDirection = CombinedGazeData.GazeDirection;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
FHitResult HitResult;
|
||||
UWorld* World = GetWorld();
|
||||
if (World != nullptr && World->LineTraceSingleByChannel(HitResult, GazeRayOrigin, GazeRayOrigin + GazeRayDirection * MaxDistance, TraceChannel)
|
||||
&& UTobiiGTOMBlueprintLibrary::IsPrimitiveComponentGazeFocusable(HitResult.GetComponent()))
|
||||
{
|
||||
FireAtGazeTargetActor = HitResult.GetActor();
|
||||
FireAtGazeTargetComponent = HitResult.GetComponent();
|
||||
FireAtGazeTargetLocation = HitResult.Location;
|
||||
}
|
||||
else
|
||||
{
|
||||
FireAtGazeTargetActor = nullptr;
|
||||
FireAtGazeTargetComponent = nullptr;
|
||||
FireAtGazeTargetLocation = GazeRayOrigin + GazeRayDirection * MaxDistance;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,490 @@
|
||||
/******************************************************************************
|
||||
* Copyright 2017- Tobii Technology AB. All rights reserved.
|
||||
*
|
||||
* @author Temaran | Fredrik Lindh | fredrik.lindh@tobii.com | https://github.com/Temaran
|
||||
* Thanks to Jochen Schwarze (schwarze@isa.de) for functions to solve square, cubic and quartic functions as found here:
|
||||
* https://github.com/erich666/GraphicsGems/blob/240a34f2ad3fa577ef57be74920db6c4b00605e4/gems/Roots3And4.c
|
||||
******************************************************************************/
|
||||
|
||||
#include "TobiiInteractionsBlueprintLibrary.h"
|
||||
#include "TobiiRootFinders.h"
|
||||
|
||||
#include "Components/WidgetComponent.h"
|
||||
#include "IEyeTracker.h"
|
||||
#include "Engine/Engine.h"
|
||||
#include "Engine/World.h"
|
||||
#include "Engine/Texture2D.h"
|
||||
#include "Engine/TextureRenderTarget2D.h"
|
||||
#include "Framework/Application/SlateApplication.h"
|
||||
#include "HAL/IConsoleManager.h"
|
||||
#include "Slate/WidgetRenderer.h"
|
||||
|
||||
UTobiiInteractionsBlueprintLibrary::UTobiiInteractionsBlueprintLibrary(const class FObjectInitializer& ObjectInitializer)
|
||||
: Super(ObjectInitializer)
|
||||
{
|
||||
}
|
||||
|
||||
/**
|
||||
* Renders a UMG Widget to a texture with the specified size.
|
||||
*
|
||||
* @param Widget The widget to be rendered.
|
||||
* @param DrawSize The size to render the Widget to. Also will be the texture size.
|
||||
* @return The texture containing the rendered widget.
|
||||
*/
|
||||
UTexture2D* UTobiiInteractionsBlueprintLibrary::TextureFromWidget(UUserWidget* const Widget, const FVector2D& DrawSize)
|
||||
{
|
||||
if (FSlateApplication::IsInitialized()
|
||||
&& Widget != nullptr && Widget->IsValidLowLevel()
|
||||
&& DrawSize.X >= 1 && DrawSize.Y >= 1)
|
||||
{
|
||||
TSharedPtr<SWidget> SlateWidget(Widget->TakeWidget());
|
||||
if (!SlateWidget.IsValid())
|
||||
{
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
FWidgetRenderer WidgetRenderer = FWidgetRenderer(true);
|
||||
|
||||
UTextureRenderTarget2D* TextureRenderTarget = WidgetRenderer.DrawWidget(SlateWidget.ToSharedRef(), DrawSize);
|
||||
// Creates Texture2D to store RenderTexture content
|
||||
UTexture2D *Texture = UTexture2D::CreateTransient(DrawSize.X, DrawSize.Y, PF_B8G8R8A8);
|
||||
#if WITH_EDITORONLY_DATA
|
||||
Texture->MipGenSettings = TMGS_NoMipmaps;
|
||||
#endif
|
||||
|
||||
// Lock and copies the data between the textures
|
||||
TArray<FColor> SurfData;
|
||||
FRenderTarget* RenderTarget = TextureRenderTarget->GameThread_GetRenderTargetResource();
|
||||
RenderTarget->ReadPixels(SurfData);
|
||||
|
||||
void* TextureData = Texture->PlatformData->Mips[0].BulkData.Lock(LOCK_READ_WRITE);
|
||||
const int32 TextureDataSize = SurfData.Num() * 4;
|
||||
FMemory::Memcpy(TextureData, SurfData.GetData(), TextureDataSize);
|
||||
Texture->PlatformData->Mips[0].BulkData.Unlock();
|
||||
Texture->UpdateResource();
|
||||
|
||||
// Free resources
|
||||
SurfData.Empty();
|
||||
TextureRenderTarget->ConditionalBeginDestroy();
|
||||
SlateWidget.Reset();
|
||||
|
||||
return Texture;
|
||||
}
|
||||
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
bool UTobiiInteractionsBlueprintLibrary::IsInfiniteScreenEnabled()
|
||||
{
|
||||
static const auto EyetrackingEnabledCVar = IConsoleManager::Get().FindConsoleVariable(TEXT("tobii.EnableEyetracking"));
|
||||
static const auto InfiniteScreenEnabledCVar = IConsoleManager::Get().FindConsoleVariable(TEXT("tobii.desktop.InfiniteScreenEnabled"));
|
||||
|
||||
if (EyetrackingEnabledCVar != nullptr && InfiniteScreenEnabledCVar != nullptr && GEngine != nullptr && GEngine->EyeTrackingDevice.IsValid())
|
||||
{
|
||||
return GEngine->EyeTrackingDevice->GetEyeTrackerStatus() >= EEyeTrackerStatus::Tracking
|
||||
&& EyetrackingEnabledCVar->GetInt()
|
||||
&& InfiniteScreenEnabledCVar->GetInt();
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
bool UTobiiInteractionsBlueprintLibrary::IsCleanUIEnabled()
|
||||
{
|
||||
static const auto EyetrackingEnabledCVar = IConsoleManager::Get().FindConsoleVariable(TEXT("tobii.EnableEyetracking"));
|
||||
static const auto CleanUIEnabledCVar = IConsoleManager::Get().FindConsoleVariable(TEXT("tobii.interaction.EnableCleanUI"));
|
||||
|
||||
if (EyetrackingEnabledCVar != nullptr && CleanUIEnabledCVar != nullptr && GEngine != nullptr && GEngine->EyeTrackingDevice.IsValid())
|
||||
{
|
||||
return GEngine->EyeTrackingDevice->GetEyeTrackerStatus() >= EEyeTrackerStatus::Tracking
|
||||
&& EyetrackingEnabledCVar->GetInt()
|
||||
&& CleanUIEnabledCVar->GetInt();
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
bool UTobiiInteractionsBlueprintLibrary::IsAimAtGazeEnabled()
|
||||
{
|
||||
static const auto EyetrackingEnabledCVar = IConsoleManager::Get().FindConsoleVariable(TEXT("tobii.EnableEyetracking"));
|
||||
static const auto AimAtGazeEnabledCVar = IConsoleManager::Get().FindConsoleVariable(TEXT("tobii.interaction.AimAtGazeEnabled"));
|
||||
|
||||
if (EyetrackingEnabledCVar != nullptr && AimAtGazeEnabledCVar != nullptr && GEngine != nullptr && GEngine->EyeTrackingDevice.IsValid())
|
||||
{
|
||||
return GEngine->EyeTrackingDevice->GetEyeTrackerStatus() >= EEyeTrackerStatus::Tracking
|
||||
&& EyetrackingEnabledCVar->GetInt()
|
||||
&& AimAtGazeEnabledCVar->GetInt();
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
bool UTobiiInteractionsBlueprintLibrary::IsFireAtGazeEnabled()
|
||||
{
|
||||
static const auto EyetrackingEnabledCVar = IConsoleManager::Get().FindConsoleVariable(TEXT("tobii.EnableEyetracking"));
|
||||
static const auto FireAtGazeEnabledCVar = IConsoleManager::Get().FindConsoleVariable(TEXT("tobii.interaction.FireAtGazeEnabled"));
|
||||
|
||||
if (EyetrackingEnabledCVar != nullptr && FireAtGazeEnabledCVar != nullptr && GEngine != nullptr && GEngine->EyeTrackingDevice.IsValid())
|
||||
{
|
||||
return GEngine->EyeTrackingDevice->GetEyeTrackerStatus() >= EEyeTrackerStatus::Tracking
|
||||
&& EyetrackingEnabledCVar->GetInt()
|
||||
&& FireAtGazeEnabledCVar->GetInt();
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
bool UTobiiInteractionsBlueprintLibrary::IsThrowAtGazeEnabled()
|
||||
{
|
||||
static const auto EyetrackingEnabledCVar = IConsoleManager::Get().FindConsoleVariable(TEXT("tobii.EnableEyetracking"));
|
||||
static const auto ThrowAtGazeEnabledCVar = IConsoleManager::Get().FindConsoleVariable(TEXT("tobii.interaction.ThrowAtGazeEnabled"));
|
||||
|
||||
if (EyetrackingEnabledCVar != nullptr && ThrowAtGazeEnabledCVar != nullptr && GEngine != nullptr && GEngine->EyeTrackingDevice.IsValid())
|
||||
{
|
||||
return GEngine->EyeTrackingDevice->GetEyeTrackerStatus() >= EEyeTrackerStatus::Tracking
|
||||
&& EyetrackingEnabledCVar->GetInt()
|
||||
&& ThrowAtGazeEnabledCVar->GetInt();
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
float UTobiiInteractionsBlueprintLibrary::CalculateSmoothPitchStep(float ViewPitch)
|
||||
{
|
||||
//Limit infinite screen yaw depending on input device pitch
|
||||
float NormalizedPitch = FRotator::NormalizeAxis(ViewPitch);
|
||||
float PitchScale = 1.0f - (FMath::Abs(NormalizedPitch) / 90.0f);
|
||||
return FMath::SmoothStep(0.0f, 1.0f, PitchScale);
|
||||
}
|
||||
|
||||
FRotator UTobiiInteractionsBlueprintLibrary::MakeInfiniteScreenCameraRotator(FRotator OriginalCameraRotation, FRotator InfiniteScreenAngles)
|
||||
{
|
||||
FQuat WorldSpaceYawRotation = FQuat(FVector::UpVector, InfiniteScreenAngles.Yaw);
|
||||
FQuat LocalSpacePitchRotation = FQuat(FVector::RightVector, -InfiniteScreenAngles.Pitch);
|
||||
|
||||
FQuat InfiniteScreenViewRotation(OriginalCameraRotation);
|
||||
InfiniteScreenViewRotation = InfiniteScreenViewRotation * LocalSpacePitchRotation; //Local space by multiplying on the right
|
||||
InfiniteScreenViewRotation = WorldSpaceYawRotation * InfiniteScreenViewRotation; //World space by multiplying on the left
|
||||
return InfiniteScreenViewRotation.Rotator();
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
void UTobiiInteractionsBlueprintLibrary::FindRealSquareRoots(float A, float B, float C, TArray<float>& OutRealRoots)
|
||||
{
|
||||
double Coefficients[3]{ C, B, A };
|
||||
double Solutions[2]{ 0.0, 0.0 };
|
||||
|
||||
int32 NrSolutions = SolveQuadric(Coefficients, Solutions);
|
||||
OutRealRoots.Empty(NrSolutions);
|
||||
for (int32 SolutionIdx = 0; SolutionIdx < NrSolutions; SolutionIdx++)
|
||||
{
|
||||
OutRealRoots.Add(Solutions[SolutionIdx]);
|
||||
}
|
||||
}
|
||||
|
||||
void UTobiiInteractionsBlueprintLibrary::FindRealCubicRoots(float A, float B, float C, float D, TArray<float>& OutRealRoots)
|
||||
{
|
||||
double Coefficients[4]{ D, C, B, A };
|
||||
double Solutions[3]{ 0.0, 0.0, 0.0 };
|
||||
|
||||
int32 NrSolutions = SolveCubic(Coefficients, Solutions);
|
||||
OutRealRoots.Empty(NrSolutions);
|
||||
for (int32 SolutionIdx = 0; SolutionIdx < NrSolutions; SolutionIdx++)
|
||||
{
|
||||
OutRealRoots.Add(Solutions[SolutionIdx]);
|
||||
}
|
||||
}
|
||||
|
||||
void UTobiiInteractionsBlueprintLibrary::FindRealQuarticRoots(float A, float B, float C, float D, float E, TArray<float>& OutRealRoots)
|
||||
{
|
||||
double Coefficients[5]{ E, D, C, B, A };
|
||||
double Solutions[4]{ 0.0, 0.0, 0.0, 0.0 };
|
||||
|
||||
int32 NrSolutions = SolveQuartic(Coefficients, Solutions);
|
||||
OutRealRoots.Empty(NrSolutions);
|
||||
for (int32 SolutionIdx = 0; SolutionIdx < NrSolutions; SolutionIdx++)
|
||||
{
|
||||
OutRealRoots.Add(Solutions[SolutionIdx]);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Try to find the appropriate acceleration to hit a moving target.
|
||||
* We do this by setting up an equation system with 4 equations in 4 unknowns.
|
||||
* First we solve for time, and then we plug that into the other equations to find the wanted acceleration.
|
||||
*
|
||||
* VARIABLES:
|
||||
* Time: t <--- Need to first solve for this
|
||||
* Projectile Pos: PPX, PPY, PPZ
|
||||
* Projectile Vel: PVX, PVY, PVZ
|
||||
* Projectile Acc: pax, pay, paz <--- Solving for this is our goal
|
||||
* Projectile AccMagnitude: PAM
|
||||
* Target Pos: TPX, TPY, TPZ
|
||||
* Target Vel: TVX, TVY, TVZ
|
||||
* Target Acc: TAX, TAY, TAZ
|
||||
*
|
||||
* EQUATIONS:
|
||||
* PPX + PVX * t + (1/2) * pax * t^2 = TPX + TVX * t + (1/2) * TAX * t^2
|
||||
* PPY + PVY * t + (1/2) * pay * t^2 = TPY + TVY * t + (1/2) * TAY * t^2
|
||||
* PPZ + PVZ * t + (1/2) * paz * t^2 = TPZ + TVZ * t + (1/2) * TAZ * t^2
|
||||
* PAM^2 = pax^2 + pay^2 + paz^2
|
||||
*
|
||||
* SOLVE FOR ACCELERATION:
|
||||
* pax = 2.0 * (TPX + TVX * t + (1/2) * TAX * t^2 - PPX - PVX * t) / t^2
|
||||
* pay = 2.0 * (TPY + TVY * t + (1/2) * TAY * t^2 - PPY - PVY * t) / t^2
|
||||
* paz = 2.0 * (TPZ + TVZ * t + (1/2) * TAZ * t^2 - PPZ - PVZ * t) / t^2
|
||||
*
|
||||
* SIMPLIFY:
|
||||
* DPX = TPX - PPX
|
||||
* DPY = TPY - PPY
|
||||
* DPZ = TPZ - PPZ
|
||||
* DVX = TVX - PVX
|
||||
* DVY = TVY - PVY
|
||||
* DVZ = TVZ - PVZ
|
||||
* pax = 2.0 * (DPX + DVX * t + (1/2) * TAX * t^2) / t^2
|
||||
* pay = 2.0 * (DPY + DVY * t + (1/2) * TAY * t^2) / t^2
|
||||
* paz = 2.0 * (DPZ + DVZ * t + (1/2) * TAZ * t^2) / t^2
|
||||
*
|
||||
* SQUARE SO WE CAN SUBSTITUTE LATER:
|
||||
* pax^2 = 4.0 * (DPX + DVX * t + (1/2) * TAX * t^2)^2 / t^4
|
||||
* pay^2 = 4.0 * (DPY + DVY * t + (1/2) * TAY * t^2)^2 / t^4
|
||||
* paz^2 = 4.0 * (DPZ + DVZ * t + (1/2) * TAZ * t^2)^2 / t^4
|
||||
*
|
||||
* EXPAND THE SQUARES SO WE CAN EXTRACT COEFFICIENTS LATER:
|
||||
* pax^2 = 4.0 * (DPX^2 + 2.0*DPX*DVX*t + DPX*TAX*t^2 + DVX^2*t^2 + DVX*TAX*t^3 + (1/4)*TAX^2*t^4) / t^4
|
||||
* pay^2 = 4.0 * (DPY^2 + 2.0*DPY*DVY*t + DPY*TAY*t^2 + DVY^2*t^2 + DVY*TAY*t^3 + (1/4)*TAY^2*t^4) / t^4
|
||||
* paz^2 = 4.0 * (DPZ^2 + 2.0*DPZ*DVZ*t + DPZ*TAZ*t^2 + DVZ^2*t^2 + DVZ*TAZ*t^3 + (1/4)*TAZ^2*t^4) / t^4
|
||||
*
|
||||
* SUBSTITUTE INTO OUR PAM EQUATION:
|
||||
* PAM^2 = 4.0 * (DPX^2 + 2.0*DPX*DVX*t + DPX*TAX*t^2 + DVX^2*t^2 + DVX*TAX*t^3 + (1/4)*TAX^2*t^4) / t^4
|
||||
* + 4.0 * (DPY^2 + 2.0*DPY*DVY*t + DPY*TAY*t^2 + DVY^2*t^2 + DVY*TAY*t^3 + (1/4)*TAY^2*t^4) / t^4
|
||||
* + 4.0 * (DPZ^2 + 2.0*DPZ*DVZ*t + DPZ*TAZ*t^2 + DVZ^2*t^2 + DVZ*TAZ*t^3 + (1/4)*TAZ^2*t^4) / t^4
|
||||
*
|
||||
* MULTIPLY BY t^4:
|
||||
* PAM^2 * t^4 = 4.0 * (DPX^2 + 2.0*DPX*DVX*t + DPX*TAX*t^2 + DVX^2*t^2 + DVX*TAX*t^3 + (1/4)*TAX^2*t^4)
|
||||
* + 4.0 * (DPY^2 + 2.0*DPY*DVY*t + DPY*TAY*t^2 + DVY^2*t^2 + DVY*TAY*t^3 + (1/4)*TAY^2*t^4)
|
||||
* + 4.0 * (DPZ^2 + 2.0*DPZ*DVZ*t + DPZ*TAZ*t^2 + DVZ^2*t^2 + DVZ*TAZ*t^3 + (1/4)*TAZ^2*t^4)
|
||||
*
|
||||
* MAKE 0 EQUATION AND FORM TERMS:
|
||||
* 0 = 4.0*DPX^2 + 8.0*DPX*DVX*t + 4.0*DPX*TAX*t^2 + 4.0*DVX^2*t^2 + 4.0*DVX*TAX*t^3 + TAX^2*t^4
|
||||
* + 4.0*DPY^2 + 8.0*DPY*DVY*t + 4.0*DPY*TAY*t^2 + 4.0*DVY^2*t^2 + 4.0*DVY*TAY*t^3 + TAY^2*t^4
|
||||
* + 4.0*DPZ^2 + 8.0*DPZ*DVZ*t + 4.0*DPZ*TAZ*t^2 + 4.0*DVZ^2*t^2 + 4.0*DVZ*TAZ*t^3 + TAZ^2*t^4
|
||||
* - PAM^2 * t^4
|
||||
*
|
||||
* ARRANGE IN COEFFICIENT FORM:
|
||||
* 0 = (TAX^2 + TAY^2 + TAZ^2 - PAM^2) * t^4
|
||||
* 4.0 * (DVX*TAX + DVY*TAY + DVZ*TAZ) * t^3
|
||||
* 4.0 * (DVX^2 + DVY^2 + DVZ^2 + DPX*TAX + DPY*TAY + DPZ*TAZ) * t^2
|
||||
* 8.0 * (DPX*DVX + DPY*DVY + DPZ*DVZ) * t^1
|
||||
* 4.0 * (DPX^2 + DPY^2 + DPZ^2) * t^0
|
||||
*
|
||||
* Solve the quartic!
|
||||
* Then finally insert the smallest root (time) into the (pax, pay, paz) formulas to get the wanted acceleration.
|
||||
*/
|
||||
bool UTobiiInteractionsBlueprintLibrary::FindNeededAccelerationForAccelerationBasedHomingProjectile(const FTobiiAccelerationBasedHomingData& InputData, FTobiiAccelerationBasedHomingResult& BestResult)
|
||||
{
|
||||
const FVector DeltaPosition = InputData.TargetPosition - InputData.ProjectilePosition;
|
||||
if (DeltaPosition.SizeSquared() < FLT_EPSILON)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
const FVector DeltaVelocity = InputData.TargetVelocity - InputData.ProjectileVelocity;
|
||||
|
||||
const double T4Coefficient = FVector::DotProduct(InputData.TargetAcceleration, InputData.TargetAcceleration) - InputData.ProjectileAccelerationMagnitude * InputData.ProjectileAccelerationMagnitude;
|
||||
const double T3Coefficient = 4.0 * FVector::DotProduct(DeltaVelocity, InputData.TargetAcceleration);
|
||||
const double T2Coefficient = 4.0 * FVector::DotProduct(DeltaVelocity, DeltaVelocity) + FVector::DotProduct(DeltaPosition, InputData.TargetAcceleration);
|
||||
const double T1Coefficient = 8.0 * FVector::DotProduct(DeltaPosition, DeltaVelocity);
|
||||
const double T0Coefficient = 4.0 * FVector::DotProduct(DeltaPosition, DeltaPosition);
|
||||
|
||||
TArray<double> Solutions;
|
||||
double DirectHitCoefficients[5]{ T0Coefficient, T1Coefficient, T2Coefficient, T3Coefficient, T4Coefficient };
|
||||
double DirectHitSolutions[4]{ 0.0, 0.0, 0.0, 0.0 };
|
||||
|
||||
int32 NrDirectHitSolutions = SolveQuartic(DirectHitCoefficients, DirectHitSolutions);
|
||||
for (int32 SolutionIdx = 0; SolutionIdx < NrDirectHitSolutions; SolutionIdx++)
|
||||
{
|
||||
const double Solution = DirectHitSolutions[SolutionIdx];
|
||||
if (FMath::IsFinite(Solution) && !FMath::IsNaN(Solution) && Solution > DBL_EPSILON)
|
||||
{
|
||||
Solutions.Add(Solution);
|
||||
}
|
||||
}
|
||||
|
||||
if (Solutions.Num() == 0 && InputData.bAttemptClosestApproachSolution)
|
||||
{
|
||||
//Since we couldn't find a direct hit, attempt to find a closest approach instead as backup.
|
||||
double ClosestApproachCoefficients[4]{ T1Coefficient, 2.0 * T2Coefficient, 3.0 * T3Coefficient, 4.0 * T4Coefficient };
|
||||
double ClosestApproachSolutions[3]{ 0.0, 0.0, 0.0 };
|
||||
|
||||
TMap<double, double> ClosestApproachDistancesToInterceptTimes;
|
||||
int32 NrClosestApproachSolutions = SolveCubic(ClosestApproachCoefficients, ClosestApproachSolutions);
|
||||
for (int32 SolutionIdx = 0; SolutionIdx < NrClosestApproachSolutions; SolutionIdx++)
|
||||
{
|
||||
const double Solution = ClosestApproachSolutions[SolutionIdx];
|
||||
|
||||
if (FMath::IsFinite(Solution) && !FMath::IsNaN(Solution) && Solution > DBL_EPSILON
|
||||
&& Solution >= 0.0 && !ClosestApproachDistancesToInterceptTimes.Contains(Solution))
|
||||
{
|
||||
const double Real = Solution;
|
||||
const double RealSq = Real * Real;
|
||||
const double RealCub = RealSq * Real;
|
||||
const double RealQuart = RealCub * Real;
|
||||
const double Distance = FMath::Abs(T4Coefficient * RealQuart + T3Coefficient * RealCub + T2Coefficient * RealSq + T1Coefficient * Real + T0Coefficient);
|
||||
ClosestApproachDistancesToInterceptTimes.Add(Distance, Real);
|
||||
}
|
||||
}
|
||||
|
||||
ClosestApproachDistancesToInterceptTimes.KeySort(TLess<float>());
|
||||
for (auto& Pair : ClosestApproachDistancesToInterceptTimes)
|
||||
{
|
||||
Solutions.Add(Pair.Value);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (Solutions.Num() == 0)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
Solutions.Sort(TLess<double>());
|
||||
const double InterceptTime = Solutions[0];
|
||||
const double InterceptTimeSquare = InterceptTime * InterceptTime;
|
||||
|
||||
BestResult.Type = ETobiiInterceptType::DirectHit;
|
||||
BestResult.ExpectedInterceptTimeSecs = InterceptTime;
|
||||
BestResult.ExpectedInterceptLocation = InputData.TargetPosition + InputData.TargetVelocity * InterceptTime + (1.0 / 2.0) * InputData.TargetAcceleration * InterceptTimeSquare;
|
||||
BestResult.SuggestedAcceleration = 2.0 * (DeltaPosition + DeltaVelocity * InterceptTime + (1.0 / 2.0) * InputData.TargetAcceleration * InterceptTimeSquare) / InterceptTimeSquare;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* Try to find the appropriate velocity to hit a moving target given a ballistic projectile.
|
||||
* We do this by setting up an equation system with 4 equations in 4 unknowns.
|
||||
* First we solve for time, and then we plug that into the other equations to find the wanted acceleration.
|
||||
*
|
||||
* VARIABLES:
|
||||
* Time: t <--- Need to first solve for this
|
||||
* Projectile Pos: PPX, PPY, PPZ
|
||||
* Projectile Vel: pvx, pvy, pvz
|
||||
* Projectile Gravity: PAX, PAY, PAZ <--- Most likely gravity
|
||||
* Projectile Apex: PAPEX <--- This is at when (1/2)*t
|
||||
* Target Pos: TPX, TPY, TPZ
|
||||
* Target Vel: TVX, TVY, TVZ
|
||||
* Target Acc: TAX, TAY, TAZ
|
||||
*
|
||||
* END EQUATIONS:
|
||||
* PPX + pvx * t + (1/2) * PAX * t^2 = TPX + TVX * t + (1/2) * TAX * t^2
|
||||
* PPY + pvy * t + (1/2) * PAY * t^2 = TPY + TVY * t + (1/2) * TAY * t^2
|
||||
* PPZ + pvz * t + (1/2) * PAZ * t^2 = TPZ + TVZ * t + (1/2) * TAZ * t^2
|
||||
*
|
||||
* KNOWN Z EQUATIONS:
|
||||
* PAPEX = PPZ + (1/2) * pvz * t + (1/8) * PAZ * t^2
|
||||
*
|
||||
* SOLVE FOR VELOCITY:
|
||||
* pvx = (TPX + TVX * t + (1/2) * TAX * t^2 - PPX - (1/2) * PAX * t^2) / t
|
||||
* pvy = (TPY + TVY * t + (1/2) * TAY * t^2 - PPY - (1/2) * PAY * t^2) / t
|
||||
* pvz = (TPZ + TVZ * t + (1/2) * TAZ * t^2 - PPZ - (1/2) * PAZ * t^2) / t
|
||||
*
|
||||
* SIMPLIFY:
|
||||
* DPX = TPX - PPX
|
||||
* DPY = TPY - PPY
|
||||
* DPZ = TPZ - PPZ
|
||||
* DAX = (1/2) * (TAX - PAX)
|
||||
* DAY = (1/2) * (TAY - PAY)
|
||||
* DAZ = (1/2) * (TAZ - PAZ)
|
||||
* pvx = (DPX + TVX * t + DAX * t^2) / t
|
||||
* pvy = (DPY + TVY * t + DAY * t^2) / t
|
||||
* pvz = (DPZ + TVZ * t + DAZ * t^2) / t
|
||||
*
|
||||
* SUBSTITUTE INTO OUR MIDPOINT EQUATIONS:
|
||||
* PAPEX = PPZ + (1/2) * [(DPZ + TVZ * t + DAZ * t^2) / t] * t + (1/8) * PAZ * t^2
|
||||
*
|
||||
* MAKE 0 EQUATION AND FORM TERMS:
|
||||
* 0 = PPZ + (1/2) * DPZ - PAPEX + (1/2) * TVZ * t + (1/2) * DAZ * t^2 + (1/8) * PAZ * t^2
|
||||
*
|
||||
* ARRANGE IN COEFFICIENT FORM:
|
||||
* 0 = ((1/2) * DAZ + (1/8) * PAZ) * t^2
|
||||
* (1/2) * TVZ * t^1
|
||||
* PPZ + (1/2) * DPZ - PAPEX * t^0
|
||||
*
|
||||
* Solve the square for time!
|
||||
* Then finally insert the roots (time) into the (pvx, pvy, pvz) formulas to get the possible velocities.
|
||||
*/
|
||||
bool UTobiiInteractionsBlueprintLibrary::FindNeededInitialVelocityForBallisticProjectile(const FTobiiBallisticData& InputData, TArray<FTobiiBallisticResult>& Results)
|
||||
{
|
||||
const FVector DeltaPosition = InputData.TargetPosition - InputData.ProjectileInitialPosition;
|
||||
if (DeltaPosition.SizeSquared() < FLT_EPSILON)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
const float ApexZ = FMath::Max(InputData.ProjectileInitialPosition.Z, InputData.TargetPosition.Z) + InputData.ProjectileApexOffsetCm;
|
||||
const FVector DeltaAcceleration = 0.5f * (InputData.TargetAcceleration - InputData.ProjectileAcceleration);
|
||||
|
||||
const double T2Coefficient = 0.5 * DeltaAcceleration.Z + 0.125 * InputData.ProjectileAcceleration.Z;
|
||||
const double T1Coefficient = 0.5 * InputData.TargetVelocity.Z;
|
||||
const double T0Coefficient = InputData.ProjectileInitialPosition.Z + 0.5 * DeltaPosition.Z - ApexZ;
|
||||
double TimeCoefficients[3] { T0Coefficient, T1Coefficient, T2Coefficient };
|
||||
double TimeSolutions[2] { 0.0, 0.0 };
|
||||
|
||||
int32 NrTimeSolutions = SolveQuadric(TimeCoefficients, TimeSolutions);
|
||||
for (int32 SolutionIdx = 0; SolutionIdx < NrTimeSolutions; SolutionIdx++)
|
||||
{
|
||||
const double Time = TimeSolutions[SolutionIdx];
|
||||
if (FMath::IsFinite(Time) && !FMath::IsNaN(Time) && Time > DBL_EPSILON)
|
||||
{
|
||||
const double HTime = Time / 2.0;
|
||||
const double HTimeSq = HTime * HTime;
|
||||
const double TimeSq = Time * Time;
|
||||
|
||||
FTobiiBallisticResult NewResult;
|
||||
NewResult.ExpectedInterceptTimeSecs = Time;
|
||||
NewResult.SuggestedInitialVelocity = (DeltaPosition + InputData.TargetVelocity * Time + DeltaAcceleration * TimeSq) / Time;
|
||||
NewResult.ExpectedInterceptLocation = InputData.ProjectileInitialPosition + NewResult.SuggestedInitialVelocity * Time + 0.5f * InputData.ProjectileAcceleration * TimeSq;
|
||||
Results.Add(NewResult);
|
||||
}
|
||||
}
|
||||
|
||||
return Results.Num() > 0;
|
||||
}
|
||||
|
||||
bool UTobiiInteractionsBlueprintLibrary::TraceBallisticProjectilePath(UObject* WorldContextObject, const FTobiiProjectileTraceData& InputData, TArray<FVector>& OutTracedPath, FHitResult& OutHitResult)
|
||||
{
|
||||
if (WorldContextObject == nullptr)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
float CurrentTime = InputData.StepSizeSecs;
|
||||
FVector StartPoint = InputData.ProjectileInitialPosition;
|
||||
FVector EndPoint = StartPoint + InputData.ProjectileVelocity * CurrentTime + 0.5f * InputData.ProjectileAcceleration * CurrentTime * CurrentTime;
|
||||
|
||||
FCollisionQueryParams CollisionParams;
|
||||
CollisionParams.AddIgnoredActors(InputData.IgnoredActors);
|
||||
|
||||
OutTracedPath.Empty();
|
||||
OutTracedPath.Add(StartPoint);
|
||||
|
||||
for (int32 StepCount = 0; StepCount < InputData.MaxNrSteps; StepCount++)
|
||||
{
|
||||
if (WorldContextObject->GetWorld()->SweepSingleByChannel(OutHitResult, StartPoint, EndPoint, FQuat::Identity, InputData.TraceChannel, FCollisionShape::MakeSphere(InputData.TraceRadiusCm), CollisionParams))
|
||||
{
|
||||
OutTracedPath.Add(OutHitResult.Location);
|
||||
return true;
|
||||
}
|
||||
else
|
||||
{
|
||||
OutTracedPath.Add(EndPoint);
|
||||
}
|
||||
|
||||
StartPoint = EndPoint;
|
||||
CurrentTime += InputData.StepSizeSecs;
|
||||
EndPoint = InputData.ProjectileInitialPosition + InputData.ProjectileVelocity * CurrentTime + 0.5f * InputData.ProjectileAcceleration * CurrentTime * CurrentTime;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
@@ -0,0 +1,11 @@
|
||||
/******************************************************************************
|
||||
* Copyright 2017- Tobii Technology AB. All rights reserved.
|
||||
*
|
||||
* @author Temaran | Fredrik Lindh | fredrik.lindh@tobii.com | https://github.com/Temaran
|
||||
******************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "CoreMinimal.h"
|
||||
|
||||
DEFINE_LOG_CATEGORY_STATIC(LogTobiiInteraction, All, All);
|
||||
32
Source/TobiiInteractions/Private/TobiiInteractionsModule.cpp
Normal file
32
Source/TobiiInteractions/Private/TobiiInteractionsModule.cpp
Normal file
@@ -0,0 +1,32 @@
|
||||
/******************************************************************************
|
||||
* Copyright 2017- Tobii Technology AB. All rights reserved.
|
||||
*
|
||||
* @author Temaran | Fredrik Lindh | fredrik.lindh@tobii.com | https://github.com/Temaran
|
||||
******************************************************************************/
|
||||
|
||||
#include "TobiiInteractionsModule.h"
|
||||
#include "TobiiInteractionsStyle.h"
|
||||
|
||||
#include "GameFramework/HUD.h"
|
||||
|
||||
IMPLEMENT_MODULE(FTobiiInteractionsModule, TobiiInteractions);
|
||||
|
||||
void FTobiiInteractionsModule::StartupModule()
|
||||
{
|
||||
#if WITH_EDITOR
|
||||
if (GIsEditor)
|
||||
{
|
||||
FTobiiInteractionsStyle::Initialize();
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
void FTobiiInteractionsModule::ShutdownModule()
|
||||
{
|
||||
#if WITH_EDITOR
|
||||
if (GIsEditor)
|
||||
{
|
||||
FTobiiInteractionsStyle::Shutdown();
|
||||
}
|
||||
#endif
|
||||
}
|
||||
19
Source/TobiiInteractions/Private/TobiiInteractionsModule.h
Normal file
19
Source/TobiiInteractions/Private/TobiiInteractionsModule.h
Normal file
@@ -0,0 +1,19 @@
|
||||
/******************************************************************************
|
||||
* Copyright 2017- Tobii Technology AB. All rights reserved.
|
||||
*
|
||||
* @author Temaran | Fredrik Lindh | fredrik.lindh@tobii.com | https://github.com/Temaran
|
||||
******************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "CoreMinimal.h"
|
||||
|
||||
#include "Modules/ModuleInterface.h"
|
||||
#include "Modules/ModuleManager.h"
|
||||
|
||||
class FTobiiInteractionsModule : public IModuleInterface
|
||||
{
|
||||
private:
|
||||
virtual void StartupModule() override;
|
||||
virtual void ShutdownModule() override;
|
||||
};
|
||||
77
Source/TobiiInteractions/Private/TobiiInteractionsStyle.cpp
Normal file
77
Source/TobiiInteractions/Private/TobiiInteractionsStyle.cpp
Normal file
@@ -0,0 +1,77 @@
|
||||
/******************************************************************************
|
||||
* Copyright 2017- Tobii Technology AB. All rights reserved.
|
||||
*
|
||||
* @author Temaran | Fredrik Lindh | fredrik.lindh@tobii.com | https://github.com/Temaran
|
||||
******************************************************************************/
|
||||
|
||||
#include "TobiiInteractionsStyle.h"
|
||||
|
||||
#include "Framework/Application/SlateApplication.h"
|
||||
#include "Slate/SlateGameResources.h"
|
||||
#include "Styling/SlateStyleRegistry.h"
|
||||
|
||||
TSharedPtr<FSlateStyleSet> FTobiiInteractionsStyle::TobiiEyetrackingInteractionsStyleInstance = nullptr;
|
||||
|
||||
void FTobiiInteractionsStyle::Initialize()
|
||||
{
|
||||
if (!TobiiEyetrackingInteractionsStyleInstance.IsValid())
|
||||
{
|
||||
TobiiEyetrackingInteractionsStyleInstance = Create();
|
||||
FSlateStyleRegistry::RegisterSlateStyle(*TobiiEyetrackingInteractionsStyleInstance);
|
||||
}
|
||||
}
|
||||
|
||||
void FTobiiInteractionsStyle::Shutdown()
|
||||
{
|
||||
FSlateStyleRegistry::UnRegisterSlateStyle(*TobiiEyetrackingInteractionsStyleInstance);
|
||||
ensure(TobiiEyetrackingInteractionsStyleInstance.IsUnique());
|
||||
TobiiEyetrackingInteractionsStyleInstance.Reset();
|
||||
}
|
||||
|
||||
FName FTobiiInteractionsStyle::GetStyleSetName()
|
||||
{
|
||||
static FName StyleSetName(TEXT("TobiiStyle"));
|
||||
return StyleSetName;
|
||||
}
|
||||
|
||||
#define IMAGE_BRUSH( RelativePath, ... ) FSlateImageBrush( Style->RootToContentDir( RelativePath, TEXT(".png") ), __VA_ARGS__ )
|
||||
#define BOX_BRUSH( RelativePath, ... ) FSlateBoxBrush( Style->RootToContentDir( RelativePath, TEXT(".png") ), __VA_ARGS__ )
|
||||
#define BORDER_BRUSH( RelativePath, ... ) FSlateBorderBrush( Style->RootToContentDir( RelativePath, TEXT(".png") ), __VA_ARGS__ )
|
||||
#define TTF_FONT( RelativePath, ... ) FSlateFontInfo( Style->RootToContentDir( RelativePath, TEXT(".ttf") ), __VA_ARGS__ )
|
||||
#define OTF_FONT( RelativePath, ... ) FSlateFontInfo( Style->RootToContentDir( RelativePath, TEXT(".otf") ), __VA_ARGS__ )
|
||||
|
||||
const FVector2D Icon16x16(16.0f, 16.0f);
|
||||
const FVector2D Icon20x20(20.0f, 20.0f);
|
||||
const FVector2D Icon40x40(40.0f, 40.0f);
|
||||
|
||||
TSharedRef<FSlateStyleSet> FTobiiInteractionsStyle::Create()
|
||||
{
|
||||
TSharedRef<FSlateStyleSet> Style = MakeShareable(new FSlateStyleSet("TobiiStyle"));
|
||||
|
||||
#if TOBII_COMPILE_AS_ENGINE_PLUGIN
|
||||
Style->SetContentRoot(FPaths::EnginePluginsDir() / TEXT("Runtime/TobiiEyetracking/Resources"));
|
||||
#else
|
||||
Style->SetContentRoot(FPaths::ProjectPluginsDir() / TEXT("TobiiEyetracking/Resources"));
|
||||
#endif
|
||||
|
||||
//The names here after ClassIcon. must match the type names of the types we want to offer the icon for.
|
||||
Style->Set("ClassIcon.TobiiCleanUIContainer", new IMAGE_BRUSH(TEXT("CleanUI16x"), Icon16x16));
|
||||
|
||||
return Style;
|
||||
}
|
||||
|
||||
#undef IMAGE_BRUSH
|
||||
#undef BOX_BRUSH
|
||||
#undef BORDER_BRUSH
|
||||
#undef TTF_FONT
|
||||
#undef OTF_FONT
|
||||
|
||||
void FTobiiInteractionsStyle::ReloadTextures()
|
||||
{
|
||||
FSlateApplication::Get().GetRenderer()->ReloadTextureResources();
|
||||
}
|
||||
|
||||
const ISlateStyle& FTobiiInteractionsStyle::Get()
|
||||
{
|
||||
return *TobiiEyetrackingInteractionsStyleInstance;
|
||||
}
|
||||
24
Source/TobiiInteractions/Private/TobiiInteractionsStyle.h
Normal file
24
Source/TobiiInteractions/Private/TobiiInteractionsStyle.h
Normal file
@@ -0,0 +1,24 @@
|
||||
/******************************************************************************
|
||||
* Copyright 2017- Tobii Technology AB. All rights reserved.
|
||||
*
|
||||
* @author Temaran | Fredrik Lindh | fredrik.lindh@tobii.com | https://github.com/Temaran
|
||||
******************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "CoreMinimal.h"
|
||||
#include "Styling/ISlateStyle.h"
|
||||
|
||||
class TOBIIINTERACTIONS_API FTobiiInteractionsStyle
|
||||
{
|
||||
public:
|
||||
static void Initialize();
|
||||
static void Shutdown();
|
||||
static void ReloadTextures();
|
||||
static const ISlateStyle& Get();
|
||||
static FName GetStyleSetName();
|
||||
|
||||
private:
|
||||
static TSharedRef<class FSlateStyleSet> Create();
|
||||
static TSharedPtr<class FSlateStyleSet> TobiiEyetrackingInteractionsStyleInstance;
|
||||
};
|
||||
279
Source/TobiiInteractions/Private/TobiiRootFinders.h
Normal file
279
Source/TobiiInteractions/Private/TobiiRootFinders.h
Normal file
@@ -0,0 +1,279 @@
|
||||
/*
|
||||
* Utility functions to find cubic and quartic roots,
|
||||
* coefficients are passed like this:
|
||||
*
|
||||
* c[0] + c[1]*x + c[2]*x^2 + c[3]*x^3 + c[4]*x^4 = 0
|
||||
*
|
||||
* The functions return the number of non-complex roots and
|
||||
* put the values into the s array.
|
||||
*
|
||||
* Author: Jochen Schwarze (schwarze@isa.de)
|
||||
*
|
||||
* Jan 26, 1990 Version for Graphics Gems
|
||||
* Oct 11, 1990 Fixed sign problem for negative q's in SolveQuartic
|
||||
* (reported by Mark Podlipec),
|
||||
* Old-style function definitions,
|
||||
* IsZero() as a macro
|
||||
* Nov 23, 1990 Some systems do not declare acos() and cbrt() in
|
||||
* <math.h>, though the functions exist in the library.
|
||||
* If large coefficients are used, EQN_EPS should be
|
||||
* reduced considerably (e.g. to 1E-30), results will be
|
||||
* correct but multiple roots might be reported more
|
||||
* than once.
|
||||
*
|
||||
* Graphics gems resources:
|
||||
* http://www.realtimerendering.com/resources/GraphicsGems/
|
||||
*
|
||||
* License:
|
||||
* All graphics gems code can be used without restrictions. [See above link]
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <math.h>
|
||||
#ifndef M_PI
|
||||
#define M_PI 3.14159265358979323846
|
||||
#endif
|
||||
|
||||
/* epsilon surrounding for near zero values */
|
||||
|
||||
#define EQN_EPS 1E-30
|
||||
#define IsZero(x) ((x) > -EQN_EPS && (x) < EQN_EPS)
|
||||
#define IsOne(x) ((x) > (1.0-EQN_EPS) && (x) < (1.0+EQN_EPS))
|
||||
|
||||
#ifdef NOCBRT
|
||||
#define cbrt(x) ((x) > 0.0 ? pow((double)(x), 1.0/3.0) : \
|
||||
((x) < 0.0 ? -pow((double)-(x), 1.0/3.0) : 0.0))
|
||||
#endif
|
||||
|
||||
int SolveQuadric(double c[3], double s[2])
|
||||
{
|
||||
double p, q, D;
|
||||
|
||||
if (IsZero(c[2]))
|
||||
{
|
||||
if (IsZero(c[1]))
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
s[0] = -c[0] / c[1];
|
||||
return 1;
|
||||
}
|
||||
|
||||
/* normal form: x^2 + px + q = 0 */
|
||||
|
||||
p = c[ 1 ] / (2 * c[ 2 ]);
|
||||
q = c[ 0 ] / c[ 2 ];
|
||||
|
||||
D = p * p - q;
|
||||
|
||||
if (IsZero(D))
|
||||
{
|
||||
s[0] = -p;
|
||||
return 1;
|
||||
}
|
||||
else if (D < 0)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
else /* if (D > 0) */
|
||||
{
|
||||
double sqrt_D = sqrt(D);
|
||||
|
||||
s[0] = sqrt_D - p;
|
||||
s[1] = -sqrt_D - p;
|
||||
return 2;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
int SolveCubic(double c[4], double s[3])
|
||||
{
|
||||
int i, num;
|
||||
double sub;
|
||||
double A, B, C;
|
||||
double sq_A, p, q;
|
||||
double cb_p, D;
|
||||
|
||||
if (IsZero(c[3]))
|
||||
{
|
||||
SolveQuadric(c, s);
|
||||
}
|
||||
|
||||
/* normal form: x^3 + Ax^2 + Bx + C = 0 */
|
||||
|
||||
A = c[2];
|
||||
B = c[1];
|
||||
C = c[0];
|
||||
|
||||
if (!IsOne(c[3]))
|
||||
{
|
||||
A /= c[3];
|
||||
B /= c[3];
|
||||
C /= c[3];
|
||||
}
|
||||
|
||||
/* substitute x = y - A/3 to eliminate quadric term:
|
||||
x^3 +px + q = 0 */
|
||||
|
||||
sq_A = A * A;
|
||||
p = 1.0/3 * (- 1.0/3 * sq_A + B);
|
||||
q = 1.0/2 * (2.0/27 * A * sq_A - 1.0/3 * A * B + C);
|
||||
|
||||
/* use Cardano's formula */
|
||||
|
||||
cb_p = p * p * p;
|
||||
D = q * q + cb_p;
|
||||
|
||||
if (IsZero(D))
|
||||
{
|
||||
if (IsZero(q)) /* one triple solution */
|
||||
{
|
||||
s[ 0 ] = 0;
|
||||
num = 1;
|
||||
}
|
||||
else /* one single and one double solution */
|
||||
{
|
||||
double u = cbrt(-q);
|
||||
s[ 0 ] = 2 * u;
|
||||
s[ 1 ] = - u;
|
||||
num = 2;
|
||||
}
|
||||
}
|
||||
else if (D < 0) /* Casus irreducibilis: three real solutions */
|
||||
{
|
||||
double phi = 1.0 / 3 * acos(-q / sqrt(-cb_p));
|
||||
double t = 2 * sqrt(-p);
|
||||
|
||||
s[0] = t * cos(phi);
|
||||
s[1] = -t * cos(phi + M_PI / 3);
|
||||
s[2] = -t * cos(phi - M_PI / 3);
|
||||
num = 3;
|
||||
}
|
||||
else /* one real solution */
|
||||
{
|
||||
double sqrt_D = sqrt(D);
|
||||
double u = cbrt(sqrt_D - q);
|
||||
double v = - cbrt(sqrt_D + q);
|
||||
|
||||
s[ 0 ] = u + v;
|
||||
num = 1;
|
||||
}
|
||||
|
||||
/* resubstitute */
|
||||
|
||||
sub = 1.0/3 * A;
|
||||
|
||||
for (i = 0; i < num; ++i)
|
||||
s[ i ] -= sub;
|
||||
|
||||
return num;
|
||||
}
|
||||
|
||||
|
||||
int SolveQuartic(double c[5], double s[4])
|
||||
{
|
||||
double coeffs[ 4 ];
|
||||
double z, u, v, sub;
|
||||
double A, B, C, D;
|
||||
double sq_A, p, q, r;
|
||||
int i, num;
|
||||
|
||||
if (IsZero(c[4]))
|
||||
{
|
||||
SolveCubic(c, s);
|
||||
}
|
||||
|
||||
/* normal form: x^4 + Ax^3 + Bx^2 + Cx + D = 0 */
|
||||
|
||||
A = c[3];
|
||||
B = c[2];
|
||||
C = c[1];
|
||||
D = c[0];
|
||||
|
||||
if (!IsOne(c[4]))
|
||||
{
|
||||
A /= c[4];
|
||||
B /= c[4];
|
||||
C /= c[4];
|
||||
D /= c[4];
|
||||
}
|
||||
|
||||
/* substitute x = y - A/4 to eliminate cubic term:
|
||||
x^4 + px^2 + qx + r = 0 */
|
||||
|
||||
sq_A = A * A;
|
||||
p = - 3.0/8 * sq_A + B;
|
||||
q = 1.0/8 * sq_A * A - 1.0/2 * A * B + C;
|
||||
r = - 3.0/256*sq_A*sq_A + 1.0/16*sq_A*B - 1.0/4*A*C + D;
|
||||
|
||||
if (IsZero(r))
|
||||
{
|
||||
/* no absolute term: y(y^3 + py + q) = 0 */
|
||||
|
||||
coeffs[0] = q;
|
||||
coeffs[1] = p;
|
||||
coeffs[2] = 0;
|
||||
coeffs[3] = 1;
|
||||
|
||||
num = SolveCubic(coeffs, s);
|
||||
|
||||
s[num++] = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
/* solve the resolvent cubic ... */
|
||||
|
||||
coeffs[ 0 ] = 1.0/2 * r * p - 1.0/8 * q * q;
|
||||
coeffs[ 1 ] = - r;
|
||||
coeffs[ 2 ] = - 1.0/2 * p;
|
||||
coeffs[ 3 ] = 1;
|
||||
|
||||
(void) SolveCubic(coeffs, s);
|
||||
|
||||
/* ... and take the one real solution ... */
|
||||
|
||||
z = s[ 0 ];
|
||||
|
||||
/* ... to build two quadric equations */
|
||||
|
||||
u = z * z - r;
|
||||
v = 2 * z - p;
|
||||
|
||||
if (IsZero(u))
|
||||
u = 0;
|
||||
else if (u > 0)
|
||||
u = sqrt(u);
|
||||
else
|
||||
return 0;
|
||||
|
||||
if (IsZero(v))
|
||||
v = 0;
|
||||
else if (v > 0)
|
||||
v = sqrt(v);
|
||||
else
|
||||
return 0;
|
||||
|
||||
coeffs[ 0 ] = z - u;
|
||||
coeffs[ 1 ] = q < 0 ? -v : v;
|
||||
coeffs[ 2 ] = 1;
|
||||
|
||||
num = SolveQuadric(coeffs, s);
|
||||
|
||||
coeffs[ 0 ]= z + u;
|
||||
coeffs[ 1 ] = q < 0 ? v : -v;
|
||||
coeffs[ 2 ] = 1;
|
||||
|
||||
num += SolveQuadric(coeffs, s + num);
|
||||
}
|
||||
|
||||
/* resubstitute */
|
||||
|
||||
sub = 1.0/4 * A;
|
||||
|
||||
for (i = 0; i < num; ++i)
|
||||
s[ i ] -= sub;
|
||||
|
||||
return num;
|
||||
}
|
||||
Reference in New Issue
Block a user