// 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
// Since d1 = 1 ==> y1 = y2 / d2
// The same argument applies to the x coordinate.
//
// NOTE: In an orthographic projection, the last step (division by
// the length of the vector) is skipped.
//
// XXX NOTE2: The above derivation relies on the
// fact that GetReferecePlaneDepth() is 1.0.
// If we ever allow this to NOT be 1, we'll need to fix this up.
const GfVec2d &min = _window.GetMin();
const GfVec2d &max = _window.GetMax();
// Construct the corner points in 3D as follows: construct a starting
// point by using the x and y coordinates of the reference plane and
// -1 as the z coordinate. Add the position of the frustum to generate
// the actual points in world-space coordinates.
GfVec3d leftBottom =
_position + _rotation.TransformDir(GfVec3d(min[0], min[1], -1.0));
GfVec3d rightTop =
_position + _rotation.TransformDir(GfVec3d(max[0], max[1], -1.0));
// Now, transform the corner points by the given matrix
leftBottom = matrix.Transform(leftBottom);
rightTop = matrix.Transform(rightTop);
// Subtract the transformed frustum position from the transformed
// corner points. Then, rotate the points using the rotation that would
// transform the view direction vector back to (0, 0, -1). This brings
// the corner points from the woorld coordinate system into the local
// frustum one.
leftBottom -= frustum._position;
rightTop -= frustum._position;
leftBottom = frustum._rotation.GetInverse().TransformDir(leftBottom);
rightTop = frustum._rotation.GetInverse().TransformDir(rightTop);
// Finally, use the similar triangles trick to bring the corner
// points back at one unit away from the point. These scaled x and
// y coordinates can be directly used to construct the new
// transformed reference plane. Skip the scaling step for an
// orthographic projection, though.
if (_projectionType == Perspective) {
leftBottom /= scale;
rightTop /= scale;
}
frustum._window.SetMin(GfVec2d(leftBottom[0], leftBottom[1]));
frustum._window.SetMax(GfVec2d(rightTop[0], rightTop[1]));
// Note that negative scales in the transform have the potential
// to flip the window. Fix it if necessary.
GfVec2d wMin = frustum._window.GetMin();
GfVec2d wMax = frustum._window.GetMax();
// Make sure left < right
if ( wMin[0] > wMax[0] ) {
std::swap( wMin[0], wMax[0] );
}
// Make sure bottom < top
if ( wMin[1] > wMax[1] ) {
std::swap( wMin[1], wMax[1] );
}
frustum._window.SetMin( wMin );
frustum._window.SetMax( wMax );
*this = frustum;
return *this;
}