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

runPosIC, VelIC, AccIC numerically correct

Browse Source
This commit is contained in:
Aik-Siong Koh
2023-06-24 23:08:29 -06:00
parent 371b13a9e0
commit c30ee64b89
110 changed files with 2171 additions and 129 deletions
Download Patch File Download Diff File Expand all files Collapse all files

33
MbDCode/TranslationConstraintIqctJqc.cpp
View File

@@ -24,3 +24,36 @@ void MbD::TranslationConstraintIqctJqc::preVelIC()
TranslationConstraintIJ::preVelIC();
pGpt = std::static_pointer_cast<DispCompIeqctJeqcKeqct>(riIeJeIe)->pvaluept();
}
void MbD::TranslationConstraintIqctJqc::fillVelICError(FColDsptr col)
{
col->atiminusNumber(iG, pGpt);
}
void MbD::TranslationConstraintIqctJqc::preAccIC()
{
TranslationConstraintIJ::preAccIC();
auto riIeJeIeqct = std::static_pointer_cast<DispCompIeqctJeqcKeqct>(riIeJeIe);
ppGpXIpt = riIeJeIeqct->ppvaluepXIpt();
ppGpEIpt = riIeJeIeqct->ppvaluepEIpt()->plusFullRow(riIeJeIeqct->ppvaluepEKpt());
ppGpXJpt = riIeJeIeqct->ppvaluepXJpt();
ppGpEJpt = riIeJeIeqct->ppvaluepEJpt();
ppGptpt = riIeJeIeqct->ppvalueptpt();
}
void MbD::TranslationConstraintIqctJqc::fillAccICIterError(FColDsptr col)
{
TranslationConstraintIqcJqc::fillAccICIterError(col);
auto efrmIqc = std::static_pointer_cast<EndFrameqc>(frmI);
auto efrmJqc = std::static_pointer_cast<EndFrameqc>(frmJ);
auto qXdotI = efrmIqc->qXdot();
auto qEdotI = efrmIqc->qEdot();
auto qXdotJ = efrmJqc->qXdot();
auto qEdotJ = efrmJqc->qEdot();
double sum = 2.0 * ppGpXIpt->timesFullColumn(qXdotI);
sum += 2.0 * ppGpEIpt->timesFullColumn(qEdotI);
sum += 2.0 * ppGpXJpt->timesFullColumn(qXdotJ);
sum += 2.0 * ppGpEJpt->timesFullColumn(qEdotJ);
sum += ppGptpt;
col->atiplusNumber(iG, sum);
}