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

(broken) gettin' some sleep

Browse Source
This commit is contained in:
John Dupuy
2023-11-04 00:04:07 -05:00
parent 67bef75bab
commit 8408c2245e
29 changed files with 1117 additions and 946 deletions
Download Patch File Download Diff File Expand all files Collapse all files

104
OndselSolver/ASMTMarker.cpp
View File

@@ -14,57 +14,57 @@
#include "MarkerFrame.h"
#include "ASMTPrincipalMassMarker.h"
using namespace MbD;
namespace MbD {
void ASMTMarker::parseASMT(std::vector<std::string>& lines)
{
readName(lines);
readPosition3D(lines);
readRotationMatrix(lines);
}
void MbD::ASMTMarker::parseASMT(std::vector<std::string>& lines)
{
readName(lines);
readPosition3D(lines);
readRotationMatrix(lines);
}
FColDsptr MbD::ASMTMarker::rpmp()
{
//p is cm
auto refItem = static_cast<ASMTRefItem*>(owner);
auto& rPrefP = refItem->position3D;
auto& aAPref = refItem->rotationMatrix;
auto& rrefmref = position3D;
auto rPmP = rPrefP->plusFullColumn(aAPref->timesFullColumn(rrefmref));
auto& principalMassMarker = static_cast<ASMTPart*>(refItem->owner)->principalMassMarker;
auto& rPcmP = principalMassMarker->position3D;
auto& aAPcm = principalMassMarker->rotationMatrix;
auto rpmp = aAPcm->transposeTimesFullColumn(rPmP->minusFullColumn(rPcmP));
return rpmp;
}
FMatDsptr MbD::ASMTMarker::aApm()
{
//p is cm
auto refItem = static_cast<ASMTRefItem*>(owner);
auto& aAPref = refItem->rotationMatrix;
auto& aArefm = rotationMatrix;
auto& principalMassMarker = static_cast<ASMTPart*>(refItem->owner)->principalMassMarker;
auto& aAPcm = principalMassMarker->rotationMatrix;
auto aApm = aAPcm->transposeTimesFullMatrix(aAPref->timesFullMatrix(aArefm));
return toFMDsptr(aApm);
}
void MbD::ASMTMarker::createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits)
{
auto mkr = CREATE<MarkerFrame>::With(name.c_str());
auto prt = std::static_pointer_cast<Part>(partOrAssembly()->mbdObject);
prt->partFrame->addMarkerFrame(mkr);
mkr->rpmp = rpmp()->times(1.0 / mbdUnits->length);
mkr->aApm = aApm();
mbdObject = mkr->endFrames->at(0);
}
void MbD::ASMTMarker::storeOnLevel(std::ofstream& os, int level)
{
storeOnLevelString(os, level, "Marker");
storeOnLevelString(os, level + 1, "Name");
storeOnLevelString(os, level + 2, name);
ASMTSpatialItem::storeOnLevel(os, level);
FColDsptr ASMTMarker::rpmp()
{
//p is cm
auto refItem = static_cast<ASMTRefItem*>(owner);
auto& rPrefP = refItem->position3D;
auto& aAPref = refItem->rotationMatrix;
auto& rrefmref = position3D;
auto rPmP = rPrefP->plusFullColumn(aAPref->timesFullColumn(rrefmref));
auto& principalMassMarker = static_cast<ASMTPart*>(refItem->owner)->principalMassMarker;
auto& rPcmP = principalMassMarker->position3D;
auto& aAPcm = principalMassMarker->rotationMatrix;
auto rpmp = aAPcm->transposeTimesFullColumn(rPmP->minusFullColumn(rPcmP));
return rpmp;
}
FMatDsptr ASMTMarker::aApm()
{
//p is cm
auto refItem = static_cast<ASMTRefItem*>(owner);
auto& aAPref = refItem->rotationMatrix;
auto& aArefm = rotationMatrix;
auto& principalMassMarker = static_cast<ASMTPart*>(refItem->owner)->principalMassMarker;
auto& aAPcm = principalMassMarker->rotationMatrix;
auto aApm = aAPcm->transposeTimesFullMatrix(aAPref->timesFullMatrix(aArefm));
return aApm;
}
void ASMTMarker::createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits)
{
auto mkr = CREATE<MarkerFrame>::With(name.c_str());
auto prt = std::static_pointer_cast<Part>(partOrAssembly()->mbdObject);
prt->partFrame->addMarkerFrame(mkr);
mkr->rpmp = rpmp()->times(1.0 / mbdUnits->length);
mkr->aApm = aApm();
mbdObject = mkr->endFrames->at(0);
}
void ASMTMarker::storeOnLevel(std::ofstream& os, int level)
{
storeOnLevelString(os, level, "Marker");
storeOnLevelString(os, level + 1, "Name");
storeOnLevelString(os, level + 2, name);
ASMTSpatialItem::storeOnLevel(os, level);
}
}

1
OndselSolver/ASMTMarker.h
View File

@@ -23,7 +23,6 @@ namespace MbD {
FMatDsptr aApm();
void createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits) override;
void storeOnLevel(std::ofstream& os, int level) override;
};
}

58
OndselSolver/ASMTMotion.cpp
View File

