From c30ee64b898ddadc035c0450e00a315cc5ee7faf Mon Sep 17 00:00:00 2001 From: Aik-Siong Koh Date: Sat, 24 Jun 2023 23:08:29 -0600 Subject: [PATCH] runPosIC, VelIC, AccIC numerically correct --- MbDCode/AbsConstraint.cpp | 19 ++ MbDCode/AbsConstraint.h | 4 +- MbDCode/AccICKineNewtonRaphson.cpp | 16 + MbDCode/AccICKineNewtonRaphson.h | 16 + MbDCode/AccICNewtonRaphson.cpp | 14 + MbDCode/AccICNewtonRaphson.h | 16 + MbDCode/AccKineNewtonRaphson.cpp | 16 + MbDCode/AccKineNewtonRaphson.h | 16 + MbDCode/AccNewtonRaphson.cpp | 112 +++++++ MbDCode/AccNewtonRaphson.h | 24 ++ MbDCode/Array.h | 5 + MbDCode/AtPointConstraintIJ.cpp | 6 + MbDCode/AtPointConstraintIJ.h | 4 +- MbDCode/AtPointConstraintIqcJc.cpp | 20 ++ MbDCode/AtPointConstraintIqcJc.h | 2 + MbDCode/AtPointConstraintIqcJqc.cpp | 22 ++ MbDCode/AtPointConstraintIqcJqc.h | 2 + MbDCode/AtPointConstraintIqctJqc.cpp | 22 ++ MbDCode/AtPointConstraintIqctJqc.h | 5 + MbDCode/BasicIntegrator.h | 2 +- MbDCode/CREATE.h | 8 +- MbDCode/CartesianFrame.h | 2 +- MbDCode/Constraint.cpp | 22 ++ MbDCode/Constraint.h | 6 +- MbDCode/ConstraintIJ.h | 2 +- MbDCode/DiagonalMatrix.h | 26 ++ MbDCode/DifferenceOperator.h | 2 +- MbDCode/DirectionCosineConstraintIJ.cpp | 6 + MbDCode/DirectionCosineConstraintIJ.h | 3 +- MbDCode/DirectionCosineConstraintIqcJc.cpp | 16 + MbDCode/DirectionCosineConstraintIqcJc.h | 2 + MbDCode/DirectionCosineConstraintIqcJqc.cpp | 21 ++ MbDCode/DirectionCosineConstraintIqcJqc.h | 2 + MbDCode/DirectionCosineConstraintIqctJqc.cpp | 26 ++ MbDCode/DirectionCosineConstraintIqctJqc.h | 3 + MbDCode/DirectionCosineIeqcJec.h | 2 +- MbDCode/DirectionCosineIeqcJeqc.h | 2 +- MbDCode/DirectionCosineIeqctJeqc.cpp | 19 ++ MbDCode/DirectionCosineIeqctJeqc.h | 3 +- MbDCode/DispCompIeqcJeqcKeqct.cpp | 52 ++++ MbDCode/DispCompIeqcJeqcKeqct.h | 7 + MbDCode/DispCompIeqctJeqcKeqct.cpp | 18 +- MbDCode/DispCompIeqctJeqcKeqct.h | 1 + MbDCode/DispCompIeqctJeqcO.cpp | 7 + MbDCode/DispCompIeqctJeqcO.h | 1 + MbDCode/EndFramec.h | 2 +- MbDCode/EndFrameqc.cpp | 20 ++ MbDCode/EndFrameqc.h | 8 +- MbDCode/EndFrameqct.cpp | 126 +++++++- MbDCode/EndFrameqct.h | 11 +- MbDCode/EulerAngleszxz.h | 2 +- MbDCode/EulerAngleszxzDDot.h | 70 +++++ MbDCode/EulerAngleszxzDot.h | 3 +- MbDCode/EulerArray.h | 3 +- MbDCode/EulerConstraint.cpp | 18 ++ MbDCode/EulerConstraint.h | 4 +- MbDCode/EulerParameters.h | 79 ++++- MbDCode/EulerParametersDDot.cpp | 1 + MbDCode/EulerParametersDDot.h | 21 ++ MbDCode/EulerParametersDot.h | 131 +++++++- MbDCode/FullColumn.h | 16 + MbDCode/FullMatrix.h | 56 ++-- MbDCode/FullVector.h | 16 + MbDCode/GEFullMatFullPv.cpp | 2 +- MbDCode/GEFullMatParPv.cpp | 2 +- MbDCode/GESpMatFullPv.cpp | 6 +- MbDCode/GESpMatFullPvPosIC.cpp | 2 +- MbDCode/GESpMatParPv.cpp | 2 +- MbDCode/GESpMatParPvMarko.cpp | 29 +- MbDCode/GESpMatParPvMarkoFast.cpp | 8 +- MbDCode/GESpMatParPvPrecise.cpp | 6 +- MbDCode/Item.cpp | 33 +- MbDCode/Item.h | 12 +- MbDCode/Joint.cpp | 35 +++ MbDCode/Joint.h | 9 +- MbDCode/LDUFullMatParPv.cpp | 2 +- MbDCode/LDUSpMatParPvMarko.cpp | 4 +- MbDCode/LDUSpMatParPvPrecise.cpp | 4 +- MbDCode/MarkerFrame.cpp | 42 +++ MbDCode/MarkerFrame.h | 12 +- MbDCode/MatrixSolver.cpp | 12 +- MbDCode/MatrixSolver.h | 2 + MbDCode/MbDCode.cpp | 309 +++++++++++++++++-- MbDCode/MbDCode.vcxproj | 10 + MbDCode/MbDCode.vcxproj.filters | 30 ++ MbDCode/NewtonRaphson.h | 4 +- MbDCode/Part.cpp | 201 +++++++++++- MbDCode/Part.h | 33 +- MbDCode/PartFrame.cpp | 88 +++++- MbDCode/PartFrame.h | 14 +- MbDCode/PosNewtonRaphson.h | 2 +- MbDCode/PrescribedMotion.h | 2 +- MbDCode/RedundantConstraint.cpp | 16 + MbDCode/RedundantConstraint.h | 4 + MbDCode/SingularMatrixError.h | 1 - MbDCode/SparseMatrix.h | 36 +++ MbDCode/SparseRow.h | 20 ++ MbDCode/SparseVector.h | 6 + MbDCode/System.cpp | 2 +- MbDCode/SystemSolver.cpp | 5 +- MbDCode/TranslationConstraintIJ.cpp | 6 + MbDCode/TranslationConstraintIJ.h | 3 +- MbDCode/TranslationConstraintIqcJc.cpp | 22 ++ MbDCode/TranslationConstraintIqcJc.h | 2 + MbDCode/TranslationConstraintIqcJqc.cpp | 29 +- MbDCode/TranslationConstraintIqcJqc.h | 2 + MbDCode/TranslationConstraintIqctJqc.cpp | 33 ++ MbDCode/TranslationConstraintIqctJqc.h | 3 + MbDCode/Variable.h | 2 +- MbDCode/VelSolver.cpp | 2 + 110 files changed, 2171 insertions(+), 129 deletions(-) create mode 100644 MbDCode/AccICKineNewtonRaphson.cpp create mode 100644 MbDCode/AccICKineNewtonRaphson.h create mode 100644 MbDCode/AccICNewtonRaphson.cpp create mode 100644 MbDCode/AccICNewtonRaphson.h create mode 100644 MbDCode/AccKineNewtonRaphson.cpp create mode 100644 MbDCode/AccKineNewtonRaphson.h create mode 100644 MbDCode/AccNewtonRaphson.cpp create mode 100644 MbDCode/AccNewtonRaphson.h create mode 100644 MbDCode/EulerParametersDDot.cpp create mode 100644 MbDCode/EulerParametersDDot.h diff --git a/MbDCode/AbsConstraint.cpp b/MbDCode/AbsConstraint.cpp index 3c945a5..b8ba416 100644 --- a/MbDCode/AbsConstraint.cpp +++ b/MbDCode/AbsConstraint.cpp @@ -48,3 +48,22 @@ void MbD::AbsConstraint::fillPosKineJacob(SpMatDsptr mat) { mat->atijplusNumber(iG, iqXminusOnePlusAxis, 1.0); } + +void MbD::AbsConstraint::fillVelICJacob(SpMatDsptr mat) +{ + this->fillPosICJacob(mat); +} + +void MbD::AbsConstraint::fillAccICIterError(FColDsptr col) +{ + col->atiplusNumber(iqXminusOnePlusAxis, lam); + auto partFrame = static_cast(owner); + double sum; + if (axis < 3) { + sum = partFrame->qXddot->at(axis); + } + else { + sum = partFrame->qEddot->at(axis - 3); + } + col->atiplusNumber(iG, sum); +} diff --git a/MbDCode/AbsConstraint.h b/MbDCode/AbsConstraint.h index 66af74d..3e1e544 100644 --- a/MbDCode/AbsConstraint.h +++ b/MbDCode/AbsConstraint.h @@ -8,12 +8,14 @@ namespace MbD { //AbsConstraint(); //AbsConstraint(const char* str); AbsConstraint(int axis); - void initialize(); + void initialize() override; void calcPostDynCorrectorIteration() override; void useEquationNumbers() override; void fillPosICJacob(SpMatDsptr mat) override; void fillPosICError(FColDsptr col) override; void fillPosKineJacob(SpMatDsptr mat) override; + void fillVelICJacob(SpMatDsptr mat) override; + void fillAccICIterError(FColDsptr col) override; int axis = -1; int iqXminusOnePlusAxis = -1; diff --git a/MbDCode/AccICKineNewtonRaphson.cpp b/MbDCode/AccICKineNewtonRaphson.cpp new file mode 100644 index 0000000..12c4c71 --- /dev/null +++ b/MbDCode/AccICKineNewtonRaphson.cpp @@ -0,0 +1,16 @@ +#include "AccICKineNewtonRaphson.h" +#include "SystemSolver.h" + +void MbD::AccICKineNewtonRaphson::initializeGlobally() +{ + AccNewtonRaphson::initializeGlobally(); + iterMax = system->iterMaxAccKine; + dxTol = system->errorTolAccKine; +} + +void MbD::AccICKineNewtonRaphson::preRun() +{ + std::string str("MbD: Solving for quasi kinematic acceleration."); + system->logString(str); + AccNewtonRaphson::preRun(); +} diff --git a/MbDCode/AccICKineNewtonRaphson.h b/MbDCode/AccICKineNewtonRaphson.h new file mode 100644 index 0000000..d615c6f --- /dev/null +++ b/MbDCode/AccICKineNewtonRaphson.h @@ -0,0 +1,16 @@ +#pragma once + +#include "AccNewtonRaphson.h" + +namespace MbD { + class AccICKineNewtonRaphson : public AccNewtonRaphson + { + // + public: + void initializeGlobally() override; + void preRun() override; + + + }; +} + diff --git a/MbDCode/AccICNewtonRaphson.cpp b/MbDCode/AccICNewtonRaphson.cpp new file mode 100644 index 0000000..39fe3e1 --- /dev/null +++ b/MbDCode/AccICNewtonRaphson.cpp @@ -0,0 +1,14 @@ +#include "AccICNewtonRaphson.h" +#include "SystemSolver.h" + +bool MbD::AccICNewtonRaphson::isConverged() +{ + return this->isConvergedToNumericalLimit(); +} + +void MbD::AccICNewtonRaphson::preRun() +{ + std::string str("MbD: Solving for quasi kinematic acceleration."); + system->logString(str); + AccNewtonRaphson::preRun(); +} diff --git a/MbDCode/AccICNewtonRaphson.h b/MbDCode/AccICNewtonRaphson.h new file mode 100644 index 0000000..f71ec79 --- /dev/null +++ b/MbDCode/AccICNewtonRaphson.h @@ -0,0 +1,16 @@ +#pragma once + +#include "AccNewtonRaphson.h" + +namespace MbD { + class AccICNewtonRaphson : public AccNewtonRaphson + { + // + public: + bool isConverged() override; + void preRun() override; + + + }; +} + diff --git a/MbDCode/AccKineNewtonRaphson.cpp b/MbDCode/AccKineNewtonRaphson.cpp new file mode 100644 index 0000000..4b0d4f6 --- /dev/null +++ b/MbDCode/AccKineNewtonRaphson.cpp @@ -0,0 +1,16 @@ +#include "AccKineNewtonRaphson.h" +#include "SystemSolver.h" + +void MbD::AccKineNewtonRaphson::initializeGlobally() +{ + AccNewtonRaphson::initializeGlobally(); + iterMax = system->iterMaxAccKine; + dxTol = system->errorTolAccKine; +} + +void MbD::AccKineNewtonRaphson::preRun() +{ + std::string str("MbD: Solving for kinematic acceleration."); + system->logString(str); + AccNewtonRaphson::preRun(); +} diff --git a/MbDCode/AccKineNewtonRaphson.h b/MbDCode/AccKineNewtonRaphson.h new file mode 100644 index 0000000..46bb080 --- /dev/null +++ b/MbDCode/AccKineNewtonRaphson.h @@ -0,0 +1,16 @@ +#pragma once + +#include "AccNewtonRaphson.h" + +namespace MbD { + class AccKineNewtonRaphson : public AccNewtonRaphson + { + // + public: + void initializeGlobally() override; + void preRun() override; + + + }; +} + diff --git a/MbDCode/AccNewtonRaphson.cpp b/MbDCode/AccNewtonRaphson.cpp new file mode 100644 index 0000000..57f270d --- /dev/null +++ b/MbDCode/AccNewtonRaphson.cpp @@ -0,0 +1,112 @@ +#include "AccNewtonRaphson.h" +#include "SystemSolver.h" +#include "Part.h" +#include "Constraint.h" +#include "SimulationStoppingError.h" +#include + +void MbD::AccNewtonRaphson::askSystemToUpdate() +{ + system->partsJointsMotionsForcesTorquesDo([&](std::shared_ptr item) { item->postAccICIteration(); }); +} + +void MbD::AccNewtonRaphson::assignEquationNumbers() +{ + auto parts = system->parts(); + //auto contactEndFrames = system->contactEndFrames(); + //auto uHolders = system->uHolders(); + auto constraints = system->allConstraints(); + int eqnNo = 0; + for (auto& part : *parts) { + part->iqX(eqnNo); + eqnNo = eqnNo + 3; + part->iqE(eqnNo); + eqnNo = eqnNo + 4; + } + //for (auto& endFrm : *contactEndFrames) { + // endFrm->is(eqnNo); + // eqnNo = eqnNo + endFrm->sSize(); + //} + //for (auto& uHolder : *uHolders) { + // uHolder->iu(eqnNo); + // eqnNo += 1; + //} + auto nEqns = eqnNo; //C++ uses index 0. + for (auto& con : *constraints) { + con->iG = eqnNo; + eqnNo += 1; + } + auto lastEqnNo = eqnNo - 1; + nEqns = eqnNo; //C++ uses index 0. + n = nEqns; +} + +void MbD::AccNewtonRaphson::fillPyPx() +{ + pypx->zeroSelf(); + system->partsJointsMotionsForcesTorquesDo([&](std::shared_ptr item) { + item->fillAccICIterJacob(pypx); + }); +} + +void MbD::AccNewtonRaphson::fillY() +{ + y->zeroSelf(); + system->partsJointsMotionsForcesTorquesDo([&](std::shared_ptr item) { + item->fillAccICIterError(y); + //std::cout << item->getName() << *y << std::endl; + }); + //std::cout << *y << std::endl; +} + +void MbD::AccNewtonRaphson::incrementIterNo() +{ + if (iterNo >= iterMax) + { + std::stringstream ss; + ss << "MbD: No convergence after " << iterNo << " iterations."; + auto str = ss.str(); + system->logString(str); + ss.str(""); + ss << "A force function of joint reactions can cause this problem"; + str = ss.str(); + system->logString(str); + ss.str(""); + ss << "if the function returns large values."; + str = ss.str(); + system->logString(str); + + throw SimulationStoppingError(""); + } + + iterNo++; +} + +void MbD::AccNewtonRaphson::initializeGlobally() +{ + SystemNewtonRaphson::initializeGlobally(); + system->partsJointsMotionsDo([&](std::shared_ptr item) { item->fillqsuddotlam(x); }); +} + +void MbD::AccNewtonRaphson::logSingularMatrixMessage() +{ + std::string str = "MbD: Some parts with zero masses or moment of inertias have infinite accelerations."; + system->logString(str); + str = "Add masses and inertias to or properly constrain those parts."; + system->logString(str); +} + +void MbD::AccNewtonRaphson::passRootToSystem() +{ + system->partsJointsMotionsDo([&](std::shared_ptr item) { item->setqsuddotlam(x); }); +} + +void MbD::AccNewtonRaphson::postRun() +{ + system->partsJointsMotionsForcesTorquesDo([&](std::shared_ptr item) { item->postAccIC(); }); +} + +void MbD::AccNewtonRaphson::preRun() +{ + system->partsJointsMotionsForcesTorquesDo([&](std::shared_ptr item) { item->preAccIC(); }); +} diff --git a/MbDCode/AccNewtonRaphson.h b/MbDCode/AccNewtonRaphson.h new file mode 100644 index 0000000..8c5b067 --- /dev/null +++ b/MbDCode/AccNewtonRaphson.h @@ -0,0 +1,24 @@ +#pragma once + +#include "SystemNewtonRaphson.h" + +namespace MbD { + class AccNewtonRaphson : public SystemNewtonRaphson + { + // + public: + void askSystemToUpdate() override; + void assignEquationNumbers() override; + void fillPyPx() override; + void fillY() override; + void incrementIterNo() override; + void initializeGlobally() override; + void logSingularMatrixMessage(); + void passRootToSystem(); + void postRun() override; + void preRun() override; + + + }; +} + diff --git a/MbDCode/Array.h b/MbDCode/Array.h index c1d7209..b6ec565 100644 --- a/MbDCode/Array.h +++ b/MbDCode/Array.h @@ -17,6 +17,7 @@ namespace MbD { Array(int count, const T& value) : std::vector(count, value) {} Array(std::vector::iterator begin, std::vector::iterator end) : std::vector(begin, end) {} Array(std::initializer_list list) : std::vector{ list } {} + virtual void initialize(); void copyFrom(std::shared_ptr> x); virtual void zeroSelf(); virtual double sumOfSquares() = 0; @@ -29,6 +30,10 @@ namespace MbD { }; template + inline void Array::initialize() + { + } + template inline void Array::copyFrom(std::shared_ptr> x) { for (int i = 0; i < x->size(); i++) { diff --git a/MbDCode/AtPointConstraintIJ.cpp b/MbDCode/AtPointConstraintIJ.cpp index 3242601..2f68031 100644 --- a/MbDCode/AtPointConstraintIJ.cpp +++ b/MbDCode/AtPointConstraintIJ.cpp @@ -63,3 +63,9 @@ void MbD::AtPointConstraintIJ::preVelIC() riIeJeO->preVelIC(); Item::preVelIC(); } + +void MbD::AtPointConstraintIJ::preAccIC() +{ + riIeJeO->preAccIC(); + Constraint::preAccIC(); +} diff --git a/MbDCode/AtPointConstraintIJ.h b/MbDCode/AtPointConstraintIJ.h index e6d05b0..4ef5252 100644 --- a/MbDCode/AtPointConstraintIJ.h +++ b/MbDCode/AtPointConstraintIJ.h @@ -10,7 +10,7 @@ namespace MbD { //axis riIeJeO public: AtPointConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj, int axisi); - void initialize(); + void initialize() override; void initializeLocally() override; void initializeGlobally() override; virtual void initriIeJeO(); @@ -20,6 +20,8 @@ namespace MbD { MbD::ConstraintType type() override; void postPosICIteration() override; void preVelIC() override; + void preAccIC() override; + int axis; std::shared_ptr riIeJeO; diff --git a/MbDCode/AtPointConstraintIqcJc.cpp b/MbDCode/AtPointConstraintIqcJc.cpp index 754d2c7..fd4f8af 100644 --- a/MbDCode/AtPointConstraintIqcJc.cpp +++ b/MbDCode/AtPointConstraintIqcJc.cpp @@ -54,3 +54,23 @@ void MbD::AtPointConstraintIqcJc::fillPosKineJacob(SpMatDsptr mat) mat->atijplusNumber(iG, iqXIminusOnePlusAxis, -1.0); mat->atijplusFullRow(iG, iqEI, pGpEI); } + +void MbD::AtPointConstraintIqcJc::fillVelICJacob(SpMatDsptr mat) +{ + mat->atijplusNumber(iG, iqXIminusOnePlusAxis, -1.0); + mat->atijplusNumber(iqXIminusOnePlusAxis, iG, -1.0); + mat->atijplusFullRow(iG, iqEI, pGpEI); + mat->atijplusFullColumn(iqEI, iG, pGpEI->transpose()); +} + +void MbD::AtPointConstraintIqcJc::fillAccICIterError(FColDsptr col) +{ + col->atiminusNumber(iqXIminusOnePlusAxis, lam); + col->atiplusFullVectortimes(iqEI, pGpEI, lam); + auto efrmIqc = std::static_pointer_cast(frmI); + auto qEdotI = efrmIqc->qEdot(); + auto sum = -efrmIqc->qXddot()->at(axis); + sum += pGpEI->timesFullColumn(efrmIqc->qEddot()); + sum += qEdotI->transposeTimesFullColumn(ppGpEIpEI->timesFullColumn(qEdotI)); + col->atiplusNumber(iG, sum); +} diff --git a/MbDCode/AtPointConstraintIqcJc.h b/MbDCode/AtPointConstraintIqcJc.h index e2abf52..913255a 100644 --- a/MbDCode/AtPointConstraintIqcJc.h +++ b/MbDCode/AtPointConstraintIqcJc.h @@ -15,6 +15,8 @@ namespace MbD { void fillPosICError(FColDsptr col) override; void fillPosICJacob(SpMatDsptr mat) override; void fillPosKineJacob(SpMatDsptr mat) override; + void fillVelICJacob(SpMatDsptr mat) override; + void fillAccICIterError(FColDsptr col) override; FRowDsptr pGpEI; FMatDsptr ppGpEIpEI; diff --git a/MbDCode/AtPointConstraintIqcJqc.cpp b/MbDCode/AtPointConstraintIqcJqc.cpp index dc29007..7efb21b 100644 --- a/MbDCode/AtPointConstraintIqcJqc.cpp +++ b/MbDCode/AtPointConstraintIqcJqc.cpp @@ -57,3 +57,25 @@ void MbD::AtPointConstraintIqcJqc::fillPosKineJacob(SpMatDsptr mat) mat->atijplusNumber(iG, iqXJminusOnePlusAxis, 1.0); mat->atijplusFullRow(iG, iqEJ, pGpEJ); } + +void MbD::AtPointConstraintIqcJqc::fillVelICJacob(SpMatDsptr mat) +{ + AtPointConstraintIqcJc::fillVelICJacob(mat); + mat->atijplusNumber(iG, iqXJminusOnePlusAxis, 1.0); + mat->atijplusNumber(iqXJminusOnePlusAxis, iG, 1.0); + mat->atijplusFullRow(iG, iqEJ, pGpEJ); + mat->atijplusFullColumn(iqEJ, iG, pGpEJ->transpose()); +} + +void MbD::AtPointConstraintIqcJqc::fillAccICIterError(FColDsptr col) +{ + AtPointConstraintIqcJc::fillAccICIterError(col); + col->atiplusNumber(iqXJminusOnePlusAxis, lam); + col->atiplusFullVectortimes(iqEJ, pGpEJ, lam); + auto efrmJqc = std::static_pointer_cast(frmJ); + auto qEdotJ = efrmJqc->qEdot(); + auto sum = efrmJqc->qXddot()->at(axis); + sum += pGpEJ->timesFullColumn(efrmJqc->qEddot()); + sum += qEdotJ->transposeTimesFullColumn(ppGpEJpEJ->timesFullColumn(qEdotJ)); + col->atiplusNumber(iG, sum); +} diff --git a/MbDCode/AtPointConstraintIqcJqc.h b/MbDCode/AtPointConstraintIqcJqc.h index 7c724bb..103de33 100644 --- a/MbDCode/AtPointConstraintIqcJqc.h +++ b/MbDCode/AtPointConstraintIqcJqc.h @@ -15,6 +15,8 @@ namespace MbD { void fillPosICError(FColDsptr col) override; void fillPosICJacob(SpMatDsptr mat) override; void fillPosKineJacob(SpMatDsptr mat) override; + void fillVelICJacob(SpMatDsptr mat) override; + void fillAccICIterError(FColDsptr col) override; FRowDsptr pGpEJ; FMatDsptr ppGpEJpEJ; diff --git a/MbDCode/AtPointConstraintIqctJqc.cpp b/MbDCode/AtPointConstraintIqctJqc.cpp index 2155f62..13a36b7 100644 --- a/MbDCode/AtPointConstraintIqctJqc.cpp +++ b/MbDCode/AtPointConstraintIqctJqc.cpp @@ -38,3 +38,25 @@ void MbD::AtPointConstraintIqctJqc::preVelIC() AtPointConstraintIJ::preVelIC(); pGpt = std::static_pointer_cast(riIeJeO)->priIeJeOpt; } + +void MbD::AtPointConstraintIqctJqc::fillVelICError(FColDsptr col) +{ + col->atiminusNumber(iG, pGpt); +} + +void MbD::AtPointConstraintIqctJqc::fillAccICIterError(FColDsptr col) +{ + AtPointConstraintIqcJqc::fillAccICIterError(col); + auto efrmIqc = std::static_pointer_cast(frmI); + auto qEdotI = efrmIqc->qEdot(); + double sum = (ppGpEIpt->timesFullColumn(qEdotI)) * 2.0; + sum += ppGptpt; + col->atiplusNumber(iG, sum); +} + +void MbD::AtPointConstraintIqctJqc::preAccIC() +{ + AtPointConstraintIJ::preAccIC(); + ppGpEIpt = std::static_pointer_cast(riIeJeO)->ppriIeJeOpEIpt; + ppGptpt = std::static_pointer_cast(riIeJeO)->ppriIeJeOptpt; +} diff --git a/MbDCode/AtPointConstraintIqctJqc.h b/MbDCode/AtPointConstraintIqctJqc.h index 40f8bc9..5f9778a 100644 --- a/MbDCode/AtPointConstraintIqctJqc.h +++ b/MbDCode/AtPointConstraintIqctJqc.h @@ -13,10 +13,15 @@ namespace MbD { void calcPostDynCorrectorIteration() override; MbD::ConstraintType type() override; void preVelIC() override; + void fillVelICError(FColDsptr col) override; + void fillAccICIterError(FColDsptr col) override; double pGpt; FRowDsptr ppGpEIpt; double ppGptpt; + void preAccIC() override; + + }; } diff --git a/MbDCode/BasicIntegrator.h b/MbDCode/BasicIntegrator.h index 9162fd9..16acc4e 100644 --- a/MbDCode/BasicIntegrator.h +++ b/MbDCode/BasicIntegrator.h @@ -19,7 +19,7 @@ namespace MbD { void initializeLocally() override; virtual void iStep(int i); virtual void postFirstStep(); - virtual void postRun() override; + void postRun() override; virtual void postStep(); virtual void preFirstStep(); virtual void preRun() override; diff --git a/MbDCode/CREATE.h b/MbDCode/CREATE.h index 58a1c0c..daae3c3 100644 --- a/MbDCode/CREATE.h +++ b/MbDCode/CREATE.h @@ -1,6 +1,7 @@ +//This header file causes wierd problems in Visual Studio when included in subclasses of std::vector or std::map. + #pragma once #include -//#include "EndFramec.h" #include "EndFrameqct.h" #include "AtPointConstraintIqctJqc.h" #include "DirectionCosineConstraintIqctJqc.h" @@ -28,6 +29,11 @@ namespace MbD { inst->initialize(); return inst; } + static std::shared_ptr With(int m, int n) { + auto inst = std::make_shared(m, n); + inst->initialize(); + return inst; + } static std::shared_ptr With(std::initializer_list listD) { auto inst = std::make_shared(listD); inst->initialize(); diff --git a/MbDCode/CartesianFrame.h b/MbDCode/CartesianFrame.h index 79917c4..2015ac4 100644 --- a/MbDCode/CartesianFrame.h +++ b/MbDCode/CartesianFrame.h @@ -7,7 +7,7 @@ namespace MbD { public: CartesianFrame(); CartesianFrame(const char* str); - void initialize(); + void initialize() override; }; } diff --git a/MbDCode/Constraint.cpp b/MbDCode/Constraint.cpp index 081bfe7..06d4351a 100644 --- a/MbDCode/Constraint.cpp +++ b/MbDCode/Constraint.cpp @@ -99,6 +99,11 @@ void MbD::Constraint::setqsulam(FColDsptr col) lam = col->at(iG); } +void MbD::Constraint::setqsudotlam(FColDsptr col) +{ + lam = col->at(iG); +} + void MbD::Constraint::fillPosICError(FColDsptr col) { col->at(iG) += aG; @@ -141,3 +146,20 @@ void MbD::Constraint::fillPosKineError(FColDsptr col) { col->atiplusNumber(iG, aG); } + +void MbD::Constraint::preAccIC() +{ + lam = 0.0; + Item::preAccIC(); +} + +void MbD::Constraint::fillAccICIterJacob(SpMatDsptr mat) +{ + //"Same as velIC." + this->fillVelICJacob(mat); +} + +void MbD::Constraint::setqsuddotlam(FColDsptr qsudotlam) +{ + lam = qsudotlam->at(iG); +} diff --git a/MbDCode/Constraint.h b/MbDCode/Constraint.h index ed2f2d2..87c61d4 100644 --- a/MbDCode/Constraint.h +++ b/MbDCode/Constraint.h @@ -11,7 +11,7 @@ namespace MbD { public: Constraint(); Constraint(const char* str); - void initialize(); + void initialize() override; void postInput() override; void setOwner(Item* x); Item* getOwner(); @@ -25,6 +25,7 @@ namespace MbD { virtual MbD::ConstraintType type(); void fillqsulam(FColDsptr col) override; void setqsulam(FColDsptr col) override; + void setqsudotlam(FColDsptr col) override; void fillPosICError(FColDsptr col) override; void removeRedundantConstraints(std::shared_ptr> redundantEqnNos) override; void reactivateRedundantConstraints() override; @@ -32,6 +33,9 @@ namespace MbD { void outputStates() override; void preDyn() override; void fillPosKineError(FColDsptr col) override; + void preAccIC() override; + void fillAccICIterJacob(SpMatDsptr mat) override; + void setqsuddotlam(FColDsptr qsudotlam) override; int iG = -1; double aG = 0.0; //Constraint function diff --git a/MbDCode/ConstraintIJ.h b/MbDCode/ConstraintIJ.h index 77059b1..ae7c106 100644 --- a/MbDCode/ConstraintIJ.h +++ b/MbDCode/ConstraintIJ.h @@ -9,7 +9,7 @@ namespace MbD { //frmI frmJ aConstant public: ConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj); - void initialize(); + void initialize() override; EndFrmcptr frmI, frmJ; double aConstant; diff --git a/MbDCode/DiagonalMatrix.h b/MbDCode/DiagonalMatrix.h index 8ae2596..52569a8 100644 --- a/MbDCode/DiagonalMatrix.h +++ b/MbDCode/DiagonalMatrix.h @@ -2,6 +2,7 @@ #include "Array.h" #include "FullColumn.h" +#include "FullMatrix.h" namespace MbD { template @@ -10,9 +11,12 @@ namespace MbD { // public: 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 < DiagonalMatrix> diagMat); + std::shared_ptr> times(T factor); std::shared_ptr> timesFullColumn(std::shared_ptr> fullCol); + std::shared_ptr> timesFullMatrix(std::shared_ptr> fullMat); int nrow() { return (int) this->size(); } @@ -32,6 +36,17 @@ namespace MbD { } } template + inline std::shared_ptr> DiagonalMatrix::times(T 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 std::shared_ptr> DiagonalMatrix::timesFullColumn(std::shared_ptr> fullCol) { //"a*b = a(i,j)b(j) sum j." @@ -44,6 +59,17 @@ namespace MbD { } return answer; } + template + inline std::shared_ptr> DiagonalMatrix::timesFullMatrix(std::shared_ptr> 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() { diff --git a/MbDCode/DifferenceOperator.h b/MbDCode/DifferenceOperator.h index 8da4b4d..b8f0095 100644 --- a/MbDCode/DifferenceOperator.h +++ b/MbDCode/DifferenceOperator.h @@ -10,7 +10,7 @@ namespace MbD { //iStep order taylorMatrix operatorMatrix time timeNodes public: void calcOperatorMatrix(); - void initialize(); + virtual void initialize(); virtual void setiStep(int i); virtual void setorder(int o); virtual void formTaylorMatrix() = 0; diff --git a/MbDCode/DirectionCosineConstraintIJ.cpp b/MbDCode/DirectionCosineConstraintIJ.cpp index d3711b6..0fcf760 100644 --- a/MbDCode/DirectionCosineConstraintIJ.cpp +++ b/MbDCode/DirectionCosineConstraintIJ.cpp @@ -64,3 +64,9 @@ void MbD::DirectionCosineConstraintIJ::preVelIC() aAijIeJe->preVelIC(); Item::preVelIC(); } + +void MbD::DirectionCosineConstraintIJ::preAccIC() +{ + aAijIeJe->preAccIC(); + Constraint::preAccIC(); +} diff --git a/MbDCode/DirectionCosineConstraintIJ.h b/MbDCode/DirectionCosineConstraintIJ.h index 706c2b8..fe71902 100644 --- a/MbDCode/DirectionCosineConstraintIJ.h +++ b/MbDCode/DirectionCosineConstraintIJ.h @@ -10,7 +10,7 @@ namespace MbD { //axisI axisJ aAijIeJe public: DirectionCosineConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj); - void initialize(); + void initialize() override; void initializeLocally() override; void initializeGlobally() override; virtual void initaAijIeJe(); @@ -20,6 +20,7 @@ namespace MbD { void postPosICIteration() override; MbD::ConstraintType type() override; void preVelIC() override; + void preAccIC() override; int axisI, axisJ; std::shared_ptr aAijIeJe; diff --git a/MbDCode/DirectionCosineConstraintIqcJc.cpp b/MbDCode/DirectionCosineConstraintIqcJc.cpp index e159a10..7bf95d3 100644 --- a/MbDCode/DirectionCosineConstraintIqcJc.cpp +++ b/MbDCode/DirectionCosineConstraintIqcJc.cpp @@ -45,3 +45,19 @@ void MbD::DirectionCosineConstraintIqcJc::fillPosKineJacob(SpMatDsptr mat) { mat->atijplusFullRow(iG, iqEI, pGpEI); } + +void MbD::DirectionCosineConstraintIqcJc::fillVelICJacob(SpMatDsptr mat) +{ + mat->atijplusFullRow(iG, iqEI, pGpEI); + mat->atijplusFullColumn(iqEI, iG, pGpEI->transpose()); +} + +void MbD::DirectionCosineConstraintIqcJc::fillAccICIterError(FColDsptr col) +{ + col->atiplusFullVector(iqEI, pGpEI->times(lam)); + auto efrmIqc = std::static_pointer_cast(frmI); + auto qEdotI = efrmIqc->qEdot(); + auto sum = pGpEI->timesFullColumn(efrmIqc->qEddot()); + sum += qEdotI->transposeTimesFullColumn(ppGpEIpEI->timesFullColumn(qEdotI)); + col->atiplusNumber(iG, sum); +} diff --git a/MbDCode/DirectionCosineConstraintIqcJc.h b/MbDCode/DirectionCosineConstraintIqcJc.h index 14f22dc..b2d2f88 100644 --- a/MbDCode/DirectionCosineConstraintIqcJc.h +++ b/MbDCode/DirectionCosineConstraintIqcJc.h @@ -14,6 +14,8 @@ namespace MbD { void fillPosICError(FColDsptr col) override; void fillPosICJacob(SpMatDsptr mat) override; void fillPosKineJacob(SpMatDsptr mat) override; + void fillVelICJacob(SpMatDsptr mat) override; + void fillAccICIterError(FColDsptr col) override; FRowDsptr pGpEI; FMatDsptr ppGpEIpEI; diff --git a/MbDCode/DirectionCosineConstraintIqcJqc.cpp b/MbDCode/DirectionCosineConstraintIqcJqc.cpp index eb8b03c..6381d57 100644 --- a/MbDCode/DirectionCosineConstraintIqcJqc.cpp +++ b/MbDCode/DirectionCosineConstraintIqcJqc.cpp @@ -52,3 +52,24 @@ void MbD::DirectionCosineConstraintIqcJqc::fillPosKineJacob(SpMatDsptr mat) DirectionCosineConstraintIqcJc::fillPosKineJacob(mat); mat->atijplusFullRow(iG, iqEJ, pGpEJ); } + +void MbD::DirectionCosineConstraintIqcJqc::fillVelICJacob(SpMatDsptr mat) +{ + DirectionCosineConstraintIqcJc::fillVelICJacob(mat); + mat->atijplusFullRow(iG, iqEJ, pGpEJ); + mat->atijplusFullColumn(iqEJ, iG, pGpEJ->transpose()); +} + +void MbD::DirectionCosineConstraintIqcJqc::fillAccICIterError(FColDsptr col) +{ + DirectionCosineConstraintIqcJc::fillAccICIterError(col); + col->atiplusFullVectortimes(iqEJ, pGpEJ, lam); + auto efrmIqc = std::static_pointer_cast(frmI); + auto efrmJqc = std::static_pointer_cast(frmJ); + auto qEdotI = efrmIqc->qEdot(); + auto qEdotJ = efrmJqc->qEdot(); + double sum = pGpEJ->timesFullColumn(efrmJqc->qEddot()); + sum += (qEdotI->transposeTimesFullColumn(ppGpEIpEJ->timesFullColumn(qEdotJ))) * 2.0; + sum += qEdotJ->transposeTimesFullColumn(ppGpEJpEJ->timesFullColumn(qEdotJ)); + col->atiplusNumber(iG, sum); +} diff --git a/MbDCode/DirectionCosineConstraintIqcJqc.h b/MbDCode/DirectionCosineConstraintIqcJqc.h index bef36a9..48d93f8 100644 --- a/MbDCode/DirectionCosineConstraintIqcJqc.h +++ b/MbDCode/DirectionCosineConstraintIqcJqc.h @@ -14,6 +14,8 @@ namespace MbD { void fillPosICError(FColDsptr col) override; void fillPosICJacob(SpMatDsptr mat) override; void fillPosKineJacob(SpMatDsptr mat) override; + void fillVelICJacob(SpMatDsptr mat) override; + void fillAccICIterError(FColDsptr col) override; FRowDsptr pGpEJ; FMatDsptr ppGpEIpEJ; diff --git a/MbDCode/DirectionCosineConstraintIqctJqc.cpp b/MbDCode/DirectionCosineConstraintIqctJqc.cpp index bd7ffe6..5f3b251 100644 --- a/MbDCode/DirectionCosineConstraintIqctJqc.cpp +++ b/MbDCode/DirectionCosineConstraintIqctJqc.cpp @@ -24,3 +24,29 @@ void MbD::DirectionCosineConstraintIqctJqc::preVelIC() DirectionCosineConstraintIJ::preVelIC(); pGpt = std::static_pointer_cast(aAijIeJe)->pAijIeJept; } + +void MbD::DirectionCosineConstraintIqctJqc::fillVelICError(FColDsptr col) +{ + col->atiminusNumber(iG, pGpt); +} + +void MbD::DirectionCosineConstraintIqctJqc::preAccIC() +{ + DirectionCosineConstraintIJ::preAccIC(); + ppGpEIpt = std::static_pointer_cast(aAijIeJe)->ppAijIeJepEIpt; + ppGpEJpt = std::static_pointer_cast(aAijIeJe)->ppAijIeJepEJpt; + ppGptpt = std::static_pointer_cast(aAijIeJe)->ppAijIeJeptpt; +} + +void MbD::DirectionCosineConstraintIqctJqc::fillAccICIterError(FColDsptr col) +{ + DirectionCosineConstraintIqcJqc::fillAccICIterError(col); + auto efrmIqc = std::static_pointer_cast(frmI); + auto efrmJqc = std::static_pointer_cast(frmJ); + auto qEdotI = efrmIqc->qEdot(); + auto qEdotJ = efrmJqc->qEdot(); + double sum = (ppGpEIpt->timesFullColumn(qEdotI)) * 2.0; + sum += (ppGpEJpt->timesFullColumn(qEdotJ)) * 2.0; + sum += ppGptpt; + col->atiplusNumber(iG, sum); +} diff --git a/MbDCode/DirectionCosineConstraintIqctJqc.h b/MbDCode/DirectionCosineConstraintIqctJqc.h index 419f79f..458fa78 100644 --- a/MbDCode/DirectionCosineConstraintIqctJqc.h +++ b/MbDCode/DirectionCosineConstraintIqctJqc.h @@ -11,6 +11,9 @@ namespace MbD { void initaAijIeJe() override; MbD::ConstraintType type() override; void preVelIC() override; + void fillVelICError(FColDsptr col) override; + void preAccIC() override; + void fillAccICIterError(FColDsptr col) override; double pGpt; FRowDsptr ppGpEIpt; diff --git a/MbDCode/DirectionCosineIeqcJec.h b/MbDCode/DirectionCosineIeqcJec.h index 3eb5e21..de59393 100644 --- a/MbDCode/DirectionCosineIeqcJec.h +++ b/MbDCode/DirectionCosineIeqcJec.h @@ -9,7 +9,7 @@ namespace MbD { public: DirectionCosineIeqcJec(); DirectionCosineIeqcJec(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj); - void initialize(); + void initialize() override; void initializeGlobally(); void calcPostDynCorrectorIteration() override; diff --git a/MbDCode/DirectionCosineIeqcJeqc.h b/MbDCode/DirectionCosineIeqcJeqc.h index a586dc4..dd6ed9b 100644 --- a/MbDCode/DirectionCosineIeqcJeqc.h +++ b/MbDCode/DirectionCosineIeqcJeqc.h @@ -9,7 +9,7 @@ namespace MbD { public: DirectionCosineIeqcJeqc(); DirectionCosineIeqcJeqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj); - void initialize(); + void initialize() override; void initializeGlobally(); void calcPostDynCorrectorIteration() override; FRowDsptr pvaluepEJ() override; diff --git a/MbDCode/DirectionCosineIeqctJeqc.cpp b/MbDCode/DirectionCosineIeqctJeqc.cpp index 54955d0..92592ef 100644 --- a/MbDCode/DirectionCosineIeqctJeqc.cpp +++ b/MbDCode/DirectionCosineIeqctJeqc.cpp @@ -44,3 +44,22 @@ double MbD::DirectionCosineIeqctJeqc::pvaluept() { return pAijIeJept; } + +void MbD::DirectionCosineIeqctJeqc::preAccIC() +{ + //| ppAjOIepEITpt ppAjOIeptpt ppAjOIepEITpti pAjOIept | + Item::preAccIC(); + auto pAjOIept = std::static_pointer_cast(frmI)->pAjOept(axisI); + auto ppAjOIepEITpt = std::static_pointer_cast(frmI)->ppAjOepETpt(axisI); + auto ppAjOIeptpt = std::static_pointer_cast(frmI)->ppAjOeptpt(axisI); + for (int i = 0; i < 4; i++) + { + auto& ppAjOIepEITpti = ppAjOIepEITpt->at(i); + ppAijIeJepEIpt->atiput(i, ppAjOIepEITpti->dot(aAjOJe)); + } + for (int i = 0; i < 4; i++) + { + ppAijIeJepEJpt->atiput(i, pAjOIept->dot(pAjOJepEJT->at(i))); + } + ppAijIeJeptpt = ppAjOIeptpt->dot(aAjOJe); +} diff --git a/MbDCode/DirectionCosineIeqctJeqc.h b/MbDCode/DirectionCosineIeqctJeqc.h index 1b1616b..587bdec 100644 --- a/MbDCode/DirectionCosineIeqctJeqc.h +++ b/MbDCode/DirectionCosineIeqctJeqc.h @@ -9,11 +9,12 @@ namespace MbD { public: DirectionCosineIeqctJeqc(); DirectionCosineIeqctJeqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj); - void initialize(); + void initialize() override; void initializeGlobally(); void calcPostDynCorrectorIteration() override; void preVelIC() override; double pvaluept(); + void preAccIC() override; double pAijIeJept; FRowDsptr ppAijIeJepEIpt; diff --git a/MbDCode/DispCompIeqcJeqcKeqct.cpp b/MbDCode/DispCompIeqcJeqcKeqct.cpp index 8afb6a4..a93fdeb 100644 --- a/MbDCode/DispCompIeqcJeqcKeqct.cpp +++ b/MbDCode/DispCompIeqcJeqcKeqct.cpp @@ -41,3 +41,55 @@ double MbD::DispCompIeqcJeqcKeqct::pvaluept() { return priIeJeKept; } + +FRowDsptr MbD::DispCompIeqcJeqcKeqct::ppvaluepXIpt() +{ + return ppriIeJeKepXIpt; +} + +FRowDsptr MbD::DispCompIeqcJeqcKeqct::ppvaluepEIpt() +{ + return ppriIeJeKepEIpt; +} + +FRowDsptr MbD::DispCompIeqcJeqcKeqct::ppvaluepEKpt() +{ + return ppriIeJeKepEKpt; +} + +FRowDsptr MbD::DispCompIeqcJeqcKeqct::ppvaluepXJpt() +{ + return ppriIeJeKepXJpt; +} + +FRowDsptr MbD::DispCompIeqcJeqcKeqct::ppvaluepEJpt() +{ + return ppriIeJeKepEJpt; +} + +double MbD::DispCompIeqcJeqcKeqct::ppvalueptpt() +{ + return ppriIeJeKeptpt; +} + +void MbD::DispCompIeqcJeqcKeqct::preAccIC() +{ + Item::preAccIC(); + auto pAjOKept = std::static_pointer_cast(efrmK)->pAjOept(axisK); + auto ppAjOKepEKTpt = std::static_pointer_cast(efrmK)->ppAjOepETpt(axisK); + auto ppAjOKeptpt = std::static_pointer_cast(efrmK)->ppAjOeptpt(axisK); + auto prIeJeOpEIT = std::static_pointer_cast(frmI)->prOeOpE->transpose()->negated(); + auto prIeJeOpEJT = std::static_pointer_cast(frmJ)->prOeOpE->transpose(); + for (int i = 0; i < 3; i++) + { + ppriIeJeKepXIpt->atiput(i, -(pAjOKept->at(i))); + ppriIeJeKepXJpt->atiput(i, pAjOKept->at(i)); + } + for (int i = 0; i < 4; i++) + { + ppriIeJeKepEIpt->atiput(i, pAjOKept->dot(prIeJeOpEIT->at(i))); + ppriIeJeKepEJpt->atiput(i, pAjOKept->dot(prIeJeOpEJT->at(i))); + ppriIeJeKepEKpt->atiput(i, ppAjOKepEKTpt->at(i)->dot(rIeJeO)); + } + ppriIeJeKeptpt = ppAjOKeptpt->dot(rIeJeO); +} diff --git a/MbDCode/DispCompIeqcJeqcKeqct.h b/MbDCode/DispCompIeqcJeqcKeqct.h index 0f6861f..6c36671 100644 --- a/MbDCode/DispCompIeqcJeqcKeqct.h +++ b/MbDCode/DispCompIeqcJeqcKeqct.h @@ -13,6 +13,13 @@ namespace MbD { void calcPostDynCorrectorIteration() override; void preVelIC() override; double pvaluept(); + FRowDsptr ppvaluepXIpt(); + FRowDsptr ppvaluepEIpt(); + FRowDsptr ppvaluepEKpt(); + FRowDsptr ppvaluepXJpt(); + FRowDsptr ppvaluepEJpt(); + double ppvalueptpt(); + void preAccIC() override; double priIeJeKept; FRowDsptr ppriIeJeKepXIpt; diff --git a/MbDCode/DispCompIeqctJeqcKeqct.cpp b/MbDCode/DispCompIeqctJeqcKeqct.cpp index 0416a87..1658fa6 100644 --- a/MbDCode/DispCompIeqctJeqcKeqct.cpp +++ b/MbDCode/DispCompIeqctJeqcKeqct.cpp @@ -14,6 +14,22 @@ MbD::DispCompIeqctJeqcKeqct::DispCompIeqctJeqcKeqct(EndFrmcptr frmi, EndFrmcptr void MbD::DispCompIeqctJeqcKeqct::preVelIC() { DispCompIeqcJeqcKeqct::preVelIC(); - auto mprIeJeOpt = std::static_pointer_cast(frmI)->prOeOpt; + auto& mprIeJeOpt = std::static_pointer_cast(frmI)->prOeOpt; priIeJeKept -= aAjOKe->dot(mprIeJeOpt); } + +void MbD::DispCompIeqctJeqcKeqct::preAccIC() +{ + DispCompIeqcJeqcKeqct::preAccIC(); + auto pAjOKept = std::static_pointer_cast(efrmK)->pAjOept(axisK); + auto efrmIqct = std::static_pointer_cast(frmI); + auto& mprIeJeOpt = efrmIqct->prOeOpt; + auto mpprIeJeOpEITpt = efrmIqct->pprOeOpEpt->transpose(); + auto& mpprIeJeOptpt = efrmIqct->pprOeOptpt; + for (int i = 0; i < 4; i++) + { + ppriIeJeKepEIpt->atiminusNumber(i, aAjOKe->dot(mpprIeJeOpEITpt->at(i))); + ppriIeJeKepEKpt->atiminusNumber(i, pAjOKepEKT->at(i)->dot(mprIeJeOpt)); + } + ppriIeJeKeptpt += -(2.0 * pAjOKept->dot(mprIeJeOpt)) - aAjOKe->dot(mpprIeJeOptpt); +} diff --git a/MbDCode/DispCompIeqctJeqcKeqct.h b/MbDCode/DispCompIeqctJeqcKeqct.h index 9dec4f3..76a589f 100644 --- a/MbDCode/DispCompIeqctJeqcKeqct.h +++ b/MbDCode/DispCompIeqctJeqcKeqct.h @@ -10,6 +10,7 @@ namespace MbD { DispCompIeqctJeqcKeqct(); DispCompIeqctJeqcKeqct(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk); void preVelIC() override; + void preAccIC() override; }; } diff --git a/MbDCode/DispCompIeqctJeqcO.cpp b/MbDCode/DispCompIeqctJeqcO.cpp index 124f8bb..2dda312 100644 --- a/MbDCode/DispCompIeqctJeqcO.cpp +++ b/MbDCode/DispCompIeqctJeqcO.cpp @@ -34,3 +34,10 @@ double MbD::DispCompIeqctJeqcO::pvaluept() { return priIeJeOpt; } + +void MbD::DispCompIeqctJeqcO::preAccIC() +{ + Item::preAccIC(); + ppriIeJeOpEIpt = (std::static_pointer_cast(frmI)->ppriOeOpEpt(axis))->negated(); + ppriIeJeOptpt = -(std::static_pointer_cast(frmI)->ppriOeOptpt(axis)); +} diff --git a/MbDCode/DispCompIeqctJeqcO.h b/MbDCode/DispCompIeqctJeqcO.h index 65f4e61..7a9e9d4 100644 --- a/MbDCode/DispCompIeqctJeqcO.h +++ b/MbDCode/DispCompIeqctJeqcO.h @@ -13,6 +13,7 @@ namespace MbD { void calcPostDynCorrectorIteration() override; void preVelIC() override; double pvaluept(); + void preAccIC() override; double priIeJeOpt; FRowDsptr ppriIeJeOpEIpt; diff --git a/MbDCode/EndFramec.h b/MbDCode/EndFramec.h index e9f9509..ae17b8c 100644 --- a/MbDCode/EndFramec.h +++ b/MbDCode/EndFramec.h @@ -19,7 +19,7 @@ namespace MbD { public: EndFramec(); EndFramec(const char* str); - void initialize(); + void initialize() override; void setMarkerFrame(MarkerFrame* markerFrm); MarkerFrame* getMarkerFrame(); void initializeLocally() override; diff --git a/MbDCode/EndFrameqc.cpp b/MbDCode/EndFrameqc.cpp index be7fd56..2f486ac 100644 --- a/MbDCode/EndFrameqc.cpp +++ b/MbDCode/EndFrameqc.cpp @@ -101,3 +101,23 @@ FRowDsptr MbD::EndFrameqc::priOeOpE(int i) { return prOeOpE->at(i); } + +FColDsptr MbD::EndFrameqc::qXdot() +{ + return markerFrame->qXdot(); +} + +std::shared_ptr> MbD::EndFrameqc::qEdot() +{ + return markerFrame->qEdot(); +} + +FColDsptr MbD::EndFrameqc::qXddot() +{ + return markerFrame->qXddot(); +} + +FColDsptr MbD::EndFrameqc::qEddot() +{ + return markerFrame->qEddot(); +} diff --git a/MbDCode/EndFrameqc.h b/MbDCode/EndFrameqc.h index 042604f..ced0e69 100644 --- a/MbDCode/EndFrameqc.h +++ b/MbDCode/EndFrameqc.h @@ -2,6 +2,8 @@ #include "EndFramec.h" #include "Symbolic.h" +#include "EulerParametersDot.h" +#include "EulerParametersDDot.h" namespace MbD { class EndFrameqct; @@ -12,7 +14,7 @@ namespace MbD { public: EndFrameqc(); EndFrameqc(const char* str); - void initialize(); + void initialize() override; void initializeGlobally() override; void initEndFrameqct() override; FMatFColDsptr ppAjOepEpE(int j); @@ -22,6 +24,10 @@ namespace MbD { int iqX(); int iqE(); FRowDsptr priOeOpE(int i); + FColDsptr qXdot(); + std::shared_ptr> qEdot(); + FColDsptr qXddot(); + FColDsptr qEddot(); FMatDsptr prOeOpE; std::shared_ptr>>> pprOeOpEpE; diff --git a/MbDCode/EndFrameqct.cpp b/MbDCode/EndFrameqct.cpp index 753aa2f..ddcc67c 100644 --- a/MbDCode/EndFrameqct.cpp +++ b/MbDCode/EndFrameqct.cpp @@ -7,6 +7,7 @@ #include "CREATE.h" #include "EulerAngleszxz.h" #include "EulerAngleszxzDot.h" +#include "EulerAngleszxzDDot.h" using namespace MbD; @@ -71,6 +72,14 @@ void EndFrameqct::initprmemptBlks() void EndFrameqct::initpprmemptptBlks() { + auto& mbdTime = System::getInstance().time; + pprmemptptBlks = std::make_shared< FullColumn>(3); + for (int i = 0; i < 3; i++) { + auto& vel = prmemptBlks->at(i); + auto var = vel->differentiateWRT(vel, mbdTime); + auto acc = var->simplified(var); + pprmemptptBlks->at(i) = acc; + } } void EndFrameqct::initpPhiThePsiptBlks() @@ -89,6 +98,14 @@ void EndFrameqct::initpPhiThePsiptBlks() void MbD::EndFrameqct::initppPhiThePsiptptBlks() { + auto& mbdTime = System::getInstance().time; + ppPhiThePsiptptBlks = std::make_shared< FullColumn>(3); + for (int i = 0; i < 3; i++) { + auto& angleVel = pPhiThePsiptBlks->at(i); + auto var = angleVel->differentiateWRT(angleVel, mbdTime); + auto angleAcc = var->simplified(var); + ppPhiThePsiptptBlks->at(i) = angleAcc; + } } void MbD::EndFrameqct::postInput() @@ -169,21 +186,70 @@ void MbD::EndFrameqct::preVelIC() Item::preVelIC(); this->evalprmempt(); this->evalpAmept(); - auto aAOm = markerFrame->aAOm; + auto& aAOm = markerFrame->aAOm; prOeOpt = aAOm->timesFullColumn(prmempt); pAOept = aAOm->timesFullMatrix(pAmept); } +void MbD::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 MbD::EndFrameqct::pAjOept(int j) { return pAOept->column(j); } +FMatDsptr MbD::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 MbD::EndFrameqct::ppAjOeptpt(int j) +{ + return ppAOeptpt->column(j); +} + double MbD::EndFrameqct::priOeOpt(int i) { return prOeOpt->at(i); } +FRowDsptr MbD::EndFrameqct::ppriOeOpEpt(int i) +{ + return pprOeOpEpt->at(i); +} + +double MbD::EndFrameqct::ppriOeOptpt(int i) +{ + return pprOeOptpt->at(i); +} + void MbD::EndFrameqct::evalprmempt() { if (rmemBlks) { @@ -216,3 +282,61 @@ void MbD::EndFrameqct::evalpAmept() pAmept = phiThePsiDot->aAdot; } } + +void MbD::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 MbD::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; + } +} + +void MbD::EndFrameqct::preAccIC() +{ + time = System::getInstance().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/MbDCode/EndFrameqct.h b/MbDCode/EndFrameqct.h index 0f5d582..26fa6f3 100644 --- a/MbDCode/EndFrameqct.h +++ b/MbDCode/EndFrameqct.h @@ -12,7 +12,7 @@ namespace MbD { public: EndFrameqct(); EndFrameqct(const char* str); - void initialize(); + void initialize() override; void initializeLocally() override; void initializeGlobally() override; void initprmemptBlks(); @@ -25,11 +25,20 @@ namespace MbD { void evalrmem(); void evalAme(); void preVelIC() override; + void postVelIC() override; FColDsptr pAjOept(int j); + FMatDsptr ppAjOepETpt(int j); + FColDsptr ppAjOeptpt(int j); double time = 0.0; double priOeOpt(int i); + FRowDsptr ppriOeOpEpt(int i); + double ppriOeOptpt(int i); void evalprmempt(); void evalpAmept(); + void evalpprmemptpt(); + void evalppAmeptpt(); + + void preAccIC() override; std::shared_ptr> rmemBlks, prmemptBlks, pprmemptptBlks; std::shared_ptr> phiThePsiBlks, pPhiThePsiptBlks, ppPhiThePsiptptBlks; diff --git a/MbDCode/EulerAngleszxz.h b/MbDCode/EulerAngleszxz.h index 04311ae..17a8830 100644 --- a/MbDCode/EulerAngleszxz.h +++ b/MbDCode/EulerAngleszxz.h @@ -13,7 +13,7 @@ namespace MbD { public: EulerAngleszxz() : EulerArray(3) {} void initialize() override; - void calc(); + void calc() override; FMatDsptr phiA, theA, psiA, aA; }; diff --git a/MbDCode/EulerAngleszxzDDot.h b/MbDCode/EulerAngleszxzDDot.h index 61e750e..4ded83d 100644 --- a/MbDCode/EulerAngleszxzDDot.h +++ b/MbDCode/EulerAngleszxzDDot.h @@ -10,9 +10,79 @@ namespace MbD { { //phiThePsiDot phiAddot theAddot psiAddot aAddot public: + EulerAngleszxzDDot() : EulerArray(3) {} + void initialize() override; + void calc() override; std::shared_ptr> phiThePsiDot; FMatDsptr phiAddot, theAddot, psiAddot, aAddot; }; + template + inline void EulerAngleszxzDDot::initialize() + { + phiAddot = std::make_shared>(3, 3); + phiAddot->zeroSelf(); + theAddot = std::make_shared>(3, 3); + theAddot->zeroSelf(); + psiAddot = std::make_shared>(3, 3); + psiAddot->zeroSelf(); + } + template + inline void EulerAngleszxzDDot::calc() + { + //| zero phiThePsi phi sphi cphi phidot phiddot cphiddot sphiddot the sthe cthe thedot theddot ctheddot stheddot + // psi spsi cpsi psidot psiddot cpsiddot spsiddot phiA theA psiA phiAdot theAdot psiAdot | + auto zero = 0.0; + auto& phiThePsi = phiThePsiDot->phiThePsi; + auto& phi = phiThePsi->at(0); + auto sphi = std::sin(phi); + auto cphi = std::cos(phi); + auto& phidot = phiThePsiDot->at(0); + auto& phiddot = this->at(0); + auto cphiddot = zero - (sphi * phiddot) - (cphi * phidot * phidot); + auto sphiddot = cphi * phiddot - (sphi * phidot * phidot); + auto& the = phiThePsi->at(1); + auto sthe = std::sin(the); + auto cthe = std::cos(the); + auto& thedot = phiThePsiDot->at(1); + auto& theddot = this->at(1); + auto ctheddot = zero - (sthe * theddot) - (cthe * thedot * thedot); + auto stheddot = cthe * theddot - (sthe * thedot * thedot); + auto& psi = phiThePsi->at(2); + auto spsi = std::sin(psi); + auto cpsi = std::cos(psi); + auto& psidot = phiThePsiDot->at(2); + auto& psiddot = this->at(2); + auto cpsiddot = zero - (spsi * psiddot) - (cpsi * psidot * psidot); + auto spsiddot = cpsi * psiddot - (spsi * psidot * psidot); + phiAddot->at(0)->atiput(0, cphiddot); + phiAddot->at(0)->atiput(1, zero - sphiddot); + phiAddot->at(1)->atiput(0, sphiddot); + phiAddot->at(1)->atiput(1, cphiddot); + theAddot->at(1)->atiput(1, ctheddot); + theAddot->at(1)->atiput(2, zero - stheddot); + theAddot->at(2)->atiput(1, stheddot); + theAddot->at(2)->atiput(2, ctheddot); + psiAddot->at(0)->atiput(0, cpsiddot); + psiAddot->at(0)->atiput(1, zero - spsiddot); + psiAddot->at(1)->atiput(0, spsiddot); + psiAddot->at(1)->atiput(1, cpsiddot); + auto& phiA = phiThePsi->phiA; + auto& theA = phiThePsi->theA; + auto& psiA = phiThePsi->psiA; + auto& phiAdot = phiThePsiDot->phiAdot; + auto& theAdot = phiThePsiDot->theAdot; + auto& psiAdot = phiThePsiDot->psiAdot; + auto mat = *(phiAddot->timesFullMatrix(theA->timesFullMatrix(psiA))) + + *(phiAdot->timesFullMatrix(theAdot->timesFullMatrix(psiA))) + + *(phiAdot->timesFullMatrix(theA->timesFullMatrix(psiAdot))) + + *(phiAdot->timesFullMatrix(theAdot->timesFullMatrix(psiA))) + + *(phiA->timesFullMatrix(theAddot->timesFullMatrix(psiA))) + + *(phiA->timesFullMatrix(theAdot->timesFullMatrix(psiAdot))) + + *(phiAdot->timesFullMatrix(theA->timesFullMatrix(psiAdot))) + + *(phiA->timesFullMatrix(theAdot->timesFullMatrix(psiAdot))) + + *(phiA->timesFullMatrix(theA->timesFullMatrix(psiAddot))); + aAddot = std::make_shared>(mat); + } } diff --git a/MbDCode/EulerAngleszxzDot.h b/MbDCode/EulerAngleszxzDot.h index 25555a8..dd0f744 100644 --- a/MbDCode/EulerAngleszxzDot.h +++ b/MbDCode/EulerAngleszxzDot.h @@ -12,7 +12,7 @@ namespace MbD { public: EulerAngleszxzDot() : EulerArray(3) {} void initialize() override; - void calc(); + void calc() override; std::shared_ptr> phiThePsi; FMatDsptr phiAdot, theAdot, psiAdot, aAdot; @@ -30,7 +30,6 @@ namespace MbD { template inline void EulerAngleszxzDot::calc() { - //| zero phi sphi cphi phidot minussphiTimesphidot cphiTimesphidot the sthe cthe thedot minusstheTimesthedot ctheTimesthedot psi spsi cpsi psidot minusspsiTimespsidot cpsiTimespsidot phiA theA psiA | auto phi = phiThePsi->at(0); auto sphi = std::sin(phi); auto cphi = std::cos(phi); diff --git a/MbDCode/EulerArray.h b/MbDCode/EulerArray.h index 82e303c..cd1c589 100644 --- a/MbDCode/EulerArray.h +++ b/MbDCode/EulerArray.h @@ -11,9 +11,10 @@ namespace MbD { EulerArray(int count) : FullColumn(count) {} EulerArray(int count, const T& value) : FullColumn(count, value) {} EulerArray(std::initializer_list list) : FullColumn{ list } {} - virtual void initialize(); + void initialize() override; void equalFullColumn(std::shared_ptr> fullCol); void equalFullColumnAt(std::shared_ptr> fullCol, int i); + virtual void calc() = 0; }; template diff --git a/MbDCode/EulerConstraint.cpp b/MbDCode/EulerConstraint.cpp index 3fb41aa..7e54efe 100644 --- a/MbDCode/EulerConstraint.cpp +++ b/MbDCode/EulerConstraint.cpp @@ -57,3 +57,21 @@ void MbD::EulerConstraint::fillPosKineJacob(SpMatDsptr mat) { mat->atijplusFullRow(iG, iqE, pGpE); } + +void MbD::EulerConstraint::fillVelICJacob(SpMatDsptr mat) +{ + mat->atijplusFullRow(iG, iqE, pGpE); + mat->atijplusFullColumn(iqE, iG, pGpE->transpose()); +} + +void MbD::EulerConstraint::fillAccICIterError(FColDsptr col) +{ + //"qdotT[ppGpqpq]*qdot." + //"qdotT[2 2 2 2 diag]*qdot." + + col->atiplusFullVectortimes(iqE, pGpE, lam); + auto partFrame = static_cast(owner); + double sum = pGpE->timesFullColumn(partFrame->qEddot); + sum += 2.0 * partFrame->qEdot->sumOfSquares(); + col->atiplusNumber(iG, sum); +} diff --git a/MbDCode/EulerConstraint.h b/MbDCode/EulerConstraint.h index d2de76d..034eb32 100644 --- a/MbDCode/EulerConstraint.h +++ b/MbDCode/EulerConstraint.h @@ -12,12 +12,14 @@ namespace MbD { public: EulerConstraint(); EulerConstraint(const char* str); - void initialize(); + void initialize() override; void calcPostDynCorrectorIteration() override; void useEquationNumbers() override; void fillPosICError(FColDsptr col) override; void fillPosICJacob(SpMatDsptr mat) override; void fillPosKineJacob(SpMatDsptr mat) override; + void fillVelICJacob(SpMatDsptr mat) override; + void fillAccICIterError(FColDsptr col) override; FRowDsptr pGpE; //partial derivative of G wrt pE int iqE = -1; diff --git a/MbDCode/EulerParameters.h b/MbDCode/EulerParameters.h index bb6a1f9..a2ca2a9 100644 --- a/MbDCode/EulerParameters.h +++ b/MbDCode/EulerParameters.h @@ -16,10 +16,13 @@ namespace MbD { EulerParameters(std::initializer_list list) : EulerArray{ list } {} static std::shared_ptr>>> ppApEpEtimesColumn(FColDsptr col); + static std::shared_ptr> pCpEtimesColumn(FColDsptr col); + static std::shared_ptr> pCTpEtimesColumn(FColDsptr col); static std::shared_ptr>>> ppApEpEtimesMatrix(FMatDsptr mat); - void initialize(); - void calc(); + + void initialize() override; + void calc() override; void calcABC(); void calcpApE(); @@ -72,6 +75,70 @@ namespace MbD { return answer; } + template<> + inline std::shared_ptr> 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 std::shared_ptr> 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 std::shared_ptr>>> EulerParameters::ppApEpEtimesMatrix(FMatDsptr mat) { @@ -182,10 +249,10 @@ namespace MbD { 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 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; diff --git a/MbDCode/EulerParametersDDot.cpp b/MbDCode/EulerParametersDDot.cpp new file mode 100644 index 0000000..022dd5e --- /dev/null +++ b/MbDCode/EulerParametersDDot.cpp @@ -0,0 +1 @@ +#include "EulerParametersDDot.h" diff --git a/MbDCode/EulerParametersDDot.h b/MbDCode/EulerParametersDDot.h new file mode 100644 index 0000000..102f88a --- /dev/null +++ b/MbDCode/EulerParametersDDot.h @@ -0,0 +1,21 @@ +#pragma once + +#include "EulerArray.h" +#include "EulerParametersDot.h" + +namespace MbD { + + template + class EulerParametersDDot : public EulerArray + { + //qEdot aAddot aBddot aCddot + public: + EulerParametersDDot(int count) : EulerArray(count) {} + EulerParametersDDot(int count, const T& value) : EulerArray(count, value) {} + EulerParametersDDot(std::initializer_list list) : EulerArray{ list } {} + + //std::shared_ptr> qEdot; + FMatDsptr aAddot, aBddot, aCddot; + }; +} + diff --git a/MbDCode/EulerParametersDot.h b/MbDCode/EulerParametersDot.h index 90baf47..52cd3a1 100644 --- a/MbDCode/EulerParametersDot.h +++ b/MbDCode/EulerParametersDot.h @@ -4,7 +4,6 @@ #include "FullColumn.h" #include "FullMatrix.h" #include "EulerParameters.h" -#include "CREATE.h" namespace MbD { @@ -17,20 +16,23 @@ namespace MbD { EulerParametersDot(int count, const T& value) : EulerArray(count, value) {} EulerParametersDot(std::initializer_list list) : EulerArray{ list } {} static std::shared_ptr> FromqEOpAndOmegaOpO(std::shared_ptr> qe, FColDsptr omeOpO); + void initialize() override; void calcAdotBdotCdot(); - void calcpAdotpE(); + void calcpAdotpE(); + FMatDsptr aB(); std::shared_ptr> qE; FMatDsptr aAdot, aBdot, aCdot; FColFMatDsptr pAdotpE; - void calc(); + void calc() override; }; template inline std::shared_ptr> EulerParametersDot::FromqEOpAndOmegaOpO(std::shared_ptr> qEOp, FColDsptr omeOpO) { - auto answer = CREATE>::With(4); + auto answer = std::make_shared>(4); + answer->initialize(); //Cannot use CREATE.h in subclasses of std::vector. Why? qEOp->calcABC(); auto aB = qEOp->aB; answer->equalFullColumn(aB->transposeTimesFullColumn(omeOpO->times(0.5))); @@ -39,16 +41,133 @@ namespace MbD { return answer; } + template + inline void EulerParametersDot::initialize() + { + 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); + } + } + template inline void EulerParametersDot::calcAdotBdotCdot() { - assert(false); + //"aAdot, aBdot and aCdot are all calculated together and only here." + auto aE0dot = this->at(0); + auto aE1dot = this->at(1); + auto aE2dot = this->at(2); + auto aE3dot = this->at(3); + auto mE0dot = -aE0dot; + auto mE1dot = -aE1dot; + auto mE2dot = -aE2dot; + aBdot->at(0)->at(0) = aE3dot; + aBdot->at(0)->at(1) = mE2dot; + aBdot->at(0)->at(2) = aE1dot; + aBdot->at(0)->at(3) = mE0dot; + aBdot->at(1)->at(0) = aE2dot; + aBdot->at(1)->at(1) = aE3dot; + aBdot->at(1)->at(2) = mE0dot; + aBdot->at(1)->at(3) = mE1dot; + aBdot->at(2)->at(0) = mE1dot; + aBdot->at(2)->at(1) = aE0dot; + aBdot->at(2)->at(2) = aE3dot; + aBdot->at(2)->at(3) = mE2dot; + aCdot->at(0)->at(0) = aE3dot; + aCdot->at(0)->at(1) = aE2dot; + aCdot->at(0)->at(2) = mE1dot; + aCdot->at(0)->at(3) = mE0dot; + aCdot->at(1)->at(0) = mE2dot; + aCdot->at(1)->at(1) = aE3dot; + aCdot->at(1)->at(2) = aE0dot; + aCdot->at(1)->at(3) = mE1dot; + aCdot->at(2)->at(0) = aE1dot; + aCdot->at(2)->at(1) = mE0dot; + aCdot->at(2)->at(2) = aE3dot; + aCdot->at(2)->at(3) = mE2dot; + aAdot = this->aB()->timesTransposeFullMatrix(aCdot)->times(2.0); } template inline void EulerParametersDot::calcpAdotpE() { - assert(false); + //"Mimic calcpApE" + //"All aE's are actually aEdot's." + 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 = pAdotpE->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 = pAdotpE->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 = pAdotpE->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 = pAdotpE->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 FMatDsptr EulerParametersDot::aB() + { + return qE->aB; } template diff --git a/MbDCode/FullColumn.h b/MbDCode/FullColumn.h index 5082fc6..0407679 100644 --- a/MbDCode/FullColumn.h +++ b/MbDCode/FullColumn.h @@ -29,6 +29,8 @@ namespace MbD { void equalFullColumnAt(std::shared_ptr> fullCol, int i); std::shared_ptr> copy(); std::shared_ptr> transpose(); + void atiplusFullColumntimes(int i, std::shared_ptr> fullCol, T factor); + T transposeTimesFullColumn(const std::shared_ptr> fullCol); virtual std::ostream& printOn(std::ostream& s) const; friend std::ostream& operator<<(std::ostream& s, const FullColumn& fullCol) @@ -132,6 +134,20 @@ namespace MbD { return std::make_shared>(*this); } template + inline void FullColumn::atiplusFullColumntimes(int i1, std::shared_ptr> 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 std::shared_ptr> fullCol) + { + return this->dot(fullCol); + } + template inline std::ostream& FullColumn::printOn(std::ostream& s) const { s << "{"; diff --git a/MbDCode/FullMatrix.h b/MbDCode/FullMatrix.h index 5520628..0a29bd0 100644 --- a/MbDCode/FullMatrix.h +++ b/MbDCode/FullMatrix.h @@ -42,10 +42,13 @@ namespace MbD { std::shared_ptr> timesFullMatrix(std::shared_ptr> fullMat); std::shared_ptr> timesTransposeFullMatrix(std::shared_ptr> fullMat); std::shared_ptr> times(double a); + std::shared_ptr> transposeTimesFullMatrix(std::shared_ptr> fullMat); std::shared_ptr> plusFullMatrix(std::shared_ptr> fullMat); std::shared_ptr> transpose(); std::shared_ptr> negated(); void symLowerWithUpper(); + void atiput(int i, std::shared_ptr> fullRow); + void atijput(int i, int j, T value); void atijputFullColumn(int i, int j, std::shared_ptr> fullCol); void atijplusFullRow(int i, int j, std::shared_ptr> fullRow); void atijplusNumber(int i, int j, double value); @@ -53,7 +56,7 @@ namespace MbD { double sumOfSquares() override; void zeroSelf() override; std::shared_ptr> copy(); - std::shared_ptr> operator+(const std::shared_ptr> fullMat); + FullMatrix operator+(const FullMatrix fullMat); std::shared_ptr> transposeTimesFullColumn(const std::shared_ptr> fullCol); }; @@ -74,18 +77,6 @@ namespace MbD { return answer; } template - inline std::shared_ptr> FullMatrix::timesFullColumn(std::shared_ptr> fullCol) - { - //"a*b = a(i,j)b(j) sum 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)->timesFullColumn(fullCol); - } - return answer; - } - template inline std::shared_ptr> FullMatrix::timesFullMatrix(std::shared_ptr> fullMat) { int m = this->nrow(); @@ -116,6 +107,11 @@ namespace MbD { return answer; } template + inline std::shared_ptr> FullMatrix::transposeTimesFullMatrix(std::shared_ptr> fullMat) + { + return this->transpose()->timesFullMatrix(fullMat); + } + template inline std::shared_ptr> FullMatrix::plusFullMatrix(std::shared_ptr> fullMat) { int n = (int)this->size(); @@ -155,6 +151,16 @@ namespace MbD { } } template + inline void FullMatrix::atiput(int i, std::shared_ptr> 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, std::shared_ptr> fullCol) { for (int ii = 0; ii < fullCol->size(); ii++) @@ -219,19 +225,33 @@ namespace MbD { return answer; } template - inline std::shared_ptr> FullMatrix::operator+(const std::shared_ptr> fullMat) + inline FullMatrix FullMatrix::operator+(const FullMatrix fullMat) { - return this->plusFullMatrix(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 std::shared_ptr> FullMatrix::transposeTimesFullColumn(std::shared_ptr> fullCol) { auto sptr = std::make_shared>(*this); - //auto aaa = fullCol->transpose(); - //auto bbb = aaa->timesFullMatrix(sptr); - //auto ccc = bbb->transpose(); return fullCol->transpose()->timesFullMatrix(sptr)->transpose(); } + template + inline std::shared_ptr> FullMatrix::timesFullColumn(std::shared_ptr> fullCol) + { + //"a*b = a(i,j)b(j) sum j." + auto nrow = this->nrow(); + auto answer = std::make_shared>(nrow); + for (int i = 0; i < nrow; i++) + { + answer->at(i) = this->at(i)->timesFullColumn(fullCol); + } + return answer; + } using FMatDsptr = std::shared_ptr>; using FMatDsptr = std::shared_ptr>; using FMatFColDsptr = std::shared_ptr>>>; diff --git a/MbDCode/FullVector.h b/MbDCode/FullVector.h index a30aef6..577ef2c 100644 --- a/MbDCode/FullVector.h +++ b/MbDCode/FullVector.h @@ -13,12 +13,14 @@ namespace MbD { FullVector(std::vector::iterator begin, std::vector::iterator end) : Array(begin, end) {} FullVector(std::initializer_list list) : Array{ list } {} double dot(std::shared_ptr> vec); + void atiput(int i, T value); void atiplusNumber(int i, T value); void atiminusNumber(int i, T value); double sumOfSquares() override; int numberOfElements() override; void zeroSelf() override; void atitimes(int i, double factor); + void atiplusFullVector(int i, std::shared_ptr fullVec); void atiplusFullVectortimes(int i, std::shared_ptr fullVec, double factor); double maxMagnitude(); @@ -34,6 +36,11 @@ namespace MbD { return answer; } template + inline void FullVector::atiput(int i, T value) + { + this->at(i) = value; + } + template inline void FullVector::atiplusNumber(int i, T value) { this->at(i) += value; @@ -83,6 +90,15 @@ namespace MbD { this->at(i) *= factor; } template + inline void FullVector::atiplusFullVector(int i1, std::shared_ptr fullVec) + { + for (int ii = 0; ii < fullVec->size(); ii++) + { + auto i = i1 + ii; + this->at(i) += fullVec->at(ii); + } + } + template inline void FullVector::atiplusFullVectortimes(int i1, std::shared_ptr fullVec, double factor) { for (int ii = 0; ii < fullVec->size(); ii++) diff --git a/MbDCode/GEFullMatFullPv.cpp b/MbDCode/GEFullMatFullPv.cpp index b80e3f6..b6e2642 100644 --- a/MbDCode/GEFullMatFullPv.cpp +++ b/MbDCode/GEFullMatFullPv.cpp @@ -42,7 +42,7 @@ void MbD::GEFullMatFullPv::doPivoting(int p) colOrder->swapElems(p, pivotCol); } pivotValues->at(p) = max; - if (max < singularPivotTolerance) throw SingularMatrixError(""); + if (max < singularPivotTolerance) throwSingularMatrixError(""); } void MbD::GEFullMatFullPv::postSolve() diff --git a/MbDCode/GEFullMatParPv.cpp b/MbDCode/GEFullMatParPv.cpp index c94477f..80373cd 100644 --- a/MbDCode/GEFullMatParPv.cpp +++ b/MbDCode/GEFullMatParPv.cpp @@ -33,7 +33,7 @@ void MbD::GEFullMatParPv::doPivoting(int p) rowOrder->swapElems(p, rowPivot); } pivotValues->at(p) = max; - if (max < singularPivotTolerance) throw SingularMatrixError(""); + if (max < singularPivotTolerance) throwSingularMatrixError(""); } void MbD::GEFullMatParPv::postSolve() diff --git a/MbDCode/GESpMatFullPv.cpp b/MbDCode/GESpMatFullPv.cpp index 8386243..7cc5293 100644 --- a/MbDCode/GESpMatFullPv.cpp +++ b/MbDCode/GESpMatFullPv.cpp @@ -45,7 +45,7 @@ void MbD::GESpMatFullPv::doPivoting(int p) positionsOfOriginalCols->at(colOrder->at(pivotCol)) = pivotCol; } pivotValues->at(p) = max; - if (max < singularPivotTolerance) throw SingularMatrixError(""); + if (max < singularPivotTolerance) throwSingularMatrixError(""); auto jp = colOrder->at(p); rowPositionsOfNonZerosInPivotColumn = rowPositionsOfNonZerosInColumns->at(jp); if (rowPositionsOfNonZerosInPivotColumn->front() == p) { @@ -120,7 +120,7 @@ void MbD::GESpMatFullPv::backSubstituteIntoDU() } } auto ji = colOrder->at(i); - answerX->at(ji) = rightHandSideB->at(i) - (sum / duii); + answerX->at(ji) = (rightHandSideB->at(i) - sum) / duii; } } @@ -157,7 +157,7 @@ void MbD::GESpMatFullPv::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fu auto& spRowi = spMat->at(i); auto maxRowMagnitude = spRowi->maxMagnitude(); if (maxRowMagnitude == 0) { - throw SingularMatrixError(""); + throwSingularMatrixError(""); } matrixA->at(i) = spRowi->conditionedWithTol(singularPivotTolerance * maxRowMagnitude); rowOrder->at(i) = i; diff --git a/MbDCode/GESpMatFullPvPosIC.cpp b/MbDCode/GESpMatFullPvPosIC.cpp index 8bedddb..63bf58c 100644 --- a/MbDCode/GESpMatFullPvPosIC.cpp +++ b/MbDCode/GESpMatFullPvPosIC.cpp @@ -71,7 +71,7 @@ void MbD::GESpMatFullPvPosIC::doPivoting(int p) auto begin = rowOrder->begin() + p; auto end = rowOrder->begin() + pivotRowLimit; auto redundantEqnNos = std::make_shared>(begin, end); - throw SingularMatrixError("", redundantEqnNos); + throwSingularMatrixError("", redundantEqnNos); } else { pivotRowLimit = *itr; diff --git a/MbDCode/GESpMatParPv.cpp b/MbDCode/GESpMatParPv.cpp index 9aabea0..b6c687b 100644 --- a/MbDCode/GESpMatParPv.cpp +++ b/MbDCode/GESpMatParPv.cpp @@ -60,7 +60,7 @@ void MbD::GESpMatParPv::backSubstituteIntoDU() duii = keyValue.second; } } - answerX->at(i) = rightHandSideB->at(i) - (sum / duii); + answerX->at(i) = (rightHandSideB->at(i) - sum) / duii; } } diff --git a/MbDCode/GESpMatParPvMarko.cpp b/MbDCode/GESpMatParPvMarko.cpp index 597ae57..5bbbbbf 100644 --- a/MbDCode/GESpMatParPvMarko.cpp +++ b/MbDCode/GESpMatParPvMarko.cpp @@ -21,7 +21,7 @@ void MbD::GESpMatParPvMarko::doPivoting(int p) while (lookForFirstNonZeroInPivotCol) { spRowi = matrixA->at(i); if (spRowi->find(p) == spRowi->end()) { - if (i <= p) throw SingularMatrixError(""); + if (i <= p) throwSingularMatrixError(""); } else { markowitzPivotColCount = 0; @@ -64,10 +64,33 @@ void MbD::GESpMatParPvMarko::doPivoting(int p) rowScalings->swapElems(p, rowPivoti); if (aip != std::numeric_limits::min()) rowPositionsOfNonZerosInPivotColumn->at(markowitzPivotColCount - 1) = rowPivoti; } - if (max < singularPivotTolerance) throw SingularMatrixError(""); + if (max < singularPivotTolerance) throwSingularMatrixError(""); } void MbD::GESpMatParPvMarko::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) { - assert(false); + //"Optimized for speed." + if (m != spMat->nrow() || n != spMat->ncol()) { + m = spMat->nrow(); + n = spMat->ncol(); + matrixA = std::make_shared>(m); + rowScalings = std::make_shared>(m); + rowPositionsOfNonZerosInPivotColumn = std::make_shared>(); + } + if (saveOriginal) { + rightHandSideB = fullCol->copy(); + } + else { + rightHandSideB = fullCol; + } + for (int i = 0; i < m; i++) + { + auto& spRowi = spMat->at(i); + auto maxRowMagnitude = spRowi->maxMagnitude(); + if (maxRowMagnitude == 0) { + throwSingularMatrixError(""); + } + rowScalings->atiput(i, 1.0 / maxRowMagnitude); + matrixA->atiput(i, spRowi->conditionedWithTol(singularPivotTolerance * maxRowMagnitude)); + } } diff --git a/MbDCode/GESpMatParPvMarkoFast.cpp b/MbDCode/GESpMatParPvMarkoFast.cpp index 8b9b80e..19cce65 100644 --- a/MbDCode/GESpMatParPvMarkoFast.cpp +++ b/MbDCode/GESpMatParPvMarkoFast.cpp @@ -26,9 +26,7 @@ void MbD::GESpMatParPvMarkoFast::preSolvewithsaveOriginal(SpMatDsptr spMat, FCol { auto& spRowi = spMat->at(i); auto maxRowMagnitude = spRowi->maxMagnitude(); - if (maxRowMagnitude == 0) { - throw SingularMatrixError(""); - } + if (maxRowMagnitude == 0) throwSingularMatrixError(""); auto scaling = 1.0 / maxRowMagnitude; matrixA->at(i) = spRowi->timesconditionedWithTol(scaling, singularPivotTolerance); rightHandSideB->atitimes(i, scaling); @@ -54,7 +52,7 @@ void MbD::GESpMatParPvMarkoFast::doPivoting(int p) while (lookForFirstNonZeroInPivotCol) { spRowi = matrixA->at(i); if (spRowi->find(p) == spRowi->end()) { - if (i <= p) throw SingularMatrixError(""); + if (i <= p) throwSingularMatrixError(""); } else { markowitzPivotColCount = 0; @@ -94,5 +92,5 @@ void MbD::GESpMatParPvMarkoFast::doPivoting(int p) rightHandSideB->swapElems(p, rowPivoti); if (aip != std::numeric_limits::min()) rowPositionsOfNonZerosInPivotColumn->at(markowitzPivotColCount - 1) = rowPivoti; } - if (max < singularPivotTolerance) throw SingularMatrixError(""); + if (max < singularPivotTolerance) throwSingularMatrixError(""); } diff --git a/MbDCode/GESpMatParPvPrecise.cpp b/MbDCode/GESpMatParPvPrecise.cpp index 05a3c43..d1bfd29 100644 --- a/MbDCode/GESpMatParPvPrecise.cpp +++ b/MbDCode/GESpMatParPvPrecise.cpp @@ -21,7 +21,7 @@ void MbD::GESpMatParPvPrecise::doPivoting(int p) while (lookForFirstNonZeroInPivotCol) { spRowi = matrixA->at(i); if (spRowi->find(p) == spRowi->end()) { - if (i <= p) throw SingularMatrixError(""); + if (i <= p) throwSingularMatrixError(""); } else { markowitzPivotColCount = 0; @@ -63,7 +63,7 @@ void MbD::GESpMatParPvPrecise::doPivoting(int p) if (aip != std::numeric_limits::min()) rowPositionsOfNonZerosInPivotColumn->at(markowitzPivotColCount - 1) = rowPivoti; } pivotValues->at(p) = max; - if (max < singularPivotTolerance) throw SingularMatrixError(""); + if (max < singularPivotTolerance) throwSingularMatrixError(""); } void MbD::GESpMatParPvPrecise::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) @@ -89,7 +89,7 @@ void MbD::GESpMatParPvPrecise::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDs { auto& spRowi = spMat->at(i); auto maxRowMagnitude = spRowi->maxMagnitude(); - if (maxRowMagnitude == 0) throw SingularMatrixError(""); + if (maxRowMagnitude == 0) throwSingularMatrixError(""); rowScalings->at(i) = 1.0 / maxRowMagnitude; matrixA->at(i) = spRowi->conditionedWithTol(singularPivotTolerance * maxRowMagnitude); rowOrder->at(i) = i; diff --git a/MbDCode/Item.cpp b/MbDCode/Item.cpp index bc683c0..3afcd6a 100644 --- a/MbDCode/Item.cpp +++ b/MbDCode/Item.cpp @@ -155,11 +155,11 @@ void MbD::Item::fillqsudotWeights(std::shared_ptr> diagMa { } -void MbD::Item::fillVelICError(FColDsptr error) +void MbD::Item::fillVelICError(FColDsptr col) { } -void MbD::Item::fillVelICJacob(SpMatDsptr jacob) +void MbD::Item::fillVelICJacob(SpMatDsptr mat) { } @@ -167,6 +167,35 @@ void MbD::Item::setqsudotlam(FColDsptr qsudotlam) { } +void MbD::Item::preAccIC() +{ + this->calcPostDynCorrectorIteration(); +} + +void MbD::Item::postAccIC() +{ +} + +void MbD::Item::postAccICIteration() +{ +} + +void MbD::Item::fillqsuddotlam(FColDsptr col) +{ +} + +void MbD::Item::fillAccICIterError(FColDsptr col) +{ +} + +void MbD::Item::fillAccICIterJacob(SpMatDsptr mat) +{ +} + +void MbD::Item::setqsuddotlam(FColDsptr qsudotlam) +{ +} + void MbD::Item::constraintsReport() { } diff --git a/MbDCode/Item.h b/MbDCode/Item.h index 67dddaa..7203a78 100644 --- a/MbDCode/Item.h +++ b/MbDCode/Item.h @@ -58,10 +58,18 @@ namespace MbD { virtual void postVelIC(); virtual void fillqsudot(FColDsptr col); virtual void fillqsudotWeights(std::shared_ptr> diagMat); - virtual void fillVelICError(FColDsptr error); - virtual void fillVelICJacob(SpMatDsptr jacob); + virtual void fillVelICError(FColDsptr col); + virtual void fillVelICJacob(SpMatDsptr mat); virtual void setqsudotlam(FColDsptr qsudotlam); + virtual void preAccIC(); + virtual void postAccIC(); + virtual void postAccICIteration(); + virtual void fillqsuddotlam(FColDsptr col); + virtual void fillAccICIterError(FColDsptr col); + virtual void fillAccICIterJacob(SpMatDsptr mat); + virtual void setqsuddotlam(FColDsptr qsudotlam); + void setName(std::string& str); const std::string& getName() const; diff --git a/MbDCode/Joint.cpp b/MbDCode/Joint.cpp index d153bd4..98f8f44 100644 --- a/MbDCode/Joint.cpp +++ b/MbDCode/Joint.cpp @@ -123,6 +123,11 @@ void MbD::Joint::setqsulam(FColDsptr col) constraintsDo([&](std::shared_ptr con) { con->setqsulam(col); }); } +void MbD::Joint::setqsudotlam(FColDsptr col) +{ + constraintsDo([&](std::shared_ptr con) { con->setqsudotlam(col); }); +} + void MbD::Joint::postPosICIteration() { constraintsDo([](std::shared_ptr constraint) { constraint->postPosICIteration(); }); @@ -216,3 +221,33 @@ void MbD::Joint::preVelIC() { constraintsDo([](std::shared_ptr constraint) { constraint->preVelIC(); }); } + +void MbD::Joint::fillVelICError(FColDsptr col) +{ + constraintsDo([&](std::shared_ptr con) { con->fillVelICError(col); }); +} + +void MbD::Joint::fillVelICJacob(SpMatDsptr mat) +{ + constraintsDo([&](std::shared_ptr constraint) { constraint->fillVelICJacob(mat); }); +} + +void MbD::Joint::preAccIC() +{ + constraintsDo([](std::shared_ptr constraint) { constraint->preAccIC(); }); +} + +void MbD::Joint::fillAccICIterError(FColDsptr col) +{ + constraintsDo([&](std::shared_ptr con) { con->fillAccICIterError(col); }); +} + +void MbD::Joint::fillAccICIterJacob(SpMatDsptr mat) +{ + constraintsDo([&](std::shared_ptr con) { con->fillAccICIterJacob(mat); }); +} + +void MbD::Joint::setqsuddotlam(FColDsptr qsudotlam) +{ + constraintsDo([&](std::shared_ptr con) { con->setqsuddotlam(qsudotlam); }); +} diff --git a/MbDCode/Joint.h b/MbDCode/Joint.h index 47914d5..5748ff7 100644 --- a/MbDCode/Joint.h +++ b/MbDCode/Joint.h @@ -15,7 +15,7 @@ namespace MbD { public: Joint(); Joint(const char* str); - void initialize(); + void initialize() override; virtual void connectsItoJ(EndFrmcptr frmI, EndFrmcptr frmJ); void initializeLocally() override; void initializeGlobally() override; @@ -34,6 +34,7 @@ namespace MbD { void fillqsudotWeights(std::shared_ptr> diagMat) override; void useEquationNumbers() override; void setqsulam(FColDsptr col) override; + void setqsudotlam(FColDsptr col) override; void postPosICIteration() override; void fillPosICError(FColDsptr col) override; void fillPosICJacob(SpMatDsptr mat) override; @@ -46,6 +47,12 @@ namespace MbD { void fillPosKineError(FColDsptr col) override; void fillPosKineJacob(SpMatDsptr mat) override; void preVelIC() override; + void fillVelICError(FColDsptr col) override; + void fillVelICJacob(SpMatDsptr mat) override; + void preAccIC() override; + void fillAccICIterError(FColDsptr col) override; + void fillAccICIterJacob(SpMatDsptr mat) override; + void setqsuddotlam(FColDsptr qsudotlam) override; EndFrmcptr frmI; EndFrmcptr frmJ; diff --git a/MbDCode/LDUFullMatParPv.cpp b/MbDCode/LDUFullMatParPv.cpp index 06fa0ef..3710ded 100644 --- a/MbDCode/LDUFullMatParPv.cpp +++ b/MbDCode/LDUFullMatParPv.cpp @@ -31,5 +31,5 @@ void MbD::LDUFullMatParPv::doPivoting(int p) rowOrder->swapElems(p, rowPivot); } pivotValues->at(p) = max; - if (max < singularPivotTolerance) throw SingularMatrixError(""); + if (max < singularPivotTolerance) throwSingularMatrixError(""); } diff --git a/MbDCode/LDUSpMatParPvMarko.cpp b/MbDCode/LDUSpMatParPvMarko.cpp index c5a8b6c..94e5311 100644 --- a/MbDCode/LDUSpMatParPvMarko.cpp +++ b/MbDCode/LDUSpMatParPvMarko.cpp @@ -17,7 +17,7 @@ void MbD::LDUSpMatParPvMarko::doPivoting(int p) while (lookForFirstNonZeroInPivotCol) { spRowi = matrixA->at(i); if (spRowi->find(p) == spRowi->end()) { - if (i <= p) throw SingularMatrixError(""); + if (i <= p) throwSingularMatrixError(""); } else { markowitzPivotColCount = 0; @@ -61,5 +61,5 @@ void MbD::LDUSpMatParPvMarko::doPivoting(int p) matrixL->swapElems(p, rowPivoti); if (aip != std::numeric_limits::min()) rowPositionsOfNonZerosInPivotColumn->at(markowitzPivotColCount - 1) = rowPivoti; } - if (max < singularPivotTolerance) throw SingularMatrixError(""); + if (max < singularPivotTolerance) throwSingularMatrixError(""); } diff --git a/MbDCode/LDUSpMatParPvPrecise.cpp b/MbDCode/LDUSpMatParPvPrecise.cpp index 0888617..dec0961 100644 --- a/MbDCode/LDUSpMatParPvPrecise.cpp +++ b/MbDCode/LDUSpMatParPvPrecise.cpp @@ -17,7 +17,7 @@ void MbD::LDUSpMatParPvPrecise::doPivoting(int p) while (lookForFirstNonZeroInPivotCol) { spRowi = matrixA->at(i); if (spRowi->find(p) == spRowi->end()) { - if (i <= p) throw SingularMatrixError(""); + if (i <= p) throwSingularMatrixError(""); } else { markowitzPivotColCount = 0; @@ -59,5 +59,5 @@ void MbD::LDUSpMatParPvPrecise::doPivoting(int p) if (aip != std::numeric_limits::min()) rowPositionsOfNonZerosInPivotColumn->at(markowitzPivotColCount - 1) = rowPivoti; } pivotValues->at(p) = max; - if (max < singularPivotTolerance) throw SingularMatrixError(""); + if (max < singularPivotTolerance) throwSingularMatrixError(""); } diff --git a/MbDCode/MarkerFrame.cpp b/MbDCode/MarkerFrame.cpp index 3ac0792..5618e8a 100644 --- a/MbDCode/MarkerFrame.cpp +++ b/MbDCode/MarkerFrame.cpp @@ -123,8 +123,14 @@ void MbD::MarkerFrame::setqsulam(FColDsptr col) endFramesDo([&](const std::shared_ptr& endFrame) { endFrame->setqsulam(col); }); } +void MbD::MarkerFrame::setqsudotlam(FColDsptr col) +{ + endFramesDo([&](const std::shared_ptr& endFrame) { endFrame->setqsudotlam(col); }); +} + void MbD::MarkerFrame::postPosICIteration() { + Item::postPosICIteration(); endFramesDo([](std::shared_ptr endFrame) { endFrame->postPosICIteration(); }); } @@ -149,6 +155,42 @@ void MbD::MarkerFrame::preVelIC() endFramesDo([](std::shared_ptr endFrame) { endFrame->preVelIC(); }); } +void MbD::MarkerFrame::postVelIC() +{ + endFramesDo([](std::shared_ptr endFrame) { endFrame->postVelIC(); }); +} + +void MbD::MarkerFrame::preAccIC() +{ + Item::preAccIC(); + endFramesDo([](std::shared_ptr endFrame) { endFrame->preAccIC(); }); +} + +FColDsptr MbD::MarkerFrame::qXdot() +{ + return partFrame->qXdot; +} + +std::shared_ptr> MbD::MarkerFrame::qEdot() +{ + return partFrame->qEdot; +} + +FColDsptr MbD::MarkerFrame::qXddot() +{ + return partFrame->qXddot; +} + +FColDsptr MbD::MarkerFrame::qEddot() +{ + return partFrame->qEddot; +} + +void MbD::MarkerFrame::setqsuddotlam(FColDsptr qsudotlam) +{ + endFramesDo([&](const std::shared_ptr& endFrame) { endFrame->setqsuddotlam(qsudotlam); }); +} + void MarkerFrame::setPartFrame(PartFrame* partFrm) { partFrame = partFrm; diff --git a/MbDCode/MarkerFrame.h b/MbDCode/MarkerFrame.h index 606972d..64c7f43 100644 --- a/MbDCode/MarkerFrame.h +++ b/MbDCode/MarkerFrame.h @@ -7,6 +7,8 @@ #include "FullColumn.h" #include "FullMatrix.h" #include "EndFramec.h" +#include "EulerParametersDot.h" +#include "EulerParametersDDot.h" namespace MbD { class PartFrame; @@ -19,7 +21,7 @@ namespace MbD { public: MarkerFrame(); MarkerFrame(const char* str); - void initialize(); + void initialize() override; void setPartFrame(PartFrame* partFrm); PartFrame* getPartFrame(); void setrpmp(FColDsptr x); @@ -40,11 +42,19 @@ namespace MbD { void fillqsudotWeights(std::shared_ptr> diagMat) override; void setqsu(FColDsptr col) override; void setqsulam(FColDsptr col) override; + void setqsudotlam(FColDsptr col) override; void postPosICIteration() override; void postPosIC() override; void preDyn() override; void storeDynState() override; void preVelIC() override; + void postVelIC() override; + void preAccIC() override; + FColDsptr qXdot(); + std::shared_ptr> qEdot(); + FColDsptr qXddot(); + FColDsptr qEddot(); + void setqsuddotlam(FColDsptr qsudotlam) override; PartFrame* partFrame; //Use raw pointer when pointing backwards. FColDsptr rpmp = std::make_shared>(3); diff --git a/MbDCode/MatrixSolver.cpp b/MbDCode/MatrixSolver.cpp index c1b6f12..48b91a3 100644 --- a/MbDCode/MatrixSolver.cpp +++ b/MbDCode/MatrixSolver.cpp @@ -57,7 +57,17 @@ void MbD::MatrixSolver::findScalingsForRowRange(int begin, int end) for (int i = begin; i < end; i++) { auto maxRowMagnitude = this->getmatrixArowimaxMagnitude(i); - if (maxRowMagnitude == 0.0) throw SingularMatrixError(""); + if (maxRowMagnitude == 0.0) throwSingularMatrixError(""); rowScalings->at(i) = 1.0 / maxRowMagnitude; } } + +void MbD::MatrixSolver::throwSingularMatrixError(const char* chars) +{ + throw SingularMatrixError(chars); +} + +void MbD::MatrixSolver::throwSingularMatrixError(const char* chars, std::shared_ptr> redunEqnNos) +{ + throw SingularMatrixError(chars, redunEqnNos); +} diff --git a/MbDCode/MatrixSolver.h b/MbDCode/MatrixSolver.h index ff2278c..2a5aad5 100644 --- a/MbDCode/MatrixSolver.h +++ b/MbDCode/MatrixSolver.h @@ -29,6 +29,8 @@ namespace MbD { virtual void postSolve() = 0; virtual void findScalingsForRowRange(int begin, int end); virtual double getmatrixArowimaxMagnitude(int i) = 0; + void throwSingularMatrixError(const char* chars); + void throwSingularMatrixError(const char* chars, std::shared_ptr> redunEqnNos); int m = 0, n = 0; FColDsptr answerX, rightHandSideB, rowScalings, pivotValues; diff --git a/MbDCode/MbDCode.cpp b/MbDCode/MbDCode.cpp index 9949c09..485e16d 100644 --- a/MbDCode/MbDCode.cpp +++ b/MbDCode/MbDCode.cpp @@ -25,10 +25,259 @@ #include "MbDCode.h" #include "Time.h" #include "CREATE.h" +#include "GESpMatParPvMarkoFast.h" +#include "GESpMatParPvPrecise.h" using namespace MbD; +void runSpMat(); +void runPiston(); +void runOndselPiston(); int main() +{ + //runSpMat(); + //runPiston(); + runOndselPiston(); +} + +void runOndselPiston() +{ + //Piston with easy input numbers for exact port from Smalltalk + std::cout << "Hello World!\n"; + System& TheSystem = System::getInstance(); + std::string name = "TheSystem"; + TheSystem.setName(name); + std::cout << "TheSystem.getName() " << TheSystem.getName() << std::endl; + auto& systemSolver = TheSystem.systemSolver; + systemSolver->errorTolPosKine = 1.0e-6; + systemSolver->errorTolAccKine = 1.0e-6; + systemSolver->iterMaxPosKine = 25; + systemSolver->iterMaxAccKine = 25; + systemSolver->tstart = 0; + systemSolver->tend = 25.0; + systemSolver->hmin = 2.5e-8; + systemSolver->hmax = 25.0; + systemSolver->hout = 1.0; + systemSolver->corAbsTol = 1.0e-6; + systemSolver->corRelTol = 1.0e-6; + systemSolver->intAbsTol = 1.0e-6; + systemSolver->intRelTol = 1.0e-6; + systemSolver->iterMaxDyn = 4; + systemSolver->orderMax = 5; + systemSolver->translationLimit = 9.6058421285615e9; + systemSolver->rotationLimit = 0.5; + + std::string str; + FColDsptr qX, qE, qXdot, omeOpO, qXddot, qEddot; + FColDsptr rpmp; + FMatDsptr aApm; + FRowDsptr fullRow; + // + auto assembly1 = CREATE::With("/Assembly1"); + std::cout << "assembly1->getName() " << assembly1->getName() << std::endl; + assembly1->m = 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); + assembly1->setqE(qE); + qXdot = std::make_shared>(ListD{ 0, 0, 0 }); + omeOpO = std::make_shared>(ListD{ 0, 0, 0 }); + assembly1->setqXdot(qXdot); + assembly1->setomeOpO(omeOpO); + qXddot = std::make_shared>(ListD{ 0, 0, 0 }); + qEddot = std::make_shared>(ListD{ 0, 0, 0, 0 }); + assembly1->setqXddot(qXddot); + assembly1->setqEddot(qEddot); + std::cout << "assembly1->getqX() " << *assembly1->getqX() << std::endl; + std::cout << "assembly1->getqE() " << *assembly1->getqE() << std::endl; + TheSystem.addPart(assembly1); + { + auto& partFrame = assembly1->partFrame; + 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{ + { 1, 0, 0 }, + { 0, 1, 0 }, + { 0, 0, 1 } + }); + marker2->setaApm(aApm); + partFrame->addMarkerFrame(marker2); + // + 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{ + { 1, 0, 0 }, + { 0, 0, 1 }, + { 0, -1, 0 } + }); + marker1->setaApm(aApm); + partFrame->addMarkerFrame(marker1); + } + assembly1->asFixed(); + // + auto crankPart1 = CREATE::With("/Assembly1/Part1"); + std::cout << "crankPart1->getName() " << crankPart1->getName() << std::endl; + crankPart1->m = 1.0; + 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); + crankPart1->setqE(qE); + qXdot = std::make_shared>(ListD{ 0, 0, 0 }); + omeOpO = std::make_shared>(ListD{ 0, 0, 0 }); + crankPart1->setqXdot(qXdot); + crankPart1->setomeOpO(omeOpO); + qXddot = std::make_shared>(ListD{ 0, 0, 0 }); + qEddot = std::make_shared>(ListD{ 0, 0, 0, 0 }); + crankPart1->setqXddot(qXddot); + crankPart1->setqEddot(qEddot); + TheSystem.parts->push_back(crankPart1); + { + auto& partFrame = crankPart1->partFrame; + 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{ + { 1, 0, 0 }, + { 0, 1, 0 }, + { 0, 0, 1 } + }); + marker1->setaApm(aApm); + partFrame->addMarkerFrame(marker1); + // + 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{ + { 1, 0, 0 }, + { 0, 1, 0 }, + { 0, 0, 1 } + }); + marker2->setaApm(aApm); + partFrame->addMarkerFrame(marker2); + } + // + auto conrodPart2 = CREATE::With("/Assembly1/Part2"); + std::cout << "conrodPart2->getName() " << conrodPart2->getName() << std::endl; + conrodPart2->m = 1.0; + 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); + conrodPart2->setqE(qE); + qXdot = std::make_shared>(ListD{ 0, 0, 0 }); + omeOpO = std::make_shared>(ListD{ 0, 0, 0 }); + conrodPart2->setqXdot(qXdot); + conrodPart2->setomeOpO(omeOpO); + qXddot = std::make_shared>(ListD{ 0, 0, 0 }); + qEddot = std::make_shared>(ListD{ 0, 0, 0, 0 }); + conrodPart2->setqXddot(qXddot); + conrodPart2->setqEddot(qEddot); + TheSystem.parts->push_back(conrodPart2); + { + auto& partFrame = conrodPart2->partFrame; + 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{ + {1.0, 0.0, 0.0}, + {0.0, 1.0, 0.0}, + {0.0, 0.0, 1.0} + }); + marker1->setaApm(aApm); + partFrame->addMarkerFrame(marker1); + // + 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{ + {1.0, 0.0, 0.0}, + {0.0, 1.0, 0.0}, + {0.0, 0.0, 1.0} + }); + marker2->setaApm(aApm); + partFrame->addMarkerFrame(marker2); + } + // + auto pistonPart3 = CREATE::With("/Assembly1/Part3"); + std::cout << "pistonPart3->getName() " << pistonPart3->getName() << std::endl; + pistonPart3->m = 1.0; + 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); + pistonPart3->setqE(qE); + qXdot = std::make_shared>(ListD{ 0, 0, 0 }); + omeOpO = std::make_shared>(ListD{ 0, 0, 0 }); + pistonPart3->setqXdot(qXdot); + pistonPart3->setomeOpO(omeOpO); + qXddot = std::make_shared>(ListD{ 0, 0, 0 }); + qEddot = std::make_shared>(ListD{ 0, 0, 0, 0 }); + pistonPart3->setqXddot(qXddot); + pistonPart3->setqEddot(qEddot); + TheSystem.parts->push_back(pistonPart3); + { + auto& partFrame = pistonPart3->partFrame; + 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{ + {0.0, 1.0, 0.0}, + {1.0, 0.0, 0.0}, + {0.0, 0.0, -1.0} + }); + marker1->setaApm(aApm); + partFrame->addMarkerFrame(marker1); + // + 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{ + {0.0, 0.0, 1.0}, + {1.0, 0.0, 0.0}, + {0.0, 1.0, 0.0} + }); + marker2->setaApm(aApm); + partFrame->addMarkerFrame(marker2); + } + // + auto revJoint1 = CREATE::With("/Assembly1/Joint1"); + std::cout << "revJoint1->getName() " << revJoint1->getName() << std::endl; + revJoint1->connectsItoJ(assembly1->partFrame->endFrame("/Assembly1/Marker2"), crankPart1->partFrame->endFrame("/Assembly1/Part1/Marker1")); + TheSystem.jointsMotions->push_back(revJoint1); + + auto revJoint2 = CREATE::With("/Assembly1/Joint2"); + std::cout << "revJoint2->getName() " << revJoint2->getName() << std::endl; + revJoint2->connectsItoJ(crankPart1->partFrame->endFrame("/Assembly1/Part1/Marker2"), conrodPart2->partFrame->endFrame("/Assembly1/Part2/Marker1")); + TheSystem.jointsMotions->push_back(revJoint2); + + auto revJoint3 = CREATE::With("/Assembly1/Joint3"); + std::cout << "revJoint3->getName() " << revJoint3->getName() << std::endl; + revJoint3->connectsItoJ(conrodPart2->partFrame->endFrame("/Assembly1/Part2/Marker2"), pistonPart3->partFrame->endFrame("/Assembly1/Part3/Marker1")); + TheSystem.jointsMotions->push_back(revJoint3); + + auto cylJoint4 = CREATE::With("/Assembly1/Joint4"); + std::cout << "cylJoint4->getName() " << cylJoint4->getName() << std::endl; + cylJoint4->connectsItoJ(pistonPart3->partFrame->endFrame("/Assembly1/Part3/Marker2"), assembly1->partFrame->endFrame("/Assembly1/Marker1")); + TheSystem.jointsMotions->push_back(cylJoint4); + + auto rotMotion1 = CREATE::With("/Assembly1/Motion1"); + rotMotion1->connectsItoJ(assembly1->partFrame->endFrame("/Assembly1/Marker2"), crankPart1->partFrame->endFrame("/Assembly1/Part1/Marker1")); + std::cout << "rotMotion1->getName() " << rotMotion1->getName() << std::endl; + auto omega = std::make_shared(6.2831853071796); + auto timeScale = std::make_shared(1.0); + auto time = std::make_shared(timeScale, TheSystem.time); + rotMotion1->phiBlk = std::make_shared(omega, time); + std::cout << "rotMotion1->phiBlk " << *(rotMotion1->phiBlk) << std::endl; + TheSystem.jointsMotions->push_back(rotMotion1); + // + TheSystem.runKINEMATICS(); +} + +void runPiston() { std::cout << "Hello World!\n"; System& TheSystem = System::getInstance(); @@ -73,9 +322,9 @@ int main() assembly1->setqXdot(qXdot); assembly1->setomeOpO(omeOpO); qXddot = std::make_shared>(ListD{ 0, 0, 0 }); - qEddot = std::make_shared>(ListD{ 0, 0, 0 }); - assembly1->setqXddot(qXdot); - assembly1->setqEddot(omeOpO); + qEddot = std::make_shared>(ListD{ 0, 0, 0, 0 }); + assembly1->setqXddot(qXddot); + assembly1->setqEddot(qEddot); std::cout << "assembly1->getqX() " << *assembly1->getqX() << std::endl; std::cout << "assembly1->getqE() " << *assembly1->getqE() << std::endl; TheSystem.addPart(assembly1); @@ -118,9 +367,9 @@ int main() crankPart1->setqXdot(qXdot); crankPart1->setomeOpO(omeOpO); qXddot = std::make_shared>(ListD{ 0, 0, 0 }); - qEddot = std::make_shared>(ListD{ 0, 0, 0 }); - crankPart1->setqXddot(qXdot); - crankPart1->setqEddot(omeOpO); + qEddot = std::make_shared>(ListD{ 0, 0, 0, 0 }); + crankPart1->setqXddot(qXddot); + crankPart1->setqEddot(qEddot); TheSystem.parts->push_back(crankPart1); { auto& partFrame = crankPart1->partFrame; @@ -157,12 +406,12 @@ int main() conrodPart2->setqE(qE); qXdot = std::make_shared>(ListD{ 0, 0.19313691560085, 0 }); omeOpO = std::make_shared>(ListD{ 1.670970041317e-34, 1.3045598281729e-34, -1.2731200314796e-35 }); - crankPart1->setqXdot(qXdot); - crankPart1->setomeOpO(omeOpO); + conrodPart2->setqXdot(qXdot); + conrodPart2->setomeOpO(omeOpO); qXddot = std::make_shared>(ListD{ 0, 0, 0 }); - qEddot = std::make_shared>(ListD{ 0, 0, 0 }); - crankPart1->setqXddot(qXdot); - crankPart1->setqEddot(omeOpO); + qEddot = std::make_shared>(ListD{ 0, 0, 0, 0 }); + conrodPart2->setqXddot(qXddot); + conrodPart2->setqEddot(qEddot); TheSystem.parts->push_back(conrodPart2); { auto& partFrame = conrodPart2->partFrame; @@ -172,7 +421,7 @@ int main() 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} + {0.0, 0.0, 1.0} }); marker1->setaApm(aApm); partFrame->addMarkerFrame(marker1); @@ -183,7 +432,7 @@ int main() 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} + {-2.2204460492503e-16, -4.1633363423443e-17, 1.0} }); marker2->setaApm(aApm); partFrame->addMarkerFrame(marker2); @@ -199,12 +448,12 @@ int main() pistonPart3->setqE(qE); qXdot = std::make_shared>(ListD{ -6.3364526821409e-32, 0.19313691560085, -1.933731897626e-34 }); omeOpO = std::make_shared>(ListD{ 1.670970041317e-34, 1.3045598281729e-34, 1.8896472173894e-50 }); - crankPart1->setqXdot(qXdot); - crankPart1->setomeOpO(omeOpO); + pistonPart3->setqXdot(qXdot); + pistonPart3->setomeOpO(omeOpO); qXddot = std::make_shared>(ListD{ 0, 0, 0 }); - qEddot = std::make_shared>(ListD{ 0, 0, 0 }); - crankPart1->setqXddot(qXdot); - crankPart1->setqEddot(omeOpO); + qEddot = std::make_shared>(ListD{ 0, 0, 0, 0 }); + pistonPart3->setqXddot(qXddot); + pistonPart3->setqEddot(qEddot); TheSystem.parts->push_back(pistonPart3); { auto& partFrame = pistonPart3->partFrame; @@ -214,7 +463,7 @@ int main() 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} + {-1.0785207688569e-32, 2.2204460492503e-16, -1.0} }); marker1->setaApm(aApm); partFrame->addMarkerFrame(marker1); @@ -225,7 +474,7 @@ int main() 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} + {-6.9388939039072e-18, 1.0, -7.4837411882581e-50} }); marker2->setaApm(aApm); partFrame->addMarkerFrame(marker2); @@ -240,7 +489,7 @@ int main() std::cout << "revJoint2->getName() " << revJoint2->getName() << std::endl; revJoint2->connectsItoJ(crankPart1->partFrame->endFrame("/Assembly1/Part1/Marker2"), conrodPart2->partFrame->endFrame("/Assembly1/Part2/Marker1")); TheSystem.jointsMotions->push_back(revJoint2); - + auto revJoint3 = CREATE::With("/Assembly1/Joint3"); std::cout << "revJoint3->getName() " << revJoint3->getName() << std::endl; revJoint3->connectsItoJ(conrodPart2->partFrame->endFrame("/Assembly1/Part2/Marker2"), pistonPart3->partFrame->endFrame("/Assembly1/Part3/Marker1")); @@ -262,4 +511,22 @@ int main() TheSystem.jointsMotions->push_back(rotMotion1); // TheSystem.runKINEMATICS(); +} + +void runSpMat() { + auto spMat = std::make_shared>(3, 3); + spMat->atijput(0, 0, 1.0); + spMat->atijput(0, 1, 1.0); + spMat->atijput(1, 0, 1.0); + spMat->atijput(1, 1, 1.0); + spMat->atijput(1, 2, 1.0); + spMat->atijput(2, 1, 1.0); + spMat->atijput(2, 2, 1.0); + auto fullCol = std::make_shared>(3); + fullCol->atiput(0, 1.0); + fullCol->atiput(1, 2.0); + fullCol->atiput(2, 3.0); + auto matSolver = CREATE::With(); + auto answer = matSolver->solvewithsaveOriginal(spMat, fullCol, true); + auto aAx = spMat->timesFullColumn(answer); } \ No newline at end of file diff --git a/MbDCode/MbDCode.vcxproj b/MbDCode/MbDCode.vcxproj index a40c891..b3cca92 100644 --- a/MbDCode/MbDCode.vcxproj +++ b/MbDCode/MbDCode.vcxproj @@ -128,6 +128,10 @@ + + + + @@ -173,6 +177,7 @@ + @@ -262,6 +267,10 @@ + + + + @@ -308,6 +317,7 @@ + diff --git a/MbDCode/MbDCode.vcxproj.filters b/MbDCode/MbDCode.vcxproj.filters index 953ceb1..616b719 100644 --- a/MbDCode/MbDCode.vcxproj.filters +++ b/MbDCode/MbDCode.vcxproj.filters @@ -411,6 +411,21 @@ Source Files + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + @@ -818,6 +833,21 @@ Header Files + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + diff --git a/MbDCode/NewtonRaphson.h b/MbDCode/NewtonRaphson.h index d28b594..38f23ae 100644 --- a/MbDCode/NewtonRaphson.h +++ b/MbDCode/NewtonRaphson.h @@ -16,7 +16,7 @@ namespace MbD { { //system xold x dx dxNorm dxNorms dxTol y yNorm yNormOld yNorms yNormTol pypx iterNo iterMax nDivergence nBackTracking twoAlp lam public: - void initialize(); + void initialize() override; void initializeLocally() override; void run() override; void setSystem(Solver* sys) override; @@ -35,7 +35,7 @@ namespace MbD { virtual void passRootToSystem() = 0; bool isConvergedToNumericalLimit(); void calcDXNormImproveRootCalcYNorm(); - virtual void postRun(); + void postRun() override; SystemSolver* system; //Use raw pointer when pointing backwards. std::shared_ptr> dxNorms, yNorms; diff --git a/MbDCode/Part.cpp b/MbDCode/Part.cpp index 63ba80d..95cb5b4 100644 --- a/MbDCode/Part.cpp +++ b/MbDCode/Part.cpp @@ -3,6 +3,8 @@ #include "System.h" #include "CREATE.h" #include "DiagonalMatrix.h" +#include "EulerParameters.h" + using namespace MbD; @@ -24,9 +26,12 @@ void Part::initialize() void Part::initializeLocally() { partFrame->initializeLocally(); - //mX: = m > 0 - //ifTrue: [StMDiagonalMatrix new:3 withAll : m] - //ifFalse : [StMDiagonalMatrix new:3 withAll : 0.0d] + if (m > 0) { + mX = std::make_shared>(3, m); + } + else { + mX = std::make_shared>(3, 0.0); + } } void Part::initializeGlobally() @@ -86,6 +91,69 @@ FColDsptr MbD::Part::getqEddot() return partFrame->getqEddot(); } +void MbD::Part::qX(FColDsptr x) +{ + partFrame->qX = x; +} + +FColDsptr MbD::Part::qX() +{ + return partFrame->qX; +} + +void MbD::Part::qE(std::shared_ptr> x) +{ + partFrame->qE = x; +} + +std::shared_ptr> MbD::Part::qE() +{ + return partFrame->qE; +} + +void MbD::Part::qXdot(FColDsptr x) +{ + partFrame->qXdot = x; +} + +FColDsptr MbD::Part::qXdot() +{ + return partFrame->qXdot; +} + +void MbD::Part::omeOpO(FColDsptr x) +{ + partFrame->setomeOpO(x); +} + +FColDsptr MbD::Part::omeOpO() +{ + assert(false); + return FColDsptr(); +} + +void MbD::Part::qXddot(FColDsptr x) +{ + partFrame->qXddot = x; +} + +FColDsptr MbD::Part::qXddot() +{ + return partFrame->qXddot; +} + +void MbD::Part::qEddot(FColDsptr x) +{ + //ToDo: Should store EulerParametersDDot + //ToDo: Need alpOpO too + partFrame->qXddot = x; +} + +FColDsptr MbD::Part::qEddot() +{ + return partFrame->qEddot; +} + void Part::setSystem(System& sys) { //May be needed in the future @@ -104,6 +172,11 @@ void MbD::Part::postInput() void MbD::Part::calcPostDynCorrectorIteration() { + this->calcmE(); + this->calcmEdot(); + this->calcpTpE(); + this->calcppTpEpE(); + this->calcppTpEpEdot(); } void MbD::Part::prePosIC() @@ -171,7 +244,7 @@ void MbD::Part::fillqsuWeights(std::shared_ptr> diagMat) { auto aJi = aJ->at(i); wqE->at(i) = (maxw * aJi / aJiMax) + minw; - } + } wqE->at(3) = minw; diagMat->atiputDiagonalMatrix(partFrame->iqX, wqX); diagMat->atiputDiagonalMatrix(partFrame->iqE, wqE); @@ -231,6 +304,11 @@ void MbD::Part::setqsulam(FColDsptr col) partFrame->setqsulam(col); } +void MbD::Part::setqsudotlam(FColDsptr col) +{ + partFrame->setqsudotlam(col); +} + void MbD::Part::postPosICIteration() { partFrame->postPosICIteration(); @@ -264,7 +342,7 @@ void MbD::Part::constraintsReport() void MbD::Part::postPosIC() { partFrame->postPosIC(); - //this->calcmE(); + this->calcmE(); } void MbD::Part::outputStates() @@ -297,3 +375,116 @@ void MbD::Part::preVelIC() { partFrame->preVelIC(); } + +void MbD::Part::postVelIC() +{ + partFrame->postVelIC(); + this->calcp(); + this->calcmEdot(); + this->calcpTpE(); + this->calcppTpEpE(); + this->calcppTpEpEdot(); +} + +void MbD::Part::fillVelICError(FColDsptr col) +{ + partFrame->fillVelICError(col); +} + +void MbD::Part::fillVelICJacob(SpMatDsptr mat) +{ + partFrame->fillVelICJacob(mat); +} + +void MbD::Part::preAccIC() +{ + partFrame->preAccIC(); + Item::preAccIC(); +} + +void MbD::Part::calcp() +{ + pX = mX->timesFullColumn(partFrame->qXdot); + pE = mE->timesFullColumn(partFrame->qEdot); +} + +void MbD::Part::calcpdot() +{ + pXdot = mX->timesFullColumn(partFrame->qXddot); + pEdot = mEdot->timesFullColumn(partFrame->qEdot)->plusFullColumn(mE->timesFullColumn(partFrame->qEddot)); +} + +void MbD::Part::calcmEdot() +{ + auto aC = partFrame->aC(); + auto aCdot = partFrame->aCdot(); + auto a4J = aJ->times(4.0); + auto term1 = aC->transposeTimesFullMatrix(a4J->timesFullMatrix(aCdot)); + auto term2 = term1->transpose(); + mEdot = term1->plusFullMatrix(term2); +} + +void MbD::Part::calcpTpE() +{ + //"pTpE is a column vector." + auto& qEdot = partFrame->qEdot; + auto aC = partFrame->aC(); + auto pCpEtimesqEdot = EulerParameters::pCpEtimesColumn(qEdot); + pTpE = (pCpEtimesqEdot->transposeTimesFullColumn(aJ->timesFullColumn(aC->timesFullColumn(qEdot))))->times(4.0); +} + +void MbD::Part::calcppTpEpE() +{ + auto& qEdot = partFrame->qEdot; + auto pCpEtimesqEdot = EulerParameters::pCpEtimesColumn(qEdot); + auto a4J = aJ->times(4.0); + ppTpEpE = pCpEtimesqEdot->transposeTimesFullMatrix(a4J->timesFullMatrix(pCpEtimesqEdot)); +} + +void MbD::Part::calcppTpEpEdot() +{ + //| qEdot aC a4J term1 pCpEtimesqEdot term2 | + auto& qEdot = partFrame->qEdot; + auto aC = partFrame->aC(); + auto a4J = aJ->times(4.0); + auto term1 = EulerParameters::pCTpEtimesColumn(a4J->timesFullColumn(aC->timesFullColumn(qEdot))); + auto pCpEtimesqEdot = EulerParameters::pCpEtimesColumn(qEdot); + auto term2 = aC->transposeTimesFullMatrix(a4J->timesFullMatrix(pCpEtimesqEdot)); + ppTpEpEdot = term1->plusFullMatrix(term2)->transpose(); +} + +void MbD::Part::calcmE() +{ + auto aC = partFrame->aC(); + mE = aC->transposeTimesFullMatrix(aJ->timesFullMatrix(aC))->times(4.0); +} + +void MbD::Part::fillAccICIterError(FColDsptr col) +{ + auto iqX = partFrame->iqX; + auto iqE = partFrame->iqE; + col->atiminusFullColumn(iqX, mX->timesFullColumn(partFrame->qXddot)); + col->atiminusFullColumn(iqE, mEdot->timesFullColumn(partFrame->qEdot)); + col->atiminusFullColumn(iqE, mE->timesFullColumn(partFrame->qEddot)); + col->atiplusFullColumn(iqE, pTpE); + partFrame->fillAccICIterError(col); +} + +void MbD::Part::fillAccICIterJacob(SpMatDsptr mat) +{ + auto iqX = partFrame->iqX; + auto iqE = partFrame->iqE; + mat->atijminusDiagonalMatrix(iqX, iqX, mX); + mat->atijminusFullMatrix(iqE, iqE, mE); + partFrame->fillAccICIterJacob(mat); +} + +std::shared_ptr> MbD::Part::qEdot() +{ + return partFrame->qEdot; +} + +void MbD::Part::setqsuddotlam(FColDsptr qsudotlam) +{ + partFrame->setqsuddotlam(qsudotlam); +} diff --git a/MbDCode/Part.h b/MbDCode/Part.h index 1062f4e..66c3479 100644 --- a/MbDCode/Part.h +++ b/MbDCode/Part.h @@ -4,6 +4,7 @@ #include "Item.h" #include "FullColumn.h" #include "FullMatrix.h" +#include "EulerParametersDot.h" namespace MbD { class System; @@ -17,7 +18,7 @@ namespace MbD { public: Part(); Part(const char* str); - void initialize(); + void initialize() override; void initializeLocally() override; void initializeGlobally() override; void setqX(FColDsptr x); @@ -32,6 +33,18 @@ namespace MbD { FColDsptr getqXddot(); void setqEddot(FColDsptr x); FColDsptr getqEddot(); + void qX(FColDsptr x); + FColDsptr qX(); + void qE(std::shared_ptr> x); + std::shared_ptr> qE(); + void qXdot(FColDsptr x); + FColDsptr qXdot(); + void omeOpO(FColDsptr x); + FColDsptr omeOpO(); + void qXddot(FColDsptr x); + FColDsptr qXddot(); + void qEddot(FColDsptr x); + FColDsptr qEddot(); void setSystem(System& sys); void asFixed(); void postInput() override; @@ -52,6 +65,7 @@ namespace MbD { void useEquationNumbers() override; void setqsu(FColDsptr col) override; void setqsulam(FColDsptr col) override; + void setqsudotlam(FColDsptr col) override; void postPosICIteration() override; void fillPosICError(FColDsptr col) override; void fillPosICJacob(SpMatDsptr mat) override; @@ -65,6 +79,21 @@ namespace MbD { void fillPosKineError(FColDsptr col) override; void fillPosKineJacob(SpMatDsptr mat) override; void preVelIC() override; + void postVelIC() override; + void fillVelICError(FColDsptr col) override; + void fillVelICJacob(SpMatDsptr mat) override; + void preAccIC() override; + void calcp(); + void calcpdot(); + void calcmEdot(); + void calcpTpE(); + void calcppTpEpE(); + void calcppTpEpEdot(); + void calcmE(); + void fillAccICIterError(FColDsptr col) override; + void fillAccICIterJacob(SpMatDsptr mat) override; + std::shared_ptr> qEdot(); + void setqsuddotlam(FColDsptr qsudotlam) override; int ipX = -1; int ipE = -1; @@ -76,7 +105,7 @@ namespace MbD { FColDsptr pE; FColDsptr pEdot; std::shared_ptr> mX; - std::shared_ptr> mE; + FMatDsptr mE; FMatDsptr mEdot; FColDsptr pTpE; FMatDsptr ppTpEpE; diff --git a/MbDCode/PartFrame.cpp b/MbDCode/PartFrame.cpp index fdba74c..c1ab959 100644 --- a/MbDCode/PartFrame.cpp +++ b/MbDCode/PartFrame.cpp @@ -55,12 +55,11 @@ FColDsptr PartFrame::getqE() { return qE; } void PartFrame::setqXdot(FColDsptr x) { - //qXdot->copy(x); + qXdot = x; } FColDsptr PartFrame::getqXdot() { - //return qXdot; - return std::make_shared>(3); + return qXdot; } void PartFrame::setomeOpO(FColDsptr omeOpO) { @@ -200,6 +199,16 @@ FMatDsptr MbD::PartFrame::aAOp() return qE->aA; } +FMatDsptr MbD::PartFrame::aC() +{ + return qE->aC; +} + +FMatDsptr MbD::PartFrame::aCdot() +{ + return qEdot->aCdot; +} + FColFMatDsptr MbD::PartFrame::pAOppE() { return qE->pApE; @@ -279,6 +288,15 @@ void MbD::PartFrame::setqsulam(FColDsptr col) aGabsDo([&](std::shared_ptr con) { con->setqsulam(col); }); } +void MbD::PartFrame::setqsudotlam(FColDsptr col) +{ + qXdot->equalFullColumnAt(col, iqX); + qEdot->equalFullColumnAt(col, iqE); + markerFramesDo([&](std::shared_ptr markerFrame) { markerFrame->setqsudotlam(col); }); + aGeu->setqsudotlam(col); + aGabsDo([&](std::shared_ptr con) { con->setqsudotlam(col); }); +} + void MbD::PartFrame::postPosICIteration() { Item::postPosICIteration(); @@ -351,6 +369,62 @@ void MbD::PartFrame::preVelIC() aGabsDo([](std::shared_ptr aGab) { aGab->preVelIC(); }); } +void MbD::PartFrame::postVelIC() +{ + qEdot->calcAdotBdotCdot(); + qEdot->calcpAdotpE(); + markerFramesDo([](std::shared_ptr markerFrame) { markerFrame->postVelIC(); }); + aGeu->postVelIC(); + aGabsDo([](std::shared_ptr aGab) { aGab->postVelIC(); }); +} + +void MbD::PartFrame::fillVelICError(FColDsptr col) +{ + markerFramesDo([&](std::shared_ptr markerFrame) { markerFrame->fillVelICError(col); }); + aGeu->fillVelICError(col); + aGabsDo([&](std::shared_ptr con) { con->fillVelICError(col); }); +} + +void MbD::PartFrame::fillVelICJacob(SpMatDsptr mat) +{ + markerFramesDo([&](std::shared_ptr markerFrame) { markerFrame->fillVelICJacob(mat); }); + aGeu->fillVelICJacob(mat); + aGabsDo([&](std::shared_ptr con) { con->fillVelICJacob(mat); }); +} + +void MbD::PartFrame::preAccIC() +{ + qXddot = std::make_shared>(3, 0.0); + qEddot = std::make_shared>(4, 0.0); + Item::preAccIC(); + markerFramesDo([](std::shared_ptr markerFrame) { markerFrame->preAccIC(); }); + aGeu->preAccIC(); + aGabsDo([](std::shared_ptr aGab) { aGab->preAccIC(); }); +} + +void MbD::PartFrame::fillAccICIterError(FColDsptr col) +{ + markerFramesDo([&](std::shared_ptr markerFrame) { markerFrame->fillAccICIterError(col); }); + aGeu->fillAccICIterError(col); + aGabsDo([&](std::shared_ptr con) { con->fillAccICIterError(col); }); +} + +void MbD::PartFrame::fillAccICIterJacob(SpMatDsptr mat) +{ + markerFramesDo([&](std::shared_ptr markerFrame) { markerFrame->fillAccICIterJacob(mat); }); + aGeu->fillAccICIterJacob(mat); + aGabsDo([&](std::shared_ptr con) { con->fillAccICIterJacob(mat); }); +} + +void MbD::PartFrame::setqsuddotlam(FColDsptr qsudotlam) +{ + qXddot->equalFullColumnAt(qsudotlam, iqX); + qEddot->equalFullColumnAt(qsudotlam, iqE); + markerFramesDo([&](std::shared_ptr markerFrame) { markerFrame->setqsuddotlam(qsudotlam); }); + aGeu->setqsuddotlam(qsudotlam); + aGabsDo([&](std::shared_ptr con) { con->setqsuddotlam(qsudotlam); }); +} + void PartFrame::asFixed() { for (int i = 0; i < 6; i++) { @@ -362,8 +436,8 @@ void PartFrame::asFixed() void MbD::PartFrame::postInput() { - //qXddot = std::make_shared>(3, 0.0); - //qEddot = std::make_shared>(4, 0.0); + qXddot = std::make_shared>(3, 0.0); + qEddot = std::make_shared>(4, 0.0); Item::postInput(); markerFramesDo([](std::shared_ptr markerFrame) { markerFrame->postInput(); }); aGeu->postInput(); @@ -374,6 +448,6 @@ void MbD::PartFrame::calcPostDynCorrectorIteration() { qE->calcABC(); qE->calcpApE(); - //qEdot->calcAdotBdotCdot(); - //qEdot->calcpAdotpE(); + qEdot->calcAdotBdotCdot(); + qEdot->calcpAdotpE(); } diff --git a/MbDCode/PartFrame.h b/MbDCode/PartFrame.h index f3e46d0..cbab5f5 100644 --- a/MbDCode/PartFrame.h +++ b/MbDCode/PartFrame.h @@ -24,7 +24,7 @@ namespace MbD { public: PartFrame(); PartFrame(const char* str); - void initialize(); + void initialize() override; void initializeLocally() override; void initializeGlobally() override; void asFixed(); @@ -57,6 +57,8 @@ namespace MbD { void prePosKine() override; FColDsptr rOpO(); FMatDsptr aAOp(); + FMatDsptr aC(); + FMatDsptr aCdot(); FColFMatDsptr pAOppE(); void fillEssenConstraints(std::shared_ptr>> essenConstraints) override; void fillRedundantConstraints(std::shared_ptr>> redunConstraints) override; @@ -69,6 +71,7 @@ namespace MbD { void useEquationNumbers() override; void setqsu(FColDsptr col) override; void setqsulam(FColDsptr col) override; + void setqsudotlam(FColDsptr col) override; void postPosICIteration() override; void fillPosICError(FColDsptr col) override; void fillPosICJacob(SpMatDsptr mat) override; @@ -78,6 +81,13 @@ namespace MbD { void storeDynState() override; void fillPosKineError(FColDsptr col) override; void preVelIC() override; + void postVelIC() override; + void fillVelICError(FColDsptr col) override; + void fillVelICJacob(SpMatDsptr mat) override; + void preAccIC() override; + void fillAccICIterError(FColDsptr col) override; + void fillAccICIterJacob(SpMatDsptr mat) override; + void setqsuddotlam(FColDsptr qsudotlam) override; Part* part = nullptr; //Use raw pointer when pointing backwards. int iqX = -1; @@ -85,7 +95,7 @@ namespace MbD { FColDsptr qX = std::make_shared>(3); std::shared_ptr> qE = CREATE>::With(4); FColDsptr qXdot = std::make_shared>(3); - std::shared_ptr> qEdot = std::make_shared>(4); + std::shared_ptr> qEdot = CREATE>::With(4); FColDsptr qXddot = std::make_shared>(3); FColDsptr qEddot = std::make_shared>(4); std::shared_ptr aGeu; diff --git a/MbDCode/PosNewtonRaphson.h b/MbDCode/PosNewtonRaphson.h index ed581ec..de4c46e 100644 --- a/MbDCode/PosNewtonRaphson.h +++ b/MbDCode/PosNewtonRaphson.h @@ -10,7 +10,7 @@ namespace MbD { void preRun() override; void incrementIterNo() override; void askSystemToUpdate() override; - virtual void postRun() override; + void postRun() override; }; } diff --git a/MbDCode/PrescribedMotion.h b/MbDCode/PrescribedMotion.h index 5653f95..5505925 100644 --- a/MbDCode/PrescribedMotion.h +++ b/MbDCode/PrescribedMotion.h @@ -10,7 +10,7 @@ namespace MbD { public: PrescribedMotion(); PrescribedMotion(const char* str); - void initialize(); + void initialize() override; void connectsItoJ(EndFrmcptr frmI, EndFrmcptr frmJ) override; Symsptr xBlk; diff --git a/MbDCode/RedundantConstraint.cpp b/MbDCode/RedundantConstraint.cpp index 2adf60b..06836e2 100644 --- a/MbDCode/RedundantConstraint.cpp +++ b/MbDCode/RedundantConstraint.cpp @@ -59,6 +59,10 @@ void MbD::RedundantConstraint::setqsulam(FColDsptr col) { } +void MbD::RedundantConstraint::setqsudotlam(FColDsptr col) +{ +} + void MbD::RedundantConstraint::fillPosICError(FColDsptr col) { } @@ -74,3 +78,15 @@ void MbD::RedundantConstraint::fillPosKineJacob(SpMatDsptr mat) void MbD::RedundantConstraint::preVelIC() { } + +void MbD::RedundantConstraint::preAccIC() +{ +} + +void MbD::RedundantConstraint::fillAccICIterError(FColDsptr col) +{ +} + +void MbD::RedundantConstraint::setqsuddotlam(FColDsptr qsudotlam) +{ +} diff --git a/MbDCode/RedundantConstraint.h b/MbDCode/RedundantConstraint.h index 681e0ec..8e51e41 100644 --- a/MbDCode/RedundantConstraint.h +++ b/MbDCode/RedundantConstraint.h @@ -20,10 +20,14 @@ namespace MbD { void fillConstraints(std::shared_ptr sptr, std::shared_ptr>> redunConstraints); void fillRedundantConstraints(std::shared_ptr sptr, std::shared_ptr>> allConstraints); void setqsulam(FColDsptr col) override; + void setqsudotlam(FColDsptr col) override; void fillPosICError(FColDsptr col) override; void fillPosKineError(FColDsptr col) override; void fillPosKineJacob(SpMatDsptr mat) override; void preVelIC() override; + void preAccIC() override; + void fillAccICIterError(FColDsptr col) override; + void setqsuddotlam(FColDsptr qsudotlam) override; std::shared_ptr constraint; }; diff --git a/MbDCode/SingularMatrixError.h b/MbDCode/SingularMatrixError.h index c4c70a5..276f25e 100644 --- a/MbDCode/SingularMatrixError.h +++ b/MbDCode/SingularMatrixError.h @@ -9,7 +9,6 @@ namespace MbD { class SingularMatrixError : virtual public std::runtime_error { protected: - std::shared_ptr> redundantEqnNos; public: diff --git a/MbDCode/SparseMatrix.h b/MbDCode/SparseMatrix.h index fc9b537..a766587 100644 --- a/MbDCode/SparseMatrix.h +++ b/MbDCode/SparseMatrix.h @@ -27,6 +27,7 @@ namespace MbD { this->push_back(row); } } + void atiput(int i, std::shared_ptr> spRow); void atijplusDiagonalMatrix(int i, int j, std::shared_ptr> diagMat); void atijminusDiagonalMatrix(int i, int j, std::shared_ptr> diagMat); double sumOfSquares() override; @@ -34,19 +35,29 @@ namespace MbD { void atijplusFullRow(int i, int j, std::shared_ptr> fullRow); void atijplusFullColumn(int i, int j, std::shared_ptr> fullCol); void atijplusFullMatrix(int i, int j, std::shared_ptr> fullMat); + void atijminusFullMatrix(int i, int j, std::shared_ptr> fullMat); void atijplusTransposeFullMatrix(int i, int j, std::shared_ptr> fullMat); void atijplusFullMatrixtimes(int i, int j, std::shared_ptr> 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); virtual std::ostream& printOn(std::ostream& s) const; friend std::ostream& operator<<(std::ostream& s, const SparseMatrix& spMat) { return spMat.printOn(s); } + std::shared_ptr> timesFullColumn(std::shared_ptr> fullCol); + }; using SpMatDsptr = std::shared_ptr>; + template + inline void SparseMatrix::atiput(int i, std::shared_ptr> spRow) + { + this->at(i) = spRow; + } + template inline void SparseMatrix::atijplusDiagonalMatrix(int i, int j, std::shared_ptr> diagMat) { @@ -105,6 +116,14 @@ namespace MbD { } } template + inline void SparseMatrix::atijminusFullMatrix(int i, int j, std::shared_ptr> fullMat) + { + for (int ii = 0; ii < fullMat->nrow(); ii++) + { + this->at(i + ii)->atiminusFullRow(j, fullMat->at(ii)); + } + } + template inline void SparseMatrix::atijplusTransposeFullMatrix(int i, int j, std::shared_ptr> fullMat) { for (int ii = 0; ii < fullMat->nrow(); ii++) @@ -131,6 +150,11 @@ namespace MbD { this->at(i)->atiminusNumber(j, value); } template + inline void SparseMatrix::atijput(int i, int j, T value) + { + this->at(i)->atiput(j, value); + } + template inline std::ostream& SparseMatrix::printOn(std::ostream& s) const { s << "SpMat[" << std::endl; @@ -141,4 +165,16 @@ namespace MbD { s << "]" << std::endl; return s; } + template + inline std::shared_ptr> SparseMatrix::timesFullColumn(std::shared_ptr> fullCol) + { + //"a*b = a(i,j)b(j) sum j." + auto nrow = this->nrow(); + auto answer = std::make_shared>(nrow); + for (int i = 0; i < nrow; i++) + { + answer->at(i) = this->at(i)->timesFullColumn(fullCol); + } + return answer; + } } \ No newline at end of file diff --git a/MbDCode/SparseRow.h b/MbDCode/SparseRow.h index c10d8c2..1a78353 100644 --- a/MbDCode/SparseRow.h +++ b/MbDCode/SparseRow.h @@ -17,7 +17,10 @@ namespace MbD { std::shared_ptr> timesconditionedWithTol(double scaling, double tol); std::shared_ptr> conditionedWithTol(double tol); void atiplusFullRow(int j, std::shared_ptr> fullRow); + void atiminusFullRow(int j, std::shared_ptr> fullRow); void atiplusFullRowtimes(int j, std::shared_ptr> fullRow, double factor); + T timesFullColumn(std::shared_ptr> fullCol); + }; using SpRowDsptr = std::shared_ptr>; template<> @@ -51,6 +54,14 @@ namespace MbD { } } template + inline void SparseRow::atiminusFullRow(int j, std::shared_ptr> fullRow) + { + for (int jj = 0; jj < fullRow->size(); jj++) + { + (*this)[j + jj] -= fullRow->at(jj); + } + } + template inline void SparseRow::atiplusFullRowtimes(int j, std::shared_ptr> fullRow, double factor) { for (int jj = 0; jj < fullRow->size(); jj++) @@ -58,5 +69,14 @@ namespace MbD { (*this)[j + jj] += fullRow->at(jj) * factor; } } + template + inline T SparseRow::timesFullColumn(std::shared_ptr> fullCol) + { + T sum = 0.0; + for (auto const& keyValue : *this) { + sum += fullCol->at(keyValue.first) * keyValue.second; + } + return sum; + } } diff --git a/MbDCode/SparseVector.h b/MbDCode/SparseVector.h index 276f19d..b634e30 100644 --- a/MbDCode/SparseVector.h +++ b/MbDCode/SparseVector.h @@ -28,6 +28,7 @@ namespace MbD { double rootMeanSquare(); int numberOfElements(); double sumOfSquares(); + void atiput(int i, T value); void atiplusNumber(int i, double value); void atiminusNumber(int i, double value); void zeroSelf(); @@ -59,6 +60,11 @@ namespace MbD { } return sum; } + template + inline void SparseVector::atiput(int i, T value) + { + (*this)[i] = value; + } template<> inline void SparseVector::atiplusNumber(int i, double value) { diff --git a/MbDCode/System.cpp b/MbDCode/System.cpp index 9d1ceb4..826acba 100644 --- a/MbDCode/System.cpp +++ b/MbDCode/System.cpp @@ -80,7 +80,7 @@ void System::initializeGlobally() std::shared_ptr> MbD::System::discontinuitiesAtIC() { - return std::shared_ptr>(); + return std::make_shared>(); } void MbD::System::jointsMotionsDo(const std::function)>& f) diff --git a/MbDCode/SystemSolver.cpp b/MbDCode/SystemSolver.cpp index 4a5ee6d..0b0ac6b 100644 --- a/MbDCode/SystemSolver.cpp +++ b/MbDCode/SystemSolver.cpp @@ -14,6 +14,7 @@ #include "PosICKineNewtonRaphson.h" #include "PosKineNewtonRaphson.h" #include "VelICSolver.h" +#include "AccICNewtonRaphson.h" using namespace MbD; @@ -81,7 +82,9 @@ void MbD::SystemSolver::runVelIC() void MbD::SystemSolver::runAccIC() { - assert(false); + icTypeSolver = CREATE::With(); + icTypeSolver->setSystem(this); + icTypeSolver->run(); } bool MbD::SystemSolver::needToRedoPosIC() diff --git a/MbDCode/TranslationConstraintIJ.cpp b/MbDCode/TranslationConstraintIJ.cpp index 250c721..1497336 100644 --- a/MbDCode/TranslationConstraintIJ.cpp +++ b/MbDCode/TranslationConstraintIJ.cpp @@ -62,3 +62,9 @@ void MbD::TranslationConstraintIJ::preVelIC() riIeJeIe->preVelIC(); Item::preVelIC(); } + +void MbD::TranslationConstraintIJ::preAccIC() +{ + riIeJeIe->preAccIC(); + Constraint::preAccIC(); +} diff --git a/MbDCode/TranslationConstraintIJ.h b/MbDCode/TranslationConstraintIJ.h index 1930970..53ed722 100644 --- a/MbDCode/TranslationConstraintIJ.h +++ b/MbDCode/TranslationConstraintIJ.h @@ -9,7 +9,7 @@ namespace MbD { //riIeJeIe public: TranslationConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj, int axisi); - void initialize(); + void initialize() override; void initializeLocally() override; void initializeGlobally() override; virtual void initriIeJeIe(); @@ -19,6 +19,7 @@ namespace MbD { MbD::ConstraintType type() override; void postPosICIteration() override; void preVelIC() override; + void preAccIC() override; int axisI; std::shared_ptr riIeJeIe; diff --git a/MbDCode/TranslationConstraintIqcJc.cpp b/MbDCode/TranslationConstraintIqcJc.cpp index 1ba83c4..6be743d 100644 --- a/MbDCode/TranslationConstraintIqcJc.cpp +++ b/MbDCode/TranslationConstraintIqcJc.cpp @@ -56,3 +56,25 @@ void MbD::TranslationConstraintIqcJc::fillPosKineJacob(SpMatDsptr mat) mat->atijplusFullRow(iG, iqXI, pGpXI); mat->atijplusFullRow(iG, iqEI, pGpEI); } + +void MbD::TranslationConstraintIqcJc::fillVelICJacob(SpMatDsptr mat) +{ + mat->atijplusFullRow(iG, iqXI, pGpXI); + mat->atijplusFullColumn(iqXI, iG, pGpXI->transpose()); + mat->atijplusFullRow(iG, iqEI, pGpEI); + mat->atijplusFullColumn(iqEI, iG, pGpEI->transpose()); +} + +void MbD::TranslationConstraintIqcJc::fillAccICIterError(FColDsptr col) +{ + col->atiplusFullVectortimes(iqXI, pGpXI, lam); + col->atiplusFullVectortimes(iqEI, pGpEI, lam); + auto efrmIqc = std::static_pointer_cast(frmI); + auto qXdotI = efrmIqc->qXdot(); + auto qEdotI = efrmIqc->qEdot(); + auto sum = pGpXI->timesFullColumn(efrmIqc->qXddot()); + sum += pGpEI->timesFullColumn(efrmIqc->qEddot()); + sum += 2.0 * (qXdotI->transposeTimesFullColumn(ppGpXIpEI->timesFullColumn(qEdotI))); + sum += qEdotI->transposeTimesFullColumn(ppGpEIpEI->timesFullColumn(qEdotI)); + col->atiplusNumber(iG, sum); +} diff --git a/MbDCode/TranslationConstraintIqcJc.h b/MbDCode/TranslationConstraintIqcJc.h index f23e1d2..4b86b8b 100644 --- a/MbDCode/TranslationConstraintIqcJc.h +++ b/MbDCode/TranslationConstraintIqcJc.h @@ -14,6 +14,8 @@ namespace MbD { void fillPosICError(FColDsptr col) override; void fillPosICJacob(SpMatDsptr mat) override; void fillPosKineJacob(SpMatDsptr mat) override; + void fillVelICJacob(SpMatDsptr mat) override; + void fillAccICIterError(FColDsptr col) override; FRowDsptr pGpXI; FRowDsptr pGpEI; diff --git a/MbDCode/TranslationConstraintIqcJqc.cpp b/MbDCode/TranslationConstraintIqcJqc.cpp index 6c09974..badfb0b 100644 --- a/MbDCode/TranslationConstraintIqcJqc.cpp +++ b/MbDCode/TranslationConstraintIqcJqc.cpp @@ -34,7 +34,7 @@ void MbD::TranslationConstraintIqcJqc::useEquationNumbers() void MbD::TranslationConstraintIqcJqc::fillPosICError(FColDsptr col) { - Constraint::fillPosICError(col); + TranslationConstraintIqcJc::fillPosICError(col); col->atiplusFullVectortimes(iqXJ, pGpXJ, lam); col->atiplusFullVectortimes(iqEJ, pGpEJ, lam); } @@ -61,3 +61,30 @@ void MbD::TranslationConstraintIqcJqc::fillPosKineJacob(SpMatDsptr mat) mat->atijplusFullRow(iG, iqXJ, pGpXJ); mat->atijplusFullRow(iG, iqEJ, pGpEJ); } + +void MbD::TranslationConstraintIqcJqc::fillVelICJacob(SpMatDsptr mat) +{ + TranslationConstraintIqcJc::fillVelICJacob(mat); + mat->atijplusFullRow(iG, iqXJ, pGpXJ); + mat->atijplusFullColumn(iqXJ, iG, pGpXJ->transpose()); + mat->atijplusFullRow(iG, iqEJ, pGpEJ); + mat->atijplusFullColumn(iqEJ, iG, pGpEJ->transpose()); +} + +void MbD::TranslationConstraintIqcJqc::fillAccICIterError(FColDsptr col) +{ + TranslationConstraintIqcJc::fillAccICIterError(col); + col->atiplusFullVectortimes(iqXJ, pGpXJ, lam); + col->atiplusFullVectortimes(iqEJ, pGpEJ, lam); + auto efrmIqc = std::static_pointer_cast(frmI); + auto efrmJqc = std::static_pointer_cast(frmJ); + auto qEdotI = efrmIqc->qEdot(); + auto qXdotJ = efrmJqc->qXdot(); + auto qEdotJ = efrmJqc->qEdot(); + double sum = pGpXJ->timesFullColumn(efrmJqc->qXddot()); + sum += pGpEJ->timesFullColumn(efrmJqc->qEddot()); + sum += 2.0 * (qEdotI->transposeTimesFullColumn(ppGpEIpXJ->timesFullColumn(qXdotJ))); + sum += 2.0 * (qEdotI->transposeTimesFullColumn(ppGpEIpEJ->timesFullColumn(qEdotJ))); + sum += qEdotJ->transposeTimesFullColumn(ppGpEJpEJ->timesFullColumn(qEdotJ)); + col->atiplusNumber(iG, sum); +} diff --git a/MbDCode/TranslationConstraintIqcJqc.h b/MbDCode/TranslationConstraintIqcJqc.h index aefc096..6da8180 100644 --- a/MbDCode/TranslationConstraintIqcJqc.h +++ b/MbDCode/TranslationConstraintIqcJqc.h @@ -14,6 +14,8 @@ namespace MbD { void fillPosICError(FColDsptr col) override; void fillPosICJacob(SpMatDsptr mat) override; void fillPosKineJacob(SpMatDsptr mat) override; + void fillVelICJacob(SpMatDsptr mat) override; + void fillAccICIterError(FColDsptr col) override; FRowDsptr pGpXJ; FRowDsptr pGpEJ; diff --git a/MbDCode/TranslationConstraintIqctJqc.cpp b/MbDCode/TranslationConstraintIqctJqc.cpp index a2052aa..b0732a4 100644 --- a/MbDCode/TranslationConstraintIqctJqc.cpp +++ b/MbDCode/TranslationConstraintIqctJqc.cpp @@ -24,3 +24,36 @@ void MbD::TranslationConstraintIqctJqc::preVelIC() TranslationConstraintIJ::preVelIC(); pGpt = std::static_pointer_cast(riIeJeIe)->pvaluept(); } + +void MbD::TranslationConstraintIqctJqc::fillVelICError(FColDsptr col) +{ + col->atiminusNumber(iG, pGpt); +} + +void MbD::TranslationConstraintIqctJqc::preAccIC() +{ + TranslationConstraintIJ::preAccIC(); + auto riIeJeIeqct = std::static_pointer_cast(riIeJeIe); + ppGpXIpt = riIeJeIeqct->ppvaluepXIpt(); + ppGpEIpt = riIeJeIeqct->ppvaluepEIpt()->plusFullRow(riIeJeIeqct->ppvaluepEKpt()); + ppGpXJpt = riIeJeIeqct->ppvaluepXJpt(); + ppGpEJpt = riIeJeIeqct->ppvaluepEJpt(); + ppGptpt = riIeJeIeqct->ppvalueptpt(); +} + +void MbD::TranslationConstraintIqctJqc::fillAccICIterError(FColDsptr col) +{ + TranslationConstraintIqcJqc::fillAccICIterError(col); + auto efrmIqc = std::static_pointer_cast(frmI); + auto efrmJqc = std::static_pointer_cast(frmJ); + auto qXdotI = efrmIqc->qXdot(); + auto qEdotI = efrmIqc->qEdot(); + auto qXdotJ = efrmJqc->qXdot(); + auto qEdotJ = efrmJqc->qEdot(); + double sum = 2.0 * ppGpXIpt->timesFullColumn(qXdotI); + sum += 2.0 * ppGpEIpt->timesFullColumn(qEdotI); + sum += 2.0 * ppGpXJpt->timesFullColumn(qXdotJ); + sum += 2.0 * ppGpEJpt->timesFullColumn(qEdotJ); + sum += ppGptpt; + col->atiplusNumber(iG, sum); +} diff --git a/MbDCode/TranslationConstraintIqctJqc.h b/MbDCode/TranslationConstraintIqctJqc.h index 587f15e..ff13259 100644 --- a/MbDCode/TranslationConstraintIqctJqc.h +++ b/MbDCode/TranslationConstraintIqctJqc.h @@ -11,6 +11,9 @@ namespace MbD { void initriIeJeIe() override; MbD::ConstraintType type() override; void preVelIC() override; + void fillVelICError(FColDsptr col) override; + void preAccIC() override; + void fillAccICIterError(FColDsptr col) override; double pGpt; FRowDsptr ppGpXIpt; diff --git a/MbDCode/Variable.h b/MbDCode/Variable.h index bb1e8da..677c0ea 100644 --- a/MbDCode/Variable.h +++ b/MbDCode/Variable.h @@ -10,7 +10,7 @@ namespace MbD { Variable(); Variable(const char* str); Variable(double val); - void initialize(); + void initialize() override; void setName(std::string& str); const std::string& getName() const; double getValue() override; diff --git a/MbDCode/VelSolver.cpp b/MbDCode/VelSolver.cpp index d012d62..249e7a3 100644 --- a/MbDCode/VelSolver.cpp +++ b/MbDCode/VelSolver.cpp @@ -4,6 +4,8 @@ #include "CREATE.h" #include "GESpMatParPvPrecise.h" #include "SingularMatrixError.h" +#include "GESpMatParPvMarko.h" +#include "GESpMatParPvMarkoFast.h" using namespace MbD;