diff --git a/CMakeLists.txt b/CMakeLists.txt index c7ce907..87255c6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -7,7 +7,22 @@ set(CMAKE_CXX_STANDARD_REQUIRED True) include(GNUInstallDirs) -add_library(OndselSolver STATIC) +if( UNIX ) + set( ONDSELSOLVER_BUILD_SHARED ON ) +ELSEIF ( APPLE ) + set( ONDSELSOLVER_BUILD_SHARED ON ) +ELSE() + set( ONDSELSOLVER_BUILD_SHARED OFF ) +ENDIF () + +if ( ONDSELSOLVER_BUILD_SHARED ) + message( STATUS "[OndselSolver] Building shared library" ) + add_library(OndselSolver SHARED) +else() + message( STATUS "[OndselSolver] Building static library" ) + add_library(OndselSolver STATIC) +endif() + set(ONDSELSOLVER_SRC OndselSolver/Array.cpp @@ -601,4 +616,4 @@ install(TARGETS OndselSolver LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/OndselSolver) install(FILES ${CMAKE_BINARY_DIR}/OndselSolver.pc - DESTINATION ${CMAKE_INSTALL_DATAROOTDIR}/pkgconfig) + DESTINATION ${CMAKE_INSTALL_DATAROOTDIR}/pkgconfig) \ No newline at end of file diff --git a/OndselSolver/ASMTAssembly.cpp b/OndselSolver/ASMTAssembly.cpp index c955aa5..793f647 100644 --- a/OndselSolver/ASMTAssembly.cpp +++ b/OndselSolver/ASMTAssembly.cpp @@ -248,7 +248,7 @@ void MbD::ASMTAssembly::runSinglePendulum() assembly->setName(str); auto pos3D = std::make_shared>(ListD{ 0, 0, 0 }); assembly->setPosition3D(pos3D); - auto rotMat = std::make_shared>(ListListD{ + auto rotMat = std::make_shared(ListListD{ { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } @@ -262,11 +262,11 @@ 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); - rotMat = std::make_shared>(ListListD{ + rotMat = std::make_shared(ListListD{ { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } @@ -279,7 +279,7 @@ void MbD::ASMTAssembly::runSinglePendulum() mkr->setName(str); pos3D = std::make_shared>(ListD{ 0, 0, 0 }); mkr->setPosition3D(pos3D); - rotMat = std::make_shared>(ListListD{ + rotMat = std::make_shared(ListListD{ { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } @@ -292,7 +292,7 @@ void MbD::ASMTAssembly::runSinglePendulum() part->setName(str); pos3D = std::make_shared>(ListD{ -0.1, -0.1, -0.1 }); part->setPosition3D(pos3D); - rotMat = std::make_shared>(ListListD{ + rotMat = std::make_shared(ListListD{ { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } @@ -307,11 +307,11 @@ 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); - rotMat = std::make_shared>(ListListD{ + rotMat = std::make_shared(ListListD{ { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } @@ -324,7 +324,7 @@ void MbD::ASMTAssembly::runSinglePendulum() mkr->setName(str); pos3D = std::make_shared>(ListD{ 0.1, 0.1, 0.1 }); mkr->setPosition3D(pos3D); - rotMat = std::make_shared>(ListListD{ + rotMat = std::make_shared(ListListD{ { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } @@ -995,9 +995,9 @@ 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 = FullMatrix>::identitysptr(3); + //principalMassMarker->rotationMatrix = FullMatrixDouble>::identitysptr(3); } std::shared_ptr MbD::ASMTAssembly::spatialContainerAt(std::shared_ptr self, std::string& longname) diff --git a/OndselSolver/ASMTConstraintSet.cpp b/OndselSolver/ASMTConstraintSet.cpp index b6db283..bd1f36c 100644 --- a/OndselSolver/ASMTConstraintSet.cpp +++ b/OndselSolver/ASMTConstraintSet.cpp @@ -10,6 +10,7 @@ #include "ASMTAssembly.h" #include "ASMTMarker.h" #include "Joint.h" +#include "FullMatrix.h" using namespace MbD; diff --git a/OndselSolver/ASMTMarker.cpp b/OndselSolver/ASMTMarker.cpp index a190e96..c356bf6 100644 --- a/OndselSolver/ASMTMarker.cpp +++ b/OndselSolver/ASMTMarker.cpp @@ -7,6 +7,7 @@ ***************************************************************************/ #include "ASMTMarker.h" +#include "FullMatrix.h" #include "ASMTRefItem.h" #include "ASMTPart.h" #include "Part.h" @@ -14,57 +15,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 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/ASMTPrincipalMassMarker.cpp b/OndselSolver/ASMTPrincipalMassMarker.cpp index d5ee7fe..eaabb3f 100644 --- a/OndselSolver/ASMTPrincipalMassMarker.cpp +++ b/OndselSolver/ASMTPrincipalMassMarker.cpp @@ -26,7 +26,7 @@ void MbD::ASMTPrincipalMassMarker::parseASMT(std::vector& lines) lines.erase(lines.begin()); assert(lines[0] == (leadingTabs + "RotationMatrix")); lines.erase(lines.begin()); - rotationMatrix = std::make_shared>(3); + rotationMatrix = std::make_shared(3); for (int i = 0; i < 3; i++) { auto row = readRowOfDoubles(lines[0]); @@ -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/ASMTSpatialItem.cpp b/OndselSolver/ASMTSpatialItem.cpp index 6f5874a..931787c 100644 --- a/OndselSolver/ASMTSpatialItem.cpp +++ b/OndselSolver/ASMTSpatialItem.cpp @@ -47,7 +47,7 @@ void MbD::ASMTSpatialItem::readRotationMatrix(std::vector& lines) { assert(lines[0].find("RotationMatrix") != std::string::npos); lines.erase(lines.begin()); - rotationMatrix = std::make_shared>(3, 0); + rotationMatrix = std::make_shared(3, 0); for (int i = 0; i < 3; i++) { auto& row = rotationMatrix->at(i); @@ -86,7 +86,7 @@ void MbD::ASMTSpatialItem::setRotationMatrix(double v11, double v12, double v13, double v21, double v22, double v23, double v31, double v32, double v33) { - rotationMatrix = std::make_shared>(ListListD{ + rotationMatrix = std::make_shared(ListListD{ { v11, v12, v13 }, { v21, v22, v23 }, { v31, v32, v33 } diff --git a/OndselSolver/ASMTSpatialItem.h b/OndselSolver/ASMTSpatialItem.h index 2f5a6e2..cf556b6 100644 --- a/OndselSolver/ASMTSpatialItem.h +++ b/OndselSolver/ASMTSpatialItem.h @@ -33,7 +33,7 @@ namespace MbD { void storeOnLevelRotationMatrix(std::ofstream& os, int level); FColDsptr position3D = std::make_shared>(3); - FMatDsptr rotationMatrix = std::make_shared>(ListListD{ + FMatDsptr rotationMatrix = std::make_shared(ListListD{ { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } diff --git a/OndselSolver/AngleZIeqcJec.cpp b/OndselSolver/AngleZIeqcJec.cpp index e6aee7c..ce9935d 100644 --- a/OndselSolver/AngleZIeqcJec.cpp +++ b/OndselSolver/AngleZIeqcJec.cpp @@ -65,7 +65,7 @@ void MbD::AngleZIeqcJec::initialize() { AngleZIecJec::initialize(); pthezpEI = std::make_shared>(4); - ppthezpEIpEI = std::make_shared>(4, 4); + ppthezpEIpEI = std::make_shared(4, 4); } FMatDsptr MbD::AngleZIeqcJec::ppvaluepEIpEI() diff --git a/OndselSolver/AngleZIeqcJeqc.cpp b/OndselSolver/AngleZIeqcJeqc.cpp index 9729777..3f1ed4f 100644 --- a/OndselSolver/AngleZIeqcJeqc.cpp +++ b/OndselSolver/AngleZIeqcJeqc.cpp @@ -85,8 +85,8 @@ void MbD::AngleZIeqcJeqc::initialize() { AngleZIeqcJec::initialize(); pthezpEJ = std::make_shared>(4); - ppthezpEIpEJ = std::make_shared>(4, 4); - ppthezpEJpEJ = std::make_shared>(4, 4); + ppthezpEIpEJ = std::make_shared(4, 4); + ppthezpEJpEJ = std::make_shared(4, 4); } FMatDsptr MbD::AngleZIeqcJeqc::ppvaluepEIpEJ() 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 a3e00ff..9737bba 100644 --- a/OndselSolver/CADSystem.cpp +++ b/OndselSolver/CADSystem.cpp @@ -87,9 +87,9 @@ 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{ + aAap = std::make_shared(ListListD{ { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } @@ -106,7 +106,7 @@ void CADSystem::runOndselSinglePendulum() auto marker2 = CREATE::With("/Assembly1/Marker2"); rpmp = std::make_shared>(ListD{ 0.0, 0.0, 0.0 }); marker2->setrpmp(rpmp); - aApm = std::make_shared>(ListListD{ + aApm = std::make_shared(ListListD{ { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } @@ -117,7 +117,7 @@ void CADSystem::runOndselSinglePendulum() auto marker1 = CREATE::With("/Assembly1/Marker1"); rpmp = std::make_shared>(ListD{ 0.0, 3.0, 0.0 }); marker1->setrpmp(rpmp); - aApm = std::make_shared>(ListListD{ + aApm = std::make_shared(ListListD{ { 1, 0, 0 }, { 0, 0, 1 }, { 0, -1, 0 } @@ -130,9 +130,9 @@ 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{ + aAap = std::make_shared(ListListD{ { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } @@ -149,7 +149,7 @@ void CADSystem::runOndselSinglePendulum() auto marker1 = CREATE::With("/Assembly1/Part1/Marker1"); rpmp = std::make_shared>(ListD{ -0.4, 0.0, 0.05 }); marker1->setrpmp(rpmp); - aApm = std::make_shared>(ListListD{ + aApm = std::make_shared(ListListD{ { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } @@ -160,7 +160,7 @@ void CADSystem::runOndselSinglePendulum() auto marker2 = CREATE::With("/Assembly1/Part1/Marker2"); rpmp = std::make_shared>(ListD{ 0.4, 0.0, 0.05 }); marker2->setrpmp(rpmp); - aApm = std::make_shared>(ListListD{ + aApm = std::make_shared(ListListD{ { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } @@ -222,9 +222,9 @@ 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{ + aAap = std::make_shared(ListListD{ { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } @@ -241,7 +241,7 @@ void CADSystem::runOndselDoublePendulum() auto marker2 = CREATE::With("/Assembly1/Marker2"); rpmp = std::make_shared>(ListD{ 0.0, 0.0, 0.0 }); marker2->setrpmp(rpmp); - aApm = std::make_shared>(ListListD{ + aApm = std::make_shared(ListListD{ { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } @@ -252,7 +252,7 @@ void CADSystem::runOndselDoublePendulum() auto marker1 = CREATE::With("/Assembly1/Marker1"); rpmp = std::make_shared>(ListD{ 0.0, 3.0, 0.0 }); marker1->setrpmp(rpmp); - aApm = std::make_shared>(ListListD{ + aApm = std::make_shared(ListListD{ { 1, 0, 0 }, { 0, 0, 1 }, { 0, -1, 0 } @@ -265,9 +265,9 @@ 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{ + aAap = std::make_shared(ListListD{ { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } @@ -284,7 +284,7 @@ void CADSystem::runOndselDoublePendulum() auto marker1 = CREATE::With("/Assembly1/Part1/Marker1"); rpmp = std::make_shared>(ListD{ -0.4, 0.0, 0.05 }); marker1->setrpmp(rpmp); - aApm = std::make_shared>(ListListD{ + aApm = std::make_shared(ListListD{ { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } @@ -295,7 +295,7 @@ void CADSystem::runOndselDoublePendulum() auto marker2 = CREATE::With("/Assembly1/Part1/Marker2"); rpmp = std::make_shared>(ListD{ 0.4, 0.0, 0.05 }); marker2->setrpmp(rpmp); - aApm = std::make_shared>(ListListD{ + aApm = std::make_shared(ListListD{ { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } @@ -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 }); @@ -325,7 +325,7 @@ void CADSystem::runOndselDoublePendulum() auto marker1 = CREATE::With("/Assembly1/Part2/Marker1"); rpmp = std::make_shared>(ListD{ -0.65, 0.0, -0.05 }); marker1->setrpmp(rpmp); - aApm = std::make_shared>(ListListD{ + aApm = std::make_shared(ListListD{ {1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.0, 1.0} @@ -336,7 +336,7 @@ void CADSystem::runOndselDoublePendulum() auto marker2 = CREATE::With("/Assembly1/Part2/Marker2"); rpmp = std::make_shared>(ListD{ 0.65, 0.0, -0.05 }); marker2->setrpmp(rpmp); - aApm = std::make_shared>(ListListD{ + aApm = std::make_shared(ListListD{ {1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.0, 1.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); @@ -417,7 +417,7 @@ void CADSystem::runOndselPiston() auto marker2 = CREATE::With("/Assembly1/Marker2"); rpmp = std::make_shared>(ListD{ 0.0, 0.0, 0.0 }); marker2->setrpmp(rpmp); - aApm = std::make_shared>(ListListD{ + aApm = std::make_shared(ListListD{ { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } @@ -428,7 +428,7 @@ void CADSystem::runOndselPiston() auto marker1 = CREATE::With("/Assembly1/Marker1"); rpmp = std::make_shared>(ListD{ 0.0, 3.0, 0.0 }); marker1->setrpmp(rpmp); - aApm = std::make_shared>(ListListD{ + aApm = std::make_shared(ListListD{ { 1, 0, 0 }, { 0, 0, 1 }, { 0, -1, 0 } @@ -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); @@ -460,7 +460,7 @@ void CADSystem::runOndselPiston() auto marker1 = CREATE::With("/Assembly1/Part1/Marker1"); rpmp = std::make_shared>(ListD{ -0.4, 0.0, 0.05 }); marker1->setrpmp(rpmp); - aApm = std::make_shared>(ListListD{ + aApm = std::make_shared(ListListD{ { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } @@ -471,7 +471,7 @@ void CADSystem::runOndselPiston() auto marker2 = CREATE::With("/Assembly1/Part1/Marker2"); rpmp = std::make_shared>(ListD{ 0.4, 0.0, 0.05 }); marker2->setrpmp(rpmp); - aApm = std::make_shared>(ListListD{ + aApm = std::make_shared(ListListD{ { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } @@ -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); @@ -502,7 +502,7 @@ void CADSystem::runOndselPiston() auto marker1 = CREATE::With("/Assembly1/Part2/Marker1"); rpmp = std::make_shared>(ListD{ -0.65, 0.0, -0.05 }); marker1->setrpmp(rpmp); - aApm = std::make_shared>(ListListD{ + aApm = std::make_shared(ListListD{ {1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.0, 1.0} @@ -513,7 +513,7 @@ void CADSystem::runOndselPiston() auto marker2 = CREATE::With("/Assembly1/Part2/Marker2"); rpmp = std::make_shared>(ListD{ 0.65, 0.0, -0.05 }); marker2->setrpmp(rpmp); - aApm = std::make_shared>(ListListD{ + aApm = std::make_shared(ListListD{ {1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.0, 1.0} @@ -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); @@ -544,7 +544,7 @@ void CADSystem::runOndselPiston() auto marker1 = CREATE::With("/Assembly1/Part3/Marker1"); rpmp = std::make_shared>(ListD{ -0.5, 0.0, 0.0 }); marker1->setrpmp(rpmp); - aApm = std::make_shared>(ListListD{ + aApm = std::make_shared(ListListD{ {0.0, 1.0, 0.0}, {1.0, 0.0, 0.0}, {0.0, 0.0, -1.0} @@ -555,7 +555,7 @@ void CADSystem::runOndselPiston() auto marker2 = CREATE::With("/Assembly1/Part3/Marker2"); rpmp = std::make_shared>(ListD{ 0.5, 0.0, 0.0 }); marker2->setrpmp(rpmp); - aApm = std::make_shared>(ListListD{ + aApm = std::make_shared(ListListD{ {0.0, 0.0, 1.0}, {1.0, 0.0, 0.0}, {0.0, 1.0, 0.0} @@ -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); @@ -654,7 +654,7 @@ void CADSystem::runPiston() auto marker2 = CREATE::With("/Assembly1/Marker2"); rpmp = std::make_shared>(ListD{ 0.0, 0.0, 0.0 }); marker2->setrpmp(rpmp); - aApm = std::make_shared>(ListListD{ + aApm = std::make_shared(ListListD{ { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } @@ -665,7 +665,7 @@ void CADSystem::runPiston() auto marker1 = CREATE::With("/Assembly1/Marker1"); rpmp = std::make_shared>(ListD{ 0.0, 2.8817526385684, 0.0 }); marker1->setrpmp(rpmp); - aApm = std::make_shared>(ListListD{ + aApm = std::make_shared(ListListD{ { 1, 0, 0 }, { 0, 0, 1 }, { 0, -1, 0 } @@ -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); @@ -697,7 +697,7 @@ void CADSystem::runPiston() auto marker1 = CREATE::With("/Assembly1/Part1/Marker1"); rpmp = std::make_shared>(ListD{ -0.38423368514246, -2.6661567755108e-17, 0.048029210642807 }); marker1->setrpmp(rpmp); - aApm = std::make_shared>(ListListD{ + aApm = std::make_shared(ListListD{ { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } @@ -708,7 +708,7 @@ void CADSystem::runPiston() auto marker2 = CREATE::With("/Assembly1/Part1/Marker2"); rpmp = std::make_shared>(ListD{ 0.38423368514246, -2.6661567755108e-17, 0.048029210642807 }); marker2->setrpmp(rpmp); - aApm = std::make_shared>(ListListD{ + aApm = std::make_shared(ListListD{ { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } @@ -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); @@ -739,7 +739,7 @@ void CADSystem::runPiston() auto marker1 = CREATE::With("/Assembly1/Part2/Marker1"); rpmp = std::make_shared>(ListD{ -0.6243797383565, 1.1997705489799e-16, -0.048029210642807 }); marker1->setrpmp(rpmp); - aApm = std::make_shared>(ListListD{ + aApm = std::make_shared(ListListD{ {1.0, 2.7755575615629e-16, 0.0}, {-2.7755575615629e-16, 1.0, 0.0}, {0.0, 0.0, 1.0} @@ -750,7 +750,7 @@ void CADSystem::runPiston() auto marker2 = CREATE::With("/Assembly1/Part2/Marker2"); rpmp = std::make_shared>(ListD{ 0.6243797383565, -2.1329254204087e-16, -0.048029210642807 }); marker2->setrpmp(rpmp); - aApm = std::make_shared>(ListListD{ + aApm = std::make_shared(ListListD{ {1.0, 2.4980018054066e-16, 2.2204460492503e-16}, {-2.4980018054066e-16, 1.0, 4.1633363423443e-17}, {-2.2204460492503e-16, -4.1633363423443e-17, 1.0} @@ -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); @@ -781,7 +781,7 @@ void CADSystem::runPiston() auto marker1 = CREATE::With("/Assembly1/Part3/Marker1"); rpmp = std::make_shared>(ListD{ -0.48029210642807, 7.6201599718927e-18, -2.816737703896e-17 }); marker1->setrpmp(rpmp); - aApm = std::make_shared>(ListListD{ + aApm = std::make_shared(ListListD{ {9.2444637330587e-33, 1.0, 2.2204460492503e-16}, {1.0, -9.2444637330587e-33, -1.0785207688569e-32}, {-1.0785207688569e-32, 2.2204460492503e-16, -1.0} @@ -792,7 +792,7 @@ void CADSystem::runPiston() auto marker2 = CREATE::With("/Assembly1/Part3/Marker2"); rpmp = std::make_shared>(ListD{ 0.48029210642807, 1.7618247880058e-17, 2.5155758471256e-17 }); marker2->setrpmp(rpmp); - aApm = std::make_shared>(ListListD{ + aApm = std::make_shared(ListListD{ {6.9388939039072e-18, -6.4146353042213e-50, 1.0}, {1.0, -6.9388939039072e-18, 6.9388939039072e-18}, {-6.9388939039072e-18, 1.0, -7.4837411882581e-50} diff --git a/OndselSolver/CREATE.h b/OndselSolver/CREATE.h index 67bdf9a..099af9b 100644 --- a/OndselSolver/CREATE.h +++ b/OndselSolver/CREATE.h @@ -6,7 +6,7 @@ * See LICENSE file for details about copyright. * ***************************************************************************/ -//This header file causes wierd problems in Visual Studio when included in subclasses of std::vector or std::map. Why? +//This header file causes weird problems in Visual Studio when included in subclasses of std::vector or std::map. Why? #pragma once diff --git a/OndselSolver/ConstVelConstraintIqcJc.cpp b/OndselSolver/ConstVelConstraintIqcJc.cpp index 992a85c..bfeaa42 100644 --- a/OndselSolver/ConstVelConstraintIqcJc.cpp +++ b/OndselSolver/ConstVelConstraintIqcJc.cpp @@ -97,7 +97,7 @@ void MbD::ConstVelConstraintIqcJc::initialize() { ConstVelConstraintIJ::initialize(); pGpEI = std::make_shared>(4); - ppGpEIpEI = std::make_shared>(4, 4); + ppGpEIpEI = std::make_shared(4, 4); } void MbD::ConstVelConstraintIqcJc::useEquationNumbers() diff --git a/OndselSolver/ConstVelConstraintIqcJqc.cpp b/OndselSolver/ConstVelConstraintIqcJqc.cpp index 2a30974..1455742 100644 --- a/OndselSolver/ConstVelConstraintIqcJqc.cpp +++ b/OndselSolver/ConstVelConstraintIqcJqc.cpp @@ -118,8 +118,8 @@ void MbD::ConstVelConstraintIqcJqc::initialize() { ConstVelConstraintIqcJc::initialize(); pGpEJ = std::make_shared>(4); - ppGpEIpEJ = std::make_shared>(4, 4); - ppGpEJpEJ = std::make_shared>(4, 4); + ppGpEIpEJ = std::make_shared(4, 4); + ppGpEJpEJ = std::make_shared(4, 4); } void MbD::ConstVelConstraintIqcJqc::useEquationNumbers() diff --git a/OndselSolver/DiagonalMatrix.cpp b/OndselSolver/DiagonalMatrix.cpp index 13bba29..c03cfc9 100644 --- a/OndselSolver/DiagonalMatrix.cpp +++ b/OndselSolver/DiagonalMatrix.cpp @@ -7,5 +7,90 @@ ***************************************************************************/ #include "DiagonalMatrix.h" +#include "FullMatrix.h" -using namespace MbD; +namespace MbD { + + DiagMatDsptr DiagonalMatrix::times(double factor) + { + auto nrow = (int)this->size(); + auto answer = std::make_shared(nrow); + for (int i = 0; i < nrow; i++) + { + answer->at(i) = this->at(i) * factor; + } + return answer; + } + void DiagonalMatrix::atiputDiagonalMatrix(int i, std::shared_ptr diagMat) + { + for (int ii = 0; ii < diagMat->size(); ii++) + { + this->at(i + ii) = diagMat->at(ii); + } + } + 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); + for (int i = 0; i < nrow; i++) + { + answer->at(i) = this->at(i) * fullCol->at(i); + } + return answer; + } + FMatDsptr DiagonalMatrix::timesFullMatrix(FMatDsptr fullMat) + { + auto nrow = (int)this->size(); + auto answer = std::make_shared(nrow); + for (int i = 0; i < nrow; i++) + { + answer->at(i) = fullMat->at(i)->times(this->at(i)); + } + return answer; + } + double DiagonalMatrix::sumOfSquares() + { + double sum = 0.0; + for (int i = 0; i < this->size(); i++) + { + double element = this->at(i); + sum += element * element; + } + return sum; + } + int DiagonalMatrix::numberOfElements() + { + auto n = (int)this->size(); + return n * n; + } + void DiagonalMatrix::zeroSelf() + { + for (int i = 0; i < this->size(); i++) { + this->at(i) = 0.0; + } + } + double DiagonalMatrix::maxMagnitude() + { + double max = 0.0; + for (int i = 0; i < this->size(); i++) + { + double element = this->at(i); + if (element < 0.0) element = -element; + if (max < element) max = element; + } + return max; + } + std::ostream& DiagonalMatrix::printOn(std::ostream& s) const + { + s << "DiagMat["; + s << this->at(0); + for (int i = 1; i < this->size(); i++) + { + s << ", " << this->at(i); + } + s << "]"; + return s; + } +} diff --git a/OndselSolver/DiagonalMatrix.h b/OndselSolver/DiagonalMatrix.h index 13010c3..000ade8 100644 --- a/OndselSolver/DiagonalMatrix.h +++ b/OndselSolver/DiagonalMatrix.h @@ -8,25 +8,26 @@ #pragma once +#include "FullColumn.ref.h" +#include "FullRow.ref.h" +#include "FullMatrix.ref.h" #include "DiagonalMatrix.ref.h" #include "Array.h" #include "FullColumn.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); - FMatsptr timesFullMatrix(FMatsptr fullMat); + 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(); } @@ -39,109 +40,6 @@ namespace MbD { double maxMagnitude() override; std::ostream& printOn(std::ostream& s) const override; - }; - template<> - inline DiagMatDsptr DiagonalMatrix::times(double factor) - { - auto nrow = (int)this->size(); - 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) - { - 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) - { - //"a*b = a(i,j)b(j) sum j." - - auto nrow = (int)this->size(); - 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 FMatsptr DiagonalMatrix::timesFullMatrix(FMatsptr fullMat) - { - auto nrow = (int)this->size(); - auto answer = std::make_shared>(nrow); - for (int i = 0; i < nrow; i++) - { - answer->at(i) = fullMat->at(i)->times(this->at(i)); - } - return answer; - } - template<> - inline double DiagonalMatrix::sumOfSquares() - { - double sum = 0.0; - for (int i = 0; i < this->size(); i++) - { - double element = this->at(i); - sum += element * element; - } - return sum; - } - template - inline int DiagonalMatrix::numberOfElements() - { - auto n = (int)this->size(); - return n * n; - } - template<> - inline void DiagonalMatrix::zeroSelf() - { - for (int i = 0; i < this->size(); i++) { - this->at(i) = 0.0; - } - } - template<> - inline double DiagonalMatrix::maxMagnitude() - { - double max = 0.0; - for (int i = 0; i < this->size(); i++) - { - double element = this->at(i); - if (element < 0.0) element = -element; - if (max < element) max = element; - } - return max; - } - template - inline double DiagonalMatrix::maxMagnitude() - { - assert(false); - return 0.0; - } - template - inline std::ostream& DiagonalMatrix::printOn(std::ostream& s) const - { - s << "DiagMat["; - s << this->at(0); - for (int i = 1; i < this->size(); i++) - { - s << ", " << this->at(i); - } - s << "]"; - return s; - } } 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/DifferenceOperator.cpp b/OndselSolver/DifferenceOperator.cpp index 5fd44d7..ffe8cb9 100644 --- a/OndselSolver/DifferenceOperator.cpp +++ b/OndselSolver/DifferenceOperator.cpp @@ -57,7 +57,7 @@ void DifferenceOperator::setorder(int o) void DifferenceOperator::instantiateTaylorMatrix() { if (taylorMatrix == nullptr || (taylorMatrix->nrow() != (order + 1))) { - taylorMatrix = std::make_shared>(order + 1, order + 1); + taylorMatrix = std::make_shared(order + 1, order + 1); } } diff --git a/OndselSolver/DirectionCosineIeqcJec.cpp b/OndselSolver/DirectionCosineIeqcJec.cpp index 6e4423b..eb64f73 100644 --- a/OndselSolver/DirectionCosineIeqcJec.cpp +++ b/OndselSolver/DirectionCosineIeqcJec.cpp @@ -24,7 +24,7 @@ void DirectionCosineIeqcJec::initialize() { DirectionCosineIecJec::initialize(); pAijIeJepEI = std::make_shared>(4); - ppAijIeJepEIpEI = std::make_shared>(4, 4); + ppAijIeJepEIpEI = std::make_shared(4, 4); } void DirectionCosineIeqcJec::initializeGlobally() diff --git a/OndselSolver/DirectionCosineIeqcJeqc.cpp b/OndselSolver/DirectionCosineIeqcJeqc.cpp index 92598c9..e01444f 100644 --- a/OndselSolver/DirectionCosineIeqcJeqc.cpp +++ b/OndselSolver/DirectionCosineIeqcJeqc.cpp @@ -24,8 +24,8 @@ void DirectionCosineIeqcJeqc::initialize() { DirectionCosineIeqcJec::initialize(); pAijIeJepEJ = std::make_shared>(4); - ppAijIeJepEIpEJ = std::make_shared>(4, 4); - ppAijIeJepEJpEJ = std::make_shared>(4, 4); + ppAijIeJepEIpEJ = std::make_shared(4, 4); + ppAijIeJepEJpEJ = std::make_shared(4, 4); } void DirectionCosineIeqcJeqc::initializeGlobally() diff --git a/OndselSolver/DispCompIecJecKeqc.cpp b/OndselSolver/DispCompIecJecKeqc.cpp index efe61f5..b423512 100644 --- a/OndselSolver/DispCompIecJecKeqc.cpp +++ b/OndselSolver/DispCompIecJecKeqc.cpp @@ -22,7 +22,7 @@ DispCompIecJecKeqc::DispCompIecJecKeqc(EndFrmsptr frmi, EndFrmsptr frmj, EndFrms void DispCompIecJecKeqc::initialize() { priIeJeKepEK = std::make_shared>(4); - ppriIeJeKepEKpEK = std::make_shared>(4, 4); + ppriIeJeKepEKpEK = std::make_shared(4, 4); } void DispCompIecJecKeqc::initializeGlobally() diff --git a/OndselSolver/DispCompIeqcJecIe.cpp b/OndselSolver/DispCompIeqcJecIe.cpp index c724c66..4706787 100644 --- a/OndselSolver/DispCompIeqcJecIe.cpp +++ b/OndselSolver/DispCompIeqcJecIe.cpp @@ -87,8 +87,8 @@ void MbD::DispCompIeqcJecIe::initialize() DispCompIecJecIe::initialize(); priIeJeIepXI = std::make_shared>(3); priIeJeIepEI = std::make_shared>(4); - ppriIeJeIepXIpEI = std::make_shared>(3, 4); - ppriIeJeIepEIpEI = std::make_shared>(4, 4); + ppriIeJeIepXIpEI = std::make_shared(3, 4); + ppriIeJeIepEIpEI = std::make_shared(4, 4); } void MbD::DispCompIeqcJecIe::initializeGlobally() diff --git a/OndselSolver/DispCompIeqcJecKeqc.cpp b/OndselSolver/DispCompIeqcJecKeqc.cpp index c25efc1..5d6c96f 100644 --- a/OndselSolver/DispCompIeqcJecKeqc.cpp +++ b/OndselSolver/DispCompIeqcJecKeqc.cpp @@ -24,9 +24,9 @@ void DispCompIeqcJecKeqc::initialize() DispCompIecJecKeqc::initialize(); priIeJeKepXI = std::make_shared>(3); priIeJeKepEI = std::make_shared>(4); - ppriIeJeKepEIpEI = std::make_shared>(4, 4); - ppriIeJeKepXIpEK = std::make_shared>(3, 4); - ppriIeJeKepEIpEK = std::make_shared>(4, 4); + ppriIeJeKepEIpEI = std::make_shared(4, 4); + ppriIeJeKepXIpEK = std::make_shared(3, 4); + ppriIeJeKepEIpEK = std::make_shared(4, 4); } void DispCompIeqcJecKeqc::calcPostDynCorrectorIteration() diff --git a/OndselSolver/DispCompIeqcJeqcIe.cpp b/OndselSolver/DispCompIeqcJeqcIe.cpp index d19e98c..e02461f 100644 --- a/OndselSolver/DispCompIeqcJeqcIe.cpp +++ b/OndselSolver/DispCompIeqcJeqcIe.cpp @@ -83,9 +83,9 @@ void MbD::DispCompIeqcJeqcIe::initialize() DispCompIeqcJecIe::initialize(); priIeJeIepXJ = std::make_shared>(3); priIeJeIepEJ = std::make_shared>(4); - ppriIeJeIepEIpXJ = std::make_shared>(4, 3); - ppriIeJeIepEIpEJ = std::make_shared>(4, 4); - ppriIeJeIepEJpEJ = std::make_shared>(4, 4); + ppriIeJeIepEIpXJ = std::make_shared(4, 3); + ppriIeJeIepEIpEJ = std::make_shared(4, 4); + ppriIeJeIepEJpEJ = std::make_shared(4, 4); } FMatDsptr MbD::DispCompIeqcJeqcIe::ppvaluepEIpEJ() diff --git a/OndselSolver/DispCompIeqcJeqcKeqc.cpp b/OndselSolver/DispCompIeqcJeqcKeqc.cpp index 61a6254..554585b 100644 --- a/OndselSolver/DispCompIeqcJeqcKeqc.cpp +++ b/OndselSolver/DispCompIeqcJeqcKeqc.cpp @@ -24,9 +24,9 @@ void DispCompIeqcJeqcKeqc::initialize() DispCompIeqcJecKeqc::initialize(); priIeJeKepXJ = std::make_shared>(3); priIeJeKepEJ = std::make_shared>(4); - ppriIeJeKepEJpEJ = std::make_shared>(4, 4); - ppriIeJeKepXJpEK = std::make_shared>(3, 4); - ppriIeJeKepEJpEK = std::make_shared>(4, 4); + ppriIeJeKepEJpEJ = std::make_shared(4, 4); + ppriIeJeKepXJpEK = std::make_shared(3, 4); + ppriIeJeKepEJpEK = std::make_shared(4, 4); } void DispCompIeqcJeqcKeqc::calcPostDynCorrectorIteration() diff --git a/OndselSolver/DistIeqcJec.cpp b/OndselSolver/DistIeqcJec.cpp index ce7ed3f..d53f49c 100644 --- a/OndselSolver/DistIeqcJec.cpp +++ b/OndselSolver/DistIeqcJec.cpp @@ -41,7 +41,7 @@ void MbD::DistIeqcJec::calcPrivate() pprIeJepXIipXI->atiput(j, element / rIeJe); } } - pprIeJepXIpEI = std::make_shared>(3, 4); + pprIeJepXIpEI = std::make_shared(3, 4); for (int i = 0; i < 3; i++) { auto& pprIeJepXIipEI = pprIeJepXIpEI->at(i); @@ -53,7 +53,7 @@ void MbD::DistIeqcJec::calcPrivate() pprIeJepXIipEI->atiput(j, element / rIeJe); } } - pprIeJepEIpEI = std::make_shared>(4, 4); + pprIeJepEIpEI = std::make_shared(4, 4); for (int i = 0; i < 4; i++) { auto& pprIeJepEIipEI = pprIeJepEIpEI->at(i); @@ -74,9 +74,9 @@ void MbD::DistIeqcJec::initialize() DistIecJec::initialize(); prIeJepXI = std::make_shared>(3); prIeJepEI = std::make_shared>(4); - pprIeJepXIpXI = std::make_shared>(3, 3); - pprIeJepXIpEI = std::make_shared>(3, 4); - pprIeJepEIpEI = std::make_shared>(4, 4); + pprIeJepXIpXI = std::make_shared(3, 3); + pprIeJepXIpEI = std::make_shared(3, 4); + pprIeJepEIpEI = std::make_shared(4, 4); } FMatDsptr MbD::DistIeqcJec::ppvaluepEIpEI() diff --git a/OndselSolver/DistIeqcJeqc.cpp b/OndselSolver/DistIeqcJeqc.cpp index 60b614d..f8e565f 100644 --- a/OndselSolver/DistIeqcJeqc.cpp +++ b/OndselSolver/DistIeqcJeqc.cpp @@ -122,13 +122,13 @@ void MbD::DistIeqcJeqc::initialize() DistIeqcJec::initialize(); prIeJepXJ = std::make_shared>(3); prIeJepEJ = std::make_shared>(4); - pprIeJepXIpXJ = std::make_shared>(3, 3); - pprIeJepEIpXJ = std::make_shared>(4, 3); - pprIeJepXJpXJ = std::make_shared>(3, 3); - pprIeJepXIpEJ = std::make_shared>(3, 4); - pprIeJepEIpEJ = std::make_shared>(4, 4); - pprIeJepXJpEJ = std::make_shared>(3, 4); - pprIeJepEJpEJ = std::make_shared>(4, 4); + pprIeJepXIpXJ = std::make_shared(3, 3); + pprIeJepEIpXJ = std::make_shared(4, 3); + pprIeJepXJpXJ = std::make_shared(3, 3); + pprIeJepXIpEJ = std::make_shared(3, 4); + pprIeJepEIpEJ = std::make_shared(4, 4); + pprIeJepXJpEJ = std::make_shared(3, 4); + pprIeJepEJpEJ = std::make_shared(4, 4); } FMatDsptr MbD::DistIeqcJeqc::ppvaluepEIpEJ() diff --git a/OndselSolver/DistxyIeqcJec.cpp b/OndselSolver/DistxyIeqcJec.cpp index 12436c2..165358a 100644 --- a/OndselSolver/DistxyIeqcJec.cpp +++ b/OndselSolver/DistxyIeqcJec.cpp @@ -161,9 +161,9 @@ void MbD::DistxyIeqcJec::initialize() DistxyIecJec::initialize(); pdistxypXI = std::make_shared>(3); pdistxypEI = std::make_shared>(4); - ppdistxypXIpXI = std::make_shared>(3, 3); - ppdistxypXIpEI = std::make_shared>(3, 4); - ppdistxypEIpEI = std::make_shared>(4, 4); + ppdistxypXIpXI = std::make_shared(3, 3); + ppdistxypXIpEI = std::make_shared(3, 4); + ppdistxypEIpEI = std::make_shared(4, 4); } FMatDsptr MbD::DistxyIeqcJec::ppvaluepEIpEI() diff --git a/OndselSolver/DistxyIeqcJeqc.cpp b/OndselSolver/DistxyIeqcJeqc.cpp index 92140ff..f422f2d 100644 --- a/OndselSolver/DistxyIeqcJeqc.cpp +++ b/OndselSolver/DistxyIeqcJeqc.cpp @@ -293,13 +293,13 @@ void MbD::DistxyIeqcJeqc::initialize() DistxyIeqcJec::initialize(); pdistxypXJ = std::make_shared>(3); pdistxypEJ = std::make_shared>(4); - ppdistxypXIpXJ = std::make_shared>(3, 3); - ppdistxypXIpEJ = std::make_shared>(3, 4); - ppdistxypEIpXJ = std::make_shared>(4, 3); - ppdistxypEIpEJ = std::make_shared>(4, 4); - ppdistxypXJpXJ = std::make_shared>(3, 3); - ppdistxypXJpEJ = std::make_shared>(3, 4); - ppdistxypEJpEJ = std::make_shared>(4, 4); + ppdistxypXIpXJ = std::make_shared(3, 3); + ppdistxypXIpEJ = std::make_shared(3, 4); + ppdistxypEIpXJ = std::make_shared(4, 3); + ppdistxypEIpEJ = std::make_shared(4, 4); + ppdistxypXJpXJ = std::make_shared(3, 3); + ppdistxypXJpEJ = std::make_shared(3, 4); + ppdistxypEJpEJ = std::make_shared(4, 4); } FMatDsptr MbD::DistxyIeqcJeqc::ppvaluepEIpEJ() diff --git a/OndselSolver/EndFramec.h b/OndselSolver/EndFramec.h index add9a5a..92a3cf7 100644 --- a/OndselSolver/EndFramec.h +++ b/OndselSolver/EndFramec.h @@ -44,7 +44,7 @@ namespace MbD { MarkerFrame* markerFrame; //Use raw pointer when pointing backwards. FColDsptr rOeO = std::make_shared>(3); - FMatDsptr aAOe = FullMatrix::identitysptr(3); + FMatDsptr aAOe = FullMatrixDouble::identitysptr(3); }; //using EndFrmsptr = std::shared_ptr; } diff --git a/OndselSolver/EndFrameqc.cpp b/OndselSolver/EndFrameqc.cpp index 026bbf8..e710821 100644 --- a/OndselSolver/EndFrameqc.cpp +++ b/OndselSolver/EndFrameqc.cpp @@ -25,10 +25,10 @@ EndFrameqc::EndFrameqc(const char* str) : EndFramec(str) { void EndFrameqc::initialize() { - prOeOpE = std::make_shared>(3, 4); - pprOeOpEpE = std::make_shared>(4, 4); + prOeOpE = std::make_shared(3, 4); + pprOeOpEpE = std::make_shared(4, 4); pAOepE = std::make_shared>(4); - ppAOepEpE = std::make_shared>(4, 4); + ppAOepEpE = std::make_shared(4, 4); } void EndFrameqc::initializeGlobally() @@ -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); @@ -80,7 +80,7 @@ void EndFrameqc::calcPostDynCorrectorIteration() FMatDsptr EndFrameqc::pAjOepET(int axis) { - auto answer = std::make_shared>(4, 3); + auto answer = std::make_shared(4, 3); for (int i = 0; i < 4; i++) { auto& answeri = answer->at(i); auto& pAOepEi = pAOepE->at(i); @@ -94,7 +94,7 @@ FMatDsptr EndFrameqc::pAjOepET(int axis) FMatDsptr EndFrameqc::ppriOeOpEpE(int ii) { - auto answer = std::make_shared>(4, 4); + auto answer = std::make_shared(4, 4); for (int i = 0; i < 4; i++) { auto& answeri = answer->at(i); auto& pprOeOpEipE = pprOeOpEpE->at(i); diff --git a/OndselSolver/EndFrameqct.cpp b/OndselSolver/EndFrameqct.cpp index 2fc215e..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 = FullMatrix::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 e2a88f9..a33456d 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 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; + } + // 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 a5e1502..48bceae 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 @@ -29,87 +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; - }; - 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, FullMatrix::rotatex(angle)); - } - else if (axis == 2) { - cA->atiput(i, FullMatrix::rotatey(angle)); - } - else if (axis == 3) { - cA->atiput(i, FullMatrix::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, FullMatrix::rotatex(angle)); - } - else if (axis == 2) { - cA->atiput(i, FullMatrix::rotatey(angle)); - } - else if (axis == 3) { - cA->atiput(i, FullMatrix::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; - } + template class EulerAngles>; + template class EulerAngles; + std::shared_ptr>> differentiateWRT(EulerAngles>& ref, std::shared_ptr var); } diff --git a/OndselSolver/EulerAnglesDDot.h b/OndselSolver/EulerAnglesDDot.h index 3c5b81e..44edb61 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, FullMatrix::rotatexrotDotrotDDot(angle, angleDot, angleDDot)); + cAddot->atiput(i, FullMatrixDouble::rotatexrotDotrotDDot(angle, angleDot, angleDDot)); } else if (axis == 2) { - cAddot->atiput(i, FullMatrix::rotateyrotDotrotDDot(angle, angleDot, angleDDot)); + cAddot->atiput(i, FullMatrixDouble::rotateyrotDotrotDDot(angle, angleDot, angleDDot)); } else if (axis == 3) { - cAddot->atiput(i, FullMatrix::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."); @@ -79,8 +79,10 @@ namespace MbD { auto term7 = phiA->timesFullMatrix(theAdot->timesFullMatrix(psiAdot)); auto term8 = phiA->timesFullMatrix(theA->timesFullMatrix(psiAddot)); - aAddot = term->plusFullMatrix(term1)->plusFullMatrix(term2)->plusFullMatrix(term3)->plusFullMatrix(term4) - ->plusFullMatrix(term5)->plusFullMatrix(term6)->plusFullMatrix(term7)->plusFullMatrix(term8); + aAddot = term->plusFullMatrix(term1)->plusFullMatrix(term2) + ->plusFullMatrix(term3)->plusFullMatrix(term4) + ->plusFullMatrix(term5)->plusFullMatrix(term6) + ->plusFullMatrix(term7)->plusFullMatrix(term8); } template inline void EulerAnglesDDot::aEulerAngles(EulerAngles* eulerAngles) diff --git a/OndselSolver/EulerAnglesDot.h b/OndselSolver/EulerAnglesDot.h index f427843..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, FullMatrix::rotatexrotDot(angle, angleDot)); + cAdot->atiput(i, FullMatrixDouble::rotatexrotDot(angle, angleDot)); } else if (axis == 2) { - cAdot->atiput(i, FullMatrix::rotateyrotDot(angle, angleDot)); + cAdot->atiput(i, FullMatrixDouble::rotateyrotDot(angle, angleDot)); } else if (axis == 3) { - cAdot->atiput(i, FullMatrix::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."); @@ -79,11 +79,11 @@ namespace MbD { auto psiAdot = cAdot->at(2); aAdot = phiAdot->timesFullMatrix(theA->timesFullMatrix(psiA)) - ->plusFullMatrix(phiA->timesFullMatrix(theAdot->timesFullMatrix(psiA))) - ->plusFullMatrix(phiA->timesFullMatrix(theA->timesFullMatrix(psiAdot))); + ->plusFullMatrix(phiA->timesFullMatrix(theAdot->timesFullMatrix(psiA))) + ->plusFullMatrix(phiA->timesFullMatrix(theA->timesFullMatrix(psiAdot))); omeF = (phiA->column(0)->times(phidot) - ->plusFullColumn(phiA->timesFullMatrix(theA)->column(1)->times(thedot)) - ->plusFullColumn(aEulerAngles->aA->column(2)->times(psidot))); + ->plusFullColumn(phiA->timesFullMatrix(theA)->column(1)->times(thedot)) + ->plusFullColumn(aEulerAngles->aA->column(2)->times(psidot))); omef = aEulerAngles->aA->transposeTimesFullColumn(omeF); } } diff --git a/OndselSolver/EulerAngleszxz.h b/OndselSolver/EulerAngleszxz.h index 55dc48f..23a7617 100644 --- a/OndselSolver/EulerAngleszxz.h +++ b/OndselSolver/EulerAngleszxz.h @@ -29,9 +29,9 @@ namespace MbD { template inline void EulerAngleszxz::initialize() { - phiA = FullMatrix::identitysptr(3); - theA = FullMatrix::identitysptr(3); - psiA = FullMatrix::identitysptr(3); + phiA = FullMatrixDouble::identitysptr(3); + theA = FullMatrixDouble::identitysptr(3); + psiA = FullMatrixDouble::identitysptr(3); } template inline void EulerAngleszxz::calc() diff --git a/OndselSolver/EulerAngleszxzDDot.h b/OndselSolver/EulerAngleszxzDDot.h index 69bc344..10879af 100644 --- a/OndselSolver/EulerAngleszxzDDot.h +++ b/OndselSolver/EulerAngleszxzDDot.h @@ -8,6 +8,7 @@ #pragma once +#include "FullMatrix.h" #include "EulerArray.h" #include "EulerAngleszxzDot.h" @@ -23,16 +24,17 @@ namespace MbD { void calc() override; std::shared_ptr> phiThePsiDot; - FMatDsptr phiAddot, theAddot, psiAddot, aAddot; + FMatDsptr phiAddot, theAddot, psiAddot; + std::shared_ptr aAddot; }; template inline void EulerAngleszxzDDot::initialize() { - phiAddot = std::make_shared>(3, 3); + phiAddot = std::make_shared(3, 3); phiAddot->zeroSelf(); - theAddot = std::make_shared>(3, 3); + theAddot = std::make_shared(3, 3); theAddot->zeroSelf(); - psiAddot = std::make_shared>(3, 3); + psiAddot = std::make_shared(3, 3); psiAddot->zeroSelf(); } template @@ -90,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 2037e70..03f56b7 100644 --- a/OndselSolver/EulerAngleszxzDot.h +++ b/OndselSolver/EulerAngleszxzDot.h @@ -28,11 +28,11 @@ namespace MbD { template inline void EulerAngleszxzDot::initialize() { - phiAdot = std::make_shared>(3, 3); + phiAdot = std::make_shared(3, 3); phiAdot->zeroSelf(); - theAdot = std::make_shared>(3, 3); + theAdot = std::make_shared(3, 3); theAdot->zeroSelf(); - psiAdot = std::make_shared>(3, 3); + psiAdot = std::make_shared(3, 3); psiAdot->zeroSelf(); } template diff --git a/OndselSolver/EulerParameters.cpp b/OndselSolver/EulerParameters.cpp index 31b5672..dc31b61 100644 --- a/OndselSolver/EulerParameters.cpp +++ b/OndselSolver/EulerParameters.cpp @@ -7,294 +7,308 @@ ***************************************************************************/ #include "EulerParameters.h" +#include "FullColumn.h" +#include "FullRow.h" +#include "FullMatrix.h" -using namespace MbD; -template<> -inline FMatFColDsptr EulerParameters::ppApEpEtimesColumn(FColDsptr col) -{ - double a2c0 = 2 * col->at(0); - double a2c1 = 2 * col->at(1); - double a2c2 = 2 * col->at(2); - double m2c0 = 0 - a2c0; - double m2c1 = 0 - a2c1; - double m2c2 = 0 - a2c2; - auto col00 = std::make_shared>(ListD{ a2c0, m2c1, m2c2 }); - auto col01 = std::make_shared>(ListD{ a2c1, a2c0, 0 }); - auto col02 = std::make_shared>(ListD{ a2c2, 0, a2c0 }); - auto col03 = std::make_shared>(ListD{ 0, m2c2, a2c1 }); - auto col11 = std::make_shared>(ListD{ m2c0, a2c1, m2c2 }); - auto col12 = std::make_shared>(ListD{ 0, a2c2, a2c1 }); - auto col13 = std::make_shared>(ListD{ a2c2, 0, m2c0 }); - 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& row0 = answer->at(0); - row0->at(0) = col00; - row0->at(1) = col01; - row0->at(2) = col02; - row0->at(3) = col03; - auto& row1 = answer->at(1); - row1->at(0) = col01; - row1->at(1) = col11; - row1->at(2) = col12; - row1->at(3) = col13; - auto& row2 = answer->at(2); - row2->at(0) = col02; - row2->at(1) = col12; - row2->at(2) = col22; - row2->at(3) = col23; - auto& row3 = answer->at(3); - row3->at(0) = col03; - row3->at(1) = col13; - row3->at(2) = col23; - row3->at(3) = col33; - return answer; -} - -template<> -inline FMatDsptr EulerParameters::pCpEtimesColumn(FColDsptr col) -{ - //"col size = 4." - auto c0 = col->at(0); - auto c1 = col->at(1); - auto c2 = col->at(2); - auto mc0 = -c0; - auto mc1 = -c1; - auto mc2 = -c2; - auto mc3 = -col->at(3); - auto answer = std::make_shared>(3, 4); - auto& row0 = answer->at(0); - auto& row1 = answer->at(1); - auto& row2 = answer->at(2); - row0->atiput(0, mc3); - row0->atiput(1, mc2); - row0->atiput(2, c1); - row0->atiput(3, c0); - row1->atiput(0, c2); - row1->atiput(1, mc3); - row1->atiput(2, mc0); - row1->atiput(3, c1); - row2->atiput(0, mc1); - row2->atiput(1, c0); - row2->atiput(2, mc3); - row2->atiput(3, c2); - return answer; -} - -template -inline FMatDsptr EulerParameters::pCTpEtimesColumn(FColDsptr col) -{ - //"col size = 3." - auto c0 = col->at(0); - auto c1 = col->at(1); - auto c2 = col->at(2); - auto mc0 = -c0; - auto mc1 = -c1; - auto mc2 = -c2; - auto answer = std::make_shared>(4, 4); - auto& row0 = answer->at(0); - auto& row1 = answer->at(1); - auto& row2 = answer->at(2); - auto& row3 = answer->at(3); - row0->atiput(0, 0.0); - row0->atiput(1, c2); - row0->atiput(2, mc1); - row0->atiput(3, c0); - row1->atiput(0, mc2); - row1->atiput(1, 0.0); - row1->atiput(2, c0); - row1->atiput(3, c1); - row2->atiput(0, c1); - row2->atiput(1, mc0); - row2->atiput(2, 0.0); - row2->atiput(3, c2); - row3->atiput(0, mc0); - row3->atiput(1, mc1); - row3->atiput(2, mc2); - row3->atiput(3, 0.0); - return answer; -} - -template<> -inline FMatFMatDsptr EulerParameters::ppApEpEtimesMatrix(FMatDsptr mat) -{ - FRowDsptr a2m0 = mat->at(0)->times(2.0); - FRowDsptr a2m1 = mat->at(1)->times(2.0); - FRowDsptr a2m2 = mat->at(2)->times(2.0); - FRowDsptr m2m0 = a2m0->negated(); - FRowDsptr m2m1 = a2m1->negated(); - FRowDsptr m2m2 = a2m2->negated(); - FRowDsptr zero = std::make_shared>(3, 0.0); - auto mat00 = std::make_shared>(ListFRD{ a2m0, m2m1, m2m2 }); - auto mat01 = std::make_shared>(ListFRD{ a2m1, a2m0, zero }); - auto mat02 = std::make_shared>(ListFRD{ a2m2, zero, a2m0 }); - auto mat03 = std::make_shared>(ListFRD{ zero, m2m2, a2m1 }); - auto mat11 = std::make_shared>(ListFRD{ m2m0, a2m1, m2m2 }); - auto mat12 = std::make_shared>(ListFRD{ zero, a2m2, a2m1 }); - auto mat13 = std::make_shared>(ListFRD{ a2m2, zero, m2m0 }); - 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& row0 = answer->at(0); - row0->at(0) = mat00; - row0->at(1) = mat01; - row0->at(2) = mat02; - row0->at(3) = mat03; - auto& row1 = answer->at(1); - row1->at(0) = mat01; - row1->at(1) = mat11; - row1->at(2) = mat12; - row1->at(3) = mat13; - auto& row2 = answer->at(2); - row2->at(0) = mat02; - row2->at(1) = mat12; - row2->at(2) = mat22; - row2->at(3) = mat23; - auto& row3 = answer->at(3); - row3->at(0) = mat03; - row3->at(1) = mat13; - row3->at(2) = mat23; - row3->at(3) = mat33; - return answer; -} - -template<> -inline void EulerParameters::initialize() -{ - aA = FullMatrix::identitysptr(3); - aB = std::make_shared>(3, 4); - aC = std::make_shared>(3, 4); - pApE = std::make_shared>(4); - for (int i = 0; i < 4; i++) +namespace MbD { + template<> + FMatFColDsptr EulerParameters::ppApEpEtimesColumn(FColDsptr col) { - pApE->at(i) = std::make_shared>(3, 3); + double a2c0 = 2 * col->at(0); + double a2c1 = 2 * col->at(1); + double a2c2 = 2 * col->at(2); + double m2c0 = 0 - a2c0; + double m2c1 = 0 - a2c1; + double m2c2 = 0 - a2c2; + auto col00 = std::make_shared>(ListD{ a2c0, m2c1, m2c2 }); + auto col01 = std::make_shared>(ListD{ a2c1, a2c0, 0 }); + auto col02 = std::make_shared>(ListD{ a2c2, 0, a2c0 }); + auto col03 = std::make_shared>(ListD{ 0, m2c2, a2c1 }); + auto col11 = std::make_shared>(ListD{ m2c0, a2c1, m2c2 }); + auto col12 = std::make_shared>(ListD{ 0, a2c2, a2c1 }); + auto col13 = std::make_shared>(ListD{ a2c2, 0, m2c0 }); + 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& row0 = answer->at(0); + row0->at(0) = col00; + row0->at(1) = col01; + row0->at(2) = col02; + row0->at(3) = col03; + auto& row1 = answer->at(1); + row1->at(0) = col01; + row1->at(1) = col11; + row1->at(2) = col12; + row1->at(3) = col13; + auto& row2 = answer->at(2); + row2->at(0) = col02; + row2->at(1) = col12; + row2->at(2) = col22; + row2->at(3) = col23; + auto& row3 = answer->at(3); + row3->at(0) = col03; + row3->at(1) = col13; + row3->at(2) = col23; + row3->at(3) = col33; + return answer; } -} -template -inline void EulerParameters::calc() -{ - this->calcABC(); - this->calcpApE(); -} -template<> -inline void EulerParameters::calcABC() -{ - double aE0 = this->at(0); - double aE1 = this->at(1); - double aE2 = this->at(2); - double aE3 = this->at(3); - double mE0 = -aE0; - double mE1 = -aE1; - double mE2 = -aE2; - FRowDsptr aBi; - aBi = aB->at(0); - aBi->at(0) = aE3; - aBi->at(1) = mE2; - aBi->at(2) = aE1; - aBi->at(3) = mE0; - aBi = aB->at(1); - aBi->at(0) = aE2; - aBi->at(1) = aE3; - aBi->at(2) = mE0; - aBi->at(3) = mE1; - aBi = aB->at(2); - aBi->at(0) = mE1; - aBi->at(1) = aE0; - aBi->at(2) = aE3; - aBi->at(3) = mE2; - FRowDsptr aCi; - aCi = aC->at(0); - aCi->at(0) = aE3; - aCi->at(1) = aE2; - aCi->at(2) = mE1; - aCi->at(3) = mE0; - aCi = aC->at(1); - aCi->at(0) = mE2; - aCi->at(1) = aE3; - aCi->at(2) = aE0; - aCi->at(3) = mE1; - aCi = aC->at(2); - aCi->at(0) = aE1; - aCi->at(1) = mE0; - aCi->at(2) = aE3; - aCi->at(3) = mE2; - aA = aB->timesTransposeFullMatrix(aC); -} -template<> -inline void EulerParameters::calcpApE() -{ - double a2E0 = 2.0 * (this->at(0)); - double a2E1 = 2.0 * (this->at(1)); - double a2E2 = 2.0 * (this->at(2)); - double a2E3 = 2.0 * (this->at(3)); - double m2E0 = -a2E0; - double m2E1 = -a2E1; - double m2E2 = -a2E2; - double m2E3 = -a2E3; - FMatDsptr pApEk; - pApEk = pApE->at(0); - FRowDsptr pAipEk; - pAipEk = pApEk->at(0); - pAipEk->at(0) = a2E0; - pAipEk->at(1) = a2E1; - pAipEk->at(2) = a2E2; - pAipEk = pApEk->at(1); - pAipEk->at(0) = a2E1; - pAipEk->at(1) = m2E0; - pAipEk->at(2) = m2E3; - pAipEk = pApEk->at(2); - pAipEk->at(0) = a2E2; - pAipEk->at(1) = a2E3; - pAipEk->at(2) = m2E0; - // - pApEk = pApE->at(1); - pAipEk = pApEk->at(0); - pAipEk->at(0) = m2E1; - pAipEk->at(1) = a2E0; - pAipEk->at(2) = a2E3; - pAipEk = pApEk->at(1); - pAipEk->at(0) = a2E0; - pAipEk->at(1) = a2E1; - pAipEk->at(2) = a2E2; - pAipEk = pApEk->at(2); - pAipEk->at(0) = m2E3; - pAipEk->at(1) = a2E2; - pAipEk->at(2) = m2E1; - // - pApEk = pApE->at(2); - pAipEk = pApEk->at(0); - pAipEk->at(0) = m2E2; - pAipEk->at(1) = m2E3; - pAipEk->at(2) = a2E0; - pAipEk = pApEk->at(1); - pAipEk->at(0) = a2E3; - pAipEk->at(1) = m2E2; - pAipEk->at(2) = a2E1; - pAipEk = pApEk->at(2); - pAipEk->at(0) = a2E0; - pAipEk->at(1) = a2E1; - pAipEk->at(2) = a2E2; - // - pApEk = pApE->at(3); - pAipEk = pApEk->at(0); - pAipEk->at(0) = a2E3; - pAipEk->at(1) = m2E2; - pAipEk->at(2) = a2E1; - pAipEk = pApEk->at(1); - pAipEk->at(0) = a2E2; - pAipEk->at(1) = a2E3; - pAipEk->at(2) = m2E0; - pAipEk = pApEk->at(2); - pAipEk->at(0) = m2E1; - pAipEk->at(1) = a2E0; - pAipEk->at(2) = a2E3; -} -template -inline void EulerParameters::conditionSelf() -{ - EulerArray::conditionSelf(); - this->normalizeSelf(); + template<> + FMatDsptr EulerParameters::pCpEtimesColumn(FColDsptr col) + { + //"col size = 4." + auto c0 = col->at(0); + auto c1 = col->at(1); + auto c2 = col->at(2); + auto mc0 = -c0; + auto mc1 = -c1; + auto mc2 = -c2; + auto mc3 = -col->at(3); + auto answer = std::make_shared(3, 4); + auto& row0 = answer->at(0); + auto& row1 = answer->at(1); + auto& row2 = answer->at(2); + row0->atiput(0, mc3); + row0->atiput(1, mc2); + row0->atiput(2, c1); + row0->atiput(3, c0); + row1->atiput(0, c2); + row1->atiput(1, mc3); + row1->atiput(2, mc0); + row1->atiput(3, c1); + row2->atiput(0, mc1); + row2->atiput(1, c0); + row2->atiput(2, mc3); + row2->atiput(3, c2); + return answer; + } + + template + FMatDsptr EulerParameters::pCTpEtimesColumn(FColDsptr col) + { + //"col size = 3." + auto c0 = col->at(0); + auto c1 = col->at(1); + auto c2 = col->at(2); + auto mc0 = -c0; + auto mc1 = -c1; + auto mc2 = -c2; + auto answer = std::make_shared(4, 4); + auto& row0 = answer->at(0); + auto& row1 = answer->at(1); + auto& row2 = answer->at(2); + auto& row3 = answer->at(3); + row0->atiput(0, 0.0); + row0->atiput(1, c2); + row0->atiput(2, mc1); + row0->atiput(3, c0); + row1->atiput(0, mc2); + row1->atiput(1, 0.0); + row1->atiput(2, c0); + row1->atiput(3, c1); + row2->atiput(0, c1); + row2->atiput(1, mc0); + row2->atiput(2, 0.0); + row2->atiput(3, c2); + row3->atiput(0, mc0); + row3->atiput(1, mc1); + row3->atiput(2, mc2); + row3->atiput(3, 0.0); + return answer; + } + + template<> + FMatFMatDsptr EulerParameters::ppApEpEtimesMatrix(FMatDsptr mat) + { + FRowDsptr a2m0 = mat->at(0)->times(2.0); + FRowDsptr a2m1 = mat->at(1)->times(2.0); + FRowDsptr a2m2 = mat->at(2)->times(2.0); + FRowDsptr m2m0 = a2m0->negated(); + FRowDsptr m2m1 = a2m1->negated(); + FRowDsptr m2m2 = a2m2->negated(); + FRowDsptr zero = std::make_shared>(3, 0.0); + auto mat00 = std::make_shared(ListFRD{ a2m0, m2m1, m2m2 }); + auto mat01 = std::make_shared(ListFRD{ a2m1, a2m0, zero }); + auto mat02 = std::make_shared(ListFRD{ a2m2, zero, a2m0 }); + auto mat03 = std::make_shared(ListFRD{ zero, m2m2, a2m1 }); + auto mat11 = std::make_shared(ListFRD{ m2m0, a2m1, m2m2 }); + auto mat12 = std::make_shared(ListFRD{ zero, a2m2, a2m1 }); + auto mat13 = std::make_shared(ListFRD{ a2m2, zero, m2m0 }); + 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& row0 = answer->at(0); + row0->at(0) = mat00; + row0->at(1) = mat01; + row0->at(2) = mat02; + row0->at(3) = mat03; + auto& row1 = answer->at(1); + row1->at(0) = mat01; + row1->at(1) = mat11; + row1->at(2) = mat12; + row1->at(3) = mat13; + auto& row2 = answer->at(2); + row2->at(0) = mat02; + row2->at(1) = mat12; + row2->at(2) = mat22; + row2->at(3) = mat23; + auto& row3 = answer->at(3); + row3->at(0) = mat03; + row3->at(1) = mat13; + row3->at(2) = mat23; + row3->at(3) = mat33; + return answer; + } + + template // this is ALWAYS double; see note below. + void EulerParameters::initialize() + { + aA = FullMatrixDouble::identitysptr(3); + aB = std::make_shared(3, 4); + aC = std::make_shared(3, 4); + pApE = std::make_shared>(4); + for (int i = 0; i < 4; i++) + { + pApE->at(i) = std::make_shared(3, 3); + } + } + +// the following can't be valid as FullMatrix instatiatiates , yet +// this class needs to see the member functions of FullMatrix +//template<> +//void EulerParameters::initialize() +//{ +//} + + template + void EulerParameters::calc() + { + this->calcABC(); + this->calcpApE(); + } + template<> + void EulerParameters::calcABC() + { + double aE0 = this->at(0); + double aE1 = this->at(1); + double aE2 = this->at(2); + double aE3 = this->at(3); + double mE0 = -aE0; + double mE1 = -aE1; + double mE2 = -aE2; + FRowDsptr aBi; + aBi = aB->at(0); + aBi->at(0) = aE3; + aBi->at(1) = mE2; + aBi->at(2) = aE1; + aBi->at(3) = mE0; + aBi = aB->at(1); + aBi->at(0) = aE2; + aBi->at(1) = aE3; + aBi->at(2) = mE0; + aBi->at(3) = mE1; + aBi = aB->at(2); + aBi->at(0) = mE1; + aBi->at(1) = aE0; + aBi->at(2) = aE3; + aBi->at(3) = mE2; + FRowDsptr aCi; + aCi = aC->at(0); + aCi->at(0) = aE3; + aCi->at(1) = aE2; + aCi->at(2) = mE1; + aCi->at(3) = mE0; + aCi = aC->at(1); + aCi->at(0) = mE2; + aCi->at(1) = aE3; + aCi->at(2) = aE0; + aCi->at(3) = mE1; + aCi = aC->at(2); + aCi->at(0) = aE1; + aCi->at(1) = mE0; + aCi->at(2) = aE3; + aCi->at(3) = mE2; + + aA = aB->timesTransposeFullMatrix(aC); + } + template<> + void EulerParameters::calcpApE() + { + double a2E0 = 2.0 * (this->at(0)); + double a2E1 = 2.0 * (this->at(1)); + double a2E2 = 2.0 * (this->at(2)); + double a2E3 = 2.0 * (this->at(3)); + double m2E0 = -a2E0; + double m2E1 = -a2E1; + double m2E2 = -a2E2; + double m2E3 = -a2E3; + FMatDsptr pApEk; + pApEk = pApE->at(0); + FRowDsptr pAipEk; + pAipEk = pApEk->at(0); + pAipEk->at(0) = a2E0; + pAipEk->at(1) = a2E1; + pAipEk->at(2) = a2E2; + pAipEk = pApEk->at(1); + pAipEk->at(0) = a2E1; + pAipEk->at(1) = m2E0; + pAipEk->at(2) = m2E3; + pAipEk = pApEk->at(2); + pAipEk->at(0) = a2E2; + pAipEk->at(1) = a2E3; + pAipEk->at(2) = m2E0; + // + pApEk = pApE->at(1); + pAipEk = pApEk->at(0); + pAipEk->at(0) = m2E1; + pAipEk->at(1) = a2E0; + pAipEk->at(2) = a2E3; + pAipEk = pApEk->at(1); + pAipEk->at(0) = a2E0; + pAipEk->at(1) = a2E1; + pAipEk->at(2) = a2E2; + pAipEk = pApEk->at(2); + pAipEk->at(0) = m2E3; + pAipEk->at(1) = a2E2; + pAipEk->at(2) = m2E1; + // + pApEk = pApE->at(2); + pAipEk = pApEk->at(0); + pAipEk->at(0) = m2E2; + pAipEk->at(1) = m2E3; + pAipEk->at(2) = a2E0; + pAipEk = pApEk->at(1); + pAipEk->at(0) = a2E3; + pAipEk->at(1) = m2E2; + pAipEk->at(2) = a2E1; + pAipEk = pApEk->at(2); + pAipEk->at(0) = a2E0; + pAipEk->at(1) = a2E1; + pAipEk->at(2) = a2E2; + // + pApEk = pApE->at(3); + pAipEk = pApEk->at(0); + pAipEk->at(0) = a2E3; + pAipEk->at(1) = m2E2; + pAipEk->at(2) = a2E1; + pAipEk = pApEk->at(1); + pAipEk->at(0) = a2E2; + pAipEk->at(1) = a2E3; + pAipEk->at(2) = m2E0; + pAipEk = pApEk->at(2); + pAipEk->at(0) = m2E1; + pAipEk->at(1) = a2E0; + pAipEk->at(2) = a2E3; + } + template + void EulerParameters::conditionSelf() + { + EulerArray::conditionSelf(); + this->normalizeSelf(); + } + + template class EulerParameters; } diff --git a/OndselSolver/EulerParameters.h b/OndselSolver/EulerParameters.h index 887ed5d..35d32ae 100644 --- a/OndselSolver/EulerParameters.h +++ b/OndselSolver/EulerParameters.h @@ -8,10 +8,12 @@ #pragma once +#include "FullColumn.ref.h" +#include "FullMatrix.ref.h" #include "EulerParameters.ref.h" #include "EulerArray.h" -#include "FullColumn.h" -#include "FullMatrix.h" +// #include "FullColumn.h" +// #include "FullMatrix.h" namespace MbD { @@ -40,11 +42,10 @@ namespace MbD { this->initialize(); 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); + static std::shared_ptr ppApEpEtimesMatrix(FMatDsptr mat); void initialize() override; diff --git a/OndselSolver/EulerParametersDot.h b/OndselSolver/EulerParametersDot.h index 46d980f..6389f13 100644 --- a/OndselSolver/EulerParametersDot.h +++ b/OndselSolver/EulerParametersDot.h @@ -55,13 +55,13 @@ namespace MbD { template inline void EulerParametersDot::initialize() { - aAdot = std::make_shared>(3, 3); - aBdot = std::make_shared>(3, 4); - aCdot = std::make_shared>(3, 4); + aAdot = std::make_shared(3, 3); + aBdot = std::make_shared(3, 4); + aCdot = std::make_shared(3, 4); pAdotpE = std::make_shared>(4); for (int i = 0; i < 4; i++) { - pAdotpE->at(i) = std::make_shared>(3, 3); + pAdotpE->at(i) = std::make_shared(3, 3); } } @@ -185,7 +185,7 @@ namespace MbD { inline FColDsptr EulerParametersDot::omeOpO() { auto aaa = this->aB(); - auto bbb = aaa->timesFullColumn(this); + auto bbb = aaa->timesFullColumn((MbD::FColsptr)this); auto ccc = bbb->times(2.0); return ccc; //return this->aB->timesFullColumn(this)->times(2.0); diff --git a/OndselSolver/FullColumn.cpp b/OndselSolver/FullColumn.cpp index e689f73..b173b08 100644 --- a/OndselSolver/FullColumn.cpp +++ b/OndselSolver/FullColumn.cpp @@ -9,5 +9,165 @@ #include #include "FullColumn.h" +#include "FullRow.h" +#include "FullMatrix.h" -using namespace MbD; +namespace MbD { + template + FColsptr FullColumn::plusFullColumn(FColsptr fullCol) + { + int n = (int) this->size(); + auto answer = std::make_shared>(n); + for (int i = 0; i < n; i++) { + answer->at(i) = this->at(i) + fullCol->at(i); + } + return answer; + } + template + FColsptr FullColumn::minusFullColumn(FColsptr fullCol) + { + int n = (int) this->size(); + auto answer = std::make_shared>(n); + for (int i = 0; i < n; i++) { + answer->at(i) = this->at(i) - fullCol->at(i); + } + return answer; + } + template<> + FColDsptr FullColumn::times(double a) + { + int n = (int)this->size(); + auto answer = std::make_shared>(n); + for (int i = 0; i < n; i++) { + answer->at(i) = this->at(i) * a; + } + return answer; + } + template + FColsptr FullColumn::times(T a) + { + assert(false); + auto answer = std::make_shared>(); + return answer; + } + template + FColsptr FullColumn::negated() + { + return this->times(-1.0); + } + template + void FullColumn::atiputFullColumn(int i, FColsptr fullCol) + { + for (int ii = 0; ii < fullCol->size(); ii++) + { + this->at(i + ii) = fullCol->at(ii); + } + } + template + void FullColumn::atiplusFullColumn(int i, FColsptr fullCol) + { + for (int ii = 0; ii < fullCol->size(); ii++) + { + this->at(i + ii) += fullCol->at(ii); + } + } + template + void FullColumn::equalSelfPlusFullColumnAt(FColsptr fullCol, int ii) + { + //self is subcolumn of fullCol + for (int i = 0; i < this->size(); i++) + { + this->at(i) += fullCol->at(ii + i); + } + } + template + void FullColumn::atiminusFullColumn(int i1, FColsptr fullCol) + { + for (int ii = 0; ii < fullCol->size(); ii++) + { + int i = i1 + ii; + this->at(i) -= fullCol->at(ii); + } + } + template + void FullColumn::equalFullColumnAt(FColsptr fullCol, int i) + { + this->equalArrayAt(fullCol, i); + //for (int ii = 0; ii < this->size(); ii++) + //{ + // this->at(ii) = fullCol->at(i + ii); + //} + } + template<> + FColDsptr FullColumn::copy() + { + auto n = (int) this->size(); + auto answer = std::make_shared>(n); + for (int i = 0; i < n; i++) + { + answer->at(i) = this->at(i); + } + return answer; + } + template + FRowsptr FullColumn::transpose() + { + return std::make_shared>(*this); + } + template + void FullColumn::atiplusFullColumntimes(int i1, FColsptr fullCol, T factor) + { + for (int ii = 0; ii < fullCol->size(); ii++) + { + int i = i1 + ii; + this->at(i) += fullCol->at(ii) * factor; + } + } + template + T FullColumn::transposeTimesFullColumn(const FColsptr fullCol) + { + return this->dot(fullCol); + } + template + void FullColumn::equalSelfPlusFullColumntimes(FColsptr fullCol, T factor) + { + this->equalSelfPlusFullVectortimes(fullCol, factor); + } + template + FColsptr FullColumn::cross(FColsptr fullCol) + { + auto a0 = this->at(0); + auto a1 = this->at(1); + auto a2 = this->at(2); + auto b0 = fullCol->at(0); + auto b1 = fullCol->at(1); + auto b2 = fullCol->at(2); + auto answer = std::make_shared>(3); + answer->atiput(0, a1 * b2 - (a2 * b1)); + answer->atiput(1, a2 * b0 - (a0 * b2)); + answer->atiput(2, a0 * b1 - (a1 * b0)); + return answer; + } + //template<> + //inline std::shared_ptr> FullColumn::simplified() + //{ + // auto n = this->size(); + // auto answer = std::make_shared>(n); + // for (int i = 0; i < n; i++) + // { + // auto func = this->at(i); + // answer->at(i) = func->simplified(func); + // } + // return answer; + //} + template + FColsptr FullColumn::simplified() + { +// assert(false); + return FColsptr(); + } + // instantiate on purpose to make visible in library api: + template class FullColumn; + template class FullColumn; + +} \ No newline at end of file diff --git a/OndselSolver/FullColumn.h b/OndselSolver/FullColumn.h index 4f495bb..a015278 100644 --- a/OndselSolver/FullColumn.h +++ b/OndselSolver/FullColumn.h @@ -14,19 +14,10 @@ #include "FullVector.h" #include "FullColumn.ref.h" #include "FullRow.ref.h" -#include "FullColumn.h" -#include "FullRow.h" +#include "FullMatrix.ref.h" +//#include "Symbolic.h" namespace MbD { -// template -// class FullColumn; -// using FColDsptr = std::shared_ptr>; -// template -// using FColsptr = std::shared_ptr>; -// template -// class FullRow; -// template -// using FRowsptr = std::shared_ptr>; class Symbolic; template @@ -58,168 +49,17 @@ namespace MbD { std::ostream& printOn(std::ostream& s) const override; }; - template - inline FColsptr FullColumn::plusFullColumn(FColsptr fullCol) - { - int n = (int) this->size(); - auto answer = std::make_shared>(n); - for (int i = 0; i < n; i++) { - answer->at(i) = this->at(i) + fullCol->at(i); - } - return answer; - } - template - inline FColsptr FullColumn::minusFullColumn(FColsptr fullCol) - { - int n = (int) this->size(); - auto answer = std::make_shared>(n); - for (int i = 0; i < n; i++) { - answer->at(i) = this->at(i) - fullCol->at(i); - } - return answer; - } - template<> - inline FColDsptr FullColumn::times(double a) - { - int n = (int)this->size(); - auto answer = std::make_shared>(n); - for (int i = 0; i < n; i++) { - answer->at(i) = this->at(i) * a; - } - return answer; - } - template - inline FColsptr FullColumn::times(T a) - { - assert(false); - } - template - inline FColsptr FullColumn::negated() - { - return this->times(-1.0); - } - template - inline void FullColumn::atiputFullColumn(int i, FColsptr fullCol) - { - for (int ii = 0; ii < fullCol->size(); ii++) - { - this->at(i + ii) = fullCol->at(ii); - } - } - template - inline void FullColumn::atiplusFullColumn(int i, FColsptr fullCol) - { - for (int ii = 0; ii < fullCol->size(); ii++) - { - this->at(i + ii) += fullCol->at(ii); - } - } - template - inline void FullColumn::equalSelfPlusFullColumnAt(FColsptr fullCol, int ii) - { - //self is subcolumn of fullCol - for (int i = 0; i < this->size(); i++) - { - this->at(i) += fullCol->at(ii + i); - } - } - template - inline void FullColumn::atiminusFullColumn(int i1, FColsptr fullCol) - { - for (int ii = 0; ii < fullCol->size(); ii++) - { - int i = i1 + ii; - this->at(i) -= fullCol->at(ii); - } - } - template - inline void FullColumn::equalFullColumnAt(FColsptr fullCol, int i) - { - this->equalArrayAt(fullCol, i); - //for (int ii = 0; ii < this->size(); ii++) - //{ - // this->at(ii) = fullCol->at(i + ii); - //} - } - template<> - inline FColDsptr FullColumn::copy() - { - auto n = (int) this->size(); - auto answer = std::make_shared>(n); - for (int i = 0; i < n; i++) - { - answer->at(i) = this->at(i); - } - return answer; - } - template - inline FRowsptr FullColumn::transpose() - { - return std::make_shared>(*this); - } - template - inline void FullColumn::atiplusFullColumntimes(int i1, FColsptr fullCol, T factor) - { - for (int ii = 0; ii < fullCol->size(); ii++) - { - int i = i1 + ii; - this->at(i) += fullCol->at(ii) * factor; - } - } - template - inline T FullColumn::transposeTimesFullColumn(const FColsptr fullCol) - { - return this->dot(fullCol); - } - template - inline void FullColumn::equalSelfPlusFullColumntimes(FColsptr fullCol, T factor) - { - this->equalSelfPlusFullVectortimes(fullCol, factor); - } - template - inline FColsptr FullColumn::cross(FColsptr fullCol) - { - auto a0 = this->at(0); - auto a1 = this->at(1); - auto a2 = this->at(2); - auto b0 = fullCol->at(0); - auto b1 = fullCol->at(1); - auto b2 = fullCol->at(2); - auto answer = std::make_shared>(3); - answer->atiput(0, a1 * b2 - (a2 * b1)); - answer->atiput(1, a2 * b0 - (a0 * b2)); - answer->atiput(2, a0 * b1 - (a1 * b0)); - return answer; - } - //template<> - //inline std::shared_ptr> FullColumn::simplified() - //{ - // auto n = this->size(); - // auto answer = std::make_shared>(n); - // for (int i = 0; i < n; i++) - // { - // auto func = this->at(i); - // answer->at(i) = func->simplified(func); - // } - // return answer; - //} - template - inline FColsptr FullColumn::simplified() - { - assert(false); - return FColsptr(); - } - template - inline std::ostream& FullColumn::printOn(std::ostream& s) const - { - s << "FullCol{"; - s << this->at(0); - for (int i = 1; i < this->size(); i++) - { - s << ", " << this->at(i); - } - s << "}"; - return s; - } + // the following "printOn" needs to be in the header for unknown reasons linker + template + std::ostream& FullColumn::printOn(std::ostream& s) const + { + s << "FullCol{"; + s << this->at(0); + for (int i = 1; i < int(this->size()); i++) + { + s << ", " << this->at(i); + } + s << "}"; + return s; + } } - diff --git a/OndselSolver/FullColumn.ref.h b/OndselSolver/FullColumn.ref.h index 7673a9c..d622179 100644 --- a/OndselSolver/FullColumn.ref.h +++ b/OndselSolver/FullColumn.ref.h @@ -1,5 +1,8 @@ #pragma once +#include +#include + namespace MbD { template class FullColumn; diff --git a/OndselSolver/FullMatrix.cpp b/OndselSolver/FullMatrix.cpp index efae947..0377eca 100644 --- a/OndselSolver/FullMatrix.cpp +++ b/OndselSolver/FullMatrix.cpp @@ -5,640 +5,680 @@ * * * See LICENSE file for details about copyright. * ***************************************************************************/ - + #include "FullMatrix.h" #include "FullColumn.h" #include "FullRow.h" #include "DiagonalMatrix.h" #include "EulerParameters.h" -using namespace MbD; - -template -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; -} -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 { + FColsptr FullMatrixDouble::timesFullColumn(FColsptr fullCol) + { +// return this->timesFullColumn(fullCol.get()); + 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; } -} -template<> -inline void FullMatrix::identity() { - this->zeroSelf(); - for (int i = 0; i < this->size(); i++) { - this->at(i)->at(i) = 1.0; +// FColsptr timesFullColumn(FullColumn* fullCol) // local +// { +// //"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; +// } + std::shared_ptr FullMatrixDouble::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 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); + std::shared_ptr FullMatrixDouble::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; } - 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); + std::shared_ptr FullMatrixDouble::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; } - return answer; -} -template -inline FMatsptr FullMatrix::timesTransposeFullMatrix(FMatsptr 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); + std::shared_ptr FullMatrixDouble::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; } - return answer; -} -template<> -inline FMatDsptr FullMatrix::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); + std::shared_ptr FullMatrixDouble::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; } - return answer; -} -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)); + std::shared_ptr FullMatrixDouble::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; } - 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)); + std::shared_ptr FullMatrixDouble::rotatexrotDotrotDDot(double the, double thedot, double 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; } - 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); + std::shared_ptr FullMatrixDouble::rotateyrotDotrotDDot(double the, double thedot, double 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; + } + std::shared_ptr FullMatrixDouble::rotatezrotDotrotDDot(double the, double thedot, double 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; + } + std::shared_ptr FullMatrixDouble::identitysptr(int n) + { + auto mat = std::make_shared(n, n); + mat->identity(); + return mat; + } + std::shared_ptr FullMatrixFullMatrixDouble::identitysptr(int n) + { + auto mat = std::make_shared(n, n); + mat->identity(); + return mat; + } + std::shared_ptr FullMatrixDouble::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; + } + void FullMatrixDouble::zeroSelf() + { + for (int i = 0; i < this->size(); i++) { + this->at(i)->zeroSelf(); } } - 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); + void FullMatrixFullMatrixDouble::zeroSelf() + { + for (int i = 0; i < this->size(); i++) { + this->at(i)->zeroSelf(); } } -} -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++) + void FullMatrixFullColumnDouble::zeroSelf() { - this->at(i1 + ii)->at(j1) = fullCol->at(ii); - } -} -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; -} -template<> -inline double FullMatrix::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; + for (int i = 0; i < this->size(); i++) { + this->at(i)->zeroSelf(); } } - - 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); + void FullMatrixDouble::identity() { + this->zeroSelf(); + for (int i = 0; i < this->size(); i++) { + this->at(i)->at(i) = 1.0; + } } - 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); + void FullMatrixFullMatrixDouble::identity() { + assert(false); +// this->zeroSelf(); +// for (int i = 0; i < this->size(); i++) { +// this->at(i)->at(i) = 1.0; +// } } - 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); + // TODO: should there be a FullMatrixFullColumnDouble version of this? + FColsptr FullMatrixDouble::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; } - 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++) + std::shared_ptr FullMatrixDouble::timesFullMatrix(std::shared_ptr fullMat) { - trace += this->at(i)->at(i); + 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 trace; -} -template -inline double FullMatrix::maxMagnitude() -{ - double max = 0.0; - for (int i = 0; i < this->size(); i++) + std::shared_ptr FullMatrixDouble::timesTransposeFullMatrix(std::shared_ptr fullMat) { - 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; + 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); } - else { - the0x = std::atan2(this->at(2)->at(1), this->at(2)->at(0)); - the1y = M_PI / -2.0; - the2z = 0.0; + return answer; + } +// 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; +// } + 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; + } + std::shared_ptr FullMatrixFullMatrixDouble::times(double a) + { + // TODO: correct action? + assert(false); + return std::make_shared(); + } + std::shared_ptr FullMatrixFullColumnDouble::times(double a) + { + // TODO: correct action? + assert(false); + return std::make_shared(); + } + std::shared_ptr FullMatrixDouble::transposeTimesFullMatrix(std::shared_ptr fullMat) + { + return this->transpose()->timesFullMatrix(fullMat); + } + 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; + } + std::shared_ptr FullMatrixDouble::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; + } + 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; + } + std::shared_ptr FullMatrixDouble::negated() + { + return this->times(-1.0); + } + void FullMatrixDouble::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); + } } } - 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; + void FullMatrixFullColumnDouble::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); + } } - 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++) + void FullMatrixDouble::atiput(int i, FRowsptr fullRow) + { + this->at(i) = fullRow; + } + void FullMatrixDouble::atijput(int i, int j, double value) + { + this->at(i)->atiput(j, value); + } + void FullMatrixDouble::atijputFullColumn(int i1, int j1, FColsptr fullCol) + { + for (int ii = 0; ii < fullCol->size(); ii++) + { + this->at(i1 + ii)->at(j1) = fullCol->at(ii); + } + } + void FullMatrixDouble::atijplusFullRow(int i, int j, FRowsptr fullRow) + { + this->at(i)->atiplusFullRow(j, fullRow); + } + void FullMatrixDouble::atijplusNumber(int i, int j, double value) { auto rowi = this->at(i); - for (int j = 0; j < n; j++) + rowi->at(j) += value; + } + void FullMatrixDouble::atijminusNumber(int i, int j, double value) + { + auto rowi = this->at(i); + rowi->at(j) -= value; + } + 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; + } + double FullMatrixFullMatrixDouble::sumOfSquares() + { + double sum = 0.0; + for (int i = 0; i < this->size(); i++) + { + sum += this->at(i)->sumOfSquares(); + } + return sum; + } + double FullMatrixFullColumnDouble::sumOfSquares() + { + double sum = 0.0; + for (int i = 0; i < this->size(); i++) + { + sum += this->at(i)->sumOfSquares(); + } + return sum; + } + 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; + } + FullMatrixDouble FullMatrixDouble::operator+(const FullMatrixDouble fullMat) + { + int n = (int)this->size(); + FullMatrixDouble answer(n); + for (int i = 0; i < n; i++) { + answer.at(i) = this->at(i)->plusFullRow(fullMat.at(i)); + } + return answer; + } + FColsptr FullMatrixDouble::transposeTimesFullColumn(FColsptr fullCol) + { + auto sptr = std::make_shared(*this); + return fullCol->transpose()->timesFullMatrix(sptr)->transpose(); + } + void FullMatrixDouble::magnifySelf(double 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()) { + std::ostream& FullMatrixDouble::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; + } + std::shared_ptr> FullMatrixDouble::asEulerParameters() + { + //"Given [A], compute Euler parameter." + + auto traceA = this->trace(); + double dum = 0.0; + double 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); + double 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; + } + double FullMatrixDouble::trace() + { + double trace = 0.0; + for (int i = 0; i < this->size(); i++) + { + trace += this->at(i)->at(i); + } + return trace; + } + double FullMatrixDouble::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; + } + double FullMatrixFullMatrixDouble::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; + } + double FullMatrixFullColumnDouble::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; + } + FColsptr FullMatrixDouble::bryantAngles() + { + auto answer = std::make_shared>(3); + auto sthe1y = this->at(0)->at(2); + double 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; + } + bool FullMatrixDouble::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++) + bool FullMatrixDouble::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++) + std::shared_ptr FullMatrixDouble::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; + } + void FullMatrixDouble::conditionSelfWithTol(double tol) + { + for (auto row : *this) { + row->conditionSelfWithTol(tol); + } } - return answer; } diff --git a/OndselSolver/FullMatrix.h b/OndselSolver/FullMatrix.h index 3ce6dca..91684ef 100644 --- a/OndselSolver/FullMatrix.h +++ b/OndselSolver/FullMatrix.h @@ -17,82 +17,133 @@ #include "DiagonalMatrix.ref.h" #include "EulerParameters.ref.h" #include "RowTypeMatrix.h" +#include "FullRow.h" // now that refs are resolved, go do the full systems +//#include "FullColumn.h" +//#include "DiagonalMatrix.h" +//#include "EulerParameters.h" namespace MbD { + // + // FULL MATRIX DOUBLE + // + class FullMatrixDouble : public RowTypeMatrix> { + public: + FullMatrixDouble() = default; + explicit FullMatrixDouble(int m) : RowTypeMatrix>(m) + { + } + FullMatrixDouble(int m, int n) { + for (int i = 0; i < m; i++) { + auto row = std::make_shared>(n); + this->push_back(row); + } + } + FullMatrixDouble(std::initializer_list> listOfRows) { + for (auto& row : listOfRows) + { + this->push_back(row); + } + } + FullMatrixDouble(std::initializer_list> list2D) { + for (auto& rowList : list2D) + { + auto row = std::make_shared>(rowList); + this->push_back(row); + } + } - template - class FullMatrix : public RowTypeMatrix> - { - public: - FullMatrix() {} - FullMatrix(int m) : RowTypeMatrix>(m) + std::shared_ptr times(double a); + std::shared_ptr timesTransposeFullMatrix(std::shared_ptr fullMat); + void identity(); + static std::shared_ptr identitysptr(int n); + double sumOfSquares() override; + 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); + static std::shared_ptr rotatexrotDotrotDDot(double angle, double angleDot, double angleDDot); + static std::shared_ptr rotateyrotDotrotDDot(double angle, double angleDot, double angleDDot); + static std::shared_ptr rotatezrotDotrotDDot(double angle, double angleDot, double angleDDot); + static std::shared_ptr tildeMatrix(FColDsptr col); + void zeroSelf() override; + FColsptr column(int j); + + void atiput(int i, FRowsptr fullRow) override; + void atijput(int i, int j, double value); + std::shared_ptr copy(); + double maxMagnitude() override; + FullMatrixDouble operator+(const FullMatrixDouble fullMat); + std::shared_ptr minusFullMatrix(std::shared_ptr fullMat); + FColsptr transposeTimesFullColumn(FColsptr fullCol); + void symLowerWithUpper(); + void atijputFullColumn(int i1, int j1, FColsptr fullCol); + void atijplusFullRow(int i, int j, FRowsptr fullRow); + void atijplusNumber(int i, int j, double value); + void atijminusNumber(int i, int j, double value); + void magnifySelf(double factor); + std::shared_ptr> asEulerParameters(); + FColsptr bryantAngles(); + double trace(); + bool isDiagonal(); + bool isDiagonalToWithin(double ratio); + std::shared_ptr asDiagonalMatrix(); + void conditionSelfWithTol(double tol); + std::ostream& printOn(std::ostream& s) const override; + FColsptr timesFullColumn(FColsptr fullCol); + // FColsptr timesFullColumn(FullColumn* fullCol); + }; + + // + // FULL MATRIX FULL MATRIX DOUBLE + // + class FullMatrixFullMatrixDouble : public RowTypeMatrix> { + public: + FullMatrixFullMatrixDouble() = default; + explicit FullMatrixFullMatrixDouble(int m) : RowTypeMatrix>(m) { } - FullMatrix(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) { - for (auto& row : listOfRows) - { - this->push_back(row); - } - } - FullMatrix(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); - void identity(); - FColsptr column(int j); - FColsptr timesFullColumn(FColsptr fullCol); - FColsptr timesFullColumn(FullColumn* fullCol); - FMatsptr timesFullMatrix(FMatsptr fullMat); - FMatsptr timesTransposeFullMatrix(FMatsptr fullMat); - FMatsptr times(T a); - FMatsptr transposeTimesFullMatrix(FMatsptr fullMat); - FMatsptr plusFullMatrix(FMatsptr fullMat); - FMatsptr minusFullMatrix(FMatsptr fullMat); - FMatsptr transpose(); - FMatsptr negated(); - void symLowerWithUpper(); - void atiput(int i, FRowsptr fullRow) override; - void atijput(int i, int j, T value); - void atijputFullColumn(int i, int j, FColsptr fullCol); - void atijplusFullRow(int i, int j, FRowsptr fullRow); - void atijplusNumber(int i, int j, T value); - void atijminusNumber(int i, int j, T value); - double sumOfSquares() override; - void zeroSelf() override; - FMatsptr copy(); - FullMatrix operator+(const FullMatrix fullMat); - FColsptr transposeTimesFullColumn(const FColsptr fullCol); - void magnifySelf(T factor); - std::shared_ptr> asEulerParameters(); - T trace(); - double maxMagnitude() override; - FColsptr bryantAngles(); - bool isDiagonal(); - bool isDiagonalToWithin(double ratio); - std::shared_ptr> asDiagonalMatrix(); - void conditionSelfWithTol(double tol); + FullMatrixFullMatrixDouble(int m, int n) { + for (int i = 0; i < m; i++) { + auto row = std::make_shared>(n); + this->push_back(row); + } + } - std::ostream& printOn(std::ostream& s) const override; - }; + double maxMagnitude() override; + void zeroSelf() override; + std::shared_ptr times(double a); + // std::shared_ptr timesTransposeFullMatrix(std::shared_ptr fullMat); + double sumOfSquares() override; + void identity(); + static std::shared_ptr identitysptr(int n); + }; + // + // FULL MATRIX FULL COLUMN DOUBLE + // + class FullMatrixFullColumnDouble : public RowTypeMatrix> { + public: + FullMatrixFullColumnDouble() = default; + explicit FullMatrixFullColumnDouble(int m) : RowTypeMatrix>(m) + { + } + FullMatrixFullColumnDouble(int m, int n) { + for (int i = 0; i < m; i++) { + auto row = std::make_shared>(n); + this->push_back(row); + } + } + + double maxMagnitude() override; + void zeroSelf() override; + double sumOfSquares() override; + void symLowerWithUpper(); + std::shared_ptr times(double a); + }; } - diff --git a/OndselSolver/FullMatrix.ref.h b/OndselSolver/FullMatrix.ref.h index be8b046..a5c0f9e 100644 --- a/OndselSolver/FullMatrix.ref.h +++ b/OndselSolver/FullMatrix.ref.h @@ -1,20 +1,19 @@ #pragma once #include "FullColumn.ref.h" +#include namespace MbD { - template - class FullMatrix; + class FullMatrixDouble; + class FullMatrixFullMatrixDouble; + class FullMatrixFullColumnDouble; - using FMatDsptr = std::shared_ptr>; + using FMatDsptr = std::shared_ptr; - template - using FMatsptr = std::shared_ptr>; - - using FMatFColDsptr = std::shared_ptr>; - using FMatFMatDsptr = std::shared_ptr>; + using FMatFMatDsptr = std::shared_ptr; using FColFMatDsptr = std::shared_ptr>; + using FMatFColDsptr = std::shared_ptr; } diff --git a/OndselSolver/FullRow.cpp b/OndselSolver/FullRow.cpp index af1b4d5..5e49af8 100644 --- a/OndselSolver/FullRow.cpp +++ b/OndselSolver/FullRow.cpp @@ -9,41 +9,55 @@ #include "FullRow.h" #include "FullMatrix.h" -using namespace MbD; - -template -FMatsptr FullRow::transposeTimesFullRow(FRowsptr fullRow) -{ - //"a*b = a(i)b(j)" - auto nrow = (int)this->size(); - auto answer = std::make_shared>(nrow); - for (int i = 0; i < nrow; i++) +namespace MbD { + template<> + std::shared_ptr FullRow::transposeTimesFullRow(FRowsptr fullRow) { - answer->atiput(i, fullRow->times(this->at(i))); + //"a*b = a(i)b(j)" + auto nrow = (int)this->size(); + auto answer = std::make_shared(nrow); + for (int i = 0; i < nrow; i++) + { + answer->atiput(i, fullRow->times(this->at(i))); + } + return answer; } - return answer; -} -template -inline FRowsptr FullRow::timesTransposeFullMatrix(FMatsptr fullMat) -{ - //"a*bT = a(1,j)b(k,j)" - int ncol = fullMat->nrow(); - auto answer = std::make_shared>(ncol); - for (int k = 0; k < ncol; k++) { - answer->at(k) = this->dot(fullMat->at(k)); - } - return answer; -} - -template -inline FRowsptr FullRow::timesFullMatrix(FMatsptr fullMat) -{ - FRowsptr answer = fullMat->at(0)->times(this->at(0)); - for (int j = 1; j < (int) this->size(); j++) + template<> + FRowsptr FullRow::timesTransposeFullMatrix(std::shared_ptr fullMat) { - answer->equalSelfPlusFullRowTimes(fullMat->at(j), this->at(j)); + //"a*bT = a(1,j)b(k,j)" + int ncol = fullMat->nrow(); + auto answer = std::make_shared>(ncol); + for (int k = 0; k < ncol; k++) { + answer->at(k) = this->dot(fullMat->at(k)); + } + return answer; + } + +// TODO: can't get the following to work, but CLion says the routine that calls it in FullMatrixFullMatrixDouble is also +// never called. +// template<> +// FRowsptr> FullRow>::timesTransposeFullMatrixForFMFMDsptr( +// std::shared_ptr fullMat) +// { +// //"a*bT = a(1,j)b(k,j)" +// int ncol = fullMat->nrow(); +// auto answer = std::make_shared>>(ncol); +// for (int k = 0; k < ncol; k++) { +// answer->at(k) = this->dot(fullMat->at(k)); +// } +// return answer; +// } + + template<> + FRowsptr FullRow::timesFullMatrix(std::shared_ptr fullMat) + { + FRowsptr answer = fullMat->at(0)->times(this->at(0)); + for (int j = 1; j < (int) this->size(); j++) + { + answer->equalSelfPlusFullRowTimes(fullMat->at(j), this->at(j)); + } + return answer; } - return answer; - //return FRowsptr(); } diff --git a/OndselSolver/FullRow.h b/OndselSolver/FullRow.h index 540e1d5..7dfe096 100644 --- a/OndselSolver/FullRow.h +++ b/OndselSolver/FullRow.h @@ -30,17 +30,18 @@ namespace MbD { FRowsptr minusFullRow(FRowsptr fullRow); T timesFullColumn(FColsptr fullCol); T timesFullColumn(FullColumn* fullCol); - FRowsptr timesFullMatrix(std::shared_ptr> fullMat); - FRowsptr timesTransposeFullMatrix(std::shared_ptr> fullMat); void equalSelfPlusFullRowTimes(FRowsptr fullRow, double factor); void equalFullRow(FRowsptr fullRow); FColsptr transpose(); FRowsptr copy(); void atiplusFullRow(int j, FRowsptr fullRow); - std::shared_ptr> transposeTimesFullRow(FRowsptr fullRow); std::ostream& printOn(std::ostream& s) const override; - }; + std::shared_ptr transposeTimesFullRow(FRowsptr fullRow); + FRowsptr timesTransposeFullMatrix(std::shared_ptr fullMat); + // FRowsptr> timesTransposeFullMatrixForFMFMDsptr(std::shared_ptr fullMat); + FRowsptr timesFullMatrix(std::shared_ptr fullMat); + }; template<> inline FRowDsptr FullRow::times(double a) @@ -143,6 +144,7 @@ namespace MbD { } s << "}"; return s; - } + }; + } diff --git a/OndselSolver/FullVector.h b/OndselSolver/FullVector.h index 228145b..5a98b61 100644 --- a/OndselSolver/FullVector.h +++ b/OndselSolver/FullVector.h @@ -66,7 +66,7 @@ namespace MbD { inline double FullVector::sumOfSquares() { double sum = 0.0; - for (int i = 0; i < this->size(); i++) + for (int i = 0; i < (int)this->size(); i++) { double element = this->at(i); sum += element * element; @@ -87,7 +87,7 @@ namespace MbD { template<> inline void FullVector::zeroSelf() { - for (int i = 0; i < this->size(); i++) { + for (int i = 0; i < (int)this->size(); i++) { this->at(i) = 0.0; } } @@ -126,7 +126,7 @@ namespace MbD { inline double FullVector::maxMagnitude() { double max = 0.0; - for (int i = 0; i < this->size(); i++) + for (int i = 0; i < (int)this->size(); i++) { double element = this->at(i); if (element < 0.0) element = -element; @@ -168,7 +168,7 @@ namespace MbD { template<> inline void FullVector::conditionSelfWithTol(double tol) { - for (int i = 0; i < this->size(); i++) + for (int i = 0; i < (int)this->size(); i++) { double element = this->at(i); if (element < 0.0) element = -element; @@ -178,15 +178,16 @@ namespace MbD { template inline void FullVector::conditionSelfWithTol(double tol) { - assert(false); - return; + assert(false && tol != tol); // clang++ flips out with warnings if you don't use 'tol' + // but suppressing that warning breaks Visual Studio. + return; // Visual Studio demands the unused return } template inline std::ostream& FullVector::printOn(std::ostream& s) const { s << "FullVec{"; s << this->at(0); - for (int i = 1; i < this->size(); i++) + for (int i = 1; i < (int)this->size(); i++) { s << ", " << this->at(i); } diff --git a/OndselSolver/GearConstraintIqcJc.cpp b/OndselSolver/GearConstraintIqcJc.cpp index cd348ff..af49703 100644 --- a/OndselSolver/GearConstraintIqcJc.cpp +++ b/OndselSolver/GearConstraintIqcJc.cpp @@ -62,7 +62,8 @@ void MbD::GearConstraintIqcJc::calc_ppGpXIpEI() void MbD::GearConstraintIqcJc::calc_ppGpXIpXI() { - ppGpXIpXI = 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/GeneralSpline.cpp b/OndselSolver/GeneralSpline.cpp index 9966cb9..ca3c2a1 100644 --- a/OndselSolver/GeneralSpline.cpp +++ b/OndselSolver/GeneralSpline.cpp @@ -119,7 +119,7 @@ void MbD::GeneralSpline::computeDerivatives() } auto solver = CREATE::With(); auto derivsVector = solver->solvewithsaveOriginal(matrix, bvector, false); - derivs = std::make_shared>(n, p); + derivs = std::make_shared(n, p); auto hmaxpowers = std::make_shared>(p); for (int j = 0; j < p; j++) { diff --git a/OndselSolver/Item.h b/OndselSolver/Item.h index 79a5690..57ea6f4 100644 --- a/OndselSolver/Item.h +++ b/OndselSolver/Item.h @@ -10,8 +10,12 @@ #include #include - +#include "FullColumn.ref.h" +#include "FullRow.ref.h" +#include "DiagonalMatrix.ref.h" +#include "FullMatrix.ref.h" #include "FullColumn.h" +#include "FullRow.h" #include "FullMatrix.h" #include "DiagonalMatrix.h" #include "SparseMatrix.h" diff --git a/OndselSolver/LDUFullMat.cpp b/OndselSolver/LDUFullMat.cpp index 532deeb..010b21e 100644 --- a/OndselSolver/LDUFullMat.cpp +++ b/OndselSolver/LDUFullMat.cpp @@ -116,7 +116,7 @@ FMatDsptr LDUFullMat::inversesaveOriginal(FMatDsptr fullMat, bool saveOriginal) this->decomposesaveOriginal(fullMat, saveOriginal); rightHandSideB = std::make_shared>(m); - auto matrixAinverse = std::make_shared >(m, n); + auto matrixAinverse = std::make_shared (m, n); for (int j = 0; j < n; j++) { rightHandSideB->zeroSelf(); diff --git a/OndselSolver/MBDynBody.cpp b/OndselSolver/MBDynBody.cpp index 6f56cfd..396ba93 100644 --- a/OndselSolver/MBDynBody.cpp +++ b/OndselSolver/MBDynBody.cpp @@ -60,7 +60,7 @@ void MbD::MBDynBody::readInertiaMatrix(std::vector& args) { auto parser = std::make_shared(); parser->variables = mbdynVariables(); - aJmat = std::make_shared>(3, 3); + aJmat = std::make_shared(3, 3); auto& str = args.at(0); if (str.find("diag") != std::string::npos) { args.erase(args.begin()); @@ -89,7 +89,7 @@ void MbD::MBDynBody::createASMT() if (aJmat->isDiagonalToWithin(1.0e-6)) { asmtMassMarker->setMomentOfInertias(aJmat->asDiagonalMatrix()); asmtMassMarker->setPosition3D(rPcmP); - asmtMassMarker->setRotationMatrix(FullMatrix::identitysptr(3)); + asmtMassMarker->setRotationMatrix(FullMatrixDouble::identitysptr(3)); auto asmtPart = asmtAssembly()->partPartialNamed(nodeName); asmtPart->setPrincipalMassMarker(asmtMassMarker); } @@ -98,7 +98,7 @@ void MbD::MBDynBody::createASMT() solver->setm(mass); solver->setJPP(aJmat); solver->setrPoP(rPcmP); - solver->setAPo(FullMatrix::identitysptr(3)); + solver->setAPo(FullMatrixDouble::identitysptr(3)); solver->setrPcmP(rPcmP); solver->calc(); asmtMassMarker->setMomentOfInertias(solver->aJpp); diff --git a/OndselSolver/MBDynItem.cpp b/OndselSolver/MBDynItem.cpp index e87fdf2..aae8444 100644 --- a/OndselSolver/MBDynItem.cpp +++ b/OndselSolver/MBDynItem.cpp @@ -155,7 +155,7 @@ FColDsptr MbD::MBDynItem::readBasicPosition(std::vector& args) FMatDsptr MbD::MBDynItem::readOrientation(std::vector& args) { - auto aAOf = FullMatrix::identitysptr(3); + auto aAOf = FullMatrixDouble::identitysptr(3); if (args.empty()) return aAOf; auto& str = args.at(0); if (str.find("reference") != std::string::npos) { @@ -202,7 +202,7 @@ FMatDsptr MbD::MBDynItem::readBasicOrientation(std::vector& args) } if (str.find("eye") != std::string::npos) { args.erase(args.begin()); - auto aAFf = FullMatrix::identitysptr(3); + auto aAFf = FullMatrixDouble::identitysptr(3); return aAFf; } auto iss = std::istringstream(str); @@ -251,7 +251,7 @@ FMatDsptr MbD::MBDynItem::readBasicOrientation(std::vector& args) else { assert(false); } - auto aAFf = FullMatrix::identitysptr(3); + auto aAFf = FullMatrixDouble::identitysptr(3); aAFf->atijputFullColumn(0, 0, vecX); aAFf->atijputFullColumn(0, 1, vecY); aAFf->atijputFullColumn(0, 2, vecZ); @@ -300,13 +300,13 @@ FMatDsptr MbD::MBDynItem::readBasicOrientation(std::vector& args) else { assert(false); } - auto aAFf = FullMatrix::identitysptr(3); + auto aAFf = FullMatrixDouble::identitysptr(3); aAFf->atijputFullColumn(0, 0, vecX); aAFf->atijputFullColumn(0, 1, vecY); aAFf->atijputFullColumn(0, 2, vecZ); return aAFf; } - auto aAFf = FullMatrix::identitysptr(3); + auto aAFf = FullMatrixDouble::identitysptr(3); for (int i = 0; i < 3; i++) { auto rowi = aAFf->at(i); diff --git a/OndselSolver/MBDynJoint.cpp b/OndselSolver/MBDynJoint.cpp index 997bf7b..02b0a45 100644 --- a/OndselSolver/MBDynJoint.cpp +++ b/OndselSolver/MBDynJoint.cpp @@ -118,7 +118,7 @@ void MbD::MBDynJoint::parseMBDyn(std::string line) mkr1->owner = this; mkr1->nodeStr = "Assembly"; mkr1->rPmP = std::make_shared>(3); - mkr1->aAPm = FullMatrix::identitysptr(3); + mkr1->aAPm = FullMatrixDouble::identitysptr(3); readMarkerJ(arguments); return; } diff --git a/OndselSolver/MBDynMarker.cpp b/OndselSolver/MBDynMarker.cpp index a8ac3a3..37fd175 100644 --- a/OndselSolver/MBDynMarker.cpp +++ b/OndselSolver/MBDynMarker.cpp @@ -11,7 +11,7 @@ using namespace MbD; void MbD::MBDynMarker::parseMBDyn(std::vector& args) { rPmP = std::make_shared>(3); - aAPm = FullMatrix::identitysptr(3); + aAPm = FullMatrixDouble::identitysptr(3); if (args.empty()) return; auto& str = args.at(0); if (str.find("reference") != std::string::npos) { diff --git a/OndselSolver/MBDynStructural.cpp b/OndselSolver/MBDynStructural.cpp index d53f5a2..992b939 100644 --- a/OndselSolver/MBDynStructural.cpp +++ b/OndselSolver/MBDynStructural.cpp @@ -12,7 +12,7 @@ using namespace MbD; MbD::MBDynStructural::MBDynStructural() { rOfO = std::make_shared>(3); - aAOf = FullMatrix::identitysptr(3); + aAOf = FullMatrixDouble::identitysptr(3); vOfO = std::make_shared>(3); omeOfO = std::make_shared>(3); } diff --git a/OndselSolver/MarkerFrame.cpp b/OndselSolver/MarkerFrame.cpp index db8fd07..72075d0 100644 --- a/OndselSolver/MarkerFrame.cpp +++ b/OndselSolver/MarkerFrame.cpp @@ -31,7 +31,7 @@ System* MarkerFrame::root() void MarkerFrame::initialize() { - prOmOpE = std::make_shared>(3, 4); + prOmOpE = std::make_shared(3, 4); pAOmpE = std::make_shared>(4); endFrames = std::make_shared>(); auto endFrm = CREATE::With(); diff --git a/OndselSolver/MarkerFrame.h b/OndselSolver/MarkerFrame.h index 7c67474..ac49416 100644 --- a/OndselSolver/MarkerFrame.h +++ b/OndselSolver/MarkerFrame.h @@ -72,9 +72,9 @@ namespace MbD { PartFrame* partFrame; //Use raw pointer when pointing backwards. FColDsptr rpmp = std::make_shared>(3); - FMatDsptr aApm = FullMatrix::identitysptr(3); + FMatDsptr aApm = FullMatrixDouble::identitysptr(3); FColDsptr rOmO = std::make_shared>(3); - FMatDsptr aAOm = FullMatrix::identitysptr(3); + FMatDsptr aAOm = FullMatrixDouble::identitysptr(3); FMatDsptr prOmOpE; FColFMatDsptr pAOmpE; FMatFColDsptr pprOmOpEpE; diff --git a/OndselSolver/MomentOfInertiaSolver.cpp b/OndselSolver/MomentOfInertiaSolver.cpp index 8fd2cd6..6a6e43e 100644 --- a/OndselSolver/MomentOfInertiaSolver.cpp +++ b/OndselSolver/MomentOfInertiaSolver.cpp @@ -7,7 +7,7 @@ using namespace MbD; void MbD::MomentOfInertiaSolver::example1() { - auto aJpp = std::make_shared>(ListListD{ + auto aJpp = std::make_shared(ListListD{ { 1, 0, 0 }, { 0, 2, 0 }, { 0, 0, 3 } @@ -179,10 +179,10 @@ void MbD::MomentOfInertiaSolver::calcJoo() if (!rPoP) { rPoP = rPcmP; - aAPo = FullMatrix::identitysptr(3); + aAPo = FullMatrixDouble::identitysptr(3); } - auto rocmPtilde = FullMatrix::tildeMatrix(rPcmP->minusFullColumn(rPoP)); - auto rPoPtilde = FullMatrix::tildeMatrix(rPoP); + auto rocmPtilde = FullMatrixDouble::tildeMatrix(rPcmP->minusFullColumn(rPoP)); + auto rPoPtilde = FullMatrixDouble::tildeMatrix(rPoP); auto term1 = aJPP; auto term21 = rPoPtilde->timesFullMatrix(rPoPtilde); auto term22 = rPoPtilde->timesFullMatrix(rocmPtilde); @@ -197,7 +197,7 @@ void MbD::MomentOfInertiaSolver::calcJpp() { //"aJcmP = aJPP + mass*(rPcmPTilde*rPcmPTilde)" - auto rPcmPtilde = FullMatrix::tildeMatrix(rPcmP); + auto rPcmPtilde = FullMatrixDouble::tildeMatrix(rPcmP); aJcmP = aJPP->plusFullMatrix(rPcmPtilde->timesFullMatrix(rPcmPtilde)->times(m)); aJcmP->symLowerWithUpper(); aJcmP->conditionSelfWithTol(aJcmP->maxMagnitude() * 1.0e-6); @@ -217,7 +217,7 @@ void MbD::MomentOfInertiaSolver::calcAPp() auto lam2 = aJpp->at(2); if (lam0 == lam1) { if (lam1 == lam2) { - aAPp = FullMatrix::identitysptr(3); + aAPp = FullMatrixDouble::identitysptr(3); } else { eigenvector1 = eigenvectorFor(lam1); @@ -238,7 +238,7 @@ void MbD::MomentOfInertiaSolver::calcAPp() if (eigenvector1->at(1) < 0.0) eigenvector1->negateSelf(); eigenvector2 = eigenvector0->cross(eigenvector1); } - aAPp = std::make_shared>(3, 3); + aAPp = std::make_shared(3, 3); aAPp->atijputFullColumn(0, 0, eigenvector0); aAPp->atijputFullColumn(0, 1, eigenvector1); aAPp->atijputFullColumn(0, 2, eigenvector2); @@ -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/OrbitAnglezIeqcJec.cpp b/OndselSolver/OrbitAnglezIeqcJec.cpp index b9bb400..e8d5208 100644 --- a/OndselSolver/OrbitAnglezIeqcJec.cpp +++ b/OndselSolver/OrbitAnglezIeqcJec.cpp @@ -138,9 +138,9 @@ void MbD::OrbitAngleZIeqcJec::initialize() OrbitAngleZIecJec::initialize(); pthezpXI = std::make_shared>(3); pthezpEI = std::make_shared>(4); - ppthezpXIpXI = std::make_shared>(3, 3); - ppthezpXIpEI = std::make_shared>(3, 4); - ppthezpEIpEI = std::make_shared>(4, 4); + ppthezpXIpXI = std::make_shared(3, 3); + ppthezpXIpEI = std::make_shared(3, 4); + ppthezpEIpEI = std::make_shared(4, 4); } FMatDsptr MbD::OrbitAngleZIeqcJec::ppvaluepEIpEI() diff --git a/OndselSolver/OrbitAnglezIeqcJeqc.cpp b/OndselSolver/OrbitAnglezIeqcJeqc.cpp index b8e689c..245fba7 100644 --- a/OndselSolver/OrbitAnglezIeqcJeqc.cpp +++ b/OndselSolver/OrbitAnglezIeqcJeqc.cpp @@ -242,13 +242,13 @@ void MbD::OrbitAngleZIeqcJeqc::initialize() OrbitAngleZIeqcJec::initialize(); pthezpXJ = std::make_shared>(3); pthezpEJ = std::make_shared>(4); - ppthezpXIpXJ = std::make_shared>(3, 3); - ppthezpXIpEJ = std::make_shared>(3, 4); - ppthezpEIpXJ = std::make_shared>(4, 3); - ppthezpEIpEJ = std::make_shared>(4, 4); - ppthezpXJpXJ = std::make_shared>(3, 3); - ppthezpXJpEJ = std::make_shared>(3, 4); - ppthezpEJpEJ = std::make_shared>(4, 4); + ppthezpXIpXJ = std::make_shared(3, 3); + ppthezpXIpEJ = std::make_shared(3, 4); + ppthezpEIpXJ = std::make_shared(4, 3); + ppthezpEIpEJ = std::make_shared(4, 4); + ppthezpXJpXJ = std::make_shared(3, 3); + ppthezpXJpEJ = std::make_shared(3, 4); + ppthezpEJpEJ = std::make_shared(4, 4); } FMatDsptr MbD::OrbitAngleZIeqcJeqc::ppvaluepEIpEJ() diff --git a/OndselSolver/Part.cpp b/OndselSolver/Part.cpp index d05ef7a..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; @@ -33,18 +35,18 @@ void Part::initialize() partFrame = CREATE::With(); partFrame->setPart(this); pTpE = std::make_shared>(4); - ppTpEpE = std::make_shared>(4, 4); - ppTpEpEdot = std::make_shared>(4, 4); + ppTpEpE = std::make_shared(4, 4); + ppTpEpEdot = std::make_shared(4, 4); } 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/RackPinConstraintIqcJc.cpp b/OndselSolver/RackPinConstraintIqcJc.cpp index d79a3d8..06bc5f7 100644 --- a/OndselSolver/RackPinConstraintIqcJc.cpp +++ b/OndselSolver/RackPinConstraintIqcJc.cpp @@ -53,12 +53,14 @@ void MbD::RackPinConstraintIqcJc::calc_pGpXI() void MbD::RackPinConstraintIqcJc::calc_ppGpEIpEI() { - ppGpEIpEI = xIeJeIe->ppvaluepEIpEI()->plusFullMatrix(thezIeJe->ppvaluepEIpEI()->times(pitchRadius)); + ppGpEIpEI = xIeJeIe->ppvaluepEIpEI() + ->plusFullMatrix(thezIeJe->ppvaluepEIpEI()->times(pitchRadius)); } void MbD::RackPinConstraintIqcJc::calc_ppGpXIpEI() { - ppGpXIpEI = xIeJeIe->ppvaluepXIpEI()->plusFullMatrix(thezIeJe->ppvaluepXIpEI()->times(pitchRadius)); + ppGpXIpEI = xIeJeIe->ppvaluepXIpEI() + ->plusFullMatrix(thezIeJe->ppvaluepXIpEI()->times(pitchRadius)); } void MbD::RackPinConstraintIqcJc::calcPostDynCorrectorIteration() diff --git a/OndselSolver/RackPinConstraintIqcJqc.cpp b/OndselSolver/RackPinConstraintIqcJqc.cpp index 8c25136..529855c 100644 --- a/OndselSolver/RackPinConstraintIqcJqc.cpp +++ b/OndselSolver/RackPinConstraintIqcJqc.cpp @@ -30,17 +30,20 @@ void MbD::RackPinConstraintIqcJqc::calc_pGpXJ() void MbD::RackPinConstraintIqcJqc::calc_ppGpEIpEJ() { - ppGpEIpEJ = xIeJeIe->ppvaluepEIpEJ()->plusFullMatrix(thezIeJe->ppvaluepEIpEJ()->times(pitchRadius)); + ppGpEIpEJ = xIeJeIe->ppvaluepEIpEJ() + ->plusFullMatrix(thezIeJe->ppvaluepEIpEJ()->times(pitchRadius)); } void MbD::RackPinConstraintIqcJqc::calc_ppGpEIpXJ() { - ppGpEIpXJ = xIeJeIe->ppvaluepEIpXJ()->plusFullMatrix(thezIeJe->ppvaluepEIpXJ()->times(pitchRadius)); + ppGpEIpXJ = xIeJeIe->ppvaluepEIpXJ() + ->plusFullMatrix(thezIeJe->ppvaluepEIpXJ()->times(pitchRadius)); } void MbD::RackPinConstraintIqcJqc::calc_ppGpEJpEJ() { - ppGpEJpEJ = xIeJeIe->ppvaluepEJpEJ()->plusFullMatrix(thezIeJe->ppvaluepEJpEJ()->times(pitchRadius)); + ppGpEJpEJ = xIeJeIe->ppvaluepEJpEJ() + ->plusFullMatrix(thezIeJe->ppvaluepEJpEJ()->times(pitchRadius)); } void MbD::RackPinConstraintIqcJqc::calcPostDynCorrectorIteration() diff --git a/OndselSolver/RedundantConstraint.cpp b/OndselSolver/RedundantConstraint.cpp index dd7c595..d1190d5 100644 --- a/OndselSolver/RedundantConstraint.cpp +++ b/OndselSolver/RedundantConstraint.cpp @@ -105,7 +105,7 @@ void RedundantConstraint::setqsuddotlam(FColDsptr col) void RedundantConstraint::discontinuityAtaddTypeTo(double t, std::shared_ptr> disconTypes) { - //"Reactivate all contraints." + //"Reactivate all constraints." assert(false); //| newSelf | //newSelf : = self constraint. diff --git a/OndselSolver/RowTypeMatrix.cpp b/OndselSolver/RowTypeMatrix.cpp index 33821e2..c607e8e 100644 --- a/OndselSolver/RowTypeMatrix.cpp +++ b/OndselSolver/RowTypeMatrix.cpp @@ -9,13 +9,3 @@ #include "RowTypeMatrix.h" using namespace MbD; - -template -int RowTypeMatrix::nrow() { - return (int) this->size(); -} - -template -int RowTypeMatrix::ncol() { - return this->at(0)->numberOfElements(); -} diff --git a/OndselSolver/RowTypeMatrix.h b/OndselSolver/RowTypeMatrix.h index 2a7fb8c..59298c9 100644 --- a/OndselSolver/RowTypeMatrix.h +++ b/OndselSolver/RowTypeMatrix.h @@ -24,8 +24,12 @@ namespace MbD { virtual void zeroSelf() override = 0; //double maxMagnitude() override; int numberOfElements() override; - int nrow(); - int ncol(); + int nrow() { + return (int) this->size(); + } + int ncol() { + return this->at(0)->numberOfElements(); + } }; template diff --git a/OndselSolver/ScrewConstraintIqcJc.cpp b/OndselSolver/ScrewConstraintIqcJc.cpp index 463954f..7cb3a1d 100644 --- a/OndselSolver/ScrewConstraintIqcJc.cpp +++ b/OndselSolver/ScrewConstraintIqcJc.cpp @@ -55,12 +55,14 @@ void MbD::ScrewConstraintIqcJc::calc_pGpXI() void MbD::ScrewConstraintIqcJc::calc_ppGpEIpEI() { - ppGpEIpEI = zIeJeIe->ppvaluepEIpEI()->times(2.0 * M_PI)->minusFullMatrix(thezIeJe->ppvaluepEIpEI()->times(pitch)); + ppGpEIpEI = zIeJeIe->ppvaluepEIpEI()->times(2.0 * M_PI) + ->minusFullMatrix(thezIeJe->ppvaluepEIpEI()->times(pitch)); } void MbD::ScrewConstraintIqcJc::calc_ppGpXIpEI() { - ppGpXIpEI = zIeJeIe->ppvaluepXIpEI()->times(2.0 * M_PI)->minusFullMatrix(thezIeJe->ppvaluepXIpEI()->times(pitch)); + ppGpXIpEI = zIeJeIe->ppvaluepXIpEI()->times(2.0 * M_PI) + ->minusFullMatrix(thezIeJe->ppvaluepXIpEI()->times(pitch)); } void MbD::ScrewConstraintIqcJc::calcPostDynCorrectorIteration() diff --git a/OndselSolver/ScrewConstraintIqcJqc.cpp b/OndselSolver/ScrewConstraintIqcJqc.cpp index e02d7fe..c56c15d 100644 --- a/OndselSolver/ScrewConstraintIqcJqc.cpp +++ b/OndselSolver/ScrewConstraintIqcJqc.cpp @@ -32,17 +32,20 @@ void MbD::ScrewConstraintIqcJqc::calc_pGpXJ() void MbD::ScrewConstraintIqcJqc::calc_ppGpEIpEJ() { - ppGpEIpEJ = zIeJeIe->ppvaluepEIpEJ()->times(2.0 * M_PI)->minusFullMatrix(thezIeJe->ppvaluepEIpEJ()->times(pitch)); + ppGpEIpEJ = zIeJeIe->ppvaluepEIpEJ()->times(2.0 * M_PI) + ->minusFullMatrix(thezIeJe->ppvaluepEIpEJ()->times(pitch)); } void MbD::ScrewConstraintIqcJqc::calc_ppGpEIpXJ() { - ppGpEIpXJ = zIeJeIe->ppvaluepEIpXJ()->times(2.0 * M_PI)->minusFullMatrix(thezIeJe->ppvaluepEIpXJ()->times(pitch)); + ppGpEIpXJ = zIeJeIe->ppvaluepEIpXJ()->times(2.0 * M_PI) + ->minusFullMatrix(thezIeJe->ppvaluepEIpXJ()->times(pitch)); } void MbD::ScrewConstraintIqcJqc::calc_ppGpEJpEJ() { - ppGpEJpEJ = zIeJeIe->ppvaluepEJpEJ()->times(2.0 * M_PI)->minusFullMatrix(thezIeJe->ppvaluepEJpEJ()->times(pitch)); + ppGpEJpEJ = zIeJeIe->ppvaluepEJpEJ()->times(2.0 * M_PI) + ->minusFullMatrix(thezIeJe->ppvaluepEJpEJ()->times(pitch)); } void MbD::ScrewConstraintIqcJqc::calcPostDynCorrectorIteration() diff --git a/OndselSolver/SparseMatrix.h b/OndselSolver/SparseMatrix.h index 22276c2..f07232f 100644 --- a/OndselSolver/SparseMatrix.h +++ b/OndselSolver/SparseMatrix.h @@ -13,6 +13,7 @@ #include "RowTypeMatrix.h" #include "SparseRow.h" #include "DiagonalMatrix.h" +#include "FullMatrix.h" namespace MbD { template @@ -47,10 +48,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 +118,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 +126,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 +134,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 +142,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++) { diff --git a/OndselSolver/StableBackwardDifference.cpp b/OndselSolver/StableBackwardDifference.cpp index c594e0f..89dc329 100644 --- a/OndselSolver/StableBackwardDifference.cpp +++ b/OndselSolver/StableBackwardDifference.cpp @@ -12,7 +12,7 @@ using namespace MbD; void StableBackwardDifference::formTaylorMatrix() { - //This form is numerically more stable and is prefered over the full Taylor Matrix. + //This form is numerically more stable and is preferred over the full Taylor Matrix. //For method order 3: //| (t1 - t) (t1 - t) ^ 2 / 2! (t1 - t) ^ 3 / 3!| |qd(t) | = | q(t1) - q(t) | //| (t2 - t) (t2 - t) ^ 2 / 2! (t2 - t) ^ 3 / 3!| |qdd(t) | |q(t2) - q(t) | @@ -28,7 +28,7 @@ void StableBackwardDifference::formTaylorMatrix() void StableBackwardDifference::instantiateTaylorMatrix() { if (taylorMatrix == nullptr || (taylorMatrix->nrow() != (order))) { - taylorMatrix = std::make_shared>(order, order); + taylorMatrix = std::make_shared(order, order); } } diff --git a/OndselSolver/TranslationConstraintIqcJc.cpp b/OndselSolver/TranslationConstraintIqcJc.cpp index 264ebce..429655c 100644 --- a/OndselSolver/TranslationConstraintIqcJc.cpp +++ b/OndselSolver/TranslationConstraintIqcJc.cpp @@ -30,8 +30,10 @@ void TranslationConstraintIqcJc::calcPostDynCorrectorIteration() pGpXI = riIeqJeIeq->pvaluepXI(); pGpEI = (riIeqJeIeq->pvaluepEI())->plusFullRow(riIeqJeIeq->pvaluepEK()); ppGpXIpEI = riIeqJeIeq->ppvaluepXIpEK(); - ppGpEIpEI = riIeqJeIeq->ppvaluepEIpEI()->plusFullMatrix(riIeqJeIeq->ppvaluepEIpEK()) - ->plusFullMatrix((riIeqJeIeq->ppvaluepEIpEK()->transpose()->plusFullMatrix(riIeqJeIeq->ppvaluepEKpEK()))); + ppGpEIpEI = riIeqJeIeq->ppvaluepEIpEI() + ->plusFullMatrix(riIeqJeIeq->ppvaluepEIpEK()) + ->plusFullMatrix((riIeqJeIeq->ppvaluepEIpEK()-> + transpose()->plusFullMatrix(riIeqJeIeq->ppvaluepEKpEK()))); } void TranslationConstraintIqcJc::useEquationNumbers() 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); }); diff --git a/OndselSolver/00backhoe.asmt b/testapp/00backhoe.asmt similarity index 100% rename from OndselSolver/00backhoe.asmt rename to testapp/00backhoe.asmt diff --git a/testapp/CMakeLists.txt b/testapp/CMakeLists.txt new file mode 100644 index 0000000..f75fd3d --- /dev/null +++ b/testapp/CMakeLists.txt @@ -0,0 +1,10 @@ +cmake_minimum_required(VERSION 3.16) + +project(OndselSolverLibrary VERSION 1.0.1 DESCRIPTION "Assembly Constraints and Multibody Dynamics code") + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED True) + +add_executable(testapp OndselSolver.cpp) +target_link_libraries(testapp OndselSolver) +target_include_directories(testapp PUBLIC "../OndselSolver") diff --git a/OndselSolver/OndselSolver.cpp b/testapp/OndselSolver.cpp similarity index 59% rename from OndselSolver/OndselSolver.cpp rename to testapp/OndselSolver.cpp index 9b88d4f..0e6e2b3 100644 --- a/OndselSolver/OndselSolver.cpp +++ b/testapp/OndselSolver.cpp @@ -13,33 +13,33 @@ *********************************************************************/ #include -#include "CADSystem.h" -#include "CREATE.h" -#include "GESpMatParPvPrecise.h" -#include "ASMTAssembly.h" -#include "MBDynSystem.h" -#include "MomentOfInertiaSolver.h" +#include "../OndselSolver/CADSystem.h" +#include "../OndselSolver/CREATE.h" +#include "../OndselSolver/GESpMatParPvPrecise.h" +#include "../OndselSolver/ASMTAssembly.h" +#include "../OndselSolver/MBDynSystem.h" +#include "../OndselSolver/MomentOfInertiaSolver.h" using namespace MbD; void runSpMat(); int main() { - ASMTAssembly::readWriteFile("piston.asmt"); - MBDynSystem::runFile("MBDynCase.mbd"); //To be completed - MBDynSystem::runFile("crank_slider.mbd"); //To be completed - //ASMTAssembly::runSinglePendulumSuperSimplified(); //Mass is missing - //ASMTAssembly::runSinglePendulumSuperSimplified2(); //DOF has infinite acceleration due to zero mass and inertias - ASMTAssembly::runSinglePendulumSimplified(); - ASMTAssembly::runSinglePendulum(); - ASMTAssembly::runFile("piston.asmt"); - ASMTAssembly::runFile("00backhoe.asmt"); - //ASMTAssembly::runFile("circular.asmt"); //Needs checking - //ASMTAssembly::runFile("cirpendu.asmt"); //Under constrained. Testing ICKine. - //ASMTAssembly::runFile("engine1.asmt"); //Needs checking - ASMTAssembly::runFile("fourbar.asmt"); - //ASMTAssembly::runFile("fourbot.asmt"); //Very large but works - ASMTAssembly::runFile("wobpump.asmt"); +// // ASMTAssembly::readWriteFile("piston.asmt"); +// MBDynSystem::runFile("MBDynCase.mbd"); //To be completed +// MBDynSystem::runFile("crank_slider.mbd"); //To be completed +// //ASMTAssembly::runSinglePendulumSuperSimplified(); //Mass is missing +// //ASMTAssembly::runSinglePendulumSuperSimplified2(); //DOF has infinite acceleration due to zero mass and inertias +// ASMTAssembly::runSinglePendulumSimplified(); +// ASMTAssembly::runSinglePendulum(); +// ASMTAssembly::runFile("piston.asmt"); +// ASMTAssembly::runFile("00backhoe.asmt"); +// //ASMTAssembly::runFile("circular.asmt"); //Needs checking +// //ASMTAssembly::runFile("cirpendu.asmt"); //Under constrained. Testing ICKine. +// //ASMTAssembly::runFile("engine1.asmt"); //Needs checking +// ASMTAssembly::runFile("fourbar.asmt"); +// //ASMTAssembly::runFile("fourbot.asmt"); //Very large but works +// ASMTAssembly::runFile("wobpump.asmt"); auto cadSystem = std::make_shared(); cadSystem->runOndselSinglePendulum(); diff --git a/OndselSolver/OndselSolver.h b/testapp/OndselSolver.h similarity index 100% rename from OndselSolver/OndselSolver.h rename to testapp/OndselSolver.h diff --git a/OndselSolver/assembly.asmt b/testapp/assembly.asmt similarity index 100% rename from OndselSolver/assembly.asmt rename to testapp/assembly.asmt diff --git a/OndselSolver/circular.asmt b/testapp/circular.asmt similarity index 92% rename from OndselSolver/circular.asmt rename to testapp/circular.asmt index 3442374..55a9c51 100644 --- a/OndselSolver/circular.asmt +++ b/testapp/circular.asmt @@ -1,7 +1,7 @@ freeCAD: 3D CAD with Motion Simulation by askoh.com Assembly Notes - (Text string: 'Copyright (C) 2000-2004, A-S Koh, All Rights Reserved.The Tree View is on the left.The Graphics View is on the right.The Selection View is at the bottom left.The XYZ axes are colored Red Green Blue (RGB). The default world axes are one meter long (about 40 inches).Units are SI units. Angles are in radians unless stated to be in degrees.A rectangular beam is connected to ground with a revolute (pin) joint which allows rotation in the XY plane. A motion is applied to the joint to rotate the beam with constant angular velocity. A similar beam is made to rotate in a plane parallel to the YZ plane. Units are SI units. Angles are in radians.To load the example for a quick look:Click/File/Open/Assembly/ to get a dialog. Enter *.asm for a list of assemblies. Select circular.asm. Click/OK. The projectiles should appear on the screen.Click /Edit/Marker Size/ to set a suitable length for the marker axes.Click ''Wireframe'' and ''Solid'' to switch between wireframe and rendered views.Click ''TiltRotate'' to show all. Drag on the handles to orientate the assembly in 3D. Tilt means tilting away from the line from the eye to the handle.Click ''ZoomIn'' and drag a rectangle over the region of interest to zoom in.Click ''Animation'' to get a dialog. Click Play to animate.Click ''TiltRotate'' to show all or ''ZoomOut'' to see more.You can use any of the View menu items during animation to change you view.Click Stop to stop the animation.To create the assembly from scratch:Click/File/New/Assembly/ to create an empty assembly. A dialog allows you to specify a unique name (Assembly1).To create the first beam:Click on (Assembly1) in the Tree View if it is not already selected.Click/Select/Plane/x=X y=Y/ to select the XY plane of the assembly.Click/Insert/Part/New/ to insert a part on the selected plane. A dialog allows you to specify a unique name (Part1).Click/Insert/Sketch/ to insert a sketch on the new part. A dialog allows you to specify a unique name (Sketch1).Click/Draw/Rectangle/ to select the rectangle tool. In the drawing area, click and drag toward the positive quadrant (increasing x and y) to create a rectangle. Click inside the rectangle if three black handles are not visible, RightClick/Position/Angle/ to get a dialog to specify the rectangle position (0.0d, 0.0d), angle (0.0d) and size (2.0d, 0.2d) precisely.Click/Insert/Feature/Extrusion/ to extrude the drawing perpendicular to the sketch plane. A dialog allows you to specify the z coordinates for extrusion (0.0d, 0.1d).Click ''WireFrame'' to make mass marker visible. Click the mass marker, RightClick/Position/Direction/ to get a dialog to specify its mass and inertias precisely. Click/''Apply Uniform Density''. Enter the density (10.0d).To connect the beam to the assembly with a revolute (pin) joint:To mark the joint attachment point on the assembly:Drag the beam to the upper right to clear up the area near the origin.Select Assembly1 from the Tree View.Click/Select/Plane/x=X y=Y/ to select the XY plane of the assembly.Click/Insert/Marker/. A dialog allows you to specify a unique name (Marker1). A RedGreenBlue (RGB) marker representing the xyz axes appears at the assembly origin.To mark the joint attachment point on the beam:Click on the wide face of the beam to hightlight it.Click/Insert/Marker/. A dialog allows you to specify a unique name (Marker1). A RedGreenBlue (RGB) marker representing the xyz axes appears at the face origin. The marker is referenced to the face which becomes the marker''s guide frame.RightClick/Position/Direction/ over the marker to get a dialog to specify the exact placement of the marker on the part. Enter the coordinates of the marker origin relative to the guide frame. (0.1d, 0.1d, 0.0d). No need to change the other values.To create the joint:Click/Kinematic/Joint/RevoluteZ/ to select the revolute (pin) joint tool. Click on the assembly marker, drag to the part marker, and release the button. A dialog allows you to specify a unique name (Joint1). A rubber band connects the ''i'' and ''j'' markers with an label ''rev'' to denote the revolute joint. A revolute joint constraints the origins of ''i'' and ''j'' to be coincident and the z axes of ''i'' and ''j'' to be colinear. Only one rotational degree of freedom exists between ''i'' and ''j''.To apply a motion to the joint:Click/Kinematic/Motion/RotationZ/ to select the z rotation tool. Click on a revolute joint rubber band. A dialog allows you to specify a unique name (Motion1). A second dialog allows you to specify a function of time describing the angle of the ''j'' x-axis relative to the ''i'' x-axis. Enter 2.0d*pi*time. Click/OK. Another rubber band is superposed on the joint but the new label is ''rmo''. If you want to change the function, click the ''rmo'' label. The superposition of the rubber bands can make it difficult to select the desired item. This problem can be overcomed by selecting the desired item directly from the Tree View.You have just learned the basics of creating markers, connections and prescribed motions.To simulate:Click/Simulation/Transient/ to start a simulation. Click/''current state'' for the desired input state. In the next dialog, set the desired end time (1.0d) and output time step (0.01d) of the simulation. The other default parameters should be acceptable.Click/OK to start the simulation. The simulation progress is shown in the Transcript window and animated in the drawing. You can use any of the View menu items during the simulation. Let the simulation run to completion or Click/Simulation/Stop/ to terminate the simulation anytime.To animate:Click ''Animation'' for a animation control window to play the animation, step frame by frame, jump to the specified current frame. You can also use any of the View menu items during the animation. Click/Stop to stop the animation.To plot marker data:If a suitable marker is not available, insert a marker anywhere on the part. Click on the marker, RightClick/Plot/, a new plot window appears. Click/Plot/Select/ to get a dialog to specify the data for the x and y axes. Multiply select with shift-click or control-click for the y-axis. Click/OK to get the plots. You can export the plot data in tabular form: Click/Plot/Export/ to specify an output filename. The file can be imported into a spreadsheet for further analysis.To plot joint or motion data:Select a joint or motion from the Tree or Graphic Views. RightClick/Plot/ and follow the same procedure for marker data.To add the second beam:Since the second beam is the same as the first, we will reuse.Select \Assembly1\Part1 in the Tree View. Expand the tree as necessary to reach Part1.Click/File/Save As/Part to save the selected part to file. Enter a filename with .prt extension (Part1.prt).Select Assembly1 in the Tree ViewClick/Select/Plane/x=X y=Y/ to select the XY plane of the assembly.Click/Insert/Part/File/ to insert a part from file. A dialog allows you to specify a unique name (Part2). Another dialog allows you to select the file Part1.prt. The second beam is inserted at the assembly origin.To connect the second beam to the assembly with a revolute (pin) joint:To mark the second joint attachment point on the assembly:Select Assembly1 in the Tree ViewClick/Select/Plane/x=X y=Y/ to select the XY plane of the assembly.Click/Insert/Marker/. A dialog allows you to specify a unique name (Marker2). A RedGreenBlue (RGB) marker representing the xyz axes appears at the assembly origin. Drag the handle to move the new marker away from the origin.Click on the marker if it does not have a black handle, RightClick/Position/Direction/ to get a dialog to specify the exact placement of the marker in the assembly. Enter the coordinates of the marker origin relative to the assembly (3.0d, 0.0d, 0.0d). Orient the z-axis to (-1.0d, 0.0d, 0.0d). Orient the x-axis to (0.0d, 0.0d, 1.0d). To mark the joint attachment point on the second beam:The marker on the second beam is ready for attachment. Drag, tilt and rotate the second beam so the the markers for joint attachment are nearby and nearly parallel. This will help the solver find the exact answer.To create the second joint:Click/Kinematic/Joint/RevoluteZ/ to select the revolute (pin) joint tool. Click on \Assembly1\Marker2, drag to the \Assembly1\Part2\Marker1, and release the button. A dialog allows you to specify a unique name (Joint2). A rubber band connects the ''i'' and ''j'' markers with an label ''rev'' to denote the revolute joint.To apply a motion to the second joint:Click/Kinematic/Motion/RotationZ/ to select the z rotation tool. Click on a revolute joint rubber band (Joint2). A dialog allows you to specify a unique name (Motion2). A second dialog allows you to specify a function of time describing the angle of the ''j'' x-axis relative to the ''i'' x-axis. Enter 4.0d*pi*time to make the motion twice as fast as the first one. Click/OK. Another rubber band is superposed on the joint but the new label is ''rmo''. If you want to change the function, click the ''rmo'' label. The superposition of the rubber bands can make it difficult to select the desired item. This problem can be overcomed by selecting the desired item directly from the Tree View.To simulate:Click/Simulation/Transient/ to start a simulation. Click/''current state'' for the desired input state. In the next dialog, set the desired end time (1.0d) and output time step (0.01d) of the simulation. The other default parameters should be acceptable.Click/OK to start the simulation. The simulation progress is shown in the Transcript window and animated in the drawing. You can use any of the View menu items during the simulation. Let the simulation run to completion or Click/Simulation/Stop/ to terminate the simulation anytime.To animate:Click ''Animation'' for a animation control window to play the animation, step frame by frame, jump to the specified current frame. You can also use any of the View menu items during the animation. Click/Stop to stop the animation. Close the window when done.' runs: (Core.RunArray runs: #(654 37 1 25 63 12 3 8 53 18 54 9 7 5 57 10 141 6 70 9 19 10 20 10 18 7 38 4 55 4 25 36 1 24 89 25 70 27 41 22 95 20 93 21 208 26 114 31 144 9 55 30 61 29 31 64 1 51 109 27 41 20 145 47 54 20 217 30 221 20 1 32 47 1 225 118 68 31 1 33 272 8 314 89 2 12 1 27 24 21 183 8 215 22 39 11 6 12 141 4 34 10 25 20 99 16 29 18 125 8 65 18 99 29 58 16 49 23 151 23 121 27 41 23 192 71 1 58 35 27 41 20 261 30 255 54 216 27 1 32 286 38 1 33 331 8 314 12 1 27 24 21 183 8 136 4 75 22 39 11 6 12 179 10 54) values: #(nil #(#bold #large) nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #(#bold #large) nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #underline nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil))) + (Text string: 'Copyright (C) 2000-2004, A-S Koh, All Rights Reserved.The Tree View is on the left.The Graphics View is on the right.The Selection View is at the bottom left.The XYZ axes are colored Red Green Blue (RGB). The default world axes are one meter long (about 40 inches).Units are SI units. Angles are in radians unless stated to be in degrees.A rectangular beam is connected to ground with a revolute (pin) joint which allows rotation in the XY plane. A motion is applied to the joint to rotate the beam with constant angular velocity. A similar beam is made to rotate in a plane parallel to the YZ plane. Units are SI units. Angles are in radians.To load the example for a quick look:Click/File/Open/Assembly/ to get a dialog. Enter *.asm for a list of assemblies. Select circular.asm. Click/OK. The projectiles should appear on the screen.Click /Edit/Marker Size/ to set a suitable length for the marker axes.Click ''Wireframe'' and ''Solid'' to switch between wireframe and rendered views.Click ''TiltRotate'' to show all. Drag on the handles to orientate the assembly in 3D. Tilt means tilting away from the line from the eye to the handle.Click ''ZoomIn'' and drag a rectangle over the region of interest to zoom in.Click ''Animation'' to get a dialog. Click Play to animate.Click ''TiltRotate'' to show all or ''ZoomOut'' to see more.You can use any of the View menu items during animation to change you view.Click Stop to stop the animation.To create the assembly from scratch:Click/File/New/Assembly/ to create an empty assembly. A dialog allows you to specify a unique name (Assembly1).To create the first beam:Click on (Assembly1) in the Tree View if it is not already selected.Click/Select/Plane/x=X y=Y/ to select the XY plane of the assembly.Click/Insert/Part/New/ to insert a part on the selected plane. A dialog allows you to specify a unique name (Part1).Click/Insert/Sketch/ to insert a sketch on the new part. A dialog allows you to specify a unique name (Sketch1).Click/Draw/Rectangle/ to select the rectangle tool. In the drawing area, click and drag toward the positive quadrant (increasing x and y) to create a rectangle. Click inside the rectangle if three black handles are not visible, RightClick/Position/Angle/ to get a dialog to specify the rectangle position (0.0d, 0.0d), angle (0.0d) and size (2.0d, 0.2d) precisely.Click/Insert/Feature/Extrusion/ to extrude the drawing perpendicular to the sketch plane. A dialog allows you to specify the z coordinates for extrusion (0.0d, 0.1d).Click ''WireFrame'' to make mass marker visible. Click the mass marker, RightClick/Position/Direction/ to get a dialog to specify its mass and inertias precisely. Click/''Apply Uniform Density''. Enter the density (10.0d).To connect the beam to the assembly with a revolute (pin) joint:To mark the joint attachment point on the assembly:Drag the beam to the upper right to clear up the area near the origin.Select Assembly1 from the Tree View.Click/Select/Plane/x=X y=Y/ to select the XY plane of the assembly.Click/Insert/Marker/. A dialog allows you to specify a unique name (Marker1). A RedGreenBlue (RGB) marker representing the xyz axes appears at the assembly origin.To mark the joint attachment point on the beam:Click on the wide face of the beam to highlight it.Click/Insert/Marker/. A dialog allows you to specify a unique name (Marker1). A RedGreenBlue (RGB) marker representing the xyz axes appears at the face origin. The marker is referenced to the face which becomes the marker''s guide frame.RightClick/Position/Direction/ over the marker to get a dialog to specify the exact placement of the marker on the part. Enter the coordinates of the marker origin relative to the guide frame. (0.1d, 0.1d, 0.0d). No need to change the other values.To create the joint:Click/Kinematic/Joint/RevoluteZ/ to select the revolute (pin) joint tool. Click on the assembly marker, drag to the part marker, and release the button. A dialog allows you to specify a unique name (Joint1). A rubber band connects the ''i'' and ''j'' markers with an label ''rev'' to denote the revolute joint. A revolute joint constraints the origins of ''i'' and ''j'' to be coincident and the z axes of ''i'' and ''j'' to be colinear. Only one rotational degree of freedom exists between ''i'' and ''j''.To apply a motion to the joint:Click/Kinematic/Motion/RotationZ/ to select the z rotation tool. Click on a revolute joint rubber band. A dialog allows you to specify a unique name (Motion1). A second dialog allows you to specify a function of time describing the angle of the ''j'' x-axis relative to the ''i'' x-axis. Enter 2.0d*pi*time. Click/OK. Another rubber band is superposed on the joint but the new label is ''rmo''. If you want to change the function, click the ''rmo'' label. The superposition of the rubber bands can make it difficult to select the desired item. This problem can be overcomed by selecting the desired item directly from the Tree View.You have just learned the basics of creating markers, connections and prescribed motions.To simulate:Click/Simulation/Transient/ to start a simulation. Click/''current state'' for the desired input state. In the next dialog, set the desired end time (1.0d) and output time step (0.01d) of the simulation. The other default parameters should be acceptable.Click/OK to start the simulation. The simulation progress is shown in the Transcript window and animated in the drawing. You can use any of the View menu items during the simulation. Let the simulation run to completion or Click/Simulation/Stop/ to terminate the simulation anytime.To animate:Click ''Animation'' for a animation control window to play the animation, step frame by frame, jump to the specified current frame. You can also use any of the View menu items during the animation. Click/Stop to stop the animation.To plot marker data:If a suitable marker is not available, insert a marker anywhere on the part. Click on the marker, RightClick/Plot/, a new plot window appears. Click/Plot/Select/ to get a dialog to specify the data for the x and y axes. Multiply select with shift-click or control-click for the y-axis. Click/OK to get the plots. You can export the plot data in tabular form: Click/Plot/Export/ to specify an output filename. The file can be imported into a spreadsheet for further analysis.To plot joint or motion data:Select a joint or motion from the Tree or Graphic Views. RightClick/Plot/ and follow the same procedure for marker data.To add the second beam:Since the second beam is the same as the first, we will reuse.Select \Assembly1\Part1 in the Tree View. Expand the tree as necessary to reach Part1.Click/File/Save As/Part to save the selected part to file. Enter a filename with .prt extension (Part1.prt).Select Assembly1 in the Tree ViewClick/Select/Plane/x=X y=Y/ to select the XY plane of the assembly.Click/Insert/Part/File/ to insert a part from file. A dialog allows you to specify a unique name (Part2). Another dialog allows you to select the file Part1.prt. The second beam is inserted at the assembly origin.To connect the second beam to the assembly with a revolute (pin) joint:To mark the second joint attachment point on the assembly:Select Assembly1 in the Tree ViewClick/Select/Plane/x=X y=Y/ to select the XY plane of the assembly.Click/Insert/Marker/. A dialog allows you to specify a unique name (Marker2). A RedGreenBlue (RGB) marker representing the xyz axes appears at the assembly origin. Drag the handle to move the new marker away from the origin.Click on the marker if it does not have a black handle, RightClick/Position/Direction/ to get a dialog to specify the exact placement of the marker in the assembly. Enter the coordinates of the marker origin relative to the assembly (3.0d, 0.0d, 0.0d). Orient the z-axis to (-1.0d, 0.0d, 0.0d). Orient the x-axis to (0.0d, 0.0d, 1.0d). To mark the joint attachment point on the second beam:The marker on the second beam is ready for attachment. Drag, tilt and rotate the second beam so the the markers for joint attachment are nearby and nearly parallel. This will help the solver find the exact answer.To create the second joint:Click/Kinematic/Joint/RevoluteZ/ to select the revolute (pin) joint tool. Click on \Assembly1\Marker2, drag to the \Assembly1\Part2\Marker1, and release the button. A dialog allows you to specify a unique name (Joint2). A rubber band connects the ''i'' and ''j'' markers with an label ''rev'' to denote the revolute joint.To apply a motion to the second joint:Click/Kinematic/Motion/RotationZ/ to select the z rotation tool. Click on a revolute joint rubber band (Joint2). A dialog allows you to specify a unique name (Motion2). A second dialog allows you to specify a function of time describing the angle of the ''j'' x-axis relative to the ''i'' x-axis. Enter 4.0d*pi*time to make the motion twice as fast as the first one. Click/OK. Another rubber band is superposed on the joint but the new label is ''rmo''. If you want to change the function, click the ''rmo'' label. The superposition of the rubber bands can make it difficult to select the desired item. This problem can be overcomed by selecting the desired item directly from the Tree View.To simulate:Click/Simulation/Transient/ to start a simulation. Click/''current state'' for the desired input state. In the next dialog, set the desired end time (1.0d) and output time step (0.01d) of the simulation. The other default parameters should be acceptable.Click/OK to start the simulation. The simulation progress is shown in the Transcript window and animated in the drawing. You can use any of the View menu items during the simulation. Let the simulation run to completion or Click/Simulation/Stop/ to terminate the simulation anytime.To animate:Click ''Animation'' for a animation control window to play the animation, step frame by frame, jump to the specified current frame. You can also use any of the View menu items during the animation. Click/Stop to stop the animation. Close the window when done.' runs: (Core.RunArray runs: #(654 37 1 25 63 12 3 8 53 18 54 9 7 5 57 10 141 6 70 9 19 10 20 10 18 7 38 4 55 4 25 36 1 24 89 25 70 27 41 22 95 20 93 21 208 26 114 31 144 9 55 30 61 29 31 64 1 51 109 27 41 20 145 47 54 20 217 30 221 20 1 32 47 1 225 118 68 31 1 33 272 8 314 89 2 12 1 27 24 21 183 8 215 22 39 11 6 12 141 4 34 10 25 20 99 16 29 18 125 8 65 18 99 29 58 16 49 23 151 23 121 27 41 23 192 71 1 58 35 27 41 20 261 30 255 54 216 27 1 32 286 38 1 33 331 8 314 12 1 27 24 21 183 8 136 4 75 22 39 11 6 12 179 10 54) values: #(nil #(#bold #large) nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #(#bold #large) nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #underline nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil))) Name Assembly1 Position3D diff --git a/OndselSolver/cirpendu.asmt b/testapp/cirpendu.asmt similarity index 100% rename from OndselSolver/cirpendu.asmt rename to testapp/cirpendu.asmt diff --git a/OndselSolver/engine1.asmt b/testapp/engine1.asmt similarity index 100% rename from OndselSolver/engine1.asmt rename to testapp/engine1.asmt diff --git a/OndselSolver/fourbar.asmt b/testapp/fourbar.asmt similarity index 100% rename from OndselSolver/fourbar.asmt rename to testapp/fourbar.asmt diff --git a/OndselSolver/fourbot.asmt b/testapp/fourbot.asmt similarity index 100% rename from OndselSolver/fourbot.asmt rename to testapp/fourbot.asmt diff --git a/OndselSolver/gyro.asmt b/testapp/gyro.asmt similarity index 96% rename from OndselSolver/gyro.asmt rename to testapp/gyro.asmt index bc41851..92e87a2 100644 --- a/OndselSolver/gyro.asmt +++ b/testapp/gyro.asmt @@ -1,7 +1,7 @@ freeCAD: 3D CAD with Motion Simulation by askoh.com Assembly Notes - (Text string: 'CAD: Copyright (C) 2000-2004, Aik-Siong Koh, All Rights Reserved.A cylinder is connected to ground with a spherical (ball) joint. Its axis passes through the joint. Gravity acts in the negative y direction. While held in the horizontal position, the cylinder is set spinning about its axis at high speed. Then it is released. Gyroscopic and gravitational forces cause the spinning cylinder to precess (rotate about the vertical) and nutate (bop up and down). Units are SI units. Angles are in radians.If the instructions below are too brief, refer to the Notes in projectile.asm and circular.asm.To load the example for a quick look:Click File/Open/Assembly/ to get a dialog. Enter *.asm for a list of assemblies. Select gyro.asm. To create the assembly from scratch:To create a cylindrical rotor:Click File/New/Assembly/ to create an empty assembly. A dialog allows you to specify a unique name (Assembly1).Click Select/Plane/x=X y=Y/ to select the XY plane of the assembly.Click Insert/Part/New/ to insert a part on the selected plane. A dialog allows you to specify a unique name (Part1).Click Insert/Sketch/ to insert a sketch on the new part. A dialog allows you to specify a unique name (Sketch1).Click Draw/Circle/ to select the circle tool. In the drawing area, click and drag to create a circle. RightClick/Position/Angle/ over the circle to get a dialog to specify the circle position (0.0d, 0.0d, 0.0d), angle (0.0d) and size (0.5d, 0.5d) precisely.Click Insert/Feature/Extrusion/ to extrude the drawing perpendicular to the sketch plane. A dialog allows you to specify the z coordinates for extrusion (0.0d, 1.0d).RightClick/Position/Direction/ over the part mass marker to get a dialog to specify its mass and inertias precisely. Click ''Apply Uniform Density''. Enter the density (10.0d).To connect the rotor to the assembly with a spherical joint:To mark the joint attachment point on the assembly:Drag the cylinder to the upper right to clear up the area near the origin.Click Select/Assembly/Assembly1/ to reselect the assembly named Assembly1.Click Select/Plane/x=X y=Y/ to select the XY plane of the assembly.Click Insert/Marker/. A dialog allows you to specify a unique name (Marker1). A RedGreenBlue (RGB) marker representing the xyz axes appears at the assembly origin.To mark the joint attachment point on the rotor:Click on the z face of the cylinder to hightlight it.Click Insert/Marker/. A dialog allows you to specify a unique name (Marker1). A RedGreenBlue (RGB) marker representing the xyz axes appears at the face origin. The marker is referenced to the face which becomes the marker''s guide frame.RightClick/Position/Direction/ to get a dialog to specify the exact placement of the marker on the part. Enter the coordinates of the marker origin relative to the guide frame. (0.0d, 0.0d, -2.0d). No need to change the other values. Tilt the cylinder slightly to get a good view of the marker.To create a spherical joint:The goal now is to connect the part to the assembly with a spherical (pin) joint.Click Kinematic/Joint/Spherical/ to select the spherical joint tool. Click on the assembly marker, drag to the part marker, and release the button. A dialog allows you to specify a unique name (Joint1). A rubber band connects the ''i'' and ''j'' markers with an label ''sph'' to denote the spherical joint.To set initial conditions:RightClick/PositionPart/Direction/ over the marker (Assembly1Part1Marker1) to get a dialog to specify its absolute position and orientation in space precisely. Enter the absolute coordinates of the marker origin in space (0.0d, 0.0d, 0.0d). Enter the marker z-axis direction in absolute coordinates (0.0d, 0.0d, 1.0d). Do not worry about the magnitude as only the direction is important. Enter the direction of a vector in the marker x-z plane in absolute coordinates (1.0d, 0.0d, 0.0d).RightClick/Velocity/ over the marker (Assembly1Part1Marker1) to get a dialog to specify its absolute velocity in space precisely. Enter velocity of marker origin in absolute components (0.0d, 0.0d, 0.0d). Enter the angular velocity of the marker in absolute components (0.0d, 0.0d, 3600.0d).To set gravity:Click Dynamic/Gravity/ for the gravity dialog. Enter (0.0d, -9.81d, 0.0d).To simulate:Click Simulation/Dynamics/ to start a simulation. Click ''current state'' for the desired input state. In the subsequent dialog, set the desired end time (10.0d) and output time step (0.05d) of the simulation. The other default parameters should be acceptable.Click OK to start the simulation. The simulation progress is shown in the Transcript window and animated in the drawing. You can use any of the View menu items during the simulation. Let the simulation run to completion or Click Simulation/Stop/ to terminate the simulation anytime.To animate:Click Simulation/Animation/ for a animation control window to play the animation, step frame by frame, jump to the specified current frame. You can also use any of the View menu items during the animation. Click Stop to stop the animation. Close the window when done.To plot:Click Simulation/Plot/ for a list of items that can have plots. Select a marker (Assembly1Part1MassMarker). A new plot window appears. Click Plot/Select/ to get a dialog to specify the data for the x and y axes. Multiply select with shift-click or control-click for the y-axis. Click OK to get the plots. You can export the plot data in tabular form: Click Plot/Export/ to specify an output filename. The file can be imported into a spreadsheet for further analysis.' runs: (Core.RunArray runs: #(505 63 14 5 12 1 2 37 89 8 4 36 1 30 1013 60 1 51 384 48 588 28 385 26 782 15 77 12 544 11 270 8 468) values: #(nil #underline #(#underline #bold) #underline #(#underline #bold) #underline nil #(#bold #large) nil #bold nil #(#bold #large) nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil))) + (Text string: 'CAD: Copyright (C) 2000-2004, Aik-Siong Koh, All Rights Reserved.A cylinder is connected to ground with a spherical (ball) joint. Its axis passes through the joint. Gravity acts in the negative y direction. While held in the horizontal position, the cylinder is set spinning about its axis at high speed. Then it is released. Gyroscopic and gravitational forces cause the spinning cylinder to precess (rotate about the vertical) and nutate (bop up and down). Units are SI units. Angles are in radians.If the instructions below are too brief, refer to the Notes in projectile.asm and circular.asm.To load the example for a quick look:Click File/Open/Assembly/ to get a dialog. Enter *.asm for a list of assemblies. Select gyro.asm. To create the assembly from scratch:To create a cylindrical rotor:Click File/New/Assembly/ to create an empty assembly. A dialog allows you to specify a unique name (Assembly1).Click Select/Plane/x=X y=Y/ to select the XY plane of the assembly.Click Insert/Part/New/ to insert a part on the selected plane. A dialog allows you to specify a unique name (Part1).Click Insert/Sketch/ to insert a sketch on the new part. A dialog allows you to specify a unique name (Sketch1).Click Draw/Circle/ to select the circle tool. In the drawing area, click and drag to create a circle. RightClick/Position/Angle/ over the circle to get a dialog to specify the circle position (0.0d, 0.0d, 0.0d), angle (0.0d) and size (0.5d, 0.5d) precisely.Click Insert/Feature/Extrusion/ to extrude the drawing perpendicular to the sketch plane. A dialog allows you to specify the z coordinates for extrusion (0.0d, 1.0d).RightClick/Position/Direction/ over the part mass marker to get a dialog to specify its mass and inertias precisely. Click ''Apply Uniform Density''. Enter the density (10.0d).To connect the rotor to the assembly with a spherical joint:To mark the joint attachment point on the assembly:Drag the cylinder to the upper right to clear up the area near the origin.Click Select/Assembly/Assembly1/ to reselect the assembly named Assembly1.Click Select/Plane/x=X y=Y/ to select the XY plane of the assembly.Click Insert/Marker/. A dialog allows you to specify a unique name (Marker1). A RedGreenBlue (RGB) marker representing the xyz axes appears at the assembly origin.To mark the joint attachment point on the rotor:Click on the z face of the cylinder to highlight it.Click Insert/Marker/. A dialog allows you to specify a unique name (Marker1). A RedGreenBlue (RGB) marker representing the xyz axes appears at the face origin. The marker is referenced to the face which becomes the marker''s guide frame.RightClick/Position/Direction/ to get a dialog to specify the exact placement of the marker on the part. Enter the coordinates of the marker origin relative to the guide frame. (0.0d, 0.0d, -2.0d). No need to change the other values. Tilt the cylinder slightly to get a good view of the marker.To create a spherical joint:The goal now is to connect the part to the assembly with a spherical (pin) joint.Click Kinematic/Joint/Spherical/ to select the spherical joint tool. Click on the assembly marker, drag to the part marker, and release the button. A dialog allows you to specify a unique name (Joint1). A rubber band connects the ''i'' and ''j'' markers with an label ''sph'' to denote the spherical joint.To set initial conditions:RightClick/PositionPart/Direction/ over the marker (Assembly1Part1Marker1) to get a dialog to specify its absolute position and orientation in space precisely. Enter the absolute coordinates of the marker origin in space (0.0d, 0.0d, 0.0d). Enter the marker z-axis direction in absolute coordinates (0.0d, 0.0d, 1.0d). Do not worry about the magnitude as only the direction is important. Enter the direction of a vector in the marker x-z plane in absolute coordinates (1.0d, 0.0d, 0.0d).RightClick/Velocity/ over the marker (Assembly1Part1Marker1) to get a dialog to specify its absolute velocity in space precisely. Enter velocity of marker origin in absolute components (0.0d, 0.0d, 0.0d). Enter the angular velocity of the marker in absolute components (0.0d, 0.0d, 3600.0d).To set gravity:Click Dynamic/Gravity/ for the gravity dialog. Enter (0.0d, -9.81d, 0.0d).To simulate:Click Simulation/Dynamics/ to start a simulation. Click ''current state'' for the desired input state. In the subsequent dialog, set the desired end time (10.0d) and output time step (0.05d) of the simulation. The other default parameters should be acceptable.Click OK to start the simulation. The simulation progress is shown in the Transcript window and animated in the drawing. You can use any of the View menu items during the simulation. Let the simulation run to completion or Click Simulation/Stop/ to terminate the simulation anytime.To animate:Click Simulation/Animation/ for a animation control window to play the animation, step frame by frame, jump to the specified current frame. You can also use any of the View menu items during the animation. Click Stop to stop the animation. Close the window when done.To plot:Click Simulation/Plot/ for a list of items that can have plots. Select a marker (Assembly1Part1MassMarker). A new plot window appears. Click Plot/Select/ to get a dialog to specify the data for the x and y axes. Multiply select with shift-click or control-click for the y-axis. Click OK to get the plots. You can export the plot data in tabular form: Click Plot/Export/ to specify an output filename. The file can be imported into a spreadsheet for further analysis.' runs: (Core.RunArray runs: #(505 63 14 5 12 1 2 37 89 8 4 36 1 30 1013 60 1 51 384 48 588 28 385 26 782 15 77 12 544 11 270 8 468) values: #(nil #underline #(#underline #bold) #underline #(#underline #bold) #underline nil #(#bold #large) nil #bold nil #(#bold #large) nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil #bold nil))) Name Gyro Position3D diff --git a/OndselSolver/piston.asmt b/testapp/piston.asmt similarity index 100% rename from OndselSolver/piston.asmt rename to testapp/piston.asmt diff --git a/OndselSolver/robot.asmt b/testapp/robot.asmt similarity index 100% rename from OndselSolver/robot.asmt rename to testapp/robot.asmt diff --git a/OndselSolver/wobpump.asmt b/testapp/wobpump.asmt similarity index 100% rename from OndselSolver/wobpump.asmt rename to testapp/wobpump.asmt