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

runVelIC

Browse Source
This commit is contained in:
Aik-Siong Koh
2023-06-18 01:06:39 -06:00
parent 3b08cd72df
commit 371b13a9e0
147 changed files with 2555 additions and 238 deletions
Download Patch File Download Diff File Expand all files Collapse all files

74
MbDCode/PartFrame.cpp
View File

@@ -64,13 +64,33 @@ FColDsptr PartFrame::getqXdot() {
}
void PartFrame::setomeOpO(FColDsptr omeOpO) {
//qEdot = EulerParametersDot<double>::FromqEOpAndOmegaOpO(qE, omeOpO);
qEdot = EulerParametersDot<double>::FromqEOpAndOmegaOpO(qE, omeOpO);
}
FColDsptr PartFrame::getomeOpO() {
return qE;
}
void MbD::PartFrame::setqXddot(FColDsptr x)
{
qXddot = x;
}
FColDsptr MbD::PartFrame::getqXddot()
{
return qXddot;
}
void MbD::PartFrame::setqEddot(FColDsptr x)
{
qEddot = x;
}
FColDsptr MbD::PartFrame::getqEddot()
{
return qEddot;
}
void PartFrame::setPart(Part* x) {
part = x;
}
@@ -160,6 +180,16 @@ void MbD::PartFrame::prePosIC()
aGabsDo([](std::shared_ptr<Constraint> aGab) { aGab->prePosIC(); });
}
void MbD::PartFrame::prePosKine()
{
iqX = -1;
iqE = -1;
this->calcPostDynCorrectorIteration();
markerFramesDo([](std::shared_ptr<MarkerFrame> markerFrm) { markerFrm->prePosKine(); });
aGeu->prePosKine();
aGabsDo([](std::shared_ptr<Constraint> aGab) { aGab->prePosKine(); });
}
FColDsptr MbD::PartFrame::rOpO()
{
return qX;
@@ -185,6 +215,12 @@ void MbD::PartFrame::fillRedundantConstraints(std::shared_ptr<std::vector<std::s
{
}
void MbD::PartFrame::fillConstraints(std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> allConstraints)
{
aGeu->fillConstraints(aGeu, allConstraints);
aGabsDo([&](std::shared_ptr<Constraint> con) { con->fillConstraints(con, allConstraints); });
}
void MbD::PartFrame::fillqsu(FColDsptr col)
{
col->atiputFullColumn(iqX, qX);
@@ -206,6 +242,18 @@ void MbD::PartFrame::fillqsulam(FColDsptr col)
aGabsDo([&](std::shared_ptr<Constraint> con) { con->fillqsulam(col); });
}
void MbD::PartFrame::fillqsudot(FColDsptr col)
{
col->atiputFullColumn(iqX, qXdot);
col->atiputFullColumn(iqE, qEdot);
markerFramesDo([&](std::shared_ptr<MarkerFrame> markerFrame) { markerFrame->fillqsudot(col); });
}
void MbD::PartFrame::fillqsudotWeights(std::shared_ptr<DiagonalMatrix<double>> diagMat)
{
markerFramesDo([&](std::shared_ptr<MarkerFrame> markerFrame) { markerFrame->fillqsudotWeights(diagMat); });
}
void MbD::PartFrame::useEquationNumbers()
{
markerFramesDo([](std::shared_ptr<MarkerFrame> markerFrame) { markerFrame->useEquationNumbers(); });
@@ -281,9 +329,31 @@ void MbD::PartFrame::preDyn()
aGabsDo([](std::shared_ptr<Constraint> aGab) { aGab->preDyn(); });
}
void MbD::PartFrame::storeDynState()
{
markerFramesDo([](std::shared_ptr<MarkerFrame> markerFrame) { markerFrame->storeDynState(); });
aGeu->storeDynState();
aGabsDo([](std::shared_ptr<Constraint> aGab) { aGab->storeDynState(); });
}
void MbD::PartFrame::fillPosKineError(FColDsptr col)
{
markerFramesDo([&](std::shared_ptr<MarkerFrame> markerFrame) { markerFrame->fillPosKineError(col); });
aGeu->fillPosKineError(col);
aGabsDo([&](std::shared_ptr<Constraint> con) { con->fillPosKineError(col); });
}
void MbD::PartFrame::preVelIC()
{
Item::preVelIC();
markerFramesDo([](std::shared_ptr<MarkerFrame> markerFrame) { markerFrame->preVelIC(); });
aGeu->preVelIC();
aGabsDo([](std::shared_ptr<Constraint> aGab) { aGab->preVelIC(); });
}
void PartFrame::asFixed()
{
for (int i = 0; i < 6; i++) {
for (int i = 0; i < 6; i++) {
auto con = CREATE<AbsConstraint>::With(i);
con->setOwner(this);
aGabs->push_back(con);