static void
_ConvertMatrixToComponents(const GfMatrix4d &matrix,
GfVec3d *translation,
GfVec3f *rotation,
GfVec3f *scale)
{
GfMatrix4d rotMat(1.0);
GfVec3d doubleScale(1.0);
GfMatrix4d scaleOrientMatUnused, perspMatUnused;
matrix.Factor(&scaleOrientMatUnused, &doubleScale, &rotMat,
translation, &perspMatUnused);
*scale = GfVec3f(doubleScale[0], doubleScale[1], doubleScale[2]);
if (!rotMat.Orthonormalize(/* issueWarning */ false))
TF_WARN("Failed to orthonormalize rotation matrix.");
_RotMatToRotXYZ(rotMat, rotation);
}
static void
_MatrixToVectorsWithPivotInvariant(
const GfMatrix4d &m,
const GfVec3d pivotPosition,
const GfVec3d pivotOrientation,
GfVec3d *translation,
GfVec3d *rotation,
GfVec3d *scale,
GfVec3d *scaleOrientation)
{
GfMatrix3d pivotOrientMat = _EulerXYZToMatrix3d(pivotOrientation);
GfMatrix4d pp = GfMatrix4d(1.0).SetTranslate( pivotPosition);
GfMatrix4d ppInv = GfMatrix4d(1.0).SetTranslate(-pivotPosition);
GfMatrix4d po = GfMatrix4d(1.0).SetRotate(pivotOrientMat);
GfMatrix4d poInv = GfMatrix4d(1.0).SetRotate(pivotOrientMat.GetInverse());
GfMatrix4d factorMe = po * pp * m * ppInv;
GfMatrix4d scaleOrientMat, factoredRotMat, perspMat;
factorMe.Factor(&scaleOrientMat, scale, &factoredRotMat,
translation, &perspMat);
GfMatrix4d rotMat = factoredRotMat * poInv;
if(not rotMat.Orthonormalize(/* issueWarning */ false))
TF_WARN("Failed to orthonormalize rotMat.");
_RotMatToRotTriplet(rotMat, rotation);
if(not scaleOrientMat.Orthonormalize(/* issueWarning */ false))
TF_WARN("Failed to orthonormalize scaleOrientMat.");
_RotMatToRotTriplet(scaleOrientMat, scaleOrientation);
}