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

hides FullMatrix; instances FullMatrixDouble FullMatrixFullMatrixDouble

Browse Source
This commit is contained in:
John Dupuy
2023-11-03 21:16:49 -05:00
parent 8dbb49d032
commit 67bef75bab
46 changed files with 966 additions and 178 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
OndselSolver/ASMTAssembly.cpp
View File

@@ -997,7 +997,7 @@ void MbD::ASMTAssembly::initprincipalMassMarker()
principalMassMarker->density = 0.0;
principalMassMarker->momentOfInertias = std::make_shared<DiagonalMatrix<double>>(3, 0);
//principalMassMarker->position3D = std::make_shared<FullColumn<double>>(3, 0);
//principalMassMarker->rotationMatrix = FullMatrix<double>>::identitysptr(3);
//principalMassMarker->rotationMatrix = FullMatrixDouble>::identitysptr(3);
}
std::shared_ptr<ASMTSpatialContainer> MbD::ASMTAssembly::spatialContainerAt(std::shared_ptr<ASMTAssembly> self, std::string& longname)

2
OndselSolver/ASMTMarker.cpp
View File

@@ -47,7 +47,7 @@ FMatDsptr MbD::ASMTMarker::aApm()
auto& principalMassMarker = static_cast<ASMTPart*>(refItem->owner)->principalMassMarker;
auto& aAPcm = principalMassMarker->rotationMatrix;
auto aApm = aAPcm->transposeTimesFullMatrix(aAPref->timesFullMatrix(aArefm));
return aApm;
return toFMDsptr(aApm);
}
void MbD::ASMTMarker::createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits)

38
OndselSolver/CADSystem.cpp
View File

