From 371b13a9e078b0dd3e902b19e3ff7abf803b2e2f Mon Sep 17 00:00:00 2001 From: Aik-Siong Koh Date: Sun, 18 Jun 2023 01:06:39 -0600 Subject: [PATCH] runVelIC --- MbDCode/AbsConstraint.cpp | 9 +- MbDCode/AbsConstraint.h | 1 + MbDCode/Array.h | 70 ++++++-- MbDCode/AtPointConstraintIJ.cpp | 6 + MbDCode/AtPointConstraintIJ.h | 1 + MbDCode/AtPointConstraintIqcJc.cpp | 10 +- MbDCode/AtPointConstraintIqcJc.h | 1 + MbDCode/AtPointConstraintIqcJqc.cpp | 11 +- MbDCode/AtPointConstraintIqcJqc.h | 1 + MbDCode/AtPointConstraintIqctJqc.cpp | 8 +- MbDCode/AtPointConstraintIqctJqc.h | 1 + MbDCode/BasicIntegrator.cpp | 28 ++-- MbDCode/BasicIntegrator.h | 4 +- MbDCode/BasicQuasiIntegrator.cpp | 7 +- MbDCode/BasicQuasiIntegrator.h | 3 +- MbDCode/Constraint.cpp | 18 +++ MbDCode/Constraint.h | 5 +- MbDCode/DiagonalMatrix.h | 4 +- MbDCode/DifferenceOperator.cpp | 30 +++- MbDCode/DifferenceOperator.h | 4 +- MbDCode/DirectionCosineConstraintIJ.cpp | 6 + MbDCode/DirectionCosineConstraintIJ.h | 1 + MbDCode/DirectionCosineConstraintIqcJc.cpp | 5 + MbDCode/DirectionCosineConstraintIqcJc.h | 1 + MbDCode/DirectionCosineConstraintIqcJqc.cpp | 6 + MbDCode/DirectionCosineConstraintIqcJqc.h | 1 + MbDCode/DirectionCosineConstraintIqctJqc.cpp | 6 + MbDCode/DirectionCosineConstraintIqctJqc.h | 1 + MbDCode/DirectionCosineIeqctJeqc.cpp | 13 ++ MbDCode/DirectionCosineIeqctJeqc.h | 2 + MbDCode/DispCompIeqcJeqcKeqct.cpp | 13 ++ MbDCode/DispCompIeqcJeqcKeqct.h | 2 + MbDCode/DispCompIeqctJeqcKeqct.cpp | 8 + MbDCode/DispCompIeqctJeqcKeqct.h | 1 + MbDCode/DispCompIeqctJeqcO.cpp | 11 ++ MbDCode/DispCompIeqctJeqcO.h | 2 + MbDCode/EndFramec.h | 4 +- MbDCode/EndFrameqct.cpp | 57 +++++++ MbDCode/EndFrameqct.h | 12 +- MbDCode/EulerAngleszxz.h | 2 +- MbDCode/EulerAngleszxzDDot.cpp | 1 + MbDCode/EulerAngleszxzDDot.h | 18 +++ MbDCode/EulerAngleszxzDot.cpp | 1 + MbDCode/EulerAngleszxzDot.h | 73 +++++++++ MbDCode/EulerArray.h | 15 +- MbDCode/EulerConstraint.cpp | 7 +- MbDCode/EulerConstraint.h | 1 + MbDCode/EulerParameters.h | 9 +- MbDCode/EulerParametersDot.h | 67 +++++--- MbDCode/FullColumn.h | 33 +++- MbDCode/FullMatrix.h | 70 ++++++-- MbDCode/FullRow.h | 33 +++- MbDCode/FullVector.h | 22 ++- MbDCode/GEFullMat.cpp | 42 ++++- MbDCode/GEFullMat.h | 7 +- MbDCode/GEFullMatFullPv.cpp | 38 ++++- MbDCode/GEFullMatParPv.cpp | 31 +++- MbDCode/GEFullMatParPv.h | 2 +- MbDCode/GESpMat.cpp | 34 ++++ MbDCode/GESpMat.h | 7 +- MbDCode/GESpMatFullPv.cpp | 58 ++++++- MbDCode/GESpMatFullPvPosIC.cpp | 10 +- MbDCode/GESpMatParPvMarko.cpp | 60 ++++++- MbDCode/GESpMatParPvMarkoFast.cpp | 13 +- MbDCode/GESpMatParPvPrecise.cpp | 86 +++++++++- MbDCode/ICKineIntegrator.cpp | 8 + MbDCode/ICKineIntegrator.h | 1 + MbDCode/Integrator.cpp | 4 + MbDCode/Integrator.h | 2 + MbDCode/IntegratorInterface.cpp | 9 +- MbDCode/IntegratorInterface.h | 8 +- MbDCode/Item.cpp | 57 +++++-- MbDCode/Item.h | 19 ++- MbDCode/Joint.cpp | 34 ++++ MbDCode/Joint.h | 7 + MbDCode/KineIntegrator.cpp | 7 + MbDCode/KineIntegrator.h | 1 + MbDCode/LDUFullMat.cpp | 160 +++++++++++++++++++ MbDCode/LDUFullMat.h | 16 ++ MbDCode/LDUFullMatParPv.cpp | 46 +++--- MbDCode/LDUFullMatParPv.h | 4 +- MbDCode/LDUSpMat.cpp | 75 +++++++++ MbDCode/LDUSpMat.h | 25 +++ MbDCode/LDUSpMatParPv.cpp | 1 + MbDCode/LDUSpMatParPv.h | 13 ++ MbDCode/LDUSpMatParPvMarko.cpp | 65 ++++++++ MbDCode/LDUSpMatParPvMarko.h | 14 ++ MbDCode/LDUSpMatParPvPrecise.cpp | 63 ++++++++ MbDCode/LDUSpMatParPvPrecise.h | 14 ++ MbDCode/MarkerFrame.cpp | 23 ++- MbDCode/MarkerFrame.h | 4 + MbDCode/MatrixDecomposition.cpp | 12 ++ MbDCode/MatrixDecomposition.h | 9 ++ MbDCode/MatrixGaussElimination.cpp | 14 -- MbDCode/MatrixGaussElimination.h | 2 - MbDCode/MatrixLDU.cpp | 16 ++ MbDCode/MatrixLDU.h | 1 + MbDCode/MatrixSolver.cpp | 17 ++ MbDCode/MatrixSolver.h | 13 +- MbDCode/MbDCode.cpp | 38 ++++- MbDCode/MbDCode.vcxproj | 20 +++ MbDCode/MbDCode.vcxproj.filters | 60 +++++++ MbDCode/NewtonRaphson.cpp | 4 +- MbDCode/NewtonRaphson.h | 4 +- MbDCode/Part.cpp | 83 ++++++++++ MbDCode/Part.h | 14 +- MbDCode/PartFrame.cpp | 74 ++++++++- MbDCode/PartFrame.h | 19 ++- MbDCode/PosICKineNewtonRaphson.cpp | 30 ++++ MbDCode/PosICNewtonRaphson.cpp | 2 +- MbDCode/PosKineNewtonRaphson.cpp | 64 ++++++++ MbDCode/PosKineNewtonRaphson.h | 6 + MbDCode/QuasiIntegrator.cpp | 13 +- MbDCode/QuasiIntegrator.h | 2 + MbDCode/RedundantConstraint.cpp | 16 ++ MbDCode/RedundantConstraint.h | 6 +- MbDCode/RowTypeMatrix.h | 2 +- MbDCode/Solver.cpp | 5 + MbDCode/Solver.h | 3 + MbDCode/SparseColumn.h | 2 +- MbDCode/SparseMatrix.h | 29 +++- MbDCode/SparseRow.h | 2 +- MbDCode/SparseVector.h | 16 +- MbDCode/StableBackwardDifference.cpp | 27 +++- MbDCode/StableBackwardDifference.h | 2 +- MbDCode/System.cpp | 14 +- MbDCode/System.h | 6 +- MbDCode/SystemSolver.cpp | 94 +++++++++-- MbDCode/SystemSolver.h | 17 +- MbDCode/TranslationConstraintIJ.cpp | 6 + MbDCode/TranslationConstraintIJ.h | 1 + MbDCode/TranslationConstraintIqcJc.cpp | 6 + MbDCode/TranslationConstraintIqcJc.h | 1 + MbDCode/TranslationConstraintIqcJqc.cpp | 7 + MbDCode/TranslationConstraintIqcJqc.h | 1 + MbDCode/TranslationConstraintIqctJqc.cpp | 6 + MbDCode/TranslationConstraintIqctJqc.h | 1 + MbDCode/Variable.cpp | 5 + MbDCode/Variable.h | 1 + MbDCode/VelICKineSolver.cpp | 1 + MbDCode/VelICKineSolver.h | 13 ++ MbDCode/VelICSolver.cpp | 70 ++++++++ MbDCode/VelICSolver.h | 17 ++ MbDCode/VelKineSolver.cpp | 3 + MbDCode/VelKineSolver.h | 13 ++ MbDCode/VelSolver.cpp | 58 +++++++ MbDCode/VelSolver.h | 31 ++++ 147 files changed, 2555 insertions(+), 238 deletions(-) create mode 100644 MbDCode/EulerAngleszxzDDot.cpp create mode 100644 MbDCode/EulerAngleszxzDDot.h create mode 100644 MbDCode/EulerAngleszxzDot.cpp create mode 100644 MbDCode/EulerAngleszxzDot.h create mode 100644 MbDCode/LDUSpMat.cpp create mode 100644 MbDCode/LDUSpMat.h create mode 100644 MbDCode/LDUSpMatParPv.cpp create mode 100644 MbDCode/LDUSpMatParPv.h create mode 100644 MbDCode/LDUSpMatParPvMarko.cpp create mode 100644 MbDCode/LDUSpMatParPvMarko.h create mode 100644 MbDCode/LDUSpMatParPvPrecise.cpp create mode 100644 MbDCode/LDUSpMatParPvPrecise.h create mode 100644 MbDCode/VelICKineSolver.cpp create mode 100644 MbDCode/VelICKineSolver.h create mode 100644 MbDCode/VelICSolver.cpp create mode 100644 MbDCode/VelICSolver.h create mode 100644 MbDCode/VelKineSolver.cpp create mode 100644 MbDCode/VelKineSolver.h create mode 100644 MbDCode/VelSolver.cpp create mode 100644 MbDCode/VelSolver.h diff --git a/MbDCode/AbsConstraint.cpp b/MbDCode/AbsConstraint.cpp index 9dec801..3c945a5 100644 --- a/MbDCode/AbsConstraint.cpp +++ b/MbDCode/AbsConstraint.cpp @@ -34,8 +34,8 @@ void MbD::AbsConstraint::useEquationNumbers() void MbD::AbsConstraint::fillPosICJacob(SpMatDsptr mat) { - (*(mat->at(iG)))[iqXminusOnePlusAxis] += 1.0; - (*(mat->at(iqXminusOnePlusAxis)))[iG] += 1.0; + mat->atijplusNumber(iG, iqXminusOnePlusAxis, 1.0); + mat->atijplusNumber(iqXminusOnePlusAxis, iG, 1.0); } void MbD::AbsConstraint::fillPosICError(FColDsptr col) @@ -43,3 +43,8 @@ void MbD::AbsConstraint::fillPosICError(FColDsptr col) Constraint::fillPosICError(col); col->at(iqXminusOnePlusAxis) += lam; } + +void MbD::AbsConstraint::fillPosKineJacob(SpMatDsptr mat) +{ + mat->atijplusNumber(iG, iqXminusOnePlusAxis, 1.0); +} diff --git a/MbDCode/AbsConstraint.h b/MbDCode/AbsConstraint.h index 9f1bf82..66af74d 100644 --- a/MbDCode/AbsConstraint.h +++ b/MbDCode/AbsConstraint.h @@ -13,6 +13,7 @@ namespace MbD { void useEquationNumbers() override; void fillPosICJacob(SpMatDsptr mat) override; void fillPosICError(FColDsptr col) override; + void fillPosKineJacob(SpMatDsptr mat) override; int axis = -1; int iqXminusOnePlusAxis = -1; diff --git a/MbDCode/Array.h b/MbDCode/Array.h index a771007..c1d7209 100644 --- a/MbDCode/Array.h +++ b/MbDCode/Array.h @@ -7,7 +7,7 @@ namespace MbD { - template + template class Array : public std::vector { public: @@ -18,11 +18,15 @@ namespace MbD { Array(std::vector::iterator begin, std::vector::iterator end) : std::vector(begin, end) {} Array(std::initializer_list list) : std::vector{ list } {} void copyFrom(std::shared_ptr> x); - virtual void zeroSelf() = 0; + virtual void zeroSelf(); virtual double sumOfSquares() = 0; double rootMeanSquare(); - virtual int numberOfElements() = 0; - void swapRows(int i, int ii); + virtual int numberOfElements(); + void swapElems(int i, int ii); + //double maxMagnitude(); + double maxMagnitudeOfVector(); + void equalArrayAt(std::shared_ptr> array, int i); + }; template inline void Array::copyFrom(std::shared_ptr> x) @@ -32,19 +36,65 @@ namespace MbD { } } template + inline void Array::zeroSelf() + { + for (int i = 0; i < this->size(); i++) { + this->at(i) = (T)0; + } + } + template inline double Array::rootMeanSquare() { return std::sqrt(this->sumOfSquares() / this->numberOfElements()); } template - inline void Array::swapRows(int i, int ii) + inline int Array::numberOfElements() + { + return (int)this->size(); + } + template + inline void Array::swapElems(int i, int ii) { auto temp = this->at(i); this->at(i) = this->at(ii); this->at(ii) = temp; } - using ListD = std::initializer_list; - using ListListD = std::initializer_list>; - using ListListPairD = std::initializer_list>>; -} - + //template + //inline double Array::maxMagnitude() + //{ + // if (std::is_arithmetic::value) { + // return this->maxMagnitudeOfVector(); + // } + // else { + // auto answer = 0.0; + // for (int i = 0; i < this->size(); i++) + // { + // auto mag = this->at(i)->maxMagnitude(); + // if (answer < mag) answer = mag; + // } + // return answer; + // } + //} + template + inline double Array::maxMagnitudeOfVector() + { + auto answer = 0.0; + for (int i = 0; i < this->size(); i++) + { + auto mag = std::abs(this->at(i)); + if (answer < mag) answer = mag; + } + return answer; + } + template + inline void Array::equalArrayAt(std::shared_ptr> array, int i) + { + for (int ii = 0; ii < this->size(); ii++) + { + this->at(ii) = array->at(i + ii); + } + } + using ListD = std::initializer_list; + using ListListD = std::initializer_list>; + using ListListPairD = std::initializer_list>>; +} \ No newline at end of file diff --git a/MbDCode/AtPointConstraintIJ.cpp b/MbDCode/AtPointConstraintIJ.cpp index a139467..3242601 100644 --- a/MbDCode/AtPointConstraintIJ.cpp +++ b/MbDCode/AtPointConstraintIJ.cpp @@ -57,3 +57,9 @@ void MbD::AtPointConstraintIJ::postPosICIteration() riIeJeO->postPosICIteration(); Item::postPosICIteration(); } + +void MbD::AtPointConstraintIJ::preVelIC() +{ + riIeJeO->preVelIC(); + Item::preVelIC(); +} diff --git a/MbDCode/AtPointConstraintIJ.h b/MbDCode/AtPointConstraintIJ.h index 3272604..e6d05b0 100644 --- a/MbDCode/AtPointConstraintIJ.h +++ b/MbDCode/AtPointConstraintIJ.h @@ -19,6 +19,7 @@ namespace MbD { void prePosIC() override; MbD::ConstraintType type() override; void postPosICIteration() override; + void preVelIC() override; int axis; std::shared_ptr riIeJeO; diff --git a/MbDCode/AtPointConstraintIqcJc.cpp b/MbDCode/AtPointConstraintIqcJc.cpp index d383390..754d2c7 100644 --- a/MbDCode/AtPointConstraintIqcJc.cpp +++ b/MbDCode/AtPointConstraintIqcJc.cpp @@ -42,9 +42,15 @@ void MbD::AtPointConstraintIqcJc::fillPosICError(FColDsptr col) void MbD::AtPointConstraintIqcJc::fillPosICJacob(SpMatDsptr mat) { - (*(mat->at(iG)))[iqXIminusOnePlusAxis] += -1.0; - (*(mat->at(iqXIminusOnePlusAxis)))[iG] += -1.0; + mat->atijplusNumber(iG, iqXIminusOnePlusAxis, -1.0); + mat->atijplusNumber(iqXIminusOnePlusAxis, iG, -1.0); mat->atijplusFullRow(iG, iqEI, pGpEI); mat->atijplusFullColumn(iqEI, iG, pGpEI->transpose()); mat->atijplusFullMatrixtimes(iqEI, iqEI, ppGpEIpEI, lam); } + +void MbD::AtPointConstraintIqcJc::fillPosKineJacob(SpMatDsptr mat) +{ + mat->atijplusNumber(iG, iqXIminusOnePlusAxis, -1.0); + mat->atijplusFullRow(iG, iqEI, pGpEI); +} diff --git a/MbDCode/AtPointConstraintIqcJc.h b/MbDCode/AtPointConstraintIqcJc.h index bbe8d77..e2abf52 100644 --- a/MbDCode/AtPointConstraintIqcJc.h +++ b/MbDCode/AtPointConstraintIqcJc.h @@ -14,6 +14,7 @@ namespace MbD { void useEquationNumbers() override; void fillPosICError(FColDsptr col) override; void fillPosICJacob(SpMatDsptr mat) override; + void fillPosKineJacob(SpMatDsptr mat) override; FRowDsptr pGpEI; FMatDsptr ppGpEIpEI; diff --git a/MbDCode/AtPointConstraintIqcJqc.cpp b/MbDCode/AtPointConstraintIqcJqc.cpp index 4a6e80a..dc29007 100644 --- a/MbDCode/AtPointConstraintIqcJqc.cpp +++ b/MbDCode/AtPointConstraintIqcJqc.cpp @@ -44,9 +44,16 @@ void MbD::AtPointConstraintIqcJqc::fillPosICError(FColDsptr col) void MbD::AtPointConstraintIqcJqc::fillPosICJacob(SpMatDsptr mat) { AtPointConstraintIqcJc::fillPosICJacob(mat); - (*(mat->at(iG)))[iqXJminusOnePlusAxis] += 1.0; - (*(mat->at(iqXJminusOnePlusAxis)))[iG] += 1.0; + mat->atijplusNumber(iG, iqXJminusOnePlusAxis, 1.0); + mat->atijplusNumber(iqXJminusOnePlusAxis, iG, 1.0); mat->atijplusFullRow(iG, iqEJ, pGpEJ); mat->atijplusFullColumn(iqEJ, iG, pGpEJ->transpose()); mat->atijplusFullMatrixtimes(iqEJ, iqEJ, ppGpEJpEJ, lam); } + +void MbD::AtPointConstraintIqcJqc::fillPosKineJacob(SpMatDsptr mat) +{ + AtPointConstraintIqcJc::fillPosKineJacob(mat); + mat->atijplusNumber(iG, iqXJminusOnePlusAxis, 1.0); + mat->atijplusFullRow(iG, iqEJ, pGpEJ); +} diff --git a/MbDCode/AtPointConstraintIqcJqc.h b/MbDCode/AtPointConstraintIqcJqc.h index 33f759e..7c724bb 100644 --- a/MbDCode/AtPointConstraintIqcJqc.h +++ b/MbDCode/AtPointConstraintIqcJqc.h @@ -14,6 +14,7 @@ namespace MbD { void useEquationNumbers() override; void fillPosICError(FColDsptr col) override; void fillPosICJacob(SpMatDsptr mat) override; + void fillPosKineJacob(SpMatDsptr mat) override; FRowDsptr pGpEJ; FMatDsptr ppGpEJpEJ; diff --git a/MbDCode/AtPointConstraintIqctJqc.cpp b/MbDCode/AtPointConstraintIqctJqc.cpp index d12feea..2155f62 100644 --- a/MbDCode/AtPointConstraintIqctJqc.cpp +++ b/MbDCode/AtPointConstraintIqctJqc.cpp @@ -12,7 +12,7 @@ AtPointConstraintIqctJqc::AtPointConstraintIqctJqc(EndFrmcptr frmi, EndFrmcptr f void MbD::AtPointConstraintIqctJqc::initializeGlobally() { riIeJeO->initializeGlobally(); - ppGpEJpEJ = (std::static_pointer_cast(riIeJeO))->ppriIeJeOpEJpEJ; + ppGpEJpEJ = std::static_pointer_cast(riIeJeO)->ppriIeJeOpEJpEJ; } void AtPointConstraintIqctJqc::initriIeJeO() @@ -32,3 +32,9 @@ MbD::ConstraintType MbD::AtPointConstraintIqctJqc::type() { return MbD::essential; } + +void MbD::AtPointConstraintIqctJqc::preVelIC() +{ + AtPointConstraintIJ::preVelIC(); + pGpt = std::static_pointer_cast(riIeJeO)->priIeJeOpt; +} diff --git a/MbDCode/AtPointConstraintIqctJqc.h b/MbDCode/AtPointConstraintIqctJqc.h index f786cf0..40f8bc9 100644 --- a/MbDCode/AtPointConstraintIqctJqc.h +++ b/MbDCode/AtPointConstraintIqctJqc.h @@ -12,6 +12,7 @@ namespace MbD { void initriIeJeO() override; void calcPostDynCorrectorIteration() override; MbD::ConstraintType type() override; + void preVelIC() override; double pGpt; FRowDsptr ppGpEIpt; diff --git a/MbDCode/BasicIntegrator.cpp b/MbDCode/BasicIntegrator.cpp index 74457f3..856e4a4 100644 --- a/MbDCode/BasicIntegrator.cpp +++ b/MbDCode/BasicIntegrator.cpp @@ -33,14 +33,14 @@ void MbD::BasicIntegrator::initializeGlobally() //"Get info from system and prepare for start of simulation." //"Integrator asks system for info. Not system setting integrator." - this->t = system->tstart; + this->sett(system->tstart); this->direction = system->direction; this->orderMax = system->orderMax(); } -void MbD::BasicIntegrator::setSystem(IntegratorInterface* sys) +void MbD::BasicIntegrator::setSystem(Solver* sys) { - system = sys; + system = static_cast(sys); } void MbD::BasicIntegrator::calcOperatorMatrix() @@ -50,7 +50,6 @@ void MbD::BasicIntegrator::calcOperatorMatrix() void MbD::BasicIntegrator::incrementTime() { - //| istepNew | tpast->insert(tpast->begin(), t); if ((int)tpast->size() > (orderMax + 1)) { tpast->pop_back(); } @@ -60,7 +59,7 @@ void MbD::BasicIntegrator::incrementTime() h = hnew; this->settnew(t + (direction * h)); this->calcOperatorMatrix(); - //system->incrementTime(tnew); + system->incrementTime(tnew); } void MbD::BasicIntegrator::incrementTry() @@ -110,7 +109,7 @@ void MbD::BasicIntegrator::preRun() void MbD::BasicIntegrator::preStep() { - assert(false); + system->preStep(); } void MbD::BasicIntegrator::reportStats() @@ -126,13 +125,24 @@ void MbD::BasicIntegrator::firstStep() void MbD::BasicIntegrator::setorder(int o) { order = o; - //opBD->setorder(o); + opBDF->setorder(o); } void MbD::BasicIntegrator::settnew(double t) { -tnew = t; -//this->time(double); + tnew = t; + this->settime(t); +} + +void MbD::BasicIntegrator::sett(double tt) +{ + t = tt; + opBDF->settime(tt); +} + +void MbD::BasicIntegrator::settime(double tt) +{ + opBDF->settime(tt); } void MbD::BasicIntegrator::subsequentSteps() diff --git a/MbDCode/BasicIntegrator.h b/MbDCode/BasicIntegrator.h index a1df5ff..9162fd9 100644 --- a/MbDCode/BasicIntegrator.h +++ b/MbDCode/BasicIntegrator.h @@ -28,13 +28,15 @@ namespace MbD { void run() override; virtual void selectOrder(); virtual void subsequentSteps(); - void setSystem(IntegratorInterface* sys); + void setSystem(Solver* sys) override; void logString(std::string& str) override; virtual void firstStep(); virtual void nextStep() = 0; virtual void setorder(int o); virtual void settnew(double t); + virtual void sett(double t); + void settime(double t); IntegratorInterface* system; int istep = 0, iTry = 0, maxTry = 0; diff --git a/MbDCode/BasicQuasiIntegrator.cpp b/MbDCode/BasicQuasiIntegrator.cpp index e15e4d7..bcc7e0a 100644 --- a/MbDCode/BasicQuasiIntegrator.cpp +++ b/MbDCode/BasicQuasiIntegrator.cpp @@ -11,7 +11,7 @@ void MbD::BasicQuasiIntegrator::firstStep() orderNew = 1; this->selectFirstStepSize(); this->incrementTime(); - //this->runInitialConditionTypeSolution(); + this->runInitialConditionTypeSolution(); //this->reportTrialStepStats(); //while (this->isRedoingFirstStep()) @@ -48,6 +48,11 @@ void MbD::BasicQuasiIntegrator::nextStep() //this->reportStepStats(); } +void MbD::BasicQuasiIntegrator::runInitialConditionTypeSolution() +{ + system->runInitialConditionTypeSolution(); +} + void MbD::BasicQuasiIntegrator::selectFirstStepSize() { if (iTry == 1) { diff --git a/MbDCode/BasicQuasiIntegrator.h b/MbDCode/BasicQuasiIntegrator.h index 8721ea0..0918edf 100644 --- a/MbDCode/BasicQuasiIntegrator.h +++ b/MbDCode/BasicQuasiIntegrator.h @@ -13,10 +13,9 @@ namespace MbD { void nextStep(); //void reportStepStats(); //void reportTrialStepStats(); - //void runInitialConditionTypeSolution(); + void runInitialConditionTypeSolution() override; void selectFirstStepSize(); //void selectStepSize(); - //void runInitialConditionTypeSolution(); }; } diff --git a/MbDCode/Constraint.cpp b/MbDCode/Constraint.cpp index 6c458a8..081bfe7 100644 --- a/MbDCode/Constraint.cpp +++ b/MbDCode/Constraint.cpp @@ -45,6 +45,14 @@ void MbD::Constraint::prePosIC() Item::prePosIC(); } +void MbD::Constraint::prePosKine() +{ + //"Preserve lam calculated from AccIC for possible use by DynIntegrator if system is not kinematic." + auto lamOld = lam; + this->prePosIC(); + lam = lamOld; +} + void MbD::Constraint::fillEssenConstraints(std::shared_ptr sptr, std::shared_ptr>> essenConstraints) { if (this->type() == MbD::essential) { @@ -71,6 +79,11 @@ void MbD::Constraint::fillRedundantConstraints(std::shared_ptr sptr, } } +void MbD::Constraint::fillConstraints(std::shared_ptr sptr, std::shared_ptr>> allConstraints) +{ + allConstraints->push_back(sptr); +} + MbD::ConstraintType MbD::Constraint::type() { return MbD::essential; @@ -123,3 +136,8 @@ void MbD::Constraint::preDyn() { mu = 0.0; } + +void MbD::Constraint::fillPosKineError(FColDsptr col) +{ + col->atiplusNumber(iG, aG); +} diff --git a/MbDCode/Constraint.h b/MbDCode/Constraint.h index 7a6c42c..ed2f2d2 100644 --- a/MbDCode/Constraint.h +++ b/MbDCode/Constraint.h @@ -16,10 +16,12 @@ namespace MbD { void setOwner(Item* x); Item* getOwner(); void prePosIC() override; + void prePosKine() override; virtual void fillEssenConstraints(std::shared_ptr sptr, std::shared_ptr>> essenConstraints); virtual void fillDispConstraints(std::shared_ptr sptr, std::shared_ptr>> dispConstraints); virtual void fillPerpenConstraints(std::shared_ptr sptr, std::shared_ptr>> perpenConstraints); - virtual void fillRedundantConstraints(std::shared_ptr sptr, std::shared_ptr>> perpenConstraints); + virtual void fillRedundantConstraints(std::shared_ptr sptr, std::shared_ptr>> redunConstraints); + virtual void fillConstraints(std::shared_ptr sptr, std::shared_ptr>> allConstraints); virtual MbD::ConstraintType type(); void fillqsulam(FColDsptr col) override; void setqsulam(FColDsptr col) override; @@ -29,6 +31,7 @@ namespace MbD { virtual bool isRedundant(); void outputStates() override; void preDyn() override; + void fillPosKineError(FColDsptr col) override; int iG = -1; double aG = 0.0; //Constraint function diff --git a/MbDCode/DiagonalMatrix.h b/MbDCode/DiagonalMatrix.h index 0dab606..8ae2596 100644 --- a/MbDCode/DiagonalMatrix.h +++ b/MbDCode/DiagonalMatrix.h @@ -4,7 +4,7 @@ #include "FullColumn.h" namespace MbD { - template + template class DiagonalMatrix : public Array { // @@ -65,7 +65,7 @@ namespace MbD { inline void DiagonalMatrix::zeroSelf() { for (int i = 0; i < this->size(); i++) { - this->at(i) = 0.0;; + this->at(i) = 0.0; } } } diff --git a/MbDCode/DifferenceOperator.cpp b/MbDCode/DifferenceOperator.cpp index 99110d7..84a49c3 100644 --- a/MbDCode/DifferenceOperator.cpp +++ b/MbDCode/DifferenceOperator.cpp @@ -1,7 +1,21 @@ +#include + #include "DifferenceOperator.h" #include "CREATE.h" #include "SingularMatrixError.h" #include "LDUFullMatParPv.h" +#include "FullRow.h" + +using namespace MbD; + +std::shared_ptr> DifferenceOperator::OneOverFactorials = []() { + auto oneOverFactorials = std::make_shared>(10); + for (int i = 0; i < oneOverFactorials->size(); i++) + { + oneOverFactorials->at(i) = 1.0 / std::tgamma(i+1); + } + return oneOverFactorials; +}(); void MbD::DifferenceOperator::calcOperatorMatrix() { @@ -12,8 +26,7 @@ void MbD::DifferenceOperator::calcOperatorMatrix() this->formTaylorMatrix(); try { - assert(false); - //operatorMatrix = CREATE::With()->inversesaveOriginal(taylorMatrix, false); + operatorMatrix = CREATE::With()->inversesaveOriginal(taylorMatrix, false); } catch (SingularMatrixError ex) { } @@ -21,8 +34,6 @@ void MbD::DifferenceOperator::calcOperatorMatrix() void MbD::DifferenceOperator::initialize() { - //OneOverFactorials: = StMFullRow new : 10. - //1 to : OneOverFactorials size do : [:i | OneOverFactorials at : i put : 1.0d / i factorial] } void MbD::DifferenceOperator::setiStep(int i) @@ -35,9 +46,9 @@ void MbD::DifferenceOperator::setorder(int o) order = o; } -void MbD::DifferenceOperator::instanciateTaylorMatrix() +void MbD::DifferenceOperator::instantiateTaylorMatrix() { - if (taylorMatrix->empty() || (taylorMatrix->nrow() != (order + 1))) { + if (taylorMatrix == nullptr || (taylorMatrix->nrow() != (order + 1))) { taylorMatrix = std::make_shared>(order + 1, order + 1); } } @@ -45,7 +56,7 @@ void MbD::DifferenceOperator::instanciateTaylorMatrix() void MbD::DifferenceOperator::formTaylorRowwithTimeNodederivative(int i, int ii, int k) { //| rowi hi hipower aij | - auto rowi = taylorMatrix->at(i); + auto& rowi = taylorMatrix->at(i); for (int j = 0; j < k; j++) { rowi->at(j) = 0.0; @@ -61,3 +72,8 @@ void MbD::DifferenceOperator::formTaylorRowwithTimeNodederivative(int i, int ii, //rowi at : j put : aij } } + +void MbD::DifferenceOperator::settime(double t) +{ + time = t; +} diff --git a/MbDCode/DifferenceOperator.h b/MbDCode/DifferenceOperator.h index c252141..8da4b4d 100644 --- a/MbDCode/DifferenceOperator.h +++ b/MbDCode/DifferenceOperator.h @@ -14,13 +14,15 @@ namespace MbD { virtual void setiStep(int i); virtual void setorder(int o); virtual void formTaylorMatrix() = 0; - virtual void instanciateTaylorMatrix(); + virtual void instantiateTaylorMatrix(); virtual void formTaylorRowwithTimeNodederivative(int i, int ii, int k); + void settime(double t); int iStep = 0, order = 0; std::shared_ptr> taylorMatrix, operatorMatrix; double time = 0; std::shared_ptr> timeNodes; + static std::shared_ptr> OneOverFactorials; }; } diff --git a/MbDCode/DirectionCosineConstraintIJ.cpp b/MbDCode/DirectionCosineConstraintIJ.cpp index b5552c1..d3711b6 100644 --- a/MbDCode/DirectionCosineConstraintIJ.cpp +++ b/MbDCode/DirectionCosineConstraintIJ.cpp @@ -58,3 +58,9 @@ MbD::ConstraintType MbD::DirectionCosineConstraintIJ::type() { return MbD::perpendicular; } + +void MbD::DirectionCosineConstraintIJ::preVelIC() +{ + aAijIeJe->preVelIC(); + Item::preVelIC(); +} diff --git a/MbDCode/DirectionCosineConstraintIJ.h b/MbDCode/DirectionCosineConstraintIJ.h index 6e18bb5..706c2b8 100644 --- a/MbDCode/DirectionCosineConstraintIJ.h +++ b/MbDCode/DirectionCosineConstraintIJ.h @@ -19,6 +19,7 @@ namespace MbD { void prePosIC() override; void postPosICIteration() override; MbD::ConstraintType type() override; + void preVelIC() override; int axisI, axisJ; std::shared_ptr aAijIeJe; diff --git a/MbDCode/DirectionCosineConstraintIqcJc.cpp b/MbDCode/DirectionCosineConstraintIqcJc.cpp index 8bfb6ce..e159a10 100644 --- a/MbDCode/DirectionCosineConstraintIqcJc.cpp +++ b/MbDCode/DirectionCosineConstraintIqcJc.cpp @@ -40,3 +40,8 @@ void MbD::DirectionCosineConstraintIqcJc::fillPosICJacob(SpMatDsptr mat) mat->atijplusFullColumn(iqEI, iG, pGpEI->transpose()); mat->atijplusFullMatrixtimes(iqEI, iqEI, ppGpEIpEI, lam); } + +void MbD::DirectionCosineConstraintIqcJc::fillPosKineJacob(SpMatDsptr mat) +{ + mat->atijplusFullRow(iG, iqEI, pGpEI); +} diff --git a/MbDCode/DirectionCosineConstraintIqcJc.h b/MbDCode/DirectionCosineConstraintIqcJc.h index 65ef5ea..14f22dc 100644 --- a/MbDCode/DirectionCosineConstraintIqcJc.h +++ b/MbDCode/DirectionCosineConstraintIqcJc.h @@ -13,6 +13,7 @@ namespace MbD { void useEquationNumbers() override; void fillPosICError(FColDsptr col) override; void fillPosICJacob(SpMatDsptr mat) override; + void fillPosKineJacob(SpMatDsptr mat) override; FRowDsptr pGpEI; FMatDsptr ppGpEIpEI; diff --git a/MbDCode/DirectionCosineConstraintIqcJqc.cpp b/MbDCode/DirectionCosineConstraintIqcJqc.cpp index b0812f3..eb8b03c 100644 --- a/MbDCode/DirectionCosineConstraintIqcJqc.cpp +++ b/MbDCode/DirectionCosineConstraintIqcJqc.cpp @@ -46,3 +46,9 @@ void MbD::DirectionCosineConstraintIqcJqc::fillPosICJacob(SpMatDsptr mat) mat->atijplusTransposeFullMatrix(iqEJ, iqEI, ppGpEIpEJlam); mat->atijplusFullMatrixtimes(iqEJ, iqEJ, ppGpEJpEJ, lam); } + +void MbD::DirectionCosineConstraintIqcJqc::fillPosKineJacob(SpMatDsptr mat) +{ + DirectionCosineConstraintIqcJc::fillPosKineJacob(mat); + mat->atijplusFullRow(iG, iqEJ, pGpEJ); +} diff --git a/MbDCode/DirectionCosineConstraintIqcJqc.h b/MbDCode/DirectionCosineConstraintIqcJqc.h index 649c8e1..bef36a9 100644 --- a/MbDCode/DirectionCosineConstraintIqcJqc.h +++ b/MbDCode/DirectionCosineConstraintIqcJqc.h @@ -13,6 +13,7 @@ namespace MbD { void useEquationNumbers() override; void fillPosICError(FColDsptr col) override; void fillPosICJacob(SpMatDsptr mat) override; + void fillPosKineJacob(SpMatDsptr mat) override; FRowDsptr pGpEJ; FMatDsptr ppGpEIpEJ; diff --git a/MbDCode/DirectionCosineConstraintIqctJqc.cpp b/MbDCode/DirectionCosineConstraintIqctJqc.cpp index b27bc59..bd7ffe6 100644 --- a/MbDCode/DirectionCosineConstraintIqctJqc.cpp +++ b/MbDCode/DirectionCosineConstraintIqctJqc.cpp @@ -18,3 +18,9 @@ MbD::ConstraintType MbD::DirectionCosineConstraintIqctJqc::type() { return MbD::essential; } + +void MbD::DirectionCosineConstraintIqctJqc::preVelIC() +{ + DirectionCosineConstraintIJ::preVelIC(); + pGpt = std::static_pointer_cast(aAijIeJe)->pAijIeJept; +} diff --git a/MbDCode/DirectionCosineConstraintIqctJqc.h b/MbDCode/DirectionCosineConstraintIqctJqc.h index 38e2fa1..419f79f 100644 --- a/MbDCode/DirectionCosineConstraintIqctJqc.h +++ b/MbDCode/DirectionCosineConstraintIqctJqc.h @@ -10,6 +10,7 @@ namespace MbD { DirectionCosineConstraintIqctJqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj); void initaAijIeJe() override; MbD::ConstraintType type() override; + void preVelIC() override; double pGpt; FRowDsptr ppGpEIpt; diff --git a/MbDCode/DirectionCosineIeqctJeqc.cpp b/MbDCode/DirectionCosineIeqctJeqc.cpp index b184775..54955d0 100644 --- a/MbDCode/DirectionCosineIeqctJeqc.cpp +++ b/MbDCode/DirectionCosineIeqctJeqc.cpp @@ -1,5 +1,6 @@ #include "DirectionCosineIeqctJeqc.h" #include "EndFrameqc.h" +#include "EndFrameqct.h" using namespace MbD; @@ -31,3 +32,15 @@ void MbD::DirectionCosineIeqctJeqc::calcPostDynCorrectorIteration() ppAjOIepEIpEI = std::static_pointer_cast(frmI)->ppAjOepEpE(axisI); DirectionCosineIeqcJeqc::calcPostDynCorrectorIteration(); } + +void MbD::DirectionCosineIeqctJeqc::preVelIC() +{ + Item::preVelIC(); + auto pAjOIept = std::static_pointer_cast(frmI)->pAjOept(axisI); + pAijIeJept = pAjOIept->dot(aAjOJe); +} + +double MbD::DirectionCosineIeqctJeqc::pvaluept() +{ + return pAijIeJept; +} diff --git a/MbDCode/DirectionCosineIeqctJeqc.h b/MbDCode/DirectionCosineIeqctJeqc.h index 5327c45..1b1616b 100644 --- a/MbDCode/DirectionCosineIeqctJeqc.h +++ b/MbDCode/DirectionCosineIeqctJeqc.h @@ -12,6 +12,8 @@ namespace MbD { void initialize(); void initializeGlobally(); void calcPostDynCorrectorIteration() override; + void preVelIC() override; + double pvaluept(); double pAijIeJept; FRowDsptr ppAijIeJepEIpt; diff --git a/MbDCode/DispCompIeqcJeqcKeqct.cpp b/MbDCode/DispCompIeqcJeqcKeqct.cpp index ffa3bdd..8afb6a4 100644 --- a/MbDCode/DispCompIeqcJeqcKeqct.cpp +++ b/MbDCode/DispCompIeqcJeqcKeqct.cpp @@ -1,5 +1,6 @@ #include "DispCompIeqcJeqcKeqct.h" #include "EndFrameqc.h" +#include "EndFrameqct.h" using namespace MbD; @@ -28,3 +29,15 @@ void MbD::DispCompIeqcJeqcKeqct::calcPostDynCorrectorIteration() ppAjOKepEKpEK = efrmKqc->ppAjOepEpE(axisK); DispCompIeqcJeqcKeqc::calcPostDynCorrectorIteration(); } + +void MbD::DispCompIeqcJeqcKeqct::preVelIC() +{ + Item::preVelIC(); + auto pAjOKept = std::static_pointer_cast(efrmK)->pAjOept(axisK); + priIeJeKept = pAjOKept->dot(rIeJeO); +} + +double MbD::DispCompIeqcJeqcKeqct::pvaluept() +{ + return priIeJeKept; +} diff --git a/MbDCode/DispCompIeqcJeqcKeqct.h b/MbDCode/DispCompIeqcJeqcKeqct.h index 0ebb021..0f6861f 100644 --- a/MbDCode/DispCompIeqcJeqcKeqct.h +++ b/MbDCode/DispCompIeqcJeqcKeqct.h @@ -11,6 +11,8 @@ namespace MbD { DispCompIeqcJeqcKeqct(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk); void initialize() override; void calcPostDynCorrectorIteration() override; + void preVelIC() override; + double pvaluept(); double priIeJeKept; FRowDsptr ppriIeJeKepXIpt; diff --git a/MbDCode/DispCompIeqctJeqcKeqct.cpp b/MbDCode/DispCompIeqctJeqcKeqct.cpp index 3958277..0416a87 100644 --- a/MbDCode/DispCompIeqctJeqcKeqct.cpp +++ b/MbDCode/DispCompIeqctJeqcKeqct.cpp @@ -1,4 +1,5 @@ #include "DispCompIeqctJeqcKeqct.h" +#include "EndFrameqct.h" using namespace MbD; @@ -9,3 +10,10 @@ MbD::DispCompIeqctJeqcKeqct::DispCompIeqctJeqcKeqct() MbD::DispCompIeqctJeqcKeqct::DispCompIeqctJeqcKeqct(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk) : DispCompIeqcJeqcKeqct(frmi, frmj, frmk, axisk) { } + +void MbD::DispCompIeqctJeqcKeqct::preVelIC() +{ + DispCompIeqcJeqcKeqct::preVelIC(); + auto mprIeJeOpt = std::static_pointer_cast(frmI)->prOeOpt; + priIeJeKept -= aAjOKe->dot(mprIeJeOpt); +} diff --git a/MbDCode/DispCompIeqctJeqcKeqct.h b/MbDCode/DispCompIeqctJeqcKeqct.h index 55a9926..9dec4f3 100644 --- a/MbDCode/DispCompIeqctJeqcKeqct.h +++ b/MbDCode/DispCompIeqctJeqcKeqct.h @@ -9,6 +9,7 @@ namespace MbD { public: DispCompIeqctJeqcKeqct(); DispCompIeqctJeqcKeqct(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk); + void preVelIC() override; }; } diff --git a/MbDCode/DispCompIeqctJeqcO.cpp b/MbDCode/DispCompIeqctJeqcO.cpp index 011b70f..124f8bb 100644 --- a/MbDCode/DispCompIeqctJeqcO.cpp +++ b/MbDCode/DispCompIeqctJeqcO.cpp @@ -23,3 +23,14 @@ void MbD::DispCompIeqctJeqcO::calcPostDynCorrectorIteration() DispCompIeqcJeqcO::calcPostDynCorrectorIteration(); ppriIeJeOpEIpEI = std::static_pointer_cast(frmI)->ppriOeOpEpE(axis)->negated(); } + +void MbD::DispCompIeqctJeqcO::preVelIC() +{ + Item::preVelIC(); + priIeJeOpt = -(std::static_pointer_cast(frmI)->priOeOpt(axis)); +} + +double MbD::DispCompIeqctJeqcO::pvaluept() +{ + return priIeJeOpt; +} diff --git a/MbDCode/DispCompIeqctJeqcO.h b/MbDCode/DispCompIeqctJeqcO.h index eb45cb4..65f4e61 100644 --- a/MbDCode/DispCompIeqctJeqcO.h +++ b/MbDCode/DispCompIeqctJeqcO.h @@ -11,6 +11,8 @@ namespace MbD { DispCompIeqctJeqcO(EndFrmcptr frmi, EndFrmcptr frmj, int axis); void initializeGlobally() override; void calcPostDynCorrectorIteration() override; + void preVelIC() override; + double pvaluept(); double priIeJeOpt; FRowDsptr ppriIeJeOpEIpt; diff --git a/MbDCode/EndFramec.h b/MbDCode/EndFramec.h index b0c2729..e9f9509 100644 --- a/MbDCode/EndFramec.h +++ b/MbDCode/EndFramec.h @@ -8,9 +8,9 @@ namespace MbD { class MarkerFrame; - //template + //template //class FullColumn; - //template + //template //class FullMatrix; class EndFramec : public CartesianFrame diff --git a/MbDCode/EndFrameqct.cpp b/MbDCode/EndFrameqct.cpp index 39ee534..753aa2f 100644 --- a/MbDCode/EndFrameqct.cpp +++ b/MbDCode/EndFrameqct.cpp @@ -6,6 +6,7 @@ #include "EulerParameters.h" #include "CREATE.h" #include "EulerAngleszxz.h" +#include "EulerAngleszxzDot.h" using namespace MbD; @@ -159,3 +160,59 @@ void MbD::EndFrameqct::evalAme() aAme = phiThePsi->aA; } } + +void MbD::EndFrameqct::preVelIC() +{ + 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); +} + +FColDsptr MbD::EndFrameqct::pAjOept(int j) +{ + return pAOept->column(j); +} + +double MbD::EndFrameqct::priOeOpt(int i) +{ + return prOeOpt->at(i); +} + +void MbD::EndFrameqct::evalprmempt() +{ + if (rmemBlks) { + for (int i = 0; i < 3; i++) + { + auto& derivative = prmemptBlks->at(i); + auto value = derivative->getValue(); + prmempt->at(i) = value; + } + } +} + +void MbD::EndFrameqct::evalpAmept() +{ + if (phiThePsiBlks) { + auto phiThePsi = CREATE>::With(); + auto phiThePsiDot = CREATE>::With(); + phiThePsiDot->phiThePsi = phiThePsi; + for (int i = 0; i < 3; i++) + { + auto& expression = phiThePsiBlks->at(i); + auto& derivative = pPhiThePsiptBlks->at(i); + auto value = expression->getValue(); + auto valueDot = derivative->getValue(); + phiThePsi->at(i) = value; + phiThePsiDot->at(i) = valueDot; + } + phiThePsi->calc(); + phiThePsiDot->calc(); + pAmept = phiThePsiDot->aAdot; + } +} diff --git a/MbDCode/EndFrameqct.h b/MbDCode/EndFrameqct.h index 4deb721..0f5d582 100644 --- a/MbDCode/EndFrameqct.h +++ b/MbDCode/EndFrameqct.h @@ -24,14 +24,20 @@ namespace MbD { void prePosIC() override; void evalrmem(); void evalAme(); - + void preVelIC() override; + FColDsptr pAjOept(int j); double time = 0.0; + double priOeOpt(int i); + void evalprmempt(); + void evalpAmept(); + std::shared_ptr> rmemBlks, prmemptBlks, pprmemptptBlks; std::shared_ptr> phiThePsiBlks, pPhiThePsiptBlks, ppPhiThePsiptptBlks; - FColDsptr rmem, prmempt, pprmemptpt, pprOeOptpt; - FMatDsptr aAme, pAmept, ppAmeptpt, ppAOeptpt; + FColDsptr rmem, prmempt, pprmemptpt, prOeOpt, pprOeOptpt; + FMatDsptr aAme, pAmept, ppAmeptpt, pAOept, ppAOeptpt; FMatDsptr pprOeOpEpt; std::shared_ptr>>> ppAOepEpt; + }; using EndFrmqctptr = std::shared_ptr; } diff --git a/MbDCode/EulerAngleszxz.h b/MbDCode/EulerAngleszxz.h index 00dfa57..04311ae 100644 --- a/MbDCode/EulerAngleszxz.h +++ b/MbDCode/EulerAngleszxz.h @@ -6,7 +6,7 @@ namespace MbD { - template + template class EulerAngleszxz : public EulerArray { //phiA theA psiA aA diff --git a/MbDCode/EulerAngleszxzDDot.cpp b/MbDCode/EulerAngleszxzDDot.cpp new file mode 100644 index 0000000..d4fbc1e --- /dev/null +++ b/MbDCode/EulerAngleszxzDDot.cpp @@ -0,0 +1 @@ +#include "EulerAngleszxzDDot.h" diff --git a/MbDCode/EulerAngleszxzDDot.h b/MbDCode/EulerAngleszxzDDot.h new file mode 100644 index 0000000..61e750e --- /dev/null +++ b/MbDCode/EulerAngleszxzDDot.h @@ -0,0 +1,18 @@ +#pragma once + +#include "EulerArray.h" +#include "EulerAngleszxzDot.h" + +namespace MbD { + + template + class EulerAngleszxzDDot : public EulerArray + { + //phiThePsiDot phiAddot theAddot psiAddot aAddot + public: + + std::shared_ptr> phiThePsiDot; + FMatDsptr phiAddot, theAddot, psiAddot, aAddot; + }; +} + diff --git a/MbDCode/EulerAngleszxzDot.cpp b/MbDCode/EulerAngleszxzDot.cpp new file mode 100644 index 0000000..16ac9e9 --- /dev/null +++ b/MbDCode/EulerAngleszxzDot.cpp @@ -0,0 +1 @@ +#include "EulerAngleszxzDot.h" diff --git a/MbDCode/EulerAngleszxzDot.h b/MbDCode/EulerAngleszxzDot.h new file mode 100644 index 0000000..25555a8 --- /dev/null +++ b/MbDCode/EulerAngleszxzDot.h @@ -0,0 +1,73 @@ +#pragma once + +#include "EulerArray.h" +#include "EulerAngleszxz.h" + +namespace MbD { + + template + class EulerAngleszxzDot : public EulerArray + { + //phiThePsi phiAdot theAdot psiAdot aAdot + public: + EulerAngleszxzDot() : EulerArray(3) {} + void initialize() override; + void calc(); + + std::shared_ptr> phiThePsi; + FMatDsptr phiAdot, theAdot, psiAdot, aAdot; + }; + template + inline void EulerAngleszxzDot::initialize() + { + phiAdot = std::make_shared>(3, 3); + phiAdot->zeroSelf(); + theAdot = std::make_shared>(3, 3); + theAdot->zeroSelf(); + psiAdot = std::make_shared>(3, 3); + psiAdot->zeroSelf(); + } + 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); + auto phidot = this->at(0); + auto minussphiTimesphidot = -(sphi * phidot); + auto cphiTimesphidot = cphi * phidot; + auto the = phiThePsi->at(1); + auto sthe = std::sin(the); + auto cthe = std::cos(the); + auto thedot = this->at(1); + auto minusstheTimesthedot = -(sthe * thedot); + auto ctheTimesthedot = cthe * thedot; + auto psi = phiThePsi->at(2); + auto spsi = std::sin(psi); + auto cpsi = std::cos(psi); + auto psidot = this->at(2); + auto minusspsiTimespsidot = -(spsi * psidot); + auto cpsiTimespsidot = cpsi * psidot; + phiAdot->at(0)->at(0) = minussphiTimesphidot; + phiAdot->at(0)->at(1) = -cphiTimesphidot; + phiAdot->at(1)->at(0) = cphiTimesphidot; + phiAdot->at(1)->at(1) = minussphiTimesphidot; + theAdot->at(1)->at(1) = minusstheTimesthedot; + theAdot->at(1)->at(2) = -ctheTimesthedot; + theAdot->at(2)->at(1) = ctheTimesthedot; + theAdot->at(2)->at(2) = minusstheTimesthedot; + psiAdot->at(0)->at(0) = minusspsiTimespsidot; + psiAdot->at(0)->at(1) = -cpsiTimespsidot; + psiAdot->at(1)->at(0) = cpsiTimespsidot; + psiAdot->at(1)->at(1) = minusspsiTimespsidot; + auto phiA = phiThePsi->phiA; + auto theA = phiThePsi->theA; + auto psiA = phiThePsi->psiA; + auto term1 = phiAdot->timesFullMatrix(theA->timesFullMatrix(psiA)); + auto term2 = phiA->timesFullMatrix(theAdot->timesFullMatrix(psiA)); + auto term3 = phiA->timesFullMatrix(theA->timesFullMatrix(psiAdot)); + aAdot = (term1->plusFullMatrix(term2))->plusFullMatrix(term3); + } +} + diff --git a/MbDCode/EulerArray.h b/MbDCode/EulerArray.h index a3dd724..82e303c 100644 --- a/MbDCode/EulerArray.h +++ b/MbDCode/EulerArray.h @@ -3,7 +3,7 @@ namespace MbD { - template + template class EulerArray : public FullColumn { // @@ -12,10 +12,23 @@ namespace MbD { EulerArray(int count, const T& value) : FullColumn(count, value) {} EulerArray(std::initializer_list list) : FullColumn{ list } {} virtual void initialize(); + void equalFullColumn(std::shared_ptr> fullCol); + void equalFullColumnAt(std::shared_ptr> fullCol, int i); + }; template inline void EulerArray::initialize() { } + template + inline void EulerArray::equalFullColumn(std::shared_ptr> fullCol) + { + this->equalArrayAt(fullCol, 0); + } + template + inline void EulerArray::equalFullColumnAt(std::shared_ptr> fullCol, int i) + { + this->equalArrayAt(fullCol, i); + } } diff --git a/MbDCode/EulerConstraint.cpp b/MbDCode/EulerConstraint.cpp index 37f4055..3fb41aa 100644 --- a/MbDCode/EulerConstraint.cpp +++ b/MbDCode/EulerConstraint.cpp @@ -49,6 +49,11 @@ void MbD::EulerConstraint::fillPosICJacob(SpMatDsptr mat) for (int i = 0; i < 4; i++) { auto ii = iqE + i; - (*(mat->at(ii)))[ii] += twolam; + mat->atijplusNumber(ii, ii, twolam); } } + +void MbD::EulerConstraint::fillPosKineJacob(SpMatDsptr mat) +{ + mat->atijplusFullRow(iG, iqE, pGpE); +} diff --git a/MbDCode/EulerConstraint.h b/MbDCode/EulerConstraint.h index 5ae7c65..d2de76d 100644 --- a/MbDCode/EulerConstraint.h +++ b/MbDCode/EulerConstraint.h @@ -17,6 +17,7 @@ namespace MbD { void useEquationNumbers() override; void fillPosICError(FColDsptr col) override; void fillPosICJacob(SpMatDsptr mat) override; + void fillPosKineJacob(SpMatDsptr mat) override; FRowDsptr pGpE; //partial derivative of G wrt pE int iqE = -1; diff --git a/MbDCode/EulerParameters.h b/MbDCode/EulerParameters.h index cdda72f..bb6a1f9 100644 --- a/MbDCode/EulerParameters.h +++ b/MbDCode/EulerParameters.h @@ -5,7 +5,7 @@ namespace MbD { - template + template class EulerParameters : public EulerArray { //aA aB aC pApE @@ -19,6 +19,7 @@ namespace MbD { static std::shared_ptr>>> ppApEpEtimesMatrix(FMatDsptr mat); void initialize(); + void calc(); void calcABC(); void calcpApE(); @@ -127,6 +128,12 @@ namespace MbD { pApE->at(i) = std::make_shared>(3, 3); } } + template + inline void EulerParameters::calc() + { + this->calcABC(); + this->calcpApE(); + } template<> inline void EulerParameters::calcABC() { diff --git a/MbDCode/EulerParametersDot.h b/MbDCode/EulerParametersDot.h index f345ef9..90baf47 100644 --- a/MbDCode/EulerParametersDot.h +++ b/MbDCode/EulerParametersDot.h @@ -4,28 +4,59 @@ #include "FullColumn.h" #include "FullMatrix.h" #include "EulerParameters.h" +#include "CREATE.h" namespace MbD { - template - class EulerParametersDot : public EulerArray - { - //qE aAdot aBdot aCdot pAdotpE - public: - EulerParametersDot(int count) : EulerArray(count) {} - 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); - std::shared_ptr> qE; - FMatDsptr aAdot, aBdot, aCdot; - FColFMatDsptr pAdotpE; - }; + template + class EulerParametersDot : public EulerArray + { + //qE aAdot aBdot aCdot pAdotpE + public: + EulerParametersDot(int count) : EulerArray(count) {} + 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 calcAdotBdotCdot(); + void calcpAdotpE(); - template - inline std::shared_ptr> EulerParametersDot::FromqEOpAndOmegaOpO(std::shared_ptr> qe, FColDsptr omeOpO) - { - return std::shared_ptr>(); - } + std::shared_ptr> qE; + FMatDsptr aAdot, aBdot, aCdot; + FColFMatDsptr pAdotpE; + void calc(); + }; + + template + inline std::shared_ptr> EulerParametersDot::FromqEOpAndOmegaOpO(std::shared_ptr> qEOp, FColDsptr omeOpO) + { + auto answer = CREATE>::With(4); + qEOp->calcABC(); + auto aB = qEOp->aB; + answer->equalFullColumn(aB->transposeTimesFullColumn(omeOpO->times(0.5))); + answer->qE = qEOp; + answer->calc(); + return answer; + } + + template + inline void EulerParametersDot::calcAdotBdotCdot() + { + assert(false); + } + + template + inline void EulerParametersDot::calcpAdotpE() + { + assert(false); + } + + template + inline void EulerParametersDot::calc() + { + qE->calc(); + this->calcAdotBdotCdot(); + this->calcpAdotpE(); + } } diff --git a/MbDCode/FullColumn.h b/MbDCode/FullColumn.h index daec667..5082fc6 100644 --- a/MbDCode/FullColumn.h +++ b/MbDCode/FullColumn.h @@ -3,10 +3,13 @@ #include #include "FullVector.h" +//#include "FullRow.h" namespace MbD { + template + class FullRow; - template + template class FullColumn : public FullVector { public: @@ -20,11 +23,13 @@ namespace MbD { std::shared_ptr> times(double a); std::shared_ptr> negated(); void atiputFullColumn(int i, std::shared_ptr> fullCol); + void atiplusFullColumn(int i, std::shared_ptr> fullCol); void equalSelfPlusFullColumnAt(std::shared_ptr> fullCol, int i); void atiminusFullColumn(int i, std::shared_ptr> fullCol); void equalFullColumnAt(std::shared_ptr> fullCol, int i); std::shared_ptr> copy(); - + std::shared_ptr> transpose(); + virtual std::ostream& printOn(std::ostream& s) const; friend std::ostream& operator<<(std::ostream& s, const FullColumn& fullCol) { @@ -76,6 +81,14 @@ namespace MbD { } } template + inline void FullColumn::atiplusFullColumn(int i, std::shared_ptr> fullCol) + { + for (int ii = 0; ii < fullCol->size(); ii++) + { + this->at(i + ii) += fullCol->at(ii); + } + } + template inline void FullColumn::equalSelfPlusFullColumnAt(std::shared_ptr> fullCol, int ii) { //self is subcolumn of fullCol @@ -94,12 +107,13 @@ namespace MbD { } } template - inline void FullColumn::equalFullColumnAt(std::shared_ptr> fullCol, int ii) + inline void FullColumn::equalFullColumnAt(std::shared_ptr> fullCol, int i) { - for (int i = 0; i < this->size(); i++) - { - this->at(i) = fullCol->at(ii + i); - } + this->equalArrayAt(fullCol, i); + //for (int ii = 0; ii < this->size(); ii++) + //{ + // this->at(ii) = fullCol->at(i + ii); + //} } template<> inline std::shared_ptr> FullColumn::copy() @@ -113,6 +127,11 @@ namespace MbD { return answer; } template + inline std::shared_ptr> FullColumn::transpose() + { + return std::make_shared>(*this); + } + template inline std::ostream& FullColumn::printOn(std::ostream& s) const { s << "{"; diff --git a/MbDCode/FullMatrix.h b/MbDCode/FullMatrix.h index 4d8e61c..5520628 100644 --- a/MbDCode/FullMatrix.h +++ b/MbDCode/FullMatrix.h @@ -4,17 +4,17 @@ #include "RowTypeMatrix.h" namespace MbD { - template + template class FullColumn; - template + template class FullRow; - template + template class FullMatrix : public RowTypeMatrix>> { public: FullMatrix() {} - FullMatrix(int m) : RowTypeMatrix>>(m) + FullMatrix(int m) : RowTypeMatrix>>(m) { } FullMatrix(int m, int n) { @@ -47,10 +47,17 @@ namespace MbD { std::shared_ptr> negated(); void symLowerWithUpper(); 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); + void atijminusNumber(int i, int j, double value); double sumOfSquares() override; void zeroSelf() override; + std::shared_ptr> copy(); + std::shared_ptr> operator+(const std::shared_ptr> fullMat); + std::shared_ptr> transposeTimesFullColumn(const std::shared_ptr> fullCol); + }; - template <> + template<> inline void FullMatrix::identity() { this->zeroSelf(); for (int i = 0; i < this->size(); i++) { @@ -59,7 +66,7 @@ namespace MbD { } template inline std::shared_ptr> FullMatrix::column(int j) { - int n = (int) this->size(); + int n = (int)this->size(); auto answer = std::make_shared>(n); for (int i = 0; i < n; i++) { answer->at(i) = this->at(i)->at(j); @@ -70,7 +77,7 @@ namespace MbD { inline std::shared_ptr> FullMatrix::timesFullColumn(std::shared_ptr> fullCol) { //"a*b = a(i,j)b(j) sum j." - int n = (int) this->size(); + int n = (int)this->size(); auto answer = std::make_shared>(n); for (int i = 0; i < n; i++) { @@ -111,7 +118,7 @@ namespace MbD { template inline std::shared_ptr> FullMatrix::plusFullMatrix(std::shared_ptr> fullMat) { - int n = (int) this->size(); + int n = (int)this->size(); auto answer = std::make_shared>(n); for (int i = 0; i < n; i++) { answer->at(i) = this->at(i)->plusFullRow(fullMat->at(i)); @@ -140,7 +147,7 @@ namespace MbD { template inline void FullMatrix::symLowerWithUpper() { - int n = (int) this->size(); + int n = (int)this->size(); for (int i = 0; i < n; i++) { for (int j = i + 1; j < n; j++) { this->at(j)->at(i) = this->at(i)->at(j); @@ -150,12 +157,28 @@ namespace MbD { template inline void FullMatrix::atijputFullColumn(int i1, int j1, std::shared_ptr> fullCol) { - auto iOffset = i1 - 1; for (int ii = 0; ii < fullCol->size(); ii++) { - this->at(iOffset + ii)->at(j1) = fullCol->at(ii); + this->at(i1 + ii)->at(j1) = fullCol->at(ii); } } + template + inline void FullMatrix::atijplusFullRow(int i, int j, std::shared_ptr> fullRow) + { + this->at(i)->atiplusFullRow(j, fullRow); + } + template + inline void FullMatrix::atijplusNumber(int i, int j, double value) + { + auto rowi = this->at(i); + rowi->at(j) += value; + } + template + inline void FullMatrix::atijminusNumber(int i, int j, double value) + { + auto rowi = this->at(i); + rowi->at(j) -= value; + } template<> inline double FullMatrix::sumOfSquares() { @@ -184,6 +207,31 @@ namespace MbD { { assert(false); } + template + inline std::shared_ptr> FullMatrix::copy() + { + auto m = (int)this->size(); + auto answer = std::make_shared>(m); + for (int i = 0; i < m; i++) + { + answer->at(i) = this->at(i)->copy(); + } + return answer; + } + template + inline std::shared_ptr> FullMatrix::operator+(const std::shared_ptr> fullMat) + { + return this->plusFullMatrix(fullMat); + } + 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(); + } using FMatDsptr = std::shared_ptr>; using FMatDsptr = std::shared_ptr>; using FMatFColDsptr = std::shared_ptr>>>; diff --git a/MbDCode/FullRow.h b/MbDCode/FullRow.h index 451c40f..f100c9e 100644 --- a/MbDCode/FullRow.h +++ b/MbDCode/FullRow.h @@ -1,18 +1,21 @@ #pragma once #include "FullVector.h" -#include "FullColumn.h" +//#include "FullColumn.h" namespace MbD { - template + template class FullMatrix; + template + class FullColumn; - template + template class FullRow : public FullVector { public: - FullRow() {} + FullRow(std::vector vec) : FullVector(vec) {} FullRow(int count) : FullVector(count) {} FullRow(int count, const T& value) : FullVector(count, value) {} + FullRow(std::vector::iterator begin, std::vector::iterator end) : FullVector(begin, end) {} FullRow(std::initializer_list list) : FullVector{ list } {} std::shared_ptr> times(double a); std::shared_ptr> negated(); @@ -23,6 +26,8 @@ namespace MbD { std::shared_ptr> timesTransposeFullMatrix(std::shared_ptr> fullMat); void equalSelfPlusFullRowTimes(std::shared_ptr> fullRow, double factor); std::shared_ptr> transpose(); + std::shared_ptr> copy(); + void atiplusFullRow(int j, std::shared_ptr> fullRow); }; template @@ -94,6 +99,26 @@ namespace MbD { { return std::make_shared>(*this); } + template<> + inline std::shared_ptr> FullRow::copy() + { + auto n = (int)this->size(); + auto answer = std::make_shared>(n); + for (int i = 0; i < n; i++) + { + answer->at(i) = this->at(i); + } + return answer; + } + template + inline void FullRow::atiplusFullRow(int j1, std::shared_ptr> fullRow) + { + for (int jj = 0; jj < fullRow->size(); jj++) + { + auto j = j1 + jj; + this->at(j) += fullRow->at(jj); + } + } template inline std::shared_ptr> FullRow::timesFullMatrix(std::shared_ptr> fullMat) { diff --git a/MbDCode/FullVector.h b/MbDCode/FullVector.h index 5c71314..a30aef6 100644 --- a/MbDCode/FullVector.h +++ b/MbDCode/FullVector.h @@ -2,7 +2,7 @@ #include "Array.h" namespace MbD { - template + template class FullVector : public Array { public: @@ -14,11 +14,13 @@ namespace MbD { FullVector(std::initializer_list list) : Array{ list } {} double dot(std::shared_ptr> vec); 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 atiplusFullVectortimes(int i, std::shared_ptr fullVec, double factor); + double maxMagnitude(); }; template @@ -36,6 +38,11 @@ namespace MbD { { this->at(i) += value; } + template + inline void FullVector::atiminusNumber(int i, T value) + { + this->at(i) -= value; + } template<> inline double FullVector::sumOfSquares() { @@ -62,7 +69,7 @@ namespace MbD { inline void FullVector::zeroSelf() { for (int i = 0; i < this->size(); i++) { - this->at(i) = 0.0;; + this->at(i) = 0.0; } } template @@ -84,4 +91,15 @@ namespace MbD { this->at(i) += fullVec->at(ii) * factor; } } + template<> + inline double FullVector::maxMagnitude() + { + auto answer = 0.0; + for (int i = 0; i < this->size(); i++) + { + auto mag = std::abs(this->at(i)); + if (answer < mag) answer = mag; + } + return answer; + } } diff --git a/MbDCode/GEFullMat.cpp b/MbDCode/GEFullMat.cpp index 86b6e50..9ebb885 100644 --- a/MbDCode/GEFullMat.cpp +++ b/MbDCode/GEFullMat.cpp @@ -11,7 +11,18 @@ void MbD::GEFullMat::forwardEliminateWithPivot(int p) void MbD::GEFullMat::backSubstituteIntoDU() { - assert(false); + answerX = std::make_shared>(n); + answerX->at(n - 1) = rightHandSideB->at(m - 1) / matrixA->at(m - 1)->at(n - 1); + for (int i = n - 2; i >= 0; i--) + { + auto rowi = matrixA->at(i); + double sum = answerX->at(n) * rowi->at(n); + for (int j = i + 1; j < n - 1; j++) + { + sum += answerX->at(j) * rowi->at(j); + } + answerX->at(i) = (rightHandSideB->at(i) - sum) / rowi->at(i); + } } void MbD::GEFullMat::postSolve() @@ -19,7 +30,36 @@ void MbD::GEFullMat::postSolve() assert(false); } +void MbD::GEFullMat::preSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal) +{ + assert(false); +} + void MbD::GEFullMat::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) { assert(false); } + +double MbD::GEFullMat::getmatrixArowimaxMagnitude(int i) +{ + return matrixA->at(i)->maxMagnitude(); +} + +FColDsptr MbD::GEFullMat::basicSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal) +{ + this->preSolvewithsaveOriginal(fullMat, fullCol, saveOriginal); + for (int p = 0; p < m; p++) + { + this->doPivoting(p); + this->forwardEliminateWithPivot(p); + } + this->backSubstituteIntoDU(); + this->postSolve(); + return answerX; +} + +FColDsptr MbD::GEFullMat::basicSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) +{ + assert(false); + return FColDsptr(); +} diff --git a/MbDCode/GEFullMat.h b/MbDCode/GEFullMat.h index 8ed60ae..fd48734 100644 --- a/MbDCode/GEFullMat.h +++ b/MbDCode/GEFullMat.h @@ -7,11 +7,16 @@ namespace MbD { { // public: - std::shared_ptr> matrixA; void forwardEliminateWithPivot(int p) override; void backSubstituteIntoDU() override; void postSolve() override; + FColDsptr basicSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal) override; + FColDsptr basicSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) override; + void preSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal) override; void preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) override; + double getmatrixArowimaxMagnitude(int i) override; + + std::shared_ptr> matrixA; }; } diff --git a/MbDCode/GEFullMatFullPv.cpp b/MbDCode/GEFullMatFullPv.cpp index 0439fe8..b80e3f6 100644 --- a/MbDCode/GEFullMatFullPv.cpp +++ b/MbDCode/GEFullMatFullPv.cpp @@ -1,12 +1,48 @@ #include #include "GEFullMatFullPv.h" +#include "SingularMatrixError.h" using namespace MbD; void MbD::GEFullMatFullPv::doPivoting(int p) { - assert(false); + //"Do full pivoting." + + //| max pivotRow pivotCol rowi aij mag | + double max = 0.0; + int pivotRow = p; + int pivotCol = p; + for (int i = p; i < m; i++) + { + auto rowi = matrixA->at(i); + for (int j = p; j < n; j++) + { + auto aij = rowi->at(j); + if (aij != 0.0) { + auto mag = aij; + if (mag < 0.0) mag = -mag; + if (max < mag) { + max = mag; + pivotRow = i; + pivotCol = j; + } + } + } + } + if (p != pivotRow) { + matrixA->swapElems(p, pivotRow); + rightHandSideB->swapElems(p, pivotRow); + rowOrder->swapElems(p, pivotRow); + } + if (p != pivotCol) { + for (auto& rowi : *matrixA) { + rowi->swapElems(p, pivotCol); + } + colOrder->swapElems(p, pivotCol); + } + pivotValues->at(p) = max; + if (max < singularPivotTolerance) throw SingularMatrixError(""); } void MbD::GEFullMatFullPv::postSolve() diff --git a/MbDCode/GEFullMatParPv.cpp b/MbDCode/GEFullMatParPv.cpp index eb5e5b4..c94477f 100644 --- a/MbDCode/GEFullMatParPv.cpp +++ b/MbDCode/GEFullMatParPv.cpp @@ -1,12 +1,39 @@ #include #include "GEFullMatParPv.h" +#include "SingularMatrixError.h" using namespace MbD; void MbD::GEFullMatParPv::doPivoting(int p) { - assert(false); + //"Use scalings. Do row pivoting." + + //| app max rowPivot aip mag | + auto app = matrixA->at(p)->at(p); + auto max = app * rowScalings->at(p); + if (max < 0.0) max = -max; + auto rowPivot = p; + for (int i = p + 1; i < m; i++) + { + auto aip = matrixA->at(i)->at(p); + if (aip != 0.0) { + auto mag = aip * rowScalings->at(i); + if (mag < 0) mag = -mag; + if (max < mag) { + max = mag; + rowPivot = i; + } + } + } + if (p != rowPivot) { + matrixA->swapElems(p, rowPivot); + rightHandSideB->swapElems(p, rowPivot); + rowScalings->swapElems(p, rowPivot); + rowOrder->swapElems(p, rowPivot); + } + pivotValues->at(p) = max; + if (max < singularPivotTolerance) throw SingularMatrixError(""); } void MbD::GEFullMatParPv::postSolve() @@ -14,7 +41,7 @@ void MbD::GEFullMatParPv::postSolve() assert(false); } -void MbD::GEFullMatParPv::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) +void MbD::GEFullMatParPv::preSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal) { assert(false); } diff --git a/MbDCode/GEFullMatParPv.h b/MbDCode/GEFullMatParPv.h index 207b9f9..4f00357 100644 --- a/MbDCode/GEFullMatParPv.h +++ b/MbDCode/GEFullMatParPv.h @@ -9,7 +9,7 @@ namespace MbD { public: void doPivoting(int p) override; void postSolve() override; - void preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) override; + void preSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal) override; }; } diff --git a/MbDCode/GESpMat.cpp b/MbDCode/GESpMat.cpp index be7d1e1..ebd211e 100644 --- a/MbDCode/GESpMat.cpp +++ b/MbDCode/GESpMat.cpp @@ -9,3 +9,37 @@ FColDsptr MbD::GESpMat::solvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCo this->timedSolvewithsaveOriginal(spMat, fullCol, saveOriginal); return answerX; } + +FColDsptr MbD::GESpMat::basicSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) +{ + this->preSolvewithsaveOriginal(spMat, fullCol, saveOriginal); + for (int p = 0; p < m; p++) + { + this->doPivoting(p); + this->forwardEliminateWithPivot(p); + } + this->backSubstituteIntoDU(); + this->postSolve(); + return answerX; +} + +FColDsptr MbD::GESpMat::basicSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal) +{ + assert(false); + return FColDsptr(); +} + +void MbD::GESpMat::preSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal) +{ + assert(false); +} + +void MbD::GESpMat::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) +{ + assert(false); +} + +double MbD::GESpMat::getmatrixArowimaxMagnitude(int i) +{ + return matrixA->at(i)->maxMagnitude(); +} diff --git a/MbDCode/GESpMat.h b/MbDCode/GESpMat.h index bb4cff5..4389c1d 100644 --- a/MbDCode/GESpMat.h +++ b/MbDCode/GESpMat.h @@ -9,10 +9,15 @@ namespace MbD { //markowitzPivotRowCount markowitzPivotColCount privateIndicesOfNonZerosInPivotRow rowPositionsOfNonZerosInPivotColumn public: FColDsptr solvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal); + FColDsptr basicSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) override; + FColDsptr basicSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal) override; + void preSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal) override; + void preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) override; + double getmatrixArowimaxMagnitude(int i) override; std::shared_ptr> matrixA; int markowitzPivotRowCount, markowitzPivotColCount; - std::shared_ptr> privateIndicesOfNonZerosInPivotRow, rowPositionsOfNonZerosInPivotColumn; + std::shared_ptr> rowPositionsOfNonZerosInPivotColumn; }; } diff --git a/MbDCode/GESpMatFullPv.cpp b/MbDCode/GESpMatFullPv.cpp index bddf335..8386243 100644 --- a/MbDCode/GESpMatFullPv.cpp +++ b/MbDCode/GESpMatFullPv.cpp @@ -7,9 +7,52 @@ using namespace MbD; void MbD::GESpMatFullPv::doPivoting(int p) { - assert(false); -} + //"Used by Gauss Elimination only." + //"Do full pivoting." + //"Swap rows but keep columns in place." + //"The elements below the diagonal are removed column by column." + auto max = 0.0; + auto pivotRow = p; + auto pivotCol = p; + for (int j = p; j < n; j++) + { + rowPositionsOfNonZerosInColumns->at(colOrder->at(j))->clear(); + } + for (int i = p; i < m; i++) + { + auto& rowi = matrixA->at(i); + for (auto const& kv : *rowi) { + rowPositionsOfNonZerosInColumns->at(kv.first)->push_back(i); + auto aij = kv.second; + auto mag = aij; + if (mag < 0.0) mag = -mag; + if (max < mag) { + max = mag; + pivotRow = i; + pivotCol = positionsOfOriginalCols->at(kv.first); + } + } + } + if (p != pivotRow) { + matrixA->swapElems(p, pivotRow); + rightHandSideB->swapElems(p, pivotRow); + rowOrder->swapElems(p, pivotRow); + } + if (p != pivotCol) { + colOrder->swapElems(p, pivotCol); + positionsOfOriginalCols->at(colOrder->at(p)) = p; + positionsOfOriginalCols->at(colOrder->at(pivotCol)) = pivotCol; + } + pivotValues->at(p) = max; + if (max < singularPivotTolerance) throw SingularMatrixError(""); + auto jp = colOrder->at(p); + rowPositionsOfNonZerosInPivotColumn = rowPositionsOfNonZerosInColumns->at(jp); + if (rowPositionsOfNonZerosInPivotColumn->front() == p) { + rowPositionsOfNonZerosInPivotColumn->erase(rowPositionsOfNonZerosInPivotColumn->begin()); + } + markowitzPivotColCount = (int)rowPositionsOfNonZerosInPivotColumn->size(); +} void MbD::GESpMatFullPv::forwardEliminateWithPivot(int p) { //app is pivot. @@ -63,7 +106,7 @@ void MbD::GESpMatFullPv::backSubstituteIntoDU() //auto rhsZeroElement = this->rhsZeroElement(); for (int i = n - 2; i >= 0; i--) { - auto rowi = matrixA->at(i); + auto& rowi = matrixA->at(i); sum = 0.0; // rhsZeroElement copy. for (auto const& keyValue : *rowi) { auto jj = keyValue.first; @@ -95,9 +138,8 @@ void MbD::GESpMatFullPv::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fu matrixA = std::make_shared>(m); pivotValues = std::make_shared>(m); rowOrder = std::make_shared>(m); - colOrder = std::make_shared>(n); + colOrder = std::make_shared>(n); positionsOfOriginalCols = std::make_shared>(m); - privateIndicesOfNonZerosInPivotRow = std::make_shared>(); rowPositionsOfNonZerosInColumns = std::make_shared>>>(n); for (int j = 0; j < n; j++) { @@ -113,11 +155,11 @@ void MbD::GESpMatFullPv::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fu for (int i = 0; i < m; i++) { auto& spRowi = spMat->at(i); - auto maxRowElement = spRowi->maxElement(); - if (maxRowElement == 0) { + auto maxRowMagnitude = spRowi->maxMagnitude(); + if (maxRowMagnitude == 0) { throw SingularMatrixError(""); } - matrixA->at(i) = spRowi->conditionedWithTol(singularPivotTolerance * maxRowElement); + matrixA->at(i) = spRowi->conditionedWithTol(singularPivotTolerance * maxRowMagnitude); rowOrder->at(i) = i; colOrder->at(i) = i; positionsOfOriginalCols->at(i) = i; diff --git a/MbDCode/GESpMatFullPvPosIC.cpp b/MbDCode/GESpMatFullPvPosIC.cpp index f68a014..8bedddb 100644 --- a/MbDCode/GESpMatFullPvPosIC.cpp +++ b/MbDCode/GESpMatFullPvPosIC.cpp @@ -32,7 +32,7 @@ void MbD::GESpMatFullPvPosIC::doPivoting(int p) { rowPositionsOfNonZerosInColumns->at(colOrder->at(j))->clear(); } - if (p == pivotRowLimit) { + if (p >= pivotRowLimit) { pivotRowLimit = *std::find_if( pivotRowLimits->begin(), pivotRowLimits->end(), [&](int limit) { return limit > p; }); @@ -53,12 +53,12 @@ void MbD::GESpMatFullPvPosIC::doPivoting(int p) } } if (p != pivotRow) { - matrixA->swapRows(p, pivotRow); - rightHandSideB->swapRows(p, pivotRow); - rowOrder->swapRows(p, pivotRow); + matrixA->swapElems(p, pivotRow); + rightHandSideB->swapElems(p, pivotRow); + rowOrder->swapElems(p, pivotRow); } if (p != pivotCol) { - colOrder->swapRows(p, pivotCol); + colOrder->swapElems(p, pivotCol); positionsOfOriginalCols->at(colOrder->at(p)) = p; positionsOfOriginalCols->at(colOrder->at(pivotCol)) = pivotCol; } diff --git a/MbDCode/GESpMatParPvMarko.cpp b/MbDCode/GESpMatParPvMarko.cpp index e21a5e5..597ae57 100644 --- a/MbDCode/GESpMatParPvMarko.cpp +++ b/MbDCode/GESpMatParPvMarko.cpp @@ -1,12 +1,70 @@ #include #include "GESpMatParPvMarko.h" +#include "SingularMatrixError.h" using namespace MbD; void MbD::GESpMatParPvMarko::doPivoting(int p) { - assert(false); + //"Search from bottom to top." + //"Check for singular pivot." + //"Do scaling. Do partial pivoting." + //"criterion := mag / (2.0d raisedTo: rowiCount)." + //| lookForFirstNonZeroInPivotCol i rowi aip criterionMax rowPivoti criterion max | + int i, rowPivoti; + double aip, mag, max, criterion, criterionMax; + SpRowDsptr spRowi; + rowPositionsOfNonZerosInPivotColumn->clear(); + auto lookForFirstNonZeroInPivotCol = true; + i = m - 1; + while (lookForFirstNonZeroInPivotCol) { + spRowi = matrixA->at(i); + if (spRowi->find(p) == spRowi->end()) { + if (i <= p) throw SingularMatrixError(""); + } + else { + markowitzPivotColCount = 0; + aip = spRowi->at(p); + mag = aip * rowScalings->at(i); + if (mag < 0) mag = -mag; + max = mag; + criterionMax = mag / std::pow(2.0, spRowi->size()); + rowPivoti = i; + lookForFirstNonZeroInPivotCol = false; + } + i--; + } + while (i >= p) { + spRowi = matrixA->at(i); + if (spRowi->find(p) == spRowi->end()) { + aip = std::numeric_limits::min(); + } + else { + aip = spRowi->at(p); + markowitzPivotColCount++; + mag = aip * rowScalings->at(i); + if (mag < 0) mag = -mag; + criterion = mag / std::pow(2.0, spRowi->size()); + if (criterion > criterionMax) { + max = mag; + criterionMax = criterion; + rowPositionsOfNonZerosInPivotColumn->push_back(rowPivoti); + rowPivoti = i; + } + else { + rowPositionsOfNonZerosInPivotColumn->push_back(i); + } + } + i--; + } + if (p != rowPivoti) { + matrixA->swapElems(p, rowPivoti); + rightHandSideB->swapElems(p, rowPivoti); + rowScalings->swapElems(p, rowPivoti); + if (aip != std::numeric_limits::min()) rowPositionsOfNonZerosInPivotColumn->at(markowitzPivotColCount - 1) = rowPivoti; + } + if (max < singularPivotTolerance) throw SingularMatrixError(""); } void MbD::GESpMatParPvMarko::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) diff --git a/MbDCode/GESpMatParPvMarkoFast.cpp b/MbDCode/GESpMatParPvMarkoFast.cpp index 6204ae3..8b9b80e 100644 --- a/MbDCode/GESpMatParPvMarkoFast.cpp +++ b/MbDCode/GESpMatParPvMarkoFast.cpp @@ -14,7 +14,6 @@ void MbD::GESpMatParPvMarkoFast::preSolvewithsaveOriginal(SpMatDsptr spMat, FCol m = spMat->nrow(); n = spMat->ncol(); matrixA = std::make_shared>(m); - privateIndicesOfNonZerosInPivotRow = std::make_shared>(); rowPositionsOfNonZerosInPivotColumn = std::make_shared>(); } if (saveOriginal) { @@ -26,11 +25,11 @@ void MbD::GESpMatParPvMarkoFast::preSolvewithsaveOriginal(SpMatDsptr spMat, FCol for (int i = 0; i < m; i++) { auto& spRowi = spMat->at(i); - auto maxRowElement = spRowi->maxElement(); - if (maxRowElement == 0) { + auto maxRowMagnitude = spRowi->maxMagnitude(); + if (maxRowMagnitude == 0) { throw SingularMatrixError(""); } - auto scaling = 1.0 / maxRowElement; + auto scaling = 1.0 / maxRowMagnitude; matrixA->at(i) = spRowi->timesconditionedWithTol(scaling, singularPivotTolerance); rightHandSideB->atitimes(i, scaling); } @@ -58,8 +57,8 @@ void MbD::GESpMatParPvMarkoFast::doPivoting(int p) if (i <= p) throw SingularMatrixError(""); } else { - aip = spRowi->at(p); markowitzPivotColCount = 0; + aip = spRowi->at(p); if (aip < 0) aip = -aip; max = aip; criterionMax = aip / std::pow(2.0, spRowi->size()); @@ -91,8 +90,8 @@ void MbD::GESpMatParPvMarkoFast::doPivoting(int p) i--; } if (p != rowPivoti) { - matrixA->swapRows(p, rowPivoti); - rightHandSideB->swapRows(p, rowPivoti); + matrixA->swapElems(p, rowPivoti); + rightHandSideB->swapElems(p, rowPivoti); if (aip != std::numeric_limits::min()) rowPositionsOfNonZerosInPivotColumn->at(markowitzPivotColCount - 1) = rowPivoti; } if (max < singularPivotTolerance) throw SingularMatrixError(""); diff --git a/MbDCode/GESpMatParPvPrecise.cpp b/MbDCode/GESpMatParPvPrecise.cpp index c174ac5..05a3c43 100644 --- a/MbDCode/GESpMatParPvPrecise.cpp +++ b/MbDCode/GESpMatParPvPrecise.cpp @@ -1,15 +1,97 @@ #include #include "GESpMatParPvPrecise.h" +#include "SingularMatrixError.h" using namespace MbD; void MbD::GESpMatParPvPrecise::doPivoting(int p) { - assert(false); + //"Search from bottom to top." + //"Use scaling vector and partial pivoting with actual swapping of rows." + //"Check for singular pivot." + //"Do scaling. Do partial pivoting." + //| max rowPivot aip mag lookForFirstNonZeroInPivotCol i | + int i, rowPivoti; + double aip, mag, max; + SpRowDsptr spRowi; + rowPositionsOfNonZerosInPivotColumn->clear(); + auto lookForFirstNonZeroInPivotCol = true; + i = m - 1; + while (lookForFirstNonZeroInPivotCol) { + spRowi = matrixA->at(i); + if (spRowi->find(p) == spRowi->end()) { + if (i <= p) throw SingularMatrixError(""); + } + else { + markowitzPivotColCount = 0; + aip = spRowi->at(p); + mag = aip * rowScalings->at(i); + if (mag < 0) mag = -mag; + max = mag; + rowPivoti = i; + lookForFirstNonZeroInPivotCol = false; + } + i--; + } + while (i >= p) { + spRowi = matrixA->at(i); + if (spRowi->find(p) == spRowi->end()) { + aip = std::numeric_limits::min(); + } + else { + aip = spRowi->at(p); + markowitzPivotColCount++; + mag = aip * rowScalings->at(i); + if (mag < 0) mag = -mag; + if (mag > max) { + max = mag; + rowPositionsOfNonZerosInPivotColumn->push_back(rowPivoti); + rowPivoti = i; + } + else { + rowPositionsOfNonZerosInPivotColumn->push_back(i); + } + } + i--; + } + if (p != rowPivoti) { + matrixA->swapElems(p, rowPivoti); + rightHandSideB->swapElems(p, rowPivoti); + rowScalings->swapElems(p, rowPivoti); + rowOrder->swapElems(p, rowPivoti); + if (aip != std::numeric_limits::min()) rowPositionsOfNonZerosInPivotColumn->at(markowitzPivotColCount - 1) = rowPivoti; + } + pivotValues->at(p) = max; + if (max < singularPivotTolerance) throw SingularMatrixError(""); } void MbD::GESpMatParPvPrecise::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) { - assert(false); + //assert(false); + //"A conditioned copy of aMatrix is solved." + if (m != spMat->nrow() || n != spMat->ncol()) { + m = spMat->nrow(); + n = spMat->ncol(); + matrixA = std::make_shared>(m); + rowScalings = std::make_shared>(m); + pivotValues = std::make_shared>(m); + rowOrder = 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) throw SingularMatrixError(""); + rowScalings->at(i) = 1.0 / maxRowMagnitude; + matrixA->at(i) = spRowi->conditionedWithTol(singularPivotTolerance * maxRowMagnitude); + rowOrder->at(i) = i; + } } diff --git a/MbDCode/ICKineIntegrator.cpp b/MbDCode/ICKineIntegrator.cpp index c9fc019..7964a74 100644 --- a/MbDCode/ICKineIntegrator.cpp +++ b/MbDCode/ICKineIntegrator.cpp @@ -1 +1,9 @@ #include "ICKineIntegrator.h" +#include "SystemSolver.h" + +void MbD::ICKineIntegrator::runInitialConditionTypeSolution() +{ + system->runPosICKine(); + system->runVelICKine(); + system->runAccICKine(); +} diff --git a/MbDCode/ICKineIntegrator.h b/MbDCode/ICKineIntegrator.h index 9557076..c196525 100644 --- a/MbDCode/ICKineIntegrator.h +++ b/MbDCode/ICKineIntegrator.h @@ -6,6 +6,7 @@ namespace MbD { { // public: + void runInitialConditionTypeSolution() override; }; } diff --git a/MbDCode/Integrator.cpp b/MbDCode/Integrator.cpp index a86356d..d67e5f4 100644 --- a/MbDCode/Integrator.cpp +++ b/MbDCode/Integrator.cpp @@ -1,3 +1,7 @@ #include "Integrator.h" using namespace MbD; + +void MbD::Integrator::setSystem(Solver* sys) +{ +} diff --git a/MbDCode/Integrator.h b/MbDCode/Integrator.h index eee8258..d2ee323 100644 --- a/MbDCode/Integrator.h +++ b/MbDCode/Integrator.h @@ -9,6 +9,8 @@ namespace MbD { { //system direction public: + void setSystem(Solver* sys) override; + virtual void runInitialConditionTypeSolution() = 0; double direction = 1; }; diff --git a/MbDCode/IntegratorInterface.cpp b/MbDCode/IntegratorInterface.cpp index 4af6d4d..6979264 100644 --- a/MbDCode/IntegratorInterface.cpp +++ b/MbDCode/IntegratorInterface.cpp @@ -17,9 +17,9 @@ tend = system->endTime(); direction = (tstart < tend) ? 1.0 : -1.0; } -void MbD::IntegratorInterface::setSystem(SystemSolver* sys) +void MbD::IntegratorInterface::setSystem(Solver* sys) { - system = sys; + system = static_cast(sys); } void MbD::IntegratorInterface::logString(std::string& str) @@ -44,3 +44,8 @@ int MbD::IntegratorInterface::orderMax() { return system->orderMax; } + +void MbD::IntegratorInterface::incrementTime(double tnew) +{ + system->settime(tnew); +} diff --git a/MbDCode/IntegratorInterface.h b/MbDCode/IntegratorInterface.h index 342d794..049d90b 100644 --- a/MbDCode/IntegratorInterface.h +++ b/MbDCode/IntegratorInterface.h @@ -13,13 +13,15 @@ namespace MbD { void initializeGlobally() override; virtual void preRun() = 0; - void setSystem(SystemSolver* sys); + void setSystem(Solver* sys) override; void logString(std::string& str) override; void run() override; int orderMax(); - virtual void preFirstStep() = 0; - virtual double suggestSmallerOrAcceptFirstStepSize(double hnew) = 0; + virtual void incrementTime(double tnew); + virtual void preFirstStep() = 0; + virtual void preStep() = 0; + virtual double suggestSmallerOrAcceptFirstStepSize(double hnew) = 0; SystemSolver* system; double tout = 0, hout = 0, hmin = 0, hmax = 0, tstart = 0, tend = 0; std::shared_ptr integrator; diff --git a/MbDCode/Item.cpp b/MbDCode/Item.cpp index c8c4368..bc683c0 100644 --- a/MbDCode/Item.cpp +++ b/MbDCode/Item.cpp @@ -59,7 +59,7 @@ void MbD::Item::fillPosKineError(FColDsptr col) { } -void MbD::Item::fillPosKineJacob(FMatDsptr mat) +void MbD::Item::fillPosKineJacob(SpMatDsptr mat) { } @@ -71,6 +71,11 @@ void MbD::Item::fillRedundantConstraints(std::shared_ptr>> allConstraints) +{ + assert(false); +} + void MbD::Item::fillqsu(FColDsptr col) { } @@ -116,6 +121,10 @@ void MbD::Item::preDynStep() { } +void MbD::Item::storeDynState() +{ +} + double MbD::Item::suggestSmallerOrAcceptDynFirstStepSize(double hnew) { //"Default is return hnew." @@ -123,6 +132,41 @@ double MbD::Item::suggestSmallerOrAcceptDynFirstStepSize(double hnew) return hnew; } +void MbD::Item::preVelIC() +{ + //"Assume positions are valid." + //"Called once before solving for velocity initial conditions." + //"Update all variable dependent instance variables needed for velIC even if they have + //been calculated in postPosIC." + //"Variables dependent on t are updated." + + this->calcPostDynCorrectorIteration(); +} + +void MbD::Item::postVelIC() +{ +} + +void MbD::Item::fillqsudot(FColDsptr col) +{ +} + +void MbD::Item::fillqsudotWeights(std::shared_ptr> diagMat) +{ +} + +void MbD::Item::fillVelICError(FColDsptr error) +{ +} + +void MbD::Item::fillVelICJacob(SpMatDsptr jacob) +{ +} + +void MbD::Item::setqsudotlam(FColDsptr qsudotlam) +{ +} + void MbD::Item::constraintsReport() { } @@ -149,16 +193,9 @@ void MbD::Item::prePosIC() calcPostDynCorrectorIteration(); } -void MbD::Item::prePostIC() -{ -} - -void MbD::Item::prePostICIteration() -{ -} - -void MbD::Item::prePostICRestart() +void MbD::Item::prePosKine() { + this->prePosIC(); } void MbD::Item::postPosIC() diff --git a/MbDCode/Item.h b/MbDCode/Item.h index 3ad56da..67dddaa 100644 --- a/MbDCode/Item.h +++ b/MbDCode/Item.h @@ -28,21 +28,21 @@ namespace MbD { virtual void logString(std::string& str); virtual void prePosIC(); + virtual void prePosKine(); + virtual void fillPosICError(FColDsptr col); virtual void fillPosICJacob(FMatDsptr mat); virtual void fillPosICJacob(SpMatDsptr mat); - virtual void prePostIC(); - virtual void prePostICIteration(); - virtual void prePostICRestart(); virtual void postPosIC(); virtual void postPosICIteration(); virtual void removeRedundantConstraints(std::shared_ptr> redundantEqnNos); virtual void reactivateRedundantConstraints(); virtual void fillPosKineError(FColDsptr col); - virtual void fillPosKineJacob(FMatDsptr mat); + virtual void fillPosKineJacob(SpMatDsptr mat); virtual void fillEssenConstraints(std::shared_ptr>> essenConstraints); virtual void fillRedundantConstraints(std::shared_ptr>> redunConstraints); - + virtual void fillConstraints(std::shared_ptr>> allConstraints); + virtual void fillqsu(FColDsptr col); virtual void fillqsuWeights(std::shared_ptr> diagMat); virtual void fillqsulam(FColDsptr col); @@ -52,8 +52,15 @@ namespace MbD { virtual std::string classname(); virtual void preDynFirstStep(); virtual void preDynStep(); + virtual void storeDynState(); virtual double suggestSmallerOrAcceptDynFirstStepSize(double hnew); - + virtual void preVelIC(); + 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 setqsudotlam(FColDsptr qsudotlam); void setName(std::string& str); const std::string& getName() const; diff --git a/MbDCode/Joint.cpp b/MbDCode/Joint.cpp index f1d1ae2..d153bd4 100644 --- a/MbDCode/Joint.cpp +++ b/MbDCode/Joint.cpp @@ -69,6 +69,11 @@ void MbD::Joint::prePosIC() constraintsDo([](std::shared_ptr constraint) { constraint->prePosIC(); }); } +void MbD::Joint::prePosKine() +{ + constraintsDo([](std::shared_ptr constraint) { constraint->prePosKine(); }); +} + void MbD::Joint::fillEssenConstraints(std::shared_ptr>> essenConstraints) { constraintsDo([&](std::shared_ptr con) { con->fillEssenConstraints(con, essenConstraints); }); @@ -89,11 +94,25 @@ void MbD::Joint::fillRedundantConstraints(std::shared_ptr con) { con->fillRedundantConstraints(con, redunConstraints); }); } +void MbD::Joint::fillConstraints(std::shared_ptr>> allConstraints) +{ + constraintsDo([&](std::shared_ptr con) { con->fillConstraints(con, allConstraints); }); +} + void MbD::Joint::fillqsulam(FColDsptr col) { constraintsDo([&](std::shared_ptr con) { con->fillqsulam(col); }); } +void MbD::Joint::fillqsudot(FColDsptr col) +{ + constraintsDo([&](std::shared_ptr con) { con->fillqsudot(col); }); +} + +void MbD::Joint::fillqsudotWeights(std::shared_ptr> diagMat) +{ +} + void MbD::Joint::useEquationNumbers() { constraintsDo([](std::shared_ptr constraint) { constraint->useEquationNumbers(); }); @@ -182,3 +201,18 @@ void MbD::Joint::preDyn() { constraintsDo([](std::shared_ptr constraint) { constraint->preDyn(); }); } + +void MbD::Joint::fillPosKineError(FColDsptr col) +{ + constraintsDo([&](std::shared_ptr con) { con->fillPosKineError(col); }); +} + +void MbD::Joint::fillPosKineJacob(SpMatDsptr mat) +{ + constraintsDo([&](std::shared_ptr constraint) { constraint->fillPosKineJacob(mat); }); +} + +void MbD::Joint::preVelIC() +{ + constraintsDo([](std::shared_ptr constraint) { constraint->preVelIC(); }); +} diff --git a/MbDCode/Joint.h b/MbDCode/Joint.h index a1f1ec7..47914d5 100644 --- a/MbDCode/Joint.h +++ b/MbDCode/Joint.h @@ -23,11 +23,15 @@ namespace MbD { void postInput() override; void addConstraint(std::shared_ptr con); void prePosIC() override; + void prePosKine() override; void fillEssenConstraints(std::shared_ptr>> essenConstraints) override; virtual void fillDispConstraints(std::shared_ptr>> dispConstraints); virtual void fillPerpenConstraints(std::shared_ptr>> perpenConstraints); void fillRedundantConstraints(std::shared_ptr>> redunConstraints) override; + void fillConstraints(std::shared_ptr>> allConstraints) override; void fillqsulam(FColDsptr col) override; + void fillqsudot(FColDsptr col) override; + void fillqsudotWeights(std::shared_ptr> diagMat) override; void useEquationNumbers() override; void setqsulam(FColDsptr col) override; void postPosICIteration() override; @@ -39,6 +43,9 @@ namespace MbD { void postPosIC() override; void outputStates() override; void preDyn() override; + void fillPosKineError(FColDsptr col) override; + void fillPosKineJacob(SpMatDsptr mat) override; + void preVelIC() override; EndFrmcptr frmI; EndFrmcptr frmJ; diff --git a/MbDCode/KineIntegrator.cpp b/MbDCode/KineIntegrator.cpp index 156939b..029793b 100644 --- a/MbDCode/KineIntegrator.cpp +++ b/MbDCode/KineIntegrator.cpp @@ -11,3 +11,10 @@ void MbD::KineIntegrator::preRun() system->logString(str); QuasiIntegrator::preRun(); } + +void MbD::KineIntegrator::runInitialConditionTypeSolution() +{ + system->runPosKine(); + system->runVelKine(); + system->runAccKine(); +} diff --git a/MbDCode/KineIntegrator.h b/MbDCode/KineIntegrator.h index e70426c..c5bc2db 100644 --- a/MbDCode/KineIntegrator.h +++ b/MbDCode/KineIntegrator.h @@ -8,6 +8,7 @@ namespace MbD { // public: void preRun() override; + void runInitialConditionTypeSolution() override; }; } diff --git a/MbDCode/LDUFullMat.cpp b/MbDCode/LDUFullMat.cpp index 9833bd4..1346bfe 100644 --- a/MbDCode/LDUFullMat.cpp +++ b/MbDCode/LDUFullMat.cpp @@ -1 +1,161 @@ #include "LDUFullMat.h" + +using namespace MbD; + +FColDsptr MbD::LDUFullMat::basicSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal) +{ + this->decomposesaveOriginal(fullMat, saveOriginal); + FColDsptr answer = this->forAndBackSubsaveOriginal(fullCol, saveOriginal); + return answer; +} + +FColDsptr MbD::LDUFullMat::basicSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) +{ + assert(false); + return FColDsptr(); +} + +void MbD::LDUFullMat::preSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal) +{ + assert(false); +} + +void MbD::LDUFullMat::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) +{ + assert(false); +} + +void MbD::LDUFullMat::forwardEliminateWithPivot(int p) +{ + //"Save factors in lower triangle for LU decomposition." + + //| rowp app rowi aip factor | + auto rowp = matrixA->at(p); + auto app = rowp->at(p); + for (int i = p + 1; i < m; i++) + { + auto rowi = matrixA->at(i); + auto aip = rowi->at(p); + auto factor = aip / app; + rowi->at(p) = factor; + if (factor != 0) { + for (int j = p + 1; j < n; j++) + { + rowi->atiminusNumber(j, factor * rowp->at(j)); + } + } + } +} + +void MbD::LDUFullMat::postSolve() +{ + assert(false); +} + +void MbD::LDUFullMat::preSolvesaveOriginal(FMatDsptr fullMat, bool saveOriginal) +{ + if (saveOriginal) { + matrixA = fullMat->copy(); + } + else { + matrixA = fullMat; + } + if (m != matrixA->nrow() || n != matrixA->ncol()) { + m = matrixA->nrow(); + n = matrixA->ncol(); + pivotValues = std::make_shared>(m); + rowOrder = std::make_shared>(m); + colOrder = std::make_shared>(n); + } + if (m == n) { + for (int i = 0; i < m; i++) + { + rowOrder->at(i) = i; + colOrder->at(i) = i; + } + } + else { + for (int i = 0; i < m; i++) + { + rowOrder->at(i) = i; + } + for (int j = 0; j < n; j++) + { + colOrder->at(j) = j; + } + } + this->findScalingsForRowRange(0, m); +} + +void MbD::LDUFullMat::decomposesaveOriginal(FMatDsptr fullMat, bool saveOriginal) +{ + this->preSolvesaveOriginal(fullMat, saveOriginal); + for (int p = 0; p < m; p++) + { + this->doPivoting(p); + this->forwardEliminateWithPivot(p); + } +} + +void MbD::LDUFullMat::decomposesaveOriginal(SpMatDsptr spMat, bool saveOriginal) +{ + assert(false); +} + +FMatDsptr MbD::LDUFullMat::inversesaveOriginal(FMatDsptr fullMat, bool saveOriginal) +{ + //"ForAndBackSub be optimized for the identity matrix." + + this->decomposesaveOriginal(fullMat, saveOriginal); + rightHandSideB = std::make_shared>(m); + auto matrixAinverse = std::make_shared >(m, n); + for (int j = 0; j < n; j++) + { + rightHandSideB->zeroSelf(); + rightHandSideB->at(j) = 1.0; + this->forAndBackSubsaveOriginal(rightHandSideB, saveOriginal); + matrixAinverse->atijputFullColumn(0, j, answerX); + } + return matrixAinverse; +} + +double MbD::LDUFullMat::getmatrixArowimaxMagnitude(int i) +{ + return matrixA->at(i)->maxMagnitude(); +} + +void MbD::LDUFullMat::forwardSubstituteIntoL() +{ + //"L is lower triangular with nonzero and ones in diagonal." + auto vectorc = std::make_shared>(n); + for (int i = 0; i < n; i++) + { + auto rowi = matrixA->at(i); + double sum = 0.0; + for (int j = 0; j < i - 1; j++) + { + sum += rowi->at(j) * vectorc->at(j); + } + vectorc->at(i) = rightHandSideB->at(i) - sum; + } + rightHandSideB = vectorc; +} + +void MbD::LDUFullMat::backSubstituteIntoDU() +{ + //"DU is upper triangular with nonzero and arbitrary diagonals." + + //| rowi sum | + answerX = std::make_shared>(n); + answerX->at(n - 1) = rightHandSideB->at(m - 1) / matrixA->at(m - 1)->at(n - 1); + for (int i = n-2; i >= 0; i--) + { + auto rowi = matrixA->at(i); + double sum = answerX->at(n) * rowi->at(n); + for (int j = i + 1; j < n - 1; j++) + { + sum += answerX->at(j) * rowi->at(j); + } + answerX->at(i) = (rightHandSideB->at(i) - sum) / rowi->at(i); + } +} diff --git a/MbDCode/LDUFullMat.h b/MbDCode/LDUFullMat.h index 900cddd..51749b6 100644 --- a/MbDCode/LDUFullMat.h +++ b/MbDCode/LDUFullMat.h @@ -7,6 +7,22 @@ namespace MbD { { // public: + FColDsptr basicSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal) override; + FColDsptr basicSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) override; + void preSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal) override; + void preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) override; + void forwardEliminateWithPivot(int p) override; + + void postSolve() override; + void preSolvesaveOriginal(FMatDsptr fullMat, bool saveOriginal); + void decomposesaveOriginal(SpMatDsptr spMat, bool saveOriginal); + void decomposesaveOriginal(FMatDsptr fullMat, bool saveOriginal); + FMatDsptr inversesaveOriginal(FMatDsptr fullMat, bool saveOriginal); + double getmatrixArowimaxMagnitude(int i) override; + void forwardSubstituteIntoL() override; + void backSubstituteIntoDU() override; + + std::shared_ptr> matrixA; }; } diff --git a/MbDCode/LDUFullMatParPv.cpp b/MbDCode/LDUFullMatParPv.cpp index 057a5d4..06fa0ef 100644 --- a/MbDCode/LDUFullMatParPv.cpp +++ b/MbDCode/LDUFullMatParPv.cpp @@ -1,27 +1,35 @@ #include "LDUFullMatParPv.h" #include "FullMatrix.h" +#include "SingularMatrixError.h" using namespace MbD; -FMatDsptr MbD::LDUFullMatParPv::inversesaveOriginal(FMatDsptr fullMat, bool saveOriginal) +void MbD::LDUFullMatParPv::doPivoting(int p) { - assert(false); - //"ForAndBackSub be optimized for the identity matrix." + //"Use scalings. Do row pivoting." - //| matrixAinverse | - //self decompose : aMatrix saveOriginal : saveOriginal. - //rightHandSideB : = StMFullColumn new : m. - //matrixAinverse : = StMFullMatrix new : m by : n. - //1 to : n - //do : - // [:j | - // rightHandSideB zeroSelf. - // rightHandSideB at : j put : 1.0d. - // self forAndBackSub : rightHandSideB saveOriginal : saveOriginal. - // matrixAinverse - // at : 1 - // and : j - // putFullColumn : answerX] . - // ^ matrixAinverse - return FMatDsptr(); + //| app max rowPivot aip mag | + auto app = matrixA->at(p)->at(p); + auto max = app * rowScalings->at(p); + if (max < 0.0) max = -max; + auto rowPivot = p; + for (int i = p + 1; i < m; i++) + { + auto aip = matrixA->at(i)->at(p); + if (aip != 0.0) { + auto mag = aip * rowScalings->at(i); + if (mag < 0) mag = -mag; + if (max < mag) { + max = mag; + rowPivot = i; + } + } + } + if (p != rowPivot) { + matrixA->swapElems(p, rowPivot); + rowScalings->swapElems(p, rowPivot); + rowOrder->swapElems(p, rowPivot); + } + pivotValues->at(p) = max; + if (max < singularPivotTolerance) throw SingularMatrixError(""); } diff --git a/MbDCode/LDUFullMatParPv.h b/MbDCode/LDUFullMatParPv.h index a99e98a..7718dca 100644 --- a/MbDCode/LDUFullMatParPv.h +++ b/MbDCode/LDUFullMatParPv.h @@ -3,11 +3,11 @@ #include "LDUFullMat.h" namespace MbD { - class LDUFullMatParPv : public LDUFullMat + class LDUFullMatParPv : public LDUFullMat { // public: - FMatDsptr inversesaveOriginal(FMatDsptr fullMat, bool saveOriginal); + void doPivoting(int p) override; }; } diff --git a/MbDCode/LDUSpMat.cpp b/MbDCode/LDUSpMat.cpp new file mode 100644 index 0000000..2d7eedb --- /dev/null +++ b/MbDCode/LDUSpMat.cpp @@ -0,0 +1,75 @@ +#include "LDUSpMat.h" +#include "FullColumn.h" + +using namespace MbD; + +FColDsptr MbD::LDUSpMat::basicSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) +{ + this->decomposesaveOriginal(spMat, saveOriginal); + FColDsptr answer = this->forAndBackSubsaveOriginal(fullCol, saveOriginal); + return answer; +} + +void MbD::LDUSpMat::decomposesaveOriginal(FMatDsptr fullMat, bool saveOriginal) +{ + assert(false); +} + +void MbD::LDUSpMat::decomposesaveOriginal(SpMatDsptr spMat, bool saveOriginal) +{ + assert(false); +} + +FColDsptr MbD::LDUSpMat::forAndBackSubsaveOriginal(FColDsptr fullCol, bool saveOriginal) +{ + assert(false); + return FColDsptr(); +} + +double MbD::LDUSpMat::getmatrixArowimaxMagnitude(int i) +{ + return matrixA->at(i)->maxMagnitude(); +} + +void MbD::LDUSpMat::forwardSubstituteIntoL() +{ + //"L is lower triangular with nonzero and ones in diagonal." + auto vectorc = std::make_shared>(n); + vectorc->at(0) = rightHandSideB->at(0); + for (int i = 1; i < n; i++) + { + auto rowi = matrixA->at(i); + double sum = 0.0; + for (auto const& keyValue : *rowi) { + int j = keyValue.first; + double duij = keyValue.second; + sum += duij * vectorc->at(j); + } + vectorc->at(i) = rightHandSideB->at(i) - sum; + } + rightHandSideB = vectorc; +} + +void MbD::LDUSpMat::backSubstituteIntoDU() +{ + //"DU is upper triangular with nonzero diagonals." + + double sum, duij; + for (int i = 0; i < m; i++) + { + rightHandSideB->at(i) = rightHandSideB->at(i) / matrixD->at(i); + } + answerX = std::make_shared>(m); + answerX->at(n - 1) = rightHandSideB->at(m - 1); + for (int i = n - 2; i >= 0; i--) + { + auto rowi = matrixU->at(i); + sum = 0.0; + for (auto const& keyValue : *rowi) { + auto j = keyValue.first; + duij = keyValue.second; + sum += answerX->at(j) * duij; + } + answerX->at(i) = rightHandSideB->at(i) - sum; + } +} diff --git a/MbDCode/LDUSpMat.h b/MbDCode/LDUSpMat.h new file mode 100644 index 0000000..5fe0205 --- /dev/null +++ b/MbDCode/LDUSpMat.h @@ -0,0 +1,25 @@ +#pragma once + +#include "MatrixLDU.h" +#include "SparseMatrix.h" + +namespace MbD { + class LDUSpMat : public MatrixLDU + { + //matrixL matrixD matrixU markowitzPivotRowCount markowitzPivotColCount privateIndicesOfNonZerosInPivotRow rowPositionsOfNonZerosInPivotColumn + public: + FColDsptr basicSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) override; + void decomposesaveOriginal(FMatDsptr fullMat, bool saveOriginal); + void decomposesaveOriginal(SpMatDsptr spMat, bool saveOriginal); + FColDsptr forAndBackSubsaveOriginal(FColDsptr fullCol, bool saveOriginal); + double getmatrixArowimaxMagnitude(int i) override; + void forwardSubstituteIntoL() override; + void backSubstituteIntoDU() override; + + std::shared_ptr> matrixA, matrixL, matrixU; + std::shared_ptr> matrixD; + int markowitzPivotRowCount, markowitzPivotColCount; + std::shared_ptr> rowPositionsOfNonZerosInPivotColumn; + }; +} + diff --git a/MbDCode/LDUSpMatParPv.cpp b/MbDCode/LDUSpMatParPv.cpp new file mode 100644 index 0000000..2dfbb7f --- /dev/null +++ b/MbDCode/LDUSpMatParPv.cpp @@ -0,0 +1 @@ +#include "LDUSpMatParPv.h" diff --git a/MbDCode/LDUSpMatParPv.h b/MbDCode/LDUSpMatParPv.h new file mode 100644 index 0000000..21c87a5 --- /dev/null +++ b/MbDCode/LDUSpMatParPv.h @@ -0,0 +1,13 @@ +#pragma once + +#include "LDUSpMat.h" + +namespace MbD { + class LDUSpMatParPv : public LDUSpMat + { + // + public: + + }; +} + diff --git a/MbDCode/LDUSpMatParPvMarko.cpp b/MbDCode/LDUSpMatParPvMarko.cpp new file mode 100644 index 0000000..c5a8b6c --- /dev/null +++ b/MbDCode/LDUSpMatParPvMarko.cpp @@ -0,0 +1,65 @@ +#include "LDUSpMatParPvMarko.h" +#include "SingularMatrixError.h" + +void MbD::LDUSpMatParPvMarko::doPivoting(int p) +{ + //"Search from bottom to top." + //"Check for singular pivot." + //"Do scaling. Do partial pivoting." + //"criterion := mag / (2.0d raisedTo: rowiCount)." + //| lookForFirstNonZeroInPivotCol i rowi aip criterionMax rowPivoti criterion max | + int i, rowPivoti; + double aip, mag, max, criterion, criterionMax; + SpRowDsptr spRowi; + rowPositionsOfNonZerosInPivotColumn->clear(); + auto lookForFirstNonZeroInPivotCol = true; + i = m - 1; + while (lookForFirstNonZeroInPivotCol) { + spRowi = matrixA->at(i); + if (spRowi->find(p) == spRowi->end()) { + if (i <= p) throw SingularMatrixError(""); + } + else { + markowitzPivotColCount = 0; + aip = spRowi->at(p); + mag = aip * rowScalings->at(i); + if (mag < 0) mag = -mag; + max = mag; + criterionMax = mag / std::pow(2.0, spRowi->size()); + rowPivoti = i; + lookForFirstNonZeroInPivotCol = false; + } + i--; + } + while (i >= p) { + spRowi = matrixA->at(i); + if (spRowi->find(p) == spRowi->end()) { + aip = std::numeric_limits::min(); + } + else { + aip = spRowi->at(p); + markowitzPivotColCount++; + mag = aip * rowScalings->at(i); + if (mag < 0) mag = -mag; + criterion = mag / std::pow(2.0, spRowi->size()); + if (criterion > criterionMax) { + max = mag; + criterionMax = criterion; + rowPositionsOfNonZerosInPivotColumn->push_back(rowPivoti); + rowPivoti = i; + } + else { + rowPositionsOfNonZerosInPivotColumn->push_back(i); + } + } + i--; + } + if (p != rowPivoti) { + matrixA->swapElems(p, rowPivoti); + rowScalings->swapElems(p, rowPivoti); + rowOrder->swapElems(p, rowPivoti); + matrixL->swapElems(p, rowPivoti); + if (aip != std::numeric_limits::min()) rowPositionsOfNonZerosInPivotColumn->at(markowitzPivotColCount - 1) = rowPivoti; + } + if (max < singularPivotTolerance) throw SingularMatrixError(""); +} diff --git a/MbDCode/LDUSpMatParPvMarko.h b/MbDCode/LDUSpMatParPvMarko.h new file mode 100644 index 0000000..f8573a1 --- /dev/null +++ b/MbDCode/LDUSpMatParPvMarko.h @@ -0,0 +1,14 @@ +#pragma once + +#include "LDUSpMatParPv.h" + +namespace MbD { + class LDUSpMatParPvMarko : public LDUSpMatParPv + { + // + public: + void doPivoting(int p) override; + + }; +} + diff --git a/MbDCode/LDUSpMatParPvPrecise.cpp b/MbDCode/LDUSpMatParPvPrecise.cpp new file mode 100644 index 0000000..0888617 --- /dev/null +++ b/MbDCode/LDUSpMatParPvPrecise.cpp @@ -0,0 +1,63 @@ +#include "LDUSpMatParPvPrecise.h" +#include "SingularMatrixError.h" + +void MbD::LDUSpMatParPvPrecise::doPivoting(int p) +{ + //"Search from bottom to top." + //"Use scaling vector and partial pivoting with actual swapping of rows." + //"Check for singular pivot." + //"Do scaling. Do partial pivoting." + //| max rowPivot aip mag lookForFirstNonZeroInPivotCol i | + int i, rowPivoti; + double aip, mag, max; + SpRowDsptr spRowi; + rowPositionsOfNonZerosInPivotColumn->clear(); + auto lookForFirstNonZeroInPivotCol = true; + i = m - 1; + while (lookForFirstNonZeroInPivotCol) { + spRowi = matrixA->at(i); + if (spRowi->find(p) == spRowi->end()) { + if (i <= p) throw SingularMatrixError(""); + } + else { + markowitzPivotColCount = 0; + aip = spRowi->at(p); + mag = aip * rowScalings->at(i); + if (mag < 0) mag = -mag; + max = mag; + rowPivoti = i; + lookForFirstNonZeroInPivotCol = false; + } + i--; + } + while (i >= p) { + spRowi = matrixA->at(i); + if (spRowi->find(p) == spRowi->end()) { + aip = std::numeric_limits::min(); + } + else { + aip = spRowi->at(p); + markowitzPivotColCount++; + mag = aip * rowScalings->at(i); + if (mag < 0) mag = -mag; + if (mag > max) { + max = mag; + rowPositionsOfNonZerosInPivotColumn->push_back(rowPivoti); + rowPivoti = i; + } + else { + rowPositionsOfNonZerosInPivotColumn->push_back(i); + } + } + i--; + } + if (p != rowPivoti) { + matrixA->swapElems(p, rowPivoti); + rowScalings->swapElems(p, rowPivoti); + rowOrder->swapElems(p, rowPivoti); + matrixL->swapElems(p, rowPivoti); + if (aip != std::numeric_limits::min()) rowPositionsOfNonZerosInPivotColumn->at(markowitzPivotColCount - 1) = rowPivoti; + } + pivotValues->at(p) = max; + if (max < singularPivotTolerance) throw SingularMatrixError(""); +} diff --git a/MbDCode/LDUSpMatParPvPrecise.h b/MbDCode/LDUSpMatParPvPrecise.h new file mode 100644 index 0000000..839a37d --- /dev/null +++ b/MbDCode/LDUSpMatParPvPrecise.h @@ -0,0 +1,14 @@ +#pragma once + +#include "LDUSpMatParPv.h" + +namespace MbD { + class LDUSpMatParPvPrecise : public LDUSpMatParPv + { + // + public: + void doPivoting(int p) override; + + }; +} + diff --git a/MbDCode/MarkerFrame.cpp b/MbDCode/MarkerFrame.cpp index b7c03d4..3ac0792 100644 --- a/MbDCode/MarkerFrame.cpp +++ b/MbDCode/MarkerFrame.cpp @@ -62,7 +62,7 @@ void MbD::MarkerFrame::calcPostDynCorrectorIteration() for (int i = 0; i < 4; i++) { auto& pAOppEi = pAOppE->at(i); - prOmOpE->atijputFullColumn(1, i, pAOppEi->timesFullColumn(rpmp)); + prOmOpE->atijputFullColumn(0, i, pAOppEi->timesFullColumn(rpmp)); pAOmpE->at(i) = pAOppEi->timesFullMatrix(aApm); } } @@ -103,6 +103,16 @@ void MbD::MarkerFrame::fillqsulam(FColDsptr col) endFramesDo([&](const std::shared_ptr& endFrame) { endFrame->fillqsulam(col); }); } +void MbD::MarkerFrame::fillqsudot(FColDsptr col) +{ + endFramesDo([&](const std::shared_ptr& endFrame) { endFrame->fillqsudot(col); }); +} + +void MbD::MarkerFrame::fillqsudotWeights(std::shared_ptr> diagMat) +{ + endFramesDo([&](const std::shared_ptr& endFrame) { endFrame->fillqsudotWeights(diagMat); }); +} + void MbD::MarkerFrame::setqsu(FColDsptr col) { endFramesDo([&](const std::shared_ptr& endFrame) { endFrame->setqsu(col); }); @@ -128,6 +138,17 @@ void MbD::MarkerFrame::preDyn() endFramesDo([](std::shared_ptr endFrame) { endFrame->preDyn(); }); } +void MbD::MarkerFrame::storeDynState() +{ + endFramesDo([](std::shared_ptr endFrame) { endFrame->storeDynState(); }); +} + +void MbD::MarkerFrame::preVelIC() +{ + Item::preVelIC(); + endFramesDo([](std::shared_ptr endFrame) { endFrame->preVelIC(); }); +} + void MarkerFrame::setPartFrame(PartFrame* partFrm) { partFrame = partFrm; diff --git a/MbDCode/MarkerFrame.h b/MbDCode/MarkerFrame.h index 306eec1..606972d 100644 --- a/MbDCode/MarkerFrame.h +++ b/MbDCode/MarkerFrame.h @@ -36,11 +36,15 @@ namespace MbD { void fillqsu(FColDsptr col) override; void fillqsuWeights(std::shared_ptr> diagMat) override; void fillqsulam(FColDsptr col) override; + void fillqsudot(FColDsptr col) override; + void fillqsudotWeights(std::shared_ptr> diagMat) override; void setqsu(FColDsptr col) override; void setqsulam(FColDsptr col) override; void postPosICIteration() override; void postPosIC() override; void preDyn() override; + void storeDynState() override; + void preVelIC() override; PartFrame* partFrame; //Use raw pointer when pointing backwards. FColDsptr rpmp = std::make_shared>(3); diff --git a/MbDCode/MatrixDecomposition.cpp b/MbDCode/MatrixDecomposition.cpp index a9f2688..60f4de3 100644 --- a/MbDCode/MatrixDecomposition.cpp +++ b/MbDCode/MatrixDecomposition.cpp @@ -1 +1,13 @@ #include "MatrixDecomposition.h" + +using namespace MbD; + +void MbD::MatrixDecomposition::applyRowOrderOnRightHandSideB() +{ + FColDsptr answer = std::make_shared>(m); + for (int i = 0; i < m; i++) + { + answer->at(i) = rightHandSideB->at(rowOrder->at(i)); + } + rightHandSideB = answer; +} diff --git a/MbDCode/MatrixDecomposition.h b/MbDCode/MatrixDecomposition.h index d6fcf07..eab1649 100644 --- a/MbDCode/MatrixDecomposition.h +++ b/MbDCode/MatrixDecomposition.h @@ -7,6 +7,15 @@ namespace MbD { { // public: + virtual FColDsptr forAndBackSubsaveOriginal(FColDsptr fullCol, bool saveOriginal) = 0; + virtual void applyRowOrderOnRightHandSideB(); + virtual void forwardSubstituteIntoL() = 0; + //virtual void backSubstituteIntoU(); + //virtual FColDsptr basicSolve(aMatrix); with : aVector saveOriginal : saveOriginal + //virtual void forwardSubstituteIntoL(); + //virtual void forwardSubstituteIntoLD(); + //virtual void postSolve(); + //virtual void preSolve : aMatrix saveOriginal : saveOriginal }; } diff --git a/MbDCode/MatrixGaussElimination.cpp b/MbDCode/MatrixGaussElimination.cpp index d80091b..5543d9e 100644 --- a/MbDCode/MatrixGaussElimination.cpp +++ b/MbDCode/MatrixGaussElimination.cpp @@ -1,17 +1,3 @@ #include "MatrixGaussElimination.h" using namespace MbD; - -FColDsptr MbD::MatrixGaussElimination::basicSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) -{ - this->preSolvewithsaveOriginal(spMat, fullCol, saveOriginal); - for (int p = 0; p < m; p++) - { - this->doPivoting(p); - this->forwardEliminateWithPivot(p); - } - - this->backSubstituteIntoDU(); - this->postSolve(); - return answerX; -} diff --git a/MbDCode/MatrixGaussElimination.h b/MbDCode/MatrixGaussElimination.h index 7bff644..f84e33d 100644 --- a/MbDCode/MatrixGaussElimination.h +++ b/MbDCode/MatrixGaussElimination.h @@ -8,9 +8,7 @@ namespace MbD { { // public: - FColDsptr basicSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) override; virtual void forwardEliminateWithPivot(int p) = 0; - virtual void backSubstituteIntoDU() = 0; }; } diff --git a/MbDCode/MatrixLDU.cpp b/MbDCode/MatrixLDU.cpp index 2222847..7c55bc6 100644 --- a/MbDCode/MatrixLDU.cpp +++ b/MbDCode/MatrixLDU.cpp @@ -1 +1,17 @@ #include "MatrixLDU.h" + +using namespace MbD; + +FColDsptr MbD::MatrixLDU::forAndBackSubsaveOriginal(FColDsptr fullCol, bool saveOriginal) +{ + if (saveOriginal) { + rightHandSideB = fullCol->copy(); + } + else { + rightHandSideB = fullCol; + } + this->applyRowOrderOnRightHandSideB(); + this->forwardSubstituteIntoL(); + this->backSubstituteIntoDU(); + return answerX; +} diff --git a/MbDCode/MatrixLDU.h b/MbDCode/MatrixLDU.h index a921835..8008814 100644 --- a/MbDCode/MatrixLDU.h +++ b/MbDCode/MatrixLDU.h @@ -7,6 +7,7 @@ namespace MbD { { // public: + FColDsptr forAndBackSubsaveOriginal(FColDsptr fullCol, bool saveOriginal) override; }; } diff --git a/MbDCode/MatrixSolver.cpp b/MbDCode/MatrixSolver.cpp index fdc7800..c1b6f12 100644 --- a/MbDCode/MatrixSolver.cpp +++ b/MbDCode/MatrixSolver.cpp @@ -6,6 +6,7 @@ #include "MatrixSolver.h" #include "SparseMatrix.h" #include "FullMatrix.h" +#include "SingularMatrixError.h" using namespace MbD; @@ -15,6 +16,10 @@ void MbD::MatrixSolver::initialize() singularPivotTolerance = 4 * std::numeric_limits::epsilon(); } +void MbD::MatrixSolver::setSystem(Solver* sys) +{ +} + FColDsptr MbD::MatrixSolver::solvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) { this->timedSolvewithsaveOriginal(spMat, fullCol, saveOriginal); @@ -44,3 +49,15 @@ FColDsptr MbD::MatrixSolver::timedSolvewithsaveOriginal(FMatDsptr fullMat, FColD { return FColDsptr(); } + +void MbD::MatrixSolver::findScalingsForRowRange(int begin, int end) +{ + //"Row element * scaling <= 1.0." + rowScalings = std::make_shared>(m); + for (int i = begin; i < end; i++) + { + auto maxRowMagnitude = this->getmatrixArowimaxMagnitude(i); + if (maxRowMagnitude == 0.0) throw SingularMatrixError(""); + rowScalings->at(i) = 1.0 / maxRowMagnitude; + } +} diff --git a/MbDCode/MatrixSolver.h b/MbDCode/MatrixSolver.h index ee8e594..ff2278c 100644 --- a/MbDCode/MatrixSolver.h +++ b/MbDCode/MatrixSolver.h @@ -13,18 +13,27 @@ namespace MbD { public: MatrixSolver(){} void initialize() override; + void setSystem(Solver* sys) override; virtual FColDsptr solvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal); virtual FColDsptr solvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal); - virtual FColDsptr timedSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal); virtual FColDsptr timedSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal); + virtual FColDsptr timedSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal); + virtual FColDsptr basicSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal) = 0; virtual FColDsptr basicSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) = 0; + virtual void preSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal) = 0; virtual void preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) = 0; virtual void doPivoting(int p) = 0; + virtual void forwardEliminateWithPivot(int p) = 0; + virtual void backSubstituteIntoDU() = 0; + virtual void postSolve() = 0; + virtual void findScalingsForRowRange(int begin, int end); + virtual double getmatrixArowimaxMagnitude(int i) = 0; int m = 0, n = 0; FColDsptr answerX, rightHandSideB, rowScalings, pivotValues; - std::shared_ptr> rowOrder, colOrder; + std::shared_ptr> rowOrder; + std::shared_ptr> colOrder; double singularPivotTolerance = 0, millisecondsToRun = 0; }; } diff --git a/MbDCode/MbDCode.cpp b/MbDCode/MbDCode.cpp index 4c57ac1..9949c09 100644 --- a/MbDCode/MbDCode.cpp +++ b/MbDCode/MbDCode.cpp @@ -55,7 +55,7 @@ int main() systemSolver->rotationLimit = 0.5; std::string str; - FColDsptr qX, qE, qXdot, omeOpO; + FColDsptr qX, qE, qXdot, omeOpO, qXddot, qEddot; FColDsptr rpmp; FMatDsptr aApm; FRowDsptr fullRow; @@ -68,10 +68,14 @@ int main() 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, 1 }); - //assembly1->setqXdot(qXdot); - //assembly1->setomeOpO(omeOpO); + 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 }); + assembly1->setqXddot(qXdot); + assembly1->setqEddot(omeOpO); std::cout << "assembly1->getqX() " << *assembly1->getqX() << std::endl; std::cout << "assembly1->getqE() " << *assembly1->getqE() << std::endl; TheSystem.addPart(assembly1); @@ -109,6 +113,14 @@ int main() 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.096568457800423, 0 }); + omeOpO = std::make_shared>(ListD{ 0, 0, 0.25132741228718 }); + 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); TheSystem.parts->push_back(crankPart1); { auto& partFrame = crankPart1->partFrame; @@ -143,6 +155,14 @@ int main() qE = std::make_shared>(ListD{ 0.0, 0.0, 0.89871703427292, 0.43852900965351 }); conrodPart2->setqX(qX); 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); + qXddot = std::make_shared>(ListD{ 0, 0, 0 }); + qEddot = std::make_shared>(ListD{ 0, 0, 0 }); + crankPart1->setqXddot(qXdot); + crankPart1->setqEddot(omeOpO); TheSystem.parts->push_back(conrodPart2); { auto& partFrame = conrodPart2->partFrame; @@ -177,6 +197,14 @@ int main() qE = std::make_shared>(ListD{ 0.70710678118655, 0.70710678118655, 0.0, 0.0 }); pistonPart3->setqX(qX); 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); + qXddot = std::make_shared>(ListD{ 0, 0, 0 }); + qEddot = std::make_shared>(ListD{ 0, 0, 0 }); + crankPart1->setqXddot(qXdot); + crankPart1->setqEddot(omeOpO); TheSystem.parts->push_back(pistonPart3); { auto& partFrame = pistonPart3->partFrame; diff --git a/MbDCode/MbDCode.vcxproj b/MbDCode/MbDCode.vcxproj index 817b7fe..a40c891 100644 --- a/MbDCode/MbDCode.vcxproj +++ b/MbDCode/MbDCode.vcxproj @@ -168,6 +168,8 @@ + + @@ -199,6 +201,10 @@ + + + + @@ -248,6 +254,10 @@ + + + + @@ -293,6 +303,8 @@ + + @@ -324,6 +336,10 @@ + + + + @@ -375,6 +391,10 @@ + + + + diff --git a/MbDCode/MbDCode.vcxproj.filters b/MbDCode/MbDCode.vcxproj.filters index 72d480d..953ceb1 100644 --- a/MbDCode/MbDCode.vcxproj.filters +++ b/MbDCode/MbDCode.vcxproj.filters @@ -381,6 +381,36 @@ Source Files + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + @@ -758,6 +788,36 @@ Header Files + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + diff --git a/MbDCode/NewtonRaphson.cpp b/MbDCode/NewtonRaphson.cpp index a89c696..56285a2 100644 --- a/MbDCode/NewtonRaphson.cpp +++ b/MbDCode/NewtonRaphson.cpp @@ -38,9 +38,9 @@ void MbD::NewtonRaphson::run() //self postRun. } -void MbD::NewtonRaphson::setSystem(SystemSolver* sys) +void MbD::NewtonRaphson::setSystem(Solver* sys) { - system = sys; + system = static_cast(sys); } void MbD::NewtonRaphson::iterate() diff --git a/MbDCode/NewtonRaphson.h b/MbDCode/NewtonRaphson.h index fbea302..d28b594 100644 --- a/MbDCode/NewtonRaphson.h +++ b/MbDCode/NewtonRaphson.h @@ -7,7 +7,7 @@ //#include "RowTypeMatrix.h" namespace MbD { - template + template class FullColumn; //class RowTypeMatrix; class SystemSolver; @@ -19,7 +19,7 @@ namespace MbD { void initialize(); void initializeLocally() override; void run() override; - void setSystem(SystemSolver* sys); + void setSystem(Solver* sys) override; void iterate(); virtual void fillY() = 0; virtual void fillPyPx() = 0; diff --git a/MbDCode/Part.cpp b/MbDCode/Part.cpp index d3b0ef2..63ba80d 100644 --- a/MbDCode/Part.cpp +++ b/MbDCode/Part.cpp @@ -66,6 +66,26 @@ FColDsptr Part::getomeOpO() { return partFrame->getomeOpO(); } +void MbD::Part::setqXddot(FColDsptr x) +{ + partFrame->setqXddot(x); +} + +FColDsptr MbD::Part::getqXddot() +{ + return partFrame->getqXddot(); +} + +void MbD::Part::setqEddot(FColDsptr x) +{ + partFrame->setqEddot(x); +} + +FColDsptr MbD::Part::getqEddot() +{ + return partFrame->getqEddot(); +} + void Part::setSystem(System& sys) { //May be needed in the future @@ -91,6 +111,11 @@ void MbD::Part::prePosIC() partFrame->prePosIC(); } +void MbD::Part::prePosKine() +{ + partFrame->prePosKine(); +} + void MbD::Part::iqX(int eqnNo) { partFrame->iqX = eqnNo; @@ -111,6 +136,11 @@ void MbD::Part::fillRedundantConstraints(std::shared_ptr>> allConstraints) +{ + partFrame->fillConstraints(allConstraints); +} + void MbD::Part::fillqsu(FColDsptr col) { partFrame->fillqsu(col); @@ -153,6 +183,39 @@ void MbD::Part::fillqsulam(FColDsptr col) partFrame->fillqsulam(col); } +void MbD::Part::fillqsudot(FColDsptr col) +{ + partFrame->fillqsudot(col); +} + +void MbD::Part::fillqsudotWeights(std::shared_ptr> diagMat) +{ + //"wqXdot and wqEdot are set to their respective inertias." + //"When the inertias are zero, they are set to a small number for positive definiteness." + //"They are not set to zero because inertialess part may be underconstrained." + //"wqEdot(4) = 0.0d is ok because there is always the euler parameter constraint." + + //| mMax aJiMax maxInertia minw maxw aJi wqXdot wqEdot | + auto mMax = System::getInstance().maximumMass(); + auto aJiMax = System::getInstance().maximumMomentOfInertia(); + auto maxInertia = std::max(mMax, aJiMax); + if (maxInertia == 0) maxInertia = 1.0; + auto minw = 1.0e-12 * maxInertia; + auto maxw = maxInertia; + auto wqXdot = std::make_shared>(3); + auto wqEdot = std::make_shared>(4); + for (int i = 0; i < 3; i++) + { + wqXdot->at(i) = (maxw * m / maxInertia) + minw; + auto aJi = aJ->at(i); + wqEdot->at(i) = (maxw * aJi / maxInertia) + minw; + } + wqEdot->at(3) = minw; + diagMat->atiputDiagonalMatrix(partFrame->iqX, wqXdot); + diagMat->atiputDiagonalMatrix(partFrame->iqE, wqEdot); + partFrame->fillqsudotWeights(diagMat); +} + void MbD::Part::useEquationNumbers() { partFrame->useEquationNumbers(); @@ -214,3 +277,23 @@ void MbD::Part::preDyn() { partFrame->preDyn(); } + +void MbD::Part::storeDynState() +{ + partFrame->storeDynState(); +} + +void MbD::Part::fillPosKineError(FColDsptr col) +{ + partFrame->fillPosKineError(col); +} + +void MbD::Part::fillPosKineJacob(SpMatDsptr mat) +{ + partFrame->fillPosKineJacob(mat); +} + +void MbD::Part::preVelIC() +{ + partFrame->preVelIC(); +} diff --git a/MbDCode/Part.h b/MbDCode/Part.h index 923416a..1062f4e 100644 --- a/MbDCode/Part.h +++ b/MbDCode/Part.h @@ -8,7 +8,7 @@ namespace MbD { class System; class PartFrame; - template + template class DiagonalMatrix; class Part : public Item @@ -28,19 +28,27 @@ namespace MbD { FColDsptr getqXdot(); void setomeOpO(FColDsptr x); FColDsptr getomeOpO(); + void setqXddot(FColDsptr x); + FColDsptr getqXddot(); + void setqEddot(FColDsptr x); + FColDsptr getqEddot(); void setSystem(System& sys); void asFixed(); void postInput() override; void calcPostDynCorrectorIteration() override; void prePosIC() override; + void prePosKine() override; void iqX(int eqnNo); void iqE(int eqnNo); void fillEssenConstraints(std::shared_ptr>> essenConstraints) override; void fillRedundantConstraints(std::shared_ptr>> redunConstraints) override; + void fillConstraints(std::shared_ptr>> allConstraints) override; void fillqsu(FColDsptr col) override; void fillqsuWeights(std::shared_ptr> diagMat) override; void fillqsulam(FColDsptr col) override; + void fillqsudot(FColDsptr col) override; + void fillqsudotWeights(std::shared_ptr> diagMat) override; void useEquationNumbers() override; void setqsu(FColDsptr col) override; void setqsulam(FColDsptr col) override; @@ -53,6 +61,10 @@ namespace MbD { void postPosIC() override; void outputStates() override; void preDyn() override; + void storeDynState() override; + void fillPosKineError(FColDsptr col) override; + void fillPosKineJacob(SpMatDsptr mat) override; + void preVelIC() override; int ipX = -1; int ipE = -1; diff --git a/MbDCode/PartFrame.cpp b/MbDCode/PartFrame.cpp index 3081b34..fdba74c 100644 --- a/MbDCode/PartFrame.cpp +++ b/MbDCode/PartFrame.cpp @@ -64,13 +64,33 @@ FColDsptr PartFrame::getqXdot() { } void PartFrame::setomeOpO(FColDsptr omeOpO) { - //qEdot = EulerParametersDot::FromqEOpAndOmegaOpO(qE, omeOpO); + qEdot = EulerParametersDot::FromqEOpAndOmegaOpO(qE, omeOpO); } FColDsptr PartFrame::getomeOpO() { return qE; } +void MbD::PartFrame::setqXddot(FColDsptr x) +{ + qXddot = x; +} + +FColDsptr MbD::PartFrame::getqXddot() +{ + return qXddot; +} + +void MbD::PartFrame::setqEddot(FColDsptr x) +{ + qEddot = x; +} + +FColDsptr MbD::PartFrame::getqEddot() +{ + return qEddot; +} + void PartFrame::setPart(Part* x) { part = x; } @@ -160,6 +180,16 @@ void MbD::PartFrame::prePosIC() aGabsDo([](std::shared_ptr aGab) { aGab->prePosIC(); }); } +void MbD::PartFrame::prePosKine() +{ + iqX = -1; + iqE = -1; + this->calcPostDynCorrectorIteration(); + markerFramesDo([](std::shared_ptr markerFrm) { markerFrm->prePosKine(); }); + aGeu->prePosKine(); + aGabsDo([](std::shared_ptr aGab) { aGab->prePosKine(); }); +} + FColDsptr MbD::PartFrame::rOpO() { return qX; @@ -185,6 +215,12 @@ void MbD::PartFrame::fillRedundantConstraints(std::shared_ptr>> allConstraints) +{ + aGeu->fillConstraints(aGeu, allConstraints); + aGabsDo([&](std::shared_ptr con) { con->fillConstraints(con, allConstraints); }); +} + void MbD::PartFrame::fillqsu(FColDsptr col) { col->atiputFullColumn(iqX, qX); @@ -206,6 +242,18 @@ void MbD::PartFrame::fillqsulam(FColDsptr col) aGabsDo([&](std::shared_ptr con) { con->fillqsulam(col); }); } +void MbD::PartFrame::fillqsudot(FColDsptr col) +{ + col->atiputFullColumn(iqX, qXdot); + col->atiputFullColumn(iqE, qEdot); + markerFramesDo([&](std::shared_ptr markerFrame) { markerFrame->fillqsudot(col); }); +} + +void MbD::PartFrame::fillqsudotWeights(std::shared_ptr> diagMat) +{ + markerFramesDo([&](std::shared_ptr markerFrame) { markerFrame->fillqsudotWeights(diagMat); }); +} + void MbD::PartFrame::useEquationNumbers() { markerFramesDo([](std::shared_ptr markerFrame) { markerFrame->useEquationNumbers(); }); @@ -281,9 +329,31 @@ void MbD::PartFrame::preDyn() aGabsDo([](std::shared_ptr aGab) { aGab->preDyn(); }); } +void MbD::PartFrame::storeDynState() +{ + markerFramesDo([](std::shared_ptr markerFrame) { markerFrame->storeDynState(); }); + aGeu->storeDynState(); + aGabsDo([](std::shared_ptr aGab) { aGab->storeDynState(); }); +} + +void MbD::PartFrame::fillPosKineError(FColDsptr col) +{ + markerFramesDo([&](std::shared_ptr markerFrame) { markerFrame->fillPosKineError(col); }); + aGeu->fillPosKineError(col); + aGabsDo([&](std::shared_ptr con) { con->fillPosKineError(col); }); +} + +void MbD::PartFrame::preVelIC() +{ + Item::preVelIC(); + markerFramesDo([](std::shared_ptr markerFrame) { markerFrame->preVelIC(); }); + aGeu->preVelIC(); + aGabsDo([](std::shared_ptr aGab) { aGab->preVelIC(); }); +} + void PartFrame::asFixed() { - for (int i = 0; i < 6; i++) { + for (int i = 0; i < 6; i++) { auto con = CREATE::With(i); con->setOwner(this); aGabs->push_back(con); diff --git a/MbDCode/PartFrame.h b/MbDCode/PartFrame.h index b0874a5..f3e46d0 100644 --- a/MbDCode/PartFrame.h +++ b/MbDCode/PartFrame.h @@ -39,6 +39,10 @@ namespace MbD { FColDsptr getqXdot(); void setomeOpO(FColDsptr x); FColDsptr getomeOpO(); + void setqXddot(FColDsptr x); + FColDsptr getqXddot(); + void setqEddot(FColDsptr x); + FColDsptr getqEddot(); void setPart(Part* x); Part* getPart(); void addMarkerFrame(std::shared_ptr x); @@ -50,14 +54,18 @@ namespace MbD { void constraintsReport() override; void prePosIC() override; + void prePosKine() override; FColDsptr rOpO(); FMatDsptr aAOp(); FColFMatDsptr pAOppE(); void fillEssenConstraints(std::shared_ptr>> essenConstraints) override; void fillRedundantConstraints(std::shared_ptr>> redunConstraints) override; + void fillConstraints(std::shared_ptr>> allConstraints) override; void fillqsu(FColDsptr col) override; void fillqsuWeights(std::shared_ptr> diagMat) override; void fillqsulam(FColDsptr col) override; + void fillqsudot(FColDsptr col) override; + void fillqsudotWeights(std::shared_ptr> diagMat) override; void useEquationNumbers() override; void setqsu(FColDsptr col) override; void setqsulam(FColDsptr col) override; @@ -67,16 +75,19 @@ namespace MbD { void postPosIC() override; void outputStates() override; void preDyn() override; + void storeDynState() override; + void fillPosKineError(FColDsptr col) override; + void preVelIC() override; Part* part = nullptr; //Use raw pointer when pointing backwards. int iqX = -1; int iqE = -1; //Position index of frame variables qX and qE in system list of variables 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); - //FColDsptr qXddot = std::make_shared>(3); - //FColDsptr qEddot = std::make_shared>(4); + FColDsptr qXdot = std::make_shared>(3); + std::shared_ptr> qEdot = std::make_shared>(4); + FColDsptr qXddot = std::make_shared>(3); + FColDsptr qEddot = std::make_shared>(4); std::shared_ptr aGeu; std::shared_ptr>> aGabs; std::shared_ptr>> markerFrames; diff --git a/MbDCode/PosICKineNewtonRaphson.cpp b/MbDCode/PosICKineNewtonRaphson.cpp index 984234c..e09a2fb 100644 --- a/MbDCode/PosICKineNewtonRaphson.cpp +++ b/MbDCode/PosICKineNewtonRaphson.cpp @@ -1,5 +1,7 @@ #include "PosICKineNewtonRaphson.h" #include "SystemSolver.h" +#include "Part.h" +#include "Constraint.h" using namespace MbD; @@ -12,4 +14,32 @@ void MbD::PosICKineNewtonRaphson::initializeGlobally() void MbD::PosICKineNewtonRaphson::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. + nqsu = nEqns; + for (auto& con : *constraints) { + con->iG = eqnNo; + eqnNo += 1; + } + auto lastEqnNo = eqnNo - 1; + nEqns = eqnNo; //C++ uses index 0. + n = nEqns; } diff --git a/MbDCode/PosICNewtonRaphson.cpp b/MbDCode/PosICNewtonRaphson.cpp index 54f6c8e..da6fafa 100644 --- a/MbDCode/PosICNewtonRaphson.cpp +++ b/MbDCode/PosICNewtonRaphson.cpp @@ -42,7 +42,7 @@ void MbD::PosICNewtonRaphson::assignEquationNumbers() //auto uHolders = system->uHolders(); auto essentialConstraints = system->essentialConstraints2(); auto displacementConstraints = system->displacementConstraints(); - auto perpendicularConstraints = system->perpendicularConstraints2(); + auto perpendicularConstraints = system->perpendicularConstraints(); int eqnNo = 0; for (auto& part : *parts) { part->iqX(eqnNo); diff --git a/MbDCode/PosKineNewtonRaphson.cpp b/MbDCode/PosKineNewtonRaphson.cpp index 62f5fbe..74084b3 100644 --- a/MbDCode/PosKineNewtonRaphson.cpp +++ b/MbDCode/PosKineNewtonRaphson.cpp @@ -1,5 +1,8 @@ #include "PosKineNewtonRaphson.h" #include "SystemSolver.h" +#include "Part.h" +#include "NotKinematicError.h" +#include "Constraint.h" using namespace MbD; @@ -10,3 +13,64 @@ void MbD::PosKineNewtonRaphson::initializeGlobally() iterMax = system->iterMaxPosKine; dxTol = system->errorTolPosKine; } + +void MbD::PosKineNewtonRaphson::fillPyPx() +{ + pypx->zeroSelf(); + system->partsJointsMotionsDo([&](std::shared_ptr item) { + item->fillPosKineJacob(pypx); + }); +} + +void MbD::PosKineNewtonRaphson::passRootToSystem() +{ + system->partsJointsMotionsDo([&](std::shared_ptr item) { item->setqsu(x); }); +} + +void MbD::PosKineNewtonRaphson::assignEquationNumbers() +{ + //"Equation order is q,s,u." + auto parts = system->parts(); + //auto contactEndFrames = system->contactEndFrames(); + //auto uHolders = system->uHolders(); + auto constraints = system->allConstraints(); + int varNo = 0; + for (auto& part : *parts) { + part->iqX(varNo); + varNo = varNo + 3; + part->iqE(varNo); + varNo = varNo + 4; + } + //for (auto& endFrm : *contactEndFrames) { + // endFrm->is(varNo); + // varNo = varNo + endFrm->sSize(); + //} + //for (auto& uHolder : *uHolders) { + // uHolder->iu(varNo); + // varNo += 1; + //} + auto eqnNo = 0; + for (auto& con : *constraints) { + con->iG = eqnNo; + eqnNo += 1; + } + n = eqnNo; //C++ uses index 0. + if (varNo != eqnNo) { + std::string str = "MbD: SYSTEM IS NOT PURE KINEMATIC."; + system->logString(str); + throw NotKinematicError(""); + } +} + +void MbD::PosKineNewtonRaphson::preRun() +{ + std::string str = "MbD: Solving for kinematic position."; + system->logString(str); + system->partsJointsMotionsDo([](std::shared_ptr item) { item->prePosKine(); }); +} + +void MbD::PosKineNewtonRaphson::fillY() +{ + y->zeroSelf(); + system->partsJointsMotionsDo([&](std::shared_ptr item) { item->fillPosKineError(y); }); +} diff --git a/MbDCode/PosKineNewtonRaphson.h b/MbDCode/PosKineNewtonRaphson.h index 97adffc..901e4ec 100644 --- a/MbDCode/PosKineNewtonRaphson.h +++ b/MbDCode/PosKineNewtonRaphson.h @@ -8,6 +8,12 @@ namespace MbD { // public: void initializeGlobally() override; + void fillPyPx() override; + void passRootToSystem() override; + void assignEquationNumbers() override; + void preRun() override; + void fillY() override; + }; } diff --git a/MbDCode/QuasiIntegrator.cpp b/MbDCode/QuasiIntegrator.cpp index 2ca5a60..04c3669 100644 --- a/MbDCode/QuasiIntegrator.cpp +++ b/MbDCode/QuasiIntegrator.cpp @@ -70,13 +70,18 @@ void MbD::QuasiIntegrator::preFirstStep() system->partsJointsMotionsForcesTorquesDo([](std::shared_ptr item) { item->preDynFirstStep(); }); } +void MbD::QuasiIntegrator::preStep() +{ + system->partsJointsMotionsForcesTorquesDo([](std::shared_ptr item) { item->preDynStep(); }); +} + double MbD::QuasiIntegrator::suggestSmallerOrAcceptFirstStepSize(double hnew) { auto hnew2 = hnew; system->partsJointsMotionsForcesTorquesDo([&](std::shared_ptr item) { hnew2 = item->suggestSmallerOrAcceptDynFirstStepSize(hnew2); }); if (hnew2 > hmax) { hnew2 = hmax; - std::string str = "StM: \Step size is at user specified maximum."; + std::string str = "StM: Step size is at user specified maximum."; this->logString(str); } if (hnew2 < hmin) { @@ -88,3 +93,9 @@ double MbD::QuasiIntegrator::suggestSmallerOrAcceptFirstStepSize(double hnew) } return hnew2; } + +void MbD::QuasiIntegrator::incrementTime(double tnew) +{ + system->partsJointsMotionsForcesTorquesDo([](std::shared_ptr item) { item->storeDynState(); }); + IntegratorInterface::incrementTime(tnew); +} diff --git a/MbDCode/QuasiIntegrator.h b/MbDCode/QuasiIntegrator.h index 4fcde8f..560cb40 100644 --- a/MbDCode/QuasiIntegrator.h +++ b/MbDCode/QuasiIntegrator.h @@ -11,7 +11,9 @@ namespace MbD { void initialize() override; void run() override; void preFirstStep(); + void preStep(); double suggestSmallerOrAcceptFirstStepSize(double hnew) override; + void incrementTime(double tnew) override; }; } diff --git a/MbDCode/RedundantConstraint.cpp b/MbDCode/RedundantConstraint.cpp index 1f2320f..2adf60b 100644 --- a/MbDCode/RedundantConstraint.cpp +++ b/MbDCode/RedundantConstraint.cpp @@ -46,6 +46,10 @@ void MbD::RedundantConstraint::fillPerpenConstraints(std::shared_ptr { } +void MbD::RedundantConstraint::fillConstraints(std::shared_ptr sptr, std::shared_ptr>> redunConstraints) +{ +} + void MbD::RedundantConstraint::fillRedundantConstraints(std::shared_ptr sptr, std::shared_ptr>> redunConstraints) { redunConstraints->push_back(sptr); @@ -58,3 +62,15 @@ void MbD::RedundantConstraint::setqsulam(FColDsptr col) void MbD::RedundantConstraint::fillPosICError(FColDsptr col) { } + +void MbD::RedundantConstraint::fillPosKineError(FColDsptr col) +{ +} + +void MbD::RedundantConstraint::fillPosKineJacob(SpMatDsptr mat) +{ +} + +void MbD::RedundantConstraint::preVelIC() +{ +} diff --git a/MbDCode/RedundantConstraint.h b/MbDCode/RedundantConstraint.h index e7c3b40..681e0ec 100644 --- a/MbDCode/RedundantConstraint.h +++ b/MbDCode/RedundantConstraint.h @@ -17,9 +17,13 @@ namespace MbD { void fillEssenConstraints(std::shared_ptr sptr, std::shared_ptr>> essenConstraints); void fillDispConstraints(std::shared_ptr sptr, std::shared_ptr>> dispConstraints); void fillPerpenConstraints(std::shared_ptr sptr, std::shared_ptr>> perpenConstraints); - void fillRedundantConstraints(std::shared_ptr sptr, std::shared_ptr>> redunConstraints); + 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 fillPosICError(FColDsptr col) override; + void fillPosKineError(FColDsptr col) override; + void fillPosKineJacob(SpMatDsptr mat) override; + void preVelIC() override; std::shared_ptr constraint; }; diff --git a/MbDCode/RowTypeMatrix.h b/MbDCode/RowTypeMatrix.h index e700425..a9192eb 100644 --- a/MbDCode/RowTypeMatrix.h +++ b/MbDCode/RowTypeMatrix.h @@ -5,7 +5,7 @@ namespace MbD { - template + template class RowTypeMatrix : public Array { public: diff --git a/MbDCode/Solver.cpp b/MbDCode/Solver.cpp index e0995f1..d120641 100644 --- a/MbDCode/Solver.cpp +++ b/MbDCode/Solver.cpp @@ -18,6 +18,11 @@ void MbD::Solver::initializeGlobally() assert(false); } +void MbD::Solver::assignEquationNumbers() +{ + assert(false); +} + void MbD::Solver::run() { assert(false); diff --git a/MbDCode/Solver.h b/MbDCode/Solver.h index 2e21f96..937e38f 100644 --- a/MbDCode/Solver.h +++ b/MbDCode/Solver.h @@ -10,12 +10,15 @@ namespace MbD { virtual void initialize(); virtual void initializeLocally(); virtual void initializeGlobally(); + virtual void assignEquationNumbers(); virtual void run(); virtual void preRun(); virtual void finalize(); virtual void reportStats(); virtual void postRun(); virtual void logString(std::string& str); + virtual void setSystem(Solver* sys) = 0; + }; } diff --git a/MbDCode/SparseColumn.h b/MbDCode/SparseColumn.h index 11e7b6c..38709c3 100644 --- a/MbDCode/SparseColumn.h +++ b/MbDCode/SparseColumn.h @@ -2,7 +2,7 @@ #include "SparseVector.h" namespace MbD { - template + template class SparseColumn : public SparseVector { public: diff --git a/MbDCode/SparseMatrix.h b/MbDCode/SparseMatrix.h index 9c795e8..fc9b537 100644 --- a/MbDCode/SparseMatrix.h +++ b/MbDCode/SparseMatrix.h @@ -6,7 +6,7 @@ #include "DiagonalMatrix.h" namespace MbD { - template + template class SparseMatrix : public RowTypeMatrix>> { public: @@ -27,6 +27,7 @@ namespace MbD { this->push_back(row); } } + void atijplusDiagonalMatrix(int i, int j, std::shared_ptr> diagMat); void atijminusDiagonalMatrix(int i, int j, std::shared_ptr> diagMat); double sumOfSquares() override; void zeroSelf() override; @@ -35,6 +36,8 @@ namespace MbD { void atijplusFullMatrix(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); virtual std::ostream& printOn(std::ostream& s) const; friend std::ostream& operator<<(std::ostream& s, const SparseMatrix& spMat) @@ -44,13 +47,23 @@ namespace MbD { }; using SpMatDsptr = std::shared_ptr>; + template + inline void SparseMatrix::atijplusDiagonalMatrix(int i, int j, std::shared_ptr> diagMat) + { + auto n = diagMat->nrow(); + for (int ii = 0; ii < n; ii++) + { + this->atijplusNumber(i + ii, j + ii, diagMat->at(ii)); + } + } + template<> inline void SparseMatrix::atijminusDiagonalMatrix(int i1, int j1, std::shared_ptr> diagMat) { auto n = diagMat->nrow(); for (int ii = 0; ii < n; ii++) { - (*(this->at(i1 + ii)))[j1 + ii] -= diagMat->at(ii); + this->atijminusNumber(i1 + ii, j1 + ii, diagMat->at(ii)); } } template @@ -80,7 +93,7 @@ namespace MbD { { for (int ii = 0; ii < fullCol->size(); ii++) { - (*(this->at(i + ii)))[j] += fullCol->at(ii); + this->atijplusNumber(i + ii, j, fullCol->at(ii)); } } template @@ -108,6 +121,16 @@ namespace MbD { } } template + inline void SparseMatrix::atijplusNumber(int i, int j, double value) + { + this->at(i)->atiplusNumber(j, value); + } + template + inline void SparseMatrix::atijminusNumber(int i, int j, double value) + { + this->at(i)->atiminusNumber(j, value); + } + template inline std::ostream& SparseMatrix::printOn(std::ostream& s) const { s << "SpMat[" << std::endl; diff --git a/MbDCode/SparseRow.h b/MbDCode/SparseRow.h index e29d230..c10d8c2 100644 --- a/MbDCode/SparseRow.h +++ b/MbDCode/SparseRow.h @@ -6,7 +6,7 @@ #include "FullRow.h" namespace MbD { - template + template class SparseRow : public SparseVector { public: diff --git a/MbDCode/SparseVector.h b/MbDCode/SparseVector.h index 46dda49..276f19d 100644 --- a/MbDCode/SparseVector.h +++ b/MbDCode/SparseVector.h @@ -4,7 +4,7 @@ #include namespace MbD { - template + template class SparseVector : public std::map { public: @@ -29,8 +29,9 @@ namespace MbD { int numberOfElements(); double sumOfSquares(); void atiplusNumber(int i, double value); + void atiminusNumber(int i, double value); void zeroSelf(); - double maxElement(); + double maxMagnitude(); virtual std::ostream& printOn(std::ostream& s) const; friend std::ostream& operator<<(std::ostream& s, const SparseVector& spVec) @@ -61,15 +62,20 @@ namespace MbD { template<> inline void SparseVector::atiplusNumber(int i, double value) { - this->at(i) += value; + (*this)[i] += value; + } + template + inline void SparseVector::atiminusNumber(int i, double value) + { + (*this)[i] -= value; } template<> inline void SparseVector::zeroSelf() { this->clear(); } - template<> - inline double SparseVector::maxElement() + template + inline double SparseVector::maxMagnitude() { double max = 0.0; for (const auto& keyValue : *this) { diff --git a/MbDCode/StableBackwardDifference.cpp b/MbDCode/StableBackwardDifference.cpp index efe3e78..4a0c504 100644 --- a/MbDCode/StableBackwardDifference.cpp +++ b/MbDCode/StableBackwardDifference.cpp @@ -1,5 +1,7 @@ #include "StableBackwardDifference.h" +using namespace MbD; + void MbD::StableBackwardDifference::formTaylorMatrix() { //This form is numerically more stable and is prefered over the full Taylor Matrix. @@ -8,21 +10,38 @@ void MbD::StableBackwardDifference::formTaylorMatrix() //| (t2 - t) (t2 - t) ^ 2 / 2! (t2 - t) ^ 3 / 3!| |qdd(t) | |q(t2) - q(t) | //| (t3 - t) (t3 - t) ^ 2 / 2! (t3 - t) ^ 3 / 3!| |qddd(t)| |q(t3) - q(t) | - this->instanciateTaylorMatrix(); + this->instantiateTaylorMatrix(); for (int i = 0; i < order; i++) { this->formTaylorRowwithTimeNodederivative(i, i, 0); } } -void MbD::StableBackwardDifference::instanciateTaylorMatrix() +void MbD::StableBackwardDifference::instantiateTaylorMatrix() { - if (taylorMatrix->empty() || (taylorMatrix->nrow() != (order))) { + if (taylorMatrix == nullptr || (taylorMatrix->nrow() != (order))) { taylorMatrix = std::make_shared>(order, order); } } void MbD::StableBackwardDifference::formTaylorRowwithTimeNodederivative(int i, int ii, int k) { - assert(false); + //| rowi hi hipower aij | + auto& rowi = taylorMatrix->at(i); + if (k > 0) { + for (int j = 0; j < k - 1; j++) + { + rowi->at(j) = 0.0; + } + rowi->at(k) = 1.0; + } + + auto hi = timeNodes->at(ii) - time; + auto hipower = 1.0; + for (int j = k; j < order; j++) + { + hipower *= hi; + auto aij = hipower * OneOverFactorials->at((size_t)(j - k)); + rowi->at(j) = aij; + } } diff --git a/MbDCode/StableBackwardDifference.h b/MbDCode/StableBackwardDifference.h index e76069f..f8a7d25 100644 --- a/MbDCode/StableBackwardDifference.h +++ b/MbDCode/StableBackwardDifference.h @@ -8,7 +8,7 @@ namespace MbD { // public: void formTaylorMatrix() override; - void instanciateTaylorMatrix() override; + void instantiateTaylorMatrix() override; void formTaylorRowwithTimeNodederivative(int i, int ii, int k) override; }; } diff --git a/MbDCode/System.cpp b/MbDCode/System.cpp index 280f39b..9d1ceb4 100644 --- a/MbDCode/System.cpp +++ b/MbDCode/System.cpp @@ -111,6 +111,11 @@ double MbD::System::mbdTimeValue() return time->getValue(); } +void MbD::System::mbdTimeValue(double t) +{ + //time->value(t); +} + std::shared_ptr>> MbD::System::essentialConstraints2() { auto essenConstraints = std::make_shared>>(); @@ -125,7 +130,7 @@ std::shared_ptr>> MbD::System::displacem return dispConstraints; } -std::shared_ptr>> MbD::System::perpendicularConstraints2() +std::shared_ptr>> MbD::System::perpendicularConstraints() { auto perpenConstraints = std::make_shared>>(); this->jointsMotionsDo([&](std::shared_ptr joint) { joint->fillPerpenConstraints(perpenConstraints); }); @@ -139,6 +144,13 @@ std::shared_ptr>> MbD::System::allRedund return redunConstraints; } +std::shared_ptr>> MbD::System::allConstraints() +{ + auto constraints = std::make_shared>>(); + this->partsJointsMotionsDo([&](std::shared_ptr item) { item->fillConstraints(constraints); }); + return constraints; +} + double MbD::System::maximumMass() { auto maxPart = std::max_element(parts->begin(), parts->end(), [](auto& a, auto& b) { return a->m < b->m; }); diff --git a/MbDCode/System.h b/MbDCode/System.h index bc5aba9..e6977e7 100644 --- a/MbDCode/System.h +++ b/MbDCode/System.h @@ -45,11 +45,13 @@ namespace MbD { void partsJointsMotionsForcesTorquesDo(const std::function )>& f); void logString(std::string& str) override; double mbdTimeValue(); + void mbdTimeValue(double t); std::shared_ptr>> essentialConstraints2(); std::shared_ptr>> displacementConstraints(); - std::shared_ptr>> perpendicularConstraints2(); + std::shared_ptr>> perpendicularConstraints(); std::shared_ptr>> allRedundantConstraints(); - + std::shared_ptr>> allConstraints(); + double maximumMass(); double maximumMomentOfInertia(); diff --git a/MbDCode/SystemSolver.cpp b/MbDCode/SystemSolver.cpp index 6579f19..4a5ee6d 100644 --- a/MbDCode/SystemSolver.cpp +++ b/MbDCode/SystemSolver.cpp @@ -11,11 +11,20 @@ #include "ICKineIntegrator.h" #include "KineIntegrator.h" #include "DiscontinuityError.h" +#include "PosICKineNewtonRaphson.h" +#include "PosKineNewtonRaphson.h" +#include "VelICSolver.h" using namespace MbD; //class PosICNewtonRaphson; +void MbD::SystemSolver::setSystem(Solver* sys) +{ + //Do not use + assert(false); +} + void MbD::SystemSolver::initialize() { tstartPasts = std::make_shared>(); @@ -43,17 +52,16 @@ void SystemSolver::runAllIC() { runPosIC(); } - break; - //runVelIC(); - //runAccIC(); - //auto discontinuities = system->discontinuitiesAtIC(); - //if (discontinuities->size() == 0) break; - //if (std::find(discontinuities->begin(), discontinuities->end(), "REBOUND") != discontinuities->end()) - //{ - // preCollision(); - // runCollisionDerivativeIC(); - // runBasicCollision(); - //} + runVelIC(); + runAccIC(); + auto discontinuities = system->discontinuitiesAtIC(); + if (discontinuities->size() == 0) break; + if (std::find(discontinuities->begin(), discontinuities->end(), "REBOUND") != discontinuities->end()) + { + preCollision(); + runCollisionDerivativeIC(); + runBasicCollision(); + } } } @@ -66,10 +74,14 @@ void MbD::SystemSolver::runPosIC() void MbD::SystemSolver::runVelIC() { + icTypeSolver = CREATE::With(); + icTypeSolver->setSystem(this); + icTypeSolver->run(); } void MbD::SystemSolver::runAccIC() { + assert(false); } bool MbD::SystemSolver::needToRedoPosIC() @@ -147,6 +159,52 @@ void MbD::SystemSolver::runQuasiKinematic() } } +void MbD::SystemSolver::runPosKine() +{ + icTypeSolver = CREATE::With(); + icTypeSolver->setSystem(this); + icTypeSolver->run(); +} + +void MbD::SystemSolver::runVelKine() +{ + assert(false); + //icTypeSolver = CREATE::With(); + //icTypeSolver->setSystem(this); + //icTypeSolver->run(); +} + +void MbD::SystemSolver::runAccKine() +{ + assert(false); + //icTypeSolver = CREATE::With(); + //icTypeSolver->setSystem(this); + //icTypeSolver->run(); +} + +void MbD::SystemSolver::runPosICKine() +{ + icTypeSolver = CREATE::With(); + icTypeSolver->setSystem(this); + icTypeSolver->run(); +} + +void MbD::SystemSolver::runVelICKine() +{ + assert(false); + //icTypeSolver = CREATE::With(); + //icTypeSolver->setSystem(this); + //icTypeSolver->run(); +} + +void MbD::SystemSolver::runAccICKine() +{ + assert(false); + //icTypeSolver = CREATE::With(); + //icTypeSolver->setSystem(this); + //icTypeSolver->run(); +} + void MbD::SystemSolver::partsJointsMotionsDo(const std::function)>& f) { system->partsJointsMotionsDo(f); @@ -172,9 +230,9 @@ std::shared_ptr>> MbD::SystemSolver::dis return system->displacementConstraints(); } -std::shared_ptr>> MbD::SystemSolver::perpendicularConstraints2() +std::shared_ptr>> MbD::SystemSolver::perpendicularConstraints() { - return system->perpendicularConstraints2(); + return system->perpendicularConstraints(); } std::shared_ptr>> MbD::SystemSolver::allRedundantConstraints() @@ -182,6 +240,11 @@ std::shared_ptr>> MbD::SystemSolver::all return system->allRedundantConstraints(); } +std::shared_ptr>> MbD::SystemSolver::allConstraints() +{ + return system->allConstraints(); +} + void MbD::SystemSolver::postNewtonRaphson() { assert(false); @@ -226,3 +289,8 @@ double MbD::SystemSolver::endTime() { return tend; } + +void MbD::SystemSolver::settime(double tnew) +{ + system->mbdTimeValue(tnew); +} diff --git a/MbDCode/SystemSolver.h b/MbDCode/SystemSolver.h index 7a78cba..af19a62 100644 --- a/MbDCode/SystemSolver.h +++ b/MbDCode/SystemSolver.h @@ -13,7 +13,7 @@ namespace MbD { class System; class Constraint; - class NewtonRaphson; + class Solver; class QuasiIntegrator; class SystemSolver : public Solver @@ -25,6 +25,7 @@ namespace MbD { SystemSolver(System* x) : system(x) { initialize(); } + void setSystem(Solver* sys) override; void initialize() override; void initializeLocally() override; void initializeGlobally() override; @@ -38,6 +39,12 @@ namespace MbD { void runBasicCollision(); void runBasicKinematic(); void runQuasiKinematic(); + void runPosKine(); + void runVelKine(); + void runAccKine(); + void runPosICKine(); + void runVelICKine(); + void runAccICKine(); void partsJointsMotionsDo(const std::function )>& f); void logString(std::string& str); std::shared_ptr>> parts(); @@ -45,8 +52,10 @@ namespace MbD { //std::shared_ptr> uHolders(); std::shared_ptr>> essentialConstraints2(); std::shared_ptr>> displacementConstraints(); - std::shared_ptr>> perpendicularConstraints2(); + std::shared_ptr>> perpendicularConstraints(); std::shared_ptr>> allRedundantConstraints(); + std::shared_ptr>> allConstraints(); + virtual void postNewtonRaphson(); void partsJointsMotionsForcesTorquesDo(const std::function )>& f); void discontinuityBlock(); @@ -56,10 +65,10 @@ namespace MbD { double minStepSize(); double firstOutputTime(); double endTime(); - + void settime(double tnew); System* system; //Use raw pointer when pointing backwards. - std::shared_ptr icTypeSolver; + std::shared_ptr icTypeSolver; std::shared_ptr>>> setsOfRedundantConstraints; double errorTolPosKine = 1.0e-6; diff --git a/MbDCode/TranslationConstraintIJ.cpp b/MbDCode/TranslationConstraintIJ.cpp index 2ff6d40..250c721 100644 --- a/MbDCode/TranslationConstraintIJ.cpp +++ b/MbDCode/TranslationConstraintIJ.cpp @@ -56,3 +56,9 @@ void MbD::TranslationConstraintIJ::postPosICIteration() riIeJeIe->postPosICIteration(); Item::postPosICIteration(); } + +void MbD::TranslationConstraintIJ::preVelIC() +{ + riIeJeIe->preVelIC(); + Item::preVelIC(); +} diff --git a/MbDCode/TranslationConstraintIJ.h b/MbDCode/TranslationConstraintIJ.h index 59b674f..1930970 100644 --- a/MbDCode/TranslationConstraintIJ.h +++ b/MbDCode/TranslationConstraintIJ.h @@ -18,6 +18,7 @@ namespace MbD { void prePosIC()override; MbD::ConstraintType type() override; void postPosICIteration() override; + void preVelIC() override; int axisI; std::shared_ptr riIeJeIe; diff --git a/MbDCode/TranslationConstraintIqcJc.cpp b/MbDCode/TranslationConstraintIqcJc.cpp index a1faa97..1ba83c4 100644 --- a/MbDCode/TranslationConstraintIqcJc.cpp +++ b/MbDCode/TranslationConstraintIqcJc.cpp @@ -50,3 +50,9 @@ void MbD::TranslationConstraintIqcJc::fillPosICJacob(SpMatDsptr mat) mat->atijplusTransposeFullMatrix(iqEI, iqXI, ppGpXIpEIlam); mat->atijplusFullMatrixtimes(iqEI, iqEI, ppGpEIpEI, lam); } + +void MbD::TranslationConstraintIqcJc::fillPosKineJacob(SpMatDsptr mat) +{ + mat->atijplusFullRow(iG, iqXI, pGpXI); + mat->atijplusFullRow(iG, iqEI, pGpEI); +} diff --git a/MbDCode/TranslationConstraintIqcJc.h b/MbDCode/TranslationConstraintIqcJc.h index 1f20055..f23e1d2 100644 --- a/MbDCode/TranslationConstraintIqcJc.h +++ b/MbDCode/TranslationConstraintIqcJc.h @@ -13,6 +13,7 @@ namespace MbD { void useEquationNumbers() override; void fillPosICError(FColDsptr col) override; void fillPosICJacob(SpMatDsptr mat) override; + void fillPosKineJacob(SpMatDsptr mat) override; FRowDsptr pGpXI; FRowDsptr pGpEI; diff --git a/MbDCode/TranslationConstraintIqcJqc.cpp b/MbDCode/TranslationConstraintIqcJqc.cpp index d1e02a9..6c09974 100644 --- a/MbDCode/TranslationConstraintIqcJqc.cpp +++ b/MbDCode/TranslationConstraintIqcJqc.cpp @@ -54,3 +54,10 @@ void MbD::TranslationConstraintIqcJqc::fillPosICJacob(SpMatDsptr mat) mat->atijplusTransposeFullMatrix(iqEJ, iqEI, ppGpEIpEJlam); mat->atijplusFullMatrixtimes(iqEJ, iqEJ, ppGpEJpEJ, lam); } + +void MbD::TranslationConstraintIqcJqc::fillPosKineJacob(SpMatDsptr mat) +{ + TranslationConstraintIqcJc::fillPosKineJacob(mat); + mat->atijplusFullRow(iG, iqXJ, pGpXJ); + mat->atijplusFullRow(iG, iqEJ, pGpEJ); +} diff --git a/MbDCode/TranslationConstraintIqcJqc.h b/MbDCode/TranslationConstraintIqcJqc.h index f1ac363..aefc096 100644 --- a/MbDCode/TranslationConstraintIqcJqc.h +++ b/MbDCode/TranslationConstraintIqcJqc.h @@ -13,6 +13,7 @@ namespace MbD { void useEquationNumbers() override; void fillPosICError(FColDsptr col) override; void fillPosICJacob(SpMatDsptr mat) override; + void fillPosKineJacob(SpMatDsptr mat) override; FRowDsptr pGpXJ; FRowDsptr pGpEJ; diff --git a/MbDCode/TranslationConstraintIqctJqc.cpp b/MbDCode/TranslationConstraintIqctJqc.cpp index 58f0f43..a2052aa 100644 --- a/MbDCode/TranslationConstraintIqctJqc.cpp +++ b/MbDCode/TranslationConstraintIqctJqc.cpp @@ -18,3 +18,9 @@ MbD::ConstraintType MbD::TranslationConstraintIqctJqc::type() { return MbD::essential; } + +void MbD::TranslationConstraintIqctJqc::preVelIC() +{ + TranslationConstraintIJ::preVelIC(); + pGpt = std::static_pointer_cast(riIeJeIe)->pvaluept(); +} diff --git a/MbDCode/TranslationConstraintIqctJqc.h b/MbDCode/TranslationConstraintIqctJqc.h index 5e52f79..587f15e 100644 --- a/MbDCode/TranslationConstraintIqctJqc.h +++ b/MbDCode/TranslationConstraintIqctJqc.h @@ -10,6 +10,7 @@ namespace MbD { TranslationConstraintIqctJqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi); void initriIeJeIe() override; MbD::ConstraintType type() override; + void preVelIC() override; double pGpt; FRowDsptr ppGpXIpt; diff --git a/MbDCode/Variable.cpp b/MbDCode/Variable.cpp index e38eaf1..12a2c40 100644 --- a/MbDCode/Variable.cpp +++ b/MbDCode/Variable.cpp @@ -39,3 +39,8 @@ std::ostream& MbD::Variable::printOn(std::ostream& s) const { return s << this->name; } + +void MbD::Variable::setValue(double val) +{ + value = val; +} diff --git a/MbDCode/Variable.h b/MbDCode/Variable.h index 2ad47e7..bb1e8da 100644 --- a/MbDCode/Variable.h +++ b/MbDCode/Variable.h @@ -15,6 +15,7 @@ namespace MbD { const std::string& getName() const; double getValue() override; std::ostream& printOn(std::ostream& s) const override; + virtual void setValue(double val); std::string name; double value; diff --git a/MbDCode/VelICKineSolver.cpp b/MbDCode/VelICKineSolver.cpp new file mode 100644 index 0000000..c15f3b1 --- /dev/null +++ b/MbDCode/VelICKineSolver.cpp @@ -0,0 +1 @@ +#include "VelICKineSolver.h" diff --git a/MbDCode/VelICKineSolver.h b/MbDCode/VelICKineSolver.h new file mode 100644 index 0000000..202b828 --- /dev/null +++ b/MbDCode/VelICKineSolver.h @@ -0,0 +1,13 @@ +#pragma once + +#include "VelICSolver.h" + +namespace MbD { + class VelICKineSolver : public VelICSolver + { + // + public: + + }; +} + diff --git a/MbDCode/VelICSolver.cpp b/MbDCode/VelICSolver.cpp new file mode 100644 index 0000000..92c3567 --- /dev/null +++ b/MbDCode/VelICSolver.cpp @@ -0,0 +1,70 @@ +#include "VelICSolver.h" +#include "SystemSolver.h" +#include "Part.h" +#include "Constraint.h" + +using namespace MbD; + +void MbD::VelICSolver::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. + nqsu = nEqns; + for (auto& con : *constraints) { + con->iG = eqnNo; + eqnNo += 1; + } + auto lastEqnNo = eqnNo - 1; + nEqns = eqnNo; //C++ uses index 0. + n = nEqns; +} + +void MbD::VelICSolver::run() +{ + std::string str = "MbD: Solving for velocity initial conditions."; + system->logString(str); + this->runBasic(); +} + +void MbD::VelICSolver::runBasic() +{ + //| qsudotOld qsudotWeights qsudotlam | + system->partsJointsMotionsDo([](std::shared_ptr item) { item->preVelIC(); }); + this->assignEquationNumbers(); + system->partsJointsMotionsDo([](std::shared_ptr item) { item->useEquationNumbers(); }); + auto qsudotOld = std::make_shared>(nqsu); + auto qsudotWeights = std::make_shared>(nqsu); + errorVector = std::make_shared>(n); + jacobian = std::make_shared>(n, n); + system->partsJointsMotionsDo([&](std::shared_ptr item) { item->fillqsudot(qsudotOld); }); + system->partsJointsMotionsDo([&](std::shared_ptr item) { item->fillqsudotWeights(qsudotWeights); }); + errorVector->zeroSelf(); + errorVector->atiplusFullColumn(0, qsudotWeights->timesFullColumn(qsudotOld)); + system->partsJointsMotionsDo([&](std::shared_ptr item) { item->fillVelICError(errorVector); }); + jacobian->zeroSelf(); + jacobian->atijplusDiagonalMatrix(0, 0, qsudotWeights); + system->partsJointsMotionsDo([&](std::shared_ptr item) { item->fillVelICJacob(jacobian); }); + matrixSolver = this->matrixSolverClassNew(); + this->solveEquations(); + auto qsudotlam = this->x; + system->partsJointsMotionsDo([&](std::shared_ptr item) { item->setqsudotlam(qsudotlam); }); + system->partsJointsMotionsDo([](std::shared_ptr item) { item->postVelIC(); }); +} diff --git a/MbDCode/VelICSolver.h b/MbDCode/VelICSolver.h new file mode 100644 index 0000000..725d7e6 --- /dev/null +++ b/MbDCode/VelICSolver.h @@ -0,0 +1,17 @@ +#pragma once + +#include "VelSolver.h" + +namespace MbD { + class VelICSolver : public VelSolver + { + //nqsu + public: + void assignEquationNumbers() override; + void run() override; + void runBasic(); + + int nqsu; + }; +} + diff --git a/MbDCode/VelKineSolver.cpp b/MbDCode/VelKineSolver.cpp new file mode 100644 index 0000000..7a6a561 --- /dev/null +++ b/MbDCode/VelKineSolver.cpp @@ -0,0 +1,3 @@ +#include "VelKineSolver.h" + +using namespace MbD; diff --git a/MbDCode/VelKineSolver.h b/MbDCode/VelKineSolver.h new file mode 100644 index 0000000..8828c38 --- /dev/null +++ b/MbDCode/VelKineSolver.h @@ -0,0 +1,13 @@ +#pragma once + +#include "VelSolver.h" + +namespace MbD { + class VelKineSolver : public VelSolver + { + // + public: + + }; +} + diff --git a/MbDCode/VelSolver.cpp b/MbDCode/VelSolver.cpp new file mode 100644 index 0000000..d012d62 --- /dev/null +++ b/MbDCode/VelSolver.cpp @@ -0,0 +1,58 @@ +#include "VelSolver.h" +#include "MatrixSolver.h" +#include "SystemSolver.h" +#include "CREATE.h" +#include "GESpMatParPvPrecise.h" +#include "SingularMatrixError.h" + +using namespace MbD; + +void MbD::VelSolver::basicSolveEquations() +{ + x = matrixSolver->solvewithsaveOriginal(jacobian, errorVector, true); +} + +void MbD::VelSolver::handleSingularMatrix() +{ + std::string str = typeid(*matrixSolver).name(); + if (str == "class MbD::GESpMatParPvMarkoFast") { + matrixSolver = CREATE::With(); + this->solveEquations(); + } + else { + str = typeid(*matrixSolver).name(); + if (str == "class MbD::GESpMatParPvPrecise") { + this->logSingularMatrixMessage(); + matrixSolver = this->matrixSolverClassNew(); + } + else { + assert(false); + } + } +} + +void MbD::VelSolver::logSingularMatrixMessage() +{ + std::string str = "MbD: Velocity solver has encountered a singular matrix."; + system->logString(str); +} + +std::shared_ptr MbD::VelSolver::matrixSolverClassNew() +{ + return CREATE::With(); +} + +void MbD::VelSolver::solveEquations() +{ + try { + this->basicSolveEquations(); + } + catch (SingularMatrixError ex) { + this->handleSingularMatrix(); + } +} + +void MbD::VelSolver::setSystem(Solver* sys) +{ + system = static_cast(sys); +} diff --git a/MbDCode/VelSolver.h b/MbDCode/VelSolver.h new file mode 100644 index 0000000..8eed930 --- /dev/null +++ b/MbDCode/VelSolver.h @@ -0,0 +1,31 @@ +#pragma once +#include + +#include "Solver.h" +#include "FullColumn.h" +#include "SparseMatrix.h" + +namespace MbD { + class MatrixSolver; + class SystemSolver; + + class VelSolver : public Solver + { + //system n x errorVector jacobian matrixSolver + public: + void basicSolveEquations(); + void handleSingularMatrix(); + void logSingularMatrixMessage(); + std::shared_ptr matrixSolverClassNew(); + void solveEquations(); + void setSystem(Solver* sys) override; + + SystemSolver* system; + int n; + FColDsptr x, errorVector; + SpMatDsptr jacobian; + std::shared_ptr matrixSolver; + + }; +} +