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

31
MbDCode/TranslationConstraintIqcJqc.cpp
View File

@@ -5,7 +5,7 @@
using namespace MbD;
TranslationConstraintIqcJqc::TranslationConstraintIqcJqc(EndFrmcptr frmi, EndFrmcptr frmj, size_t axisi) :
TranslationConstraintIqcJqc::TranslationConstraintIqcJqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi) :
TranslationConstraintIqcJc(frmi, frmj, axisi)
{
}
@@ -17,6 +17,12 @@ void MbD::TranslationConstraintIqcJqc::initriIeJeIe()
void MbD::TranslationConstraintIqcJqc::calcPostDynCorrectorIteration()
{
TranslationConstraintIqcJc::calcPostDynCorrectorIteration();
pGpXJ = riIeJeIe->pvaluepXJ();
pGpEJ = riIeJeIe->pvaluepEJ();
ppGpEIpXJ = riIeJeIe->ppvaluepXJpEK()->transpose();
ppGpEIpEJ = riIeJeIe->ppvaluepEJpEK()->transpose();
ppGpEJpEJ = riIeJeIe->ppvaluepEJpEJ();
}
void MbD::TranslationConstraintIqcJqc::useEquationNumbers()
@@ -25,3 +31,26 @@ void MbD::TranslationConstraintIqcJqc::useEquationNumbers()
iqXJ = std::static_pointer_cast<EndFrameqc>(frmJ)->iqX();
iqEJ = std::static_pointer_cast<EndFrameqc>(frmJ)->iqE();
}
void MbD::TranslationConstraintIqcJqc::fillPosICError(FColDsptr col)
{
Constraint::fillPosICError(col);
col->atiplusFullVectortimes(iqXJ, pGpXJ, lam);
col->atiplusFullVectortimes(iqEJ, pGpEJ, lam);
}
void MbD::TranslationConstraintIqcJqc::fillPosICJacob(SpMatDsptr mat)
{
TranslationConstraintIqcJc::fillPosICJacob(mat);
mat->atijplusFullRow(iG, iqXJ, pGpXJ);
mat->atijplusFullColumn(iqXJ, iG, pGpXJ->transpose());
mat->atijplusFullRow(iG, iqEJ, pGpEJ);
mat->atijplusFullColumn(iqEJ, iG, pGpEJ->transpose());
auto ppGpEIpXJlam = ppGpEIpXJ->times(lam);
mat->atijplusFullMatrix(iqEI, iqXJ, ppGpEIpXJlam);
mat->atijplusTransposeFullMatrix(iqXJ, iqEI, ppGpEIpXJlam);
auto ppGpEIpEJlam = ppGpEIpEJ->times(lam);
mat->atijplusFullMatrix(iqEI, iqEJ, ppGpEIpEJlam);
mat->atijplusTransposeFullMatrix(iqEJ, iqEI, ppGpEIpEJlam);
mat->atijplusFullMatrixtimes(iqEJ, iqEJ, ppGpEJpEJ, lam);
}