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;
FVector SafeRecipScale3D = GetSafeScaleReciprocal(Scale3D);
Result.Scale3D = Other.Scale3D*SafeRecipScale3D;
Result.Rotation = Other.Rotation*Rotation.Inverse();
Result.Translation = Other.Translation - Result.Scale3D * ( Result.Rotation * Translation );
#if DEBUG_INVERSE_TRANSFORM
FMatrix AM = ToMatrixWithScale();
FMatrix BM = Other.ToMatrixWithScale();
Result.DebugEqualMatrix(AM.InverseFast() * BM);
#endif
return Result;
}
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;
FVector SafeRecipScale3D = GetSafeScaleReciprocal(Other.Scale3D);
Result.Scale3D = Scale3D*SafeRecipScale3D;
if (Other.Rotation.IsNormalized() == false)
{
return FTransform::Identity;
}
FQuat Inverse = Other.Rotation.Inverse();
Result.Rotation = Inverse*Rotation;
Result.Translation = (Inverse*(Translation - Other.Translation))*(SafeRecipScale3D);
#if DEBUG_INVERSE_TRANSFORM
FMatrix AM = ToMatrixWithScale();
FMatrix BM = Other.ToMatrixWithScale();
Result.DebugEqualMatrix(AM * BM.InverseFast());
#endif
return Result;
}
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;
}
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;
}
