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

19
MbDCode/DirectionCosineConstraintIqcJqc.cpp
View File

@@ -5,7 +5,7 @@
using namespace MbD;
DirectionCosineConstraintIqcJqc::DirectionCosineConstraintIqcJqc(EndFrmcptr frmi, EndFrmcptr frmj, size_t axisi, size_t axisj) :
DirectionCosineConstraintIqcJqc::DirectionCosineConstraintIqcJqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj) :
DirectionCosineConstraintIqcJc(frmi, frmj, axisi, axisj)
{
}
@@ -29,3 +29,20 @@ void MbD::DirectionCosineConstraintIqcJqc::useEquationNumbers()
DirectionCosineConstraintIqcJc::useEquationNumbers();
iqEJ = std::static_pointer_cast<EndFrameqc>(frmJ)->iqE();
}
void MbD::DirectionCosineConstraintIqcJqc::fillPosICError(FColDsptr col)
{
DirectionCosineConstraintIqcJc::fillPosICError(col);
col->atiplusFullVectortimes(iqEJ, pGpEJ, lam);
}
void MbD::DirectionCosineConstraintIqcJqc::fillPosICJacob(SpMatDsptr mat)
{
DirectionCosineConstraintIqcJc::fillPosICJacob(mat);
mat->atijplusFullRow(iG, iqEJ, pGpEJ);
mat->atijplusFullColumn(iqEJ, iG, pGpEJ->transpose());
auto ppGpEIpEJlam = ppGpEIpEJ->times(lam);
mat->atijplusFullMatrix(iqEI, iqEJ, ppGpEIpEJlam);
mat->atijplusTransposeFullMatrix(iqEJ, iqEI, ppGpEIpEJlam);
mat->atijplusFullMatrixtimes(iqEJ, iqEJ, ppGpEJpEJ, lam);
}