@@ -9,36 +9,36 @@
#include "ASMTMotion.h"
using namespace MbD;
namespace MbD {
void ASMTMotion::readMotionSeries(std::vector<std::string>& lines)
{
std::string str = lines[0];
std::string substr = "MotionSeries";
auto pos = str.find(substr);
assert(pos != std::string::npos);
str.erase(0, pos + substr.length());
auto seriesName = readString(str);
assert(fullName("") == seriesName);
lines.erase(lines.begin());
readFXonIs(lines);
readFYonIs(lines);
readFZonIs(lines);
readTXonIs(lines);
readTYonIs(lines);
readTZonIs(lines);
}
void MbD::ASMTMotion::readMotionSeries(std::vector<std::string>& lines)
{
std::string str = lines[0];
std::string substr = "MotionSeries";
auto pos = str.find(substr);
assert(pos != std::string::npos);
str.erase(0, pos + substr.length());
auto seriesName = readString(str);
assert(fullName("") == seriesName);
lines.erase(lines.begin());
readFXonIs(lines);
readFYonIs(lines);
readFZonIs(lines);
readTXonIs(lines);
readTYonIs(lines);
readTZonIs(lines);
}
void ASMTMotion::initMarkers()
{
}
void MbD::ASMTMotion::initMarkers()
{
}
void ASMTMotion::storeOnLevel(std::ofstream& os, int level)
{
assert(false);
}
void MbD::ASMTMotion::storeOnLevel(std::ofstream& os, int level)
{
assert(false);
}
void MbD::ASMTMotion::storeOnTimeSeries(std::ofstream& os)
{
assert(false);
void ASMTMotion::storeOnTimeSeries(std::ofstream& os)
{
assert(false);
}
}

6
OndselSolver/DiagonalMatrix.h
View File

@@ -26,7 +26,7 @@ namespace MbD {
void atiputDiagonalMatrix(int i, std::shared_ptr<DiagonalMatrix<T>> diagMat);
DiagMatsptr<T> times(T factor);
FColsptr<T> timesFullColumn(FColsptr<T> fullCol);
FMatsptr<T> timesFullMatrix(FMatsptr<T> fullMat);
FMatDsptr timesFullMatrix(FMatDsptr fullMat);
int nrow() {
return (int)this->size();
}
@@ -79,10 +79,10 @@ namespace MbD {
return answer;
}
template<typename T>
inline FMatsptr<T> DiagonalMatrix<T>::timesFullMatrix(FMatsptr<T> fullMat)
inline FMatDsptr DiagonalMatrix<T>::timesFullMatrix(FMatDsptr fullMat)
{
auto nrow = (int)this->size();
auto answer = std::make_shared<FullMatrix<T>>(nrow);
auto answer = std::make_shared<FullMatrixDouble>(nrow);
for (int i = 0; i < nrow; i++)
{
answer->at(i) = fullMat->at(i)->times(this->at(i));

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 = toFMDsptr(std::static_pointer_cast<EndFrameqc>(frmI)->ppriOeOpEpE(axis)->negated());
ppriIeJeOpEIpEI = 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 = toFMDsptr(pAjOIepEIT->timesFullMatrix(prIeJeOpEJ));
ppriIeJeIepEIpEJ = 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 = toFMDsptr(std::static_pointer_cast<EndFrameqct>(frmI)->ppriOeOpEpE(axis)->negated());
ppriIeJeOpEIpEI = 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 = 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 = ppGpEIpEI->plusFullMatrix(xIeJeIe->ppvaluepEIpEI()->times(xIeJeIe->value()));
ppGpEIpEI = ppGpEIpEI->plusFullMatrix(yIeJeIe->pvaluepEI()->transposeTimesFullRow(yIeJeIe->pvaluepEI()));
ppGpEIpEI = ppGpEIpEI->plusFullMatrix(yIeJeIe->ppvaluepEIpEI()->times(yIeJeIe->value()));
ppGpEIpEI->magnifySelf(2.0);
}
void MbD::DistancexyConstraintIqcJc::calc_ppGpXIpEI()
{
ppGpXIpEI = (xIeJeIe->pvaluepXI()->transposeTimesFullRow(xIeJeIe->pvaluepEI()));
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 = ppGpXIpEI->plusFullMatrix(xIeJeIe->ppvaluepXIpEI()->times(xIeJeIe->value()));
ppGpXIpEI = ppGpXIpEI->plusFullMatrix(yIeJeIe->pvaluepXI()->transposeTimesFullRow(yIeJeIe->pvaluepEI()));
ppGpXIpEI = 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 = toFMDsptr(ppGpXIpXI->plusFullMatrix(yIeJeIe->pvaluepXI()->transposeTimesFullRow(yIeJeIe->pvaluepXI())));
ppGpXIpXI = 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 = toFMDsptr(ppGpXIpXJ->plusFullMatrix(yIeJeIe->pvaluepXI()->transposeTimesFullRow(yIeJeIe->pvaluepXJ())));
ppGpXIpXJ = ppGpXIpXJ->plusFullMatrix(yIeJeIe->pvaluepXI()->transposeTimesFullRow(yIeJeIe->pvaluepXJ()));
ppGpXIpXJ->magnifySelf(2.0);
}
void MbD::DistancexyConstraintIqcJqc::calc_ppGpEIpXJ()
{
ppGpEIpXJ = (xIeJeIe->pvaluepEI()->transposeTimesFullRow(xIeJeIe->pvaluepXJ()));
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 = ppGpEIpXJ->plusFullMatrix(xIeJeIe->ppvaluepEIpXJ()->times(xIeJeIe->value()));
ppGpEIpXJ = ppGpEIpXJ->plusFullMatrix(yIeJeIe->pvaluepEI()->transposeTimesFullRow(yIeJeIe->pvaluepXJ()));
ppGpEIpXJ = 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 = toFMDsptr(ppGpXJpXJ->plusFullMatrix(yIeJeIe->pvaluepXJ()->transposeTimesFullRow(yIeJeIe->pvaluepXJ())));
ppGpXJpXJ = 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 = toFMDsptr(ppGpXIpEJ->plusFullMatrix(yIeJeIe->pvaluepXI()->transposeTimesFullRow(yIeJeIe->pvaluepEJ())));
ppGpXIpEJ = ppGpXIpEJ->plusFullMatrix(yIeJeIe->pvaluepXI()->transposeTimesFullRow(yIeJeIe->pvaluepEJ()));
ppGpXIpEJ->magnifySelf(2.0);
}
void MbD::DistancexyConstraintIqcJqc::calc_ppGpEIpEJ()
{
ppGpEIpEJ = (xIeJeIe->pvaluepEI()->transposeTimesFullRow(xIeJeIe->pvaluepEJ()));
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 = ppGpEIpEJ->plusFullMatrix(xIeJeIe->ppvaluepEIpEJ()->times(xIeJeIe->value()));
ppGpEIpEJ = ppGpEIpEJ->plusFullMatrix(yIeJeIe->pvaluepEI()->transposeTimesFullRow(yIeJeIe->pvaluepEJ()));
ppGpEIpEJ = 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 = toFMDsptr(ppGpXJpEJ->plusFullMatrix(yIeJeIe->pvaluepXJ()->transposeTimesFullRow(yIeJeIe->pvaluepEJ())));
ppGpXJpEJ = ppGpXJpEJ->plusFullMatrix(yIeJeIe->pvaluepXJ()->transposeTimesFullRow(yIeJeIe->pvaluepEJ()));
ppGpXJpEJ->magnifySelf(2.0);
}
void MbD::DistancexyConstraintIqcJqc::calc_ppGpEJpEJ()
{
ppGpEJpEJ = (xIeJeIe->pvaluepEJ()->transposeTimesFullRow(xIeJeIe->pvaluepEJ()));
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 = ppGpEJpEJ->plusFullMatrix(xIeJeIe->ppvaluepEJpEJ()->times(xIeJeIe->value()));
ppGpEJpEJ = ppGpEJpEJ->plusFullMatrix(yIeJeIe->pvaluepEJ()->transposeTimesFullRow(yIeJeIe->pvaluepEJ()));
ppGpEJpEJ = ppGpEJpEJ->plusFullMatrix(yIeJeIe->ppvaluepEJpEJ()->times(yIeJeIe->value()));
ppGpEJpEJ->magnifySelf(2.0);
}

2
OndselSolver/EndFramec.cpp
View File

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

4
OndselSolver/EndFrameqc.cpp
View File

@@ -26,7 +26,7 @@ EndFrameqc::EndFrameqc(const char* str) : EndFramec(str) {
void EndFrameqc::initialize()
{
prOeOpE = std::make_shared<FullMatrixDouble>(3, 4);
pprOeOpEpE = std::make_shared<FullMatrix<FColDsptr>>(4, 4);
pprOeOpEpE = std::make_shared<FullMatrixFullColumnDouble>(4, 4);
pAOepE = std::make_shared<FullColumn<FMatDsptr>>(4);
ppAOepEpE = std::make_shared<FullMatrixFullMatrixDouble>(4, 4);
}
@@ -59,7 +59,7 @@ void MbD::EndFrameqc::initEndFrameqct2()
FMatFColDsptr EndFrameqc::ppAjOepEpE(int jj)
{
auto answer = std::make_shared<FullMatrix<FColDsptr>>(4, 4);
auto answer = std::make_shared<FullMatrixFullColumnDouble>(4, 4);
for (int i = 0; i < 4; i++) {
auto& answeri = answer->at(i);
auto& ppAOepEipE = ppAOepEpE->at(i);

16
OndselSolver/EndFrameqct.cpp
View File

@@ -142,12 +142,12 @@ void EndFrameqct::calcPostDynCorrectorIteration()
}
auto rpep = markerFrame->rpmp->plusFullColumn(markerFrame->aApm->timesFullColumn(rmem));
pprOeOpEpE = EulerParameters<double>::ppApEpEtimesColumn(rpep);
aAOe = toFMDsptr(aAOm->timesFullMatrix(aAme));
aAOe = aAOm->timesFullMatrix(aAme);
for (int i = 0; i < 4; i++)
{
pAOepE->at(i) = toFMDsptr(pAOmpE->at(i)->timesFullMatrix(aAme));
pAOepE->at(i) = pAOmpE->at(i)->timesFullMatrix(aAme);
}
auto aApe = toFMDsptr(markerFrame->aApm->timesFullMatrix(aAme));
auto aApe = 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 = toFMDsptr(aAOm->timesFullMatrix(pAmept));
pAOept = aAOm->timesFullMatrix(pAmept);
}
void EndFrameqct::postVelIC()
@@ -213,7 +213,7 @@ void EndFrameqct::postVelIC()
}
for (int i = 0; i < 4; i++)
{
ppAOepEpt->atiput(i, toFMDsptr(pAOmpE->at(i)->timesFullMatrix(pAmept)));
ppAOepEpt->atiput(i, pAOmpE->at(i)->timesFullMatrix(pAmept));
}
}
@@ -326,7 +326,7 @@ void EndFrameqct::evalppAmeptpt()
phiThePsi->calc();
phiThePsiDot->calc();
phiThePsiDDot->calc();
ppAmeptpt = toFMDsptr(phiThePsiDDot->aAddot);
ppAmeptpt = phiThePsiDDot->aAddot;
}
}
@@ -353,11 +353,11 @@ void EndFrameqct::preAccIC()
this->evalpAmept();
auto& aAOm = markerFrame->aAOm;
prOeOpt = aAOm->timesFullColumn(prmempt);
pAOept = toFMDsptr(aAOm->timesFullMatrix(pAmept));
pAOept = aAOm->timesFullMatrix(pAmept);
Item::preAccIC();
this->evalpprmemptpt();
this->evalppAmeptpt();
aAOm = markerFrame->aAOm;
pprOeOptpt = aAOm->timesFullColumn(pprmemptpt);
ppAOeptpt = toFMDsptr(aAOm->timesFullMatrix(ppAmeptpt));
ppAOeptpt = aAOm->timesFullMatrix(ppAmeptpt);
}

