void FPhysScene::SetKinematicTarget(FBodyInstance * BodyInstance, const FTransform & TargetTransform)
{
TargetTransform.DiagnosticCheckNaN_All();
#if WITH_PHYSX
if (PxRigidDynamic * PRigidDynamic = BodyInstance->GetPxRigidDynamic())
{
#if WITH_SUBSTEPPING
if (IsSubstepping())
{
FPhysSubstepTask * PhysSubStepper = PhysSubSteppers[SceneType(BodyInstance)];
PhysSubStepper->SetKinematicTarget(BodyInstance, TargetTransform);
}
else
#endif
{
const PxTransform PNewPose = U2PTransform(TargetTransform);
check(PNewPose.isValid());
SCOPED_SCENE_WRITE_LOCK(PRigidDynamic->getScene());
PRigidDynamic->setKinematicTarget(PNewPose);
}
}
#endif
}
void FPhysSubstepTask::SetKinematicTarget_AssumesLocked(FBodyInstance* Body, const FTransform& TM)
{
#if WITH_PHYSX
TM.DiagnosticCheckNaN_All();
//We only interpolate kinematic actors
if (!Body->IsNonKinematic())
{
FKinematicTarget KinmaticTarget(Body, TM);
FPhysTarget & TargetState = PhysTargetBuffers[External].FindOrAdd(Body);
TargetState.bKinematicTarget = true;
TargetState.KinematicTarget = KinmaticTarget;
}
#endif
}
FTransform FTransform::GetRelativeTransform(const FTransform& Other) const
{
// A * B(-1) = VQS(B)(-1) (VQS (A))
//
// Scale = S(A)/S(B)
// Rotation = Q(B)(-1) * Q(A)
// Translation = 1/S(B) *[Q(B)(-1)*(T(A)-T(B))*Q(B)]
// where A = this, B = Other
FTransform Result;
if (Other.IsRotationNormalized() == false)
{
return FTransform::Identity;
}
// Scale = S(A)/S(B)
static ScalarRegister STolerance(SMALL_NUMBER);
VectorRegister VSafeScale3D = VectorSet_W0(GetSafeScaleReciprocal(Other.Scale3D, STolerance));
VectorRegister VScale3D = VectorMultiply(Scale3D, VSafeScale3D);
//VQTranslation = ( ( T(A).X - T(B).X ), ( T(A).Y - T(B).Y ), ( T(A).Z - T(B).Z), 0.f );
VectorRegister VQTranslation = VectorSet_W0(VectorSubtract(Translation, Other.Translation));
// Translation = 1/S(B) *[Q(B)(-1)*(T(A)-T(B))*Q(B)]
VectorRegister VInverseRot = VectorQuaternionInverse(Other.Rotation);
VectorRegister VQT = VectorQuaternionMultiply2(VInverseRot, VQTranslation);
VectorRegister VR = VectorQuaternionMultiply2(VQT, Other.Rotation);
VectorRegister VTranslation = VectorMultiply(VR, VSafeScale3D);
// Rotation = Q(B)(-1) * Q(A)
VectorRegister VRotation = VectorQuaternionMultiply2(VInverseRot, Rotation );
Result.Scale3D = VScale3D;
Result.Translation = VTranslation;
Result.Rotation = VRotation;
Result.DiagnosticCheckNaN_All();
#if DEBUG_INVERSE_TRANSFORM
FMatrix AM = ToMatrixWithScale();
FMatrix BM = Other.ToMatrixWithScale();
Result.DebugEqualMatrix(AM * BM.InverseFast());
#endif
return Result;
}
void FPhysSubstepTask::SetKinematicTarget(FBodyInstance * Body, const FTransform & TM)
{
check(Body);
TM.DiagnosticCheckNaN_All();
PxRigidDynamic * PRigidDynamic = Body->GetPxRigidDynamic();
SCOPED_SCENE_READ_LOCK(PRigidDynamic->getScene());
//We only interpolate kinematic actors
if (!IsRigidDynamicNonKinematic(PRigidDynamic))
{
FKinematicTarget KinmaticTarget(Body, TM);
FPhysTarget & TargetState = PhysTargetBuffers[External].FindOrAdd(Body);
TargetState.bKinematicTarget = true;
TargetState.KinematicTarget = KinmaticTarget;
}
}
FTransform FTransform::GetRelativeTransformReverse(const FTransform& Other) const
{
// A (-1) * B = VQS(B)(VQS (A)(-1))
//
// Scale = S(B)/S(A)
// Rotation = Q(B) * Q(A)(-1)
// Translation = T(B)-S(B)/S(A) *[Q(B)*Q(A)(-1)*T(A)*Q(A)*Q(B)(-1)]
// where A = this, and B = Other
FTransform Result;
// Scale = S(B)/S(A)
VectorRegister VSafeScale3D = VectorSet_W0(GetSafeScaleReciprocal(Scale3D));
VectorRegister VScale3D = VectorMultiply(Other.Scale3D, VSafeScale3D);
// Rotation = Q(B) * Q(A)(-1)
VectorRegister VInverseRot = VectorQuaternionInverse(Rotation);
VectorRegister VRotation = VectorQuaternionMultiply2(Other.Rotation, VInverseRot );
// [Q(B)*Q(A)(-1)*T(A)*Q(A)*Q(B)(-1)]
VInverseRot = VectorQuaternionInverse(VRotation);
VectorRegister VQT = VectorQuaternionMultiply2(VRotation, Translation);
VectorRegister VR = VectorQuaternionMultiply2(VQT, VInverseRot);
// Translation = T(B)-S(B)/S(A) *[Q(B)*Q(A)(-1)*T(A)*Q(A)*Q(B)(-1)]
VectorRegister VTranslation = VectorSet_W0(VectorSubtract(Other.Translation, VectorMultiply(VScale3D, VR)));
Result.Scale3D = VScale3D;
Result.Translation = VTranslation;
Result.Rotation = VRotation;
Result.DiagnosticCheckNaN_All();
#if DEBUG_INVERSE_TRANSFORM
FMatrix AM = ToMatrixWithScale();
FMatrix BM = Other.ToMatrixWithScale();
Result.DebugEqualMatrix(AM.InverseFast() * BM);
#endif
return Result;
}
void FPhysScene::SetKinematicTarget_AssumesLocked(FBodyInstance* BodyInstance, const FTransform& TargetTransform, bool bAllowSubstepping)
{
TargetTransform.DiagnosticCheckNaN_All();
#if WITH_PHYSX
if (PxRigidDynamic * PRigidDynamic = BodyInstance->GetPxRigidDynamic_AssumesLocked())
{
#if WITH_SUBSTEPPING
uint32 BodySceneType = SceneType_AssumesLocked(BodyInstance);
if (bAllowSubstepping && IsSubstepping(BodySceneType))
{
FPhysSubstepTask * PhysSubStepper = PhysSubSteppers[BodySceneType];
PhysSubStepper->SetKinematicTarget_AssumesLocked(BodyInstance, TargetTransform);
}
else
#endif
{
const PxTransform PNewPose = U2PTransform(TargetTransform);
check(PNewPose.isValid());
PRigidDynamic->setKinematicTarget(PNewPose);
}
}
#endif
}
