manolas
/
MySources
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
MySources/drmsimulationmodel.cpp

3156 lines
159 KiB
C++
Executable File
Raw Blame History

#include "drmsimulationmodel.hpp"
#include "linearsimulationmodel.hpp"
#include "simulationhistoryplotter.hpp"
#include <Eigen/Geometry>
#include <algorithm>
#include <chrono>
#include <execution>
#ifdef ENABLE_OPENMP
#include <omp.h>
#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<VertexIndex, std::unordered_set<DoFType>> fixedVertices;
fixedVertices[0] = std::unordered_set<DoFType>{0, 1, 2};
fixedVertices[beam.VN() - 1] = std::unordered_set<DoFType>{1, 2};
std::unordered_map<VertexIndex, Vector6d> nodalForces{
{beam.VN() / 2, Vector6d({0, 1000, 1000, 0, 0, 0})}};
// Forced displacements
std::unordered_map<size_t, Eigen::Vector3d> nodalForcedDisplacements;
nodalForcedDisplacements.insert({beam.VN() - 1, Eigen::Vector3d(-0.2, 0, 0)});
SimulationJob beamSimulationJob{std::make_shared<SimulationMesh>(beam),
"First paper example",
// SimulationJob::constructFixedVerticesSpanGrid(spanGridSize,
// mesh.VN()),
fixedVertices,
nodalForces,
nodalForcedDisplacements};
beamSimulationJob.pMesh->setBeamMaterial(0.3, 200 * 1e9);
assert(CrossSectionType::name == CylindricalBeamDimensions::name);
beamSimulationJob.pMesh->setBeamCrossSection(CrossSectionType{0.03, 0.026});
Settings settings;
settings.Dtini = 0.1;
settings.xi = 0.9969;
settings.maxDRMIterations = 0;
formFinder.mSettings.totalResidualForcesNormThreshold = 1;
settings.shouldDraw = false;
settings.beVerbose = true;
settings.shouldCreatePlots = true;
SimulationResults simpleBeam_simulationResults
= formFinder.executeSimulation(std::make_shared<SimulationJob>(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<DoFType>{2};
fixedVertices[4] = std::unordered_set<DoFType>{2};
fixedVertices[16] = std::unordered_set<DoFType>{2};
fixedVertices[20] = std::unordered_set<DoFType>{2};
//// Center node
fixedVertices[10] = std::unordered_set<DoFType>{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<SimulationMesh>(shortSpanGrid),
"Short span gridshell",
fixedVertices,
{},
nodalForcedDisplacements};
shortSpanGridshellSimulationJob.pMesh->setBeamMaterial(0.3, 200 * 1e9);
assert(typeid(CrossSectionType) == typeid(CylindricalBeamDimensions));
shortSpanGridshellSimulationJob.pMesh->setBeamCrossSection(CrossSectionType{0.03, 0.026});
SimulationResults shortSpanGridshellSimulationResults
= formFinder.executeSimulation(std::make_shared<SimulationJob>(
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<SimulationMesh>(longSpanGrid),
"long span gridshell",
fixedVertices,
{},
nodalForcedDisplacements};
longSpanGridshell_simulationJob.pMesh->setBeamMaterial(0.3, 200 * 1e9);
if (typeid(CrossSectionType) != typeid(CylindricalBeamDimensions)) {
std::cerr << "A cylindrical cross section is expected for running the "
"paper examples."
<< std::endl;
}
longSpanGridshell_simulationJob.pMesh->setBeamCrossSection(CrossSectionType{0.03, 0.026});
SimulationResults longSpanGridshell_simulationResults
= formFinder.executeSimulation(std::make_shared<SimulationJob>(
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<SimulationJob> &pJob)
{
//#ifdef USE_ENSMALLEN
// this->pJob = pJob;
//#endif
pMesh.reset();
pMesh = std::make_unique<SimulationMesh>(*pJob->pMesh);
vcg::tri::UpdateBounding<SimulationMesh>::Box(*pMesh);
constrainedVertices.clear();
constrainedVertices = pJob->constrainedVertices;
if (!pJob->nodalForcedDisplacements.empty()) {
for (std::pair<VertexIndex, Eigen::Vector3d> 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<SimulationJob> &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<double>::max();
reset(pJob);
#ifdef POLYSCOPE_DEFINED
if (mSettings.shouldDraw || mSettings.debugModeStep.has_value()) {
PolyscopeInterface::init();
polyscope::registerCurveNetwork(meshPolyscopeLabel,
pMesh->getEigenVertices(),
pMesh->getEigenEdges());
polyscope::registerCurveNetwork("Initial_" + meshPolyscopeLabel,
pMesh->getEigenVertices(),
pMesh->getEigenEdges())
->setRadius(0.002);
// registerWorldAxes();
}
#endif
if (!pJob->nodalForcedDisplacements.empty() && pJob->nodalExternalForces.empty()) {
shouldApplyInitialDistortion = true;
}
updateElementalFrames();
}
VectorType DRMSimulationModel::computeDisplacementDifferenceDerivative(
const EdgeType &e, const DifferentiateWithRespectTo &dui) const
{
VectorType displacementDiffDeriv(0, 0, 0);
const DoFType &dofi = dui.dofi;
const bool differentiateWithRespectToANonEdgeNode = e.cV(0) != &dui.v && e.cV(1) != &dui.v;
if (differentiateWithRespectToANonEdgeNode || dofi > 2) {
return displacementDiffDeriv;
}
if (e.cV(0) == &dui.v) {
displacementDiffDeriv[dofi] = -1;
} else if (e.cV(1) == &dui.v) {
displacementDiffDeriv[dofi] = 1;
}
return displacementDiffDeriv;
}
VectorType DRMSimulationModel::computeDerivativeOfNormal(const VertexType &v,
const DifferentiateWithRespectTo &dui) const
{
const size_t vi = pMesh->getIndex(v);
VectorType normalDerivative(0, 0, 0);
if (&dui.v != &v || (dui.dofi == 0 || dui.dofi == 1 || dui.dofi == 2 || dui.dofi == 5)) {
return normalDerivative;
}
const VectorType &n = v.cN();
const double &nx = n[0], ny = n[1];
const double nxnyMagnitude = std::pow(nx, 2) + std::pow(ny, 2);
if (dui.dofi == 3) {
if (nxnyMagnitude + 1e-5 >= 1) {
const double normalDerivativeX = 1 / sqrt(nxnyMagnitude)
- std::pow(nx, 2) / std::pow(nxnyMagnitude, 1.5);
const double normalDerivativeY = -nx * ny / std::pow(nxnyMagnitude, 1.5);
const double normalDerivativeZ = 0;
normalDerivative = VectorType(normalDerivativeX, normalDerivativeY, normalDerivativeZ);
} else {
const double normalDerivativeX = 1;
const double normalDerivativeY = 0;
const double normalDerivativeZ = -nx / std::sqrt(1 - nxnyMagnitude);
normalDerivative = VectorType(normalDerivativeX, normalDerivativeY, normalDerivativeZ);
}
} else if (dui.dofi == 4) {
if (nxnyMagnitude + 1e-5 >= 1) {
const double normalDerivativeX = -nx * ny / std::pow(nxnyMagnitude, 1.5);
const double normalDerivativeY = 1 / sqrt(nxnyMagnitude)
- std::pow(ny, 2) / std::pow(nxnyMagnitude, 1.5);
const double normalDerivativeZ = 0;
normalDerivative = VectorType(normalDerivativeX, normalDerivativeY, normalDerivativeZ);
} else {
const double normalDerivativeX = 0;
const double normalDerivativeY = 1;
const double normalDerivativeZ = -ny / std::sqrt(1 - nxnyMagnitude);
normalDerivative = VectorType(normalDerivativeX, normalDerivativeY, normalDerivativeZ);
}
}
const bool normalDerivativeIsFinite = std::isfinite(normalDerivative[0])
&& std::isfinite(normalDerivative[1])
&& std::isfinite(normalDerivative[2]);
assert(normalDerivativeIsFinite);
const bool shouldBreak = mCurrentSimulationStep == 118 && vi == 1 && dui.dofi == 3;
return normalDerivative;
}
double DRMSimulationModel::computeDerivativeElementLength(
const EdgeType &e, const DifferentiateWithRespectTo &dui) const
{
if (e.cV(0) != &dui.v && e.cV(1) != &dui.v) {
return 0;
}
const VectorType &X_j = e.cP(0);
const VectorType &X_jplus1 = e.cP(1);
const VectorType positionVectorDiff = X_jplus1 - X_j;
const VectorType displacementDiffDeriv = computeDisplacementDifferenceDerivative(e, dui);
const double edgeLength = pMesh->elements[e].length;
const double L_kDeriv = positionVectorDiff * displacementDiffDeriv / edgeLength;
return L_kDeriv;
}
double DRMSimulationModel::computeDerivativeOfNorm(const VectorType &x,
const VectorType &derivativeOfX)
{
return x.dot(derivativeOfX) / x.Norm();
}
VectorType DRMSimulationModel::computeDerivativeOfCrossProduct(const VectorType &a,
const VectorType &derivativeOfA,
const VectorType &b,
const VectorType &derivativeOfB)
{
const auto firstTerm = Cross(derivativeOfA, b);
const auto secondTerm = Cross(a, derivativeOfB);
return firstTerm + secondTerm;
}
VectorType DRMSimulationModel::computeDerivativeOfR(const EdgeType &e,
const DifferentiateWithRespectTo &dui) const
{
const VertexType &v_j = *e.cV(0);
const VertexType &v_jplus1 = *e.cV(1);
const VectorType normal_j = v_j.cN();
const VectorType normal_jplus1 = v_jplus1.cN();
const VectorType derivativeOfNormal_j = &v_j == &dui.v && dui.dofi > 2
? pMesh->nodes[v_j].derivativeOfNormal[dui.dofi]
: VectorType(0, 0, 0);
const VectorType derivativeOfNormal_jplus1
= &v_jplus1 == &dui.v && dui.dofi > 2 ? pMesh->nodes[v_jplus1].derivativeOfNormal[dui.dofi]
: VectorType(0, 0, 0);
const VectorType derivativeOfSumOfNormals = derivativeOfNormal_j + derivativeOfNormal_jplus1;
const VectorType sumOfNormals = normal_j + normal_jplus1;
const double normOfSumOfNormals = sumOfNormals.Norm();
assert(normOfSumOfNormals != 0);
const double derivativeOfNormOfSumOfNormals = computeDerivativeOfNorm(sumOfNormals,
derivativeOfSumOfNormals);
const VectorType derivativeOfR_firstTerm = -sumOfNormals * derivativeOfNormOfSumOfNormals
/ std::pow(normOfSumOfNormals, 2);
const VectorType derivativeOfR_secondTerm = derivativeOfSumOfNormals / normOfSumOfNormals;
const VectorType derivativeOfR = derivativeOfR_firstTerm + derivativeOfR_secondTerm;
assert(std::isfinite(derivativeOfR[0]) && std::isfinite(derivativeOfR[1])
&& std::isfinite(derivativeOfR[2]));
return derivativeOfR;
}
VectorType DRMSimulationModel::computeDerivativeT1(const EdgeType &e,
const DifferentiateWithRespectTo &dui) const
{
const VectorType &X_j = e.cP(0);
const VectorType &X_jplus1 = e.cP(1);
const VectorType edgeVector = X_jplus1 - X_j;
const double L_kDerivative = computeDerivativeElementLength(e, dui);
const double edgeLength = pMesh->elements[e].length;
const VectorType firstTerm = -edgeVector * L_kDerivative / std::pow(edgeLength, 2);
const VectorType secondTerm = computeDisplacementDifferenceDerivative(e, dui) / edgeLength;
const VectorType t1Derivative = firstTerm + secondTerm;
return t1Derivative;
}
VectorType DRMSimulationModel::computeDerivativeT2(const EdgeType &e,
const DifferentiateWithRespectTo &dui) const
{
const DoFType dofi = dui.dofi;
const VertexType &v_j = *e.cV(0);
const size_t vi_j = pMesh->getIndex(v_j);
const VertexType &v_jplus1 = *e.cV(1);
const size_t vi_jplus1 = pMesh->getIndex(v_jplus1);
const VectorType r = (v_j.cN() + v_jplus1.cN()).Normalize();
const VectorType derivativeR_j = dofi > 2 && &v_j == &dui.v
? pMesh->elements[e].derivativeR_j[dui.dofi]
: VectorType(0, 0, 0);
const VectorType derivativeR_jplus1 = dofi > 2 && &v_jplus1 == &dui.v
? pMesh->elements[e].derivativeR_jplus1[dui.dofi]
: VectorType(0, 0, 0);
const VectorType derivativeR = derivativeR_j + derivativeR_jplus1;
const VectorType &t1 = pMesh->elements[e].frame.t1;
const VectorType derivativeT1_j = dofi < 3 && &v_j == &dui.v
? pMesh->elements[e].derivativeT1_j[dui.dofi]
: VectorType(0, 0, 0);
const VectorType derivativeT1_jplus1 = dofi < 3 && &v_jplus1 == &dui.v
? pMesh->elements[e].derivativeT1_jplus1[dui.dofi]
: VectorType(0, 0, 0);
const VectorType derivativeT1 = derivativeT1_j + derivativeT1_jplus1;
const VectorType derivativeOfRCrossT1 = computeDerivativeOfCrossProduct(r,
derivativeR,
t1,
derivativeT1);
const VectorType rCrossT1 = Cross(r, t1);
const double normOfRCrossT1 = rCrossT1.Norm();
const double derivativeNormRCrossT1 = computeDerivativeOfNorm(rCrossT1, derivativeOfRCrossT1);
const VectorType t2Deriv_firstTerm = -(rCrossT1 * derivativeNormRCrossT1)
/ std::pow(normOfRCrossT1, 2);
const VectorType t2Deriv_secondTerm = derivativeOfRCrossT1 / normOfRCrossT1;
const VectorType t2Deriv = t2Deriv_firstTerm + t2Deriv_secondTerm;
const double t2DerivNorm = t2Deriv.Norm();
assert(std::isfinite(t2DerivNorm));
const bool shouldBreak = mCurrentSimulationStep == 118 && (vi_jplus1 == 1 || vi_j == 1)
&& dofi == 3;
return t2Deriv;
}
VectorType DRMSimulationModel::computeDerivativeT3(const EdgeType &e,
const DifferentiateWithRespectTo &dui) const
{
const Element &element = pMesh->elements[e];
const VectorType &t1 = element.frame.t1;
const VectorType &t2 = element.frame.t2;
const VectorType t1CrossT2 = Cross(t1, t2);
const VertexType &v_j = *e.cV(0);
const size_t vi_j = pMesh->getIndex(v_j);
const VertexType &v_jplus1 = *e.cV(1);
const size_t vi_jplus1 = pMesh->getIndex(v_jplus1);
const VectorType derivativeT1_j = dui.dofi < 3 && &v_j == &dui.v
? pMesh->elements[e].derivativeT1_j[dui.dofi]
: VectorType(0, 0, 0);
const VectorType derivativeT1_jplus1 = dui.dofi < 3 && &v_jplus1 == &dui.v
? pMesh->elements[e].derivativeT1_jplus1[dui.dofi]
: VectorType(0, 0, 0);
const VectorType derivativeT1 = derivativeT1_j + derivativeT1_jplus1;
// const VectorType derivativeOfT2 = computeDerivativeT2(e, dui);
// const VectorType derivativeT2_j =
// &v_j == &dui.v
// ? mesh->elements[e].derivativeT2_j[dui.dofi]
// : VectorType(0, 0, 0);
// const VectorType derivativeT2_jplus1 =
// &v_jplus1 == &dui.v
// ? mesh->elements[e].derivativeT2_jplus1[dui.dofi]
// : VectorType(0, 0, 0);
VectorType derivativeT2(0, 0, 0);
if (&v_j == &dui.v) {
derivativeT2 = pMesh->elements[e].derivativeT2_j[dui.dofi];
} else if (&v_jplus1 == &dui.v) {
derivativeT2 = pMesh->elements[e].derivativeT2_jplus1[dui.dofi];
}
const VectorType derivativeT1CrossT2 = computeDerivativeOfCrossProduct(t1,
derivativeT1,
t2,
derivativeT2);
const double derivativeOfNormT1CrossT2 = computeDerivativeOfNorm(t1CrossT2, derivativeT1CrossT2);
const double normT1CrossT2 = t1CrossT2.Norm();
const VectorType t3Deriv_firstTerm = -(t1CrossT2 * derivativeOfNormT1CrossT2)
/ std::pow(normT1CrossT2, 2);
const VectorType t3Deriv_secondTerm = derivativeT1CrossT2 / normT1CrossT2;
const VectorType t3Deriv = t3Deriv_firstTerm + t3Deriv_secondTerm;
assert(std::isfinite(t3Deriv[0]) && std::isfinite(t3Deriv[1]) && std::isfinite(t3Deriv[2]));
return t3Deriv;
}
double DRMSimulationModel::computeDerivativeTheta1(const EdgeType &e,
const VertexIndex &evi,
const VertexIndex &dwrt_evi,
const DoFType &dwrt_dofi) const
{
const VertexType &v = *e.cV(evi);
const size_t vi = pMesh->getIndex(v);
const Element &element = pMesh->elements[e];
const VectorType derivativeT1 = element.derivativeT1[dwrt_evi][dwrt_dofi];
const VectorType derivativeT3 = element.derivativeT3[dwrt_evi][dwrt_dofi];
const VectorType nDerivative = evi != dwrt_evi ? VectorType(0, 0, 0)
: pMesh->nodes[v].derivativeOfNormal[dwrt_dofi];
const VectorType n = v.cN();
const VectorType &t1 = element.frame.t1;
const VectorType &t3 = element.frame.t3;
const double theta1Derivative = derivativeT1 * Cross(t3, n)
+ t1 * (Cross(derivativeT3, n) + Cross(t3, nDerivative));
const bool shouldBreak = mCurrentSimulationStep == 118 && vi == 1 && dwrt_dofi == 3;
return theta1Derivative;
}
double DRMSimulationModel::computeDerivativeTheta2(const EdgeType &e,
const VertexIndex &evi,
const VertexIndex &dwrt_evi,
const DoFType &dwrt_dofi) const
{
const VertexType &v = *e.cV(evi);
const Element &element = pMesh->elements[e];
const VectorType derivativeT2 = element.derivativeT2[dwrt_evi][dwrt_dofi];
const VectorType derivativeT3 = element.derivativeT3[dwrt_evi][dwrt_dofi];
const VectorType n = v.cN();
const VectorType &t2 = element.frame.t2;
const VectorType &t3 = element.frame.t3;
const VectorType nDerivative = dwrt_evi == evi ? pMesh->nodes[v].derivativeOfNormal[dwrt_dofi]
: VectorType(0, 0, 0);
const double theta2Derivative = derivativeT2 * Cross(t3, n)
+ t2 * (Cross(derivativeT3, n) + Cross(t3, nDerivative));
return theta2Derivative;
}
double DRMSimulationModel::computeTheta3(const EdgeType &e, const VertexType &v)
{
const VertexIndex &vi = pMesh->nodes[v].vi;
// assert(e.cV(0) == &v || e.cV(1) == &v);
const Element &elem = pMesh->elements[e];
const EdgeIndex &ei = elem.ei;
const Element::LocalFrame &ef = elem.frame;
const VectorType &t1 = ef.t1;
const VectorType &n = v.cN();
const Node &node = pMesh->nodes[v];
const double &nR = node.nR; // TODO: This makes the function non-const.
// Should be refactored in the future
double theta3;
const bool shouldBreak = mCurrentSimulationStep == 12970;
if (&e == node.referenceElement) {
// Use nR as theta3 only for the first star edge
return nR;
}
// std::vector<int> 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<EdgeIndex, double> &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<double> rotationMatrix;
rotationMatrix.SetRotateRad(rotationAngle, n);
const double cosRotationAngle = cos(rotationAngle);
const double sinRotationAngle = sin(rotationAngle);
const VectorType f2 = (f1 * cosRotationAngle + Cross(n, f1) * sinRotationAngle).Normalize();
const VectorType &t1Current = t1;
const VectorType f3 = Cross(t1Current, f2);
Element &element = pMesh->elements[e];
// Save for computing theta3Derivative
if (&v == e.cV(0)) {
element.f1_j = f1;
element.f2_j = f2;
element.f3_j = f3;
element.cosRotationAngle_j = cosRotationAngle;
element.sinRotationAngle_j = sinRotationAngle;
} else {
element.f1_jplus1 = f1;
element.f2_jplus1 = f2;
element.f3_jplus1 = f3;
element.cosRotationAngle_jplus1 = cosRotationAngle;
element.sinRotationAngle_jplus1 = sinRotationAngle;
}
theta3 = f3.dot(n);
return theta3;
}
double DRMSimulationModel::computeDerivativeTheta3(const EdgeType &e,
const VertexType &v,
const DifferentiateWithRespectTo &dui) const
{
const Node &node = pMesh->nodes[v];
const VertexIndex &vi = pMesh->nodes[v].vi;
if (&e == node.referenceElement && !isRigidSupport[vi]) {
if (dui.dofi == DoF::Nr && &dui.v == &v) {
return 1;
} else {
return 0;
}
}
// assert(e.cV(0) == &v || e.cV(1) == &v);
const Element &element = pMesh->elements[e];
const Element::LocalFrame &ef = element.frame;
const VectorType &t1 = ef.t1;
const VectorType &n = v.cN();
const DoFType &dofi = dui.dofi;
const VertexPointer &vp_j = e.cV(0);
const VertexPointer &vp_jplus1 = e.cV(1);
double derivativeTheta3_dofi = 0;
if (isRigidSupport[vi]) {
const VectorType &t1Initial = computeT1Vector(pMesh->nodes[vp_j].initialLocation,
pMesh->nodes[vp_jplus1].initialLocation);
VectorType g1 = Cross(t1, t1Initial);
const VectorType derivativeInitialT1_dofi(0, 0, 0);
const VectorType derivativeT1_j = dui.dofi < 3 && vp_j == &dui.v
? element.derivativeT1_j[dui.dofi]
: VectorType(0, 0, 0);
const VectorType derivativeT1_jplus1 = dui.dofi < 3 && vp_jplus1 == &dui.v
? element.derivativeT1_jplus1[dui.dofi]
: VectorType(0, 0, 0);
const VectorType derivativeT1_dofi = derivativeT1_j + derivativeT1_jplus1;
// VectorType derivativeT1_dofi(0, 0, 0);
// if (dui.dofi < 3) {
// if (&v_j == &dui.v) {
// derivativeT1_dofi = mesh->elements[e].derivativeT1_j[dui.dofi];
// } else if (&v_jplus1 == &dui.v) {
// derivativeT1_dofi =
// mesh->elements[e].derivativeT1_jplus1[dui.dofi];
// }
// }
const VectorType derivativeG1_firstTerm = Cross(derivativeT1_dofi, t1Initial);
const VectorType derivativeG1_secondTerm = Cross(t1, derivativeInitialT1_dofi);
const VectorType derivativeG1 = derivativeG1_firstTerm + derivativeG1_secondTerm;
const VectorType derivativeNormal = &v == &dui.v && dui.dofi > 2
? node.derivativeOfNormal[dui.dofi]
: VectorType(0, 0, 0);
derivativeTheta3_dofi = derivativeG1 * n + g1 * derivativeNormal;
if (std::isnan(derivativeTheta3_dofi)) {
std::cerr << "nan derivative theta3 rigid" << std::endl;
}
return derivativeTheta3_dofi;
}
EdgeType &refElem = *node.referenceElement;
const VectorType &t1_k = pMesh->elements[refElem].frame.t1;
VectorType f1, f2, f3;
double cosRotationAngle, sinRotationAngle;
if (e.cV(0) == &v) {
f1 = element.f1_j;
cosRotationAngle = element.cosRotationAngle_j;
sinRotationAngle = element.sinRotationAngle_j;
f2 = element.f2_j;
f3 = element.f3_j;
} else {
f1 = element.f1_jplus1;
cosRotationAngle = element.cosRotationAngle_jplus1;
sinRotationAngle = element.sinRotationAngle_jplus1;
f2 = element.f2_jplus1;
f3 = element.f3_jplus1;
}
const VectorType &t1_kplus1 = t1;
const VectorType f1Normalized = f1 / f1.Norm();
VectorType derivativeF1(0, 0, 0);
VectorType derivativeF2(0, 0, 0);
VectorType derivativeF3(0, 0, 0);
if (dui.dofi < 3) {
const VectorType derivativeT1_kplus1_j = vp_j == &dui.v ? element.derivativeT1_j[dui.dofi]
: VectorType(0, 0, 0);
const VectorType derivativeT1_kplus1_jplus1 = vp_jplus1 == &dui.v
? element.derivativeT1_jplus1[dui.dofi]
: VectorType(0, 0, 0);
const VectorType derivativeT1_kplus1 = derivativeT1_kplus1_j + derivativeT1_kplus1_jplus1;
const VectorType derivativeT1_k_j = refElem.cV(0) == &dui.v
? pMesh->elements[refElem].derivativeT1_j[dui.dofi]
: VectorType(0, 0, 0);
const VectorType derivativeT1_k_jplus1
= refElem.cV(1) == &dui.v ? pMesh->elements[refElem].derivativeT1_jplus1[dui.dofi]
: VectorType(0, 0, 0);
const VectorType derivativeT1_k = derivativeT1_k_j + derivativeT1_k_jplus1;
derivativeF1 = derivativeT1_k - (n * (derivativeT1_k * n));
const double f1Norm = f1.Norm();
const double derivativeF1Norm = f1 * derivativeF1 / f1Norm;
const VectorType derivativeF1Normalized = -f1 * derivativeF1Norm / (f1Norm * f1Norm)
+ derivativeF1 / f1Norm;
derivativeF2 = derivativeF1Normalized * cosRotationAngle
+ Cross(n, derivativeF1Normalized) * sinRotationAngle;
const VectorType derivativeF3_firstTerm = Cross(derivativeT1_kplus1, f2);
const VectorType derivativeF3_secondTerm = Cross(t1_kplus1, derivativeF2);
derivativeF3 = derivativeF3_firstTerm + derivativeF3_secondTerm;
derivativeTheta3_dofi = derivativeF3 * n;
} else if (dui.dofi == DoF::Nr && &dui.v == &v) {
derivativeF2 = f1Normalized * (-sinRotationAngle)
+ Cross(n, f1Normalized) * cosRotationAngle;
derivativeF3 = Cross(t1_kplus1, derivativeF2);
derivativeTheta3_dofi = derivativeF3 * n;
} else { // 2<dofi<5
if (&v == &dui.v) {
const VectorType &derivativeOfNormal = node.derivativeOfNormal[dofi];
derivativeF1 = -(n * (t1_k * derivativeOfNormal) + derivativeOfNormal * (t1_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(derivativeOfNormal, f1Normalized)
+ Cross(n, derivativeF1Normalized))
* sinRotationAngle;
derivativeF3 = Cross(t1_kplus1, derivativeF2);
derivativeTheta3_dofi = derivativeF3 * n + f3 * derivativeOfNormal;
}
}
if (std::isnan(derivativeTheta3_dofi)) {
std::cerr << "nan derivative theta3" << std::endl;
}
return derivativeTheta3_dofi;
}
double DRMSimulationModel::computeTotalInternalPotentialEnergy()
{
double totalInternalPotentialEnergy = 0;
for (const SimulationMesh::EdgeType &e : pMesh->edge) {
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<VertexIndex, std::unordered_set<DoFType>> &fixedVertices)
{
// std::vector<Vector6d> internalForcesParallel(mesh->vert.size());
std::vector<std::vector<std::pair<int, Vector6d>>> internalForcesContributionsFromEachEdge(
pMesh->EN(), std::vector<std::pair<int, Vector6d>>(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<std::pair<int, Vector6d>>
internalForcesContributionFromThisEdge(4, {-1, Vector6d()});
for (VertexIndex evi = 0; evi < 2; evi++) {
const SimulationMesh::VertexType &ev = *e.cV(evi);
const Node &edgeNode = pMesh->nodes[ev];
internalForcesContributionFromThisEdge[evi].first = edgeNode.vi;
const VertexPointer &rev_j = edgeNode.referenceElement->cV(0);
const VertexPointer &rev_jplus1 = edgeNode.referenceElement->cV(1);
// refElemOtherVertex can be precomputed
const VertexPointer &refElemOtherVertex = rev_j == &ev ? rev_jplus1 : rev_j;
const Node &refElemOtherVertexNode = pMesh->nodes[refElemOtherVertex];
if (edgeNode.referenceElement != &e) {
internalForcesContributionFromThisEdge[evi + 2].first = refElemOtherVertexNode.vi;
}
const size_t vi = edgeNode.vi;
// #pragma omp parallel for schedule(static) num_threads(6)
for (DoFType dofi = DoF::Ux; dofi < DoF::NumDoF; dofi++) {
const bool isDofConstrainedFor_ev = isVertexConstrained[edgeNode.vi]
&& fixedVertices.at(edgeNode.vi)
.contains(dofi);
if (!isDofConstrainedFor_ev) {
DifferentiateWithRespectTo dui{ev, dofi};
// Axial force computation
const double e_k = element.length - element.initialLength;
const double e_kDeriv = computeDerivativeElementLength(e, dui);
const double axialForce_dofi = e_kDeriv * e_k * element.rigidity.axial;
#ifdef POLYSCOPE_DEFINED
if (std::isnan(axialForce_dofi)) {
std::cerr << "nan in axial" << evi << std::endl;
}
#endif
// Torsional force computation
const double theta1_j_deriv = computeDerivativeTheta1(e, 0, evi, dofi);
const double theta1_jplus1_deriv = computeDerivativeTheta1(e, 1, evi, dofi);
const double theta1Diff = theta1_jplus1 - theta1_j;
const double theta1DiffDerivative = theta1_jplus1_deriv - theta1_j_deriv;
const double torsionalForce_dofi = theta1DiffDerivative * theta1Diff
* element.rigidity.torsional;
#ifdef POLYSCOPE_DEFINED
if (std::isnan(torsionalForce_dofi)) {
std::cerr << "nan in torsional" << evi << std::endl;
}
#endif
// First bending force computation
////theta2_j derivative
const double theta2_j_deriv = computeDerivativeTheta2(e, 0, evi, dofi);
////theta2_jplus1 derivative
const double theta2_jplus1_deriv = computeDerivativeTheta2(e, 1, evi, dofi);
////1st in bracket term
const double firstBendingForce_inBracketsTerm_0 = theta2_j_deriv * 2
* theta2_j;
////2nd in bracket term
const double firstBendingForce_inBracketsTerm_1 = theta2_jplus1_deriv
* theta2_j;
////3rd in bracket term
const double firstBendingForce_inBracketsTerm_2 = theta2_j_deriv
* theta2_jplus1;
////4th in bracket term
const double firstBendingForce_inBracketsTerm_3 = 2 * theta2_jplus1_deriv
* theta2_jplus1;
// 3rd term computation
const double firstBendingForce_inBracketsTerm
= firstBendingForce_inBracketsTerm_0
+ firstBendingForce_inBracketsTerm_1
+ firstBendingForce_inBracketsTerm_2
+ firstBendingForce_inBracketsTerm_3;
const double firstBendingForce_dofi = firstBendingForce_inBracketsTerm
* element.rigidity.firstBending;
// Second bending force computation
////theta2_j derivative
const double theta3_j_deriv = computeDerivativeTheta3(e, ev_j, dui);
////theta2_jplus1 derivative
const double theta3_jplus1_deriv = computeDerivativeTheta3(e,
ev_jplus1,
dui);
////1st in bracket term
const double secondBendingForce_inBracketsTerm_0 = theta3_j_deriv * 2
* theta3_j;
////2nd in bracket term
const double secondBendingForce_inBracketsTerm_1 = theta3_jplus1_deriv
* theta3_j;
////3rd in bracket term
const double secondBendingForce_inBracketsTerm_2 = theta3_j_deriv
* theta3_jplus1;
////4th in bracket term
const double secondBendingForce_inBracketsTerm_3 = 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[evi].second[dofi]
= axialForce_dofi + firstBendingForce_dofi + secondBendingForce_dofi
+ torsionalForce_dofi;
}
if (edgeNode.referenceElement != &e) {
const bool isDofConstrainedFor_refElemOtherVertex
= isVertexConstrained[refElemOtherVertexNode.vi]
&& fixedVertices.at(refElemOtherVertexNode.vi).contains(dofi);
if (!isDofConstrainedFor_refElemOtherVertex) {
DifferentiateWithRespectTo dui{*refElemOtherVertex, dofi};
////theta3_j derivative
const double theta3_j_deriv = computeDerivativeTheta3(e, ev_j, dui);
////theta3_jplus1 derivative
const double theta3_jplus1_deriv = computeDerivativeTheta3(e,
ev_jplus1,
dui);
////1st in bracket term
const double secondBendingForce_inBracketsTerm_0 = theta3_j_deriv * 2
* theta3_j;
////2nd in bracket term
const double secondBendingForce_inBracketsTerm_1 = theta3_jplus1_deriv
* theta3_j;
////3rd in bracket term
const double secondBendingForce_inBracketsTerm_2 = theta3_j_deriv
* theta3_jplus1;
////4th in bracket term
const double secondBendingForce_inBracketsTerm_3 = theta3_jplus1_deriv
* 2 * theta3_jplus1;
// 4th term computation
const double secondBendingForce_inBracketsTerm
= secondBendingForce_inBracketsTerm_0
+ secondBendingForce_inBracketsTerm_1
+ secondBendingForce_inBracketsTerm_2
+ secondBendingForce_inBracketsTerm_3;
const double secondBendingForce_dofi = secondBendingForce_inBracketsTerm
* element.rigidity.secondBending;
internalForcesContributionFromThisEdge[evi + 2].second[dofi]
= secondBendingForce_dofi;
}
}
}
}
internalForcesContributionsFromEachEdge[ei] = internalForcesContributionFromThisEdge;
});
//#pragma omp parallel for schedule(static) num_threads(8)
for (size_t vi = 0; vi < pMesh->VN(); vi++) {
Node::Forces &force = pMesh->nodes[vi].force;
// Vector6d ext = force.external;
// if (mCurrentSimulationStep <= 100) {
// ext[3] = 0;
// ext[4] = 0;
// }
force.residual = force.external;
force.internal = 0;
}
for (size_t ei = 0; ei < pMesh->EN(); ei++) {
for (int i = 0; i < 4; i++) {
std::pair<int, Vector6d> internalForcePair
= internalForcesContributionsFromEachEdge[ei][i];
int vi = internalForcePair.first;
if (i > 1 && vi == -1) {
continue;
}
Node::Forces &force = pMesh->nodes[vi].force;
force.internal = force.internal + internalForcePair.second;
force.residual = force.residual + (internalForcePair.second * -1);
#ifdef POLYSCOPE_DEFINED
if (std::isnan(internalForcePair.second.norm())) {
std::cerr << "nan on edge" << ei << std::endl;
}
#endif
}
}
double totalResidualForcesNorm = 0;
Vector6d sumOfResidualForces(0);
for (size_t vi = 0; vi < pMesh->VN(); vi++) {
Node::Forces &force = pMesh->nodes[vi].force;
const Vector6d &nodeResidualForce = force.residual;
sumOfResidualForces = sumOfResidualForces + nodeResidualForce;
const double residualForceNorm = nodeResidualForce.norm();
// const double residualForceNorm = nodeResidualForce.getTranslation().norm();
totalResidualForcesNorm += residualForceNorm;
#ifdef POLYSCOPE_DEFINED
if (std::isnan(force.residual.norm())) {
std::cout << "residual " << vi << ":" << force.residual.toString() << 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<VertexIndex, Vector6d> &nodalForces,
const std::unordered_map<VertexIndex, std::unordered_set<DoFType>> &fixedVertices)
{
externalMomentsNorm = 0;
double totalExternalForcesNorm = 0;
for (const std::pair<VertexIndex, Vector6d> &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<std::array<Vector6d, 4>> DRMSimulationModel::computeInternalForces(
const std::unordered_map<VertexIndex, std::unordered_set<DoFType>> &fixedVertices)
{
std::vector<std::array<std::pair<int, Vector6d>, 4>>
internalForcesContributionsFromEachEdge_axial(pMesh->EN(),
std::array<std::pair<int, Vector6d>, 4>{
std::pair<int, Vector6d>(-1, Vector6d())});
std::vector<std::array<std::pair<int, Vector6d>, 4>>
internalForcesContributionsFromEachEdge_torsion(pMesh->EN(),
std::array<std::pair<int, Vector6d>, 4>{
std::pair<int, Vector6d>(-1,
Vector6d())});
std::vector<std::array<std::pair<int, Vector6d>, 4>>
internalForcesContributionsFromEachEdge_firstBending(
pMesh->EN(),
std::array<std::pair<int, Vector6d>, 4>{std::pair<int, Vector6d>(-1, Vector6d())});
std::vector<std::array<std::pair<int, Vector6d>, 4>>
internalForcesContributionsFromEachEdge_secondBending(
pMesh->EN(),
std::array<std::pair<int, Vector6d>, 4>{std::pair<int, Vector6d>(-1, Vector6d())});
// omp_lock_t writelock;
// omp_init_lock(&writelock);
//#ifdef ENABLE_OPENMP
//#pragma omp parallel for schedule(static) num_threads(5)
//#endif
std::for_each(
#ifdef ENABLE_PARALLEL_DRM
std::execution::par_unseq,
#endif
pMesh->edge.begin(),
pMesh->edge.end(),
[&](const EdgeType &e)
// for (int ei = 0; ei < pMesh->EN(); ei++)
{
const int ei = pMesh->getIndex(e);
// const EdgeType &e = pMesh->edge[ei];
const SimulationMesh::VertexType &ev_j = *e.cV(0);
const SimulationMesh::VertexType &ev_jplus1 = *e.cV(1);
const Element &element = pMesh->elements[e];
const Element::LocalFrame &ef = element.frame;
const VectorType t3CrossN_j = Cross(ef.t3, ev_j.cN());
const VectorType t3CrossN_jplus1 = Cross(ef.t3, ev_jplus1.cN());
const double theta1_j = ef.t1.dot(t3CrossN_j);
const double theta1_jplus1 = ef.t1.dot(t3CrossN_jplus1);
const double theta2_j = ef.t2.dot(t3CrossN_j);
const double theta2_jplus1 = ef.t2.dot(t3CrossN_jplus1);
const double theta3_j = computeTheta3(e, ev_j);
const double theta3_jplus1 = computeTheta3(e, ev_jplus1);
// element.rotationalDisplacements_j.theta1 = theta1_j;
// element.rotationalDisplacements_jplus1.theta1 = theta1_jplus1;
// element.rotationalDisplacements_j.theta2 = theta2_j;
// element.rotationalDisplacements_jplus1.theta2 = theta2_jplus1;
// element.rotationalDisplacements_j.theta3 = theta3_j;
// element.rotationalDisplacements_jplus1.theta3 = theta3_jplus1;
// std::array<std::pair<int, Vector6d>, 4> internalForcesContributionFromThisEdge{
// std::pair<int, Vector6d>(-1, Vector6d())};
std::array<std::pair<int, Vector6d>, 4> internalForcesContributionFromThisEdge_axial{
std::pair<int, Vector6d>(-1, Vector6d())};
std::array<std::pair<int, Vector6d>, 4> internalForcesContributionFromThisEdge_torsion{
std::pair<int, Vector6d>(-1, Vector6d())};
std::array<std::pair<int, Vector6d>, 4>
internalForcesContributionFromThisEdge_firstBending{
std::pair<int, Vector6d>(-1, Vector6d())};
std::array<std::pair<int, Vector6d>, 4>
internalForcesContributionFromThisEdge_secondBending{
std::pair<int, Vector6d>(-1, Vector6d())};
for (VertexIndex evi = 0; evi < 2; evi++) {
const SimulationMesh::VertexType &ev = *e.cV(evi);
const Node &edgeNode = pMesh->nodes[ev];
internalForcesContributionFromThisEdge_axial[evi].first = edgeNode.vi;
const VertexPointer &rev_j = edgeNode.referenceElement->cV(0);
const VertexPointer &rev_jplus1 = edgeNode.referenceElement->cV(1);
// refElemOtherVertex can be precomputed
const VertexPointer &refElemOtherVertex = rev_j == &ev ? rev_jplus1 : rev_j;
const Node &refElemOtherVertexNode = pMesh->nodes[refElemOtherVertex];
if (edgeNode.referenceElement != &e) {
internalForcesContributionFromThisEdge_axial[evi + 2].first
= refElemOtherVertexNode.vi;
}
const size_t vi = edgeNode.vi;
// #pragma omp parallel for schedule(static) num_threads(6)
for (DoFType dofi = DoF::Ux; dofi < DoF::NumDoF; dofi++) {
const bool isDofConstrainedFor_ev = isVertexConstrained[edgeNode.vi]
&& fixedVertices.at(edgeNode.vi)
.contains(dofi);
if (!isDofConstrainedFor_ev) {
DifferentiateWithRespectTo dui{ev, dofi};
// Axial force computation
const double e_k = element.length - element.initialLength;
const double e_kDeriv = computeDerivativeElementLength(e, dui);
const double axialForce_dofi = e_kDeriv * e_k * element.rigidity.axial;
#ifdef POLYSCOPE_DEFINED
if (std::isnan(axialForce_dofi)) {
std::cerr << "nan in axial" << evi << std::endl;
}
#endif
// Torsional force computation
const double theta1_j_deriv = computeDerivativeTheta1(e, 0, evi, dofi);
const double theta1_jplus1_deriv = computeDerivativeTheta1(e, 1, evi, dofi);
const double theta1Diff = theta1_jplus1 - theta1_j;
const double theta1DiffDerivative = theta1_jplus1_deriv - theta1_j_deriv;
const double torsionalForce_dofi = theta1DiffDerivative * theta1Diff
* element.rigidity.torsional;
#ifdef POLYSCOPE_DEFINED
if (std::isnan(torsionalForce_dofi)) {
std::cerr << "nan in torsional" << evi << std::endl;
}
#endif
// First bending force computation
////theta2_j derivative
const double theta2_j_deriv = computeDerivativeTheta2(e, 0, evi, dofi);
////theta2_jplus1 derivative
const double theta2_jplus1_deriv = computeDerivativeTheta2(e, 1, evi, dofi);
////1st in bracket term
const double firstBendingForce_inBracketsTerm_0 = theta2_j_deriv * 2
* theta2_j;
////2nd in bracket term
const double firstBendingForce_inBracketsTerm_1 = theta2_jplus1_deriv
* theta2_j;
////3rd in bracket term
const double firstBendingForce_inBracketsTerm_2 = theta2_j_deriv
* theta2_jplus1;
////4th in bracket term
const double firstBendingForce_inBracketsTerm_3 = 2 * theta2_jplus1_deriv
* theta2_jplus1;
// 3rd term computation
const double firstBendingForce_inBracketsTerm
= firstBendingForce_inBracketsTerm_0
+ firstBendingForce_inBracketsTerm_1
+ firstBendingForce_inBracketsTerm_2
+ firstBendingForce_inBracketsTerm_3;
const double firstBendingForce_dofi = firstBendingForce_inBracketsTerm
* element.rigidity.firstBending;
// Second bending force computation
////theta2_j derivative
const double theta3_j_deriv = computeDerivativeTheta3(e, ev_j, dui);
////theta2_jplus1 derivative
const double theta3_jplus1_deriv = computeDerivativeTheta3(e,
ev_jplus1,
dui);
////1st in bracket term
const double secondBendingForce_inBracketsTerm_0 = theta3_j_deriv * 2
* theta3_j;
////2nd in bracket term
const double secondBendingForce_inBracketsTerm_1 = theta3_jplus1_deriv
* theta3_j;
////3rd in bracket term
const double secondBendingForce_inBracketsTerm_2 = theta3_j_deriv
* theta3_jplus1;
////4th in bracket term
const double secondBendingForce_inBracketsTerm_3 = theta3_jplus1_deriv * 2
* theta3_jplus1;
// 3rd term computation
const double secondBendingForce_inBracketsTerm
= secondBendingForce_inBracketsTerm_0
+ secondBendingForce_inBracketsTerm_1
+ secondBendingForce_inBracketsTerm_2
+ secondBendingForce_inBracketsTerm_3;
double secondBendingForce_dofi = secondBendingForce_inBracketsTerm
* element.rigidity.secondBending;
internalForcesContributionFromThisEdge_axial[evi].second[dofi]
= axialForce_dofi;
internalForcesContributionFromThisEdge_torsion[evi].second[dofi]
= torsionalForce_dofi;
internalForcesContributionFromThisEdge_firstBending[evi].second[dofi]
= firstBendingForce_dofi;
internalForcesContributionFromThisEdge_secondBending[evi].second[dofi]
= secondBendingForce_dofi;
}
if (edgeNode.referenceElement != &e) {
const bool isDofConstrainedFor_refElemOtherVertex
= isVertexConstrained[refElemOtherVertexNode.vi]
&& fixedVertices.at(refElemOtherVertexNode.vi).contains(dofi);
if (!isDofConstrainedFor_refElemOtherVertex) {
DifferentiateWithRespectTo dui{*refElemOtherVertex, dofi};
////theta3_j derivative
const double theta3_j_deriv = computeDerivativeTheta3(e, ev_j, dui);
////theta3_jplus1 derivative
const double theta3_jplus1_deriv = computeDerivativeTheta3(e,
ev_jplus1,
dui);
////1st in bracket term
const double secondBendingForce_inBracketsTerm_0 = theta3_j_deriv * 2
* theta3_j;
////2nd in bracket term
const double secondBendingForce_inBracketsTerm_1 = theta3_jplus1_deriv
* theta3_j;
////3rd in bracket term
const double secondBendingForce_inBracketsTerm_2 = theta3_j_deriv
* theta3_jplus1;
////4th in bracket term
const double secondBendingForce_inBracketsTerm_3 = theta3_jplus1_deriv
* 2 * theta3_jplus1;
// 4th term computation
const double secondBendingForce_inBracketsTerm
= secondBendingForce_inBracketsTerm_0
+ secondBendingForce_inBracketsTerm_1
+ secondBendingForce_inBracketsTerm_2
+ secondBendingForce_inBracketsTerm_3;
const double secondBendingForce_dofi = secondBendingForce_inBracketsTerm
* element.rigidity.secondBending;
internalForcesContributionFromThisEdge_secondBending[evi + 2].second[dofi]
= secondBendingForce_dofi;
}
}
}
}
internalForcesContributionsFromEachEdge_axial[ei]
= internalForcesContributionFromThisEdge_axial;
internalForcesContributionsFromEachEdge_torsion[ei]
= internalForcesContributionFromThisEdge_torsion;
internalForcesContributionsFromEachEdge_firstBending[ei]
= internalForcesContributionFromThisEdge_firstBending;
internalForcesContributionsFromEachEdge_secondBending[ei]
= internalForcesContributionFromThisEdge_secondBending;
});
//#pragma omp parallel for schedule(static) num_threads(8)
std::vector<std::array<Vector6d, 4>> perVertexInternalForces(pMesh->VN(), {0});
for (size_t ei = 0; ei < pMesh->EN(); ei++) {
for (int i = 0; i < 4; i++) {
std::pair<int, Vector6d> internalForcePair_axial
= internalForcesContributionsFromEachEdge_axial[ei][i];
std::pair<int, Vector6d> internalForcePair_torsion
= internalForcesContributionsFromEachEdge_torsion[ei][i];
std::pair<int, Vector6d> internalForcePair_firstBending
= internalForcesContributionsFromEachEdge_firstBending[ei][i];
std::pair<int, Vector6d> internalForcePair_secondBending
= internalForcesContributionsFromEachEdge_secondBending[ei][i];
int vi = internalForcePair_axial.first;
if (i > 1 && vi == -1) {
continue;
}
perVertexInternalForces[vi][0] = perVertexInternalForces[vi][0]
+ internalForcePair_axial.second;
perVertexInternalForces[vi][1] = perVertexInternalForces[vi][1]
+ internalForcePair_torsion.second;
perVertexInternalForces[vi][2] = perVertexInternalForces[vi][2]
+ internalForcePair_firstBending.second;
perVertexInternalForces[vi][3] = perVertexInternalForces[vi][3]
+ internalForcePair_secondBending.second;
}
}
return perVertexInternalForces;
}
void DRMSimulationModel::updateResidualForces()
{
pMesh->totalResidualForcesNorm = 0;
for (const VertexType &v : pMesh->vert) {
Node &node = pMesh->nodes[v];
node.force.residual = node.force.external - node.force.internal;
const double residualForceNorm = (node.force.residual).norm();
pMesh->totalResidualForcesNorm += residualForceNorm;
}
}
void DRMSimulationModel::computeRigidSupports()
{
isRigidSupport.clear();
isRigidSupport.resize(pMesh->VN(), false);
for (const VertexType &v : pMesh->vert) {
const VertexIndex vi = pMesh->nodes[v].vi;
const bool isVertexConstrained = constrainedVertices.contains(vi);
if (isVertexConstrained) {
auto constrainedDoFType = constrainedVertices.at(vi);
const bool hasAllDoFTypeConstrained = constrainedDoFType.contains(DoF::Ux)
&& constrainedDoFType.contains(DoF::Uy)
&& constrainedDoFType.contains(DoF::Uz)
&& constrainedDoFType.contains(DoF::Nx)
&& constrainedDoFType.contains(DoF::Ny)
&& constrainedDoFType.contains(DoF::Nr);
if (hasAllDoFTypeConstrained) {
isRigidSupport[vi] = true;
}
}
}
}
void DRMSimulationModel::updateNormalDerivatives()
{
for (const VertexType &v : pMesh->vert) {
for (DoFType dofi = DoF::Nx; dofi < DoF::NumDoF; dofi++) {
DifferentiateWithRespectTo dui{v, dofi};
pMesh->nodes[v].derivativeOfNormal[dofi] = computeDerivativeOfNormal(v, dui);
}
}
}
void DRMSimulationModel::updateT1Derivatives()
{
for (const EdgeType &e : pMesh->edge) {
for (DoFType dofi = DoF::Ux; dofi < DoF::Nx; dofi++) {
DifferentiateWithRespectTo dui_v0{*e.cV(0), dofi};
Element &element = pMesh->elements[e];
element.derivativeT1_j[dofi] = computeDerivativeT1(e, dui_v0);
DifferentiateWithRespectTo dui_v1{*e.cV(1), dofi};
element.derivativeT1_jplus1[dofi] = computeDerivativeT1(e, dui_v1);
element.derivativeT1[0][dofi] = element.derivativeT1_j[dofi];
element.derivativeT1[1][dofi] = element.derivativeT1_jplus1[dofi];
}
}
}
void DRMSimulationModel::updateT2Derivatives()
{
for (const EdgeType &e : pMesh->edge) {
for (DoFType dofi = DoF::Ux; dofi < DoF::NumDoF; dofi++) {
DifferentiateWithRespectTo dui_v0{*e.cV(0), dofi};
pMesh->elements[e].derivativeT2_j[dofi] = computeDerivativeT2(e, dui_v0);
DifferentiateWithRespectTo dui_v1{*e.cV(1), dofi};
pMesh->elements[e].derivativeT2_jplus1[dofi] = computeDerivativeT2(e, dui_v1);
Element &element = pMesh->elements[e];
element.derivativeT2[0][dofi] = element.derivativeT2_j[dofi];
element.derivativeT2[1][dofi] = element.derivativeT2_jplus1[dofi];
}
}
}
void DRMSimulationModel::updateT3Derivatives()
{
for (const EdgeType &e : pMesh->edge) {
for (DoFType dofi = DoF::Ux; dofi < DoF::NumDoF; dofi++) {
DifferentiateWithRespectTo dui_v0{*e.cV(0), dofi};
Element &element = pMesh->elements[e];
element.derivativeT3_j[dofi] = computeDerivativeT3(e, dui_v0);
DifferentiateWithRespectTo dui_v1{*e.cV(1), dofi};
element.derivativeT3_jplus1[dofi] = computeDerivativeT3(e, dui_v1);
element.derivativeT3[0][dofi] = element.derivativeT3_j[dofi];
element.derivativeT3[1][dofi] = element.derivativeT3_jplus1[dofi];
}
}
}
void DRMSimulationModel::updateRDerivatives()
{
for (const EdgeType &e : pMesh->edge) {
for (DoFType dofi = DoF::Nx; dofi < DoF::NumDoF; dofi++) {
DifferentiateWithRespectTo dui_v0{*e.cV(0), dofi};
pMesh->elements[e].derivativeR_j[dofi] = computeDerivativeOfR(e, dui_v0);
DifferentiateWithRespectTo dui_v1{*e.cV(1), dofi};
pMesh->elements[e].derivativeR_jplus1[dofi] = computeDerivativeOfR(e, dui_v1);
}
}
}
void DRMSimulationModel::updateElementalLengths()
{
pMesh->updateElementalLengths();
}
DRMSimulationModel::DRMSimulationModel() {}
void DRMSimulationModel::updateNodalMasses()
{
double gamma = mSettings.gamma;
const size_t untilStep = 4000;
if (shouldTemporarilyDampForces && mCurrentSimulationStep < untilStep) {
gamma *= 1e6 * (1 - static_cast<double>(mCurrentSimulationStep) / untilStep);
}
// if (mCurrentSimulationStep == static_cast<size_t>(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];
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<VertexIndex, std::unordered_set<DoFType>> &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<VertexIndex, std::unordered_set<DoFType>> &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<VertexIndex, std::unordered_set<DoFType>> &fixedVertices)
{
for (VertexType &v : pMesh->vert) {
updateNodePosition(v, fixedVertices);
updateNodeNormal(v, fixedVertices);
updateNodeNr(v);
}
updateElementalFrames();
if (mSettings.shouldDraw) {
pMesh->updateEigenEdgeAndVertices();
}
}
void DRMSimulationModel::updateElementalFrames()
{
for (const EdgeType &e : pMesh->edge) {
const VectorType elementNormal = (e.cV(0)->cN() + e.cV(1)->cN()).Normalize();
pMesh->elements[e].frame = computeElementFrame(e.cP(0), e.cP(1), elementNormal);
}
}
void DRMSimulationModel::applyForcedDisplacements(
const std::unordered_map<VertexIndex, Eigen::Vector3d> &nodalForcedDisplacements)
{
for (const std::pair<VertexIndex, Eigen::Vector3d> 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<double>(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<VertexIndex, VectorType> nodalForcedRotations)
{
for (const std::pair<VertexIndex, VectorType> vertexIndexDesiredNormalPair :
nodalForcedRotations) {
const VertexIndex vi = vertexIndexDesiredNormalPair.first;
Node &node = pMesh->nodes[vi];
pMesh->vert[vi].N() = vertexIndexDesiredNormalPair.second;
node.displacements = Vector6d(
{node.displacements[0],
node.displacements[1],
node.displacements[2],
vertexIndexDesiredNormalPair.second[0] - node.initialNormal[0],
vertexIndexDesiredNormalPair.second[1] - node.initialNormal[1],
node.displacements[5]});
}
}
// NOTE: Is this correct? Should the kinetic energy be computed like that?
void DRMSimulationModel::updateKineticEnergy()
{
pMesh->previousTotalKineticEnergy = pMesh->currentTotalKineticEnergy;
pMesh->currentTotalKineticEnergy = 0;
pMesh->currentTotalTranslationalKineticEnergy = 0;
for (const VertexType &v : pMesh->vert) {
Node &node = pMesh->nodes[v];
node.kineticEnergy = 0;
const double translationalVelocityNorm = std::sqrt(std::pow(node.velocity[0], 2)
+ std::pow(node.velocity[1], 2)
+ std::pow(node.velocity[2], 2));
const double nodeTranslationalKineticEnergy = 0.5 * node.mass.translational
* pow(translationalVelocityNorm, 2);
const double nodeRotationalKineticEnergy
= 0.5
* (node.mass.rotationalJ * pow(node.velocity[3], 2)
+ node.mass.rotationalI3 * pow(node.velocity[4], 2)
+ node.mass.rotationalI2 * pow(node.velocity[5], 2));
node.kineticEnergy = nodeTranslationalKineticEnergy + nodeRotationalKineticEnergy;
assert(node.kineticEnergy < 1e15);
pMesh->currentTotalKineticEnergy += node.kineticEnergy;
pMesh->currentTotalTranslationalKineticEnergy += nodeTranslationalKineticEnergy;
}
// assert(mesh->currentTotalKineticEnergy < 100000000000000);
}
void DRMSimulationModel::resetVelocities()
{
for (const VertexType &v : pMesh->vert) {
pMesh->nodes[v].velocity =
// pMesh->nodes[v].acceleration * Dt
// * 0.5; // NOTE: Do I reset the previous
// // velocity?
// // reset
// // current to 0 or this?
0;
}
updateKineticEnergy();
}
void DRMSimulationModel::updatePositionsOnTheFly(
const std::unordered_map<VertexIndex, std::unordered_set<DoFType>> &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.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<SimulationJob> &pJob)
{
std::vector<Vector6d> 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();
}
#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<double> perVertexInternalForces_axial = [=]() {
std::vector<double> axialInternalForces;
axialInternalForces.reserve(results.perVertexInternalForces.size());
for (const std::array<Vector6d, 4> &internalForces : results.perVertexInternalForces) {
axialInternalForces.push_back(internalForces[0].norm());
}
return axialInternalForces;
}();
const auto pCurvNet = pMesh->registerForDrawing();
pCurvNet->addNodeScalarQuantity("axial forces", perVertexInternalForces_axial);
std::vector<double> perVertexInternalForces_torsion = [=]() {
std::vector<double> axialInternalForces;
axialInternalForces.reserve(results.perVertexInternalForces.size());
for (const std::array<Vector6d, 4> &internalForces : results.perVertexInternalForces) {
axialInternalForces.push_back(internalForces[1].norm());
}
return axialInternalForces;
}();
pCurvNet->addNodeScalarQuantity("torsional forces", perVertexInternalForces_torsion);
std::vector<double> perVertexInternalForces_firstBending = [=]() {
std::vector<double> axialInternalForces;
axialInternalForces.reserve(results.perVertexInternalForces.size());
for (const std::array<Vector6d, 4> &internalForces : results.perVertexInternalForces) {
axialInternalForces.push_back(internalForces[2].norm());
}
return axialInternalForces;
}();
pCurvNet->addNodeScalarQuantity("first bending forces",
perVertexInternalForces_firstBending);
std::vector<double> perVertexInternalForces_secondBending = [=]() {
std::vector<double> axialInternalForces;
axialInternalForces.reserve(results.perVertexInternalForces.size());
for (const std::array<Vector6d, 4> &internalForces : results.perVertexInternalForces) {
axialInternalForces.push_back(internalForces[3].norm());
}
return axialInternalForces;
}();
pCurvNet->addNodeScalarQuantity("second bending forces",
perVertexInternalForces_secondBending);
polyscope::show();
#endif
results.rotationalDisplacementQuaternion.resize(pMesh->VN());
results.debug_q_f1.resize(pMesh->VN());
results.debug_q_normal.resize(pMesh->VN());
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<Eigen::Vector3d>();
const Eigen::Vector3d nDeformed_eigen
= pMesh->vert[vi].cN().ToEigenVector<Eigen::Vector3d>();
Eigen::Quaternion<double> q_normal;
q_normal.setFromTwoVectors(nInitial_eigen, nDeformed_eigen);
Eigen::Quaternion<double> q_nr_nDeformed;
q_nr_nDeformed = Eigen::AngleAxis<double>(pMesh->nodes[vi].nR, nDeformed_eigen);
Eigen::Quaternion<double> q_nr_nInit;
q_nr_nInit = Eigen::AngleAxis<double>(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<double> q_f1_nInit; //nr is with respect to f1
q_f1_nInit.setFromTwoVectors(referenceF1_initial.ToEigenVector<Eigen::Vector3d>(),
referenceF1_deformed.ToEigenVector<Eigen::Vector3d>());
Eigen::Quaternion<double> 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<Eigen::Vector3d>(),
referenceF1_deformed_def.ToEigenVector<Eigen::Vector3d>());
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<double> q_t1;
// q_t1.setFromTwoVectors(referenceT1_initial.ToEigenVector<Eigen::Vector3d>(),
// referenceT1_deformed.ToEigenVector<Eigen::Vector3d>());
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++;
}
}
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::microseconds>(
std::chrono::steady_clock::now() - begin)
.count()
* 1e-6
/ (static_cast<double>(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::string &screenshotsFolder)
{
// update positions
// polyscope::getCurveNetwork("Undeformed edge mesh")->setEnabled(false);
polyscope::CurveNetwork *meshPolyscopeHandle = polyscope::getCurveNetwork(meshPolyscopeLabel);
meshPolyscopeHandle->updateNodePositions(pMesh->getEigenVertices());
// Vertex quantities
std::vector<double> kineticEnergies(pMesh->VN());
std::vector<std::array<double, 3>> nodeNormals(pMesh->VN());
std::vector<std::array<double, 3>> internalForces(pMesh->VN());
std::vector<std::array<double, 3>> externalForces(pMesh->VN());
std::vector<std::array<double, 3>> externalMoments(pMesh->VN());
std::vector<double> internalForcesNorm(pMesh->VN());
std::vector<std::array<double, 3>> internalAxialForces(pMesh->VN());
std::vector<std::array<double, 3>> internalFirstBendingTranslationForces(
pMesh->VN());
std::vector<std::array<double, 3>> internalFirstBendingRotationForces(
pMesh->VN());
std::vector<std::array<double, 3>> internalSecondBendingTranslationForces(
pMesh->VN());
std::vector<std::array<double, 3>> internalSecondBendingRotationForces(
pMesh->VN());
std::vector<double> nRs(pMesh->VN());
std::vector<double> theta2(pMesh->VN());
std::vector<double> theta3(pMesh->VN());
std::vector<std::array<double, 3>> residualForces(pMesh->VN());
std::vector<double> residualForcesNorm(pMesh->VN());
std::vector<double> accelerationX(pMesh->VN());
for (const VertexType &v : pMesh->vert) {
kineticEnergies[pMesh->getIndex(v)] = pMesh->nodes[v].kineticEnergy;
const VectorType n = v.cN();
nodeNormals[pMesh->getIndex(v)] = {n[0], n[1], n[2]};
// per node internal forces
const Vector6d nodeforce = pMesh->nodes[v].force.internal * (-1);
internalForces[pMesh->getIndex(v)] = {nodeforce[0], nodeforce[1],
nodeforce[2]};
internalForcesNorm[pMesh->getIndex(v)] = nodeforce.norm();
// External force
const Vector6d nodeExternalForce = pMesh->nodes[v].force.external;
externalForces[pMesh->getIndex(v)] = {
nodeExternalForce[0], nodeExternalForce[1], nodeExternalForce[2]};
externalMoments[pMesh->getIndex(v)] = {nodeExternalForce[3],
nodeExternalForce[4], 0};
internalAxialForces[pMesh->getIndex(v)] = {nodeforce[0], nodeforce[1],
nodeforce[2]};
const Node &node = pMesh->nodes[v];
const Vector6d nodeForceFirst = node.force.internalFirstBending * (-1);
internalFirstBendingTranslationForces[pMesh->getIndex(v)] = {
nodeForceFirst[0], nodeForceFirst[1], nodeForceFirst[2]};
internalFirstBendingRotationForces[pMesh->getIndex(v)] = {
nodeForceFirst[3], nodeForceFirst[4], 0};
const Vector6d nodeForceSecond = node.force.internalSecondBending * (-1);
internalSecondBendingTranslationForces[pMesh->getIndex(v)] = {
nodeForceSecond[0], nodeForceSecond[1], nodeForceSecond[2]};
internalSecondBendingRotationForces[pMesh->getIndex(v)] = {
nodeForceSecond[3], nodeForceSecond[4], 0};
nRs[pMesh->getIndex(v)] = node.nR;
const Vector6d nodeResidualForce = pMesh->nodes[v].force.residual;
residualForces[pMesh->getIndex(v)] = {
nodeResidualForce[0], nodeResidualForce[1], nodeResidualForce[2]};
residualForcesNorm[pMesh->getIndex(v)] = nodeResidualForce.norm();
accelerationX[pMesh->getIndex(v)] = pMesh->nodes[v].acceleration[0];
}
meshPolyscopeHandle->addNodeScalarQuantity("Kinetic Energy", kineticEnergies)->setEnabled(false);
meshPolyscopeHandle->addNodeVectorQuantity("Node normals", nodeNormals)->setEnabled(true);
meshPolyscopeHandle->addNodeVectorQuantity("Internal force", internalForces)->setEnabled(false);
meshPolyscopeHandle->addNodeVectorQuantity("External force", externalForces)->setEnabled(false);
meshPolyscopeHandle->addNodeVectorQuantity("External Moments", externalMoments)->setEnabled(true);
meshPolyscopeHandle->addNodeScalarQuantity("Internal force norm", internalForcesNorm)
->setEnabled(true);
meshPolyscopeHandle->setRadius(0.002);
// polyscope::getCurveNetwork(meshPolyscopeLabel)
// ->addNodeVectorQuantity("Internal Axial force", internalAxialForces)
// ->setEnabled(false);
// polyscope::getCurveNetwork(meshPolyscopeLabel)
// ->addNodeVectorQuantity("First bending force-Translation",
// internalFirstBendingTranslationForces)
// ->setEnabled(false);
// polyscope::getCurveNetwork(meshPolyscopeLabel)
// ->addNodeVectorQuantity("First bending force-Rotation",
// internalFirstBendingRotationForces)
// ->setEnabled(false);
// polyscope::getCurveNetwork(meshPolyscopeLabel)
// ->addNodeVectorQuantity("Second bending force-Translation",
// internalSecondBendingTranslationForces)
// ->setEnabled(false);
// polyscope::getCurveNetwork(meshPolyscopeLabel)
// ->addNodeVectorQuantity("Second bending force-Rotation",
// internalSecondBendingRotationForces)
// ->setEnabled(false);
polyscope::getCurveNetwork(meshPolyscopeLabel)
->addNodeScalarQuantity("nR", nRs)
->setEnabled(false);
// polyscope::getCurveNetwork(meshPolyscopeLabel)
// ->addNodeScalarQuantity("theta3", theta3)
// ->setEnabled(false);
// polyscope::getCurveNetwork(meshPolyscopeLabel)
// ->addNodeScalarQuantity("theta2", theta2)
// ->setEnabled(false);
polyscope::getCurveNetwork(meshPolyscopeLabel)
->addNodeVectorQuantity("Residual force", residualForces)
->setEnabled(false);
polyscope::getCurveNetwork(meshPolyscopeLabel)
->addNodeScalarQuantity("Residual force norm", residualForcesNorm)
->setEnabled(false);
// polyscope::getCurveNetwork(meshPolyscopeLabel)
// ->addNodeScalarQuantity("Node acceleration x", accelerationX);
// Edge quantities
std::vector<double> A(pMesh->EN());
std::vector<double> J(pMesh->EN());
std::vector<double> I2(pMesh->EN());
std::vector<double> 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(meshPolyscopeLabel)->addEdgeScalarQuantity("A", A);
polyscope::getCurveNetwork(meshPolyscopeLabel)->addEdgeScalarQuantity("J", J);
polyscope::getCurveNetwork(meshPolyscopeLabel)
->addEdgeScalarQuantity("I2", I2);
polyscope::getCurveNetwork(meshPolyscopeLabel)->addEdgeScalarQuantity("I3", I3);
// Specify the callback
static bool calledOnce = false;
if (!calledOnce) {
PolyscopeInterface::addUserCallback([&]() {
// Since options::openImGuiWindowForUserCallback == true by default,
// we can immediately start using ImGui commands to build a UI
ImGui::PushItemWidth(100); // Make ui elements 100 pixels wide,
// instead of full width. Must have
// matching PopItemWidth() below.
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<SimulationJob> &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::Vector3d>();
Eigen::Quaternion<double> 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<Eigen::Vector3d>();
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<Eigen::Vector3d>();
// const VectorType &n_initial = node.initialNormal;
const Eigen::Vector3d referenceF1_initial
= (referenceT1_initial - (nInitial_eigen * (referenceT1_initial.dot(nInitial_eigen))))
.normalized();
Eigen::Quaternion<double> q_f1; //nr is with respect to f1
q_f1.setFromTwoVectors(referenceF1_initial, referenceF1_deformed);
Eigen::Quaternion<double> q_normal;
q_normal.setFromTwoVectors(nInitial_eigen, nDeformed_eigen);
Eigen::Quaternion<double> 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<double> 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<SimulationJob> &pJob)
{
// if (!mSettings.viscousDampingFactor.has_value()) {
// const bool shouldCapDisplacements = mSettings.displacementCap.has_value();
for (VertexType &v : pMesh->vert) {
Node &node = pMesh->nodes[v];
Vector6d stepDisplacement = node.velocity * 0.5 * Dt;
// if (shouldCapDisplacements
// && stepDisplacement.getTranslation().norm() > mSettings.displacementCap) {
// stepDisplacement = stepDisplacement
// * (*mSettings.displacementCap
// / stepDisplacement.getTranslation().norm());
// }
node.displacements = node.displacements - stepDisplacement;
}
applyDisplacements(constrainedVertices);
if (!pJob->nodalForcedDisplacements.empty()) {
applyForcedDisplacements(
pJob->nodalForcedDisplacements);
}
updateElementalLengths();
// }
// const 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;
}
SimulationResults DRMSimulationModel::executeSimulation(const std::shared_ptr<SimulationJob> &pJob)
{
auto beginTime = std::chrono::high_resolution_clock::now();
updateNodalMasses();
// std::unordered_map<VertexIndex, Vector6d> nodalExternalForces = pJob->nodalExternalForces;
// double totalExternalForcesNorm = 0;
// Vector6d sumOfExternalForces(0);
// for (auto &nodalForce : nodalExternalForces) {
// const double percentageOfExternalLoads = double(externalLoadStep)
// / mSettings.desiredGradualExternalLoadsSteps;
// nodalForce.second = nodalForce.second * percentageOfExternalLoads;
// totalExternalForcesNorm += nodalForce.second.norm();
// // sumOfExternalForces = sumOfExternalForces + nodalForce.second;
// }
updateNodalExternalForces(pJob->nodalExternalForces, constrainedVertices);
if (!pJob->nodalExternalForces.empty()) {
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<double> residualForcesMovingAverageHistorySample;
std::vector<double> percentageOfConvergence;
// double residualForcesMovingAverageDerivativeMax = 0;
while (!mSettings.maxDRMIterations.has_value()
|| mCurrentSimulationStep < mSettings.maxDRMIterations.value()) {
if ((mSettings.debugModeStep.has_value() && mCurrentSimulationStep == 50000)) {
// std::filesystem::create_directory("./PatternOptimizationNonConv");
// pJob->save("./PatternOptimizationNonConv");
// Dt = mSettings.Dtini;
}
// if (mCurrentSimulationStep == 500 && shouldTemporarilyDampForces) {
// Dt = mSettings.Dtini;
// }
// while (true) {
updateNormalDerivatives();
updateT1Derivatives();
updateRDerivatives();
updateT2Derivatives();
updateT3Derivatives();
const bool shouldBreak = mCurrentSimulationStep == 12970;
updateResidualForcesOnTheFly(constrainedVertices);
// TODO: write parallel function for updating positions
// TODO: Make a single function out of updateResidualForcesOnTheFly
// updatePositionsOnTheFly
// updatePositionsOnTheFly(constrainedVertices);
updateNodalMasses();
updateNodalAccelerations();
updateNodalVelocities();
updateNodalDisplacements();
applyDisplacements(constrainedVertices);
if (!pJob->nodalForcedDisplacements.empty()) {
applyForcedDisplacements(
pJob->nodalForcedDisplacements);
}
// if (!pJob->nodalForcedNormals.empty()) {
// applyForcedNormals(pJob->nodalForcedNormals);
// }
updateElementalLengths();
mCurrentSimulationStep++;
if (std::isnan(pMesh->currentTotalKineticEnergy)) {
std::cout << pMesh->currentTotalKineticEnergy << std::endl;
if (mSettings.beVerbose) {
std::cerr << "Infinite kinetic energy at step " << mCurrentSimulationStep
<< ". Exiting.." << std::endl;
}
break;
}
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());
}
}
//normalized sum of displacements
// double sumOfDisplacementsNorm = 0;
// for (size_t vi = 0; vi < pMesh->VN(); vi++) {
// sumOfDisplacementsNorm += pMesh->nodes[vi].displacements.getTranslation().norm();
// }
// sumOfDisplacementsNorm /= pMesh->bbox.Diag();
// pMesh->sumOfNormalizedDisplacementNorms = sumOfDisplacementsNorm;
//moving average
// // pMesh->residualForcesMovingAverage = (pMesh->totalResidualForcesNorm
// // + mCurrentSimulationStep
// // * pMesh->residualForcesMovingAverage)
// // / (1 + mCurrentSimulationStep);
pMesh->residualForcesMovingAverage += pMesh->totalResidualForcesNorm
/ movingAverageSampleSize;
residualForcesMovingAverageHistorySample.push(pMesh->residualForcesMovingAverage);
if (movingAverageSampleSize < residualForcesMovingAverageHistorySample.size()) {
const double firstElementValue = residualForcesMovingAverageHistorySample.front();
pMesh->residualForcesMovingAverage -= firstElementValue / movingAverageSampleSize;
residualForcesMovingAverageHistorySample.pop();
// pMesh->residualForcesMovingAverageDerivativeNorm
// = std::abs((residualForcesMovingAverageHistorySample.back()
// - residualForcesMovingAverageHistorySample.front()))
// / (movingAverageSampleSize);
// residualForcesMovingAverageDerivativeMax
// = std::max(pMesh->residualForcesMovingAverageDerivativeNorm,
// residualForcesMovingAverageDerivativeMax);
// pMesh->residualForcesMovingAverageDerivativeNorm
// /= residualForcesMovingAverageDerivativeMax;
// // std::cout << "Normalized derivative:"
// // << residualForcesMovingAverageDerivativeNorm
// // << std::endl;
}
// pMesh->previousTotalPotentialEnergykN =
// pMesh->currentTotalPotentialEnergykN;
// pMesh->currentTotalPotentialEnergykN = computeTotalPotentialEnergy() / 1000;
// timePerNodePerIterationHistor.push_back(timePerNodePerIteration);
if (mSettings.beVerbose && mSettings.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<std::chrono::minutes>(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) {
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",
// movingAveragesDerivatives_norm);
// SimulationResultsReporter::createPlot("Number of Steps",
// "Residual Forces mov aver",
// history.residualForcesMovingAverage,
// {},
// 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",
// "Sum of normalized displacement norms",
// history.sumOfNormalizedDisplacementNorms /*,
// std::filesystem::path("./")
// .append("SumOfNormalizedDisplacementNorms_" + graphSuffix + ".png")
// .string()*/
// ,
// {},
// history.redMarks);
// SimulationResultsReporter::createPlot("Number of Steps",
// "Percentage of convergence",
// percentageOfConvergence,
// {},
// history.redMarks);
}
#ifdef POLYSCOPE_DEFINED
if (mSettings.shouldDraw && mSettings.debugModeStep.has_value()
&& mCurrentSimulationStep % mSettings.debugModeStep.value() == 0) /* &&
currentSimulationStep > maxDRMIterations*/
{
// std::string saveTo = std::filesystem::current_path()
// .append("Debugging_files")
// .append("Screenshots")
// .string();
draw();
// yValues = history.kineticEnergy;
// plotHandle = matplot::scatter(xPlot, yValues);
// label = "Log of Kinetic energy";
// plotHandle->legend_string(label);
// shouldUseKineticEnergyThreshold = true;
}
#endif
// t = t + Dt;
// std::cout << "Kinetic energy:" << mesh.currentTotalKineticEnergy
// << std::endl;
// std::cout << "Residual forces norm:" << mesh.totalResidualForcesNorm
// << std::endl;
const bool fullfillsResidualForcesNormTerminationCriterion
= !mSettings.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();
if ((fullfillsAverageResidualForcesNormTerminationCriterion
|| fullfillsResidualForcesNormTerminationCriterion)
&& 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;
}
// Residual forces norm convergence
if ((pMesh->previousTotalKineticEnergy > pMesh->currentTotalKineticEnergy
// && mCurrentSimulationStep > movingAverageSampleSize
)
/* || (pMesh->totalResidualForcesNorm / mSettings.totalResidualForcesNormThreshold <= 1
&& mCurrentSimulationStep > 1)*/
/*||
mesh->previousTotalPotentialEnergykN >
mesh->currentTotalPotentialEnergykN*/
/*|| mesh->currentTotalPotentialEnergykN < minPotentialEnergy*/) {
// if (pMesh->totalResidualForcesNorm < totalResidualForcesNormThreshold) {
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<VertexIndex, Vector6d> 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.useKineticDamping) {
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();
}
}
auto endTime = std::chrono::high_resolution_clock::now();
SimulationResults results = computeResults(pJob);
results.executionTime = std::chrono::duration_cast<std::chrono::seconds>(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);
polyscope::removeCurveNetwork("Initial_" + meshPolyscopeLabel);
}
#endif
return results;
}
SimulationResults DRMSimulationModel::executeSimulation(const std::shared_ptr<SimulationJob> &pJob,
const Settings &settings,
const SimulationResults &solutionGuess)
{
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);
// 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
return results;
}
//#ifdef USE_ENSMALLEN
//void DRMSimulationModel::setJob(const std::shared_ptr<SimulationJob> &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)
//{
// 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::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();
// std::cout << "PE:" << PE << 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(useKineticDamping)] = totalResidualForcesNormThreshold;
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(useKineticDamping)] = useKineticDamping;
// 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;
}
Reference in New Issue View Git Blame Copy Permalink