78
OndselSolver/EulerAngles.cpp
View File

@@ -7,3 +7,81 @@
***************************************************************************/
#include "EulerAngles.h"
namespace MbD {
template<typename T>
inline void EulerAngles<T>::initialize()
{
assert(false);
}
template<>
inline void EulerAngles<Symsptr>::calc()
{
cA = std::make_shared<FullColumn<FMatDsptr>>(3);
for (int i = 0; i < 3; i++)
{
auto axis = rotOrder->at(i);
auto angle = this->at(i)->getValue();
if (axis == 1) {
cA->atiput(i, FullMatrixDouble::rotatex(angle));
}
else if (axis == 2) {
cA->atiput(i, FullMatrixDouble::rotatey(angle));
}
else if (axis == 3) {
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.");
}
}
aA = cA->at(0)->timesFullMatrix(cA->at(1)->timesFullMatrix(cA->at(2)));
}
template<>
inline void EulerAngles<double>::calc()
{
cA = std::make_shared<FullColumn<FMatDsptr>>(3);
for (int i = 0; i < 3; i++)
{
auto axis = rotOrder->at(i);
auto angle = this->at(i);
if (axis == 1) {
cA->atiput(i, FullMatrixDouble::rotatex(angle));
}
else if (axis == 2) {
cA->atiput(i, FullMatrixDouble::rotatey(angle));
}
else if (axis == 3) {
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.");
}
}
aA = cA->at(0)->timesFullMatrix(cA->at(1)->timesFullMatrix(cA->at(2)));
}
template<typename T>
inline void EulerAngles<T>::calc()
{
assert(false);
}
template<typename T>
inline std::shared_ptr<EulerAnglesDot<T>> EulerAngles<T>::differentiateWRT(T var)
{
auto derivatives = std::make_shared<EulerAnglesDot<T>>();
std::transform(this->begin(), this->end(), derivatives->begin(),
[var](T term) { return term->differentiateWRT(var); }
);
derivatives->aEulerAngles = this;
return derivatives;
}
template<typename T>
inline void EulerAngles<T>::setRotOrder(int i, int j, int k)
{
rotOrder = std::make_shared<FullColumn<int>>(3);
rotOrder->at(0) = i;
rotOrder->at(1) = j;
rotOrder->at(2) = k;
}
}

