102 lines
3.1 KiB
C++
102 lines
3.1 KiB
C++
#include <memory>
|
|
|
|
#include "QuasiIntegrator.h"
|
|
#include "Item.h"
|
|
#include "SystemSolver.h"
|
|
#include "CREATE.h"
|
|
#include "BasicQuasiIntegrator.h"
|
|
#include "SingularMatrixError.h"
|
|
#include "SimulationStoppingError.h"
|
|
#include "TooSmallStepSizeError.h"
|
|
#include "TooManyTriesError.h"
|
|
#include "SingularMatrixError.h"
|
|
|
|
using namespace MbD;
|
|
|
|
void MbD::QuasiIntegrator::preRun()
|
|
{
|
|
system->partsJointsMotionsForcesTorquesDo([](std::shared_ptr<Item> item) { item->preDyn(); });
|
|
}
|
|
|
|
void MbD::QuasiIntegrator::initialize()
|
|
{
|
|
Solver::initialize();
|
|
integrator = CREATE<BasicQuasiIntegrator>::With();
|
|
integrator->setSystem(this);
|
|
}
|
|
|
|
void MbD::QuasiIntegrator::run()
|
|
{
|
|
try {
|
|
try {
|
|
try {
|
|
IntegratorInterface::run();
|
|
}
|
|
catch (SingularMatrixError ex) {
|
|
std::stringstream ss;
|
|
ss << "MbD: Solver has encountered a singular matrix." << std::endl;
|
|
ss << "MbD: Check to see if a massless or a very low mass part is under constrained." << std::endl;
|
|
ss << "MbD: Check to see if the system is in a locked position." << std::endl;
|
|
ss << "MbD: Check to see if the error tolerance is too demanding." << std::endl;
|
|
ss << "MbD: Check to see if a curve-curve is about to have multiple contact points." << std::endl;
|
|
auto str = ss.str();
|
|
this->logString(str);
|
|
throw SimulationStoppingError("");
|
|
}
|
|
}
|
|
catch (TooSmallStepSizeError ex) {
|
|
std::stringstream ss;
|
|
ss << "MbD: Step size is prevented from going below the user specified minimum." << std::endl;
|
|
ss << "MbD: Check to see if the system is in a locked position." << std::endl;
|
|
ss << "MbD: Check to see if a curve-curve is about to have multiple contact points." << std::endl;
|
|
ss << "MbD: If they are not, lower the permitted minimum step size." << std::endl;
|
|
auto str = ss.str();
|
|
this->logString(str);
|
|
throw SimulationStoppingError("");
|
|
}
|
|
}
|
|
catch (TooManyTriesError ex) {
|
|
std::stringstream ss;
|
|
ss << "MbD: Check to see if the error tolerance is too demanding." << std::endl;
|
|
auto str = ss.str();
|
|
this->logString(str);
|
|
throw SimulationStoppingError("");
|
|
}
|
|
|
|
}
|
|
|
|
void MbD::QuasiIntegrator::preFirstStep()
|
|
{
|
|
system->partsJointsMotionsForcesTorquesDo([](std::shared_ptr<Item> item) { item->preDynFirstStep(); });
|
|
}
|
|
|
|
void MbD::QuasiIntegrator::preStep()
|
|
{
|
|
system->partsJointsMotionsForcesTorquesDo([](std::shared_ptr<Item> item) { item->preDynStep(); });
|
|
}
|
|
|
|
double MbD::QuasiIntegrator::suggestSmallerOrAcceptFirstStepSize(double hnew)
|
|
{
|
|
auto hnew2 = hnew;
|
|
system->partsJointsMotionsForcesTorquesDo([&](std::shared_ptr<Item> item) { hnew2 = item->suggestSmallerOrAcceptDynFirstStepSize(hnew2); });
|
|
if (hnew2 > hmax) {
|
|
hnew2 = hmax;
|
|
std::string str = "StM: Step size is at user specified maximum.";
|
|
this->logString(str);
|
|
}
|
|
if (hnew2 < hmin) {
|
|
std::stringstream ss;
|
|
ss << "StM: Step size " << hnew2 << " < " << hmin << " user specified minimum.";
|
|
auto str = ss.str();
|
|
this->logString(str);
|
|
throw TooSmallStepSizeError("");
|
|
}
|
|
return hnew2;
|
|
}
|
|
|
|
void MbD::QuasiIntegrator::incrementTime(double tnew)
|
|
{
|
|
system->partsJointsMotionsForcesTorquesDo([](std::shared_ptr<Item> item) { item->storeDynState(); });
|
|
IntegratorInterface::incrementTime(tnew);
|
|
}
|