void UPrimitiveComponent::AddForceAtLocation(FVector Force, FVector Location, FName BoneName)
{
if (FBodyInstance* BI = GetBodyInstance(BoneName))
{
WarnInvalidPhysicsOperations(LOCTEXT("AddForceAtLocation", "AddForceAtLocation"), BI);
BI->AddForceAtPosition(Force, Location);
}
}
void UPrimitiveComponent::PutRigidBodyToSleep(FName BoneName)
{
FBodyInstance* BI = GetBodyInstance(BoneName);
if(BI)
{
BI->PutInstanceToSleep();
}
}
void UPrimitiveComponent::SetMassScale(FName BoneName, float InMassScale)
{
if (FBodyInstance* BI = GetBodyInstance(BoneName))
{
WarnInvalidPhysicsOperations(LOCTEXT("SetMassScale", "SetMassScale"), BI);
BI->SetMassScale(InMassScale);
}
}
void UPrimitiveComponent::SetPhysicsAngularVelocity(FVector NewAngVel, bool bAddToCurrent, FName BoneName)
{
if (FBodyInstance* BI = GetBodyInstance(BoneName))
{
WarnInvalidPhysicsOperations(LOCTEXT("SetPhysicsAngularVelocity", "SetPhysicsAngularVelocity"), nullptr);
BI->SetAngularVelocity(NewAngVel, bAddToCurrent);
}
}
void UPrimitiveComponent::SetEnableGravity(bool bGravityEnabled)
{
FBodyInstance* BI = GetBodyInstance();
if (BI)
{
BI->SetEnableGravity(bGravityEnabled);
}
}
void UPrimitiveComponent::WakeRigidBody(FName BoneName)
{
FBodyInstance* BI = GetBodyInstance(BoneName);
if(BI)
{
BI->WakeInstance();
}
}
void UPrimitiveComponent::AddAngularImpulse(FVector Impulse, FName BoneName, bool bVelChange)
{
if (FBodyInstance* BI = GetBodyInstance(BoneName))
{
WarnInvalidPhysicsOperations(LOCTEXT("AddAngularImpulse", "AddAngularImpulse"), BI);
BI->AddAngularImpulse(Impulse, bVelChange);
}
}
void UPrimitiveComponent::AddTorque(FVector Torque, FName BoneName, bool bAccelChange)
{
if (FBodyInstance* BI = GetBodyInstance(BoneName))
{
WarnInvalidPhysicsOperations(LOCTEXT("AddTorque", "AddTorque"), BI);
BI->AddTorque(Torque, true, bAccelChange);
}
}
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());
return true;
}
}
return false;
}
FVector UPrimitiveComponent::GetInertiaTensor(FName BoneName /* = NAME_None */) const
{
if(FBodyInstance* BI = GetBodyInstance(BoneName))
{
return BI->GetBodyInertiaTensor();
}
return FVector::ZeroVector;
}
float UPrimitiveComponent::GetMassScale(FName BoneName /*= NAME_None*/) const
{
if (FBodyInstance* BI = GetBodyInstance(BoneName))
{
return BI->MassScale;
}
return 0.0f;
}
FVector UPrimitiveComponent::GetCenterOfMass(FName BoneName)
{
if (FBodyInstance* ComponentBodyInstance = GetBodyInstance(BoneName))
{
return ComponentBodyInstance->GetCOMPosition();
}
return FVector::ZeroVector;
}
FVector UPrimitiveComponent::GetPhysicsAngularVelocity(FName BoneName)
{
FBodyInstance* const BI = GetBodyInstance(BoneName);
if(BI != NULL)
{
return BI->GetUnrealWorldAngularVelocity();
}
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
{
if (FBodyInstance* BI = GetBodyInstance())
{
WarnInvalidPhysicsOperations(LOCTEXT("GetMass", "GetMass"), BI);
return BI->GetBodyMass();
}
return 0.0f;
}
float UPrimitiveComponent::GetAngularDamping() const
{
FBodyInstance* BI = GetBodyInstance();
if (BI)
{
return BI->AngularDamping;
}
return 0.f;
}
bool UPrimitiveComponent::IsGravityEnabled() const
{
FBodyInstance* BI = GetBodyInstance();
if (BI)
{
return BI->bEnableGravity;
}
return false;
}
float UPrimitiveComponent::GetMass() const
{
FBodyInstance* BI = GetBodyInstance();
if (BI)
{
return BI->GetBodyMass();
}
return 0.0f;
}
void UPhysicsConstraintComponent::UpdateConstraintFrames()
{
FTransform A1Transform = GetBodyTransform(EConstraintFrame::Frame1);
A1Transform.RemoveScaling();
FTransform A2Transform = GetBodyTransform(EConstraintFrame::Frame2);
A2Transform.RemoveScaling();
// World ref frame
const FVector WPos = GetComponentLocation();
const FVector WPri = ComponentToWorld.GetUnitAxis( EAxis::X );
const FVector WOrth = ComponentToWorld.GetUnitAxis( EAxis::Y );
ConstraintInstance.Pos1 = A1Transform.InverseTransformPosition(WPos);
ConstraintInstance.PriAxis1 = A1Transform.InverseTransformVectorNoScale(WPri);
ConstraintInstance.SecAxis1 = A1Transform.InverseTransformVectorNoScale(WOrth);
const FVector RotatedX = ConstraintInstance.AngularRotationOffset.RotateVector(FVector(1,0,0));
const FVector RotatedY = ConstraintInstance.AngularRotationOffset.RotateVector(FVector(0,1,0));
const FVector WPri2 = ComponentToWorld.TransformVectorNoScale(RotatedX);
const FVector WOrth2 = ComponentToWorld.TransformVectorNoScale(RotatedY);
ConstraintInstance.Pos2 = A2Transform.InverseTransformPosition(WPos);
ConstraintInstance.PriAxis2 = A2Transform.InverseTransformVectorNoScale(WPri2);
ConstraintInstance.SecAxis2 = A2Transform.InverseTransformVectorNoScale(WOrth2);
//Constraint instance is given our reference frame scale and uses it to scale position.
//Note that the scale passed in is also used for limits, so we first undo the position scale so that it's consistent.
//Note that in the case where there is no body instance, the position is given in world space and there is no scaling.
const float RefScale = FMath::Max(GetConstraintScale(), 0.01f);
if(GetBodyInstance(EConstraintFrame::Frame1))
{
ConstraintInstance.Pos1 /= RefScale;
}
if (GetBodyInstance(EConstraintFrame::Frame2))
{
ConstraintInstance.Pos2 /= RefScale;
}
}
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);
}
}
void UDestructibleComponent::SetMaterial(int32 ElementIndex, UMaterialInterface* Material)
{
// Mesh component handles render side materials
Super::SetMaterial(ElementIndex, Material);
// Update physical properties of the chunks in the mesh
GetBodyInstance()->UpdatePhysicalMaterials();
int32 NumBones = SkeletalMesh->RefSkeleton.GetNum();
for(int32 BoneIdx = 0 ; BoneIdx < NumBones ; ++BoneIdx)
{
FName BoneName = SkeletalMesh->RefSkeleton.GetBoneName(BoneIdx);
if(FBodyInstance* Instance = GetBodyInstance(BoneName))
{
Instance->UpdatePhysicalMaterials();
}
}
#if WITH_APEX
// Set new template parameters for the apex actor, so they take effect before fracturing too.
if(ApexDestructibleActor)
{
physx::apex::NxPhysX3DescTemplate* Template = ApexDestructibleActor->createPhysX3DescTemplate();
if(ApexDestructibleActor->getPhysX3Template(*Template))
{
UPhysicalMaterial* SimpleMaterial = GetBodyInstance()->GetSimplePhysicalMaterial();
check(SimpleMaterial);
PxMaterial* PhysxMat = SimpleMaterial->GetPhysXMaterial();
Template->setMaterials(&PhysxMat, 1);
ApexDestructibleActor->setPhysX3Template(Template);
}
Template->release();
}
#endif
}
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;
}
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);
}
}
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
}
}
}
}
}
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;
}
