/* * 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; }