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 }