C++ GfMatrix4d::ExtractRotation方法代码示例

// Assumes rotationOrder is XYZ.
static void
_RotMatToRotXYZ(
    const GfMatrix4d &rotMat,
    GfVec3f *rotXYZ)
{
    GfRotation rot = rotMat.ExtractRotation();
    GfVec3d angles = rot.Decompose(GfVec3d::ZAxis(),
                                   GfVec3d::YAxis(),
                                   GfVec3d::XAxis());
    *rotXYZ = GfVec3f(angles[2], angles[1], angles[0]);
}

// Assumes rotationOrder is XYZ.
static void
_RotMatToRotTriplet(
    const GfMatrix4d &rotMat,
    GfVec3d *rotTriplet)
{
    GfRotation rot = rotMat.ExtractRotation();
    GfVec3d angles = rot.Decompose(GfVec3d::ZAxis(),
                                   GfVec3d::YAxis(),
                                   GfVec3d::XAxis());
    (*rotTriplet)[0] = angles[2];
    (*rotTriplet)[1] = angles[1];
    (*rotTriplet)[2] = angles[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());
}

GfFrustum &
GfFrustum::Transform(const GfMatrix4d &matrix)
{
    // We'll need the old parameters as we build up the new ones, so, work
    // on a newly instantiated frustum. We'll replace the contents of
    // this frustum with it once we are done. Note that _dirty is true
    // by default, so, there is no need to initialize it here.
    GfFrustum frustum;

    // Copy the projection type
    frustum._projectionType = _projectionType;

    // Transform the position of the frustum
    frustum._position = matrix.Transform(_position);

    // Transform the rotation as follows:
    //   1. build view and direction vectors
    //   2. transform them with the given matrix
    //   3. normalize the vectors and cross them to build an orthonormal frame
    //   4. construct a rotation matrix
    //   5. extract the new rotation from the matrix
    
    // Generate view direction and up vector
    GfVec3d viewDir = ComputeViewDirection();
    GfVec3d upVec   = ComputeUpVector();

    // Transform by matrix
    GfVec3d viewDirPrime = matrix.TransformDir(viewDir);
    GfVec3d upVecPrime = matrix.TransformDir(upVec);

    // Normalize. Save the vec size since it will be used to scale near/far.
    double scale = viewDirPrime.Normalize();
    upVecPrime.Normalize();

    // Cross them to get the third axis. Voila. We have an orthonormal frame.
    GfVec3d viewRightPrime = GfCross(viewDirPrime, upVecPrime);
    viewRightPrime.Normalize();

    // Construct a rotation matrix using the axes.
    //
    //  [ right     0 ]
    //  [ up        1 ]
    //  [ -viewDir  0 ]
    //  [ 0  0   0  1 ]
    GfMatrix4d rotMatrix;
    rotMatrix.SetIdentity();
    // first row
    rotMatrix[0][0] = viewRightPrime[0];
    rotMatrix[0][1] = viewRightPrime[1];
    rotMatrix[0][2] = viewRightPrime[2];

    // second row
    rotMatrix[1][0] = upVecPrime[0];
    rotMatrix[1][1] = upVecPrime[1];
    rotMatrix[1][2] = upVecPrime[2];

    // third row
    rotMatrix[2][0] = -viewDirPrime[0];
    rotMatrix[2][1] = -viewDirPrime[1];
    rotMatrix[2][2] = -viewDirPrime[2];

    // Extract rotation
    frustum._rotation = rotMatrix.ExtractRotation();

    // Since we applied the matrix to the direction vector, we can use
    // its length to find out the scaling that needs to applied to the
    // near and far plane. 
    frustum._nearFar = _nearFar * scale;

    // Use the same length to scale the view distance
    frustum._viewDistance = _viewDistance * scale;

    // Transform the reference plane as follows:
    //
    //   - construct two 3D points that are on the reference plane 
    //     (left/bottom and right/top corner of the reference window) 
    //   - transform the points with the given matrix
    //   - move the window back to one unit from the viewpoint and
    //     extract the 2D coordinates that would form the new reference
    //     window
    //
    //     A note on how we do the last "move" of the reference window:
    //     Using similar triangles and the fact that the reference window
    //     is one unit away from the viewpoint, one can show that it's 
    //     sufficient to divide the x and y components of the transformed
    //     corners by the length of the transformed direction vector.
    //
    //     A 2D diagram helps:
    //
    //                            |
    //                            |
    //               |            |
    //       * ------+------------+
    //      vp       |y1          |
    //                            |
    //       \--d1--/             |y2
    //
    //       \-------d2----------/
    //
    //     So, y1/y2 = d1/d2 ==> y1 = y2 * d1/d2 
//.........这里部分代码省略.........