C++ FTransform::DiagnosticCheckNaN_All方法代码示例

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
}