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

29
MbDCode/TranslationConstraintIqcJqc.cpp
View File

@@ -34,7 +34,7 @@ void MbD::TranslationConstraintIqcJqc::useEquationNumbers()
void MbD::TranslationConstraintIqcJqc::fillPosICError(FColDsptr col)
{
Constraint::fillPosICError(col);
TranslationConstraintIqcJc::fillPosICError(col);
col->atiplusFullVectortimes(iqXJ, pGpXJ, lam);
col->atiplusFullVectortimes(iqEJ, pGpEJ, lam);
}
@@ -61,3 +61,30 @@ void MbD::TranslationConstraintIqcJqc::fillPosKineJacob(SpMatDsptr mat)
mat->atijplusFullRow(iG, iqXJ, pGpXJ);
mat->atijplusFullRow(iG, iqEJ, pGpEJ);
}
void MbD::TranslationConstraintIqcJqc::fillVelICJacob(SpMatDsptr mat)
{
TranslationConstraintIqcJc::fillVelICJacob(mat);
mat->atijplusFullRow(iG, iqXJ, pGpXJ);
mat->atijplusFullColumn(iqXJ, iG, pGpXJ->transpose());
mat->atijplusFullRow(iG, iqEJ, pGpEJ);
mat->atijplusFullColumn(iqEJ, iG, pGpEJ->transpose());
}
void MbD::TranslationConstraintIqcJqc::fillAccICIterError(FColDsptr col)
{
TranslationConstraintIqcJc::fillAccICIterError(col);
col->atiplusFullVectortimes(iqXJ, pGpXJ, lam);
col->atiplusFullVectortimes(iqEJ, pGpEJ, lam);
auto efrmIqc = std::static_pointer_cast<EndFrameqc>(frmI);
auto efrmJqc = std::static_pointer_cast<EndFrameqc>(frmJ);
auto qEdotI = efrmIqc->qEdot();
auto qXdotJ = efrmJqc->qXdot();
auto qEdotJ = efrmJqc->qEdot();
double sum = pGpXJ->timesFullColumn(efrmJqc->qXddot());
sum += pGpEJ->timesFullColumn(efrmJqc->qEddot());
sum += 2.0 * (qEdotI->transposeTimesFullColumn(ppGpEIpXJ->timesFullColumn(qXdotJ)));
sum += 2.0 * (qEdotI->transposeTimesFullColumn(ppGpEIpEJ->timesFullColumn(qEdotJ)));
sum += qEdotJ->transposeTimesFullColumn(ppGpEJpEJ->timesFullColumn(qEdotJ));
col->atiplusNumber(iG, sum);
}