diff --git a/CMakeLists.txt b/CMakeLists.txt index 7624411..b2ce619 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,11 +2,12 @@ cmake_minimum_required(VERSION 3.0) project(MySources) file(GLOB MySourcesFiles ${CMAKE_CURRENT_LIST_DIR}/*.hpp ${CMAKE_CURRENT_LIST_DIR}/*.cpp) -add_library(${PROJECT_NAME} STATIC ${MySourcesFiles} ) +add_library(${PROJECT_NAME} ${MySourcesFiles} ) target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_20) #Download external dependencies NOTE: If the user has one of these libs it shouldn't be downloaded again. include(${CMAKE_CURRENT_LIST_DIR}/cmake/DownloadProject.cmake) +include(FetchContent) if (CMAKE_VERSION VERSION_LESS 3.2) set(UPDATE_DISCONNECTED_IF_AVAILABLE "") else() @@ -19,6 +20,13 @@ endif() ##Create directory for the external libraries file(MAKE_DIRECTORY ${EXTERNAL_DEPS_DIR}) +set(DRMSimulationModelDir "/home/iason/Coding/Libraries/DRMSimulationModel") +message("drm dir:" ${DRMSimulationModelDir}) +add_subdirectory(${DRMSimulationModelDir} "/home/iason/Coding/build/DRMSimulationModel") +target_link_libraries(${PROJECT_NAME} PUBLIC DRMSimulationModel) +target_include_directories(${PROJECT_NAME} PUBLIC ${DRMSimulationModelDir}) + +#target_sources(${PROJECT_NAME} PUBLIC ${DRMSimulationModelDir}/simulationmodel.hpp) ##dlib #set(DLIB_BIN_DIR ${CMAKE_CURRENT_BINARY_DIR}/dlib) #download_project(PROJ DLIB @@ -36,56 +44,77 @@ file(MAKE_DIRECTORY ${EXTERNAL_DEPS_DIR}) #endif() #add_compile_definitions(DLIB_DEFINED) -if(${USE_POLYSCOPE}) - set(POLYSCOPE_BINARY_DIR ${CMAKE_CURRENT_BINARY_DIR}/polyscope) - download_project(PROJ POLYSCOPE +## polyscope +FetchContent_Declare(polyscope GIT_REPOSITORY https://github.com/nmwsharp/polyscope.git - GIT_TAG master - PREFIX ${EXTERNAL_DEPS_DIR} - BINARY_DIR ${POLYSCOPE_BINARY_DIR} - ${UPDATE_DISCONNECTED_IF_AVAILABLE} - ) - add_subdirectory(${POLYSCOPE_SOURCE_DIR} ${POLYSCOPE_BINARY_DIR}) - add_compile_definitions(POLYSCOPE_DEFINED) - target_sources(${PROJECT_NAME} PUBLIC ${POLYSCOPE_SOURCE_DIR}/deps/imgui/imgui/misc/cpp/imgui_stdlib.cpp) + GIT_TAG master + ) +FetchContent_MakeAvailable(polyscope) +target_include_directories(${PROJECT_NAME} PUBLIC ${polyscope_SOURCE_DIR}/include) +target_link_libraries(${PROJECT_NAME} PUBLIC polyscope) +#if(NOT TARGET polyscope AND ${USE_POLYSCOPE}) +# set(POLYSCOPE_BINARY_DIR ${CMAKE_CURRENT_BINARY_DIR}/polyscope) +# download_project(PROJ POLYSCOPE +# GIT_REPOSITORY https://github.com/nmwsharp/polyscope.git +# GIT_TAG master +# PREFIX ${EXTERNAL_DEPS_DIR} +# BINARY_DIR ${POLYSCOPE_BINARY_DIR} +# ${UPDATE_DISCONNECTED_IF_AVAILABLE} +# ) +# add_subdirectory(${POLYSCOPE_SOURCE_DIR} ${POLYSCOPE_BINARY_DIR}) +# add_compile_definitions(POLYSCOPE_DEFINED) +# target_sources(${PROJECT_NAME} PUBLIC ${POLYSCOPE_SOURCE_DIR}/deps/imgui/imgui/misc/cpp/imgui_stdlib.cpp) - message("Using polyscope..") - target_include_directories(${PROJECT_NAME} PUBLIC ${POLYSCOPE_SOURCE_DIR}/include) - target_link_libraries(${PROJECT_NAME} PUBLIC polyscope) -endif() +# message("Using polyscope..") +# target_include_directories(${PROJECT_NAME} PUBLIC ${POLYSCOPE_SOURCE_DIR}/include) +# target_link_libraries(${PROJECT_NAME} PUBLIC polyscope) +#endif() ##vcglib devel branch -download_project(PROJ vcglib_devel +FetchContent_Declare(vcglib GIT_REPOSITORY https://gitea-s2i2s.isti.cnr.it/manolas/vcglib.git GIT_TAG devel - PREFIX ${EXTERNAL_DEPS_DIR} - ${UPDATE_DISCONNECTED_IF_AVAILABLE} -) -add_subdirectory(${vcglib_devel_SOURCE_DIR} ${vcglib_devel_BINARY_DIR}) -target_sources(${PROJECT_NAME} PUBLIC ${vcglib_devel_SOURCE_DIR}/wrap/ply/plylib.cpp) + ) +FetchContent_MakeAvailable(vcglib) +target_include_directories(${PROJECT_NAME} PUBLIC ${vcglib_SOURCE_DIR}) +target_sources(${PROJECT_NAME} PUBLIC ${vcglib_SOURCE_DIR}/wrap/ply/plylib.cpp) -##matplot++ lib -set(MATPLOTPLUSPLUS_BINARY_DIR ${CMAKE_CURRENT_BINARY_DIR}/matplot) -download_project(PROJ MATPLOTPLUSPLUS +###matplot++ lib +FetchContent_Declare(matplot GIT_REPOSITORY https://github.com/alandefreitas/matplotplusplus - GIT_TAG master - BINARY_DIR ${MATPLOTPLUSPLUS_BINARY_DIR} - PREFIX ${EXTERNAL_DEPS_DIR} - ${UPDATE_DISCONNECTED_IF_AVAILABLE} - ) -add_subdirectory(${MATPLOTPLUSPLUS_SOURCE_DIR} ${MATPLOTPLUSPLUS_BINARY_DIR}) + GIT_TAG master + ) +FetchContent_MakeAvailable(matplot) +target_include_directories(${PROJECT_NAME} PUBLIC ${matplot_SOURCE_DIR}) +#set(MATPLOTPLUSPLUS_BINARY_DIR ${CMAKE_CURRENT_BINARY_DIR}/matplot) +#download_project(PROJ MATPLOTPLUSPLUS +# GIT_TAG master +# BINARY_DIR ${MATPLOTPLUSPLUS_BINARY_DIR} +# PREFIX ${EXTERNAL_DEPS_DIR} +# ${UPDATE_DISCONNECTED_IF_AVAILABLE} +# ) +#add_subdirectory(${MATPLOTPLUSPLUS_SOURCE_DIR} ${MATPLOTPLUSPLUS_BINARY_DIR}) ##threed-beam-fea -set(threed-beam-fea_BINARY_DIR ${CMAKE_CURRENT_BINARY_DIR}/threed-beam-fea) -download_project(PROJ threed-beam-fea -# GIT_REPOSITORY https://github.com/IasonManolas/threed-beam-fea.git +if(NOT TARGET ThreedBeamFEA) +FetchContent_Declare(threed-beam-fea GIT_REPOSITORY https://gitea-s2i2s.isti.cnr.it/manolas/threed-beam-fea.git - GIT_TAG master - BINARY_DIR ${threed-beam-fea_BINARY_DIR} - PREFIX ${EXTERNAL_DEPS_DIR} - ${UPDATE_DISCONNECTED_IF_AVAILABLE} -) -add_subdirectory(${threed-beam-fea_SOURCE_DIR} ${threed-beam-fea_BINARY_DIR}) + GIT_TAG master + ) +FetchContent_MakeAvailable(threed-beam-fea) +endif() +target_link_libraries(${PROJECT_NAME} PUBLIC ThreedBeamFEA) +target_include_directories(${PROJECT_NAME} PUBLIC ${threed-beam-fea_SOURCE_DIR}) + +#set(threed-beam-fea_BINARY_DIR ${CMAKE_CURRENT_BINARY_DIR}/threed-beam-fea) +#download_project(PROJ threed-beam-fea +## GIT_REPOSITORY https://github.com/IasonManolas/threed-beam-fea.git +# GIT_TAG master +# BINARY_DIR ${threed-beam-fea_BINARY_DIR} +# PREFIX ${EXTERNAL_DEPS_DIR} +# ${UPDATE_DISCONNECTED_IF_AVAILABLE} +#) +#add_subdirectory(${threed-beam-fea_SOURCE_DIR} ${threed-beam-fea_BINARY_DIR}) find_package(Eigen3 3.4 REQUIRED) if(MSVC) @@ -97,17 +126,16 @@ if(${MYSOURCES_STATIC_LINK}) endif() if(${MYSOURCES_STATIC_LINK}) message("Linking statically here") - target_link_libraries(${PROJECT_NAME} PUBLIC -static Eigen3::Eigen matplot ThreedBeamFEA pthread gfortran quadmath) + target_link_libraries(${PROJECT_NAME} PUBLIC -static Eigen3::Eigen matplot pthread gfortran quadmath) else() - target_link_libraries(${PROJECT_NAME} PUBLIC Eigen3::Eigen matplot ThreedBeamFEA tbb pthread) + target_link_libraries(${PROJECT_NAME} PUBLIC Eigen3::Eigen matplot tbb pthread) endif() target_link_directories(MySources PUBLIC ${CMAKE_CURRENT_LIST_DIR}/boost_graph/libs) target_include_directories(${PROJECT_NAME} PUBLIC ${CMAKE_CURRENT_LIST_DIR}/boost_graph PUBLIC ${vcglib_devel_SOURCE_DIR} - PUBLIC ${threed-beam-fea_SOURCE_DIR} - PUBLIC ${MATPLOTPLUSPLUS_SOURCE_DIR}/source +# PUBLIC ${threed-beam-fea_SOURCE_DIR} ) @@ -118,13 +146,32 @@ add_subdirectory(${ARMADILLO_SOURCE_DIR} ${ARMADILLO_BIN_DIR}) target_include_directories(${PROJECT_NAME} PUBLIC ${ARMADILLO_SOURCE_DIR}/include) add_compile_definitions(ARMA_DONT_USE_WRAPPER) target_link_libraries(${PROJECT_NAME} PUBLIC "/home/iason/Coding/Libraries/armadillo/build/libarmadillo.a" #[[blas lapack]]) +find_package(Armadillo REQUIRED) +target_link_libraries(${PROJECT_NAME} PUBLIC ${ARMADILLO_LIBRARIES}) +#if(NOT TARGET ThreedBeamFEA) +#FetchContent_Declare(armadillo +# GIT_REPOSITORY https://gitlab.com/conradsnicta/armadillo-code.git +# GIT_TAG "11.2.x" +# ) +#FetchContent_MakeAvailable(armadillo) #find_package(Armadillo REQUIRED) -#target_link_libraries(${PROJECT_NAME} PUBLIC ${ARMADILLO_LIBRARIES}) +#endif() +#target_link_libraries(${PROJECT_NAME} PUBLIC "/home/iason/Coding/build/FormFInder/Debug/_deps/armadillo-build/libarmadillo.a") + +###ENSMALLEN +#FetchContent_Declare(ensmallen +# GIT_REPOSITORY https://github.com/mlpack/ensmallen.git +# GIT_TAG master +# ) +#FetchContent_MakeAvailable(ensmallen) +#target_link_libraries(${PROJECT_NAME} PRIVATE ensmallen) +#target_include_directories(${PROJECT_NAME} +#PUBLIC ${ensmallen_SOURCE_DIR}/include) +#add_compile_definitions(USE_ENSMALLEN) -##ENSMALLEN set(ENSMALLEN_BINARY_DIR ${CMAKE_CURRENT_BINARY_DIR}/ensmallen) download_project(PROJ ENSMALLEN - GIT_REPOSITORY https://github.com/mlpack/ensmallen.git + GIT_REPOSITORY https://github.com/mlpack/ensmallen.git GIT_TAG master BINARY_DIR ${ENSMALLEN_BINARY_DIR} PREFIX ${EXTERNAL_DEPS_DIR} @@ -190,16 +237,21 @@ target_link_libraries(${PROJECT_NAME} PUBLIC "/home/iason/Coding/Libraries/chron ##TBB if(NOT TARGET tbb_static AND NOT TARGET tbb) - set(TBB_BINARY_DIR ${CMAKE_CURRENT_BINARY_DIR}/tbb) - download_project(PROJ TBB - GIT_REPOSITORY https://github.com/wjakob/tbb.git - GIT_TAG master - PREFIX ${EXTERNAL_DEPS_DIR} - BINARY_DIR ${TBB_BINARY_DIR} - ${UPDATE_DISCONNECTED_IF_AVAILABLE} - ) - option(TBB_BUILD_TESTS "Build TBB tests and enable testing infrastructure" OFF) - add_subdirectory(${TBB_SOURCE_DIR} ${TBB_BINARY_DIR}) -link_directories(${TBB_BINARY_DIR}) -target_link_libraries(${PROJECT_NAME} PRIVATE tbb_static) +# set(TBB_BINARY_DIR ${CMAKE_CURRENT_BINARY_DIR}/tbb) +# download_project(PROJ TBB +# GIT_REPOSITORY https://github.com/wjakob/tbb.git +# GIT_TAG master +# PREFIX ${EXTERNAL_DEPS_DIR} +# BINARY_DIR ${TBB_BINARY_DIR} +# ${UPDATE_DISCONNECTED_IF_AVAILABLE} +# ) +# option(TBB_BUILD_TESTS "Build TBB tests and enable testing infrastructure" OFF) +# add_subdirectory(${TBB_SOURCE_DIR} ${TBB_BINARY_DIR}) +#link_directories(${TBB_BINARY_DIR}) + FetchContent_Declare(tbb + GIT_REPOSITORY https://github.com/wjakob/tbb.git + GIT_TAG master + ) + FetchContent_MakeAvailable(tbb) + target_link_libraries(${PROJECT_NAME} PRIVATE tbb_static) endif() diff --git a/beam.hpp b/beam.hpp deleted file mode 100755 index 1ec6689..0000000 --- a/beam.hpp +++ /dev/null @@ -1,157 +0,0 @@ -#ifndef BEAM_HPP -#define BEAM_HPP -#include "nlohmann/json.hpp" -#include -#include -#include -#include - -struct BeamDimensions -{ - double dim1, dim2; - double A{0}; // cross sectional area - - struct MomentsOfInertia - { - double I2{0}; // second moment of inertia - double I3{0}; // third moment of inertia - double J{0}; // torsional constant (polar moment of inertia) - } inertia; - -public: - double getDim1() const; - double getDim2() const; - - virtual void to_json(nlohmann::json &j, const BeamDimensions &beamDim) const; - - virtual void from_json(const nlohmann::json &j, BeamDimensions &beamDim) const; - virtual double getDrawingRadius() const = 0; -}; - -inline double BeamDimensions::getDim2() const -{ - return dim2; -} - -inline void BeamDimensions::to_json(nlohmann::json &j, const BeamDimensions &beamDim) const -{ - j = nlohmann::json{{"BeamDimension.dim1", beamDim.dim1}, {"BeamDimension.dim2", beamDim.dim2}}; -} - -inline void BeamDimensions::from_json(const nlohmann::json &j, BeamDimensions &beamDim) const -{ - const std::string jsonKey_dim1 = "BeamDimension.dim1"; - const std::string jsonKey_dim2 = "BeamDimension.dim2"; - if (j.contains(jsonKey_dim1)) { - j.at(jsonKey_dim1).get_to(beamDim.dim1); - } - if (j.contains(jsonKey_dim2)) { - j.at(jsonKey_dim2).get_to(beamDim.dim2); - } -} - -inline double BeamDimensions::getDim1() const -{ - return dim1; -} - -struct RectangularBeamDimensions : public BeamDimensions -{ - inline static std::string name{"Rectangular"}; - inline const static double defaultSize = 0.002; - - RectangularBeamDimensions(const double &width, const double &height) - { - assert(width > 0 && height > 0); - dim1 = width; - dim2 = height; - updateProperties(); - } - RectangularBeamDimensions() - { - dim1 = defaultSize; - dim2 = defaultSize; - updateProperties(); - } - - std::string toString() const - { - return std::string("w=") + std::to_string(dim1) + std::string(" h=") + std::to_string(dim2); - } - - void updateProperties() - { - A = dim1 * dim2; - inertia.I2 = dim1 * std::pow(dim2, 3) / 12; - inertia.I3 = dim2 * std::pow(dim1, 3) / 12; - inertia.J = inertia.I2 + inertia.I3; - } - static void computeMomentsOfInertia(const RectangularBeamDimensions &dimensions, - MomentsOfInertia &inertia); - double getWidth() const { return dim1; } - double getHeight() const { return dim2; } - double getDrawingRadius() const override; -}; - -inline double RectangularBeamDimensions::getDrawingRadius() const -{ - return getWidth() / std::sqrt(2); -} - -struct CylindricalBeamDimensions : public BeamDimensions -{ - inline static std::string name{"Cylindrical"}; - // https://www.engineeringtoolbox.com/area-moment-inertia-d_1328.html - CylindricalBeamDimensions(const double &outsideDiameter, const double &insideDiameter) - { - assert(outsideDiameter > 0 && insideDiameter > 0 && outsideDiameter > insideDiameter); - dim1 = insideDiameter; - dim2 = outsideDiameter; - updateProperties(); - } - CylindricalBeamDimensions() - { - dim1 = 0.026; - dim2 = 0.03; - updateProperties(); - } - void updateProperties() - { - A = M_PI * (std::pow(getOutterDiameter(), 2) - std::pow(getInnerDiameter(), 2)) / 4; - inertia.I2 = M_PI * (std::pow(getOutterDiameter(), 4) - std::pow(getInnerDiameter(), 4)) - / 64; - inertia.I3 = inertia.I2; - inertia.J = inertia.I2 + inertia.I3; - } - double getInnerDiameter() const { return dim1; } - double getOutterDiameter() const { return dim2; } - double getDrawingRadius() const override; -}; - -inline double CylindricalBeamDimensions::getDrawingRadius() const -{ - return getOutterDiameter(); -} - -struct ElementMaterial -{ - double poissonsRatio; - double youngsModulus; - double G; - ElementMaterial(const double &poissonsRatio, const double &youngsModulus) - : poissonsRatio(poissonsRatio), youngsModulus(youngsModulus) - { - assert(poissonsRatio <= 0.5 && poissonsRatio >= -1); - updateShearModulus(); - } - ElementMaterial() : poissonsRatio(0.3), youngsModulus(1e9) { updateShearModulus(); } - std::string toString() const - { - return std::string("Material:") + std::string("\nPoisson's ratio=") - + std::to_string(poissonsRatio) + std::string("\nYoung's Modulus(GPa)=") - + std::to_string(youngsModulus / 1e9); - } - void updateShearModulus() { G = youngsModulus / (2 * (1 + poissonsRatio)); } -}; - -#endif // BEAM_HPP diff --git a/drmsimulationmodel.cpp b/drmsimulationmodel.cpp deleted file mode 100755 index 1333e80..0000000 --- a/drmsimulationmodel.cpp +++ /dev/null @@ -1,3293 +0,0 @@ -#include "drmsimulationmodel.hpp" -#include "linearsimulationmodel.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.totalResidualForcesNormThreshold = 1; - settings.shouldDraw = false; - settings.beVerbose = true; - // settings.debugModeStep = 1000; - // settings.shouldDraw = 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}); - DRMSimulationModel formFinder2; - SimulationResults shortSpanGridshellSimulationResults - = formFinder2.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}); - - DRMSimulationModel formFinder3; - SimulationResults longSpanGridshell_simulationResults - = formFinder3.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(const std::shared_ptr &pJob) -{ - //#ifdef USE_ENSMALLEN - // this->pJob = pJob; - //#endif - pMesh.reset(); - pMesh = std::make_unique(*pJob->pMesh); - vcg::tri::UpdateBounding::Box(*pMesh); - - constrainedVertices.clear(); - constrainedVertices = pJob->constrainedVertices; - if (!pJob->nodalForcedDisplacements.empty()) { - for (std::pair viDisplPair : pJob->nodalForcedDisplacements) { - const VertexIndex vi = viDisplPair.first; - constrainedVertices[vi].insert({0, 1, 2}); - } - } - computeRigidSupports(); - isVertexConstrained.resize(pMesh->VN(), false); - for (auto fixedVertex : constrainedVertices) { - isVertexConstrained[fixedVertex.first] = true; - } -} - -void DRMSimulationModel::reset(const std::shared_ptr &pJob, const Settings &settings) -{ - mSettings = settings; - mCurrentSimulationStep = 0; - history.clear(); - history.label = pJob->pMesh->getLabel() + "_" + pJob->getLabel(); - plotYValues.clear(); - plotHandle.reset(); - checkedForMaximumMoment = false; - externalMomentsNorm = 0; - Dt = settings.Dtini; - numOfDampings = 0; - shouldTemporarilyDampForces = false; - externalLoadStep = 1; - isVertexConstrained.clear(); - minTotalResidualForcesNorm = std::numeric_limits::max(); - - reset(pJob); - -#ifdef POLYSCOPE_DEFINED - if (mSettings.shouldDraw || mSettings.debugModeStep.has_value()) { - PolyscopeInterface::init(); - polyscope::registerCurveNetwork(meshPolyscopeLabel + "_" + pMesh->getLabel(), - pMesh->getEigenVertices(), - pMesh->getEigenEdges()); - polyscope::registerCurveNetwork("Initial_" + meshPolyscopeLabel + "_" + pMesh->getLabel(), - pMesh->getEigenVertices(), - pMesh->getEigenEdges()) - ->setRadius(0.002); - // registerWorldAxes(); - } -#endif - - if (!pJob->nodalForcedDisplacements.empty() && pJob->nodalExternalForces.empty()) { - shouldApplyInitialDistortion = true; - } - updateElementalFrames(); -} - -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 - - // 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; - } - const double 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.dimensions.A - / (2 * element.initialLength); - const double theta1Diff = theta1_jplus1 - theta1_j; - const double torsionalPE = element.material.G * element.dimensions.inertia.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.dimensions.inertia.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.dimensions.inertia.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::for_each(pMesh->nodes._handle->data.begin(), - pMesh->nodes._handle->data.end(), - [](Node &node) { - node.force.internalAxial = 0; - node.force.internalTorsion = 0; - node.force.internalFirstBending = 0; - node.force.internalSecondBending = 0; - }); - - std::vector>> internalForcesContributionsFromEachEdge( - pMesh->EN(), std::vector>(4, {-1, Vector6d()})); - - // omp_lock_t writelock; - // omp_init_lock(&writelock); - //#ifdef ENABLE_OPENMP - //#pragma omp parallel for schedule(static) num_threads(5) - //#endif - std::for_each( -#ifdef ENABLE_PARALLEL_DRM - std::execution::par_unseq, -#endif - pMesh->edge.begin(), - pMesh->edge.end(), - [&](const EdgeType &e) - // for (int ei = 0; ei < pMesh->EN(); ei++) - { - const int ei = pMesh->getIndex(e); - // const EdgeType &e = pMesh->edge[ei]; - const SimulationMesh::VertexType &ev_j = *e.cV(0); - const SimulationMesh::VertexType &ev_jplus1 = *e.cV(1); - const Element &element = pMesh->elements[e]; - const Element::LocalFrame &ef = element.frame; - const VectorType t3CrossN_j = Cross(ef.t3, ev_j.cN()); - const VectorType t3CrossN_jplus1 = Cross(ef.t3, ev_jplus1.cN()); - const double theta1_j = ef.t1.dot(t3CrossN_j); - const double theta1_jplus1 = ef.t1.dot(t3CrossN_jplus1); - const double theta2_j = ef.t2.dot(t3CrossN_j); - const double theta2_jplus1 = ef.t2.dot(t3CrossN_jplus1); - const double theta3_j = computeTheta3(e, ev_j); - const double theta3_jplus1 = computeTheta3(e, ev_jplus1); - // element.rotationalDisplacements_j.theta1 = theta1_j; - // element.rotationalDisplacements_jplus1.theta1 = theta1_jplus1; - // element.rotationalDisplacements_j.theta2 = theta2_j; - // element.rotationalDisplacements_jplus1.theta2 = theta2_jplus1; - // element.rotationalDisplacements_j.theta3 = theta3_j; - // element.rotationalDisplacements_jplus1.theta3 = theta3_jplus1; - std::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; - pMesh->nodes[edgeNode.vi].force.internalAxial[dofi] += axialForce_dofi; - -#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; - pMesh->nodes[edgeNode.vi].force.internalTorsion[dofi] += torsionalForce_dofi; -#ifdef POLYSCOPE_DEFINED - if (std::isnan(torsionalForce_dofi)) { - std::cerr << "nan in torsional" << evi << std::endl; - std::terminate(); - } -#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; - pMesh->nodes[edgeNode.vi].force.internalFirstBending[dofi] - += firstBendingForce_dofi; - - // Second bending force computation - ////theta2_j derivative - const double theta3_j_deriv = computeDerivativeTheta3(e, ev_j, dui); - ////theta2_jplus1 derivative - const double theta3_jplus1_deriv = computeDerivativeTheta3(e, - ev_jplus1, - dui); - ////1st in bracket term - const double secondBendingForce_inBracketsTerm_0 = theta3_j_deriv * 2 - * theta3_j; - ////2nd in bracket term - const double secondBendingForce_inBracketsTerm_1 = theta3_jplus1_deriv - * theta3_j; - ////3rd in bracket term - const double secondBendingForce_inBracketsTerm_2 = theta3_j_deriv - * theta3_jplus1; - ////4th in bracket term - const double secondBendingForce_inBracketsTerm_3 = theta3_jplus1_deriv * 2 - * theta3_jplus1; - // 3rd term computation - const double secondBendingForce_inBracketsTerm - = secondBendingForce_inBracketsTerm_0 - + secondBendingForce_inBracketsTerm_1 - + secondBendingForce_inBracketsTerm_2 - + secondBendingForce_inBracketsTerm_3; - double secondBendingForce_dofi = secondBendingForce_inBracketsTerm - * element.rigidity.secondBending; - pMesh->nodes[edgeNode.vi].force.internalSecondBending[dofi] - += secondBendingForce_dofi; - 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; - pMesh->nodes[refElemOtherVertexNode.vi].force.internalSecondBending[dofi] - += secondBendingForce_dofi; - } - } - } - } - internalForcesContributionsFromEachEdge[ei] = internalForcesContributionFromThisEdge; - }); - - std::for_each(pMesh->nodes._handle->data.begin(), - pMesh->nodes._handle->data.end(), - [](Node &node) { - Node::Forces &force = node.force; - 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; - std::terminate(); - } -#endif - } - } -#ifdef POLYSCOPE_DEFINED - std::for_each(pMesh->nodes._handle->data.begin(), - pMesh->nodes._handle->data.end(), - [](Node &node) { - Node::Forces &force = node.force; - const Vector6d debug_residual = force.external - force.internalAxial - - force.internalTorsion - - force.internalFirstBending - - force.internalSecondBending; - assert((force.residual - debug_residual).norm() < 1e-5); - }); -#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(); - totalResidualForcesNorm += residualForceNorm; -#ifdef POLYSCOPE_DEFINED - if (std::isnan(force.residual.norm())) { - std::cout << "residual " << vi << ":" << force.residual.toString() << std::endl; - std::terminate(); - } -// std::cout << "residual " << vi << ":" << force.residual.norm() << std::endl; -#endif - } - pMesh->totalResidualForcesNorm = 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; - double totalExternalForcesNorm = 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; - totalExternalForcesNorm += node.force.external.norm(); - } - - pMesh->totalExternalForcesNorm = totalExternalForcesNorm; -} - -std::vector> DRMSimulationModel::computeInternalForces( - const std::unordered_map> &fixedVertices) -{ - std::vector, 4>> - internalForcesContributionsFromEachEdge_axial(pMesh->EN(), - std::array, 4>{ - std::pair(-1, Vector6d())}); - std::vector, 4>> - internalForcesContributionsFromEachEdge_torsion(pMesh->EN(), - std::array, 4>{ - std::pair(-1, - Vector6d())}); - std::vector, 4>> - internalForcesContributionsFromEachEdge_firstBending( - pMesh->EN(), - std::array, 4>{std::pair(-1, Vector6d())}); - std::vector, 4>> - internalForcesContributionsFromEachEdge_secondBending( - pMesh->EN(), - std::array, 4>{std::pair(-1, Vector6d())}); - // omp_lock_t writelock; - // omp_init_lock(&writelock); - //#ifdef ENABLE_OPENMP - //#pragma omp parallel for schedule(static) num_threads(5) - //#endif - std::for_each( -#ifdef ENABLE_PARALLEL_DRM - std::execution::par_unseq, -#endif - pMesh->edge.begin(), - pMesh->edge.end(), - [&](const EdgeType &e) - // for (int ei = 0; ei < pMesh->EN(); ei++) - { - const int ei = pMesh->getIndex(e); - // const EdgeType &e = pMesh->edge[ei]; - const SimulationMesh::VertexType &ev_j = *e.cV(0); - const SimulationMesh::VertexType &ev_jplus1 = *e.cV(1); - const Element &element = pMesh->elements[e]; - const Element::LocalFrame &ef = element.frame; - const VectorType t3CrossN_j = Cross(ef.t3, ev_j.cN()); - const VectorType t3CrossN_jplus1 = Cross(ef.t3, ev_jplus1.cN()); - const double theta1_j = ef.t1.dot(t3CrossN_j); - const double theta1_jplus1 = ef.t1.dot(t3CrossN_jplus1); - const double theta2_j = ef.t2.dot(t3CrossN_j); - const double theta2_jplus1 = ef.t2.dot(t3CrossN_jplus1); - const double theta3_j = computeTheta3(e, ev_j); - const double theta3_jplus1 = computeTheta3(e, ev_jplus1); - // element.rotationalDisplacements_j.theta1 = theta1_j; - // element.rotationalDisplacements_jplus1.theta1 = theta1_jplus1; - // element.rotationalDisplacements_j.theta2 = theta2_j; - // element.rotationalDisplacements_jplus1.theta2 = theta2_jplus1; - // element.rotationalDisplacements_j.theta3 = theta3_j; - // element.rotationalDisplacements_jplus1.theta3 = theta3_jplus1; - // std::array, 4> internalForcesContributionFromThisEdge{ - // std::pair(-1, Vector6d())}; - std::array, 4> internalForcesContributionFromThisEdge_axial{ - std::pair(-1, Vector6d())}; - std::array, 4> internalForcesContributionFromThisEdge_torsion{ - std::pair(-1, Vector6d())}; - std::array, 4> - internalForcesContributionFromThisEdge_firstBending{ - std::pair(-1, Vector6d())}; - std::array, 4> - internalForcesContributionFromThisEdge_secondBending{ - std::pair(-1, Vector6d())}; - for (VertexIndex evi = 0; evi < 2; evi++) { - const SimulationMesh::VertexType &ev = *e.cV(evi); - const Node &edgeNode = pMesh->nodes[ev]; - internalForcesContributionFromThisEdge_axial[evi].first = edgeNode.vi; - - const VertexPointer &rev_j = edgeNode.referenceElement->cV(0); - const VertexPointer &rev_jplus1 = edgeNode.referenceElement->cV(1); - // refElemOtherVertex can be precomputed - const VertexPointer &refElemOtherVertex = rev_j == &ev ? rev_jplus1 : rev_j; - const Node &refElemOtherVertexNode = pMesh->nodes[refElemOtherVertex]; - if (edgeNode.referenceElement != &e) { - internalForcesContributionFromThisEdge_axial[evi + 2].first - = refElemOtherVertexNode.vi; - } - const size_t vi = edgeNode.vi; - // #pragma omp parallel for schedule(static) num_threads(6) - for (DoFType dofi = DoF::Ux; dofi < DoF::NumDoF; dofi++) { - const bool isDofConstrainedFor_ev = isVertexConstrained[edgeNode.vi] - && fixedVertices.at(edgeNode.vi) - .contains(dofi); - if (!isDofConstrainedFor_ev) { - DifferentiateWithRespectTo dui{ev, dofi}; - // Axial force computation - const double e_k = element.length - element.initialLength; - const double e_kDeriv = computeDerivativeElementLength(e, dui); - const double axialForce_dofi = e_kDeriv * e_k * element.rigidity.axial; - -#ifdef POLYSCOPE_DEFINED - if (std::isnan(axialForce_dofi)) { - std::cerr << "nan in axial" << evi << std::endl; - } -#endif - - // Torsional force computation - const double theta1_j_deriv = computeDerivativeTheta1(e, 0, evi, dofi); - const double theta1_jplus1_deriv = computeDerivativeTheta1(e, 1, evi, dofi); - const double theta1Diff = theta1_jplus1 - theta1_j; - const double theta1DiffDerivative = theta1_jplus1_deriv - theta1_j_deriv; - const double torsionalForce_dofi = theta1DiffDerivative * theta1Diff - * element.rigidity.torsional; -#ifdef POLYSCOPE_DEFINED - if (std::isnan(torsionalForce_dofi)) { - std::cerr << "nan in torsional" << evi << std::endl; - } -#endif - - // First bending force computation - ////theta2_j derivative - const double theta2_j_deriv = computeDerivativeTheta2(e, 0, evi, dofi); - ////theta2_jplus1 derivative - const double theta2_jplus1_deriv = computeDerivativeTheta2(e, 1, evi, dofi); - ////1st in bracket term - const double firstBendingForce_inBracketsTerm_0 = theta2_j_deriv * 2 - * theta2_j; - ////2nd in bracket term - const double firstBendingForce_inBracketsTerm_1 = theta2_jplus1_deriv - * theta2_j; - ////3rd in bracket term - const double firstBendingForce_inBracketsTerm_2 = theta2_j_deriv - * theta2_jplus1; - ////4th in bracket term - const double firstBendingForce_inBracketsTerm_3 = 2 * theta2_jplus1_deriv - * theta2_jplus1; - // 3rd term computation - const double firstBendingForce_inBracketsTerm - = firstBendingForce_inBracketsTerm_0 - + firstBendingForce_inBracketsTerm_1 - + firstBendingForce_inBracketsTerm_2 - + firstBendingForce_inBracketsTerm_3; - const double firstBendingForce_dofi = firstBendingForce_inBracketsTerm - * element.rigidity.firstBending; - - // Second bending force computation - ////theta2_j derivative - const double theta3_j_deriv = computeDerivativeTheta3(e, ev_j, dui); - ////theta2_jplus1 derivative - const double theta3_jplus1_deriv = computeDerivativeTheta3(e, - ev_jplus1, - dui); - ////1st in bracket term - const double secondBendingForce_inBracketsTerm_0 = theta3_j_deriv * 2 - * theta3_j; - ////2nd in bracket term - const double secondBendingForce_inBracketsTerm_1 = theta3_jplus1_deriv - * theta3_j; - ////3rd in bracket term - const double secondBendingForce_inBracketsTerm_2 = theta3_j_deriv - * theta3_jplus1; - ////4th in bracket term - const double secondBendingForce_inBracketsTerm_3 = theta3_jplus1_deriv * 2 - * theta3_jplus1; - // 3rd term computation - const double secondBendingForce_inBracketsTerm - = secondBendingForce_inBracketsTerm_0 - + secondBendingForce_inBracketsTerm_1 - + secondBendingForce_inBracketsTerm_2 - + secondBendingForce_inBracketsTerm_3; - double secondBendingForce_dofi = secondBendingForce_inBracketsTerm - * element.rigidity.secondBending; - internalForcesContributionFromThisEdge_axial[evi].second[dofi] - = axialForce_dofi; - internalForcesContributionFromThisEdge_torsion[evi].second[dofi] - = torsionalForce_dofi; - internalForcesContributionFromThisEdge_firstBending[evi].second[dofi] - = firstBendingForce_dofi; - internalForcesContributionFromThisEdge_secondBending[evi].second[dofi] - = secondBendingForce_dofi; - } - if (edgeNode.referenceElement != &e) { - const bool isDofConstrainedFor_refElemOtherVertex - = isVertexConstrained[refElemOtherVertexNode.vi] - && fixedVertices.at(refElemOtherVertexNode.vi).contains(dofi); - if (!isDofConstrainedFor_refElemOtherVertex) { - DifferentiateWithRespectTo dui{*refElemOtherVertex, dofi}; - ////theta3_j derivative - const double theta3_j_deriv = computeDerivativeTheta3(e, ev_j, dui); - ////theta3_jplus1 derivative - const double theta3_jplus1_deriv = computeDerivativeTheta3(e, - ev_jplus1, - dui); - ////1st in bracket term - const double secondBendingForce_inBracketsTerm_0 = theta3_j_deriv * 2 - * theta3_j; - ////2nd in bracket term - const double secondBendingForce_inBracketsTerm_1 = theta3_jplus1_deriv - * theta3_j; - ////3rd in bracket term - const double secondBendingForce_inBracketsTerm_2 = theta3_j_deriv - * theta3_jplus1; - ////4th in bracket term - const double secondBendingForce_inBracketsTerm_3 = theta3_jplus1_deriv - * 2 * theta3_jplus1; - - // 4th term computation - const double secondBendingForce_inBracketsTerm - = secondBendingForce_inBracketsTerm_0 - + secondBendingForce_inBracketsTerm_1 - + secondBendingForce_inBracketsTerm_2 - + secondBendingForce_inBracketsTerm_3; - const double secondBendingForce_dofi = secondBendingForce_inBracketsTerm - * element.rigidity.secondBending; - internalForcesContributionFromThisEdge_secondBending[evi + 2].second[dofi] - = secondBendingForce_dofi; - } - } - } - } - internalForcesContributionsFromEachEdge_axial[ei] - = internalForcesContributionFromThisEdge_axial; - internalForcesContributionsFromEachEdge_torsion[ei] - = internalForcesContributionFromThisEdge_torsion; - internalForcesContributionsFromEachEdge_firstBending[ei] - = internalForcesContributionFromThisEdge_firstBending; - internalForcesContributionsFromEachEdge_secondBending[ei] - = internalForcesContributionFromThisEdge_secondBending; - }); - - //#pragma omp parallel for schedule(static) num_threads(8) - - std::vector> perVertexInternalForces(pMesh->VN(), {0}); - for (size_t ei = 0; ei < pMesh->EN(); ei++) { - for (int i = 0; i < 4; i++) { - std::pair internalForcePair_axial - = internalForcesContributionsFromEachEdge_axial[ei][i]; - std::pair internalForcePair_torsion - = internalForcesContributionsFromEachEdge_torsion[ei][i]; - std::pair internalForcePair_firstBending - = internalForcesContributionsFromEachEdge_firstBending[ei][i]; - std::pair internalForcePair_secondBending - = internalForcesContributionsFromEachEdge_secondBending[ei][i]; - int vi = internalForcePair_axial.first; - if (i > 1 && vi == -1) { - continue; - } - perVertexInternalForces[vi][0] = perVertexInternalForces[vi][0] - + internalForcePair_axial.second; - perVertexInternalForces[vi][1] = perVertexInternalForces[vi][1] - + internalForcePair_torsion.second; - perVertexInternalForces[vi][2] = perVertexInternalForces[vi][2] - + internalForcePair_firstBending.second; - perVertexInternalForces[vi][3] = perVertexInternalForces[vi][3] - + internalForcePair_secondBending.second; - } - } - - return perVertexInternalForces; -} - -void DRMSimulationModel::updateResidualForces() -{ - pMesh->totalResidualForcesNorm = 0; - 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++) { - const auto ei = pMesh->getIndex(e); - 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); - } - // if (mCurrentSimulationStep == static_cast(1.4 * untilStep) - // && shouldTemporarilyDampForces) { - // Dt = mSettings.Dtini; - // } - 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.dimensions.A - / elem.length; - translationalSumSk += SkTranslational; - const double lengthToThe3 = std::pow(elem.length, 3); - const long double SkRotational_I2 = elem.material.youngsModulus * elem.dimensions.inertia.I2 - / lengthToThe3; - const long double SkRotational_I3 = elem.material.youngsModulus * elem.dimensions.inertia.I3 - / lengthToThe3; - const long double SkRotational_J = elem.material.youngsModulus * elem.dimensions.inertia.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.viscousDampingFactor.has_value()) { - //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 - * mSettings.viscousDampingFactor.value(); - } - 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]; - node.previousVelocity = node.velocity; - if (mSettings.viscousDampingFactor.has_value()) { - 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.totalTranslationalKineticEnergyThreshold = 1e-8; -#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) -{ - std::for_each( -#ifdef ENABLE_PARALLEL_DRM - std::execution::par_unseq, -#endif - pMesh->vert.begin(), - pMesh->vert.end(), - [&](auto &v) { - 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->pre_previousTotalKineticEnergy = pMesh->previousTotalKineticEnergy; - pMesh->pre_previousTotalTranslationalKineticEnergy - = pMesh->previousTotalTranslationalKineticEnergy; - pMesh->pre_previousTotalRotationalKineticEnergy = pMesh->previousTotalRotationalKineticEnergy; - pMesh->previousTotalKineticEnergy = pMesh->currentTotalKineticEnergy; - pMesh->previousTotalTranslationalKineticEnergy = pMesh->currentTotalTranslationalKineticEnergy; - pMesh->previousTotalRotationalKineticEnergy = pMesh->currentTotalRotationalKineticEnergy; - pMesh->currentTotalKineticEnergy = 0; - pMesh->currentTotalTranslationalKineticEnergy = 0; - pMesh->currentTotalRotationalKineticEnergy = 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; - pMesh->currentTotalRotationalKineticEnergy += nodeRotationalKineticEnergy; - } - // 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; - pMesh->nodes[v].previousVelocity = 0; - } - pMesh->pre_previousTotalKineticEnergy = 0; - pMesh->pre_previousTotalTranslationalKineticEnergy = 0; - pMesh->pre_previousTotalRotationalKineticEnergy = 0; - pMesh->previousTotalKineticEnergy = 0; - pMesh->previousTotalTranslationalKineticEnergy = 0; - pMesh->previousTotalRotationalKineticEnergy = 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.dimensions.A - / elem.length; - translationalSumSk += SkTranslational; - const double lengthToThe3 = std::pow(elem.length, 3); - const double SkRotational_I2 = elem.material.youngsModulus * elem.dimensions.inertia.I2 - / lengthToThe3; // TODO: I2->t2,I3->t3,t1->polar inertia - const double SkRotational_I3 = elem.material.youngsModulus * elem.dimensions.inertia.I3 - / lengthToThe3; // TODO: I2->t2,I3->t3,t1->polar inertia - const double SkRotational_J = elem.material.youngsModulus * elem.dimensions.inertia.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.previousVelocity = node.velocity; - 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.pJob = pJob; - results.history = history; - results.displacements = displacements; - results.setLabelPrefix("DRM"); - - if (mSettings.maxDRMIterations.has_value() - && mCurrentSimulationStep == mSettings.maxDRMIterations && mCurrentSimulationStep != 0) { - if (mSettings.beVerbose) { - std::cout << "Did not reach equilibrium before reaching the maximum number " - "of DRM steps (" - << mSettings.maxDRMIterations.value() << "). 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.debugModeStep.has_value()) { - draw(pJob); - } -#endif - if (!std::isnan(pMesh->currentTotalKineticEnergy)) { - results.debug_drmDisplacements = results.displacements; - results.internalPotentialEnergy = computeTotalInternalPotentialEnergy(); - -#ifdef COMPUTE_INTERNAL_FORCES - results.perVertexInternalForces = computeInternalForces(pJob->constrainedVertices); -// std::vector perVertexInternalForces_axial = [=]() { -// std::vector axialInternalForces; -// axialInternalForces.reserve(results.perVertexInternalForces.size()); -// for (const std::array &internalForces : results.perVertexInternalForces) { -// axialInternalForces.push_back(internalForces[0].norm()); -// } -// return axialInternalForces; -// }(); -// const auto pCurvNet = pMesh->registerForDrawing(); -// pCurvNet->addNodeScalarQuantity("axial forces", perVertexInternalForces_axial); -// std::vector perVertexInternalForces_torsion = [=]() { -// std::vector axialInternalForces; -// axialInternalForces.reserve(results.perVertexInternalForces.size()); -// for (const std::array &internalForces : results.perVertexInternalForces) { -// axialInternalForces.push_back(internalForces[1].norm()); -// } -// return axialInternalForces; -// }(); -// pCurvNet->addNodeScalarQuantity("torsional forces", perVertexInternalForces_torsion); -// std::vector perVertexInternalForces_firstBending = [=]() { -// std::vector axialInternalForces; -// axialInternalForces.reserve(results.perVertexInternalForces.size()); -// for (const std::array &internalForces : results.perVertexInternalForces) { -// axialInternalForces.push_back(internalForces[2].norm()); -// } -// return axialInternalForces; -// }(); -// pCurvNet->addNodeScalarQuantity("first bending forces", -// perVertexInternalForces_firstBending); -// std::vector perVertexInternalForces_secondBending = [=]() { -// std::vector axialInternalForces; -// axialInternalForces.reserve(results.perVertexInternalForces.size()); -// for (const std::array &internalForces : results.perVertexInternalForces) { -// axialInternalForces.push_back(internalForces[3].norm()); -// } -// return axialInternalForces; -// }(); -// pCurvNet->addNodeScalarQuantity("second bending forces", -// perVertexInternalForces_secondBending); -// polyscope::show(); -#endif - results.rotationalDisplacementQuaternion.resize(pMesh->VN()); - results.debug_q_f1.resize(pMesh->VN()); - results.debug_q_normal.resize(pMesh->VN()); - 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] - //Eigen::Quaterniond R - = (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] - // R - .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++; - } - } - - results.simulationModelUsed = label; - - 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 << std::endl; - printCurrentState(); -} - -#ifdef POLYSCOPE_DEFINED -void DRMSimulationModel::draw(const std::shared_ptr &pJob, - const std::string &screenshotsFolder) -{ - // update positions - // polyscope::getCurveNetwork("Undeformed edge mesh")->setEnabled(false); - auto deformedMeshPolyscopeLabel = meshPolyscopeLabel + "_" + pMesh->getLabel(); - polyscope::CurveNetwork *meshPolyscopeHandle = polyscope::getCurveNetwork( - deformedMeshPolyscopeLabel); - 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->addNodeScalarQuantity("Internal force norm", internalForcesNorm) - ->setEnabled(true); - meshPolyscopeHandle->setRadius(0.002); - // meshPolyscopeHandle->addNodeVectorQuantity("External force", externalForces)->setEnabled(true); - // meshPolyscopeHandle->addNodeVectorQuantity("External Moments", externalMoments)->setEnabled(true); - pJob->registerForDrawing(deformedMeshPolyscopeLabel, true); - // 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(deformedMeshPolyscopeLabel) - ->addNodeScalarQuantity("nR", nRs) - ->setEnabled(false); - // polyscope::getCurveNetwork(meshPolyscopeLabel) - // ->addNodeScalarQuantity("theta3", theta3) - // ->setEnabled(false); - // polyscope::getCurveNetwork(meshPolyscopeLabel) - // ->addNodeScalarQuantity("theta2", theta2) - // ->setEnabled(false); - polyscope::getCurveNetwork(deformedMeshPolyscopeLabel) - ->addNodeVectorQuantity("Residual force", residualForces) - ->setEnabled(false); - polyscope::getCurveNetwork(deformedMeshPolyscopeLabel) - ->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].dimensions.A; - J[ei] = pMesh->elements[e].dimensions.inertia.J; - I2[ei] = pMesh->elements[e].dimensions.inertia.I2; - I3[ei] = pMesh->elements[e].dimensions.inertia.I3; - } - - polyscope::getCurveNetwork(deformedMeshPolyscopeLabel)->addEdgeScalarQuantity("A", A); - polyscope::getCurveNetwork(deformedMeshPolyscopeLabel)->addEdgeScalarQuantity("J", J); - polyscope::getCurveNetwork(deformedMeshPolyscopeLabel)->addEdgeScalarQuantity("I2", I2); - polyscope::getCurveNetwork(deformedMeshPolyscopeLabel)->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. - - static int debugModeStep = mSettings.debugModeStep.has_value() - ? mSettings.debugModeStep.value() - : 0; - if (ImGui::InputInt("Simulation debug step", - &debugModeStep)) { // set a int variable - if (debugModeStep != 0) { - *mSettings.debugModeStep = debugModeStep; - } - } - // 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(); -} -void DRMSimulationModel::applyKineticDamping(const std::shared_ptr &pJob) -{ - // if (!mSettings.viscousDampingFactor.has_value()) { - // const bool shouldCapDisplacements = mSettings.displacementCap.has_value(); - // const double &KE1 = pMesh->pre_previousTotalKineticEnergy; - // const double &KE2 = pMesh->previousTotalKineticEnergy; - // const double &KE3 = pMesh->currentTotalKineticEnergy; - const double &KE1 = pMesh->pre_previousTotalTranslationalKineticEnergy; - const double &KE2 = pMesh->previousTotalTranslationalKineticEnergy; - const double &KE3 = pMesh->currentTotalTranslationalKineticEnergy; - const double bitaDt = 0.5 * Dt * (3 + (-3 * KE1 + 4 * KE2 - KE3) / (KE1 - 2 * KE2 + KE3)); - 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()); - // } - - // Vector6d stepDisplacement = node.velocity * 0.5 * Dt; - Vector6d stepDisplacement = node.velocity * Dt + node.previousVelocity * bitaDt; - - node.displacements = node.displacements - stepDisplacement; - } - applyDisplacements(constrainedVertices); - if (!pJob->nodalForcedDisplacements.empty()) { - applyForcedDisplacements( - - pJob->nodalForcedDisplacements); - } - updateElementalLengths(); - // } - // 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); - // } - resetVelocities(); - ++numOfDampings; -} - -void DRMSimulationModel::updateDerivatives() -{ - updateNormalDerivatives(); - updateT1Derivatives(); - updateRDerivatives(); - updateT2Derivatives(); - updateT3Derivatives(); -} - -SimulationResults DRMSimulationModel::executeSimulation(const std::shared_ptr &pJob) -{ - auto beginTime = std::chrono::high_resolution_clock::now(); - - updateNodalMasses(); - updateNodalExternalForces(pJob->nodalExternalForces, constrainedVertices); - if (!pJob->nodalExternalForces.empty() - && !mSettings.totalResidualForcesNormThreshold.has_value()) { - mSettings.totalResidualForcesNormThreshold - = mSettings.totalExternalForcesNormPercentageTermination - * pMesh->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.averageResidualForcesCriterionThreshold.has_value()) { - std::cout << "average/extNorm threshold:" - << *mSettings.averageResidualForcesCriterionThreshold << std::endl; - } - } - - if (mSettings.beVerbose) { - std::cout << "Executing simulation for mesh with:" << pMesh->VN() << " nodes and " - << pMesh->EN() << " elements." << std::endl; - } - - const size_t movingAverageSampleSize = 200; - std::queue residualForcesMovingAverageHistorySample; - std::vector percentageOfConvergence; - // double residualForcesMovingAverageDerivativeMax = 0; - while (!mSettings.maxDRMIterations.has_value() - || mCurrentSimulationStep < mSettings.maxDRMIterations.value()) { - updateDerivatives(); - 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); - } - 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; - } - - if (minTotalResidualForcesNorm > pMesh->totalResidualForcesNorm) { - minTotalResidualForcesNorm = pMesh->totalResidualForcesNorm; - static int lastSavedStep = mCurrentSimulationStep; - if (mSettings.saveIntermediateBestStates.has_value() - && mSettings.saveIntermediateBestStates.value() - && mCurrentSimulationStep - lastSavedStep > 20000) { - lastSavedStep = mCurrentSimulationStep; - computeResults(pJob).save("./IntermediateResults_" + pJob->getLabel()); - } - } - - // pMesh->previousTotalPotentialEnergykN = - // pMesh->currentTotalPotentialEnergykN; - // pMesh->currentTotalPotentialEnergykN = computeTotalPotentialEnergy() / 1000; - // timePerNodePerIterationHistor.push_back(timePerNodePerIteration); - if (mSettings.beVerbose && mSettings.debugModeStep.has_value() - && mCurrentSimulationStep % mSettings.debugModeStep.value() == 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.averageResidualForcesCriterionThreshold.has_value()) { - std::cout << "Best percentage of target (average):" - << 100 * mSettings.averageResidualForcesCriterionThreshold.value() - * pMesh->totalExternalForcesNorm * pMesh->VN() - / minTotalResidualForcesNorm - << "%" << std::endl; - } else { - std::cout << "Best percentage of target:" - << 100 * mSettings.totalExternalForcesNormPercentageTermination - * pMesh->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.debugModeStep.has_value()) - && mCurrentSimulationStep != 0) { - //normalized sum of displacements - double sumOfDisplacementsNorm = 0; - for (size_t vi = 0; vi < pMesh->VN(); vi++) { - sumOfDisplacementsNorm += pMesh->nodes[vi].displacements.getTranslation().norm(); - } - pMesh->perVertexAverageNormalizedDisplacementNorm = sumOfDisplacementsNorm - / (pMesh->bbox.Diag() - * pMesh->VN()); - - //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; - } - history.stepPulse(*pMesh); - // percentageOfConvergence.push_back(100 * mSettings.totalResidualForcesNormThreshold - // / pMesh->totalResidualForcesNorm); - } - - if (mSettings.shouldCreatePlots && mSettings.debugModeStep.has_value() - && mCurrentSimulationStep % mSettings.debugModeStep.value() == 0) { - // SimulationResultsReporter::createPlot("Number of Steps", - // "Residual Forces mov aver deriv", - // history - // .residualForcesMovingAverageDerivativesLog); - // SimulationResultsReporter::createPlot("Number of Steps", - // "Residual Forces mov aver", - // history.residualForcesMovingAverage, - // {}, - // history.redMarks); - // SimulationResultsReporter::createPlot("Number of Steps", - // "Log of Sum of axial forces norms", - // history.logOfSumOfAxialForcesNorm, - // {}, - // history.redMarks); - // SimulationResultsReporter::createPlot("Number of Steps", - // "Log of Sum of torsion forces norms", - // history.logOfSumOfTorsionForcesNorm, - // {}, - // history.redMarks); - // SimulationResultsReporter::createPlot("Number of Steps", - // "Log of Sum of 1stBending forces norms", - // history.logOfSumOfFirstBendingForcesNorm, - // {}, - // history.redMarks); - // SimulationResultsReporter::createPlot("Number of Steps", - // "Log of Sum of 2ndBending forces norms", - // history.logOfSumOfSecondBendingForcesNorm, - // {}, - // history.redMarks); - // 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", - // "Per vertex average normalized displacement norms", - // history.perVertexAverageNormalizedDisplacementNorm /*, - // 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.debugModeStep.has_value() - && mCurrentSimulationStep % mSettings.debugModeStep.value() == 0) /* && - -currentSimulationStep > maxDRMIterations*/ - { - // std::string saveTo = std::filesystem::current_path() - // .append("Debugging_files") - // .append("Screenshots") - // .string(); - draw(pJob); - // 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.averageResidualForcesCriterionThreshold.has_value() - && pMesh->totalResidualForcesNorm / pMesh->totalExternalForcesNorm - < mSettings.totalExternalForcesNormPercentageTermination; - const bool fullfillsAverageResidualForcesNormTerminationCriterion - = mSettings.averageResidualForcesCriterionThreshold.has_value() - && (pMesh->totalResidualForcesNorm / pMesh->VN()) / pMesh->totalExternalForcesNorm - < mSettings.averageResidualForcesCriterionThreshold.value(); - const bool fullfillsResidualForcesNormThreshold - = mSettings.totalResidualForcesNormThreshold.has_value() - && pMesh->totalResidualForcesNorm < mSettings.totalResidualForcesNormThreshold; - if ((fullfillsAverageResidualForcesNormTerminationCriterion - || fullfillsResidualForcesNormTerminationCriterion - || fullfillsResidualForcesNormThreshold) - && numOfDampings > 0 - && (pJob->nodalForcedDisplacements.empty() - || mCurrentSimulationStep > mSettings.gradualForcedDisplacementSteps)) { - 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 (fullfillsAverageResidualForcesNormTerminationCriterion) { - std::cout << "Converged using average residual forces norm threshold criterion" - << std::endl; - } - } - break; - } - - const bool isKineticEnergyPeak - = /* pMesh->previousTotalKineticEnergy > pMesh->currentTotalKineticEnergy - ||*/ - (mSettings.useTotalRotationalKineticEnergyForKineticDamping - && pMesh->previousTotalRotationalKineticEnergy - > pMesh->currentTotalRotationalKineticEnergy) - || (mSettings.useTranslationalKineticEnergyForKineticDamping - && pMesh->previousTotalTranslationalKineticEnergy - > pMesh->currentTotalTranslationalKineticEnergy); - if (isKineticEnergyPeak) { - const bool fullfillsKineticEnergyTerminationCriterion - = mSettings.totalTranslationalKineticEnergyThreshold.has_value() - && pMesh->currentTotalTranslationalKineticEnergy - < mSettings.totalTranslationalKineticEnergyThreshold.value() - && 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 - ; - if (shouldTerminate) { - if (mSettings.beVerbose /*&& !mSettings.isDebugMode*/) { - std::cout << "Simulation converged." << std::endl; - printCurrentState(); - if (fullfillsKineticEnergyTerminationCriterion) { - std::cout << "The kinetic energy of the system was " - " used as a convergence 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; - // } - // } - // } - } - - // if (mSettings.useTranslationalKineticEnergyForKineticDamping) { - applyKineticDamping(pJob); - Dt *= mSettings.xi; - if (mSettings.shouldCreatePlots) { - history.redMarks.push_back(mCurrentSimulationStep); - } - // } - // if (mSettings.isDebugMode) { - // std::cout << Dt << std::endl; - // } - // std::cout << "Number of dampings:" << numOfDampings << endl; - // draw(); - } - } - //#ifdef USE_ENSMALLEN - // } - //#endif - auto endTime = std::chrono::high_resolution_clock::now(); - SimulationResults results = computeResults(pJob); - results.executionTime - = std::chrono::duration_cast(endTime - beginTime).count(); - - if (!mSettings.debugModeStep.has_value() && mSettings.shouldCreatePlots) { - SimulationResultsReporter reporter; - reporter.reportResults({results}, "Results", pJob->pMesh->getLabel()); - } - -#ifdef POLYSCOPE_DEFINED - if (mSettings.shouldDraw || mSettings.debugModeStep.has_value()) { - polyscope::removeCurveNetwork(meshPolyscopeLabel + "_" + pMesh->getLabel()); - polyscope::removeCurveNetwork("Initial_" + meshPolyscopeLabel + "_" + pMesh->getLabel()); - } -#endif - return results; -} - -void DRMSimulationModel::setStructure(const std::shared_ptr &pMesh) -{ - std::cout << "This function is currently not implemented" << std::endl; - assert(false); - std::terminate(); -} - -SimulationResults DRMSimulationModel::executeSimulation(const std::shared_ptr &pJob, - const Settings &settings, - const SimulationResults &solutionGuess) -{ - std::chrono::steady_clock::time_point begin = std::chrono::steady_clock::now(); - reset(pJob, settings); - assert(pJob->pMesh != nullptr); - - if (!solutionGuess.displacements.empty()) { - assert(!mSettings.linearGuessForceScaleFactor.has_value()); - applySolutionGuess(solutionGuess, pJob); - shouldTemporarilyDampForces = true; - } - - if (mSettings.linearGuessForceScaleFactor.has_value()) { - const double forceScaleFactor = mSettings.linearGuessForceScaleFactor.value(); - for (auto &externalForce : pJob->nodalExternalForces) { - externalForce.second = externalForce.second * forceScaleFactor; - } - LinearSimulationModel linearSimulationModel; - SimulationResults simulationResults_fullPatternLinearModel = linearSimulationModel - .executeSimulation(pJob); - - if (simulationResults_fullPatternLinearModel.converged) { - // simulationResults_fullPatternLinearModel.save(fullResultsFolderPath); - for (auto &externalForce : pJob->nodalExternalForces) { - externalForce.second = externalForce.second / forceScaleFactor; - } - - applySolutionGuess(simulationResults_fullPatternLinearModel, pJob); - shouldTemporarilyDampForces = true; - } - } - - SimulationResults results = executeSimulation(pJob); //calling the virtual function - std::chrono::steady_clock::time_point end = std::chrono::steady_clock::now(); - if (settings.beVerbose) { - std::cout << "Simulation converged in " - << std::chrono::duration_cast(end - begin).count() - / (60 * 1000.0) - << " min" << std::endl; - } - - return results; -} - -//#ifdef USE_ENSMALLEN - -//void DRMSimulationModel::setJob(const std::shared_ptr &pJob) -//{ -// reset(pJob); - -// updateNodalExternalForces(pJob->nodalExternalForces, constrainedVertices); -// // arma::mat externalForces(pMesh->VN() * NumDoF); -// // for (int vi = 0; vi < pMesh->VN(); vi++) { -// // const Vector6d &nodeExternalForce = pMesh.vert[vi].node.externalForce; -// // for (DoFType dofi = DoF::Ux; dofi < DoF::NumDoF; dofi++) { -// // externalForces(vi * NumDoF + dofi) = nodeExternalForce[dofi]; -// // } -// // } -//} - -//SimulationMesh *DRMSimulationModel::getDeformedMesh(const arma::mat &x, -// const std::shared_ptr &pJob) -//{ -// for (int vi = 0; vi < pMesh->VN(); vi++) { -// Node &node = pMesh->nodes[vi]; -// for (DoFType dofi = DoF::Ux; dofi < DoF::NumDoF; dofi++) { -// node.displacements[dofi] = x(vi * DoF::NumDoF + dofi); -// } -// } - -// applyDisplacements(constrainedVertices); -// if (!pJob->nodalForcedDisplacements.empty()) { -// applyForcedDisplacements(pJob->nodalForcedDisplacements); -// } - -// return pMesh.get(); -//} - -//double DRMSimulationModel::Evaluate(const arma::mat &x) -//{ -// for (int vi = 0; vi < pMesh->VN(); vi++) { -// Node &node = pMesh->nodes[vi]; -// for (DoFType dofi = DoF::Ux; dofi < DoF::NumDoF; dofi++) { -// node.displacements[dofi] = x(vi * DoF::NumDoF + dofi); -// } -// } - -// applyDisplacements(constrainedVertices); -// if (!pJob->nodalForcedDisplacements.empty()) { -// applyForcedDisplacements(pJob->nodalForcedDisplacements); -// } -// updateElementalLengths(); -// updateNormalDerivatives(); -// updateT1Derivatives(); -// updateRDerivatives(); -// updateT2Derivatives(); -// updateT3Derivatives(); -// updateResidualForcesOnTheFly(constrainedVertices); -// const double PE = computeTotalPotentialEnergy(); -// // const double PE = computeTotalInternalPotentialEnergy(); -// // std::cout << "PE:" << PE << std::endl; -// return pMesh->totalResidualForcesNorm; -//} - -//double DRMSimulationModel::EvaluateWithGradient(const arma::mat &x, arma::mat &g) -//{ -// for (int vi = 0; vi < pMesh->VN(); vi++) { -// Node &node = pMesh->nodes[vi]; -// for (DoFType dofi = DoF::Ux; dofi < DoF::NumDoF; dofi++) { -// node.displacements[dofi] = x(vi * DoF::NumDoF + dofi); -// } -// } - -// applyDisplacements(constrainedVertices); -// if (!pJob->nodalForcedDisplacements.empty()) { -// applyForcedDisplacements(pJob->nodalForcedDisplacements); -// } -// updateElementalLengths(); -// updateNormalDerivatives(); -// updateT1Derivatives(); -// updateRDerivatives(); -// updateT2Derivatives(); -// updateT3Derivatives(); -// updateResidualForcesOnTheFly(constrainedVertices); -// for (int vi = 0; vi < pMesh->VN(); vi++) { -// Node &node = pMesh->nodes[vi]; -// for (DoFType dofi = DoF::Ux; dofi < DoF::NumDoF; dofi++) { -// g(vi * NumDoF + dofi) = node.force.residual[dofi]; -// } -// } -// mCurrentSimulationStep++; -// const double PE = computeTotalPotentialEnergy(); -// // const double PE = computeTotalInternalPotentialEnergy(); -// std::cout << "PE:" << PE << std::endl; -// double sumOfDisplacements = 0; -// for (int xi = 0; xi < pJob->pMesh->VN() * NumDoF; xi++) { -// sumOfDisplacements += x(xi) * x(xi); -// } -// std::cout << "Sum of disp:" << std::sqrt(sumOfDisplacements) << std::endl; -// return PE; -//} - -//#endif - -void DRMSimulationModel::Settings::save(const std::filesystem::path &jsonFilePath) const -{ - nlohmann::json json; - json[GET_VARIABLE_NAME(shouldDraw)] = shouldDraw; - json[GET_VARIABLE_NAME(beVerbose)] = beVerbose; - json[GET_VARIABLE_NAME(shouldCreatePlots)] = shouldCreatePlots; - json[GET_VARIABLE_NAME(Dtini)] = Dtini; - json[GET_VARIABLE_NAME(xi)] = xi; - json[GET_VARIABLE_NAME(gamma)] = gamma; - json[GET_VARIABLE_NAME(useTranslationalKineticEnergyForKineticDamping)] - = useTranslationalKineticEnergyForKineticDamping; - if (maxDRMIterations.has_value()) { - json[GET_VARIABLE_NAME(jsonLabels.maxDRMIterations)] = maxDRMIterations.value(); - } - if (debugModeStep.has_value()) { - json[GET_VARIABLE_NAME(debugModeStep)] = debugModeStep.value(); - } - if (totalTranslationalKineticEnergyThreshold.has_value()) { - json[GET_VARIABLE_NAME(totalTranslationalKineticEnergyThreshold)] - = totalTranslationalKineticEnergyThreshold.value(); - } - if (averageResidualForcesCriterionThreshold.has_value()) { - json[GET_VARIABLE_NAME(averageResidualForcesCriterionThreshold)] - = averageResidualForcesCriterionThreshold.value(); - } - if (linearGuessForceScaleFactor.has_value()) { - json[GET_VARIABLE_NAME(linearGuessForceScaleFactor)] = linearGuessForceScaleFactor.value(); - } - if (viscousDampingFactor.has_value()) { - json[GET_VARIABLE_NAME(viscousDampingFactor)] = viscousDampingFactor.value(); - } - json[GET_VARIABLE_NAME(useTranslationalKineticEnergyForKineticDamping)] - = useTranslationalKineticEnergyForKineticDamping; - // if (intermediateResultsSaveStep.has_value()) { - // json[GET_VARIABLE_NAME(intermediateResultsSaveStep)] = intermediateResultsSaveStep.value(); - // } - - // if (saveIntermediateBestStates.has_value()) { - // json[GET_VARIABLE_NAME(saveIntermediateBestStates)] = saveIntermediateBestStates.value() - // ? "true" - // : "false"; - // } - - std::filesystem::create_directories(jsonFilePath.parent_path()); - std::ofstream jsonFile(jsonFilePath); - std::cout << "Saving DRM settings to:" << jsonFilePath << std::endl; - jsonFile << json; - jsonFile.close(); -} - -bool DRMSimulationModel::Settings::load(const std::filesystem::path &jsonFilePath) -{ - if (!std::filesystem::exists(std::filesystem::path(jsonFilePath))) { - std::cerr << "The json file does not exist. Json file provided:" << jsonFilePath.string() - << std::endl; - assert(false); - return false; - } - - if (std::filesystem::path(jsonFilePath).extension() != ".json") { - std::cerr << "A json file is expected as input. The given file has the " - "following extension:" - << std::filesystem::path(jsonFilePath).extension() << std::endl; - assert(false); - return false; - } - - nlohmann::json json; - std::ifstream ifs(jsonFilePath.string()); - ifs >> json; - - if (json.contains(GET_VARIABLE_NAME(shouldDraw))) { - shouldDraw = json.at(GET_VARIABLE_NAME(shouldDraw)) == "true" ? true : false; - } - if (json.contains(GET_VARIABLE_NAME(beVerbose))) { - beVerbose = json.at(GET_VARIABLE_NAME(beVerbose)) == "true" ? true : false; - } - if (json.contains(GET_VARIABLE_NAME(shouldCreatePlots))) { - shouldCreatePlots = json.at(GET_VARIABLE_NAME(shouldCreatePlots)) == "true" ? true : false; - } - if (json.contains(GET_VARIABLE_NAME(Dtini))) { - Dtini = json.at(GET_VARIABLE_NAME(Dtini)); - } - if (json.contains(GET_VARIABLE_NAME(xi))) { - xi = json.at(GET_VARIABLE_NAME(xi)); - } - if (json.contains(GET_VARIABLE_NAME(maxDRMIterations))) { - maxDRMIterations = json.at(GET_VARIABLE_NAME(maxDRMIterations)); - } - if (json.contains(GET_VARIABLE_NAME(debugModeStep))) { - debugModeStep = json.at(GET_VARIABLE_NAME(debugModeStep)); - } - if (json.contains(GET_VARIABLE_NAME(totalTranslationalKineticEnergyThreshold))) { - totalTranslationalKineticEnergyThreshold = json.at( - GET_VARIABLE_NAME(totalTranslationalKineticEnergyThreshold)); - } - if (json.contains(GET_VARIABLE_NAME(gamma))) { - gamma = json.at(GET_VARIABLE_NAME(gamma)); - } - if (json.contains(GET_VARIABLE_NAME(averageResidualForcesCriterionThreshold))) { - averageResidualForcesCriterionThreshold = json.at( - GET_VARIABLE_NAME(averageResidualForcesCriterionThreshold)); - } - if (json.contains(GET_VARIABLE_NAME(linearGuessForceScaleFactor))) { - linearGuessForceScaleFactor = json.at(GET_VARIABLE_NAME(linearGuessForceScaleFactor)); - } - - // if (json.contains(GET_VARIABLE_NAME(intermediateResultsSaveStep))) { - // intermediateResultsSaveStep = json.at(GET_VARIABLE_NAME(intermediateResultsSaveStep)); - // } - - if (json.contains(GET_VARIABLE_NAME(saveIntermediateBestStates))) { - saveIntermediateBestStates = json.at(GET_VARIABLE_NAME(saveIntermediateBestStates)) - == "true" - ? true - : false; - } - // std::cout << json.dump() << std::endl; - - return true; -} diff --git a/drmsimulationmodel.hpp b/drmsimulationmodel.hpp deleted file mode 100755 index b7e4f2a..0000000 --- a/drmsimulationmodel.hpp +++ /dev/null @@ -1,304 +0,0 @@ -#ifndef BEAMFORMFINDER_HPP -#define BEAMFORMFINDER_HPP - -//#ifdef USE_MATPLOT -//#include "matplot.h" -#include -//#endif -#include "simulationmesh.hpp" -#include "simulationmodel.hpp" -#include -#include -#include - -#ifdef USE_ENSMALLEN -#include -#include -#endif - -class 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 SimulationModel -{ -public: - struct Settings - { - bool useTranslationalKineticEnergyForKineticDamping{true}; - bool useTotalRotationalKineticEnergyForKineticDamping{false}; - bool shouldDraw{false}; - bool beVerbose{false}; - bool shouldCreatePlots{false}; - // int drawingStep{0}; - // double residualForcesMovingAverageDerivativeNormThreshold{1e-8}; - // double residualForcesMovingAverageNormThreshold{1e-8}; - double Dtini{0.1}; - double xi{0.9969}; - std::optional translationalKineticEnergyThreshold; - int gradualForcedDisplacementSteps{50}; - // int desiredGradualExternalLoadsSteps{1}; - double gamma{0.8}; - std::optional totalResidualForcesNormThreshold; - double totalExternalForcesNormPercentageTermination{1e-5}; - std::optional maxDRMIterations; - std::optional debugModeStep; - std::optional totalTranslationalKineticEnergyThreshold; - std::optional averageResidualForcesCriterionThreshold; - std::optional linearGuessForceScaleFactor; - // std::optional intermediateResultsSaveStep; - std::optional saveIntermediateBestStates; - std::optional viscousDampingFactor; - Settings() {} - void save(const std::filesystem::path &jsonFilePath) const; - bool load(const std::filesystem::path &filePath); - struct JsonLabels - { - const std::string shouldDraw{"shouldDraw"}; - const std::string beVerbose{"beVerbose"}; - const std::string shouldCreatePlots{"shouldCreatePlots"}; - const std::string Dtini{"DtIni"}; - const std::string xi{"xi"}; - const std::string gamma{"gamma"}; - const std::string totalResidualForcesNormThreshold; - const std::string maxDRMIterations{"maxIterations"}; - const std::string debugModeStep{"debugModeStep"}; - const std::string totalTranslationalKineticEnergyThreshold{ - "totalTranslationaKineticEnergyThreshold"}; - const std::string averageResidualForcesCriterionThreshold{ - "averageResidualForcesThreshold"}; - const std::string linearGuessForceScaleFactor{"linearGuessForceScaleFactor"}; - const std::string viscousDampingFactor{"viscousDampingFactor"}; - }; - static JsonLabels jsonLabels; - }; - inline const static std::string label{"DRM"}; - -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}; - 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}; - //#ifdef USE_ENSMALLEN - // std::shared_ptr pJob; - //#endif - void reset(const std::shared_ptr &pJob, const Settings &settings); - 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 std::shared_ptr &pJob, const std::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; - - - 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); - - void applyKineticDamping(const std::shared_ptr &pJob); - - virtual SimulationResults executeSimulation(const std::shared_ptr &pJob) override; - void setStructure(const std::shared_ptr &pMesh) override; - - void reset(const std::shared_ptr &pJob); - - std::vector> computeInternalForces( - const std::unordered_map> &fixedVertices); - - void updateDerivatives(); - - public: - DRMSimulationModel(); - SimulationResults executeSimulation(const std::shared_ptr &pJob, - const Settings &settings, - const SimulationResults &solutionGuess = SimulationResults()); - //#ifdef USE_ENSMALLEN - // std::shared_ptr pJob; - // double EvaluateWithGradient(const arma::mat &x, arma::mat &g); - // void setJob(const std::shared_ptr &pJob); - // SimulationMesh *getDeformedMesh(const arma::mat &x, const std::shared_ptr &pJob); - // double Evaluate(const arma::mat &x); - //#endif - - static void runUnitTests(); -}; - -inline DRMSimulationModel::Settings::JsonLabels DRMSimulationModel::Settings::jsonLabels; - -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/edgemesh.cpp b/edgemesh.cpp deleted file mode 100755 index 9346701..0000000 --- a/edgemesh.cpp +++ /dev/null @@ -1,608 +0,0 @@ -#include "edgemesh.hpp" -#include "vcg/simplex/face/topology.h" -#include -//#include -#include -#include - -Eigen::MatrixX2i VCGEdgeMesh::getEigenEdges() const { return eigenEdges; } - -std::vector VCGEdgeMesh::computeEdges() -{ - computeEdges(eigenEdges); - std::vector edges(eigenEdges.rows()); - for (int ei = 0; ei < eigenEdges.rows(); ei++) { - edges[ei] = vcg::Point2i(eigenEdges(ei, 0), eigenEdges(ei, 1)); - } - return edges; -} - -Eigen::MatrixX3d VCGEdgeMesh::getEigenVertices() const -{ - // getVertices(eigenVertices); - return eigenVertices; -} - -Eigen::MatrixX3d VCGEdgeMesh::getEigenEdgeNormals() const { - return eigenEdgeNormals; -} - -bool VCGEdgeMesh::save(const std::filesystem::path &meshFilePath) -{ - std::string filename = meshFilePath; - if (filename.empty()) { - filename = std::filesystem::current_path().append(getLabel() + ".ply").string(); - } else if (std::filesystem::is_directory(std::filesystem::path(meshFilePath))) { - filename = std::filesystem::path(meshFilePath).append(getLabel() + ".ply").string(); - } - assert(std::filesystem::path(filename).extension().string() == ".ply"); - unsigned int mask = 0; - mask |= vcg::tri::io::Mask::IOM_VERTCOORD; - mask |= vcg::tri::io::Mask::IOM_EDGEINDEX; - mask |= vcg::tri::io::Mask::IOM_VERTNORMAL; - mask |= vcg::tri::io::Mask::IOM_VERTCOLOR; - // if (nanoply::NanoPlyWrapper::SaveModel(filename.c_str(), *this, mask, false) != 0) { - if (std::filesystem::is_directory(meshFilePath.parent_path())) { - std::filesystem::create_directories(meshFilePath.parent_path()); - } - if (vcg::tri::io::Exporter::Save(*this, filename.c_str(), mask) != 0) { - return false; - } - return true; -} - -void VCGEdgeMesh::GeneratedRegularSquaredPattern( - const double angleDeg, std::vector> &pattern, - const size_t &desiredNumberOfSamples) { - static const size_t piSamples = 10; - - // generate a pattern in a 1x1 quad - const vcg::Point2d offset(0, 0); - - const size_t samplesNo = desiredNumberOfSamples; - // std::max(desiredNumberOfSamples, size_t(piSamples * (angleDeg / - // 180))); - const double angle = vcg::math::ToRad(angleDeg); - - pattern.clear(); - - // first arm - std::vector arm; - { - for (int k = 0; k <= samplesNo; k++) { - const double t = double(k) / samplesNo; - const double a = (1 - t) * angle; - // const double r = vcg::math::Sin(t*M_PI_2) /*(1-((1-t)*(1-t)))*/ * - // 0.5; - const double r = t * 0.5; // linear - - vcg::Point2d p(vcg::math::Cos(a), vcg::math::Sin(a)); - - arm.push_back((p * r)); - } - pattern.push_back(arm); - } - - // other arms - for (int i = 0; i < 3; i++) { - for (vcg::Point2d &p : arm) { - p = vcg::Point2d(-p.Y(), p.X()); - } - pattern.push_back(arm); - } - - assert(pattern.size() == 4); - - // offset all - for (auto &arm : pattern) { - for (vcg::Point2d &p : arm) { - p += offset; - } - } -} - -void VCGEdgeMesh::createSpiral(const float °reesOfArm, - const size_t &numberOfSamples) { - std::vector> spiralPoints; - GeneratedRegularSquaredPattern(degreesOfArm, spiralPoints, numberOfSamples); - - for (size_t armIndex = 0; armIndex < spiralPoints.size(); armIndex++) { - for (size_t pointIndex = 0; pointIndex < spiralPoints[armIndex].size() - 1; - pointIndex++) { - const vcg::Point2d p0 = spiralPoints[armIndex][pointIndex]; - const vcg::Point2d p1 = spiralPoints[armIndex][pointIndex + 1]; - CoordType n(0, 0, 1); - auto ei = vcg::tri::Allocator::AddEdge( - *this, VCGEdgeMesh::CoordType(p0.X(), p0.Y(), 0), - VCGEdgeMesh::CoordType(p1.X(), p1.Y(), 0)); - ei->cV(0)->N() = n; - ei->cV(1)->N() = n; - } - } - - // setDefaultAttributes(); -} - -bool VCGEdgeMesh::createSpanGrid(const size_t squareGridDimension) { - return createSpanGrid(squareGridDimension, squareGridDimension); -} - -bool VCGEdgeMesh::createSpanGrid(const size_t desiredWidth, - const size_t desiredHeight) { - std::cout << "Grid of dimensions:" << desiredWidth << "," << desiredHeight - << std::endl; - const VCGEdgeMesh::CoordType n(0, 0, 1); - int x = 0; - int y = 0; - // for (size_t vi = 0; vi < numberOfVertices; vi++) { - while (y <= desiredHeight) { - // std::cout << x << " " << y << std::endl; - auto p = VCGEdgeMesh::CoordType(x, y, 0); - vcg::tri::Allocator::AddVertex(*this, p, n); - x++; - if (x > desiredWidth) { - x = 0; - y++; - } - } - - for (size_t vi = 0; vi < VN(); vi++) { - int x = vi % (desiredWidth + 1); - int y = vi / (desiredWidth + 1); - const bool isCornerNode = (y == 0 && x == 0) || - (y == 0 && x == desiredWidth) || - (y == desiredHeight && x == 0) || - (y == desiredHeight && x == desiredWidth); - if (isCornerNode) { - continue; - } - if (y == 0) { // row 0.Connect with node above - vcg::tri::Allocator::AddEdge(*this, vi, - vi + desiredWidth + 1); - continue; - } else if (x == 0) { // col 0.Connect with node to the right - vcg::tri::Allocator::AddEdge(*this, vi, vi + 1); - continue; - } else if (y == desiredHeight) { // row 0.Connect with node below - // vcg::tri::Allocator::AddEdge(*this, vi, - // vi - (desiredWidth + - // 1)); - continue; - } else if (x == desiredWidth) { // row 0.Connect with node to the left - // vcg::tri::Allocator::AddEdge(*this, vi, vi - 1); - continue; - } - - vcg::tri::Allocator::AddEdge(*this, vi, vi + desiredWidth + 1); - vcg::tri::Allocator::AddEdge(*this, vi, vi + 1); - // vcg::tri::Allocator::AddEdge(*this, vi, - // vi - (desiredWidth + 1)); - // vcg::tri::Allocator::AddEdge(*this, vi, vi - 1); - } - - vcg::tri::Allocator::DeleteVertex(*this, vert[0]); - vcg::tri::Allocator::DeleteVertex(*this, vert[desiredWidth]); - vcg::tri::Allocator::DeleteVertex( - *this, vert[desiredHeight * (desiredWidth + 1)]); - vcg::tri::Allocator::DeleteVertex( - *this, vert[(desiredHeight + 1) * (desiredWidth + 1) - 1]); - vcg::tri::Allocator::CompactVertexVector(*this); - computeEdges(eigenEdges); - computeVertices(eigenVertices); - // vcg::tri::Allocator::CompactEdgeVector(*this); - - // const size_t numberOfEdges = - // desiredHeight * (desiredWidth - 1) + desiredWidth * (desiredHeight - - // 1); - // handleBeamDimensions._handle->data.resize( - // numberOfEdges, CylindricalElementDimensions(0.03, 0.026)); - // handleBeamMaterial._handle->data.resize(numberOfEdges, - // ElementMaterial(0.3, 200)); - - return true; -} - -bool VCGEdgeMesh::load(const std::filesystem::path &meshFilePath) -{ - std::string usedPath = meshFilePath; - if (std::filesystem::path(meshFilePath).is_relative()) { - usedPath = std::filesystem::absolute(meshFilePath).string(); - } - assert(std::filesystem::exists(usedPath)); - Clear(); - // if (!loadUsingNanoply(usedPath)) { - // std::cerr << "Error: Unable to open " + usedPath << std::endl; - // return false; - // } - if (!loadUsingDefaultLoader(usedPath)) { - std::cerr << "Error: Unable to open " + usedPath << std::endl; - return false; - } - - computeEdges(eigenEdges); - computeVertices(eigenVertices); - vcg::tri::UpdateTopology::VertexEdge(*this); - label = std::filesystem::path(meshFilePath).stem().string(); - - const bool printDebugInfo = false; - if (printDebugInfo) { - std::cout << meshFilePath << " was loaded successfuly." << std::endl; - std::cout << "Mesh has " << EN() << " edges." << std::endl; - } - - label = std::filesystem::path(meshFilePath).stem().string(); - return true; -} - -//bool VCGEdgeMesh::loadUsingNanoply(const std::string &plyFilename) { - -// this->Clear(); -// // assert(plyFileHasAllRequiredFields(plyFilename)); -// // Load the ply file -// unsigned int mask = 0; -// mask |= nanoply::NanoPlyWrapper::IO_VERTCOORD; -// mask |= nanoply::NanoPlyWrapper::IO_VERTNORMAL; -// mask |= nanoply::NanoPlyWrapper::IO_VERTCOLOR; -// mask |= nanoply::NanoPlyWrapper::IO_EDGEINDEX; -// if (nanoply::NanoPlyWrapper::LoadModel(plyFilename.c_str(), -// *this, mask) != 0) { -// return false; -// } -// return true; -//} - -bool VCGEdgeMesh::loadUsingDefaultLoader(const std::string &plyFilePath) -{ - Clear(); - // assert(plyFileHasAllRequiredFields(plyFilename)); - // Load the ply file - int mask = 0; - mask |= vcg::tri::io::Mask::IOM_VERTCOORD; - mask |= vcg::tri::io::Mask::IOM_VERTNORMAL; - mask |= vcg::tri::io::Mask::IOM_VERTCOLOR; - mask |= vcg::tri::io::Mask::IOM_EDGEINDEX; - // if (nanoply::NanoPlyWrapper::LoadModel(plyFilename.c_str(), - // *this, mask) != 0) { - const int loadErrorCode = vcg::tri::io::Importer::Open(*this, - plyFilePath.c_str(), - mask); - if (loadErrorCode != 0) { - std::cerr << vcg::tri::io::Importer::ErrorMsg(loadErrorCode) << std::endl; - return false; - } - return true; -} - -// bool VCGEdgeMesh::plyFileHasAllRequiredFields(const std::string -// &plyFilename) -// { -// const nanoply::Info info(plyFilename); -// const std::vector::const_iterator edgeElemIt = -// std::find_if(info.elemVec.begin(), info.elemVec.end(), -// [&](const nanoply::PlyElement &plyElem) { -// return plyElem.plyElem == nanoply::NNP_EDGE_ELEM; -// }); -// if (edgeElemIt == info.elemVec.end()) { -// std::cerr << "Ply file is missing edge elements." << std::endl; -// return false; -// } - -// const std::vector &edgePropertyVector = -// edgeElemIt->propVec; -// return hasPlyEdgeProperty(plyFilename, edgePropertyVector, -// plyPropertyBeamDimensionsID) && -// hasPlyEdgeProperty(plyFilename, edgePropertyVector, -// plyPropertyBeamMaterialID); -//} - -Eigen::MatrixX3d VCGEdgeMesh::getNormals() const { - Eigen::MatrixX3d vertexNormals; - vertexNormals.resize(VN(), 3); - - for (int vertexIndex = 0; vertexIndex < VN(); vertexIndex++) { - VCGEdgeMesh::CoordType vertexNormal = - vert[static_cast(vertexIndex)].cN(); - vertexNormals.row(vertexIndex) = - vertexNormal.ToEigenVector(); - } - - return vertexNormals; -} -void VCGEdgeMesh::computeEdges(Eigen::MatrixX3d &edgeStartingPoints, - Eigen::MatrixX3d &edgeEndingPoints) const -{ - edgeStartingPoints.resize(EN(), 3); - edgeEndingPoints.resize(EN(), 3); - for (int edgeIndex = 0; edgeIndex < EN(); edgeIndex++) { - const VCGEdgeMesh::EdgeType &edge = this->edge[edgeIndex]; - edgeStartingPoints.row(edgeIndex) = edge.cP(0).ToEigenVector(); - edgeEndingPoints.row(edgeIndex) = edge.cP(1).ToEigenVector(); - } -} - -VCGEdgeMesh::VCGEdgeMesh() {} - -void VCGEdgeMesh::updateEigenEdgeAndVertices() { -#ifdef POLYSCOPE_DEFINED - computeEdges(eigenEdges); - computeVertices(eigenVertices); -#endif -} - -bool VCGEdgeMesh::copy(const VCGEdgeMesh &mesh) -{ - vcg::tri::Append::MeshCopyConst(*this, mesh); - label = mesh.getLabel(); - eigenEdges = mesh.getEigenEdges(); - // assert(eigenEdges.rows() != 0); - // if (eigenEdges.rows() == 0) { - // getEdges(eigenEdges); - // } - eigenVertices = mesh.getEigenVertices(); - // assert(eigenVertices.rows() != 0); - // if (eigenVertices.rows() == 0) { - // getVertices(eigenVertices); - // } - vcg::tri::UpdateTopology::VertexEdge(*this); - - return true; -} - -void VCGEdgeMesh::set(const std::vector &vertexPositions, const std::vector &edges) -{ - Clear(); - for (int ei = 0; ei < edges.size(); ei += 2) { - const int vi0 = edges[ei]; - const int vi1 = edges[ei + 1]; - const int vi0_offset = 3 * vi0; - const int vi1_offset = 3 * vi1; - const CoordType p0(vertexPositions[vi0_offset], - vertexPositions[vi0_offset + 1], - vertexPositions[vi0_offset + 2]); - const CoordType p1(vertexPositions[vi1_offset], - vertexPositions[vi1_offset + 1], - vertexPositions[vi1_offset + 2]); - auto eIt = vcg::tri::Allocator::AddEdge(*this, p0, p1); - CoordType n(0, 0, 1); - eIt->cV(0)->N() = n; - eIt->cV(1)->N() = n; - } - // removeDuplicateVertices(); - - updateEigenEdgeAndVertices(); -} - -void VCGEdgeMesh::removeDuplicateVertices() -{ - vcg::tri::Clean::MergeCloseVertex(*this, 0.000061524); - vcg::tri::Allocator::CompactVertexVector(*this); - vcg::tri::Allocator::CompactEdgeVector(*this); - vcg::tri::UpdateTopology::VertexEdge(*this); -} - -void VCGEdgeMesh::removeDuplicateVertices( - vcg::tri::Allocator::PointerUpdater &pu_vertices, - vcg::tri::Allocator::PointerUpdater &pu_edges) -{ - vcg::tri::Clean::MergeCloseVertex(*this, 0.000061524); - vcg::tri::Allocator::CompactVertexVector(*this, pu_vertices); - vcg::tri::Allocator::CompactEdgeVector(*this, pu_edges); - vcg::tri::UpdateTopology::VertexEdge(*this); -} - -std::string VCGEdgeMesh::ToSvgText(const VCGEdgeMesh &edgeMesh) -{ - // Measures in mm (?) - constexpr double ImgPadding = 5; - //constexpr double PatternSize = 200 * 19.230769231; - //constexpr double PatternSize = 200 * 17.913461538; - constexpr double PatternSize = 1000; - constexpr double ImgSize = 2 * ImgPadding + PatternSize; - - constexpr double StrokeWidth = 5; - VCGEdgeMesh edgeMeshCopy; - edgeMeshCopy.copy(edgeMesh); - - vcg::tri::UpdateBounding::Box(edgeMeshCopy); - const double maxDim = edgeMeshCopy.bbox.Dim()[edgeMeshCopy.bbox.MaxDim()]; - vcg::tri::UpdatePosition::Translate( - edgeMeshCopy, - VCGEdgeMesh::CoordType(maxDim / 2.0 - edgeMeshCopy.bbox.Center().X(), - maxDim / 2.0 - edgeMeshCopy.bbox.Center().Y(), - 0)); - vcg::tri::UpdatePosition::Scale(edgeMeshCopy, PatternSize / maxDim); - - // debug - // vcg::tri::UpdateBounding::Box(em); - // std::cout << "pattern size " - // << em.bbox.min.X() << " " - // << em.bbox.max.X() << " " - // << em.bbox.min.Y() << " " - // << em.bbox.max.Y() << " " << std::endl; - - std::stringstream ss; - - // svg header - ss << "" << std::endl - << "" - << std::endl; - - // size & background - ss << "" - << std::endl; - ss << "" << std::endl; - - for (const auto &e : edgeMeshCopy.edge) { - const auto &p0 = e.cP(0); - const auto &p1 = e.cP(1); - ss << "" << std::endl; - } - - ss << "" << std::endl; - return ss.str(); -} - -void VCGEdgeMesh::writeToSvg(const std::filesystem::path &writeToPath) const -{ - // retrieve filepath for saving svg - const std::filesystem::path svgPath = [=]() { - if (writeToPath.empty()) { - return std::filesystem::current_path().append(getLabel()).concat(".svg"); - } - return std::filesystem::absolute(writeToPath); - }(); - // save to svg file - std::cout << "saving to " << svgPath << std::endl; - - std::ofstream ofs(svgPath); - if (!ofs.is_open()) { - std::cout << "unable to save to " << svgPath << std::endl; - assert(false); - return; - } - - ofs << ToSvgText(*this); - ofs.close(); -} - -void VCGEdgeMesh::deleteDanglingVertices() { - vcg::tri::Allocator::PointerUpdater pu; - deleteDanglingVertices(pu); -} - -void VCGEdgeMesh::deleteDanglingVertices(vcg::tri::Allocator::PointerUpdater &pu) { - for (VertexType &v : vert) { - std::vector incidentElements; - vcg::edge::VEStarVE(&v, incidentElements); - if (incidentElements.size() == 0 && !v.IsD()) { - vcg::tri::Allocator::DeleteVertex(*this, v); - } - } - - vcg::tri::Allocator::CompactVertexVector(*this, pu); - vcg::tri::Allocator::CompactEdgeVector(*this); - - updateEigenEdgeAndVertices(); -} - -void VCGEdgeMesh::computeVertices(Eigen::MatrixX3d &vertices) -{ - vertices = Eigen::MatrixX3d(); - vertices.resize(VN(), 3); - for (int vi = 0; vi < VN(); vi++) { - if (vert[vi].IsD()) { - continue; - } - VCGEdgeMesh::CoordType vertexCoordinates = - vert[static_cast(vi)].cP(); - vertices.row(vi) = vertexCoordinates.ToEigenVector(); - } -} - -void VCGEdgeMesh::computeEdges(Eigen::MatrixX2i &edges) -{ - edges = Eigen::MatrixX2i(); - edges.resize(EN(), 2); - for (int edgeIndex = 0; edgeIndex < EN(); edgeIndex++) { - const VCGEdgeMesh::EdgeType &edge = this->edge[edgeIndex]; - assert(!edge.IsD()); - auto vp0 = edge.cV(0); - auto vp1 = edge.cV(1); - assert(vcg::tri::IsValidPointer(*this, vp0) && vcg::tri::IsValidPointer(*this, vp1)); - const size_t vi0 = vcg::tri::Index(*this, vp0); - const size_t vi1 = vcg::tri::Index(*this, vp1); - assert(vi0 != -1 && vi1 != -1); - edges.row(edgeIndex) = Eigen::Vector2i(vi0, vi1); - } -} - -void VCGEdgeMesh::printVertexCoordinates(const size_t &vi) const -{ - std::cout << "vi:" << vi << " " << vert[vi].cP()[0] << " " << vert[vi].cP()[1] << " " - << vert[vi].cP()[2] << std::endl; -} - -#ifdef POLYSCOPE_DEFINED -void VCGEdgeMesh::markVertices(const std::vector &vertsToMark) -{ - if (vertsToMark.empty()) { - return; - } - - std::vector> nodeColors(VN(), {0, 0, 0}); - for (const size_t vi : vertsToMark) { - nodeColors[vi] = {1, 0, 0}; - } - - polyscope::getCurveNetwork(getLabel()) - ->addNodeColorQuantity("Marked vertices" + getLabel(), nodeColors) - ->setEnabled(true); -} - -//TODO: make const getEigenVertices is not -polyscope::CurveNetwork *VCGEdgeMesh::registerForDrawing( - const std::optional> &desiredColor, - const double &desiredRadius, - const bool &shouldEnable) -{ - PolyscopeInterface::init(); - const double drawingRadius = desiredRadius; - updateEigenEdgeAndVertices(); - polyscope::CurveNetwork *polyscopeHandle_edgeMesh - = polyscope::registerCurveNetwork(label, getEigenVertices(), getEigenEdges()); - // std::cout << "EDGES:" << polyscopeHandle_edgeMesh->nEdges() << std::endl; - assert(polyscopeHandle_edgeMesh->nEdges() == getEigenEdges().rows() - && polyscopeHandle_edgeMesh->nNodes() == getEigenVertices().rows()); - - polyscopeHandle_edgeMesh->setEnabled(shouldEnable); - polyscopeHandle_edgeMesh->setRadius(drawingRadius, false); - if (desiredColor.has_value()) { - const glm::vec3 desiredColor_glm(desiredColor.value()[0], - desiredColor.value()[1], - desiredColor.value()[2]); - polyscopeHandle_edgeMesh->setColor(/*glm::normalize(*/ desiredColor_glm /*)*/); - } - - return polyscopeHandle_edgeMesh; -} - -void VCGEdgeMesh::unregister() const { - if (!polyscope::hasCurveNetwork(label)) { - std::cerr << "No curve network registered with a name: " << getLabel() - << std::endl; - std::cerr << "Nothing to remove." << std::endl; - return; - } - polyscope::removeCurveNetwork(label); -} - -void VCGEdgeMesh::drawInitialFrames(polyscope::CurveNetwork *polyscopeHandle_initialMesh) const -{ - Eigen::MatrixX3d frameInitialX(VN(), 3); - Eigen::MatrixX3d frameInitialY(VN(), 3); - Eigen::MatrixX3d frameInitialZ(VN(), 3); - for (int vi = 0; vi < VN(); vi++) { - frameInitialX.row(vi) = Eigen::Vector3d(1, 0, 0); - frameInitialY.row(vi) = Eigen::Vector3d(0, 1, 0); - frameInitialZ.row(vi) = Eigen::Vector3d(0, 0, 1); - } - polyscopeHandle_initialMesh->addNodeVectorQuantity("FrameX", frameInitialX) - ->setVectorColor(glm::vec3(1, 0, 0)) - ->setEnabled(true); - polyscopeHandle_initialMesh->addNodeVectorQuantity("FrameY", frameInitialY) - ->setVectorColor(glm::vec3(0, 1, 0)) - ->setEnabled(true); - polyscopeHandle_initialMesh->addNodeVectorQuantity("FrameZ", frameInitialZ) - ->setVectorColor(glm::vec3(0, 0, 1)) - ->setEnabled(true); -} -#endif diff --git a/edgemesh.hpp b/edgemesh.hpp deleted file mode 100755 index 9862526..0000000 --- a/edgemesh.hpp +++ /dev/null @@ -1,131 +0,0 @@ -#ifndef EDGEMESH_HPP -#define EDGEMESH_HPP -#include "beam.hpp" -#include "mesh.hpp" -#include "utilities.hpp" -#include -#include -#include -#include - -#ifdef POLYSCOPE_DEFINED -#include -#endif - -using EdgeIndex = size_t; - -class VCGEdgeMeshEdgeType; -class VCGEdgeMeshVertexType; - -struct VCGEdgeMeshUsedTypes -: public vcg::UsedTypes::AsVertexType, - vcg::Use::AsEdgeType> {}; - -class VCGEdgeMeshVertexType -: public vcg::Vertex {}; -class VCGEdgeMeshEdgeType -: public vcg::Edge {}; - -class VCGEdgeMesh - : public vcg::tri::TriMesh, std::vector>, - public Mesh -{ -protected: - Eigen::MatrixX2i eigenEdges; - Eigen::MatrixX3d eigenVertices; - Eigen::MatrixX3d eigenEdgeNormals; - - void computeEdges(Eigen::MatrixX2i &edges); - void computeVertices(Eigen::MatrixX3d &vertices); - -public: - VCGEdgeMesh(); - template - size_t getIndex(const MeshElement &meshElement) - { - return vcg::tri::Index(*this, meshElement); - } - void updateEigenEdgeAndVertices(); - /* - * The copy function shold be a virtual function of the base interface Mesh class. - * https://stackoverflow.com/questions/2354210/can-a-class-member-function-template-be-virtual - * use type erasure (?) - * */ - bool copy(const VCGEdgeMesh &mesh); - - void set(const std::vector &vertexPositions, const std::vector &edges); - - void removeDuplicateVertices(); - virtual void deleteDanglingVertices(); - virtual void deleteDanglingVertices( - vcg::tri::Allocator::PointerUpdater &pu); - - void computeEdges(Eigen::MatrixX3d &edgeStartingPoints, Eigen::MatrixX3d &edgeEndingPoints) const; - - Eigen::MatrixX3d getNormals() const; - - bool plyFileHasAllRequiredFields(const std::string &plyFilename); - - // bool loadUsingNanoply(const std::string &plyFilename); - - bool load(const std::filesystem::path &meshFilePath) override; - bool save(const std::filesystem::path &meshFilePath = std::filesystem::path()) override; - - bool createSpanGrid(const size_t squareGridDimension); - bool createSpanGrid(const size_t desiredWidth, const size_t desiredHeight); - void createSpiral(const float °reesOfArm, const size_t &numberOfSamples); - - Eigen::MatrixX2i getEigenEdges() const; - std::vector computeEdges(); - - Eigen::MatrixX3d getEigenVertices() const; - Eigen::MatrixX3d getEigenEdgeNormals() const; - void printVertexCoordinates(const size_t &vi) const; -#ifdef POLYSCOPE_DEFINED - polyscope::CurveNetwork *registerForDrawing( - const std::optional> &desiredColor = std::nullopt, - const double &desiredRadius = 0.001, - const bool &shouldEnable = true); - void unregister() const; - void drawInitialFrames(polyscope::CurveNetwork *polyscopeHandle_initialMesh) const; -#endif - void removeDuplicateVertices( - vcg::tri::Allocator::PointerUpdater &pu_vertices, - vcg::tri::Allocator::PointerUpdater &pu_edges); - - void centerMesh() - { - CoordType centerOfMass(0, 0, 0); - - for (const auto &v : vert) { - centerOfMass = centerOfMass + v.cP(); - } - centerOfMass = centerOfMass / VN(); - for (auto &v : vert) { - v.P() = v.cP() - centerOfMass; - } - } - - static std::string ToSvgText(const VCGEdgeMesh &edgeMesh); - void writeToSvg(const std::filesystem::path &writeToPath = std::filesystem::path()) const; - - void markVertices(const std::vector &vertsToMark); - -private: - void GeneratedRegularSquaredPattern(const double angleDeg, - std::vector> &pattern, - const size_t &desiredNumberOfSamples); - bool loadUsingDefaultLoader(const std::string &plyFilePath); -}; - -using VectorType = VCGEdgeMesh::CoordType; -using CoordType = VCGEdgeMesh::CoordType; -using VertexPointer = VCGEdgeMesh::VertexPointer; -using EdgePointer = VCGEdgeMesh::EdgePointer; -using ConstVCGEdgeMesh = VCGEdgeMesh; - -#endif // EDGEMESH_HPP diff --git a/linearsimulationmodel.cpp b/linearsimulationmodel.cpp deleted file mode 100644 index e1f19c6..0000000 --- a/linearsimulationmodel.cpp +++ /dev/null @@ -1,274 +0,0 @@ -#include "linearsimulationmodel.hpp" -//#include "gsl/gsl" - -std::vector LinearSimulationModel::getFeaElements( - const std::shared_ptr &pMesh) -{ - const int numberOfEdges = pMesh->EN(); - std::vector elements(numberOfEdges); - for (int edgeIndex = 0; edgeIndex < numberOfEdges; edgeIndex++) { - const SimulationMesh::CoordType &evn0 = pMesh->edge[edgeIndex].cV(0)->cN(); - const SimulationMesh::CoordType &evn1 = pMesh->edge[edgeIndex].cV(1)->cN(); - const std::vector nAverageVector{(evn0[0] + evn1[0]) / 2, - (evn0[1] + evn1[1]) / 2, - (evn0[2] + evn1[2]) / 2}; - const Element &element = pMesh->elements[edgeIndex]; - const double E = element.material.youngsModulus; - fea::Props feaProperties(E * element.dimensions.A, - E * element.dimensions.inertia.I3, - E * element.dimensions.inertia.I2, - element.material.G * element.dimensions.inertia.J, - nAverageVector); - const int vi0 = pMesh->getIndex(pMesh->edge[edgeIndex].cV(0)); - const int vi1 = pMesh->getIndex(pMesh->edge[edgeIndex].cV(1)); - elements[edgeIndex] = fea::Elem(vi0, vi1, feaProperties); - } - - return elements; -} - -std::vector LinearSimulationModel::getFeaNodes( - const std::shared_ptr &pMesh) -{ - const int numberOfNodes = pMesh->VN(); - std::vector feaNodes(numberOfNodes); - for (int vi = 0; vi < numberOfNodes; vi++) { - const CoordType &p = pMesh->vert[vi].cP(); - feaNodes[vi] = fea::Node(p[0], p[1], p[2]); - } - - return feaNodes; -} - -std::vector LinearSimulationModel::getFeaFixedNodes( - const std::shared_ptr &job) -{ - std::vector boundaryConditions; - // boundaryConditions.reserve(job->constrainedVertices.size() * 6 - // + job->nodalForcedDisplacements.size() * 3); - for (auto fixedVertex : job->constrainedVertices) { - const int vertexIndex = fixedVertex.first; - for (int dofIndex : fixedVertex.second) { - boundaryConditions.emplace_back( - fea::BC(vertexIndex, static_cast(dofIndex), 0)); - } - } - - for (auto forcedDisplacement : job->nodalForcedDisplacements) { - const int vi = forcedDisplacement.first; - for (int dofIndex = 0; dofIndex < 3; dofIndex++) { - boundaryConditions.emplace_back( - fea::BC(vi, static_cast(dofIndex), forcedDisplacement.second[dofIndex])); - } - } - - return boundaryConditions; -} - -std::vector LinearSimulationModel::getFeaNodalForces( - const std::shared_ptr &job) -{ - std::vector nodalForces; - nodalForces.reserve(job->nodalExternalForces.size() * 6); - - for (auto nodalForce : job->nodalExternalForces) { - for (int dofIndex = 0; dofIndex < 6; dofIndex++) { - if (nodalForce.second[dofIndex] == 0) { - continue; - } - fea::Force f(nodalForce.first, dofIndex, nodalForce.second[dofIndex]); - nodalForces.emplace_back(f); - } - } - - return nodalForces; -} - -SimulationResults LinearSimulationModel::getResults( - const fea::Summary &feaSummary, const std::shared_ptr &simulationJob) -{ - SimulationResults results; - results.converged = feaSummary.converged; - if (!results.converged) { - return results; - } - - results.executionTime = feaSummary.total_time_in_ms * 1000; - // displacements - results.displacements.resize(feaSummary.num_nodes); - results.rotationalDisplacementQuaternion.resize(feaSummary.num_nodes); - for (int vi = 0; vi < feaSummary.num_nodes; vi++) { - results.displacements[vi] = Vector6d(feaSummary.nodal_displacements[vi]); - - const Vector6d &nodalDisplacement = results.displacements[vi]; - Eigen::Quaternion q_nx; - q_nx = Eigen::AngleAxis(nodalDisplacement[3], Eigen::Vector3d(1, 0, 0)); - Eigen::Quaternion q_ny; - q_ny = Eigen::AngleAxis(nodalDisplacement[4], Eigen::Vector3d(0, 1, 0)); - Eigen::Quaternion q_nz; - q_nz = Eigen::AngleAxis(nodalDisplacement[5], Eigen::Vector3d(0, 0, 1)); - results.rotationalDisplacementQuaternion[vi] = q_nx * q_ny * q_nz; - } - results.setLabelPrefix("Linear"); - - // // Convert forces - // // Convert to vector of eigen matrices of the form force component-> per - // // Edge - // const int numDof = 6; - // const size_t numberOfEdges = feaSummary.element_forces.size(); - // edgeForces = - // std::vector(numDof, Eigen::VectorXd(2 * - // numberOfEdges)); - // for (gsl::index edgeIndex = 0; edgeIndex < numberOfEdges; edgeIndex++) { - // for (gsl::index forceComponentIndex = 0; forceComponentIndex < numDof; - // forceComponentIndex++) { - // (edgeForces[forceComponentIndex])(2 * edgeIndex) = - // feaSummary.element_forces[edgeIndex][forceComponentIndex]; - // (edgeForces[forceComponentIndex])(2 * edgeIndex + 1) = - // feaSummary.element_forces[edgeIndex][numDof + - // forceComponentIndex]; - // } - // } - - for (int ei = 0; ei < feaSummary.num_elems; ei++) { - const std::vector &elementForces = feaSummary.element_forces[ei]; - const Element &element = simulationJob->pMesh->elements[ei]; - //Axial - const double elementPotentialEnergy_axial_v0 = std::pow(elementForces[0], 2) - * element.initialLength - / (4 * element.material.youngsModulus - * element.dimensions.A); - const double elementPotentialEnergy_axial_v1 = std::pow(elementForces[6], 2) - * element.initialLength - / (4 * element.material.youngsModulus - * element.dimensions.A); - const double elementPotentialEnergy_axial = elementPotentialEnergy_axial_v0 - + elementPotentialEnergy_axial_v1; - //Shear - const double elementPotentialEnergy_shearY_v0 = std::pow(elementForces[1], 2) - * element.initialLength - / (4 * element.dimensions.A - * element.material.G); - const double elementPotentialEnergy_shearZ_v0 = std::pow(elementForces[2], 2) - * element.initialLength - / (4 * element.dimensions.A - * element.material.G); - const double elementPotentialEnergy_shearY_v1 = std::pow(elementForces[7], 2) - * element.initialLength - / (4 * element.dimensions.A - * element.material.G); - const double elementPotentialEnergy_shearZ_v1 = std::pow(elementForces[8], 2) - * element.initialLength - / (4 * element.dimensions.A - * element.material.G); - const double elementPotentialEnergy_shear = elementPotentialEnergy_shearY_v0 - + elementPotentialEnergy_shearZ_v0 - + elementPotentialEnergy_shearY_v1 - + elementPotentialEnergy_shearZ_v1; - //Bending - const double elementPotentialEnergy_bendingY_v0 = std::pow(elementForces[4], 2) - * element.initialLength - / (4 * element.material.youngsModulus - * element.dimensions.inertia.I2); - const double elementPotentialEnergy_bendingZ_v0 = std::pow(elementForces[5], 2) - * element.initialLength - / (4 * element.material.youngsModulus - * element.dimensions.inertia.I3); - const double elementPotentialEnergy_bendingY_v1 = std::pow(elementForces[10], 2) - * element.initialLength - / (4 * element.material.youngsModulus - * element.dimensions.inertia.I2); - const double elementPotentialEnergy_bendingZ_v1 = std::pow(elementForces[11], 2) - * element.initialLength - / (4 * element.material.youngsModulus - * element.dimensions.inertia.I3); - const double elementPotentialEnergy_bending = elementPotentialEnergy_bendingY_v0 - + elementPotentialEnergy_bendingZ_v0 - + elementPotentialEnergy_bendingY_v1 - + elementPotentialEnergy_bendingZ_v1; - //Torsion - const double elementPotentialEnergy_torsion_v0 = std::pow(elementForces[3], 2) - * element.initialLength - / (4 * element.dimensions.inertia.J - * element.material.G); - const double elementPotentialEnergy_torsion_v1 = std::pow(elementForces[9], 2) - * element.initialLength - / (4 * element.dimensions.inertia.J - * element.material.G); - const double elementPotentialEnergy_torsion = elementPotentialEnergy_torsion_v0 - + elementPotentialEnergy_torsion_v1; - const double elementInternalPotentialEnergy = elementPotentialEnergy_axial - + elementPotentialEnergy_shear - + elementPotentialEnergy_bending - + elementPotentialEnergy_torsion; - results.internalPotentialEnergy += elementInternalPotentialEnergy; - } - results.pJob = simulationJob; - - return results; -} - -SimulationResults LinearSimulationModel::executeSimulation( - const std::shared_ptr &pSimulationJob) -{ - assert(pSimulationJob->pMesh->VN() != 0); - const bool wasInitializedWithDifferentStructure = pSimulationJob->pMesh->EN() - != simulator.structure.elems.size() - || pSimulationJob->pMesh->VN() - != simulator.structure.nodes.size(); - if (!simulator.wasInitialized() || wasInitializedWithDifferentStructure) { - setStructure(pSimulationJob->pMesh); - } - // printInfo(job); - - // create the default options - fea::Options opts; - opts.save_elemental_forces = false; - opts.save_nodal_displacements = false; - opts.save_nodal_forces = false; - opts.save_report = false; - opts.save_tie_forces = false; - // if (!elementalForcesOutputFilepath.empty()) { - // opts.save_elemental_forces = true; - // opts.elemental_forces_filename = elementalForcesOutputFilepath; - // } - // if (!nodalDisplacementsOutputFilepath.empty()) { - // opts.save_nodal_displacements = true; - // opts.nodal_displacements_filename = nodalDisplacementsOutputFilepath; - // } - - // have the program output status updates - opts.verbose = false; - - // form an empty vector of ties - std::vector ties; - - // also create an empty list of equations - std::vector equations; - auto fixedVertices = getFeaFixedNodes(pSimulationJob); - auto nodalForces = getFeaNodalForces(pSimulationJob); - fea::Summary feaResults = simulator.solve(fixedVertices, nodalForces, ties, equations, opts); - - SimulationResults results = getResults(feaResults, pSimulationJob); - results.pJob = pSimulationJob; - return results; -} - -void LinearSimulationModel::setStructure(const std::shared_ptr &pMesh) -{ - fea::BeamStructure structure(getFeaNodes(pMesh), getFeaElements(pMesh)); - simulator.initialize(structure); -} - -void LinearSimulationModel::printInfo(const fea::BeamStructure &job) -{ - std::cout << "Details regarding the fea::Job:" << std::endl; - std::cout << "Nodes:" << std::endl; - for (fea::Node n : job.nodes) { - std::cout << n << std::endl; - } - std::cout << "Elements:" << std::endl; - for (Eigen::Vector2i e : job.elems) { - std::cout << e << std::endl; - } -} diff --git a/linearsimulationmodel.hpp b/linearsimulationmodel.hpp deleted file mode 100644 index 211e1b9..0000000 --- a/linearsimulationmodel.hpp +++ /dev/null @@ -1,32 +0,0 @@ -#ifndef LINEARSIMULATIONMODEL_HPP -#define LINEARSIMULATIONMODEL_HPP - -#include "simulationmodel.hpp" -#include "threed_beam_fea.h" -#include -#include - -class LinearSimulationModel : public SimulationModel -{ -public: - LinearSimulationModel(){ - - } - static std::vector getFeaElements(const std::shared_ptr &pMesh); - - static std::vector getFeaNodes(const std::shared_ptr &pMesh); - static std::vector getFeaFixedNodes(const std::shared_ptr &job); - - static std::vector getFeaNodalForces(const std::shared_ptr &job); - static SimulationResults getResults(const fea::Summary &feaSummary, - const std::shared_ptr &simulationJob); - - SimulationResults executeSimulation(const std::shared_ptr &simulationJob); - void setStructure(const std::shared_ptr &pMesh); - -private: - fea::ThreedBeamFEA simulator; - static void printInfo(const fea::BeamStructure &job); -}; - -#endif // LINEARSIMULATIONMODEL_HPP diff --git a/mesh.hpp b/mesh.hpp deleted file mode 100755 index 4b36635..0000000 --- a/mesh.hpp +++ /dev/null @@ -1,37 +0,0 @@ -#ifndef MESH_HPP -#define MESH_HPP - -#include -#include - -class Mesh -{ -protected: - std::string label{"empty_label"}; - -public: - virtual ~Mesh() = default; - virtual bool load(const std::filesystem::path &meshFilePath) { return false; } - virtual bool save(const std::filesystem::path &meshFilePath) { return false; } - - std::string getLabel() const; - void setLabel(const std::string &newLabel); - void prependToLabel(const std::string &text); - void appendToLabel(const std::string &text); -}; - -inline std::string Mesh::getLabel() const { return label; } - -inline void Mesh::setLabel(const std::string &newLabel) { label = newLabel; } - -inline void Mesh::prependToLabel(const std::string &text) -{ - label = text + label; -} - -inline void Mesh::appendToLabel(const std::string &text) -{ - label = label + text; -} - -#endif // MESH_HPP diff --git a/simulation_structs.hpp b/simulation_structs.hpp deleted file mode 100755 index 8c38916..0000000 --- a/simulation_structs.hpp +++ /dev/null @@ -1,1026 +0,0 @@ -#ifndef SIMULATIONSTRUCTS_HPP -#define SIMULATIONSTRUCTS_HPP -#include "csvfile.hpp" -#include "nlohmann/json.hpp" -#include "simulationmesh.hpp" -#include "utilities.hpp" -#include -#include -#include - -#ifdef POLYSCOPE_DEFINED -#include -#endif -namespace Eigen { -template -void write_binary(const std::string &filename, const Matrix &matrix) { - std::ofstream out(filename, - std::ios::out | std::ios::binary | std::ios::trunc); - typename Matrix::Index rows = matrix.rows(), cols = matrix.cols(); - out.write((char *)(&rows), sizeof(typename Matrix::Index)); - out.write((char *)(&cols), sizeof(typename Matrix::Index)); - out.write((char *)matrix.data(), - rows * cols * sizeof(typename Matrix::Scalar)); - out.close(); -} -template -void read_binary(const std::string &filename, Matrix &matrix) { - std::ifstream in(filename, std::ios::in | std::ios::binary); - typename Matrix::Index rows = 0, cols = 0; - in.read((char *)(&rows), sizeof(typename Matrix::Index)); - in.read((char *)(&cols), sizeof(typename Matrix::Index)); - matrix.resize(rows, cols); - in.read((char *)matrix.data(), rows * cols * sizeof(typename Matrix::Scalar)); - in.close(); -} -//const static IOFormat CSVFormat(StreamPrecision, DontAlignCols, ", ", "\n"); -//template -//void writeToCSV(const std::string &filename, Matrix &matrix) -//{ -// ofstream file(filename.c_str()); -// file << matrix.format(CSVFormat); -//} -} // namespace Eigen - - -struct SimulationHistory { - SimulationHistory() {} - - size_t numberOfSteps{0}; - std::string label; - std::vector logResidualForces; - std::vector kineticEnergy; - std::vector potentialEnergies; - std::vector redMarks; - std::vector greenMarks; - std::vector residualForcesMovingAverage; - std::vector perVertexAverageNormalizedDisplacementNorm; - std::vector residualForcesMovingAverageDerivativesLog; - //internal forces - std::vector logOfSumOfAxialForcesNorm; - std::vector logOfSumOfTorsionForcesNorm; - std::vector logOfSumOfFirstBendingForcesNorm; - std::vector logOfSumOfSecondBendingForcesNorm; - - void markRed(const size_t &stepNumber) { redMarks.push_back(stepNumber); } - - void markGreen(const size_t &stepNumber) { greenMarks.push_back(stepNumber); } - - void stepPulse(const SimulationMesh &mesh) - { - kineticEnergy.push_back(std::log10(mesh.currentTotalKineticEnergy)); - // potentialEnergy.push_back(mesh.totalPotentialEnergykN); - logResidualForces.push_back(std::log10(mesh.totalResidualForcesNorm)); - residualForcesMovingAverage.push_back(std::log10(mesh.residualForcesMovingAverage)); - perVertexAverageNormalizedDisplacementNorm.push_back( - mesh.perVertexAverageNormalizedDisplacementNorm); - residualForcesMovingAverageDerivativesLog.push_back( - std::log(mesh.residualForcesMovingAverageDerivativeNorm)); - - //Internal forces - const double axialSumOfNorms = std::accumulate(mesh.nodes._handle->data.begin(), - mesh.nodes._handle->data.end(), - 0.0, - [](const double &sum, const Node &node) { - return sum - + node.force.internalAxial.norm(); - }); - logOfSumOfAxialForcesNorm.push_back(std::log(axialSumOfNorms)); - - const double torsionSumOfNorms - = std::accumulate(mesh.nodes._handle->data.begin(), - mesh.nodes._handle->data.end(), - 0.0, - [](const double &sum, const Node &node) { - return sum + node.force.internalTorsion.norm(); - }); - logOfSumOfTorsionForcesNorm.push_back(std::log(torsionSumOfNorms)); - - const double firstBendingSumOfNorms - = std::accumulate(mesh.nodes._handle->data.begin(), - mesh.nodes._handle->data.end(), - 0.0, - [](const double &sum, const Node &node) { - return sum + node.force.internalFirstBending.norm(); - }); - logOfSumOfFirstBendingForcesNorm.push_back(std::log(firstBendingSumOfNorms)); - - const double secondBendingSumOfNorms - = std::accumulate(mesh.nodes._handle->data.begin(), - mesh.nodes._handle->data.end(), - 0.0, - [](const double &sum, const Node &node) { - return sum + node.force.internalSecondBending.norm(); - }); - logOfSumOfSecondBendingForcesNorm.push_back(std::log(secondBendingSumOfNorms)); - } - - void clear() - { - logResidualForces.clear(); - kineticEnergy.clear(); - potentialEnergies.clear(); - residualForcesMovingAverage.clear(); - perVertexAverageNormalizedDisplacementNorm.clear(); - // residualForcesMovingAverageDerivativesLog.clear(); - } -}; - - -namespace nlohmann { - -template <> struct adl_serializer> { - static void to_json(json &j, - const std::unordered_map &value) { - // calls the "to_json" method in T's namespace - } - - static void from_json(const nlohmann::json &j, - std::unordered_map &m) { - std::cout << "Entered." << std::endl; - for (const auto &p : j) { - m.emplace(p.at(0).template get(), - p.at(1).template get>()); - } - } -}; -} // namespace nlohmann - -class SimulationJob { - // const std::unordered_map nodalForcedNormals; - // json labels - struct JSONLabels { - inline static std::string meshFilename{"mesh filename"}; - inline static std::string forcedDisplacements{"forced displacements"}; - inline static std::string constrainedVertices{"fixed vertices"}; - inline static std::string nodalForces{"forces"}; - inline static std::string label{"label"}; - inline static std::string meshLabel{ - "meshLabel"}; // TODO: should be in the savePly function of the - // simulation mesh class - } jsonLabels; - -#ifdef POLYSCOPE_DEFINED - const std::string polyscopeLabel_bcAsPointCloud{"BC_spheres"}; -#endif - - public: - inline static std::string jsonDefaultFileName = "SimulationJob.json"; - std::shared_ptr pMesh; - std::string label{"empty_job"}; - std::unordered_map> constrainedVertices; - std::unordered_map nodalExternalForces; - std::unordered_map nodalForcedDisplacements; - SimulationJob(const std::shared_ptr &m, - const std::string &label, - const std::unordered_map> &cv, - const std::unordered_map &ef = {}, - const std::unordered_map &fd = {}) - : pMesh(m), label(label), constrainedVertices(cv), nodalExternalForces(ef), - nodalForcedDisplacements(fd) - {} - SimulationJob() {} - SimulationJob(const std::string &jsonFilename) - { - load(jsonFilename); - } - - bool isEmpty() - { - return constrainedVertices.empty() && nodalExternalForces.empty() - && nodalForcedDisplacements.empty() && pMesh == nullptr; - } - void remap(const std::unordered_map &sourceToDestinationViMap, - SimulationJob &destination_simulationJob) const - { - std::unordered_map> destination_fixedVertices; - for (const auto &source_fixedVertex : this->constrainedVertices) { - destination_fixedVertices[sourceToDestinationViMap.at(source_fixedVertex.first)] - = source_fixedVertex.second; - } - - std::unordered_map destination_nodalForces; - for (const auto &source_nodalForces : this->nodalExternalForces) { - destination_nodalForces[sourceToDestinationViMap.at(source_nodalForces.first)] - = source_nodalForces.second; - } - - std::unordered_map destination_forcedDisplacements; - for (const auto &source_forcedDisplacements : this->nodalForcedDisplacements) { - destination_forcedDisplacements[sourceToDestinationViMap.at( - source_forcedDisplacements.first)] - = source_forcedDisplacements.second; - } - - destination_simulationJob.constrainedVertices = destination_fixedVertices; - destination_simulationJob.nodalExternalForces = destination_nodalForces; - destination_simulationJob.label = this->getLabel(); - destination_simulationJob.nodalForcedDisplacements = destination_forcedDisplacements; - } - SimulationJob getCopy() const { - SimulationJob jobCopy; - jobCopy.pMesh = std::make_shared(); - jobCopy.pMesh->copy(*pMesh); - jobCopy.label = label; - jobCopy.constrainedVertices = constrainedVertices; - jobCopy.nodalExternalForces = nodalExternalForces; - jobCopy.nodalForcedDisplacements = nodalForcedDisplacements; - - return jobCopy; - } - std::string getLabel() const { return label; } - - std::string toString() const { - nlohmann::json json; - if (!constrainedVertices.empty()) { - json[jsonLabels.constrainedVertices] = constrainedVertices; - } - if (!nodalExternalForces.empty()) { - std::unordered_map> arrForces; - for (const auto &f : nodalExternalForces) { - arrForces[f.first] = f.second; - } - json[jsonLabels.nodalForces] = arrForces; - } - - return json.dump(); - } - bool operator==(const SimulationJob &otherSimulationJob) - { - return this->toString() == otherSimulationJob.toString(); - } - - void clear() - { - label = "empty_job"; - constrainedVertices.clear(); - nodalExternalForces.clear(); - nodalForcedDisplacements.clear(); - if (pMesh.use_count() == 1) { - std::cout << "Job mesh is deleted" << std::endl; - } - pMesh.reset(); - } - - bool load(const std::string &jsonFilename, const bool &shouldLoadMesh = true) - { - label = "empty_job"; - constrainedVertices.clear(); - nodalExternalForces.clear(); - nodalForcedDisplacements.clear(); - const bool beVerbose = false; - if (std::filesystem::path(jsonFilename).extension() != ".json") { - std::cerr << "A json file is expected as input. The given file has the " - "following extension:" - << std::filesystem::path(jsonFilename).extension() << std::endl; - assert(false); - return false; - } - - if (!std::filesystem::exists(std::filesystem::path(jsonFilename))) { - std::cerr << "The json file does not exist. Json file provided:" << jsonFilename - << std::endl; - assert(false); - return false; - } - - if (beVerbose) { - std::cout << "Loading json file:" << jsonFilename << std::endl; - } - nlohmann::json json; - std::ifstream ifs(jsonFilename); - ifs >> json; - - if (shouldLoadMesh) { - pMesh.reset(); - pMesh = std::make_shared(); - if (json.contains(jsonLabels.meshFilename)) { - const std::string relativeFilepath = json[jsonLabels.meshFilename]; - const auto meshFilepath = std::filesystem::path( - std::filesystem::path(jsonFilename).parent_path()) - .append(relativeFilepath); - pMesh->load(meshFilepath.string()); - pMesh->setLabel(json[jsonLabels.meshLabel]); // FIXME: This should be exported using - // nanoply but nanoply might not be able - // to write a string(??) - } - - if (json.contains(jsonLabels.meshLabel)) { - pMesh->setLabel(json[jsonLabels.meshLabel]); - } - } - - if (json.contains(jsonLabels.constrainedVertices)) { - constrainedVertices = - // auto conV = - json[jsonLabels.constrainedVertices] - .get>>(); - if (beVerbose) { - std::cout << "Loaded constrained vertices. Number of constrained " - "vertices found:" - << constrainedVertices.size() << std::endl; - } - } - - if (json.contains(jsonLabels.nodalForces)) { - auto f = - json[jsonLabels.nodalForces].get>>(); - for (const auto &force : f) { - nodalExternalForces[force.first] = Vector6d(force.second); - } - if (beVerbose) { - std::cout << "Loaded forces. Number of forces found:" << nodalExternalForces.size() - << std::endl; - } - } - - if (json.contains(jsonLabels.forcedDisplacements)) { - // auto conV = - std::unordered_map> forcedDisp - = json[jsonLabels.forcedDisplacements] - .get>>(); - - for (const auto &fd : forcedDisp) { - nodalForcedDisplacements[fd.first] = Eigen::Vector3d(fd.second[0], - fd.second[1], - fd.second[2]); - } - if (beVerbose) { - std::cout << "Loaded forced displacements. Number of forced displaced" - "vertices found:" - << nodalForcedDisplacements.size() << std::endl; - } - } - - if (json.contains(jsonLabels.label)) { - label = json[jsonLabels.label]; - } - - return true; - } - - bool save(const std::string &folderDirectory) const - { - const std::filesystem::path pathFolderDirectory(folderDirectory); - if (!std::filesystem::is_directory(pathFolderDirectory)) { - std::cerr << "A folder directory is expected for saving the simulation " - "job. Exiting.." - << std::endl; - return false; - } - - bool returnValue = true; - std::string jsonFilename( - std::filesystem::path(pathFolderDirectory) - // .append(label + "_" + pMesh->getLabel() + "_simulationJob.json") - .append("SimulationJob.json") - .string()); - - const std::string meshFilename = std::filesystem::absolute( - std::filesystem::canonical( - std::filesystem::path(pathFolderDirectory))) - .append(pMesh->getLabel() + ".ply") - .string(); - returnValue = pMesh->save(meshFilename); - nlohmann::json json; -json[jsonLabels.meshFilename]= std::filesystem::relative(std::filesystem::path(meshFilename),std::filesystem::path(jsonFilename).parent_path()).string(); - json[jsonLabels.meshLabel] - = pMesh->getLabel(); // FIXME: This should be exported using nanoply but - // nanoply might not be able to write a string(??) - if (!constrainedVertices.empty()) { - json[jsonLabels.constrainedVertices] = constrainedVertices; - } - if (!nodalExternalForces.empty()) { - std::unordered_map> arrForces; - for (const auto &f : nodalExternalForces) { - arrForces[f.first] = f.second; - } - json[jsonLabels.nodalForces] = arrForces; - } - if (!nodalForcedDisplacements.empty()) { - std::unordered_map> forcedDisp; - for (const auto &fd : nodalForcedDisplacements) { - forcedDisp[fd.first] = {fd.second[0], fd.second[1], fd.second[2]}; - } - - json[jsonLabels.forcedDisplacements] = forcedDisp; - } - - if (!label.empty()) { - json[jsonLabels.label] = label; - } - if (!pMesh->getLabel().empty()) { - json[jsonLabels.meshLabel] = pMesh->getLabel(); - } - std::ofstream jsonFile(jsonFilename); - jsonFile << json; - jsonFile.close(); - // std::cout << "Saved simulation job as:" << jsonFilename << std::endl; - - return returnValue; - } -#ifdef POLYSCOPE_DEFINED - void registerForDrawing(const std::string &meshLabel, const bool &shouldEnable = false) const - { - PolyscopeInterface::init(); - if (meshLabel.empty()) { - assert(false); - std::cerr << "Expects a mesh label on which to draw the simulation job." << std::endl; - return; - } - - if (!polyscope::hasCurveNetwork(meshLabel)) { - assert(false); - std::cerr << "Expects mesh already being registered to draw the " - "simulation job. No struct named " - + meshLabel - << std::endl; - return; - } - std::vector> nodeColors(pMesh->VN()); - for (const auto &fixedVertex : constrainedVertices) { - const bool hasRotationalDoFConstrained = fixedVertex.second.contains(3) - || fixedVertex.second.contains(4) - || fixedVertex.second.contains(5); - const bool hasTranslationalDoFConstrained = fixedVertex.second.contains(0) - || fixedVertex.second.contains(1) - || fixedVertex.second.contains(2); - if (hasTranslationalDoFConstrained && !hasRotationalDoFConstrained) { - nodeColors[fixedVertex.first] = {0, 0, 1}; - } else if (!hasTranslationalDoFConstrained && hasRotationalDoFConstrained) { - nodeColors[fixedVertex.first] = {0, 1, 0}; - } else { - nodeColors[fixedVertex.first] = {0, 1, 1}; - } - } - if (!nodalForcedDisplacements.empty()) { - for (const std::pair &viDisplPair : - nodalForcedDisplacements) { - const VertexIndex vi = viDisplPair.first; - nodeColors[vi][0] += 1; - nodeColors[vi][0] /= 2; - nodeColors[vi][1] += 0; - nodeColors[vi][1] /= 2; - nodeColors[vi][2] += 0; - nodeColors[vi][2] /= 2; - } - } - std::for_each(nodeColors.begin(), nodeColors.end(), [](std::array &color) { - const double norm = sqrt(std::pow(color[0], 2) + std::pow(color[1], 2) - + std::pow(color[2], 2)); - if (norm > std::pow(10, -7)) { - color[0] /= norm; - color[1] /= norm; - color[2] /= norm; - } - }); - - if (!nodeColors.empty()) { - polyscope::getCurveNetwork(meshLabel) - ->addNodeColorQuantity("Boundary conditions", nodeColors) - ->setEnabled(shouldEnable); - } - drawBcAsSpheres(); - - // per node external forces - std::vector> externalForces(pMesh->VN()); - for (const auto &forcePair : nodalExternalForces) { - auto index = forcePair.first; - auto force = forcePair.second; - externalForces[index] = {force[0], force[1], force[2]}; - } - - if (!externalForces.empty()) { - const std::string polyscopeLabel_externalForces = "External force"; - polyscope::getCurveNetwork(meshLabel)->removeQuantity(polyscopeLabel_externalForces); - polyscope::CurveNetworkNodeVectorQuantity *externalForcesVectors - = polyscope::getCurveNetwork(meshLabel) - ->addNodeVectorQuantity(polyscopeLabel_externalForces, externalForces); - - const std::array color_loads{1.0, 0, 0}; - externalForcesVectors->setVectorColor( - glm::vec3(color_loads[0], color_loads[1], color_loads[2])); - externalForcesVectors->setEnabled(shouldEnable); - } - // per node external moments - bool hasExternalMoments = false; - std::vector> externalMoments(pMesh->VN()); - for (const auto &forcePair : nodalExternalForces) { - auto index = forcePair.first; - const Vector6d &load = forcePair.second; - if (load.getRotation().norm() != 0) { - hasExternalMoments = true; - } - externalMoments[index] = {load[3], load[4], load[5]}; - } - - if (hasExternalMoments) { - polyscope::getCurveNetwork(meshLabel) - ->addNodeVectorQuantity("External moment", externalMoments) - ->setEnabled(shouldEnable); - } - } - void unregister(const std::string &meshLabel) const - { - if (polyscope::getCurveNetwork(meshLabel) == nullptr) { - return; - } - if (!nodalExternalForces.empty()) { - polyscope::getCurveNetwork(meshLabel)->removeQuantity("External force"); - } - if (!constrainedVertices.empty()) { - polyscope::getCurveNetwork(meshLabel)->removeQuantity("Boundary conditions"); - polyscope::removeStructure(polyscopeLabel_bcAsPointCloud, false); - } - - // per node external moments - bool hasExternalMoments = false; - for (const auto &forcePair : nodalExternalForces) { - const Vector6d &load = forcePair.second; - if (load.getRotation().norm() != 0) { - hasExternalMoments = true; - break; - } - } - if (hasExternalMoments) { - polyscope::getCurveNetwork(meshLabel)->removeQuantity("External moment"); - } - } - - void drawBcAsSpheres() const - { - polyscope::removeStructure(polyscopeLabel_bcAsPointCloud, false); - std::vector bcPos; - std::vector bcColors; - for (std::pair> bc : constrainedVertices) { - bcPos.push_back(glm::vec3(pMesh->vert[bc.first].cP()[0], - pMesh->vert[bc.first].cP()[1], - pMesh->vert[bc.first].cP()[2])); - const bool hasRotationalDoFConstrained = bc.second.contains(3) || bc.second.contains(4) - || bc.second.contains(5); - const bool hasTranslationalDoFConstrained = bc.second.contains(0) || bc.second.contains(1) - || bc.second.contains(2); - - std::array color_fixedTranslation{63.0 / 255, 191.0 / 255, 127.0 / 255}; - std::array color_rigid({1, 0, 0}); - if (hasTranslationalDoFConstrained && !hasRotationalDoFConstrained) { - bcColors.push_back(glm::vec3(color_fixedTranslation[0], - color_fixedTranslation[1], - color_fixedTranslation[2])); - } else if (!hasTranslationalDoFConstrained && hasRotationalDoFConstrained) { - bcColors.push_back(glm::vec3(0, 0, 1)); - } else { - bcColors.push_back(glm::vec3(color_rigid[0], color_rigid[1], color_rigid[2])); - } - } - - auto bcPolyscopePointCloud = polyscope::registerPointCloud(polyscopeLabel_bcAsPointCloud, - bcPos); - bcPolyscopePointCloud->addColorQuantity("bc_colors", bcColors)->setEnabled(true); - } -#endif // POLYSCOPE_DEFINED -}; -struct SimulationResults -{ - /*TODO: remove rotationalDisplacementQuaternion since the last three components of the displacments - * vector contains the same info using euler angles -*/ - inline const static std::string defaultJsonFilename{"SimulationResults.json"}; - bool converged{false}; - std::shared_ptr pJob; - SimulationHistory history; - std::vector debug_drmDisplacements; - std::vector> debug_q_f1; //per vertex - std::vector> debug_q_normal; //per vertex - std::vector> debug_q_nr; //per vertex - std::vector displacements; - std::vector> rotationalDisplacementQuaternion; //per vertex - double internalPotentialEnergy{0}; - double executionTime{0}; - std::vector> perVertexInternalForces; //axial,torsion,bending1,bending2 - std::string simulationModelUsed{"empty label"}; - std::string labelPrefix{"deformed"}; - inline static char deliminator{' '}; - SimulationResults() { pJob = std::make_shared(); } - - std::vector getTranslationalDisplacements() const - { - std::vector translationalDisplacements(displacements.size()); - std::transform(displacements.begin(), - displacements.end(), - translationalDisplacements.begin(), - [&](const Vector6d &d) { return VectorType(d[0], d[1], d[2]); }); - - return translationalDisplacements; - } - - void setLabelPrefix(const std::string &lp) { labelPrefix += deliminator + lp; } - std::string getLabel() const - { - return labelPrefix + deliminator + simulationModelUsed + deliminator - + pJob->pMesh->getLabel() + deliminator + pJob->getLabel(); - } - - bool saveDeformedModel(const std::string &outputFolder = std::string()) - { - VCGEdgeMesh m; - vcg::tri::Append::MeshCopy(m, *pJob->pMesh); - - for (int vi = 0; vi < m.VN(); vi++) { - m.vert[vi].P() = m.vert[vi].P() - + CoordType(displacements[vi][0], - displacements[vi][1], - displacements[vi][2]); - } - return m.save(std::filesystem::path(outputFolder).append(getLabel() + ".ply").string()); - } - - void saveInternalForces(const std::filesystem::path &outputDirPath) - { - std::cout << "out to:" << outputDirPath << std::endl; - const std::filesystem::path internalForcesDirPath = std::filesystem::path(outputDirPath); - std::filesystem::create_directories(internalForcesDirPath); - csvFile csv_axial6d(std::filesystem::path(internalForcesDirPath) - .append("forces_axial_6d.csv"), - true); - csvFile csv_axialMagn(std::filesystem::path(internalForcesDirPath) - .append("forces_axial_magn.csv"), - true); - csvFile csv_torsion6d(std::filesystem::path(internalForcesDirPath) - .append("forces_torsion_6d.csv"), - true); - csvFile csv_torsionMagn(std::filesystem::path(internalForcesDirPath) - .append("forces_torsion_magn.csv"), - true); - csvFile csv_firstBending6d(std::filesystem::path(internalForcesDirPath) - .append("forces_firstBending_6d.csv"), - true); - csvFile csv_firstBendingMagn(std::filesystem::path(internalForcesDirPath) - .append("forces_firstBending_magn.csv"), - true); - csvFile csv_secondBending6d(std::filesystem::path(internalForcesDirPath) - .append("forces_secondBending_6d.csv"), - true); - csvFile csv_secondBendingMagn(std::filesystem::path(internalForcesDirPath) - .append("forces_secondBending_magn.csv"), - true); - for (const std::array &internalForce : perVertexInternalForces) { - for (int dofi = 0; dofi < 6; dofi++) { - csv_axial6d << internalForce[0][dofi]; - csv_torsion6d << internalForce[1][dofi]; - csv_firstBending6d << internalForce[2][dofi]; - csv_secondBending6d << internalForce[3][dofi]; - } - csv_axial6d << endrow; - csv_torsion6d << endrow; - csv_firstBending6d << endrow; - csv_secondBending6d << endrow; - csv_axialMagn << internalForce[0].norm() << endrow; - csv_torsionMagn << internalForce[1].norm() << endrow; - csv_firstBendingMagn << internalForce[2].norm() << endrow; - csv_secondBendingMagn << internalForce[3].norm() << endrow; - } - } - - void save(const std::string &outputFolder = std::string()) - { - const std::filesystem::path outputFolderPath = outputFolder.empty() - ? std::filesystem::current_path() - : std::filesystem::path(outputFolder); - // std::cout << "Saving results to:" << outputFolderPath << std::endl; - std::filesystem::path simulationJobOutputFolderPath - = std::filesystem::path(outputFolderPath).append("SimulationJob"); - std::filesystem::create_directories(simulationJobOutputFolderPath); - pJob->save(simulationJobOutputFolderPath.string()); - const std::string filename(getLabel() + "_displacements.eigenBin"); - - Eigen::MatrixXd m = Utilities::toEigenMatrix(displacements); - const std::filesystem::path resultsFolderPath( - std::filesystem::path(outputFolder).append("Results")); - std::filesystem::create_directories(resultsFolderPath); - Eigen::write_binary(std::filesystem::path(resultsFolderPath).append(filename).string(), m); - - nlohmann::json json; - - json[GET_VARIABLE_NAME(internalPotentialEnergy)] = internalPotentialEnergy; - //Write internal forces - if (!perVertexInternalForces.empty()) { - std::vector internalForces_axial(perVertexInternalForces.size()); - std::vector internalForces_torsion(perVertexInternalForces.size()); - std::vector internalForces_firstBending(perVertexInternalForces.size()); - std::vector internalForces_secondBending(perVertexInternalForces.size()); - for (int vi = 0; vi < pJob->pMesh->VN(); vi++) { - internalForces_axial[vi] = perVertexInternalForces[vi][0]; - internalForces_torsion[vi] = perVertexInternalForces[vi][1]; - internalForces_firstBending[vi] = perVertexInternalForces[vi][2]; - internalForces_secondBending[vi] = perVertexInternalForces[vi][3]; - } - json[std::string(GET_VARIABLE_NAME(perVertexInternalForces)) + "_axial"] - = internalForces_axial; - json[std::string(GET_VARIABLE_NAME(perVertexInternalForces)) + "_torsion"] - = internalForces_torsion; - json[std::string(GET_VARIABLE_NAME(perVertexInternalForces)) + "_firstBending"] - = internalForces_firstBending; - json[std::string(GET_VARIABLE_NAME(perVertexInternalForces)) + "_secondBending"] - = internalForces_secondBending; - } - std::filesystem::path jsonFilePath( - std::filesystem::path(resultsFolderPath).append(defaultJsonFilename)); - std::ofstream jsonFile(jsonFilePath.string()); - jsonFile << json; - jsonFile.close(); - - saveDeformedModel(resultsFolderPath.string()); - } - - // The comparison of the results happens comparing the 6-dof nodal - // displacements - bool isEqual(const Eigen::MatrixXd &nodalDisplacements, double &error) - { - assert(nodalDisplacements.cols() == 6); - Eigen::MatrixXd eigenDisplacements = Utilities::toEigenMatrix(this->displacements); - const double errorNorm = (eigenDisplacements - nodalDisplacements).norm(); - error = errorNorm; - return errorNorm < 1e-10; - // return eigenDisplacements.isApprox(nodalDisplacements); - } - - void load(const std::filesystem::path &loadFromPath, const std::filesystem::path &loadJobFrom) - { - pJob->load(std::filesystem::path(loadJobFrom).append("SimulationJob.json").string()); - load(loadFromPath); - } - void load(const std::filesystem::path &loadFromPath, const std::shared_ptr &pJob) - { - this->pJob = pJob; - load(loadFromPath); - } - - template< - typename Container, - typename T = std::decay_t()))>, - typename = std::enable_if_t>>> - static double computeDistance(const SimulationResults &resultsA, - const SimulationResults &resultsB, - const Container &resultsAToResultsBViMap - /*,const std::unordered_map */) - { - double distance = 0; - for (std::pair resultsAToResultsBViPair : resultsAToResultsBViMap) { - const double vertexToVertexDistance - = (resultsA.displacements[resultsAToResultsBViPair.first].getTranslation() - - resultsB.displacements[resultsAToResultsBViPair.second].getTranslation()) - .norm(); - distance += vertexToVertexDistance; - } - return distance; - } - - double computeDistance(const SimulationResults &other, - const std::unordered_map &thisToOtherViMap) const - { - return computeDistance(*this, other, thisToOtherViMap); - } - -#ifdef POLYSCOPE_DEFINED - void unregister() const - { - if (!polyscope::hasCurveNetwork(getLabel())) { - std::cerr << "No curve network registered with a name: " << getLabel() << std::endl; - std::cerr << "Nothing to remove." << std::endl; - return; - } - pJob->unregister(getLabel()); - polyscope::removeCurveNetwork(getLabel()); - } - - polyscope::CurveNetwork *registerForDrawing( - const std::optional> &desiredColor = std::nullopt, - const bool &shouldEnable = true /*, - const bool &shouldShowFrames = false*/) const - { - if (!converged) { - std::cerr << "Simulation did not converge. Drawing nothing." << std::endl; - return nullptr; - } - PolyscopeInterface::init(); - const std::shared_ptr &simulationMesh = pJob->pMesh; - polyscope::CurveNetwork *polyscopeHandle_deformedEdmeMesh; - const std::string &meshLabel = getLabel(); - if (!polyscope::hasStructure(meshLabel)) { - const auto &verts = simulationMesh->getEigenVertices(); - const auto &edges = simulationMesh->getEigenEdges(); - polyscopeHandle_deformedEdmeMesh = polyscope::registerCurveNetwork(meshLabel, - verts, - edges); - } else { - polyscopeHandle_deformedEdmeMesh = polyscope::getCurveNetwork(meshLabel); - } - polyscopeHandle_deformedEdmeMesh->setEnabled(shouldEnable); - const double drawingRadius = simulationMesh->getBeamDimensions()[0].getDrawingRadius(); - polyscopeHandle_deformedEdmeMesh->setRadius(drawingRadius /*0.00025*/, false); - if (desiredColor.has_value()) { - const glm::vec3 desiredColor_glm(desiredColor.value()[0], - desiredColor.value()[1], - desiredColor.value()[2]); - polyscopeHandle_deformedEdmeMesh->setColor(desiredColor_glm); - } - Eigen::MatrixX3d nodalDisplacements(simulationMesh->VN(), 3); - Eigen::MatrixX3d framesX(simulationMesh->VN(), 3); - Eigen::MatrixX3d framesY(simulationMesh->VN(), 3); - Eigen::MatrixX3d framesZ(simulationMesh->VN(), 3); - - Eigen::MatrixX3d framesX_initial(simulationMesh->VN(), 3); - Eigen::MatrixX3d framesY_initial(simulationMesh->VN(), 3); - Eigen::MatrixX3d framesZ_initial(simulationMesh->VN(), 3); - - // std::unordered_set interfaceNodes{1, 3, 5, 7, 9, 11}; - std::unordered_set interfaceNodes{3, 7, 11, 15, 19, 23}; - // std::unordered_set interfaceNodes{}; - for (VertexIndex vi = 0; vi < simulationMesh->VN(); vi++) { - const Vector6d &nodalDisplacement = displacements[vi]; - nodalDisplacements.row(vi) = Eigen::Vector3d(nodalDisplacement[0], - nodalDisplacement[1], - nodalDisplacement[2]); - // Eigen::Quaternion Rx(Eigen::AngleAxis(nodalDisplacement[2],Eigen::Vector3d(1, 0, 0))); - // Eigen::Quaternion Ry(Eigen::AngleAxis(nodalDisplacement[4],Eigen::Vector3d(0, 1, 0))); - // Eigen::Quaternion Rz(Eigen::AngleAxis(nodalDisplacement[5],Eigen::Vector3d(0, 0, 1))); - // Eigen::Quaternion R=Rx*Ry*Rz; - if (interfaceNodes.contains(vi)) { - assert(!rotationalDisplacementQuaternion.empty()); - auto deformedNormal = rotationalDisplacementQuaternion[vi] - * Eigen::Vector3d(0, 0, 1); - auto deformedFrameY = rotationalDisplacementQuaternion[vi] - * Eigen::Vector3d(0, 1, 0); - auto deformedFrameX = rotationalDisplacementQuaternion[vi] - * 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]); - framesX_initial.row(vi) = Eigen::Vector3d(1, 0, 0); - framesY_initial.row(vi) = Eigen::Vector3d(0, 1, 0); - framesZ_initial.row(vi) = Eigen::Vector3d(0, 0, 1); - } else { - framesX.row(vi) = Eigen::Vector3d(0, 0, 0); - framesY.row(vi) = Eigen::Vector3d(0, 0, 0); - framesZ.row(vi) = Eigen::Vector3d(0, 0, 0); - framesX_initial.row(vi) = Eigen::Vector3d(0, 0, 0); - framesY_initial.row(vi) = Eigen::Vector3d(0, 0, 0); - framesZ_initial.row(vi) = Eigen::Vector3d(0, 0, 0); - } - } - polyscopeHandle_deformedEdmeMesh->updateNodePositions(simulationMesh->getEigenVertices() - + nodalDisplacements); - - const double frameRadius_default = 0.035; - const double frameLength_default = 0.035; - const bool shouldEnable_default = true; - //if(showFramesOn.contains(vi)){ - auto polyscopeHandle_frameX = polyscopeHandle_deformedEdmeMesh - ->addNodeVectorQuantity("FrameX", framesX); - polyscopeHandle_frameX->setVectorLengthScale(frameLength_default); - polyscopeHandle_frameX->setVectorRadius(frameRadius_default); - polyscopeHandle_frameX->setVectorColor( - /*polyscope::getNextUniqueColor()*/ glm::vec3(1, 0, 0)); - auto polyscopeHandle_frameY = polyscopeHandle_deformedEdmeMesh - ->addNodeVectorQuantity("FrameY", framesY); - polyscopeHandle_frameY->setVectorLengthScale(frameLength_default); - polyscopeHandle_frameY->setVectorRadius(frameRadius_default); - polyscopeHandle_frameY->setVectorColor( - /*polyscope::getNextUniqueColor()*/ glm::vec3(0, 1, 0)); - auto polyscopeHandle_frameZ = polyscopeHandle_deformedEdmeMesh - ->addNodeVectorQuantity("FrameZ", framesZ); - polyscopeHandle_frameZ->setVectorLengthScale(frameLength_default); - polyscopeHandle_frameZ->setVectorRadius(frameRadius_default); - polyscopeHandle_frameZ->setVectorColor( - /*polyscope::getNextUniqueColor()*/ glm::vec3(0, 0, 1)); - - // if (!polyscope::hasCurveNetwork(mesh->getLabel())) { - // const std::array initialColor({0, 0, 0}); - // /*auto polyscopeHandle_initialMesh =*/mesh->registerForDrawing(initialColor); - // } - - // auto polyscopeHandle_frameX_initial = polyscopeHandle_initialMesh - // ->addNodeVectorQuantity("FrameX", framesX_initial); - // polyscopeHandle_frameX_initial->setVectorLengthScale(frameLength_default); - // polyscopeHandle_frameX_initial->setVectorRadius(frameRadius_default); - // polyscopeHandle_frameX_initial->setVectorColor( - // /*polyscope::getNextUniqueColor()*/ glm::vec3(1, 0, 0)); - // auto polyscopeHandle_frameY_initial = polyscopeHandle_initialMesh - // ->addNodeVectorQuantity("FrameY", framesY_initial); - // polyscopeHandle_frameY_initial->setVectorLengthScale(frameLength_default); - // polyscopeHandle_frameY_initial->setVectorRadius(frameRadius_default); - // polyscopeHandle_frameY_initial->setVectorColor( - // /*polyscope::getNextUniqueColor()*/ glm::vec3(0, 1, 0)); - // auto polyscopeHandle_frameZ_initial = polyscopeHandle_initialMesh - // ->addNodeVectorQuantity("FrameZ", framesZ_initial); - // polyscopeHandle_frameZ_initial->setVectorLengthScale(frameLength_default); - // polyscopeHandle_frameZ_initial->setVectorRadius(frameRadius_default); - // polyscopeHandle_frameZ_initial->setVectorColor( - // /*polyscope::getNextUniqueColor()*/ glm::vec3(0, 0, 1)); - // //} - pJob->registerForDrawing(getLabel(), false); - // static bool wasExecuted =false; - // if (!wasExecuted) { - // std::function callback = [&]() { - // static bool showFrames = shouldShowFrames; - - // if (ImGui::Checkbox("Show Frames", &showFrames) && showFrames) { - // polyscopeHandle_frameX->setEnabled(showFrames); - // polyscopeHandle_frameY->setEnabled(showFrames); - // polyscopeHandle_frameZ->setEnabled(showFrames); - // } - // }; - - // PolyscopeInterface::addUserCallback(callback); - // wasExecuted = true; - // } - return polyscopeHandle_deformedEdmeMesh; - } -#endif -private: - bool load(const std::filesystem::path &loadFromPath) - { - converged = true; //assuming it has converged - assert(pJob != nullptr); - //load job - //Use the first .eigenBin file for loading the displacements - for (auto const &entry : std::filesystem::recursive_directory_iterator(loadFromPath)) { - if (std::filesystem::is_regular_file(entry) && entry.path().extension() == ".eigenBin") { - Eigen::MatrixXd displacements_eigen; - Eigen::read_binary(entry.path().string(), displacements_eigen); - displacements = Utilities::fromEigenMatrix(displacements_eigen); - break; - } - } - - rotationalDisplacementQuaternion.resize(pJob->pMesh->VN()); - for (int vi = 0; vi < pJob->pMesh->VN(); vi++) { - rotationalDisplacementQuaternion[vi] = Eigen::AngleAxisd(displacements[vi][3], - Eigen::Vector3d::UnitX()) - * Eigen::AngleAxisd(displacements[vi][4], - Eigen::Vector3d::UnitY()) - * Eigen::AngleAxisd(displacements[vi][5], - Eigen::Vector3d::UnitZ()); - } - - const std::filesystem::path jsonFilepath = std::filesystem::path(loadFromPath) - .append(defaultJsonFilename); - if (std::filesystem::exists(jsonFilepath)) { - std::ifstream ifs(jsonFilepath); - nlohmann::json json; - ifs >> json; - // if (json.contains(GET_VARIABLE_NAME(internalPotentialEnergy))) { - // internalPotentialEnergy = json.at(GET_VARIABLE_NAME(internalPotentialEnergy)); - // } - if (json.contains(std::string(GET_VARIABLE_NAME(perVertexInternalForces)) + "_axial")) { - perVertexInternalForces.resize(pJob->pMesh->VN()); - std::vector perVertexInternalForces_axial - = static_cast>(json.at( - std::string(GET_VARIABLE_NAME(perVertexInternalForces)) + "_axial")); - for (int vi = 0; vi < pJob->pMesh->VN(); vi++) { - perVertexInternalForces[vi][0] = perVertexInternalForces_axial[vi]; - } - } - if (json.contains(std::string(GET_VARIABLE_NAME(perVertexInternalForces)) - + "_torsion")) { - perVertexInternalForces.resize(pJob->pMesh->VN()); - std::vector perVertexInternalForces_axial - = static_cast>(json.at( - std::string(GET_VARIABLE_NAME(perVertexInternalForces)) + "_torsion")); - for (int vi = 0; vi < pJob->pMesh->VN(); vi++) { - perVertexInternalForces[vi][0] = perVertexInternalForces_axial[vi]; - } - } - if (json.contains(std::string(GET_VARIABLE_NAME(perVertexInternalForces)) - + "_firstBending")) { - perVertexInternalForces.resize(pJob->pMesh->VN()); - std::vector perVertexInternalForces_axial - = static_cast>(json.at( - std::string(GET_VARIABLE_NAME(perVertexInternalForces)) + "_firstBending")); - for (int vi = 0; vi < pJob->pMesh->VN(); vi++) { - perVertexInternalForces[vi][0] = perVertexInternalForces_axial[vi]; - } - } - if (json.contains(std::string(GET_VARIABLE_NAME(perVertexInternalForces)) - + "_secondBending")) { - perVertexInternalForces.resize(pJob->pMesh->VN()); - std::vector perVertexInternalForces_axial - = static_cast>(json.at( - std::string(GET_VARIABLE_NAME(perVertexInternalForces)) + "_secondBending")); - for (int vi = 0; vi < pJob->pMesh->VN(); vi++) { - perVertexInternalForces[vi][0] = perVertexInternalForces_axial[vi]; - } - } - } - return true; - } -}; - -#endif // SIMULATIONHISTORY_HPP diff --git a/simulationhistoryplotter.hpp b/simulationhistoryplotter.hpp deleted file mode 100755 index f4f329f..0000000 --- a/simulationhistoryplotter.hpp +++ /dev/null @@ -1,176 +0,0 @@ -#ifndef SIMULATIONHISTORYPLOTTER_HPP -#define SIMULATIONHISTORYPLOTTER_HPP - -#include "simulation_structs.hpp" -#include "simulationmesh.hpp" -#include "utilities.hpp" -#include -#include - -struct SimulationResultsReporter { - using VertexType = VCGEdgeMesh::VertexType; - using CoordType = VCGEdgeMesh::CoordType; - - SimulationResultsReporter() {} - - void writeStatistics(const SimulationResults &results, - const std::string &reportFolderPath) { - std::ofstream file; - file.open(std::filesystem::path(reportFolderPath).append("results.txt").string()); - - const size_t numberOfSteps = results.history.numberOfSteps; - file << "Number of steps " << numberOfSteps << "\n"; - - // file << "Force threshold used " << 1000 << "\n"; - - // assert(numberOfSteps == results.history.potentialEnergy.size() && - // numberOfSteps == results.history.residualForces.size()); - // Write kinetic energies - const SimulationHistory &history = results.history; - if (!history.kineticEnergy.empty()) { - file << "Kinetic energies" - << "\n"; - for (size_t step = 0; step < numberOfSteps; step++) { - file << history.kineticEnergy[step] << "\n"; - } - file << "\n"; - } - - if (!history.logResidualForces.empty()) { - file << "Residual forces" - << "\n"; - for (size_t step = 0; step < numberOfSteps; step++) { - file << history.logResidualForces[step] << "\n"; - } - file << "\n"; - } - - if (!history.potentialEnergies.empty()) { - file << "Potential energies" - << "\n"; - for (size_t step = 0; step < numberOfSteps; step++) { - file << history.potentialEnergies[step] << "\n"; - } - file << "\n"; - } - file.close(); - } - - void reportHistory(const SimulationHistory &history, - const std::string &reportFolderPath, - const std::string &graphSuffix = std::string()) - { - const auto simulationResultPath = std::filesystem::path(reportFolderPath).append(history.label); - std::filesystem::create_directories(simulationResultPath); - createPlots(history, simulationResultPath.string(), graphSuffix); - } - - void reportResults(const std::vector &results, - const std::string &reportFolderPath, - const std::string &graphSuffix = std::string()) { - if (results.empty()) { - return; - } - - // std::filesystem::remove_all(debuggerFolder); - std::filesystem::create_directory(reportFolderPath); - for (const SimulationResults &simulationResult : results) { - const auto simulationResultPath = - std::filesystem::path(reportFolderPath) - .append(simulationResult.getLabel()); - std::filesystem::create_directory(simulationResultPath.string()); - - createPlots(simulationResult.history, simulationResultPath.string(), - graphSuffix); - writeStatistics(simulationResult, simulationResultPath.string()); - } - } - - static void createPlot(const std::string &xLabel, - const std::string &yLabel, - const std::vector &YvaluesToPlot, - const std::string &saveTo = {}, - const std::vector &markPoints = {}) - { - if (YvaluesToPlot.size() < 2) { - return; - } - std::vector colors(YvaluesToPlot.size(), 0.5); - std::vector markerSizes(YvaluesToPlot.size(), 5); - if (!markPoints.empty()) { - for (const auto pointIndex : markPoints) { - colors[pointIndex] = 0.9; - markerSizes[pointIndex] = 14; - } - } - std::vector x = matplot::linspace(0, YvaluesToPlot.size() - 1, YvaluesToPlot.size()); - Utilities::createPlot(xLabel, yLabel, x, YvaluesToPlot, markerSizes, colors, saveTo); - } - - void createPlots(const SimulationHistory &history, - const std::string &reportFolderPath, - const std::string &graphSuffix) - { - const auto graphsFolder = std::filesystem::path(reportFolderPath).append("Graphs"); - std::filesystem::remove_all(graphsFolder); - std::filesystem::create_directory(graphsFolder.string()); - - if (!history.kineticEnergy.empty()) { - createPlot("Number of Iterations", - "Log of Kinetic Energy log", - history.kineticEnergy, - std::filesystem::path(graphsFolder) - .append("KineticEnergyLog_" + graphSuffix + ".png") - .string(), - history.redMarks); - } - - if (!history.logResidualForces.empty()) { - createPlot("Number of Iterations", - "Residual Forces norm log", - history.logResidualForces, - std::filesystem::path(graphsFolder) - .append("ResidualForcesLog_" + graphSuffix + ".png") - .string(), - history.redMarks); - } - - if (!history.potentialEnergies.empty()) { - createPlot("Number of Iterations", - "Potential energy", - history.potentialEnergies, - std::filesystem::path(graphsFolder) - .append("PotentialEnergy_" + graphSuffix + ".png") - .string(), - history.redMarks); - } - if (!history.residualForcesMovingAverage.empty()) { - createPlot("Number of Iterations", - "Residual forces moving average", - history.residualForcesMovingAverage, - std::filesystem::path(graphsFolder) - .append("ResidualForcesMovingAverage_" + graphSuffix + ".png") - .string(), - history.redMarks); - } - // if (!history.residualForcesMovingAverageDerivativesLog.empty()) { - // createPlot("Number of Iterations", - // "Residual forces moving average derivative log", - // history.residualForcesMovingAverageDerivativesLog, - // std::filesystem::path(graphsFolder) - // .append("ResidualForcesMovingAverageDerivativesLog_" + graphSuffix + ".png") - // .string()); - // } - if (!history.perVertexAverageNormalizedDisplacementNorm.empty()) { - createPlot("Number of Iterations", - "Sum of normalized displacement norms", - history.perVertexAverageNormalizedDisplacementNorm, - std::filesystem::path(graphsFolder) - .append("SumOfNormalizedDisplacementNorms_" + graphSuffix + ".png") - .string(), - history.redMarks); - } - } -}; - -#endif // SIMULATIONHISTORYPLOTTER_HPP diff --git a/simulationmesh.cpp b/simulationmesh.cpp deleted file mode 100755 index a3ba074..0000000 --- a/simulationmesh.cpp +++ /dev/null @@ -1,528 +0,0 @@ -#include "simulationmesh.hpp" -//#include - -SimulationMesh::SimulationMesh() { - elements = vcg::tri::Allocator::GetPerEdgeAttribute( - *this, std::string("Elements")); - nodes = vcg::tri::Allocator::GetPerVertexAttribute( - *this, std::string("Nodes")); -} - -SimulationMesh::SimulationMesh(VCGEdgeMesh &mesh) { - vcg::tri::MeshAssert::VertexNormalNormalized(mesh); - - VCGEdgeMesh::copy(mesh); - - elements = vcg::tri::Allocator::GetPerEdgeAttribute( - *this, std::string("Elements")); - nodes = vcg::tri::Allocator::GetPerVertexAttribute(*this, - std::string("Nodes")); - initializeNodes(); - initializeElements(); -} - -SimulationMesh::~SimulationMesh() { - vcg::tri::Allocator::DeletePerEdgeAttribute(*this, - elements); - vcg::tri::Allocator::DeletePerVertexAttribute(*this, - nodes); -} - -SimulationMesh::SimulationMesh(PatternGeometry &pattern) { - vcg::tri::MeshAssert::VertexNormalNormalized(pattern); - VCGEdgeMesh::copy(pattern); - - elements = vcg::tri::Allocator::GetPerEdgeAttribute( - *this, std::string("Elements")); - nodes = vcg::tri::Allocator::GetPerVertexAttribute( - *this, std::string("Nodes")); - initializeNodes(); - initializeElements(); -} - -SimulationMesh::SimulationMesh(SimulationMesh &m) -{ - vcg::tri::MeshAssert::VertexNormalNormalized(m); - VCGEdgeMesh::copy(m); - - elements = vcg::tri::Allocator::GetPerEdgeAttribute(*this, - std::string( - "Elements")); - nodes = vcg::tri::Allocator::GetPerVertexAttribute(*this, - std::string("Nodes")); - initializeNodes(); - - for (size_t ei = 0; ei < EN(); ei++) { - elements[ei] = m.elements[ei]; - } - reset(); -} - -SimulationMesh::SimulationMesh(VCGTriMesh &triMesh) -{ - vcg::tri::Append::MeshCopy(*this, triMesh); - label = triMesh.getLabel(); - // eigenEdges = triMesh.getEigenEdges(); - // if (eigenEdges.rows() == 0) { - computeEdges(eigenEdges); - // } - // eigenVertices = triMesh.getEigenVertices(); - // if (eigenVertices.rows() == 0) { - computeVertices(eigenVertices); - // } - vcg::tri::UpdateTopology::VertexEdge(*this); - - elements = vcg::tri::Allocator::GetPerEdgeAttribute(*this, - std::string( - "Elements")); - nodes = vcg::tri::Allocator::GetPerVertexAttribute(*this, - std::string("Nodes")); - initializeNodes(); - initializeElements(); - reset(); -} - -void SimulationMesh::computeElementalProperties() { - const std::vector elementalDimensions = getBeamDimensions(); - const std::vector elementalMaterials = getBeamMaterial(); - assert(EN() == elementalDimensions.size() && - elementalDimensions.size() == elementalMaterials.size()); - - for (const EdgeType &e : edge) { - const EdgeIndex ei = getIndex(e); - elements[e].setDimensions(elementalDimensions[ei]); - elements[e].setMaterial(elementalMaterials[ei]); - } -} - -void SimulationMesh::initializeNodes() { - // set initial and previous locations, - for (const VertexType &v : vert) { - const VertexIndex vi = getIndex(v); - Node &node = nodes[v]; - node.vi = vi; - node.initialLocation = v.cP(); - node.initialNormal = v.cN(); - node.derivativeOfNormal.resize(6, VectorType(0, 0, 0)); - - node.displacements[3] = - v.cN()[0]; // initialize nx diplacement with vertex normal x - // component - node.displacements[4] = v.cN()[1]; // initialize ny(in the paper) diplacement with vertex - // normal - // y component. - - // Initialize incident elements - std::vector incidentElements; - vcg::edge::VEStarVE(&v, incidentElements); - // assert( - // vcg::tri::IsValidPointer(*this, incidentElements[0]) && - // incidentElements.size() > 0); - if (incidentElements.size() != 0) { - nodes[v].incidentElements = incidentElements; - node.referenceElement = getReferenceElement(v); - // std::vector incidentElementsIndices(node.incidentElements.size()); - // if (drawGlobal && vi == 5) { - // std::vector edgeColors(EN(), glm::vec3(0, 1, 0)); - // std::vector vertexColors(VN(), glm::vec3(0, 1, 0)); - // vertexColors[vi] = glm::vec3(0, 0, 1); - // for (int iei = 0; iei < incidentElementsIndices.size(); iei++) { - // incidentElementsIndices[iei] = this->getIndex(node.incidentElements[iei]); - // edgeColors[incidentElementsIndices[iei]] = glm::vec3(1, 0, 0); - // } - // polyHandle->addEdgeColorQuantity("chosenE", edgeColors)->setEnabled(true); - // polyHandle->addNodeColorQuantity("chosenV", vertexColors)->setEnabled(true); - // draw(); - // } - // const int referenceElementIndex = getIndex(node.referenceElement); - // Initialze alpha angles - - const EdgeType &referenceElement = *node.referenceElement; - const VectorType t01 = computeT1Vector(referenceElement.cP(0), referenceElement.cP(1)); - const VectorType f01 = (t01 - (v.cN() * (t01.dot(v.cN())))).Normalize(); - node.alphaAngles.reserve(incidentElements.size()); - - for (const VCGEdgeMesh::EdgePointer &ep : nodes[v].incidentElements) { - assert(referenceElement.cV(0) == ep->cV(0) || referenceElement.cV(0) == ep->cV(1) - || referenceElement.cV(1) == ep->cV(0) || referenceElement.cV(1) == ep->cV(1)); - const VectorType t1 = computeT1Vector(*ep); - const VectorType f1 = t1 - (v.cN() * (t1.dot(v.cN()))).Normalize(); - const EdgeIndex ei = getIndex(ep); - const double alphaAngle = computeAngle(f01, f1, v.cN()); - node.alphaAngles.emplace_back(std::make_pair(ei, alphaAngle)); - } - } - } -} - -void SimulationMesh::reset() { - for (const EdgeType &e : edge) { - Element &element = elements[e]; - element.ei = getIndex(e); - const VCGEdgeMesh::CoordType p0 = e.cP(0); - const VCGEdgeMesh::CoordType p1 = e.cP(1); - const vcg::Segment3 s(p0, p1); - element.initialLength = s.Length(); - element.length = element.initialLength; - element.updateRigidity(); - } - - for (const VertexType &v : vert) { - Node &node = nodes[v]; - node.vi = getIndex(v); - node.initialLocation = v.cP(); - node.initialNormal = v.cN(); - node.derivativeOfNormal.resize(6, VectorType(0, 0, 0)); - - node.displacements[3] = v.cN()[0]; // initialize nx diplacement with vertex normal x - // component - node.displacements[4] = v.cN()[1]; // initialize ny(in the paper) diplacement with vertex - // normal - // y component. - - const EdgeType &referenceElement = *getReferenceElement(v); - const VectorType t01 = computeT1Vector(referenceElement.cP(0), referenceElement.cP(1)); - const VectorType f01 = (t01 - (v.cN() * (t01.dot(v.cN())))).Normalize(); - node.alphaAngles.clear(); - node.alphaAngles.reserve(node.incidentElements.size()); - for (const VCGEdgeMesh::EdgePointer &ep : nodes[v].incidentElements) { - assert(referenceElement.cV(0) == ep->cV(0) || referenceElement.cV(0) == ep->cV(1) - || referenceElement.cV(1) == ep->cV(0) || referenceElement.cV(1) == ep->cV(1)); - const VectorType t1 = computeT1Vector(*ep); - const VectorType f1 = t1 - (v.cN() * (t1.dot(v.cN()))).Normalize(); - const EdgeIndex ei = getIndex(ep); - const double alphaAngle = computeAngle(f01, f1, v.cN()); - node.alphaAngles.emplace_back(std::make_pair(ei, alphaAngle)); - } - } -} - -void SimulationMesh::initializeElements() { - for (const EdgeType &e : edge) { - Element &element = elements[e]; - element.ei = getIndex(e); - // Initialize dimensions - element.dimensions = CrossSectionType(); - // Initialize material - element.material = ElementMaterial(); - // Initialize lengths - const VCGEdgeMesh::CoordType p0 = e.cP(0); - const VCGEdgeMesh::CoordType p1 = e.cP(1); - const vcg::Segment3 s(p0, p1); - element.initialLength = s.Length(); - element.length = element.initialLength; - // Initialize const factors - element.updateRigidity(); - element.derivativeT1.resize( - 2, std::vector(6, VectorType(0, 0, 0))); - element.derivativeT2.resize( - 2, std::vector(6, VectorType(0, 0, 0))); - element.derivativeT3.resize( - 2, std::vector(6, VectorType(0, 0, 0))); - element.derivativeT1_jplus1.resize(6, VectorType(0, 0, 0)); - element.derivativeT1_j.resize(6, VectorType(0, 0, 0)); - element.derivativeT1_jplus1.resize(6, VectorType(0, 0, 0)); - element.derivativeT2_j.resize(6, VectorType(0, 0, 0)); - element.derivativeT2_jplus1.resize(6, VectorType(0, 0, 0)); - element.derivativeT3_j.resize(6, VectorType(0, 0, 0)); - element.derivativeT3_jplus1.resize(6, VectorType(0, 0, 0)); - element.derivativeR_j.resize(6, VectorType(0, 0, 0)); - element.derivativeR_jplus1.resize(6, VectorType(0, 0, 0)); - } - updateElementalFrames(); -} - -void SimulationMesh::updateElementalLengths() { - for (const EdgeType &e : edge) { - const EdgeIndex ei = getIndex(e); - const VertexIndex vi0 = getIndex(e.cV(0)); - const VCGEdgeMesh::CoordType p0 = e.cP(0); - const VertexIndex vi1 = getIndex(e.cV(1)); - const VCGEdgeMesh::CoordType p1 = e.cP(1); - const vcg::Segment3 s(p0, p1); - const double elementLength = s.Length(); - elements[e].length = elementLength; - } -} - -void SimulationMesh::updateElementalFrames() { - for (const EdgeType &e : edge) { - const VectorType elementNormal = - (e.cV(0)->cN() + e.cV(1)->cN()).Normalize(); - elements[e].frame = computeElementFrame(e.cP(0), e.cP(1), elementNormal); - } -} - -#ifdef POLYSCOPE_DEFINED -polyscope::CurveNetwork *SimulationMesh::registerForDrawing( - const std::optional> &desiredColor, const bool &shouldEnable) -{ - const double drawingRadius = getBeamDimensions()[0].getDrawingRadius(); - // std::cout << __FUNCTION__ << " revert this" << std::endl; - return VCGEdgeMesh::registerForDrawing(desiredColor, /*0.08*/ drawingRadius, shouldEnable); -} - -void SimulationMesh::unregister() const -{ - VCGEdgeMesh::unregister(); -} -#endif - -void SimulationMesh::setBeamCrossSection(const CrossSectionType &beamDimensions) -{ - for (size_t ei = 0; ei < EN(); ei++) { - elements[ei].dimensions = beamDimensions; - elements[ei].updateRigidity(); - } -} - -void SimulationMesh::setBeamMaterial(const double &pr, const double &ym) { - for (size_t ei = 0; ei < EN(); ei++) { - elements[ei].setMaterial(ElementMaterial{pr, ym}); - elements[ei].updateRigidity(); - } -} - -std::vector SimulationMesh::getBeamDimensions() -{ - std::vector beamDimensions(EN()); - for (size_t ei = 0; ei < EN(); ei++) { - beamDimensions[ei] = elements[ei].dimensions; - } - return beamDimensions; -} - -std::vector SimulationMesh::getBeamMaterial() -{ - std::vector beamMaterial(EN()); - for (size_t ei = 0; ei < EN(); ei++) { - beamMaterial[ei] = elements[ei].material; - } - return beamMaterial; -} - -bool SimulationMesh::load(const std::string &plyFilename) -{ - this->Clear(); - // assert(plyFileHasAllRequiredFields(plyFilename)); - // Load the ply file - // VCGEdgeMesh::PerEdgeAttributeHandle handleBeamDimensions = - // vcg::tri::Allocator::AddPerEdgeAttribute< - // CrossSectionType>(*this, plyPropertyBeamDimensionsID); - // VCGEdgeMesh::PerEdgeAttributeHandle handleBeamMaterial = - // vcg::tri::Allocator::AddPerEdgeAttribute( - // *this, plyPropertyBeamMaterialID); - // nanoply::NanoPlyWrapper::CustomAttributeDescriptor - // customAttrib; - // customAttrib.GetMeshAttrib(plyFilename); - // customAttrib.AddEdgeAttribDescriptor( - // plyPropertyBeamDimensionsID, nanoply::NNP_LIST_INT8_FLOAT64, nullptr); - // /*FIXME: Since I allow CrossSectionType to take two types I should export the - // * type as well such that that when loaded the correct type of cross section - // * is used. - // */ - // customAttrib.AddEdgeAttribDescriptor( - // plyPropertyBeamMaterialID, nanoply::NNP_LIST_INT8_FLOAT64, nullptr); - - // VCGEdgeMesh::PerEdgeAttributeHandle> - // handleBeamProperties = - // vcg::tri::Allocator::AddPerEdgeAttribute< - // std::array>(*this, plyPropertyBeamProperties); - // customAttrib.AddEdgeAttribDescriptor, double, 6>( - // plyPropertyBeamProperties, nanoply::NNP_LIST_INT8_FLOAT64, nullptr); - - // VCGEdgeMesh::PerEdgeAttributeHandle - // handleBeamRigidityContants; - // customAttrib.AddEdgeAttribDescriptor( - // plyPropertyBeamRigidityConstantsID, nanoply::NNP_LIST_INT8_FLOAT32, - // nullptr); - // unsigned int mask = 0; - // mask |= nanoply::NanoPlyWrapper::IO_VERTCOORD; - // mask |= nanoply::NanoPlyWrapper::IO_VERTNORMAL; - // mask |= nanoply::NanoPlyWrapper::IO_EDGEINDEX; - // mask |= nanoply::NanoPlyWrapper::IO_EDGEATTRIB; - // mask |= nanoply::NanoPlyWrapper::IO_MESHATTRIB; - if (!VCGEdgeMesh::load(plyFilename)) { - return false; - } - - // elements = vcg::tri::Allocator::GetPerEdgeAttribute( - // *this, std::string("Elements")); - // nodes = vcg::tri::Allocator::GetPerVertexAttribute( - // *this, std::string("Nodes")); - vcg::tri::UpdateTopology::VertexEdge(*this); - initializeNodes(); - initializeElements(); - setBeamMaterial(0.3, 1 * 1e9); - updateEigenEdgeAndVertices(); - - // if (!handleBeamProperties._handle->data.empty()) { - // for (size_t ei = 0; ei < EN(); ei++) { - // elements[ei] = - // Element::Properties(handleBeamProperties[ei]); - // elements[ei].updateRigidity(); - // } - // } - // for (size_t ei = 0; ei < EN(); ei++) { - // elements[ei].setDimensions(handleBeamDimensions[ei]); - // elements[ei].setMaterial(handleBeamMaterial[ei]); - // elements[ei].updateRigidity(); - // } - - bool normalsAreAbsent = vert[0].cN().Norm() < 0.000001; - if (normalsAreAbsent) { - CoordType normalVector(0, 0, 1); - std::cout << "Warning: Normals are missing from " << plyFilename - << ". Added normal vector:" << toString(normalVector) << std::endl; - for (auto &v : vert) { - v.N() = normalVector; - } - } - - return true; -} - -bool SimulationMesh::save(const std::string &plyFilename) -{ - std::string filename = plyFilename; - if (filename.empty()) { - filename = std::filesystem::current_path().append(getLabel() + ".ply").string(); - } - // nanoply::NanoPlyWrapper::CustomAttributeDescriptor customAttrib; - // customAttrib.GetMeshAttrib(filename); - - // std::vector dimensions = getBeamDimensions(); - // customAttrib.AddEdgeAttribDescriptor(plyPropertyBeamDimensionsID, - // nanoply::NNP_LIST_INT8_FLOAT64, - // dimensions.data()); - // std::vector material = getBeamMaterial(); - // customAttrib.AddEdgeAttribDescriptor(plyPropertyBeamMaterialID, - // nanoply::NNP_LIST_INT8_FLOAT64, - // material.data()); - // unsigned int mask = 0; - // mask |= nanoply::NanoPlyWrapper::IO_VERTCOORD; - // mask |= nanoply::NanoPlyWrapper::IO_EDGEINDEX; - // mask |= nanoply::NanoPlyWrapper::IO_EDGEATTRIB; - // mask |= nanoply::NanoPlyWrapper::IO_VERTNORMAL; - // if (nanoply::NanoPlyWrapper::SaveModel(filename.c_str(), - // *this, - // mask, - // customAttrib, - // false) - // != 1) { - // return false; - // } - if (!VCGEdgeMesh::save(plyFilename)) { - return false; - } - return true; -} - -SimulationMesh::EdgePointer -SimulationMesh::getReferenceElement(const VCGEdgeMesh::VertexType &v) { - const VertexIndex vi = getIndex(v); - return nodes[v].incidentElements[0]; -} - -VectorType computeT1Vector(const SimulationMesh::EdgeType &e) -{ - return computeT1Vector(e.cP(0), e.cP(1)); -} - -VectorType computeT1Vector(const CoordType &p0, const CoordType &p1) { - const VectorType t1 = (p1 - p0).Normalize(); - return t1; -} - -Element::LocalFrame computeElementFrame(const CoordType &p0, - const CoordType &p1, - const VectorType &elementNormal) { - const VectorType t1 = computeT1Vector(p0, p1); - const VectorType t2 = (elementNormal ^ t1).Normalize(); - const VectorType t3 = (t1 ^ t2).Normalize(); - - return Element::LocalFrame{t1, t2, t3}; -} - -double computeAngle(const VectorType &vector0, const VectorType &vector1, - const VectorType &normalVector) { - double cosAngle = vector0.dot(vector1); - const double epsilon = std::pow(10, -6); - if (abs(cosAngle) > 1 && abs(cosAngle) < 1 + epsilon) { - if (cosAngle > 0) { - cosAngle = 1; - - } else { - cosAngle = -1; - } - } - assert(abs(cosAngle) <= 1); - const double angle = - acos(cosAngle); // NOTE: I compute the alpha angle not between - // two consecutive elements but rather between - // the first and the ith. Is this correct? - assert(!std::isnan(angle)); - - const VectorType cp = vector0 ^ vector1; - if (cp.dot(normalVector) < 0) { - return -angle; - } - return angle; -} - -//void Element::computeMaterialProperties(const ElementMaterial &material) { -// G = material.youngsModulus / (2 * (1 + material.poissonsRatio)); -//} - -//void Element::computeCrossSectionArea(const RectangularBeamDimensions &dimensions, double &A) -//{ -// A = dimensions.b * dimensions.h; -//} - -//void Element::computeDimensionsProperties( -// const RectangularBeamDimensions &dimensions) { -// assert(typeid(CrossSectionType) == typeid(RectangularBeamDimensions)); -// computeCrossSectionArea(dimensions, A); -// computeMomentsOfInertia(dimensions, dimensions.inertia); -//} - -//void Element::computeDimensionsProperties( -// const CylindricalBeamDimensions &dimensions) { -// assert(typeid(CrossSectionType) == typeid(CylindricalBeamDimensions)); -// A = M_PI * (std::pow(dimensions.od / 2, 2) - std::pow(dimensions.id / 2, 2)); -// dimensions.inertia.I2 = M_PI * (std::pow(dimensions.od, 4) - std::pow(dimensions.id, 4)) / 64; -// dimensions.inertia.I3 = dimensions.inertia.I2; -// dimensions.inertia.J = dimensions.inertia.I2 + dimensions.inertia.I3; -//} - -void Element::setDimensions(const CrossSectionType &dimensions) { - this->dimensions = dimensions; - assert(this->dimensions.A == dimensions.A); - // computeDimensionsProperties(dimensions); - updateRigidity(); -} - -void Element::setMaterial(const ElementMaterial &material) -{ - this->material = material; - // computeMaterialProperties(material); - updateRigidity(); -} - -double Element::getMass(const double &materialDensity) -{ - const double beamVolume = dimensions.A * length; - return beamVolume * materialDensity; -} - -void Element::updateRigidity() { - // assert(initialLength != 0); - rigidity.axial = material.youngsModulus * dimensions.A / initialLength; - // assert(rigidity.axial != 0); - rigidity.torsional = material.G * dimensions.inertia.J / initialLength; - // assert(rigidity.torsional != 0); - rigidity.firstBending = 2 * material.youngsModulus * dimensions.inertia.I2 / initialLength; - // assert(rigidity.firstBending != 0); - rigidity.secondBending = 2 * material.youngsModulus * dimensions.inertia.I3 / initialLength; - // assert(rigidity.secondBending != 0); -} diff --git a/simulationmesh.hpp b/simulationmesh.hpp deleted file mode 100755 index 21374e8..0000000 --- a/simulationmesh.hpp +++ /dev/null @@ -1,198 +0,0 @@ -#ifndef SIMULATIONMESH_HPP -#define SIMULATIONMESH_HPP - -#include "edgemesh.hpp" -#include "trianglepatterngeometry.hpp" -#include - -//extern bool drawGlobal; -struct Element; -struct Node; -//using CrossSectionType = RectangularBeamDimensions; -using CrossSectionType = CylindricalBeamDimensions; - -class SimulationMesh : public VCGEdgeMesh { -private: - void computeElementalProperties(); - void initializeElements(); - void initializeNodes(); - EdgePointer getReferenceElement(const VertexType &v); - - const std::string plyPropertyBeamDimensionsID{"beam_dimensions"}; - const std::string plyPropertyBeamMaterialID{"beam_material"}; - -public: - PerEdgeAttributeHandle elements; - PerVertexAttributeHandle nodes; - ~SimulationMesh(); - SimulationMesh(PatternGeometry &pattern); - SimulationMesh(ConstVCGEdgeMesh &edgeMesh); - SimulationMesh(SimulationMesh &elementalMesh); - SimulationMesh(VCGTriMesh &triMesh); - void updateElementalLengths(); - void updateIncidentElements(); - - std::vector - getIncidentElements(const VCGEdgeMesh::VertexType &v); - virtual bool load(const std::string &plyFilename); - std::vector getBeamDimensions(); - std::vector getBeamMaterial(); - - double pre_previousTotalKineticEnergy{0}; - double pre_previousTotalTranslationalKineticEnergy{0}; - double pre_previousTotalRotationalKineticEnergy{0}; - double previousTotalKineticEnergy{0}; - double previousTotalTranslationalKineticEnergy{0}; - double previousTotalRotationalKineticEnergy{0}; - double previousTotalResidualForcesNorm{0}; - double currentTotalKineticEnergy{0}; - double currentTotalTranslationalKineticEnergy{0}; - double currentTotalRotationalKineticEnergy{0}; - double totalResidualForcesNorm{0}; - double totalExternalForcesNorm{0}; - double averageResidualForcesNorm{0}; - double currentTotalPotentialEnergykN{0}; - double previousTotalPotentialEnergykN{0}; - double residualForcesMovingAverageDerivativeNorm{0}; - double residualForcesMovingAverage{0}; - double perVertexAverageNormalizedDisplacementNorm{0}; - bool save(const std::string &plyFilename = std::string()); - void setBeamCrossSection(const CrossSectionType &beamDimensions); - void setBeamMaterial(const double &pr, const double &ym); - void reset(); - SimulationMesh(); - void updateElementalFrames(); - -#ifdef POLYSCOPE_DEFINED - polyscope::CurveNetwork *registerForDrawing( - const std::optional> &desiredColor = std::nullopt, - const bool &shouldEnable = true); - void unregister() const; -#endif -}; - -struct Element -{ - CrossSectionType dimensions; - ElementMaterial material; - - void computeMaterialProperties(const ElementMaterial &material); - // void computeDimensionsProperties(const RectangularBeamDimensions &dimensions); - // void computeDimensionsProperties(const CylindricalBeamDimensions &dimensions); - void setDimensions(const CrossSectionType &dimensions); - void setMaterial(const ElementMaterial &material); - double getMass(const double &matrialDensity); - - struct LocalFrame - { - VectorType t1; - VectorType t2; - VectorType t3; - }; - - EdgeIndex ei; - double length{0}; - double initialLength; - LocalFrame frame; - - struct Rigidity - { - double axial; - double torsional; - double firstBending; - double secondBending; - std::string toString() const - { - return std::string("Rigidity:") + std::string("\nAxial=") + std::to_string(axial) - + std::string("\nTorsional=") + std::to_string(torsional) - + std::string("\nFirstBending=") + std::to_string(firstBending) - + std::string("\nSecondBending=") + std::to_string(secondBending); - } - }; - Rigidity rigidity; - void updateRigidity(); - - VectorType f1_j; - VectorType f1_jplus1; - VectorType f2_j; - VectorType f2_jplus1; - VectorType f3_j; - VectorType f3_jplus1; - double cosRotationAngle_j; - double cosRotationAngle_jplus1; - double sinRotationAngle_j; - double sinRotationAngle_jplus1; - std::vector> derivativeT1; - std::vector> derivativeT2; - std::vector> derivativeT3; - std::vector derivativeT1_j; - std::vector derivativeT1_jplus1; - std::vector derivativeT2_j; - std::vector derivativeT2_jplus1; - std::vector derivativeT3_j; - std::vector derivativeT3_jplus1; - std::vector derivativeR_j; - std::vector derivativeR_jplus1; - struct RotationalDisplacements - { - double theta1{0}, theta2{0}, theta3{0}; - }; - RotationalDisplacements rotationalDisplacements_j; - RotationalDisplacements rotationalDisplacements_jplus1; - - static void computeCrossSectionArea(const RectangularBeamDimensions &dimensions, double &A); -}; - -struct Node { - struct Forces { - Vector6d external{0}; - Vector6d internal{0}; - Vector6d residual{0}; - Vector6d internalAxial{0}; - Vector6d internalTorsion{0}; - Vector6d internalFirstBending{0}; - Vector6d internalSecondBending{0}; - bool hasExternalForce() const { return external.isZero(); } - }; - - struct Mass { - double translational; - double rotationalI2; - double rotationalI3; - double rotationalJ; - }; - - Mass mass; - Vector6d mass_6d; - Vector6d damping_6d; - VertexIndex vi; - CoordType initialLocation; - CoordType initialNormal; - Vector6d acceleration{0}; - Forces force; - Vector6d previousVelocity{0}; - Vector6d velocity{0}; - double kineticEnergy{0}; - Vector6d displacements{0}; - double nR{0}; - // std::unordered_map - // alphaAngles; // contains the initial angles between the first star element - // // incident to this node and the other elements of the star - // // has size equal to the valence of the vertex - std::vector> alphaAngles; - - std::vector incidentElements; - std::vector derivativeOfNormal; - SimulationMesh::EdgePointer referenceElement; -}; - -Element::LocalFrame computeElementFrame(const CoordType &p0, - const CoordType &p1, - const VectorType &elementNormal); -VectorType computeT1Vector(const SimulationMesh::EdgeType &e); - -VectorType computeT1Vector(const CoordType &p0, const CoordType &p1); -double computeAngle(const VectorType &vector0, const VectorType &vector1, - const VectorType &normalVector); - -#endif // ELEMENTALMESH_HPP diff --git a/simulationmodel.hpp b/simulationmodel.hpp deleted file mode 100644 index 4f59297..0000000 --- a/simulationmodel.hpp +++ /dev/null @@ -1,15 +0,0 @@ -#ifndef SIMULATIONMODEL_HPP -#define SIMULATIONMODEL_HPP - -#include "simulation_structs.hpp" - -class SimulationModel -{ -public: - virtual SimulationResults executeSimulation(const std::shared_ptr &simulationJob) - = 0; - virtual void setStructure(const std::shared_ptr &pMesh) = 0; - virtual ~SimulationModel() = default; -}; - -#endif // SIMULATIONMODEL_HPP diff --git a/utilities.cpp b/utilities.cpp deleted file mode 100644 index 8592f80..0000000 --- a/utilities.cpp +++ /dev/null @@ -1,108 +0,0 @@ -#include "utilities.hpp" -#include "matplot/matplot.h" - -void Utilities::createPlot(const std::string &xLabel, - const std::string &yLabel, - const std::vector &x, - const std::vector &y, - const std::vector &markerSizes, - const std::vector &c, - const std::string &saveTo) -{ - // matplot::figure(true); - matplot::xlabel(xLabel); - matplot::ylabel(yLabel); - matplot::grid(matplot::on); - matplot::scatter(x, y, markerSizes, c)->marker_face(true); - if (!saveTo.empty()) { - matplot::save(saveTo); - } -} -#ifdef POLYSCOPE_DEFINED -#include "polyscope/curve_network.h" -#include "polyscope/pick.h" -#include "polyscope/polyscope.h" -#include - -void PolyscopeInterface::mainCallback() -{ - ImGui::PushItemWidth(100); // Make ui elements 100 pixels wide, - // instead of full width. Must have - // matching PopItemWidth() below. - - for (std::function &userCallback : globalPolyscopeData.userCallbacks) { - userCallback(); - } - - ImGui::PopItemWidth(); -} - -void PolyscopeInterface::addUserCallback(const std::function &userCallback) -{ - globalPolyscopeData.userCallbacks.push_back(userCallback); -} - -void PolyscopeInterface::deinitPolyscope() -{ - if (!polyscope::state::initialized) { - return; - } - - polyscope::render::engine->shutdownImGui(); -} - -void PolyscopeInterface::init() -{ - if (polyscope::state::initialized) { - return; - } - polyscope::init(); - polyscope::options::groundPlaneEnabled = false; - polyscope::view::upDir = polyscope::view::UpDir::ZUp; - - polyscope::state::userCallback = &mainCallback; - polyscope::options::autocenterStructures = false; - polyscope::options::autoscaleStructures = false; -} - -std::pair PolyscopeInterface::getSelection() -{ - std::pair selection = polyscope::pick::getSelection(); - if (selection.first == nullptr) { - return std::make_pair(std::string(), 0); - } - return std::make_pair(selection.first->name, selection.second); -} - -void PolyscopeInterface::registerWorldAxes() -{ - PolyscopeInterface::init(); - - Eigen::MatrixX3d axesPositions(4, 3); - axesPositions.row(0) = Eigen::Vector3d(0, 0, 0); - // axesPositions.row(1) = Eigen::Vector3d(polyscope::state::lengthScale, 0, 0); - // axesPositions.row(2) = Eigen::Vector3d(0, polyscope::state::lengthScale, 0); - // axesPositions.row(3) = Eigen::Vector3d(0, 0, polyscope::state::lengthScale); - axesPositions.row(1) = Eigen::Vector3d(1, 0, 0); - axesPositions.row(2) = Eigen::Vector3d(0, 1, 0); - axesPositions.row(3) = Eigen::Vector3d(0, 0, 1); - - Eigen::MatrixX2i axesEdges(3, 2); - axesEdges.row(0) = Eigen::Vector2i(0, 1); - axesEdges.row(1) = Eigen::Vector2i(0, 2); - axesEdges.row(2) = Eigen::Vector2i(0, 3); - Eigen::MatrixX3d axesColors(3, 3); - axesColors.row(0) = Eigen::Vector3d(1, 0, 0); - axesColors.row(1) = Eigen::Vector3d(0, 1, 0); - axesColors.row(2) = Eigen::Vector3d(0, 0, 1); - - const std::string worldAxesName = "World Axes"; - polyscope::registerCurveNetwork(worldAxesName, axesPositions, axesEdges); - polyscope::getCurveNetwork(worldAxesName)->setRadius(0.0001, false); - const std::string worldAxesColorName = worldAxesName + " Color"; - polyscope::getCurveNetwork(worldAxesName) - ->addEdgeColorQuantity(worldAxesColorName, axesColors) - ->setEnabled(true); -} - -#endif diff --git a/utilities.hpp b/utilities.hpp deleted file mode 100755 index 4966135..0000000 --- a/utilities.hpp +++ /dev/null @@ -1,409 +0,0 @@ -#ifndef UTILITIES_H -#define UTILITIES_H - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#define GET_VARIABLE_NAME(Variable) (#Variable) - -struct Vector6d : public std::array { - Vector6d() { - for (size_t i = 0; i < 6; i++) { - this->operator[](i) = 0; - } - } - - Vector6d(const std::vector &v) { - assert(v.size() == 6); - std::copy(v.begin(), v.end(), this->begin()); - } - - Vector6d(const double &d) { - for (size_t i = 0; i < 6; i++) { - this->operator[](i) = d; - } - } - - Vector6d(const std::array &arr) : std::array(arr) {} - - Vector6d(const std::initializer_list &initList) { - std::copy(initList.begin(), initList.end(), std::begin(*this)); - } - - Vector6d operator*(const double &d) const { - Vector6d result; - for (size_t i = 0; i < 6; i++) { - result[i] = this->operator[](i) * d; - } - return result; - } - - Vector6d operator*(const Vector6d &v) const { - Vector6d result; - for (size_t i = 0; i < 6; i++) { - result[i] = this->operator[](i) * v[i]; - } - return result; - } - - Vector6d operator/(const double &d) const { - Vector6d result; - for (size_t i = 0; i < 6; i++) { - result[i] = this->operator[](i) / d; - } - return result; - } - - Vector6d operator/(const Vector6d &v) const - { - Vector6d result; - for (size_t i = 0; i < 6; i++) { - result[i] = this->operator[](i) / v[i]; - } - return result; - } - - Vector6d operator+(const Vector6d &v) const { - Vector6d result; - for (size_t i = 0; i < 6; i++) { - result[i] = this->operator[](i) + v[i]; - } - return result; - } - - Vector6d operator-(const Vector6d &v) const { - Vector6d result; - for (size_t i = 0; i < 6; i++) { - result[i] = this->operator[](i) - v[i]; - } - return result; - } - - Vector6d inverted() const { - Vector6d result; - for (size_t i = 0; i < 6; i++) { - assert(this->operator[](i) != 0); - result[i] = 1 / this->operator[](i); - } - return result; - } - bool isZero() const { - for (size_t i = 0; i < 6; i++) { - if (this->operator[](i) != 0) - return false; - } - return true; - } - - double squaredNorm() const { - double squaredNorm = 0; - std::for_each(this->begin(), std::end(*this), - [&](const double &v) { squaredNorm += pow(v, 2); }); - return squaredNorm; - } - - double norm() const { return sqrt(squaredNorm()); } - - bool isFinite() const { - return std::any_of(std::begin(*this), std::end(*this), [](const double &v) { - if (!std::isfinite(v)) { - return false; - } - return true; - }); - } - - Eigen::Vector3d getTranslation() const { - return Eigen::Vector3d(this->operator[](0), this->operator[](1), - this->operator[](2)); - } - - Eigen::Vector3d getRotation() const - { - return Eigen::Vector3d(this->operator[](3), this->operator[](4), this->operator[](5)); - } - - std::string toString() const - { - std::string s; - for (int i = 0; i < 6; i++) { - s.append(std::to_string(this->operator[](i)) + ","); - } - s.pop_back(); - return s; - } -}; - -namespace Utilities { -template -std::string to_string_with_precision(const T a_value, const int n = 2) -{ - std::ostringstream out; - out.precision(n); - out << std::fixed << a_value; - return out.str(); -} - -inline bool compareNat(const std::string &a, const std::string &b) -{ - if (a.empty()) - return true; - if (b.empty()) - return false; - if (std::isdigit(a[0]) && !std::isdigit(b[0])) - return true; - if (!std::isdigit(a[0]) && std::isdigit(b[0])) - return false; - if (!std::isdigit(a[0]) && !std::isdigit(b[0])) { - if (std::toupper(a[0]) == std::toupper(b[0])) - return compareNat(a.substr(1), b.substr(1)); - return (std::toupper(a[0]) < std::toupper(b[0])); - } - - // Both strings begin with digit --> parse both numbers - std::istringstream issa(a); - std::istringstream issb(b); - int ia, ib; - issa >> ia; - issb >> ib; - if (ia != ib) - return ia < ib; - - // Numbers are the same --> remove numbers and recurse - std::string anew, bnew; - std::getline(issa, anew); - std::getline(issb, bnew); - return (compareNat(anew, bnew)); -} - -inline std::string_view leftTrimSpaces(const std::string_view& str) -{ -std::string_view trimmedString=str; - const auto pos(str.find_first_not_of(" \t\n\r\f\v")); - trimmedString.remove_prefix(std::min(pos, trimmedString.length())); - return trimmedString; -} - -inline std::string_view rightTrimSpaces(const std::string_view& str) -{ -std::string_view trimmedString=str; - const auto pos(trimmedString.find_last_not_of(" \t\n\r\f\v")); - trimmedString.remove_suffix(std::min(trimmedString.length() - pos - 1,trimmedString.length())); - return trimmedString; -} - -inline std::string_view trimLeftAndRightSpaces(std::string_view str) -{ -std::string_view trimmedString=str; - trimmedString = leftTrimSpaces(trimmedString); - trimmedString = rightTrimSpaces(trimmedString); - return trimmedString; -} - -template -inline void normalize(InputIt itBegin, InputIt itEnd) -{ - const auto squaredSumOfElements = std::accumulate(itBegin, - itEnd, - 0.0, - [](const auto &sum, const auto &el) { - return sum + el * el; - }); - assert(squaredSumOfElements != 0); - std::transform(itBegin, itEnd, itBegin, [&](auto &element) { - return element / std::sqrt(squaredSumOfElements); - }); -} - -inline std::vector split(const std::string& text, std::string delim) -{ - std::vector vec; - size_t pos = 0, prevPos = 0; - while (1) { - pos = text.find(delim, prevPos); - if (pos == std::string::npos) { - vec.push_back(text.substr(prevPos)); - return vec; - } - - vec.push_back(text.substr(prevPos, pos - prevPos)); - prevPos = pos + delim.length(); - } -} - -inline std::string toString(const std::vector> &vv) -{ - std::string s; - s.append("{"); - for (const std::vector &v : vv) { - s.append("{"); - for (const int &i : v) { - s.append(std::to_string(i) + ","); - } - s.pop_back(); - s.append("}"); - } - s.append("}"); - return s; -} -inline void parseIntegers(const std::string &str, std::vector &result) -{ - typedef std::regex_iterator re_iterator; - typedef re_iterator::value_type re_iterated; - - std::regex re("(\\d+)"); - - re_iterator rit(str.begin(), str.end(), re); - re_iterator rend; - - std::transform(rit, rend, std::back_inserter(result), [](const re_iterated &it) { - return std::stoi(it[1]); - }); -} - -inline Eigen::MatrixXd toEigenMatrix(const std::vector &v) { - Eigen::MatrixXd m(v.size(), 6); - - for (size_t vi = 0; vi < v.size(); vi++) { - const Vector6d &vec = v[vi]; - for (size_t i = 0; i < 6; i++) { - m(vi, i) = vec[i]; - } - } - - return m; -} - -inline std::vector fromEigenMatrix(const Eigen::MatrixXd &m) -{ - std::vector v(m.rows()); - - for (size_t vi = 0; vi < m.rows(); vi++) { - const Eigen::RowVectorXd &row = m.row(vi); - for (size_t i = 0; i < 6; i++) { - v[vi][i] = row(i); - } - } - - return v; -} -// std::string convertToLowercase(const std::string &s) { -// std::string lowercase; -// std::transform(s.begin(), s.end(), lowercase.begin(), -// [](unsigned char c) { return std::tolower(c); }); -// return lowercase; -//} -// bool hasExtension(const std::string &filename, const std::string &extension) -// { -// const std::filesystem::path path(filename); -// if (!path.has_extension()) { -// std::cerr << "Error: No file extension found in " << filename << -// std::endl; return false; -// } - -// const std::string detectedExtension = path.extension().string(); - -// if (convertToLowercase(detectedExtension) != convertToLowercase(extension)) -// { -// std::cerr << "Error: detected extension is " + detectedExtension + -// " and not " + extension -// << std::endl; -// return false; -// } -// return true; -//} - -inline std::filesystem::path getFilepathWithExtension(const std::filesystem::path &folderPath, - const std::string &extension) -{ - assert(std::filesystem::exists(folderPath)); - for (const std::filesystem::directory_entry &dirEntry : - std::filesystem::directory_iterator(folderPath)) { - if (dirEntry.is_regular_file() && std::filesystem::path(dirEntry).extension() == extension) { - return std::filesystem::path(dirEntry); - } - } - - return ""; -} - -void createPlot(const std::string &xLabel, - const std::string &yLabel, - const std::vector &x, - const std::vector &y, - const std::vector &markerSizes, - const std::vector &c, - const std::string &saveTo = {}); - -} // namespace Utilities - -#ifdef POLYSCOPE_DEFINED - -namespace PolyscopeInterface { -inline struct GlobalPolyscopeData -{ - std::vector> userCallbacks; -} globalPolyscopeData; - -void mainCallback(); - -void addUserCallback(const std::function &userCallback); - -void deinitPolyscope(); - -void init(); -using PolyscopeLabel = std::string; -std::pair getSelection(); - -void registerWorldAxes(); -} // namespace PolyscopeInterface - -#endif - -// namespace ConfigurationFile { - -//} -//} // namespace ConfigurationFile -template -void constructInverseMap(const T1 &map, T2 &oppositeMap) { - assert(!map.empty()); - oppositeMap.clear(); - for (const auto &mapIt : map) { - oppositeMap[mapIt.second] = mapIt.first; - } -} - -template std::string toString(const T &v) { - return "(" + std::to_string(v[0]) + "," + std::to_string(v[1]) + "," + - std::to_string(v[2]) + ")"; -} - -template -size_t computeHashUnordered(const std::vector &v) -{ - size_t hash = 0; - for (const auto &el : v) { - hash += std::hash{}(el); - } - return hash; -} - -inline size_t computeHashOrdered(const std::vector &v) -{ - std::string elementsString; - for (const auto &el : v) { - elementsString += std::to_string(el); - } - - return std::hash{}(elementsString); -} - -#endif // UTILITIES_H