diff --git a/CMakeLists.txt b/CMakeLists.txt index c24ff9a..e43756a 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -43,6 +43,17 @@ download_project(PROJ threed-beam-fea ) add_subdirectory(${threed-beam-fea_SOURCE_DIR} ${threed-beam-fea_BINARY_DIR}) +##TBB +download_project(PROJ TBB + GIT_REPOSITORY https://github.com/wjakob/tbb.git + GIT_TAG master + PREFIX ${EXTERNAL_DEPS_DIR} + ${UPDATE_DISCONNECTED_IF_AVAILABLE} +) +add_subdirectory(${TBB_SOURCE_DIR} ${TBB_BINARY_DIR}) +link_directories(${TBB_BINARY_DIR}) +message(${TBB_BINARY_DIR}) + ###Eigen 3 NOTE: Eigen is required on the system the code is ran find_package(Eigen3 3.3 REQUIRED) @@ -53,6 +64,7 @@ endif(MSVC) #link_directories(${CMAKE_CURRENT_LIST_DIR}/boost_graph/libs) file(GLOB MySourcesFiles ${CMAKE_CURRENT_LIST_DIR}/*.hpp ${CMAKE_CURRENT_LIST_DIR}/*.cpp) +#set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -ltbb") add_library(${PROJECT_NAME} ${MySourcesFiles} ${vcglib_devel_SOURCE_DIR}/wrap/ply/plylib.cpp) @@ -64,9 +76,8 @@ target_include_directories(${PROJECT_NAME} ) if(${USE_POLYSCOPE}) - find_package(OpenMP REQUIRED) - target_link_libraries(${PROJECT_NAME} Eigen3::Eigen matplot polyscope glad ThreedBeamFEA OpenMP::OpenMP_CXX) + target_link_libraries(${PROJECT_NAME} Eigen3::Eigen matplot polyscope glad ThreedBeamFEA ${TBB_BINARY_DIR}/libtbb_static.a pthread) else() - target_link_libraries(${PROJECT_NAME} -static Eigen3::Eigen matplot ThreedBeamFEA) + target_link_libraries(${PROJECT_NAME} -static Eigen3::Eigen matplot ThreedBeamFEA ${TBB_BINARY_DIR}/libtbb_static.a pthread) endif() target_link_directories(MySources PUBLIC ${CMAKE_CURRENT_LIST_DIR}/boost_graph/libs) diff --git a/drmsimulationmodel.cpp b/drmsimulationmodel.cpp index d36eeff..477ebaa 100755 --- a/drmsimulationmodel.cpp +++ b/drmsimulationmodel.cpp @@ -222,7 +222,7 @@ void DRMSimulationModel::reset() mCurrentSimulationStep = 0; history.clear(); constrainedVertices.clear(); - rigidSupports.clear(); + isRigidSupport.clear(); pMesh.reset(); plotYValues.clear(); plotHandle.reset(); @@ -807,136 +807,113 @@ void DRMSimulationModel::updateResidualForcesOnTheFly( // omp_lock_t writelock; // omp_init_lock(&writelock); //#ifdef ENABLE_OPENMP - //#pragma omp parallel for schedule(static) num_threads(8) + //#pragma omp parallel for schedule(static) num_threads(5) //#endif - for (int ei = 0; ei < pMesh->EN(); ei++) { - 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 bool shouldBreak = mCurrentSimulationStep == 12970; - 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::vector> internalForcesContributionFromThisEdge(4, - {-1, - Vector6d()}); - for (VertexIndex evi = 0; evi < 2; evi++) { - const SimulationMesh::VertexType &ev = *e.cV(evi); - const Node &edgeNode = pMesh->nodes[ev]; - internalForcesContributionFromThisEdge[evi].first = edgeNode.vi; + std::for_each( + std::execution::par_unseq, + 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 bool shouldBreak = mCurrentSimulationStep == 12970; + 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::vector> + internalForcesContributionFromThisEdge(4, {-1, Vector6d()}); + for (VertexIndex evi = 0; evi < 2; evi++) { + const SimulationMesh::VertexType &ev = *e.cV(evi); + const Node &edgeNode = pMesh->nodes[ev]; + internalForcesContributionFromThisEdge[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[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 = 2 * theta3_jplus1_deriv - * 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[evi].second[dofi] - = axialForce_dofi + firstBendingForce_dofi + secondBendingForce_dofi - + torsionalForce_dofi; - } + 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) { - const bool isDofConstrainedFor_refElemOtherVertex - = isVertexConstrained[refElemOtherVertexNode.vi] - && fixedVertices.at(refElemOtherVertexNode.vi).contains(dofi); - if (!isDofConstrainedFor_refElemOtherVertex) { - DifferentiateWithRespectTo dui{*refElemOtherVertex, dofi}; - ////theta3_j derivative + internalForcesContributionFromThisEdge[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); - ////theta3_jplus1 derivative + ////theta2_jplus1 derivative const double theta3_jplus1_deriv = computeDerivativeTheta3(e, ev_jplus1, dui); @@ -950,25 +927,61 @@ void DRMSimulationModel::updateResidualForcesOnTheFly( const double secondBendingForce_inBracketsTerm_2 = theta3_j_deriv * theta3_jplus1; ////4th in bracket term - const double secondBendingForce_inBracketsTerm_3 = theta3_jplus1_deriv * 2 + const double secondBendingForce_inBracketsTerm_3 = 2 * theta3_jplus1_deriv * theta3_jplus1; - - // 4th term computation + // 3rd 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[evi + 2].second[dofi] - = secondBendingForce_dofi; + double secondBendingForce_dofi = secondBendingForce_inBracketsTerm + * element.rigidity.secondBending; + internalForcesContributionFromThisEdge[evi].second[dofi] + = axialForce_dofi + firstBendingForce_dofi + secondBendingForce_dofi + + torsionalForce_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[evi + 2].second[dofi] + = secondBendingForce_dofi; + } } } } - } - internalForcesContributionsFromEachEdge[ei] = internalForcesContributionFromThisEdge; - } + internalForcesContributionsFromEachEdge[ei] = internalForcesContributionFromThisEdge; + }); //#pragma omp parallel for schedule(static) num_threads(8) @@ -1251,7 +1264,7 @@ void DRMSimulationModel::updateNodalMasses() pMesh->nodes[v].damping_6d[DoF::Nr] = 2 * std::sqrt(rotationalSumSk_I2 * pMesh->nodes[v].mass_6d[DoF::Nr]); - pMesh->nodes[v].damping_6d = pMesh->nodes[v].damping_6d * 1e-2; + pMesh->nodes[v].damping_6d = pMesh->nodes[v].damping_6d * 1e-3; } assert(std::pow(mSettings.Dtini, 2.0) * translationalSumSk / pMesh->nodes[v].mass.translational @@ -1296,8 +1309,8 @@ void DRMSimulationModel::updateNodalVelocities() Node &node = pMesh->nodes[v]; if (mSettings.useViscousDamping) { const Vector6d massOverDt = node.mass_6d / Dt; - const Vector6d visciousDampingFactor(viscuousDampingConstant / 2); - // const Vector6d &visciousDampingFactor = node.damping_6d; + // const Vector6d visciousDampingFactor(viscuousDampingConstant / 2); + const Vector6d &visciousDampingFactor = node.damping_6d; const Vector6d denominator = massOverDt + visciousDampingFactor / 2; const Vector6d firstTermNominator = massOverDt - visciousDampingFactor / 2; const Vector6d firstTerm = node.velocity * firstTermNominator / denominator; @@ -2609,3 +2622,19 @@ mesh->currentTotalPotentialEnergykN*/ return results; } + +bool DRMSimulationModel::Settings::save(const filesystem::__cxx11::path &folderPath) const +{ + bool returnValue = true; + std::filesystem::create_directories(folderPath); + nlohmann::json json; + json[jsonLabels.meshFilename] + = std::filesystem::relative(std::filesystem::path(meshFilename), + std::filesystem::path(jsonFilename).parent_path()) + .string(); + const std::string jsonFilename = "drmSettings.json"; + std::ofstream jsonFile(std::filesystem::path(folderPath).append(jsonFilename)); + jsonFile << json; +} + +bool DRMSimulationModel::Settings::load(const filesystem::__cxx11::path &filePath) const {} diff --git a/drmsimulationmodel.cpp.orig b/drmsimulationmodel.cpp.orig new file mode 100755 index 0000000..cba92b1 --- /dev/null +++ b/drmsimulationmodel.cpp.orig @@ -0,0 +1,2671 @@ +#include "drmsimulationmodel.hpp" +#include "simulationhistoryplotter.hpp" +#include +#include +#include +#include + +#ifdef ENABLE_OPENMP +#include +#endif + +void DRMSimulationModel::runUnitTests() +{ + const std::filesystem::path groundOfTruthFolder{ + "/home/iason/Coding/Libraries/MySources/formFinder_unitTestFiles"}; + + DRMSimulationModel formFinder; + + // First example of the paper + VCGEdgeMesh beam; + // const size_t spanGridSize = 11; + // mesh.createSpanGrid(spanGridSize); + beam.load(std::filesystem::path(groundOfTruthFolder).append("simpleBeam.ply").string()); + std::unordered_map> fixedVertices; + fixedVertices[0] = std::unordered_set{0, 1, 2}; + fixedVertices[beam.VN() - 1] = std::unordered_set{1, 2}; + std::unordered_map nodalForces{ + {beam.VN() / 2, Vector6d({0, 1000, 1000, 0, 0, 0})}}; + // Forced displacements + std::unordered_map nodalForcedDisplacements; + nodalForcedDisplacements.insert({beam.VN() - 1, Eigen::Vector3d(-0.2, 0, 0)}); + + SimulationJob beamSimulationJob{std::make_shared(beam), + "First paper example", + // SimulationJob::constructFixedVerticesSpanGrid(spanGridSize, + // mesh.VN()), + fixedVertices, + nodalForces, + nodalForcedDisplacements}; + beamSimulationJob.pMesh->setBeamMaterial(0.3, 200 * 1e9); + assert(CrossSectionType::name == CylindricalBeamDimensions::name); + + beamSimulationJob.pMesh->setBeamCrossSection(CrossSectionType{0.03, 0.026}); + Settings settings; + settings.Dtini = 0.1; + settings.xi = 0.9969; + settings.maxDRMIterations = 0; + formFinder.mSettings.totalResidualForcesNormThreshold = 1; + settings.shouldDraw = false; + settings.beVerbose = true; + settings.shouldCreatePlots = true; + SimulationResults simpleBeam_simulationResults + = formFinder.executeSimulation(std::make_shared(beamSimulationJob), settings); + simpleBeam_simulationResults.save(); + const std::string simpleBeamGroundOfTruthBinaryFilename + = std::filesystem::path(groundOfTruthFolder) + .append("SimpleBeam_displacements.eigenBin") + .string(); + assert(std::filesystem::exists(std::filesystem::path(simpleBeamGroundOfTruthBinaryFilename))); + Eigen::MatrixXd simpleBeam_groundOfTruthDisplacements; + Eigen::read_binary(simpleBeamGroundOfTruthBinaryFilename, simpleBeam_groundOfTruthDisplacements); + + double error; + if (!simpleBeam_simulationResults.isEqual(simpleBeam_groundOfTruthDisplacements, error)) { + std::cerr << "Error:Beam simulation produces wrong results. Error:" << error << std::endl; + // return; + } +#ifdef POLYSCOPE_DEFINED + beam.registerForDrawing(); + simpleBeam_simulationResults.registerForDrawing(); + polyscope::show(); + beam.unregister(); + simpleBeam_simulationResults.unregister(); +#endif + + // Second example of the paper + VCGEdgeMesh shortSpanGrid; + // const size_t spanGridSize = 11; + // mesh.createSpanGrid(spanGridSize); + shortSpanGrid.load( + std::filesystem::path(groundOfTruthFolder).append("shortSpanGridshell.ply").string()); + + fixedVertices.clear(); + //// Corner nodes + fixedVertices[0] = std::unordered_set{2}; + fixedVertices[4] = std::unordered_set{2}; + fixedVertices[16] = std::unordered_set{2}; + fixedVertices[20] = std::unordered_set{2}; + //// Center node + fixedVertices[10] = std::unordered_set{0, 1, 3, 4, 5}; + + nodalForcedDisplacements.clear(); + nodalForcedDisplacements.insert({{0, Eigen::Vector3d(0.1, 0.1, 0)}, + {4, Eigen::Vector3d(-0.1, 0.1, 0)}, + {16, Eigen::Vector3d(0.1, -0.1, 0)}, + {20, Eigen::Vector3d(-0.1, -0.1, 0)}}); + + SimulationJob shortSpanGridshellSimulationJob{std::make_shared(shortSpanGrid), + "Short span gridshell", + fixedVertices, + {}, + nodalForcedDisplacements}; + shortSpanGridshellSimulationJob.pMesh->setBeamMaterial(0.3, 200 * 1e9); + assert(typeid(CrossSectionType) == typeid(CylindricalBeamDimensions)); + shortSpanGridshellSimulationJob.pMesh->setBeamCrossSection(CrossSectionType{0.03, 0.026}); + SimulationResults shortSpanGridshellSimulationResults + = formFinder.executeSimulation(std::make_shared( + shortSpanGridshellSimulationJob), + settings); + shortSpanGridshellSimulationResults.save(); + + const std::string groundOfTruthBinaryFilename + = std::filesystem::path(groundOfTruthFolder) + .append("ShortSpanGridshell_displacements.eigenBin") + .string(); + assert(std::filesystem::exists(std::filesystem::path(groundOfTruthBinaryFilename))); + Eigen::MatrixXd shortSpanGridshell_groundOfTruthDisplacements; + Eigen::read_binary(groundOfTruthBinaryFilename, shortSpanGridshell_groundOfTruthDisplacements); + // shortSpanGridshellSimulationResults.registerForDrawing( + // shortSpanGridshellSimulationJob); + // polyscope::show(); + assert(shortSpanGridshellSimulationResults.isEqual(shortSpanGridshell_groundOfTruthDisplacements, + error)); + if (!shortSpanGridshellSimulationResults.isEqual(shortSpanGridshell_groundOfTruthDisplacements, + error)) { + std::cerr << "Error:Short span simulation produces wrong results. Error:" << error + << std::endl; + // return; + } +#ifdef POLYSCOPE_DEFINED + shortSpanGrid.registerForDrawing(); + shortSpanGridshellSimulationResults.registerForDrawing(); + polyscope::show(); + shortSpanGrid.unregister(); + shortSpanGridshellSimulationResults.unregister(); +#endif + + // Third example of the paper + VCGEdgeMesh longSpanGrid; + longSpanGrid.load( + std::filesystem::path(groundOfTruthFolder).append("longSpanGridshell.ply").string()); + const size_t spanGridSize = 11; + + fixedVertices.clear(); + for (size_t vi = 0; vi < spanGridSize - 1; vi++) { + fixedVertices[vi] = {0, 2}; + } + for (size_t vi = longSpanGrid.VN() - 1 - (spanGridSize - 2); vi < longSpanGrid.VN(); vi++) { + fixedVertices[vi] = {0, 2}; + } + for (size_t vi = spanGridSize - 1; + vi < longSpanGrid.VN() - 1 - (spanGridSize - 2) - spanGridSize + 1; + vi += spanGridSize + 1) { + fixedVertices[vi] = {1, 2}; + fixedVertices[vi + spanGridSize] = {1, 2}; + } + + nodalForcedDisplacements.clear(); + const size_t horizontalOffset = std::floor((spanGridSize - 2) / 2); + nodalForcedDisplacements.insert( + {{horizontalOffset, Eigen::Vector3d(0, 0.3, 0)}, + {horizontalOffset + 1, Eigen::Vector3d(0, 0.3, 0)}, + {spanGridSize * (spanGridSize + 1) - 2 + horizontalOffset, Eigen::Vector3d(0, -0.3, 0)}, + {spanGridSize * (spanGridSize + 1) - 2 + horizontalOffset + 1, Eigen::Vector3d(0, -0.3, 0)}, + {std::floor(spanGridSize / 2) * (spanGridSize + 1) - 2, Eigen::Vector3d(0.3, 0, 0)}, + {(std::floor(spanGridSize / 2) + 1) * (spanGridSize + 1) - 2, Eigen::Vector3d(0.3, 0, 0)}, + {std::floor(spanGridSize / 2) * (spanGridSize + 1) - 2 + spanGridSize, + Eigen::Vector3d(-0.3, 0, 0)}, + {(std::floor(spanGridSize / 2) + 1) * (spanGridSize + 1) - 2 + spanGridSize, + Eigen::Vector3d(-0.3, 0, 0)}}); + + SimulationJob longSpanGridshell_simulationJob{std::make_shared(longSpanGrid), + "long span gridshell", + fixedVertices, + {}, + nodalForcedDisplacements}; + longSpanGridshell_simulationJob.pMesh->setBeamMaterial(0.3, 200 * 1e9); + if (typeid(CrossSectionType) != typeid(CylindricalBeamDimensions)) { + std::cerr << "A cylindrical cross section is expected for running the " + "paper examples." + << std::endl; + } + longSpanGridshell_simulationJob.pMesh->setBeamCrossSection(CrossSectionType{0.03, 0.026}); + + SimulationResults longSpanGridshell_simulationResults + = formFinder.executeSimulation(std::make_shared( + longSpanGridshell_simulationJob), + settings); + longSpanGridshell_simulationResults.save(); + + const std::string longSpan_groundOfTruthBinaryFilename + = std::filesystem::path(groundOfTruthFolder) + .append("LongSpanGridshell_displacements.eigenBin") + .string(); + assert(std::filesystem::exists(std::filesystem::path(longSpan_groundOfTruthBinaryFilename))); + Eigen::MatrixXd longSpanGridshell_groundOfTruthDisplacements; + Eigen::read_binary(longSpan_groundOfTruthBinaryFilename, + longSpanGridshell_groundOfTruthDisplacements); + assert(longSpanGridshell_simulationResults.isEqual(longSpanGridshell_groundOfTruthDisplacements, + error)); + if (!longSpanGridshell_simulationResults.isEqual(longSpanGridshell_groundOfTruthDisplacements, + error)) { + std::cerr << "Error:Long span simulation produces wrong results. Error:" << error + << std::endl; + // return; + } +#ifdef POLYSCOPE_DEFINED + longSpanGrid.registerForDrawing(); + longSpanGridshell_simulationResults.registerForDrawing(); + polyscope::show(); + longSpanGrid.unregister(); + longSpanGridshell_simulationResults.unregister(); +#endif + + std::cout << "Form finder unit tests succesufully passed." << std::endl; + + // polyscope::show(); +} + +void DRMSimulationModel::reset() +{ + mCurrentSimulationStep = 0; + history.clear(); + constrainedVertices.clear(); + pMesh.reset(); + plotYValues.clear(); + plotHandle.reset(); + checkedForMaximumMoment = false; + externalMomentsNorm = 0; + Dt = mSettings.Dtini; + numOfDampings = 0; + shouldTemporarilyDampForces = false; + externalLoadStep = 1; + isVertexConstrained.clear(); + minTotalResidualForcesNorm = std::numeric_limits::max(); +} + +VectorType DRMSimulationModel::computeDisplacementDifferenceDerivative( + const EdgeType &e, const DifferentiateWithRespectTo &dui) const +{ + VectorType displacementDiffDeriv(0, 0, 0); + const DoFType &dofi = dui.dofi; + const bool differentiateWithRespectToANonEdgeNode = e.cV(0) != &dui.v && e.cV(1) != &dui.v; + if (differentiateWithRespectToANonEdgeNode || dofi > 2) { + return displacementDiffDeriv; + } + + if (e.cV(0) == &dui.v) { + displacementDiffDeriv[dofi] = -1; + } else if (e.cV(1) == &dui.v) { + displacementDiffDeriv[dofi] = 1; + } + + return displacementDiffDeriv; +} + +VectorType DRMSimulationModel::computeDerivativeOfNormal(const VertexType &v, + const DifferentiateWithRespectTo &dui) const +{ + const size_t vi = pMesh->getIndex(v); + VectorType normalDerivative(0, 0, 0); + if (&dui.v != &v || (dui.dofi == 0 || dui.dofi == 1 || dui.dofi == 2 || dui.dofi == 5)) { + return normalDerivative; + } + const VectorType &n = v.cN(); + const double &nx = n[0], ny = n[1]; + const double nxnyMagnitude = std::pow(nx, 2) + std::pow(ny, 2); + + if (dui.dofi == 3) { + if (nxnyMagnitude + 1e-5 >= 1) { + const double normalDerivativeX = 1 / sqrt(nxnyMagnitude) + - std::pow(nx, 2) / std::pow(nxnyMagnitude, 1.5); + const double normalDerivativeY = -nx * ny / std::pow(nxnyMagnitude, 1.5); + const double normalDerivativeZ = 0; + normalDerivative = VectorType(normalDerivativeX, normalDerivativeY, normalDerivativeZ); + } else { + const double normalDerivativeX = 1; + const double normalDerivativeY = 0; + const double normalDerivativeZ = -nx / std::sqrt(1 - nxnyMagnitude); + normalDerivative = VectorType(normalDerivativeX, normalDerivativeY, normalDerivativeZ); + } + } else if (dui.dofi == 4) { + if (nxnyMagnitude + 1e-5 >= 1) { + const double normalDerivativeX = -nx * ny / std::pow(nxnyMagnitude, 1.5); + const double normalDerivativeY = 1 / sqrt(nxnyMagnitude) + - std::pow(ny, 2) / std::pow(nxnyMagnitude, 1.5); + const double normalDerivativeZ = 0; + normalDerivative = VectorType(normalDerivativeX, normalDerivativeY, normalDerivativeZ); + } else { + const double normalDerivativeX = 0; + const double normalDerivativeY = 1; + const double normalDerivativeZ = -ny / std::sqrt(1 - nxnyMagnitude); + normalDerivative = VectorType(normalDerivativeX, normalDerivativeY, normalDerivativeZ); + } + } + + const bool normalDerivativeIsFinite = std::isfinite(normalDerivative[0]) + && std::isfinite(normalDerivative[1]) + && std::isfinite(normalDerivative[2]); + assert(normalDerivativeIsFinite); + const bool shouldBreak = mCurrentSimulationStep == 118 && vi == 1 && dui.dofi == 3; + + return normalDerivative; +} + +double DRMSimulationModel::computeDerivativeElementLength( + const EdgeType &e, const DifferentiateWithRespectTo &dui) const +{ + if (e.cV(0) != &dui.v && e.cV(1) != &dui.v) { + return 0; + } + + const VectorType &X_j = e.cP(0); + const VectorType &X_jplus1 = e.cP(1); + const VectorType positionVectorDiff = X_jplus1 - X_j; + const VectorType displacementDiffDeriv = computeDisplacementDifferenceDerivative(e, dui); + const double edgeLength = pMesh->elements[e].length; + const double L_kDeriv = positionVectorDiff * displacementDiffDeriv / edgeLength; + return L_kDeriv; +} + +double DRMSimulationModel::computeDerivativeOfNorm(const VectorType &x, + const VectorType &derivativeOfX) +{ + return x.dot(derivativeOfX) / x.Norm(); +} + +VectorType DRMSimulationModel::computeDerivativeOfCrossProduct(const VectorType &a, + const VectorType &derivativeOfA, + const VectorType &b, + const VectorType &derivativeOfB) +{ + const auto firstTerm = Cross(derivativeOfA, b); + const auto secondTerm = Cross(a, derivativeOfB); + return firstTerm + secondTerm; +} + +VectorType DRMSimulationModel::computeDerivativeOfR(const EdgeType &e, + const DifferentiateWithRespectTo &dui) const +{ + const VertexType &v_j = *e.cV(0); + const VertexType &v_jplus1 = *e.cV(1); + const VectorType normal_j = v_j.cN(); + const VectorType normal_jplus1 = v_jplus1.cN(); + const VectorType derivativeOfNormal_j = &v_j == &dui.v && dui.dofi > 2 + ? pMesh->nodes[v_j].derivativeOfNormal[dui.dofi] + : VectorType(0, 0, 0); + const VectorType derivativeOfNormal_jplus1 + = &v_jplus1 == &dui.v && dui.dofi > 2 ? pMesh->nodes[v_jplus1].derivativeOfNormal[dui.dofi] + : VectorType(0, 0, 0); + + const VectorType derivativeOfSumOfNormals = derivativeOfNormal_j + derivativeOfNormal_jplus1; + const VectorType sumOfNormals = normal_j + normal_jplus1; + const double normOfSumOfNormals = sumOfNormals.Norm(); + assert(normOfSumOfNormals != 0); + const double derivativeOfNormOfSumOfNormals = computeDerivativeOfNorm(sumOfNormals, + derivativeOfSumOfNormals); + + const VectorType derivativeOfR_firstTerm = -sumOfNormals * derivativeOfNormOfSumOfNormals + / std::pow(normOfSumOfNormals, 2); + const VectorType derivativeOfR_secondTerm = derivativeOfSumOfNormals / normOfSumOfNormals; + const VectorType derivativeOfR = derivativeOfR_firstTerm + derivativeOfR_secondTerm; + + assert(std::isfinite(derivativeOfR[0]) && std::isfinite(derivativeOfR[1]) + && std::isfinite(derivativeOfR[2])); + + return derivativeOfR; +} + +VectorType DRMSimulationModel::computeDerivativeT1(const EdgeType &e, + const DifferentiateWithRespectTo &dui) const +{ + const VectorType &X_j = e.cP(0); + const VectorType &X_jplus1 = e.cP(1); + const VectorType edgeVector = X_jplus1 - X_j; + const double L_kDerivative = computeDerivativeElementLength(e, dui); + const double edgeLength = pMesh->elements[e].length; + const VectorType firstTerm = -edgeVector * L_kDerivative / std::pow(edgeLength, 2); + const VectorType secondTerm = computeDisplacementDifferenceDerivative(e, dui) / edgeLength; + const VectorType t1Derivative = firstTerm + secondTerm; + + return t1Derivative; +} + +VectorType DRMSimulationModel::computeDerivativeT2(const EdgeType &e, + const DifferentiateWithRespectTo &dui) const +{ + const DoFType dofi = dui.dofi; + + const VertexType &v_j = *e.cV(0); + const size_t vi_j = pMesh->getIndex(v_j); + const VertexType &v_jplus1 = *e.cV(1); + const size_t vi_jplus1 = pMesh->getIndex(v_jplus1); + + const VectorType r = (v_j.cN() + v_jplus1.cN()).Normalize(); + const VectorType derivativeR_j = dofi > 2 && &v_j == &dui.v + ? pMesh->elements[e].derivativeR_j[dui.dofi] + : VectorType(0, 0, 0); + const VectorType derivativeR_jplus1 = dofi > 2 && &v_jplus1 == &dui.v + ? pMesh->elements[e].derivativeR_jplus1[dui.dofi] + : VectorType(0, 0, 0); + const VectorType derivativeR = derivativeR_j + derivativeR_jplus1; + + const VectorType &t1 = pMesh->elements[e].frame.t1; + const VectorType derivativeT1_j = dofi < 3 && &v_j == &dui.v + ? pMesh->elements[e].derivativeT1_j[dui.dofi] + : VectorType(0, 0, 0); + const VectorType derivativeT1_jplus1 = dofi < 3 && &v_jplus1 == &dui.v + ? pMesh->elements[e].derivativeT1_jplus1[dui.dofi] + : VectorType(0, 0, 0); + const VectorType derivativeT1 = derivativeT1_j + derivativeT1_jplus1; + + const VectorType derivativeOfRCrossT1 = computeDerivativeOfCrossProduct(r, + derivativeR, + t1, + derivativeT1); + const VectorType rCrossT1 = Cross(r, t1); + const double normOfRCrossT1 = rCrossT1.Norm(); + const double derivativeNormRCrossT1 = computeDerivativeOfNorm(rCrossT1, derivativeOfRCrossT1); + + const VectorType t2Deriv_firstTerm = -(rCrossT1 * derivativeNormRCrossT1) + / std::pow(normOfRCrossT1, 2); + const VectorType t2Deriv_secondTerm = derivativeOfRCrossT1 / normOfRCrossT1; + const VectorType t2Deriv = t2Deriv_firstTerm + t2Deriv_secondTerm; + + const double t2DerivNorm = t2Deriv.Norm(); + assert(std::isfinite(t2DerivNorm)); + const bool shouldBreak = mCurrentSimulationStep == 118 && (vi_jplus1 == 1 || vi_j == 1) + && dofi == 3; + return t2Deriv; +} + +VectorType DRMSimulationModel::computeDerivativeT3(const EdgeType &e, + const DifferentiateWithRespectTo &dui) const +{ + const Element &element = pMesh->elements[e]; + const VectorType &t1 = element.frame.t1; + const VectorType &t2 = element.frame.t2; + const VectorType t1CrossT2 = Cross(t1, t2); + const VertexType &v_j = *e.cV(0); + const size_t vi_j = pMesh->getIndex(v_j); + const VertexType &v_jplus1 = *e.cV(1); + const size_t vi_jplus1 = pMesh->getIndex(v_jplus1); + const VectorType derivativeT1_j = dui.dofi < 3 && &v_j == &dui.v + ? pMesh->elements[e].derivativeT1_j[dui.dofi] + : VectorType(0, 0, 0); + const VectorType derivativeT1_jplus1 = dui.dofi < 3 && &v_jplus1 == &dui.v + ? pMesh->elements[e].derivativeT1_jplus1[dui.dofi] + : VectorType(0, 0, 0); + const VectorType derivativeT1 = derivativeT1_j + derivativeT1_jplus1; + + // const VectorType derivativeOfT2 = computeDerivativeT2(e, dui); + // const VectorType derivativeT2_j = + // &v_j == &dui.v + // ? mesh->elements[e].derivativeT2_j[dui.dofi] + // : VectorType(0, 0, 0); + // const VectorType derivativeT2_jplus1 = + // &v_jplus1 == &dui.v + // ? mesh->elements[e].derivativeT2_jplus1[dui.dofi] + // : VectorType(0, 0, 0); + VectorType derivativeT2(0, 0, 0); + if (&v_j == &dui.v) { + derivativeT2 = pMesh->elements[e].derivativeT2_j[dui.dofi]; + } else if (&v_jplus1 == &dui.v) { + derivativeT2 = pMesh->elements[e].derivativeT2_jplus1[dui.dofi]; + } + + const VectorType derivativeT1CrossT2 = computeDerivativeOfCrossProduct(t1, + derivativeT1, + t2, + derivativeT2); + const double derivativeOfNormT1CrossT2 = computeDerivativeOfNorm(t1CrossT2, derivativeT1CrossT2); + const double normT1CrossT2 = t1CrossT2.Norm(); + + const VectorType t3Deriv_firstTerm = -(t1CrossT2 * derivativeOfNormT1CrossT2) + / std::pow(normT1CrossT2, 2); + const VectorType t3Deriv_secondTerm = derivativeT1CrossT2 / normT1CrossT2; + const VectorType t3Deriv = t3Deriv_firstTerm + t3Deriv_secondTerm; + + assert(std::isfinite(t3Deriv[0]) && std::isfinite(t3Deriv[1]) && std::isfinite(t3Deriv[2])); + return t3Deriv; +} + +double DRMSimulationModel::computeDerivativeTheta1(const EdgeType &e, + const VertexIndex &evi, + const VertexIndex &dwrt_evi, + const DoFType &dwrt_dofi) const +{ + const VertexType &v = *e.cV(evi); + const size_t vi = pMesh->getIndex(v); + const Element &element = pMesh->elements[e]; + const VectorType derivativeT1 = element.derivativeT1[dwrt_evi][dwrt_dofi]; + const VectorType derivativeT3 = element.derivativeT3[dwrt_evi][dwrt_dofi]; + const VectorType nDerivative = evi != dwrt_evi ? VectorType(0, 0, 0) + : pMesh->nodes[v].derivativeOfNormal[dwrt_dofi]; + const VectorType n = v.cN(); + const VectorType &t1 = element.frame.t1; + const VectorType &t3 = element.frame.t3; + const double theta1Derivative = derivativeT1 * Cross(t3, n) + + t1 * (Cross(derivativeT3, n) + Cross(t3, nDerivative)); + const bool shouldBreak = mCurrentSimulationStep == 118 && vi == 1 && dwrt_dofi == 3; + + return theta1Derivative; +} + +double DRMSimulationModel::computeDerivativeTheta2(const EdgeType &e, + const VertexIndex &evi, + const VertexIndex &dwrt_evi, + const DoFType &dwrt_dofi) const +{ + const VertexType &v = *e.cV(evi); + + const Element &element = pMesh->elements[e]; + const VectorType derivativeT2 = element.derivativeT2[dwrt_evi][dwrt_dofi]; + const VectorType derivativeT3 = element.derivativeT3[dwrt_evi][dwrt_dofi]; + + const VectorType n = v.cN(); + const VectorType &t2 = element.frame.t2; + const VectorType &t3 = element.frame.t3; + const VectorType nDerivative = dwrt_evi == evi ? pMesh->nodes[v].derivativeOfNormal[dwrt_dofi] + : VectorType(0, 0, 0); + const double theta2Derivative = derivativeT2 * Cross(t3, n) + + t2 * (Cross(derivativeT3, n) + Cross(t3, nDerivative)); + + return theta2Derivative; +} + +double DRMSimulationModel::computeTheta3(const EdgeType &e, const VertexType &v) +{ + const VertexIndex &vi = pMesh->nodes[v].vi; + // assert(e.cV(0) == &v || e.cV(1) == &v); + + const Element &elem = pMesh->elements[e]; + const EdgeIndex &ei = elem.ei; + const Element::LocalFrame &ef = elem.frame; + const VectorType &t1 = ef.t1; + const VectorType &n = v.cN(); + const Node &node = pMesh->nodes[v]; + const double &nR = node.nR; // TODO: This makes the function non-const. + // Should be refactored in the future + + double theta3; + const bool shouldBreak = mCurrentSimulationStep == 12970; + if (&e == node.referenceElement) { + // Use nR as theta3 only for the first star edge + return nR; + } + // std::vector incidentElementsIndices(node.incidentElements.size()); + // for (int iei = 0; iei < incidentElementsIndices.size(); iei++) { + // incidentElementsIndices[iei] = pMesh->getIndex(node.incidentElements[iei]); + // } + assert(pMesh->getIndex(e) == ei); + // assert(node.alphaAngles.contains(ei)); + const double alphaAngle = std::find_if(node.alphaAngles.begin(), + node.alphaAngles.end(), + [&](const std::pair &p) { + return elem.ei == p.first; + }) + ->second; + const EdgeType &refElem = *node.referenceElement; + const double rotationAngle = nR + alphaAngle; + + // const VectorType &t1_k = computeT1Vector(refElem); + const VectorType &t1_k = pMesh->elements[refElem].frame.t1; + const VectorType f1 = (t1_k - (n * (t1_k * n))).Normalize(); + vcg::Matrix44 rotationMatrix; + rotationMatrix.SetRotateRad(rotationAngle, n); + const double cosRotationAngle = cos(rotationAngle); + const double sinRotationAngle = sin(rotationAngle); + const VectorType f2 = (f1 * cosRotationAngle + Cross(n, f1) * sinRotationAngle).Normalize(); + const VectorType &t1Current = t1; + const VectorType f3 = Cross(t1Current, f2); + + Element &element = pMesh->elements[e]; + // Save for computing theta3Derivative + if (&v == e.cV(0)) { + element.f1_j = f1; + element.f2_j = f2; + element.f3_j = f3; + element.cosRotationAngle_j = cosRotationAngle; + element.sinRotationAngle_j = sinRotationAngle; + } else { + element.f1_jplus1 = f1; + element.f2_jplus1 = f2; + element.f3_jplus1 = f3; + element.cosRotationAngle_jplus1 = cosRotationAngle; + element.sinRotationAngle_jplus1 = sinRotationAngle; + } + theta3 = f3.dot(n); + + return theta3; +} + +double DRMSimulationModel::computeDerivativeTheta3(const EdgeType &e, + const VertexType &v, + const DifferentiateWithRespectTo &dui) const +{ + const Node &node = pMesh->nodes[v]; + const VertexIndex &vi = pMesh->nodes[v].vi; + if (&e == node.referenceElement && !isRigidSupport[vi]) { + if (dui.dofi == DoF::Nr && &dui.v == &v) { + return 1; + } else { + return 0; + } + } + // assert(e.cV(0) == &v || e.cV(1) == &v); + + const Element &element = pMesh->elements[e]; + const Element::LocalFrame &ef = element.frame; + const VectorType &t1 = ef.t1; + const VectorType &n = v.cN(); + const DoFType &dofi = dui.dofi; + const VertexPointer &vp_j = e.cV(0); + const VertexPointer &vp_jplus1 = e.cV(1); + + double derivativeTheta3_dofi = 0; + if (isRigidSupport[vi]) { + const VectorType &t1Initial = computeT1Vector(pMesh->nodes[vp_j].initialLocation, + pMesh->nodes[vp_jplus1].initialLocation); + VectorType g1 = Cross(t1, t1Initial); + + const VectorType derivativeInitialT1_dofi(0, 0, 0); + const VectorType derivativeT1_j = dui.dofi < 3 && vp_j == &dui.v + ? element.derivativeT1_j[dui.dofi] + : VectorType(0, 0, 0); + const VectorType derivativeT1_jplus1 = dui.dofi < 3 && vp_jplus1 == &dui.v + ? element.derivativeT1_jplus1[dui.dofi] + : VectorType(0, 0, 0); + const VectorType derivativeT1_dofi = derivativeT1_j + derivativeT1_jplus1; + // VectorType derivativeT1_dofi(0, 0, 0); + // if (dui.dofi < 3) { + // if (&v_j == &dui.v) { + // derivativeT1_dofi = mesh->elements[e].derivativeT1_j[dui.dofi]; + // } else if (&v_jplus1 == &dui.v) { + // derivativeT1_dofi = + // mesh->elements[e].derivativeT1_jplus1[dui.dofi]; + // } + // } + + const VectorType derivativeG1_firstTerm = Cross(derivativeT1_dofi, t1Initial); + const VectorType derivativeG1_secondTerm = Cross(t1, derivativeInitialT1_dofi); + const VectorType derivativeG1 = derivativeG1_firstTerm + derivativeG1_secondTerm; + const VectorType derivativeNormal = &v == &dui.v && dui.dofi > 2 + ? node.derivativeOfNormal[dui.dofi] + : VectorType(0, 0, 0); + derivativeTheta3_dofi = derivativeG1 * n + g1 * derivativeNormal; + if (std::isnan(derivativeTheta3_dofi)) { + std::cerr << "nan derivative theta3 rigid" << std::endl; + } + return derivativeTheta3_dofi; + } + EdgeType &refElem = *node.referenceElement; + const VectorType &t1_k = pMesh->elements[refElem].frame.t1; + VectorType f1, f2, f3; + double cosRotationAngle, sinRotationAngle; + if (e.cV(0) == &v) { + f1 = element.f1_j; + cosRotationAngle = element.cosRotationAngle_j; + sinRotationAngle = element.sinRotationAngle_j; + f2 = element.f2_j; + f3 = element.f3_j; + } else { + f1 = element.f1_jplus1; + cosRotationAngle = element.cosRotationAngle_jplus1; + sinRotationAngle = element.sinRotationAngle_jplus1; + f2 = element.f2_jplus1; + f3 = element.f3_jplus1; + } + const VectorType &t1_kplus1 = t1; + const VectorType f1Normalized = f1 / f1.Norm(); + + VectorType derivativeF1(0, 0, 0); + VectorType derivativeF2(0, 0, 0); + VectorType derivativeF3(0, 0, 0); + if (dui.dofi < 3) { + const VectorType derivativeT1_kplus1_j = vp_j == &dui.v ? element.derivativeT1_j[dui.dofi] + : VectorType(0, 0, 0); + const VectorType derivativeT1_kplus1_jplus1 = vp_jplus1 == &dui.v + ? element.derivativeT1_jplus1[dui.dofi] + : VectorType(0, 0, 0); + const VectorType derivativeT1_kplus1 = derivativeT1_kplus1_j + derivativeT1_kplus1_jplus1; + + const VectorType derivativeT1_k_j = refElem.cV(0) == &dui.v + ? pMesh->elements[refElem].derivativeT1_j[dui.dofi] + : VectorType(0, 0, 0); + const VectorType derivativeT1_k_jplus1 + = refElem.cV(1) == &dui.v ? pMesh->elements[refElem].derivativeT1_jplus1[dui.dofi] + : VectorType(0, 0, 0); + const VectorType derivativeT1_k = derivativeT1_k_j + derivativeT1_k_jplus1; + + derivativeF1 = derivativeT1_k - (n * (derivativeT1_k * n)); + + const double f1Norm = f1.Norm(); + const double derivativeF1Norm = f1 * derivativeF1 / f1Norm; + const VectorType derivativeF1Normalized = -f1 * derivativeF1Norm / (f1Norm * f1Norm) + + derivativeF1 / f1Norm; + + derivativeF2 = derivativeF1Normalized * cosRotationAngle + + Cross(n, derivativeF1Normalized) * sinRotationAngle; + const VectorType derivativeF3_firstTerm = Cross(derivativeT1_kplus1, f2); + const VectorType derivativeF3_secondTerm = Cross(t1_kplus1, derivativeF2); + derivativeF3 = derivativeF3_firstTerm + derivativeF3_secondTerm; + derivativeTheta3_dofi = derivativeF3 * n; + + } else if (dui.dofi == DoF::Nr && &dui.v == &v) { + derivativeF2 = f1Normalized * (-sinRotationAngle) + + Cross(n, f1Normalized) * cosRotationAngle; + derivativeF3 = Cross(t1_kplus1, derivativeF2); + derivativeTheta3_dofi = derivativeF3 * n; + } else { // 2edge) { + const Element &element = pMesh->elements[e]; + const SimulationMesh::VertexType &ev_j = *e.cV(0); + const SimulationMesh::VertexType &ev_jplus1 = *e.cV(1); + 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); + + const EdgeIndex ei = pMesh->getIndex(e); + const double e_k = element.length - element.initialLength; + const double axialPE = pow(e_k, 2) * element.material.youngsModulus * element.A + / (2 * element.initialLength); + const double theta1Diff = theta1_jplus1 - theta1_j; + const double torsionalPE = element.G * element.J * pow(theta1Diff, 2) + / (2 * element.initialLength); + const double firstBendingPE_inBracketsTerm = 4 * pow(theta2_j, 2) + + 4 * theta2_j * theta2_jplus1 + + 4 * pow(theta2_jplus1, 2); + const double firstBendingPE = firstBendingPE_inBracketsTerm * element.material.youngsModulus + * element.I2 / (2 * element.initialLength); + const double secondBendingPE_inBracketsTerm = 4 * pow(theta3_j, 2) + + 4 * theta3_j * theta3_jplus1 + + 4 * pow(theta3_jplus1, 2); + const double secondBendingPE = secondBendingPE_inBracketsTerm + * element.material.youngsModulus * element.I3 + / (2 * element.initialLength); + + totalInternalPotentialEnergy += axialPE + torsionalPE + firstBendingPE + secondBendingPE; + int i = 0; + i++; + } + + return totalInternalPotentialEnergy; +} + +double DRMSimulationModel::computeTotalPotentialEnergy() +{ + double totalExternalPotentialEnergy = 0; + for (const SimulationMesh::VertexType &v : pMesh->vert) { + const Node &node = pMesh->nodes[v]; + if (!node.force.hasExternalForce()) { + continue; + } + const auto nodeDisplacement = v.cP() - node.initialLocation; + const SimulationMesh::CoordType externalForce(node.force.external[0], + node.force.external[1], + node.force.external[2]); + totalExternalPotentialEnergy += externalForce.dot(nodeDisplacement); + } + + const double totalInternalPotentialEnergy = computeTotalInternalPotentialEnergy(); + + return totalInternalPotentialEnergy - totalExternalPotentialEnergy; +} + +void DRMSimulationModel::updateResidualForcesOnTheFly( + const std::unordered_map> &fixedVertices) +{ + // std::vector internalForcesParallel(mesh->vert.size()); + + std::vector>> internalForcesContributionsFromEachEdge( + pMesh->EN(), std::vector>(4, {-1, Vector6d()})); + // omp_lock_t writelock; + // omp_init_lock(&writelock); +<<<<<<< HEAD +#ifdef ENABLE_OPENMP +#pragma omp parallel for schedule(static) num_threads(5) +#endif +======= + //#ifdef ENABLE_OPENMP + //#pragma omp parallel for schedule(static) num_threads(5) + //#endif +>>>>>>> master + for (int ei = 0; ei < pMesh->EN(); ei++) { + 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 bool shouldBreak = mCurrentSimulationStep == 12970; + 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::vector> internalForcesContributionFromThisEdge(4, + {-1, + Vector6d()}); + for (VertexIndex evi = 0; evi < 2; evi++) { + const SimulationMesh::VertexType &ev = *e.cV(evi); + const Node &edgeNode = pMesh->nodes[ev]; + internalForcesContributionFromThisEdge[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[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 = 2 * theta3_jplus1_deriv + * 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[evi].second[dofi] + = axialForce_dofi + firstBendingForce_dofi + secondBendingForce_dofi + + torsionalForce_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[evi + 2].second[dofi] + = secondBendingForce_dofi; + } + } + } + } + internalForcesContributionsFromEachEdge[ei] = internalForcesContributionFromThisEdge; + } + + //#pragma omp parallel for schedule(static) num_threads(8) + + for (size_t vi = 0; vi < pMesh->VN(); vi++) { + Node::Forces &force = pMesh->nodes[vi].force; + // Vector6d ext = force.external; + // if (mCurrentSimulationStep <= 100) { + // ext[3] = 0; + // ext[4] = 0; + // } + force.residual = force.external; + force.internal = 0; + } + for (size_t ei = 0; ei < pMesh->EN(); ei++) { + for (int i = 0; i < 4; i++) { + std::pair internalForcePair + = internalForcesContributionsFromEachEdge[ei][i]; + int vi = internalForcePair.first; + if (i > 1 && vi == -1) { + continue; + } + Node::Forces &force = pMesh->nodes[vi].force; + force.internal = force.internal + internalForcePair.second; + force.residual = force.residual + (internalForcePair.second * -1); +#ifdef POLYSCOPE_DEFINED + if (std::isnan(internalForcePair.second.norm())) { + std::cerr << "nan on edge" << ei << std::endl; + } +#endif + } + } + double totalResidualForcesNorm = 0; + Vector6d sumOfResidualForces(0); + for (size_t vi = 0; vi < pMesh->VN(); vi++) { + Node::Forces &force = pMesh->nodes[vi].force; + const Vector6d &nodeResidualForce = force.residual; + sumOfResidualForces = sumOfResidualForces + nodeResidualForce; + const double residualForceNorm = nodeResidualForce.norm(); + // const double residualForceNorm = nodeResidualForce.getTranslation().norm(); + const bool shouldTrigger = mCurrentSimulationStep == 500; + totalResidualForcesNorm += residualForceNorm; +#ifdef POLYSCOPE_DEFINED + if (std::isnan(force.residual.norm())) { + std::cout << "residual " << vi << ":" << force.residual.toString() << std::endl; + } +#endif + } + pMesh->previousTotalResidualForcesNorm = pMesh->totalResidualForcesNorm; + pMesh->totalResidualForcesNorm = totalResidualForcesNorm; + if (mSettings.beVerbose) { + if (minTotalResidualForcesNorm > pMesh->totalResidualForcesNorm) { + minTotalResidualForcesNorm = pMesh->totalResidualForcesNorm; + } + } + pMesh->averageResidualForcesNorm = totalResidualForcesNorm / pMesh->VN(); + // pMesh->averageResidualForcesNorm = sumOfResidualForces.norm() / pMesh->VN(); + + // plotYValues.push_back(totalResidualForcesNorm); + // auto xPlot = matplot::linspace(0, plotYValues.size(), plotYValues.size()); + // plotHandle = matplot::scatter(xPlot, plotYValues); +} + +void DRMSimulationModel::updateNodalExternalForces( + const std::unordered_map &nodalForces, + const std::unordered_map> &fixedVertices) +{ + externalMomentsNorm = 0; + for (const std::pair &nodalForce : nodalForces) { + const VertexIndex nodeIndex = nodalForce.first; + const bool isNodeConstrained = fixedVertices.contains(nodeIndex); + Node &node = pMesh->nodes[nodeIndex]; + Vector6d nodalExternalForce(0); + for (DoFType dofi = 0; dofi < 6; dofi++) { + const bool isDofConstrained = isNodeConstrained + && fixedVertices.at(nodeIndex).contains(dofi); + if (isDofConstrained) { + continue; + } + nodalExternalForce[dofi] = nodalForce.second[dofi]; + } + externalMomentsNorm += std::sqrt(pow(nodalExternalForce[3], 2) + + pow(nodalExternalForce[4], 2) + + pow(nodalExternalForce[5], 2)); + + /* + * The external moments are given as a rotation around an axis. + * In this implementation we model moments as rotation of the normal vector + * and because of that we need to transform the moments. + */ + + if (externalMomentsNorm != 0) { + VectorType momentAxis(nodalExternalForce[3], + nodalExternalForce[4], + nodalExternalForce[5]); // rotation around this vector + VectorType transformedVector = vcg::RotationMatrix(VectorType(0, 0, 1), + vcg::math::ToRad(-90.0)) + * momentAxis; + nodalExternalForce[3] = transformedVector[0]; + nodalExternalForce[4] = transformedVector[1]; + nodalExternalForce[5] = transformedVector[2]; + // node.nR = transformedVector[2]; + } + + node.force.external = nodalExternalForce; + } +} + +void DRMSimulationModel::updateResidualForces() +{ + pMesh->totalResidualForcesNorm = 0; + for (const VertexType &v : pMesh->vert) { + Node &node = pMesh->nodes[v]; + node.force.residual = node.force.external - node.force.internal; + const double residualForceNorm = (node.force.residual).norm(); + pMesh->totalResidualForcesNorm += residualForceNorm; + } +} + +void DRMSimulationModel::computeRigidSupports() +{ + isRigidSupport.clear(); + isRigidSupport.resize(pMesh->VN(), false); + for (const VertexType &v : pMesh->vert) { + const VertexIndex vi = pMesh->nodes[v].vi; + const bool isVertexConstrained = constrainedVertices.contains(vi); + if (isVertexConstrained) { + auto constrainedDoFType = constrainedVertices.at(vi); + const bool hasAllDoFTypeConstrained = constrainedDoFType.contains(DoF::Ux) + && constrainedDoFType.contains(DoF::Uy) + && constrainedDoFType.contains(DoF::Uz) + && constrainedDoFType.contains(DoF::Nx) + && constrainedDoFType.contains(DoF::Ny) + && constrainedDoFType.contains(DoF::Nr); + if (hasAllDoFTypeConstrained) { + isRigidSupport[vi] = true; + } + } + } +} + +void DRMSimulationModel::updateNormalDerivatives() +{ + for (const VertexType &v : pMesh->vert) { + for (DoFType dofi = DoF::Nx; dofi < DoF::NumDoF; dofi++) { + DifferentiateWithRespectTo dui{v, dofi}; + pMesh->nodes[v].derivativeOfNormal[dofi] = computeDerivativeOfNormal(v, dui); + } + } +} + +void DRMSimulationModel::updateT1Derivatives() +{ + for (const EdgeType &e : pMesh->edge) { + for (DoFType dofi = DoF::Ux; dofi < DoF::Nx; dofi++) { + DifferentiateWithRespectTo dui_v0{*e.cV(0), dofi}; + Element &element = pMesh->elements[e]; + element.derivativeT1_j[dofi] = computeDerivativeT1(e, dui_v0); + DifferentiateWithRespectTo dui_v1{*e.cV(1), dofi}; + element.derivativeT1_jplus1[dofi] = computeDerivativeT1(e, dui_v1); + + element.derivativeT1[0][dofi] = element.derivativeT1_j[dofi]; + element.derivativeT1[1][dofi] = element.derivativeT1_jplus1[dofi]; + } + } +} + +void DRMSimulationModel::updateT2Derivatives() +{ + for (const EdgeType &e : pMesh->edge) { + for (DoFType dofi = DoF::Ux; dofi < DoF::NumDoF; dofi++) { + DifferentiateWithRespectTo dui_v0{*e.cV(0), dofi}; + pMesh->elements[e].derivativeT2_j[dofi] = computeDerivativeT2(e, dui_v0); + DifferentiateWithRespectTo dui_v1{*e.cV(1), dofi}; + pMesh->elements[e].derivativeT2_jplus1[dofi] = computeDerivativeT2(e, dui_v1); + + Element &element = pMesh->elements[e]; + element.derivativeT2[0][dofi] = element.derivativeT2_j[dofi]; + element.derivativeT2[1][dofi] = element.derivativeT2_jplus1[dofi]; + } + } +} + +void DRMSimulationModel::updateT3Derivatives() +{ + for (const EdgeType &e : pMesh->edge) { + for (DoFType dofi = DoF::Ux; dofi < DoF::NumDoF; dofi++) { + DifferentiateWithRespectTo dui_v0{*e.cV(0), dofi}; + Element &element = pMesh->elements[e]; + element.derivativeT3_j[dofi] = computeDerivativeT3(e, dui_v0); + DifferentiateWithRespectTo dui_v1{*e.cV(1), dofi}; + element.derivativeT3_jplus1[dofi] = computeDerivativeT3(e, dui_v1); + + element.derivativeT3[0][dofi] = element.derivativeT3_j[dofi]; + element.derivativeT3[1][dofi] = element.derivativeT3_jplus1[dofi]; + } + } +} + +void DRMSimulationModel::updateRDerivatives() +{ + for (const EdgeType &e : pMesh->edge) { + for (DoFType dofi = DoF::Nx; dofi < DoF::NumDoF; dofi++) { + DifferentiateWithRespectTo dui_v0{*e.cV(0), dofi}; + pMesh->elements[e].derivativeR_j[dofi] = computeDerivativeOfR(e, dui_v0); + DifferentiateWithRespectTo dui_v1{*e.cV(1), dofi}; + pMesh->elements[e].derivativeR_jplus1[dofi] = computeDerivativeOfR(e, dui_v1); + } + } +} + +void DRMSimulationModel::updateElementalLengths() +{ + pMesh->updateElementalLengths(); +} + +DRMSimulationModel::DRMSimulationModel() {} + +void DRMSimulationModel::updateNodalMasses() +{ + double gamma = mSettings.gamma; + const size_t untilStep = 4000; + if (shouldTemporarilyDampForces && mCurrentSimulationStep < untilStep) { + gamma *= 1e6 * (1 - static_cast(mCurrentSimulationStep) / untilStep); + } +<<<<<<< HEAD + if (mCurrentSimulationStep == static_cast(1.2 * untilStep) + && shouldTemporarilyDampForces) { + Dt = mSettings.Dtini; + } +======= + // if (mCurrentSimulationStep == static_cast(1.4 * untilStep) + // && shouldTemporarilyDampForces) { + // Dt = mSettings.Dtini; + // } +>>>>>>> master + for (VertexType &v : pMesh->vert) { + const size_t vi = pMesh->getIndex(v); + double translationalSumSk = 0; + double rotationalSumSk_I2 = 0; + double rotationalSumSk_I3 = 0; + double rotationalSumSk_J = 0; + for (const EdgePointer &ep : pMesh->nodes[v].incidentElements) { + const size_t ei = pMesh->getIndex(ep); + const Element &elem = pMesh->elements[ep]; + const double SkTranslational = elem.material.youngsModulus * elem.A / elem.length; + translationalSumSk += SkTranslational; + const double lengthToThe3 = std::pow(elem.length, 3); + const long double SkRotational_I2 = elem.material.youngsModulus * elem.I2 + / lengthToThe3; + const long double SkRotational_I3 = elem.material.youngsModulus * elem.I3 + / lengthToThe3; + const long double SkRotational_J = elem.material.youngsModulus * elem.J / lengthToThe3; + rotationalSumSk_I2 += SkRotational_I2; + rotationalSumSk_I3 += SkRotational_I3; + rotationalSumSk_J += SkRotational_J; + assert(rotationalSumSk_I2 != 0); + assert(rotationalSumSk_I3 != 0); + assert(rotationalSumSk_J != 0); + } + pMesh->nodes[v].mass.translational = gamma * pow(mSettings.Dtini, 2) * 2 + * translationalSumSk; + pMesh->nodes[v].mass.rotationalI2 = gamma * pow(mSettings.Dtini, 2) * 8 + * rotationalSumSk_I2; + pMesh->nodes[v].mass.rotationalI3 = gamma * pow(mSettings.Dtini, 2) * 8 + * rotationalSumSk_I3; + pMesh->nodes[v].mass.rotationalJ = gamma * pow(mSettings.Dtini, 2) * 8 * rotationalSumSk_J; + + //fill 6d mass vector + pMesh->nodes[v].mass_6d[DoF::Ux] = pMesh->nodes[v].mass.translational; + pMesh->nodes[v].mass_6d[DoF::Uy] = pMesh->nodes[v].mass.translational; + pMesh->nodes[v].mass_6d[DoF::Uz] = pMesh->nodes[v].mass.translational; + pMesh->nodes[v].mass_6d[DoF::Nx] = pMesh->nodes[v].mass.rotationalJ; + pMesh->nodes[v].mass_6d[DoF::Ny] = pMesh->nodes[v].mass.rotationalI3; + pMesh->nodes[v].mass_6d[DoF::Nr] = pMesh->nodes[v].mass.rotationalI2; + if (mSettings.useViscousDamping) { + //fill 6d damping vector + const double translationalDampingFactor + = 2 * std::sqrt(translationalSumSk * pMesh->nodes[v].mass.translational); + pMesh->nodes[v].damping_6d[DoF::Ux] = translationalDampingFactor; + pMesh->nodes[v].damping_6d[DoF::Uy] = translationalDampingFactor; + pMesh->nodes[v].damping_6d[DoF::Uz] = translationalDampingFactor; + pMesh->nodes[v].damping_6d[DoF::Nx] = 2 + * std::sqrt(rotationalSumSk_J + * pMesh->nodes[v].mass_6d[DoF::Nx]); + pMesh->nodes[v].damping_6d[DoF::Ny] = 2 + * std::sqrt(rotationalSumSk_I3 + * pMesh->nodes[v].mass_6d[DoF::Ny]); + pMesh->nodes[v].damping_6d[DoF::Nr] = 2 + * std::sqrt(rotationalSumSk_I2 + * pMesh->nodes[v].mass_6d[DoF::Nr]); + pMesh->nodes[v].damping_6d = pMesh->nodes[v].damping_6d * 1e-2; + } + assert(std::pow(mSettings.Dtini, 2.0) * translationalSumSk + / pMesh->nodes[v].mass.translational + < 2); + assert(std::pow(mSettings.Dtini, 2.0) * rotationalSumSk_I2 + / pMesh->nodes[v].mass.rotationalI2 + < 2); + assert(std::pow(mSettings.Dtini, 2.0) * rotationalSumSk_I3 + / pMesh->nodes[v].mass.rotationalI3 + < 2); + assert(std::pow(mSettings.Dtini, 2.0) * rotationalSumSk_J / pMesh->nodes[v].mass.rotationalJ + < 2); + } +} + +void DRMSimulationModel::updateNodalAccelerations() +{ + for (VertexType &v : pMesh->vert) { + Node &node = pMesh->nodes[v]; + const VertexIndex vi = pMesh->getIndex(v); + node.acceleration = node.force.residual / node.mass_6d; +#ifdef POLYSCOPE_DEFINED + if (std::isnan(node.acceleration.norm())) { + std::cout << "acceleration " << vi << ":" << node.acceleration.toString() << std::endl; + } +#endif + // if (vi == 10) { + // std::cout << "Acceleration:" << node.acceleration[0] << " " << node.acceleration[1] + // << " " << node.acceleration[2] << std::endl; + // } + + // if (shouldTemporarilyDampForces && mCurrentSimulationStep < 700) { + // node.acceleration = node.acceleration * 1e-2; + // } + } +} + +void DRMSimulationModel::updateNodalVelocities() +{ + for (VertexType &v : pMesh->vert) { + const VertexIndex vi = pMesh->getIndex(v); + Node &node = pMesh->nodes[v]; + if (mSettings.useViscousDamping) { + const Vector6d massOverDt = node.mass_6d / Dt; + const Vector6d visciousDampingFactor(viscuousDampingConstant / 2); + // const Vector6d &visciousDampingFactor = node.damping_6d; + const Vector6d denominator = massOverDt + visciousDampingFactor / 2; + const Vector6d firstTermNominator = massOverDt - visciousDampingFactor / 2; + const Vector6d firstTerm = node.velocity * firstTermNominator / denominator; + const Vector6d secondTerm = node.force.residual / denominator; + node.velocity = firstTerm + secondTerm; + } else { + node.velocity = node.velocity + node.acceleration * Dt; + } +#ifdef POLYSCOPE_DEFINED + if (std::isnan(node.velocity.norm())) { + std::cout << "Velocity " << vi << ":" << node.velocity.toString() << std::endl; + } +#endif + } + updateKineticEnergy(); +} + +void DRMSimulationModel::updateNodalDisplacements() +{ + const bool shouldCapDisplacements = mSettings.displacementCap.has_value(); + for (VertexType &v : pMesh->vert) { + Node &node = pMesh->nodes[v]; + Vector6d stepDisplacement = node.velocity * Dt; + if (shouldCapDisplacements + && stepDisplacement.getTranslation().norm() > mSettings.displacementCap) { + stepDisplacement = stepDisplacement + * (*mSettings.displacementCap + / stepDisplacement.getTranslation().norm()); + std::cout << "Displacement capped" << std::endl; + } + node.displacements = node.displacements + stepDisplacement; + // if (mSettings.isDebugMode && mSettings.beVerbose && pMesh->getIndex(v) == 40) { + // std::cout << "Node " << node.vi << ":" << endl; + // std::cout << node.displacements[0] << " " << node.displacements[0] << " " + // << node.displacements[1] << " " << node.displacements[2] << " " + // << node.displacements[3] << " " << node.displacements[4] << " " + // << node.displacements[5] << std::endl; + // } + } +} + +void DRMSimulationModel::updateNodePosition( + VertexType &v, const std::unordered_map> &fixedVertices) +{ + Node &node = pMesh->nodes[v]; + const VertexIndex &vi = pMesh->nodes[v].vi; + + VectorType displacementVector(0, 0, 0); + if (!fixedVertices.contains(vi) || !fixedVertices.at(vi).contains(0)) { + displacementVector += VectorType(node.displacements[0], 0, 0); + } + if (!fixedVertices.contains(vi) || !fixedVertices.at(vi).contains(1)) { + displacementVector += VectorType(0, node.displacements[1], 0); + } + if (!fixedVertices.contains(vi) || !fixedVertices.at(vi).contains(2)) { + displacementVector += VectorType(0, 0, node.displacements[2]); + } + v.P() = node.initialLocation + displacementVector; + if (shouldApplyInitialDistortion && mCurrentSimulationStep < 100) { + //TODO:The initial displacement should depend on the model and should only be applied if the forced displacements applied are out of plane + VectorType desiredInitialDisplacement(0.0015, 0.0015, 0.0015); + v.P() = v.P() + desiredInitialDisplacement; + } +} + +void DRMSimulationModel::updateNodeNr(VertexType &v) +{ + const VertexIndex &vi = pMesh->nodes[v].vi; + Node &node = pMesh->nodes[v]; + if (!isRigidSupport[vi]) { + node.nR = node.displacements[5]; + } else { + const EdgePointer &refElem = node.referenceElement; + const VectorType &refT1 = pMesh->elements[refElem].frame.t1; + + const VectorType &t1Initial = computeT1Vector(pMesh->nodes[refElem->cV(0)].initialLocation, + pMesh->nodes[refElem->cV(1)].initialLocation); + VectorType g1 = Cross(refT1, t1Initial); + node.nR = g1.dot(v.cN()); + } +} + +void DRMSimulationModel::updateNodeNormal( + VertexType &v, const std::unordered_map> &fixedVertices) +{ + Node &node = pMesh->nodes[v]; + const VertexIndex &vi = node.vi; + // if (vi == 1) { + // std::cout << "PRE:" << mesh->vert[1].N()[0] << " " << + // mesh->vert[1].N()[1] + // << " " << mesh->vert[1].N()[2] << std::endl; + // } + VectorType normalDisplacementVector(0, 0, 0); + if (!fixedVertices.contains(vi) || !fixedVertices.at(vi).contains(3)) { + normalDisplacementVector += VectorType(node.displacements[3], 0, 0); + } + if (!fixedVertices.contains(vi) || !fixedVertices.at(vi).contains(4)) { + normalDisplacementVector += VectorType(0, node.displacements[4], 0); + } + const double nxnyMagnitudePre = std::pow(v.N()[0], 2) + std::pow(v.N()[1], 2); + v.N() = node.initialNormal + normalDisplacementVector; + const double &nx = v.N()[0], ny = v.N()[1], nz = v.N()[2]; + const double nxnyMagnitude = std::pow(nx, 2) + std::pow(ny, 2); + VectorType n = v.N(); + const bool shouldBreak = mCurrentSimulationStep == 118 && vi == 3; + if (nxnyMagnitude > 1) { + VectorType newNormal(nx / std::sqrt(nxnyMagnitude), ny / std::sqrt(nxnyMagnitude), 0); + v.N() = newNormal; + + /*If an external moment caused the normal to lay on the xy plane this means + * that in order to disable its effect a greater internal force is needed + * than what is possible (the constraint on the z of the normals imposes a + * constraint on the maximum internal force). Because of that the + * totalResidualForcesNorm can't drop below the magnitude of external moment + * applied on vertex vi. In order to allow termination of the simulation + * when the described phenomenon happens we allow the termination of the + * algorithm if the kinetic energy of the system drops below the set + * threshold. + * */ + const bool viHasMoments = node.force.external[3] != 0 || node.force.external[4] != 0; + if (!checkedForMaximumMoment && viHasMoments) { + mSettings.shouldUseTranslationalKineticEnergyThreshold = true; +#ifdef POLYSCOPE_DEFINED + std::cout << "Maximum moment reached.The Kinetic energy of the system will " + "be used as a convergence criterion" + << std::endl; +#endif + checkedForMaximumMoment = true; + } + + } else { + const double nzSquared = 1.0 - nxnyMagnitude; + const double nz = std::sqrt(nzSquared); + VectorType newNormal(nx, ny, nz); + v.N() = newNormal; + } + + // if (!fixedVertices.contains(vi) || !fixedVertices.at(vi).contains(DoF::Nr)) { + // if (vi == 1) { + // std::cout << "AFTER:" << mesh->vert[1].N()[0] << " " << + // mesh->vert[1].N()[1] + // << " " << mesh->vert[1].N()[2] << std::endl; + // } +} + +void DRMSimulationModel::applyDisplacements( + const std::unordered_map> &fixedVertices) +{ + for (VertexType &v : pMesh->vert) { + updateNodePosition(v, fixedVertices); + updateNodeNormal(v, fixedVertices); + updateNodeNr(v); + } + updateElementalFrames(); + if (mSettings.shouldDraw) { + pMesh->updateEigenEdgeAndVertices(); + } +} + +void DRMSimulationModel::updateElementalFrames() +{ + for (const EdgeType &e : pMesh->edge) { + const VectorType elementNormal = (e.cV(0)->cN() + e.cV(1)->cN()).Normalize(); + pMesh->elements[e].frame = computeElementFrame(e.cP(0), e.cP(1), elementNormal); + } +} + +void DRMSimulationModel::applyForcedDisplacements( + const std::unordered_map &nodalForcedDisplacements) +{ + for (const std::pair vertexIndexDisplacementPair : + nodalForcedDisplacements) { + const VertexIndex vi = vertexIndexDisplacementPair.first; + const Eigen::Vector3d vertexDisplacement = vertexIndexDisplacementPair.second; + Node &node = pMesh->nodes[vi]; + VectorType displacementVector(vertexDisplacement(0), + vertexDisplacement(1), + vertexDisplacement(2)); + if (mCurrentSimulationStep < mSettings.gradualForcedDisplacementSteps) { + displacementVector *= mCurrentSimulationStep + / static_cast(mSettings.gradualForcedDisplacementSteps); + } + pMesh->vert[vi].P() = node.initialLocation + displacementVector; + node.displacements = Vector6d({displacementVector[0], + displacementVector[1], + displacementVector[2], + node.displacements[3], + node.displacements[4], + node.displacements[5]}); + } + + if (mSettings.shouldDraw) { + pMesh->updateEigenEdgeAndVertices(); + } +} + +void DRMSimulationModel::applyForcedNormals( + const std::unordered_map nodalForcedRotations) +{ + for (const std::pair vertexIndexDesiredNormalPair : + nodalForcedRotations) { + const VertexIndex vi = vertexIndexDesiredNormalPair.first; + + Node &node = pMesh->nodes[vi]; + pMesh->vert[vi].N() = vertexIndexDesiredNormalPair.second; + node.displacements = Vector6d( + {node.displacements[0], + node.displacements[1], + node.displacements[2], + vertexIndexDesiredNormalPair.second[0] - node.initialNormal[0], + vertexIndexDesiredNormalPair.second[1] - node.initialNormal[1], + node.displacements[5]}); + } +} + +// NOTE: Is this correct? Should the kinetic energy be computed like that? +void DRMSimulationModel::updateKineticEnergy() +{ + pMesh->previousTotalKineticEnergy = pMesh->currentTotalKineticEnergy; + pMesh->currentTotalKineticEnergy = 0; + pMesh->currentTotalTranslationalKineticEnergy = 0; + for (const VertexType &v : pMesh->vert) { + Node &node = pMesh->nodes[v]; + node.kineticEnergy = 0; + + const double translationalVelocityNorm = std::sqrt(std::pow(node.velocity[0], 2) + + std::pow(node.velocity[1], 2) + + std::pow(node.velocity[2], 2)); + const double nodeTranslationalKineticEnergy = 0.5 * node.mass.translational + * pow(translationalVelocityNorm, 2); + + const double nodeRotationalKineticEnergy + = 0.5 + * (node.mass.rotationalJ * pow(node.velocity[3], 2) + + node.mass.rotationalI3 * pow(node.velocity[4], 2) + + node.mass.rotationalI2 * pow(node.velocity[5], 2)); + + node.kineticEnergy = nodeTranslationalKineticEnergy + nodeRotationalKineticEnergy; + assert(node.kineticEnergy < 1e15); + + pMesh->currentTotalKineticEnergy += node.kineticEnergy; + pMesh->currentTotalTranslationalKineticEnergy += nodeTranslationalKineticEnergy; + } + // assert(mesh->currentTotalKineticEnergy < 100000000000000); +} + +void DRMSimulationModel::resetVelocities() +{ + for (const VertexType &v : pMesh->vert) { + pMesh->nodes[v].velocity = + // pMesh->nodes[v].acceleration * Dt + // * 0.5; // NOTE: Do I reset the previous + // // velocity? + // // reset + // // current to 0 or this? + 0; + } + updateKineticEnergy(); +} + +void DRMSimulationModel::updatePositionsOnTheFly( + const std::unordered_map> &fixedVertices) +{ + const double gamma = 0.8; + for (VertexType &v : pMesh->vert) { + double translationalSumSk = 0; + double rotationalSumSk_I2 = 0; + double rotationalSumSk_I3 = 0; + double rotationalSumSk_J = 0; + for (const EdgePointer &ep : pMesh->nodes[v].incidentElements) { + const Element &elem = pMesh->elements[ep]; + const double SkTranslational = elem.material.youngsModulus * elem.A / elem.length; + translationalSumSk += SkTranslational; + const double lengthToThe3 = std::pow(elem.length, 3); + const double SkRotational_I2 = elem.material.youngsModulus * elem.I2 + / lengthToThe3; // TODO: I2->t2,I3->t3,t1->polar inertia + const double SkRotational_I3 = elem.material.youngsModulus * elem.I3 + / lengthToThe3; // TODO: I2->t2,I3->t3,t1->polar inertia + const double SkRotational_J = elem.material.youngsModulus * elem.J + / lengthToThe3; // TODO: I2->t2,I3->t3,t1->polar inertia + rotationalSumSk_I2 += SkRotational_I2; + rotationalSumSk_I3 += SkRotational_I3; + rotationalSumSk_J += SkRotational_J; + // assert(rotationalSumSk_I2 != 0); + // assert(rotationalSumSk_I3 != 0); + // assert(rotationalSumSk_J != 0); + } + pMesh->nodes[v].mass.translational = gamma * pow(mSettings.Dtini, 2) * 2 + * translationalSumSk; + pMesh->nodes[v].mass.rotationalI2 = gamma * pow(mSettings.Dtini, 2) * 8 + * rotationalSumSk_I2; + pMesh->nodes[v].mass.rotationalI3 = gamma * pow(mSettings.Dtini, 2) * 8 + * rotationalSumSk_I3; + pMesh->nodes[v].mass.rotationalJ = gamma * pow(mSettings.Dtini, 2) * 8 * rotationalSumSk_J; + + // assert(std::pow(mSettings.Dtini, 2.0) * translationalSumSk / + // mesh->nodes[v].translationalMass < + // 2); + // assert(std::pow(mSettings.Dtini, 2.0) * rotationalSumSk_I2 / + // mesh->nodes[v].rotationalMass_I2 < + // 2); + // assert(std::pow(mSettings.Dtini, 2.0) * rotationalSumSk_I3 / + // mesh->nodes[v].rotationalMass_I3 < + // 2); + // assert(std::pow(mSettings.Dtini, 2.0) * rotationalSumSk_J / + // mesh->nodes[v].rotationalMass_J < + // 2); + } + + for (VertexType &v : pMesh->vert) { + Node &node = pMesh->nodes[v]; + for (DoFType dofi = DoF::Ux; dofi < DoF::NumDoF; dofi++) { + if (dofi == DoF::Ux || dofi == DoF::Uy || dofi == DoF::Uz) { + node.acceleration[dofi] = node.force.residual[dofi] / node.mass.translational; + } else if (dofi == DoF::Nx) { + node.acceleration[dofi] = node.force.residual[dofi] / node.mass.rotationalJ; + } else if (dofi == DoF::Ny) { + node.acceleration[dofi] = node.force.residual[dofi] / node.mass.rotationalI3; + } else if (dofi == DoF::Nr) { + node.acceleration[dofi] = node.force.residual[dofi] / node.mass.rotationalI2; + } + } + } + + for (VertexType &v : pMesh->vert) { + Node &node = pMesh->nodes[v]; + node.velocity = node.velocity + node.acceleration * Dt; + } + updateKineticEnergy(); + + for (VertexType &v : pMesh->vert) { + Node &node = pMesh->nodes[v]; + node.displacements = node.displacements + node.velocity * Dt; + } + + for (VertexType &v : pMesh->vert) { + updateNodePosition(v, fixedVertices); + Node &node = pMesh->nodes[v]; + const VertexIndex &vi = node.vi; + VectorType normalDisplacementVector(0, 0, 0); + if (!fixedVertices.contains(vi) || !fixedVertices.at(vi).contains(3)) { + normalDisplacementVector += VectorType(node.displacements[3], 0, 0); + } + if (!fixedVertices.contains(vi) || !fixedVertices.at(vi).contains(4)) { + normalDisplacementVector += VectorType(0, node.displacements[4], 0); + } + v.N() = node.initialNormal + normalDisplacementVector; + const double &nx = v.N()[0], ny = v.N()[1]; + const double nxnyMagnitude = std::pow(nx, 2) + std::pow(ny, 2); + if (nxnyMagnitude > 1) { + v.N() = VectorType(nx / std::sqrt(nxnyMagnitude), ny / std::sqrt(nxnyMagnitude), 0); + } else { + const double nzSquared = 1.0 - nxnyMagnitude; + const double nz = std::sqrt(nzSquared); + VectorType newNormal(nx, ny, nz); + v.N() = newNormal; + } + if (!isRigidSupport[vi]) { + node.nR = node.displacements[5]; + } else { + } + } + updateElementalFrames(); +} + +SimulationResults DRMSimulationModel::computeResults(const std::shared_ptr &pJob) +{ + std::vector displacements(pMesh->VN()); + for (size_t vi = 0; vi < pMesh->VN(); vi++) { + displacements[vi] = pMesh->nodes[vi].displacements; + } + history.numberOfSteps = mCurrentSimulationStep; + SimulationResults results; + results.converged = true; + results.job = pJob; + results.history = history; + results.displacements = displacements; + return results; +} + +void DRMSimulationModel::printCurrentState() const +{ + std::cout << "Simulation steps executed:" << mCurrentSimulationStep << std::endl; + std::cout << "Residual forces norm: " << pMesh->totalResidualForcesNorm << std::endl; + std::cout << "Average Residual forces norm/extForcesNorm: " + << pMesh->totalResidualForcesNorm / pMesh->VN() / pMesh->totalExternalForcesNorm + << std::endl; + std::cout << "Moving average residual forces:" << pMesh->residualForcesMovingAverage + << std::endl; + std::cout << "Kinetic energy:" << pMesh->currentTotalKineticEnergy << std::endl; + static std::chrono::steady_clock::time_point begin = std::chrono::steady_clock::now(); + const double timePerNodePerIteration = std::chrono::duration_cast( + std::chrono::steady_clock::now() - begin) + .count() + * 1e-6 + / (static_cast(mCurrentSimulationStep) + * pMesh->VN()); + std::cout << "Total potential:" << pMesh->currentTotalPotentialEnergykN << " kNm" << std::endl; + std::cout << "time(s)/(iterations*node) = " << timePerNodePerIteration << std::endl; + std::cout << "Mov aver deriv norm:" << pMesh->residualForcesMovingAverageDerivativeNorm + << std::endl; + std::cout << "xi:" << mSettings.xi << std::endl; + std::cout << "Dt:" << Dt << std::endl; +} + +void DRMSimulationModel::printDebugInfo() const +{ + std::cout << pMesh->elements[0].rigidity.toString() << std::endl; + std::cout << "Number of dampings:" << numOfDampings << endl; + printCurrentState(); +} + +#ifdef POLYSCOPE_DEFINED +void DRMSimulationModel::draw(const std::string &screenshotsFolder) +{ + // update positions + // polyscope::getCurveNetwork("Undeformed edge mesh")->setEnabled(false); + polyscope::CurveNetwork *meshPolyscopeHandle = polyscope::getCurveNetwork(meshPolyscopeLabel); + meshPolyscopeHandle->updateNodePositions(pMesh->getEigenVertices()); + + // Vertex quantities + std::vector kineticEnergies(pMesh->VN()); + std::vector> nodeNormals(pMesh->VN()); + std::vector> internalForces(pMesh->VN()); + std::vector> externalForces(pMesh->VN()); + std::vector> externalMoments(pMesh->VN()); + std::vector internalForcesNorm(pMesh->VN()); + std::vector> internalAxialForces(pMesh->VN()); + std::vector> internalFirstBendingTranslationForces( + pMesh->VN()); + std::vector> internalFirstBendingRotationForces( + pMesh->VN()); + std::vector> internalSecondBendingTranslationForces( + pMesh->VN()); + std::vector> internalSecondBendingRotationForces( + pMesh->VN()); + std::vector nRs(pMesh->VN()); + std::vector theta2(pMesh->VN()); + std::vector theta3(pMesh->VN()); + std::vector> residualForces(pMesh->VN()); + std::vector residualForcesNorm(pMesh->VN()); + std::vector accelerationX(pMesh->VN()); + for (const VertexType &v : pMesh->vert) { + kineticEnergies[pMesh->getIndex(v)] = pMesh->nodes[v].kineticEnergy; + const VectorType n = v.cN(); + nodeNormals[pMesh->getIndex(v)] = {n[0], n[1], n[2]}; + // per node internal forces + const Vector6d nodeforce = pMesh->nodes[v].force.internal * (-1); + internalForces[pMesh->getIndex(v)] = {nodeforce[0], nodeforce[1], + nodeforce[2]}; + internalForcesNorm[pMesh->getIndex(v)] = nodeforce.norm(); + // External force + const Vector6d nodeExternalForce = pMesh->nodes[v].force.external; + externalForces[pMesh->getIndex(v)] = { + nodeExternalForce[0], nodeExternalForce[1], nodeExternalForce[2]}; + externalMoments[pMesh->getIndex(v)] = {nodeExternalForce[3], + nodeExternalForce[4], 0}; + internalAxialForces[pMesh->getIndex(v)] = {nodeforce[0], nodeforce[1], + nodeforce[2]}; + const Node &node = pMesh->nodes[v]; + const Vector6d nodeForceFirst = node.force.internalFirstBending * (-1); + internalFirstBendingTranslationForces[pMesh->getIndex(v)] = { + nodeForceFirst[0], nodeForceFirst[1], nodeForceFirst[2]}; + internalFirstBendingRotationForces[pMesh->getIndex(v)] = { + nodeForceFirst[3], nodeForceFirst[4], 0}; + + const Vector6d nodeForceSecond = node.force.internalSecondBending * (-1); + internalSecondBendingTranslationForces[pMesh->getIndex(v)] = { + nodeForceSecond[0], nodeForceSecond[1], nodeForceSecond[2]}; + internalSecondBendingRotationForces[pMesh->getIndex(v)] = { + nodeForceSecond[3], nodeForceSecond[4], 0}; + nRs[pMesh->getIndex(v)] = node.nR; + const Vector6d nodeResidualForce = pMesh->nodes[v].force.residual; + residualForces[pMesh->getIndex(v)] = { + nodeResidualForce[0], nodeResidualForce[1], nodeResidualForce[2]}; + residualForcesNorm[pMesh->getIndex(v)] = nodeResidualForce.norm(); + accelerationX[pMesh->getIndex(v)] = pMesh->nodes[v].acceleration[0]; + } + meshPolyscopeHandle->addNodeScalarQuantity("Kinetic Energy", kineticEnergies)->setEnabled(false); + meshPolyscopeHandle->addNodeVectorQuantity("Node normals", nodeNormals)->setEnabled(true); + meshPolyscopeHandle->addNodeVectorQuantity("Internal force", internalForces)->setEnabled(false); + meshPolyscopeHandle->addNodeVectorQuantity("External force", externalForces)->setEnabled(false); + meshPolyscopeHandle->addNodeVectorQuantity("External Moments", externalMoments)->setEnabled(true); + meshPolyscopeHandle->addNodeScalarQuantity("Internal force norm", internalForcesNorm) + ->setEnabled(true); + meshPolyscopeHandle->setRadius(0.002); + // polyscope::getCurveNetwork(meshPolyscopeLabel) + // ->addNodeVectorQuantity("Internal Axial force", internalAxialForces) + // ->setEnabled(false); + // polyscope::getCurveNetwork(meshPolyscopeLabel) + // ->addNodeVectorQuantity("First bending force-Translation", + // internalFirstBendingTranslationForces) + // ->setEnabled(false); + // polyscope::getCurveNetwork(meshPolyscopeLabel) + // ->addNodeVectorQuantity("First bending force-Rotation", + // internalFirstBendingRotationForces) + // ->setEnabled(false); + + // polyscope::getCurveNetwork(meshPolyscopeLabel) + // ->addNodeVectorQuantity("Second bending force-Translation", + // internalSecondBendingTranslationForces) + // ->setEnabled(false); + // polyscope::getCurveNetwork(meshPolyscopeLabel) + // ->addNodeVectorQuantity("Second bending force-Rotation", + // internalSecondBendingRotationForces) + // ->setEnabled(false); + polyscope::getCurveNetwork(meshPolyscopeLabel) + ->addNodeScalarQuantity("nR", nRs) + ->setEnabled(false); + // polyscope::getCurveNetwork(meshPolyscopeLabel) + // ->addNodeScalarQuantity("theta3", theta3) + // ->setEnabled(false); + // polyscope::getCurveNetwork(meshPolyscopeLabel) + // ->addNodeScalarQuantity("theta2", theta2) + // ->setEnabled(false); + polyscope::getCurveNetwork(meshPolyscopeLabel) + ->addNodeVectorQuantity("Residual force", residualForces) + ->setEnabled(false); + polyscope::getCurveNetwork(meshPolyscopeLabel) + ->addNodeScalarQuantity("Residual force norm", residualForcesNorm) + ->setEnabled(false); + // polyscope::getCurveNetwork(meshPolyscopeLabel) + // ->addNodeScalarQuantity("Node acceleration x", accelerationX); + + // Edge quantities + std::vector A(pMesh->EN()); + std::vector J(pMesh->EN()); + std::vector I2(pMesh->EN()); + std::vector I3(pMesh->EN()); + for (const EdgeType &e : pMesh->edge) { + const size_t ei = pMesh->getIndex(e); + A[ei] = pMesh->elements[e].A; + J[ei] = pMesh->elements[e].J; + I2[ei] = pMesh->elements[e].I2; + I3[ei] = pMesh->elements[e].I3; + } + + polyscope::getCurveNetwork(meshPolyscopeLabel)->addEdgeScalarQuantity("A", A); + polyscope::getCurveNetwork(meshPolyscopeLabel)->addEdgeScalarQuantity("J", J); + polyscope::getCurveNetwork(meshPolyscopeLabel) + ->addEdgeScalarQuantity("I2", I2); + polyscope::getCurveNetwork(meshPolyscopeLabel)->addEdgeScalarQuantity("I3", I3); + + // Specify the callback + static bool calledOnce = false; + if (!calledOnce) { + PolyscopeInterface::addUserCallback([&]() { + // Since options::openImGuiWindowForUserCallback == true by default, + // we can immediately start using ImGui commands to build a UI + + ImGui::PushItemWidth(100); // Make ui elements 100 pixels wide, + // instead of full width. Must have + // matching PopItemWidth() below. + + ImGui::InputInt("Simulation debug step", + &mSettings.debugModeStep); // set a int variable + mSettings.drawingStep = mSettings.debugModeStep; + ImGui::Checkbox("Enable drawing", + &mSettings.shouldDraw); // set a int variable + ImGui::Text("Number of simulation steps: %zu", mCurrentSimulationStep); + + ImGui::PopItemWidth(); + }); + calledOnce = true; + } + + if (!screenshotsFolder.empty()) { + static bool firstDraw = true; + if (firstDraw) { + for (const auto &entry : + std::filesystem::directory_iterator(screenshotsFolder)) + std::filesystem::remove_all(entry.path()); + // polyscope::view::processZoom(5); + firstDraw = false; + } + polyscope::screenshot( + std::filesystem::path(screenshotsFolder) + .append(std::to_string(mCurrentSimulationStep) + ".png") + .string(), + false); + } + polyscope::show(); +} +#endif + +void DRMSimulationModel::applySolutionGuess(const SimulationResults &solutionGuess, + const std::shared_ptr &pJob) +{ + assert(solutionGuess.displacements.size() == pMesh->VN() + && solutionGuess.rotationalDisplacementQuaternion.size() == pMesh->VN()); + + for (size_t vi = 0; vi < pMesh->VN(); vi++) { + Node &node = pMesh->nodes[vi]; + Eigen::Vector3d translationalDisplacements(solutionGuess.displacements[vi].getTranslation()); + if (!pJob->constrainedVertices.contains(vi) + || !pJob->constrainedVertices.at(vi).contains(0)) { + node.displacements[0] = translationalDisplacements[0]; + } + if (!pJob->constrainedVertices.contains(vi) + || !pJob->constrainedVertices.at(vi).contains(1)) { + node.displacements[1] = translationalDisplacements[1]; + } + if (!pJob->constrainedVertices.contains(vi) + || !pJob->constrainedVertices.at(vi).contains(2)) { + node.displacements[2] = translationalDisplacements[2]; + } + + updateNodePosition(pMesh->vert[vi], pJob->constrainedVertices); + } + + for (size_t vi = 0; vi < pMesh->VN(); vi++) { + Node &node = pMesh->nodes[vi]; + const Eigen::Vector3d nInitial_eigen = node.initialNormal.ToEigenVector(); + Eigen::Quaternion q; + Eigen::Vector3d eulerAngles = solutionGuess.displacements[vi].getRotation(); + q = Eigen::AngleAxisd(eulerAngles[0], Eigen::Vector3d::UnitX()) + * Eigen::AngleAxisd(eulerAngles[1], Eigen::Vector3d::UnitY()) + * Eigen::AngleAxisd(eulerAngles[2], Eigen::Vector3d::UnitZ()); + + Eigen::Vector3d nDeformed_eigen = (q * nInitial_eigen) /*.normalized()*/; + nDeformed_eigen.normalized(); + // Eigen::Vector3d n_groundOfTruth = solutionGuess.debug_q_normal[vi] * nInitial_eigen; + // n_groundOfTruth.normalized(); + if (!pJob->constrainedVertices.contains(vi) + || !pJob->constrainedVertices.at(vi).contains(3)) { + node.displacements[3] = (nDeformed_eigen - nInitial_eigen)[0]; + } + if (!pJob->constrainedVertices.contains(vi) + || !pJob->constrainedVertices.at(vi).contains(4)) { + node.displacements[4] = (nDeformed_eigen - nInitial_eigen)[1]; + } + updateNodeNormal(pMesh->vert[vi], pJob->constrainedVertices); + // const auto debug_rightNy = solutionGuess.debug_drmDisplacements[vi][4]; + + Eigen::Vector3d referenceT1_deformed = computeT1Vector(node.referenceElement->cP(0), + node.referenceElement->cP(1)) + .ToEigenVector(); + const Eigen::Vector3d referenceF1_deformed + = (referenceT1_deformed - (nInitial_eigen * (referenceT1_deformed.dot(nInitial_eigen)))) + .normalized(); + + const Eigen::Vector3d referenceT1_initial + = computeT1Vector(pMesh->nodes[node.referenceElement->cV(0)].initialLocation, + pMesh->nodes[node.referenceElement->cV(1)].initialLocation) + .ToEigenVector(); + // const VectorType &n_initial = node.initialNormal; + const Eigen::Vector3d referenceF1_initial + = (referenceT1_initial - (nInitial_eigen * (referenceT1_initial.dot(nInitial_eigen)))) + .normalized(); + Eigen::Quaternion q_f1; //nr is with respect to f1 + q_f1.setFromTwoVectors(referenceF1_initial, referenceF1_deformed); + Eigen::Quaternion q_normal; + q_normal.setFromTwoVectors(nInitial_eigen, nDeformed_eigen); + Eigen::Quaternion q_nr = q_f1.inverse() * q_normal.inverse() * q; + q_nr.w() = q_nr.w() >= 1 ? 1 : q_nr.w(); + q_nr.w() = q_nr.w() <= -1 ? -1 : q_nr.w(); + + const double nr_0To2pi_pos = 2 * std::acos(q_nr.w()); + // const double nr_0To2pi_groundOfTruth = 2 * std::acos(solutionGuess.debug_q_nr[vi].w()); + const double nr_0To2pi = nr_0To2pi_pos <= M_PI ? nr_0To2pi_pos : (nr_0To2pi_pos - 2 * M_PI); + Eigen::Vector3d deformedNormal_debug(q_nr.x() * sin(nr_0To2pi_pos / 2), + q_nr.y() * sin(nr_0To2pi_pos / 2), + q_nr.z() * sin(nr_0To2pi_pos / 2)); + /*deformedNormal_debug =*/deformedNormal_debug.normalize(); +#ifdef POLYSCOPE_DEFINED + if (std::isnan(deformedNormal_debug.norm())) { + std::cerr << "nr_0To2pi_pos:" << nr_0To2pi_pos << std::endl; + std::cerr << "q_nrx:" << q_nr.x() << std::endl; + std::cerr << "q_nry:" << q_nr.y() << std::endl; + std::cerr << "q_nrz:" << q_nr.z() << std::endl; + std::cerr << "q_nrw:" << q_nr.w() << std::endl; + std::cerr << "nan deformedNormal in guess" << std::endl; + } +#endif + const double nr = deformedNormal_debug.dot(nDeformed_eigen) > 0 ? nr_0To2pi : -nr_0To2pi; + if (!pJob->constrainedVertices.contains(vi) + || !pJob->constrainedVertices.at(vi).contains(5)) { + node.displacements[5] = nr; + } + // const double nr_asin = q_nr.x() + if (isRigidSupport[vi]) { + const EdgePointer &refElem = node.referenceElement; + const VectorType &refT1 = computeT1Vector(refElem->cP(0), refElem->cP(1)); + + const VectorType &t1Initial + = computeT1Vector(pMesh->nodes[refElem->cV(0)].initialLocation, + pMesh->nodes[refElem->cV(1)].initialLocation); + VectorType g1 = Cross(refT1, t1Initial); + node.nR = g1.dot(pMesh->vert[vi].cN()); + + } else { + node.displacements[5] = nr; + } + } + updateElementalFrames(); + + applyDisplacements(constrainedVertices); + if (!pJob->nodalForcedDisplacements.empty()) { + applyForcedDisplacements( + pJob->nodalForcedDisplacements); + } + updateElementalLengths(); + + // // registerWorldAxes(); + // Eigen::MatrixX3d framesX(pMesh->VN(), 3); + // Eigen::MatrixX3d framesY(pMesh->VN(), 3); + // Eigen::MatrixX3d framesZ(pMesh->VN(), 3); + // for (VertexIndex vi = 0; vi < pMesh->VN(); vi++) { + // Node &node = pMesh->nodes[vi]; + // Eigen::Vector3d translationalDisplacements(solutionGuess.displacements[vi].getTranslation()); + // node.displacements[0] = translationalDisplacements[0]; + // node.displacements[1] = translationalDisplacements[1]; + // node.displacements[2] = translationalDisplacements[2]; + // Eigen::Quaternion q; + // Eigen::Vector3d eulerAngles = solutionGuess.displacements[vi].getRotation(); + // q = Eigen::AngleAxisd(eulerAngles[0], Eigen::Vector3d::UnitX()) + // * Eigen::AngleAxisd(eulerAngles[1], Eigen::Vector3d::UnitY()) + // * Eigen::AngleAxisd(eulerAngles[2], Eigen::Vector3d::UnitZ()); + + // auto deformedNormal = q * Eigen::Vector3d(0, 0, 1); + // auto deformedFrameY = q * Eigen::Vector3d(0, 1, 0); + // auto deformedFrameX = q * Eigen::Vector3d(1, 0, 0); + // framesX.row(vi) = Eigen::Vector3d(deformedFrameX[0], deformedFrameX[1], deformedFrameX[2]); + // framesY.row(vi) = Eigen::Vector3d(deformedFrameY[0], deformedFrameY[1], deformedFrameY[2]); + // framesZ.row(vi) = Eigen::Vector3d(deformedNormal[0], deformedNormal[1], deformedNormal[2]); + // } + // polyscope::CurveNetwork *meshPolyscopeHandle = polyscope::getCurveNetwork(meshPolyscopeLabel); + // meshPolyscopeHandle->updateNodePositions(pMesh->getEigenVertices()); + + // //if(showFramesOn.contains(vi)){ + // auto polyscopeHandle_frameX = meshPolyscopeHandle->addNodeVectorQuantity("FrameX", framesX); + // polyscopeHandle_frameX->setVectorLengthScale(0.01); + // polyscopeHandle_frameX->setVectorRadius(0.01); + // polyscopeHandle_frameX->setVectorColor( + // /*polyscope::getNextUniqueColor()*/ glm::vec3(1, 0, 0)); + // auto polyscopeHandle_frameY = meshPolyscopeHandle->addNodeVectorQuantity("FrameY", framesY); + // polyscopeHandle_frameY->setVectorLengthScale(0.01); + // polyscopeHandle_frameY->setVectorRadius(0.01); + // polyscopeHandle_frameY->setVectorColor( + // /*polyscope::getNextUniqueColor()*/ glm::vec3(0, 1, 0)); + // auto polyscopeHandle_frameZ = meshPolyscopeHandle->addNodeVectorQuantity("FrameZ", framesZ); + // polyscopeHandle_frameZ->setVectorLengthScale(0.01); + // polyscopeHandle_frameZ->setVectorRadius(0.01); + // polyscopeHandle_frameZ->setVectorColor( + // /*polyscope::getNextUniqueColor()*/ glm::vec3(0, 0, 1)); + // //} + // polyscope::show(); +} + +SimulationResults DRMSimulationModel::executeSimulation(const std::shared_ptr &pJob, + const Settings &settings, + const SimulationResults &solutionGuess) +{ + assert(pJob->pMesh.operator bool()); + auto beginTime = std::chrono::high_resolution_clock::now(); + mSettings = settings; + reset(); + + history.label = pJob->pMesh->getLabel() + "_" + pJob->getLabel(); + + // if (!pJob->nodalExternalForces.empty()) { + // double externalForcesNorm = 0; + // for (const auto &externalForce : pJob->nodalExternalForces) { + // externalForcesNorm += externalForce.second.norm(); + // } + // mSettings.totalResidualForcesNormThreshold = externalForcesNorm * 1e-2; + // } + + constrainedVertices = pJob->constrainedVertices; + if (!pJob->nodalForcedDisplacements.empty()) { + for (std::pair viDisplPair : pJob->nodalForcedDisplacements) { + const VertexIndex vi = viDisplPair.first; + constrainedVertices[vi].insert({0, 1, 2}); + } + } + // if (!pJob->nodalForcedNormals.empty()) { + // for (std::pair viNormalPair : + // pJob->nodalForcedDisplacements) { + // assert(viNormalPair3second[2] >= 0); + // } + // } + + pMesh.reset(); + pMesh = std::make_unique(*pJob->pMesh); + if (mSettings.beVerbose) { + std::cout << "Executing simulation for mesh with:" << pMesh->VN() << " nodes and " + << pMesh->EN() << " elements." << std::endl; + } + vcg::tri::UpdateBounding::Box(*pMesh); + computeRigidSupports(); + isVertexConstrained.resize(pMesh->VN(), false); +<<<<<<< HEAD + for (auto fixedVertex : pJob->constrainedVertices) { +======= + for (auto fixedVertex : constrainedVertices) { +>>>>>>> master + isVertexConstrained[fixedVertex.first] = true; + } + +#ifdef POLYSCOPE_DEFINED + if (mSettings.shouldDraw || mSettings.isDebugMode) { + PolyscopeInterface::init(); + polyscope::registerCurveNetwork(meshPolyscopeLabel, + pMesh->getEigenVertices(), + pMesh->getEigenEdges()); + polyscope::registerCurveNetwork("Initial_" + meshPolyscopeLabel, + pMesh->getEigenVertices(), + pMesh->getEigenEdges()) + ->setRadius(0.002); + // registerWorldAxes(); + } +#endif + + if (!pJob->nodalForcedDisplacements.empty() && pJob->nodalExternalForces.empty()) { + shouldApplyInitialDistortion = true; + } + updateElementalFrames(); + if (!solutionGuess.displacements.empty()) { + //#ifdef POLYSCOPE_DEFINED + // if (mSettings.shouldDraw || mSettings.isDebugMode) { + // solutionGuess.registerForDrawing(); + // polyscope::show(); + // solutionGuess.unregister(); + // } + //#endif + + applySolutionGuess(solutionGuess, pJob); + shouldTemporarilyDampForces = true; + // Dt = mSettings.Dtini * 0.9; + } + + updateNodalMasses(); + std::unordered_map nodalExternalForces = pJob->nodalExternalForces; + double totalExternalForcesNorm = 0; + // Vector6d sumOfExternalForces(0); + for (auto &nodalForce : nodalExternalForces) { + const double percentageOfExternalLoads = double(externalLoadStep) + / mSettings.desiredGradualExternalLoadsSteps; + nodalForce.second = nodalForce.second * percentageOfExternalLoads; + totalExternalForcesNorm += nodalForce.second.norm(); + // sumOfExternalForces = sumOfExternalForces + nodalForce.second; + } + pMesh->totalExternalForcesNorm = totalExternalForcesNorm; + updateNodalExternalForces(nodalExternalForces, constrainedVertices); + if (!nodalExternalForces.empty()) { + mSettings.totalResidualForcesNormThreshold + = settings.totalExternalForcesNormPercentageTermination * totalExternalForcesNorm; + } else { + mSettings.totalResidualForcesNormThreshold = 1e-3; + std::cout << "No forces setted default residual forces norm threshold" << std::endl; + } + if (mSettings.beVerbose) { + std::cout << "totalResidualForcesNormThreshold:" + << mSettings.totalResidualForcesNormThreshold << std::endl; + if (mSettings.useAverage) { + std::cout << "average/extNorm threshold:" + << mSettings.averageResidualForcesCriterionThreshold << std::endl; + } + } + const size_t movingAverageSampleSize = 200; + std::queue residualForcesMovingAverageHistorySample; + std::vector percentageOfConvergence; + + // double residualForcesMovingAverageDerivativeMax = 0; + while (settings.maxDRMIterations == 0 || mCurrentSimulationStep < settings.maxDRMIterations) { + if ((mSettings.isDebugMode && mCurrentSimulationStep == 50000)) { + // std::filesystem::create_directory("./PatternOptimizationNonConv"); + // pJob->save("./PatternOptimizationNonConv"); + // Dt = mSettings.Dtini; + } + // if (mCurrentSimulationStep == 500 && shouldTemporarilyDampForces) { + // Dt = mSettings.Dtini; + // } + // while (true) { + updateNormalDerivatives(); + updateT1Derivatives(); + updateRDerivatives(); + updateT2Derivatives(); + updateT3Derivatives(); + const bool shouldBreak = mCurrentSimulationStep == 12970; + updateResidualForcesOnTheFly(constrainedVertices); + + // TODO: write parallel function for updating positions + // TODO: Make a single function out of updateResidualForcesOnTheFly + // updatePositionsOnTheFly + // updatePositionsOnTheFly(constrainedVertices); + updateNodalMasses(); + updateNodalAccelerations(); + updateNodalVelocities(); + updateNodalDisplacements(); + applyDisplacements(constrainedVertices); + if (!pJob->nodalForcedDisplacements.empty()) { + applyForcedDisplacements( + + pJob->nodalForcedDisplacements); + } + // if (!pJob->nodalForcedNormals.empty()) { + // applyForcedNormals(pJob->nodalForcedNormals); + // } + updateElementalLengths(); + mCurrentSimulationStep++; + if (std::isnan(pMesh->currentTotalKineticEnergy)) { + std::cout << pMesh->currentTotalKineticEnergy << std::endl; + if (mSettings.beVerbose) { + std::cerr << "Infinite kinetic energy at step " << mCurrentSimulationStep + << ". Exiting.." << std::endl; + } + break; + } + + //normalized sum of displacements + // double sumOfDisplacementsNorm = 0; + // for (size_t vi = 0; vi < pMesh->VN(); vi++) { + // sumOfDisplacementsNorm += pMesh->nodes[vi].displacements.getTranslation().norm(); + // } + // sumOfDisplacementsNorm /= pMesh->bbox.Diag(); + // pMesh->sumOfNormalizedDisplacementNorms = sumOfDisplacementsNorm; + + //moving average + // // pMesh->residualForcesMovingAverage = (pMesh->totalResidualForcesNorm + // // + mCurrentSimulationStep + // // * pMesh->residualForcesMovingAverage) + // // / (1 + mCurrentSimulationStep); + pMesh->residualForcesMovingAverage += pMesh->totalResidualForcesNorm + / movingAverageSampleSize; + residualForcesMovingAverageHistorySample.push(pMesh->residualForcesMovingAverage); + if (movingAverageSampleSize < residualForcesMovingAverageHistorySample.size()) { + const double firstElementValue = residualForcesMovingAverageHistorySample.front(); + pMesh->residualForcesMovingAverage -= firstElementValue / movingAverageSampleSize; + residualForcesMovingAverageHistorySample.pop(); + + // pMesh->residualForcesMovingAverageDerivativeNorm + // = std::abs((residualForcesMovingAverageHistorySample.back() + // - residualForcesMovingAverageHistorySample.front())) + // / (movingAverageSampleSize); + // residualForcesMovingAverageDerivativeMax + // = std::max(pMesh->residualForcesMovingAverageDerivativeNorm, + // residualForcesMovingAverageDerivativeMax); + // pMesh->residualForcesMovingAverageDerivativeNorm + // /= residualForcesMovingAverageDerivativeMax; + // // std::cout << "Normalized derivative:" + // // << residualForcesMovingAverageDerivativeNorm + // // << std::endl; + } + + // pMesh->previousTotalPotentialEnergykN = + // pMesh->currentTotalPotentialEnergykN; + // pMesh->currentTotalPotentialEnergykN = computeTotalPotentialEnergy() / 1000; + // timePerNodePerIterationHistor.push_back(timePerNodePerIteration); + if (mSettings.beVerbose && mSettings.isDebugMode + && mCurrentSimulationStep % mSettings.debugModeStep == 0) { + printCurrentState(); + + // auto t2 = std::chrono::high_resolution_clock::now(); + // const double executionTime_min + // = std::chrono::duration_cast(t2 - beginTime).count(); + // std::cout << "Execution time(min):" << executionTime_min << std::endl; + if (mSettings.useAverage) { + std::cout << "Best percentage of target (average):" + << 100 * mSettings.averageResidualForcesCriterionThreshold + * totalExternalForcesNorm + / (minTotalResidualForcesNorm / pMesh->VN()) + << "%" << std::endl; + } + std::cout << "Best percentage of target:" + << 100 * mSettings.totalExternalForcesNormPercentageTermination + * totalExternalForcesNorm / minTotalResidualForcesNorm + << "%" << std::endl; + // SimulationResultsReporter::createPlot("Number of Steps", + // "Residual Forces mov aver", + // history.residualForcesMovingAverage); + // SimulationResultsReporter::createPlot("Number of Steps", + // "Residual Forces mov aver deriv", + // movingAveragesDerivatives); + // draw(); + // SimulationResulnodalForcedDisplacementstsReporter::createPlot("Number of Steps", + // "Time/(#nodes*#iterations)", + // timePerNodePerIterationHistory); + } + + if ((mSettings.shouldCreatePlots || mSettings.isDebugMode) && mCurrentSimulationStep != 0) { + history.stepPulse(*pMesh); + percentageOfConvergence.push_back(100 * mSettings.totalResidualForcesNormThreshold + / pMesh->totalResidualForcesNorm); + } + + if (mSettings.shouldCreatePlots && mSettings.isDebugMode + && mCurrentSimulationStep % mSettings.debugModeStep == 0) { + // SimulationResultsReporter::createPlot("Number of Steps", + // "Residual Forces mov aver deriv", + // movingAveragesDerivatives_norm); + // SimulationResultsReporter::createPlot("Number of Steps", + // "Residual Forces mov aver", + // history.residualForcesMovingAverage); + // SimulationResultsReporter::createPlot("Number of Steps", + // "Log of Residual Forces", + // history.logResidualForces, + // {}, + // history.redMarks); + SimulationResultsReporter::createPlot("Number of Steps", + "Log of Kinetic energy", + history.kineticEnergy, + {}, + history.redMarks); + // SimulationResultsReporter reporter; + // reporter.reportHistory(history, "IntermediateResults", pJob->pMesh->getLabel()); + // SimulationResultsReporter::createPlot("Number of Iterations", + // "Sum of normalized displacement norms", + // history.sumOfNormalizedDisplacementNorms /*, + // std::filesystem::path("./") + // .append("SumOfNormalizedDisplacementNorms_" + graphSuffix + ".png") + // .string()*/ + // , + // {}, + // history.redMarks); + // SimulationResultsReporter::createPlot("Number of Steps", + // "Percentage of convergence", + // percentageOfConvergence, + // {}, + // history.redMarks); + } + +#ifdef POLYSCOPE_DEFINED + if (mSettings.shouldDraw && mSettings.isDebugMode + && mCurrentSimulationStep % mSettings.drawingStep == 0) /* && + +currentSimulationStep > maxDRMIterations*/ + { + // std::string saveTo = std::filesystem::current_path() + // .append("Debugging_files") + // .append("Screenshots") + // .string(); + draw(); + // yValues = history.kineticEnergy; + // plotHandle = matplot::scatter(xPlot, yValues); + // label = "Log of Kinetic energy"; + // plotHandle->legend_string(label); + + // shouldUseKineticEnergyThreshold = true; + } +#endif + + // t = t + Dt; + // std::cout << "Kinetic energy:" << mesh.currentTotalKineticEnergy + // << std::endl; + // std::cout << "Residual forces norm:" << mesh.totalResidualForcesNorm + // << std::endl; + const bool fullfillsResidualForcesNormTerminationCriterion + = !mSettings.useAverage + && pMesh->totalResidualForcesNorm / totalExternalForcesNorm + < mSettings.totalExternalForcesNormPercentageTermination; + const bool fullfillsAverageResidualForcesNormTerminationCriterion + = mSettings.useAverage + && (pMesh->totalResidualForcesNorm / pMesh->VN()) / totalExternalForcesNorm + < mSettings.averageResidualForcesCriterionThreshold; +<<<<<<< HEAD + if (fullfillsResidualForcesNormTerminationCriterion + || fullfillsResidualForcesNormTerminationCriterion) { + if (mSettings.beVerbose /*&& !mSettings.isDebugMode*/) { + std::cout << "Simulation converged." << std::endl; + printCurrentState(); + } + break; + } + // Residual forces norm convergence + if ((mSettings.useKineticDamping) + && (pMesh->previousTotalKineticEnergy > pMesh->currentTotalKineticEnergy + + // && mCurrentSimulationStep > movingAverageSampleSize + && (pJob->nodalForcedDisplacements.empty() + || mCurrentSimulationStep > mSettings.gradualForcedDisplacementSteps)) +======= + // Residual forces norm convergence + if (((pMesh->previousTotalKineticEnergy > pMesh->currentTotalKineticEnergy + || fullfillsAverageResidualForcesNormTerminationCriterion + || fullfillsResidualForcesNormTerminationCriterion) + // && mCurrentSimulationStep > movingAverageSampleSize + && (pJob->nodalForcedDisplacements.empty() + || mCurrentSimulationStep > mSettings.gradualForcedDisplacementSteps)) +>>>>>>> master + /* || (pMesh->totalResidualForcesNorm / mSettings.totalResidualForcesNormThreshold <= 1 + && mCurrentSimulationStep > 1)*/ + /*|| +mesh->previousTotalPotentialEnergykN > +mesh->currentTotalPotentialEnergykN*/ + /*|| mesh->currentTotalPotentialEnergykN < minPotentialEnergy*/) { + // if (pMesh->totalResidualForcesNorm < totalResidualForcesNormThreshold) { + const bool fullfillsKineticEnergyTerminationCriterion + = mSettings.shouldUseTranslationalKineticEnergyThreshold + && pMesh->currentTotalTranslationalKineticEnergy + < mSettings.totalTranslationalKineticEnergyThreshold + && mCurrentSimulationStep > 20 && numOfDampings > 0; + const bool fullfillsMovingAverageNormTerminationCriterion + = pMesh->residualForcesMovingAverage + < mSettings.residualForcesMovingAverageNormThreshold; + /*pMesh->residualForcesMovingAverage < totalResidualForcesNormThreshold*/ + const bool fullfillsMovingAverageDerivativeNormTerminationCriterion + = pMesh->residualForcesMovingAverageDerivativeNorm < 1e-4; + const bool shouldTerminate + = fullfillsKineticEnergyTerminationCriterion + // || fullfillsMovingAverageNormTerminationCriterion + // || fullfillsMovingAverageDerivativeNormTerminationCriterion + || fullfillsAverageResidualForcesNormTerminationCriterion + || fullfillsResidualForcesNormTerminationCriterion; + if (shouldTerminate && mCurrentSimulationStep > 100) { + if (mSettings.beVerbose /*&& !mSettings.isDebugMode*/) { + std::cout << "Simulation converged." << std::endl; + printCurrentState(); + if (fullfillsResidualForcesNormTerminationCriterion) { + std::cout << "Converged using residual forces norm threshold criterion" + << std::endl; + } else if (fullfillsKineticEnergyTerminationCriterion) { + std::cout << "The kinetic energy of the system was " + " used as a convergence criterion" + << std::endl; + } else if (fullfillsMovingAverageNormTerminationCriterion) { + std::cout + << "Converged using residual forces moving average derivative norm " + "threshold criterion" + << std::endl; + } + } + if (mSettings.desiredGradualExternalLoadsSteps == externalLoadStep) { + break; + } else { + externalLoadStep++; + std::unordered_map nodalExternalForces + = pJob->nodalExternalForces; + double totalExternalForcesNorm = 0; + for (auto &nodalForce : nodalExternalForces) { + const double percentageOfExternalLoads + = double(externalLoadStep) / mSettings.desiredGradualExternalLoadsSteps; + nodalForce.second = nodalForce.second * percentageOfExternalLoads; + totalExternalForcesNorm += nodalForce.second.norm(); + } + updateNodalExternalForces(nodalExternalForces, constrainedVertices); + + if (!nodalExternalForces.empty()) { + mSettings.totalResidualForcesNormThreshold = 1e-2 * totalExternalForcesNorm; + } + if (mSettings.beVerbose) { + std::cout << "totalResidualForcesNormThreshold:" + << mSettings.totalResidualForcesNormThreshold << std::endl; + } + } + // } + } + +<<<<<<< HEAD + if (!mSettings.useViscousDamping) { + const bool shouldCapDisplacements = mSettings.displacementCap.has_value(); + for (VertexType &v : pMesh->vert) { + Node &node = pMesh->nodes[v]; + Vector6d stepDisplacement = node.velocity * 0.5 * Dt; + if (shouldCapDisplacements + && stepDisplacement.getTranslation().norm() > mSettings.displacementCap) { + stepDisplacement = stepDisplacement + * (*mSettings.displacementCap + / stepDisplacement.getTranslation().norm()); + } + node.displacements = node.displacements - stepDisplacement; + } + applyDisplacements(constrainedVertices); + if (!pJob->nodalForcedDisplacements.empty()) { + applyForcedDisplacements( + + pJob->nodalForcedDisplacements); + } + updateElementalLengths(); + } +======= + const bool shouldCapDisplacements = mSettings.displacementCap.has_value(); + for (VertexType &v : pMesh->vert) { + Node &node = pMesh->nodes[v]; + Vector6d stepDisplacement = node.velocity * 0.5 * Dt; + if (shouldCapDisplacements + && stepDisplacement.getTranslation().norm() > mSettings.displacementCap) { + stepDisplacement = stepDisplacement + * (*mSettings.displacementCap + / stepDisplacement.getTranslation().norm()); + } + node.displacements = node.displacements - stepDisplacement; + } + applyDisplacements(constrainedVertices); + if (!pJob->nodalForcedDisplacements.empty()) { + applyForcedDisplacements( + + pJob->nodalForcedDisplacements); + } + updateElementalLengths(); +>>>>>>> master + + // const double triggerPercentage = 0.01; + // const double xi_min = 0.55; + // const double xi_init = 0.9969; + // if (mSettings.totalResidualForcesNormThreshold / pMesh->totalResidualForcesNorm + // > triggerPercentage) { + // mSettings.xi = ((xi_min - xi_init) + // * (mSettings.totalResidualForcesNormThreshold + // / pMesh->totalResidualForcesNorm) + // + xi_init - triggerPercentage * xi_min) + // / (1 - triggerPercentage); + // } +<<<<<<< HEAD + if (mSettings.useKineticDamping) { + resetVelocities(); + } +======= + resetVelocities(); +>>>>>>> master + Dt *= mSettings.xi; + // if (mSettings.isDebugMode) { + // std::cout << Dt << std::endl; + // } + ++numOfDampings; + if (mSettings.shouldCreatePlots) { + history.redMarks.push_back(mCurrentSimulationStep); + } + // std::cout << "Number of dampings:" << numOfDampings << endl; + // draw(); + } + } + auto endTime = std::chrono::high_resolution_clock::now(); + + SimulationResults results = computeResults(pJob); + results.executionTime = std::chrono::duration_cast(endTime - beginTime) + .count(); + + if (mCurrentSimulationStep == settings.maxDRMIterations && mCurrentSimulationStep != 0) { + if (mSettings.beVerbose) { + std::cout << "Did not reach equilibrium before reaching the maximum number " + "of DRM steps (" + << settings.maxDRMIterations << "). Breaking simulation" << std::endl; + } + results.converged = false; + } else if (std::isnan(pMesh->currentTotalKineticEnergy)) { + if (mSettings.beVerbose) { + std::cerr << "Simulation did not converge due to infinite kinetic energy." << std::endl; + } + results.converged = false; + } + // mesh.printVertexCoordinates(mesh.VN() / 2); +#ifdef POLYSCOPE_DEFINED + if (mSettings.shouldDraw && !mSettings.isDebugMode) { + draw(); + } +#endif + if (!std::isnan(pMesh->currentTotalKineticEnergy)) { + results.debug_drmDisplacements = results.displacements; + results.internalPotentialEnergy = computeTotalInternalPotentialEnergy(); + + results.rotationalDisplacementQuaternion.resize(pMesh->VN()); + results.debug_q_f1.resize(pMesh->VN()); + results.debug_q_normal.resize(pMesh->VN()); + results.debug_q_nr.resize(pMesh->VN()); + for (int vi = 0; vi < pMesh->VN(); vi++) { + const Node &node = pMesh->nodes[vi]; + const Eigen::Vector3d nInitial_eigen = node.initialNormal + .ToEigenVector(); + const Eigen::Vector3d nDeformed_eigen + = pMesh->vert[vi].cN().ToEigenVector(); + + Eigen::Quaternion q_normal; + q_normal.setFromTwoVectors(nInitial_eigen, nDeformed_eigen); + Eigen::Quaternion q_nr_nDeformed; + q_nr_nDeformed = Eigen::AngleAxis(pMesh->nodes[vi].nR, nDeformed_eigen); + Eigen::Quaternion q_nr_nInit; + q_nr_nInit = Eigen::AngleAxis(pMesh->nodes[vi].nR, nInitial_eigen); + const auto w = q_nr_nDeformed.w(); + const auto theta = 2 * acos(q_nr_nDeformed.w()); + const auto nr = pMesh->nodes[vi].nR; + Eigen::Vector3d deformedNormal_debug(q_nr_nDeformed.x() * sin(theta / 2), + q_nr_nDeformed.y() * sin(theta / 2), + q_nr_nDeformed.z() * sin(theta / 2)); + deformedNormal_debug.normalize(); + // const double nr = nr_0To2pi <= M_PI ? nr_0To2pi : (nr_0To2pi - 2 * M_PI); + const double nr_debug = deformedNormal_debug.dot(nDeformed_eigen) > 0 ? theta : -theta; + assert(pMesh->nodes[vi].nR - nr_debug < 1e-6); + VectorType referenceT1_deformed = pMesh->elements[node.referenceElement].frame.t1; + const VectorType &nDeformed = pMesh->vert[vi].cN(); + const VectorType referenceF1_deformed + = (referenceT1_deformed + - (node.initialNormal * (referenceT1_deformed * node.initialNormal))) + .Normalize(); + + const VectorType referenceT1_initial + = computeT1Vector(pMesh->nodes[node.referenceElement->cV(0)].initialLocation, + pMesh->nodes[node.referenceElement->cV(1)].initialLocation); + // const VectorType &n_initial = node.initialNormal; + const VectorType referenceF1_initial = (referenceT1_initial + - (node.initialNormal + * (referenceT1_initial * node.initialNormal))) + .Normalize(); + Eigen::Quaternion q_f1_nInit; //nr is with respect to f1 + q_f1_nInit.setFromTwoVectors(referenceF1_initial.ToEigenVector(), + referenceF1_deformed.ToEigenVector()); + + Eigen::Quaternion q_f1_nDeformed; //nr is with respect to f1 + // const VectorType &n_initial = node.initialNormal; + const VectorType referenceF1_initial_def + = (referenceT1_initial - (nDeformed * (referenceT1_initial * nDeformed))).Normalize(); + const VectorType referenceF1_deformed_def = (referenceT1_deformed + - (nDeformed + * (referenceT1_deformed * nDeformed))) + .Normalize(); + q_f1_nDeformed + .setFromTwoVectors(referenceF1_initial_def.ToEigenVector(), + referenceF1_deformed_def.ToEigenVector()); + const auto debug_qf1_nDef = (q_f1_nDeformed * q_nr_nDeformed) * nDeformed_eigen; + const auto debug_qf1_nInit = (q_f1_nInit * q_nr_nInit) * nInitial_eigen; + const auto debug_deformedNormal_f1Init = (q_normal * (q_f1_nInit * q_nr_nInit)) + * nInitial_eigen; + const auto debug_deformedNormal_f1Def = ((q_nr_nDeformed * q_f1_nDeformed) * q_normal) + * nInitial_eigen; + // Eigen::Quaternion q_t1; + // q_t1.setFromTwoVectors(referenceT1_initial.ToEigenVector(), + // referenceT1_deformed.ToEigenVector()); + + results.debug_q_f1[vi] = q_f1_nInit; + results.debug_q_normal[vi] = q_normal; + results.debug_q_nr[vi] = q_nr_nInit; + results.rotationalDisplacementQuaternion[vi] + = (q_normal + * (q_f1_nInit * q_nr_nInit)); //q_f1_nDeformed * q_nr_nDeformed * q_normal; + //Update the displacement vector to contain the euler angles + const Eigen::Vector3d eulerAngles = results.rotationalDisplacementQuaternion[vi] + .toRotationMatrix() + .eulerAngles(0, 1, 2); + results.displacements[vi][3] = eulerAngles[0]; + results.displacements[vi][4] = eulerAngles[1]; + results.displacements[vi][5] = eulerAngles[2]; + + // Eigen::Quaterniond q_test = Eigen::AngleAxisd(eulerAngles[0], Eigen::Vector3d::UnitX()) + // * Eigen::AngleAxisd(eulerAngles[1], Eigen::Vector3d::UnitY()) + // * Eigen::AngleAxisd(eulerAngles[2], Eigen::Vector3d::UnitZ()); + + // const double dot_test = results.rotationalDisplacementQuaternion[vi].dot(q_test); + // assert(dot_test > 1 - 1e5); + + int i = 0; + i++; + } + } + + if (!mSettings.isDebugMode && mSettings.shouldCreatePlots) { + SimulationResultsReporter reporter; + reporter.reportResults({results}, "Results", pJob->pMesh->getLabel()); + } + +#ifdef POLYSCOPE_DEFINED + if (mSettings.shouldDraw || mSettings.isDebugMode) { + polyscope::removeCurveNetwork(meshPolyscopeLabel); + polyscope::removeCurveNetwork("Initial_" + meshPolyscopeLabel); + } +#endif + + return results; +} diff --git a/drmsimulationmodel.hpp b/drmsimulationmodel.hpp index 7e1d6d8..e4ca03b 100755 --- a/drmsimulationmodel.hpp +++ b/drmsimulationmodel.hpp @@ -25,30 +25,32 @@ class DRMSimulationModel public: struct Settings { - bool isDebugMode{false}; - int debugModeStep{100000}; + // bool isDebugMode{false}; + std::optional debugModeStep{0}; bool shouldDraw{false}; bool beVerbose{false}; bool shouldCreatePlots{false}; - int drawingStep{1}; - double totalTranslationalKineticEnergyThreshold{1e-8}; - double residualForcesMovingAverageDerivativeNormThreshold{1e-8}; - double residualForcesMovingAverageNormThreshold{1e-8}; + // int drawingStep{0}; + // double residualForcesMovingAverageDerivativeNormThreshold{1e-8}; + // double residualForcesMovingAverageNormThreshold{1e-8}; double Dtini{0.1}; double xi{0.9969}; int maxDRMIterations{0}; - bool shouldUseTranslationalKineticEnergyThreshold{false}; - int gradualForcedDisplacementSteps{50}; - int desiredGradualExternalLoadsSteps{1}; + std::optional shouldUseTranslationalKineticEnergyThreshold; + // int gradualForcedDisplacementSteps{50}; + // int desiredGradualExternalLoadsSteps{1}; double gamma{0.8}; std::optional displacementCap; double totalResidualForcesNormThreshold{1e-3}; double totalExternalForcesNormPercentageTermination{1e-3}; - bool useAverage{false}; - double averageResidualForcesCriterionThreshold{1e-5}; + std::optional averageResidualForcesCriterionThreshold{1e-5}; Settings() {} + bool save(const std::filesystem::path &folderPath) const; + bool load(const std::filesystem::path &filePath) const; bool useViscousDamping{false}; bool useKineticDamping{true}; + struct JsonLabels + {}; }; private: diff --git a/drmsimulationmodel.hpp.orig b/drmsimulationmodel.hpp.orig new file mode 100755 index 0000000..16adac2 --- /dev/null +++ b/drmsimulationmodel.hpp.orig @@ -0,0 +1,254 @@ +#ifndef BEAMFORMFINDER_HPP +#define BEAMFORMFINDER_HPP + +#include "simulationmesh.hpp" +#include "matplot/matplot.h" +#include "simulation_structs.hpp" +#include +#include +#include + +struct SimulationJob; + +enum DoF { Ux = 0, Uy, Uz, Nx, Ny, Nr, NumDoF }; +using DoFType = int; +using EdgeType = VCGEdgeMesh::EdgeType; +using VertexType = VCGEdgeMesh::VertexType; + +struct DifferentiateWithRespectTo { + const VertexType &v; + const DoFType &dofi; +}; + +class DRMSimulationModel +{ +public: + struct Settings + { + bool isDebugMode{false}; + int debugModeStep{100000}; + bool shouldDraw{false}; + bool beVerbose{false}; + bool shouldCreatePlots{false}; + int drawingStep{1}; + double totalTranslationalKineticEnergyThreshold{1e-8}; + double residualForcesMovingAverageDerivativeNormThreshold{1e-8}; + double residualForcesMovingAverageNormThreshold{1e-8}; + double Dtini{0.1}; + double xi{0.9969}; + int maxDRMIterations{0}; + bool shouldUseTranslationalKineticEnergyThreshold{false}; + int gradualForcedDisplacementSteps{50}; + int desiredGradualExternalLoadsSteps{1}; + double gamma{0.8}; + std::optional displacementCap; + double totalResidualForcesNormThreshold{1e-3}; + double totalExternalForcesNormPercentageTermination{1e-3}; + bool useAverage{false}; + double averageResidualForcesCriterionThreshold{1e-5}; + Settings() {} + bool useViscousDamping{false}; + bool useKineticDamping{true}; + }; + +private: + Settings mSettings; + double Dt{mSettings.Dtini}; + bool checkedForMaximumMoment{false}; + bool shouldTemporarilyDampForces{false}; + bool shouldTemporarilyAmplifyForces{true}; + double externalMomentsNorm{0}; + size_t mCurrentSimulationStep{0}; + matplot::line_handle plotHandle; + std::vector plotYValues; + size_t numOfDampings{0}; + int externalLoadStep{1}; +<<<<<<< HEAD + const double viscuousDampingConstant{100}; +======= +>>>>>>> master + std::vector isVertexConstrained; + std::vector isRigidSupport; + double minTotalResidualForcesNorm{std::numeric_limits::max()}; + + const std::string meshPolyscopeLabel{"Simulation mesh"}; + std::unique_ptr pMesh; + std::unordered_map> constrainedVertices; + SimulationHistory history; + // Eigen::Tensor theta3Derivatives; + // std::unordered_map theta3Derivatives; + bool shouldApplyInitialDistortion = false; + + void reset(); + void updateNodalInternalForces( + const std::unordered_map> &fixedVertices); + void updateNodalExternalForces( + const std::unordered_map &nodalForces, + const std::unordered_map> &fixedVertices); + void updateResidualForces(); + void updateRotationalDisplacements(); + void updateElementalLengths(); + + void updateNodalMasses(); + + void updateNodalAccelerations(); + + void updateNodalVelocities(); + + void updateNodalDisplacements(); + + void applyForcedDisplacements( + const std::unordered_map &nodalForcedDisplacements); + + Vector6d computeElementTorsionalForce(const Element &element, + const Vector6d &displacementDifference, + const std::unordered_set &constrainedDof); + + // BeamFormFinder::Vector6d computeElementFirstBendingForce( + // const Element &element, const Vector6d &displacementDifference, + // const std::unordered_set &constrainedDof); + + // BeamFormFinder::Vector6d computeElementSecondBendingForce( + // const Element &element, const Vector6d &displacementDifference, + // const std::unordered_set &constrainedDof); + + void updateKineticEnergy(); + + void resetVelocities(); + + SimulationResults computeResults(const std::shared_ptr &pJob); + + void updateNodePosition( + VertexType &v, + const std::unordered_map> &fixedVertices); + + void applyDisplacements( + const std::unordered_map> &fixedVertices); + +#ifdef POLYSCOPE_DEFINED + void draw(const string &screenshotsFolder= {}); +#endif + void + updateNodalInternalForce(Vector6d &nodalInternalForce, + const Vector6d &elementInternalForce, + const std::unordered_set &nodalFixedDof); + + Vector6d computeElementInternalForce( + const Element &elem, const Node &n0, const Node &n1, + const std::unordered_set &n0ConstrainedDof, + const std::unordered_set &n1ConstrainedDof); + + Vector6d computeElementAxialForce(const ::EdgeType &e) const; + VectorType computeDisplacementDifferenceDerivative( + const EdgeType &e, const DifferentiateWithRespectTo &dui) const; + double + computeDerivativeElementLength(const EdgeType &e, + const DifferentiateWithRespectTo &dui) const; + + VectorType computeDerivativeT1(const EdgeType &e, + const DifferentiateWithRespectTo &dui) const; + + VectorType + computeDerivativeOfNormal(const VertexType &v, + const DifferentiateWithRespectTo &dui) const; + + VectorType computeDerivativeT3(const EdgeType &e, + const DifferentiateWithRespectTo &dui) const; + + VectorType computeDerivativeT2(const EdgeType &e, + const DifferentiateWithRespectTo &dui) const; + + double computeDerivativeTheta2(const EdgeType &e, const VertexIndex &evi, + const VertexIndex &dwrt_evi, + const DoFType &dwrt_dofi) const; + + void updateElementalFrames(); + + VectorType computeDerivativeOfR(const EdgeType &e, const DifferentiateWithRespectTo &dui) const; + + // bool isRigidSupport(const VertexType &v) const; + + static double computeDerivativeOfNorm(const VectorType &x, + const VectorType &derivativeOfX); + static VectorType computeDerivativeOfCrossProduct( + const VectorType &a, const VectorType &derivativeOfA, const VectorType &b, + const VectorType &derivativeOfB); + + double computeTheta3(const EdgeType &e, const VertexType &v); + double computeDerivativeTheta3(const EdgeType &e, const VertexType &v, + const DifferentiateWithRespectTo &dui) const; + double computeTotalPotentialEnergy(); + void computeRigidSupports(); + void updateNormalDerivatives(); + void updateT1Derivatives(); + void updateT2Derivatives(); + void updateT3Derivatives(); + void updateRDerivatives(); + + double computeDerivativeTheta1(const EdgeType &e, const VertexIndex &evi, + const VertexIndex &dwrt_evi, + const DoFType &dwrt_dofi) const; + + // void updatePositionsOnTheFly( + // const std::unordered_map> + // &fixedVertices); + + void updateResidualForcesOnTheFly( + const std::unordered_map> + &fixedVertices); + + void updatePositionsOnTheFly( + const std::unordered_map> + &fixedVertices); + + void updateNodeNormal( + VertexType &v, + const std::unordered_map> + &fixedVertices); + + void applyForcedNormals( + const std::unordered_map nodalForcedRotations); + + void printCurrentState() const; + + void printDebugInfo() const; + + double computeTotalInternalPotentialEnergy(); + + void applySolutionGuess(const SimulationResults &solutionGuess, + const std::shared_ptr &pJob); + + void updateNodeNr(VertexType &v); + + public: + DRMSimulationModel(); + SimulationResults executeSimulation(const std::shared_ptr &pJob, + const Settings &settings = Settings(), + const SimulationResults &solutionGuess = SimulationResults()); + + static void runUnitTests(); +}; + +template PointType Cross(PointType p1, PointType p2) { + return p1 ^ p2; +} + +inline size_t currentStep{0}; +inline bool TriggerBreakpoint(const VertexIndex &vi, const EdgeIndex &ei, + const DoFType &dofi) { + const size_t numberOfVertices = 10; + const VertexIndex middleNodeIndex = numberOfVertices / 2; + // return vi == middleNodeIndex && dofi == 1; + return dofi == 1 && ((vi == 1 && ei == 0) || (vi == 9 && ei == 9)); +} + +inline bool TriggerBreakpoint(const VertexIndex &vi, const EdgeIndex &ei) { + const size_t numberOfVertices = 10; + const VertexIndex middleNodeIndex = numberOfVertices / 2; + return (vi == middleNodeIndex); + // return (vi == 0 || vi == numberOfVertices - 1) && currentStep == 1; + return (vi == 1 && ei == 0) || (vi == 9 && ei == 9); +} + +#endif // BEAMFORMFINDER_HPP diff --git a/topologyenumerator.cpp b/topologyenumerator.cpp index dba4356..d62a085 100755 --- a/topologyenumerator.cpp +++ b/topologyenumerator.cpp @@ -191,7 +191,8 @@ void TopologyEnumerator::computeValidPatterns(const std::vector &reduced patternGeometryAllEdges.getVertices(), intersectingEdges, validEdges); - // statistics.print(setupString, perEdgeResultPath); + statistics.print(setupString, perEdgeResultPath); + statistics.reset(); } } else { std::cout << "Computing " + setupString << " with " << numberOfDesiredEdges << " edges." @@ -372,6 +373,20 @@ std::vector TopologyEnumerator::getValidEdges( return validEdges; } +void TopologyEnumerator::exportPattern(const std::filesystem::path &saveToPath, + PatternGeometry &patternGeometry, + const bool saveTilledPattern) const +{ + const std::string patternName = patternGeometry.getLabel(); + std::filesystem::create_directory(saveToPath); + patternGeometry.save(std::filesystem::path(saveToPath).append(patternName).string() + ".ply"); + if (saveTilledPattern) { + PatternGeometry tiledPatternGeometry = PatternGeometry::createTile(patternGeometry); + tiledPatternGeometry.save( + std::filesystem::path(saveToPath).append(patternName + "_tiled").string() + ".ply"); + } +} + void TopologyEnumerator::computeValidPatterns( const std::vector &numberOfNodesPerSlot, const size_t &numberOfDesiredEdges, @@ -418,6 +433,8 @@ void TopologyEnumerator::computeValidPatterns( // std::string previousPatternBinaryRepresentation(validEdges.size(),'0'); size_t patternIndex = 0; bool validPatternsExist = false; + const bool exportTilledPattern = false; + const bool saveCompressedFormat = false; do { patternIndex++; const std::string patternName = std::to_string(patternIndex); @@ -438,6 +455,7 @@ void TopologyEnumerator::computeValidPatterns( PatternGeometry patternGeometry; patternGeometry.add(allVertices, patternEdges); + patternGeometry.setLabel(patternName); // Check if pattern contains intersecting edges const bool patternContainsIntersectingEdges @@ -448,21 +466,13 @@ void TopologyEnumerator::computeValidPatterns( statistics.numberOfPatternsWithIntersectingEdges++; if (debugIsOn) { if (savePlyFiles) { - PatternGeometry tiledPatternGeometry = PatternGeometry::createTile( - patternGeometry); - auto intersectingPatternsPath = std::filesystem::path(resultsPath) - .append("Intersecting"); - std::filesystem::create_directory(intersectingPatternsPath); - patternGeometry.save( - std::filesystem::path(intersectingPatternsPath).append(patternName).string() - + ".ply"); - tiledPatternGeometry.save(std::filesystem::path(intersectingPatternsPath) - .append(patternName + "_tiled") - .string() - + ".ply"); + exportPattern(std::filesystem::path(resultsPath).append("Intersecting"), + patternGeometry, + exportTilledPattern); } + } else { + continue; // should be uncommented in order to improve performance } - continue; // should be uncommented in order to improve performance } const bool tiledPatternHasEdgesWithAngleSmallerThanThreshold @@ -470,18 +480,10 @@ void TopologyEnumerator::computeValidPatterns( if (tiledPatternHasEdgesWithAngleSmallerThanThreshold) { if (debugIsOn /*|| savePlyFiles*/) { if (savePlyFiles) { - auto danglingEdgesPath = std::filesystem::path(resultsPath) - .append("ExceedingAngleThreshold"); - std::filesystem::create_directory(danglingEdgesPath); - patternGeometry.save( - std::filesystem::path(danglingEdgesPath).append(patternName).string() - + ".ply"); - PatternGeometry tiledPatternGeometry = PatternGeometry::createTile( - patternGeometry); // the marked nodes of hasDanglingEdges are - tiledPatternGeometry.save(std::filesystem::path(danglingEdgesPath) - .append(patternName + "_tiled") - .string() - + ".ply"); + exportPattern(std::filesystem::path(resultsPath) + .append("ExceedingAngleThreshold"), + patternGeometry, + exportTilledPattern); } } else { continue; @@ -493,18 +495,9 @@ void TopologyEnumerator::computeValidPatterns( if (tiledPatternHasNodeWithValenceGreaterThanDesired) { if (debugIsOn) { if (savePlyFiles) { - auto danglingEdgesPath = std::filesystem::path(resultsPath) - .append("HighValencePatterns"); - std::filesystem::create_directory(danglingEdgesPath); - patternGeometry.save( - std::filesystem::path(danglingEdgesPath).append(patternName).string() - + ".ply"); - PatternGeometry tiledPatternGeometry = PatternGeometry::createTile( - patternGeometry); // the marked nodes of hasDanglingEdges are - tiledPatternGeometry.save(std::filesystem::path(danglingEdgesPath) - .append(patternName + "_tiled") - .string() - + ".ply"); + auto highValencePath = std::filesystem::path(resultsPath) + .append("HighValencePatterns"); + exportPattern(highValencePath, patternGeometry, exportTilledPattern); } } else { continue; @@ -522,28 +515,19 @@ void TopologyEnumerator::computeValidPatterns( // Check dangling edges with vcg method // const bool vcg_tiledPatternHasDangling = // tiledPatternGeometry.hasUntiledDanglingEdges(); - if (tiledPatternHasDanglingEdges && !hasFloatingComponents /*&& !hasArticulationPoints*/) { + if (tiledPatternHasDanglingEdges /*&& !hasFloatingComponents && !hasArticulationPoints*/) { statistics.numberOfPatternsWithADanglingEdgeOrNode++; if (debugIsOn) { if (savePlyFiles) { auto danglingEdgesPath = std::filesystem::path(resultsPath).append("Dangling"); - std::filesystem::create_directory(danglingEdgesPath); - patternGeometry.save( - std::filesystem::path(danglingEdgesPath).append(patternName).string() - + ".ply"); - PatternGeometry tiledPatternGeometry = PatternGeometry::createTile( - patternGeometry); // the marked nodes of hasDanglingEdges are - tiledPatternGeometry.save(std::filesystem::path(danglingEdgesPath) - .append(patternName + "_tiled") - .string() - + ".ply"); + exportPattern(danglingEdgesPath, patternGeometry, exportTilledPattern); } } else { continue; } } - if (hasFloatingComponents && !hasArticulationPoints && !tiledPatternHasDanglingEdges) { + if (hasFloatingComponents /*&& !hasArticulationPoints && !tiledPatternHasDanglingEdges */) { statistics.numberOfPatternsWithMoreThanASingleCC++; if (debugIsOn) { if (savePlyFiles) { @@ -599,34 +583,25 @@ void TopologyEnumerator::computeValidPatterns( assert(colorsRegistered == eCC[0].first); - tiledPatternGeometry.save(std::filesystem::path(moreThanOneCCPath) - .append(patternName + "_tiled") - .string() - + ".ply"); + if (exportTilledPattern) { + tiledPatternGeometry.save(std::filesystem::path(moreThanOneCCPath) + .append(patternName + "_tiled") + .string() + + ".ply"); + } } } else { continue; } } - if (hasArticulationPoints && !hasFloatingComponents && !tiledPatternHasDanglingEdges) { + if (hasArticulationPoints /*&& !hasFloatingComponents && !tiledPatternHasDanglingEdges */) { statistics.numberOfPatternsWithArticulationPoints++; if (debugIsOn) { if (savePlyFiles) { auto articulationPointsPath = std::filesystem::path(resultsPath) .append("ArticulationPoints"); - std::filesystem::create_directory(articulationPointsPath); - patternGeometry.save( - std::filesystem::path(articulationPointsPath).append(patternName).string() - + ".ply"); - PatternGeometry tiledPatternGeometry = PatternGeometry::createTile( - patternGeometry); // the marked nodes of hasDanglingEdges are - tiledPatternGeometry.save(std::filesystem::path(articulationPointsPath) - .append(patternName + "_tiled") - .string() - + ".ply"); - - // std::cout << "Pattern:" << patternName << std::endl; + exportPattern(articulationPointsPath, patternGeometry, exportTilledPattern); } } else { continue; @@ -649,39 +624,27 @@ void TopologyEnumerator::computeValidPatterns( statistics.numberOfValidPatterns++; validPatternsExist = true; if (savePlyFiles) { - // if (numberOfDesiredEdges == 4) { - // std::cout << "Saving:" - // << std::filesystem::path(validPatternsPath) - // .append(patternName) - // .string() + - // ".ply" - // << std::endl; - // } - patternGeometry.save( - std::filesystem::path(validPatternsPath).append(patternName).string() + ".ply"); - PatternGeometry tiledPatternGeometry = PatternGeometry::createTile( - patternGeometry); // the marked nodes of hasDanglingEdges are - tiledPatternGeometry.save( - std::filesystem::path(validPatternsPath).append(patternName + "_tiled").string() - + ".ply"); + exportPattern(validPatternsPath, patternGeometry, exportTilledPattern); } - PatternIO::Pattern pattern; - pattern.edges = patternEdges; - pattern.name = patternIndex; - patternSet.patterns.emplace_back(pattern); - // Save valid patterns - // if (patternIndex% patternSetBufferSize == 0) { - if (statistics.numberOfValidPatterns % patternSetBufferSize == 0) { - PatternIO::save(compressedPatternsFilePath, patternSet); - patternSet.patterns.clear(); - patternSet.patterns.reserve(patternSetBufferSize); + if (saveCompressedFormat) { + PatternIO::Pattern pattern; + pattern.edges = patternEdges; + pattern.name = patternIndex; + patternSet.patterns.emplace_back(pattern); + // Save valid patterns + // if (patternIndex% patternSetBufferSize == 0) { + if (statistics.numberOfValidPatterns % patternSetBufferSize == 0) { + PatternIO::save(compressedPatternsFilePath, patternSet); + patternSet.patterns.clear(); + patternSet.patterns.reserve(patternSetBufferSize); + } } } // assert(vcg_tiledPatternHasDangling == tiledPatternHasDanglingEdges); } while (std::next_permutation(patternBinaryRepresentation.begin(), patternBinaryRepresentation.end())); - if (!patternSet.patterns.empty()) { + if (!patternSet.patterns.empty() && saveCompressedFormat) { PatternIO::save(compressedPatternsFilePath, patternSet); } diff --git a/topologyenumerator.hpp b/topologyenumerator.hpp index a11855e..7871ce6 100755 --- a/topologyenumerator.hpp +++ b/topologyenumerator.hpp @@ -1,12 +1,12 @@ #ifndef TOPOLOGYENUMERATOR_HPP #define TOPOLOGYENUMERATOR_HPP +#include "nlohmann/json.hpp" #include "patternIO.hpp" #include "trianglepatterngeometry.hpp" #include "trianglepattterntopology.hpp" #include #include #include -//#include #include #include @@ -63,28 +63,26 @@ class TopologyEnumerator { size_t numberOfPatternsWithADanglingEdgeOrNode{0}; size_t numberOfPatternsWithArticulationPoints{0}; size_t numberOfValidPatterns{0}; - // nlohmann::json convertToJson() const { - // nlohmann::json json; - // json["numPossibleEdges"] = numberOfPossibleEdges; - // json["numCoincideEdges"] = numberOfCoincideEdges; - // json["numDuplicateEdges"] = numberOfDuplicateEdges; - // json["numValidEdges"] = numberOfValidEdges; - // json["numIntersectingEdgePairs"] = numberOfIntersectingEdgePairs; - // json["numPatterns"] = numberOfPatterns; - // // json["numIntersectingEdgesOverAllPatterns"] = - // // numberOfIntersectingEdgesOverAllPatterns; - // json["numPatternsWithIntersectingEdges"] = - // numberOfPatternsWithIntersectingEdges; - // json["numPatternsWithNotASingleCC"] = - // numberOfPatternsWithMoreThanASingleCC; - // json["numPatternsWithDangling"] = - // numberOfPatternsWithADanglingEdgeOrNode; - // json["numPatternsWithArticulationPoints"] = - // numberOfPatternsWithArticulationPoints; - // json["numValidPatterns"] = numberOfValidPatterns; + nlohmann::json convertToJson() const + { + nlohmann::json json; + json["numPossibleEdges"] = numberOfPossibleEdges; + json["numCoincideEdges"] = numberOfCoincideEdges; + json["numDuplicateEdges"] = numberOfDuplicateEdges; + json["numValidEdges"] = numberOfValidEdges; + json["numIntersectingEdgePairs"] = numberOfIntersectingEdgePairs; + json["numPatterns"] = numberOfPatterns; + // json["numIntersectingEdgesOverAllPatterns"] = + // numberOfIntersectingEdgesOverAllPatterns; + json["numPatternsWithIntersectingEdges"] = numberOfPatternsWithIntersectingEdges; + json["numPatternsWithNotASingleCC"] = numberOfPatternsWithMoreThanASingleCC; + json["numPatternsWithDangling"] = numberOfPatternsWithADanglingEdgeOrNode; + json["numPatternsWithArticulationPoints"] = numberOfPatternsWithArticulationPoints; + json["numValidPatterns"] = numberOfValidPatterns; + + return json; + } - // return json; - // } void print(const std::string &setupString, const std::filesystem::path &directoryPath) const { std::cout << "The setup " << setupString << std::endl; @@ -112,20 +110,37 @@ class TopologyEnumerator { << " patterns found with a dangling node or edge" << std::endl; std::cout << numberOfPatternsWithArticulationPoints << " patterns found with an articulation point" << std::endl; - std::cout << numberOfValidPatterns << " valid patterns were found" - << std::endl; - // if (!directoryPath.empty()) { - // auto json = convertToJson(); + std::cout << numberOfValidPatterns << " valid patterns were found" << std::endl; + if (!directoryPath.empty()) { + auto json = convertToJson(); - // std::ofstream file; - // file.open(std::filesystem::path(directoryPath) - // .append("statistics.csv") - // .string()); - // file << "setup," << setupString << "\n"; - // for (const auto &el : json.items()) { - // file << el.key() << "," << el.value() << "\n"; - // } - // } + std::ofstream file; + file.open(std::filesystem::path(directoryPath).append("statistics.csv").string()); + file << "setup," << setupString << "\n"; + for (const auto &el : json.items()) { + file << el.key() << ","; + } + file << "\n"; + for (const auto &el : json.items()) { + file << el.value() << ","; + } + file << "\n"; + } + } + + void reset() + { + numberOfPossibleEdges = 0; + numberOfCoincideEdges = 0; + numberOfDuplicateEdges = 0; + numberOfValidEdges = 0; + numberOfIntersectingEdgePairs = 0; + numberOfPatterns = 0; + numberOfPatternsWithIntersectingEdges = 0; + numberOfPatternsWithMoreThanASingleCC = 0; + numberOfPatternsWithADanglingEdgeOrNode = 0; + numberOfPatternsWithArticulationPoints = 0; + numberOfValidPatterns = 0; } }; @@ -173,8 +188,10 @@ private: const size_t &numberOfDesiredEdges, const std::filesystem::path &resultsPath, const std::vector &vertices, - const std::unordered_map> - &intersectingEdges, + const std::unordered_map> &intersectingEdges, const std::vector &validEdges); + void exportPattern(const std::filesystem::path &saveToPath, + PatternGeometry &patternGeometry, + const bool saveTilledPattern) const; }; #endif // TOPOLOGYENUMERATOR_HPP