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; }
/** * Set current transform and the relative to ParentTransform. * Equates to This = This->GetRelativeTransform(Parent), but saves the intermediate FTransform storage and copy. */ void FTransform::SetToRelativeTransform(const FTransform& ParentTransform) { // 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 #if DEBUG_INVERSE_TRANSFORM FMatrix AM = ToMatrixWithScale(); FMatrix BM = ParentTransform.ToMatrixWithScale(); #endif checkSlow(ParentTransform.IsRotationNormalized()); // Scale = S(A)/S(B) VectorRegister VSafeScale3D = VectorSet_W0(GetSafeScaleReciprocal(ParentTransform.Scale3D)); Scale3D = 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, ParentTransform.Translation)); // Translation = 1/S(B) *[Q(B)(-1)*(T(A)-T(B))*Q(B)] VectorRegister VInverseParentRot = VectorQuaternionInverse(ParentTransform.Rotation); VectorRegister VQT = VectorQuaternionMultiply2(VInverseParentRot, VQTranslation); VectorRegister VR = VectorQuaternionMultiply2(VQT, ParentTransform.Rotation); Translation = VectorMultiply(VR, VSafeScale3D); // Rotation = Q(B)(-1) * Q(A) Rotation = VectorQuaternionMultiply2(VInverseParentRot, Rotation ); DiagnosticCheckNaN_All(); #if DEBUG_INVERSE_TRANSFORM DebugEqualMatrix(AM * BM.InverseFast()); #endif }