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

systemSolver->runBasicKinematic();

Browse Source
This commit is contained in:
Aik-Siong Koh
2023-06-11 07:15:20 -06:00
parent d848450907
commit 3b08cd72df
182 changed files with 2789 additions and 535 deletions
Download Patch File Download Diff File Expand all files Collapse all files

22
MbDCode/EulerConstraint.cpp
View File

@@ -15,6 +15,7 @@ EulerConstraint::EulerConstraint(const char* str) : Constraint(str)
void EulerConstraint::initialize()
{
Constraint::initialize();
pGpE = std::make_shared<FullRow<double>>(4);
}
@@ -22,7 +23,7 @@ void MbD::EulerConstraint::calcPostDynCorrectorIteration()
{
auto& qE = static_cast<PartFrame*>(owner)->qE;
aG = qE->sumOfSquares() - 1.0;
for (size_t i = 0; i < 4; i++)
for (int i = 0; i < 4; i++)
{
pGpE->at(i) = 2.0 * qE->at(i);
}
@@ -32,3 +33,22 @@ void MbD::EulerConstraint::useEquationNumbers()
{
iqE = static_cast<PartFrame*>(owner)->iqE;
}
void MbD::EulerConstraint::fillPosICError(FColDsptr col)
{
Constraint::fillPosICError(col);
col->atiplusFullVectortimes(iqE, pGpE, lam);
}
void MbD::EulerConstraint::fillPosICJacob(SpMatDsptr mat)
{
//"ppGpEpE is a diag(2,2,2,2)."
mat->atijplusFullRow(iG, iqE, pGpE);
mat->atijplusFullColumn(iqE, iG, pGpE->transpose());
auto twolam = 2.0 * lam;
for (int i = 0; i < 4; i++)
{
auto ii = iqE + i;
(*(mat->at(ii)))[ii] += twolam;
}
}