kindred/solver
1
0
Fork 0
Code Issues 10 Pull Requests 1 Actions Packages Projects Releases Wiki Activity

runVelIC

Browse Source
This commit is contained in:
Aik-Siong Koh
2023-06-18 01:06:39 -06:00
parent 3b08cd72df
commit 371b13a9e0
147 changed files with 2555 additions and 238 deletions
Download Patch File Download Diff File Expand all files Collapse all files

70
MbDCode/VelICSolver.cpp Normal file
View File

@@ -0,0 +1,70 @@
#include "VelICSolver.h"
#include "SystemSolver.h"
#include "Part.h"
#include "Constraint.h"
using namespace MbD;
void MbD::VelICSolver::assignEquationNumbers()
{
auto parts = system->parts();
//auto contactEndFrames = system->contactEndFrames();
//auto uHolders = system->uHolders();
auto constraints = system->allConstraints();
int eqnNo = 0;
for (auto& part : *parts) {
part->iqX(eqnNo);
eqnNo = eqnNo + 3;
part->iqE(eqnNo);
eqnNo = eqnNo + 4;
}
//for (auto& endFrm : *contactEndFrames) {
// endFrm->is(eqnNo);
// eqnNo = eqnNo + endFrm->sSize();
//}
//for (auto& uHolder : *uHolders) {
// uHolder->iu(eqnNo);
// eqnNo += 1;
//}
auto nEqns = eqnNo; //C++ uses index 0.
nqsu = nEqns;
for (auto& con : *constraints) {
con->iG = eqnNo;
eqnNo += 1;
}
auto lastEqnNo = eqnNo - 1;
nEqns = eqnNo; //C++ uses index 0.
n = nEqns;
}
void MbD::VelICSolver::run()
{
std::string str = "MbD: Solving for velocity initial conditions.";
system->logString(str);
this->runBasic();
}
void MbD::VelICSolver::runBasic()
{
//| qsudotOld qsudotWeights qsudotlam |
system->partsJointsMotionsDo([](std::shared_ptr<Item> item) { item->preVelIC(); });
this->assignEquationNumbers();
system->partsJointsMotionsDo([](std::shared_ptr<Item> item) { item->useEquationNumbers(); });
auto qsudotOld = std::make_shared<FullColumn<double>>(nqsu);
auto qsudotWeights = std::make_shared<DiagonalMatrix<double>>(nqsu);
errorVector = std::make_shared<FullColumn<double>>(n);
jacobian = std::make_shared<SparseMatrix<double>>(n, n);
system->partsJointsMotionsDo([&](std::shared_ptr<Item> item) { item->fillqsudot(qsudotOld); });
system->partsJointsMotionsDo([&](std::shared_ptr<Item> item) { item->fillqsudotWeights(qsudotWeights); });
errorVector->zeroSelf();
errorVector->atiplusFullColumn(0, qsudotWeights->timesFullColumn(qsudotOld));
system->partsJointsMotionsDo([&](std::shared_ptr<Item> item) { item->fillVelICError(errorVector); });
jacobian->zeroSelf();
jacobian->atijplusDiagonalMatrix(0, 0, qsudotWeights);
system->partsJointsMotionsDo([&](std::shared_ptr<Item> item) { item->fillVelICJacob(jacobian); });
matrixSolver = this->matrixSolverClassNew();
this->solveEquations();
auto qsudotlam = this->x;
system->partsJointsMotionsDo([&](std::shared_ptr<Item> item) { item->setqsudotlam(qsudotlam); });
system->partsJointsMotionsDo([](std::shared_ptr<Item> item) { item->postVelIC(); });
}