612 lines
24 KiB
C++
612 lines
24 KiB
C++
// Copyright Epic Games, Inc. All Rights Reserved.
|
|
|
|
#pragma once
|
|
|
|
#include "BuoyancyTypes.h"
|
|
#include "Chaos/DebugDrawQueue.h"
|
|
#include "DrawDebugHelpers.h"
|
|
#include "Chaos/Particle/ParticleUtilities.h"
|
|
|
|
// Frequently accessed runtime physical values
|
|
struct FBuoyancyPhysicsState
|
|
{
|
|
FBuoyancyPhysicsState()
|
|
: UpDir(FVector::UpVector)
|
|
, ForwardDir(FVector::ForwardVector)
|
|
, RightDir(FVector::RightVector)
|
|
, LinearVelocity(FVector::ZeroVector)
|
|
, AngularVelocityRad(FVector::ZeroVector)
|
|
, ForwardSpeed(0.f)
|
|
, ForwardSpeedKmh(0.f)
|
|
, RightSpeed(0.f)
|
|
, NumPontoonsInWater(0)
|
|
, RiverPontoonIndex(-1)
|
|
, bIsInWaterBody(false)
|
|
{ }
|
|
FVector UpDir;
|
|
FVector ForwardDir;
|
|
FVector RightDir;
|
|
FVector LinearVelocity;
|
|
FVector AngularVelocityRad;
|
|
TArray<TPair<FSphericalPontoon, EBuoyancyEvent>> Events;
|
|
float LinearSpeed;
|
|
float LinearSpeedKmh;
|
|
float ForwardSpeed; // Current speed in the direction of the forward axis of the body instance's transform (not steering forward direction)
|
|
float ForwardSpeedKmh;
|
|
float RightSpeed; // Current speed in the direction of the right axis
|
|
int32 NumPontoonsInWater;
|
|
int32 RiverPontoonIndex;
|
|
bool bIsInWaterBody;
|
|
};
|
|
|
|
struct FBuoyancyComponentBaseAsyncAux : public FBuoyancyComponentAsyncAux
|
|
{
|
|
FBuoyancyAuxData AuxData;
|
|
|
|
FBuoyancyComponentBaseAsyncAux()
|
|
{ }
|
|
};
|
|
struct FBuoyancyComponentBaseAsyncInput : public FBuoyancyComponentAsyncInput
|
|
{
|
|
TArray<UWaterBodyComponent*> WaterBodyComponents;
|
|
TArray<FSphericalPontoon> Pontoons;
|
|
float SmoothedWorldTimeSeconds = 0.f;
|
|
|
|
FBuoyancyComponentBaseAsyncInput()
|
|
: FBuoyancyComponentAsyncInput(EAsyncBuoyancyComponentDataType::AsyncBuoyancyBase)
|
|
{ }
|
|
|
|
FBuoyancyComponentBaseAsyncInput(EAsyncBuoyancyComponentDataType InType)
|
|
: FBuoyancyComponentAsyncInput(InType)
|
|
{ }
|
|
|
|
WATER_API virtual TUniquePtr<struct FBuoyancyComponentAsyncOutput> PreSimulate(UWorld* World, const float DeltaSeconds, const float TotalSeconds, FBuoyancyComponentAsyncAux* Aux, const TMap<UWaterBodyComponent*, TUniquePtr<FSolverSafeWaterBodyData>>& WaterBodyComponentData) const override;
|
|
virtual ~FBuoyancyComponentBaseAsyncInput() = default;
|
|
};
|
|
|
|
struct FBuoyancySimOutput
|
|
{
|
|
FBuoyancySimOutput() = default;
|
|
|
|
FBuoyancySimOutput(const FBuoyancyPhysicsState& State)
|
|
: bIsInWaterBody(State.bIsInWaterBody)
|
|
, Events(State.Events)
|
|
{ }
|
|
|
|
bool bIsInWaterBody;
|
|
TArray<TPair<FSphericalPontoon, EBuoyancyEvent>> Events;
|
|
};
|
|
|
|
struct FBuoyancyComponentBaseAsyncOutput : public FBuoyancyComponentAsyncOutput
|
|
{
|
|
FBuoyancySimOutput SimOutput;
|
|
FBuoyancyAuxData AuxData;
|
|
FBuoyancyComponentBaseAsyncOutput()
|
|
: FBuoyancyComponentAsyncOutput(EAsyncBuoyancyComponentDataType::AsyncBuoyancyBase)
|
|
{ }
|
|
|
|
FBuoyancyComponentBaseAsyncOutput(EAsyncBuoyancyComponentDataType InType)
|
|
: FBuoyancyComponentAsyncOutput(InType)
|
|
{ }
|
|
};
|
|
|
|
class FBuoyancyComponentSim
|
|
{
|
|
public:
|
|
|
|
using TParticleUtilities = Chaos::FParticleUtilitiesXR;
|
|
|
|
FBuoyancyComponentSim()
|
|
{ }
|
|
|
|
template <typename TBody, typename TAux, typename TOut>
|
|
static void Update(const float DeltaSeconds, const float TotalSeconds, const UWorld* World, TBody* Body, const FBuoyancyData& BuoyancyData, TAux& Aux, const TMap<UWaterBodyComponent*, TUniquePtr<FSolverSafeWaterBodyData>>& WaterBodyComponentData, TOut& Out)
|
|
{
|
|
FBuoyancyPhysicsState State;
|
|
FBuoyancyComponentSim::UpdatePhysicsState(Body, State);
|
|
FBuoyancyComponentSim::UpdateBuoyancy(Body, State, BuoyancyData, Aux, WaterBodyComponentData);
|
|
//FBuoyancyComponentSim::UpdateWaterControl(Body, State, BuoyancyData, Aux);
|
|
|
|
if (BuoyancyData.bApplyDragForcesInWater)
|
|
{
|
|
FBuoyancyComponentSim::ApplyLinearDrag(Body, BuoyancyData, State);
|
|
FBuoyancyComponentSim::ApplyAngularDrag(Body, BuoyancyData, State);
|
|
}
|
|
|
|
FBuoyancyComponentSim::ApplyBuoyancy(Body, Aux, State);
|
|
|
|
if (BuoyancyData.bApplyRiverForces)
|
|
{
|
|
State.RiverPontoonIndex = -1;
|
|
|
|
// Attempt to use the specified pontoon
|
|
if (Aux.Pontoons.Num() > BuoyancyData.RiverPontoonIndex)
|
|
{
|
|
const FSolverSafeWaterBodyData* WaterBody = Aux.Pontoons[BuoyancyData.RiverPontoonIndex].SolverWaterBody;
|
|
if (WaterBody && WaterBody->WaterBodyType == EWaterBodyType::River)
|
|
{
|
|
State.RiverPontoonIndex = BuoyancyData.RiverPontoonIndex;
|
|
}
|
|
}
|
|
|
|
// If it is not in a river, use any pontoon that is in a river
|
|
if (State.RiverPontoonIndex == -1)
|
|
{
|
|
for (int i = 0; i < Aux.Pontoons.Num(); ++i)
|
|
{
|
|
const FSolverSafeWaterBodyData* WaterBody = Aux.Pontoons[i].SolverWaterBody;
|
|
if (WaterBody && WaterBody->WaterBodyType == EWaterBodyType::River)
|
|
{
|
|
State.RiverPontoonIndex = i;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (BuoyancyData.bApplyDownstreamAngularRotation)
|
|
{
|
|
FBuoyancyComponentSim::ApplyTorqueForDownstreamAngularRotation(DeltaSeconds, Body, State, BuoyancyData, Aux);
|
|
}
|
|
|
|
FBuoyancyComponentSim::ApplyWaterForce(Body, BuoyancyData, State, Aux, DeltaSeconds);
|
|
}
|
|
|
|
// Copy into the output state
|
|
Out = TOut(State);
|
|
}
|
|
|
|
|
|
// Computes torque to rotate the object downstream
|
|
template <typename TBody, typename TState, typename TAux>
|
|
static void ApplyTorqueForDownstreamAngularRotation(const float DeltaSeconds, TBody* Body, TState& State, const FBuoyancyData& BuoyancyData, TAux& Aux)
|
|
{
|
|
FVector AddedTorque(FVector::ZeroVector);
|
|
|
|
if (State.RiverPontoonIndex != -1 && Aux.Pontoons.Num() > State.RiverPontoonIndex)
|
|
{
|
|
const float RotationModifier = BuoyancyData.DownstreamRotationStrength;
|
|
const float Stiffness = BuoyancyData.DownstreamRotationStiffness;
|
|
const float Damping = BuoyancyData.DownstreamRotationAngularDamping;
|
|
const float MaxAccel = BuoyancyData.DownstreamMaxAcceleration;
|
|
|
|
const FVector DownstreamWaterVelocity = Aux.Pontoons[State.RiverPontoonIndex].WaterVelocity;
|
|
const FVector DesiredRotation = GetWorldTM(Body).TransformVectorNoScale(BuoyancyData.DownstreamAxisOfRotation);
|
|
const FVector AxisOfAlignment = DesiredRotation;
|
|
const FQuat CurUpToTargetUp = FQuat::FindBetweenNormals(AxisOfAlignment, DownstreamWaterVelocity);
|
|
const FVector Axis = CurUpToTargetUp.GetRotationAxis();
|
|
|
|
float Angle = CurUpToTargetUp.GetAngle();
|
|
Angle = FMath::RadiansToDegrees(FMath::UnwindRadians(Angle));
|
|
|
|
float Strength = (Angle * Stiffness - FVector::DotProduct(State.AngularVelocityRad, Axis) * Damping);
|
|
Strength *= RotationModifier;
|
|
Strength = FMath::Clamp(Strength, -MaxAccel, MaxAccel);
|
|
|
|
AddedTorque = Axis * Strength;
|
|
}
|
|
|
|
AddTorque(Body, AddedTorque);
|
|
}
|
|
|
|
static float GetWaterHeight(const TArray<FSolverSafeWaterBodyData*>& WaterBodies, FVector Position, float InWaveReferenceTime, const TMap<const FSolverSafeWaterBodyData*, float>& SplineKeyMap, float DefaultHeight, FSolverSafeWaterBodyData*& OutWaterBody, float& OutWaterDepth, FVector& OutWaterPlaneLocation, FVector& OutWaterPlaneNormal, FVector& OutWaterSurfacePosition, FVector& OutWaterVelocity, int32& OutWaterBodyIdx, bool bShouldIncludeWaves = true)
|
|
{
|
|
float WaterHeight = DefaultHeight;
|
|
OutWaterBody = nullptr;
|
|
OutWaterDepth = 0.f;
|
|
OutWaterPlaneLocation = FVector::ZeroVector;
|
|
OutWaterPlaneNormal = FVector::UpVector;
|
|
|
|
float MaxImmersionDepth = -1.f;
|
|
for (FSolverSafeWaterBodyData* CurrentWaterBody : WaterBodies)
|
|
{
|
|
if (CurrentWaterBody)
|
|
{
|
|
const float SplineInputKey = SplineKeyMap.FindRef(CurrentWaterBody);
|
|
|
|
EWaterBodyQueryFlags QueryFlags =
|
|
EWaterBodyQueryFlags::ComputeLocation
|
|
| EWaterBodyQueryFlags::ComputeNormal
|
|
| EWaterBodyQueryFlags::ComputeImmersionDepth
|
|
| EWaterBodyQueryFlags::ComputeVelocity;
|
|
|
|
if (bShouldIncludeWaves)
|
|
{
|
|
QueryFlags |= EWaterBodyQueryFlags::IncludeWaves;
|
|
}
|
|
|
|
FWaterBodyQueryResult QueryResult = CurrentWaterBody->QueryWaterInfoClosestToWorldLocation(Position, QueryFlags, InWaveReferenceTime, SplineInputKey);
|
|
if (QueryResult.IsInWater() && QueryResult.GetImmersionDepth() > MaxImmersionDepth)
|
|
{
|
|
check(!QueryResult.IsInExclusionVolume());
|
|
WaterHeight = Position.Z + QueryResult.GetImmersionDepth();
|
|
OutWaterBody = CurrentWaterBody;
|
|
if (EnumHasAnyFlags(QueryResult.GetQueryFlags(), EWaterBodyQueryFlags::ComputeDepth))
|
|
{
|
|
OutWaterDepth = QueryResult.GetWaterSurfaceDepth();
|
|
}
|
|
OutWaterPlaneLocation = QueryResult.GetWaterPlaneLocation();
|
|
OutWaterPlaneNormal = QueryResult.GetWaterPlaneNormal();
|
|
OutWaterSurfacePosition = QueryResult.GetWaterSurfaceLocation();
|
|
OutWaterVelocity = QueryResult.GetVelocity();
|
|
OutWaterBodyIdx = CurrentWaterBody ? CurrentWaterBody->WaterBodyIndex : 0;
|
|
MaxImmersionDepth = QueryResult.GetImmersionDepth();
|
|
}
|
|
}
|
|
}
|
|
return WaterHeight;
|
|
}
|
|
|
|
static FVector ComputeWaterForce(FSphericalPontoon& Pontoon, const FBuoyancyData& BuoyancyData, const FVector& BodyVelocity, float DeltaTime)
|
|
{
|
|
FVector FinalAcceleration = FVector::ZeroVector;
|
|
|
|
const FSolverSafeWaterBodyData* WaterBody = Pontoon.SolverWaterBody;
|
|
if (WaterBody && WaterBody->WaterBodyType == EWaterBodyType::River)
|
|
{
|
|
float InputKey = Pontoon.SolverSplineInputKeys[Pontoon.SolverWaterBody];
|
|
const FVector SplinePointLocation = WaterBody->WaterSpline.GetLocationAtSplineInputKey(InputKey);
|
|
FVector LateralPushDirection = FVector::ZeroVector;
|
|
|
|
float LateralDistanceScale = 1.0f;
|
|
|
|
if (BuoyancyData.WaterShorePushFactor >= 0.0f)
|
|
{
|
|
const FVector ShoreDirection = (Pontoon.CenterLocation - SplinePointLocation).GetSafeNormal2D();
|
|
LateralPushDirection = ShoreDirection;
|
|
}
|
|
else
|
|
{
|
|
const FVector CenterRiverLocation = FVector(SplinePointLocation.X, SplinePointLocation.Y, 0);
|
|
const FVector CenterPontoonLocation = FVector(Pontoon.CenterLocation.X, Pontoon.CenterLocation.Y, 0);
|
|
const FVector CenterRiverDirection = (CenterRiverLocation - CenterPontoonLocation).GetSafeNormal2D();
|
|
LateralPushDirection = CenterRiverDirection;
|
|
|
|
const float DistanceFromCenter = FVector::Distance(CenterRiverLocation, CenterPontoonLocation);
|
|
const float DistanceFromPath = DistanceFromCenter - BuoyancyData.RiverTraversalPathWidth;
|
|
|
|
if (DistanceFromPath > 0)
|
|
{
|
|
// Apply a force inwards towards the path
|
|
LateralDistanceScale = FMath::Lerp(0.0f, 1.0f, DistanceFromPath / BuoyancyData.RiverTraversalPathWidth);
|
|
}
|
|
else
|
|
{
|
|
// Apply a drag to any horizontal movement for stabilization within the path. Stronger drag closer to center of path, and at faster speeds.
|
|
const float BodySpeedInLateralDir = FVector::DotProduct(BodyVelocity, LateralPushDirection.GetSafeNormal());
|
|
const float BodySpeedLateralDrag = -FMath::Lerp(0.0f, 1.0f, BodySpeedInLateralDir / BuoyancyData.MaxWaterForce);
|
|
const float CentralProximityDragMultiplier = FMath::Lerp(0.0f, 1.0f, (2.0f * DistanceFromCenter) / BuoyancyData.RiverTraversalPathWidth);
|
|
LateralDistanceScale = BodySpeedLateralDrag * CentralProximityDragMultiplier;
|
|
}
|
|
}
|
|
|
|
|
|
const FVector WaterVelocity = WaterBody->GetWaterVelocityVectorAtSplineInputKey(InputKey) * BuoyancyData.WaterVelocityStrength;
|
|
const float RiverWaterSpeed = WaterBody->GetWaterVelocityAtSplineInputKey(InputKey);
|
|
const float BodySpeedInWater = FVector::DotProduct(BodyVelocity, WaterVelocity.GetSafeNormal());
|
|
const float BodySpeedInWaterDir = BuoyancyData.bAllowCurrentWhenMovingFastUpstream ? BodySpeedInWater : FMath::Abs(BodySpeedInWater);
|
|
const bool bApplyRiverForces = (BodySpeedInWaterDir < RiverWaterSpeed);
|
|
|
|
if (bApplyRiverForces || BuoyancyData.bAlwaysAllowLateralPush)
|
|
{
|
|
const FVector LateralVelocity = LateralPushDirection * LateralDistanceScale * FMath::Abs(BuoyancyData.WaterShorePushFactor);
|
|
const FVector LateralAcceleration = LateralVelocity / DeltaTime;
|
|
FinalAcceleration = FinalAcceleration + LateralAcceleration.GetClampedToSize(-BuoyancyData.MaxShorePushForce, BuoyancyData.MaxShorePushForce);
|
|
}
|
|
|
|
if (bApplyRiverForces)
|
|
{
|
|
const FVector WaterAcceleration = (WaterVelocity / DeltaTime);
|
|
FinalAcceleration = FinalAcceleration + WaterAcceleration.GetClampedToSize(-BuoyancyData.MaxWaterForce, BuoyancyData.MaxWaterForce);
|
|
}
|
|
}
|
|
|
|
return FinalAcceleration;
|
|
}
|
|
|
|
static void ComputeBuoyancy(const FBuoyancyData& BuoyancyData, FSphericalPontoon& Pontoon, float ForwardSpeedKmh, float VelocityZ)
|
|
{
|
|
auto ComputeBuoyantForce = [&](FVector CenterLocation, float Radius, float InBuoyancyCoefficient, float CurrentWaterLevel) -> float
|
|
{
|
|
const float Bottom = CenterLocation.Z - Radius;
|
|
const float SubDiff = FMath::Clamp(CurrentWaterLevel - Bottom, 0.f, 2.f * Radius);
|
|
|
|
// The following was obtained by integrating the volume of a sphere
|
|
// over a linear section of SubmersionDiff length.
|
|
static const float Pi = (float)PI;
|
|
const float SubDiffSq = SubDiff * SubDiff;
|
|
const float SubVolume = (Pi / 3.f) * SubDiffSq * ((3.f * Radius) - SubDiff);
|
|
|
|
//#if ENABLE_DRAW_DEBUG
|
|
// if (CVarWaterDebugBuoyancy.GetValueOnAnyThread())
|
|
// {
|
|
// const FVector WaterPoint = FVector(CenterLocation.X, CenterLocation.Y, CurrentWaterLevel);
|
|
// DrawDebugLine(GetWorld(), WaterPoint - 50.f * FVector::ForwardVector, WaterPoint + 50.f * FVector::ForwardVector, FColor::Blue, false, -1.f, 0, 3.f);
|
|
// DrawDebugLine(GetWorld(), WaterPoint - 50.f * FVector::RightVector, WaterPoint + 50.f * FVector::RightVector, FColor::Blue, false, -1.f, 0, 3.f);
|
|
// }
|
|
//#endif
|
|
const float FirstOrderDrag = BuoyancyData.BuoyancyDamp * VelocityZ;
|
|
const float SecondOrderDrag = FMath::Sign(VelocityZ) * BuoyancyData.BuoyancyDamp2 * VelocityZ * VelocityZ;
|
|
const float DampingFactor = -FMath::Max(FirstOrderDrag + SecondOrderDrag, 0.f);
|
|
// The buoyant force scales with submersed volume
|
|
return SubVolume * (InBuoyancyCoefficient)+DampingFactor;
|
|
};
|
|
|
|
const float MinVelocity = BuoyancyData.BuoyancyRampMinVelocity;
|
|
const float MaxVelocity = BuoyancyData.BuoyancyRampMaxVelocity;
|
|
const float RampFactor = FMath::Clamp((ForwardSpeedKmh - MinVelocity) / (MaxVelocity - MinVelocity), 0.f, 1.f);
|
|
const float BuoyancyRamp = RampFactor * (BuoyancyData.BuoyancyRampMax - 1);
|
|
float BuoyancyCoefficientWithRamp = BuoyancyData.BuoyancyCoefficient * (1 + BuoyancyRamp);
|
|
|
|
const float BuoyantForce = FMath::Clamp(ComputeBuoyantForce(Pontoon.CenterLocation, Pontoon.Radius, BuoyancyCoefficientWithRamp, Pontoon.WaterHeight), 0.f, BuoyancyData.MaxBuoyantForce);
|
|
Pontoon.LocalForce = FVector::UpVector * BuoyantForce * Pontoon.PontoonCoefficient;
|
|
}
|
|
|
|
template <typename TBody, typename TState, typename TAux>
|
|
static void UpdateBuoyancy(const TBody* Body, TState& State, const FBuoyancyData& BuoyancyData, TAux& Aux, const TMap<UWaterBodyComponent*, TUniquePtr<FSolverSafeWaterBodyData>>& WaterBodyComponentData)
|
|
{
|
|
State.NumPontoonsInWater = 0;
|
|
|
|
TArray<FSolverSafeWaterBodyData*> SolverWaterBodies;
|
|
for (const UWaterBodyComponent* WaterBodyComponent : Aux.WaterBodyComponents)
|
|
{
|
|
if (const TUniquePtr<FSolverSafeWaterBodyData>* WaterDataPtrPtr = WaterBodyComponentData.Find(WaterBodyComponent))
|
|
{
|
|
FSolverSafeWaterBodyData& SolverWaterBody = **WaterDataPtrPtr;
|
|
SolverWaterBodies.Add(&SolverWaterBody);
|
|
}
|
|
}
|
|
|
|
int PontoonIndex = 0;
|
|
for (FSphericalPontoon& Pontoon : Aux.Pontoons)
|
|
{
|
|
//if (PontoonConfiguration & (1 << PontoonIndex))
|
|
{
|
|
const Chaos::FRigidTransform3 CurrentTransform = TParticleUtilities::GetActorWorldTransform(Body);
|
|
if (Pontoon.bUseCenterSocket)
|
|
{
|
|
Pontoon.CenterLocation = CurrentTransform.TransformPosition(Pontoon.SocketTransform.GetLocation()) + Pontoon.Offset;
|
|
Pontoon.SocketRotation = CurrentTransform.TransformRotation(Pontoon.SocketTransform.GetRotation());
|
|
}
|
|
else
|
|
{
|
|
Pontoon.CenterLocation = CurrentTransform.TransformPosition(Pontoon.RelativeLocation);
|
|
}
|
|
|
|
//GetWaterSplineKey(Pontoon.CenterLocation, Pontoon.SplineInputKeys, Pontoon.SplineSegments);
|
|
{
|
|
Pontoon.SolverSplineInputKeys.Reset();
|
|
for (const FSolverSafeWaterBodyData* WaterBody : SolverWaterBodies)
|
|
{
|
|
if (WaterBody && WaterBody->WaterBodyType == EWaterBodyType::River)
|
|
{
|
|
float SplineInputKey;
|
|
//if (CVarWaterUseSplineKeyOptimization.GetValueOnAnyThread())
|
|
//{
|
|
// SplineInputKey = GetWaterSplineKeyFast(Location, WaterBody, OutSegmentMap);
|
|
//}
|
|
//else
|
|
{
|
|
SplineInputKey = WaterBody->WaterSpline.FindInputKeyClosestToWorldLocation(Pontoon.CenterLocation);
|
|
}
|
|
Pontoon.SolverSplineInputKeys.Add(WaterBody, SplineInputKey);
|
|
}
|
|
}
|
|
}
|
|
|
|
const FVector PontoonBottom = Pontoon.CenterLocation - FVector(0, 0, Pontoon.Radius);
|
|
const float DefaultHeight = -100000.f;
|
|
/*Pass in large negative default value so we don't accidentally assume we're in water when we're not.*/
|
|
Pontoon.WaterHeight = GetWaterHeight(SolverWaterBodies, PontoonBottom - FVector::UpVector * 100.f, Aux.SmoothedWorldTimeSeconds, Pontoon.SolverSplineInputKeys, DefaultHeight, Pontoon.SolverWaterBody, Pontoon.WaterDepth, Pontoon.WaterPlaneLocation, Pontoon.WaterPlaneNormal, Pontoon.WaterSurfacePosition, Pontoon.WaterVelocity, Pontoon.WaterBodyIndex);
|
|
|
|
const bool bPrevIsInWater = Pontoon.bIsInWater;
|
|
const float ImmersionDepth = Pontoon.WaterHeight - PontoonBottom.Z;
|
|
/*check if the pontoon is currently in water*/
|
|
if (ImmersionDepth >= 0.f)
|
|
{
|
|
Pontoon.bIsInWater = true;
|
|
Pontoon.ImmersionDepth = ImmersionDepth;
|
|
State.NumPontoonsInWater++;
|
|
}
|
|
else
|
|
{
|
|
Pontoon.bIsInWater = false;
|
|
Pontoon.ImmersionDepth = 0.f;
|
|
}
|
|
|
|
ComputeBuoyancy(BuoyancyData, Pontoon, State.ForwardSpeedKmh, State.LinearVelocity.Z);
|
|
|
|
if (Pontoon.bIsInWater && !bPrevIsInWater)
|
|
{
|
|
Pontoon.SplineSegments.Reset();
|
|
// BlueprintImplementables don't really work on the actor component level unfortunately, so call back in to the function defined on the actor itself.
|
|
//OnPontoonEnteredWater(Pontoon);
|
|
State.Events.Add(MakeTuple(Pontoon, EBuoyancyEvent::EnteredWaterBody));
|
|
}
|
|
if (!Pontoon.bIsInWater && bPrevIsInWater)
|
|
{
|
|
Pontoon.SplineSegments.Reset();
|
|
State.Events.Add(MakeTuple(Pontoon, EBuoyancyEvent::ExitedWaterBody));
|
|
//OnPontoonExitedWater(Pontoon);
|
|
}
|
|
}
|
|
PontoonIndex++;
|
|
}
|
|
#if CHAOS_DEBUG_DRAW
|
|
if (CVarWaterDebugBuoyancy.GetValueOnAnyThread())
|
|
{
|
|
const float NumPoints = CVarWaterBuoyancyDebugPoints.GetValueOnAnyThread();
|
|
const float Size = CVarWaterBuoyancyDebugSize.GetValueOnAnyThread();
|
|
const float StartOffset = NumPoints * 0.5f;
|
|
const float Scale = Size / NumPoints;
|
|
TMap<const FSolverSafeWaterBodyData*, float> DebugSplineKeyMap;
|
|
Chaos::FDebugDrawQueue::GetInstance().SetEnabled(true);
|
|
for (int i = 0; i < NumPoints; ++i)
|
|
{
|
|
for (int j = 0; j < NumPoints; ++j)
|
|
{
|
|
FVector Location = TParticleUtilities::GetCoMWorldPosition(Body) + (FVector::RightVector * (i - StartOffset) * Scale) + (FVector::ForwardVector * (j - StartOffset) * Scale);
|
|
|
|
FSphericalPontoon DummyPontoon;
|
|
for (const FSolverSafeWaterBodyData* WaterBody : SolverWaterBodies)
|
|
{
|
|
if (WaterBody && WaterBody->WaterBodyType == EWaterBodyType::River)
|
|
{
|
|
float SplineInputKey;
|
|
//if (CVarWaterUseSplineKeyOptimization.GetValueOnAnyThread())
|
|
//{
|
|
// SplineInputKey = GetWaterSplineKeyFast(Location, WaterBody, OutSegmentMap);
|
|
//}
|
|
//else
|
|
{
|
|
SplineInputKey = WaterBody->WaterSpline.FindInputKeyClosestToWorldLocation(Location);
|
|
}
|
|
DummyPontoon.SolverSplineInputKeys.Add(WaterBody, SplineInputKey);
|
|
}
|
|
}
|
|
|
|
|
|
const float WaterHeight = GetWaterHeight(SolverWaterBodies, Location - FVector::UpVector * 100.f, Aux.SmoothedWorldTimeSeconds, DummyPontoon.SolverSplineInputKeys, /*DefaultHeight*/-100000.f, DummyPontoon.SolverWaterBody, DummyPontoon.WaterDepth, DummyPontoon.WaterPlaneLocation, DummyPontoon.WaterPlaneNormal, DummyPontoon.WaterSurfacePosition, DummyPontoon.WaterVelocity, DummyPontoon.WaterBodyIndex);
|
|
const FVector DebugPoint(Location.X, Location.Y, WaterHeight);
|
|
Chaos::FDebugDrawQueue::GetInstance().DrawDebugPoint(DebugPoint, FColor::Green, false, -1.f, 0, 5.f);
|
|
}
|
|
}
|
|
|
|
for (FSphericalPontoon& Pontoon : Aux.Pontoons)
|
|
{
|
|
Chaos::FDebugDrawQueue::GetInstance().DrawDebugSphere(Pontoon.CenterLocation, Pontoon.Radius, 16, Pontoon.bIsInWater ? FColor::Blue : FColor::Red, false, -1.f, 0, 1.f);
|
|
}
|
|
}
|
|
#endif
|
|
|
|
State.bIsInWaterBody = State.NumPontoonsInWater > 0;
|
|
}
|
|
|
|
template <typename TBody, typename TAux, typename TState>
|
|
static void ApplyBuoyancy(TBody* Body, TAux& Aux, const TState& State)
|
|
{
|
|
//int PontoonIndex = 0;
|
|
for (const FSphericalPontoon& Pontoon : Aux.Pontoons)
|
|
{
|
|
//if (PontoonConfiguration & (1 << PontoonIndex))
|
|
//{
|
|
AddForceAtPositionWorld(Body, Pontoon.LocalForce, Pontoon.CenterLocation);
|
|
//}
|
|
//PontoonIndex++;
|
|
}
|
|
}
|
|
|
|
template <typename TBody, typename TState>
|
|
static void UpdatePhysicsState(const TBody* Body, TState& State)
|
|
{
|
|
State.LinearVelocity = GetVelocity(Body);
|
|
State.AngularVelocityRad = GetAngularVelocity(Body);
|
|
State.LinearSpeed = State.LinearVelocity.Size();
|
|
State.LinearSpeedKmh = ToKmH(State.LinearSpeed);
|
|
State.ForwardDir = GetWorldTM(Body).GetUnitAxis(EAxis::X);
|
|
State.RightDir = GetWorldTM(Body).GetUnitAxis(EAxis::Y);
|
|
State.UpDir = GetWorldTM(Body).GetUnitAxis(EAxis::Z);
|
|
State.ForwardSpeed = FVector::DotProduct(State.ForwardDir, State.LinearVelocity);
|
|
State.ForwardSpeedKmh = ToKmH(State.ForwardSpeed);
|
|
State.RightSpeed = FVector::DotProduct(State.RightDir, State.LinearVelocity);
|
|
}
|
|
|
|
template <typename TBody>
|
|
static FVector GetVelocity(const TBody* Body)
|
|
{
|
|
return Body->V();
|
|
}
|
|
|
|
template <typename TBody>
|
|
static FVector GetAngularVelocity(const TBody* Body)
|
|
{
|
|
return Body->W();
|
|
}
|
|
|
|
static float ToKmH(float Speed)
|
|
{
|
|
return Speed * 0.036f;
|
|
}
|
|
|
|
template <typename TBody>
|
|
static FTransform GetWorldTM(const TBody* Body)
|
|
{
|
|
//return FTransform::Identity;
|
|
return TParticleUtilities::GetActorWorldTransform(Body);
|
|
}
|
|
|
|
template <typename TBody>
|
|
static void AddForce(TBody* Body, const FVector& Force)
|
|
{
|
|
if (ensure(!Force.ContainsNaN()))
|
|
{
|
|
Body->AddForce((Force * Body->M()));
|
|
}
|
|
}
|
|
|
|
template <typename TBody>
|
|
static void AddForceAtPositionWorld(TBody* Body, const FVector& WorldForce, const FVector& WorldPosition)
|
|
{
|
|
if (ensure(!WorldForce.ContainsNaN() && !WorldPosition.ContainsNaN()))
|
|
{
|
|
const Chaos::FVec3 WorldCOM = TParticleUtilities::GetCoMWorldPosition(Body);
|
|
const Chaos::FVec3 WorldTorque = Chaos::FVec3::CrossProduct(WorldPosition - WorldCOM, WorldForce);
|
|
Body->AddForce(WorldForce);
|
|
Body->AddTorque(WorldTorque);
|
|
}
|
|
}
|
|
|
|
template <typename TBody>
|
|
static void AddTorque(TBody* Body, const FVector& TorqueRadians)
|
|
{
|
|
if (ensure(!TorqueRadians.ContainsNaN()))
|
|
{
|
|
Body->AddTorque((TParticleUtilities::GetWorldInertia(Body) * TorqueRadians));
|
|
}
|
|
}
|
|
|
|
template <typename TBody, typename TState, typename TAux>
|
|
static void ApplyWaterForce(TBody* Body, const FBuoyancyData& BuoyancyData, const TState& State, TAux& Aux, float DeltaSeconds)
|
|
{
|
|
FVector WaterForce = FVector::ZeroVector;
|
|
|
|
if (State.RiverPontoonIndex != -1 && Aux.Pontoons.Num() > State.RiverPontoonIndex)
|
|
{
|
|
WaterForce = ComputeWaterForce(Aux.Pontoons[State.RiverPontoonIndex], BuoyancyData, State.LinearVelocity, DeltaSeconds);
|
|
}
|
|
|
|
AddForce(Body, WaterForce);
|
|
}
|
|
|
|
template <typename TBody, typename TState>
|
|
static void ApplyLinearDrag(TBody* Body, const FBuoyancyData& BuoyancyData, const TState& State)
|
|
{
|
|
if (State.bIsInWaterBody)
|
|
{
|
|
FVector DragForce = FVector::ZeroVector;
|
|
|
|
FVector PlaneVelocity = State.LinearVelocity;
|
|
PlaneVelocity.Z = 0;
|
|
const FVector VelocityDir = PlaneVelocity.GetSafeNormal();
|
|
const float SpeedKmh = ToKmH(PlaneVelocity.Size());
|
|
const float ClampedSpeed = FMath::Clamp(SpeedKmh, -BuoyancyData.MaxDragSpeed, BuoyancyData.MaxDragSpeed);
|
|
|
|
const float Resistance = ClampedSpeed * BuoyancyData.DragCoefficient;
|
|
DragForce += -Resistance * VelocityDir;
|
|
|
|
const float Resistance2 = ClampedSpeed * ClampedSpeed * BuoyancyData.DragCoefficient2;
|
|
DragForce += -Resistance2 * VelocityDir * FMath::Sign(SpeedKmh);
|
|
AddForce(Body, DragForce);
|
|
}
|
|
}
|
|
|
|
template <typename TBody, typename TState>
|
|
static void ApplyAngularDrag(TBody* Body, const FBuoyancyData& BuoyancyData, const TState& State)
|
|
{
|
|
if (State.bIsInWaterBody)
|
|
{
|
|
const FVector DragTorque = -State.AngularVelocityRad * BuoyancyData.AngularDragCoefficient;
|
|
AddTorque(Body, DragTorque);
|
|
}
|
|
}
|
|
};
|