void
GfFrustum::SetPositionAndRotationFromMatrix(
const GfMatrix4d &camToWorldXf)
{
// First conform matrix to be...
GfMatrix4d conformedXf = camToWorldXf;
// ... right handed
if (!conformedXf.IsRightHanded()) {
static GfMatrix4d flip(GfVec4d(-1.0, 1.0, 1.0, 1.0));
conformedXf = flip * conformedXf;
}
// ... and orthonormal
conformedXf.Orthonormalize();
SetRotation(conformedXf.ExtractRotation());
SetPosition(conformedXf.ExtractTranslation());
}
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);
}