2581 lines
127 KiB
C++
Executable File
2581 lines
127 KiB
C++
Executable File
#include "drmsimulationmodel.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()
|
|
{
|
|
mCurrentSimulationStep = 0;
|
|
history.clear();
|
|
constrainedVertices.clear();
|
|
pMesh.reset();
|
|
plotYValues.clear();
|
|
plotHandle.reset();
|
|
checkedForMaximumMoment = false;
|
|
externalMomentsNorm = 0;
|
|
Dt = mSettings.Dtini;
|
|
numOfDampings = 0;
|
|
shouldTemporarilyDampForces = false;
|
|
externalLoadStep = 1;
|
|
isVertexConstrained.clear();
|
|
minTotalResidualForcesNorm = std::numeric_limits<double>::max();
|
|
}
|
|
|
|
VectorType DRMSimulationModel::computeDisplacementDifferenceDerivative(
|
|
const EdgeType &e, const DifferentiateWithRespectTo &dui) const
|
|
{
|
|
VectorType displacementDiffDeriv(0, 0, 0);
|
|
const DoFType &dofi = dui.dofi;
|
|
const bool differentiateWithRespectToANonEdgeNode = e.cV(0) != &dui.v && e.cV(1) != &dui.v;
|
|
if (differentiateWithRespectToANonEdgeNode || dofi > 2) {
|
|
return displacementDiffDeriv;
|
|
}
|
|
|
|
if (e.cV(0) == &dui.v) {
|
|
displacementDiffDeriv[dofi] = -1;
|
|
} else if (e.cV(1) == &dui.v) {
|
|
displacementDiffDeriv[dofi] = 1;
|
|
}
|
|
|
|
return displacementDiffDeriv;
|
|
}
|
|
|
|
VectorType DRMSimulationModel::computeDerivativeOfNormal(const VertexType &v,
|
|
const DifferentiateWithRespectTo &dui) const
|
|
{
|
|
const size_t vi = pMesh->getIndex(v);
|
|
VectorType normalDerivative(0, 0, 0);
|
|
if (&dui.v != &v || (dui.dofi == 0 || dui.dofi == 1 || dui.dofi == 2 || dui.dofi == 5)) {
|
|
return normalDerivative;
|
|
}
|
|
const VectorType &n = v.cN();
|
|
const double &nx = n[0], ny = n[1];
|
|
const double nxnyMagnitude = std::pow(nx, 2) + std::pow(ny, 2);
|
|
|
|
if (dui.dofi == 3) {
|
|
if (nxnyMagnitude + 1e-5 >= 1) {
|
|
const double normalDerivativeX = 1 / sqrt(nxnyMagnitude)
|
|
- std::pow(nx, 2) / std::pow(nxnyMagnitude, 1.5);
|
|
const double normalDerivativeY = -nx * ny / std::pow(nxnyMagnitude, 1.5);
|
|
const double normalDerivativeZ = 0;
|
|
normalDerivative = VectorType(normalDerivativeX, normalDerivativeY, normalDerivativeZ);
|
|
} else {
|
|
const double normalDerivativeX = 1;
|
|
const double normalDerivativeY = 0;
|
|
const double normalDerivativeZ = -nx / std::sqrt(1 - nxnyMagnitude);
|
|
normalDerivative = VectorType(normalDerivativeX, normalDerivativeY, normalDerivativeZ);
|
|
}
|
|
} else if (dui.dofi == 4) {
|
|
if (nxnyMagnitude + 1e-5 >= 1) {
|
|
const double normalDerivativeX = -nx * ny / std::pow(nxnyMagnitude, 1.5);
|
|
const double normalDerivativeY = 1 / sqrt(nxnyMagnitude)
|
|
- std::pow(ny, 2) / std::pow(nxnyMagnitude, 1.5);
|
|
const double normalDerivativeZ = 0;
|
|
normalDerivative = VectorType(normalDerivativeX, normalDerivativeY, normalDerivativeZ);
|
|
} else {
|
|
const double normalDerivativeX = 0;
|
|
const double normalDerivativeY = 1;
|
|
const double normalDerivativeZ = -ny / std::sqrt(1 - nxnyMagnitude);
|
|
normalDerivative = VectorType(normalDerivativeX, normalDerivativeY, normalDerivativeZ);
|
|
}
|
|
}
|
|
|
|
const bool normalDerivativeIsFinite = std::isfinite(normalDerivative[0])
|
|
&& std::isfinite(normalDerivative[1])
|
|
&& std::isfinite(normalDerivative[2]);
|
|
assert(normalDerivativeIsFinite);
|
|
const bool shouldBreak = mCurrentSimulationStep == 118 && vi == 1 && dui.dofi == 3;
|
|
|
|
return normalDerivative;
|
|
}
|
|
|
|
double DRMSimulationModel::computeDerivativeElementLength(
|
|
const EdgeType &e, const DifferentiateWithRespectTo &dui) const
|
|
{
|
|
if (e.cV(0) != &dui.v && e.cV(1) != &dui.v) {
|
|
return 0;
|
|
}
|
|
|
|
const VectorType &X_j = e.cP(0);
|
|
const VectorType &X_jplus1 = e.cP(1);
|
|
const VectorType positionVectorDiff = X_jplus1 - X_j;
|
|
const VectorType displacementDiffDeriv = computeDisplacementDifferenceDerivative(e, dui);
|
|
const double edgeLength = pMesh->elements[e].length;
|
|
const double L_kDeriv = positionVectorDiff * displacementDiffDeriv / edgeLength;
|
|
return L_kDeriv;
|
|
}
|
|
|
|
double DRMSimulationModel::computeDerivativeOfNorm(const VectorType &x,
|
|
const VectorType &derivativeOfX)
|
|
{
|
|
return x.dot(derivativeOfX) / x.Norm();
|
|
}
|
|
|
|
VectorType DRMSimulationModel::computeDerivativeOfCrossProduct(const VectorType &a,
|
|
const VectorType &derivativeOfA,
|
|
const VectorType &b,
|
|
const VectorType &derivativeOfB)
|
|
{
|
|
const auto firstTerm = Cross(derivativeOfA, b);
|
|
const auto secondTerm = Cross(a, derivativeOfB);
|
|
return firstTerm + secondTerm;
|
|
}
|
|
|
|
VectorType DRMSimulationModel::computeDerivativeOfR(const EdgeType &e,
|
|
const DifferentiateWithRespectTo &dui) const
|
|
{
|
|
const VertexType &v_j = *e.cV(0);
|
|
const VertexType &v_jplus1 = *e.cV(1);
|
|
const VectorType normal_j = v_j.cN();
|
|
const VectorType normal_jplus1 = v_jplus1.cN();
|
|
const VectorType derivativeOfNormal_j = &v_j == &dui.v && dui.dofi > 2
|
|
? pMesh->nodes[v_j].derivativeOfNormal[dui.dofi]
|
|
: VectorType(0, 0, 0);
|
|
const VectorType derivativeOfNormal_jplus1
|
|
= &v_jplus1 == &dui.v && dui.dofi > 2 ? pMesh->nodes[v_jplus1].derivativeOfNormal[dui.dofi]
|
|
: VectorType(0, 0, 0);
|
|
|
|
const VectorType derivativeOfSumOfNormals = derivativeOfNormal_j + derivativeOfNormal_jplus1;
|
|
const VectorType sumOfNormals = normal_j + normal_jplus1;
|
|
const double normOfSumOfNormals = sumOfNormals.Norm();
|
|
assert(normOfSumOfNormals != 0);
|
|
const double derivativeOfNormOfSumOfNormals = computeDerivativeOfNorm(sumOfNormals,
|
|
derivativeOfSumOfNormals);
|
|
|
|
const VectorType derivativeOfR_firstTerm = -sumOfNormals * derivativeOfNormOfSumOfNormals
|
|
/ std::pow(normOfSumOfNormals, 2);
|
|
const VectorType derivativeOfR_secondTerm = derivativeOfSumOfNormals / normOfSumOfNormals;
|
|
const VectorType derivativeOfR = derivativeOfR_firstTerm + derivativeOfR_secondTerm;
|
|
|
|
assert(std::isfinite(derivativeOfR[0]) && std::isfinite(derivativeOfR[1])
|
|
&& std::isfinite(derivativeOfR[2]));
|
|
|
|
return derivativeOfR;
|
|
}
|
|
|
|
VectorType DRMSimulationModel::computeDerivativeT1(const EdgeType &e,
|
|
const DifferentiateWithRespectTo &dui) const
|
|
{
|
|
const VectorType &X_j = e.cP(0);
|
|
const VectorType &X_jplus1 = e.cP(1);
|
|
const VectorType edgeVector = X_jplus1 - X_j;
|
|
const double L_kDerivative = computeDerivativeElementLength(e, dui);
|
|
const double edgeLength = pMesh->elements[e].length;
|
|
const VectorType firstTerm = -edgeVector * L_kDerivative / std::pow(edgeLength, 2);
|
|
const VectorType secondTerm = computeDisplacementDifferenceDerivative(e, dui) / edgeLength;
|
|
const VectorType t1Derivative = firstTerm + secondTerm;
|
|
|
|
return t1Derivative;
|
|
}
|
|
|
|
VectorType DRMSimulationModel::computeDerivativeT2(const EdgeType &e,
|
|
const DifferentiateWithRespectTo &dui) const
|
|
{
|
|
const DoFType dofi = dui.dofi;
|
|
|
|
const VertexType &v_j = *e.cV(0);
|
|
const size_t vi_j = pMesh->getIndex(v_j);
|
|
const VertexType &v_jplus1 = *e.cV(1);
|
|
const size_t vi_jplus1 = pMesh->getIndex(v_jplus1);
|
|
|
|
const VectorType r = (v_j.cN() + v_jplus1.cN()).Normalize();
|
|
const VectorType derivativeR_j = dofi > 2 && &v_j == &dui.v
|
|
? pMesh->elements[e].derivativeR_j[dui.dofi]
|
|
: VectorType(0, 0, 0);
|
|
const VectorType derivativeR_jplus1 = dofi > 2 && &v_jplus1 == &dui.v
|
|
? pMesh->elements[e].derivativeR_jplus1[dui.dofi]
|
|
: VectorType(0, 0, 0);
|
|
const VectorType derivativeR = derivativeR_j + derivativeR_jplus1;
|
|
|
|
const VectorType &t1 = pMesh->elements[e].frame.t1;
|
|
const VectorType derivativeT1_j = dofi < 3 && &v_j == &dui.v
|
|
? pMesh->elements[e].derivativeT1_j[dui.dofi]
|
|
: VectorType(0, 0, 0);
|
|
const VectorType derivativeT1_jplus1 = dofi < 3 && &v_jplus1 == &dui.v
|
|
? pMesh->elements[e].derivativeT1_jplus1[dui.dofi]
|
|
: VectorType(0, 0, 0);
|
|
const VectorType derivativeT1 = derivativeT1_j + derivativeT1_jplus1;
|
|
|
|
const VectorType derivativeOfRCrossT1 = computeDerivativeOfCrossProduct(r,
|
|
derivativeR,
|
|
t1,
|
|
derivativeT1);
|
|
const VectorType rCrossT1 = Cross(r, t1);
|
|
const double normOfRCrossT1 = rCrossT1.Norm();
|
|
const double derivativeNormRCrossT1 = computeDerivativeOfNorm(rCrossT1, derivativeOfRCrossT1);
|
|
|
|
const VectorType t2Deriv_firstTerm = -(rCrossT1 * derivativeNormRCrossT1)
|
|
/ std::pow(normOfRCrossT1, 2);
|
|
const VectorType t2Deriv_secondTerm = derivativeOfRCrossT1 / normOfRCrossT1;
|
|
const VectorType t2Deriv = t2Deriv_firstTerm + t2Deriv_secondTerm;
|
|
|
|
const double t2DerivNorm = t2Deriv.Norm();
|
|
assert(std::isfinite(t2DerivNorm));
|
|
const bool shouldBreak = mCurrentSimulationStep == 118 && (vi_jplus1 == 1 || vi_j == 1)
|
|
&& dofi == 3;
|
|
return t2Deriv;
|
|
}
|
|
|
|
VectorType DRMSimulationModel::computeDerivativeT3(const EdgeType &e,
|
|
const DifferentiateWithRespectTo &dui) const
|
|
{
|
|
const Element &element = pMesh->elements[e];
|
|
const VectorType &t1 = element.frame.t1;
|
|
const VectorType &t2 = element.frame.t2;
|
|
const VectorType t1CrossT2 = Cross(t1, t2);
|
|
const VertexType &v_j = *e.cV(0);
|
|
const size_t vi_j = pMesh->getIndex(v_j);
|
|
const VertexType &v_jplus1 = *e.cV(1);
|
|
const size_t vi_jplus1 = pMesh->getIndex(v_jplus1);
|
|
const VectorType derivativeT1_j = dui.dofi < 3 && &v_j == &dui.v
|
|
? pMesh->elements[e].derivativeT1_j[dui.dofi]
|
|
: VectorType(0, 0, 0);
|
|
const VectorType derivativeT1_jplus1 = dui.dofi < 3 && &v_jplus1 == &dui.v
|
|
? pMesh->elements[e].derivativeT1_jplus1[dui.dofi]
|
|
: VectorType(0, 0, 0);
|
|
const VectorType derivativeT1 = derivativeT1_j + derivativeT1_jplus1;
|
|
|
|
// const VectorType derivativeOfT2 = computeDerivativeT2(e, dui);
|
|
// const VectorType derivativeT2_j =
|
|
// &v_j == &dui.v
|
|
// ? mesh->elements[e].derivativeT2_j[dui.dofi]
|
|
// : VectorType(0, 0, 0);
|
|
// const VectorType derivativeT2_jplus1 =
|
|
// &v_jplus1 == &dui.v
|
|
// ? mesh->elements[e].derivativeT2_jplus1[dui.dofi]
|
|
// : VectorType(0, 0, 0);
|
|
VectorType derivativeT2(0, 0, 0);
|
|
if (&v_j == &dui.v) {
|
|
derivativeT2 = pMesh->elements[e].derivativeT2_j[dui.dofi];
|
|
} else if (&v_jplus1 == &dui.v) {
|
|
derivativeT2 = pMesh->elements[e].derivativeT2_jplus1[dui.dofi];
|
|
}
|
|
|
|
const VectorType derivativeT1CrossT2 = computeDerivativeOfCrossProduct(t1,
|
|
derivativeT1,
|
|
t2,
|
|
derivativeT2);
|
|
const double derivativeOfNormT1CrossT2 = computeDerivativeOfNorm(t1CrossT2, derivativeT1CrossT2);
|
|
const double normT1CrossT2 = t1CrossT2.Norm();
|
|
|
|
const VectorType t3Deriv_firstTerm = -(t1CrossT2 * derivativeOfNormT1CrossT2)
|
|
/ std::pow(normT1CrossT2, 2);
|
|
const VectorType t3Deriv_secondTerm = derivativeT1CrossT2 / normT1CrossT2;
|
|
const VectorType t3Deriv = t3Deriv_firstTerm + t3Deriv_secondTerm;
|
|
|
|
assert(std::isfinite(t3Deriv[0]) && std::isfinite(t3Deriv[1]) && std::isfinite(t3Deriv[2]));
|
|
return t3Deriv;
|
|
}
|
|
|
|
double DRMSimulationModel::computeDerivativeTheta1(const EdgeType &e,
|
|
const VertexIndex &evi,
|
|
const VertexIndex &dwrt_evi,
|
|
const DoFType &dwrt_dofi) const
|
|
{
|
|
const VertexType &v = *e.cV(evi);
|
|
const size_t vi = pMesh->getIndex(v);
|
|
const Element &element = pMesh->elements[e];
|
|
const VectorType derivativeT1 = element.derivativeT1[dwrt_evi][dwrt_dofi];
|
|
const VectorType derivativeT3 = element.derivativeT3[dwrt_evi][dwrt_dofi];
|
|
const VectorType nDerivative = evi != dwrt_evi ? VectorType(0, 0, 0)
|
|
: pMesh->nodes[v].derivativeOfNormal[dwrt_dofi];
|
|
const VectorType n = v.cN();
|
|
const VectorType &t1 = element.frame.t1;
|
|
const VectorType &t3 = element.frame.t3;
|
|
const double theta1Derivative = derivativeT1 * Cross(t3, n)
|
|
+ t1 * (Cross(derivativeT3, n) + Cross(t3, nDerivative));
|
|
const bool shouldBreak = mCurrentSimulationStep == 118 && vi == 1 && dwrt_dofi == 3;
|
|
|
|
return theta1Derivative;
|
|
}
|
|
|
|
double DRMSimulationModel::computeDerivativeTheta2(const EdgeType &e,
|
|
const VertexIndex &evi,
|
|
const VertexIndex &dwrt_evi,
|
|
const DoFType &dwrt_dofi) const
|
|
{
|
|
const VertexType &v = *e.cV(evi);
|
|
|
|
const Element &element = pMesh->elements[e];
|
|
const VectorType derivativeT2 = element.derivativeT2[dwrt_evi][dwrt_dofi];
|
|
const VectorType derivativeT3 = element.derivativeT3[dwrt_evi][dwrt_dofi];
|
|
|
|
const VectorType n = v.cN();
|
|
const VectorType &t2 = element.frame.t2;
|
|
const VectorType &t3 = element.frame.t3;
|
|
const VectorType nDerivative = dwrt_evi == evi ? pMesh->nodes[v].derivativeOfNormal[dwrt_dofi]
|
|
: VectorType(0, 0, 0);
|
|
const double theta2Derivative = derivativeT2 * Cross(t3, n)
|
|
+ t2 * (Cross(derivativeT3, n) + Cross(t3, nDerivative));
|
|
|
|
return theta2Derivative;
|
|
}
|
|
|
|
double DRMSimulationModel::computeTheta3(const EdgeType &e, const VertexType &v)
|
|
{
|
|
const VertexIndex &vi = pMesh->nodes[v].vi;
|
|
// assert(e.cV(0) == &v || e.cV(1) == &v);
|
|
|
|
const Element &elem = pMesh->elements[e];
|
|
const EdgeIndex &ei = elem.ei;
|
|
const Element::LocalFrame &ef = elem.frame;
|
|
const VectorType &t1 = ef.t1;
|
|
const VectorType &n = v.cN();
|
|
const Node &node = pMesh->nodes[v];
|
|
const double &nR = node.nR; // TODO: This makes the function non-const.
|
|
// Should be refactored in the future
|
|
|
|
double theta3;
|
|
const bool shouldBreak = mCurrentSimulationStep == 12970;
|
|
if (&e == node.referenceElement) {
|
|
// Use nR as theta3 only for the first star edge
|
|
return nR;
|
|
}
|
|
// std::vector<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 VectorType derivativeF1Normalized = -f1 * (f1 * derivativeF1)
|
|
/ (f1Norm * 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 VectorType derivativeF1Normalized = -f1 * (f1 * derivativeF1)
|
|
/ (f1Norm * 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.A
|
|
/ (2 * element.initialLength);
|
|
const double theta1Diff = theta1_jplus1 - theta1_j;
|
|
const double torsionalPE = element.G * element.J * pow(theta1Diff, 2)
|
|
/ (2 * element.initialLength);
|
|
const double firstBendingPE_inBracketsTerm = 4 * pow(theta2_j, 2)
|
|
+ 4 * theta2_j * theta2_jplus1
|
|
+ 4 * pow(theta2_jplus1, 2);
|
|
const double firstBendingPE = firstBendingPE_inBracketsTerm * element.material.youngsModulus
|
|
* element.I2 / (2 * element.initialLength);
|
|
const double secondBendingPE_inBracketsTerm = 4 * pow(theta3_j, 2)
|
|
+ 4 * theta3_j * theta3_jplus1
|
|
+ 4 * pow(theta3_jplus1, 2);
|
|
const double secondBendingPE = secondBendingPE_inBracketsTerm
|
|
* element.material.youngsModulus * element.I3
|
|
/ (2 * element.initialLength);
|
|
|
|
totalInternalPotentialEnergy += axialPE + torsionalPE + firstBendingPE + secondBendingPE;
|
|
int i = 0;
|
|
i++;
|
|
}
|
|
|
|
return totalInternalPotentialEnergy;
|
|
}
|
|
|
|
double DRMSimulationModel::computeTotalPotentialEnergy()
|
|
{
|
|
double totalExternalPotentialEnergy = 0;
|
|
for (const SimulationMesh::VertexType &v : pMesh->vert) {
|
|
const Node &node = pMesh->nodes[v];
|
|
if (!node.force.hasExternalForce()) {
|
|
continue;
|
|
}
|
|
const auto nodeDisplacement = v.cP() - node.initialLocation;
|
|
const SimulationMesh::CoordType externalForce(node.force.external[0],
|
|
node.force.external[1],
|
|
node.force.external[2]);
|
|
totalExternalPotentialEnergy += externalForce.dot(nodeDisplacement);
|
|
}
|
|
|
|
const double totalInternalPotentialEnergy = computeTotalInternalPotentialEnergy();
|
|
|
|
return totalInternalPotentialEnergy - totalExternalPotentialEnergy;
|
|
}
|
|
|
|
void DRMSimulationModel::updateResidualForcesOnTheFly(
|
|
const std::unordered_map<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
|
|
for (int ei = 0; ei < pMesh->EN(); ei++) {
|
|
const EdgeType &e = pMesh->edge[ei];
|
|
const SimulationMesh::VertexType &ev_j = *e.cV(0);
|
|
const SimulationMesh::VertexType &ev_jplus1 = *e.cV(1);
|
|
const Element &element = pMesh->elements[e];
|
|
const Element::LocalFrame &ef = element.frame;
|
|
const VectorType t3CrossN_j = Cross(ef.t3, ev_j.cN());
|
|
const VectorType t3CrossN_jplus1 = Cross(ef.t3, ev_jplus1.cN());
|
|
const double theta1_j = ef.t1.dot(t3CrossN_j);
|
|
const double theta1_jplus1 = ef.t1.dot(t3CrossN_jplus1);
|
|
const double theta2_j = ef.t2.dot(t3CrossN_j);
|
|
const double theta2_jplus1 = ef.t2.dot(t3CrossN_jplus1);
|
|
const bool shouldBreak = mCurrentSimulationStep == 12970;
|
|
const double theta3_j = computeTheta3(e, ev_j);
|
|
const double theta3_jplus1 = computeTheta3(e, ev_jplus1);
|
|
// element.rotationalDisplacements_j.theta1 = theta1_j;
|
|
// element.rotationalDisplacements_jplus1.theta1 = theta1_jplus1;
|
|
// element.rotationalDisplacements_j.theta2 = theta2_j;
|
|
// element.rotationalDisplacements_jplus1.theta2 = theta2_jplus1;
|
|
// element.rotationalDisplacements_j.theta3 = theta3_j;
|
|
// element.rotationalDisplacements_jplus1.theta3 = theta3_jplus1;
|
|
std::vector<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 = 2 * theta3_jplus1_deriv
|
|
* theta3_jplus1;
|
|
// 3rd term computation
|
|
const double secondBendingForce_inBracketsTerm
|
|
= secondBendingForce_inBracketsTerm_0 + secondBendingForce_inBracketsTerm_1
|
|
+ secondBendingForce_inBracketsTerm_2
|
|
+ secondBendingForce_inBracketsTerm_3;
|
|
double secondBendingForce_dofi = secondBendingForce_inBracketsTerm
|
|
* element.rigidity.secondBending;
|
|
internalForcesContributionFromThisEdge[evi].second[dofi]
|
|
= axialForce_dofi + firstBendingForce_dofi + secondBendingForce_dofi
|
|
+ torsionalForce_dofi;
|
|
}
|
|
if (edgeNode.referenceElement != &e) {
|
|
const bool isDofConstrainedFor_refElemOtherVertex
|
|
= isVertexConstrained[refElemOtherVertexNode.vi]
|
|
&& fixedVertices.at(refElemOtherVertexNode.vi).contains(dofi);
|
|
if (!isDofConstrainedFor_refElemOtherVertex) {
|
|
DifferentiateWithRespectTo dui{*refElemOtherVertex, dofi};
|
|
////theta3_j derivative
|
|
const double theta3_j_deriv = computeDerivativeTheta3(e, ev_j, dui);
|
|
////theta3_jplus1 derivative
|
|
const double theta3_jplus1_deriv = computeDerivativeTheta3(e,
|
|
ev_jplus1,
|
|
dui);
|
|
////1st in bracket term
|
|
const double secondBendingForce_inBracketsTerm_0 = theta3_j_deriv * 2
|
|
* theta3_j;
|
|
////2nd in bracket term
|
|
const double secondBendingForce_inBracketsTerm_1 = theta3_jplus1_deriv
|
|
* theta3_j;
|
|
////3rd in bracket term
|
|
const double secondBendingForce_inBracketsTerm_2 = theta3_j_deriv
|
|
* theta3_jplus1;
|
|
////4th in bracket term
|
|
const double secondBendingForce_inBracketsTerm_3 = theta3_jplus1_deriv * 2
|
|
* theta3_jplus1;
|
|
|
|
// 4th term computation
|
|
const double secondBendingForce_inBracketsTerm
|
|
= secondBendingForce_inBracketsTerm_0
|
|
+ secondBendingForce_inBracketsTerm_1
|
|
+ secondBendingForce_inBracketsTerm_2
|
|
+ secondBendingForce_inBracketsTerm_3;
|
|
const double secondBendingForce_dofi = secondBendingForce_inBracketsTerm
|
|
* element.rigidity.secondBending;
|
|
internalForcesContributionFromThisEdge[evi + 2].second[dofi]
|
|
= secondBendingForce_dofi;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
internalForcesContributionsFromEachEdge[ei] = internalForcesContributionFromThisEdge;
|
|
}
|
|
|
|
//#pragma omp parallel for schedule(static) num_threads(8)
|
|
|
|
for (size_t vi = 0; vi < pMesh->VN(); vi++) {
|
|
Node::Forces &force = pMesh->nodes[vi].force;
|
|
// Vector6d ext = force.external;
|
|
// if (mCurrentSimulationStep <= 100) {
|
|
// ext[3] = 0;
|
|
// ext[4] = 0;
|
|
// }
|
|
force.residual = force.external;
|
|
force.internal = 0;
|
|
}
|
|
for (size_t ei = 0; ei < pMesh->EN(); ei++) {
|
|
for (int i = 0; i < 4; i++) {
|
|
std::pair<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();
|
|
const bool shouldTrigger = std::isnan(residualForceNorm);
|
|
totalResidualForcesNorm += residualForceNorm;
|
|
#ifdef POLYSCOPE_DEFINED
|
|
if (std::isnan(force.residual.norm())) {
|
|
std::cout << "residual " << vi << ":" << force.residual.toString() << std::endl;
|
|
}
|
|
#endif
|
|
}
|
|
pMesh->previousTotalResidualForcesNorm = pMesh->totalResidualForcesNorm;
|
|
pMesh->totalResidualForcesNorm = totalResidualForcesNorm;
|
|
if (mSettings.beVerbose) {
|
|
if (minTotalResidualForcesNorm > pMesh->totalResidualForcesNorm) {
|
|
minTotalResidualForcesNorm = pMesh->totalResidualForcesNorm;
|
|
}
|
|
}
|
|
pMesh->averageResidualForcesNorm = totalResidualForcesNorm / pMesh->VN();
|
|
// pMesh->averageResidualForcesNorm = sumOfResidualForces.norm() / pMesh->VN();
|
|
|
|
// plotYValues.push_back(totalResidualForcesNorm);
|
|
// auto xPlot = matplot::linspace(0, plotYValues.size(), plotYValues.size());
|
|
// plotHandle = matplot::scatter(xPlot, plotYValues);
|
|
}
|
|
|
|
void DRMSimulationModel::updateNodalExternalForces(
|
|
const std::unordered_map<VertexIndex, Vector6d> &nodalForces,
|
|
const std::unordered_map<VertexIndex, std::unordered_set<DoFType>> &fixedVertices)
|
|
{
|
|
externalMomentsNorm = 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;
|
|
}
|
|
}
|
|
|
|
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.A / elem.length;
|
|
translationalSumSk += SkTranslational;
|
|
const double lengthToThe3 = std::pow(elem.length, 3);
|
|
const long double SkRotational_I2 = elem.material.youngsModulus * elem.I2
|
|
/ lengthToThe3;
|
|
const long double SkRotational_I3 = elem.material.youngsModulus * elem.I3
|
|
/ lengthToThe3;
|
|
const long double SkRotational_J = elem.material.youngsModulus * elem.J / lengthToThe3;
|
|
rotationalSumSk_I2 += SkRotational_I2;
|
|
rotationalSumSk_I3 += SkRotational_I3;
|
|
rotationalSumSk_J += SkRotational_J;
|
|
assert(rotationalSumSk_I2 != 0);
|
|
assert(rotationalSumSk_I3 != 0);
|
|
assert(rotationalSumSk_J != 0);
|
|
}
|
|
pMesh->nodes[v].mass.translational = gamma * pow(mSettings.Dtini, 2) * 2
|
|
* translationalSumSk;
|
|
pMesh->nodes[v].mass.rotationalI2 = gamma * pow(mSettings.Dtini, 2) * 8
|
|
* rotationalSumSk_I2;
|
|
pMesh->nodes[v].mass.rotationalI3 = gamma * pow(mSettings.Dtini, 2) * 8
|
|
* rotationalSumSk_I3;
|
|
pMesh->nodes[v].mass.rotationalJ = gamma * pow(mSettings.Dtini, 2) * 8 * rotationalSumSk_J;
|
|
|
|
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);
|
|
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;
|
|
}
|
|
}
|
|
#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];
|
|
#ifdef POLYSCOPE_DEFINED
|
|
if (std::isnan(node.velocity.norm())) {
|
|
std::cout << "Velocity " << vi << ":" << node.velocity.toString() << std::endl;
|
|
}
|
|
#endif
|
|
node.velocity = node.velocity + node.acceleration * Dt;
|
|
}
|
|
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.shouldUseTranslationalKineticEnergyThreshold = true;
|
|
#ifdef POLYSCOPE_DEFINED
|
|
std::cout << "Maximum moment reached.The Kinetic energy of the system will "
|
|
"be used as a convergence criterion"
|
|
<< std::endl;
|
|
#endif
|
|
checkedForMaximumMoment = true;
|
|
}
|
|
|
|
} else {
|
|
const double nzSquared = 1.0 - nxnyMagnitude;
|
|
const double nz = std::sqrt(nzSquared);
|
|
VectorType newNormal(nx, ny, nz);
|
|
v.N() = newNormal;
|
|
}
|
|
|
|
// if (!fixedVertices.contains(vi) || !fixedVertices.at(vi).contains(DoF::Nr)) {
|
|
// if (vi == 1) {
|
|
// std::cout << "AFTER:" << mesh->vert[1].N()[0] << " " <<
|
|
// mesh->vert[1].N()[1]
|
|
// << " " << mesh->vert[1].N()[2] << std::endl;
|
|
// }
|
|
}
|
|
|
|
void DRMSimulationModel::applyDisplacements(
|
|
const std::unordered_map<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.A / elem.length;
|
|
translationalSumSk += SkTranslational;
|
|
const double lengthToThe3 = std::pow(elem.length, 3);
|
|
const double SkRotational_I2 = elem.material.youngsModulus * elem.I2
|
|
/ lengthToThe3; // TODO: I2->t2,I3->t3,t1->polar inertia
|
|
const double SkRotational_I3 = elem.material.youngsModulus * elem.I3
|
|
/ lengthToThe3; // TODO: I2->t2,I3->t3,t1->polar inertia
|
|
const double SkRotational_J = elem.material.youngsModulus * elem.J
|
|
/ lengthToThe3; // TODO: I2->t2,I3->t3,t1->polar inertia
|
|
rotationalSumSk_I2 += SkRotational_I2;
|
|
rotationalSumSk_I3 += SkRotational_I3;
|
|
rotationalSumSk_J += SkRotational_J;
|
|
// assert(rotationalSumSk_I2 != 0);
|
|
// assert(rotationalSumSk_I3 != 0);
|
|
// assert(rotationalSumSk_J != 0);
|
|
}
|
|
pMesh->nodes[v].mass.translational = gamma * pow(mSettings.Dtini, 2) * 2
|
|
* translationalSumSk;
|
|
pMesh->nodes[v].mass.rotationalI2 = gamma * pow(mSettings.Dtini, 2) * 8
|
|
* rotationalSumSk_I2;
|
|
pMesh->nodes[v].mass.rotationalI3 = gamma * pow(mSettings.Dtini, 2) * 8
|
|
* rotationalSumSk_I3;
|
|
pMesh->nodes[v].mass.rotationalJ = gamma * pow(mSettings.Dtini, 2) * 8 * rotationalSumSk_J;
|
|
|
|
// assert(std::pow(mSettings.Dtini, 2.0) * translationalSumSk /
|
|
// mesh->nodes[v].translationalMass <
|
|
// 2);
|
|
// assert(std::pow(mSettings.Dtini, 2.0) * rotationalSumSk_I2 /
|
|
// mesh->nodes[v].rotationalMass_I2 <
|
|
// 2);
|
|
// assert(std::pow(mSettings.Dtini, 2.0) * rotationalSumSk_I3 /
|
|
// mesh->nodes[v].rotationalMass_I3 <
|
|
// 2);
|
|
// assert(std::pow(mSettings.Dtini, 2.0) * rotationalSumSk_J /
|
|
// mesh->nodes[v].rotationalMass_J <
|
|
// 2);
|
|
}
|
|
|
|
for (VertexType &v : pMesh->vert) {
|
|
Node &node = pMesh->nodes[v];
|
|
for (DoFType dofi = DoF::Ux; dofi < DoF::NumDoF; dofi++) {
|
|
if (dofi == DoF::Ux || dofi == DoF::Uy || dofi == DoF::Uz) {
|
|
node.acceleration[dofi] = node.force.residual[dofi] / node.mass.translational;
|
|
} else if (dofi == DoF::Nx) {
|
|
node.acceleration[dofi] = node.force.residual[dofi] / node.mass.rotationalJ;
|
|
} else if (dofi == DoF::Ny) {
|
|
node.acceleration[dofi] = node.force.residual[dofi] / node.mass.rotationalI3;
|
|
} else if (dofi == DoF::Nr) {
|
|
node.acceleration[dofi] = node.force.residual[dofi] / node.mass.rotationalI2;
|
|
}
|
|
}
|
|
}
|
|
|
|
for (VertexType &v : pMesh->vert) {
|
|
Node &node = pMesh->nodes[v];
|
|
node.velocity = node.velocity + node.acceleration * Dt;
|
|
}
|
|
updateKineticEnergy();
|
|
|
|
for (VertexType &v : pMesh->vert) {
|
|
Node &node = pMesh->nodes[v];
|
|
node.displacements = node.displacements + node.velocity * Dt;
|
|
}
|
|
|
|
for (VertexType &v : pMesh->vert) {
|
|
updateNodePosition(v, fixedVertices);
|
|
Node &node = pMesh->nodes[v];
|
|
const VertexIndex &vi = node.vi;
|
|
VectorType normalDisplacementVector(0, 0, 0);
|
|
if (!fixedVertices.contains(vi) || !fixedVertices.at(vi).contains(3)) {
|
|
normalDisplacementVector += VectorType(node.displacements[3], 0, 0);
|
|
}
|
|
if (!fixedVertices.contains(vi) || !fixedVertices.at(vi).contains(4)) {
|
|
normalDisplacementVector += VectorType(0, node.displacements[4], 0);
|
|
}
|
|
v.N() = node.initialNormal + normalDisplacementVector;
|
|
const double &nx = v.N()[0], ny = v.N()[1];
|
|
const double nxnyMagnitude = std::pow(nx, 2) + std::pow(ny, 2);
|
|
if (nxnyMagnitude > 1) {
|
|
v.N() = VectorType(nx / std::sqrt(nxnyMagnitude), ny / std::sqrt(nxnyMagnitude), 0);
|
|
} else {
|
|
const double nzSquared = 1.0 - nxnyMagnitude;
|
|
const double nz = std::sqrt(nzSquared);
|
|
VectorType newNormal(nx, ny, nz);
|
|
v.N() = newNormal;
|
|
}
|
|
if (!isRigidSupport[vi]) {
|
|
node.nR = node.displacements[5];
|
|
} else {
|
|
}
|
|
}
|
|
updateElementalFrames();
|
|
}
|
|
|
|
SimulationResults DRMSimulationModel::computeResults(const std::shared_ptr<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.job = pJob;
|
|
results.history = history;
|
|
results.displacements = displacements;
|
|
return results;
|
|
}
|
|
|
|
void DRMSimulationModel::printCurrentState() const
|
|
{
|
|
std::cout << "Simulation steps executed:" << mCurrentSimulationStep << std::endl;
|
|
std::cout << "Residual forces norm: " << pMesh->totalResidualForcesNorm << std::endl;
|
|
std::cout << "Average Residual forces norm/extForcesNorm: "
|
|
<< pMesh->totalResidualForcesNorm / pMesh->VN() / pMesh->totalExternalForcesNorm
|
|
<< std::endl;
|
|
std::cout << "Moving average residual forces:" << pMesh->residualForcesMovingAverage
|
|
<< std::endl;
|
|
std::cout << "Kinetic energy:" << pMesh->currentTotalKineticEnergy << std::endl;
|
|
static std::chrono::steady_clock::time_point begin = std::chrono::steady_clock::now();
|
|
const double timePerNodePerIteration = std::chrono::duration_cast<std::chrono::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 << 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(false);
|
|
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].A;
|
|
J[ei] = pMesh->elements[e].J;
|
|
I2[ei] = pMesh->elements[e].I2;
|
|
I3[ei] = pMesh->elements[e].I3;
|
|
}
|
|
|
|
polyscope::getCurveNetwork(meshPolyscopeLabel)->addEdgeScalarQuantity("A", A);
|
|
polyscope::getCurveNetwork(meshPolyscopeLabel)->addEdgeScalarQuantity("J", J);
|
|
polyscope::getCurveNetwork(meshPolyscopeLabel)
|
|
->addEdgeScalarQuantity("I2", I2);
|
|
polyscope::getCurveNetwork(meshPolyscopeLabel)->addEdgeScalarQuantity("I3", I3);
|
|
|
|
// Specify the callback
|
|
static bool calledOnce = false;
|
|
if (!calledOnce) {
|
|
PolyscopeInterface::addUserCallback([&]() {
|
|
// Since options::openImGuiWindowForUserCallback == true by default,
|
|
// we can immediately start using ImGui commands to build a UI
|
|
|
|
ImGui::PushItemWidth(100); // Make ui elements 100 pixels wide,
|
|
// instead of full width. Must have
|
|
// matching PopItemWidth() below.
|
|
|
|
ImGui::InputInt("Simulation debug step",
|
|
&mSettings.debugModeStep); // set a int variable
|
|
mSettings.drawingStep = mSettings.debugModeStep;
|
|
ImGui::Checkbox("Enable drawing",
|
|
&mSettings.shouldDraw); // set a int variable
|
|
ImGui::Text("Number of simulation steps: %zu", mCurrentSimulationStep);
|
|
|
|
ImGui::PopItemWidth();
|
|
});
|
|
calledOnce = true;
|
|
}
|
|
|
|
if (!screenshotsFolder.empty()) {
|
|
static bool firstDraw = true;
|
|
if (firstDraw) {
|
|
for (const auto &entry :
|
|
std::filesystem::directory_iterator(screenshotsFolder))
|
|
std::filesystem::remove_all(entry.path());
|
|
// polyscope::view::processZoom(5);
|
|
firstDraw = false;
|
|
}
|
|
polyscope::screenshot(
|
|
std::filesystem::path(screenshotsFolder)
|
|
.append(std::to_string(mCurrentSimulationStep) + ".png")
|
|
.string(),
|
|
false);
|
|
}
|
|
polyscope::show();
|
|
}
|
|
#endif
|
|
|
|
void DRMSimulationModel::applySolutionGuess(const SimulationResults &solutionGuess,
|
|
const std::shared_ptr<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();
|
|
}
|
|
|
|
SimulationResults DRMSimulationModel::executeSimulation(const std::shared_ptr<SimulationJob> &pJob,
|
|
const Settings &settings,
|
|
const SimulationResults &solutionGuess)
|
|
{
|
|
assert(pJob->pMesh.operator bool());
|
|
auto beginTime = std::chrono::high_resolution_clock::now();
|
|
mSettings = settings;
|
|
reset();
|
|
|
|
history.label = pJob->pMesh->getLabel() + "_" + pJob->getLabel();
|
|
|
|
// if (!pJob->nodalExternalForces.empty()) {
|
|
// double externalForcesNorm = 0;
|
|
// for (const auto &externalForce : pJob->nodalExternalForces) {
|
|
// externalForcesNorm += externalForce.second.norm();
|
|
// }
|
|
// mSettings.totalResidualForcesNormThreshold = externalForcesNorm * 1e-2;
|
|
// }
|
|
|
|
constrainedVertices = pJob->constrainedVertices;
|
|
if (!pJob->nodalForcedDisplacements.empty()) {
|
|
for (std::pair<VertexIndex, Eigen::Vector3d> viDisplPair : pJob->nodalForcedDisplacements) {
|
|
const VertexIndex vi = viDisplPair.first;
|
|
constrainedVertices[vi].insert({0, 1, 2});
|
|
}
|
|
}
|
|
// if (!pJob->nodalForcedNormals.empty()) {
|
|
// for (std::pair<VertexIndex, Eigen::Vector3d> viNormalPair :
|
|
// pJob->nodalForcedDisplacements) {
|
|
// assert(viNormalPair3second[2] >= 0);
|
|
// }
|
|
// }
|
|
|
|
pMesh.reset();
|
|
pMesh = std::make_unique<SimulationMesh>(*pJob->pMesh);
|
|
if (mSettings.beVerbose) {
|
|
std::cout << "Executing simulation for mesh with:" << pMesh->VN() << " nodes and "
|
|
<< pMesh->EN() << " elements." << std::endl;
|
|
}
|
|
vcg::tri::UpdateBounding<SimulationMesh>::Box(*pMesh);
|
|
computeRigidSupports();
|
|
isVertexConstrained.resize(pMesh->VN(), false);
|
|
for (auto fixedVertex : pJob->constrainedVertices) {
|
|
isVertexConstrained[fixedVertex.first] = true;
|
|
}
|
|
|
|
#ifdef POLYSCOPE_DEFINED
|
|
if (mSettings.shouldDraw || mSettings.isDebugMode) {
|
|
PolyscopeInterface::init();
|
|
polyscope::registerCurveNetwork(meshPolyscopeLabel,
|
|
pMesh->getEigenVertices(),
|
|
pMesh->getEigenEdges());
|
|
polyscope::registerCurveNetwork("Initial_" + meshPolyscopeLabel,
|
|
pMesh->getEigenVertices(),
|
|
pMesh->getEigenEdges())
|
|
->setRadius(0.002);
|
|
// registerWorldAxes();
|
|
}
|
|
#endif
|
|
|
|
if (!pJob->nodalForcedDisplacements.empty() && pJob->nodalExternalForces.empty()) {
|
|
shouldApplyInitialDistortion = true;
|
|
}
|
|
updateElementalFrames();
|
|
if (!solutionGuess.displacements.empty()) {
|
|
//#ifdef POLYSCOPE_DEFINED
|
|
// if (mSettings.shouldDraw || mSettings.isDebugMode) {
|
|
// solutionGuess.registerForDrawing();
|
|
// polyscope::show();
|
|
// solutionGuess.unregister();
|
|
// }
|
|
//#endif
|
|
|
|
applySolutionGuess(solutionGuess, pJob);
|
|
shouldTemporarilyDampForces = true;
|
|
// Dt = mSettings.Dtini * 0.9;
|
|
}
|
|
|
|
updateNodalMasses();
|
|
std::unordered_map<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;
|
|
}
|
|
pMesh->totalExternalForcesNorm = totalExternalForcesNorm;
|
|
updateNodalExternalForces(nodalExternalForces, constrainedVertices);
|
|
if (!nodalExternalForces.empty()) {
|
|
mSettings.totalResidualForcesNormThreshold
|
|
= settings.totalExternalForcesNormPercentageTermination * totalExternalForcesNorm;
|
|
} else {
|
|
mSettings.totalResidualForcesNormThreshold = 1e-3;
|
|
std::cout << "No forces setted default residual forces norm threshold" << std::endl;
|
|
}
|
|
if (mSettings.beVerbose) {
|
|
std::cout << "totalResidualForcesNormThreshold:"
|
|
<< mSettings.totalResidualForcesNormThreshold << std::endl;
|
|
if (mSettings.useAverage) {
|
|
std::cout << "average/extNorm threshold:"
|
|
<< mSettings.averageResidualForcesCriterionThreshold << std::endl;
|
|
}
|
|
}
|
|
const size_t movingAverageSampleSize = 200;
|
|
std::queue<double> residualForcesMovingAverageHistorySample;
|
|
std::vector<double> percentageOfConvergence;
|
|
|
|
// double residualForcesMovingAverageDerivativeMax = 0;
|
|
while (settings.maxDRMIterations == 0 || mCurrentSimulationStep < settings.maxDRMIterations) {
|
|
if ((mSettings.isDebugMode && mCurrentSimulationStep == 50000)) {
|
|
// std::filesystem::create_directory("./PatternOptimizationNonConv");
|
|
// pJob->save("./PatternOptimizationNonConv");
|
|
// Dt = mSettings.Dtini;
|
|
}
|
|
// if (mCurrentSimulationStep == 500 && shouldTemporarilyDampForces) {
|
|
// Dt = mSettings.Dtini;
|
|
// }
|
|
// while (true) {
|
|
updateNormalDerivatives();
|
|
updateT1Derivatives();
|
|
updateRDerivatives();
|
|
updateT2Derivatives();
|
|
updateT3Derivatives();
|
|
const bool shouldBreak = mCurrentSimulationStep == 12970;
|
|
updateResidualForcesOnTheFly(constrainedVertices);
|
|
|
|
// TODO: write parallel function for updating positions
|
|
// TODO: Make a single function out of updateResidualForcesOnTheFly
|
|
// updatePositionsOnTheFly
|
|
// updatePositionsOnTheFly(constrainedVertices);
|
|
updateNodalMasses();
|
|
updateNodalAccelerations();
|
|
updateNodalVelocities();
|
|
updateNodalDisplacements();
|
|
applyDisplacements(constrainedVertices);
|
|
if (!pJob->nodalForcedDisplacements.empty()) {
|
|
applyForcedDisplacements(
|
|
|
|
pJob->nodalForcedDisplacements);
|
|
}
|
|
// if (!pJob->nodalForcedNormals.empty()) {
|
|
// applyForcedNormals(pJob->nodalForcedNormals);
|
|
// }
|
|
updateElementalLengths();
|
|
mCurrentSimulationStep++;
|
|
if (std::isnan(pMesh->currentTotalKineticEnergy)) {
|
|
std::cout << pMesh->currentTotalKineticEnergy << std::endl;
|
|
if (mSettings.beVerbose) {
|
|
std::cerr << "Infinite kinetic energy at step " << mCurrentSimulationStep
|
|
<< ". Exiting.." << std::endl;
|
|
}
|
|
break;
|
|
}
|
|
|
|
//normalized sum of displacements
|
|
// double sumOfDisplacementsNorm = 0;
|
|
// for (size_t vi = 0; vi < pMesh->VN(); vi++) {
|
|
// sumOfDisplacementsNorm += pMesh->nodes[vi].displacements.getTranslation().norm();
|
|
// }
|
|
// sumOfDisplacementsNorm /= pMesh->bbox.Diag();
|
|
// pMesh->sumOfNormalizedDisplacementNorms = sumOfDisplacementsNorm;
|
|
|
|
//moving average
|
|
// // pMesh->residualForcesMovingAverage = (pMesh->totalResidualForcesNorm
|
|
// // + mCurrentSimulationStep
|
|
// // * pMesh->residualForcesMovingAverage)
|
|
// // / (1 + mCurrentSimulationStep);
|
|
pMesh->residualForcesMovingAverage += pMesh->totalResidualForcesNorm
|
|
/ movingAverageSampleSize;
|
|
residualForcesMovingAverageHistorySample.push(pMesh->residualForcesMovingAverage);
|
|
if (movingAverageSampleSize < residualForcesMovingAverageHistorySample.size()) {
|
|
const double firstElementValue = residualForcesMovingAverageHistorySample.front();
|
|
pMesh->residualForcesMovingAverage -= firstElementValue / movingAverageSampleSize;
|
|
residualForcesMovingAverageHistorySample.pop();
|
|
|
|
// pMesh->residualForcesMovingAverageDerivativeNorm
|
|
// = std::abs((residualForcesMovingAverageHistorySample.back()
|
|
// - residualForcesMovingAverageHistorySample.front()))
|
|
// / (movingAverageSampleSize);
|
|
// residualForcesMovingAverageDerivativeMax
|
|
// = std::max(pMesh->residualForcesMovingAverageDerivativeNorm,
|
|
// residualForcesMovingAverageDerivativeMax);
|
|
// pMesh->residualForcesMovingAverageDerivativeNorm
|
|
// /= residualForcesMovingAverageDerivativeMax;
|
|
// // std::cout << "Normalized derivative:"
|
|
// // << residualForcesMovingAverageDerivativeNorm
|
|
// // << std::endl;
|
|
}
|
|
|
|
// pMesh->previousTotalPotentialEnergykN =
|
|
// pMesh->currentTotalPotentialEnergykN;
|
|
// pMesh->currentTotalPotentialEnergykN = computeTotalPotentialEnergy() / 1000;
|
|
// timePerNodePerIterationHistor.push_back(timePerNodePerIteration);
|
|
if (mSettings.beVerbose && mSettings.isDebugMode
|
|
&& mCurrentSimulationStep % mSettings.debugModeStep == 0) {
|
|
printCurrentState();
|
|
|
|
// auto t2 = std::chrono::high_resolution_clock::now();
|
|
// const double executionTime_min
|
|
// = std::chrono::duration_cast<std::chrono::minutes>(t2 - beginTime).count();
|
|
// std::cout << "Execution time(min):" << executionTime_min << std::endl;
|
|
if (mSettings.useAverage) {
|
|
std::cout << "Best percentage of target (average):"
|
|
<< 100 * mSettings.averageResidualForcesCriterionThreshold
|
|
* totalExternalForcesNorm
|
|
/ (minTotalResidualForcesNorm / pMesh->VN())
|
|
<< "%" << std::endl;
|
|
}
|
|
std::cout << "Best percentage of target:"
|
|
<< 100 * mSettings.totalExternalForcesNormPercentageTermination
|
|
* totalExternalForcesNorm / minTotalResidualForcesNorm
|
|
<< "%" << std::endl;
|
|
// SimulationResultsReporter::createPlot("Number of Steps",
|
|
// "Residual Forces mov aver",
|
|
// history.residualForcesMovingAverage);
|
|
// SimulationResultsReporter::createPlot("Number of Steps",
|
|
// "Residual Forces mov aver deriv",
|
|
// movingAveragesDerivatives);
|
|
// draw();
|
|
// SimulationResulnodalForcedDisplacementstsReporter::createPlot("Number of Steps",
|
|
// "Time/(#nodes*#iterations)",
|
|
// timePerNodePerIterationHistory);
|
|
}
|
|
|
|
if ((mSettings.shouldCreatePlots || mSettings.isDebugMode) && mCurrentSimulationStep != 0) {
|
|
history.stepPulse(*pMesh);
|
|
percentageOfConvergence.push_back(100 * mSettings.totalResidualForcesNormThreshold
|
|
/ pMesh->totalResidualForcesNorm);
|
|
}
|
|
|
|
if (mSettings.shouldCreatePlots && mSettings.isDebugMode
|
|
&& mCurrentSimulationStep % mSettings.debugModeStep == 0) {
|
|
// SimulationResultsReporter::createPlot("Number of Steps",
|
|
// "Residual Forces mov aver deriv",
|
|
// movingAveragesDerivatives_norm);
|
|
SimulationResultsReporter::createPlot("Number of Steps",
|
|
"Residual Forces mov aver",
|
|
history.residualForcesMovingAverage);
|
|
// SimulationResultsReporter::createPlot("Number of Steps",
|
|
// "Log of Residual Forces",
|
|
// history.logResidualForces,
|
|
// {},
|
|
// history.redMarks);
|
|
// SimulationResultsReporter::createPlot("Number of Steps",
|
|
// "Log of Kinetic energy",
|
|
// history.kineticEnergy,
|
|
// {},
|
|
// history.redMarks);
|
|
// SimulationResultsReporter reporter;
|
|
// reporter.reportHistory(history, "IntermediateResults", pJob->pMesh->getLabel());
|
|
// SimulationResultsReporter::createPlot("Number of Iterations",
|
|
// "Sum of normalized displacement norms",
|
|
// history.sumOfNormalizedDisplacementNorms /*,
|
|
// std::filesystem::path("./")
|
|
// .append("SumOfNormalizedDisplacementNorms_" + graphSuffix + ".png")
|
|
// .string()*/
|
|
// ,
|
|
// {},
|
|
// history.redMarks);
|
|
// SimulationResultsReporter::createPlot("Number of Steps",
|
|
// "Percentage of convergence",
|
|
// percentageOfConvergence,
|
|
// {},
|
|
// history.redMarks);
|
|
}
|
|
|
|
#ifdef POLYSCOPE_DEFINED
|
|
if (mSettings.shouldDraw && mSettings.isDebugMode
|
|
&& mCurrentSimulationStep % mSettings.drawingStep == 0) /* &&
|
|
|
|
currentSimulationStep > maxDRMIterations*/
|
|
{
|
|
// std::string saveTo = std::filesystem::current_path()
|
|
// .append("Debugging_files")
|
|
// .append("Screenshots")
|
|
// .string();
|
|
draw();
|
|
// yValues = history.kineticEnergy;
|
|
// plotHandle = matplot::scatter(xPlot, yValues);
|
|
// label = "Log of Kinetic energy";
|
|
// plotHandle->legend_string(label);
|
|
|
|
// shouldUseKineticEnergyThreshold = true;
|
|
}
|
|
#endif
|
|
|
|
// t = t + Dt;
|
|
// std::cout << "Kinetic energy:" << mesh.currentTotalKineticEnergy
|
|
// << std::endl;
|
|
// std::cout << "Residual forces norm:" << mesh.totalResidualForcesNorm
|
|
// << std::endl;
|
|
const bool fullfillsResidualForcesNormTerminationCriterion
|
|
= !mSettings.useAverage
|
|
&& pMesh->totalResidualForcesNorm / totalExternalForcesNorm
|
|
< mSettings.totalExternalForcesNormPercentageTermination;
|
|
const bool fullfillsAverageResidualForcesNormTerminationCriterion
|
|
= mSettings.useAverage
|
|
&& (pMesh->totalResidualForcesNorm / pMesh->VN()) / totalExternalForcesNorm
|
|
< mSettings.averageResidualForcesCriterionThreshold;
|
|
// Residual forces norm convergence
|
|
if (((pMesh->previousTotalKineticEnergy > pMesh->currentTotalKineticEnergy
|
|
|| fullfillsAverageResidualForcesNormTerminationCriterion
|
|
|| fullfillsResidualForcesNormTerminationCriterion)
|
|
// && mCurrentSimulationStep > movingAverageSampleSize
|
|
&& (pJob->nodalForcedDisplacements.empty()
|
|
|| mCurrentSimulationStep > mSettings.gradualForcedDisplacementSteps))
|
|
/* || (pMesh->totalResidualForcesNorm / mSettings.totalResidualForcesNormThreshold <= 1
|
|
&& mCurrentSimulationStep > 1)*/
|
|
/*||
|
|
mesh->previousTotalPotentialEnergykN >
|
|
mesh->currentTotalPotentialEnergykN*/
|
|
/*|| mesh->currentTotalPotentialEnergykN < minPotentialEnergy*/) {
|
|
// if (pMesh->totalResidualForcesNorm < totalResidualForcesNormThreshold) {
|
|
const bool fullfillsKineticEnergyTerminationCriterion
|
|
= mSettings.shouldUseTranslationalKineticEnergyThreshold
|
|
&& pMesh->currentTotalTranslationalKineticEnergy
|
|
< mSettings.totalTranslationalKineticEnergyThreshold
|
|
&& mCurrentSimulationStep > 20 && numOfDampings > 0;
|
|
const bool fullfillsMovingAverageNormTerminationCriterion
|
|
= pMesh->residualForcesMovingAverage
|
|
< mSettings.residualForcesMovingAverageNormThreshold;
|
|
/*pMesh->residualForcesMovingAverage < totalResidualForcesNormThreshold*/
|
|
const bool fullfillsMovingAverageDerivativeNormTerminationCriterion
|
|
= pMesh->residualForcesMovingAverageDerivativeNorm < 1e-4;
|
|
const bool shouldTerminate
|
|
= fullfillsKineticEnergyTerminationCriterion
|
|
// || fullfillsMovingAverageNormTerminationCriterion
|
|
// || fullfillsMovingAverageDerivativeNormTerminationCriterion
|
|
|| fullfillsAverageResidualForcesNormTerminationCriterion
|
|
|| fullfillsResidualForcesNormTerminationCriterion;
|
|
if (shouldTerminate && mCurrentSimulationStep > 100) {
|
|
if (mSettings.beVerbose /*&& !mSettings.isDebugMode*/) {
|
|
std::cout << "Simulation converged." << std::endl;
|
|
printCurrentState();
|
|
if (fullfillsResidualForcesNormTerminationCriterion) {
|
|
std::cout << "Converged using residual forces norm threshold criterion"
|
|
<< std::endl;
|
|
} else if (fullfillsKineticEnergyTerminationCriterion) {
|
|
std::cout << "The kinetic energy of the system was "
|
|
" used as a convergence criterion"
|
|
<< std::endl;
|
|
} else if (fullfillsMovingAverageNormTerminationCriterion) {
|
|
std::cout
|
|
<< "Converged using residual forces moving average derivative norm "
|
|
"threshold criterion"
|
|
<< std::endl;
|
|
}
|
|
}
|
|
if (mSettings.desiredGradualExternalLoadsSteps == externalLoadStep) {
|
|
break;
|
|
} else {
|
|
externalLoadStep++;
|
|
std::unordered_map<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;
|
|
}
|
|
}
|
|
// }
|
|
}
|
|
|
|
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();
|
|
Dt *= mSettings.xi;
|
|
// if (mSettings.isDebugMode) {
|
|
// std::cout << Dt << std::endl;
|
|
// }
|
|
++numOfDampings;
|
|
if (mSettings.shouldCreatePlots) {
|
|
history.redMarks.push_back(mCurrentSimulationStep);
|
|
}
|
|
// std::cout << "Number of dampings:" << numOfDampings << endl;
|
|
// draw();
|
|
}
|
|
}
|
|
auto endTime = std::chrono::high_resolution_clock::now();
|
|
|
|
SimulationResults results = computeResults(pJob);
|
|
results.executionTime = std::chrono::duration_cast<std::chrono::seconds>(endTime - beginTime)
|
|
.count();
|
|
|
|
if (mCurrentSimulationStep == settings.maxDRMIterations && mCurrentSimulationStep != 0) {
|
|
if (mSettings.beVerbose) {
|
|
std::cout << "Did not reach equilibrium before reaching the maximum number "
|
|
"of DRM steps ("
|
|
<< settings.maxDRMIterations << "). Breaking simulation" << std::endl;
|
|
}
|
|
results.converged = false;
|
|
} else if (std::isnan(pMesh->currentTotalKineticEnergy)) {
|
|
if (mSettings.beVerbose) {
|
|
std::cerr << "Simulation did not converge due to infinite kinetic energy." << std::endl;
|
|
}
|
|
results.converged = false;
|
|
}
|
|
// mesh.printVertexCoordinates(mesh.VN() / 2);
|
|
#ifdef POLYSCOPE_DEFINED
|
|
if (mSettings.shouldDraw && !mSettings.isDebugMode) {
|
|
draw();
|
|
}
|
|
#endif
|
|
if (!std::isnan(pMesh->currentTotalKineticEnergy)) {
|
|
results.debug_drmDisplacements = results.displacements;
|
|
results.internalPotentialEnergy = computeTotalInternalPotentialEnergy();
|
|
|
|
results.rotationalDisplacementQuaternion.resize(pMesh->VN());
|
|
results.debug_q_f1.resize(pMesh->VN());
|
|
results.debug_q_normal.resize(pMesh->VN());
|
|
results.debug_q_nr.resize(pMesh->VN());
|
|
for (int vi = 0; vi < pMesh->VN(); vi++) {
|
|
const Node &node = pMesh->nodes[vi];
|
|
const Eigen::Vector3d nInitial_eigen = node.initialNormal
|
|
.ToEigenVector<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]
|
|
= (q_normal
|
|
* (q_f1_nInit * q_nr_nInit)); //q_f1_nDeformed * q_nr_nDeformed * q_normal;
|
|
//Update the displacement vector to contain the euler angles
|
|
const Eigen::Vector3d eulerAngles = results.rotationalDisplacementQuaternion[vi]
|
|
.toRotationMatrix()
|
|
.eulerAngles(0, 1, 2);
|
|
results.displacements[vi][3] = eulerAngles[0];
|
|
results.displacements[vi][4] = eulerAngles[1];
|
|
results.displacements[vi][5] = eulerAngles[2];
|
|
|
|
// Eigen::Quaterniond q_test = Eigen::AngleAxisd(eulerAngles[0], Eigen::Vector3d::UnitX())
|
|
// * Eigen::AngleAxisd(eulerAngles[1], Eigen::Vector3d::UnitY())
|
|
// * Eigen::AngleAxisd(eulerAngles[2], Eigen::Vector3d::UnitZ());
|
|
|
|
// const double dot_test = results.rotationalDisplacementQuaternion[vi].dot(q_test);
|
|
// assert(dot_test > 1 - 1e5);
|
|
|
|
int i = 0;
|
|
i++;
|
|
}
|
|
}
|
|
|
|
if (!mSettings.isDebugMode && mSettings.shouldCreatePlots) {
|
|
SimulationResultsReporter reporter;
|
|
reporter.reportResults({results}, "Results", pJob->pMesh->getLabel());
|
|
}
|
|
|
|
#ifdef POLYSCOPE_DEFINED
|
|
if (mSettings.shouldDraw || mSettings.isDebugMode) {
|
|
polyscope::removeCurveNetwork(meshPolyscopeLabel);
|
|
polyscope::removeCurveNetwork("Initial_" + meshPolyscopeLabel);
|
|
}
|
|
#endif
|
|
|
|
return results;
|
|
}
|