diff --git a/drmsimulationmodel.cpp b/drmsimulationmodel.cpp index 64712da..ae36bd8 100755 --- a/drmsimulationmodel.cpp +++ b/drmsimulationmodel.cpp @@ -1131,6 +1131,260 @@ void DRMSimulationModel::updateNodalExternalForces( pMesh->totalExternalForcesNorm = totalExternalForcesNorm; } +std::vector> DRMSimulationModel::computeInternalForces( + const std::unordered_map> &fixedVertices) +{ + std::vector, 4>> + internalForcesContributionsFromEachEdge_axial(pMesh->EN(), + std::array, 4>{ + std::pair(-1, Vector6d())}); + std::vector, 4>> + internalForcesContributionsFromEachEdge_torsion(pMesh->EN(), + std::array, 4>{ + std::pair(-1, + Vector6d())}); + std::vector, 4>> + internalForcesContributionsFromEachEdge_firstBending( + pMesh->EN(), + std::array, 4>{std::pair(-1, Vector6d())}); + std::vector, 4>> + internalForcesContributionsFromEachEdge_secondBending( + pMesh->EN(), + std::array, 4>{std::pair(-1, Vector6d())}); + // omp_lock_t writelock; + // omp_init_lock(&writelock); + //#ifdef ENABLE_OPENMP + //#pragma omp parallel for schedule(static) num_threads(5) + //#endif + std::for_each( +#ifdef ENABLE_PARALLEL_DRM + std::execution::par_unseq, +#endif + pMesh->edge.begin(), + pMesh->edge.end(), + [&](const EdgeType &e) + // for (int ei = 0; ei < pMesh->EN(); ei++) + { + const int ei = pMesh->getIndex(e); + // const EdgeType &e = pMesh->edge[ei]; + const SimulationMesh::VertexType &ev_j = *e.cV(0); + const SimulationMesh::VertexType &ev_jplus1 = *e.cV(1); + const Element &element = pMesh->elements[e]; + const Element::LocalFrame &ef = element.frame; + const VectorType t3CrossN_j = Cross(ef.t3, ev_j.cN()); + const VectorType t3CrossN_jplus1 = Cross(ef.t3, ev_jplus1.cN()); + const double theta1_j = ef.t1.dot(t3CrossN_j); + const double theta1_jplus1 = ef.t1.dot(t3CrossN_jplus1); + const double theta2_j = ef.t2.dot(t3CrossN_j); + const double theta2_jplus1 = ef.t2.dot(t3CrossN_jplus1); + const double theta3_j = computeTheta3(e, ev_j); + const double theta3_jplus1 = computeTheta3(e, ev_jplus1); + // element.rotationalDisplacements_j.theta1 = theta1_j; + // element.rotationalDisplacements_jplus1.theta1 = theta1_jplus1; + // element.rotationalDisplacements_j.theta2 = theta2_j; + // element.rotationalDisplacements_jplus1.theta2 = theta2_jplus1; + // element.rotationalDisplacements_j.theta3 = theta3_j; + // element.rotationalDisplacements_jplus1.theta3 = theta3_jplus1; + // std::array, 4> internalForcesContributionFromThisEdge{ + // std::pair(-1, Vector6d())}; + std::array, 4> internalForcesContributionFromThisEdge_axial{ + std::pair(-1, Vector6d())}; + std::array, 4> internalForcesContributionFromThisEdge_torsion{ + std::pair(-1, Vector6d())}; + std::array, 4> + internalForcesContributionFromThisEdge_firstBending{ + std::pair(-1, Vector6d())}; + std::array, 4> + internalForcesContributionFromThisEdge_secondBending{ + std::pair(-1, Vector6d())}; + for (VertexIndex evi = 0; evi < 2; evi++) { + const SimulationMesh::VertexType &ev = *e.cV(evi); + const Node &edgeNode = pMesh->nodes[ev]; + internalForcesContributionFromThisEdge_axial[evi].first = edgeNode.vi; + + const VertexPointer &rev_j = edgeNode.referenceElement->cV(0); + const VertexPointer &rev_jplus1 = edgeNode.referenceElement->cV(1); + // refElemOtherVertex can be precomputed + const VertexPointer &refElemOtherVertex = rev_j == &ev ? rev_jplus1 : rev_j; + const Node &refElemOtherVertexNode = pMesh->nodes[refElemOtherVertex]; + if (edgeNode.referenceElement != &e) { + internalForcesContributionFromThisEdge_axial[evi + 2].first + = refElemOtherVertexNode.vi; + } + const size_t vi = edgeNode.vi; + // #pragma omp parallel for schedule(static) num_threads(6) + for (DoFType dofi = DoF::Ux; dofi < DoF::NumDoF; dofi++) { + const bool isDofConstrainedFor_ev = isVertexConstrained[edgeNode.vi] + && fixedVertices.at(edgeNode.vi) + .contains(dofi); + if (!isDofConstrainedFor_ev) { + DifferentiateWithRespectTo dui{ev, dofi}; + // Axial force computation + const double e_k = element.length - element.initialLength; + const double e_kDeriv = computeDerivativeElementLength(e, dui); + const double axialForce_dofi = e_kDeriv * e_k * element.rigidity.axial; + +#ifdef POLYSCOPE_DEFINED + if (std::isnan(axialForce_dofi)) { + std::cerr << "nan in axial" << evi << std::endl; + } +#endif + + // Torsional force computation + const double theta1_j_deriv = computeDerivativeTheta1(e, 0, evi, dofi); + const double theta1_jplus1_deriv = computeDerivativeTheta1(e, 1, evi, dofi); + const double theta1Diff = theta1_jplus1 - theta1_j; + const double theta1DiffDerivative = theta1_jplus1_deriv - theta1_j_deriv; + const double torsionalForce_dofi = theta1DiffDerivative * theta1Diff + * element.rigidity.torsional; +#ifdef POLYSCOPE_DEFINED + if (std::isnan(torsionalForce_dofi)) { + std::cerr << "nan in torsional" << evi << std::endl; + } +#endif + + // First bending force computation + ////theta2_j derivative + const double theta2_j_deriv = computeDerivativeTheta2(e, 0, evi, dofi); + ////theta2_jplus1 derivative + const double theta2_jplus1_deriv = computeDerivativeTheta2(e, 1, evi, dofi); + ////1st in bracket term + const double firstBendingForce_inBracketsTerm_0 = theta2_j_deriv * 2 + * theta2_j; + ////2nd in bracket term + const double firstBendingForce_inBracketsTerm_1 = theta2_jplus1_deriv + * theta2_j; + ////3rd in bracket term + const double firstBendingForce_inBracketsTerm_2 = theta2_j_deriv + * theta2_jplus1; + ////4th in bracket term + const double firstBendingForce_inBracketsTerm_3 = 2 * theta2_jplus1_deriv + * theta2_jplus1; + // 3rd term computation + const double firstBendingForce_inBracketsTerm + = firstBendingForce_inBracketsTerm_0 + + firstBendingForce_inBracketsTerm_1 + + firstBendingForce_inBracketsTerm_2 + + firstBendingForce_inBracketsTerm_3; + const double firstBendingForce_dofi = firstBendingForce_inBracketsTerm + * element.rigidity.firstBending; + + // Second bending force computation + ////theta2_j derivative + const double theta3_j_deriv = computeDerivativeTheta3(e, ev_j, dui); + ////theta2_jplus1 derivative + const double theta3_jplus1_deriv = computeDerivativeTheta3(e, + ev_jplus1, + dui); + ////1st in bracket term + const double secondBendingForce_inBracketsTerm_0 = theta3_j_deriv * 2 + * theta3_j; + ////2nd in bracket term + const double secondBendingForce_inBracketsTerm_1 = theta3_jplus1_deriv + * theta3_j; + ////3rd in bracket term + const double secondBendingForce_inBracketsTerm_2 = theta3_j_deriv + * theta3_jplus1; + ////4th in bracket term + const double secondBendingForce_inBracketsTerm_3 = theta3_jplus1_deriv * 2 + * theta3_jplus1; + // 3rd term computation + const double secondBendingForce_inBracketsTerm + = secondBendingForce_inBracketsTerm_0 + + secondBendingForce_inBracketsTerm_1 + + secondBendingForce_inBracketsTerm_2 + + secondBendingForce_inBracketsTerm_3; + double secondBendingForce_dofi = secondBendingForce_inBracketsTerm + * element.rigidity.secondBending; + internalForcesContributionFromThisEdge_axial[evi].second[dofi] + = axialForce_dofi; + internalForcesContributionFromThisEdge_torsion[evi].second[dofi] + = torsionalForce_dofi; + internalForcesContributionFromThisEdge_firstBending[evi].second[dofi] + = firstBendingForce_dofi; + internalForcesContributionFromThisEdge_secondBending[evi].second[dofi] + = secondBendingForce_dofi; + } + if (edgeNode.referenceElement != &e) { + const bool isDofConstrainedFor_refElemOtherVertex + = isVertexConstrained[refElemOtherVertexNode.vi] + && fixedVertices.at(refElemOtherVertexNode.vi).contains(dofi); + if (!isDofConstrainedFor_refElemOtherVertex) { + DifferentiateWithRespectTo dui{*refElemOtherVertex, dofi}; + ////theta3_j derivative + const double theta3_j_deriv = computeDerivativeTheta3(e, ev_j, dui); + ////theta3_jplus1 derivative + const double theta3_jplus1_deriv = computeDerivativeTheta3(e, + ev_jplus1, + dui); + ////1st in bracket term + const double secondBendingForce_inBracketsTerm_0 = theta3_j_deriv * 2 + * theta3_j; + ////2nd in bracket term + const double secondBendingForce_inBracketsTerm_1 = theta3_jplus1_deriv + * theta3_j; + ////3rd in bracket term + const double secondBendingForce_inBracketsTerm_2 = theta3_j_deriv + * theta3_jplus1; + ////4th in bracket term + const double secondBendingForce_inBracketsTerm_3 = theta3_jplus1_deriv + * 2 * theta3_jplus1; + + // 4th term computation + const double secondBendingForce_inBracketsTerm + = secondBendingForce_inBracketsTerm_0 + + secondBendingForce_inBracketsTerm_1 + + secondBendingForce_inBracketsTerm_2 + + secondBendingForce_inBracketsTerm_3; + const double secondBendingForce_dofi = secondBendingForce_inBracketsTerm + * element.rigidity.secondBending; + internalForcesContributionFromThisEdge_secondBending[evi + 2].second[dofi] + = secondBendingForce_dofi; + } + } + } + } + internalForcesContributionsFromEachEdge_axial[ei] + = internalForcesContributionFromThisEdge_axial; + internalForcesContributionsFromEachEdge_torsion[ei] + = internalForcesContributionFromThisEdge_torsion; + internalForcesContributionsFromEachEdge_firstBending[ei] + = internalForcesContributionFromThisEdge_firstBending; + internalForcesContributionsFromEachEdge_secondBending[ei] + = internalForcesContributionFromThisEdge_secondBending; + }); + + //#pragma omp parallel for schedule(static) num_threads(8) + + std::vector> perVertexInternalForces(pMesh->VN(), {0}); + for (size_t ei = 0; ei < pMesh->EN(); ei++) { + for (int i = 0; i < 4; i++) { + std::pair internalForcePair_axial + = internalForcesContributionsFromEachEdge_axial[ei][i]; + std::pair internalForcePair_torsion + = internalForcesContributionsFromEachEdge_torsion[ei][i]; + std::pair internalForcePair_firstBending + = internalForcesContributionsFromEachEdge_firstBending[ei][i]; + std::pair internalForcePair_secondBending + = internalForcesContributionsFromEachEdge_secondBending[ei][i]; + int vi = internalForcePair_axial.first; + if (i > 1 && vi == -1) { + continue; + } + perVertexInternalForces[vi][0] = perVertexInternalForces[vi][0] + + internalForcePair_axial.second; + perVertexInternalForces[vi][1] = perVertexInternalForces[vi][1] + + internalForcePair_torsion.second; + perVertexInternalForces[vi][2] = perVertexInternalForces[vi][2] + + internalForcePair_firstBending.second; + perVertexInternalForces[vi][3] = perVertexInternalForces[vi][3] + + internalForcePair_secondBending.second; + } + } + + return perVertexInternalForces; +} + void DRMSimulationModel::updateResidualForces() { pMesh->totalResidualForcesNorm = 0; @@ -1762,6 +2016,49 @@ SimulationResults DRMSimulationModel::computeResults(const std::shared_ptrconstrainedVertices); + std::vector perVertexInternalForces_axial = [=]() { + std::vector axialInternalForces; + axialInternalForces.reserve(results.perVertexInternalForces.size()); + for (const std::array &internalForces : results.perVertexInternalForces) { + axialInternalForces.push_back(internalForces[0].norm()); + } + return axialInternalForces; + }(); + const auto pCurvNet = pMesh->registerForDrawing(); + pCurvNet->addNodeScalarQuantity("axial forces", perVertexInternalForces_axial); + std::vector perVertexInternalForces_torsion = [=]() { + std::vector axialInternalForces; + axialInternalForces.reserve(results.perVertexInternalForces.size()); + for (const std::array &internalForces : results.perVertexInternalForces) { + axialInternalForces.push_back(internalForces[1].norm()); + } + return axialInternalForces; + }(); + pCurvNet->addNodeScalarQuantity("torsional forces", perVertexInternalForces_torsion); + std::vector perVertexInternalForces_firstBending = [=]() { + std::vector axialInternalForces; + axialInternalForces.reserve(results.perVertexInternalForces.size()); + for (const std::array &internalForces : results.perVertexInternalForces) { + axialInternalForces.push_back(internalForces[2].norm()); + } + return axialInternalForces; + }(); + pCurvNet->addNodeScalarQuantity("first bending forces", + perVertexInternalForces_firstBending); + std::vector perVertexInternalForces_secondBending = [=]() { + std::vector axialInternalForces; + axialInternalForces.reserve(results.perVertexInternalForces.size()); + for (const std::array &internalForces : results.perVertexInternalForces) { + axialInternalForces.push_back(internalForces[3].norm()); + } + return axialInternalForces; + }(); + pCurvNet->addNodeScalarQuantity("second bending forces", + perVertexInternalForces_secondBending); + polyscope::show(); +#endif results.rotationalDisplacementQuaternion.resize(pMesh->VN()); results.debug_q_f1.resize(pMesh->VN()); results.debug_q_normal.resize(pMesh->VN()); @@ -2666,6 +2963,7 @@ SimulationResults DRMSimulationModel::executeSimulation(const std::shared_ptr &pJob); + std::vector> computeInternalForces( + const std::unordered_map> &fixedVertices); + public: DRMSimulationModel(); SimulationResults executeSimulation(const std::shared_ptr &pJob,