From 8408c2245e7a93d437c729b2da1118cdfba8fe9b Mon Sep 17 00:00:00 2001 From: John Dupuy Date: Sat, 4 Nov 2023 00:04:07 -0500 Subject: [PATCH] (broken) gettin' some sleep --- OndselSolver/ASMTMarker.cpp | 104 +- OndselSolver/ASMTMarker.h | 1 - OndselSolver/ASMTMotion.cpp | 58 +- OndselSolver/DiagonalMatrix.h | 6 +- OndselSolver/DispCompIeqcJecO.cpp | 2 +- OndselSolver/DispCompIeqcJeqcIe.cpp | 2 +- OndselSolver/DispCompIeqctJeqcO.cpp | 2 +- OndselSolver/DistancexyConstraintIqcJc.cpp | 14 +- OndselSolver/DistancexyConstraintIqcJqc.cpp | 26 +- OndselSolver/EndFramec.cpp | 2 +- OndselSolver/EndFrameqc.cpp | 4 +- OndselSolver/EndFrameqct.cpp | 16 +- OndselSolver/EulerAngles.cpp | 78 + OndselSolver/EulerAngles.h | 76 - OndselSolver/EulerAnglesDDot.h | 6 +- OndselSolver/EulerAnglesDot.h | 10 +- OndselSolver/EulerAngleszxz.h | 2 +- OndselSolver/EulerAngleszxzDDot.h | 4 +- OndselSolver/EulerAngleszxzDot.h | 2 +- OndselSolver/EulerParameters.cpp | 6 +- OndselSolver/EulerParameters.h | 2 +- OndselSolver/FullMatrix.cpp | 1462 +++++++++++-------- OndselSolver/FullMatrix.h | 124 +- OndselSolver/FullMatrix.ref.h | 12 +- OndselSolver/GearConstraintIqcJc.cpp | 8 +- OndselSolver/GearConstraintIqcJqc.cpp | 14 +- OndselSolver/LDUFullMat.cpp | 2 +- OndselSolver/MomentOfInertiaSolver.cpp | 2 +- OndselSolver/SparseMatrix.h | 16 +- 29 files changed, 1117 insertions(+), 946 deletions(-) diff --git a/OndselSolver/ASMTMarker.cpp b/OndselSolver/ASMTMarker.cpp index dd60b31..db8d78a 100644 --- a/OndselSolver/ASMTMarker.cpp +++ b/OndselSolver/ASMTMarker.cpp @@ -14,57 +14,57 @@ #include "MarkerFrame.h" #include "ASMTPrincipalMassMarker.h" -using namespace MbD; +namespace MbD { + void ASMTMarker::parseASMT(std::vector& lines) + { + readName(lines); + readPosition3D(lines); + readRotationMatrix(lines); + } -void MbD::ASMTMarker::parseASMT(std::vector& lines) -{ - readName(lines); - readPosition3D(lines); - readRotationMatrix(lines); -} - -FColDsptr MbD::ASMTMarker::rpmp() -{ - //p is cm - auto refItem = static_cast(owner); - auto& rPrefP = refItem->position3D; - auto& aAPref = refItem->rotationMatrix; - auto& rrefmref = position3D; - auto rPmP = rPrefP->plusFullColumn(aAPref->timesFullColumn(rrefmref)); - auto& principalMassMarker = static_cast(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(owner); - auto& aAPref = refItem->rotationMatrix; - auto& aArefm = rotationMatrix; - auto& principalMassMarker = static_cast(refItem->owner)->principalMassMarker; - auto& aAPcm = principalMassMarker->rotationMatrix; - auto aApm = aAPcm->transposeTimesFullMatrix(aAPref->timesFullMatrix(aArefm)); - return toFMDsptr(aApm); -} - -void MbD::ASMTMarker::createMbD(std::shared_ptr mbdSys, std::shared_ptr mbdUnits) -{ - auto mkr = CREATE::With(name.c_str()); - auto prt = std::static_pointer_cast(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(owner); + auto& rPrefP = refItem->position3D; + auto& aAPref = refItem->rotationMatrix; + auto& rrefmref = position3D; + auto rPmP = rPrefP->plusFullColumn(aAPref->timesFullColumn(rrefmref)); + auto& principalMassMarker = static_cast(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(owner); + auto& aAPref = refItem->rotationMatrix; + auto& aArefm = rotationMatrix; + auto& principalMassMarker = static_cast(refItem->owner)->principalMassMarker; + auto& aAPcm = principalMassMarker->rotationMatrix; + auto aApm = aAPcm->transposeTimesFullMatrix(aAPref->timesFullMatrix(aArefm)); + return aApm; + } + + void ASMTMarker::createMbD(std::shared_ptr mbdSys, std::shared_ptr mbdUnits) + { + auto mkr = CREATE::With(name.c_str()); + auto prt = std::static_pointer_cast(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); + } } diff --git a/OndselSolver/ASMTMarker.h b/OndselSolver/ASMTMarker.h index a8d8eeb..07ed6f6 100644 --- a/OndselSolver/ASMTMarker.h +++ b/OndselSolver/ASMTMarker.h @@ -23,7 +23,6 @@ namespace MbD { FMatDsptr aApm(); void createMbD(std::shared_ptr mbdSys, std::shared_ptr mbdUnits) override; void storeOnLevel(std::ofstream& os, int level) override; - }; } diff --git a/OndselSolver/ASMTMotion.cpp b/OndselSolver/ASMTMotion.cpp index 8cb66bc..884e482 100644 --- a/OndselSolver/ASMTMotion.cpp +++ b/OndselSolver/ASMTMotion.cpp @@ -9,36 +9,36 @@ #include "ASMTMotion.h" -using namespace MbD; +namespace MbD { + void ASMTMotion::readMotionSeries(std::vector& 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& 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); + } } diff --git a/OndselSolver/DiagonalMatrix.h b/OndselSolver/DiagonalMatrix.h index 13010c3..f158195 100644 --- a/OndselSolver/DiagonalMatrix.h +++ b/OndselSolver/DiagonalMatrix.h @@ -26,7 +26,7 @@ namespace MbD { void atiputDiagonalMatrix(int i, std::shared_ptr> diagMat); DiagMatsptr times(T factor); FColsptr timesFullColumn(FColsptr fullCol); - FMatsptr timesFullMatrix(FMatsptr fullMat); + FMatDsptr timesFullMatrix(FMatDsptr fullMat); int nrow() { return (int)this->size(); } @@ -79,10 +79,10 @@ namespace MbD { return answer; } template - inline FMatsptr DiagonalMatrix::timesFullMatrix(FMatsptr fullMat) + inline FMatDsptr DiagonalMatrix::timesFullMatrix(FMatDsptr fullMat) { auto nrow = (int)this->size(); - auto answer = std::make_shared>(nrow); + auto answer = std::make_shared(nrow); for (int i = 0; i < nrow; i++) { answer->at(i) = fullMat->at(i)->times(this->at(i)); diff --git a/OndselSolver/DispCompIeqcJecO.cpp b/OndselSolver/DispCompIeqcJecO.cpp index bd701fd..87960a0 100644 --- a/OndselSolver/DispCompIeqcJecO.cpp +++ b/OndselSolver/DispCompIeqcJecO.cpp @@ -23,7 +23,7 @@ void DispCompIeqcJecO::initializeGlobally() { priIeJeOpXI = std::make_shared>(3, 0.0); priIeJeOpXI->at(axis) = -1.0; - ppriIeJeOpEIpEI = toFMDsptr(std::static_pointer_cast(frmI)->ppriOeOpEpE(axis)->negated()); + ppriIeJeOpEIpEI = std::static_pointer_cast(frmI)->ppriOeOpEpE(axis)->negated(); } FMatDsptr MbD::DispCompIeqcJecO::ppvaluepEIpEI() diff --git a/OndselSolver/DispCompIeqcJeqcIe.cpp b/OndselSolver/DispCompIeqcJeqcIe.cpp index 9dc5ec1..e02461f 100644 --- a/OndselSolver/DispCompIeqcJeqcIe.cpp +++ b/OndselSolver/DispCompIeqcJeqcIe.cpp @@ -23,7 +23,7 @@ void MbD::DispCompIeqcJeqcIe::calc_ppvaluepEIpEJ() { auto frmJeqc = std::static_pointer_cast(frmJ); auto& prIeJeOpEJ = frmJeqc->prOeOpE; - ppriIeJeIepEIpEJ = toFMDsptr(pAjOIepEIT->timesFullMatrix(prIeJeOpEJ)); + ppriIeJeIepEIpEJ = pAjOIepEIT->timesFullMatrix(prIeJeOpEJ); } void MbD::DispCompIeqcJeqcIe::calc_ppvaluepEIpXJ() diff --git a/OndselSolver/DispCompIeqctJeqcO.cpp b/OndselSolver/DispCompIeqctJeqcO.cpp index 90c4d03..56b2ec2 100644 --- a/OndselSolver/DispCompIeqctJeqcO.cpp +++ b/OndselSolver/DispCompIeqctJeqcO.cpp @@ -39,7 +39,7 @@ void DispCompIeqctJeqcO::calcPostDynCorrectorIteration() { //"ppriIeJeOpEIpEI is not a constant now." DispCompIeqcJeqcO::calcPostDynCorrectorIteration(); - ppriIeJeOpEIpEI = toFMDsptr(std::static_pointer_cast(frmI)->ppriOeOpEpE(axis)->negated()); + ppriIeJeOpEIpEI = std::static_pointer_cast(frmI)->ppriOeOpEpE(axis)->negated(); } void DispCompIeqctJeqcO::preVelIC() diff --git a/OndselSolver/DistancexyConstraintIqcJc.cpp b/OndselSolver/DistancexyConstraintIqcJc.cpp index 1229c74..b60d526 100644 --- a/OndselSolver/DistancexyConstraintIqcJc.cpp +++ b/OndselSolver/DistancexyConstraintIqcJc.cpp @@ -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); } diff --git a/OndselSolver/DistancexyConstraintIqcJqc.cpp b/OndselSolver/DistancexyConstraintIqcJqc.cpp index 2d34f63..270c836 100644 --- a/OndselSolver/DistancexyConstraintIqcJqc.cpp +++ b/OndselSolver/DistancexyConstraintIqcJqc.cpp @@ -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); } diff --git a/OndselSolver/EndFramec.cpp b/OndselSolver/EndFramec.cpp index a7c71c9..84bc5bf 100644 --- a/OndselSolver/EndFramec.cpp +++ b/OndselSolver/EndFramec.cpp @@ -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() diff --git a/OndselSolver/EndFrameqc.cpp b/OndselSolver/EndFrameqc.cpp index 61619f4..e710821 100644 --- a/OndselSolver/EndFrameqc.cpp +++ b/OndselSolver/EndFrameqc.cpp @@ -26,7 +26,7 @@ EndFrameqc::EndFrameqc(const char* str) : EndFramec(str) { void EndFrameqc::initialize() { prOeOpE = std::make_shared(3, 4); - pprOeOpEpE = std::make_shared>(4, 4); + pprOeOpEpE = std::make_shared(4, 4); pAOepE = std::make_shared>(4); ppAOepEpE = std::make_shared(4, 4); } @@ -59,7 +59,7 @@ void MbD::EndFrameqc::initEndFrameqct2() FMatFColDsptr EndFrameqc::ppAjOepEpE(int jj) { - auto answer = std::make_shared>(4, 4); + auto answer = std::make_shared(4, 4); for (int i = 0; i < 4; i++) { auto& answeri = answer->at(i); auto& ppAOepEipE = ppAOepEpE->at(i); diff --git a/OndselSolver/EndFrameqct.cpp b/OndselSolver/EndFrameqct.cpp index ba8ba1f..dfc7848 100644 --- a/OndselSolver/EndFrameqct.cpp +++ b/OndselSolver/EndFrameqct.cpp @@ -142,12 +142,12 @@ void EndFrameqct::calcPostDynCorrectorIteration() } auto rpep = markerFrame->rpmp->plusFullColumn(markerFrame->aApm->timesFullColumn(rmem)); pprOeOpEpE = EulerParameters::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::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); } diff --git a/OndselSolver/EulerAngles.cpp b/OndselSolver/EulerAngles.cpp index e2a88f9..d243282 100644 --- a/OndselSolver/EulerAngles.cpp +++ b/OndselSolver/EulerAngles.cpp @@ -7,3 +7,81 @@ ***************************************************************************/ #include "EulerAngles.h" + +namespace MbD { + template + inline void EulerAngles::initialize() + { + assert(false); + } + template<> + inline void EulerAngles::calc() + { + cA = std::make_shared>(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::calc() + { + cA = std::make_shared>(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 + inline void EulerAngles::calc() + { + assert(false); + } + template + inline std::shared_ptr> EulerAngles::differentiateWRT(T var) + { + auto derivatives = std::make_shared>(); + std::transform(this->begin(), this->end(), derivatives->begin(), + [var](T term) { return term->differentiateWRT(var); } + ); + derivatives->aEulerAngles = this; + return derivatives; + } + template + inline void EulerAngles::setRotOrder(int i, int j, int k) + { + rotOrder = std::make_shared>(3); + rotOrder->at(0) = i; + rotOrder->at(1) = j; + rotOrder->at(2) = k; + } +} + diff --git a/OndselSolver/EulerAngles.h b/OndselSolver/EulerAngles.h index 030a493..ef3a678 100644 --- a/OndselSolver/EulerAngles.h +++ b/OndselSolver/EulerAngles.h @@ -16,8 +16,6 @@ #include "Symbolic.h" namespace MbD { - //template - //class EulerAnglesDot; template class EulerAngles : public EulerArray @@ -37,79 +35,5 @@ namespace MbD { FMatDsptr aA; }; - template - inline void EulerAngles::initialize() - { - assert(false); - } - template<> - inline void EulerAngles::calc() - { - cA = std::make_shared>(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::calc() - { - cA = std::make_shared>(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 - inline void EulerAngles::calc() - { - assert(false); - } - template - inline std::shared_ptr> EulerAngles::differentiateWRT(T var) - { - auto derivatives = std::make_shared>(); - std::transform(this->begin(), this->end(), derivatives->begin(), - [var](T term) { return term->differentiateWRT(var); } - ); - derivatives->aEulerAngles = this; - return derivatives; - } - template - inline void EulerAngles::setRotOrder(int i, int j, int k) - { - rotOrder = std::make_shared>(3); - rotOrder->at(0) = i; - rotOrder->at(1) = j; - rotOrder->at(2) = k; - } } diff --git a/OndselSolver/EulerAnglesDDot.h b/OndselSolver/EulerAnglesDDot.h index 547fc2c..f724905 100644 --- a/OndselSolver/EulerAnglesDDot.h +++ b/OndselSolver/EulerAnglesDDot.h @@ -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."); diff --git a/OndselSolver/EulerAnglesDot.h b/OndselSolver/EulerAnglesDot.h index 1200506..e02c7a3 100644 --- a/OndselSolver/EulerAnglesDot.h +++ b/OndselSolver/EulerAnglesDot.h @@ -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))); diff --git a/OndselSolver/EulerAngleszxz.h b/OndselSolver/EulerAngleszxz.h index 01404f5..23a7617 100644 --- a/OndselSolver/EulerAngleszxz.h +++ b/OndselSolver/EulerAngleszxz.h @@ -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)); } } diff --git a/OndselSolver/EulerAngleszxzDDot.h b/OndselSolver/EulerAngleszxzDDot.h index 70c4b3d..10879af 100644 --- a/OndselSolver/EulerAngleszxzDDot.h +++ b/OndselSolver/EulerAngleszxzDDot.h @@ -25,7 +25,7 @@ namespace MbD { std::shared_ptr> phiThePsiDot; FMatDsptr phiAddot, theAddot, psiAddot; - std::shared_ptr> aAddot; + std::shared_ptr aAddot; }; template inline void EulerAngleszxzDDot::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>(mat); + aAddot = std::make_shared(mat); } } diff --git a/OndselSolver/EulerAngleszxzDot.h b/OndselSolver/EulerAngleszxzDot.h index f7c333f..03f56b7 100644 --- a/OndselSolver/EulerAngleszxzDot.h +++ b/OndselSolver/EulerAngleszxzDot.h @@ -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); } } diff --git a/OndselSolver/EulerParameters.cpp b/OndselSolver/EulerParameters.cpp index 27ac4f1..4c4b999 100644 --- a/OndselSolver/EulerParameters.cpp +++ b/OndselSolver/EulerParameters.cpp @@ -28,7 +28,7 @@ inline FMatFColDsptr EulerParameters::ppApEpEtimesColumn(FColDsptr col) auto col22 = std::make_shared>(ListD{ m2c0, m2c1, a2c2 }); auto col23 = std::make_shared>(ListD{ m2c1, a2c0, 0 }); auto col33 = std::make_shared>(ListD{ a2c0, a2c1, a2c2 }); - auto answer = std::make_shared>(4, 4); + auto answer = std::make_shared(4, 4); auto& row0 = answer->at(0); row0->at(0) = col00; row0->at(1) = col01; @@ -136,7 +136,7 @@ inline FMatFMatDsptr EulerParameters::ppApEpEtimesMatrix(FMatDsptr mat) auto mat22 = std::make_shared(ListFRD{ m2m0, m2m1, a2m2 }); auto mat23 = std::make_shared(ListFRD{ m2m1, a2m0, zero }); auto mat33 = std::make_shared(ListFRD{ a2m0, a2m1, a2m2 }); - auto answer = std::make_shared>(4, 4); + auto answer = std::make_shared(4, 4); auto& row0 = answer->at(0); row0->at(0) = mat00; row0->at(1) = mat01; @@ -157,7 +157,7 @@ inline FMatFMatDsptr EulerParameters::ppApEpEtimesMatrix(FMatDsptr mat) row3->at(1) = mat13; row3->at(2) = mat23; row3->at(3) = mat33; - return toFMFMDsptr(answer); + return answer; } template<> diff --git a/OndselSolver/EulerParameters.h b/OndselSolver/EulerParameters.h index 1a320ec..05d475b 100644 --- a/OndselSolver/EulerParameters.h +++ b/OndselSolver/EulerParameters.h @@ -41,7 +41,7 @@ namespace MbD { this->calc(); } - static std::shared_ptr>> ppApEpEtimesColumn(FColDsptr col); + static std::shared_ptr ppApEpEtimesColumn(FColDsptr col); static FMatDsptr pCpEtimesColumn(FColDsptr col); static FMatDsptr pCTpEtimesColumn(FColDsptr col); static std::shared_ptr ppApEpEtimesMatrix(FMatDsptr mat); diff --git a/OndselSolver/FullMatrix.cpp b/OndselSolver/FullMatrix.cpp index 1e07729..5989ad5 100644 --- a/OndselSolver/FullMatrix.cpp +++ b/OndselSolver/FullMatrix.cpp @@ -12,687 +12,855 @@ #include "DiagonalMatrix.h" #include "EulerParameters.h" -using namespace MbD; - -template -inline FMatsptr FullMatrix::rotatex(T the) -{ - auto sthe = std::sin(the); - auto cthe = std::cos(the); - auto rotMat = std::make_shared>(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 -inline FMatsptr FullMatrix::rotatey(T the) -{ - auto sthe = std::sin(the); - auto cthe = std::cos(the); - auto rotMat = std::make_shared>(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 -inline FMatsptr FullMatrix::rotatez(T the) -{ - auto sthe = std::sin(the); - auto cthe = std::cos(the); - auto rotMat = std::make_shared>(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 -inline FMatsptr FullMatrix::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>(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 -inline FMatsptr FullMatrix::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>(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 -inline FMatsptr FullMatrix::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>(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 -inline FMatsptr FullMatrix::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>(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 -inline FMatsptr FullMatrix::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>(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 -inline FMatsptr FullMatrix::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>(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 -inline FMatsptr FullMatrix::identitysptr(int n) -{ - auto mat = std::make_shared>(n, n); - mat->identity(); - return mat; -} -inline std::shared_ptr FullMatrixDouble::identitysptr(int n) -{ - auto mat = std::make_shared(n, n); - mat->identity(); - return mat; -} -inline std::shared_ptr FullMatrixFullMatrixDouble::identitysptr(int n) -{ - auto mat = std::make_shared(n, n); - mat->identity(); - return mat; -} -template -inline FMatsptr FullMatrix::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(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::zeroSelf() -{ - for (int i = 0; i < this->size(); i++) { - this->at(i)->zeroSelf(); +namespace MbD { +// template +// +// inline std::shared_ptr> rotatex(T the) +// { +// auto sthe = std::sin(the); +// auto cthe = std::cos(the); +// auto rotMat = std::make_shared>(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 +// inline std::shared_ptr> rotatey(T the) +// { +// auto sthe = std::sin(the); +// auto cthe = std::cos(the); +// auto rotMat = std::make_shared>(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 +// inline std::shared_ptr> rotatez(T the) +// { +// auto sthe = std::sin(the); +// auto cthe = std::cos(the); +// auto rotMat = std::make_shared>(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; +// } + inline std::shared_ptr rotatex(double the) + { + auto sthe = std::sin(the); + auto cthe = std::cos(the); + auto rotMat = std::make_shared(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 -inline void FullMatrix::identity() { - assert(false); -} -inline void FullMatrixDouble::identity() { - this->zeroSelf(); - for (int i = 0; i < this->size(); i++) { - this->at(i)->at(i) = 1.0; + inline std::shared_ptr rotatey(double the) + { + auto sthe = std::sin(the); + auto cthe = std::cos(the); + auto rotMat = std::make_shared(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; } -} -inline void FullMatrixFullMatrixDouble::identity() { - assert(false); + inline std::shared_ptr rotatez(double the) + { + auto sthe = std::sin(the); + auto cthe = std::cos(the); + auto rotMat = std::make_shared(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 +// inline std::shared_ptr> 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>(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 +// inline std::shared_ptr> 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>(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 +// inline std::shared_ptr> 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>(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; +// } + inline std::shared_ptr rotatexrotDot(double the, double thedot) + { + auto sthe = std::sin(the); + auto cthe = std::cos(the); + auto sthedot = cthe * thedot; + auto cthedot = -sthe * thedot; + auto rotMat = std::make_shared(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; + } + inline std::shared_ptr rotateyrotDot(double the, double thedot) + { + auto sthe = std::sin(the); + auto cthe = std::cos(the); + auto sthedot = cthe * thedot; + auto cthedot = -sthe * thedot; + auto rotMat = std::make_shared(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; + } + inline std::shared_ptr rotatezrotDot(double the, double thedot) + { + auto sthe = std::sin(the); + auto cthe = std::cos(the); + auto sthedot = cthe * thedot; + auto cthedot = -sthe * thedot; + auto rotMat = std::make_shared(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 + inline std::shared_ptr> 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>(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 + inline std::shared_ptr> 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>(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 + inline std::shared_ptr> 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>(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 + inline std::shared_ptr> FullMatrixTemplate::identitysptr(int n) + { + auto mat = std::make_shared>(n, n); + mat->identity(); + return mat; + } + inline std::shared_ptr FullMatrixDouble::identitysptr(int n) + { + auto mat = std::make_shared(n, n); + mat->identity(); + return mat; + } + inline std::shared_ptr FullMatrixFullMatrixDouble::identitysptr(int n) + { + auto mat = std::make_shared(n, n); + mat->identity(); + return mat; + } + template + inline std::shared_ptr> FullMatrixTemplate::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(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 FullMatrixTemplate::zeroSelf() + { + for (int i = 0; i < this->size(); i++) { + this->at(i)->zeroSelf(); + } + } + template + inline void FullMatrixTemplate::identity() { + assert(false); + } + inline void FullMatrixDouble::identity() { + this->zeroSelf(); + for (int i = 0; i < this->size(); i++) { + this->at(i)->at(i) = 1.0; + } + } + inline void FullMatrixFullMatrixDouble::identity() { + assert(false); // this->zeroSelf(); // for (int i = 0; i < this->size(); i++) { // this->at(i)->at(i) = 1.0; // } -} -template -inline FColsptr FullMatrix::column(int j) { - int n = (int)this->size(); - auto answer = std::make_shared>(n); - for (int i = 0; i < n; i++) { - answer->at(i) = this->at(i)->at(j); } - return answer; -} -template -inline FMatsptr FullMatrix::timesFullMatrix(FMatsptr fullMat) -{ - int m = this->nrow(); - auto answer = std::make_shared>(m); - for (int i = 0; i < m; i++) { - answer->at(i) = this->at(i)->timesFullMatrix(fullMat); + template + inline FColsptr FullMatrixTemplate::column(int j) { + int n = (int)this->size(); + auto answer = std::make_shared>(n); + for (int i = 0; i < n; i++) { + answer->at(i) = this->at(i)->at(j); + } + return answer; } - return answer; -} -template -inline FMatsptr FullMatrix::timesTransposeFullMatrix(FMatsptr fullMat) -{ - assert(false); -// int nrow = this->nrow(); -// auto answer = std::make_shared>(nrow); -// for (int i = 0; i < nrow; i++) { -// answer->at(i) = this->at(i)->timesTransposeFullMatrix(fullMat); +// template +// inline std::shared_ptr> FullMatrixTemplate::timesFullMatrix(std::shared_ptr> fullMat) +// { +// int m = this->nrow(); +// auto answer = std::make_shared>(m); +// for (int i = 0; i < m; i++) { +// answer->at(i) = this->at(i)->timesFullMatrix(fullMat); +// } +// return answer; // } -// return answer; -} -inline std::shared_ptr FullMatrixDouble::timesTransposeFullMatrix(std::shared_ptr fullMat) -{ - int nrow = this->nrow(); - auto answer = std::make_shared(nrow); - for (int i = 0; i < nrow; i++) { - answer->at(i) = this->at(i)->timesTransposeFullMatrix(fullMat); + inline std::shared_ptr FullMatrixDouble::timesFullMatrix(std::shared_ptr fullMat) + { + int m = this->nrow(); + auto answer = std::make_shared(m); + for (int i = 0; i < m; i++) { + answer->at(i) = this->at(i)->timesFullMatrix(fullMat); + } + return answer; } - return answer; -} -inline std::shared_ptr FullMatrixFullMatrixDouble::timesTransposeFullMatrix(std::shared_ptr fullMat) -{ - int nrow = this->nrow(); - auto answer = std::make_shared(nrow); - for (int i = 0; i < nrow; i++) { - answer->at(i) = this->at(i)->timesTransposeFullMatrixForFMFMDsptr(fullMat); + inline std::shared_ptr FullMatrixDouble::timesTransposeFullMatrix(std::shared_ptr fullMat) + { + int nrow = this->nrow(); + auto answer = std::make_shared(nrow); + for (int i = 0; i < nrow; i++) { + answer->at(i) = this->at(i)->timesTransposeFullMatrix(fullMat); + } + return answer; } - return answer; -} -inline std::shared_ptr FullMatrixDouble::times(double a) -{ - int m = this->nrow(); - auto answer = std::make_shared(m); - for (int i = 0; i < m; i++) { - // auto x = this->at(i); - answer->at(i) = this->at(i)->times(a); + inline std::shared_ptr FullMatrixFullMatrixDouble::timesTransposeFullMatrix(std::shared_ptr fullMat) + { + int nrow = this->nrow(); + auto answer = std::make_shared(nrow); + for (int i = 0; i < nrow; i++) { + answer->at(i) = this->at(i)->timesTransposeFullMatrixForFMFMDsptr(fullMat); + } + return answer; } - return answer; -} -inline std::shared_ptr FullMatrixFullMatrixDouble::times(double a) -{ - assert(false); -} -template -inline FMatsptr FullMatrix::times(T a) -{ - assert(false); -} -template -inline FMatsptr FullMatrix::transposeTimesFullMatrix(FMatsptr fullMat) -{ - return this->transpose()->timesFullMatrix(fullMat); -} -template -inline FMatsptr FullMatrix::plusFullMatrix(FMatsptr fullMat) -{ - int n = (int)this->size(); - auto answer = std::make_shared>(n); - for (int i = 0; i < n; i++) { - answer->at(i) = this->at(i)->plusFullRow(fullMat->at(i)); + inline std::shared_ptr FullMatrixDouble::times(double a) + { + int m = this->nrow(); + auto answer = std::make_shared(m); + for (int i = 0; i < m; i++) { + // auto x = this->at(i); + answer->at(i) = this->at(i)->times(a); + } + return answer; } - return answer; -} -template -inline FMatsptr FullMatrix::minusFullMatrix(FMatsptr fullMat) -{ - int n = (int)this->size(); - auto answer = std::make_shared>(n); - for (int i = 0; i < n; i++) { - answer->at(i) = this->at(i)->minusFullRow(fullMat->at(i)); + inline std::shared_ptr FullMatrixFullMatrixDouble::times(double a) + { + assert(false); } - return answer; -} -template -inline FMatsptr FullMatrix::transpose() -{ - int nrow = this->nrow(); - auto ncol = this->ncol(); - auto answer = std::make_shared>(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); + inline std::shared_ptr FullMatrixFullColumnDouble::times(double a) + { +// int m = this->nrow(); +// auto answer = std::make_shared(m); +// for (int i = 0; i < m; i++) { +// answer->at(i) = this->at(i)->times(a); +// } +// return answer; + assert(false); + } + template + inline std::shared_ptr> FullMatrixTemplate::times(T a) + { + assert(false); + } +// template +// inline std::shared_ptr> FullMatrixTemplate::transposeTimesFullMatrix(std::shared_ptr> fullMat) +// { +// return this->transpose()->timesFullMatrix(fullMat); +// } + inline std::shared_ptr FullMatrixDouble::transposeTimesFullMatrix(std::shared_ptr fullMat) + { + return this->transpose()->timesFullMatrix(fullMat); + } +// template +// inline std::shared_ptr> FullMatrixTemplate::plusFullMatrix(std::shared_ptr> fullMat) +// { +// int n = (int)this->size(); +// auto answer = std::make_shared>(n); +// for (int i = 0; i < n; i++) { +// answer->at(i) = this->at(i)->plusFullRow(fullMat->at(i)); +// } +// return answer; +// } + inline std::shared_ptr FullMatrixDouble::plusFullMatrix(std::shared_ptr fullMat) + { + int n = (int)this->size(); + auto answer = std::make_shared(n); + for (int i = 0; i < n; i++) { + answer->at(i) = this->at(i)->plusFullRow(fullMat->at(i)); + } + return answer; + } + template + inline std::shared_ptr> FullMatrixTemplate::minusFullMatrix(std::shared_ptr> fullMat) + { + int n = (int)this->size(); + auto answer = std::make_shared>(n); + for (int i = 0; i < n; i++) { + answer->at(i) = this->at(i)->minusFullRow(fullMat->at(i)); + } + return answer; + } +// template +// inline std::shared_ptr> FullMatrixTemplate::transpose() +// { +// int nrow = this->nrow(); +// auto ncol = this->ncol(); +// auto answer = std::make_shared>(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; +// } + inline std::shared_ptr FullMatrixDouble::transpose() { + int nrow = this->nrow(); + auto ncol = this->ncol(); + auto answer = std::make_shared(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 +// inline std::shared_ptr> FullMatrixTemplate::negated() +// { +// return this->times(-1.0); +// } + inline std::shared_ptr FullMatrixDouble::negated() + { + return this->times(-1.0); + } + template + inline void FullMatrixTemplate::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); + } } } - return answer; -} -template -inline FMatsptr FullMatrix::negated() -{ - return this->times(-1.0); -} -template -inline void FullMatrix::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 + inline void FullMatrixTemplate::atiput(int i, FRowsptr fullRow) + { + this->at(i) = fullRow; + } + template + inline void FullMatrixTemplate::atijput(int i, int j, T value) + { + this->at(i)->atiput(j, value); + } + template + inline void FullMatrixTemplate::atijputFullColumn(int i1, int j1, FColsptr fullCol) + { + for (int ii = 0; ii < fullCol->size(); ii++) + { + this->at(i1 + ii)->at(j1) = fullCol->at(ii); } } -} -template -inline void FullMatrix::atiput(int i, FRowsptr fullRow) -{ - this->at(i) = fullRow; -} -template -inline void FullMatrix::atijput(int i, int j, T value) -{ - this->at(i)->atiput(j, value); -} -template -inline void FullMatrix::atijputFullColumn(int i1, int j1, FColsptr fullCol) -{ - for (int ii = 0; ii < fullCol->size(); ii++) + template + inline void FullMatrixTemplate::atijplusFullRow(int i, int j, FRowsptr fullRow) { - this->at(i1 + ii)->at(j1) = fullCol->at(ii); + this->at(i)->atiplusFullRow(j, fullRow); } -} -template -inline void FullMatrix::atijplusFullRow(int i, int j, FRowsptr fullRow) -{ - this->at(i)->atiplusFullRow(j, fullRow); -} -template -inline void FullMatrix::atijplusNumber(int i, int j, T value) -{ - auto rowi = this->at(i); - rowi->at(j) += value; -} -template -inline void FullMatrix::atijminusNumber(int i, int j, T value) -{ - auto rowi = this->at(i); - rowi->at(j) -= value; -} -inline double FullMatrixDouble::sumOfSquares() -{ - double sum = 0.0; - for (int i = 0; i < this->size(); i++) - { - sum += this->at(i)->sumOfSquares(); - } - return sum; -} -inline double FullMatrixFullMatrixDouble::sumOfSquares() -{ - double sum = 0.0; - for (int i = 0; i < this->size(); i++) - { - sum += this->at(i)->sumOfSquares(); - } - return sum; -} - -template -inline double FullMatrix::sumOfSquares() -{ - assert(false); - return 0.0; -} -template -inline void FullMatrix::zeroSelf() -{ - assert(false); -} -template -inline FMatsptr FullMatrix::copy() -{ - auto m = (int)this->size(); - auto answer = std::make_shared>(m); - for (int i = 0; i < m; i++) - { - answer->at(i) = this->at(i)->copy(); - } - return answer; -} -template -inline FullMatrix FullMatrix::operator+(const FullMatrix fullMat) -{ - int n = (int)this->size(); - auto answer = FullMatrix(n); - for (int i = 0; i < n; i++) { - answer.at(i) = this->at(i)->plusFullRow(fullMat.at(i)); - } - return answer; -} -template -inline FColsptr FullMatrix::transposeTimesFullColumn(FColsptr fullCol) -{ - auto sptr = std::make_shared>(*this); - return fullCol->transpose()->timesFullMatrix(sptr)->transpose(); -} -template -inline void FullMatrix::magnifySelf(T factor) -{ - for (int i = 0; i < this->size(); i++) { - this->at(i)->magnifySelf(factor); - } -} -template -inline std::ostream& FullMatrix::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 -inline std::shared_ptr> FullMatrix::asEulerParameters() -{ - //"Given [A], compute Euler parameter." - - auto traceA = this->trace(); - T dum = 0.0; - T dumSq = 0.0; - //auto qE = CREATE>::With(4); //Cannot use CREATE.h in subclasses of std::vector. Why? - auto qE = std::make_shared>(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 -inline T FullMatrix::trace() -{ - T trace = 0.0; - for (int i = 0; i < this->size(); i++) - { - trace += this->at(i)->at(i); - } - return trace; -} -template -inline double FullMatrix::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 -inline FColsptr FullMatrix::bryantAngles() -{ - auto answer = std::make_shared>(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 -inline bool FullMatrix::isDiagonal() -{ - auto m = this->nrow(); - auto n = this->ncol(); - if (m != n) return false; - for (int i = 0; i < m; i++) + template + inline void FullMatrixTemplate::atijplusNumber(int i, int j, T value) { auto rowi = this->at(i); - for (int j = 0; j < n; j++) + rowi->at(j) += value; + } + template + inline void FullMatrixTemplate::atijminusNumber(int i, int j, T value) + { + auto rowi = this->at(i); + rowi->at(j) -= value; + } + inline double FullMatrixDouble::sumOfSquares() + { + double sum = 0.0; + for (int i = 0; i < this->size(); i++) { - if (i != j && rowi->at(j) != 0) return false; + sum += this->at(i)->sumOfSquares(); + } + return sum; + } + inline double FullMatrixFullMatrixDouble::sumOfSquares() + { + double sum = 0.0; + for (int i = 0; i < this->size(); i++) + { + sum += this->at(i)->sumOfSquares(); + } + return sum; + } + + template + inline double FullMatrixTemplate::sumOfSquares() + { + assert(false); + return 0.0; + } + template + inline void FullMatrixTemplate::zeroSelf() + { + assert(false); + } +// template +// inline std::shared_ptr> FullMatrixTemplate::copy() +// { +// auto m = (int)this->size(); +// auto answer = std::make_shared>(m); +// for (int i = 0; i < m; i++) +// { +// answer->at(i) = this->at(i)->copy(); +// } +// return answer; +// } + inline std::shared_ptr FullMatrixDouble::copy() + { + auto m = (int)this->size(); + auto answer = std::make_shared(m); + for (int i = 0; i < m; i++) + { + answer->at(i) = this->at(i)->copy(); + } + return answer; + } + template + inline FullMatrixTemplate FullMatrixTemplate::operator+(const FullMatrixTemplate fullMat) + { + int n = (int)this->size(); + auto answer = FullMatrixTemplate(n); + for (int i = 0; i < n; i++) { + answer.at(i) = this->at(i)->plusFullRow(fullMat.at(i)); + } + return answer; + } + template + inline FColsptr FullMatrixTemplate::transposeTimesFullColumn(FColsptr fullCol) + { + auto sptr = std::make_shared>(*this); + return fullCol->transpose()->timesFullMatrix(sptr)->transpose(); + } + template + inline void FullMatrixTemplate::magnifySelf(T factor) + { + for (int i = 0; i < this->size(); i++) { + this->at(i)->magnifySelf(factor); } } - return true; -} -template -inline bool FullMatrix::isDiagonalToWithin(double ratio) -{ - double maxMag = this->maxMagnitude(); - auto tol = ratio * maxMag; - auto nrow = this->nrow(); - if (nrow == this->ncol()) { + template + inline std::ostream& FullMatrixTemplate::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 + inline std::shared_ptr> FullMatrixTemplate::asEulerParameters() + { + //"Given [A], compute Euler parameter." + + auto traceA = this->trace(); + T dum = 0.0; + T dumSq = 0.0; + //auto qE = CREATE>::With(4); //Cannot use CREATE.h in subclasses of std::vector. Why? + auto qE = std::make_shared>(4); + qE->initialize(); + auto OneMinusTraceDivFour = (1.0 - traceA) / 4.0; for (int i = 0; i < 3; i++) { - for (int j = i + 1; j < 3; j++) + 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 + inline T FullMatrixTemplate::trace() + { + T trace = 0.0; + for (int i = 0; i < this->size(); i++) + { + trace += this->at(i)->at(i); + } + return trace; + } + template + inline double FullMatrixTemplate::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 + inline FColsptr FullMatrixTemplate::bryantAngles() + { + auto answer = std::make_shared>(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 + inline bool FullMatrixTemplate::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 (std::abs(this->at(i)->at(j)) > tol) return false; - if (std::abs(this->at(j)->at(i)) > tol) return false; + if (i != j && rowi->at(j) != 0) return false; } } return true; } - else { - return false; - } -} -template -inline std::shared_ptr> FullMatrix::asDiagonalMatrix() -{ - int nrow = this->nrow(); - auto diagMat = std::make_shared>(nrow); - for (int i = 0; i < nrow; i++) + template + inline bool FullMatrixTemplate::isDiagonalToWithin(double ratio) { - diagMat->atiput(i, this->at(i)->at(i)); + 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; + } } - return diagMat; -} -template -inline void FullMatrix::conditionSelfWithTol(double tol) -{ - for (auto row : *this) { - row->conditionSelfWithTol(tol); - } -} -template -inline FColsptr FullMatrix::timesFullColumn(FColsptr fullCol) -{ - return this->timesFullColumn(fullCol.get()); -} -template -inline FColsptr FullMatrix::timesFullColumn(FullColumn* fullCol) -{ - //"a*b = a(i,j)b(j) sum j." - auto nrow = this->nrow(); - auto answer = std::make_shared>(nrow); - for (int i = 0; i < nrow; i++) + template + inline std::shared_ptr> FullMatrixTemplate::asDiagonalMatrix() { - answer->at(i) = this->at(i)->timesFullColumn(fullCol); + int nrow = this->nrow(); + auto diagMat = std::make_shared>(nrow); + for (int i = 0; i < nrow; i++) + { + diagMat->atiput(i, this->at(i)->at(i)); + } + return diagMat; + } + template + inline void FullMatrixTemplate::conditionSelfWithTol(double tol) + { + for (auto row : *this) { + row->conditionSelfWithTol(tol); + } + } + template + inline FColsptr FullMatrixTemplate::timesFullColumn(FColsptr fullCol) + { + return this->timesFullColumn(fullCol.get()); + } + template + inline FColsptr FullMatrixTemplate::timesFullColumn(FullColumn* fullCol) + { + //"a*b = a(i,j)b(j) sum j." + auto nrow = this->nrow(); + auto answer = std::make_shared>(nrow); + for (int i = 0; i < nrow; i++) + { + answer->at(i) = this->at(i)->timesFullColumn(fullCol); + } + return answer; } - return answer; } diff --git a/OndselSolver/FullMatrix.h b/OndselSolver/FullMatrix.h index ad94b0b..28a8eba 100644 --- a/OndselSolver/FullMatrix.h +++ b/OndselSolver/FullMatrix.h @@ -21,64 +21,58 @@ namespace MbD { template - class FullMatrix; - - template - using FMatsptr = std::shared_ptr>; - - template - class FullMatrix : public RowTypeMatrix> + class FullMatrixTemplate : public RowTypeMatrix> { public: - FullMatrix() {}; - explicit FullMatrix(int m) : RowTypeMatrix>(m) + FullMatrixTemplate() = default; + explicit FullMatrixTemplate(int m) : RowTypeMatrix>(m) { } - FullMatrix(int m, int n) { + FullMatrixTemplate(int m, int n) { for (int i = 0; i < m; i++) { auto row = std::make_shared>(n); this->push_back(row); } } - FullMatrix(std::initializer_list> listOfRows) { + FullMatrixTemplate(std::initializer_list> listOfRows) { for (auto& row : listOfRows) { this->push_back(row); } } - FullMatrix(std::initializer_list> list2D) { + FullMatrixTemplate(std::initializer_list> list2D) { for (auto& rowList : list2D) { auto row = std::make_shared>(rowList); this->push_back(row); } } - static FMatsptr rotatex(T angle); - static FMatsptr rotatey(T angle); - static FMatsptr rotatez(T angle); - static FMatsptr rotatexrotDot(T angle, T angledot); - static FMatsptr rotateyrotDot(T angle, T angledot); - static FMatsptr rotatezrotDot(T angle, T angledot); - static FMatsptr rotatexrotDotrotDDot(T angle, T angleDot, T angleDDot); - static FMatsptr rotateyrotDotrotDDot(T angle, T angleDot, T angleDDot); - static FMatsptr rotatezrotDotrotDDot(T angle, T angleDot, T angleDDot); - static FMatsptr identitysptr(int n); - static FMatsptr tildeMatrix(FColDsptr col); +// static std::shared_ptr> rotatex(T angle); +// static std::shared_ptr> rotatey(T angle); +// static std::shared_ptr> rotatez(T angle); +// static std::shared_ptr> rotatexrotDot(T angle, T angledot); +// static std::shared_ptr> rotateyrotDot(T angle, T angledot); +// static std::shared_ptr> rotatezrotDot(T angle, T angledot); + static std::shared_ptr> rotatexrotDotrotDDot(T angle, T angleDot, T angleDDot); + static std::shared_ptr> rotateyrotDotrotDDot(T angle, T angleDot, T angleDDot); + static std::shared_ptr> rotatezrotDotrotDDot(T angle, T angleDot, T angleDDot); + static std::shared_ptr> identitysptr(int n); + static std::shared_ptr> tildeMatrix(FColDsptr col); virtual void identity(); FColsptr column(int j); FColsptr timesFullColumn(FColsptr fullCol); FColsptr timesFullColumn(FullColumn* fullCol); - FMatsptr timesFullMatrix(FMatsptr fullMat); + // std::shared_ptr> timesFullMatrix(std::shared_ptr> fullMat); - virtual FMatsptr timesTransposeFullMatrix(FMatsptr fullMat); + virtual std::shared_ptr> timesTransposeFullMatrix(std::shared_ptr> fullMat); - FMatsptr times(T a); - FMatsptr transposeTimesFullMatrix(FMatsptr fullMat); - FMatsptr plusFullMatrix(FMatsptr fullMat); - FMatsptr minusFullMatrix(FMatsptr fullMat); - FMatsptr transpose(); - FMatsptr negated(); + std::shared_ptr> times(T a); + // std::shared_ptr> transposeTimesFullMatrix(std::shared_ptr> fullMat); + // std::shared_ptr> plusFullMatrix(std::shared_ptr> fullMat); + std::shared_ptr> minusFullMatrix(std::shared_ptr> fullMat); + // std::shared_ptr> transpose(); +// std::shared_ptr> negated(); void symLowerWithUpper(); void atiput(int i, FRowsptr 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 copy(); - FullMatrix operator+(const FullMatrix fullMat); +// std::shared_ptr> copy(); + FullMatrixTemplate operator+(const FullMatrixTemplate fullMat); FColsptr transposeTimesFullColumn(const FColsptr fullCol); void magnifySelf(T factor); std::shared_ptr> asEulerParameters(); @@ -107,51 +101,61 @@ namespace MbD { // // FULL MATRIX DOUBLE instantiation // - class FullMatrixDouble : public FullMatrix { + class FullMatrixDouble : public FullMatrixTemplate { public: - FullMatrixDouble() : FullMatrix() {}; - explicit FullMatrixDouble(int m) : FullMatrix(m) {}; - FullMatrixDouble(int m, int n) : FullMatrix(m, n) {}; - FullMatrixDouble(std::initializer_list> list2D) : FullMatrix(list2D) {} - FullMatrixDouble(std::initializer_list> listOfRows) : FullMatrix(listOfRows) {}; + FullMatrixDouble() : FullMatrixTemplate() {}; + explicit FullMatrixDouble(int m) : FullMatrixTemplate(m) {}; + FullMatrixDouble(int m, int n) : FullMatrixTemplate(m, n) {}; + FullMatrixDouble(std::initializer_list> list2D) : FullMatrixTemplate(list2D) {} + FullMatrixDouble(std::initializer_list> listOfRows) : FullMatrixTemplate(listOfRows) {}; std::shared_ptr times(double a); std::shared_ptr timesTransposeFullMatrix(std::shared_ptr fullMat); - double sumOfSquares() override; void identity() override; static std::shared_ptr identitysptr(int n); + double sumOfSquares(); + std::shared_ptr transposeTimesFullMatrix(std::shared_ptr fullMat); + std::shared_ptr timesFullMatrix(std::shared_ptr fullMat); + std::shared_ptr transpose(); + std::shared_ptr negated(); + std::shared_ptr plusFullMatrix(std::shared_ptr fullMat); + static std::shared_ptr rotatex(double angle); + static std::shared_ptr rotatey(double angle); + static std::shared_ptr rotatez(double angle); + static std::shared_ptr rotatexrotDot(double angle, double angledot); + static std::shared_ptr rotateyrotDot(double angle, double angledot); + static std::shared_ptr rotatezrotDot(double angle, double angledot); + std::shared_ptr copy(); }; - using FMatDsptr = std::shared_ptr; - std::shared_ptr toFMDsptr(FMatsptr s) { - return std::static_pointer_cast(s); - } // // FULL MATRIX FULL MATRIX DOUBLE instantiation // - class FullMatrixFullMatrixDouble : public FullMatrix { + class FullMatrixFullMatrixDouble : public FullMatrixTemplate { public: - FullMatrixFullMatrixDouble() : FullMatrix() {}; - explicit FullMatrixFullMatrixDouble(int m) : FullMatrix(m) {}; - FullMatrixFullMatrixDouble(int m, int n) : FullMatrix(m, n) {}; + FullMatrixFullMatrixDouble() : FullMatrixTemplate() {}; + explicit FullMatrixFullMatrixDouble(int m) : FullMatrixTemplate(m) {}; + FullMatrixFullMatrixDouble(int m, int n) : FullMatrixTemplate(m, n) {}; std::shared_ptr times(double a); std::shared_ptr timesTransposeFullMatrix(std::shared_ptr fullMat); double sumOfSquares() override; void identity() override; static std::shared_ptr identitysptr(int n); - - - }; - using FMatFMatDsptr = std::shared_ptr; - std::shared_ptr toFMFMDsptr(std::shared_ptr> s) { - return std::static_pointer_cast(s); - } - - using FColFMatDsptr = std::shared_ptr>; - using FMatFColDsptr = std::shared_ptr>; - + // + // FULL MATRIX FULL COLUMN DOUBLE instantiation + // + class FullMatrixFullColumnDouble : public FullMatrixTemplate { + public: + FullMatrixFullColumnDouble() : FullMatrixTemplate() {}; + explicit FullMatrixFullColumnDouble(int m) : FullMatrixTemplate(m) {}; + FullMatrixFullColumnDouble(int m, int n) : FullMatrixTemplate(m, n) {}; + std::shared_ptr times(double a); + std::shared_ptr timesTransposeFullMatrix(std::shared_ptr fullMat); + double sumOfSquares() override; + void identity() override; + static std::shared_ptr identitysptr(int n); + }; } - diff --git a/OndselSolver/FullMatrix.ref.h b/OndselSolver/FullMatrix.ref.h index f250568..db820ec 100644 --- a/OndselSolver/FullMatrix.ref.h +++ b/OndselSolver/FullMatrix.ref.h @@ -5,16 +5,14 @@ namespace MbD { class FullMatrixDouble; class FullMatrixFullMatrixDouble; + class FullMatrixFullColumnDouble; -// using FMatDsptr = std::shared_ptr; + using FMatDsptr = std::shared_ptr; -// template -// using FMatsptr = std::shared_ptr>; + using FMatFMatDsptr = std::shared_ptr; -// using FMatFColDsptr = std::shared_ptr>; -// using FMatFMatDsptr = std::shared_ptr>; -// -// using FColFMatDsptr = std::shared_ptr>; + using FColFMatDsptr = std::shared_ptr>; + using FMatFColDsptr = std::shared_ptr; } diff --git a/OndselSolver/GearConstraintIqcJc.cpp b/OndselSolver/GearConstraintIqcJc.cpp index a5cff8c..af49703 100644 --- a/OndselSolver/GearConstraintIqcJc.cpp +++ b/OndselSolver/GearConstraintIqcJc.cpp @@ -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() diff --git a/OndselSolver/GearConstraintIqcJqc.cpp b/OndselSolver/GearConstraintIqcJqc.cpp index 418f198..717c815 100644 --- a/OndselSolver/GearConstraintIqcJqc.cpp +++ b/OndselSolver/GearConstraintIqcJqc.cpp @@ -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() diff --git a/OndselSolver/LDUFullMat.cpp b/OndselSolver/LDUFullMat.cpp index b3d0498..010b21e 100644 --- a/OndselSolver/LDUFullMat.cpp +++ b/OndselSolver/LDUFullMat.cpp @@ -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; diff --git a/OndselSolver/MomentOfInertiaSolver.cpp b/OndselSolver/MomentOfInertiaSolver.cpp index 04240a9..a5acf7a 100644 --- a/OndselSolver/MomentOfInertiaSolver.cpp +++ b/OndselSolver/MomentOfInertiaSolver.cpp @@ -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(); diff --git a/OndselSolver/SparseMatrix.h b/OndselSolver/SparseMatrix.h index 22276c2..eea8d69 100644 --- a/OndselSolver/SparseMatrix.h +++ b/OndselSolver/SparseMatrix.h @@ -47,10 +47,10 @@ namespace MbD { void zeroSelf() override; void atijplusFullRow(int i, int j, FRowsptr fullRow); void atijplusFullColumn(int i, int j, FColsptr fullCol); - void atijplusFullMatrix(int i, int j, FMatsptr fullMat); - void atijminusFullMatrix(int i, int j, FMatsptr fullMat); - void atijplusTransposeFullMatrix(int i, int j, FMatsptr fullMat); - void atijplusFullMatrixtimes(int i, int j, FMatsptr 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 - inline void SparseMatrix::atijplusFullMatrix(int i, int j, FMatsptr fullMat) + inline void SparseMatrix::atijplusFullMatrix(int i, int j, FMatDsptr fullMat) { for (int ii = 0; ii < fullMat->nrow(); ii++) { @@ -125,7 +125,7 @@ namespace MbD { } } template - inline void SparseMatrix::atijminusFullMatrix(int i, int j, FMatsptr fullMat) + inline void SparseMatrix::atijminusFullMatrix(int i, int j, FMatDsptr fullMat) { for (int ii = 0; ii < fullMat->nrow(); ii++) { @@ -133,7 +133,7 @@ namespace MbD { } } template - inline void SparseMatrix::atijplusTransposeFullMatrix(int i, int j, FMatsptr fullMat) + inline void SparseMatrix::atijplusTransposeFullMatrix(int i, int j, FMatDsptr fullMat) { for (int ii = 0; ii < fullMat->nrow(); ii++) { @@ -141,7 +141,7 @@ namespace MbD { } } template - inline void SparseMatrix::atijplusFullMatrixtimes(int i, int j, FMatsptr fullMat, T factor) + inline void SparseMatrix::atijplusFullMatrixtimes(int i, int j, FMatDsptr fullMat, T factor) { for (int ii = 0; ii < fullMat->nrow(); ii++) {