76
OndselSolver/EulerAngles.h
View File

@@ -16,8 +16,6 @@
#include "Symbolic.h"
namespace MbD {
//template<typename T>
//class EulerAnglesDot;
template<typename T>
class EulerAngles : public EulerArray<T>
@@ -37,79 +35,5 @@ namespace MbD {
FMatDsptr aA;
};
template<typename T>
inline void EulerAngles<T>::initialize()
{
assert(false);
}
template<>
inline void EulerAngles<Symsptr>::calc()
{
cA = std::make_shared<FullColumn<FMatDsptr>>(3);
for (int i = 0; i < 3; i++)
{
auto axis = rotOrder->at(i);
auto angle = this->at(i)->getValue();
if (axis == 1) {
cA->atiput(i, toFMDsptr(FullMatrixDouble::rotatex(angle)));
}
else if (axis == 2) {
cA->atiput(i, toFMDsptr(FullMatrixDouble::rotatey(angle)));
}
else if (axis == 3) {
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 = toFMDsptr(cA->at(0)->timesFullMatrix(cA->at(1)->timesFullMatrix(cA->at(2))));
}
template<>
inline void EulerAngles<double>::calc()
{
cA = std::make_shared<FullColumn<FMatDsptr>>(3);
for (int i = 0; i < 3; i++)
{
auto axis = rotOrder->at(i);
auto angle = this->at(i);
if (axis == 1) {
cA->atiput(i, toFMDsptr(FullMatrixDouble::rotatex(angle)));
}
else if (axis == 2) {
cA->atiput(i, toFMDsptr(FullMatrixDouble::rotatey(angle)));
}
else if (axis == 3) {
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 = toFMDsptr(cA->at(0)->timesFullMatrix(cA->at(1)->timesFullMatrix(cA->at(2))));
}
template<typename T>
inline void EulerAngles<T>::calc()
{
assert(false);
}
template<typename T>
inline std::shared_ptr<EulerAnglesDot<T>> EulerAngles<T>::differentiateWRT(T var)
{
auto derivatives = std::make_shared<EulerAnglesDot<T>>();
std::transform(this->begin(), this->end(), derivatives->begin(),
[var](T term) { return term->differentiateWRT(var); }
);
derivatives->aEulerAngles = this;
return derivatives;
}
template<typename T>
inline void EulerAngles<T>::setRotOrder(int i, int j, int k)
{
rotOrder = std::make_shared<FullColumn<int>>(3);
rotOrder->at(0) = i;
rotOrder->at(1) = j;
rotOrder->at(2) = k;
}
}

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

10
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, toFMDsptr(FullMatrixDouble::rotatexrotDot(angle, angleDot)));
cAdot->atiput(i, FullMatrixDouble::rotatexrotDot(angle, angleDot));
}
else if (axis == 2) {
cAdot->atiput(i, toFMDsptr(FullMatrixDouble::rotateyrotDot(angle, angleDot)));
cAdot->atiput(i, FullMatrixDouble::rotateyrotDot(angle, angleDot));
}
else if (axis == 3) {
cAdot->atiput(i, toFMDsptr(FullMatrixDouble::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.");
@@ -78,9 +78,9 @@ namespace MbD {
auto theAdot = cAdot->at(1);
auto psiAdot = cAdot->at(2);
aAdot = toFMDsptr(phiAdot->timesFullMatrix(theA->timesFullMatrix(psiA))
aAdot = phiAdot->timesFullMatrix(theA->timesFullMatrix(psiA))
->plusFullMatrix(phiA->timesFullMatrix(theAdot->timesFullMatrix(psiA)))
->plusFullMatrix(phiA->timesFullMatrix(theA->timesFullMatrix(psiAdot))));
->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)));

2
OndselSolver/EulerAngleszxz.h
View File

@@ -67,7 +67,7 @@ namespace MbD {
psiAi = psiA->at(1);
psiAi->at(0) = spsi;
psiAi->at(1) = cpsi;
aA = toFMDsptr(phiA->timesFullMatrix(theA->timesFullMatrix(psiA)));
aA = phiA->timesFullMatrix(theA->timesFullMatrix(psiA));
}
}

4
OndselSolver/EulerAngleszxzDDot.h
View File

@@ -25,7 +25,7 @@ namespace MbD {
std::shared_ptr<EulerAngleszxzDot<double>> phiThePsiDot;
FMatDsptr phiAddot, theAddot, psiAddot;
std::shared_ptr<FullMatrix<double>> aAddot;
std::shared_ptr<FullMatrixDouble> aAddot;
};
template<typename T>
inline void EulerAngleszxzDDot<T>::initialize()
@@ -92,7 +92,7 @@ namespace MbD {
+ *(phiAdot->timesFullMatrix(theA->timesFullMatrix(psiAdot)))
+ *(phiA->timesFullMatrix(theAdot->timesFullMatrix(psiAdot)))
+ *(phiA->timesFullMatrix(theA->timesFullMatrix(psiAddot)));
aAddot = std::make_shared<FullMatrix<double>>(mat);
aAddot = std::make_shared<FullMatrixDouble>(mat);
}
}

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 = toFMDsptr((term1->plusFullMatrix(term2))->plusFullMatrix(term3));
aAdot = (term1->plusFullMatrix(term2))->plusFullMatrix(term3);
}
}

