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

getPosition3D getQuarternions added

Browse Source
This commit is contained in:
Aik-Siong Koh
2023-10-05 15:10:01 -06:00
parent 9522b62c66
commit 26ca616f53
4 changed files with 26 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
OndselSolver/ASMTSpatialContainer.cpp
View File

@@ -307,14 +307,18 @@ void MbD::ASMTSpatialContainer::updateFromMbD()
auto aAOp = mbdPart->aAOp();
auto vOcmO = mbdPart->qXdot()->times(mbdUnts->velocity);
auto omeOPO = mbdPart->omeOpO()->times(mbdUnts->omega);
omega3D = omeOPO;
auto aOcmO = mbdPart->qXddot()->times(mbdUnts->acceleration);
auto alpOPO = mbdPart->alpOpO()->times(mbdUnts->alpha);
auto& rPcmP = principalMassMarker->position3D;
auto& aAPp = principalMassMarker->rotationMatrix;
auto aAOP = aAOp->timesTransposeFullMatrix(aAPp);
rotationMatrix = aAOP;
auto rPcmO = aAOP->timesFullColumn(rPcmP);
auto rOPO = rOcmO->minusFullColumn(rPcmO);
position3D = rOPO;
auto vOPO = vOcmO->minusFullColumn(omeOPO->cross(rPcmO));
velocity3D = vOPO;
auto aOPO = aOcmO->minusFullColumn(alpOPO->cross(rPcmO))->minusFullColumn(omeOPO->cross(omeOPO->cross(rPcmO)));
xs->push_back(rOPO->at(0));
ys->push_back(rOPO->at(1));