Added computation of internal forces in the converged state

drmsimulationmodel.cpp

@@ -1131,6 +1131,260 @@ void DRMSimulationModel::updateNodalExternalForces(
     pMesh->totalExternalForcesNorm = totalExternalForcesNorm;
 }
 
+std::vector<std::array<Vector6d, 4>> DRMSimulationModel::computeInternalForces(
+    const std::unordered_map<VertexIndex, std::unordered_set<DoFType>> &fixedVertices)
+{
+    std::vector<std::array<std::pair<int, Vector6d>, 4>>
+        internalForcesContributionsFromEachEdge_axial(pMesh->EN(),
+                                                      std::array<std::pair<int, Vector6d>, 4>{
+                                                          std::pair<int, Vector6d>(-1, Vector6d())});
+    std::vector<std::array<std::pair<int, Vector6d>, 4>>
+        internalForcesContributionsFromEachEdge_torsion(pMesh->EN(),
+                                                        std::array<std::pair<int, Vector6d>, 4>{
+                                                            std::pair<int, Vector6d>(-1,
+                                                                                     Vector6d())});
+    std::vector<std::array<std::pair<int, Vector6d>, 4>>
+        internalForcesContributionsFromEachEdge_firstBending(
+            pMesh->EN(),
+            std::array<std::pair<int, Vector6d>, 4>{std::pair<int, Vector6d>(-1, Vector6d())});
+    std::vector<std::array<std::pair<int, Vector6d>, 4>>
+        internalForcesContributionsFromEachEdge_secondBending(
+            pMesh->EN(),
+            std::array<std::pair<int, Vector6d>, 4>{std::pair<int, Vector6d>(-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<std::pair<int, Vector6d>, 4> internalForcesContributionFromThisEdge{
+            //                std::pair<int, Vector6d>(-1, Vector6d())};
+            std::array<std::pair<int, Vector6d>, 4> internalForcesContributionFromThisEdge_axial{
+                std::pair<int, Vector6d>(-1, Vector6d())};
+            std::array<std::pair<int, Vector6d>, 4> internalForcesContributionFromThisEdge_torsion{
+                std::pair<int, Vector6d>(-1, Vector6d())};
+            std::array<std::pair<int, Vector6d>, 4>
+                internalForcesContributionFromThisEdge_firstBending{
+                    std::pair<int, Vector6d>(-1, Vector6d())};
+            std::array<std::pair<int, Vector6d>, 4>
+                internalForcesContributionFromThisEdge_secondBending{
+                    std::pair<int, Vector6d>(-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<std::array<Vector6d, 4>> perVertexInternalForces(pMesh->VN(), {0});
+    for (size_t ei = 0; ei < pMesh->EN(); ei++) {
+        for (int i = 0; i < 4; i++) {
+            std::pair<int, Vector6d> internalForcePair_axial
+                = internalForcesContributionsFromEachEdge_axial[ei][i];
+            std::pair<int, Vector6d> internalForcePair_torsion
+                = internalForcesContributionsFromEachEdge_torsion[ei][i];
+            std::pair<int, Vector6d> internalForcePair_firstBending
+                = internalForcesContributionsFromEachEdge_firstBending[ei][i];
+            std::pair<int, Vector6d> 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_ptr<Simul
         results.debug_drmDisplacements = results.displacements;
         results.internalPotentialEnergy = computeTotalInternalPotentialEnergy();
 
+#ifdef COMPUTE_INTERNAL_FORCES
+        results.perVertexInternalForces = computeInternalForces(pJob->constrainedVertices);
+        std::vector<double> perVertexInternalForces_axial = [=]() {
+            std::vector<double> axialInternalForces;
+            axialInternalForces.reserve(results.perVertexInternalForces.size());
+            for (const std::array<Vector6d, 4> &internalForces : results.perVertexInternalForces) {
+                axialInternalForces.push_back(internalForces[0].norm());
+            }
+            return axialInternalForces;
+        }();
+        const auto pCurvNet = pMesh->registerForDrawing();
+        pCurvNet->addNodeScalarQuantity("axial forces", perVertexInternalForces_axial);
+        std::vector<double> perVertexInternalForces_torsion = [=]() {
+            std::vector<double> axialInternalForces;
+            axialInternalForces.reserve(results.perVertexInternalForces.size());
+            for (const std::array<Vector6d, 4> &internalForces : results.perVertexInternalForces) {
+                axialInternalForces.push_back(internalForces[1].norm());
+            }
+            return axialInternalForces;
+        }();
+        pCurvNet->addNodeScalarQuantity("torsional forces", perVertexInternalForces_torsion);
+        std::vector<double> perVertexInternalForces_firstBending = [=]() {
+            std::vector<double> axialInternalForces;
+            axialInternalForces.reserve(results.perVertexInternalForces.size());
+            for (const std::array<Vector6d, 4> &internalForces : results.perVertexInternalForces) {
+                axialInternalForces.push_back(internalForces[2].norm());
+            }
+            return axialInternalForces;
+        }();
+        pCurvNet->addNodeScalarQuantity("first bending forces",
+                                        perVertexInternalForces_firstBending);
+        std::vector<double> perVertexInternalForces_secondBending = [=]() {
+            std::vector<double> axialInternalForces;
+            axialInternalForces.reserve(results.perVertexInternalForces.size());
+            for (const std::array<Vector6d, 4> &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<Si
     }
 
     SimulationResults results = executeSimulation(pJob); //calling the virtual function
+
     return results;
 }
 

drmsimulationmodel.hpp

@@ -254,6 +254,9 @@ private:
 
   void reset(const std::shared_ptr<SimulationJob> &pJob);
 
+  std::vector<std::array<Vector6d, 4>> computeInternalForces(
+      const std::unordered_map<VertexIndex, std::unordered_set<DoFType>> &fixedVertices);
+
   public:
   DRMSimulationModel();
   SimulationResults executeSimulation(const std::shared_ptr<SimulationJob> &pJob,