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

18
MbDCode/DispCompIeqcJecKeqc.cpp
View File

@@ -7,7 +7,7 @@ MbD::DispCompIeqcJecKeqc::DispCompIeqcJecKeqc()
{
}
MbD::DispCompIeqcJecKeqc::DispCompIeqcJecKeqc(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, size_t axisk) : DispCompIecJecKeqc(frmi, frmj, frmk, axisk)
MbD::DispCompIeqcJecKeqc::DispCompIeqcJecKeqc(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk) : DispCompIecJecKeqc(frmi, frmj, frmk, axisk)
{
}
@@ -27,39 +27,39 @@ void MbD::DispCompIeqcJecKeqc::calcPostDynCorrectorIteration()
auto frmIqc = std::static_pointer_cast<EndFrameqc>(frmI);
auto mprIeJeOpEIT = frmIqc->prOeOpE->transpose();
auto mpprIeJeOpEIpEI = frmIqc->pprOeOpEpE;
for (size_t i = 0; i < 3; i++)
for (int i = 0; i < 3; i++)
{
priIeJeKepXI->at(i) = 0.0 - (aAjOKe->at(i));
}
for (size_t i = 0; i < 4; i++)
for (int i = 0; i < 4; i++)
{
priIeJeKepEI->at(i) = 0.0 - (aAjOKe->dot(mprIeJeOpEIT->at(i)));
}
for (size_t i = 0; i < 3; i++)
for (int i = 0; i < 3; i++)
{
auto& ppriIeJeKepXIipEK = ppriIeJeKepXIpEK->at(i);
for (size_t j = 0; j < 4; j++)
for (int j = 0; j < 4; j++)
{
ppriIeJeKepXIipEK->at(j) = 0.0 - (pAjOKepEKT->at(j)->at(i));
}
}
for (size_t i = 0; i < 4; i++)
for (int i = 0; i < 4; i++)
{
auto& mpprIeJeOpEIipEI = mpprIeJeOpEIpEI->at(i);
auto& ppriIeJeKepEIipEI = ppriIeJeKepEIpEI->at(i);
ppriIeJeKepEIipEI->at(i) = 0.0 - (aAjOKe->dot(mpprIeJeOpEIipEI->at(i)));
for (size_t j = 0; j < 4; j++)
for (int j = 0; j < 4; j++)
{
auto ppriIeJeKepEIipEIj = 0.0 - (aAjOKe->dot(mpprIeJeOpEIipEI->at(j)));
ppriIeJeKepEIipEI->at(j) = ppriIeJeKepEIipEIj;
ppriIeJeKepEIpEI->at(j)->at(i) = ppriIeJeKepEIipEIj;
}
}
for (size_t i = 0; i < 4; i++)
for (int i = 0; i < 4; i++)
{
auto& mprIeJeOpEITi = mprIeJeOpEIT->at(i);
auto& ppriIeJeKepEIipEK = ppriIeJeKepEIpEK->at(i);
for (size_t j = 0; j < 4; j++)
for (int j = 0; j < 4; j++)
{
ppriIeJeKepEIipEK->at(j) = 0.0 - (pAjOKepEKT->at(j)->dot(mprIeJeOpEITi));
}