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

stopping point

Browse Source
This commit is contained in:
John Dupuy
2023-11-03 13:57:11 -05:00
parent 654c07a68e
commit 8dbb49d032
46 changed files with 142 additions and 780 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
OndselSolver/ASMTAssembly.cpp
View File

@@ -248,7 +248,7 @@ void MbD::ASMTAssembly::runSinglePendulum()
assembly->setName(str);
auto pos3D = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0 });
assembly->setPosition3D(pos3D);
auto rotMat = std::make_shared<FullMatrix<double>>(ListListD{
auto rotMat = std::make_shared<FullMatrixDouble>(ListListD{
{ 1, 0, 0 },
{ 0, 1, 0 },
{ 0, 0, 1 }
@@ -266,7 +266,7 @@ void MbD::ASMTAssembly::runSinglePendulum()
massMarker->setMomentOfInertias(aJ);
pos3D = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0 });
massMarker->setPosition3D(pos3D);
rotMat = std::make_shared<FullMatrix<double>>(ListListD{
rotMat = std::make_shared<FullMatrixDouble>(ListListD{
{ 1, 0, 0 },
{ 0, 1, 0 },
{ 0, 0, 1 }
@@ -279,7 +279,7 @@ void MbD::ASMTAssembly::runSinglePendulum()
mkr->setName(str);
pos3D = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0 });
mkr->setPosition3D(pos3D);
rotMat = std::make_shared<FullMatrix<double>>(ListListD{
rotMat = std::make_shared<FullMatrixDouble>(ListListD{
{ 1, 0, 0 },
{ 0, 1, 0 },
{ 0, 0, 1 }
@@ -292,7 +292,7 @@ void MbD::ASMTAssembly::runSinglePendulum()
part->setName(str);
pos3D = std::make_shared<FullColumn<double>>(ListD{ -0.1, -0.1, -0.1 });
part->setPosition3D(pos3D);
rotMat = std::make_shared<FullMatrix<double>>(ListListD{
rotMat = std::make_shared<FullMatrixDouble>(ListListD{
{ 1, 0, 0 },
{ 0, 1, 0 },
{ 0, 0, 1 }
@@ -311,7 +311,7 @@ void MbD::ASMTAssembly::runSinglePendulum()
massMarker->setMomentOfInertias(aJ);
pos3D = std::make_shared<FullColumn<double>>(ListD{ 0.5, 0.1, 0.05 });
massMarker->setPosition3D(pos3D);
rotMat = std::make_shared<FullMatrix<double>>(ListListD{
rotMat = std::make_shared<FullMatrixDouble>(ListListD{
{ 1, 0, 0 },
{ 0, 1, 0 },
{ 0, 0, 1 }
@@ -324,7 +324,7 @@ void MbD::ASMTAssembly::runSinglePendulum()
mkr->setName(str);
pos3D = std::make_shared<FullColumn<double>>(ListD{ 0.1, 0.1, 0.1 });
mkr->setPosition3D(pos3D);
rotMat = std::make_shared<FullMatrix<double>>(ListListD{
rotMat = std::make_shared<FullMatrixDouble>(ListListD{
{ 1, 0, 0 },
{ 0, 1, 0 },
{ 0, 0, 1 }

2
OndselSolver/ASMTPrincipalMassMarker.cpp
View File

@@ -26,7 +26,7 @@ void MbD::ASMTPrincipalMassMarker::parseASMT(std::vector<std::string>& lines)
lines.erase(lines.begin());
assert(lines[0] == (leadingTabs + "RotationMatrix"));
lines.erase(lines.begin());
rotationMatrix = std::make_shared<FullMatrix<double>>(3);
rotationMatrix = std::make_shared<FullMatrixDouble>(3);
for (int i = 0; i < 3; i++)
{
auto row = readRowOfDoubles(lines[0]);

4
OndselSolver/ASMTSpatialItem.cpp
View File

@@ -47,7 +47,7 @@ void MbD::ASMTSpatialItem::readRotationMatrix(std::vector<std::string>& lines)
{
assert(lines[0].find("RotationMatrix") != std::string::npos);
lines.erase(lines.begin());
rotationMatrix = std::make_shared<FullMatrix<double>>(3, 0);
rotationMatrix = std::make_shared<FullMatrixDouble>(3, 0);
for (int i = 0; i < 3; i++)
{
auto& row = rotationMatrix->at(i);
@@ -86,7 +86,7 @@ void MbD::ASMTSpatialItem::setRotationMatrix(double v11, double v12, double v13,
double v21, double v22, double v23,
double v31, double v32, double v33)
{
rotationMatrix = std::make_shared<FullMatrix<double>>(ListListD{
rotationMatrix = std::make_shared<FullMatrixDouble>(ListListD{
{ v11, v12, v13 },
{ v21, v22, v23 },
{ v31, v32, v33 }

2
OndselSolver/ASMTSpatialItem.h
View File

@@ -33,7 +33,7 @@ namespace MbD {
void storeOnLevelRotationMatrix(std::ofstream& os, int level);
FColDsptr position3D = std::make_shared<FullColumn<double>>(3);
FMatDsptr rotationMatrix = std::make_shared<FullMatrix<double>>(ListListD{
FMatDsptr rotationMatrix = std::make_shared<FullMatrixDouble>(ListListD{
{ 1, 0, 0 },
{ 0, 1, 0 },
{ 0, 0, 1 }

2
OndselSolver/AngleZIeqcJec.cpp
View File

@@ -65,7 +65,7 @@ void MbD::AngleZIeqcJec::initialize()
{
AngleZIecJec::initialize();
pthezpEI = std::make_shared<FullRow<double>>(4);
ppthezpEIpEI = std::make_shared<FullMatrix<double>>(4, 4);
ppthezpEIpEI = std::make_shared<FullMatrixDouble>(4, 4);
}
FMatDsptr MbD::AngleZIeqcJec::ppvaluepEIpEI()

4
OndselSolver/AngleZIeqcJeqc.cpp
View File

@@ -85,8 +85,8 @@ void MbD::AngleZIeqcJeqc::initialize()
{
AngleZIeqcJec::initialize();
pthezpEJ = std::make_shared<FullRow<double>>(4);
ppthezpEIpEJ = std::make_shared<FullMatrix<double>>(4, 4);
ppthezpEJpEJ = std::make_shared<FullMatrix<double>>(4, 4);
ppthezpEIpEJ = std::make_shared<FullMatrixDouble>(4, 4);
ppthezpEJpEJ = std::make_shared<FullMatrixDouble>(4, 4);
}
FMatDsptr MbD::AngleZIeqcJeqc::ppvaluepEIpEJ()

22
OndselSolver/CADSystem.cpp
View File

@@ -89,7 +89,7 @@ void CADSystem::runOndselSinglePendulum()
assembly1->m = 0.0;
assembly1->aJ = std::make_shared<DiagonalMatrix<double>>(ListD{ 0, 0, 0 });
qX = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0 });
aAap = std::make_shared<FullMatrix<double>>(ListListD{
aAap = std::make_shared<FullMatrixDouble>(ListListD{
{ 1, 0, 0 },
{ 0, 1, 0 },
{ 0, 0, 1 }
@@ -106,7 +106,7 @@ void CADSystem::runOndselSinglePendulum()
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 }
@@ -117,7 +117,7 @@ void CADSystem::runOndselSinglePendulum()
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 }
@@ -132,7 +132,7 @@ void CADSystem::runOndselSinglePendulum()
crankPart1->m = 1.0;
crankPart1->aJ = std::make_shared<DiagonalMatrix<double>>(ListD{ 1, 1, 1 });
qX = std::make_shared<FullColumn<double>>(ListD{ 0.4, 0.0, -0.05 });
aAap = std::make_shared<FullMatrix<double>>(ListListD{
aAap = std::make_shared<FullMatrixDouble>(ListListD{
{ 1, 0, 0 },
{ 0, 1, 0 },
{ 0, 0, 1 }
@@ -149,7 +149,7 @@ void CADSystem::runOndselSinglePendulum()
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 }
@@ -160,7 +160,7 @@ void CADSystem::runOndselSinglePendulum()
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 }
@@ -224,7 +224,7 @@ void CADSystem::runOndselDoublePendulum()
assembly1->m = 0.0;
assembly1->aJ = std::make_shared<DiagonalMatrix<double>>(ListD{ 0, 0, 0 });
qX = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0 });
aAap = std::make_shared<FullMatrix<double>>(ListListD{
aAap = std::make_shared<FullMatrixDouble>(ListListD{
{ 1, 0, 0 },
{ 0, 1, 0 },
{ 0, 0, 1 }
@@ -241,7 +241,7 @@ void CADSystem::runOndselDoublePendulum()
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 }
@@ -252,7 +252,7 @@ void CADSystem::runOndselDoublePendulum()
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 }
@@ -267,7 +267,7 @@ void CADSystem::runOndselDoublePendulum()
crankPart1->m = 1.0;
crankPart1->aJ = std::make_shared<DiagonalMatrix<double>>(ListD{ 1, 1, 1 });
qX = std::make_shared<FullColumn<double>>(ListD{ 0.4, 0.0, -0.05 });
aAap = std::make_shared<FullMatrix<double>>(ListListD{
aAap = std::make_shared<FullMatrixDouble>(ListListD{
{ 1, 0, 0 },
{ 0, 1, 0 },
{ 0, 0, 1 }
@@ -284,7 +284,7 @@ void CADSystem::runOndselDoublePendulum()
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 }

4
OndselSolver/ConstVelConstraintIqcJqc.cpp
View File

@@ -118,8 +118,8 @@ void MbD::ConstVelConstraintIqcJqc::initialize()
{
ConstVelConstraintIqcJc::initialize();
pGpEJ = std::make_shared<FullRow<double>>(4);
ppGpEIpEJ = std::make_shared<FullMatrix<double>>(4, 4);
ppGpEJpEJ = std::make_shared<FullMatrix<double>>(4, 4);
ppGpEIpEJ = std::make_shared<FullMatrixDouble>(4, 4);
ppGpEJpEJ = std::make_shared<FullMatrixDouble>(4, 4);
}
void MbD::ConstVelConstraintIqcJqc::useEquationNumbers()

2
OndselSolver/DifferenceOperator.cpp
View File

@@ -57,7 +57,7 @@ void DifferenceOperator::setorder(int o)
void DifferenceOperator::instantiateTaylorMatrix()
{
if (taylorMatrix == nullptr || (taylorMatrix->nrow() != (order + 1))) {
taylorMatrix = std::make_shared<FullMatrix<double>>(order + 1, order + 1);
taylorMatrix = std::make_shared<FullMatrixDouble>(order + 1, order + 1);
}
}

2
OndselSolver/DirectionCosineIeqcJec.cpp
View File

@@ -24,7 +24,7 @@ void DirectionCosineIeqcJec::initialize()
{
DirectionCosineIecJec::initialize();
pAijIeJepEI = std::make_shared<FullRow<double>>(4);
ppAijIeJepEIpEI = std::make_shared<FullMatrix<double>>(4, 4);
ppAijIeJepEIpEI = std::make_shared<FullMatrixDouble>(4, 4);
}
void DirectionCosineIeqcJec::initializeGlobally()

2
OndselSolver/DirectionCosineIeqcJeqc.cpp
View File

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

4
OndselSolver/DispCompIeqcJecIe.cpp
View File

@@ -87,8 +87,8 @@ void MbD::DispCompIeqcJecIe::initialize()
DispCompIecJecIe::initialize();
priIeJeIepXI = std::make_shared<FullRow<double>>(3);
priIeJeIepEI = std::make_shared<FullRow<double>>(4);
ppriIeJeIepXIpEI = std::make_shared<FullMatrix<double>>(3, 4);
ppriIeJeIepEIpEI = std::make_shared<FullMatrix<double>>(4, 4);
ppriIeJeIepXIpEI = std::make_shared<FullMatrixDouble>(3, 4);
ppriIeJeIepEIpEI = std::make_shared<FullMatrixDouble>(4, 4);
}
void MbD::DispCompIeqcJecIe::initializeGlobally()

6
OndselSolver/DispCompIeqcJecKeqc.cpp
View File

@@ -24,9 +24,9 @@ void DispCompIeqcJecKeqc::initialize()
DispCompIecJecKeqc::initialize();
priIeJeKepXI = std::make_shared<FullRow<double>>(3);
priIeJeKepEI = std::make_shared<FullRow<double>>(4);
ppriIeJeKepEIpEI = std::make_shared<FullMatrix<double>>(4, 4);
ppriIeJeKepXIpEK = std::make_shared<FullMatrix<double>>(3, 4);
ppriIeJeKepEIpEK = std::make_shared<FullMatrix<double>>(4, 4);
ppriIeJeKepEIpEI = std::make_shared<FullMatrixDouble>(4, 4);
ppriIeJeKepXIpEK = std::make_shared<FullMatrixDouble>(3, 4);
ppriIeJeKepEIpEK = std::make_shared<FullMatrixDouble>(4, 4);
}
void DispCompIeqcJecKeqc::calcPostDynCorrectorIteration()

6
OndselSolver/DispCompIeqcJeqcIe.cpp
View File

@@ -83,9 +83,9 @@ void MbD::DispCompIeqcJeqcIe::initialize()
DispCompIeqcJecIe::initialize();
priIeJeIepXJ = std::make_shared<FullRow<double>>(3);
priIeJeIepEJ = std::make_shared<FullRow<double>>(4);
ppriIeJeIepEIpXJ = std::make_shared<FullMatrix<double>>(4, 3);
ppriIeJeIepEIpEJ = std::make_shared<FullMatrix<double>>(4, 4);
ppriIeJeIepEJpEJ = std::make_shared<FullMatrix<double>>(4, 4);
ppriIeJeIepEIpXJ = std::make_shared<FullMatrixDouble>(4, 3);
ppriIeJeIepEIpEJ = std::make_shared<FullMatrixDouble>(4, 4);
ppriIeJeIepEJpEJ = std::make_shared<FullMatrixDouble>(4, 4);
}
FMatDsptr MbD::DispCompIeqcJeqcIe::ppvaluepEIpEJ()

6
OndselSolver/DispCompIeqcJeqcKeqc.cpp
View File

@@ -24,9 +24,9 @@ void DispCompIeqcJeqcKeqc::initialize()
DispCompIeqcJecKeqc::initialize();
priIeJeKepXJ = std::make_shared<FullRow<double>>(3);
priIeJeKepEJ = std::make_shared<FullRow<double>>(4);
ppriIeJeKepEJpEJ = std::make_shared<FullMatrix<double>>(4, 4);
ppriIeJeKepXJpEK = std::make_shared<FullMatrix<double>>(3, 4);
ppriIeJeKepEJpEK = std::make_shared<FullMatrix<double>>(4, 4);
ppriIeJeKepEJpEJ = std::make_shared<FullMatrixDouble>(4, 4);
ppriIeJeKepXJpEK = std::make_shared<FullMatrixDouble>(3, 4);
ppriIeJeKepEJpEK = std::make_shared<FullMatrixDouble>(4, 4);
}
void DispCompIeqcJeqcKeqc::calcPostDynCorrectorIteration()

10
OndselSolver/DistIeqcJec.cpp
View File

@@ -41,7 +41,7 @@ void MbD::DistIeqcJec::calcPrivate()
pprIeJepXIipXI->atiput(j, element / rIeJe);
}
}
pprIeJepXIpEI = std::make_shared<FullMatrix<double>>(3, 4);
pprIeJepXIpEI = std::make_shared<FullMatrixDouble>(3, 4);
for (int i = 0; i < 3; i++)
{
auto& pprIeJepXIipEI = pprIeJepXIpEI->at(i);
@@ -53,7 +53,7 @@ void MbD::DistIeqcJec::calcPrivate()
pprIeJepXIipEI->atiput(j, element / rIeJe);
}
}
pprIeJepEIpEI = std::make_shared<FullMatrix<double>>(4, 4);
pprIeJepEIpEI = std::make_shared<FullMatrixDouble>(4, 4);
for (int i = 0; i < 4; i++)
{
auto& pprIeJepEIipEI = pprIeJepEIpEI->at(i);
@@ -74,9 +74,9 @@ void MbD::DistIeqcJec::initialize()
DistIecJec::initialize();
prIeJepXI = std::make_shared<FullRow<double>>(3);
prIeJepEI = std::make_shared<FullRow<double>>(4);
pprIeJepXIpXI = std::make_shared<FullMatrix<double>>(3, 3);
pprIeJepXIpEI = std::make_shared<FullMatrix<double>>(3, 4);
pprIeJepEIpEI = std::make_shared<FullMatrix<double>>(4, 4);
pprIeJepXIpXI = std::make_shared<FullMatrixDouble>(3, 3);
pprIeJepXIpEI = std::make_shared<FullMatrixDouble>(3, 4);
pprIeJepEIpEI = std::make_shared<FullMatrixDouble>(4, 4);
}
FMatDsptr MbD::DistIeqcJec::ppvaluepEIpEI()

14
OndselSolver/DistIeqcJeqc.cpp
View File

@@ -122,13 +122,13 @@ void MbD::DistIeqcJeqc::initialize()
DistIeqcJec::initialize();
prIeJepXJ = std::make_shared<FullRow<double>>(3);
prIeJepEJ = std::make_shared<FullRow<double>>(4);
pprIeJepXIpXJ = std::make_shared<FullMatrix<double>>(3, 3);
pprIeJepEIpXJ = std::make_shared<FullMatrix<double>>(4, 3);
pprIeJepXJpXJ = std::make_shared<FullMatrix<double>>(3, 3);
pprIeJepXIpEJ = std::make_shared<FullMatrix<double>>(3, 4);
pprIeJepEIpEJ = std::make_shared<FullMatrix<double>>(4, 4);
pprIeJepXJpEJ = std::make_shared<FullMatrix<double>>(3, 4);
pprIeJepEJpEJ = std::make_shared<FullMatrix<double>>(4, 4);
pprIeJepXIpXJ = std::make_shared<FullMatrixDouble>(3, 3);
pprIeJepEIpXJ = std::make_shared<FullMatrixDouble>(4, 3);
pprIeJepXJpXJ = std::make_shared<FullMatrixDouble>(3, 3);
pprIeJepXIpEJ = std::make_shared<FullMatrixDouble>(3, 4);
pprIeJepEIpEJ = std::make_shared<FullMatrixDouble>(4, 4);
pprIeJepXJpEJ = std::make_shared<FullMatrixDouble>(3, 4);
pprIeJepEJpEJ = std::make_shared<FullMatrixDouble>(4, 4);
}
FMatDsptr MbD::DistIeqcJeqc::ppvaluepEIpEJ()

6
OndselSolver/DistxyIeqcJec.cpp
View File

@@ -161,9 +161,9 @@ void MbD::DistxyIeqcJec::initialize()
DistxyIecJec::initialize();
pdistxypXI = std::make_shared<FullRow<double>>(3);
pdistxypEI = std::make_shared<FullRow<double>>(4);
ppdistxypXIpXI = std::make_shared<FullMatrix<double>>(3, 3);
ppdistxypXIpEI = std::make_shared<FullMatrix<double>>(3, 4);
ppdistxypEIpEI = std::make_shared<FullMatrix<double>>(4, 4);
ppdistxypXIpXI = std::make_shared<FullMatrixDouble>(3, 3);
ppdistxypXIpEI = std::make_shared<FullMatrixDouble>(3, 4);
ppdistxypEIpEI = std::make_shared<FullMatrixDouble>(4, 4);
}
FMatDsptr MbD::DistxyIeqcJec::ppvaluepEIpEI()

2
OndselSolver/EndFramec.h
View File

@@ -44,7 +44,7 @@ namespace MbD {
MarkerFrame* markerFrame; //Use raw pointer when pointing backwards.
FColDsptr rOeO = std::make_shared<FullColumn<double>>(3);
FMatDsptr aAOe = FullMatrix<double>::identitysptr(3);
FMatDsptr aAOe = FullMatrixDouble::identitysptr(3);
};
//using EndFrmsptr = std::shared_ptr<EndFramec>;
}

6
OndselSolver/EndFrameqc.cpp
View File

@@ -25,7 +25,7 @@ EndFrameqc::EndFrameqc(const char* str) : EndFramec(str) {
void EndFrameqc::initialize()
{
prOeOpE = std::make_shared<FullMatrix<double>>(3, 4);
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);
@@ -80,7 +80,7 @@ void EndFrameqc::calcPostDynCorrectorIteration()
FMatDsptr EndFrameqc::pAjOepET(int axis)
{
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);
auto& pAOepEi = pAOepE->at(i);
@@ -94,7 +94,7 @@ FMatDsptr EndFrameqc::pAjOepET(int axis)
FMatDsptr EndFrameqc::ppriOeOpEpE(int ii)
{
auto answer = std::make_shared<FullMatrix<double>>(4, 4);
auto answer = std::make_shared<FullMatrixDouble>(4, 4);
for (int i = 0; i < 4; i++) {
auto& answeri = answer->at(i);
auto& pprOeOpEipE = pprOeOpEpE->at(i);

2
OndselSolver/EndFrameqct.cpp
View File

@@ -31,7 +31,7 @@ void EndFrameqct::initialize()
rmem = std::make_shared<FullColumn<double>>(3);
prmempt = std::make_shared<FullColumn<double>>(3);
pprmemptpt = std::make_shared<FullColumn<double>>(3);
aAme = FullMatrix<double>::identitysptr(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);

10
OndselSolver/EulerAngles.h
View File

@@ -51,13 +51,13 @@ namespace MbD {
auto axis = rotOrder->at(i);
auto angle = this->at(i)->getValue();
if (axis == 1) {
cA->atiput(i, FullMatrix<double>::rotatex(angle));
cA->atiput(i, FullMatrixDouble::rotatex(angle));
}
else if (axis == 2) {
cA->atiput(i, FullMatrix<double>::rotatey(angle));
cA->atiput(i, FullMatrixDouble::rotatey(angle));
}
else if (axis == 3) {
cA->atiput(i, FullMatrix<double>::rotatez(angle));
cA->atiput(i, FullMatrixDouble::rotatez(angle));
}
else {
throw std::runtime_error("Euler angle rotation order must be any permutation of 1,2,3 without consecutive repeats.");
@@ -74,10 +74,10 @@ namespace MbD {
auto axis = rotOrder->at(i);
auto angle = this->at(i);
if (axis == 1) {
cA->atiput(i, FullMatrix<double>::rotatex(angle));
cA->atiput(i, FullMatrixDouble::rotatex(angle));
}
else if (axis == 2) {
cA->atiput(i, FullMatrix<double>::rotatey(angle));
cA->atiput(i, FullMatrixDouble::rotatey(angle));
}
else if (axis == 3) {
cA->atiput(i, FullMatrix<double>::rotatez(angle));

6
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, FullMatrix<double>::rotatexrotDotrotDDot(angle, angleDot, angleDDot));
cAddot->atiput(i, FullMatrixDouble::rotatexrotDotrotDDot(angle, angleDot, angleDDot));
}
else if (axis == 2) {
cAddot->atiput(i, FullMatrix<double>::rotateyrotDotrotDDot(angle, angleDot, angleDDot));
cAddot->atiput(i, FullMatrixDouble::rotateyrotDotrotDDot(angle, angleDot, angleDDot));
}
else if (axis == 3) {
cAddot->atiput(i, FullMatrix<double>::rotatezrotDotrotDDot(angle, angleDot, angleDDot));
cAddot->atiput(i, 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.");

6
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, FullMatrix<double>::rotatexrotDot(angle, angleDot));
cAdot->atiput(i, FullMatrixDouble::rotatexrotDot(angle, angleDot));
}
else if (axis == 2) {
cAdot->atiput(i, FullMatrix<double>::rotateyrotDot(angle, angleDot));
cAdot->atiput(i, FullMatrixDouble::rotateyrotDot(angle, angleDot));
}
else if (axis == 3) {
cAdot->atiput(i, FullMatrix<double>::rotatezrotDot(angle, angleDot));
cAdot->atiput(i, FullMatrixDouble::rotatezrotDot(angle, angleDot));
}
else {
throw std::runtime_error("Euler angle rotation order must be any permutation of 1,2,3 without consecutive repeats.");

6
OndselSolver/EulerAngleszxzDot.h
View File

@@ -28,11 +28,11 @@ namespace MbD {
template<typename T>
inline void EulerAngleszxzDot<T>::initialize()
{
phiAdot = std::make_shared<FullMatrix<double>>(3, 3);
phiAdot = std::make_shared<FullMatrixDouble>(3, 3);
phiAdot->zeroSelf();
theAdot = std::make_shared<FullMatrix<double>>(3, 3);
theAdot = std::make_shared<FullMatrixDouble>(3, 3);
theAdot->zeroSelf();
psiAdot = std::make_shared<FullMatrix<double>>(3, 3);
psiAdot = std::make_shared<FullMatrixDouble>(3, 3);
psiAdot->zeroSelf();
}
template<typename T>

8
OndselSolver/EulerParameters.cpp
View File

@@ -63,7 +63,7 @@ inline FMatDsptr EulerParameters<double>::pCpEtimesColumn(FColDsptr col)
auto mc1 = -c1;
auto mc2 = -c2;
auto mc3 = -col->at(3);
auto answer = std::make_shared<FullMatrix<double>>(3, 4);
auto answer = std::make_shared<FullMatrixDouble>(3, 4);
auto& row0 = answer->at(0);
auto& row1 = answer->at(1);
auto& row2 = answer->at(2);
@@ -92,7 +92,7 @@ inline FMatDsptr EulerParameters<T>::pCTpEtimesColumn(FColDsptr col)
auto mc0 = -c0;
auto mc1 = -c1;
auto mc2 = -c2;
auto answer = std::make_shared<FullMatrix<double>>(4, 4);
auto answer = std::make_shared<FullMatrixDouble>(4, 4);
auto& row0 = answer->at(0);
auto& row1 = answer->at(1);
auto& row2 = answer->at(2);
@@ -126,8 +126,8 @@ inline FMatFMatDsptr EulerParameters<double>::ppApEpEtimesMatrix(FMatDsptr mat)
FRowDsptr m2m1 = a2m1->negated();
FRowDsptr m2m2 = a2m2->negated();
FRowDsptr zero = std::make_shared<FullRow<double>>(3, 0.0);
auto mat00 = std::make_shared<FullMatrix<double>>(ListFRD{ a2m0, m2m1, m2m2 });
auto mat01 = std::make_shared<FullMatrix<double>>(ListFRD{ a2m1, a2m0, zero });
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 });

644
OndselSolver/FullMatrix.cpp
View File

@@ -1,644 +0,0 @@
/***************************************************************************
* 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;
}
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<FullMatrix<double>>(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<>
inline void FullMatrix<double>::identity() {
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)
{
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;
}
template<>
inline FMatDsptr FullMatrix<double>::times(double a)
{
int m = this->nrow();
auto answer = std::make_shared<FullMatrix<double>>(m);
for (int i = 0; i < m; i++) {
answer->at(i) = this->at(i)->times(a);
}
return answer;
}
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;
}
template<>
inline double FullMatrix<double>::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;
}

15
OndselSolver/FullMatrix.h
View File

@@ -19,8 +19,13 @@
#include "RowTypeMatrix.h"
namespace MbD {
template<typename T>
class FullMatrix;
template<typename T>
template<typename T>
using FMatsptr = std::shared_ptr<FullMatrix<T>>;
template<typename T>
class FullMatrix : public RowTypeMatrix<FRowsptr<T>>
{
public:
@@ -94,5 +99,13 @@ namespace MbD {
std::ostream& printOn(std::ostream& s) const override;
};
class FullMatrixDouble : public FullMatrix<double> {};
using FMatDsptr = std::shared_ptr<MbD::FullMatrixDouble>;
using FMatFColDsptr = std::shared_ptr<FullMatrix<FColDsptr>>;
using FMatFMatDsptr = std::shared_ptr<FullMatrix<FMatDsptr>>;
using FColFMatDsptr = std::shared_ptr<FullColumn<FMatDsptr>>;
}

17
OndselSolver/FullMatrix.ref.h
View File

@@ -3,18 +3,17 @@
#include "FullColumn.ref.h"
namespace MbD {
template<typename T>
class FullMatrix;
class FullMatrixDouble;
using FMatDsptr = std::shared_ptr<MbD::FullMatrix<double>>;
// using FMatDsptr = std::shared_ptr<MbD::FullMatrixDouble>;
template<typename T>
using FMatsptr = std::shared_ptr<FullMatrix<T>>;
// template<typename T>
// using FMatsptr = std::shared_ptr<FullMatrix<T>>;
using FMatFColDsptr = std::shared_ptr<FullMatrix<FColDsptr>>;
using FMatFMatDsptr = std::shared_ptr<FullMatrix<FMatDsptr>>;
using FColFMatDsptr = std::shared_ptr<FullColumn<FMatDsptr>>;
// using FMatFColDsptr = std::shared_ptr<FullMatrix<FColDsptr>>;
// using FMatFMatDsptr = std::shared_ptr<FullMatrix<FMatDsptr>>;
//
// using FColFMatDsptr = std::shared_ptr<FullColumn<FMatDsptr>>;
}

8
OndselSolver/FullRow.cpp
View File

@@ -12,11 +12,11 @@
using namespace MbD;
template<typename T>
FMatsptr<T> FullRow<T>::transposeTimesFullRow(FRowsptr<T> fullRow)
std::shared_ptr<FullMatrixDouble> FullRow<T>::transposeTimesFullRow(std::shared_ptr<FullMatrixDouble> fullRow)
{
//"a*b = a(i)b(j)"
auto nrow = (int)this->size();
auto answer = std::make_shared<FullMatrix<double>>(nrow);
auto answer = std::make_shared<FullMatrixDouble>(nrow);
for (int i = 0; i < nrow; i++)
{
answer->atiput(i, fullRow->times(this->at(i)));
@@ -25,7 +25,7 @@ FMatsptr<T> FullRow<T>::transposeTimesFullRow(FRowsptr<T> fullRow)
}
template<typename T>
inline FRowsptr<T> FullRow<T>::timesTransposeFullMatrix(FMatsptr<T> fullMat)
inline FRowsptr<T> FullRow<T>::timesTransposeFullMatrix(std::shared_ptr<FullMatrixDouble> fullMat)
{
//"a*bT = a(1,j)b(k,j)"
int ncol = fullMat->nrow();
@@ -37,7 +37,7 @@ inline FRowsptr<T> FullRow<T>::timesTransposeFullMatrix(FMatsptr<T> fullMat)
}
template<typename T>
inline FRowsptr<T> FullRow<T>::timesFullMatrix(FMatsptr<T> fullMat)
inline FRowsptr<T> FullRow<T>::timesFullMatrix(std::shared_ptr<FullMatrixDouble> fullMat)
{
FRowsptr<T> answer = fullMat->at(0)->times(this->at(0));
for (int j = 1; j < (int) this->size(); j++)

6
OndselSolver/FullRow.h
View File

@@ -30,14 +30,14 @@ namespace MbD {
FRowsptr<T> minusFullRow(FRowsptr<T> fullRow);
T timesFullColumn(FColsptr<T> fullCol);
T timesFullColumn(FullColumn<T>* fullCol);
FRowsptr<T> timesFullMatrix(std::shared_ptr<FullMatrix<T>> fullMat);
FRowsptr<T> timesTransposeFullMatrix(std::shared_ptr<FullMatrix<T>> fullMat);
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<FullMatrix<T>> transposeTimesFullRow(FRowsptr<T> fullRow);
std::shared_ptr<FullMatrixDouble> transposeTimesFullRow(std::shared_ptr<FullMatrixDouble> fullRow);
std::ostream& printOn(std::ostream& s) const override;
};

2
OndselSolver/GeneralSpline.cpp
View File

@@ -119,7 +119,7 @@ void MbD::GeneralSpline::computeDerivatives()
}
auto solver = CREATE<GESpMatParPvMarkoFast>::With();
auto derivsVector = solver->solvewithsaveOriginal(matrix, bvector, false);
derivs = std::make_shared<FullMatrix<double>>(n, p);
derivs = std::make_shared<FullMatrixDouble>(n, p);
auto hmaxpowers = std::make_shared<FullColumn<double>>(p);
for (int j = 0; j < p; j++)
{

2
OndselSolver/LDUFullMat.cpp
View File

@@ -116,7 +116,7 @@ FMatDsptr LDUFullMat::inversesaveOriginal(FMatDsptr fullMat, bool saveOriginal)
this->decomposesaveOriginal(fullMat, saveOriginal);
rightHandSideB = std::make_shared<FullColumn<double>>(m);
auto matrixAinverse = std::make_shared <FullMatrix<double>>(m, n);
auto matrixAinverse = std::make_shared <FullMatrixDouble>(m, n);
for (int j = 0; j < n; j++)
{
rightHandSideB->zeroSelf();

6
OndselSolver/MBDynBody.cpp
View File

@@ -60,7 +60,7 @@ void MbD::MBDynBody::readInertiaMatrix(std::vector<std::string>& args)
{
auto parser = std::make_shared<SymbolicParser>();
parser->variables = mbdynVariables();
aJmat = std::make_shared<FullMatrix<double>>(3, 3);
aJmat = std::make_shared<FullMatrixDouble>(3, 3);
auto& str = args.at(0);
if (str.find("diag") != std::string::npos) {
args.erase(args.begin());
@@ -89,7 +89,7 @@ void MbD::MBDynBody::createASMT()
if (aJmat->isDiagonalToWithin(1.0e-6)) {
asmtMassMarker->setMomentOfInertias(aJmat->asDiagonalMatrix());
asmtMassMarker->setPosition3D(rPcmP);
asmtMassMarker->setRotationMatrix(FullMatrix<double>::identitysptr(3));
asmtMassMarker->setRotationMatrix(FullMatrixDouble::identitysptr(3));
auto asmtPart = asmtAssembly()->partPartialNamed(nodeName);
asmtPart->setPrincipalMassMarker(asmtMassMarker);
}
@@ -98,7 +98,7 @@ void MbD::MBDynBody::createASMT()
solver->setm(mass);
solver->setJPP(aJmat);
solver->setrPoP(rPcmP);
solver->setAPo(FullMatrix<double>::identitysptr(3));
solver->setAPo(FullMatrixDouble::identitysptr(3));
solver->setrPcmP(rPcmP);
solver->calc();
asmtMassMarker->setMomentOfInertias(solver->aJpp);

6
OndselSolver/MBDynItem.cpp
View File

@@ -155,7 +155,7 @@ FColDsptr MbD::MBDynItem::readBasicPosition(std::vector<std::string>& args)
FMatDsptr MbD::MBDynItem::readOrientation(std::vector<std::string>& args)
{
auto aAOf = FullMatrix<double>::identitysptr(3);
auto aAOf = FullMatrixDouble::identitysptr(3);
if (args.empty()) return aAOf;
auto& str = args.at(0);
if (str.find("reference") != std::string::npos) {
@@ -202,7 +202,7 @@ FMatDsptr MbD::MBDynItem::readBasicOrientation(std::vector<std::string>& args)
}
if (str.find("eye") != std::string::npos) {
args.erase(args.begin());
auto aAFf = FullMatrix<double>::identitysptr(3);
auto aAFf = FullMatrixDouble::identitysptr(3);
return aAFf;
}
auto iss = std::istringstream(str);
@@ -251,7 +251,7 @@ 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);

2
OndselSolver/MBDynJoint.cpp
View File

@@ -118,7 +118,7 @@ void MbD::MBDynJoint::parseMBDyn(std::string line)
mkr1->owner = this;
mkr1->nodeStr = "Assembly";
mkr1->rPmP = std::make_shared<FullColumn<double>>(3);
mkr1->aAPm = FullMatrix<double>::identitysptr(3);
mkr1->aAPm = FullMatrixDouble::identitysptr(3);
readMarkerJ(arguments);
return;
}

2
OndselSolver/MBDynMarker.cpp
View File

@@ -11,7 +11,7 @@ using namespace MbD;
void MbD::MBDynMarker::parseMBDyn(std::vector<std::string>& args)
{
rPmP = std::make_shared<FullColumn<double>>(3);
aAPm = FullMatrix<double>::identitysptr(3);
aAPm = FullMatrixDouble::identitysptr(3);
if (args.empty()) return;
auto& str = args.at(0);
if (str.find("reference") != std::string::npos) {

2
OndselSolver/MBDynStructural.cpp
View File

@@ -12,7 +12,7 @@ using namespace MbD;
MbD::MBDynStructural::MBDynStructural()
{
rOfO = std::make_shared<FullColumn<double>>(3);
aAOf = FullMatrix<double>::identitysptr(3);
aAOf = FullMatrixDouble::identitysptr(3);
vOfO = std::make_shared<FullColumn<double>>(3);
omeOfO = std::make_shared<FullColumn<double>>(3);
}

2
OndselSolver/MarkerFrame.cpp
View File

@@ -31,7 +31,7 @@ System* MarkerFrame::root()
void MarkerFrame::initialize()
{
prOmOpE = std::make_shared<FullMatrix<double>>(3, 4);
prOmOpE = std::make_shared<FullMatrixDouble>(3, 4);
pAOmpE = std::make_shared<FullColumn<FMatDsptr>>(4);
endFrames = std::make_shared<std::vector<EndFrmsptr>>();
auto endFrm = CREATE<EndFrameqc>::With();

8
OndselSolver/MomentOfInertiaSolver.cpp
View File

@@ -7,7 +7,7 @@ using namespace MbD;
void MbD::MomentOfInertiaSolver::example1()
{
auto aJpp = std::make_shared<FullMatrix<double>>(ListListD{
auto aJpp = std::make_shared<FullMatrixDouble>(ListListD{
{ 1, 0, 0 },
{ 0, 2, 0 },
{ 0, 0, 3 }
@@ -179,10 +179,10 @@ void MbD::MomentOfInertiaSolver::calcJoo()
if (!rPoP) {
rPoP = rPcmP;
aAPo = FullMatrix<double>::identitysptr(3);
aAPo = FullMatrixDouble::identitysptr(3);
}
auto rocmPtilde = FullMatrix<double>::tildeMatrix(rPcmP->minusFullColumn(rPoP));
auto rPoPtilde = FullMatrix<double>::tildeMatrix(rPoP);
auto rocmPtilde = FullMatrixDouble::tildeMatrix(rPcmP->minusFullColumn(rPoP));
auto rPoPtilde = FullMatrixDouble::tildeMatrix(rPoP);
auto term1 = aJPP;
auto term21 = rPoPtilde->timesFullMatrix(rPoPtilde);
auto term22 = rPoPtilde->timesFullMatrix(rocmPtilde);

6
OndselSolver/OrbitAnglezIeqcJec.cpp
View File

@@ -138,9 +138,9 @@ void MbD::OrbitAngleZIeqcJec::initialize()
OrbitAngleZIecJec::initialize();
pthezpXI = std::make_shared<FullRow<double>>(3);
pthezpEI = std::make_shared<FullRow<double>>(4);
ppthezpXIpXI = std::make_shared<FullMatrix<double>>(3, 3);
ppthezpXIpEI = std::make_shared<FullMatrix<double>>(3, 4);
ppthezpEIpEI = std::make_shared<FullMatrix<double>>(4, 4);
ppthezpXIpXI = std::make_shared<FullMatrixDouble>(3, 3);
ppthezpXIpEI = std::make_shared<FullMatrixDouble>(3, 4);
ppthezpEIpEI = std::make_shared<FullMatrixDouble>(4, 4);
}
FMatDsptr MbD::OrbitAngleZIeqcJec::ppvaluepEIpEI()

14
OndselSolver/OrbitAnglezIeqcJeqc.cpp
View File

@@ -242,13 +242,13 @@ void MbD::OrbitAngleZIeqcJeqc::initialize()
OrbitAngleZIeqcJec::initialize();
pthezpXJ = std::make_shared<FullRow<double>>(3);
pthezpEJ = std::make_shared<FullRow<double>>(4);
ppthezpXIpXJ = std::make_shared<FullMatrix<double>>(3, 3);
ppthezpXIpEJ = std::make_shared<FullMatrix<double>>(3, 4);
ppthezpEIpXJ = std::make_shared<FullMatrix<double>>(4, 3);
ppthezpEIpEJ = std::make_shared<FullMatrix<double>>(4, 4);
ppthezpXJpXJ = std::make_shared<FullMatrix<double>>(3, 3);
ppthezpXJpEJ = std::make_shared<FullMatrix<double>>(3, 4);
ppthezpEJpEJ = std::make_shared<FullMatrix<double>>(4, 4);
ppthezpXIpXJ = std::make_shared<FullMatrixDouble>(3, 3);
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);
ppthezpXJpEJ = std::make_shared<FullMatrixDouble>(3, 4);
ppthezpEJpEJ = std::make_shared<FullMatrixDouble>(4, 4);
}
FMatDsptr MbD::OrbitAngleZIeqcJeqc::ppvaluepEIpEJ()

4
OndselSolver/Part.cpp
View File

@@ -33,8 +33,8 @@ void Part::initialize()
partFrame = CREATE<PartFrame>::With();
partFrame->setPart(this);
pTpE = std::make_shared<FullColumn<double>>(4);
ppTpEpE = std::make_shared<FullMatrix<double>>(4, 4);
ppTpEpEdot = std::make_shared<FullMatrix<double>>(4, 4);
ppTpEpE = std::make_shared<FullMatrixDouble>(4, 4);
ppTpEpEdot = std::make_shared<FullMatrixDouble>(4, 4);
}
void Part::initializeLocally()

10
OndselSolver/RowTypeMatrix.cpp
View File

@@ -9,13 +9,3 @@
#include "RowTypeMatrix.h"
using namespace MbD;
template<typename T>
int RowTypeMatrix<T>::nrow() {
return (int) this->size();
}
template<typename T>
int RowTypeMatrix<T>::ncol() {
return this->at(0)->numberOfElements();
}

8
OndselSolver/RowTypeMatrix.h
View File

@@ -24,8 +24,12 @@ namespace MbD {
virtual void zeroSelf() override = 0;
//double maxMagnitude() override;
int numberOfElements() override;
int nrow();
int ncol();
int nrow() {
return (int) this->size();
}
int ncol() {
return this->at(0)->numberOfElements();
}
};
template<typename T>

2
OndselSolver/StableBackwardDifference.cpp
View File

@@ -28,7 +28,7 @@ void StableBackwardDifference::formTaylorMatrix()
void StableBackwardDifference::instantiateTaylorMatrix()
{
if (taylorMatrix == nullptr || (taylorMatrix->nrow() != (order))) {
taylorMatrix = std::make_shared<FullMatrix<double>>(order, order);
taylorMatrix = std::make_shared<FullMatrixDouble>(order, order);
}
}