6
OndselSolver/EulerParameters.cpp
View File

@@ -28,7 +28,7 @@ inline FMatFColDsptr EulerParameters<double>::ppApEpEtimesColumn(FColDsptr col)
auto col22 = std::make_shared<FullColumn<double>>(ListD{ m2c0, m2c1, a2c2 });
auto col23 = std::make_shared<FullColumn<double>>(ListD{ m2c1, a2c0, 0 });
auto col33 = std::make_shared<FullColumn<double>>(ListD{ a2c0, a2c1, a2c2 });
auto answer = std::make_shared<FullMatrix<FColDsptr>>(4, 4);
auto answer = std::make_shared<FullMatrixFullColumnDouble>(4, 4);
auto& row0 = answer->at(0);
row0->at(0) = col00;
row0->at(1) = col01;
@@ -136,7 +136,7 @@ inline FMatFMatDsptr EulerParameters<double>::ppApEpEtimesMatrix(FMatDsptr mat)
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 answer = std::make_shared<FullMatrixFullMatrixDouble>(4, 4);
auto& row0 = answer->at(0);
row0->at(0) = mat00;
row0->at(1) = mat01;
@@ -157,7 +157,7 @@ inline FMatFMatDsptr EulerParameters<double>::ppApEpEtimesMatrix(FMatDsptr mat)
row3->at(1) = mat13;
row3->at(2) = mat23;
row3->at(3) = mat33;
return toFMFMDsptr(answer);
return answer;
}
template<>

2
OndselSolver/EulerParameters.h
View File

@@ -41,7 +41,7 @@ namespace MbD {
this->calc();
}
static std::shared_ptr<FullMatrix<FColsptr<T>>> ppApEpEtimesColumn(FColDsptr col);
static std::shared_ptr<FullMatrixFullColumnDouble> ppApEpEtimesColumn(FColDsptr col);
static FMatDsptr pCpEtimesColumn(FColDsptr col);
static FMatDsptr pCTpEtimesColumn(FColDsptr col);
static std::shared_ptr<FullMatrixFullMatrixDouble> ppApEpEtimesMatrix(FMatDsptr mat);

1462
OndselSolver/FullMatrix.cpp
View File

File diff suppressed because it is too large Load Diff

124
OndselSolver/FullMatrix.h
View File

