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

18
MbDCode/EulerConstraint.cpp
View File

@@ -57,3 +57,21 @@ void MbD::EulerConstraint::fillPosKineJacob(SpMatDsptr mat)
{
mat->atijplusFullRow(iG, iqE, pGpE);
}
void MbD::EulerConstraint::fillVelICJacob(SpMatDsptr mat)
{
mat->atijplusFullRow(iG, iqE, pGpE);
mat->atijplusFullColumn(iqE, iG, pGpE->transpose());
}
void MbD::EulerConstraint::fillAccICIterError(FColDsptr col)
{
//"qdotT[ppGpqpq]*qdot."
//"qdotT[2 2 2 2 diag]*qdot."
col->atiplusFullVectortimes(iqE, pGpE, lam);
auto partFrame = static_cast<PartFrame*>(owner);
double sum = pGpE->timesFullColumn(partFrame->qEddot);
sum += 2.0 * partFrame->qEdot->sumOfSquares();
col->atiplusNumber(iG, sum);
}