/*
* Make the matrix orthonormal in place using an iterative method.
* It is potentially slower if the matrix is far from orthonormal (i.e. if
* the row basis vectors are close to colinear) but in the common case
* of near-orthonormality it should be just as fast.
*
* The translation part is left intact. If the translation is represented as
* a homogenous coordinate (i.e. a non-unity lower right corner), it is divided
* out.
*/
bool
GfMatrix4f::Orthonormalize(bool issueWarning)
{
// orthogonalize and normalize row vectors
GfVec3d r0(_mtx[0][0],_mtx[0][1],_mtx[0][2]);
GfVec3d r1(_mtx[1][0],_mtx[1][1],_mtx[1][2]);
GfVec3d r2(_mtx[2][0],_mtx[2][1],_mtx[2][2]);
bool result = GfVec3d::OrthogonalizeBasis(&r0, &r1, &r2, true);
_mtx[0][0] = r0[0];
_mtx[0][1] = r0[1];
_mtx[0][2] = r0[2];
_mtx[1][0] = r1[0];
_mtx[1][1] = r1[1];
_mtx[1][2] = r1[2];
_mtx[2][0] = r2[0];
_mtx[2][1] = r2[1];
_mtx[2][2] = r2[2];
// divide out any homogeneous coordinate - unless it's zero
if (_mtx[3][3] != 1.0 && !GfIsClose(_mtx[3][3], 0.0, GF_MIN_VECTOR_LENGTH))
{
_mtx[3][0] /= _mtx[3][3];
_mtx[3][1] /= _mtx[3][3];
_mtx[3][2] /= _mtx[3][3];
_mtx[3][3] = 1.0;
}
if (!result && issueWarning)
TF_WARN("OrthogonalizeBasis did not converge, matrix may not be "
"orthonormal.");
return result;
}
/// Returns STATIC or ANIMATED if an extra translate is needed to compensate for
/// Maya's instancer translation behavior on the given prototype DAG node.
/// (This function may return false positives, which are OK but will simply
/// contribute extra data. It should never return false negatives, which
/// would cause correctness problems.)
bool
PxrUsdTranslators_InstancerWriter::_NeedsExtraInstancerTranslate(
const MDagPath& prototypeDagPath,
bool* instancerTranslateAnimated) const
{
// XXX: Maybe we could be smarter here and figure out if the animation
// affects instancerTranslate?
bool animated = !_GetExportArgs().timeSamples.empty() &&
MAnimUtil::isAnimated(prototypeDagPath.node(), false);
if (animated) {
*instancerTranslateAnimated = true;
return true;
}
GfVec3d origin;
bool translated =
_GetTransformedOriginInLocalSpace(prototypeDagPath, &origin) &&
!GfIsClose(origin, GfVec3d(0.0), _EPSILON);
if (translated) {
*instancerTranslateAnimated = false;
return true;
}
return false;
}
bool
GfIsClose(GfMatrix3f const &m1, GfMatrix3f const &m2, double tolerance)
{
for(size_t row = 0; row < 3; ++row) {
for(size_t col = 0; col < 3; ++col) {
if(!GfIsClose(m1[row][col], m2[row][col], tolerance))
return false;
}
}
return true;
}
static
bool
_GetLightingParam(
const MIntArray& intValues,
const MFloatArray& floatValues,
bool& paramValue)
{
bool gotParamValue = false;
if (intValues.length() > 0) {
paramValue = (intValues[0] == 1);
gotParamValue = true;
} else if (floatValues.length() > 0) {
paramValue = GfIsClose(floatValues[0], 1.0f, 1e-5);
gotParamValue = true;
}
return gotParamValue;
}
bool
GfIsClose(const GfRGBA &v1, const GfRGBA &v2, double tolerance)
{
return GfIsClose(v1._rgba, v2._rgba, tolerance);
}
bool MayaMeshWriter::_addDisplayPrimvars(
UsdGeomGprim &primSchema,
const MFnMesh::MColorRepresentation colorRep,
const VtArray<GfVec3f>& RGBData,
const VtArray<float>& AlphaData,
const TfToken& interpolation,
const VtArray<int>& assignmentIndices,
const int unassignedValueIndex,
const bool clamped,
const bool authored)
{
// If we already have an authored value, don't try to write a new one.
UsdAttribute colorAttr = primSchema.GetDisplayColorAttr();
if (!colorAttr.HasAuthoredValueOpinion() && !RGBData.empty()) {
UsdGeomPrimvar displayColor = primSchema.CreateDisplayColorPrimvar();
if (interpolation != displayColor.GetInterpolation()) {
displayColor.SetInterpolation(interpolation);
}
displayColor.Set(RGBData);
if (!assignmentIndices.empty()) {
displayColor.SetIndices(assignmentIndices);
if (unassignedValueIndex != displayColor.GetUnauthoredValuesIndex()) {
displayColor.SetUnauthoredValuesIndex(unassignedValueIndex);
}
}
bool authRGB = authored;
if (colorRep == MFnMesh::kAlpha) {
authRGB = false;
}
if (authRGB) {
if (clamped) {
PxrUsdMayaRoundTripUtil::MarkPrimvarAsClamped(displayColor);
}
}
else {
PxrUsdMayaRoundTripUtil::MarkAttributeAsMayaGenerated(colorAttr);
}
}
UsdAttribute alphaAttr = primSchema.GetDisplayOpacityAttr();
if (!alphaAttr.HasAuthoredValueOpinion() && !AlphaData.empty()) {
// we consider a single alpha value that is 1.0 to be the "default"
// value. We only want to write values that are not the "default".
bool hasDefaultAlpha = AlphaData.size() == 1 && GfIsClose(AlphaData[0], 1.0, 1e-9);
if (!hasDefaultAlpha) {
UsdGeomPrimvar displayOpacity = primSchema.CreateDisplayOpacityPrimvar();
if (interpolation != displayOpacity.GetInterpolation()) {
displayOpacity.SetInterpolation(interpolation);
}
displayOpacity.Set(AlphaData);
if (!assignmentIndices.empty()) {
displayOpacity.SetIndices(assignmentIndices);
if (unassignedValueIndex != displayOpacity.GetUnauthoredValuesIndex()) {
displayOpacity.SetUnauthoredValuesIndex(unassignedValueIndex);
}
}
bool authAlpha = authored;
if (colorRep == MFnMesh::kRGB) {
authAlpha = false;
}
if (authAlpha) {
if (clamped) {
PxrUsdMayaRoundTripUtil::MarkPrimvarAsClamped(displayOpacity);
}
}
else {
PxrUsdMayaRoundTripUtil::MarkAttributeAsMayaGenerated(alphaAttr);
}
}
}
return true;
}
// Populate the AnimationChannel vector with various ops based on the Maya
// transformation logic If scale and/or rotate pivot are declared, create
// inverse ops in the appropriate order
void MayaTransformWriter::pushTransformStack(
const MFnTransform& iTrans,
const UsdGeomXformable& usdXformable,
bool writeAnim)
{
// NOTE: I think this logic and the logic in MayaTransformReader
// should be merged so the concept of "CommonAPI" stays centralized.
//
// By default we assume that the xform conforms to the common API
// (xlate,pivot,rotate,scale,pivotINVERTED) As soon as we encounter any
// additional xform (compensation translates for pivots, rotateAxis or
// shear) we are not conforming anymore
bool conformsToCommonAPI = true;
// Keep track of where we have rotate and scale Pivots and their inverse so
// that we can combine them later if possible
unsigned int rotPivotIdx = -1, rotPivotINVIdx = -1, scalePivotIdx = -1, scalePivotINVIdx = -1;
// Check if the Maya prim inheritTransform
MPlug inheritPlug = iTrans.findPlug("inheritsTransform");
if (!inheritPlug.isNull()) {
if(!inheritPlug.asBool()) {
usdXformable.SetResetXformStack(true);
}
}
// inspect the translate, no suffix to be closer compatibility with common API
_GatherAnimChannel(TRANSLATE, iTrans, "translate", "X", "Y", "Z", &mAnimChanList, writeAnim, false);
// inspect the rotate pivot translate
if (_GatherAnimChannel(TRANSLATE, iTrans, "rotatePivotTranslate", "X", "Y", "Z", &mAnimChanList, writeAnim, true)) {
conformsToCommonAPI = false;
}
// inspect the rotate pivot
bool hasRotatePivot = _GatherAnimChannel(TRANSLATE, iTrans, "rotatePivot", "X", "Y", "Z", &mAnimChanList, writeAnim, true);
if (hasRotatePivot) {
rotPivotIdx = mAnimChanList.size()-1;
}
// inspect the rotate, no suffix to be closer compatibility with common API
_GatherAnimChannel(ROTATE, iTrans, "rotate", "X", "Y", "Z", &mAnimChanList, writeAnim, false);
// inspect the rotateAxis/orientation
if (_GatherAnimChannel(ROTATE, iTrans, "rotateAxis", "X", "Y", "Z", &mAnimChanList, writeAnim, true)) {
conformsToCommonAPI = false;
}
// invert the rotate pivot
if (hasRotatePivot) {
AnimChannel chan;
chan.usdOpType = UsdGeomXformOp::TypeTranslate;
chan.precision = UsdGeomXformOp::PrecisionFloat;
chan.opName = "rotatePivot";
chan.isInverse = true;
mAnimChanList.push_back(chan);
rotPivotINVIdx = mAnimChanList.size()-1;
}
// inspect the scale pivot translation
if (_GatherAnimChannel(TRANSLATE, iTrans, "scalePivotTranslate", "X", "Y", "Z", &mAnimChanList, writeAnim, true)) {
conformsToCommonAPI = false;
}
// inspect the scale pivot point
bool hasScalePivot = _GatherAnimChannel(TRANSLATE, iTrans, "scalePivot", "X", "Y", "Z", &mAnimChanList, writeAnim, true);
if (hasScalePivot) {
scalePivotIdx = mAnimChanList.size()-1;
}
// inspect the shear. Even if we have one xform on the xform list, it represents a share so we should name it
if (_GatherAnimChannel(SHEAR, iTrans, "shear", "XY", "XZ", "YZ", &mAnimChanList, writeAnim, true)) {
conformsToCommonAPI = false;
}
// add the scale. no suffix to be closer compatibility with common API
_GatherAnimChannel(SCALE, iTrans, "scale", "X", "Y", "Z", &mAnimChanList, writeAnim, false);
// inverse the scale pivot point
if (hasScalePivot) {
AnimChannel chan;
chan.usdOpType = UsdGeomXformOp::TypeTranslate;
chan.precision = UsdGeomXformOp::PrecisionFloat;
chan.opName = "scalePivot";
chan.isInverse = true;
mAnimChanList.push_back(chan);
scalePivotINVIdx = mAnimChanList.size()-1;
}
// If still potential common API, check if the pivots are the same and NOT animated/connected
if (hasRotatePivot != hasScalePivot) {
conformsToCommonAPI = false;
}
if (conformsToCommonAPI && hasRotatePivot && hasScalePivot) {
AnimChannel rotPivChan, scalePivChan;
rotPivChan = mAnimChanList[rotPivotIdx];
scalePivChan = mAnimChanList[scalePivotIdx];
// If they have different sampleType or are ANIMATED, does not conformsToCommonAPI anymore
for (unsigned int i = 0;i<3;i++) {
if (rotPivChan.sampleType[i] != scalePivChan.sampleType[i] ||
rotPivChan.sampleType[i] == ANIMATED) {
conformsToCommonAPI = false;
}
}
// If The defaultValue is not the same, does not conformsToCommonAPI anymore
if (!GfIsClose(rotPivChan.defValue, scalePivChan.defValue, 1e-9)) {
conformsToCommonAPI = false;
}
// If opType, usdType or precision are not the same, does not conformsToCommonAPI anymore
if (rotPivChan.opType != scalePivChan.opType ||
rotPivChan.usdOpType != scalePivChan.usdOpType ||
rotPivChan.precision != scalePivChan.precision) {
conformsToCommonAPI = false;
}
if (conformsToCommonAPI) {
// To Merge, we first rename rotatePivot and the scalePivot inverse
// to pivot. Then we remove the scalePivot and the inverse of the
// rotatePivot.
//
// This means that pivot and its inverse will wrap rotate and scale
// since no other ops have been found
//
// NOTE: scalePivotIdx > rotPivotINVIdx
mAnimChanList[rotPivotIdx].opName = "pivot";
mAnimChanList[scalePivotINVIdx].opName = "pivot";
mAnimChanList.erase(mAnimChanList.begin()+scalePivotIdx);
mAnimChanList.erase(mAnimChanList.begin()+rotPivotINVIdx);
}
}
// Loop over anim channel vector and create corresponding XFormOps
// including the inverse ones if needed
TF_FOR_ALL(iter, mAnimChanList) {
AnimChannel& animChan = *iter;
animChan.op = usdXformable.AddXformOp(
animChan.usdOpType, animChan.precision,
TfToken(animChan.opName),
animChan.isInverse);
}
// Creates an AnimChannel from a Maya compound attribute if there is meaningful
// data. This means we found data that is non-identity.
//
// returns true if we extracted an AnimChannel and false otherwise (e.g., the
// data was identity).
static bool
_GatherAnimChannel(
XFormOpType opType,
const MFnTransform& iTrans,
MString parentName,
MString xName, MString yName, MString zName,
std::vector<AnimChannel>* oAnimChanList,
bool isWritingAnimation,
bool setOpName)
{
AnimChannel chan;
chan.opType = opType;
chan.isInverse = false;
if (setOpName) {
chan.opName = parentName.asChar();
}
// We default to single precision (later we set the main translate op and
// shear to double)
chan.precision = UsdGeomXformOp::PrecisionFloat;
unsigned int validComponents = 0;
// this is to handle the case where there is a connection to the parent
// plug but not to the child plugs, if the connection is there and you are
// not forcing static, then all of the children are considered animated
int parentSample = PxrUsdMayaUtil::getSampledType(iTrans.findPlug(parentName),false);
// Determine what plug are needed based on default value & being
// connected/animated
MStringArray channels;
channels.append(parentName+xName);
channels.append(parentName+yName);
channels.append(parentName+zName);
GfVec3d nullValue(opType == SCALE ? 1.0 : 0.0);
for (unsigned int i = 0; i<3; i++) {
// Find the plug and retrieve the data as the channel default value. It
// won't be updated if the channel is NOT ANIMATED
chan.plug[i] = iTrans.findPlug(channels[i]);
double plugValue = chan.plug[i].asDouble();
chan.defValue[i] = opType == ROTATE ? GfRadiansToDegrees(plugValue) : plugValue;
chan.sampleType[i] = NO_XFORM;
// If we allow animation and either the parentsample or local sample is
// not 0 then we havea ANIMATED sample else we have a scale and the
// value is NOT 1 or if the value is NOT 0 then we have a static xform
if ((parentSample != 0 || PxrUsdMayaUtil::getSampledType(chan.plug[i], true) != 0) &&
isWritingAnimation) {
chan.sampleType[i] = ANIMATED;
validComponents++;
}
else if (!GfIsClose(chan.defValue[i], nullValue[i], 1e-7)) {
chan.sampleType[i] = STATIC;
validComponents++;
}
}
// If there are valid component, then we will add the animation channel.
// Rotates with 1 component will be optimized to single axis rotation
if (validComponents>0) {
if (opType == SCALE) {
chan.usdOpType = UsdGeomXformOp::TypeScale;
} else if (opType == TRANSLATE) {
chan.usdOpType = UsdGeomXformOp::TypeTranslate;
// The main translate is set to double precision
if (parentName == "translate") {
chan.precision = UsdGeomXformOp::PrecisionDouble;
}
} else if (opType == ROTATE) {
chan.usdOpType = UsdGeomXformOp::TypeRotateXYZ;
if (validComponents == 1) {
if (chan.sampleType[0] != NO_XFORM) chan.usdOpType = UsdGeomXformOp::TypeRotateX;
if (chan.sampleType[1] != NO_XFORM) chan.usdOpType = UsdGeomXformOp::TypeRotateY;
if (chan.sampleType[2] != NO_XFORM) chan.usdOpType = UsdGeomXformOp::TypeRotateZ;
}
else {
// Rotation Order ONLY applies to the "rotate" attribute
if (parentName == "rotate") {
switch (iTrans.rotationOrder()) {
case MTransformationMatrix::kYZX:
chan.usdOpType = UsdGeomXformOp::TypeRotateYZX;
break;
case MTransformationMatrix::kZXY:
chan.usdOpType = UsdGeomXformOp::TypeRotateZXY;
break;
case MTransformationMatrix::kXZY:
chan.usdOpType = UsdGeomXformOp::TypeRotateXZY;
break;
case MTransformationMatrix::kYXZ:
chan.usdOpType = UsdGeomXformOp::TypeRotateYXZ;
break;
case MTransformationMatrix::kZYX:
chan.usdOpType = UsdGeomXformOp::TypeRotateZYX;
break;
default: break;
}
}
}
}
else if (opType == SHEAR) {
chan.usdOpType = UsdGeomXformOp::TypeTransform;
chan.precision = UsdGeomXformOp::PrecisionDouble;
}
else {
return false;
}
oAnimChanList->push_back(chan);
return true;
}
return false;
}
/*
* Given 3 basis vectors *tx, *ty, *tz, orthogonalize and optionally normalize
* them.
*
* This uses an iterative method that is very stable even when the vectors
* are far from orthogonal (close to colinear). The number of iterations
* and thus the computation time does increase as the vectors become
* close to colinear, however.
*
* If the iteration fails to converge, returns false with vectors as close to
* orthogonal as possible.
*/
bool
GfOrthogonalizeBasis(GfVec3f *tx, GfVec3f *ty, GfVec3f *tz,
bool normalize, double eps)
{
GfVec3f ax,bx,cx,ay,by,cy,az,bz,cz;
if (normalize) {
GfNormalize(tx);
GfNormalize(ty);
GfNormalize(tz);
ax = *tx;
ay = *ty;
az = *tz;
} else {
ax = *tx;
ay = *ty;
az = *tz;
ax.Normalize();
ay.Normalize();
az.Normalize();
}
/* Check for colinear vectors. This is not only a quick-out: the
* error computation below will evaluate to zero if there's no change
* after an iteration, which can happen either because we have a good
* solution or because the vectors are colinear. So we have to check
* the colinear case beforehand, or we'll get fooled in the error
* computation.
*/
if (GfIsClose(ax,ay,eps) || GfIsClose(ax,az,eps) || GfIsClose(ay,az,eps)) {
return false;
}
const int MAX_ITERS = 20;
int iter;
for (iter = 0; iter < MAX_ITERS; ++iter) {
bx = *tx;
by = *ty;
bz = *tz;
bx -= GfDot(ay,bx) * ay;
bx -= GfDot(az,bx) * az;
by -= GfDot(ax,by) * ax;
by -= GfDot(az,by) * az;
bz -= GfDot(ax,bz) * ax;
bz -= GfDot(ay,bz) * ay;
cx = 0.5*(*tx + bx);
cy = 0.5*(*ty + by);
cz = 0.5*(*tz + bz);
if (normalize) {
cx.Normalize();
cy.Normalize();
cz.Normalize();
}
GfVec3f xDiff = *tx - cx;
GfVec3f yDiff = *ty - cy;
GfVec3f zDiff = *tz - cz;
double error =
GfDot(xDiff,xDiff) + GfDot(yDiff,yDiff) + GfDot(zDiff,zDiff);
// error is squared, so compare to squared tolerance
if (error < GfSqr(eps))
break;
*tx = cx;
*ty = cy;
*tz = cz;
ax = *tx;
ay = *ty;
az = *tz;
if (!normalize) {
ax.Normalize();
ay.Normalize();
az.Normalize();
}
}
return iter < MAX_ITERS;
}
/* static */
GfMatrix4d
UsdGeomXformOp::GetOpTransform(UsdGeomXformOp::Type const opType,
VtValue const &opVal,
bool isInverseOp)
{
// This will be the most common case.
if (opType == TypeTransform) {
GfMatrix4d mat(1.);
bool isMatrixVal = true;
if (opVal.IsHolding<GfMatrix4d>()) {
mat = opVal.UncheckedGet<GfMatrix4d>();
} else if (opVal.IsHolding<GfMatrix4f>()) {
mat = GfMatrix4d(opVal.UncheckedGet<GfMatrix4f>());
} else {
isMatrixVal = false;
TF_CODING_ERROR("Invalid combination of opType (%s) "
"and opVal (%s). Returning identity matrix.",
TfEnum::GetName(opType).c_str(),
TfStringify(opVal).c_str());
return GfMatrix4d(1.);
}
if (isMatrixVal && isInverseOp) {
double determinant=0;
mat = mat.GetInverse(&determinant);
if (GfIsClose(determinant, 0.0, 1e-9)) {
TF_CODING_ERROR("Cannot invert singular transform op with "
"value %s.", TfStringify(opVal).c_str());
}
}
return mat;
}
double doubleVal = 0.;
bool isScalarVal = true;
if (opVal.IsHolding<double>()) {
doubleVal = opVal.UncheckedGet<double>();
} else if (opVal.IsHolding<float>()) {
doubleVal = opVal.UncheckedGet<float>();
} else if (opVal.IsHolding<GfHalf>()) {
doubleVal = opVal.UncheckedGet<GfHalf>();
} else {
isScalarVal = false;
}
if (isScalarVal) {
if (isInverseOp)
doubleVal = -doubleVal;
if (opType == TypeRotateX) {
return GfMatrix4d(1.).SetRotate(GfRotation(GfVec3d::XAxis(), doubleVal));
} else if (opType == TypeRotateY) {
return GfMatrix4d(1.).SetRotate(GfRotation(GfVec3d::YAxis(), doubleVal));
} else if (opType == TypeRotateZ) {
return GfMatrix4d(1.).SetRotate(GfRotation(GfVec3d::ZAxis(), doubleVal));
}
}
GfVec3d vec3dVal = GfVec3d(0.);
bool isVecVal = true;
if (opVal.IsHolding<GfVec3f>()) {
vec3dVal = opVal.UncheckedGet<GfVec3f>();
} else if (opVal.IsHolding<GfVec3d>()) {
vec3dVal = opVal.UncheckedGet<GfVec3d>();
} else if (opVal.IsHolding<GfVec3h>()) {
vec3dVal = opVal.UncheckedGet<GfVec3h>();
} else {
isVecVal = false;
}
if (isVecVal) {
switch(opType) {
case TypeTranslate:
if (isInverseOp)
vec3dVal = -vec3dVal;
return GfMatrix4d(1.).SetTranslate(vec3dVal);
case TypeScale:
if (isInverseOp) {
vec3dVal = GfVec3d(1/vec3dVal[0],
1/vec3dVal[1],
1/vec3dVal[2]);
}
return GfMatrix4d(1.).SetScale(vec3dVal);
default: {
if (isInverseOp)
vec3dVal = -vec3dVal;
// Must be one of the 3-axis rotates.
GfMatrix3d xRot(GfRotation(GfVec3d::XAxis(), vec3dVal[0]));
GfMatrix3d yRot(GfRotation(GfVec3d::YAxis(), vec3dVal[1]));
GfMatrix3d zRot(GfRotation(GfVec3d::ZAxis(), vec3dVal[2]));
GfMatrix3d rotationMat(1.);
switch (opType) {
case TypeRotateXYZ:
// Inv(ABC) = Inv(C) * Inv(B) * Inv(A)
rotationMat = !isInverseOp ? (xRot * yRot * zRot)
: (zRot * yRot * xRot);
break;
case TypeRotateXZY:
rotationMat = !isInverseOp ? (xRot * zRot * yRot)
: (yRot * zRot * xRot);
break;
case TypeRotateYXZ:
rotationMat = !isInverseOp ? (yRot * xRot * zRot)
: (zRot * xRot * yRot);
break;
case TypeRotateYZX:
rotationMat = !isInverseOp ? (yRot * zRot * xRot)
: (xRot * zRot * yRot);
break;
case TypeRotateZXY:
rotationMat = !isInverseOp ? (zRot * xRot * yRot)
: (yRot * xRot * zRot);
break;
case TypeRotateZYX:
rotationMat = !isInverseOp ? (zRot * yRot * xRot)
: (xRot * yRot * zRot);
break;
default:
TF_CODING_ERROR("Invalid combination of opType (%s) "
"and opVal (%s). Returning identity matrix.",
TfEnum::GetName(opType).c_str(),
TfStringify(opVal).c_str());
return GfMatrix4d(1.);
}
return GfMatrix4d(1.).SetRotate(rotationMat);
}
}
}
if (opType == TypeOrient) {
GfQuatd quatVal(0);
if (opVal.IsHolding<GfQuatd>())
quatVal = opVal.UncheckedGet<GfQuatd>();
else if (opVal.IsHolding<GfQuatf>()) {
const GfQuatf &quatf = opVal.UncheckedGet<GfQuatf>();
quatVal = GfQuatd(quatf.GetReal(), quatf.GetImaginary());
} else if (opVal.IsHolding<GfQuath>()) {
const GfQuath &quath = opVal.UncheckedGet<GfQuath>();
quatVal = GfQuatd(quath.GetReal(), quath.GetImaginary());
}
GfRotation quatRotation(quatVal);
if (isInverseOp)
quatRotation = quatRotation.GetInverse();
return GfMatrix4d(quatRotation, GfVec3d(0.));
}
TF_CODING_ERROR("Invalid combination of opType (%s) and opVal (%s). "
"Returning identity matrix.", TfEnum::GetName(opType).c_str(),
TfStringify(opVal).c_str());
return GfMatrix4d(1.);
}
bool
GfRay::_SolveQuadratic(const double a,
const double b,
const double c,
double *enterDistance,
double *exitDistance) const
{
if (GfIsClose(a, 0.0, tolerance)) {
if (GfIsClose(b, 0.0, tolerance)) {
// Degenerate solution
return false;
}
double t = -c / b;
if (t < 0.0) {
return false;
}
if (enterDistance) {
*enterDistance = t;
}
if (exitDistance) {
*exitDistance = t;
}
return true;
}
// Discriminant
double disc = GfSqr(b) - 4.0 * a * c;
if (GfIsClose(disc, 0.0, tolerance)) {
// Tangent
double t = -b / (2.0 * a);
if (t < 0.0) {
return false;
}
if (enterDistance) {
*enterDistance = t;
}
if (exitDistance) {
*exitDistance = t;
}
return true;
}
if (disc < 0.0) {
// No intersection
return false;
}
// Two intersection points
double q = -0.5 * (b + copysign(1.0, b) * GfSqrt(disc));
double t0 = q / a;
double t1 = c / q;
if (t0 > t1) {
std::swap(t0, t1);
}
if (t1 >= 0) {
if (enterDistance) {
*enterDistance = t0;
}
if (exitDistance) {
*exitDistance = t1;
}
return true;
}
return false;
}
bool
GfFindClosestPoints( const GfLine2d &l1, const GfLine2d &l2,
GfVec2d *closest1, GfVec2d *closest2,
double *t1, double *t2 )
{
// Define terms:
// p1 = line 1's position
// d1 = line 1's direction
// p2 = line 2's position
// d2 = line 2's direction
const GfVec2d &p1 = l1._p0;
const GfVec2d &d1 = l1._dir;
const GfVec2d &p2 = l2._p0;
const GfVec2d &d2 = l2._dir;
// We want to find points closest1 and closest2 on each line.
// Their parametric definitions are:
// closest1 = p1 + t1 * d1
// closest2 = p2 + t2 * d2
//
// We know that the line connecting closest1 and closest2 is
// perpendicular to both the ray and the line segment. So:
// d1 . (closest2 - closest1) = 0
// d2 . (closest2 - closest1) = 0
//
// Substituting gives us:
// d1 . [ (p2 + t2 * d2) - (p1 + t1 * d1) ] = 0
// d2 . [ (p2 + t2 * d2) - (p1 + t1 * d1) ] = 0
//
// Rearranging terms gives us:
// t2 * (d1.d2) - t1 * (d1.d1) = d1.p1 - d1.p2
// t2 * (d2.d2) - t1 * (d2.d1) = d2.p1 - d2.p2
//
// Substitute to simplify:
// a = d1.d2
// b = d1.d1
// c = d1.p1 - d1.p2
// d = d2.d2
// e = d2.d1 (== a, if you're paying attention)
// f = d2.p1 - d2.p2
double a = GfDot(d1, d2);
double b = GfDot(d1, d1);
double c = GfDot(d1, p1) - GfDot(d1, p2);
double d = GfDot(d2, d2);
double e = a;
double f = GfDot(d2, p1) - GfDot(d2, p2);
// And we end up with:
// t2 * a - t1 * b = c
// t2 * d - t1 * e = f
//
// Solve for t1 and t2:
// t1 = (c * d - a * f) / (a * e - b * d)
// t2 = (c * e - b * f) / (a * e - b * d)
//
// Note the identical denominators...
double denom = a * e - b * d;
// Denominator == 0 means the lines are parallel; no intersection.
if ( GfIsClose( denom, 0, 1e-6 ) )
return false;
double lt1 = (c * d - a * f) / denom;
double lt2 = (c * e - b * f) / denom;
if ( closest1 )
*closest1 = l1.GetPoint( lt1 );
if ( closest2 )
*closest2 = l2.GetPoint( lt2 );
if ( t1 )
*t1 = lt1;
if ( t2 )
*t2 = lt2;
return true;
}
