Exemple #1
0
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);
}