From c406ad8b006ef25006c1c816b4a6deef977bc301 Mon Sep 17 00:00:00 2001 From: John Dupuy Date: Mon, 6 Nov 2023 19:44:07 -0600 Subject: [PATCH] DiagonalMatrix is now double only also. --- OndselSolver/ASMTAssembly.cpp | 6 +- OndselSolver/ASMTPrincipalMassMarker.cpp | 4 +- OndselSolver/ASMTPrincipalMassMarker.h | 2 +- OndselSolver/AnyPosICNewtonRaphson.cpp | 2 +- OndselSolver/CADSystem.cpp | 26 +- OndselSolver/CMakeLists.txt | 2 + OndselSolver/DiagonalMatrix.cpp | 42 +- OndselSolver/DiagonalMatrix.h | 21 +- OndselSolver/DiagonalMatrix.ref.h | 6 +- OndselSolver/EndFrameqct.cpp | 645 +++++++++++------------ OndselSolver/EndFrameqct.h | 1 - OndselSolver/EndFrameqct2.cpp | 2 +- OndselSolver/EulerAngles.cpp | 21 +- OndselSolver/EulerAngles.h | 7 +- OndselSolver/EulerParameters.cpp | 22 +- OndselSolver/EulerParameters.h | 2 - OndselSolver/FullMatrix.cpp | 4 +- OndselSolver/FullMatrix.h | 2 +- OndselSolver/MBDynNode.cpp | 47 +- OndselSolver/MomentOfInertiaSolver.cpp | 8 +- OndselSolver/Part.cpp | 26 +- OndselSolver/Part.h | 8 +- OndselSolver/VelICSolver.cpp | 2 +- 23 files changed, 421 insertions(+), 487 deletions(-) diff --git a/OndselSolver/ASMTAssembly.cpp b/OndselSolver/ASMTAssembly.cpp index dc830b9..793f647 100644 --- a/OndselSolver/ASMTAssembly.cpp +++ b/OndselSolver/ASMTAssembly.cpp @@ -262,7 +262,7 @@ void MbD::ASMTAssembly::runSinglePendulum() auto massMarker = std::make_shared(); massMarker->setMass(0.0); massMarker->setDensity(0.0); - auto aJ = std::make_shared>(ListD{ 0, 0, 0 }); + auto aJ = std::make_shared(ListD{ 0, 0, 0 }); massMarker->setMomentOfInertias(aJ); pos3D = std::make_shared>(ListD{ 0, 0, 0 }); massMarker->setPosition3D(pos3D); @@ -307,7 +307,7 @@ void MbD::ASMTAssembly::runSinglePendulum() massMarker = std::make_shared(); massMarker->setMass(0.2); massMarker->setDensity(10.0); - aJ = std::make_shared>(ListD{ 8.3333333333333e-4, 0.016833333333333, 0.017333333333333 }); + aJ = std::make_shared(ListD{ 8.3333333333333e-4, 0.016833333333333, 0.017333333333333 }); massMarker->setMomentOfInertias(aJ); pos3D = std::make_shared>(ListD{ 0.5, 0.1, 0.05 }); massMarker->setPosition3D(pos3D); @@ -995,7 +995,7 @@ void MbD::ASMTAssembly::initprincipalMassMarker() principalMassMarker = std::make_shared(); principalMassMarker->mass = 0.0; principalMassMarker->density = 0.0; - principalMassMarker->momentOfInertias = std::make_shared>(3, 0); + principalMassMarker->momentOfInertias = std::make_shared(3, 0); //principalMassMarker->position3D = std::make_shared>(3, 0); //principalMassMarker->rotationMatrix = FullMatrixDouble>::identitysptr(3); } diff --git a/OndselSolver/ASMTPrincipalMassMarker.cpp b/OndselSolver/ASMTPrincipalMassMarker.cpp index 5b813b3..eaabb3f 100644 --- a/OndselSolver/ASMTPrincipalMassMarker.cpp +++ b/OndselSolver/ASMTPrincipalMassMarker.cpp @@ -39,7 +39,7 @@ void MbD::ASMTPrincipalMassMarker::parseASMT(std::vector& lines) lines.erase(lines.begin()); assert(lines[0] == (leadingTabs + "MomentOfInertias")); lines.erase(lines.begin()); - momentOfInertias = std::make_shared>(3); + momentOfInertias = std::make_shared(3); auto row = readRowOfDoubles(lines[0]); lines.erase(lines.begin()); for (int i = 0; i < 3; i++) @@ -70,7 +70,7 @@ void MbD::ASMTPrincipalMassMarker::setMomentOfInertias(DiagMatDsptr mat) // Overloads to simplify syntax. void MbD::ASMTPrincipalMassMarker::setMomentOfInertias(double a, double b, double c) { - momentOfInertias = std::make_shared>(ListD{ a, b, c }); + momentOfInertias = std::make_shared(ListD{ a, b, c }); } void MbD::ASMTPrincipalMassMarker::storeOnLevel(std::ofstream& os, int level) diff --git a/OndselSolver/ASMTPrincipalMassMarker.h b/OndselSolver/ASMTPrincipalMassMarker.h index 2e5740d..bd02ab0 100644 --- a/OndselSolver/ASMTPrincipalMassMarker.h +++ b/OndselSolver/ASMTPrincipalMassMarker.h @@ -26,7 +26,7 @@ namespace MbD { double mass = 0.0; double density = 0.0; - DiagMatDsptr momentOfInertias = std::make_shared>(ListD{ 0.,0.,0. }); + DiagMatDsptr momentOfInertias = std::make_shared(ListD{ 0.,0.,0. }); }; } diff --git a/OndselSolver/AnyPosICNewtonRaphson.cpp b/OndselSolver/AnyPosICNewtonRaphson.cpp index 824a0e1..7e4fd46 100644 --- a/OndselSolver/AnyPosICNewtonRaphson.cpp +++ b/OndselSolver/AnyPosICNewtonRaphson.cpp @@ -32,7 +32,7 @@ void AnyPosICNewtonRaphson::initializeGlobally() void AnyPosICNewtonRaphson::createVectorsAndMatrices() { qsuOld = std::make_shared>(nqsu); - qsuWeights = std::make_shared>(nqsu); + qsuWeights = std::make_shared(nqsu); SystemNewtonRaphson::createVectorsAndMatrices(); } diff --git a/OndselSolver/CADSystem.cpp b/OndselSolver/CADSystem.cpp index 900eb83..9737bba 100644 --- a/OndselSolver/CADSystem.cpp +++ b/OndselSolver/CADSystem.cpp @@ -87,7 +87,7 @@ void CADSystem::runOndselSinglePendulum() auto assembly1 = CREATE::With("/Assembly1"); std::cout << "assembly1->name " << assembly1->name << std::endl; assembly1->m = 0.0; - assembly1->aJ = std::make_shared>(ListD{ 0, 0, 0 }); + assembly1->aJ = std::make_shared(ListD{ 0, 0, 0 }); qX = std::make_shared>(ListD{ 0, 0, 0 }); aAap = std::make_shared(ListListD{ { 1, 0, 0 }, @@ -130,7 +130,7 @@ void CADSystem::runOndselSinglePendulum() auto crankPart1 = CREATE::With("/Assembly1/Part1"); std::cout << "crankPart1->name " << crankPart1->name << std::endl; crankPart1->m = 1.0; - crankPart1->aJ = std::make_shared>(ListD{ 1, 1, 1 }); + crankPart1->aJ = std::make_shared(ListD{ 1, 1, 1 }); qX = std::make_shared>(ListD{ 0.4, 0.0, -0.05 }); aAap = std::make_shared(ListListD{ { 1, 0, 0 }, @@ -222,7 +222,7 @@ void CADSystem::runOndselDoublePendulum() auto assembly1 = CREATE::With("/Assembly1"); std::cout << "assembly1->name " << assembly1->name << std::endl; assembly1->m = 0.0; - assembly1->aJ = std::make_shared>(ListD{ 0, 0, 0 }); + assembly1->aJ = std::make_shared(ListD{ 0, 0, 0 }); qX = std::make_shared>(ListD{ 0, 0, 0 }); aAap = std::make_shared(ListListD{ { 1, 0, 0 }, @@ -265,7 +265,7 @@ void CADSystem::runOndselDoublePendulum() auto crankPart1 = CREATE::With("/Assembly1/Part1"); std::cout << "crankPart1->name " << crankPart1->name << std::endl; crankPart1->m = 1.0; - crankPart1->aJ = std::make_shared>(ListD{ 1, 1, 1 }); + crankPart1->aJ = std::make_shared(ListD{ 1, 1, 1 }); qX = std::make_shared>(ListD{ 0.4, 0.0, -0.05 }); aAap = std::make_shared(ListListD{ { 1, 0, 0 }, @@ -307,7 +307,7 @@ void CADSystem::runOndselDoublePendulum() auto conrodPart2 = CREATE::With("/Assembly1/Part2"); std::cout << "conrodPart2->name " << conrodPart2->name << std::endl; conrodPart2->m = 1.0; - conrodPart2->aJ = std::make_shared>(ListD{ 1, 1, 1 }); + conrodPart2->aJ = std::make_shared(ListD{ 1, 1, 1 }); qX = std::make_shared>(ListD{ 0.15, 0.1, 0.05 }); qE = std::make_shared>(ListD{ 0.0, 0.0, 1.0, 0.0 }); auto eulerParameters = CREATE>::With(ListD{ 0.0, 0.0, 1.0, 0.0 }); @@ -396,7 +396,7 @@ void CADSystem::runOndselPiston() auto assembly1 = CREATE::With("/Assembly1"); std::cout << "assembly1->name " << assembly1->name << std::endl; assembly1->m = 0.0; - assembly1->aJ = std::make_shared>(ListD{ 0, 0, 0 }); + assembly1->aJ = std::make_shared(ListD{ 0, 0, 0 }); qX = std::make_shared>(ListD{ 0, 0, 0 }); qE = std::make_shared>(ListD{ 0, 0, 0, 1 }); assembly1->setqX(qX); @@ -441,7 +441,7 @@ void CADSystem::runOndselPiston() auto crankPart1 = CREATE::With("/Assembly1/Part1"); std::cout << "crankPart1->name " << crankPart1->name << std::endl; crankPart1->m = 1.0; - crankPart1->aJ = std::make_shared>(ListD{ 1, 1, 1 }); + crankPart1->aJ = std::make_shared(ListD{ 1, 1, 1 }); qX = std::make_shared>(ListD{ 0.4, 0.0, -0.05 }); qE = std::make_shared>(ListD{ 0.0, 0.0, 0.0, 1.0 }); crankPart1->setqX(qX); @@ -483,7 +483,7 @@ void CADSystem::runOndselPiston() auto conrodPart2 = CREATE::With("/Assembly1/Part2"); std::cout << "conrodPart2->name " << conrodPart2->name << std::endl; conrodPart2->m = 1.0; - conrodPart2->aJ = std::make_shared>(ListD{ 1, 1, 1 }); + conrodPart2->aJ = std::make_shared(ListD{ 1, 1, 1 }); qX = std::make_shared>(ListD{ 0.15, 0.1, 0.05 }); qE = std::make_shared>(ListD{ 0.0, 0.0, 1.0, 0.0 }); conrodPart2->setqX(qX); @@ -525,7 +525,7 @@ void CADSystem::runOndselPiston() auto pistonPart3 = CREATE::With("/Assembly1/Part3"); std::cout << "pistonPart3->name " << pistonPart3->name << std::endl; pistonPart3->m = 1.0; - pistonPart3->aJ = std::make_shared>(ListD{ 1, 1, 1 }); + pistonPart3->aJ = std::make_shared(ListD{ 1, 1, 1 }); qX = std::make_shared>(ListD{ -0.0, 1.5, 0.0 }); qE = std::make_shared>(ListD{ 0.70710678118655, 0.70710678118655, 0.0, 0.0 }); pistonPart3->setqX(qX); @@ -633,7 +633,7 @@ void CADSystem::runPiston() auto assembly1 = CREATE::With("/Assembly1"); std::cout << "assembly1->name " << assembly1->name << std::endl; assembly1->m = 0.0; - assembly1->aJ = std::make_shared>(ListD{ 0, 0, 0 }); + assembly1->aJ = std::make_shared(ListD{ 0, 0, 0 }); qX = std::make_shared>(ListD{ 0, 0, 0 }); qE = std::make_shared>(ListD{ 0, 0, 0, 1 }); assembly1->setqX(qX); @@ -678,7 +678,7 @@ void CADSystem::runPiston() auto crankPart1 = CREATE::With("/Assembly1/Part1"); std::cout << "crankPart1->name " << crankPart1->name << std::endl; crankPart1->m = 0.045210530089461; - crankPart1->aJ = std::make_shared>(ListD{ 1.7381980042084e-4, 0.003511159968501, 0.0036154518487535 }); + crankPart1->aJ = std::make_shared(ListD{ 1.7381980042084e-4, 0.003511159968501, 0.0036154518487535 }); qX = std::make_shared>(ListD{ 0.38423368514246, 2.6661567755108e-17, -0.048029210642807 }); qE = std::make_shared>(ListD{ 0.0, 0.0, 0.0, 1.0 }); crankPart1->setqX(qX); @@ -720,7 +720,7 @@ void CADSystem::runPiston() auto conrodPart2 = CREATE::With("/Assembly1/Part2"); std::cout << "conrodPart2->name " << conrodPart2->name << std::endl; conrodPart2->m = 0.067815795134192; - conrodPart2->aJ = std::make_shared>(ListD{ 2.6072970063126e-4, 0.011784982468533, 0.011941420288912 }); + conrodPart2->aJ = std::make_shared(ListD{ 2.6072970063126e-4, 0.011784982468533, 0.011941420288912 }); qX = std::make_shared>(ListD{ 0.38423368514246, 0.49215295678475, 0.048029210642807 }); qE = std::make_shared>(ListD{ 0.0, 0.0, 0.89871703427292, 0.43852900965351 }); conrodPart2->setqX(qX); @@ -762,7 +762,7 @@ void CADSystem::runPiston() auto pistonPart3 = CREATE::With("/Assembly1/Part3"); std::cout << "pistonPart3->name " << pistonPart3->name << std::endl; pistonPart3->m = 1.730132083368; - pistonPart3->aJ = std::make_shared>(ListD{ 0.19449049546716, 0.23028116340971, 0.23028116340971 }); + pistonPart3->aJ = std::make_shared(ListD{ 0.19449049546716, 0.23028116340971, 0.23028116340971 }); qX = std::make_shared>(ListD{ -1.283972762056e-18, 1.4645980199976, -4.7652385308244e-17 }); qE = std::make_shared>(ListD{ 0.70710678118655, 0.70710678118655, 0.0, 0.0 }); pistonPart3->setqX(qX); diff --git a/OndselSolver/CMakeLists.txt b/OndselSolver/CMakeLists.txt index 73738f7..a5ad462 100644 --- a/OndselSolver/CMakeLists.txt +++ b/OndselSolver/CMakeLists.txt @@ -5,6 +5,8 @@ project(OndselSolverLibrary VERSION 1.0.1 DESCRIPTION "Assembly Constraints and set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED True) +SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -gdwarf-4") + include(GNUInstallDirs) file(GLOB ONDSELSOLVER_SOURCES "*.cpp") diff --git a/OndselSolver/DiagonalMatrix.cpp b/OndselSolver/DiagonalMatrix.cpp index e561c87..c03cfc9 100644 --- a/OndselSolver/DiagonalMatrix.cpp +++ b/OndselSolver/DiagonalMatrix.cpp @@ -11,45 +11,36 @@ namespace MbD { - template<> - inline DiagMatDsptr DiagonalMatrix::times(double factor) + DiagMatDsptr DiagonalMatrix::times(double factor) { 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) = this->at(i) * factor; } return answer; } - template - inline void DiagonalMatrix::atiputDiagonalMatrix(int i, std::shared_ptr> diagMat) + void DiagonalMatrix::atiputDiagonalMatrix(int i, std::shared_ptr diagMat) { for (int ii = 0; ii < diagMat->size(); ii++) { this->at(i + ii) = diagMat->at(ii); } } - template - inline DiagMatsptr DiagonalMatrix::times(T factor) - { - assert(false); - } - template - inline FColsptr DiagonalMatrix::timesFullColumn(FColsptr fullCol) + FColsptr DiagonalMatrix::timesFullColumn(FColsptr fullCol) { //"a*b = a(i,j)b(j) sum j." 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) = this->at(i) * fullCol->at(i); } return answer; } - template - inline FMatDsptr DiagonalMatrix::timesFullMatrix(FMatDsptr fullMat) + FMatDsptr DiagonalMatrix::timesFullMatrix(FMatDsptr fullMat) { auto nrow = (int)this->size(); auto answer = std::make_shared(nrow); @@ -59,8 +50,7 @@ namespace MbD { } return answer; } - template<> - inline double DiagonalMatrix::sumOfSquares() + double DiagonalMatrix::sumOfSquares() { double sum = 0.0; for (int i = 0; i < this->size(); i++) @@ -70,21 +60,18 @@ namespace MbD { } return sum; } - template - inline int DiagonalMatrix::numberOfElements() + int DiagonalMatrix::numberOfElements() { auto n = (int)this->size(); return n * n; } - template<> - inline void DiagonalMatrix::zeroSelf() + void DiagonalMatrix::zeroSelf() { for (int i = 0; i < this->size(); i++) { this->at(i) = 0.0; } } - template<> - inline double DiagonalMatrix::maxMagnitude() + double DiagonalMatrix::maxMagnitude() { double max = 0.0; for (int i = 0; i < this->size(); i++) @@ -95,14 +82,7 @@ namespace MbD { } return max; } - template - inline double DiagonalMatrix::maxMagnitude() - { - assert(false); - return 0.0; - } - template - inline std::ostream& DiagonalMatrix::printOn(std::ostream& s) const + std::ostream& DiagonalMatrix::printOn(std::ostream& s) const { s << "DiagMat["; s << this->at(0); diff --git a/OndselSolver/DiagonalMatrix.h b/OndselSolver/DiagonalMatrix.h index 4abae1b..000ade8 100644 --- a/OndselSolver/DiagonalMatrix.h +++ b/OndselSolver/DiagonalMatrix.h @@ -14,22 +14,19 @@ #include "DiagonalMatrix.ref.h" #include "Array.h" #include "FullColumn.h" -//#include "FullRow.h" -// #include "FullMatrix.h" namespace MbD { - template - class DiagonalMatrix : public Array + class DiagonalMatrix : public Array { // public: - DiagonalMatrix() : Array() {} - DiagonalMatrix(int count) : Array(count) {} - DiagonalMatrix(int count, const T& value) : Array(count, value) {} - DiagonalMatrix(std::initializer_list list) : Array{ list } {} - void atiputDiagonalMatrix(int i, std::shared_ptr> diagMat); - DiagMatsptr times(T factor); - FColsptr timesFullColumn(FColsptr fullCol); + DiagonalMatrix() : Array() {} + explicit DiagonalMatrix(int count) : Array(count) {} + DiagonalMatrix(int count, const double& value) : Array(count, value) {} + DiagonalMatrix(std::initializer_list list) : Array{ list } {} + void atiputDiagonalMatrix(int i, std::shared_ptr diagMat); + DiagMatDsptr times(double factor); + FColsptr timesFullColumn(FColsptr fullCol); FMatDsptr timesFullMatrix(FMatDsptr fullMat); int nrow() { return (int)this->size(); @@ -44,7 +41,5 @@ namespace MbD { std::ostream& printOn(std::ostream& s) const override; }; - - template class DiagonalMatrix; } diff --git a/OndselSolver/DiagonalMatrix.ref.h b/OndselSolver/DiagonalMatrix.ref.h index ea01948..15127de 100644 --- a/OndselSolver/DiagonalMatrix.ref.h +++ b/OndselSolver/DiagonalMatrix.ref.h @@ -3,9 +3,7 @@ #include namespace MbD { - template class DiagonalMatrix; - template - using DiagMatsptr = std::shared_ptr>; - using DiagMatDsptr = std::shared_ptr>; + using DiagMatsptr = std::shared_ptr; + using DiagMatDsptr = std::shared_ptr; } \ No newline at end of file diff --git a/OndselSolver/EndFrameqct.cpp b/OndselSolver/EndFrameqct.cpp index dfc7848..b0eb3f1 100644 --- a/OndselSolver/EndFrameqct.cpp +++ b/OndselSolver/EndFrameqct.cpp @@ -9,7 +9,6 @@ #include "EndFrameqct.h" #include "MarkerFrame.h" #include "System.h" -#include "Symbolic.h" #include "Time.h" #include "EulerParameters.h" #include "CREATE.h" @@ -17,347 +16,309 @@ #include "EulerAngleszxzDot.h" #include "EulerAngleszxzDDot.h" -using namespace MbD; +namespace MbD { + template class EulerParameters; -EndFrameqct::EndFrameqct() { + EndFrameqct::EndFrameqct() { + } + + EndFrameqct::EndFrameqct(const char *str) : EndFrameqc(str) { + } + + void EndFrameqct::initialize() { + EndFrameqc::initialize(); + rmem = std::make_shared>(3); + prmempt = std::make_shared>(3); + pprmemptpt = std::make_shared>(3); + aAme = FullMatrixDouble::identitysptr(3); + pAmept = std::make_shared(3, 3); + ppAmeptpt = std::make_shared(3, 3); + pprOeOpEpt = std::make_shared(3, 4); + pprOeOptpt = std::make_shared>(3); + ppAOepEpt = std::make_shared>(4); + ppAOeptpt = std::make_shared(3, 3); + } + + void EndFrameqct::initializeLocally() { + if (!rmemBlks) { + rmem->zeroSelf(); + prmempt->zeroSelf(); + pprmemptpt->zeroSelf(); + } + if (!phiThePsiBlks) { + aAme->identity(); + pAmept->zeroSelf(); + ppAmeptpt->zeroSelf(); + } + } + + void EndFrameqct::initializeGlobally() { + if (rmemBlks) { + initprmemptBlks(); + initpprmemptptBlks(); + } + if (phiThePsiBlks) { + initpPhiThePsiptBlks(); + initppPhiThePsiptptBlks(); + } + } + + void EndFrameqct::initprmemptBlks() { + auto &mbdTime = this->root()->time; + prmemptBlks = std::make_shared>(3); + for (int i = 0; i < 3; i++) { + auto &disp = rmemBlks->at(i); + auto var = disp->differentiateWRT(mbdTime); + auto vel = var->simplified(var); + prmemptBlks->at(i) = vel; + } + } + + void EndFrameqct::initpprmemptptBlks() { + auto &mbdTime = this->root()->time; + pprmemptptBlks = std::make_shared>(3); + for (int i = 0; i < 3; i++) { + auto &vel = prmemptBlks->at(i); + auto var = vel->differentiateWRT(mbdTime); + auto acc = var->simplified(var); + pprmemptptBlks->at(i) = acc; + } + } + + void EndFrameqct::initpPhiThePsiptBlks() { + auto &mbdTime = this->root()->time; + pPhiThePsiptBlks = std::make_shared>(3); + for (int i = 0; i < 3; i++) { + auto &angle = phiThePsiBlks->at(i); + auto var = angle->differentiateWRT(mbdTime); + //std::cout << "var " << *var << std::endl; + auto vel = var->simplified(var); + //std::cout << "vel " << *vel << std::endl; + pPhiThePsiptBlks->at(i) = vel; + } + } + + void EndFrameqct::initppPhiThePsiptptBlks() { + auto &mbdTime = this->root()->time; + ppPhiThePsiptptBlks = std::make_shared>(3); + for (int i = 0; i < 3; i++) { + auto &angleVel = pPhiThePsiptBlks->at(i); + auto var = angleVel->differentiateWRT(mbdTime); + auto angleAcc = var->simplified(var); + ppPhiThePsiptptBlks->at(i) = angleAcc; + } + } + + void EndFrameqct::postInput() { + this->evalrmem(); + this->evalAme(); + Item::postInput(); + } + + void EndFrameqct::calcPostDynCorrectorIteration() { + auto &rOmO = markerFrame->rOmO; + auto &aAOm = markerFrame->aAOm; + rOeO = rOmO->plusFullColumn(aAOm->timesFullColumn(rmem)); + auto &prOmOpE = markerFrame->prOmOpE; + auto &pAOmpE = markerFrame->pAOmpE; + for (int i = 0; i < 3; i++) { + auto &prOmOpEi = prOmOpE->at(i); + auto &prOeOpEi = prOeOpE->at(i); + for (int j = 0; j < 4; j++) { + auto prOeOpEij = prOmOpEi->at(j) + pAOmpE->at(j)->at(i)->timesFullColumn(rmem); + prOeOpEi->at(j) = prOeOpEij; + } + } + auto rpep = markerFrame->rpmp->plusFullColumn(markerFrame->aApm->timesFullColumn(rmem)); + pprOeOpEpE = EulerParameters::ppApEpEtimesColumn(rpep); + aAOe = aAOm->timesFullMatrix(aAme); + for (int i = 0; i < 4; i++) { + pAOepE->at(i) = pAOmpE->at(i)->timesFullMatrix(aAme); + } + auto aApe = markerFrame->aApm->timesFullMatrix(aAme); + ppAOepEpE = EulerParameters::ppApEpEtimesMatrix(aApe); + } + + void EndFrameqct::prePosIC() { + time = this->root()->mbdTimeValue(); + this->evalrmem(); + this->evalAme(); + EndFrameqc::prePosIC(); + } + + void EndFrameqct::evalrmem() { + if (rmemBlks) { + for (int i = 0; i < 3; i++) { + auto &expression = rmemBlks->at(i); + double value = expression->getValue(); + rmem->at(i) = value; + } + } + } + + void EndFrameqct::evalAme() { + if (phiThePsiBlks) { + auto phiThePsi = CREATE>::With(); + for (int i = 0; i < 3; i++) { + auto &expression = phiThePsiBlks->at(i); + auto value = expression->getValue(); + phiThePsi->at(i) = value; + } + phiThePsi->calc(); + aAme = phiThePsi->aA; + } + } + + void EndFrameqct::preVelIC() { + time = this->root()->mbdTimeValue(); + this->evalrmem(); + this->evalAme(); + Item::preVelIC(); + this->evalprmempt(); + this->evalpAmept(); + auto &aAOm = markerFrame->aAOm; + prOeOpt = aAOm->timesFullColumn(prmempt); + pAOept = aAOm->timesFullMatrix(pAmept); + } + + void EndFrameqct::postVelIC() { + auto &pAOmpE = markerFrame->pAOmpE; + for (int i = 0; i < 3; i++) { + auto &pprOeOpEpti = pprOeOpEpt->at(i); + for (int j = 0; j < 4; j++) { + auto pprOeOpEptij = pAOmpE->at(j)->at(i)->dot(prmempt); + pprOeOpEpti->atiput(j, pprOeOpEptij); + } + } + for (int i = 0; i < 4; i++) { + ppAOepEpt->atiput(i, pAOmpE->at(i)->timesFullMatrix(pAmept)); + } + } + + FColDsptr EndFrameqct::pAjOept(int j) { + return pAOept->column(j); + } + + FMatDsptr EndFrameqct::ppAjOepETpt(int jj) { + auto answer = std::make_shared(4, 3); + for (int i = 0; i < 4; i++) { + auto &answeri = answer->at(i); + auto &ppAOepEipt = ppAOepEpt->at(i); + for (int j = 0; j < 3; j++) { + auto &answerij = ppAOepEipt->at(j)->at(jj); + answeri->atiput(j, answerij); + } + } + return answer; + } + + FColDsptr EndFrameqct::ppAjOeptpt(int j) { + return ppAOeptpt->column(j); + } + + double EndFrameqct::priOeOpt(int i) { + return prOeOpt->at(i); + } + + FRowDsptr EndFrameqct::ppriOeOpEpt(int i) { + return pprOeOpEpt->at(i); + } + + double EndFrameqct::ppriOeOptpt(int i) { + return pprOeOptpt->at(i); + } + + void EndFrameqct::evalprmempt() { + if (rmemBlks) { + for (int i = 0; i < 3; i++) { + auto &derivative = prmemptBlks->at(i); + auto value = derivative->getValue(); + prmempt->at(i) = value; + } + } + } + + void EndFrameqct::evalpAmept() { + if (phiThePsiBlks) { + auto phiThePsi = CREATE>::With(); + auto phiThePsiDot = CREATE>::With(); + phiThePsiDot->phiThePsi = phiThePsi; + for (int i = 0; i < 3; i++) { + auto &expression = phiThePsiBlks->at(i); + auto &derivative = pPhiThePsiptBlks->at(i); + auto value = expression->getValue(); + auto valueDot = derivative->getValue(); + phiThePsi->at(i) = value; + phiThePsiDot->at(i) = valueDot; + } + phiThePsi->calc(); + phiThePsiDot->calc(); + pAmept = phiThePsiDot->aAdot; + } + } + + void EndFrameqct::evalpprmemptpt() { + if (rmemBlks) { + for (int i = 0; i < 3; i++) { + auto &secondDerivative = pprmemptptBlks->at(i); + auto value = secondDerivative->getValue(); + pprmemptpt->atiput(i, value); + } + } + } + + void EndFrameqct::evalppAmeptpt() { + if (phiThePsiBlks) { + auto phiThePsi = CREATE>::With(); + auto phiThePsiDot = CREATE>::With(); + phiThePsiDot->phiThePsi = phiThePsi; + auto phiThePsiDDot = CREATE>::With(); + phiThePsiDDot->phiThePsiDot = phiThePsiDot; + for (int i = 0; i < 3; i++) { + auto &expression = phiThePsiBlks->at(i); + auto &derivative = pPhiThePsiptBlks->at(i); + auto &secondDerivative = ppPhiThePsiptptBlks->at(i); + auto value = expression->getValue(); + auto valueDot = derivative->getValue(); + auto valueDDot = secondDerivative->getValue(); + phiThePsi->atiput(i, value); + phiThePsiDot->atiput(i, valueDot); + phiThePsiDDot->atiput(i, valueDDot); + } + phiThePsi->calc(); + phiThePsiDot->calc(); + phiThePsiDDot->calc(); + ppAmeptpt = phiThePsiDDot->aAddot; + } + } + + FColDsptr EndFrameqct::rmeO() { + return markerFrame->aAOm->timesFullColumn(rmem); + } + + FColDsptr EndFrameqct::rpep() { + auto &rpmp = markerFrame->rpmp; + auto &aApm = markerFrame->aApm; + auto rpep = rpmp->plusFullColumn(aApm->timesFullColumn(rmem)); + return rpep; + } + + void EndFrameqct::preAccIC() { + time = this->root()->mbdTimeValue(); + this->evalrmem(); + this->evalAme(); + Item::preVelIC(); + this->evalprmempt(); + this->evalpAmept(); + auto &aAOm = markerFrame->aAOm; + prOeOpt = aAOm->timesFullColumn(prmempt); + pAOept = aAOm->timesFullMatrix(pAmept); + Item::preAccIC(); + this->evalpprmemptpt(); + this->evalppAmeptpt(); + aAOm = markerFrame->aAOm; + pprOeOptpt = aAOm->timesFullColumn(pprmemptpt); + ppAOeptpt = aAOm->timesFullMatrix(ppAmeptpt); + } } -EndFrameqct::EndFrameqct(const char* str) : EndFrameqc(str) { -} - -void EndFrameqct::initialize() -{ - EndFrameqc::initialize(); - rmem = std::make_shared>(3); - prmempt = std::make_shared>(3); - pprmemptpt = std::make_shared>(3); - aAme = FullMatrixDouble::identitysptr(3); - pAmept = std::make_shared(3, 3); - ppAmeptpt = std::make_shared(3, 3); - pprOeOpEpt = std::make_shared(3, 4); - pprOeOptpt = std::make_shared>(3); - ppAOepEpt = std::make_shared>(4); - ppAOeptpt = std::make_shared(3, 3); -} - -void EndFrameqct::initializeLocally() -{ - if (!rmemBlks) { - rmem->zeroSelf(); - prmempt->zeroSelf(); - pprmemptpt->zeroSelf(); - } - if (!phiThePsiBlks) { - aAme->identity(); - pAmept->zeroSelf(); - ppAmeptpt->zeroSelf(); - } -} - -void EndFrameqct::initializeGlobally() -{ - if (rmemBlks) { - initprmemptBlks(); - initpprmemptptBlks(); - } - if (phiThePsiBlks) { - initpPhiThePsiptBlks(); - initppPhiThePsiptptBlks(); - } -} - -void EndFrameqct::initprmemptBlks() -{ - auto& mbdTime = this->root()->time; - prmemptBlks = std::make_shared< FullColumn>(3); - for (int i = 0; i < 3; i++) { - auto& disp = rmemBlks->at(i); - auto var = disp->differentiateWRT(mbdTime); - auto vel = var->simplified(var); - prmemptBlks->at(i) = vel; - } -} - -void EndFrameqct::initpprmemptptBlks() -{ - auto& mbdTime = this->root()->time; - pprmemptptBlks = std::make_shared< FullColumn>(3); - for (int i = 0; i < 3; i++) { - auto& vel = prmemptBlks->at(i); - auto var = vel->differentiateWRT(mbdTime); - auto acc = var->simplified(var); - pprmemptptBlks->at(i) = acc; - } -} - -void EndFrameqct::initpPhiThePsiptBlks() -{ - auto& mbdTime = this->root()->time; - pPhiThePsiptBlks = std::make_shared< FullColumn>(3); - for (int i = 0; i < 3; i++) { - auto& angle = phiThePsiBlks->at(i); - auto var = angle->differentiateWRT(mbdTime); - //std::cout << "var " << *var << std::endl; - auto vel = var->simplified(var); - //std::cout << "vel " << *vel << std::endl; - pPhiThePsiptBlks->at(i) = vel; - } -} - -void EndFrameqct::initppPhiThePsiptptBlks() -{ - auto& mbdTime = this->root()->time; - ppPhiThePsiptptBlks = std::make_shared< FullColumn>(3); - for (int i = 0; i < 3; i++) { - auto& angleVel = pPhiThePsiptBlks->at(i); - auto var = angleVel->differentiateWRT(mbdTime); - auto angleAcc = var->simplified(var); - ppPhiThePsiptptBlks->at(i) = angleAcc; - } -} - -void EndFrameqct::postInput() -{ - this->evalrmem(); - this->evalAme(); - Item::postInput(); -} - -void EndFrameqct::calcPostDynCorrectorIteration() -{ - auto& rOmO = markerFrame->rOmO; - auto& aAOm = markerFrame->aAOm; - rOeO = rOmO->plusFullColumn(aAOm->timesFullColumn(rmem)); - auto& prOmOpE = markerFrame->prOmOpE; - auto& pAOmpE = markerFrame->pAOmpE; - for (int i = 0; i < 3; i++) - { - auto& prOmOpEi = prOmOpE->at(i); - auto& prOeOpEi = prOeOpE->at(i); - for (int j = 0; j < 4; j++) - { - auto prOeOpEij = prOmOpEi->at(j) + pAOmpE->at(j)->at(i)->timesFullColumn(rmem); - prOeOpEi->at(j) = prOeOpEij; - } - } - auto rpep = markerFrame->rpmp->plusFullColumn(markerFrame->aApm->timesFullColumn(rmem)); - pprOeOpEpE = EulerParameters::ppApEpEtimesColumn(rpep); - aAOe = aAOm->timesFullMatrix(aAme); - for (int i = 0; i < 4; i++) - { - pAOepE->at(i) = pAOmpE->at(i)->timesFullMatrix(aAme); - } - auto aApe = markerFrame->aApm->timesFullMatrix(aAme); - ppAOepEpE = EulerParameters::ppApEpEtimesMatrix(aApe); -} - -void EndFrameqct::prePosIC() -{ - time = this->root()->mbdTimeValue(); - this->evalrmem(); - this->evalAme(); - EndFrameqc::prePosIC(); -} - -void EndFrameqct::evalrmem() -{ - if (rmemBlks) { - for (int i = 0; i < 3; i++) - { - auto& expression = rmemBlks->at(i); - double value = expression->getValue(); - rmem->at(i) = value; - } - } -} - -void EndFrameqct::evalAme() -{ - if (phiThePsiBlks) { - auto phiThePsi = CREATE>::With(); - for (int i = 0; i < 3; i++) - { - auto& expression = phiThePsiBlks->at(i); - auto value = expression->getValue(); - phiThePsi->at(i) = value; - } - phiThePsi->calc(); - aAme = phiThePsi->aA; - } -} - -void EndFrameqct::preVelIC() -{ - time = this->root()->mbdTimeValue(); - this->evalrmem(); - this->evalAme(); - Item::preVelIC(); - this->evalprmempt(); - this->evalpAmept(); - auto& aAOm = markerFrame->aAOm; - prOeOpt = aAOm->timesFullColumn(prmempt); - pAOept = aAOm->timesFullMatrix(pAmept); -} - -void EndFrameqct::postVelIC() -{ - auto& pAOmpE = markerFrame->pAOmpE; - for (int i = 0; i < 3; i++) - { - auto& pprOeOpEpti = pprOeOpEpt->at(i); - for (int j = 0; j < 4; j++) - { - auto pprOeOpEptij = pAOmpE->at(j)->at(i)->dot(prmempt); - pprOeOpEpti->atiput(j, pprOeOpEptij); - } - } - for (int i = 0; i < 4; i++) - { - ppAOepEpt->atiput(i, pAOmpE->at(i)->timesFullMatrix(pAmept)); - } -} - -FColDsptr EndFrameqct::pAjOept(int j) -{ - return pAOept->column(j); -} - -FMatDsptr EndFrameqct::ppAjOepETpt(int jj) -{ - auto answer = std::make_shared(4, 3); - for (int i = 0; i < 4; i++) - { - auto& answeri = answer->at(i); - auto& ppAOepEipt = ppAOepEpt->at(i); - for (int j = 0; j < 3; j++) - { - auto& answerij = ppAOepEipt->at(j)->at(jj); - answeri->atiput(j, answerij); - } - } - return answer; -} - -FColDsptr EndFrameqct::ppAjOeptpt(int j) -{ - return ppAOeptpt->column(j); -} - -double EndFrameqct::priOeOpt(int i) -{ - return prOeOpt->at(i); -} - -FRowDsptr EndFrameqct::ppriOeOpEpt(int i) -{ - return pprOeOpEpt->at(i); -} - -double EndFrameqct::ppriOeOptpt(int i) -{ - return pprOeOptpt->at(i); -} - -void EndFrameqct::evalprmempt() -{ - if (rmemBlks) { - for (int i = 0; i < 3; i++) - { - auto& derivative = prmemptBlks->at(i); - auto value = derivative->getValue(); - prmempt->at(i) = value; - } - } -} - -void EndFrameqct::evalpAmept() -{ - if (phiThePsiBlks) { - auto phiThePsi = CREATE>::With(); - auto phiThePsiDot = CREATE>::With(); - phiThePsiDot->phiThePsi = phiThePsi; - for (int i = 0; i < 3; i++) - { - auto& expression = phiThePsiBlks->at(i); - auto& derivative = pPhiThePsiptBlks->at(i); - auto value = expression->getValue(); - auto valueDot = derivative->getValue(); - phiThePsi->at(i) = value; - phiThePsiDot->at(i) = valueDot; - } - phiThePsi->calc(); - phiThePsiDot->calc(); - pAmept = phiThePsiDot->aAdot; - } -} - -void EndFrameqct::evalpprmemptpt() -{ - if (rmemBlks) { - for (int i = 0; i < 3; i++) - { - auto& secondDerivative = pprmemptptBlks->at(i); - auto value = secondDerivative->getValue(); - pprmemptpt->atiput(i, value); - } - } -} - -void EndFrameqct::evalppAmeptpt() -{ - if (phiThePsiBlks) { - auto phiThePsi = CREATE>::With(); - auto phiThePsiDot = CREATE>::With(); - phiThePsiDot->phiThePsi = phiThePsi; - auto phiThePsiDDot = CREATE>::With(); - phiThePsiDDot->phiThePsiDot = phiThePsiDot; - for (int i = 0; i < 3; i++) - { - auto& expression = phiThePsiBlks->at(i); - auto& derivative = pPhiThePsiptBlks->at(i); - auto& secondDerivative = ppPhiThePsiptptBlks->at(i); - auto value = expression->getValue(); - auto valueDot = derivative->getValue(); - auto valueDDot = secondDerivative->getValue(); - phiThePsi->atiput(i, value); - phiThePsiDot->atiput(i, valueDot); - phiThePsiDDot->atiput(i, valueDDot); - } - phiThePsi->calc(); - phiThePsiDot->calc(); - phiThePsiDDot->calc(); - ppAmeptpt = phiThePsiDDot->aAddot; - } -} - -FColDsptr EndFrameqct::rmeO() -{ - return markerFrame->aAOm->timesFullColumn(rmem); -} - -FColDsptr EndFrameqct::rpep() -{ - auto& rpmp = markerFrame->rpmp; - auto& aApm = markerFrame->aApm; - auto rpep = rpmp->plusFullColumn(aApm->timesFullColumn(rmem)); - return rpep; -} - -void EndFrameqct::preAccIC() -{ - time = this->root()->mbdTimeValue(); - this->evalrmem(); - this->evalAme(); - Item::preVelIC(); - this->evalprmempt(); - this->evalpAmept(); - auto& aAOm = markerFrame->aAOm; - prOeOpt = aAOm->timesFullColumn(prmempt); - pAOept = aAOm->timesFullMatrix(pAmept); - Item::preAccIC(); - this->evalpprmemptpt(); - this->evalppAmeptpt(); - aAOm = markerFrame->aAOm; - pprOeOptpt = aAOm->timesFullColumn(pprmemptpt); - ppAOeptpt = aAOm->timesFullMatrix(ppAmeptpt); -} diff --git a/OndselSolver/EndFrameqct.h b/OndselSolver/EndFrameqct.h index aa6125d..bd06d7e 100644 --- a/OndselSolver/EndFrameqct.h +++ b/OndselSolver/EndFrameqct.h @@ -58,7 +58,6 @@ namespace MbD { FMatDsptr aAme, pAmept, ppAmeptpt, pAOept, ppAOeptpt; FMatDsptr pprOeOpEpt; FColFMatDsptr ppAOepEpt; - }; } diff --git a/OndselSolver/EndFrameqct2.cpp b/OndselSolver/EndFrameqct2.cpp index fc8512f..b2f0ce4 100644 --- a/OndselSolver/EndFrameqct2.cpp +++ b/OndselSolver/EndFrameqct2.cpp @@ -29,7 +29,7 @@ void EndFrameqct2::initpPhiThePsiptBlks() { auto& mbdTime = this->root()->time; auto eulerAngles = std::static_pointer_cast>(phiThePsiBlks); - pPhiThePsiptBlks = eulerAngles->differentiateWRT(mbdTime); + pPhiThePsiptBlks = differentiateWRT(*eulerAngles, mbdTime); } void EndFrameqct2::initppPhiThePsiptptBlks() diff --git a/OndselSolver/EulerAngles.cpp b/OndselSolver/EulerAngles.cpp index 00dd6ee..a33456d 100644 --- a/OndselSolver/EulerAngles.cpp +++ b/OndselSolver/EulerAngles.cpp @@ -66,16 +66,6 @@ namespace MbD { 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); @@ -83,6 +73,15 @@ namespace MbD { rotOrder->at(1) = j; rotOrder->at(2) = k; } - template class EulerAngles>; + // type-specific helper functions + std::shared_ptr>> differentiateWRT(EulerAngles>& ref, std::shared_ptr var) + { + auto derivatives = std::make_shared>>(); + std::transform(ref.begin(), ref.end(), derivatives->begin(), + [var](std::shared_ptr term) { return term->differentiateWRT(var); } + ); + derivatives->aEulerAngles = &ref; + return derivatives; + } } diff --git a/OndselSolver/EulerAngles.h b/OndselSolver/EulerAngles.h index 1a8fa81..48bceae 100644 --- a/OndselSolver/EulerAngles.h +++ b/OndselSolver/EulerAngles.h @@ -27,15 +27,14 @@ namespace MbD { EulerAngles(std::initializer_list list) : EulerArray{ list } {} void initialize() override; void calc() override; - std::shared_ptr> differentiateWRT(T var); void setRotOrder(int i, int j, int k); std::shared_ptr> rotOrder; FColFMatDsptr cA; FMatDsptr aA; }; -// // NOTE: do NOT instantiate EulerAngles as a whole as differentiateWRT breaks -// template <> -// void EulerAngles::setRotOrder(int, int, int); + template class EulerAngles>; + template class EulerAngles; + std::shared_ptr>> differentiateWRT(EulerAngles>& ref, std::shared_ptr var); } diff --git a/OndselSolver/EulerParameters.cpp b/OndselSolver/EulerParameters.cpp index b26cd71..dc31b61 100644 --- a/OndselSolver/EulerParameters.cpp +++ b/OndselSolver/EulerParameters.cpp @@ -13,7 +13,7 @@ namespace MbD { template<> - inline FMatFColDsptr EulerParameters::ppApEpEtimesColumn(FColDsptr col) + FMatFColDsptr EulerParameters::ppApEpEtimesColumn(FColDsptr col) { double a2c0 = 2 * col->at(0); double a2c1 = 2 * col->at(1); @@ -56,7 +56,7 @@ namespace MbD { } template<> - inline FMatDsptr EulerParameters::pCpEtimesColumn(FColDsptr col) + FMatDsptr EulerParameters::pCpEtimesColumn(FColDsptr col) { //"col size = 4." auto c0 = col->at(0); @@ -86,7 +86,7 @@ namespace MbD { } template - inline FMatDsptr EulerParameters::pCTpEtimesColumn(FColDsptr col) + FMatDsptr EulerParameters::pCTpEtimesColumn(FColDsptr col) { //"col size = 3." auto c0 = col->at(0); @@ -120,7 +120,7 @@ namespace MbD { } template<> - inline FMatFMatDsptr EulerParameters::ppApEpEtimesMatrix(FMatDsptr mat) + FMatFMatDsptr EulerParameters::ppApEpEtimesMatrix(FMatDsptr mat) { FRowDsptr a2m0 = mat->at(0)->times(2.0); FRowDsptr a2m1 = mat->at(1)->times(2.0); @@ -164,7 +164,7 @@ namespace MbD { } template // this is ALWAYS double; see note below. - inline void EulerParameters::initialize() + void EulerParameters::initialize() { aA = FullMatrixDouble::identitysptr(3); aB = std::make_shared(3, 4); @@ -179,18 +179,18 @@ namespace MbD { // the following can't be valid as FullMatrix instatiatiates , yet // this class needs to see the member functions of FullMatrix //template<> -//inline void EulerParameters::initialize() +//void EulerParameters::initialize() //{ //} template - inline void EulerParameters::calc() + void EulerParameters::calc() { this->calcABC(); this->calcpApE(); } template<> - inline void EulerParameters::calcABC() + void EulerParameters::calcABC() { double aE0 = this->at(0); double aE1 = this->at(1); @@ -235,7 +235,7 @@ namespace MbD { aA = aB->timesTransposeFullMatrix(aC); } template<> - inline void EulerParameters::calcpApE() + void EulerParameters::calcpApE() { double a2E0 = 2.0 * (this->at(0)); double a2E1 = 2.0 * (this->at(1)); @@ -304,9 +304,11 @@ namespace MbD { pAipEk->at(2) = a2E3; } template - inline void EulerParameters::conditionSelf() + void EulerParameters::conditionSelf() { EulerArray::conditionSelf(); this->normalizeSelf(); } + + template class EulerParameters; } diff --git a/OndselSolver/EulerParameters.h b/OndselSolver/EulerParameters.h index 1dc1c8b..35d32ae 100644 --- a/OndselSolver/EulerParameters.h +++ b/OndselSolver/EulerParameters.h @@ -59,6 +59,4 @@ namespace MbD { FMatDsptr aC; FColFMatDsptr pApE; }; - - template class EulerParameters; } diff --git a/OndselSolver/FullMatrix.cpp b/OndselSolver/FullMatrix.cpp index 7296c7f..0377eca 100644 --- a/OndselSolver/FullMatrix.cpp +++ b/OndselSolver/FullMatrix.cpp @@ -665,10 +665,10 @@ namespace MbD { return false; } } - std::shared_ptr> FullMatrixDouble::asDiagonalMatrix() + std::shared_ptr FullMatrixDouble::asDiagonalMatrix() { int nrow = this->nrow(); - auto diagMat = std::make_shared>(nrow); + auto diagMat = std::make_shared(nrow); for (int i = 0; i < nrow; i++) { diagMat->atiput(i, this->at(i)->at(i)); diff --git a/OndselSolver/FullMatrix.h b/OndselSolver/FullMatrix.h index 468dc3e..91684ef 100644 --- a/OndselSolver/FullMatrix.h +++ b/OndselSolver/FullMatrix.h @@ -93,7 +93,7 @@ namespace MbD { double trace(); bool isDiagonal(); bool isDiagonalToWithin(double ratio); - std::shared_ptr> asDiagonalMatrix(); + std::shared_ptr asDiagonalMatrix(); void conditionSelfWithTol(double tol); std::ostream& printOn(std::ostream& s) const override; FColsptr timesFullColumn(FColsptr fullCol); diff --git a/OndselSolver/MBDynNode.cpp b/OndselSolver/MBDynNode.cpp index 49f114a..675f5fa 100644 --- a/OndselSolver/MBDynNode.cpp +++ b/OndselSolver/MBDynNode.cpp @@ -20,29 +20,28 @@ void MbD::MBDynNode::outputLine(int i, std::ostream& os) auto x = asmtPart->xs->at(i); auto y = asmtPart->ys->at(i); auto z = asmtPart->zs->at(i); - // TODO: undo the breaking I just did on purpose -// auto bryantAngles = std::make_shared>(); -// bryantAngles->setRotOrder(1, 2, 3); -// bryantAngles->at(0) = asmtPart->bryxs->at(i); -// bryantAngles->at(1) = asmtPart->bryys->at(i); -// bryantAngles->at(2) = asmtPart->bryzs->at(i); -// bryantAngles->calc(); -// auto aA = bryantAngles->aA; -// auto vx = asmtPart->vxs->at(i); -// auto vy = asmtPart->vys->at(i); -// auto vz = asmtPart->vzs->at(i); -// auto omex = asmtPart->omexs->at(i); -// auto omey = asmtPart->omeys->at(i); -// auto omez = asmtPart->omezs->at(i); -// os << id << " "; -// os << x << " " << y << " " << z << " "; -// auto row = aA->at(0); -// os << row->at(0) << " " << row->at(1) << " " << row->at(2) << " "; -// row = aA->at(1); -// os << row->at(0) << " " << row->at(1) << " " << row->at(2) << " "; -// row = aA->at(2); -// os << row->at(0) << " " << row->at(1) << " " << row->at(2) << " "; -// os << vx << " " << vy << " " << vz << " "; -// os << omex << " " << omey << " " << omez << " "; + auto bryantAngles = std::make_shared>(); + bryantAngles->setRotOrder(1, 2, 3); + bryantAngles->at(0) = asmtPart->bryxs->at(i); + bryantAngles->at(1) = asmtPart->bryys->at(i); + bryantAngles->at(2) = asmtPart->bryzs->at(i); + bryantAngles->calc(); + auto aA = bryantAngles->aA; + auto vx = asmtPart->vxs->at(i); + auto vy = asmtPart->vys->at(i); + auto vz = asmtPart->vzs->at(i); + auto omex = asmtPart->omexs->at(i); + auto omey = asmtPart->omeys->at(i); + auto omez = asmtPart->omezs->at(i); + os << id << " "; + os << x << " " << y << " " << z << " "; + auto row = aA->at(0); + os << row->at(0) << " " << row->at(1) << " " << row->at(2) << " "; + row = aA->at(1); + os << row->at(0) << " " << row->at(1) << " " << row->at(2) << " "; + row = aA->at(2); + os << row->at(0) << " " << row->at(1) << " " << row->at(2) << " "; + os << vx << " " << vy << " " << vz << " "; + os << omex << " " << omey << " " << omez << " "; os << std::endl; } diff --git a/OndselSolver/MomentOfInertiaSolver.cpp b/OndselSolver/MomentOfInertiaSolver.cpp index 56401e2..6a6e43e 100644 --- a/OndselSolver/MomentOfInertiaSolver.cpp +++ b/OndselSolver/MomentOfInertiaSolver.cpp @@ -295,7 +295,7 @@ void MbD::MomentOfInertiaSolver::calcJppFromDiagJcmP() //"Eigenvalues are orders from smallest to largest." double average; - auto sortedJ = std::make_shared>(); + auto sortedJ = std::make_shared(); sortedJ->push_back(aJcmP->at(0)->at(0)); sortedJ->push_back(aJcmP->at(1)->at(1)); sortedJ->push_back(aJcmP->at(2)->at(2)); @@ -326,7 +326,7 @@ void MbD::MomentOfInertiaSolver::calcJppFromDiagJcmP() lam2 = average; } } - aJpp = std::make_shared>(ListD{ lam0, lam1, lam2 }); + aJpp = std::make_shared(ListD{ lam0, lam1, lam2 }); } void MbD::MomentOfInertiaSolver::calcJppFromFullJcmP() @@ -351,7 +351,7 @@ void MbD::MomentOfInertiaSolver::calcJppFromFullJcmP() auto phiDiv3 = modifiedArcCos(-q / std::sqrt(-p * p * p)) / 3.0; auto twoSqrtMinusp = 2.0 * std::sqrt(-p); auto piDiv3 = M_PI / 3.0; - auto sortedJ = std::make_shared>(); + auto sortedJ = std::make_shared(); sortedJ->push_back(twoSqrtMinusp * std::cos(phiDiv3)); sortedJ->push_back(twoSqrtMinusp * -std::cos(phiDiv3 + piDiv3)); sortedJ->push_back(twoSqrtMinusp * -std::cos(phiDiv3 - piDiv3)); @@ -382,7 +382,7 @@ void MbD::MomentOfInertiaSolver::calcJppFromFullJcmP() lam2 = average; } } - aJpp = std::make_shared>(ListD{ lam0, lam1, lam2 }); + aJpp = std::make_shared(ListD{ lam0, lam1, lam2 }); } double MbD::MomentOfInertiaSolver::modifiedArcCos(double val) diff --git a/OndselSolver/Part.cpp b/OndselSolver/Part.cpp index 701874c..20e881a 100644 --- a/OndselSolver/Part.cpp +++ b/OndselSolver/Part.cpp @@ -10,9 +10,11 @@ #include "PartFrame.h" #include "System.h" #include "CREATE.h" -#include "DiagonalMatrix.h" #include "EulerParameters.h" #include "PosVelAccData.h" +#include "FullColumn.h" +#include "FullMatrix.h" +#include "DiagonalMatrix.h" using namespace MbD; @@ -41,10 +43,10 @@ void Part::initializeLocally() { partFrame->initializeLocally(); if (m > 0) { - mX = std::make_shared>(3, m); + mX = std::make_shared(3, m); } else { - mX = std::make_shared>(3, 0.0); + mX = std::make_shared(3, 0.0); } } @@ -269,8 +271,8 @@ void Part::fillqsuWeights(DiagMatDsptr diagMat) auto aJiMax = this->root()->maximumMomentOfInertia(); double minw = 1.0e3; double maxw = 1.0e6; - auto wqX = std::make_shared>(3); - auto wqE = std::make_shared>(4); + auto wqX = std::make_shared(3); + auto wqE = std::make_shared(4); if (mMax == 0) { mMax = 1.0; } for (int i = 0; i < 3; i++) { @@ -317,8 +319,8 @@ void Part::fillqsudotWeights(DiagMatDsptr diagMat) if (maxInertia == 0) maxInertia = 1.0; double minw = 1.0e-12 * maxInertia; double maxw = maxInertia; - auto wqXdot = std::make_shared>(3); - auto wqEdot = std::make_shared>(4); + auto wqXdot = std::make_shared(3); + auto wqEdot = std::make_shared(4); for (int i = 0; i < 3; i++) { wqXdot->at(i) = (maxw * m / maxInertia) + minw; @@ -449,11 +451,11 @@ void Part::calcp() pE = mE->timesFullColumn(partFrame->qEdot); } -void Part::calcpdot() -{ - pXdot = mX->timesFullColumn(partFrame->qXddot); - pEdot = mEdot->timesFullColumn(partFrame->qEdot)->plusFullColumn(mE->timesFullColumn(partFrame->qEddot)); -} +//void Part::calcpdot() +//{ +// pXdot = mX->timesFullColumn(partFrame->qXddot); +// pEdot = mEdot->timesFullColumn(partFrame->qEdot)->plusFullColumn(mE->timesFullColumn(partFrame->qEddot)); +//} void Part::calcmEdot() { diff --git a/OndselSolver/Part.h b/OndselSolver/Part.h index 399f18f..df53e94 100644 --- a/OndselSolver/Part.h +++ b/OndselSolver/Part.h @@ -11,14 +11,14 @@ #include #include "Item.h" -#include "FullColumn.h" -#include "FullMatrix.h" +#include "FullColumn.ref.h" +#include "FullMatrix.ref.h" +#include "DiagonalMatrix.ref.h" #include "EulerParametersDot.h" namespace MbD { class System; class PartFrame; - template class DiagonalMatrix; class Part : public Item { @@ -100,7 +100,7 @@ namespace MbD { void fillVelICJacob(SpMatDsptr mat) override; void preAccIC() override; void calcp(); - void calcpdot(); +// void calcpdot(); void calcmEdot(); void calcpTpE(); void calcppTpEpE(); diff --git a/OndselSolver/VelICSolver.cpp b/OndselSolver/VelICSolver.cpp index b36ab99..07f582d 100644 --- a/OndselSolver/VelICSolver.cpp +++ b/OndselSolver/VelICSolver.cpp @@ -59,7 +59,7 @@ void VelICSolver::runBasic() this->assignEquationNumbers(); system->partsJointsMotionsDo([](std::shared_ptr item) { item->useEquationNumbers(); }); auto qsudotOld = std::make_shared>(nqsu); - auto qsudotWeights = std::make_shared>(nqsu); + auto qsudotWeights = std::make_shared(nqsu); errorVector = std::make_shared>(n); jacobian = std::make_shared>(n, n); system->partsJointsMotionsDo([&](std::shared_ptr item) { item->fillqsudot(qsudotOld); });