@@ -21,64 +21,58 @@
namespace MbD {
template<typename T>
class FullMatrix;
template<typename T>
using FMatsptr = std::shared_ptr<FullMatrix<T>>;
template<typename T>
class FullMatrix : public RowTypeMatrix<FRowsptr<T>>
class FullMatrixTemplate : public RowTypeMatrix<FRowsptr<T>>
{
public:
FullMatrix() {};
explicit FullMatrix(int m) : RowTypeMatrix<FRowsptr<T>>(m)
FullMatrixTemplate() = default;
explicit FullMatrixTemplate(int m) : RowTypeMatrix<FRowsptr<T>>(m)
{
}
FullMatrix(int m, int n) {
FullMatrixTemplate(int m, int n) {
for (int i = 0; i < m; i++) {
auto row = std::make_shared<FullRow<T>>(n);
this->push_back(row);
}
}
FullMatrix(std::initializer_list<FRowsptr<T>> listOfRows) {
FullMatrixTemplate(std::initializer_list<FRowsptr<T>> listOfRows) {
for (auto& row : listOfRows)
{
this->push_back(row);
}
}
FullMatrix(std::initializer_list<std::initializer_list<T>> list2D) {
FullMatrixTemplate(std::initializer_list<std::initializer_list<T>> list2D) {
for (auto& rowList : list2D)
{
auto row = std::make_shared<FullRow<T>>(rowList);
this->push_back(row);
}
}
static FMatsptr<T> rotatex(T angle);
static FMatsptr<T> rotatey(T angle);
static FMatsptr<T> rotatez(T angle);
static FMatsptr<T> rotatexrotDot(T angle, T angledot);
static FMatsptr<T> rotateyrotDot(T angle, T angledot);
static FMatsptr<T> rotatezrotDot(T angle, T angledot);
static FMatsptr<T> rotatexrotDotrotDDot(T angle, T angleDot, T angleDDot);
static FMatsptr<T> rotateyrotDotrotDDot(T angle, T angleDot, T angleDDot);
static FMatsptr<T> rotatezrotDotrotDDot(T angle, T angleDot, T angleDDot);
static FMatsptr<T> identitysptr(int n);
static FMatsptr<T> tildeMatrix(FColDsptr col);
// static std::shared_ptr<FullMatrixTemplate<T>> rotatex(T angle);
// static std::shared_ptr<FullMatrixTemplate<T>> rotatey(T angle);
// static std::shared_ptr<FullMatrixTemplate<T>> rotatez(T angle);
// static std::shared_ptr<FullMatrixTemplate<T>> rotatexrotDot(T angle, T angledot);
// static std::shared_ptr<FullMatrixTemplate<T>> rotateyrotDot(T angle, T angledot);
// static std::shared_ptr<FullMatrixTemplate<T>> rotatezrotDot(T angle, T angledot);
static std::shared_ptr<FullMatrixTemplate<T>> rotatexrotDotrotDDot(T angle, T angleDot, T angleDDot);
static std::shared_ptr<FullMatrixTemplate<T>> rotateyrotDotrotDDot(T angle, T angleDot, T angleDDot);
static std::shared_ptr<FullMatrixTemplate<T>> rotatezrotDotrotDDot(T angle, T angleDot, T angleDDot);
static std::shared_ptr<FullMatrixTemplate<T>> identitysptr(int n);
static std::shared_ptr<FullMatrixTemplate<T>> tildeMatrix(FColDsptr col);
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);
// std::shared_ptr<FullMatrixTemplate<T>> timesFullMatrix(std::shared_ptr<FullMatrixTemplate<T>> fullMat);
virtual FMatsptr<T> timesTransposeFullMatrix(FMatsptr<T> fullMat);
virtual std::shared_ptr<FullMatrixTemplate<T>> timesTransposeFullMatrix(std::shared_ptr<FullMatrixTemplate<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);
FMatsptr<T> transpose();
FMatsptr<T> negated();
std::shared_ptr<FullMatrixTemplate<T>> times(T a);
// std::shared_ptr<FullMatrixTemplate<T>> transposeTimesFullMatrix(std::shared_ptr<FullMatrixTemplate<T>> fullMat);
// std::shared_ptr<FullMatrixTemplate<T>> plusFullMatrix(std::shared_ptr<FullMatrixTemplate<T>> fullMat);
std::shared_ptr<FullMatrixTemplate<T>> minusFullMatrix(std::shared_ptr<FullMatrixTemplate<T>> fullMat);
// std::shared_ptr<FullMatrixTemplate<T>> transpose();
// std::shared_ptr<FullMatrixTemplate<T>> negated();
void symLowerWithUpper();
void atiput(int i, FRowsptr<T> fullRow) override;
void atijput(int i, int j, T value);
@@ -88,8 +82,8 @@ namespace MbD {
void atijminusNumber(int i, int j, T value);
double sumOfSquares() override;
void zeroSelf() override;
FMatsptr<T> copy();
FullMatrix<T> operator+(const FullMatrix<T> fullMat);
// std::shared_ptr<FullMatrixTemplate<T>> copy();
FullMatrixTemplate<T> operator+(const FullMatrixTemplate<T> fullMat);
FColsptr<T> transposeTimesFullColumn(const FColsptr<T> fullCol);
void magnifySelf(T factor);
std::shared_ptr<EulerParameters<T>> asEulerParameters();
@@ -107,51 +101,61 @@ namespace MbD {
//
// FULL MATRIX DOUBLE instantiation
//
class FullMatrixDouble : public FullMatrix<double> {
class FullMatrixDouble : public FullMatrixTemplate<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) {};
FullMatrixDouble() : FullMatrixTemplate<double>() {};
explicit FullMatrixDouble(int m) : FullMatrixTemplate<double>(m) {};
FullMatrixDouble(int m, int n) : FullMatrixTemplate<double>(m, n) {};
FullMatrixDouble(std::initializer_list<std::initializer_list<double>> list2D) : FullMatrixTemplate<double>(list2D) {}
FullMatrixDouble(std::initializer_list<FRowsptr<double>> listOfRows) : FullMatrixTemplate<double>(listOfRows) {};
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);
double sumOfSquares();
std::shared_ptr<FullMatrixDouble> transposeTimesFullMatrix(std::shared_ptr<FullMatrixDouble> fullMat);
std::shared_ptr<FullMatrixDouble> timesFullMatrix(std::shared_ptr<FullMatrixDouble> fullMat);
std::shared_ptr<FullMatrixDouble> transpose();
std::shared_ptr<FullMatrixDouble> negated();
std::shared_ptr<FullMatrixDouble> plusFullMatrix(std::shared_ptr<FullMatrixDouble> fullMat);
static std::shared_ptr<FullMatrixDouble> rotatex(double angle);
static std::shared_ptr<FullMatrixDouble> rotatey(double angle);
static std::shared_ptr<FullMatrixDouble> rotatez(double angle);
static std::shared_ptr<FullMatrixDouble> rotatexrotDot(double angle, double angledot);
static std::shared_ptr<FullMatrixDouble> rotateyrotDot(double angle, double angledot);
static std::shared_ptr<FullMatrixDouble> rotatezrotDot(double angle, double angledot);
std::shared_ptr<FullMatrixDouble> copy();
};
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> {
class FullMatrixFullMatrixDouble : public FullMatrixTemplate<FMatDsptr> {
public:
FullMatrixFullMatrixDouble() : FullMatrix<FMatDsptr>() {};
explicit FullMatrixFullMatrixDouble(int m) : FullMatrix<FMatDsptr>(m) {};
FullMatrixFullMatrixDouble(int m, int n) : FullMatrix<FMatDsptr>(m, n) {};
FullMatrixFullMatrixDouble() : FullMatrixTemplate<FMatDsptr>() {};
explicit FullMatrixFullMatrixDouble(int m) : FullMatrixTemplate<FMatDsptr>(m) {};
FullMatrixFullMatrixDouble(int m, int n) : FullMatrixTemplate<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>>;
//
// FULL MATRIX FULL COLUMN DOUBLE instantiation
//
class FullMatrixFullColumnDouble : public FullMatrixTemplate<FColDsptr> {
public:
FullMatrixFullColumnDouble() : FullMatrixTemplate<FColDsptr>() {};
explicit FullMatrixFullColumnDouble(int m) : FullMatrixTemplate<FColDsptr>(m) {};
FullMatrixFullColumnDouble(int m, int n) : FullMatrixTemplate<FColDsptr>(m, n) {};
std::shared_ptr<FullMatrixFullColumnDouble> times(double a);
std::shared_ptr<FullMatrixFullColumnDouble> timesTransposeFullMatrix(std::shared_ptr<FullMatrixFullColumnDouble> fullMat);
double sumOfSquares() override;
void identity() override;
static std::shared_ptr<MbD::FullMatrixFullColumnDouble> identitysptr(int n);
};
}

12
OndselSolver/FullMatrix.ref.h
View File

@@ -5,16 +5,14 @@
namespace MbD {
class FullMatrixDouble;
class FullMatrixFullMatrixDouble;
class FullMatrixFullColumnDouble;
// using FMatDsptr = std::shared_ptr<MbD::FullMatrixDouble>;
using FMatDsptr = std::shared_ptr<FullMatrixDouble>;
// template<typename T>
// using FMatsptr = std::shared_ptr<FullMatrix<T>>;
using FMatFMatDsptr = std::shared_ptr<FullMatrixFullMatrixDouble>;
// using FMatFColDsptr = std::shared_ptr<FullMatrix<FColDsptr>>;
// using FMatFMatDsptr = std::shared_ptr<FullMatrix<FMatDsptr>>;
//
// using FColFMatDsptr = std::shared_ptr<FullColumn<FMatDsptr>>;
using FColFMatDsptr = std::shared_ptr<FullColumn<FMatDsptr>>;
using FMatFColDsptr = std::shared_ptr<FullMatrixFullColumnDouble>;
}

8
OndselSolver/GearConstraintIqcJc.cpp
View File

@@ -52,18 +52,18 @@ void MbD::GearConstraintIqcJc::calc_pGpXI()
void MbD::GearConstraintIqcJc::calc_ppGpEIpEI()
{
ppGpEIpEI = toFMDsptr(orbitJeIe->ppvaluepEJpEJ()->plusFullMatrix(orbitIeJe->ppvaluepEIpEI()->times(this->ratio())));
ppGpEIpEI = orbitJeIe->ppvaluepEJpEJ()->plusFullMatrix(orbitIeJe->ppvaluepEIpEI()->times(this->ratio()));
}
void MbD::GearConstraintIqcJc::calc_ppGpXIpEI()
{
ppGpXIpEI = toFMDsptr(orbitJeIe->ppvaluepXJpEJ()->plusFullMatrix(orbitIeJe->ppvaluepXIpEI()->times(this->ratio())));
ppGpXIpEI = orbitJeIe->ppvaluepXJpEJ()->plusFullMatrix(orbitIeJe->ppvaluepXIpEI()->times(this->ratio()));
}
void MbD::GearConstraintIqcJc::calc_ppGpXIpXI()
{
ppGpXIpXI = toFMDsptr(orbitJeIe->ppvaluepXJpXJ()
->plusFullMatrix(orbitIeJe->ppvaluepXIpXI()->times(this->ratio())));
ppGpXIpXI = 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 = toFMDsptr(orbitJeIe->ppvaluepEIpEJ()->transpose()->plusFullMatrix(orbitIeJe->ppvaluepEIpEJ()->times(this->ratio())));
ppGpEIpEJ = orbitJeIe->ppvaluepEIpEJ()->transpose()->plusFullMatrix(orbitIeJe->ppvaluepEIpEJ()->times(this->ratio()));
}
void MbD::GearConstraintIqcJqc::calc_ppGpEIpXJ()
{
ppGpEIpXJ = toFMDsptr(orbitJeIe->ppvaluepXIpEJ()->transpose()->plusFullMatrix(orbitIeJe->ppvaluepEIpXJ()->times(this->ratio())));
ppGpEIpXJ = orbitJeIe->ppvaluepXIpEJ()->transpose()->plusFullMatrix(orbitIeJe->ppvaluepEIpXJ()->times(this->ratio()));
}
void MbD::GearConstraintIqcJqc::calc_ppGpEJpEJ()
{
ppGpEJpEJ = toFMDsptr(orbitJeIe->ppvaluepEIpEI()->plusFullMatrix(orbitIeJe->ppvaluepEJpEJ()->times(this->ratio())));
ppGpEJpEJ = orbitJeIe->ppvaluepEIpEI()->plusFullMatrix(orbitIeJe->ppvaluepEJpEJ()->times(this->ratio()));
}
void MbD::GearConstraintIqcJqc::calc_ppGpXIpEJ()
{
ppGpXIpEJ = toFMDsptr(orbitJeIe->ppvaluepEIpXJ()->transpose()->plusFullMatrix(orbitIeJe->ppvaluepXIpEJ()->times(this->ratio())));
ppGpXIpEJ = orbitJeIe->ppvaluepEIpXJ()->transpose()->plusFullMatrix(orbitIeJe->ppvaluepXIpEJ()->times(this->ratio()));
}
void MbD::GearConstraintIqcJqc::calc_ppGpXIpXJ()
{
ppGpXIpXJ = toFMDsptr(orbitJeIe->ppvaluepXIpXJ()->transpose()->plusFullMatrix(orbitIeJe->ppvaluepXIpXJ()->times(this->ratio())));
ppGpXIpXJ = orbitJeIe->ppvaluepXIpXJ()->transpose()->plusFullMatrix(orbitIeJe->ppvaluepXIpXJ()->times(this->ratio()));
}
void MbD::GearConstraintIqcJqc::calc_ppGpXJpEJ()
{
ppGpXJpEJ = toFMDsptr(orbitJeIe->ppvaluepXIpEI()->plusFullMatrix(orbitIeJe->ppvaluepXJpEJ()->times(this->ratio())));
ppGpXJpEJ = orbitJeIe->ppvaluepXIpEI()->plusFullMatrix(orbitIeJe->ppvaluepXJpEJ()->times(this->ratio()));
}
void MbD::GearConstraintIqcJqc::calc_ppGpXJpXJ()
{
ppGpXJpXJ = toFMDsptr(orbitJeIe->ppvaluepXIpXI()->plusFullMatrix(orbitIeJe->ppvaluepXJpXJ()->times(this->ratio())));
ppGpXJpXJ = orbitJeIe->ppvaluepXIpXI()->plusFullMatrix(orbitIeJe->ppvaluepXJpXJ()->times(this->ratio()));
}
void MbD::GearConstraintIqcJqc::calcPostDynCorrectorIteration()

2
OndselSolver/LDUFullMat.cpp
View File

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

2
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 = toFMDsptr(aApP->transpose());
auto aAPo = aApP->transpose();
solver->setAPo(aAPo);
solver->setrPcmP(rPoP);
solver->calc();

16
OndselSolver/SparseMatrix.h
View File

@@ -47,10 +47,10 @@ namespace MbD {
void zeroSelf() override;
void atijplusFullRow(int i, int j, FRowsptr<T> fullRow);
void atijplusFullColumn(int i, int j, FColsptr<T> fullCol);
void atijplusFullMatrix(int i, int j, FMatsptr<T> fullMat);
void atijminusFullMatrix(int i, int j, FMatsptr<T> fullMat);
void atijplusTransposeFullMatrix(int i, int j, FMatsptr<T> fullMat);
void atijplusFullMatrixtimes(int i, int j, FMatsptr<T> fullMat, T factor);
void atijplusFullMatrix(int i, int j, FMatDsptr fullMat);
void atijminusFullMatrix(int i, int j, FMatDsptr fullMat);
void atijplusTransposeFullMatrix(int i, int j, FMatDsptr fullMat);
void atijplusFullMatrixtimes(int i, int j, FMatDsptr fullMat, T factor);
void atijplusNumber(int i, int j, double value);
void atijminusNumber(int i, int j, double value);
void atijput(int i, int j, T value);
@@ -117,7 +117,7 @@ namespace MbD {
}
}
template<typename T>
inline void SparseMatrix<T>::atijplusFullMatrix(int i, int j, FMatsptr<T> fullMat)
inline void SparseMatrix<T>::atijplusFullMatrix(int i, int j, FMatDsptr fullMat)
{
for (int ii = 0; ii < fullMat->nrow(); ii++)
{
@@ -125,7 +125,7 @@ namespace MbD {
}
}
template<typename T>
inline void SparseMatrix<T>::atijminusFullMatrix(int i, int j, FMatsptr<T> fullMat)
inline void SparseMatrix<T>::atijminusFullMatrix(int i, int j, FMatDsptr fullMat)
{
for (int ii = 0; ii < fullMat->nrow(); ii++)
{
@@ -133,7 +133,7 @@ namespace MbD {
}
}
template<typename T>
inline void SparseMatrix<T>::atijplusTransposeFullMatrix(int i, int j, FMatsptr<T> fullMat)
inline void SparseMatrix<T>::atijplusTransposeFullMatrix(int i, int j, FMatDsptr fullMat)
{
for (int ii = 0; ii < fullMat->nrow(); ii++)
{
@@ -141,7 +141,7 @@ namespace MbD {
}
}
template<typename T>
inline void SparseMatrix<T>::atijplusFullMatrixtimes(int i, int j, FMatsptr<T> fullMat, T factor)
inline void SparseMatrix<T>::atijplusFullMatrixtimes(int i, int j, FMatDsptr fullMat, T factor)
{
for (int ii = 0; ii < fullMat->nrow(); ii++)
{