FVector UTKMathFunctionLibrary::GetVelocityAtPoint(UPrimitiveComponent* Target, FVector Point, FName BoneName, bool DrawDebugInfo)
{
//FTransform Transform = Target->GetComponentTransform();
//FVector LocalLinearVelocity = Transform.InverseTransformVectorNoScale(Target->GetPhysicsLinearVelocity());
//FVector LocalAngularVelocity = Transform.InverseTransformVectorNoScale(Target->GetPhysicsAngularVelocity());
//FVector ResultPointVelocity = LocalLinearVelocity + FVector::CrossProduct(FVector::DegreesToRadians(LocalAngularVelocity), Transform.InverseTransformVectorNoScale(Point - Target->GetCenterOfMass()));
if (!Target) return FVector::ZeroVector;
//You can actually get it from the physx body instance instead.
FBodyInstance* BI = Target->GetBodyInstance(BoneName);
if (BI && BI->IsValidBodyInstance())
{
FVector PointVelocity = BI->GetUnrealWorldVelocityAtPoint(Point);
UWorld* TheWorld = Target->GetWorld();
if (DrawDebugInfo && TheWorld)
{
FColor DefaultColor(255,200,0);
DrawDebugPoint(TheWorld, Point, 10, DefaultColor);
DrawDebugString(TheWorld, Point, FString::SanitizeFloat(PointVelocity.Size()), NULL, FColor::White, 0.0f);
}
return PointVelocity;
}
return FVector::ZeroVector;
}
static bool HasteTrace(UWorld* InWorld, FHitResult& OutHit, FVector InStart, FVector InEnd, FName InTraceTag, bool InbReturnFaceIndex = false)
{
FCollisionQueryParams QueryParams(InTraceTag, true);
QueryParams.bReturnFaceIndex = InbReturnFaceIndex;
bool bResult = true;
while (true)
{
bResult = InWorld->LineTraceSingleByChannel(OutHit, InStart, InEnd, ECC_WorldStatic, QueryParams);
if (bResult)
{
// In the editor traces can hit "No Collision" type actors, so ugh.
FBodyInstance* BodyInstance = OutHit.Component->GetBodyInstance();
if (BodyInstance->GetCollisionEnabled() != ECollisionEnabled::QueryAndPhysics || BodyInstance->GetResponseToChannel(ECC_WorldStatic) != ECR_Block)
{
AActor* Actor = OutHit.Actor.Get();
if (Actor)
{
QueryParams.AddIgnoredActor(Actor);
}
InStart = OutHit.ImpactPoint;
continue;
}
}
break;
}
return bResult;
}
bool UCollisionProfile::ReadConfig(FName ProfileName, FBodyInstance& BodyInstance) const
{
FCollisionResponseTemplate Template;
// first check redirect
// if that fails, just get profile
if ( CheckRedirect(ProfileName, BodyInstance, Template) ||
GetProfileTemplate(ProfileName, Template) )
{
// note that this can be called during loading or run-time (because of the function)
// from property, it just uses property handle to set all data
// but we can't use functions - i.e. SetCollisionEnabled -
// which will reset ProfileName by default
BodyInstance.CollisionEnabled = Template.CollisionEnabled;
BodyInstance.ObjectType = Template.ObjectType;
BodyInstance.CollisionResponses.SetCollisionResponseContainer(Template.ResponseToChannels);
BodyInstance.ResponseToChannels_DEPRECATED = Template.ResponseToChannels;
// if valid instance, make sure to update physics filter data
if (BodyInstance.IsValidBodyInstance())
{
BodyInstance.UpdatePhysicsFilterData();
}
return true;
}
return false;
}
FVector USkeletalMeshComponent::GetClosestCollidingRigidBodyLocation(const FVector& TestLocation) const
{
float BestDistSq = BIG_NUMBER;
FVector Best = TestLocation;
UPhysicsAsset* PhysicsAsset = GetPhysicsAsset();
if( PhysicsAsset )
{
for (int32 i=0; i<Bodies.Num(); i++)
{
FBodyInstance* BodyInstance = Bodies[i];
if( BodyInstance && BodyInstance->IsValidBodyInstance() && (BodyInstance->GetCollisionEnabled() != ECollisionEnabled::NoCollision) )
{
const FVector BodyLocation = BodyInstance->GetUnrealWorldTransform().GetTranslation();
const float DistSq = (BodyLocation - TestLocation).SizeSquared();
if( DistSq < BestDistSq )
{
Best = BodyLocation;
BestDistSq = DistSq;
}
}
}
}
return Best;
}
bool UPrimitiveComponent::WeldToImplementation(USceneComponent * InParent, FName ParentSocketName /* = Name_None */, bool bWeldSimulatedChild /* = false */)
{
//WeldToInternal assumes attachment is already done
if (AttachParent != InParent || AttachSocketName != ParentSocketName)
{
return false;
}
//Check that we can actually our own socket name
FBodyInstance* BI = GetBodyInstance(NAME_None, false);
if (BI == NULL)
{
return false;
}
if (BI->ShouldInstanceSimulatingPhysics() && bWeldSimulatedChild == false)
{
return false;
}
UnWeldFromParent(); //make sure to unweld from wherever we currently are
FName SocketName;
UPrimitiveComponent * RootComponent = GetRootWelded(this, ParentSocketName, &SocketName, true);
if (RootComponent)
{
if (FBodyInstance* RootBI = RootComponent->GetBodyInstance(SocketName, false))
{
if (BI->WeldParent == RootBI) //already welded so stop
{
return true;
}
BI->bWelded = true;
//There are multiple cases to handle:
//Root is kinematic, simulated
//Child is kinematic, simulated
//Child always inherits from root
//if root is kinematic simply set child to be kinematic and we're done
if (RootComponent->IsSimulatingPhysics(SocketName) == false)
{
BI->WeldParent = NULL;
SetSimulatePhysics(false);
return false; //return false because we need to continue with regular body initialization
}
//root is simulated so we actually weld the body
FTransform RelativeTM = RootComponent == AttachParent ? GetRelativeTransform() : GetComponentToWorld().GetRelativeTransform(RootComponent->GetComponentToWorld()); //if direct parent we already have relative. Otherwise compute it
RootBI->Weld(BI, GetComponentToWorld());
BI->WeldParent = RootBI;
return true;
}
}
return false;
}
void UPrimitiveComponent::WakeRigidBody(FName BoneName)
{
FBodyInstance* BI = GetBodyInstance(BoneName);
if(BI)
{
BI->WakeInstance();
}
}
void UPrimitiveComponent::SetEnableGravity(bool bGravityEnabled)
{
FBodyInstance* BI = GetBodyInstance();
if (BI)
{
BI->SetEnableGravity(bGravityEnabled);
}
}
void UPrimitiveComponent::SetMassScale(FName BoneName, float InMassScale)
{
FBodyInstance* BI = GetBodyInstance(BoneName);
if (BI)
{
BI->SetMassScale(InMassScale);
}
}
void UPrimitiveComponent::PutRigidBodyToSleep(FName BoneName)
{
FBodyInstance* BI = GetBodyInstance(BoneName);
if(BI)
{
BI->PutInstanceToSleep();
}
}
FVector UBuoyancyComponent::GetVelocityAtPoint(UPrimitiveComponent* Target, FVector Point, FName BoneName)
{
FBodyInstance* BI = Target->GetBodyInstance(BoneName);
if (BI != NULL && BI->IsValidBodyInstance())
{
return BI->GetUnrealWorldVelocityAtPoint(Point);
}
return FVector::ZeroVector;
}
FVector UPrimitiveComponent::GetPhysicsLinearVelocity(FName BoneName)
{
FBodyInstance* BI = GetBodyInstance(BoneName);
if(BI != NULL)
{
return BI->GetUnrealWorldVelocity();
}
return FVector(0,0,0);
}
void UPrimitiveComponent::SetAngularDamping(float InDamping)
{
FBodyInstance* BI = GetBodyInstance();
if (BI)
{
BI->AngularDamping = InDamping;
BI->UpdateDampingProperties();
}
}
bool UPrimitiveComponent::RigidBodyIsAwake(FName BoneName)
{
FBodyInstance* BI = GetBodyInstance(BoneName);
if(BI)
{
return BI->IsInstanceAwake();
}
return false;
}
float UPrimitiveComponent::GetMass() const
{
FBodyInstance* BI = GetBodyInstance();
if (BI)
{
return BI->GetBodyMass();
}
return 0.0f;
}
void UTKMathFunctionLibrary::SetCenterOfMassOffset(UPrimitiveComponent* Target, FVector Offset, FName BoneName)
{
if (!Target) return;
FBodyInstance* BI = Target->GetBodyInstance(BoneName);
if (BI && BI->IsValidBodyInstance())
{
BI->COMNudge = Offset;
BI->UpdateMassProperties();
}
}
void UPrimitiveComponent::SetConstraintMode(EDOFMode::Type ConstraintMode)
{
FBodyInstance * RootBI = GetBodyInstance(NAME_None, false);
if (RootBI == NULL || IsPendingKill())
{
return;
}
RootBI->SetDOFLock(ConstraintMode);
}
bool UPrimitiveComponent::IsSimulatingPhysics(FName BoneName) const
{
FBodyInstance* BodyInst = GetBodyInstance(BoneName);
if(BodyInst != NULL)
{
return BodyInst->IsInstanceSimulatingPhysics();
}
else
{
return false;
}
}
float UPrimitiveComponent::GetDistanceToCollision(const FVector& Point, FVector& ClosestPointOnCollision) const
{
ClosestPointOnCollision=Point;
FBodyInstance* BodyInst = GetBodyInstance();
if(BodyInst != NULL)
{
return BodyInst->GetDistanceToBody(Point, ClosestPointOnCollision);
}
return -1.f;
}
FVector UPrimitiveComponent::GetComponentVelocity() const
{
if (IsSimulatingPhysics())
{
FBodyInstance* BodyInst = GetBodyInstance();
if(BodyInst != NULL)
{
return BodyInst->GetUnrealWorldVelocity();
}
}
return Super::GetComponentVelocity();
}
void UPrimitiveComponent::AddRadialForce(FVector Origin, float Radius, float Strength, ERadialImpulseFalloff Falloff, bool bAccelChange)
{
if(bIgnoreRadialForce)
{
return;
}
FBodyInstance* BI = GetBodyInstance();
if (BI)
{
BI->AddRadialForceToBody(Origin, Radius, Strength, Falloff, bAccelChange);
}
}
bool UGripMotionControllerComponent::TeleportMoveGrippedActor(AActor * GrippedActorToMove)
{
if (!GrippedActorToMove || !GrippedActors.Num())
return false;
FTransform WorldTransform;
FTransform InverseTransform = this->GetComponentTransform().Inverse();
for (int i = GrippedActors.Num() - 1; i >= 0; --i)
{
if (GrippedActors[i].Actor == GrippedActorToMove)
{
// GetRelativeTransformReverse had some serious f*****g floating point errors associated with it that was f*****g everything up
// Not sure whats wrong with the function but I might want to push a patch out eventually
WorldTransform = GrippedActors[i].RelativeTransform.GetRelativeTransform(InverseTransform);
// Need to use WITH teleport for this function so that the velocity isn't updated and without sweep so that they don't collide
GrippedActors[i].Actor->SetActorTransform(WorldTransform, false, NULL, ETeleportType::TeleportPhysics);
FBPActorPhysicsHandleInformation * Handle = GetPhysicsGrip(GrippedActors[i]);
if (Handle && Handle->KinActorData)
{
{
PxScene* PScene = GetPhysXSceneFromIndex(Handle->SceneIndex);
if (PScene)
{
SCOPED_SCENE_WRITE_LOCK(PScene);
Handle->KinActorData->setKinematicTarget(PxTransform(U2PVector(WorldTransform.GetLocation()), Handle->KinActorData->getGlobalPose().q));
Handle->KinActorData->setGlobalPose(PxTransform(U2PVector(WorldTransform.GetLocation()), Handle->KinActorData->getGlobalPose().q));
}
}
//Handle->KinActorData->setGlobalPose(PxTransform(U2PVector(WorldTransform.GetLocation()), Handle->KinActorData->getGlobalPose().q));
UPrimitiveComponent *root = Cast<UPrimitiveComponent>(GrippedActors[i].Actor->GetRootComponent());
if (root)
{
FBodyInstance * body = root->GetBodyInstance();
if (body)
{
body->SetBodyTransform(WorldTransform, ETeleportType::TeleportPhysics);
}
}
}
return true;
}
}
return false;
}
void FPhysSubstepTask::SubstepInterpolation(float InAlpha, float DeltaTime)
{
#if WITH_PHYSX
#if WITH_APEX
SCOPED_APEX_SCENE_WRITE_LOCK(PAScene);
PxScene * PScene = PAScene->getPhysXScene();
#else
PxScene * PScene = PAScene;
SCOPED_SCENE_WRITE_LOCK(PScene);
#endif
/** Note: We lock the entire scene before iterating. The assumption is that removing an FBodyInstance from the map will also be wrapped by this lock */
PhysTargetMap& Targets = PhysTargetBuffers[!External];
for (PhysTargetMap::TIterator Itr = Targets.CreateIterator(); Itr; ++Itr)
{
FPhysTarget & PhysTarget = Itr.Value();
FBodyInstance * BodyInstance = Itr.Key();
PxRigidBody* PRigidBody = BodyInstance->GetPxRigidBody_AssumesLocked();
if (PRigidBody == NULL)
{
continue;
}
//We should only be iterating over actors that belong to this scene
check(PRigidBody->getScene() == PScene);
if (!IsKinematicHelper(PRigidBody))
{
ApplyCustomPhysics(PhysTarget, BodyInstance, DeltaTime);
ApplyForces_AssumesLocked(PhysTarget, BodyInstance);
ApplyTorques_AssumesLocked(PhysTarget, BodyInstance);
ApplyRadialForces_AssumesLocked(PhysTarget, BodyInstance);
}else
{
InterpolateKinematicActor_AssumesLocked(PhysTarget, BodyInstance, InAlpha);
}
}
/** Final substep */
if (InAlpha >= 1.f)
{
Targets.Empty(Targets.Num());
}
#endif
}
void FPhysScene::SyncComponentsToBodies(uint32 SceneType)
{
#if WITH_PHYSX
PxScene* PScene = GetPhysXScene(SceneType);
check(PScene);
SCENE_LOCK_READ(PScene);
PxU32 NumTransforms = 0;
const PxActiveTransform* PActiveTransforms = PScene->getActiveTransforms(NumTransforms);
SCENE_UNLOCK_READ(PScene);
for(PxU32 TransformIdx=0; TransformIdx<NumTransforms; TransformIdx++)
{
const PxActiveTransform& PActiveTransform = PActiveTransforms[TransformIdx];
FBodyInstance* BodyInst = FPhysxUserData::Get<FBodyInstance>(PActiveTransform.userData);
if( BodyInst != NULL &&
BodyInst->InstanceBodyIndex == INDEX_NONE &&
BodyInst->OwnerComponent != NULL &&
BodyInst->IsInstanceSimulatingPhysics() )
{
check(BodyInst->OwnerComponent->IsRegistered()); // shouldn't have a physics body for a non-registered component!
AActor* Owner = BodyInst->OwnerComponent->GetOwner();
// See if the transform is actually different, and if so, move the component to match physics
const FTransform NewTransform = BodyInst->GetUnrealWorldTransform();
if(!NewTransform.EqualsNoScale(BodyInst->OwnerComponent->ComponentToWorld))
{
const FVector MoveBy = NewTransform.GetLocation() - BodyInst->OwnerComponent->ComponentToWorld.GetLocation();
const FRotator NewRotation = NewTransform.Rotator();
//UE_LOG(LogTemp, Log, TEXT("MOVING: %s"), *BodyInst->OwnerComponent->GetPathName());
//@warning: do not reference BodyInstance again after calling MoveComponent() - events from the move could have made it unusable (destroying the actor, SetPhysics(), etc)
BodyInst->OwnerComponent->MoveComponent(MoveBy, NewRotation, false, NULL, MOVECOMP_SkipPhysicsMove);
}
// Check if we didn't fall out of the world
if(Owner != NULL && !Owner->IsPendingKill())
{
Owner->CheckStillInWorld();
}
}
}
#endif
}
void UCustomMovementComponent::CapsuleHited(class UPrimitiveComponent* MyComp, class AActor* Other, class UPrimitiveComponent* OtherComp, bool bSelfMoved, FVector HitLocation, FVector HitNormal, FVector NormalImpulse, const FHitResult& Hit)
{
const float OnGroundHitDot = FVector::DotProduct(HitNormal, CapsuleComponent->GetUpVector());
if (OnGroundHitDot > 0.75f)
{
bIsJumping = false;
}
CapsuleHitResult = Hit;
if (!bEnablePhysicsInteraction)
{
return;
}
if (OtherComp != NULL && OtherComp->IsAnySimulatingPhysics())
{
const FVector OtherLoc = OtherComp->GetComponentLocation();
const FVector Loc = CapsuleComponent->GetComponentLocation();
FVector ImpulseDir = (OtherLoc - Loc).GetSafeNormal();
ImpulseDir = FVector::VectorPlaneProject(ImpulseDir, -CurrentGravityInfo.GravityDirection);
ImpulseDir = (ImpulseDir + GetMovementVelocity().GetSafeNormal()) * 0.5f;
ImpulseDir.Normalize();
float TouchForceFactorModified = HitForceFactor;
if (bHitForceScaledToMass)
{
FBodyInstance* BI = OtherComp->GetBodyInstance();
TouchForceFactorModified *= BI ? BI->GetBodyMass() : 1.0f;
}
float ImpulseStrength = GetMovementVelocity().Size() * TouchForceFactorModified;
FVector Impulse = ImpulseDir * ImpulseStrength;
float dot = FVector::DotProduct(HitNormal, CapsuleComponent->GetUpVector());
if (dot > 0.99f && !bAllowDownwardForce)
{
return;
}
OtherComp->AddImpulseAtLocation(Impulse, HitLocation);
}
}
void FPhysSubstepTask::SubstepInterpolation(float InAlpha)
{
#if WITH_PHYSX
#if WITH_APEX
PxScene * PScene = PAScene->getPhysXScene();
#else
PxScene * PScene = PAScene;
#endif
PhysTargetMap & Targets = PhysTargetBuffers[!External];
/** Note: We lock the entire scene before iterating. The assumption is that removing an FBodyInstance from the map will also be wrapped by this lock */
SCENE_LOCK_WRITE(PScene);
for (PhysTargetMap::TIterator Itr = Targets.CreateIterator(); Itr; ++Itr)
{
FPhysTarget & PhysTarget = Itr.Value();
FBodyInstance* BodyInstance = Itr.Key();
PxRigidDynamic * PRigidDynamic = BodyInstance->GetPxRigidDynamic();
if (PRigidDynamic == NULL)
{
continue;
}
//We should only be iterating over actors that belong to this scene
check(PRigidDynamic->getScene() == PScene);
ApplyForces(PhysTarget, BodyInstance);
ApplyTorques(PhysTarget, BodyInstance);
InterpolateKinematicActor(PhysTarget, BodyInstance, InAlpha);
}
/** Final substep */
if (InAlpha >= 1.f)
{
Targets.Empty(Targets.Num());
}
SCENE_UNLOCK_WRITE(PScene);
#endif
}
void UPrimitiveComponent::SyncComponentToRBPhysics()
{
if(!IsRegistered())
{
UE_LOG(LogPhysics, Log, TEXT("SyncComponentToRBPhysics : Component not registered (%s)"), *GetPathName());
return;
}
// BodyInstance we are going to sync the component to
FBodyInstance* UseBI = GetBodyInstance();
if(UseBI == NULL || !UseBI->IsValidBodyInstance())
{
UE_LOG(LogPhysics, Log, TEXT("SyncComponentToRBPhysics : Missing or invalid BodyInstance (%s)"), *GetPathName());
return;
}
AActor* Owner = GetOwner();
if(Owner != NULL)
{
if (Owner->IsPendingKill() || !Owner->CheckStillInWorld())
{
return;
}
}
if (IsPendingKill() || !IsSimulatingPhysics())
{
return;
}
// See if the transform is actually different, and if so, move the component to match physics
const FTransform NewTransform = GetComponentTransformFromBodyInstance(UseBI);
if(!NewTransform.EqualsNoScale(ComponentToWorld))
{
const FVector MoveBy = NewTransform.GetLocation() - ComponentToWorld.GetLocation();
const FQuat NewRotation = NewTransform.GetRotation();
//@warning: do not reference BodyInstance again after calling MoveComponent() - events from the move could have made it unusable (destroying the actor, SetPhysics(), etc)
MoveComponent(MoveBy, NewRotation, false, NULL, MOVECOMP_SkipPhysicsMove);
}
}
bool UPrimitiveComponent::GetRigidBodyState(FRigidBodyState& OutState, FName BoneName)
{
FBodyInstance* BI = GetBodyInstance(BoneName);
if (BI && BI->IsInstanceSimulatingPhysics())
{
FTransform BodyTM = BI->GetUnrealWorldTransform();
OutState.Position = BodyTM.GetTranslation();
OutState.Quaternion = BodyTM.GetRotation();
OutState.LinVel = BI->GetUnrealWorldVelocity();
OutState.AngVel = BI->GetUnrealWorldAngularVelocity();
OutState.Flags = (BI->IsInstanceAwake() ? ERigidBodyFlags::None : ERigidBodyFlags::Sleeping);
return true;
}
return false;
}
bool UPrimitiveComponent::ApplyRigidBodyState(const FRigidBodyState& NewState, const FRigidBodyErrorCorrection& ErrorCorrection, FVector& OutDeltaPos, FName BoneName)
{
bool bRestoredState = true;
FBodyInstance* BI = GetBodyInstance(BoneName);
if (BI && BI->IsInstanceSimulatingPhysics())
{
// failure cases
const float QuatSizeSqr = NewState.Quaternion.SizeSquared();
if (QuatSizeSqr < KINDA_SMALL_NUMBER)
{
UE_LOG(LogPhysics, Warning, TEXT("Invalid zero quaternion set for body. (%s:%s)"), *GetName(), *BoneName.ToString());
return bRestoredState;
}
else if (FMath::Abs(QuatSizeSqr - 1.f) > KINDA_SMALL_NUMBER)
{
UE_LOG(LogPhysics, Warning, TEXT("Quaternion (%f %f %f %f) with non-unit magnitude detected. (%s:%s)"),
NewState.Quaternion.X, NewState.Quaternion.Y, NewState.Quaternion.Z, NewState.Quaternion.W, *GetName(), *BoneName.ToString() );
return bRestoredState;
}
FRigidBodyState CurrentState;
GetRigidBodyState(CurrentState, BoneName);
const bool bShouldSleep = (NewState.Flags & ERigidBodyFlags::Sleeping) != 0;
/////// POSITION CORRECTION ///////
// Find out how much of a correction we are making
const FVector DeltaPos = NewState.Position - CurrentState.Position;
const float DeltaMagSq = DeltaPos.SizeSquared();
const float BodyLinearSpeedSq = CurrentState.LinVel.SizeSquared();
// Snap position by default (big correction, or we are moving too slowly)
FVector UpdatedPos = NewState.Position;
FVector FixLinVel = FVector::ZeroVector;
// If its a small correction and velocity is above threshold, only make a partial correction,
// and calculate a velocity that would fix it over 'fixTime'.
if (DeltaMagSq < ErrorCorrection.LinearDeltaThresholdSq &&
BodyLinearSpeedSq >= ErrorCorrection.BodySpeedThresholdSq)
{
UpdatedPos = FMath::Lerp(CurrentState.Position, NewState.Position, ErrorCorrection.LinearInterpAlpha);
FixLinVel = (NewState.Position - UpdatedPos) * ErrorCorrection.LinearRecipFixTime;
}
// Get the linear correction
OutDeltaPos = UpdatedPos - CurrentState.Position;
/////// ORIENTATION CORRECTION ///////
// Get quaternion that takes us from old to new
const FQuat InvCurrentQuat = CurrentState.Quaternion.Inverse();
const FQuat DeltaQuat = NewState.Quaternion * InvCurrentQuat;
FVector DeltaAxis;
float DeltaAng; // radians
DeltaQuat.ToAxisAndAngle(DeltaAxis, DeltaAng);
DeltaAng = FMath::UnwindRadians(DeltaAng);
// Snap rotation by default (big correction, or we are moving too slowly)
FQuat UpdatedQuat = NewState.Quaternion;
FVector FixAngVel = FVector::ZeroVector; // degrees per second
// If the error is small, and we are moving, try to move smoothly to it
if (FMath::Abs(DeltaAng) < ErrorCorrection.AngularDeltaThreshold )
{
UpdatedQuat = FMath::Lerp(CurrentState.Quaternion, NewState.Quaternion, ErrorCorrection.AngularInterpAlpha);
FixAngVel = DeltaAxis.GetSafeNormal() * FMath::RadiansToDegrees(DeltaAng) * (1.f - ErrorCorrection.AngularInterpAlpha) * ErrorCorrection.AngularRecipFixTime;
}
/////// BODY UPDATE ///////
BI->SetBodyTransform(FTransform(UpdatedQuat, UpdatedPos), true);
BI->SetLinearVelocity(NewState.LinVel + FixLinVel, false);
BI->SetAngularVelocity(NewState.AngVel + FixAngVel, false);
// state is restored when no velocity corrections are required
bRestoredState = (FixLinVel.SizeSquared() < KINDA_SMALL_NUMBER) && (FixAngVel.SizeSquared() < KINDA_SMALL_NUMBER);
/////// SLEEP UPDATE ///////
const bool bIsAwake = BI->IsInstanceAwake();
if (bIsAwake && (bShouldSleep && bRestoredState))
{
BI->PutInstanceToSleep();
}
else if (!bIsAwake)
{
BI->WakeInstance();
}
}
return bRestoredState;
}
void UBuoyancyForceComponent::TickComponent(float DeltaTime, enum ELevelTick TickType, FActorComponentTickFunction *ThisTickFunction)
{
Super::TickComponent(DeltaTime, TickType, ThisTickFunction);
// If disabled or we are not attached to a parent component, return.
if (!bIsActive || !GetAttachParent()) return;
if (!OceanManager) return;
UPrimitiveComponent* BasePrimComp = Cast<UPrimitiveComponent>(GetAttachParent());
if (!BasePrimComp) return;
if (!BasePrimComp->IsSimulatingPhysics())
{
if (!SnapToSurfaceIfNoPhysics) return;
UE_LOG(LogTemp, Warning, TEXT("Running in no physics mode.."));
float waveHeight = OceanManager->GetWaveHeightValue(BasePrimComp->GetComponentLocation(), World, true, TwoGerstnerIterations).Z;
BasePrimComp->SetWorldLocation(FVector(BasePrimComp->GetComponentLocation().X, BasePrimComp->GetComponentLocation().Y, waveHeight));
return;
}
//Get gravity
float Gravity = BasePrimComp->GetPhysicsVolume()->GetGravityZ();
//--------------- If Skeletal ---------------
USkeletalMeshComponent* SkeletalComp = Cast<USkeletalMeshComponent>(GetAttachParent());
if (SkeletalComp && ApplyForceToBones)
{
TArray<FName> BoneNames;
SkeletalComp->GetBoneNames(BoneNames);
for (int32 Itr = 0; Itr < BoneNames.Num(); Itr++)
{
FBodyInstance* BI = SkeletalComp->GetBodyInstance(BoneNames[Itr], false);
if (BI && BI->IsValidBodyInstance()
&& BI->bEnableGravity) //Buoyancy doesn't exist without gravity
{
bool isUnderwater = false;
//FVector worldBoneLoc = SkeletalComp->GetBoneLocation(BoneNames[Itr]);
FVector worldBoneLoc = BI->GetCOMPosition(); //Use center of mass of the bone's physics body instead of bone's location
FVector waveHeight = OceanManager->GetWaveHeightValue(worldBoneLoc, World, true, TwoGerstnerIterations);
float BoneDensity = MeshDensity;
float BoneTestRadius = FMath::Abs(TestPointRadius);
float SignedBoneRadius = FMath::Sign(Gravity) * TestPointRadius; //Direction of radius (test radius is actually a Z offset, should probably rename it!). Just in case we need an upside down world.
//Get density & radius from the override array, if available.
for (int pointIndex = 0; pointIndex < BoneOverride.Num(); pointIndex++)
{
FStructBoneOverride Override = BoneOverride[pointIndex];
if (Override.BoneName.IsEqual(BoneNames[Itr]))
{
BoneDensity = Override.Density;
BoneTestRadius = FMath::Abs(Override.TestRadius);
SignedBoneRadius = FMath::Sign(Gravity) * BoneTestRadius;
}
}
//If test point radius is below water surface, add buoyancy force.
if (waveHeight.Z > (worldBoneLoc.Z + SignedBoneRadius))
{
isUnderwater = true;
float DepthMultiplier = (waveHeight.Z - (worldBoneLoc.Z + SignedBoneRadius)) / (BoneTestRadius * 2);
DepthMultiplier = FMath::Clamp(DepthMultiplier, 0.f, 1.f);
float Mass = SkeletalComp->CalculateMass(BoneNames[Itr]); //Mass of this specific bone's physics body
/**
* --------
* Buoyancy force formula: (Volume(Mass / Density) * Fluid Density * -Gravity) / Total Points * Depth Multiplier
* --------
*/
float BuoyancyForceZ = Mass / BoneDensity * FluidDensity * -Gravity * DepthMultiplier;
//Velocity damping.
FVector DampingForce = -BI->GetUnrealWorldVelocity() * VelocityDamper * Mass * DepthMultiplier;
//Experimental xy wave force
if (EnableWaveForces)
{
float waveVelocity = FMath::Clamp(BI->GetUnrealWorldVelocity().Z, -20.f, 150.f) * (1 - DepthMultiplier);
DampingForce += FVector(OceanManager->GlobalWaveDirection.X, OceanManager->GlobalWaveDirection.Y, 0) * Mass * waveVelocity * WaveForceMultiplier;
}
//Add force to this bone
BI->AddForce(FVector(DampingForce.X, DampingForce.Y, DampingForce.Z + BuoyancyForceZ));
//BasePrimComp->AddForceAtLocation(FVector(DampingForce.X, DampingForce.Y, DampingForce.Z + BuoyancyForceZ), worldBoneLoc, BoneNames[Itr]);
}
//Apply fluid damping & clamp velocity
if (isUnderwater)
{
BI->SetLinearVelocity(-BI->GetUnrealWorldVelocity() * (FluidLinearDamping / 10), true);
BI->SetAngularVelocity(-BI->GetUnrealWorldAngularVelocity() * (FluidAngularDamping / 10), true);
//Clamp the velocity to MaxUnderwaterVelocity
if (ClampMaxVelocity && BI->GetUnrealWorldVelocity().Size() > MaxUnderwaterVelocity)
{
FVector Velocity = BI->GetUnrealWorldVelocity().GetSafeNormal() * MaxUnderwaterVelocity;
BI->SetLinearVelocity(Velocity, false);
}
}
if (DrawDebugPoints)
{
FColor DebugColor = FLinearColor(0.8, 0.7, 0.2, 0.8).ToRGBE();
if (isUnderwater) { DebugColor = FLinearColor(0, 0.2, 0.7, 0.8).ToRGBE(); } //Blue color underwater, yellow out of watter
DrawDebugSphere(World, worldBoneLoc, BoneTestRadius, 8, DebugColor);
}
}
}
return;
}
//--------------------------------------------------------
float TotalPoints = TestPoints.Num();
if (TotalPoints < 1) return;
int PointsUnderWater = 0;
for (int pointIndex = 0; pointIndex < TotalPoints; pointIndex++)
{
if (!TestPoints.IsValidIndex(pointIndex)) return; //Array size changed during runtime
bool isUnderwater = false;
FVector testPoint = TestPoints[pointIndex];
FVector worldTestPoint = BasePrimComp->GetComponentTransform().TransformPosition(testPoint);
FVector waveHeight = OceanManager->GetWaveHeightValue(worldTestPoint, World, !EnableWaveForces, TwoGerstnerIterations);
//Direction of radius (test radius is actually a Z offset, should probably rename it!). Just in case we need an upside down world.
float SignedRadius = FMath::Sign(BasePrimComp->GetPhysicsVolume()->GetGravityZ()) * TestPointRadius;
//If test point radius is below water surface, add buoyancy force.
if (waveHeight.Z > (worldTestPoint.Z + SignedRadius)
&& BasePrimComp->IsGravityEnabled()) //Buoyancy doesn't exist without gravity
{
PointsUnderWater++;
isUnderwater = true;
float DepthMultiplier = (waveHeight.Z - (worldTestPoint.Z + SignedRadius)) / (TestPointRadius * 2);
DepthMultiplier = FMath::Clamp(DepthMultiplier, 0.f, 1.f);
//If we have a point density override, use the overridden value instead of MeshDensity
float PointDensity = PointDensityOverride.IsValidIndex(pointIndex) ? PointDensityOverride[pointIndex] : MeshDensity;
/**
* --------
* Buoyancy force formula: (Volume(Mass / Density) * Fluid Density * -Gravity) / Total Points * Depth Multiplier
* --------
*/
float BuoyancyForceZ = BasePrimComp->GetMass() / PointDensity * FluidDensity * -Gravity / TotalPoints * DepthMultiplier;
//Experimental velocity damping using VelocityAtPoint.
FVector DampingForce = -GetUnrealVelocityAtPoint(BasePrimComp, worldTestPoint) * VelocityDamper * BasePrimComp->GetMass() * DepthMultiplier;
//Experimental xy wave force
if (EnableWaveForces)
{
DampingForce += BasePrimComp->GetMass() * FVector2D(waveHeight.X, waveHeight.Y).Size() * FVector(OceanManager->GlobalWaveDirection.X, OceanManager->GlobalWaveDirection.Y, 0) * WaveForceMultiplier / TotalPoints;
//float waveVelocity = FMath::Clamp(GetUnrealVelocityAtPoint(BasePrimComp, worldTestPoint).Z, -20.f, 150.f) * (1 - DepthMultiplier);
//DampingForce += OceanManager->GlobalWaveDirection * BasePrimComp->GetMass() * waveVelocity * WaveForceMultiplier / TotalPoints;
}
//Add force for this test point
BasePrimComp->AddForceAtLocation(FVector(DampingForce.X, DampingForce.Y, DampingForce.Z + BuoyancyForceZ), worldTestPoint);
}
if (DrawDebugPoints)
{
FColor DebugColor = FLinearColor(0.8, 0.7, 0.2, 0.8).ToRGBE();
if (isUnderwater) { DebugColor = FLinearColor(0, 0.2, 0.7, 0.8).ToRGBE(); } //Blue color underwater, yellow out of watter
DrawDebugSphere(World, worldTestPoint, TestPointRadius, 8, DebugColor);
}
}
//Clamp the velocity to MaxUnderwaterVelocity if there is any point underwater
if (ClampMaxVelocity && PointsUnderWater > 0
&& BasePrimComp->GetPhysicsLinearVelocity().Size() > MaxUnderwaterVelocity)
{
FVector Velocity = BasePrimComp->GetPhysicsLinearVelocity().GetSafeNormal() * MaxUnderwaterVelocity;
BasePrimComp->SetPhysicsLinearVelocity(Velocity);
}
//Update damping based on number of underwater test points
BasePrimComp->SetLinearDamping(_baseLinearDamping + FluidLinearDamping / TotalPoints * PointsUnderWater);
BasePrimComp->SetAngularDamping(_baseAngularDamping + FluidAngularDamping / TotalPoints * PointsUnderWater);
}
void UPrimitiveComponent::UnWeldFromParent()
{
FBodyInstance* NewRootBI = GetBodyInstance(NAME_None, false);
UWorld* CurrentWorld = GetWorld();
if (NewRootBI == NULL || NewRootBI->bWelded == false || CurrentWorld == nullptr || IsPendingKill())
{
return;
}
FName SocketName;
UPrimitiveComponent * RootComponent = GetRootWelded(this, AttachSocketName, &SocketName);
if (RootComponent)
{
if (FBodyInstance* RootBI = RootComponent->GetBodyInstance(SocketName, false))
{
bool bRootIsBeingDeleted = RootComponent->HasAnyFlags(RF_PendingKill) || RootComponent->HasAnyFlags(RF_Unreachable);
if (!bRootIsBeingDeleted)
{
//create new root
RootBI->UnWeld(NewRootBI); //don't bother fixing up shapes if RootComponent is about to be deleted
}
NewRootBI->bWelded = false;
const FBodyInstance* PrevWeldParent = NewRootBI->WeldParent;
NewRootBI->WeldParent = nullptr;
bool bHasBodySetup = GetBodySetup() != nullptr;
//if BodyInstance hasn't already been created we need to initialize it
if (bHasBodySetup && NewRootBI->IsValidBodyInstance() == false)
{
bool bPrevAutoWeld = NewRootBI->bAutoWeld;
NewRootBI->bAutoWeld = false;
NewRootBI->InitBody(GetBodySetup(), GetComponentToWorld(), this, CurrentWorld->GetPhysicsScene());
NewRootBI->bAutoWeld = bPrevAutoWeld;
}
if(PrevWeldParent == nullptr) //our parent is kinematic so no need to do any unwelding/rewelding of children
{
return;
}
//now weld its children to it
TArray<FBodyInstance*> ChildrenBodies;
TArray<FName> ChildrenLabels;
GetWeldedBodies(ChildrenBodies, ChildrenLabels);
for (int32 ChildIdx = 0; ChildIdx < ChildrenBodies.Num(); ++ChildIdx)
{
FBodyInstance* ChildBI = ChildrenBodies[ChildIdx];
if (ChildBI != NewRootBI)
{
if (!bRootIsBeingDeleted)
{
RootBI->UnWeld(ChildBI);
}
//At this point, NewRootBI must be kinematic because it's being unwelded. It's up to the code that simulates to call Weld on the children as needed
ChildBI->WeldParent = nullptr; //null because we are currently kinematic
}
}
}
}
}
