void PolygonLine :: computeIntersectionPoints(const FloatArray &iXStart, const FloatArray &iXEnd, std :: vector< FloatArray > &oIntersectionPoints) const { const double detTol = 1.0e-15; int numSeg = this->giveNrVertices() - 1; for(int segIndex = 1; segIndex <= numSeg; segIndex++) { const FloatArray &xStart = this->giveVertex(segIndex); const FloatArray &xEnd = this->giveVertex(segIndex+1); const FloatArray t1 = {xEnd(0) - xStart(0), xEnd(1) - xStart(1)}; const FloatArray t2 = {iXEnd(0) - iXStart(0), iXEnd(1) - iXStart(1)}; double xi1 = 0.0, xi2 = 0.0; int maxIter = 1; for(int iter = 0; iter < maxIter; iter++) { FloatArray temp = {iXStart(0) + xi2*t2(0) - xStart(0) - xi1*t1(0), iXStart(1) + xi2*t2(1) - xStart(1) - xi1*t1(1)}; FloatArray res = {-t1.dotProduct(temp), t2.dotProduct(temp)}; //printf("iter: %d res: %e\n", iter, res.computeNorm() ); FloatMatrix K(2,2); K(0,0) = t1.dotProduct(t1); K(0,1) = -t1.dotProduct(t2); K(1,0) = -t1.dotProduct(t2); K(1,1) = t2.dotProduct(t2); double detK = K.giveDeterminant(); if(detK < detTol) { return; } FloatMatrix KInv; KInv.beInverseOf(K); FloatArray dxi; dxi.beProductOf(KInv, res); xi1 -= dxi(0); xi2 -= dxi(1); } // printf("xi1: %e xi2: %e\n", xi1, xi2); if(xi1 >= 0.0 && xi1 <= 1.0 && xi2 >= 0.0 && xi2 <= 1.0) { FloatArray pos = xStart; pos.add(xi1, t1); oIntersectionPoints.push_back(pos); } } }
void Node2NodeContact :: computeGap(FloatArray &answer, TimeStep *tStep) { FloatArray xs, xm, uS, uM; xs = *this->slaveNode->giveCoordinates(); xm = *this->masterNode->giveCoordinates(); this->slaveNode->giveUnknownVector(uS, {D_u, D_v, D_w}, VM_Total, tStep, true); this->masterNode->giveUnknownVector(uM, {D_u, D_v, D_w}, VM_Total, tStep, true); xs.add(uS); xm.add(uM); FloatArray dx = xs-xm; FloatArray normal = this->giveNormal(); answer = {dx.dotProduct(normal), 0.0, 0.0}; //printf("normal gap = %e \n", answer.at(1)); if ( answer.at(1) < 0.0 ) { //printf("normal gap = %e \n", answer.at(1)); this->inContact = true; // store in gp? } else { this->inContact = false; } }
void TrabBoneNLEmbed :: giveRealStressVector(FloatArray &answer, MatResponseForm form, GaussPoint *gp, const FloatArray &strainVector, TimeStep *atTime) { TrabBoneNLEmbedStatus *nlStatus = ( TrabBoneNLEmbedStatus * ) this->giveStatus(gp); double tempDam, tempTSED; FloatArray plasDef, totalStress; FloatMatrix compliance, elasticity; compliance.resize(6, 6); this->constructIsoComplTensor(compliance, eps0, nu0); elasticity.beInverseOf(compliance); tempDam = 0; plasDef.resize(6); totalStress.beProductOf(elasticity, strainVector); tempTSED = 0.5 * strainVector.dotProduct(totalStress); answer.resize(6); answer = totalStress; nlStatus->setTempDam(tempDam); nlStatus->letTempStrainVectorBe(strainVector); nlStatus->letTempStressVectorBe(answer); nlStatus->setTempTSED(tempTSED); }
double IntElLine1PhF :: computeAreaAround(IntegrationPoint *ip) { FloatArray G; this->computeCovarBaseVectorAt(ip, G); double weight = ip->giveWeight(); double ds = sqrt( G.dotProduct(G) ) * weight; if ( this->axisymmode ) { int numNodes = this->giveNumberOfNodes(); FloatArray N; this->interp.evalN( N, ip->giveNaturalCoordinates(), FEIElementGeometryWrapper(this) ); // interpolate radius double r = 0.0; for ( int i = 1; i <= N.giveSize(); i++ ) { double X_i = 0.5 * ( this->giveNode(i)->giveCoordinate(1) + this->giveNode(i + numNodes / 2)->giveCoordinate(1) ); // X-coord of the fictious mid surface r += N.at(i) * X_i; } return ds * r; } else { // regular 2d double thickness = this->giveCrossSection()->give(CS_Thickness, ip); return ds * thickness; } }
void TrabBoneEmbed :: giveRealStressVector(FloatArray &answer, MatResponseForm form, GaussPoint *gp, const FloatArray &totalStrain, TimeStep *atTime) { double tempDam, tempTSED; FloatArray newTotalDef, plasDef; FloatArray totalStress; FloatMatrix compliance, elasticity; this->constructIsoComplTensor(compliance, eps0, nu0); elasticity.beInverseOf(compliance); TrabBoneEmbedStatus *status = ( TrabBoneEmbedStatus * ) this->giveStatus(gp); this->initGpForNewStep(gp); performPlasticityReturn(gp, totalStrain); tempDam = computeDamage(gp, atTime); plasDef.resize(6); totalStress.beProductOf(elasticity, totalStrain); tempTSED = 0.5 * totalStrain.dotProduct(totalStress); answer.resize(6); answer = totalStress; status->setTempDam(tempDam); status->letTempStrainVectorBe(totalStrain); status->letTempStressVectorBe(answer); status->setTempTSED(tempTSED); }
void EnrichmentItem :: calcPolarCoord(double &oR, double &oTheta, const FloatArray &iOrigin, const FloatArray &iPos, const FloatArray &iN, const FloatArray &iT, const EfInput &iEfInput, bool iFlipTangent) { FloatArray q = { iPos.at(1) - iOrigin.at(1), iPos.at(2) - iOrigin.at(2) }; const double tol = 1.0e-20; // Compute polar coordinates oR = iOrigin.distance(iPos); if ( oR > tol ) { q.times(1.0 / oR); } const double pi = M_PI; // if( q.dotProduct(iT) > 0.0 ) { // oTheta = asin( q.dotProduct(iN) ); // } else { // if ( q.dotProduct(iN) > 0.0 ) { // oTheta = pi - asin( q.dotProduct(iN) ); // } else { // oTheta = -pi - asin( q.dotProduct(iN) ); // } // } const double tol_q = 1.0e-3; double phi = iEfInput.mLevelSet; if ( iFlipTangent ) { phi *= -1.0; } double phi_r = 0.0; if ( oR > tol ) { phi_r = fabs(phi / oR); } if ( phi_r > 1.0 - XfemTolerances :: giveApproxZero() ) { phi_r = 1.0 - XfemTolerances :: giveApproxZero(); } if ( iEfInput.mArcPos < tol_q || iEfInput.mArcPos > ( 1.0 - tol_q ) ) { double q_dot_n = q.dotProduct(iN); if ( q_dot_n > 1.0 - XfemTolerances :: giveApproxZero() ) { q_dot_n = 1.0 - XfemTolerances :: giveApproxZero(); } oTheta = asin(q_dot_n); } else { if ( phi > 0.0 ) { oTheta = pi - asin( fabs(phi_r) ); } else { oTheta = -pi + asin( fabs(phi_r) ); } } }
double Line :: computeTangentialDistanceToEnd(FloatArray *point) { FloatArray projection; this->computeProjection(projection); FloatArray tmp; tmp.beDifferenceOf(* point, mVertices [ 1 ]); return tmp.dotProduct(projection) / projection.computeNorm(); }
double StructuralInterfaceElementPhF :: computeDamageAt(GaussPoint *gp, ValueModeType valueMode, TimeStep *stepN) { // d = N_d * a_d FloatArray dVec; computeDamageUnknowns(dVec, valueMode, stepN); FloatArray Nvec; this->giveInterpolation()->evalN(Nvec, gp->giveNaturalCoordinates(), FEIElementGeometryWrapper(this)); //return Nvec.dotProduct(dVec); return Nvec.dotProduct( {dVec.at(1), dVec.at(2) }); }
double PhaseFieldElement :: computeDamageAt(GaussPoint *gp, ValueModeType valueMode, TimeStep *stepN) { // d = N_d * a_d NLStructuralElement *el = this->giveElement(); FloatArray dVec; computeDamageUnknowns(dVec, valueMode, stepN); FloatArray Nvec; el->giveInterpolation()->evalN(Nvec, *gp->giveNaturalCoordinates(), FEIElementGeometryWrapper(el)); return Nvec.dotProduct(dVec); }
double IntElLine1 :: computeAreaAround(IntegrationPoint *ip) { FloatArray G; this->computeCovarBaseVectorAt(ip, G); double weight = ip->giveWeight(); double ds = sqrt( G.dotProduct(G) ) * weight; double thickness = this->giveCrossSection()->give(CS_Thickness, ip); return ds * thickness; }
double SolutionbasedShapeFunction :: giveUnknown(ValueModeType mode, TimeStep *tStep, ActiveDof *dof) { // Return value of pertinent quantity in coordinate given by dof FloatArray shapeFunctionValues; computeDofTransformation(dof, shapeFunctionValues); FloatArray gamma; myNode->giveUnknownVector(gamma, myDofIDs, mode, tStep); // alpha1, alpha2,... double out = shapeFunctionValues.dotProduct(gamma); return out; }
double ParallelContext :: localDotProduct(const FloatArray &a, const FloatArray &b) { #ifdef __PARALLEL_MODE if ( emodel->isParallel() ) { double val = 0.0, val_tot = 0.0; int size = a.giveSize(); Natural2LocalOrdering *n2l = this->giveN2Lmap(); for ( int i = 0; i < size; i++ ) { if ( n2l->giveNewEq(i + 1) ) { val += a(i) * b(i); } } MPI_Allreduce( & val, & val_tot, 1, MPI_DOUBLE, MPI_SUM, this->emodel->giveParallelComm() ); return val_tot; } #endif return a.dotProduct(b); }
void PLHoopStressCirc :: propagateInterfaces(Domain &iDomain, EnrichmentDomain &ioEnrDom) { // Fetch crack tip data TipInfo tipInfoStart, tipInfoEnd; ioEnrDom.giveTipInfos(tipInfoStart, tipInfoEnd); std :: vector< TipInfo >tipInfo = {tipInfoStart, tipInfoEnd}; SpatialLocalizer *localizer = iDomain.giveSpatialLocalizer(); for ( size_t tipIndex = 0; tipIndex < tipInfo.size(); tipIndex++ ) { // Construct circle points on an arc from -90 to 90 degrees double angle = -90.0 + mAngleInc; std :: vector< double >angles; while ( angle <= ( 90.0 - mAngleInc ) ) { angles.push_back(angle * M_PI / 180.0); angle += mAngleInc; } const FloatArray &xT = tipInfo [ tipIndex ].mGlobalCoord; const FloatArray &t = tipInfo [ tipIndex ].mTangDir; const FloatArray &n = tipInfo [ tipIndex ].mNormalDir; // It is meaningless to propagate a tip that is not inside any element Element *el = localizer->giveElementContainingPoint(tipInfo [ tipIndex ].mGlobalCoord); if ( el != NULL ) { std :: vector< FloatArray >circPoints; for ( size_t i = 0; i < angles.size(); i++ ) { FloatArray tangent(2); tangent.zero(); tangent.add(cos(angles [ i ]), t); tangent.add(sin(angles [ i ]), n); tangent.normalize(); FloatArray x(xT); x.add(mRadius, tangent); circPoints.push_back(x); } std :: vector< double >sigTTArray, sigRTArray; // Loop over circle points for ( size_t pointIndex = 0; pointIndex < circPoints.size(); pointIndex++ ) { FloatArray stressVec; if ( mUseRadialBasisFunc ) { // Interpolate stress with radial basis functions // Choose a cut-off length l: // take the distance between two nodes in the element containing the // crack tip multiplied by a constant factor. // ( This choice implies that we hope that the element has reasonable // aspect ratio.) const FloatArray &x1 = * ( el->giveDofManager(1)->giveCoordinates() ); const FloatArray &x2 = * ( el->giveDofManager(2)->giveCoordinates() ); const double l = 1.0 * x1.distance(x2); // Use the octree to get all elements that have // at least one Gauss point in a certain region around the tip. const double searchRadius = 3.0 * l; std :: set< int >elIndices; localizer->giveAllElementsWithIpWithinBox(elIndices, circPoints [ pointIndex ], searchRadius); // Loop over the elements and Gauss points obtained. // Evaluate the interpolation. FloatArray sumQiWiVi; double sumWiVi = 0.0; for ( int elIndex: elIndices ) { Element *gpEl = iDomain.giveElement(elIndex); IntegrationRule *iRule = gpEl->giveDefaultIntegrationRulePtr(); for ( GaussPoint *gp_i: *iRule ) { //////////////////////////////////////// // Compute global gp coordinates FloatArray N; FEInterpolation *interp = gpEl->giveInterpolation(); interp->evalN( N, * ( gp_i->giveCoordinates() ), FEIElementGeometryWrapper(gpEl) ); // Compute global coordinates of Gauss point FloatArray globalCoord(2); globalCoord.zero(); for ( int i = 1; i <= gpEl->giveNumberOfDofManagers(); i++ ) { DofManager *dMan = gpEl->giveDofManager(i); globalCoord.at(1) += N.at(i) * dMan->giveCoordinate(1); globalCoord.at(2) += N.at(i) * dMan->giveCoordinate(2); } //////////////////////////////////////// // Compute weight of kernel function FloatArray tipToGP; tipToGP.beDifferenceOf(globalCoord, xT); bool inFrontOfCrack = true; if ( tipToGP.dotProduct(t) < 0.0 ) { inFrontOfCrack = false; } double r = circPoints [ pointIndex ].distance(globalCoord); if ( r < l && inFrontOfCrack ) { double w = ( ( l - r ) / ( pow(2.0 * M_PI, 1.5) * pow(l, 3) ) ) * exp( -0.5 * pow(r, 2) / pow(l, 2) ); // Compute gp volume double V = gpEl->computeVolumeAround(gp_i); // Get stress StructuralMaterialStatus *ms = dynamic_cast< StructuralMaterialStatus * >( gp_i->giveMaterialStatus() ); if ( ms == NULL ) { OOFEM_ERROR("failed to fetch MaterialStatus."); } FloatArray stressVecGP = ms->giveStressVector(); if ( sumQiWiVi.giveSize() != stressVecGP.giveSize() ) { sumQiWiVi.resize( stressVecGP.giveSize() ); sumQiWiVi.zero(); } // Add to numerator sumQiWiVi.add(w * V, stressVecGP); // Add to denominator sumWiVi += w * V; } } } if ( fabs(sumWiVi) > 1.0e-12 ) { stressVec.beScaled(1.0 / sumWiVi, sumQiWiVi); } else { // Take stress from closest Gauss point int region = 1; bool useCZGP = false; GaussPoint &gp = * ( localizer->giveClosestIP(circPoints [ pointIndex ], region, useCZGP) ); // Compute stresses StructuralMaterialStatus *ms = dynamic_cast< StructuralMaterialStatus * >( gp.giveMaterialStatus() ); if ( ms == NULL ) { OOFEM_ERROR("failed to fetch MaterialStatus."); } stressVec = ms->giveStressVector(); } } else { // Take stress from closest Gauss point int region = 1; bool useCZGP = false; GaussPoint &gp = * ( localizer->giveClosestIP(circPoints [ pointIndex ], region, useCZGP) ); // Compute stresses StructuralMaterialStatus *ms = dynamic_cast< StructuralMaterialStatus * >( gp.giveMaterialStatus() ); if ( ms == NULL ) { OOFEM_ERROR("failed to fetch MaterialStatus."); } stressVec = ms->giveStressVector(); } FloatMatrix stress(2, 2); int shearPos = stressVec.giveSize(); stress.at(1, 1) = stressVec.at(1); stress.at(1, 2) = stressVec.at(shearPos); stress.at(2, 1) = stressVec.at(shearPos); stress.at(2, 2) = stressVec.at(2); // Rotation matrix FloatMatrix rot(2, 2); rot.at(1, 1) = cos(angles [ pointIndex ]); rot.at(1, 2) = -sin(angles [ pointIndex ]); rot.at(2, 1) = sin(angles [ pointIndex ]); rot.at(2, 2) = cos(angles [ pointIndex ]); FloatArray tRot, nRot; tRot.beProductOf(rot, t); nRot.beProductOf(rot, n); FloatMatrix rotTot(2, 2); rotTot.setColumn(tRot, 1); rotTot.setColumn(nRot, 2); FloatMatrix tmp, stressRot; tmp.beTProductOf(rotTot, stress); stressRot.beProductOf(tmp, rotTot); const double sigThetaTheta = stressRot.at(2, 2); sigTTArray.push_back(sigThetaTheta); const double sigRTheta = stressRot.at(1, 2); sigRTArray.push_back(sigRTheta); } ////////////////////////////// // Compute propagation angle // Find angles that fulfill sigRT = 0 const double stressTol = 1.0e-9; double maxSigTT = 0.0, maxAngle = 0.0; bool foundZeroLevel = false; for ( size_t segIndex = 0; segIndex < ( circPoints.size() - 1 ); segIndex++ ) { // If the shear stress sigRT changes sign over the segment if ( sigRTArray [ segIndex ] * sigRTArray [ segIndex + 1 ] < stressTol ) { // Compute location of zero level double xi = EnrichmentItem :: calcXiZeroLevel(sigRTArray [ segIndex ], sigRTArray [ segIndex + 1 ]); double theta = 0.5 * ( 1.0 - xi ) * angles [ segIndex ] + 0.5 * ( 1.0 + xi ) * angles [ segIndex + 1 ]; double sigThetaTheta = 0.5 * ( 1.0 - xi ) * sigTTArray [ segIndex ] + 0.5 * ( 1.0 + xi ) * sigTTArray [ segIndex + 1 ]; // printf("Found candidate: theta: %e sigThetaTheta: %e\n", theta, sigThetaTheta); if ( sigThetaTheta > maxSigTT ) { foundZeroLevel = true; maxSigTT = sigThetaTheta; maxAngle = theta; } } } if ( !foundZeroLevel ) { printf("No zero level was found.\n"); } if ( iDomain.giveXfemManager()->giveVtkDebug() ) { XFEMDebugTools :: WriteArrayToMatlab("sigTTvsAngle.m", angles, sigTTArray); XFEMDebugTools :: WriteArrayToMatlab("sigRTvsAngle.m", angles, sigRTArray); XFEMDebugTools :: WriteArrayToGnuplot("sigTTvsAngle.dat", angles, sigTTArray); XFEMDebugTools :: WriteArrayToGnuplot("sigRTvsAngle.dat", angles, sigRTArray); } // Compare with threshold if ( maxSigTT > mHoopStressThreshold && foundZeroLevel ) { // Rotation matrix FloatMatrix rot(2, 2); rot.at(1, 1) = cos(maxAngle); rot.at(1, 2) = -sin(maxAngle); rot.at(2, 1) = sin(maxAngle); rot.at(2, 2) = cos(maxAngle); FloatArray dir; dir.beProductOf(rot, tipInfo [ tipIndex ].mTangDir); // Fill up struct std :: vector< TipPropagation >tipPropagations; TipPropagation tipProp; tipProp.mTipIndex = tipIndex; tipProp.mPropagationDir = dir; tipProp.mPropagationLength = mIncrementLength; tipPropagations.push_back(tipProp); // Propagate ioEnrDom.propagateTips(tipPropagations); } } } }
void SolutionbasedShapeFunction :: computeCorrectionFactors(modeStruct &myMode, IntArray *Dofs, double *am, double *ap) { /* * *Compute c0, cp, cm, Bp, Bm, Ap and Am */ double A0p = 0.0, App = 0.0, A0m = 0.0, Amm = 0.0, Bp = 0.0, Bm = 0.0, c0 = 0.0, cp = 0.0, cm = 0.0; EngngModel *m = myMode.myEngngModel; Set *mySet = m->giveDomain(1)->giveSet(externalSet); IntArray BoundaryList = mySet->giveBoundaryList(); for ( int i = 0; i < BoundaryList.giveSize() / 2; i++ ) { int ElementID = BoundaryList(2 * i); int Boundary = BoundaryList(2 * i + 1); Element *thisElement = m->giveDomain(1)->giveElement(ElementID); FEInterpolation *geoInterpolation = thisElement->giveInterpolation(); IntArray bnodes, zNodes, pNodes, mNodes; FloatMatrix nodeValues; geoInterpolation->boundaryGiveNodes(bnodes, Boundary); nodeValues.resize( this->dofs.giveSize(), bnodes.giveSize() ); nodeValues.zero(); // Change to global ID for bnodes and identify the intersection of bnodes and the zero boundary splitBoundaryNodeIDs(myMode, * thisElement, bnodes, pNodes, mNodes, zNodes, nodeValues); std :: unique_ptr< IntegrationRule >iRule(geoInterpolation->giveBoundaryIntegrationRule(order, Boundary)); for ( GaussPoint *gp: *iRule ) { FloatArray *lcoords = gp->giveCoordinates(); FloatArray gcoords, normal, N; FloatArray Phi; double detJ = fabs( geoInterpolation->boundaryGiveTransformationJacobian( Boundary, * lcoords, FEIElementGeometryWrapper(thisElement) ) ) * gp->giveWeight(); geoInterpolation->boundaryEvalNormal( normal, Boundary, * lcoords, FEIElementGeometryWrapper(thisElement) ); geoInterpolation->boundaryEvalN( N, Boundary, * lcoords, FEIElementGeometryWrapper(thisElement) ); geoInterpolation->boundaryLocal2Global( gcoords, Boundary, * lcoords, FEIElementGeometryWrapper(thisElement) ); FloatArray pPhi, mPhi, zPhi; pPhi.resize( Dofs->giveSize() ); pPhi.zero(); mPhi.resize( Dofs->giveSize() ); mPhi.zero(); zPhi.resize( Dofs->giveSize() ); zPhi.zero(); // Build phi (analytical averaging, not projected onto the mesh) computeBaseFunctionValueAt(Phi, gcoords, * Dofs, * myMode.myEngngModel); // Build zPhi for this DofID for ( int l = 1; l <= zNodes.giveSize(); l++ ) { int nodeID = zNodes.at(l); for ( int m = 1; m <= this->dofs.giveSize(); m++ ) { zPhi.at(m) = zPhi.at(m) + N.at(nodeID) * nodeValues.at(m, nodeID); } } // Build pPhi for this DofID for ( int l = 1; l <= pNodes.giveSize(); l++ ) { int nodeID = pNodes.at(l); for ( int m = 1; m <= this->dofs.giveSize(); m++ ) { pPhi.at(m) = pPhi.at(m) + N.at(nodeID) * nodeValues.at(m, nodeID); } } // Build mPhi for this DofID for ( int l = 1; l <= mNodes.giveSize(); l++ ) { int nodeID = mNodes.at(l); for ( int m = 1; m <= this->dofs.giveSize(); m++ ) { mPhi.at(m) = mPhi.at(m) + N.at(nodeID) * nodeValues.at(m, nodeID); } } c0 = c0 + zPhi.dotProduct(normal, 3) * detJ; cp = cp + pPhi.dotProduct(normal, 3) * detJ; cm = cm + mPhi.dotProduct(normal, 3) * detJ; App = App + pPhi.dotProduct(pPhi, 3) * detJ; Amm = Amm + mPhi.dotProduct(mPhi, 3) * detJ; A0p = A0p + zPhi.dotProduct(pPhi, 3) * detJ; A0m = A0m + zPhi.dotProduct(mPhi, 3) * detJ; Bp = Bp + Phi.dotProduct(pPhi, 3) * detJ; Bm = Bm + Phi.dotProduct(mPhi, 3) * detJ; } } * am = -( A0m * cp * cp - Bm * cp * cp - A0p * cm * cp + App * c0 * cm + Bp * cm * cp ) / ( App * cm * cm + Amm * cp * cp ); * ap = -( A0p * cm * cm - Bp * cm * cm - A0m * cm * cp + Amm * c0 * cp + Bm * cm * cp ) / ( App * cm * cm + Amm * cp * cp ); }
void PolygonLine :: computeNormalSignDist(double &oDist, const FloatArray &iPoint) const { const FloatArray &point = {iPoint[0], iPoint[1]}; oDist = std :: numeric_limits< double > :: max(); int numSeg = this->giveNrVertices() - 1; // TODO: This can probably be done in a nicer way. // Ensure that we work in 2d. const int dim = 2; for ( int segId = 1; segId <= numSeg; segId++ ) { // Crack segment const FloatArray &crackP1( this->giveVertex ( segId ) ); const FloatArray &crackP2( this->giveVertex ( segId + 1 ) ); double dist2 = 0.0; if ( segId == 1 ) { // Vector from start P1 to point X FloatArray u = {point.at(1) - crackP1.at(1), point.at(2) - crackP1.at(2)}; // Line tangent vector FloatArray t = {crackP2.at(1) - crackP1.at(1), crackP2.at(2) - crackP1.at(2)}; double l2 = t.computeSquaredNorm(); if ( l2 > 0.0 ) { double l = t.normalize(); double s = dot(u, t); if ( s > l ) { // X is closest to P2 dist2 = point.distance_square(crackP2); } else { double xi = s / l; FloatArray q = ( 1.0 - xi ) * crackP1 + xi * crackP2; dist2 = point.distance_square(q); } } else { // If the points P1 and P2 coincide, // we can compute the distance to any // of these points. dist2 = point.distance_square(crackP1); } } else if ( segId == numSeg ) { // Vector from start P1 to point X FloatArray u = {point.at(1) - crackP1.at(1), point.at(2) - crackP1.at(2)}; // Line tangent vector FloatArray t = {crackP2.at(1) - crackP1.at(1), crackP2.at(2) - crackP1.at(2)}; double l2 = t.computeSquaredNorm(); if ( l2 > 0.0 ) { double l = t.normalize(); double s = dot(u, t); if ( s < 0.0 ) { // X is closest to P1 dist2 = point.distance_square(crackP1); } else { double xi = s / l; FloatArray q = ( 1.0 - xi ) * crackP1 + xi * crackP2; dist2 = point.distance_square(q); } } else { // If the points P1 and P2 coincide, // we can compute the distance to any // of these points. dist2 = point.distance_square(crackP1); } } else { double arcPos = -1.0, dummy; dist2 = point.distance_square(crackP1, crackP2, arcPos, dummy); } if ( dist2 < oDist*oDist ) { FloatArray lineToP; lineToP.beDifferenceOf(point, crackP1, dim); FloatArray t; t.beDifferenceOf(crackP2, crackP1, dim); FloatArray n = {-t.at(2), t.at(1)}; oDist = sgn( lineToP.dotProduct(n) ) * sqrt(dist2); } } }
double BasicGeometry :: computeLineDistance(const FloatArray &iP1, const FloatArray &iP2, const FloatArray &iQ1, const FloatArray &iQ2) { FloatArray u; u.beDifferenceOf(iP2, iP1); const double LengthP = u.computeNorm(); FloatArray v; v.beDifferenceOf(iQ2, iQ1); const double LengthQ = v.computeNorm(); // Regularization coefficients (to make it possible to solve when lines are parallel) const double c1 = (1.0e-14)*LengthP*LengthP; const double c2 = (1.0e-14)*LengthQ*LengthQ; const size_t minIter = 2; const size_t maxIter = 5; const double absTol = 1.0e-12; double xi = 0.0, eta = 0.0; FloatArray d; d = iP1; d.add(xi,u); d.add(-1.0, iQ1); d.add(-eta, v); FloatMatrix K(2,2), KInv; FloatArray dXi; bool lockXi = false, lockEta = false; for(size_t iter = 0; iter < maxIter; iter++) { if(xi < 0.0) { xi = 0.0; lockXi = true; } if(xi > 1.0) { xi = 1.0; lockXi = true; } if(eta < 0.0) { eta = 0.0; lockEta = true; } if(eta > 1.0) { eta = 1.0; lockEta = true; } FloatArray R = { d.dotProduct(u) + c1*xi, -d.dotProduct(v) + c2*eta}; if(lockXi) { R[0] = 0.0; } if(lockEta) { R[1] = 0.0; } const double res = R.computeNorm(); // printf("iter: %lu res: %e\n", iter, res); if(res < absTol && iter >= minIter) { // printf("xi: %e eta: %e\n", xi, eta); break; } K(0,0) = -u.dotProduct(u)-c1; K(0,1) = u.dotProduct(v); K(1,0) = u.dotProduct(v); K(1,1) = -v.dotProduct(v)-c2; if(lockXi) { K(0,0) = -1.0; K(0,1) = K(1,0) = 0.0; } if(lockEta) { K(0,1) = K(1,0) = 0.0; K(1,1) = -1.0; } KInv.beInverseOf(K); dXi.beProductOf(KInv, R); xi += dXi[0]; eta += dXi[1]; d = iP1; d.add(xi,u); d.add(-1.0, iQ1); d.add(-eta, v); } if(xi < 0.0) { xi = 0.0; } if(xi > 1.0) { xi = 1.0; } if(eta < 0.0) { eta = 0.0; } if(eta > 1.0) { eta = 1.0; } d = iP1; d.add(xi,u); d.add(-1.0, iQ1); d.add(-eta, v); const double dist = d.computeNorm(); return dist; }