/** Interpolates kinematic actor transform - Assumes caller has obtained writer lock */
void FPhysSubstepTask::InterpolateKinematicActor_AssumesLocked(const FPhysTarget& PhysTarget, FBodyInstance* BodyInstance, float InAlpha)
{
#if WITH_PHYSX
PxRigidDynamic * PRigidDynamic = BodyInstance->GetPxRigidDynamic_AssumesLocked();
InAlpha = FMath::Clamp(InAlpha, 0.f, 1.f);
/** Interpolate kinematic actors */
if (PhysTarget.bKinematicTarget)
{
//It's possible that the actor is no longer kinematic and is now simulating. In that case do nothing
if (!BodyInstance->IsNonKinematic())
{
const FKinematicTarget& KinematicTarget = PhysTarget.KinematicTarget;
const FTransform& TargetTM = KinematicTarget.TargetTM;
const FTransform& StartTM = KinematicTarget.OriginalTM;
FTransform InterTM = FTransform::Identity;
InterTM.SetLocation(FMath::Lerp(StartTM.GetLocation(), TargetTM.GetLocation(), InAlpha));
InterTM.SetRotation(FMath::Lerp(StartTM.GetRotation(), TargetTM.GetRotation(), InAlpha));
const PxTransform PNewPose = U2PTransform(InterTM);
check(PNewPose.isValid());
PRigidDynamic->setKinematicTarget(PNewPose);
}
}
#endif
}
/** Interpolates kinematic actor transform - Assumes caller has obtained writer lock */
void FPhysSubstepTask::InterpolateKinematicActor(const FPhysTarget & PhysTarget, FBodyInstance * BodyInstance, float Alpha)
{
#if WITH_PHYSX
PxRigidDynamic * PRigidDynamic = BodyInstance->GetPxRigidDynamic();
Alpha = FMath::Clamp(Alpha, 0.f, 1.f);
/** Interpolate kinematic actors */
if (PhysTarget.bKinematicTarget)
{
//We should only be interpolating kinematic actors
check(!IsRigidDynamicNonKinematic(PRigidDynamic));
const FKinematicTarget & KinematicTarget = PhysTarget.KinematicTarget;
const FTransform & TargetTM = KinematicTarget.TargetTM;
const FTransform & StartTM = KinematicTarget.OriginalTM;
FTransform InterTM = FTransform::Identity;
InterTM.SetLocation(FMath::Lerp(StartTM.GetLocation(), TargetTM.GetLocation(), Alpha));
InterTM.SetRotation(FMath::Lerp(StartTM.GetRotation(), TargetTM.GetRotation(), Alpha));
const PxTransform PNewPose = U2PTransform(InterTM);
check(PNewPose.isValid());
PRigidDynamic->setKinematicTarget(PNewPose);
}
#endif
}
void UPhysicsHandleComponent::UpdateHandleTransform(const FTransform& NewTransform)
{
if(!KinActorData)
{
return;
}
#if WITH_PHYSX
bool bChangedPosition = true;
bool bChangedRotation = true;
PxRigidDynamic* KinActor = KinActorData;
// Check if the new location is worthy of change
PxVec3 PNewLocation = U2PVector(NewTransform.GetTranslation());
PxVec3 PCurrentLocation = KinActor->getGlobalPose().p;
if((PNewLocation - PCurrentLocation).magnitudeSquared() <= 0.01f*0.01f)
{
PNewLocation = PCurrentLocation;
bChangedPosition = false;
}
// Check if the new rotation is worthy of change
PxQuat PNewOrientation = U2PQuat(NewTransform.GetRotation());
PxQuat PCurrentOrientation = KinActor->getGlobalPose().q;
if(!(FMath::Abs(PNewOrientation.dot(PCurrentOrientation)) < (1.f - KINDA_SMALL_NUMBER)))
{
PNewOrientation = PCurrentOrientation;
bChangedRotation = false;
}
// Don't call moveKinematic if it hasn't changed - that will stop bodies from going to sleep.
if (bChangedPosition || bChangedRotation)
{
KinActor->setKinematicTarget(PxTransform(PNewLocation, PNewOrientation));
//LOC_MOD
//PxD6Joint* Joint = (PxD6Joint*) HandleData;
//if(Joint)// && (PNewLocation - PCurrentLocation).magnitudeSquared() > 0.01f*0.01f)
//{
// PxRigidActor* Actor0, *Actor1;
// Joint->getActors(Actor0, Actor1);
// //Joint->setDrivePosition(PxTransform(Actor0->getGlobalPose().transformInv(PNewLocation)));
// Joint->setDrivePosition(PxTransform::createIdentity());
// //Joint->setDriveVelocity(PxVec3(0), PxVec3(0));
//}
}
#endif // WITH_PHYSX
}
void MirrorActor::updatePose(const PxTransform &pose)
{
if ( mMirrorActor )
{
if ( mMirrorActor->getType() == PxActorType::eRIGID_STATIC )
{
PxRigidStatic *p = static_cast< PxRigidStatic *>(mMirrorActor);
p->setGlobalPose(pose);
}
else
{
PxRigidDynamic *p = static_cast< PxRigidDynamic *>(mMirrorActor);
p->setKinematicTarget(pose);
}
}
}
void UGripMotionControllerComponent::UpdatePhysicsHandleTransform(const FBPActorGripInformation &GrippedActor, const FTransform& NewTransform)
{
if (!GrippedActor.Actor && !GrippedActor.Component)
return;
FBPActorPhysicsHandleInformation * HandleInfo = GetPhysicsGrip(GrippedActor);
if (!HandleInfo || !HandleInfo->KinActorData)
return;
#if WITH_PHYSX
bool bChangedPosition = true;
bool bChangedRotation = true;
PxRigidDynamic* KinActor = HandleInfo->KinActorData;
PxScene* PScene = GetPhysXSceneFromIndex(HandleInfo->SceneIndex);
SCOPED_SCENE_WRITE_LOCK(PScene);
// Check if the new location is worthy of change
PxVec3 PNewLocation = U2PVector(NewTransform.GetTranslation());
PxVec3 PCurrentLocation = KinActor->getGlobalPose().p;
if ((PNewLocation - PCurrentLocation).magnitudeSquared() <= 0.01f*0.01f)
{
PNewLocation = PCurrentLocation;
bChangedPosition = false;
}
// Check if the new rotation is worthy of change
PxQuat PNewOrientation = U2PQuat(NewTransform.GetRotation());
PxQuat PCurrentOrientation = KinActor->getGlobalPose().q;
if ((FMath::Abs(PNewOrientation.dot(PCurrentOrientation)) > (1.f - SMALL_NUMBER)))
{
PNewOrientation = PCurrentOrientation;
bChangedRotation = false;
}
// Don't call moveKinematic if it hasn't changed - that will stop bodies from going to sleep.
if (bChangedPosition || bChangedRotation)
{
KinActor->setKinematicTarget(PxTransform(PNewLocation, PNewOrientation));
}
#endif // WITH_PHYSX
}