@@ -295,7 +295,7 @@ void CADSystem::runOndselDoublePendulum()
auto marker2 = CREATE<MarkerFrame>::With("/Assembly1/Part1/Marker2");
rpmp = std::make_shared<FullColumn<double>>(ListD{ 0.4, 0.0, 0.05 });
marker2->setrpmp(rpmp);
aApm = std::make_shared<FullMatrix<double>>(ListListD{
aApm = std::make_shared<FullMatrixDouble>(ListListD{
{ 1, 0, 0 },
{ 0, 1, 0 },
{ 0, 0, 1 }
@@ -325,7 +325,7 @@ void CADSystem::runOndselDoublePendulum()
auto marker1 = CREATE<MarkerFrame>::With("/Assembly1/Part2/Marker1");
rpmp = std::make_shared<FullColumn<double>>(ListD{ -0.65, 0.0, -0.05 });
marker1->setrpmp(rpmp);
aApm = std::make_shared<FullMatrix<double>>(ListListD{
aApm = std::make_shared<FullMatrixDouble>(ListListD{
{1.0, 0.0, 0.0},
{0.0, 1.0, 0.0},
{0.0, 0.0, 1.0}
@@ -336,7 +336,7 @@ void CADSystem::runOndselDoublePendulum()
auto marker2 = CREATE<MarkerFrame>::With("/Assembly1/Part2/Marker2");
rpmp = std::make_shared<FullColumn<double>>(ListD{ 0.65, 0.0, -0.05 });
marker2->setrpmp(rpmp);
aApm = std::make_shared<FullMatrix<double>>(ListListD{
aApm = std::make_shared<FullMatrixDouble>(ListListD{
{1.0, 0.0, 0.0},
{0.0, 1.0, 0.0},
{0.0, 0.0, 1.0}
@@ -417,7 +417,7 @@ void CADSystem::runOndselPiston()
auto marker2 = CREATE<MarkerFrame>::With("/Assembly1/Marker2");
rpmp = std::make_shared<FullColumn<double>>(ListD{ 0.0, 0.0, 0.0 });
marker2->setrpmp(rpmp);
aApm = std::make_shared<FullMatrix<double>>(ListListD{
aApm = std::make_shared<FullMatrixDouble>(ListListD{
{ 1, 0, 0 },
{ 0, 1, 0 },
{ 0, 0, 1 }
@@ -428,7 +428,7 @@ void CADSystem::runOndselPiston()
auto marker1 = CREATE<MarkerFrame>::With("/Assembly1/Marker1");
rpmp = std::make_shared<FullColumn<double>>(ListD{ 0.0, 3.0, 0.0 });
marker1->setrpmp(rpmp);
aApm = std::make_shared<FullMatrix<double>>(ListListD{
aApm = std::make_shared<FullMatrixDouble>(ListListD{
{ 1, 0, 0 },
{ 0, 0, 1 },
{ 0, -1, 0 }
@@ -460,7 +460,7 @@ void CADSystem::runOndselPiston()
auto marker1 = CREATE<MarkerFrame>::With("/Assembly1/Part1/Marker1");
rpmp = std::make_shared<FullColumn<double>>(ListD{ -0.4, 0.0, 0.05 });
marker1->setrpmp(rpmp);
aApm = std::make_shared<FullMatrix<double>>(ListListD{
aApm = std::make_shared<FullMatrixDouble>(ListListD{
{ 1, 0, 0 },
{ 0, 1, 0 },
{ 0, 0, 1 }
@@ -471,7 +471,7 @@ void CADSystem::runOndselPiston()
auto marker2 = CREATE<MarkerFrame>::With("/Assembly1/Part1/Marker2");
rpmp = std::make_shared<FullColumn<double>>(ListD{ 0.4, 0.0, 0.05 });
marker2->setrpmp(rpmp);
aApm = std::make_shared<FullMatrix<double>>(ListListD{
aApm = std::make_shared<FullMatrixDouble>(ListListD{
{ 1, 0, 0 },
{ 0, 1, 0 },
{ 0, 0, 1 }
@@ -502,7 +502,7 @@ void CADSystem::runOndselPiston()
auto marker1 = CREATE<MarkerFrame>::With("/Assembly1/Part2/Marker1");
rpmp = std::make_shared<FullColumn<double>>(ListD{ -0.65, 0.0, -0.05 });
marker1->setrpmp(rpmp);
aApm = std::make_shared<FullMatrix<double>>(ListListD{
aApm = std::make_shared<FullMatrixDouble>(ListListD{
{1.0, 0.0, 0.0},
{0.0, 1.0, 0.0},
{0.0, 0.0, 1.0}
@@ -513,7 +513,7 @@ void CADSystem::runOndselPiston()
auto marker2 = CREATE<MarkerFrame>::With("/Assembly1/Part2/Marker2");
rpmp = std::make_shared<FullColumn<double>>(ListD{ 0.65, 0.0, -0.05 });
marker2->setrpmp(rpmp);
aApm = std::make_shared<FullMatrix<double>>(ListListD{
aApm = std::make_shared<FullMatrixDouble>(ListListD{
{1.0, 0.0, 0.0},
{0.0, 1.0, 0.0},
{0.0, 0.0, 1.0}
@@ -544,7 +544,7 @@ void CADSystem::runOndselPiston()
auto marker1 = CREATE<MarkerFrame>::With("/Assembly1/Part3/Marker1");
rpmp = std::make_shared<FullColumn<double>>(ListD{ -0.5, 0.0, 0.0 });
marker1->setrpmp(rpmp);
aApm = std::make_shared<FullMatrix<double>>(ListListD{
aApm = std::make_shared<FullMatrixDouble>(ListListD{
{0.0, 1.0, 0.0},
{1.0, 0.0, 0.0},
{0.0, 0.0, -1.0}
@@ -555,7 +555,7 @@ void CADSystem::runOndselPiston()
auto marker2 = CREATE<MarkerFrame>::With("/Assembly1/Part3/Marker2");
rpmp = std::make_shared<FullColumn<double>>(ListD{ 0.5, 0.0, 0.0 });
marker2->setrpmp(rpmp);
aApm = std::make_shared<FullMatrix<double>>(ListListD{
aApm = std::make_shared<FullMatrixDouble>(ListListD{
{0.0, 0.0, 1.0},
{1.0, 0.0, 0.0},
{0.0, 1.0, 0.0}
@@ -654,7 +654,7 @@ void CADSystem::runPiston()
auto marker2 = CREATE<MarkerFrame>::With("/Assembly1/Marker2");
rpmp = std::make_shared<FullColumn<double>>(ListD{ 0.0, 0.0, 0.0 });
marker2->setrpmp(rpmp);
aApm = std::make_shared<FullMatrix<double>>(ListListD{
aApm = std::make_shared<FullMatrixDouble>(ListListD{
{ 1, 0, 0 },
{ 0, 1, 0 },
{ 0, 0, 1 }
@@ -665,7 +665,7 @@ void CADSystem::runPiston()
auto marker1 = CREATE<MarkerFrame>::With("/Assembly1/Marker1");
rpmp = std::make_shared<FullColumn<double>>(ListD{ 0.0, 2.8817526385684, 0.0 });
marker1->setrpmp(rpmp);
aApm = std::make_shared<FullMatrix<double>>(ListListD{
aApm = std::make_shared<FullMatrixDouble>(ListListD{
{ 1, 0, 0 },
{ 0, 0, 1 },
{ 0, -1, 0 }
@@ -697,7 +697,7 @@ void CADSystem::runPiston()
auto marker1 = CREATE<MarkerFrame>::With("/Assembly1/Part1/Marker1");
rpmp = std::make_shared<FullColumn<double>>(ListD{ -0.38423368514246, -2.6661567755108e-17, 0.048029210642807 });
marker1->setrpmp(rpmp);
aApm = std::make_shared<FullMatrix<double>>(ListListD{
aApm = std::make_shared<FullMatrixDouble>(ListListD{
{ 1, 0, 0 },
{ 0, 1, 0 },
{ 0, 0, 1 }
@@ -708,7 +708,7 @@ void CADSystem::runPiston()
auto marker2 = CREATE<MarkerFrame>::With("/Assembly1/Part1/Marker2");
rpmp = std::make_shared<FullColumn<double>>(ListD{ 0.38423368514246, -2.6661567755108e-17, 0.048029210642807 });
marker2->setrpmp(rpmp);
aApm = std::make_shared<FullMatrix<double>>(ListListD{
aApm = std::make_shared<FullMatrixDouble>(ListListD{
{ 1, 0, 0 },
{ 0, 1, 0 },
{ 0, 0, 1 }
@@ -739,7 +739,7 @@ void CADSystem::runPiston()
auto marker1 = CREATE<MarkerFrame>::With("/Assembly1/Part2/Marker1");
rpmp = std::make_shared<FullColumn<double>>(ListD{ -0.6243797383565, 1.1997705489799e-16, -0.048029210642807 });
marker1->setrpmp(rpmp);
aApm = std::make_shared<FullMatrix<double>>(ListListD{
aApm = std::make_shared<FullMatrixDouble>(ListListD{
{1.0, 2.7755575615629e-16, 0.0},
{-2.7755575615629e-16, 1.0, 0.0},
{0.0, 0.0, 1.0}
@@ -750,7 +750,7 @@ void CADSystem::runPiston()
auto marker2 = CREATE<MarkerFrame>::With("/Assembly1/Part2/Marker2");
rpmp = std::make_shared<FullColumn<double>>(ListD{ 0.6243797383565, -2.1329254204087e-16, -0.048029210642807 });
marker2->setrpmp(rpmp);
aApm = std::make_shared<FullMatrix<double>>(ListListD{
aApm = std::make_shared<FullMatrixDouble>(ListListD{
{1.0, 2.4980018054066e-16, 2.2204460492503e-16},
{-2.4980018054066e-16, 1.0, 4.1633363423443e-17},
{-2.2204460492503e-16, -4.1633363423443e-17, 1.0}
@@ -781,7 +781,7 @@ void CADSystem::runPiston()
auto marker1 = CREATE<MarkerFrame>::With("/Assembly1/Part3/Marker1");
rpmp = std::make_shared<FullColumn<double>>(ListD{ -0.48029210642807, 7.6201599718927e-18, -2.816737703896e-17 });
marker1->setrpmp(rpmp);
aApm = std::make_shared<FullMatrix<double>>(ListListD{
aApm = std::make_shared<FullMatrixDouble>(ListListD{
{9.2444637330587e-33, 1.0, 2.2204460492503e-16},
{1.0, -9.2444637330587e-33, -1.0785207688569e-32},
{-1.0785207688569e-32, 2.2204460492503e-16, -1.0}
@@ -792,7 +792,7 @@ void CADSystem::runPiston()
auto marker2 = CREATE<MarkerFrame>::With("/Assembly1/Part3/Marker2");
rpmp = std::make_shared<FullColumn<double>>(ListD{ 0.48029210642807, 1.7618247880058e-17, 2.5155758471256e-17 });
marker2->setrpmp(rpmp);
aApm = std::make_shared<FullMatrix<double>>(ListListD{
aApm = std::make_shared<FullMatrixDouble>(ListListD{
{6.9388939039072e-18, -6.4146353042213e-50, 1.0},
{1.0, -6.9388939039072e-18, 6.9388939039072e-18},
{-6.9388939039072e-18, 1.0, -7.4837411882581e-50}

2
OndselSolver/ConstVelConstraintIqcJc.cpp
View File

@@ -97,7 +97,7 @@ void MbD::ConstVelConstraintIqcJc::initialize()
{
ConstVelConstraintIJ::initialize();
pGpEI = std::make_shared<FullRow<double>>(4);
ppGpEIpEI = std::make_shared<FullMatrix<double>>(4, 4);
ppGpEIpEI = std::make_shared<FullMatrixDouble>(4, 4);
}
void MbD::ConstVelConstraintIqcJc::useEquationNumbers()

2
OndselSolver/DirectionCosineIeqcJeqc.cpp
View File

@@ -25,7 +25,7 @@ void DirectionCosineIeqcJeqc::initialize()
DirectionCosineIeqcJec::initialize();
pAijIeJepEJ = std::make_shared<FullRow<double>>(4);
ppAijIeJepEIpEJ = std::make_shared<FullMatrixDouble>(4, 4);
ppAijIeJepEJpEJ = std::make_shared<FullMatrix<double>>(4, 4);
ppAijIeJepEJpEJ = std::make_shared<FullMatrixDouble>(4, 4);
}
void DirectionCosineIeqcJeqc::initializeGlobally()

2
OndselSolver/DispCompIecJecKeqc.cpp
View File

@@ -22,7 +22,7 @@ DispCompIecJecKeqc::DispCompIecJecKeqc(EndFrmsptr frmi, EndFrmsptr frmj, EndFrms
void DispCompIecJecKeqc::initialize()
{
priIeJeKepEK = std::make_shared<FullRow<double>>(4);
ppriIeJeKepEKpEK = std::make_shared<FullMatrix<double>>(4, 4);
ppriIeJeKepEKpEK = std::make_shared<FullMatrixDouble>(4, 4);
}
void DispCompIecJecKeqc::initializeGlobally()

2
OndselSolver/DispCompIeqcJecO.cpp
View File

@@ -23,7 +23,7 @@ void DispCompIeqcJecO::initializeGlobally()
{
priIeJeOpXI = std::make_shared<FullRow<double>>(3, 0.0);
priIeJeOpXI->at(axis) = -1.0;
ppriIeJeOpEIpEI = std::static_pointer_cast<EndFrameqc>(frmI)->ppriOeOpEpE(axis)->negated();
ppriIeJeOpEIpEI = toFMDsptr(std::static_pointer_cast<EndFrameqc>(frmI)->ppriOeOpEpE(axis)->negated());
}
FMatDsptr MbD::DispCompIeqcJecO::ppvaluepEIpEI()

2
OndselSolver/DispCompIeqcJeqcIe.cpp
View File

@@ -23,7 +23,7 @@ void MbD::DispCompIeqcJeqcIe::calc_ppvaluepEIpEJ()
{
auto frmJeqc = std::static_pointer_cast<EndFrameqc>(frmJ);
auto& prIeJeOpEJ = frmJeqc->prOeOpE;
ppriIeJeIepEIpEJ = pAjOIepEIT->timesFullMatrix(prIeJeOpEJ);
ppriIeJeIepEIpEJ = toFMDsptr(pAjOIepEIT->timesFullMatrix(prIeJeOpEJ));
}
void MbD::DispCompIeqcJeqcIe::calc_ppvaluepEIpXJ()

2
OndselSolver/DispCompIeqctJeqcO.cpp
View File

@@ -39,7 +39,7 @@ void DispCompIeqctJeqcO::calcPostDynCorrectorIteration()
{
//"ppriIeJeOpEIpEI is not a constant now."
DispCompIeqcJeqcO::calcPostDynCorrectorIteration();
ppriIeJeOpEIpEI = std::static_pointer_cast<EndFrameqct>(frmI)->ppriOeOpEpE(axis)->negated();
ppriIeJeOpEIpEI = toFMDsptr(std::static_pointer_cast<EndFrameqct>(frmI)->ppriOeOpEpE(axis)->negated());
}
void DispCompIeqctJeqcO::preVelIC()

14
OndselSolver/DistancexyConstraintIqcJc.cpp
View File

@@ -65,18 +65,18 @@ void MbD::DistancexyConstraintIqcJc::calc_pGpXI()
void MbD::DistancexyConstraintIqcJc::calc_ppGpEIpEI()
{
ppGpEIpEI = (xIeJeIe->pvaluepEI()->transposeTimesFullRow(xIeJeIe->pvaluepEI()));
ppGpEIpEI = ppGpEIpEI->plusFullMatrix(xIeJeIe->ppvaluepEIpEI()->times(xIeJeIe->value()));
ppGpEIpEI = ppGpEIpEI->plusFullMatrix(yIeJeIe->pvaluepEI()->transposeTimesFullRow(yIeJeIe->pvaluepEI()));
ppGpEIpEI = ppGpEIpEI->plusFullMatrix(yIeJeIe->ppvaluepEIpEI()->times(yIeJeIe->value()));
ppGpEIpEI = toFMDsptr(ppGpEIpEI->plusFullMatrix(xIeJeIe->ppvaluepEIpEI()->times(xIeJeIe->value())));
ppGpEIpEI = toFMDsptr(ppGpEIpEI->plusFullMatrix(yIeJeIe->pvaluepEI()->transposeTimesFullRow(yIeJeIe->pvaluepEI())));
ppGpEIpEI = toFMDsptr(ppGpEIpEI->plusFullMatrix(yIeJeIe->ppvaluepEIpEI()->times(yIeJeIe->value())));
ppGpEIpEI->magnifySelf(2.0);
}
void MbD::DistancexyConstraintIqcJc::calc_ppGpXIpEI()
{
ppGpXIpEI = (xIeJeIe->pvaluepXI()->transposeTimesFullRow(xIeJeIe->pvaluepEI()));
ppGpXIpEI = ppGpXIpEI->plusFullMatrix(xIeJeIe->ppvaluepXIpEI()->times(xIeJeIe->value()));
ppGpXIpEI = ppGpXIpEI->plusFullMatrix(yIeJeIe->pvaluepXI()->transposeTimesFullRow(yIeJeIe->pvaluepEI()));
ppGpXIpEI = ppGpXIpEI->plusFullMatrix(yIeJeIe->ppvaluepXIpEI()->times(yIeJeIe->value()));
ppGpXIpEI = toFMDsptr(ppGpXIpEI->plusFullMatrix(xIeJeIe->ppvaluepXIpEI()->times(xIeJeIe->value())));
ppGpXIpEI = toFMDsptr(ppGpXIpEI->plusFullMatrix(yIeJeIe->pvaluepXI()->transposeTimesFullRow(yIeJeIe->pvaluepEI())));
ppGpXIpEI = toFMDsptr(ppGpXIpEI->plusFullMatrix(yIeJeIe->ppvaluepXIpEI()->times(yIeJeIe->value())));
ppGpXIpEI->magnifySelf(2.0);
}
@@ -85,7 +85,7 @@ void MbD::DistancexyConstraintIqcJc::calc_ppGpXIpXI()
//xIeJeIe ppvaluepXIpXI = 0
//yIeJeIe ppvaluepXIpXI = 0
ppGpXIpXI = (xIeJeIe->pvaluepXI()->transposeTimesFullRow(xIeJeIe->pvaluepXI()));
ppGpXIpXI = ppGpXIpXI->plusFullMatrix(yIeJeIe->pvaluepXI()->transposeTimesFullRow(yIeJeIe->pvaluepXI()));
ppGpXIpXI = toFMDsptr(ppGpXIpXI->plusFullMatrix(yIeJeIe->pvaluepXI()->transposeTimesFullRow(yIeJeIe->pvaluepXI())));
ppGpXIpXI->magnifySelf(2.0);
}

26
OndselSolver/DistancexyConstraintIqcJqc.cpp
View File

@@ -30,16 +30,16 @@ void MbD::DistancexyConstraintIqcJqc::calc_ppGpXIpXJ()
//xIeJeIe ppvaluepXIpXJ = 0
//yIeJeIe ppvaluepXIpXJ = 0
ppGpXIpXJ = (xIeJeIe->pvaluepXI()->transposeTimesFullRow(xIeJeIe->pvaluepXJ()));
ppGpXIpXJ = ppGpXIpXJ->plusFullMatrix(yIeJeIe->pvaluepXI()->transposeTimesFullRow(yIeJeIe->pvaluepXJ()));
ppGpXIpXJ = toFMDsptr(ppGpXIpXJ->plusFullMatrix(yIeJeIe->pvaluepXI()->transposeTimesFullRow(yIeJeIe->pvaluepXJ())));
ppGpXIpXJ->magnifySelf(2.0);
}
void MbD::DistancexyConstraintIqcJqc::calc_ppGpEIpXJ()
{
ppGpEIpXJ = (xIeJeIe->pvaluepEI()->transposeTimesFullRow(xIeJeIe->pvaluepXJ()));
ppGpEIpXJ = ppGpEIpXJ->plusFullMatrix(xIeJeIe->ppvaluepEIpXJ()->times(xIeJeIe->value()));
ppGpEIpXJ = ppGpEIpXJ->plusFullMatrix(yIeJeIe->pvaluepEI()->transposeTimesFullRow(yIeJeIe->pvaluepXJ()));
ppGpEIpXJ = ppGpEIpXJ->plusFullMatrix(yIeJeIe->ppvaluepEIpXJ()->times(yIeJeIe->value()));
ppGpEIpXJ = toFMDsptr(ppGpEIpXJ->plusFullMatrix(xIeJeIe->ppvaluepEIpXJ()->times(xIeJeIe->value())));
ppGpEIpXJ = toFMDsptr(ppGpEIpXJ->plusFullMatrix(yIeJeIe->pvaluepEI()->transposeTimesFullRow(yIeJeIe->pvaluepXJ())));
ppGpEIpXJ = toFMDsptr(ppGpEIpXJ->plusFullMatrix(yIeJeIe->ppvaluepEIpXJ()->times(yIeJeIe->value())));
ppGpEIpXJ->magnifySelf(2.0);
}
@@ -48,7 +48,7 @@ void MbD::DistancexyConstraintIqcJqc::calc_ppGpXJpXJ()
//xIeJeIe ppvaluepXJpXJ = 0
//yIeJeIe ppvaluepXJpXJ = 0
ppGpXJpXJ = (xIeJeIe->pvaluepXJ()->transposeTimesFullRow(xIeJeIe->pvaluepXJ()));
ppGpXJpXJ = ppGpXJpXJ->plusFullMatrix(yIeJeIe->pvaluepXJ()->transposeTimesFullRow(yIeJeIe->pvaluepXJ()));
ppGpXJpXJ = toFMDsptr(ppGpXJpXJ->plusFullMatrix(yIeJeIe->pvaluepXJ()->transposeTimesFullRow(yIeJeIe->pvaluepXJ())));
ppGpXJpXJ->magnifySelf(2.0);
}
@@ -57,16 +57,16 @@ void MbD::DistancexyConstraintIqcJqc::calc_ppGpXIpEJ()
//xIeJeIe ppvaluepXIpEJ = 0
//yIeJeIe ppvaluepXIpEJ = 0
ppGpXIpEJ = (xIeJeIe->pvaluepXI()->transposeTimesFullRow(xIeJeIe->pvaluepEJ()));
ppGpXIpEJ = ppGpXIpEJ->plusFullMatrix(yIeJeIe->pvaluepXI()->transposeTimesFullRow(yIeJeIe->pvaluepEJ()));
ppGpXIpEJ = toFMDsptr(ppGpXIpEJ->plusFullMatrix(yIeJeIe->pvaluepXI()->transposeTimesFullRow(yIeJeIe->pvaluepEJ())));
ppGpXIpEJ->magnifySelf(2.0);
}
void MbD::DistancexyConstraintIqcJqc::calc_ppGpEIpEJ()
{
ppGpEIpEJ = (xIeJeIe->pvaluepEI()->transposeTimesFullRow(xIeJeIe->pvaluepEJ()));
ppGpEIpEJ = ppGpEIpEJ->plusFullMatrix(xIeJeIe->ppvaluepEIpEJ()->times(xIeJeIe->value()));
ppGpEIpEJ = ppGpEIpEJ->plusFullMatrix(yIeJeIe->pvaluepEI()->transposeTimesFullRow(yIeJeIe->pvaluepEJ()));
ppGpEIpEJ = ppGpEIpEJ->plusFullMatrix(yIeJeIe->ppvaluepEIpEJ()->times(yIeJeIe->value()));
ppGpEIpEJ = toFMDsptr(ppGpEIpEJ->plusFullMatrix(xIeJeIe->ppvaluepEIpEJ()->times(xIeJeIe->value())));
ppGpEIpEJ = toFMDsptr(ppGpEIpEJ->plusFullMatrix(yIeJeIe->pvaluepEI()->transposeTimesFullRow(yIeJeIe->pvaluepEJ())));
ppGpEIpEJ = toFMDsptr(ppGpEIpEJ->plusFullMatrix(yIeJeIe->ppvaluepEIpEJ()->times(yIeJeIe->value())));
ppGpEIpEJ->magnifySelf(2.0);
}
@@ -75,16 +75,16 @@ void MbD::DistancexyConstraintIqcJqc::calc_ppGpXJpEJ()
//xIeJeIe ppvaluepXJpEJ = 0
//yIeJeIe ppvaluepXJpEJ = 0
ppGpXJpEJ = (xIeJeIe->pvaluepXJ()->transposeTimesFullRow(xIeJeIe->pvaluepEJ()));
ppGpXJpEJ = ppGpXJpEJ->plusFullMatrix(yIeJeIe->pvaluepXJ()->transposeTimesFullRow(yIeJeIe->pvaluepEJ()));
ppGpXJpEJ = toFMDsptr(ppGpXJpEJ->plusFullMatrix(yIeJeIe->pvaluepXJ()->transposeTimesFullRow(yIeJeIe->pvaluepEJ())));
ppGpXJpEJ->magnifySelf(2.0);
}
void MbD::DistancexyConstraintIqcJqc::calc_ppGpEJpEJ()
{
ppGpEJpEJ = (xIeJeIe->pvaluepEJ()->transposeTimesFullRow(xIeJeIe->pvaluepEJ()));
ppGpEJpEJ = ppGpEJpEJ->plusFullMatrix(xIeJeIe->ppvaluepEJpEJ()->times(xIeJeIe->value()));
ppGpEJpEJ = ppGpEJpEJ->plusFullMatrix(yIeJeIe->pvaluepEJ()->transposeTimesFullRow(yIeJeIe->pvaluepEJ()));
ppGpEJpEJ = ppGpEJpEJ->plusFullMatrix(yIeJeIe->ppvaluepEJpEJ()->times(yIeJeIe->value()));
ppGpEJpEJ = toFMDsptr(ppGpEJpEJ->plusFullMatrix(xIeJeIe->ppvaluepEJpEJ()->times(xIeJeIe->value())));
ppGpEJpEJ = toFMDsptr(ppGpEJpEJ->plusFullMatrix(yIeJeIe->pvaluepEJ()->transposeTimesFullRow(yIeJeIe->pvaluepEJ())));
ppGpEJpEJ = toFMDsptr(ppGpEJpEJ->plusFullMatrix(yIeJeIe->ppvaluepEJpEJ()->times(yIeJeIe->value())));
ppGpEJpEJ->magnifySelf(2.0);
}

14
OndselSolver/DistxyIeqcJeqc.cpp
View File

@@ -293,13 +293,13 @@ void MbD::DistxyIeqcJeqc::initialize()
DistxyIeqcJec::initialize();
pdistxypXJ = std::make_shared<FullRow<double>>(3);
pdistxypEJ = std::make_shared<FullRow<double>>(4);
ppdistxypXIpXJ = std::make_shared<FullMatrix<double>>(3, 3);
ppdistxypXIpEJ = std::make_shared<FullMatrix<double>>(3, 4);
ppdistxypEIpXJ = std::make_shared<FullMatrix<double>>(4, 3);
ppdistxypEIpEJ = std::make_shared<FullMatrix<double>>(4, 4);
ppdistxypXJpXJ = std::make_shared<FullMatrix<double>>(3, 3);
ppdistxypXJpEJ = std::make_shared<FullMatrix<double>>(3, 4);
ppdistxypEJpEJ = std::make_shared<FullMatrix<double>>(4, 4);
ppdistxypXIpXJ = std::make_shared<FullMatrixDouble>(3, 3);
ppdistxypXIpEJ = std::make_shared<FullMatrixDouble>(3, 4);
ppdistxypEIpXJ = std::make_shared<FullMatrixDouble>(4, 3);
ppdistxypEIpEJ = std::make_shared<FullMatrixDouble>(4, 4);
ppdistxypXJpXJ = std::make_shared<FullMatrixDouble>(3, 3);
ppdistxypXJpEJ = std::make_shared<FullMatrixDouble>(3, 4);
ppdistxypEJpEJ = std::make_shared<FullMatrixDouble>(4, 4);
}
FMatDsptr MbD::DistxyIeqcJeqc::ppvaluepEIpEJ()

2
OndselSolver/EndFramec.cpp
View File

@@ -24,7 +24,7 @@ EndFramec::EndFramec(const char* str) : CartesianFrame(str) {
FMatDsptr MbD::EndFramec::aAeO()
{
return aAOe->transpose();
return toFMDsptr(aAOe->transpose());
}
System* EndFramec::root()

2
OndselSolver/EndFrameqc.cpp
View File

@@ -28,7 +28,7 @@ void EndFrameqc::initialize()
prOeOpE = std::make_shared<FullMatrixDouble>(3, 4);
pprOeOpEpE = std::make_shared<FullMatrix<FColDsptr>>(4, 4);
pAOepE = std::make_shared<FullColumn<FMatDsptr>>(4);
ppAOepEpE = std::make_shared<FullMatrix<FMatDsptr>>(4, 4);
ppAOepEpE = std::make_shared<FullMatrixFullMatrixDouble>(4, 4);
}
void EndFrameqc::initializeGlobally()

26
OndselSolver/EndFrameqct.cpp
View File

@@ -32,12 +32,12 @@ void EndFrameqct::initialize()
prmempt = std::make_shared<FullColumn<double>>(3);
pprmemptpt = std::make_shared<FullColumn<double>>(3);
aAme = FullMatrixDouble::identitysptr(3);
pAmept = std::make_shared<FullMatrix<double>>(3, 3);
ppAmeptpt = std::make_shared<FullMatrix<double>>(3, 3);
pprOeOpEpt = std::make_shared<FullMatrix<double>>(3, 4);
pAmept = std::make_shared<FullMatrixDouble>(3, 3);
ppAmeptpt = std::make_shared<FullMatrixDouble>(3, 3);
pprOeOpEpt = std::make_shared<FullMatrixDouble>(3, 4);
pprOeOptpt = std::make_shared<FullColumn<double>>(3);
ppAOepEpt = std::make_shared<FullColumn<FMatDsptr>>(4);
ppAOeptpt = std::make_shared<FullMatrix<double>>(3, 3);
ppAOeptpt = std::make_shared<FullMatrixDouble>(3, 3);
}
void EndFrameqct::initializeLocally()
@@ -142,12 +142,12 @@ void EndFrameqct::calcPostDynCorrectorIteration()
}
auto rpep = markerFrame->rpmp->plusFullColumn(markerFrame->aApm->timesFullColumn(rmem));
pprOeOpEpE = EulerParameters<double>::ppApEpEtimesColumn(rpep);
aAOe = aAOm->timesFullMatrix(aAme);
aAOe = toFMDsptr(aAOm->timesFullMatrix(aAme));
for (int i = 0; i < 4; i++)
{
pAOepE->at(i) = pAOmpE->at(i)->timesFullMatrix(aAme);
pAOepE->at(i) = toFMDsptr(pAOmpE->at(i)->timesFullMatrix(aAme));
}
auto aApe = markerFrame->aApm->timesFullMatrix(aAme);
auto aApe = toFMDsptr(markerFrame->aApm->timesFullMatrix(aAme));
ppAOepEpE = EulerParameters<double>::ppApEpEtimesMatrix(aApe);
}
@@ -196,7 +196,7 @@ void EndFrameqct::preVelIC()
this->evalpAmept();
auto& aAOm = markerFrame->aAOm;
prOeOpt = aAOm->timesFullColumn(prmempt);
pAOept = aAOm->timesFullMatrix(pAmept);
pAOept = toFMDsptr(aAOm->timesFullMatrix(pAmept));
}
void EndFrameqct::postVelIC()
@@ -213,7 +213,7 @@ void EndFrameqct::postVelIC()
}
for (int i = 0; i < 4; i++)
{
ppAOepEpt->atiput(i, pAOmpE->at(i)->timesFullMatrix(pAmept));
ppAOepEpt->atiput(i, toFMDsptr(pAOmpE->at(i)->timesFullMatrix(pAmept)));
}
}
@@ -224,7 +224,7 @@ FColDsptr EndFrameqct::pAjOept(int j)
FMatDsptr EndFrameqct::ppAjOepETpt(int jj)
{
auto answer = std::make_shared<FullMatrix<double>>(4, 3);
auto answer = std::make_shared<FullMatrixDouble>(4, 3);
for (int i = 0; i < 4; i++)
{
auto& answeri = answer->at(i);
@@ -326,7 +326,7 @@ void EndFrameqct::evalppAmeptpt()
phiThePsi->calc();
phiThePsiDot->calc();
phiThePsiDDot->calc();
ppAmeptpt = phiThePsiDDot->aAddot;
ppAmeptpt = toFMDsptr(phiThePsiDDot->aAddot);
}
}
@@ -353,11 +353,11 @@ void EndFrameqct::preAccIC()
this->evalpAmept();
auto& aAOm = markerFrame->aAOm;
prOeOpt = aAOm->timesFullColumn(prmempt);
pAOept = aAOm->timesFullMatrix(pAmept);
pAOept = toFMDsptr(aAOm->timesFullMatrix(pAmept));
Item::preAccIC();
this->evalpprmemptpt();
this->evalppAmeptpt();
aAOm = markerFrame->aAOm;
pprOeOptpt = aAOm->timesFullColumn(pprmemptpt);
ppAOeptpt = aAOm->timesFullMatrix(ppAmeptpt);
ppAOeptpt = toFMDsptr(aAOm->timesFullMatrix(ppAmeptpt));
}

16
OndselSolver/EulerAngles.h
View File

@@ -51,19 +51,19 @@ namespace MbD {
auto axis = rotOrder->at(i);
auto angle = this->at(i)->getValue();
if (axis == 1) {
cA->atiput(i, FullMatrixDouble::rotatex(angle));
cA->atiput(i, toFMDsptr(FullMatrixDouble::rotatex(angle)));
}
else if (axis == 2) {
cA->atiput(i, FullMatrixDouble::rotatey(angle));
cA->atiput(i, toFMDsptr(FullMatrixDouble::rotatey(angle)));
}
else if (axis == 3) {
cA->atiput(i, FullMatrixDouble::rotatez(angle));
cA->atiput(i, toFMDsptr(FullMatrixDouble::rotatez(angle)));
}
else {
throw std::runtime_error("Euler angle rotation order must be any permutation of 1,2,3 without consecutive repeats.");
}
}
aA = cA->at(0)->timesFullMatrix(cA->at(1)->timesFullMatrix(cA->at(2)));
aA = toFMDsptr(cA->at(0)->timesFullMatrix(cA->at(1)->timesFullMatrix(cA->at(2))));
}
template<>
inline void EulerAngles<double>::calc()
@@ -74,19 +74,19 @@ namespace MbD {
auto axis = rotOrder->at(i);
auto angle = this->at(i);
if (axis == 1) {
cA->atiput(i, FullMatrixDouble::rotatex(angle));
cA->atiput(i, toFMDsptr(FullMatrixDouble::rotatex(angle)));
}
else if (axis == 2) {
cA->atiput(i, FullMatrixDouble::rotatey(angle));
cA->atiput(i, toFMDsptr(FullMatrixDouble::rotatey(angle)));
}
else if (axis == 3) {
cA->atiput(i, FullMatrix<double>::rotatez(angle));
cA->atiput(i, toFMDsptr(FullMatrixDouble::rotatez(angle)));
}
else {
throw std::runtime_error("Euler angle rotation order must be any permutation of 1,2,3 without consecutive repeats.");
}
}
aA = cA->at(0)->timesFullMatrix(cA->at(1)->timesFullMatrix(cA->at(2)));
aA = toFMDsptr(cA->at(0)->timesFullMatrix(cA->at(1)->timesFullMatrix(cA->at(2))));
}
template<typename T>
inline void EulerAngles<T>::calc()

12
OndselSolver/EulerAnglesDDot.h
View File

@@ -47,13 +47,13 @@ namespace MbD {
auto angleDot = aEulerAnglesDot->at(i)->getValue();
auto angleDDot = this->at(i)->getValue();
if (axis == 1) {
cAddot->atiput(i, FullMatrixDouble::rotatexrotDotrotDDot(angle, angleDot, angleDDot));
cAddot->atiput(i, toFMDsptr(FullMatrixDouble::rotatexrotDotrotDDot(angle, angleDot, angleDDot)));
}
else if (axis == 2) {
cAddot->atiput(i, FullMatrixDouble::rotateyrotDotrotDDot(angle, angleDot, angleDDot));
cAddot->atiput(i, toFMDsptr(FullMatrixDouble::rotateyrotDotrotDDot(angle, angleDot, angleDDot)));
}
else if (axis == 3) {
cAddot->atiput(i, FullMatrixDouble::rotatezrotDotrotDDot(angle, angleDot, angleDDot));
cAddot->atiput(i, toFMDsptr(FullMatrixDouble::rotatezrotDotrotDDot(angle, angleDot, angleDDot)));
}
else {
throw std::runtime_error("Euler angle rotation order must be any permutation of 1,2,3 without consecutive repeats.");
@@ -79,8 +79,10 @@ namespace MbD {
auto term7 = phiA->timesFullMatrix(theAdot->timesFullMatrix(psiAdot));
auto term8 = phiA->timesFullMatrix(theA->timesFullMatrix(psiAddot));
aAddot = term->plusFullMatrix(term1)->plusFullMatrix(term2)->plusFullMatrix(term3)->plusFullMatrix(term4)
->plusFullMatrix(term5)->plusFullMatrix(term6)->plusFullMatrix(term7)->plusFullMatrix(term8);
aAddot = toFMDsptr(term->plusFullMatrix(term1)->plusFullMatrix(term2)
->plusFullMatrix(term3)->plusFullMatrix(term4)
->plusFullMatrix(term5)->plusFullMatrix(term6)
->plusFullMatrix(term7)->plusFullMatrix(term8));
}
template<typename T>
inline void EulerAnglesDDot<T>::aEulerAngles(EulerAngles<T>* eulerAngles)

16
OndselSolver/EulerAnglesDot.h
View File

@@ -56,13 +56,13 @@ namespace MbD {
auto angle = aEulerAngles->at(i)->getValue();
auto angleDot = this->at(i)->getValue();
if (axis == 1) {
cAdot->atiput(i, FullMatrixDouble::rotatexrotDot(angle, angleDot));
cAdot->atiput(i, toFMDsptr(FullMatrixDouble::rotatexrotDot(angle, angleDot)));
}
else if (axis == 2) {
cAdot->atiput(i, FullMatrixDouble::rotateyrotDot(angle, angleDot));
cAdot->atiput(i, toFMDsptr(FullMatrixDouble::rotateyrotDot(angle, angleDot)));
}
else if (axis == 3) {
cAdot->atiput(i, FullMatrixDouble::rotatezrotDot(angle, angleDot));
cAdot->atiput(i, toFMDsptr(FullMatrixDouble::rotatezrotDot(angle, angleDot)));
}
else {
throw std::runtime_error("Euler angle rotation order must be any permutation of 1,2,3 without consecutive repeats.");
@@ -78,12 +78,12 @@ namespace MbD {
auto theAdot = cAdot->at(1);
auto psiAdot = cAdot->at(2);
aAdot = phiAdot->timesFullMatrix(theA->timesFullMatrix(psiA))
->plusFullMatrix(phiA->timesFullMatrix(theAdot->timesFullMatrix(psiA)))
->plusFullMatrix(phiA->timesFullMatrix(theA->timesFullMatrix(psiAdot)));
aAdot = toFMDsptr(phiAdot->timesFullMatrix(theA->timesFullMatrix(psiA))
->plusFullMatrix(phiA->timesFullMatrix(theAdot->timesFullMatrix(psiA)))
->plusFullMatrix(phiA->timesFullMatrix(theA->timesFullMatrix(psiAdot))));
omeF = (phiA->column(0)->times(phidot)
->plusFullColumn(phiA->timesFullMatrix(theA)->column(1)->times(thedot))
->plusFullColumn(aEulerAngles->aA->column(2)->times(psidot)));
->plusFullColumn(phiA->timesFullMatrix(theA)->column(1)->times(thedot))
->plusFullColumn(aEulerAngles->aA->column(2)->times(psidot)));
omef = aEulerAngles->aA->transposeTimesFullColumn(omeF);
}
}

8
OndselSolver/EulerAngleszxz.h
View File

@@ -29,9 +29,9 @@ namespace MbD {
template<typename T>
inline void EulerAngleszxz<T>::initialize()
{
phiA = FullMatrix<T>::identitysptr(3);
theA = FullMatrix<T>::identitysptr(3);
psiA = FullMatrix<T>::identitysptr(3);
phiA = FullMatrixDouble::identitysptr(3);
theA = FullMatrixDouble::identitysptr(3);
psiA = FullMatrixDouble::identitysptr(3);
}
template<typename T>
inline void EulerAngleszxz<T>::calc()
@@ -67,7 +67,7 @@ namespace MbD {
psiAi = psiA->at(1);
psiAi->at(0) = spsi;
psiAi->at(1) = cpsi;
aA = phiA->timesFullMatrix(theA->timesFullMatrix(psiA));
aA = toFMDsptr(phiA->timesFullMatrix(theA->timesFullMatrix(psiA)));
}
}

10
OndselSolver/EulerAngleszxzDDot.h
View File

@@ -8,6 +8,7 @@
#pragma once
#include "FullMatrix.h"
#include "EulerArray.h"
#include "EulerAngleszxzDot.h"
@@ -23,16 +24,17 @@ namespace MbD {
void calc() override;
std::shared_ptr<EulerAngleszxzDot<double>> phiThePsiDot;
FMatDsptr phiAddot, theAddot, psiAddot, aAddot;
FMatDsptr phiAddot, theAddot, psiAddot;
std::shared_ptr<FullMatrix<double>> aAddot;
};
template<typename T>
inline void EulerAngleszxzDDot<T>::initialize()
{
phiAddot = std::make_shared<FullMatrix<double>>(3, 3);
phiAddot = std::make_shared<FullMatrixDouble>(3, 3);
phiAddot->zeroSelf();
theAddot = std::make_shared<FullMatrix<double>>(3, 3);
theAddot = std::make_shared<FullMatrixDouble>(3, 3);
theAddot->zeroSelf();
psiAddot = std::make_shared<FullMatrix<double>>(3, 3);
psiAddot = std::make_shared<FullMatrixDouble>(3, 3);
psiAddot->zeroSelf();
}
template<typename T>

2
OndselSolver/EulerAngleszxzDot.h
View File

@@ -74,7 +74,7 @@ namespace MbD {
auto term1 = phiAdot->timesFullMatrix(theA->timesFullMatrix(psiA));
auto term2 = phiA->timesFullMatrix(theAdot->timesFullMatrix(psiA));
auto term3 = phiA->timesFullMatrix(theA->timesFullMatrix(psiAdot));
aAdot = (term1->plusFullMatrix(term2))->plusFullMatrix(term3);
aAdot = toFMDsptr((term1->plusFullMatrix(term2))->plusFullMatrix(term3));
}
}

26
OndselSolver/EulerParameters.cpp
View File

@@ -128,14 +128,14 @@ inline FMatFMatDsptr EulerParameters<double>::ppApEpEtimesMatrix(FMatDsptr mat)
FRowDsptr zero = std::make_shared<FullRow<double>>(3, 0.0);
auto mat00 = std::make_shared<FullMatrixDouble>(ListFRD{ a2m0, m2m1, m2m2 });
auto mat01 = std::make_shared<FullMatrixDouble>(ListFRD{ a2m1, a2m0, zero });
auto mat02 = std::make_shared<FullMatrix<double>>(ListFRD{ a2m2, zero, a2m0 });
auto mat03 = std::make_shared<FullMatrix<double>>(ListFRD{ zero, m2m2, a2m1 });
auto mat11 = std::make_shared<FullMatrix<double>>(ListFRD{ m2m0, a2m1, m2m2 });
auto mat12 = std::make_shared<FullMatrix<double>>(ListFRD{ zero, a2m2, a2m1 });
auto mat13 = std::make_shared<FullMatrix<double>>(ListFRD{ a2m2, zero, m2m0 });
auto mat22 = std::make_shared<FullMatrix<double>>(ListFRD{ m2m0, m2m1, a2m2 });
auto mat23 = std::make_shared<FullMatrix<double>>(ListFRD{ m2m1, a2m0, zero });
auto mat33 = std::make_shared<FullMatrix<double>>(ListFRD{ a2m0, a2m1, a2m2 });
auto mat02 = std::make_shared<FullMatrixDouble>(ListFRD{ a2m2, zero, a2m0 });
auto mat03 = std::make_shared<FullMatrixDouble>(ListFRD{ zero, m2m2, a2m1 });
auto mat11 = std::make_shared<FullMatrixDouble>(ListFRD{ m2m0, a2m1, m2m2 });
auto mat12 = std::make_shared<FullMatrixDouble>(ListFRD{ zero, a2m2, a2m1 });
auto mat13 = std::make_shared<FullMatrixDouble>(ListFRD{ a2m2, zero, m2m0 });
auto mat22 = std::make_shared<FullMatrixDouble>(ListFRD{ m2m0, m2m1, a2m2 });
auto mat23 = std::make_shared<FullMatrixDouble>(ListFRD{ m2m1, a2m0, zero });
auto mat33 = std::make_shared<FullMatrixDouble>(ListFRD{ a2m0, a2m1, a2m2 });
auto answer = std::make_shared<FullMatrix<FMatDsptr>>(4, 4);
auto& row0 = answer->at(0);
row0->at(0) = mat00;
@@ -157,19 +157,19 @@ inline FMatFMatDsptr EulerParameters<double>::ppApEpEtimesMatrix(FMatDsptr mat)
row3->at(1) = mat13;
row3->at(2) = mat23;
row3->at(3) = mat33;
return answer;
return toFMFMDsptr(answer);
}
template<>
inline void EulerParameters<double>::initialize()
{
aA = FullMatrix<double>::identitysptr(3);
aB = std::make_shared<FullMatrix<double>>(3, 4);
aC = std::make_shared<FullMatrix<double>>(3, 4);
aA = FullMatrixDouble::identitysptr(3);
aB = std::make_shared<FullMatrixDouble>(3, 4);
aC = std::make_shared<FullMatrixDouble>(3, 4);
pApE = std::make_shared<FullColumn<FMatDsptr>>(4);
for (int i = 0; i < 4; i++)
{
pApE->at(i) = std::make_shared<FullMatrix<double>>(3, 3);
pApE->at(i) = std::make_shared<FullMatrixDouble>(3, 3);
}
}
template<typename T>

2
OndselSolver/EulerParameters.h
View File

@@ -44,7 +44,7 @@ namespace MbD {
static std::shared_ptr<FullMatrix<FColsptr<T>>> ppApEpEtimesColumn(FColDsptr col);
static FMatDsptr pCpEtimesColumn(FColDsptr col);
static FMatDsptr pCTpEtimesColumn(FColDsptr col);
static std::shared_ptr<FullMatrix<FMatsptr<T>>> ppApEpEtimesMatrix(FMatDsptr mat);
static std::shared_ptr<FullMatrixFullMatrixDouble> ppApEpEtimesMatrix(FMatDsptr mat);
void initialize() override;

8
OndselSolver/EulerParametersDot.h
View File

@@ -55,13 +55,13 @@ namespace MbD {
template<typename T>
inline void EulerParametersDot<T>::initialize()
{
aAdot = std::make_shared<FullMatrix<double>>(3, 3);
aBdot = std::make_shared<FullMatrix<double>>(3, 4);
aCdot = std::make_shared<FullMatrix<double>>(3, 4);
aAdot = std::make_shared<FullMatrixDouble>(3, 3);
aBdot = std::make_shared<FullMatrixDouble>(3, 4);
aCdot = std::make_shared<FullMatrixDouble>(3, 4);
pAdotpE = std::make_shared<FullColumn<FMatDsptr>>(4);
for (int i = 0; i < 4; i++)
{
pAdotpE->at(i) = std::make_shared<FullMatrix<double>>(3, 3);
pAdotpE->at(i) = std::make_shared<FullMatrixDouble>(3, 3);
}
}

698
OndselSolver/FullMatrix.cpp
View File

@@ -0,0 +1,698 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#include "FullMatrix.h"
#include "FullColumn.h"
#include "FullRow.h"
#include "DiagonalMatrix.h"
#include "EulerParameters.h"
using namespace MbD;
template<typename T>
inline FMatsptr<T> FullMatrix<T>::rotatex(T the)
{
auto sthe = std::sin(the);
auto cthe = std::cos(the);
auto rotMat = std::make_shared<FullMatrix<T>>(3, 3);
auto row0 = rotMat->at(0);
row0->atiput(0, 1.0);
row0->atiput(1, 0.0);
row0->atiput(2, 0.0);
auto row1 = rotMat->at(1);
row1->atiput(0, 0.0);
row1->atiput(1, cthe);
row1->atiput(2, -sthe);
auto row2 = rotMat->at(2);
row2->atiput(0, 0.0);
row2->atiput(1, sthe);
row2->atiput(2, cthe);
return rotMat;
}
template<typename T>
inline FMatsptr<T> FullMatrix<T>::rotatey(T the)
{
auto sthe = std::sin(the);
auto cthe = std::cos(the);
auto rotMat = std::make_shared<FullMatrix<T>>(3, 3);
auto row0 = rotMat->at(0);
row0->atiput(0, cthe);
row0->atiput(1, 0.0);
row0->atiput(2, sthe);
auto row1 = rotMat->at(1);
row1->atiput(0, 0.0);
row1->atiput(1, 1.0);
row1->atiput(2, 0.0);
auto row2 = rotMat->at(2);
row2->atiput(0, -sthe);
row2->atiput(1, 0.0);
row2->atiput(2, cthe);
return rotMat;
}
template<typename T>
inline FMatsptr<T> FullMatrix<T>::rotatez(T the)
{
auto sthe = std::sin(the);
auto cthe = std::cos(the);
auto rotMat = std::make_shared<FullMatrix<T>>(3, 3);
auto row0 = rotMat->at(0);
row0->atiput(0, cthe);
row0->atiput(1, -sthe);
row0->atiput(2, 0.0);
auto row1 = rotMat->at(1);
row1->atiput(0, sthe);
row1->atiput(1, cthe);
row1->atiput(2, 0.0);
auto row2 = rotMat->at(2);
row2->atiput(0, 0.0);
row2->atiput(1, 0.0);
row2->atiput(2, 1.0);
return rotMat;
}
template<typename T>
inline FMatsptr<T> FullMatrix<T>::rotatexrotDot(T the, T thedot)
{
auto sthe = std::sin(the);
auto cthe = std::cos(the);
auto sthedot = cthe * thedot;
auto cthedot = -sthe * thedot;
auto rotMat = std::make_shared<FullMatrix<T>>(3, 3);
auto row0 = rotMat->at(0);
row0->atiput(0, 0.0);
row0->atiput(1, 0.0);
row0->atiput(2, 0.0);
auto row1 = rotMat->at(1);
row1->atiput(0, 0.0);
row1->atiput(1, cthedot);
row1->atiput(2, -sthedot);
auto row2 = rotMat->at(2);
row2->atiput(0, 0.0);
row2->atiput(1, sthedot);
row2->atiput(2, cthedot);
return rotMat;
}
template<typename T>
inline FMatsptr<T> FullMatrix<T>::rotateyrotDot(T the, T thedot)
{
auto sthe = std::sin(the);
auto cthe = std::cos(the);
auto sthedot = cthe * thedot;
auto cthedot = -sthe * thedot;
auto rotMat = std::make_shared<FullMatrix<T>>(3, 3);
auto row0 = rotMat->at(0);
row0->atiput(0, cthedot);
row0->atiput(1, 0.0);
row0->atiput(2, sthedot);
auto row1 = rotMat->at(1);
row1->atiput(0, 0.0);
row1->atiput(1, 0.0);
row1->atiput(2, 0.0);
auto row2 = rotMat->at(2);
row2->atiput(0, -sthedot);
row2->atiput(1, 0.0);
row2->atiput(2, cthedot);
return rotMat;
}
template<typename T>
inline FMatsptr<T> FullMatrix<T>::rotatezrotDot(T the, T thedot)
{
auto sthe = std::sin(the);
auto cthe = std::cos(the);
auto sthedot = cthe * thedot;
auto cthedot = -sthe * thedot;
auto rotMat = std::make_shared<FullMatrix<T>>(3, 3);
auto row0 = rotMat->at(0);
row0->atiput(0, cthedot);
row0->atiput(1, -sthedot);
row0->atiput(2, 0.0);
auto row1 = rotMat->at(1);
row1->atiput(0, sthedot);
row1->atiput(1, cthedot);
row1->atiput(2, 0.0);
auto row2 = rotMat->at(2);
row2->atiput(0, 0.0);
row2->atiput(1, 0.0);
row2->atiput(2, 0.0);
return rotMat;
}
template<typename T>
inline FMatsptr<T> FullMatrix<T>::rotatexrotDotrotDDot(T the, T thedot, T theddot)
{
auto sthe = std::sin(the);
auto cthe = std::cos(the);
auto sthedot = cthe * thedot;
auto cthedot = -sthe * thedot;
auto stheddot = cthedot * thedot + (cthe * theddot);
auto ctheddot = -(sthedot * thedot) - (sthe * theddot);
auto rotMat = std::make_shared<FullMatrix<T>>(3, 3);
auto row0 = rotMat->at(0);
row0->atiput(0, 0.0);
row0->atiput(1, 0.0);
row0->atiput(2, 0.0);
auto row1 = rotMat->at(1);
row1->atiput(0, 0.0);
row1->atiput(1, ctheddot);
row1->atiput(2, -stheddot);
auto row2 = rotMat->at(2);
row2->atiput(0, 0.0);
row2->atiput(1, stheddot);
row2->atiput(2, ctheddot);
return rotMat;
}
template<typename T>
inline FMatsptr<T> FullMatrix<T>::rotateyrotDotrotDDot(T the, T thedot, T theddot)
{
auto sthe = std::sin(the);
auto cthe = std::cos(the);
auto sthedot = cthe * thedot;
auto cthedot = -sthe * thedot;
auto stheddot = cthedot * thedot + (cthe * theddot);
auto ctheddot = -(sthedot * thedot) - (sthe * theddot);
auto rotMat = std::make_shared<FullMatrix<T>>(3, 3);
auto row0 = rotMat->at(0);
row0->atiput(0, ctheddot);
row0->atiput(1, 0.0);
row0->atiput(2, stheddot);
auto row1 = rotMat->at(1);
row1->atiput(0, 0.0);
row1->atiput(1, 0.0);
row1->atiput(2, 0.0);
auto row2 = rotMat->at(2);
row2->atiput(0, -stheddot);
row2->atiput(1, 0.0);
row2->atiput(2, ctheddot);
return rotMat;
}
template<typename T>
inline FMatsptr<T> FullMatrix<T>::rotatezrotDotrotDDot(T the, T thedot, T theddot)
{
auto sthe = std::sin(the);
auto cthe = std::cos(the);
auto sthedot = cthe * thedot;
auto cthedot = -sthe * thedot;
auto stheddot = cthedot * thedot + (cthe * theddot);
auto ctheddot = -(sthedot * thedot) - (sthe * theddot);
auto rotMat = std::make_shared<FullMatrix<T>>(3, 3);
auto row0 = rotMat->at(0);
row0->atiput(0, ctheddot);
row0->atiput(1, -stheddot);
row0->atiput(2, 0.0);
auto row1 = rotMat->at(1);
row1->atiput(0, stheddot);
row1->atiput(1, ctheddot);
row1->atiput(2, 0.0);
auto row2 = rotMat->at(2);
row2->atiput(0, 0.0);
row2->atiput(1, 0.0);
row2->atiput(2, 0.0);
return rotMat;
}
template<typename T>
inline FMatsptr<T> FullMatrix<T>::identitysptr(int n)
{
auto mat = std::make_shared<FullMatrix<T>>(n, n);
mat->identity();
return mat;
}
inline std::shared_ptr<MbD::FullMatrixDouble> FullMatrixDouble::identitysptr(int n)
{
auto mat = std::make_shared<FullMatrixDouble>(n, n);
mat->identity();
return mat;
}
inline std::shared_ptr<MbD::FullMatrixFullMatrixDouble> FullMatrixFullMatrixDouble::identitysptr(int n)
{
auto mat = std::make_shared<FullMatrixFullMatrixDouble>(n, n);
mat->identity();
return mat;
}
template<typename T>
inline FMatsptr<T> FullMatrix<T>::tildeMatrix(FColDsptr col)
{
//"tildeMatrix is skew symmetric matrix related to angular velocity and cross product."
if (col->size() != 3) throw std::runtime_error("Column is not of dimension 3");
auto tilde = std::make_shared<FullMatrixDouble>(3, 3);
auto c0 = col->at(0);
auto c1 = col->at(1);
auto c2 = col->at(2);
tilde->atijput(0, 0, 0.0);
tilde->atijput(1, 1, 0.0);
tilde->atijput(2, 2, 0.0);
tilde->atijput(1, 2, -c0);
tilde->atijput(0, 2, c1);
tilde->atijput(0, 1, -c2);
tilde->atijput(1, 0, c2);
tilde->atijput(2, 0, -c1);
tilde->atijput(2, 1, c0);
return tilde;
}
template<>
inline void FullMatrix<double>::zeroSelf()
{
for (int i = 0; i < this->size(); i++) {
this->at(i)->zeroSelf();
}
}
template<typename T>
inline void FullMatrix<T>::identity() {
assert(false);
}
inline void FullMatrixDouble::identity() {
this->zeroSelf();
for (int i = 0; i < this->size(); i++) {
this->at(i)->at(i) = 1.0;
}
}
inline void FullMatrixFullMatrixDouble::identity() {
assert(false);
// this->zeroSelf();
// for (int i = 0; i < this->size(); i++) {
// this->at(i)->at(i) = 1.0;
// }
}
template<typename T>
inline FColsptr<T> FullMatrix<T>::column(int j) {
int n = (int)this->size();
auto answer = std::make_shared<FullColumn<T>>(n);
for (int i = 0; i < n; i++) {
answer->at(i) = this->at(i)->at(j);
}
return answer;
}
template<typename T>
inline FMatsptr<T> FullMatrix<T>::timesFullMatrix(FMatsptr<T> fullMat)
{
int m = this->nrow();
auto answer = std::make_shared<FullMatrix<T>>(m);
for (int i = 0; i < m; i++) {
answer->at(i) = this->at(i)->timesFullMatrix(fullMat);
}
return answer;
}
template<typename T>
inline FMatsptr<T> FullMatrix<T>::timesTransposeFullMatrix(FMatsptr<T> fullMat)
{
assert(false);
// int nrow = this->nrow();
// auto answer = std::make_shared<FullMatrix<T>>(nrow);
// for (int i = 0; i < nrow; i++) {
// answer->at(i) = this->at(i)->timesTransposeFullMatrix(fullMat);
// }
// return answer;
}
inline std::shared_ptr<FullMatrixDouble> FullMatrixDouble::timesTransposeFullMatrix(std::shared_ptr<FullMatrixDouble> fullMat)
{
int nrow = this->nrow();
auto answer = std::make_shared<FullMatrixDouble>(nrow);
for (int i = 0; i < nrow; i++) {
answer->at(i) = this->at(i)->timesTransposeFullMatrix(fullMat);
}
return answer;
}
inline std::shared_ptr<FullMatrixFullMatrixDouble> FullMatrixFullMatrixDouble::timesTransposeFullMatrix(std::shared_ptr<FullMatrixFullMatrixDouble> fullMat)
{
int nrow = this->nrow();
auto answer = std::make_shared<FullMatrixFullMatrixDouble>(nrow);
for (int i = 0; i < nrow; i++) {
answer->at(i) = this->at(i)->timesTransposeFullMatrixForFMFMDsptr(fullMat);
}
return answer;
}
inline std::shared_ptr<FullMatrixDouble> FullMatrixDouble::times(double a)
{
int m = this->nrow();
auto answer = std::make_shared<FullMatrixDouble>(m);
for (int i = 0; i < m; i++) {
// auto x = this->at(i);
answer->at(i) = this->at(i)->times(a);
}
return answer;
}
inline std::shared_ptr<FullMatrixFullMatrixDouble> FullMatrixFullMatrixDouble::times(double a)
{
assert(false);
}
template<typename T>
inline FMatsptr<T> FullMatrix<T>::times(T a)
{
assert(false);
}
template<typename T>
inline FMatsptr<T> FullMatrix<T>::transposeTimesFullMatrix(FMatsptr<T> fullMat)
{
return this->transpose()->timesFullMatrix(fullMat);
}
template<typename T>
inline FMatsptr<T> FullMatrix<T>::plusFullMatrix(FMatsptr<T> fullMat)
{
int n = (int)this->size();
auto answer = std::make_shared<FullMatrix<T>>(n);
for (int i = 0; i < n; i++) {
answer->at(i) = this->at(i)->plusFullRow(fullMat->at(i));
}
return answer;
}
template<typename T>
inline FMatsptr<T> FullMatrix<T>::minusFullMatrix(FMatsptr<T> fullMat)
{
int n = (int)this->size();
auto answer = std::make_shared<FullMatrix<T>>(n);
for (int i = 0; i < n; i++) {
answer->at(i) = this->at(i)->minusFullRow(fullMat->at(i));
}
return answer;
}
template<typename T>
inline FMatsptr<T> FullMatrix<T>::transpose()
{
int nrow = this->nrow();
auto ncol = this->ncol();
auto answer = std::make_shared<FullMatrix<T>>(ncol, nrow);
for (int i = 0; i < nrow; i++) {
auto& row = this->at(i);
for (int j = 0; j < ncol; j++) {
answer->at(j)->at(i) = row->at(j);
}
}
return answer;
}
template<typename T>
inline FMatsptr<T> FullMatrix<T>::negated()
{
return this->times(-1.0);
}
template<typename T>
inline void FullMatrix<T>::symLowerWithUpper()
{
int n = (int)this->size();
for (int i = 0; i < n; i++) {
for (int j = i + 1; j < n; j++) {
this->at(j)->at(i) = this->at(i)->at(j);
}
}
}
template<typename T>
inline void FullMatrix<T>::atiput(int i, FRowsptr<T> fullRow)
{
this->at(i) = fullRow;
}
template<typename T>
inline void FullMatrix<T>::atijput(int i, int j, T value)
{
this->at(i)->atiput(j, value);
}
template<typename T>
inline void FullMatrix<T>::atijputFullColumn(int i1, int j1, FColsptr<T> fullCol)
{
for (int ii = 0; ii < fullCol->size(); ii++)
{
this->at(i1 + ii)->at(j1) = fullCol->at(ii);
}
}
template<typename T>
inline void FullMatrix<T>::atijplusFullRow(int i, int j, FRowsptr<T> fullRow)
{
this->at(i)->atiplusFullRow(j, fullRow);
}
template<typename T>
inline void FullMatrix<T>::atijplusNumber(int i, int j, T value)
{
auto rowi = this->at(i);
rowi->at(j) += value;
}
template<typename T>
inline void FullMatrix<T>::atijminusNumber(int i, int j, T value)
{
auto rowi = this->at(i);
rowi->at(j) -= value;
}
inline double FullMatrixDouble::sumOfSquares()
{
double sum = 0.0;
for (int i = 0; i < this->size(); i++)
{
sum += this->at(i)->sumOfSquares();
}
return sum;
}
inline double FullMatrixFullMatrixDouble::sumOfSquares()
{
double sum = 0.0;
for (int i = 0; i < this->size(); i++)
{
sum += this->at(i)->sumOfSquares();
}
return sum;
}
template<typename T>
inline double FullMatrix<T>::sumOfSquares()
{
assert(false);
return 0.0;
}
template<typename T>
inline void FullMatrix<T>::zeroSelf()
{
assert(false);
}
template<typename T>
inline FMatsptr<T> FullMatrix<T>::copy()
{
auto m = (int)this->size();
auto answer = std::make_shared<FullMatrix<T>>(m);
for (int i = 0; i < m; i++)
{
answer->at(i) = this->at(i)->copy();
}
return answer;
}
template<typename T>
inline FullMatrix<T> FullMatrix<T>::operator+(const FullMatrix<T> fullMat)
{
int n = (int)this->size();
auto answer = FullMatrix<T>(n);
for (int i = 0; i < n; i++) {
answer.at(i) = this->at(i)->plusFullRow(fullMat.at(i));
}
return answer;
}
template<typename T>
inline FColsptr<T> FullMatrix<T>::transposeTimesFullColumn(FColsptr<T> fullCol)
{
auto sptr = std::make_shared<FullMatrix<T>>(*this);
return fullCol->transpose()->timesFullMatrix(sptr)->transpose();
}
template<typename T>
inline void FullMatrix<T>::magnifySelf(T factor)
{
for (int i = 0; i < this->size(); i++) {
this->at(i)->magnifySelf(factor);
}
}
template<typename T>
inline std::ostream& FullMatrix<T>::printOn(std::ostream& s) const
{
s << "FullMat[" << std::endl;
for (int i = 0; i < this->size(); i++)
{
s << *(this->at(i)) << std::endl;
}
s << "]";
return s;
}
template<typename T>
inline std::shared_ptr<EulerParameters<T>> FullMatrix<T>::asEulerParameters()
{
//"Given [A], compute Euler parameter."
auto traceA = this->trace();
T dum = 0.0;
T dumSq = 0.0;
//auto qE = CREATE<EulerParameters<double>>::With(4); //Cannot use CREATE.h in subclasses of std::vector. Why?
auto qE = std::make_shared<EulerParameters<T>>(4);
qE->initialize();
auto OneMinusTraceDivFour = (1.0 - traceA) / 4.0;
for (int i = 0; i < 3; i++)
{
dumSq = this->at(i)->at(i) / 2.0 + OneMinusTraceDivFour;
dum = (dumSq > 0.0) ? std::sqrt(dumSq) : 0.0;
qE->atiput(i, dum);
}
dumSq = (1.0 + traceA) / 4.0;
dum = (dumSq > 0.0) ? std::sqrt(dumSq) : 0.0;
qE->atiput(3, dum);
T max = 0.0;
int maxE = -1;
for (int i = 0; i < 4; i++)
{
auto num = qE->at(i);
if (max < num) {
max = num;
maxE = i;
}
}
if (maxE == 0) {
auto FourE = 4.0 * qE->at(0);
qE->atiput(1, (this->at(0)->at(1) + this->at(1)->at(0)) / FourE);
qE->atiput(2, (this->at(0)->at(2) + this->at(2)->at(0)) / FourE);
qE->atiput(3, (this->at(2)->at(1) - this->at(1)->at(2)) / FourE);
}
else if (maxE == 1) {
auto FourE = 4.0 * qE->at(1);
qE->atiput(0, (this->at(0)->at(1) + this->at(1)->at(0)) / FourE);
qE->atiput(2, (this->at(1)->at(2) + this->at(2)->at(1)) / FourE);
qE->atiput(3, (this->at(0)->at(2) - this->at(2)->at(0)) / FourE);
}
else if (maxE == 2) {
auto FourE = 4.0 * qE->at(2);
qE->atiput(0, (this->at(0)->at(2) + this->at(2)->at(0)) / FourE);
qE->atiput(1, (this->at(1)->at(2) + this->at(2)->at(1)) / FourE);
qE->atiput(3, (this->at(1)->at(0) - this->at(0)->at(1)) / FourE);
}
else if (maxE == 3) {
auto FourE = 4.0 * qE->at(3);
qE->atiput(0, (this->at(2)->at(1) - this->at(1)->at(2)) / FourE);
qE->atiput(1, (this->at(0)->at(2) - this->at(2)->at(0)) / FourE);
qE->atiput(2, (this->at(1)->at(0) - this->at(0)->at(1)) / FourE);
}
qE->conditionSelf();
qE->calc();
return qE;
}
template<typename T>
inline T FullMatrix<T>::trace()
{
T trace = 0.0;
for (int i = 0; i < this->size(); i++)
{
trace += this->at(i)->at(i);
}
return trace;
}
template<typename T>
inline double FullMatrix<T>::maxMagnitude()
{
double max = 0.0;
for (int i = 0; i < this->size(); i++)
{
double element = this->at(i)->maxMagnitude();
if (max < element) max = element;
}
return max;
}
template<typename T>
inline FColsptr<T> FullMatrix<T>::bryantAngles()
{
auto answer = std::make_shared<FullColumn<T>>(3);
auto sthe1y = this->at(0)->at(2);
T the0x, the1y, the2z, cthe0x, sthe0x, y, x;
if (std::abs(sthe1y) > 0.9999) {
if (sthe1y > 0.0) {
the0x = std::atan2(this->at(1)->at(0), this->at(1)->at(1));
the1y = M_PI / 2.0;
the2z = 0.0;
}
else {
the0x = std::atan2(this->at(2)->at(1), this->at(2)->at(0));
the1y = M_PI / -2.0;
the2z = 0.0;
}
}
else {
the0x = std::atan2(-this->at(1)->at(2), this->at(2)->at(2));
cthe0x = std::cos(the0x);
sthe0x = std::sin(the0x);
y = sthe1y;
if (std::abs(cthe0x) > std::abs(sthe0x)) {
x = this->at(2)->at(2) / cthe0x;
}
else {
x = this->at(1)->at(2) / -sthe0x;
}
the1y = std::atan2(y, x);
the2z = std::atan2(-this->at(0)->at(1), this->at(0)->at(0));
}
answer->atiput(0, the0x);
answer->atiput(1, the1y);
answer->atiput(2, the2z);
return answer;
}
template<typename T>
inline bool FullMatrix<T>::isDiagonal()
{
auto m = this->nrow();
auto n = this->ncol();
if (m != n) return false;
for (int i = 0; i < m; i++)
{
auto rowi = this->at(i);
for (int j = 0; j < n; j++)
{
if (i != j && rowi->at(j) != 0) return false;
}
}
return true;
}
template<typename T>
inline bool FullMatrix<T>::isDiagonalToWithin(double ratio)
{
double maxMag = this->maxMagnitude();
auto tol = ratio * maxMag;
auto nrow = this->nrow();
if (nrow == this->ncol()) {
for (int i = 0; i < 3; i++)
{
for (int j = i + 1; j < 3; j++)
{
if (std::abs(this->at(i)->at(j)) > tol) return false;
if (std::abs(this->at(j)->at(i)) > tol) return false;
}
}
return true;
}
else {
return false;
}
}
template<typename T>
inline std::shared_ptr<DiagonalMatrix<T>> FullMatrix<T>::asDiagonalMatrix()
{
int nrow = this->nrow();
auto diagMat = std::make_shared<DiagonalMatrix<T>>(nrow);
for (int i = 0; i < nrow; i++)
{
diagMat->atiput(i, this->at(i)->at(i));
}
return diagMat;
}
template<typename T>
inline void FullMatrix<T>::conditionSelfWithTol(double tol)
{
for (auto row : *this) {
row->conditionSelfWithTol(tol);
}
}
template<typename T>
inline FColsptr<T> FullMatrix<T>::timesFullColumn(FColsptr<T> fullCol)
{
return this->timesFullColumn(fullCol.get());
}
template<typename T>
inline FColsptr<T> FullMatrix<T>::timesFullColumn(FullColumn<T>* fullCol)
{
//"a*b = a(i,j)b(j) sum j."
auto nrow = this->nrow();
auto answer = std::make_shared<FullColumn<T>>(nrow);
for (int i = 0; i < nrow; i++)
{
answer->at(i) = this->at(i)->timesFullColumn(fullCol);
}
return answer;
}

64
OndselSolver/FullMatrix.h
View File

@@ -17,6 +17,7 @@
#include "DiagonalMatrix.ref.h"
#include "EulerParameters.ref.h"
#include "RowTypeMatrix.h"
#include "FullRow.h" // exception to normal include pattern
namespace MbD {
template<typename T>
@@ -29,8 +30,8 @@ namespace MbD {
class FullMatrix : public RowTypeMatrix<FRowsptr<T>>
{
public:
FullMatrix() {}
FullMatrix(int m) : RowTypeMatrix<FRowsptr<T>>(m)
FullMatrix() {};
explicit FullMatrix(int m) : RowTypeMatrix<FRowsptr<T>>(m)
{
}
FullMatrix(int m, int n) {
@@ -63,13 +64,16 @@ namespace MbD {
static FMatsptr<T> rotatezrotDotrotDDot(T angle, T angleDot, T angleDDot);
static FMatsptr<T> identitysptr(int n);
static FMatsptr<T> tildeMatrix(FColDsptr col);
void identity();
virtual void identity();
FColsptr<T> column(int j);
FColsptr<T> timesFullColumn(FColsptr<T> fullCol);
FColsptr<T> timesFullColumn(FullColumn<T>* fullCol);
FMatsptr<T> timesFullMatrix(FMatsptr<T> fullMat);
FMatsptr<T> timesTransposeFullMatrix(FMatsptr<T> fullMat);
FMatsptr<T> times(T a);
virtual FMatsptr<T> timesTransposeFullMatrix(FMatsptr<T> fullMat);
FMatsptr<T> times(T a);
FMatsptr<T> transposeTimesFullMatrix(FMatsptr<T> fullMat);
FMatsptr<T> plusFullMatrix(FMatsptr<T> fullMat);
FMatsptr<T> minusFullMatrix(FMatsptr<T> fullMat);
@@ -100,12 +104,54 @@ namespace MbD {
std::ostream& printOn(std::ostream& s) const override;
};
class FullMatrixDouble : public FullMatrix<double> {};
using FMatDsptr = std::shared_ptr<MbD::FullMatrixDouble>;
//
// FULL MATRIX DOUBLE instantiation
//
class FullMatrixDouble : public FullMatrix<double> {
public:
FullMatrixDouble() : FullMatrix<double>() {};
explicit FullMatrixDouble(int m) : FullMatrix<double>(m) {};
FullMatrixDouble(int m, int n) : FullMatrix<double>(m, n) {};
FullMatrixDouble(std::initializer_list<std::initializer_list<double>> list2D) : FullMatrix<double>(list2D) {}
FullMatrixDouble(std::initializer_list<FRowsptr<double>> listOfRows) : FullMatrix<double>(listOfRows) {};
using FMatFColDsptr = std::shared_ptr<FullMatrix<FColDsptr>>;
using FMatFMatDsptr = std::shared_ptr<FullMatrix<FMatDsptr>>;
std::shared_ptr<FullMatrixDouble> times(double a);
std::shared_ptr<FullMatrixDouble> timesTransposeFullMatrix(std::shared_ptr<FullMatrixDouble> fullMat);
double sumOfSquares() override;
void identity() override;
static std::shared_ptr<MbD::FullMatrixDouble> identitysptr(int n);
};
using FMatDsptr = std::shared_ptr<MbD::FullMatrixDouble>;
std::shared_ptr<FullMatrixDouble> toFMDsptr(FMatsptr<double> s) {
return std::static_pointer_cast<FullMatrixDouble>(s);
}
//
// FULL MATRIX FULL MATRIX DOUBLE instantiation
//
class FullMatrixFullMatrixDouble : public FullMatrix<FMatDsptr> {
public:
FullMatrixFullMatrixDouble() : FullMatrix<FMatDsptr>() {};
explicit FullMatrixFullMatrixDouble(int m) : FullMatrix<FMatDsptr>(m) {};
FullMatrixFullMatrixDouble(int m, int n) : FullMatrix<FMatDsptr>(m, n) {};
std::shared_ptr<FullMatrixFullMatrixDouble> times(double a);
std::shared_ptr<FullMatrixFullMatrixDouble> timesTransposeFullMatrix(std::shared_ptr<FullMatrixFullMatrixDouble> fullMat);
double sumOfSquares() override;
void identity() override;
static std::shared_ptr<MbD::FullMatrixFullMatrixDouble> identitysptr(int n);
};
using FMatFMatDsptr = std::shared_ptr<FullMatrixFullMatrixDouble>;
std::shared_ptr<FullMatrixFullMatrixDouble> toFMFMDsptr(std::shared_ptr<FullMatrix<FMatDsptr>> s) {
return std::static_pointer_cast<FullMatrixFullMatrixDouble>(s);
}
using FColFMatDsptr = std::shared_ptr<FullColumn<FMatDsptr>>;
using FMatFColDsptr = std::shared_ptr<FullMatrix<FColDsptr>>;
}

1
OndselSolver/FullMatrix.ref.h
View File

@@ -4,6 +4,7 @@
namespace MbD {
class FullMatrixDouble;
class FullMatrixFullMatrixDouble;
// using FMatDsptr = std::shared_ptr<MbD::FullMatrixDouble>;

26
OndselSolver/FullRow.cpp
View File

@@ -12,7 +12,7 @@
using namespace MbD;
template<typename T>
std::shared_ptr<FullMatrixDouble> FullRow<T>::transposeTimesFullRow(std::shared_ptr<FullMatrixDouble> fullRow)
std::shared_ptr<FullMatrixDouble> FullRow<T>::transposeTimesFullRow(FRowsptr<T> fullRow)
{
//"a*b = a(i)b(j)"
auto nrow = (int)this->size();
@@ -36,6 +36,18 @@ inline FRowsptr<T> FullRow<T>::timesTransposeFullMatrix(std::shared_ptr<FullMatr
return answer;
}
template<typename T>
inline FRowsptr<T> FullRow<T>::timesTransposeFullMatrixForFMFMDsptr(std::shared_ptr<FullMatrixFullMatrixDouble> fullMat)
{
//"a*bT = a(1,j)b(k,j)"
int ncol = fullMat->nrow();
auto answer = std::make_shared<FullRow<T>>(ncol);
for (int k = 0; k < ncol; k++) {
answer->at(k) = this->dot(fullMat->at(k));
}
return answer;
}
template<typename T>
inline FRowsptr<T> FullRow<T>::timesFullMatrix(std::shared_ptr<FullMatrixDouble> fullMat)
{
@@ -45,5 +57,15 @@ inline FRowsptr<T> FullRow<T>::timesFullMatrix(std::shared_ptr<FullMatrixDouble>
answer->equalSelfPlusFullRowTimes(fullMat->at(j), this->at(j));
}
return answer;
//return FRowsptr<T>();
}
template<typename T>
inline FRowsptr<T> FullRow<T>::timesFullMatrixForFMFMDsptr(std::shared_ptr<FullMatrixFullMatrixDouble> fullMat)
{
FRowsptr<T> answer = fullMat->at(0)->times(this->at(0));
for (int j = 1; j < (int) this->size(); j++)
{
answer->equalSelfPlusFullRowTimes(fullMat->at(j), this->at(j));
}
return answer;
}

12
OndselSolver/FullRow.h
View File

@@ -30,17 +30,21 @@ namespace MbD {
FRowsptr<T> minusFullRow(FRowsptr<T> fullRow);
T timesFullColumn(FColsptr<T> fullCol);
T timesFullColumn(FullColumn<T>* fullCol);
FRowsptr<T> timesFullMatrix(std::shared_ptr<FullMatrixDouble> fullMat);
FRowsptr<T> timesTransposeFullMatrix(std::shared_ptr<FullMatrixDouble> fullMat);
void equalSelfPlusFullRowTimes(FRowsptr<T> fullRow, double factor);
void equalFullRow(FRowsptr<T> fullRow);
FColsptr<T> transpose();
FRowsptr<T> copy();
void atiplusFullRow(int j, FRowsptr<T> fullRow);
std::shared_ptr<FullMatrixDouble> transposeTimesFullRow(std::shared_ptr<FullMatrixDouble> fullRow);
std::ostream& printOn(std::ostream& s) const override;
};
FRowsptr<T> timesFullMatrix(std::shared_ptr<FullMatrixDouble> fullMat);
FRowsptr<T> timesTransposeFullMatrix(std::shared_ptr<FullMatrixDouble> fullMat);
std::shared_ptr<FullMatrixDouble> transposeTimesFullRow(FRowsptr<T> fullRow);
FRowsptr<T> timesFullMatrixForFMFMDsptr(std::shared_ptr<FullMatrixFullMatrixDouble> fullMat);
FRowsptr<T> timesTransposeFullMatrixForFMFMDsptr(std::shared_ptr<FullMatrixFullMatrixDouble> fullMat);
// std::shared_ptr<FullMatrixFullMatrixDouble> transposeTimesFullRow(FRowsptr<T> fullRow);
};
template<>
inline FRowDsptr FullRow<double>::times(double a)

7
OndselSolver/GearConstraintIqcJc.cpp
View File

@@ -52,17 +52,18 @@ void MbD::GearConstraintIqcJc::calc_pGpXI()
void MbD::GearConstraintIqcJc::calc_ppGpEIpEI()
{
ppGpEIpEI = orbitJeIe->ppvaluepEJpEJ()->plusFullMatrix(orbitIeJe->ppvaluepEIpEI()->times(this->ratio()));
ppGpEIpEI = toFMDsptr(orbitJeIe->ppvaluepEJpEJ()->plusFullMatrix(orbitIeJe->ppvaluepEIpEI()->times(this->ratio())));
}
void MbD::GearConstraintIqcJc::calc_ppGpXIpEI()
{
ppGpXIpEI = orbitJeIe->ppvaluepXJpEJ()->plusFullMatrix(orbitIeJe->ppvaluepXIpEI()->times(this->ratio()));
ppGpXIpEI = toFMDsptr(orbitJeIe->ppvaluepXJpEJ()->plusFullMatrix(orbitIeJe->ppvaluepXIpEI()->times(this->ratio())));
}
void MbD::GearConstraintIqcJc::calc_ppGpXIpXI()
{
ppGpXIpXI = orbitJeIe->ppvaluepXJpXJ()->plusFullMatrix(orbitIeJe->ppvaluepXIpXI()->times(this->ratio()));
ppGpXIpXI = toFMDsptr(orbitJeIe->ppvaluepXJpXJ()
->plusFullMatrix(orbitIeJe->ppvaluepXIpXI()->times(this->ratio())));
}
void MbD::GearConstraintIqcJc::calcPostDynCorrectorIteration()

14
OndselSolver/GearConstraintIqcJqc.cpp
View File

@@ -29,37 +29,37 @@ void MbD::GearConstraintIqcJqc::calc_pGpXJ()
void MbD::GearConstraintIqcJqc::calc_ppGpEIpEJ()
{
ppGpEIpEJ = orbitJeIe->ppvaluepEIpEJ()->transpose()->plusFullMatrix(orbitIeJe->ppvaluepEIpEJ()->times(this->ratio()));
ppGpEIpEJ = toFMDsptr(orbitJeIe->ppvaluepEIpEJ()->transpose()->plusFullMatrix(orbitIeJe->ppvaluepEIpEJ()->times(this->ratio())));
}
void MbD::GearConstraintIqcJqc::calc_ppGpEIpXJ()
{
ppGpEIpXJ = orbitJeIe->ppvaluepXIpEJ()->transpose()->plusFullMatrix(orbitIeJe->ppvaluepEIpXJ()->times(this->ratio()));
ppGpEIpXJ = toFMDsptr(orbitJeIe->ppvaluepXIpEJ()->transpose()->plusFullMatrix(orbitIeJe->ppvaluepEIpXJ()->times(this->ratio())));
}
void MbD::GearConstraintIqcJqc::calc_ppGpEJpEJ()
{
ppGpEJpEJ = orbitJeIe->ppvaluepEIpEI()->plusFullMatrix(orbitIeJe->ppvaluepEJpEJ()->times(this->ratio()));
ppGpEJpEJ = toFMDsptr(orbitJeIe->ppvaluepEIpEI()->plusFullMatrix(orbitIeJe->ppvaluepEJpEJ()->times(this->ratio())));
}
void MbD::GearConstraintIqcJqc::calc_ppGpXIpEJ()
{
ppGpXIpEJ = orbitJeIe->ppvaluepEIpXJ()->transpose()->plusFullMatrix(orbitIeJe->ppvaluepXIpEJ()->times(this->ratio()));
ppGpXIpEJ = toFMDsptr(orbitJeIe->ppvaluepEIpXJ()->transpose()->plusFullMatrix(orbitIeJe->ppvaluepXIpEJ()->times(this->ratio())));
}
void MbD::GearConstraintIqcJqc::calc_ppGpXIpXJ()
{
ppGpXIpXJ = orbitJeIe->ppvaluepXIpXJ()->transpose()->plusFullMatrix(orbitIeJe->ppvaluepXIpXJ()->times(this->ratio()));
ppGpXIpXJ = toFMDsptr(orbitJeIe->ppvaluepXIpXJ()->transpose()->plusFullMatrix(orbitIeJe->ppvaluepXIpXJ()->times(this->ratio())));
}
void MbD::GearConstraintIqcJqc::calc_ppGpXJpEJ()
{
ppGpXJpEJ = orbitJeIe->ppvaluepXIpEI()->plusFullMatrix(orbitIeJe->ppvaluepXJpEJ()->times(this->ratio()));
ppGpXJpEJ = toFMDsptr(orbitJeIe->ppvaluepXIpEI()->plusFullMatrix(orbitIeJe->ppvaluepXJpEJ()->times(this->ratio())));
}
void MbD::GearConstraintIqcJqc::calc_ppGpXJpXJ()
{
ppGpXJpXJ = orbitJeIe->ppvaluepXIpXI()->plusFullMatrix(orbitIeJe->ppvaluepXJpXJ()->times(this->ratio()));
ppGpXJpXJ = toFMDsptr(orbitJeIe->ppvaluepXIpXI()->plusFullMatrix(orbitIeJe->ppvaluepXJpXJ()->times(this->ratio())));
}
void MbD::GearConstraintIqcJqc::calcPostDynCorrectorIteration()

2
OndselSolver/Item.h
View File

@@ -12,7 +12,7 @@
#include <vector>
#include "FullColumn.h"
#include "FullMatrix.h"
// #include "FullMatrix.h"
#include "DiagonalMatrix.h"
#include "SparseMatrix.h"
#include "enum.h"

2
OndselSolver/LDUFullMat.cpp
View File

@@ -63,7 +63,7 @@ void LDUFullMat::postSolve()
void LDUFullMat::preSolvesaveOriginal(FMatDsptr fullMat, bool saveOriginal)
{
if (saveOriginal) {
matrixA = fullMat->copy();
matrixA = toFMDsptr(fullMat->copy());
}
else {
matrixA = fullMat;

6
OndselSolver/MBDynItem.cpp
View File

@@ -164,7 +164,7 @@ FMatDsptr MbD::MBDynItem::readOrientation(std::vector<std::string>& args)
auto ref = mbdynReferences()->at(refName);
auto aAFf = readBasicOrientation(args);
auto aAOF = ref->aAOf;
aAOf = aAOF->timesFullMatrix(aAFf);
aAOf = toFMDsptr(aAOF->timesFullMatrix(aAFf));
}
else if (str.find("hinge") != std::string::npos) {
args.erase(args.begin());
@@ -300,13 +300,13 @@ FMatDsptr MbD::MBDynItem::readBasicOrientation(std::vector<std::string>& args)
else {
assert(false);
}
auto aAFf = FullMatrix<double>::identitysptr(3);
auto aAFf = FullMatrixDouble::identitysptr(3);
aAFf->atijputFullColumn(0, 0, vecX);
aAFf->atijputFullColumn(0, 1, vecY);
aAFf->atijputFullColumn(0, 2, vecZ);
return aAFf;
}
auto aAFf = FullMatrix<double>::identitysptr(3);
auto aAFf = FullMatrixDouble::identitysptr(3);
for (int i = 0; i < 3; i++)
{
auto rowi = aAFf->at(i);

2
OndselSolver/MBDynMarker.cpp
View File

@@ -21,7 +21,7 @@ void MbD::MBDynMarker::parseMBDyn(std::vector<std::string>& args)
auto rOmO = readPosition(args);
auto aAOm = readOrientation(args);
rPmP = aAOP->transposeTimesFullColumn(rOmO->minusFullColumn(rOPO));
aAPm = aAOP->transposeTimesFullMatrix(aAOm);
aAPm = toFMDsptr(aAOP->transposeTimesFullMatrix(aAOm));
}
else if (str.find("offset") != std::string::npos) {
rPmP = readPosition(args);

4
OndselSolver/MarkerFrame.cpp
View File

@@ -70,13 +70,13 @@ void MarkerFrame::calcPostDynCorrectorIteration()
auto rOpO = partFrame->rOpO();
auto aAOp = partFrame->aAOp();
rOmO = rOpO->plusFullColumn(aAOp->timesFullColumn(rpmp));
aAOm = aAOp->timesFullMatrix(aApm);
aAOm = toFMDsptr(aAOp->timesFullMatrix(aApm));
auto pAOppE = partFrame->pAOppE();
for (int i = 0; i < 4; i++)
{
auto& pAOppEi = pAOppE->at(i);
prOmOpE->atijputFullColumn(0, i, pAOppEi->timesFullColumn(rpmp));
pAOmpE->at(i) = pAOppEi->timesFullMatrix(aApm);
pAOmpE->at(i) = toFMDsptr(pAOppEi->timesFullMatrix(aApm));
}
}

4
OndselSolver/MarkerFrame.h
View File

@@ -72,9 +72,9 @@ namespace MbD {
PartFrame* partFrame; //Use raw pointer when pointing backwards.
FColDsptr rpmp = std::make_shared<FullColumn<double>>(3);
FMatDsptr aApm = FullMatrix<double>::identitysptr(3);
FMatDsptr aApm = FullMatrixDouble::identitysptr(3);
FColDsptr rOmO = std::make_shared<FullColumn<double>>(3);
FMatDsptr aAOm = FullMatrix<double>::identitysptr(3);
FMatDsptr aAOm = FullMatrixDouble::identitysptr(3);
FMatDsptr prOmOpE;
FColFMatDsptr pAOmpE;
FMatFColDsptr pprOmOpEpE;

14
OndselSolver/MomentOfInertiaSolver.cpp
View File

@@ -32,7 +32,7 @@ void MbD::MomentOfInertiaSolver::example1()
solver->setJPP(aJPP);
auto rPoP = aApP->transposeTimesFullColumn(rpPp->negated());
solver->setrPoP(rPoP);
auto aAPo = aApP->transpose();
auto aAPo = toFMDsptr(aApP->transpose());
solver->setAPo(aAPo);
solver->setrPcmP(rPoP);
solver->calc();
@@ -188,7 +188,7 @@ void MbD::MomentOfInertiaSolver::calcJoo()
auto term22 = rPoPtilde->timesFullMatrix(rocmPtilde);
auto term23 = term22->transpose();
auto term2 = term21->plusFullMatrix(term22)->plusFullMatrix(term23)->times(m);
aJoo = aAPo->transposeTimesFullMatrix(term1->plusFullMatrix(term2))->timesFullMatrix(aAPo);
aJoo = toFMDsptr(aAPo->transposeTimesFullMatrix(term1->plusFullMatrix(term2))->timesFullMatrix(aAPo));
aJoo->symLowerWithUpper();
aJoo->conditionSelfWithTol(aJoo->maxMagnitude() * 1.0e-6);
}
@@ -197,8 +197,8 @@ void MbD::MomentOfInertiaSolver::calcJpp()
{
//"aJcmP = aJPP + mass*(rPcmPTilde*rPcmPTilde)"
auto rPcmPtilde = FullMatrix<double>::tildeMatrix(rPcmP);
aJcmP = aJPP->plusFullMatrix(rPcmPtilde->timesFullMatrix(rPcmPtilde)->times(m));
auto rPcmPtilde = FullMatrixDouble::tildeMatrix(rPcmP);
aJcmP = toFMDsptr(aJPP->plusFullMatrix(rPcmPtilde->timesFullMatrix(rPcmPtilde)->times(m)));
aJcmP->symLowerWithUpper();
aJcmP->conditionSelfWithTol(aJcmP->maxMagnitude() * 1.0e-6);
if (aJcmP->isDiagonal()) {
@@ -217,7 +217,7 @@ void MbD::MomentOfInertiaSolver::calcAPp()
auto lam2 = aJpp->at(2);
if (lam0 == lam1) {
if (lam1 == lam2) {
aAPp = FullMatrix<double>::identitysptr(3);
aAPp = FullMatrixDouble::identitysptr(3);
}
else {
eigenvector1 = eigenvectorFor(lam1);
@@ -238,7 +238,7 @@ void MbD::MomentOfInertiaSolver::calcAPp()
if (eigenvector1->at(1) < 0.0) eigenvector1->negateSelf();
eigenvector2 = eigenvector0->cross(eigenvector1);
}
aAPp = std::make_shared<FullMatrix<double>>(3, 3);
aAPp = std::make_shared<FullMatrixDouble>(3, 3);
aAPp->atijputFullColumn(0, 0, eigenvector0);
aAPp->atijputFullColumn(0, 1, eigenvector1);
aAPp->atijputFullColumn(0, 2, eigenvector2);
@@ -249,7 +249,7 @@ FColDsptr MbD::MomentOfInertiaSolver::eigenvectorFor(double lam)
//"[aJcmP] - lam[I]."
double e0, e1, e2;
aJcmPcopy = aJcmP->copy();
aJcmPcopy = toFMDsptr(aJcmP->copy());
colOrder = std::make_shared<FullRow<int>>(3);
auto eigenvector = std::make_shared<FullColumn<double>>(3);
for (int i = 0; i < 3; i++)

2
OndselSolver/OrbitAnglezIeqcJeqc.cpp
View File

@@ -243,7 +243,7 @@ void MbD::OrbitAngleZIeqcJeqc::initialize()
pthezpXJ = std::make_shared<FullRow<double>>(3);
pthezpEJ = std::make_shared<FullRow<double>>(4);
ppthezpXIpXJ = std::make_shared<FullMatrixDouble>(3, 3);
ppthezpXIpEJ = std::make_shared<FullMatrixDouble>>(3, 4);
ppthezpXIpEJ = std::make_shared<FullMatrixDouble>(3, 4);
ppthezpEIpXJ = std::make_shared<FullMatrixDouble>(4, 3);
ppthezpEIpEJ = std::make_shared<FullMatrixDouble>(4, 4);
ppthezpXJpXJ = std::make_shared<FullMatrixDouble>(3, 3);

8
OndselSolver/Part.cpp
View File

@@ -462,7 +462,7 @@ void Part::calcmEdot()
auto a4J = aJ->times(4.0);
auto term1 = aC->transposeTimesFullMatrix(a4J->timesFullMatrix(aCdot));
auto term2 = term1->transpose();
mEdot = term1->plusFullMatrix(term2);
mEdot = toFMDsptr(term1->plusFullMatrix(term2));
}
void Part::calcpTpE()
@@ -479,7 +479,7 @@ void Part::calcppTpEpE()
auto& qEdot = partFrame->qEdot;
auto pCpEtimesqEdot = EulerParameters<double>::pCpEtimesColumn(qEdot);
auto a4J = aJ->times(4.0);
ppTpEpE = pCpEtimesqEdot->transposeTimesFullMatrix(a4J->timesFullMatrix(pCpEtimesqEdot));
ppTpEpE = toFMDsptr(pCpEtimesqEdot->transposeTimesFullMatrix(a4J->timesFullMatrix(pCpEtimesqEdot)));
}
void Part::calcppTpEpEdot()
@@ -491,13 +491,13 @@ void Part::calcppTpEpEdot()
auto term1 = EulerParameters<double>::pCTpEtimesColumn(a4J->timesFullColumn(aC->timesFullColumn(qEdot)));
auto pCpEtimesqEdot = EulerParameters<double>::pCpEtimesColumn(qEdot);
auto term2 = aC->transposeTimesFullMatrix(a4J->timesFullMatrix(pCpEtimesqEdot));
ppTpEpEdot = term1->plusFullMatrix(term2)->transpose();
ppTpEpEdot = toFMDsptr(term1->plusFullMatrix(term2)->transpose());
}
void Part::calcmE()
{
auto aC = partFrame->aC();
mE = aC->transposeTimesFullMatrix(aJ->timesFullMatrix(aC))->times(4.0);
mE = toFMDsptr(aC->transposeTimesFullMatrix(aJ->timesFullMatrix(aC))->times(4.0));
}
void Part::fillAccICIterError(FColDsptr col)

6
OndselSolver/RackPinConstraintIqcJc.cpp
View File

@@ -53,12 +53,14 @@ void MbD::RackPinConstraintIqcJc::calc_pGpXI()
void MbD::RackPinConstraintIqcJc::calc_ppGpEIpEI()
{
ppGpEIpEI = xIeJeIe->ppvaluepEIpEI()->plusFullMatrix(thezIeJe->ppvaluepEIpEI()->times(pitchRadius));
ppGpEIpEI = toFMDsptr(xIeJeIe->ppvaluepEIpEI()
->plusFullMatrix(thezIeJe->ppvaluepEIpEI()->times(pitchRadius)));
}
void MbD::RackPinConstraintIqcJc::calc_ppGpXIpEI()
{
ppGpXIpEI = xIeJeIe->ppvaluepXIpEI()->plusFullMatrix(thezIeJe->ppvaluepXIpEI()->times(pitchRadius));
ppGpXIpEI = toFMDsptr(xIeJeIe->ppvaluepXIpEI()
->plusFullMatrix(thezIeJe->ppvaluepXIpEI()->times(pitchRadius)));
}
void MbD::RackPinConstraintIqcJc::calcPostDynCorrectorIteration()

9
OndselSolver/RackPinConstraintIqcJqc.cpp
View File

@@ -30,17 +30,20 @@ void MbD::RackPinConstraintIqcJqc::calc_pGpXJ()
void MbD::RackPinConstraintIqcJqc::calc_ppGpEIpEJ()
{
ppGpEIpEJ = xIeJeIe->ppvaluepEIpEJ()->plusFullMatrix(thezIeJe->ppvaluepEIpEJ()->times(pitchRadius));
ppGpEIpEJ = toFMDsptr(xIeJeIe->ppvaluepEIpEJ()
->plusFullMatrix(thezIeJe->ppvaluepEIpEJ()->times(pitchRadius)));
}
void MbD::RackPinConstraintIqcJqc::calc_ppGpEIpXJ()
{
ppGpEIpXJ = xIeJeIe->ppvaluepEIpXJ()->plusFullMatrix(thezIeJe->ppvaluepEIpXJ()->times(pitchRadius));
ppGpEIpXJ = toFMDsptr(xIeJeIe->ppvaluepEIpXJ()
->plusFullMatrix(thezIeJe->ppvaluepEIpXJ()->times(pitchRadius)));
}
void MbD::RackPinConstraintIqcJqc::calc_ppGpEJpEJ()
{
ppGpEJpEJ = xIeJeIe->ppvaluepEJpEJ()->plusFullMatrix(thezIeJe->ppvaluepEJpEJ()->times(pitchRadius));
ppGpEJpEJ = toFMDsptr(xIeJeIe->ppvaluepEJpEJ()
->plusFullMatrix(thezIeJe->ppvaluepEJpEJ()->times(pitchRadius)));
}
void MbD::RackPinConstraintIqcJqc::calcPostDynCorrectorIteration()

6
OndselSolver/ScrewConstraintIqcJc.cpp
View File

@@ -55,12 +55,14 @@ void MbD::ScrewConstraintIqcJc::calc_pGpXI()
void MbD::ScrewConstraintIqcJc::calc_ppGpEIpEI()
{
ppGpEIpEI = zIeJeIe->ppvaluepEIpEI()->times(2.0 * M_PI)->minusFullMatrix(thezIeJe->ppvaluepEIpEI()->times(pitch));
ppGpEIpEI = toFMDsptr(zIeJeIe->ppvaluepEIpEI()->times(2.0 * M_PI)
->minusFullMatrix(thezIeJe->ppvaluepEIpEI()->times(pitch)));
}
void MbD::ScrewConstraintIqcJc::calc_ppGpXIpEI()
{
ppGpXIpEI = zIeJeIe->ppvaluepXIpEI()->times(2.0 * M_PI)->minusFullMatrix(thezIeJe->ppvaluepXIpEI()->times(pitch));
ppGpXIpEI = toFMDsptr(zIeJeIe->ppvaluepXIpEI()->times(2.0 * M_PI)
->minusFullMatrix(thezIeJe->ppvaluepXIpEI()->times(pitch)));
}
void MbD::ScrewConstraintIqcJc::calcPostDynCorrectorIteration()

9
OndselSolver/ScrewConstraintIqcJqc.cpp
View File

@@ -32,17 +32,20 @@ void MbD::ScrewConstraintIqcJqc::calc_pGpXJ()
void MbD::ScrewConstraintIqcJqc::calc_ppGpEIpEJ()
{
ppGpEIpEJ = zIeJeIe->ppvaluepEIpEJ()->times(2.0 * M_PI)->minusFullMatrix(thezIeJe->ppvaluepEIpEJ()->times(pitch));
ppGpEIpEJ = toFMDsptr(zIeJeIe->ppvaluepEIpEJ()->times(2.0 * M_PI)
->minusFullMatrix(thezIeJe->ppvaluepEIpEJ()->times(pitch)));
}
void MbD::ScrewConstraintIqcJqc::calc_ppGpEIpXJ()
{
ppGpEIpXJ = zIeJeIe->ppvaluepEIpXJ()->times(2.0 * M_PI)->minusFullMatrix(thezIeJe->ppvaluepEIpXJ()->times(pitch));
ppGpEIpXJ = toFMDsptr(zIeJeIe->ppvaluepEIpXJ()->times(2.0 * M_PI)
->minusFullMatrix(thezIeJe->ppvaluepEIpXJ()->times(pitch)));
}
void MbD::ScrewConstraintIqcJqc::calc_ppGpEJpEJ()
{
ppGpEJpEJ = zIeJeIe->ppvaluepEJpEJ()->times(2.0 * M_PI)->minusFullMatrix(thezIeJe->ppvaluepEJpEJ()->times(pitch));
ppGpEJpEJ = toFMDsptr(zIeJeIe->ppvaluepEJpEJ()->times(2.0 * M_PI)
->minusFullMatrix(thezIeJe->ppvaluepEJpEJ()->times(pitch)));
}
void MbD::ScrewConstraintIqcJqc::calcPostDynCorrectorIteration()

6
OndselSolver/TranslationConstraintIqcJc.cpp
View File

@@ -30,8 +30,10 @@ void TranslationConstraintIqcJc::calcPostDynCorrectorIteration()
pGpXI = riIeqJeIeq->pvaluepXI();
pGpEI = (riIeqJeIeq->pvaluepEI())->plusFullRow(riIeqJeIeq->pvaluepEK());
ppGpXIpEI = riIeqJeIeq->ppvaluepXIpEK();
ppGpEIpEI = riIeqJeIeq->ppvaluepEIpEI()->plusFullMatrix(riIeqJeIeq->ppvaluepEIpEK())
->plusFullMatrix((riIeqJeIeq->ppvaluepEIpEK()->transpose()->plusFullMatrix(riIeqJeIeq->ppvaluepEKpEK())));
ppGpEIpEI = toFMDsptr(riIeqJeIeq->ppvaluepEIpEI()
->plusFullMatrix(riIeqJeIeq->ppvaluepEIpEK())
->plusFullMatrix((riIeqJeIeq->ppvaluepEIpEK()->
transpose()->plusFullMatrix(riIeqJeIeq->ppvaluepEKpEK()))));
}
void TranslationConstraintIqcJc::useEquationNumbers()

4
OndselSolver/TranslationConstraintIqcJqc.cpp
View File

@@ -28,8 +28,8 @@ void TranslationConstraintIqcJqc::calcPostDynCorrectorIteration()
TranslationConstraintIqcJc::calcPostDynCorrectorIteration();
pGpXJ = riIeJeIe->pvaluepXJ();
pGpEJ = riIeJeIe->pvaluepEJ();
ppGpEIpXJ = riIeJeIe->ppvaluepXJpEK()->transpose();
ppGpEIpEJ = riIeJeIe->ppvaluepEJpEK()->transpose();
ppGpEIpXJ = toFMDsptr(riIeJeIe->ppvaluepXJpEK()->transpose());
ppGpEIpEJ = toFMDsptr(riIeJeIe->ppvaluepEJpEK()->transpose());
ppGpEJpEJ = riIeJeIe->ppvaluepEJpEJ();
}