diff --git a/MbDCode/AbsConstraint.cpp b/MbDCode/AbsConstraint.cpp index 22b8015..9dec801 100644 --- a/MbDCode/AbsConstraint.cpp +++ b/MbDCode/AbsConstraint.cpp @@ -2,20 +2,19 @@ #include "PartFrame.h" using namespace MbD; +// +//AbsConstraint::AbsConstraint() {} +// +//AbsConstraint::AbsConstraint(const char* str) : Constraint(str) {} -AbsConstraint::AbsConstraint() {} - -AbsConstraint::AbsConstraint(const char* str) : Constraint(str) {} - -AbsConstraint::AbsConstraint(size_t i) +AbsConstraint::AbsConstraint(int i) { axis = i; } void AbsConstraint::initialize() { - axis = 0; - iqXminusOnePlusAxis = 0; + Constraint::initialize(); } void MbD::AbsConstraint::calcPostDynCorrectorIteration() @@ -30,11 +29,17 @@ void MbD::AbsConstraint::calcPostDynCorrectorIteration() void MbD::AbsConstraint::useEquationNumbers() { - iqXminusOnePlusAxis = static_cast(owner)->iqX - 1 + axis; + iqXminusOnePlusAxis = static_cast(owner)->iqX + axis; } void MbD::AbsConstraint::fillPosICJacob(SpMatDsptr mat) { - mat->atijplusNumber(iG, iqXminusOnePlusAxis, 1.0); - mat->atijplusNumber(iqXminusOnePlusAxis, iG, 1.0); + (*(mat->at(iG)))[iqXminusOnePlusAxis] += 1.0; + (*(mat->at(iqXminusOnePlusAxis)))[iG] += 1.0; +} + +void MbD::AbsConstraint::fillPosICError(FColDsptr col) +{ + Constraint::fillPosICError(col); + col->at(iqXminusOnePlusAxis) += lam; } diff --git a/MbDCode/AbsConstraint.h b/MbDCode/AbsConstraint.h index 650bd7d..9f1bf82 100644 --- a/MbDCode/AbsConstraint.h +++ b/MbDCode/AbsConstraint.h @@ -5,16 +5,17 @@ namespace MbD { { //axis iqXminusOnePlusAxis public: - AbsConstraint(); - AbsConstraint(const char* str); - AbsConstraint(size_t axis); + //AbsConstraint(); + //AbsConstraint(const char* str); + AbsConstraint(int axis); void initialize(); void calcPostDynCorrectorIteration() override; void useEquationNumbers() override; void fillPosICJacob(SpMatDsptr mat) override; + void fillPosICError(FColDsptr col) override; - size_t axis = -1; - size_t iqXminusOnePlusAxis = -1; + int axis = -1; + int iqXminusOnePlusAxis = -1; }; } diff --git a/MbDCode/AnyPosICNewtonRaphson.cpp b/MbDCode/AnyPosICNewtonRaphson.cpp index 0c26284..1f75321 100644 --- a/MbDCode/AnyPosICNewtonRaphson.cpp +++ b/MbDCode/AnyPosICNewtonRaphson.cpp @@ -1,6 +1,7 @@ #include "AnyPosICNewtonRaphson.h" #include "SystemSolver.h" #include "Item.h" +#include using namespace MbD; @@ -33,17 +34,25 @@ void MbD::AnyPosICNewtonRaphson::fillY() newMinusOld->equalSelfPlusFullColumnAt(x, 0); y->zeroSelf(); y->atiminusFullColumn(0, (qsuWeights->timesFullColumn(newMinusOld))); - system->partsJointsMotionsDo([&](std::shared_ptr item) { item->fillPosICError(y); }); + system->partsJointsMotionsDo([&](std::shared_ptr item) { + item->fillPosICError(y); + //std::cout << *y << std::endl; + }); + //std::cout << *y << std::endl; } void MbD::AnyPosICNewtonRaphson::fillPyPx() { pypx->zeroSelf(); pypx->atijminusDiagonalMatrix(0, 0, qsuWeights); - system->partsJointsMotionsDo([&](std::shared_ptr item) {item->fillPosICJacob(pypx); }); + system->partsJointsMotionsDo([&](std::shared_ptr item) { + item->fillPosICJacob(pypx); + //std::cout << *(pypx->at(3)) << std::endl; + }); + //std::cout << *pypx << std::endl; } void MbD::AnyPosICNewtonRaphson::passRootToSystem() { - system->partsJointsMotionsDo([&](std::shared_ptr item) {item->setqsulam(x); }); + system->partsJointsMotionsDo([&](std::shared_ptr item) { item->setqsulam(x); }); } diff --git a/MbDCode/AnyPosICNewtonRaphson.h b/MbDCode/AnyPosICNewtonRaphson.h index a2c1c08..547b2af 100644 --- a/MbDCode/AnyPosICNewtonRaphson.h +++ b/MbDCode/AnyPosICNewtonRaphson.h @@ -16,10 +16,10 @@ namespace MbD { void fillPyPx() override; void passRootToSystem() override; - size_t nqsu = -1; + int nqsu = -1; std::shared_ptr> qsuOld; std::shared_ptr> qsuWeights; - size_t nSingularMatrixError = -1; + int nSingularMatrixError = -1; }; } diff --git a/MbDCode/Array.h b/MbDCode/Array.h index 8a4a66f..a771007 100644 --- a/MbDCode/Array.h +++ b/MbDCode/Array.h @@ -12,33 +12,37 @@ namespace MbD { { public: Array() {} - Array(size_t count) : std::vector(count) {} - Array(size_t count, const T& value) : std::vector(count, value) {} + Array(std::vector vec) : std::vector(vec) {} + Array(int count) : std::vector(count) {} + Array(int count, const T& value) : std::vector(count, value) {} + Array(std::vector::iterator begin, std::vector::iterator end) : std::vector(begin, end) {} Array(std::initializer_list list) : std::vector{ list } {} void copyFrom(std::shared_ptr> x); virtual void zeroSelf() = 0; virtual double sumOfSquares() = 0; double rootMeanSquare(); - virtual size_t numberOfElements() = 0; + virtual int numberOfElements() = 0; + void swapRows(int i, int ii); }; template inline void Array::copyFrom(std::shared_ptr> x) { - for (size_t i = 0; i < x->size(); i++) { + for (int i = 0; i < x->size(); i++) { this->at(i) = x->at(i); } } - //template <> - //inline void Array::zeroSelf() { - // for (size_t i = 0; i < this->size(); i++) { - // this->at(i) = 0.0;; - // } - //} template inline double Array::rootMeanSquare() { return std::sqrt(this->sumOfSquares() / this->numberOfElements()); } + template + inline void Array::swapRows(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>>; diff --git a/MbDCode/AtPointConstraintIJ.cpp b/MbDCode/AtPointConstraintIJ.cpp index 593d418..a139467 100644 --- a/MbDCode/AtPointConstraintIJ.cpp +++ b/MbDCode/AtPointConstraintIJ.cpp @@ -4,13 +4,14 @@ using namespace MbD; -AtPointConstraintIJ::AtPointConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj, size_t axisi) : +AtPointConstraintIJ::AtPointConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj, int axisi) : ConstraintIJ(frmi, frmj), axis(axisi) { } void AtPointConstraintIJ::initialize() { + ConstraintIJ::initialize(); initriIeJeO(); } diff --git a/MbDCode/AtPointConstraintIJ.h b/MbDCode/AtPointConstraintIJ.h index cf96989..3272604 100644 --- a/MbDCode/AtPointConstraintIJ.h +++ b/MbDCode/AtPointConstraintIJ.h @@ -9,7 +9,7 @@ namespace MbD { { //axis riIeJeO public: - AtPointConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj, size_t axisi); + AtPointConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj, int axisi); void initialize(); void initializeLocally() override; void initializeGlobally() override; @@ -20,7 +20,7 @@ namespace MbD { MbD::ConstraintType type() override; void postPosICIteration() override; - size_t axis; + int axis; std::shared_ptr riIeJeO; }; } diff --git a/MbDCode/AtPointConstraintIqcJc.cpp b/MbDCode/AtPointConstraintIqcJc.cpp index c34f8ab..d383390 100644 --- a/MbDCode/AtPointConstraintIqcJc.cpp +++ b/MbDCode/AtPointConstraintIqcJc.cpp @@ -5,7 +5,7 @@ using namespace MbD; -AtPointConstraintIqcJc::AtPointConstraintIqcJc(EndFrmcptr frmi, EndFrmcptr frmj, size_t axisi) : +AtPointConstraintIqcJc::AtPointConstraintIqcJc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi) : AtPointConstraintIJ(frmi, frmj, axisi) { } @@ -29,6 +29,22 @@ void MbD::AtPointConstraintIqcJc::calcPostDynCorrectorIteration() void MbD::AtPointConstraintIqcJc::useEquationNumbers() { - iqXIminusOnePlusAxis = std::static_pointer_cast(frmI)->iqX() - 1 + axis; + iqXIminusOnePlusAxis = std::static_pointer_cast(frmI)->iqX() + axis; iqEI = std::static_pointer_cast(frmI)->iqE(); } + +void MbD::AtPointConstraintIqcJc::fillPosICError(FColDsptr col) +{ + Constraint::fillPosICError(col); + col->at(iqXIminusOnePlusAxis) -= lam; + col->atiplusFullVectortimes(iqEI, pGpEI, lam); +} + +void MbD::AtPointConstraintIqcJc::fillPosICJacob(SpMatDsptr mat) +{ + (*(mat->at(iG)))[iqXIminusOnePlusAxis] += -1.0; + (*(mat->at(iqXIminusOnePlusAxis)))[iG] += -1.0; + mat->atijplusFullRow(iG, iqEI, pGpEI); + mat->atijplusFullColumn(iqEI, iG, pGpEI->transpose()); + mat->atijplusFullMatrixtimes(iqEI, iqEI, ppGpEIpEI, lam); +} diff --git a/MbDCode/AtPointConstraintIqcJc.h b/MbDCode/AtPointConstraintIqcJc.h index bfb469c..bbe8d77 100644 --- a/MbDCode/AtPointConstraintIqcJc.h +++ b/MbDCode/AtPointConstraintIqcJc.h @@ -7,16 +7,18 @@ namespace MbD { { //pGpEI ppGpEIpEI iqXIminusOnePlusAxis iqEI public: - AtPointConstraintIqcJc(EndFrmcptr frmi, EndFrmcptr frmj, size_t axisi); + AtPointConstraintIqcJc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi); void initializeGlobally() override; void initriIeJeO() override; void calcPostDynCorrectorIteration() override; void useEquationNumbers() override; + void fillPosICError(FColDsptr col) override; + void fillPosICJacob(SpMatDsptr mat) override; FRowDsptr pGpEI; FMatDsptr ppGpEIpEI; - size_t iqXIminusOnePlusAxis = -1; - size_t iqEI = -1; + int iqXIminusOnePlusAxis = -1; + int iqEI = -1; }; } diff --git a/MbDCode/AtPointConstraintIqcJqc.cpp b/MbDCode/AtPointConstraintIqcJqc.cpp index 53e2cff..4a6e80a 100644 --- a/MbDCode/AtPointConstraintIqcJqc.cpp +++ b/MbDCode/AtPointConstraintIqcJqc.cpp @@ -5,7 +5,7 @@ using namespace MbD; -AtPointConstraintIqcJqc::AtPointConstraintIqcJqc(EndFrmcptr frmi, EndFrmcptr frmj, size_t axisi) : +AtPointConstraintIqcJqc::AtPointConstraintIqcJqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi) : AtPointConstraintIqcJc(frmi, frmj, axisi) { } @@ -30,6 +30,23 @@ void MbD::AtPointConstraintIqcJqc::calcPostDynCorrectorIteration() void MbD::AtPointConstraintIqcJqc::useEquationNumbers() { AtPointConstraintIqcJc::useEquationNumbers(); - iqXJminusOnePlusAxis = std::static_pointer_cast(frmJ)->iqX() - 1 + axis; + iqXJminusOnePlusAxis = std::static_pointer_cast(frmJ)->iqX() + axis; iqEJ = std::static_pointer_cast(frmJ)->iqE(); } + +void MbD::AtPointConstraintIqcJqc::fillPosICError(FColDsptr col) +{ + AtPointConstraintIqcJc::fillPosICError(col); + col->at(iqXJminusOnePlusAxis) += lam; + col->atiplusFullVectortimes(iqEJ, pGpEJ, lam); +} + +void MbD::AtPointConstraintIqcJqc::fillPosICJacob(SpMatDsptr mat) +{ + AtPointConstraintIqcJc::fillPosICJacob(mat); + (*(mat->at(iG)))[iqXJminusOnePlusAxis] += 1.0; + (*(mat->at(iqXJminusOnePlusAxis)))[iG] += 1.0; + mat->atijplusFullRow(iG, iqEJ, pGpEJ); + mat->atijplusFullColumn(iqEJ, iG, pGpEJ->transpose()); + mat->atijplusFullMatrixtimes(iqEJ, iqEJ, ppGpEJpEJ, lam); +} diff --git a/MbDCode/AtPointConstraintIqcJqc.h b/MbDCode/AtPointConstraintIqcJqc.h index f49820a..33f759e 100644 --- a/MbDCode/AtPointConstraintIqcJqc.h +++ b/MbDCode/AtPointConstraintIqcJqc.h @@ -7,15 +7,17 @@ namespace MbD { { //pGpEJ ppGpEJpEJ iqXJminusOnePlusAxis iqEJ public: - AtPointConstraintIqcJqc(EndFrmcptr frmi, EndFrmcptr frmj, size_t axisi); + AtPointConstraintIqcJqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi); void initializeGlobally() override; void initriIeJeO() override; void calcPostDynCorrectorIteration() override; void useEquationNumbers() override; + void fillPosICError(FColDsptr col) override; + void fillPosICJacob(SpMatDsptr mat) override; FRowDsptr pGpEJ; FMatDsptr ppGpEJpEJ; - size_t iqXJminusOnePlusAxis = -1, iqEJ = -1; + int iqXJminusOnePlusAxis = -1, iqEJ = -1; }; } diff --git a/MbDCode/AtPointConstraintIqctJqc.cpp b/MbDCode/AtPointConstraintIqctJqc.cpp index 24e0d19..d12feea 100644 --- a/MbDCode/AtPointConstraintIqctJqc.cpp +++ b/MbDCode/AtPointConstraintIqctJqc.cpp @@ -4,7 +4,7 @@ using namespace MbD; -AtPointConstraintIqctJqc::AtPointConstraintIqctJqc(EndFrmcptr frmi, EndFrmcptr frmj, size_t axisi) : +AtPointConstraintIqctJqc::AtPointConstraintIqctJqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi) : AtPointConstraintIqcJqc(frmi, frmj, axisi) { } diff --git a/MbDCode/AtPointConstraintIqctJqc.h b/MbDCode/AtPointConstraintIqctJqc.h index a4edd24..f786cf0 100644 --- a/MbDCode/AtPointConstraintIqctJqc.h +++ b/MbDCode/AtPointConstraintIqctJqc.h @@ -7,7 +7,7 @@ namespace MbD { { //pGpt ppGpEIpt ppGptpt public: - AtPointConstraintIqctJqc(EndFrmcptr frmi, EndFrmcptr frmj, size_t axisi); + AtPointConstraintIqctJqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi); void initializeGlobally() override; void initriIeJeO() override; void calcPostDynCorrectorIteration() override; diff --git a/MbDCode/BasicIntegrator.cpp b/MbDCode/BasicIntegrator.cpp index 2cc20d0..74457f3 100644 --- a/MbDCode/BasicIntegrator.cpp +++ b/MbDCode/BasicIntegrator.cpp @@ -1,3 +1,141 @@ #include "BasicIntegrator.h" +#include "CREATE.h" +#include "StableBackwardDifference.h" +#include "IntegratorInterface.h" using namespace MbD; + +void MbD::BasicIntegrator::initializeLocally() +{ + _continue = true; +} + +void MbD::BasicIntegrator::iStep(int integer) +{ + istep = integer; + opBDF->setiStep(integer); +} + +void MbD::BasicIntegrator::postFirstStep() +{ +} + +void MbD::BasicIntegrator::postRun() +{ +} + +void MbD::BasicIntegrator::postStep() +{ +} + +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->direction = system->direction; + this->orderMax = system->orderMax(); +} + +void MbD::BasicIntegrator::setSystem(IntegratorInterface* sys) +{ + system = sys; +} + +void MbD::BasicIntegrator::calcOperatorMatrix() +{ + opBDF->calcOperatorMatrix(); +} + +void MbD::BasicIntegrator::incrementTime() +{ + //| istepNew | + tpast->insert(tpast->begin(), t); + + if ((int)tpast->size() > (orderMax + 1)) { tpast->pop_back(); } + auto istepNew = istep + 1; + this->iStep(istepNew); + this->setorder(orderNew); + h = hnew; + this->settnew(t + (direction * h)); + this->calcOperatorMatrix(); + //system->incrementTime(tnew); +} + +void MbD::BasicIntegrator::incrementTry() +{ + assert(false); +} + +void MbD::BasicIntegrator::initialize() +{ + Solver::initialize(); + //statistics = IdentityDictionary new. + tpast = std::make_shared>(); + opBDF = CREATE::With(); + opBDF->timeNodes = tpast; +} + +void MbD::BasicIntegrator::logString(std::string& str) +{ + system->logString(str); +} + +void MbD::BasicIntegrator::run() +{ + this->preRun(); + this->initializeLocally(); + this->initializeGlobally(); + this->firstStep(); + this->subsequentSteps(); + this->finalize(); + this->reportStats(); + this->postRun(); +} + +void MbD::BasicIntegrator::selectOrder() +{ + assert(false); +} + +void MbD::BasicIntegrator::preFirstStep() +{ + system->preFirstStep(); +} + +void MbD::BasicIntegrator::preRun() +{ +} + +void MbD::BasicIntegrator::preStep() +{ + assert(false); +} + +void MbD::BasicIntegrator::reportStats() +{ + assert(false); +} + +void MbD::BasicIntegrator::firstStep() +{ + assert(false); +} + +void MbD::BasicIntegrator::setorder(int o) +{ + order = o; + //opBD->setorder(o); +} + +void MbD::BasicIntegrator::settnew(double t) +{ +tnew = t; +//this->time(double); +} + +void MbD::BasicIntegrator::subsequentSteps() +{ + while (_continue) { this->nextStep(); } +} diff --git a/MbDCode/BasicIntegrator.h b/MbDCode/BasicIntegrator.h index 3914a6b..a1df5ff 100644 --- a/MbDCode/BasicIntegrator.h +++ b/MbDCode/BasicIntegrator.h @@ -1,13 +1,48 @@ #pragma once +#include #include "Integrator.h" namespace MbD { + class IntegratorInterface; + class DifferenceOperator; + class BasicIntegrator : public Integrator { - // + //istep iTry maxTry tpast t tnew h hnew order orderNew orderMax opBDF continue public: + virtual void calcOperatorMatrix(); + virtual void incrementTime(); + virtual void incrementTry(); + void initialize() override; + void initializeGlobally() override; + void initializeLocally() override; + virtual void iStep(int i); + virtual void postFirstStep(); + virtual void postRun() override; + virtual void postStep(); + virtual void preFirstStep(); + virtual void preRun() override; + virtual void preStep(); + virtual void reportStats() override; + void run() override; + virtual void selectOrder(); + virtual void subsequentSteps(); + void setSystem(IntegratorInterface* sys); + void logString(std::string& str) override; + virtual void firstStep(); + virtual void nextStep() = 0; + + virtual void setorder(int o); + virtual void settnew(double t); + IntegratorInterface* system; + int istep = 0, iTry = 0, maxTry = 0; + std::shared_ptr> tpast; + double t = 0, tnew = 0, h = 0, hnew = 0; + int order = 0, orderNew = 0, orderMax = 0; + std::shared_ptr opBDF; + bool _continue = false; }; } diff --git a/MbDCode/BasicQuasiIntegrator.cpp b/MbDCode/BasicQuasiIntegrator.cpp index 4c91a4f..e15e4d7 100644 --- a/MbDCode/BasicQuasiIntegrator.cpp +++ b/MbDCode/BasicQuasiIntegrator.cpp @@ -1,3 +1,60 @@ #include "BasicQuasiIntegrator.h" +#include "IntegratorInterface.h" using namespace MbD; + +void MbD::BasicQuasiIntegrator::firstStep() +{ + istep = 0; + this->preFirstStep(); + iTry = 1; + orderNew = 1; + this->selectFirstStepSize(); + this->incrementTime(); + //this->runInitialConditionTypeSolution(); + //this->reportTrialStepStats(); + + //while (this->isRedoingFirstStep()) + //{ + // this->incrementTry(); + // orderNew = 1; + // this->selectFirstStepSize(); + // this->changeTime(); + // this->runInitialConditionTypeSolution(); + // this->reportTrialStepStats(); + //} + //this->postFirstStep(); + //this->reportStepStats(); +} + +void MbD::BasicQuasiIntegrator::nextStep() +{ + this->preStep(); + iTry = 1; + this->selectOrder(); + //this->selectStepSize(); + //this->incrementTime(); + //this->runInitialConditionTypeSolution(); + //this->reportTrialStepStats(); + //while (this->isRedoingStep()) { + // this->incrementTry(); + // this->selectOrder(); + // this->selectStepSize(); + // this->changeTime(); + // this->runInitialConditionTypeSolution(); + // this->reportTrialStepStats(); + //} + //this->postStep(); + //this->reportStepStats(); +} + +void MbD::BasicQuasiIntegrator::selectFirstStepSize() +{ + if (iTry == 1) { + hnew = direction * (system->tout - t); + } + else { + hnew = 0.25 * h; + } + hnew = system->suggestSmallerOrAcceptFirstStepSize(hnew); +} diff --git a/MbDCode/BasicQuasiIntegrator.h b/MbDCode/BasicQuasiIntegrator.h index 6b8f0d8..8721ea0 100644 --- a/MbDCode/BasicQuasiIntegrator.h +++ b/MbDCode/BasicQuasiIntegrator.h @@ -3,11 +3,20 @@ #include "BasicIntegrator.h" namespace MbD { - class BasicQuasiIntegrator : public BasicIntegrator - { - // - public: - - }; + class BasicQuasiIntegrator : public BasicIntegrator + { + // + public: + void firstStep(); + //bool isRedoingFirstStep(); + //bool isRedoingStep(); + void nextStep(); + //void reportStepStats(); + //void reportTrialStepStats(); + //void runInitialConditionTypeSolution(); + void selectFirstStepSize(); + //void selectStepSize(); + //void runInitialConditionTypeSolution(); + }; } diff --git a/MbDCode/CREATE.h b/MbDCode/CREATE.h index 8c18079..58a1c0c 100644 --- a/MbDCode/CREATE.h +++ b/MbDCode/CREATE.h @@ -23,7 +23,7 @@ namespace MbD { inst->initialize(); return inst; } - static std::shared_ptr With(size_t n) { + static std::shared_ptr With(int n) { auto inst = std::make_shared(n); inst->initialize(); return inst; @@ -33,17 +33,17 @@ namespace MbD { inst->initialize(); return inst; } - static std::shared_ptr With(EndFrmcptr frmi, EndFrmcptr frmj, size_t axis) { + static std::shared_ptr With(EndFrmcptr frmi, EndFrmcptr frmj, int axis) { auto inst = std::make_shared(frmi, frmj, axis); inst->initialize(); return inst; } - static std::shared_ptr With(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, size_t axisk) { + static std::shared_ptr With(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk) { auto inst = std::make_shared(frmi, frmj, frmk, axisk); inst->initialize(); return inst; } - static std::shared_ptr ConstraintWith(EndFrmcptr frmi, EndFrmcptr frmj, size_t axis) { + static std::shared_ptr ConstraintWith(EndFrmcptr frmi, EndFrmcptr frmj, int axis) { std::shared_ptr inst; std::string str = typeid(T(frmi, frmj, axis)).name(); if (str == "class MbD::AtPointConstraintIJ") { @@ -65,12 +65,12 @@ namespace MbD { inst->initialize(); return inst; } - static std::shared_ptr With(EndFrmcptr frmi, EndFrmcptr frmj, size_t axisi, size_t axisj) { + static std::shared_ptr With(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj) { auto inst = std::make_shared(frmi, frmj, axisi, axisj); inst->initialize(); return inst; } - static std::shared_ptr ConstraintWith(EndFrmcptr frmi, EndFrmcptr frmj, size_t axisi, size_t axisj) { + static std::shared_ptr ConstraintWith(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj) { std::shared_ptr inst; std::string str = typeid(T(frmi, frmj, axisi, axisj)).name(); if (str == "class MbD::DirectionCosineConstraintIJ") { diff --git a/MbDCode/Constraint.cpp b/MbDCode/Constraint.cpp index ab0bfc9..6c458a8 100644 --- a/MbDCode/Constraint.cpp +++ b/MbDCode/Constraint.cpp @@ -1,4 +1,8 @@ +#include +#include + #include "Constraint.h" +#include "FullColumn.h" #include "enum.h" using namespace MbD; @@ -13,9 +17,9 @@ Constraint::Constraint(const char* str) : Item(str) void Constraint::initialize() { - iG = -1; - aG = 0.0; - lam = 0.0; + auto now = std::chrono::high_resolution_clock::now(); + auto nanoseconds = now.time_since_epoch().count(); + name = std::to_string(nanoseconds); } void MbD::Constraint::postInput() @@ -41,25 +45,32 @@ void MbD::Constraint::prePosIC() Item::prePosIC(); } -void MbD::Constraint::fillEssenConstraints(std::shared_ptrsptr, std::shared_ptr>> essenConstraints) +void MbD::Constraint::fillEssenConstraints(std::shared_ptr sptr, std::shared_ptr>> essenConstraints) { if (this->type() == MbD::essential) { essenConstraints->push_back(sptr); } } -void MbD::Constraint::fillDispConstraints(std::shared_ptrsptr, std::shared_ptr>> dispConstraints) +void MbD::Constraint::fillDispConstraints(std::shared_ptr sptr, std::shared_ptr>> dispConstraints) { if (this->type() == MbD::displacement) { dispConstraints->push_back(sptr); } } -void MbD::Constraint::fillPerpenConstraints(std::shared_ptrsptr, std::shared_ptr>> perpenConstraints) +void MbD::Constraint::fillPerpenConstraints(std::shared_ptr sptr, std::shared_ptr>> perpenConstraints) { if (this->type() == MbD::perpendicular) { perpenConstraints->push_back(sptr); } } +void MbD::Constraint::fillRedundantConstraints(std::shared_ptr sptr, std::shared_ptr>> redunConstraints) +{ + if (this->type() == MbD::redundant) { + redunConstraints->push_back(sptr); + } +} + MbD::ConstraintType MbD::Constraint::type() { return MbD::essential; @@ -74,3 +85,41 @@ void MbD::Constraint::setqsulam(FColDsptr col) { lam = col->at(iG); } + +void MbD::Constraint::fillPosICError(FColDsptr col) +{ + col->at(iG) += aG; +} + +void MbD::Constraint::removeRedundantConstraints(std::shared_ptr> redundantEqnNos) +{ + //My owner should handle this. + assert(false); +} + +void MbD::Constraint::reactivateRedundantConstraints() +{ + //My owner should handle this. + assert(false); +} + +bool MbD::Constraint::isRedundant() +{ + return false; +} + +void MbD::Constraint::outputStates() +{ + Item::outputStates(); + std::stringstream ss; + ss << "iG = " << iG << std::endl; + ss << "aG = " << aG << std::endl; + ss << "lam = " << lam << std::endl; + auto str = ss.str(); + this->logString(str); +} + +void MbD::Constraint::preDyn() +{ + mu = 0.0; +} diff --git a/MbDCode/Constraint.h b/MbDCode/Constraint.h index c7dfc44..7a6c42c 100644 --- a/MbDCode/Constraint.h +++ b/MbDCode/Constraint.h @@ -16,16 +16,24 @@ namespace MbD { void setOwner(Item* x); Item* getOwner(); void prePosIC() override; - virtual void fillEssenConstraints(std::shared_ptrsptr, std::shared_ptr>> essenConstraints); - virtual void fillDispConstraints(std::shared_ptrsptr, std::shared_ptr>> dispConstraints); - virtual void fillPerpenConstraints(std::shared_ptrsptr, std::shared_ptr>> perpenConstraints); + 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 MbD::ConstraintType type(); void fillqsulam(FColDsptr col) override; void setqsulam(FColDsptr col) override; - - size_t iG; - double aG; //Constraint function - double lam; //Lambda is Lagrange Multiplier + void fillPosICError(FColDsptr col) override; + void removeRedundantConstraints(std::shared_ptr> redundantEqnNos) override; + void reactivateRedundantConstraints() override; + virtual bool isRedundant(); + void outputStates() override; + void preDyn() override; + + int iG = -1; + double aG = 0.0; //Constraint function + double lam = 0.0; //Lambda is Lagrange Multiplier + double mu = 0, lamDeriv = 0; Item* owner; //A Joint or PartFrame owns the constraint. //Use raw pointer when pointing backwards. }; } diff --git a/MbDCode/DiagonalMatrix.h b/MbDCode/DiagonalMatrix.h index a3d8671..0dab606 100644 --- a/MbDCode/DiagonalMatrix.h +++ b/MbDCode/DiagonalMatrix.h @@ -9,24 +9,24 @@ namespace MbD { { // public: - DiagonalMatrix(size_t count) : Array(count) {} + DiagonalMatrix(int count) : Array(count) {} DiagonalMatrix(std::initializer_list list) : Array{ list } {} - void atiputDiagonalMatrix(size_t i, std::shared_ptr < DiagonalMatrix> diagMat); + void atiputDiagonalMatrix(int i, std::shared_ptr < DiagonalMatrix> diagMat); std::shared_ptr> timesFullColumn(std::shared_ptr> fullCol); - size_t nrow() { - return this->size(); + int nrow() { + return (int) this->size(); } - size_t ncol() { - return this->size(); + int ncol() { + return (int) this->size(); } double sumOfSquares() override; - size_t numberOfElements() override; + int numberOfElements() override; void zeroSelf() override; }; template - inline void DiagonalMatrix::atiputDiagonalMatrix(size_t i, std::shared_ptr> diagMat) + inline void DiagonalMatrix::atiputDiagonalMatrix(int i, std::shared_ptr> diagMat) { - for (size_t ii = 0; ii < diagMat->size(); ii++) + for (int ii = 0; ii < diagMat->size(); ii++) { this->at(i + ii) = diagMat->at(ii); } @@ -36,9 +36,9 @@ namespace MbD { { //"a*b = a(i,j)b(j) sum j." - auto nrow = this->size(); + auto nrow = (int) this->size(); auto answer = std::make_shared>(nrow); - for (size_t i = 0; i < nrow; i++) + for (int i = 0; i < nrow; i++) { answer->at(i) = this->at(i) * fullCol->at(i); } @@ -48,7 +48,7 @@ namespace MbD { inline double DiagonalMatrix::sumOfSquares() { double sum = 0.0; - for (size_t i = 0; i < this->size(); i++) + for (int i = 0; i < this->size(); i++) { double element = this->at(i); sum += element * element; @@ -56,15 +56,15 @@ namespace MbD { return sum; } template - inline size_t DiagonalMatrix::numberOfElements() + inline int DiagonalMatrix::numberOfElements() { - auto n = this->size(); + auto n = (int) this->size(); return n * n; } template<> inline void DiagonalMatrix::zeroSelf() { - for (size_t i = 0; i < this->size(); i++) { + for (int i = 0; i < this->size(); i++) { this->at(i) = 0.0;; } } diff --git a/MbDCode/DifferenceOperator.cpp b/MbDCode/DifferenceOperator.cpp new file mode 100644 index 0000000..99110d7 --- /dev/null +++ b/MbDCode/DifferenceOperator.cpp @@ -0,0 +1,63 @@ +#include "DifferenceOperator.h" +#include "CREATE.h" +#include "SingularMatrixError.h" +#include "LDUFullMatParPv.h" + +void MbD::DifferenceOperator::calcOperatorMatrix() +{ + //Compute operatorMatrix such that + //value(time) : = (operatorMatrix at : 1) timesColumn : series. + //valuedot(time) : = (operatorMatrix at : 2) timesColumn : series. + //valueddot(time) : = (operatorMatrix at : 3) timesColumn : series. + + this->formTaylorMatrix(); + try { + assert(false); + //operatorMatrix = CREATE::With()->inversesaveOriginal(taylorMatrix, false); + } + catch (SingularMatrixError ex) { + } +} + +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) +{ + iStep = i; +} + +void MbD::DifferenceOperator::setorder(int o) +{ + order = o; +} + +void MbD::DifferenceOperator::instanciateTaylorMatrix() +{ + if (taylorMatrix->empty() || (taylorMatrix->nrow() != (order + 1))) { + taylorMatrix = std::make_shared>(order + 1, order + 1); + } +} + +void MbD::DifferenceOperator::formTaylorRowwithTimeNodederivative(int i, int ii, int k) +{ + //| rowi hi hipower aij | + auto rowi = taylorMatrix->at(i); + for (int j = 0; j < k; 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 + 1; j < order + 1; j++) + { + hipower = hipower * hi; + assert(false); + //aij = hipower * (OneOverFactorials at : j - k - 1); + //rowi at : j put : aij + } +} diff --git a/MbDCode/DifferenceOperator.h b/MbDCode/DifferenceOperator.h new file mode 100644 index 0000000..c252141 --- /dev/null +++ b/MbDCode/DifferenceOperator.h @@ -0,0 +1,26 @@ +#pragma once + +#include + +#include "FullMatrix.h" + +namespace MbD { + class DifferenceOperator + { + //iStep order taylorMatrix operatorMatrix time timeNodes + public: + void calcOperatorMatrix(); + void initialize(); + virtual void setiStep(int i); + virtual void setorder(int o); + virtual void formTaylorMatrix() = 0; + virtual void instanciateTaylorMatrix(); + virtual void formTaylorRowwithTimeNodederivative(int i, int ii, int k); + + int iStep = 0, order = 0; + std::shared_ptr> taylorMatrix, operatorMatrix; + double time = 0; + std::shared_ptr> timeNodes; + }; +} + diff --git a/MbDCode/DirectionCosineConstraintIJ.cpp b/MbDCode/DirectionCosineConstraintIJ.cpp index b86a902..b5552c1 100644 --- a/MbDCode/DirectionCosineConstraintIJ.cpp +++ b/MbDCode/DirectionCosineConstraintIJ.cpp @@ -5,13 +5,14 @@ using namespace MbD; -DirectionCosineConstraintIJ::DirectionCosineConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj, size_t axisi, size_t axisj) : +DirectionCosineConstraintIJ::DirectionCosineConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj) : ConstraintIJ(frmi, frmj), axisI(axisi), axisJ(axisj) { } void DirectionCosineConstraintIJ::initialize() { + ConstraintIJ::initialize(); initaAijIeJe(); } diff --git a/MbDCode/DirectionCosineConstraintIJ.h b/MbDCode/DirectionCosineConstraintIJ.h index fc1f12b..6e18bb5 100644 --- a/MbDCode/DirectionCosineConstraintIJ.h +++ b/MbDCode/DirectionCosineConstraintIJ.h @@ -9,7 +9,7 @@ namespace MbD { { //axisI axisJ aAijIeJe public: - DirectionCosineConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj, size_t axisi, size_t axisj); + DirectionCosineConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj); void initialize(); void initializeLocally() override; void initializeGlobally() override; @@ -20,7 +20,7 @@ namespace MbD { void postPosICIteration() override; MbD::ConstraintType type() override; - size_t axisI, axisJ; + int axisI, axisJ; std::shared_ptr aAijIeJe; }; } diff --git a/MbDCode/DirectionCosineConstraintIqcJc.cpp b/MbDCode/DirectionCosineConstraintIqcJc.cpp index dbfeef4..8bfb6ce 100644 --- a/MbDCode/DirectionCosineConstraintIqcJc.cpp +++ b/MbDCode/DirectionCosineConstraintIqcJc.cpp @@ -5,7 +5,7 @@ using namespace MbD; -DirectionCosineConstraintIqcJc::DirectionCosineConstraintIqcJc(EndFrmcptr frmi, EndFrmcptr frmj, size_t axisi, size_t axisj) : +DirectionCosineConstraintIqcJc::DirectionCosineConstraintIqcJc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj) : DirectionCosineConstraintIJ(frmi, frmj, axisi, axisj) { } @@ -27,3 +27,16 @@ void MbD::DirectionCosineConstraintIqcJc::useEquationNumbers() { iqEI = std::static_pointer_cast(frmI)->iqE(); } + +void MbD::DirectionCosineConstraintIqcJc::fillPosICError(FColDsptr col) +{ + Constraint::fillPosICError(col); + col->atiplusFullVectortimes(iqEI, pGpEI, lam); +} + +void MbD::DirectionCosineConstraintIqcJc::fillPosICJacob(SpMatDsptr mat) +{ + mat->atijplusFullRow(iG, iqEI, pGpEI); + mat->atijplusFullColumn(iqEI, iG, pGpEI->transpose()); + mat->atijplusFullMatrixtimes(iqEI, iqEI, ppGpEIpEI, lam); +} diff --git a/MbDCode/DirectionCosineConstraintIqcJc.h b/MbDCode/DirectionCosineConstraintIqcJc.h index 0d6d8fb..65ef5ea 100644 --- a/MbDCode/DirectionCosineConstraintIqcJc.h +++ b/MbDCode/DirectionCosineConstraintIqcJc.h @@ -7,14 +7,16 @@ namespace MbD { { //pGpEI ppGpEIpEI iqEI public: - DirectionCosineConstraintIqcJc(EndFrmcptr frmi, EndFrmcptr frmj, size_t axisi, size_t axisj); + DirectionCosineConstraintIqcJc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj); void initaAijIeJe() override; void calcPostDynCorrectorIteration() override; void useEquationNumbers() override; + void fillPosICError(FColDsptr col) override; + void fillPosICJacob(SpMatDsptr mat) override; FRowDsptr pGpEI; FMatDsptr ppGpEIpEI; - size_t iqEI = -1; + int iqEI = -1; }; } diff --git a/MbDCode/DirectionCosineConstraintIqcJqc.cpp b/MbDCode/DirectionCosineConstraintIqcJqc.cpp index cbbed6b..b0812f3 100644 --- a/MbDCode/DirectionCosineConstraintIqcJqc.cpp +++ b/MbDCode/DirectionCosineConstraintIqcJqc.cpp @@ -5,7 +5,7 @@ using namespace MbD; -DirectionCosineConstraintIqcJqc::DirectionCosineConstraintIqcJqc(EndFrmcptr frmi, EndFrmcptr frmj, size_t axisi, size_t axisj) : +DirectionCosineConstraintIqcJqc::DirectionCosineConstraintIqcJqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj) : DirectionCosineConstraintIqcJc(frmi, frmj, axisi, axisj) { } @@ -29,3 +29,20 @@ void MbD::DirectionCosineConstraintIqcJqc::useEquationNumbers() DirectionCosineConstraintIqcJc::useEquationNumbers(); iqEJ = std::static_pointer_cast(frmJ)->iqE(); } + +void MbD::DirectionCosineConstraintIqcJqc::fillPosICError(FColDsptr col) +{ + DirectionCosineConstraintIqcJc::fillPosICError(col); + col->atiplusFullVectortimes(iqEJ, pGpEJ, lam); +} + +void MbD::DirectionCosineConstraintIqcJqc::fillPosICJacob(SpMatDsptr mat) +{ + DirectionCosineConstraintIqcJc::fillPosICJacob(mat); + mat->atijplusFullRow(iG, iqEJ, pGpEJ); + mat->atijplusFullColumn(iqEJ, iG, pGpEJ->transpose()); + auto ppGpEIpEJlam = ppGpEIpEJ->times(lam); + mat->atijplusFullMatrix(iqEI, iqEJ, ppGpEIpEJlam); + mat->atijplusTransposeFullMatrix(iqEJ, iqEI, ppGpEIpEJlam); + mat->atijplusFullMatrixtimes(iqEJ, iqEJ, ppGpEJpEJ, lam); +} diff --git a/MbDCode/DirectionCosineConstraintIqcJqc.h b/MbDCode/DirectionCosineConstraintIqcJqc.h index a019ff7..649c8e1 100644 --- a/MbDCode/DirectionCosineConstraintIqcJqc.h +++ b/MbDCode/DirectionCosineConstraintIqcJqc.h @@ -7,15 +7,17 @@ namespace MbD { { //pGpEJ ppGpEIpEJ ppGpEJpEJ iqEJ public: - DirectionCosineConstraintIqcJqc(EndFrmcptr frmi, EndFrmcptr frmj, size_t axisi, size_t axisj); + DirectionCosineConstraintIqcJqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj); void initaAijIeJe() override; void calcPostDynCorrectorIteration() override; void useEquationNumbers() override; + void fillPosICError(FColDsptr col) override; + void fillPosICJacob(SpMatDsptr mat) override; FRowDsptr pGpEJ; FMatDsptr ppGpEIpEJ; FMatDsptr ppGpEJpEJ; - size_t iqEJ = -1; + int iqEJ = -1; }; } diff --git a/MbDCode/DirectionCosineConstraintIqctJqc.cpp b/MbDCode/DirectionCosineConstraintIqctJqc.cpp index 536d401..b27bc59 100644 --- a/MbDCode/DirectionCosineConstraintIqctJqc.cpp +++ b/MbDCode/DirectionCosineConstraintIqctJqc.cpp @@ -4,7 +4,7 @@ using namespace MbD; -DirectionCosineConstraintIqctJqc::DirectionCosineConstraintIqctJqc(EndFrmcptr frmi, EndFrmcptr frmj, size_t axisi, size_t axisj) : +DirectionCosineConstraintIqctJqc::DirectionCosineConstraintIqctJqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj) : DirectionCosineConstraintIqcJqc(frmi, frmj, axisi, axisj) { } diff --git a/MbDCode/DirectionCosineConstraintIqctJqc.h b/MbDCode/DirectionCosineConstraintIqctJqc.h index b6b0a28..38e2fa1 100644 --- a/MbDCode/DirectionCosineConstraintIqctJqc.h +++ b/MbDCode/DirectionCosineConstraintIqctJqc.h @@ -7,7 +7,7 @@ namespace MbD { { //pGpt ppGpEIpt ppGpEJpt ppGptpt public: - DirectionCosineConstraintIqctJqc(EndFrmcptr frmi, EndFrmcptr frmj, size_t axisi, size_t axisj); + DirectionCosineConstraintIqctJqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj); void initaAijIeJe() override; MbD::ConstraintType type() override; diff --git a/MbDCode/DirectionCosineIecJec.cpp b/MbDCode/DirectionCosineIecJec.cpp index 1b0c7c5..3f40ab4 100644 --- a/MbDCode/DirectionCosineIecJec.cpp +++ b/MbDCode/DirectionCosineIecJec.cpp @@ -9,7 +9,7 @@ DirectionCosineIecJec::DirectionCosineIecJec() { } -DirectionCosineIecJec::DirectionCosineIecJec(EndFrmcptr frmi, EndFrmcptr frmj, size_t axisi, size_t axisj) : +DirectionCosineIecJec::DirectionCosineIecJec(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj) : KinematicIeJe(frmi, frmj), axisI(axisi), axisJ(axisj) { @@ -21,3 +21,33 @@ void MbD::DirectionCosineIecJec::calcPostDynCorrectorIteration() aAjOJe = frmJ->aAjOe(axisJ); aAijIeJe = aAjOIe->dot(aAjOJe); } + +FRowDsptr MbD::DirectionCosineIecJec::pvaluepXJ() +{ + assert(false); + return FRowDsptr(); +} + +FRowDsptr MbD::DirectionCosineIecJec::pvaluepEJ() +{ + assert(false); + return FRowDsptr(); +} + +FMatDsptr MbD::DirectionCosineIecJec::ppvaluepXJpEK() +{ + assert(false); + return FMatDsptr(); +} + +FMatDsptr MbD::DirectionCosineIecJec::ppvaluepEJpEK() +{ + assert(false); + return FMatDsptr(); +} + +FMatDsptr MbD::DirectionCosineIecJec::ppvaluepEJpEJ() +{ + assert(false); + return FMatDsptr(); +} diff --git a/MbDCode/DirectionCosineIecJec.h b/MbDCode/DirectionCosineIecJec.h index eb05b6a..c2de495 100644 --- a/MbDCode/DirectionCosineIecJec.h +++ b/MbDCode/DirectionCosineIecJec.h @@ -12,10 +12,15 @@ namespace MbD { //aAijIeJe axisI axisJ aAjOIe aAjOJe public: DirectionCosineIecJec(); - DirectionCosineIecJec(EndFrmcptr frmi, EndFrmcptr frmj, size_t axisi, size_t axisj); + DirectionCosineIecJec(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj); void calcPostDynCorrectorIteration() override; + FRowDsptr pvaluepXJ() override; + FRowDsptr pvaluepEJ() override; + FMatDsptr ppvaluepXJpEK() override; + FMatDsptr ppvaluepEJpEK() override; + FMatDsptr ppvaluepEJpEJ() override; - size_t axisI, axisJ; //0, 1, 2 = x, y, z + int axisI, axisJ; //0, 1, 2 = x, y, z double aAijIeJe; std::shared_ptr> aAjOIe, aAjOJe; }; diff --git a/MbDCode/DirectionCosineIeqcJec.cpp b/MbDCode/DirectionCosineIeqcJec.cpp index b75949a..3975ae7 100644 --- a/MbDCode/DirectionCosineIeqcJec.cpp +++ b/MbDCode/DirectionCosineIeqcJec.cpp @@ -7,7 +7,7 @@ DirectionCosineIeqcJec::DirectionCosineIeqcJec() { } -DirectionCosineIeqcJec::DirectionCosineIeqcJec(EndFrmcptr frmi, EndFrmcptr frmj, size_t axisi, size_t axisj) : +DirectionCosineIeqcJec::DirectionCosineIeqcJec(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj) : DirectionCosineIecJec(frmi, frmj, axisi, axisj) { } @@ -28,15 +28,15 @@ void MbD::DirectionCosineIeqcJec::calcPostDynCorrectorIteration() { DirectionCosineIecJec::calcPostDynCorrectorIteration(); pAjOIepEIT = std::static_pointer_cast(frmI)->pAjOepET(axisI); - for (size_t i = 0; i < 4; i++) + for (int i = 0; i < 4; i++) { pAijIeJepEI->at(i) = pAjOIepEIT->at(i)->dot(aAjOJe); } - for (size_t i = 0; i < 4; i++) + for (int i = 0; i < 4; i++) { auto& ppAijIeJepEIipEI = ppAijIeJepEIpEI->at(i); auto& ppAjOIepEIipEI = ppAjOIepEIpEI->at(i); - for (size_t j = 0; j < 4; j++) + for (int j = 0; j < 4; j++) { ppAijIeJepEIipEI->at(j) = ppAjOIepEIipEI->at(j)->dot(aAjOJe); } diff --git a/MbDCode/DirectionCosineIeqcJec.h b/MbDCode/DirectionCosineIeqcJec.h index 1be5a20..3eb5e21 100644 --- a/MbDCode/DirectionCosineIeqcJec.h +++ b/MbDCode/DirectionCosineIeqcJec.h @@ -8,7 +8,7 @@ namespace MbD { //pAijIeJepEI ppAijIeJepEIpEI pAjOIepEIT ppAjOIepEIpEI public: DirectionCosineIeqcJec(); - DirectionCosineIeqcJec(EndFrmcptr frmi, EndFrmcptr frmj, size_t axisi, size_t axisj); + DirectionCosineIeqcJec(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj); void initialize(); void initializeGlobally(); void calcPostDynCorrectorIteration() override; diff --git a/MbDCode/DirectionCosineIeqcJeqc.cpp b/MbDCode/DirectionCosineIeqcJeqc.cpp index c2a74de..902b114 100644 --- a/MbDCode/DirectionCosineIeqcJeqc.cpp +++ b/MbDCode/DirectionCosineIeqcJeqc.cpp @@ -7,7 +7,7 @@ DirectionCosineIeqcJeqc::DirectionCosineIeqcJeqc() { } -DirectionCosineIeqcJeqc::DirectionCosineIeqcJeqc(EndFrmcptr frmi, EndFrmcptr frmj, size_t axisi, size_t axisj) : +DirectionCosineIeqcJeqc::DirectionCosineIeqcJeqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj) : DirectionCosineIeqcJec(frmi, frmj, axisi, axisj) { } @@ -30,26 +30,36 @@ void MbD::DirectionCosineIeqcJeqc::calcPostDynCorrectorIteration() { DirectionCosineIeqcJec::calcPostDynCorrectorIteration(); pAjOJepEJT = std::static_pointer_cast(frmJ)->pAjOepET(axisJ); - for (size_t i = 0; i < 4; i++) + for (int i = 0; i < 4; i++) { pAijIeJepEJ->at(i) = aAjOIe->dot(pAjOJepEJT->at(i)); } - for (size_t i = 0; i < 4; i++) + for (int i = 0; i < 4; i++) { auto& ppAijIeJepEIipEJ = ppAijIeJepEIpEJ->at(i); - for (size_t j = 0; j < 4; j++) + for (int j = 0; j < 4; j++) { ppAijIeJepEIipEJ->at(j) = pAjOIepEIT->at(i)->dot(pAjOJepEJT->at(j)); } } - for (size_t i = 0; i < 4; i++) + for (int i = 0; i < 4; i++) { auto& ppAijIeJepEJipEJ = ppAijIeJepEJpEJ->at(i); auto& ppAjOJepEJipEJ = ppAjOJepEJpEJ->at(i); - for (size_t j = 0; j < 4; j++) + for (int j = 0; j < 4; j++) { ppAijIeJepEJipEJ->at(j) = aAjOIe->dot(ppAjOJepEJipEJ->at(j)); } } ppAijIeJepEJpEJ->symLowerWithUpper(); } + +FRowDsptr MbD::DirectionCosineIeqcJeqc::pvaluepEJ() +{ + return pAijIeJepEJ; +} + +FMatDsptr MbD::DirectionCosineIeqcJeqc::ppvaluepEJpEJ() +{ + return ppAijIeJepEJpEJ; +} diff --git a/MbDCode/DirectionCosineIeqcJeqc.h b/MbDCode/DirectionCosineIeqcJeqc.h index 68a457f..a586dc4 100644 --- a/MbDCode/DirectionCosineIeqcJeqc.h +++ b/MbDCode/DirectionCosineIeqcJeqc.h @@ -8,10 +8,12 @@ namespace MbD { //pAijIeJepEJ ppAijIeJepEIpEJ ppAijIeJepEJpEJ pAjOJepEJT ppAjOJepEJpEJ public: DirectionCosineIeqcJeqc(); - DirectionCosineIeqcJeqc(EndFrmcptr frmi, EndFrmcptr frmj, size_t axisi, size_t axisj); + DirectionCosineIeqcJeqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj); void initialize(); void initializeGlobally(); void calcPostDynCorrectorIteration() override; + FRowDsptr pvaluepEJ() override; + FMatDsptr ppvaluepEJpEJ() override; FRowDsptr pAijIeJepEJ; FMatDsptr ppAijIeJepEIpEJ; diff --git a/MbDCode/DirectionCosineIeqctJeqc.cpp b/MbDCode/DirectionCosineIeqctJeqc.cpp index 4258024..b184775 100644 --- a/MbDCode/DirectionCosineIeqctJeqc.cpp +++ b/MbDCode/DirectionCosineIeqctJeqc.cpp @@ -7,7 +7,7 @@ DirectionCosineIeqctJeqc::DirectionCosineIeqctJeqc() { } -DirectionCosineIeqctJeqc::DirectionCosineIeqctJeqc(EndFrmcptr frmi, EndFrmcptr frmj, size_t axisi, size_t axisj) : +DirectionCosineIeqctJeqc::DirectionCosineIeqctJeqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj) : DirectionCosineIeqcJeqc(frmi, frmj, axisi, axisj) { } diff --git a/MbDCode/DirectionCosineIeqctJeqc.h b/MbDCode/DirectionCosineIeqctJeqc.h index fe3c740..5327c45 100644 --- a/MbDCode/DirectionCosineIeqctJeqc.h +++ b/MbDCode/DirectionCosineIeqctJeqc.h @@ -8,7 +8,7 @@ namespace MbD { //pAijIeJept ppAijIeJepEIpt ppAijIeJepEJpt ppAijIeJeptpt public: DirectionCosineIeqctJeqc(); - DirectionCosineIeqctJeqc(EndFrmcptr frmi, EndFrmcptr frmj, size_t axisi, size_t axisj); + DirectionCosineIeqctJeqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj); void initialize(); void initializeGlobally(); void calcPostDynCorrectorIteration() override; diff --git a/MbDCode/DiscontinuityError.cpp b/MbDCode/DiscontinuityError.cpp new file mode 100644 index 0000000..3b71e36 --- /dev/null +++ b/MbDCode/DiscontinuityError.cpp @@ -0,0 +1,7 @@ +#include "DiscontinuityError.h" + +using namespace MbD; + +MbD::DiscontinuityError::DiscontinuityError(const std::string& msg) : std::runtime_error(msg) +{ +} diff --git a/MbDCode/DiscontinuityError.h b/MbDCode/DiscontinuityError.h new file mode 100644 index 0000000..850f6f8 --- /dev/null +++ b/MbDCode/DiscontinuityError.h @@ -0,0 +1,15 @@ +#pragma once +#include +#include +#include + +namespace MbD { + class DiscontinuityError : virtual public std::runtime_error + { + + public: + //DiscontinuityError(); + explicit DiscontinuityError(const std::string& msg); + virtual ~DiscontinuityError() noexcept {} + }; +} diff --git a/MbDCode/DispCompIecJecKec.cpp b/MbDCode/DispCompIecJecKec.cpp index c841487..713419e 100644 --- a/MbDCode/DispCompIecJecKec.cpp +++ b/MbDCode/DispCompIecJecKec.cpp @@ -6,10 +6,40 @@ MbD::DispCompIecJecKec::DispCompIecJecKec() { } -MbD::DispCompIecJecKec::DispCompIecJecKec(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, size_t axisk): KinematicIeJe(frmi, frmj), efrmK(frmk), axisK(axisk) +MbD::DispCompIecJecKec::DispCompIecJecKec(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk): KinematicIeJe(frmi, frmj), efrmK(frmk), axisK(axisk) { } +FRowDsptr MbD::DispCompIecJecKec::pvaluepXJ() +{ + assert(false); + return FRowDsptr(); +} + +FRowDsptr MbD::DispCompIecJecKec::pvaluepEJ() +{ + assert(false); + return FRowDsptr(); +} + +FMatDsptr MbD::DispCompIecJecKec::ppvaluepXJpEK() +{ + assert(false); + return FMatDsptr(); +} + +FMatDsptr MbD::DispCompIecJecKec::ppvaluepEJpEK() +{ + assert(false); + return FMatDsptr(); +} + +FMatDsptr MbD::DispCompIecJecKec::ppvaluepEJpEJ() +{ + assert(false); + return FMatDsptr(); +} + double MbD::DispCompIecJecKec::value() { return riIeJeKe; diff --git a/MbDCode/DispCompIecJecKec.h b/MbDCode/DispCompIecJecKec.h index 1462da3..5e6826c 100644 --- a/MbDCode/DispCompIecJecKec.h +++ b/MbDCode/DispCompIecJecKec.h @@ -8,11 +8,16 @@ namespace MbD { //efrmK axisK riIeJeKe aAjOKe rIeJeO public: DispCompIecJecKec(); - DispCompIecJecKec(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, size_t axisk); - + DispCompIecJecKec(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk); + FRowDsptr pvaluepXJ() override; + FRowDsptr pvaluepEJ() override; + FMatDsptr ppvaluepXJpEK() override; + FMatDsptr ppvaluepEJpEK() override; + FMatDsptr ppvaluepEJpEJ() override; + virtual double value(); EndFrmcptr efrmK; - size_t axisK; + int axisK; double riIeJeKe; FColDsptr aAjOKe; FColDsptr rIeJeO; diff --git a/MbDCode/DispCompIecJecKeqc.cpp b/MbDCode/DispCompIecJecKeqc.cpp index 46f7511..788b069 100644 --- a/MbDCode/DispCompIecJecKeqc.cpp +++ b/MbDCode/DispCompIecJecKeqc.cpp @@ -7,7 +7,7 @@ MbD::DispCompIecJecKeqc::DispCompIecJecKeqc() { } -MbD::DispCompIecJecKeqc::DispCompIecJecKeqc(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, size_t axisk) : DispCompIecJecKec(frmi, frmj, frmk, axisk) +MbD::DispCompIecJecKeqc::DispCompIecJecKeqc(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk) : DispCompIecJecKec(frmi, frmj, frmk, axisk) { } @@ -32,13 +32,13 @@ void MbD::DispCompIecJecKeqc::calcPostDynCorrectorIteration() riIeJeKe = aAjOKe->dot(rIeJeO); pAjOKepEKT = efrmKqc->pAjOepET(axisK); ppAjOKepEKpEK = efrmKqc->ppAjOepEpE(axisK); - for (size_t i = 0; i < 4; i++) + for (int i = 0; i < 4; i++) { priIeJeKepEK->at(i) = ((pAjOKepEKT->at(i))->dot(rIeJeO)); auto& ppAjOKepEKipEK = ppAjOKepEKpEK->at(i); auto& ppriIeJeKepEKipEK = ppriIeJeKepEKpEK->at(i); ppriIeJeKepEKipEK->at(i) = ((ppAjOKepEKipEK->at(i))->dot(rIeJeO)); - for (size_t j = i + 1; j < 4; j++) + for (int j = i + 1; j < 4; j++) { auto ppriIeJeKepEKipEKj = (ppAjOKepEKipEK->at(i))->dot(rIeJeO); ppriIeJeKepEKipEK->at(j) = ppriIeJeKepEKipEKj; diff --git a/MbDCode/DispCompIecJecKeqc.h b/MbDCode/DispCompIecJecKeqc.h index 06861ff..c559993 100644 --- a/MbDCode/DispCompIecJecKeqc.h +++ b/MbDCode/DispCompIecJecKeqc.h @@ -8,7 +8,7 @@ namespace MbD { //priIeJeKepEK ppriIeJeKepEKpEK pAjOKepEKT ppAjOKepEKpEK public: DispCompIecJecKeqc(); - DispCompIecJecKeqc(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, size_t axisk); + DispCompIecJecKeqc(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk); void initialize() override; void initializeGlobally() override; void calcPostDynCorrectorIteration() override; diff --git a/MbDCode/DispCompIecJecO.cpp b/MbDCode/DispCompIecJecO.cpp index a7b2f92..acc8958 100644 --- a/MbDCode/DispCompIecJecO.cpp +++ b/MbDCode/DispCompIecJecO.cpp @@ -6,7 +6,7 @@ MbD::DispCompIecJecO::DispCompIecJecO() { } -MbD::DispCompIecJecO::DispCompIecJecO(EndFrmcptr frmi, EndFrmcptr frmj, size_t axis) : KinematicIeJe(frmi, frmj), axis(axis) +MbD::DispCompIecJecO::DispCompIecJecO(EndFrmcptr frmi, EndFrmcptr frmj, int axis) : KinematicIeJe(frmi, frmj), axis(axis) { } @@ -14,3 +14,33 @@ void MbD::DispCompIecJecO::calcPostDynCorrectorIteration() { riIeJeO = frmJ->riOeO(axis) - frmI->riOeO(axis); } + +FRowDsptr MbD::DispCompIecJecO::pvaluepXJ() +{ + assert(false); + return FRowDsptr(); +} + +FRowDsptr MbD::DispCompIecJecO::pvaluepEJ() +{ + assert(false); + return FRowDsptr(); +} + +FMatDsptr MbD::DispCompIecJecO::ppvaluepXJpEK() +{ + assert(false); + return FMatDsptr(); +} + +FMatDsptr MbD::DispCompIecJecO::ppvaluepEJpEK() +{ + assert(false); + return FMatDsptr(); +} + +FMatDsptr MbD::DispCompIecJecO::ppvaluepEJpEJ() +{ + assert(false); + return FMatDsptr(); +} diff --git a/MbDCode/DispCompIecJecO.h b/MbDCode/DispCompIecJecO.h index df0c279..91fdeac 100644 --- a/MbDCode/DispCompIecJecO.h +++ b/MbDCode/DispCompIecJecO.h @@ -8,10 +8,15 @@ namespace MbD { //axis riIeJeO public: DispCompIecJecO(); - DispCompIecJecO(EndFrmcptr frmi, EndFrmcptr frmj, size_t axis); + DispCompIecJecO(EndFrmcptr frmi, EndFrmcptr frmj, int axis); void calcPostDynCorrectorIteration() override; + FRowDsptr pvaluepXJ() override; + FRowDsptr pvaluepEJ() override; + FMatDsptr ppvaluepXJpEK() override; + FMatDsptr ppvaluepEJpEK() override; + FMatDsptr ppvaluepEJpEJ() override; - size_t axis = -1; + int axis = -1; double riIeJeO; }; } diff --git a/MbDCode/DispCompIeqcJecKeqc.cpp b/MbDCode/DispCompIeqcJecKeqc.cpp index 42f0d82..ef615b5 100644 --- a/MbDCode/DispCompIeqcJecKeqc.cpp +++ b/MbDCode/DispCompIeqcJecKeqc.cpp @@ -7,7 +7,7 @@ MbD::DispCompIeqcJecKeqc::DispCompIeqcJecKeqc() { } -MbD::DispCompIeqcJecKeqc::DispCompIeqcJecKeqc(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, size_t axisk) : DispCompIecJecKeqc(frmi, frmj, frmk, axisk) +MbD::DispCompIeqcJecKeqc::DispCompIeqcJecKeqc(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk) : DispCompIecJecKeqc(frmi, frmj, frmk, axisk) { } @@ -27,39 +27,39 @@ void MbD::DispCompIeqcJecKeqc::calcPostDynCorrectorIteration() auto frmIqc = std::static_pointer_cast(frmI); auto mprIeJeOpEIT = frmIqc->prOeOpE->transpose(); auto mpprIeJeOpEIpEI = frmIqc->pprOeOpEpE; - for (size_t i = 0; i < 3; i++) + for (int i = 0; i < 3; i++) { priIeJeKepXI->at(i) = 0.0 - (aAjOKe->at(i)); } - for (size_t i = 0; i < 4; i++) + for (int i = 0; i < 4; i++) { priIeJeKepEI->at(i) = 0.0 - (aAjOKe->dot(mprIeJeOpEIT->at(i))); } - for (size_t i = 0; i < 3; i++) + for (int i = 0; i < 3; i++) { auto& ppriIeJeKepXIipEK = ppriIeJeKepXIpEK->at(i); - for (size_t j = 0; j < 4; j++) + for (int j = 0; j < 4; j++) { ppriIeJeKepXIipEK->at(j) = 0.0 - (pAjOKepEKT->at(j)->at(i)); } } - for (size_t i = 0; i < 4; i++) + for (int i = 0; i < 4; i++) { auto& mpprIeJeOpEIipEI = mpprIeJeOpEIpEI->at(i); auto& ppriIeJeKepEIipEI = ppriIeJeKepEIpEI->at(i); ppriIeJeKepEIipEI->at(i) = 0.0 - (aAjOKe->dot(mpprIeJeOpEIipEI->at(i))); - for (size_t j = 0; j < 4; j++) + for (int j = 0; j < 4; j++) { auto ppriIeJeKepEIipEIj = 0.0 - (aAjOKe->dot(mpprIeJeOpEIipEI->at(j))); ppriIeJeKepEIipEI->at(j) = ppriIeJeKepEIipEIj; ppriIeJeKepEIpEI->at(j)->at(i) = ppriIeJeKepEIipEIj; } } - for (size_t i = 0; i < 4; i++) + for (int i = 0; i < 4; i++) { auto& mprIeJeOpEITi = mprIeJeOpEIT->at(i); auto& ppriIeJeKepEIipEK = ppriIeJeKepEIpEK->at(i); - for (size_t j = 0; j < 4; j++) + for (int j = 0; j < 4; j++) { ppriIeJeKepEIipEK->at(j) = 0.0 - (pAjOKepEKT->at(j)->dot(mprIeJeOpEITi)); } diff --git a/MbDCode/DispCompIeqcJecKeqc.h b/MbDCode/DispCompIeqcJecKeqc.h index 8105286..7e28a3a 100644 --- a/MbDCode/DispCompIeqcJecKeqc.h +++ b/MbDCode/DispCompIeqcJecKeqc.h @@ -8,7 +8,7 @@ namespace MbD { //priIeJeKepXI priIeJeKepEI ppriIeJeKepXIpEK ppriIeJeKepEIpEI ppriIeJeKepEIpEK public: DispCompIeqcJecKeqc(); - DispCompIeqcJecKeqc(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, size_t axisk); + DispCompIeqcJecKeqc(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk); void initialize() override; void calcPostDynCorrectorIteration() override; diff --git a/MbDCode/DispCompIeqcJecO.cpp b/MbDCode/DispCompIeqcJecO.cpp index 936b0cf..253cd95 100644 --- a/MbDCode/DispCompIeqcJecO.cpp +++ b/MbDCode/DispCompIeqcJecO.cpp @@ -7,7 +7,7 @@ MbD::DispCompIeqcJecO::DispCompIeqcJecO() { } -MbD::DispCompIeqcJecO::DispCompIeqcJecO(EndFrmcptr frmi, EndFrmcptr frmj, size_t axis) : DispCompIecJecO(frmi, frmj, axis) +MbD::DispCompIeqcJecO::DispCompIeqcJecO(EndFrmcptr frmi, EndFrmcptr frmj, int axis) : DispCompIecJecO(frmi, frmj, axis) { } diff --git a/MbDCode/DispCompIeqcJecO.h b/MbDCode/DispCompIeqcJecO.h index 9097ca3..862b51d 100644 --- a/MbDCode/DispCompIeqcJecO.h +++ b/MbDCode/DispCompIeqcJecO.h @@ -8,7 +8,7 @@ namespace MbD { //priIeJeOpXI priIeJeOpEI ppriIeJeOpEIpEI public: DispCompIeqcJecO(); - DispCompIeqcJecO(EndFrmcptr frmi, EndFrmcptr frmj, size_t axis); + DispCompIeqcJecO(EndFrmcptr frmi, EndFrmcptr frmj, int axis); void initializeGlobally() override; void calcPostDynCorrectorIteration() override; diff --git a/MbDCode/DispCompIeqcJeqcKeqc.cpp b/MbDCode/DispCompIeqcJeqcKeqc.cpp index c32aadd..a381b45 100644 --- a/MbDCode/DispCompIeqcJeqcKeqc.cpp +++ b/MbDCode/DispCompIeqcJeqcKeqc.cpp @@ -7,7 +7,7 @@ MbD::DispCompIeqcJeqcKeqc::DispCompIeqcJeqcKeqc() { } -MbD::DispCompIeqcJeqcKeqc::DispCompIeqcJeqcKeqc(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, size_t axisk) : DispCompIeqcJecKeqc(frmi, frmj, frmk, axisk) +MbD::DispCompIeqcJeqcKeqc::DispCompIeqcJeqcKeqc(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk) : DispCompIeqcJecKeqc(frmi, frmj, frmk, axisk) { } @@ -27,41 +27,66 @@ void MbD::DispCompIeqcJeqcKeqc::calcPostDynCorrectorIteration() auto frmJqc = std::static_pointer_cast(frmJ); auto prIeJeOpEJT = frmJqc->prOeOpE->transpose(); auto pprIeJeOpEJpEJ = frmJqc->pprOeOpEpE; - for (size_t i = 0; i < 3; i++) + for (int i = 0; i < 3; i++) { priIeJeKepXJ->at(i) = (aAjOKe->at(i)); } - for (size_t i = 0; i < 4; i++) + for (int i = 0; i < 4; i++) { priIeJeKepEJ->at(i) = (aAjOKe->dot(prIeJeOpEJT->at(i))); } - for (size_t i = 0; i < 3; i++) + for (int i = 0; i < 3; i++) { auto& ppriIeJeKepXJipEK = ppriIeJeKepXJpEK->at(i); - for (size_t j = 0; j < 4; j++) + for (int j = 0; j < 4; j++) { ppriIeJeKepXJipEK->at(j) = (pAjOKepEKT->at(j)->at(i)); } } - for (size_t i = 0; i < 4; i++) + for (int i = 0; i < 4; i++) { auto& pprIeJeOpEJipEJ = pprIeJeOpEJpEJ->at(i); auto& ppriIeJeKepEJipEJ = ppriIeJeKepEJpEJ->at(i); ppriIeJeKepEJipEJ->at(i) = (aAjOKe->dot(pprIeJeOpEJipEJ->at(i))); - for (size_t j = 0; j < 4; j++) + for (int j = 0; j < 4; j++) { auto ppriIeJeKepEJipEJj = (aAjOKe->dot(pprIeJeOpEJipEJ->at(j))); ppriIeJeKepEJipEJ->at(j) = ppriIeJeKepEJipEJj; ppriIeJeKepEJpEJ->at(j)->at(i) = ppriIeJeKepEJipEJj; } } - for (size_t i = 0; i < 4; i++) + for (int i = 0; i < 4; i++) { auto& prIeJeOpEJTi = prIeJeOpEJT->at(i); auto& ppriIeJeKepEJipEK = ppriIeJeKepEJpEK->at(i); - for (size_t j = 0; j < 4; j++) + for (int j = 0; j < 4; j++) { ppriIeJeKepEJipEK->at(j) = (pAjOKepEKT->at(j)->dot(prIeJeOpEJTi)); } } } + +FRowDsptr MbD::DispCompIeqcJeqcKeqc::pvaluepXJ() +{ + return priIeJeKepXJ; +} + +FRowDsptr MbD::DispCompIeqcJeqcKeqc::pvaluepEJ() +{ + return priIeJeKepEJ; +} + +FMatDsptr MbD::DispCompIeqcJeqcKeqc::ppvaluepXJpEK() +{ + return ppriIeJeKepXJpEK; +} + +FMatDsptr MbD::DispCompIeqcJeqcKeqc::ppvaluepEJpEK() +{ + return ppriIeJeKepEJpEK; +} + +FMatDsptr MbD::DispCompIeqcJeqcKeqc::ppvaluepEJpEJ() +{ + return ppriIeJeKepEJpEJ; +} diff --git a/MbDCode/DispCompIeqcJeqcKeqc.h b/MbDCode/DispCompIeqcJeqcKeqc.h index 8168e61..8bc80c5 100644 --- a/MbDCode/DispCompIeqcJeqcKeqc.h +++ b/MbDCode/DispCompIeqcJeqcKeqc.h @@ -8,9 +8,14 @@ namespace MbD { //priIeJeKepXJ priIeJeKepEJ ppriIeJeKepXJpEK ppriIeJeKepEJpEJ ppriIeJeKepEJpEK public: DispCompIeqcJeqcKeqc(); - DispCompIeqcJeqcKeqc(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, size_t axisk); + DispCompIeqcJeqcKeqc(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk); void initialize() override; void calcPostDynCorrectorIteration() override; + FRowDsptr pvaluepXJ() override; + FRowDsptr pvaluepEJ() override; + FMatDsptr ppvaluepXJpEK(); + FMatDsptr ppvaluepEJpEK(); + FMatDsptr ppvaluepEJpEJ() override; FRowDsptr priIeJeKepXJ; FRowDsptr priIeJeKepEJ; diff --git a/MbDCode/DispCompIeqcJeqcKeqct.cpp b/MbDCode/DispCompIeqcJeqcKeqct.cpp index 8c38858..ffa3bdd 100644 --- a/MbDCode/DispCompIeqcJeqcKeqct.cpp +++ b/MbDCode/DispCompIeqcJeqcKeqct.cpp @@ -7,7 +7,7 @@ MbD::DispCompIeqcJeqcKeqct::DispCompIeqcJeqcKeqct() { } -MbD::DispCompIeqcJeqcKeqct::DispCompIeqcJeqcKeqct(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, size_t axisk) : DispCompIeqcJeqcKeqc(frmi, frmj, frmk, axisk) +MbD::DispCompIeqcJeqcKeqct::DispCompIeqcJeqcKeqct(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk) : DispCompIeqcJeqcKeqc(frmi, frmj, frmk, axisk) { } diff --git a/MbDCode/DispCompIeqcJeqcKeqct.h b/MbDCode/DispCompIeqcJeqcKeqct.h index db76557..0ebb021 100644 --- a/MbDCode/DispCompIeqcJeqcKeqct.h +++ b/MbDCode/DispCompIeqcJeqcKeqct.h @@ -8,7 +8,7 @@ namespace MbD { //priIeJeKept ppriIeJeKepXIpt ppriIeJeKepEIpt ppriIeJeKepXJpt ppriIeJeKepEJpt ppriIeJeKepEKpt ppriIeJeKeptpt public: DispCompIeqcJeqcKeqct(); - DispCompIeqcJeqcKeqct(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, size_t axisk); + DispCompIeqcJeqcKeqct(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk); void initialize() override; void calcPostDynCorrectorIteration() override; diff --git a/MbDCode/DispCompIeqcJeqcO.cpp b/MbDCode/DispCompIeqcJeqcO.cpp index 8393ec1..e1e3bf2 100644 --- a/MbDCode/DispCompIeqcJeqcO.cpp +++ b/MbDCode/DispCompIeqcJeqcO.cpp @@ -7,7 +7,7 @@ MbD::DispCompIeqcJeqcO::DispCompIeqcJeqcO() { } -MbD::DispCompIeqcJeqcO::DispCompIeqcJeqcO(EndFrmcptr frmi, EndFrmcptr frmj, size_t axis) : DispCompIeqcJecO(frmi, frmj, axis) +MbD::DispCompIeqcJeqcO::DispCompIeqcJeqcO(EndFrmcptr frmi, EndFrmcptr frmj, int axis) : DispCompIeqcJecO(frmi, frmj, axis) { } @@ -24,3 +24,18 @@ void MbD::DispCompIeqcJeqcO::calcPostDynCorrectorIteration() DispCompIeqcJecO::calcPostDynCorrectorIteration(); priIeJeOpEJ = std::static_pointer_cast(frmJ)->priOeOpE(axis); } + +FRowDsptr MbD::DispCompIeqcJeqcO::pvaluepXJ() +{ + return priIeJeOpXJ; +} + +FRowDsptr MbD::DispCompIeqcJeqcO::pvaluepEJ() +{ + return priIeJeOpEJ; +} + +FMatDsptr MbD::DispCompIeqcJeqcO::ppvaluepEJpEJ() +{ + return ppriIeJeOpEJpEJ; +} diff --git a/MbDCode/DispCompIeqcJeqcO.h b/MbDCode/DispCompIeqcJeqcO.h index 12a8b40..fa75b1f 100644 --- a/MbDCode/DispCompIeqcJeqcO.h +++ b/MbDCode/DispCompIeqcJeqcO.h @@ -8,9 +8,12 @@ namespace MbD { //priIeJeOpXJ priIeJeOpEJ ppriIeJeOpEJpEJ public: DispCompIeqcJeqcO(); - DispCompIeqcJeqcO(EndFrmcptr frmi, EndFrmcptr frmj, size_t axis); + DispCompIeqcJeqcO(EndFrmcptr frmi, EndFrmcptr frmj, int axis); void initializeGlobally() override; void calcPostDynCorrectorIteration() override; + FRowDsptr pvaluepXJ() override; + FRowDsptr pvaluepEJ() override; + FMatDsptr ppvaluepEJpEJ() override; FRowDsptr priIeJeOpXJ; FRowDsptr priIeJeOpEJ; diff --git a/MbDCode/DispCompIeqctJeqcKeqct.cpp b/MbDCode/DispCompIeqctJeqcKeqct.cpp index 5d83764..3958277 100644 --- a/MbDCode/DispCompIeqctJeqcKeqct.cpp +++ b/MbDCode/DispCompIeqctJeqcKeqct.cpp @@ -6,6 +6,6 @@ MbD::DispCompIeqctJeqcKeqct::DispCompIeqctJeqcKeqct() { } -MbD::DispCompIeqctJeqcKeqct::DispCompIeqctJeqcKeqct(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, size_t axisk) : DispCompIeqcJeqcKeqct(frmi, frmj, frmk, axisk) +MbD::DispCompIeqctJeqcKeqct::DispCompIeqctJeqcKeqct(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk) : DispCompIeqcJeqcKeqct(frmi, frmj, frmk, axisk) { } diff --git a/MbDCode/DispCompIeqctJeqcKeqct.h b/MbDCode/DispCompIeqctJeqcKeqct.h index f3ae91b..55a9926 100644 --- a/MbDCode/DispCompIeqctJeqcKeqct.h +++ b/MbDCode/DispCompIeqctJeqcKeqct.h @@ -8,7 +8,7 @@ namespace MbD { // public: DispCompIeqctJeqcKeqct(); - DispCompIeqctJeqcKeqct(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, size_t axisk); + DispCompIeqctJeqcKeqct(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk); }; } diff --git a/MbDCode/DispCompIeqctJeqcO.cpp b/MbDCode/DispCompIeqctJeqcO.cpp index 73e0f6f..011b70f 100644 --- a/MbDCode/DispCompIeqctJeqcO.cpp +++ b/MbDCode/DispCompIeqctJeqcO.cpp @@ -7,7 +7,7 @@ MbD::DispCompIeqctJeqcO::DispCompIeqctJeqcO() { } -MbD::DispCompIeqctJeqcO::DispCompIeqctJeqcO(EndFrmcptr frmi, EndFrmcptr frmj, size_t axis) : DispCompIeqcJeqcO(frmi, frmj, axis) +MbD::DispCompIeqctJeqcO::DispCompIeqctJeqcO(EndFrmcptr frmi, EndFrmcptr frmj, int axis) : DispCompIeqcJeqcO(frmi, frmj, axis) { } diff --git a/MbDCode/DispCompIeqctJeqcO.h b/MbDCode/DispCompIeqctJeqcO.h index ceecc15..eb45cb4 100644 --- a/MbDCode/DispCompIeqctJeqcO.h +++ b/MbDCode/DispCompIeqctJeqcO.h @@ -8,7 +8,7 @@ namespace MbD { //priIeJeOpt ppriIeJeOpEIpt ppriIeJeOptpt public: DispCompIeqctJeqcO(); - DispCompIeqctJeqcO(EndFrmcptr frmi, EndFrmcptr frmj, size_t axis); + DispCompIeqctJeqcO(EndFrmcptr frmi, EndFrmcptr frmj, int axis); void initializeGlobally() override; void calcPostDynCorrectorIteration() override; diff --git a/MbDCode/EndFramec.cpp b/MbDCode/EndFramec.cpp index c5a4362..5f2d8fd 100644 --- a/MbDCode/EndFramec.cpp +++ b/MbDCode/EndFramec.cpp @@ -30,10 +30,6 @@ void EndFramec::initializeLocally() { } -void EndFramec::initializeGlobally() -{ -} - void EndFramec::initEndFrameqct() { assert(false); @@ -45,12 +41,12 @@ void MbD::EndFramec::calcPostDynCorrectorIteration() aAOe = markerFrame->aAOm; } -FColDsptr MbD::EndFramec::aAjOe(size_t j) +FColDsptr MbD::EndFramec::aAjOe(int j) { return aAOe->column(j); } -double MbD::EndFramec::riOeO(size_t i) +double MbD::EndFramec::riOeO(int i) { return rOeO->at(i); } diff --git a/MbDCode/EndFramec.h b/MbDCode/EndFramec.h index 021a6ea..b0c2729 100644 --- a/MbDCode/EndFramec.h +++ b/MbDCode/EndFramec.h @@ -23,11 +23,10 @@ namespace MbD { void setMarkerFrame(MarkerFrame* markerFrm); MarkerFrame* getMarkerFrame(); void initializeLocally() override; - void initializeGlobally() override; virtual void initEndFrameqct(); void calcPostDynCorrectorIteration() override; - FColDsptr aAjOe(size_t j); - double riOeO(size_t i); + FColDsptr aAjOe(int j); + double riOeO(int i); MarkerFrame* markerFrame; //Use raw pointer when pointing backwards. FColDsptr rOeO = std::make_shared>(3); diff --git a/MbDCode/EndFrameqc.cpp b/MbDCode/EndFrameqc.cpp index 9a9642b..be7fd56 100644 --- a/MbDCode/EndFrameqc.cpp +++ b/MbDCode/EndFrameqc.cpp @@ -38,13 +38,13 @@ void EndFrameqc::initEndFrameqct() endFrameqct->setMarkerFrame(markerFrame); } -FMatFColDsptr MbD::EndFrameqc::ppAjOepEpE(size_t jj) +FMatFColDsptr MbD::EndFrameqc::ppAjOepEpE(int jj) { auto answer = std::make_shared>>>(4, 4); - for (size_t i = 0; i < 4; i++) { + for (int i = 0; i < 4; i++) { auto& answeri = answer->at(i); auto& ppAOepEipE = ppAOepEpE->at(i); - for (size_t j = i; j < 4; j++) { + for (int j = i; j < 4; j++) { answeri->at(j) = ppAOepEipE->at(j)->column(jj); } } @@ -59,13 +59,13 @@ void MbD::EndFrameqc::calcPostDynCorrectorIteration() pAOepE = markerFrame->pAOmpE; } -FMatDsptr MbD::EndFrameqc::pAjOepET(size_t axis) +FMatDsptr MbD::EndFrameqc::pAjOepET(int axis) { auto answer = std::make_shared>(4, 3); - for (size_t i = 0; i < 4; i++) { + for (int i = 0; i < 4; i++) { auto& answeri = answer->at(i); auto& pAOepEi = pAOepE->at(i); - for (size_t j = 0; j < 3; j++) { + for (int j = 0; j < 3; j++) { auto& answerij = pAOepEi->at(j)->at(axis); answeri->at(j) = answerij; } @@ -73,13 +73,13 @@ FMatDsptr MbD::EndFrameqc::pAjOepET(size_t axis) return answer; } -FMatDsptr MbD::EndFrameqc::ppriOeOpEpE(size_t ii) +FMatDsptr MbD::EndFrameqc::ppriOeOpEpE(int ii) { auto answer = std::make_shared>(4, 4); - for (size_t i = 0; i < 4; i++) { + for (int i = 0; i < 4; i++) { auto& answeri = answer->at(i); auto& pprOeOpEipE = pprOeOpEpE->at(i); - for (size_t j = 0; j < 4; j++) { + for (int j = 0; j < 4; j++) { auto& answerij = pprOeOpEipE->at(j)->at(ii); answeri->at(j) = answerij; } @@ -87,17 +87,17 @@ FMatDsptr MbD::EndFrameqc::ppriOeOpEpE(size_t ii) return answer; } -size_t MbD::EndFrameqc::iqX() +int MbD::EndFrameqc::iqX() { return markerFrame->iqX(); } -size_t MbD::EndFrameqc::iqE() +int MbD::EndFrameqc::iqE() { return markerFrame->iqE(); } -FRowDsptr MbD::EndFrameqc::priOeOpE(size_t i) +FRowDsptr MbD::EndFrameqc::priOeOpE(int i) { return prOeOpE->at(i); } diff --git a/MbDCode/EndFrameqc.h b/MbDCode/EndFrameqc.h index e5ac3e2..042604f 100644 --- a/MbDCode/EndFrameqc.h +++ b/MbDCode/EndFrameqc.h @@ -15,13 +15,13 @@ namespace MbD { void initialize(); void initializeGlobally() override; void initEndFrameqct() override; - FMatFColDsptr ppAjOepEpE(size_t j); + FMatFColDsptr ppAjOepEpE(int j); void calcPostDynCorrectorIteration() override; - FMatDsptr pAjOepET(size_t j); - FMatDsptr ppriOeOpEpE(size_t i); - size_t iqX(); - size_t iqE(); - FRowDsptr priOeOpE(size_t i); + FMatDsptr pAjOepET(int j); + FMatDsptr ppriOeOpEpE(int i); + int iqX(); + int iqE(); + FRowDsptr priOeOpE(int i); FMatDsptr prOeOpE; std::shared_ptr>>> pprOeOpEpE; diff --git a/MbDCode/EndFrameqct.cpp b/MbDCode/EndFrameqct.cpp index 5ae3a7b..39ee534 100644 --- a/MbDCode/EndFrameqct.cpp +++ b/MbDCode/EndFrameqct.cpp @@ -60,7 +60,7 @@ void EndFrameqct::initprmemptBlks() { auto& mbdTime = System::getInstance().time; prmemptBlks = std::make_shared< FullColumn>(3); - for (size_t i = 0; i < 3; i++) { + for (int i = 0; i < 3; i++) { auto& disp = rmemBlks->at(i); auto var = disp->differentiateWRT(disp, mbdTime); auto vel = var->simplified(var); @@ -76,12 +76,12 @@ void EndFrameqct::initpPhiThePsiptBlks() { auto& mbdTime = System::getInstance().time; pPhiThePsiptBlks = std::make_shared< FullColumn>(3); - for (size_t i = 0; i < 3; i++) { + for (int i = 0; i < 3; i++) { auto& angle = phiThePsiBlks->at(i); auto var = angle->differentiateWRT(angle, mbdTime); - std::cout << "var " << *var << std::endl; + //std::cout << "var " << *var << std::endl; auto vel = var->simplified(var); - std::cout << "vel " << *vel << std::endl; + //std::cout << "vel " << *vel << std::endl; pPhiThePsiptBlks->at(i) = vel; } } @@ -104,11 +104,11 @@ void MbD::EndFrameqct::calcPostDynCorrectorIteration() rOeO = rOmO->plusFullColumn(aAOm->timesFullColumn(rmem)); auto& prOmOpE = markerFrame->prOmOpE; auto& pAOmpE = markerFrame->pAOmpE; - for (size_t i = 0; i < 3; i++) + for (int i = 0; i < 3; i++) { auto& prOmOpEi = prOmOpE->at(i); auto& prOeOpEi = prOeOpE->at(i); - for (size_t j = 0; j < 4; j++) + for (int j = 0; j < 4; j++) { auto prOeOpEij = prOmOpEi->at(j) + pAOmpE->at(j)->at(i)->timesFullColumn(rmem); prOeOpEi->at(j) = prOeOpEij; @@ -117,7 +117,7 @@ void MbD::EndFrameqct::calcPostDynCorrectorIteration() auto rpep = markerFrame->rpmp->plusFullColumn(markerFrame->aApm->timesFullColumn(rmem)); pprOeOpEpE = EulerParameters::ppApEpEtimesColumn(rpep); aAOe = aAOm->timesFullMatrix(aAme); - for (size_t i = 0; i < 4; i++) + for (int i = 0; i < 4; i++) { pAOepE->at(i) = pAOmpE->at(i)->timesFullMatrix(aAme); } @@ -136,7 +136,7 @@ void MbD::EndFrameqct::prePosIC() void MbD::EndFrameqct::evalrmem() { if (rmemBlks) { - for (size_t i = 0; i < 3; i++) + for (int i = 0; i < 3; i++) { auto& expression = rmemBlks->at(i); double value = expression->getValue(); @@ -149,7 +149,7 @@ void MbD::EndFrameqct::evalAme() { if (phiThePsiBlks) { auto phiThePsi = CREATE>::With(); - for (size_t i = 0; i < 3; i++) + for (int i = 0; i < 3; i++) { auto& expression = phiThePsiBlks->at(i); auto value = expression->getValue(); diff --git a/MbDCode/EulerArray.h b/MbDCode/EulerArray.h index dd2e25b..a3dd724 100644 --- a/MbDCode/EulerArray.h +++ b/MbDCode/EulerArray.h @@ -8,8 +8,8 @@ namespace MbD { { // public: - EulerArray(size_t count) : FullColumn(count) {} - EulerArray(size_t count, const T& value) : FullColumn(count, value) {} + EulerArray(int count) : FullColumn(count) {} + EulerArray(int count, const T& value) : FullColumn(count, value) {} EulerArray(std::initializer_list list) : FullColumn{ list } {} virtual void initialize(); }; diff --git a/MbDCode/EulerConstraint.cpp b/MbDCode/EulerConstraint.cpp index 96c9e19..37f4055 100644 --- a/MbDCode/EulerConstraint.cpp +++ b/MbDCode/EulerConstraint.cpp @@ -15,6 +15,7 @@ EulerConstraint::EulerConstraint(const char* str) : Constraint(str) void EulerConstraint::initialize() { + Constraint::initialize(); pGpE = std::make_shared>(4); } @@ -22,7 +23,7 @@ void MbD::EulerConstraint::calcPostDynCorrectorIteration() { auto& qE = static_cast(owner)->qE; aG = qE->sumOfSquares() - 1.0; - for (size_t i = 0; i < 4; i++) + for (int i = 0; i < 4; i++) { pGpE->at(i) = 2.0 * qE->at(i); } @@ -32,3 +33,22 @@ void MbD::EulerConstraint::useEquationNumbers() { iqE = static_cast(owner)->iqE; } + +void MbD::EulerConstraint::fillPosICError(FColDsptr col) +{ + Constraint::fillPosICError(col); + col->atiplusFullVectortimes(iqE, pGpE, lam); +} + +void MbD::EulerConstraint::fillPosICJacob(SpMatDsptr mat) +{ + //"ppGpEpE is a diag(2,2,2,2)." + mat->atijplusFullRow(iG, iqE, pGpE); + mat->atijplusFullColumn(iqE, iG, pGpE->transpose()); + auto twolam = 2.0 * lam; + for (int i = 0; i < 4; i++) + { + auto ii = iqE + i; + (*(mat->at(ii)))[ii] += twolam; + } +} diff --git a/MbDCode/EulerConstraint.h b/MbDCode/EulerConstraint.h index cfd8e41..5ae7c65 100644 --- a/MbDCode/EulerConstraint.h +++ b/MbDCode/EulerConstraint.h @@ -15,9 +15,11 @@ namespace MbD { void initialize(); void calcPostDynCorrectorIteration() override; void useEquationNumbers() override; + void fillPosICError(FColDsptr col) override; + void fillPosICJacob(SpMatDsptr mat) override; FRowDsptr pGpE; //partial derivative of G wrt pE - size_t iqE = -1; + int iqE = -1; }; } diff --git a/MbDCode/EulerParameters.h b/MbDCode/EulerParameters.h index 0dcc120..cdda72f 100644 --- a/MbDCode/EulerParameters.h +++ b/MbDCode/EulerParameters.h @@ -11,8 +11,8 @@ namespace MbD { //aA aB aC pApE public: EulerParameters() : EulerArray(4) {} - EulerParameters(size_t count) : EulerArray(count) {} - EulerParameters(size_t count, const T& value) : EulerArray(count, value) {} + EulerParameters(int count) : EulerArray(count) {} + EulerParameters(int count, const T& value) : EulerArray(count, value) {} EulerParameters(std::initializer_list list) : EulerArray{ list } {} static std::shared_ptr>>> ppApEpEtimesColumn(FColDsptr col); @@ -122,7 +122,7 @@ namespace MbD { aB = std::make_shared>(3, 4); aC = std::make_shared>(3, 4); pApE = std::make_shared>>>(4); - for (size_t i = 0; i < 4; i++) + for (int i = 0; i < 4; i++) { pApE->at(i) = std::make_shared>(3, 3); } diff --git a/MbDCode/EulerParametersDot.h b/MbDCode/EulerParametersDot.h index 74b2928..f345ef9 100644 --- a/MbDCode/EulerParametersDot.h +++ b/MbDCode/EulerParametersDot.h @@ -12,8 +12,8 @@ namespace MbD { { //qE aAdot aBdot aCdot pAdotpE public: - EulerParametersDot(size_t count) : EulerArray(count) {} - EulerParametersDot(size_t count, const T& value) : EulerArray(count, value) {} + 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; diff --git a/MbDCode/FullColumn.h b/MbDCode/FullColumn.h index 54cafe0..daec667 100644 --- a/MbDCode/FullColumn.h +++ b/MbDCode/FullColumn.h @@ -10,39 +10,34 @@ namespace MbD { class FullColumn : public FullVector { public: - FullColumn(size_t count) : FullVector(count) {} - FullColumn(size_t count, const T& value) : FullVector(count, value) {} + FullColumn(std::vector vec) : FullVector(vec) {} + FullColumn(int count) : FullVector(count) {} + FullColumn(int count, const T& value) : FullVector(count, value) {} + FullColumn(std::vector::iterator begin, std::vector::iterator end) : FullVector(begin, end) {} FullColumn(std::initializer_list list) : FullVector{ list } {} std::shared_ptr> plusFullColumn(std::shared_ptr> fullCol); std::shared_ptr> minusFullColumn(std::shared_ptr> fullCol); std::shared_ptr> times(double a); std::shared_ptr> negated(); - void atiputFullColumn(size_t i, std::shared_ptr> fullCol); - void equalSelfPlusFullColumnAt(std::shared_ptr> fullCol, size_t i); - void atiminusFullColumn(size_t i, std::shared_ptr> fullCol); - void equalFullColumnAt(std::shared_ptr> fullCol, size_t i); + void atiputFullColumn(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::string toString() + virtual std::ostream& printOn(std::ostream& s) const; + friend std::ostream& operator<<(std::ostream& s, const FullColumn& fullCol) { - std::stringstream ss; - - ss << "FullColumn { "; - for (size_t i = 0; i < this->size() - 1; i++) { - ss << this->at(i) << ", "; - } - ss << this->back() << " }"; - return ss.str(); + return fullCol.printOn(s); } }; using FColDsptr = std::shared_ptr>; template inline std::shared_ptr> FullColumn::plusFullColumn(std::shared_ptr> fullCol) { - size_t n = this->size(); + int n = (int) this->size(); auto answer = std::make_shared>(n); - for (size_t i = 0; i < n; i++) { + for (int i = 0; i < n; i++) { answer->at(i) = this->at(i) + fullCol->at(i); } return answer; @@ -50,9 +45,9 @@ namespace MbD { template inline std::shared_ptr> FullColumn::minusFullColumn(std::shared_ptr> fullCol) { - size_t n = this->size(); + int n = (int) this->size(); auto answer = std::make_shared>(n); - for (size_t i = 0; i < n; i++) { + for (int i = 0; i < n; i++) { answer->at(i) = this->at(i) - fullCol->at(i); } return answer; @@ -60,9 +55,9 @@ namespace MbD { template inline std::shared_ptr> FullColumn::times(double a) { - size_t n = this->size(); + int n = (int) this->size(); auto answer = std::make_shared(n); - for (size_t i = 0; i < n; i++) { + for (int i = 0; i < n; i++) { answer->at(i) = this->at(i) * a; } return answer; @@ -73,35 +68,35 @@ namespace MbD { return this->times(-1.0); } template - inline void FullColumn::atiputFullColumn(size_t i, std::shared_ptr> fullCol) + inline void FullColumn::atiputFullColumn(int i, std::shared_ptr> fullCol) { - for (size_t ii = 0; ii < fullCol->size(); ii++) + for (int ii = 0; ii < fullCol->size(); ii++) { this->at(i + ii) = fullCol->at(ii); } } template - inline void FullColumn::equalSelfPlusFullColumnAt(std::shared_ptr> fullCol, size_t ii) + inline void FullColumn::equalSelfPlusFullColumnAt(std::shared_ptr> fullCol, int ii) { //self is subcolumn of fullCol - for (size_t i = 0; i < this->size(); i++) + for (int i = 0; i < this->size(); i++) { this->at(i) += fullCol->at(ii + i); } } template - inline void FullColumn::atiminusFullColumn(size_t i1, std::shared_ptr> fullCol) + inline void FullColumn::atiminusFullColumn(int i1, std::shared_ptr> fullCol) { - for (size_t ii = 0; ii < fullCol->size(); ii++) + for (int ii = 0; ii < fullCol->size(); ii++) { - size_t i = i1 + ii; + int i = i1 + ii; this->at(i) -= fullCol->at(ii); } } template - inline void FullColumn::equalFullColumnAt(std::shared_ptr> fullCol, size_t ii) + inline void FullColumn::equalFullColumnAt(std::shared_ptr> fullCol, int ii) { - for (size_t i = 0; i < this->size(); i++) + for (int i = 0; i < this->size(); i++) { this->at(i) = fullCol->at(ii + i); } @@ -109,13 +104,25 @@ namespace MbD { template<> inline std::shared_ptr> FullColumn::copy() { - auto n = this->size(); + auto n = (int) this->size(); auto answer = std::make_shared>(n); - for (size_t i = 0; i < n; i++) + for (int i = 0; i < n; i++) { answer->at(i) = this->at(i); } return answer; } + template + inline std::ostream& FullColumn::printOn(std::ostream& s) const + { + s << "{"; + s << this->at(0); + for (int i = 1; i < this->size(); i++) + { + s << ", " << this->at(i); + } + s << "}"; + return s; + } } diff --git a/MbDCode/FullMatrix.h b/MbDCode/FullMatrix.h index 512b510..4d8e61c 100644 --- a/MbDCode/FullMatrix.h +++ b/MbDCode/FullMatrix.h @@ -14,11 +14,11 @@ namespace MbD { { public: FullMatrix() {} - FullMatrix(size_t m) : RowTypeMatrix>>(m) + FullMatrix(int m) : RowTypeMatrix>>(m) { } - FullMatrix(size_t m, size_t n) { - for (size_t i = 0; i < m; i++) { + FullMatrix(int m, int n) { + for (int i = 0; i < m; i++) { auto row = std::make_shared>(n); this->push_back(row); } @@ -37,7 +37,7 @@ namespace MbD { } } void identity(); - std::shared_ptr> column(size_t j); + std::shared_ptr> column(int j); std::shared_ptr> timesFullColumn(std::shared_ptr> fullCol); std::shared_ptr> timesFullMatrix(std::shared_ptr> fullMat); std::shared_ptr> timesTransposeFullMatrix(std::shared_ptr> fullMat); @@ -46,23 +46,22 @@ namespace MbD { std::shared_ptr> transpose(); std::shared_ptr> negated(); void symLowerWithUpper(); - void atijputFullColumn(size_t i, size_t j, std::shared_ptr> fullCol); + void atijputFullColumn(int i, int j, std::shared_ptr> fullCol); double sumOfSquares() override; - void atijplusNumber(size_t i, size_t j, double value); void zeroSelf() override; }; template <> inline void FullMatrix::identity() { this->zeroSelf(); - for (size_t i = 0; i < this->size(); i++) { + for (int i = 0; i < this->size(); i++) { this->at(i)->at(i) = 1.0; } } template - inline std::shared_ptr> FullMatrix::column(size_t j) { - size_t n = this->size(); + inline std::shared_ptr> FullMatrix::column(int j) { + int n = (int) this->size(); auto answer = std::make_shared>(n); - for (size_t i = 0; i < n; i++) { + for (int i = 0; i < n; i++) { answer->at(i) = this->at(i)->at(j); } return answer; @@ -71,9 +70,9 @@ namespace MbD { inline std::shared_ptr> FullMatrix::timesFullColumn(std::shared_ptr> fullCol) { //"a*b = a(i,j)b(j) sum j." - size_t n = this->size(); + int n = (int) this->size(); auto answer = std::make_shared>(n); - for (size_t i = 0; i < n; i++) + for (int i = 0; i < n; i++) { answer->at(i) = this->at(i)->timesFullColumn(fullCol); } @@ -82,9 +81,9 @@ namespace MbD { template inline std::shared_ptr> FullMatrix::timesFullMatrix(std::shared_ptr> fullMat) { - size_t m = this->nrow(); + int m = this->nrow(); auto answer = std::make_shared>(m); - for (size_t i = 0; i < m; i++) { + for (int i = 0; i < m; i++) { answer->at(i) = this->at(i)->timesFullMatrix(fullMat); } return answer; @@ -92,9 +91,9 @@ namespace MbD { template inline std::shared_ptr> FullMatrix::timesTransposeFullMatrix(std::shared_ptr> fullMat) { - size_t nrow = this->nrow(); + int nrow = this->nrow(); auto answer = std::make_shared>(nrow); - for (size_t i = 0; i < nrow; i++) { + for (int i = 0; i < nrow; i++) { answer->at(i) = this->at(i)->timesTransposeFullMatrix(fullMat); } return answer; @@ -102,9 +101,9 @@ namespace MbD { template inline std::shared_ptr> FullMatrix::times(double a) { - size_t m = this->nrow(); + int m = this->nrow(); auto answer = std::make_shared>(m); - for (size_t i = 0; i < m; i++) { + for (int i = 0; i < m; i++) { answer->at(i) = this->at(i)->times(a); } return answer; @@ -112,9 +111,9 @@ namespace MbD { template inline std::shared_ptr> FullMatrix::plusFullMatrix(std::shared_ptr> fullMat) { - size_t n = this->size(); + int n = (int) this->size(); auto answer = std::make_shared>(n); - for (size_t i = 0; i < n; i++) { + for (int i = 0; i < n; i++) { answer->at(i) = this->at(i)->plusFullRow(fullMat->at(i)); } return answer; @@ -122,12 +121,12 @@ namespace MbD { template inline std::shared_ptr> FullMatrix::transpose() { - size_t nrow = this->nrow(); + int nrow = this->nrow(); auto ncol = this->ncol(); auto answer = std::make_shared>(ncol, nrow); - for (size_t i = 0; i < nrow; i++) { + for (int i = 0; i < nrow; i++) { auto& row = this->at(i); - for (size_t j = 0; j < ncol; j++) { + for (int j = 0; j < ncol; j++) { answer->at(j)->at(i) = row->at(j); } } @@ -141,18 +140,18 @@ namespace MbD { template inline void FullMatrix::symLowerWithUpper() { - size_t n = this->size(); - for (size_t i = 0; i < n; i++) { - for (size_t j = i + 1; j < n; j++) { + int n = (int) this->size(); + for (int i = 0; i < n; i++) { + for (int j = i + 1; j < n; j++) { this->at(j)->at(i) = this->at(i)->at(j); } } } template - inline void FullMatrix::atijputFullColumn(size_t i1, size_t j1, std::shared_ptr> fullCol) + inline void FullMatrix::atijputFullColumn(int i1, int j1, std::shared_ptr> fullCol) { auto iOffset = i1 - 1; - for (size_t ii = 0; ii < fullCol->size(); ii++) + for (int ii = 0; ii < fullCol->size(); ii++) { this->at(iOffset + ii)->at(j1) = fullCol->at(ii); } @@ -161,7 +160,7 @@ namespace MbD { inline double FullMatrix::sumOfSquares() { double sum = 0.0; - for (size_t i = 0; i < this->size(); i++) + for (int i = 0; i < this->size(); i++) { sum += this->at(i)->sumOfSquares(); } @@ -174,19 +173,9 @@ namespace MbD { return 0.0; } template<> - inline void FullMatrix::atijplusNumber(size_t i, size_t j, double value) - { - this->at(i)->atiplusNumber(j, value); - } - template - inline void FullMatrix::atijplusNumber(size_t i, size_t j, double value) - { - assert(false); - } - template<> inline void FullMatrix::zeroSelf() { - for (size_t i = 0; i < this->size(); i++) { + for (int i = 0; i < this->size(); i++) { this->at(i)->zeroSelf(); } } diff --git a/MbDCode/FullRow.h b/MbDCode/FullRow.h index 9117f8e..451c40f 100644 --- a/MbDCode/FullRow.h +++ b/MbDCode/FullRow.h @@ -11,8 +11,8 @@ namespace MbD { { public: FullRow() {} - FullRow(size_t count) : FullVector(count) {} - FullRow(size_t count, const T& value) : FullVector(count, value) {} + FullRow(int count) : FullVector(count) {} + FullRow(int count, const T& value) : FullVector(count, value) {} FullRow(std::initializer_list list) : FullVector{ list } {} std::shared_ptr> times(double a); std::shared_ptr> negated(); @@ -22,14 +22,15 @@ namespace MbD { std::shared_ptr> timesFullMatrix(std::shared_ptr> fullMat); std::shared_ptr> timesTransposeFullMatrix(std::shared_ptr> fullMat); void equalSelfPlusFullRowTimes(std::shared_ptr> fullRow, double factor); + std::shared_ptr> transpose(); }; template inline std::shared_ptr> FullRow::times(double a) { - size_t n = this->size(); + int n = (int) this->size(); auto answer = std::make_shared(n); - for (size_t i = 0; i < n; i++) { + for (int i = 0; i < n; i++) { answer->at(i) = this->at(i) * a; } return answer; @@ -42,9 +43,9 @@ namespace MbD { template inline std::shared_ptr> FullRow::plusFullRow(std::shared_ptr> fullRow) { - size_t n = this->size(); + int n = (int) this->size(); auto answer = std::make_shared>(n); - for (size_t i = 0; i < n; i++) { + for (int i = 0; i < n; i++) { answer->at(i) = this->at(i) + fullRow->at(i); } return answer; @@ -52,9 +53,9 @@ namespace MbD { template inline std::shared_ptr> FullRow::minusFullRow(std::shared_ptr> fullRow) { - size_t n = this->size(); + int n = (int) this->size(); auto answer = std::make_shared>(n); - for (size_t i = 0; i < n; i++) { + for (int i = 0; i < n; i++) { answer->at(i) = this->at(i) - fullRow->at(i); } return answer; @@ -63,7 +64,7 @@ namespace MbD { inline T FullRow::timesFullColumn(std::shared_ptr> fullCol) { auto answer = this->at(0) * fullCol->at(0); - for (size_t i = 1; i < this->size(); i++) + for (int i = 1; i < this->size(); i++) { answer += this->at(i) * fullCol->at(i); } @@ -73,9 +74,9 @@ namespace MbD { inline std::shared_ptr> FullRow::timesTransposeFullMatrix(std::shared_ptr> fullMat) { //"a*bT = a(1,j)b(k,j)" - size_t ncol = fullMat->nrow(); + int ncol = fullMat->nrow(); auto answer = std::make_shared>(ncol); - for (size_t k = 0; k < ncol; k++) { + for (int k = 0; k < ncol; k++) { answer->at(k) = this->dot(fullMat->at(k)); } return answer; @@ -83,16 +84,21 @@ namespace MbD { template inline void FullRow::equalSelfPlusFullRowTimes(std::shared_ptr> fullRow, double factor) { - for (size_t i = 0; i < this->size(); i++) + for (int i = 0; i < this->size(); i++) { this->at(i) += fullRow->at(i) * factor; } } template + inline std::shared_ptr> FullRow::transpose() + { + return std::make_shared>(*this); + } + template inline std::shared_ptr> FullRow::timesFullMatrix(std::shared_ptr> fullMat) { std::shared_ptr> answer = fullMat->at(0)->times(this->at(0)); - for (size_t j = 1; j < this->size(); j++) + for (int j = 1; j < (int) this->size(); j++) { answer->equalSelfPlusFullRowTimes(fullMat->at(j), this->at(j)); } diff --git a/MbDCode/FullVector.h b/MbDCode/FullVector.h index 07f770e..5c71314 100644 --- a/MbDCode/FullVector.h +++ b/MbDCode/FullVector.h @@ -7,28 +7,32 @@ namespace MbD { { public: FullVector() {} - FullVector(size_t count) : Array(count) {} - FullVector(size_t count, const T& value) : Array(count, value) {} + FullVector(std::vector vec) : Array(vec) {} + FullVector(int count) : Array(count) {} + FullVector(int count, const T& value) : Array(count, value) {} + FullVector(std::vector::iterator begin, std::vector::iterator end) : Array(begin, end) {} FullVector(std::initializer_list list) : Array{ list } {} double dot(std::shared_ptr> vec); - void atiplusNumber(size_t i, T value); + void atiplusNumber(int i, T value); double sumOfSquares() override; - size_t numberOfElements() override; + int numberOfElements() override; void zeroSelf() override; - void atitimes(size_t i, double factor); + void atitimes(int i, double factor); + void atiplusFullVectortimes(int i, std::shared_ptr fullVec, double factor); + }; template inline double FullVector::dot(std::shared_ptr> vec) { - size_t n = this->size(); + int n = (int) this->size(); double answer = 0.0; - for (size_t i = 0; i < n; i++) { + for (int i = 0; i < n; i++) { answer += this->at(i) * vec->at(i); } return answer; } template - inline void FullVector::atiplusNumber(size_t i, T value) + inline void FullVector::atiplusNumber(int i, T value) { this->at(i) += value; } @@ -36,7 +40,7 @@ namespace MbD { inline double FullVector::sumOfSquares() { double sum = 0.0; - for (size_t i = 0; i < this->size(); i++) + for (int i = 0; i < this->size(); i++) { double element = this->at(i); sum += element * element; @@ -50,14 +54,14 @@ namespace MbD { return 0.0; } template - inline size_t FullVector::numberOfElements() + inline int FullVector::numberOfElements() { - return this->size(); + return (int) this->size(); } template<> inline void FullVector::zeroSelf() { - for (size_t i = 0; i < this->size(); i++) { + for (int i = 0; i < this->size(); i++) { this->at(i) = 0.0;; } } @@ -67,8 +71,17 @@ namespace MbD { assert(false); } template - inline void FullVector::atitimes(size_t i, double factor) + inline void FullVector::atitimes(int i, double factor) { this->at(i) *= factor; } + template + inline void FullVector::atiplusFullVectortimes(int i1, std::shared_ptr fullVec, double factor) + { + for (int ii = 0; ii < fullVec->size(); ii++) + { + auto i = i1 + ii; + this->at(i) += fullVec->at(ii) * factor; + } + } } diff --git a/MbDCode/FunctionWithManyArgs.cpp b/MbDCode/FunctionWithManyArgs.cpp index d3589e3..48831ee 100644 --- a/MbDCode/FunctionWithManyArgs.cpp +++ b/MbDCode/FunctionWithManyArgs.cpp @@ -25,7 +25,7 @@ FunctionWithManyArgs::FunctionWithManyArgs(Symsptr term, Symsptr term1, Symsptr FunctionWithManyArgs::FunctionWithManyArgs(std::shared_ptr> _terms) { terms = std::make_shared>(); - for (size_t i = 0; i < _terms->size(); i++) + for (int i = 0; i < _terms->size(); i++) terms->push_back(_terms->at(i)); } diff --git a/MbDCode/GEFullMat.cpp b/MbDCode/GEFullMat.cpp index 3c0bbdb..86b6e50 100644 --- a/MbDCode/GEFullMat.cpp +++ b/MbDCode/GEFullMat.cpp @@ -4,7 +4,7 @@ using namespace MbD; -void MbD::GEFullMat::forwardEliminateWithPivot(size_t p) +void MbD::GEFullMat::forwardEliminateWithPivot(int p) { assert(false); } diff --git a/MbDCode/GEFullMat.h b/MbDCode/GEFullMat.h index 64178ca..8ed60ae 100644 --- a/MbDCode/GEFullMat.h +++ b/MbDCode/GEFullMat.h @@ -8,7 +8,7 @@ namespace MbD { // public: std::shared_ptr> matrixA; - void forwardEliminateWithPivot(size_t p) override; + void forwardEliminateWithPivot(int p) override; void backSubstituteIntoDU() override; void postSolve() override; void preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) override; diff --git a/MbDCode/GEFullMatFullPv.cpp b/MbDCode/GEFullMatFullPv.cpp index baa1282..0439fe8 100644 --- a/MbDCode/GEFullMatFullPv.cpp +++ b/MbDCode/GEFullMatFullPv.cpp @@ -4,7 +4,7 @@ using namespace MbD; -void MbD::GEFullMatFullPv::doPivoting(size_t p) +void MbD::GEFullMatFullPv::doPivoting(int p) { assert(false); } diff --git a/MbDCode/GEFullMatFullPv.h b/MbDCode/GEFullMatFullPv.h index 89f2c06..bdbb145 100644 --- a/MbDCode/GEFullMatFullPv.h +++ b/MbDCode/GEFullMatFullPv.h @@ -7,7 +7,7 @@ namespace MbD { { // public: - void doPivoting(size_t p) override; + void doPivoting(int p) override; void postSolve() override; }; } diff --git a/MbDCode/GEFullMatParPv.cpp b/MbDCode/GEFullMatParPv.cpp index 4f75df5..eb5e5b4 100644 --- a/MbDCode/GEFullMatParPv.cpp +++ b/MbDCode/GEFullMatParPv.cpp @@ -4,7 +4,7 @@ using namespace MbD; -void MbD::GEFullMatParPv::doPivoting(size_t p) +void MbD::GEFullMatParPv::doPivoting(int p) { assert(false); } diff --git a/MbDCode/GEFullMatParPv.h b/MbDCode/GEFullMatParPv.h index c7a731a..207b9f9 100644 --- a/MbDCode/GEFullMatParPv.h +++ b/MbDCode/GEFullMatParPv.h @@ -7,7 +7,7 @@ namespace MbD { { // public: - void doPivoting(size_t p) override; + void doPivoting(int p) override; void postSolve() override; void preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) override; }; diff --git a/MbDCode/GESpMat3.cpp b/MbDCode/GESpMat.cpp similarity index 92% rename from MbDCode/GESpMat3.cpp rename to MbDCode/GESpMat.cpp index fe713c3..be7d1e1 100644 --- a/MbDCode/GESpMat3.cpp +++ b/MbDCode/GESpMat.cpp @@ -1,4 +1,4 @@ -#include "GESpMat3.h" +#include "GESpMat.h" #include "FullColumn.h" #include "SparseMatrix.h" diff --git a/MbDCode/GESpMat3.h b/MbDCode/GESpMat.h similarity index 71% rename from MbDCode/GESpMat3.h rename to MbDCode/GESpMat.h index 515865f..bb4cff5 100644 --- a/MbDCode/GESpMat3.h +++ b/MbDCode/GESpMat.h @@ -11,8 +11,8 @@ namespace MbD { FColDsptr solvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal); std::shared_ptr> matrixA; - size_t markowitzPivotRowCount, markowitzPivotColCount; - std::shared_ptr> privateIndicesOfNonZerosInPivotRow, rowPositionsOfNonZerosInPivotColumn; + int markowitzPivotRowCount, markowitzPivotColCount; + std::shared_ptr> privateIndicesOfNonZerosInPivotRow, rowPositionsOfNonZerosInPivotColumn; }; } diff --git a/MbDCode/GESpMatFullPv.cpp b/MbDCode/GESpMatFullPv.cpp index 29e4b00..bddf335 100644 --- a/MbDCode/GESpMatFullPv.cpp +++ b/MbDCode/GESpMatFullPv.cpp @@ -1,22 +1,84 @@ #include #include "GESpMatFullPv.h" +#include "SingularMatrixError.h" using namespace MbD; -void MbD::GESpMatFullPv::doPivoting(size_t p) +void MbD::GESpMatFullPv::doPivoting(int p) { assert(false); } -void MbD::GESpMatFullPv::forwardEliminateWithPivot(size_t p) +void MbD::GESpMatFullPv::forwardEliminateWithPivot(int p) { - assert(false); + //app is pivot. + //i > p, j > p + //aip is eliminated. + //apj can cause fill - in. + //Columns are in place. + //"rightHandSideB may be multidimensional." + + auto jp = colOrder->at(p); + auto& rowp = matrixA->at(p); + auto app = rowp->at(jp); + auto elementsInPivotRow = std::make_shared*>>(rowp->size() - 1); + int index = 0; + for (auto const& keyValue : *rowp) { + if (keyValue.first != jp) { + elementsInPivotRow->at(index) = (&keyValue); + index++; + } + } + auto bp = rightHandSideB->at(p); + for (int ii = 0; ii < markowitzPivotColCount; ii++) + { + auto i = rowPositionsOfNonZerosInPivotColumn->at(ii); + auto& spRowi = matrixA->at(i); + if (spRowi->find(jp) == spRowi->end()) continue; + auto aip = spRowi->at(jp); + spRowi->erase(jp); + auto factor = aip / app; + for (auto keyValue : *elementsInPivotRow) + { + auto j = keyValue->first; + auto apj = keyValue->second; + (*spRowi)[j] -= factor * apj; + } + rightHandSideB->at(i) -= bp * factor; + } } void MbD::GESpMatFullPv::backSubstituteIntoDU() { - assert(false); + //"Use colOrder to get DU in upper triangular with nonzero diagonals." + //"Formula given by Eqn. 9.26 and 9.27 in Chapra's text 2nd Edition." + + double sum, duij, duii; + //answerX = rightHandSideB->copyEmpty(); + assert(m == n); + answerX = std::make_shared>(m); + auto jn = colOrder->at(n); + answerX->at(jn) = rightHandSideB->at(m) / matrixA->at(m)->at(jn); + //auto rhsZeroElement = this->rhsZeroElement(); + for (int i = n - 2; i >= 0; i--) + { + auto rowi = matrixA->at(i); + sum = 0.0; // rhsZeroElement copy. + for (auto const& keyValue : *rowi) { + auto jj = keyValue.first; + auto j = positionsOfOriginalCols->at(jj); + if (j > i) { + duij = keyValue.second; + sum += answerX->at(jj) * duij; + } + else { + duii = keyValue.second; + } + } + auto ji = colOrder->at(i); + answerX->at(ji) = rightHandSideB->at(i) - (sum / duii); + } } void MbD::GESpMatFullPv::postSolve() @@ -26,5 +88,38 @@ void MbD::GESpMatFullPv::postSolve() void MbD::GESpMatFullPv::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) { - assert(false); + //"A conditioned copy of spMat is solved." + if (m != spMat->nrow() || n != spMat->ncol()) { + m = spMat->nrow(); + n = spMat->ncol(); + matrixA = std::make_shared>(m); + pivotValues = std::make_shared>(m); + rowOrder = std::make_shared>(m); + 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++) + { + rowPositionsOfNonZerosInColumns->at(j) = std::make_shared>(); + } + } + if (saveOriginal) { + rightHandSideB = fullCol->copy(); + } + else { + rightHandSideB = fullCol; + } + for (int i = 0; i < m; i++) + { + auto& spRowi = spMat->at(i); + auto maxRowElement = spRowi->maxElement(); + if (maxRowElement == 0) { + throw SingularMatrixError(""); + } + matrixA->at(i) = spRowi->conditionedWithTol(singularPivotTolerance * maxRowElement); + rowOrder->at(i) = i; + colOrder->at(i) = i; + positionsOfOriginalCols->at(i) = i; + } } diff --git a/MbDCode/GESpMatFullPv.h b/MbDCode/GESpMatFullPv.h index e3bc146..e0cee8f 100644 --- a/MbDCode/GESpMatFullPv.h +++ b/MbDCode/GESpMatFullPv.h @@ -1,20 +1,20 @@ #pragma once -#include "GESpMat3.h" +#include "GESpMat.h" namespace MbD { class GESpMatFullPv : public GESpMat { //positionsOfOriginalCols rowPositionsOfNonZerosInColumns public: - void doPivoting(size_t p) override; - void forwardEliminateWithPivot(size_t p) override; + void doPivoting(int p) override; + void forwardEliminateWithPivot(int p) override; void backSubstituteIntoDU() override; void postSolve() override; void preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) override; - std::shared_ptr> positionsOfOriginalCols; - std::shared_ptr>>> rowPositionsOfNonZerosInColumns; + std::shared_ptr> positionsOfOriginalCols; + std::shared_ptr>>> rowPositionsOfNonZerosInColumns; }; } diff --git a/MbDCode/GESpMatFullPvPosIC.cpp b/MbDCode/GESpMatFullPvPosIC.cpp index e6fff04..f68a014 100644 --- a/MbDCode/GESpMatFullPvPosIC.cpp +++ b/MbDCode/GESpMatFullPvPosIC.cpp @@ -2,8 +2,7 @@ #include #include "GESpMatFullPvPosIC.h" -//#include "FullColumn.h" -//#include "SparseMatrix.h" +#include "SingularMatrixError.h" #include "PosICNewtonRaphson.h" using namespace MbD; @@ -12,7 +11,7 @@ void MbD::GESpMatFullPvPosIC::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsp { GESpMatFullPv::preSolvewithsaveOriginal(spMat, fullCol, saveOriginal); if (system == nullptr) { - pivotRowLimits = std::make_shared>(); + pivotRowLimits = std::make_shared>(); } else { pivotRowLimits = system->pivotRowLimits; @@ -20,27 +19,76 @@ void MbD::GESpMatFullPvPosIC::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsp pivotRowLimit = -1; } -void MbD::GESpMatFullPvPosIC::doPivoting(size_t p) +void MbD::GESpMatFullPvPosIC::doPivoting(int p) { //"Used by Gauss Elimination only." //"Swap rows but keep columns in place." //"The elements below the diagonal are removed column by column." - assert(false); auto max = 0.0; auto pivotRow = p; auto pivotCol = p; - for (size_t j = p; j < n; j++) + for (int j = p; j < n; j++) { rowPositionsOfNonZerosInColumns->at(colOrder->at(j))->clear(); } - if (p > pivotRowLimit) { + if (p == pivotRowLimit) { pivotRowLimit = *std::find_if( pivotRowLimits->begin(), pivotRowLimits->end(), - [&](size_t limit) { return limit >= p; }); + [&](int limit) { return limit > p; }); } - for (size_t i = p; i < pivotRowLimit; i++) + for (int i = p; i < pivotRowLimit; 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->swapRows(p, pivotRow); + rightHandSideB->swapRows(p, pivotRow); + rowOrder->swapRows(p, pivotRow); + } + if (p != pivotCol) { + colOrder->swapRows(p, pivotCol); + positionsOfOriginalCols->at(colOrder->at(p)) = p; + positionsOfOriginalCols->at(colOrder->at(pivotCol)) = pivotCol; + } + pivotValues->at(p) = max; + if (max < singularPivotTolerance) { + auto itr = std::find_if( + pivotRowLimits->begin(), pivotRowLimits->end(), + [&](int limit) { return limit > pivotRowLimit; }); + if (itr == pivotRowLimits->end()) { + auto begin = rowOrder->begin() + p; + auto end = rowOrder->begin() + pivotRowLimit; + auto redundantEqnNos = std::make_shared>(begin, end); + throw SingularMatrixError("", redundantEqnNos); + } + else { + pivotRowLimit = *itr; + } + return this->doPivoting(p); + } + auto jp = colOrder->at(p); + rowPositionsOfNonZerosInPivotColumn = rowPositionsOfNonZerosInColumns->at(jp); + for (int i = pivotRowLimit; i < m; i++) + { + auto& spRowi = matrixA->at(i); + if (spRowi->find(jp) != spRowi->end()) { + rowPositionsOfNonZerosInPivotColumn->push_back(i); + } + } + if (rowPositionsOfNonZerosInPivotColumn->front() == p) { + rowPositionsOfNonZerosInPivotColumn->erase(rowPositionsOfNonZerosInPivotColumn->begin()); + } + markowitzPivotColCount = (int) rowPositionsOfNonZerosInPivotColumn->size(); +} \ No newline at end of file diff --git a/MbDCode/GESpMatFullPvPosIC.h b/MbDCode/GESpMatFullPvPosIC.h index 22d91a9..6b3a7ed 100644 --- a/MbDCode/GESpMatFullPvPosIC.h +++ b/MbDCode/GESpMatFullPvPosIC.h @@ -10,11 +10,11 @@ namespace MbD { //system pivotRowLimits pivotRowLimit public: void preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) override; - void doPivoting(size_t p) override; + void doPivoting(int p) override; PosICNewtonRaphson* system; //Use raw pointer when pointing backwards. - std::shared_ptr> pivotRowLimits; - size_t pivotRowLimit; + std::shared_ptr> pivotRowLimits; + int pivotRowLimit; }; } diff --git a/MbDCode/GESpMatParPv.cpp b/MbDCode/GESpMatParPv.cpp index 04f359d..9aabea0 100644 --- a/MbDCode/GESpMatParPv.cpp +++ b/MbDCode/GESpMatParPv.cpp @@ -4,17 +4,66 @@ using namespace MbD; -void MbD::GESpMatParPv::forwardEliminateWithPivot(size_t p) +void MbD::GESpMatParPv::forwardEliminateWithPivot(int p) { - assert(false); + //"rightHandSideB may be multidimensional." + + auto& rowp = matrixA->at(p); + auto app = rowp->at(p); + auto elementsInPivotRow = std::make_shared*>>(rowp->size() - 1); + int index = 0; + for (auto const& keyValue : *rowp) { + if (keyValue.first != p) { + elementsInPivotRow->at(index) = (&keyValue); + index++; + } + } + auto bp = rightHandSideB->at(p); + for (int ii = 0; ii < markowitzPivotColCount; ii++) + { + auto i = rowPositionsOfNonZerosInPivotColumn->at(ii); + auto& rowi = matrixA->at(i); + auto aip = rowi->at(p); + rowi->erase(p); + auto factor = aip / app; + for (auto keyValue : *elementsInPivotRow) + { + auto j = keyValue->first; + auto apj = keyValue->second; + (*rowi)[j] -= factor * apj; + } + rightHandSideB->at(i) -= bp * factor; + } } void MbD::GESpMatParPv::backSubstituteIntoDU() { - assert(false); + //"DU is upper triangular with nonzero diagonals." + + double sum, duij, duii; + //answerX = rightHandSideB->copyEmpty(); + assert(m == n); + answerX = std::make_shared>(m); + answerX->at(n - 1) = rightHandSideB->at(m - 1) / matrixA->at(m - 1)->at(n - 1); + //auto rhsZeroElement = this->rhsZeroElement(); + for (int i = n - 2; i >= 0; i--) + { + auto rowi = matrixA->at(i); + sum = 0.0; // rhsZeroElement copy. + for (auto const& keyValue : *rowi) { + auto j = keyValue.first; + if (j > i) { + duij = keyValue.second; + sum += answerX->at(j) * duij; + } + else { + duii = keyValue.second; + } + } + answerX->at(i) = rightHandSideB->at(i) - (sum / duii); + } } void MbD::GESpMatParPv::postSolve() { - assert(false); } diff --git a/MbDCode/GESpMatParPv.h b/MbDCode/GESpMatParPv.h index 06a8cf6..06ed01a 100644 --- a/MbDCode/GESpMatParPv.h +++ b/MbDCode/GESpMatParPv.h @@ -1,13 +1,13 @@ #pragma once -#include "GESpMat3.h" +#include "GESpMat.h" namespace MbD { class GESpMatParPv : public GESpMat { // public: - void forwardEliminateWithPivot(size_t p) override; + void forwardEliminateWithPivot(int p) override; void backSubstituteIntoDU() override; void postSolve() override; diff --git a/MbDCode/GESpMatParPvMarko.cpp b/MbDCode/GESpMatParPvMarko.cpp index 14402a7..e21a5e5 100644 --- a/MbDCode/GESpMatParPvMarko.cpp +++ b/MbDCode/GESpMatParPvMarko.cpp @@ -4,7 +4,7 @@ using namespace MbD; -void MbD::GESpMatParPvMarko::doPivoting(size_t p) +void MbD::GESpMatParPvMarko::doPivoting(int p) { assert(false); } diff --git a/MbDCode/GESpMatParPvMarko.h b/MbDCode/GESpMatParPvMarko.h index 41d93d5..2fadbb6 100644 --- a/MbDCode/GESpMatParPvMarko.h +++ b/MbDCode/GESpMatParPvMarko.h @@ -7,7 +7,7 @@ namespace MbD { { // public: - void doPivoting(size_t p) override; + void doPivoting(int p) override; void preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) override; }; diff --git a/MbDCode/GESpMatParPvMarkoFast.cpp b/MbDCode/GESpMatParPvMarkoFast.cpp index b4e3dc6..6204ae3 100644 --- a/MbDCode/GESpMatParPvMarkoFast.cpp +++ b/MbDCode/GESpMatParPvMarkoFast.cpp @@ -14,8 +14,8 @@ 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>(); + privateIndicesOfNonZerosInPivotRow = std::make_shared>(); + rowPositionsOfNonZerosInPivotColumn = std::make_shared>(); } if (saveOriginal) { rightHandSideB = fullCol->copy(); @@ -23,7 +23,7 @@ void MbD::GESpMatParPvMarkoFast::preSolvewithsaveOriginal(SpMatDsptr spMat, FCol else { rightHandSideB = fullCol; } - for (size_t i = 0; i < m; i++) + for (int i = 0; i < m; i++) { auto& spRowi = spMat->at(i); auto maxRowElement = spRowi->maxElement(); @@ -36,7 +36,64 @@ void MbD::GESpMatParPvMarkoFast::preSolvewithsaveOriginal(SpMatDsptr spMat, FCol } } -void MbD::GESpMatParPvMarkoFast::doPivoting(size_t p) +void MbD::GESpMatParPvMarkoFast::doPivoting(int p) { - assert(false); + //"Search from bottom to top." + //"Optimized for speed. No check for singular pivot." + //"Do partial pivoting." + //"criterion := mag / (2.0d raisedTo: rowiCount)." + //"Rows are explicitly scaled in preSolve:." + //"Pivot size are nieither checked nor stored." + + //| lookForFirstNonZeroInPivotCol i rowi aip criterionMax rowPivoti criterion max | + int i, rowPivoti; + double aip, 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 { + aip = spRowi->at(p); + markowitzPivotColCount = 0; + if (aip < 0) aip = -aip; + max = aip; + criterionMax = aip / 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++; + if (aip < 0) aip = -aip; + criterion = aip / std::pow(2.0, spRowi->size()); + if (criterion > criterionMax) { + max = aip; + criterionMax = criterion; + rowPositionsOfNonZerosInPivotColumn->push_back(rowPivoti); + rowPivoti = i; + } + else { + rowPositionsOfNonZerosInPivotColumn->push_back(i); + } + } + i--; + } + if (p != rowPivoti) { + matrixA->swapRows(p, rowPivoti); + rightHandSideB->swapRows(p, rowPivoti); + if (aip != std::numeric_limits::min()) rowPositionsOfNonZerosInPivotColumn->at(markowitzPivotColCount - 1) = rowPivoti; + } + if (max < singularPivotTolerance) throw SingularMatrixError(""); } diff --git a/MbDCode/GESpMatParPvMarkoFast.h b/MbDCode/GESpMatParPvMarkoFast.h index 178f6f5..d2d373c 100644 --- a/MbDCode/GESpMatParPvMarkoFast.h +++ b/MbDCode/GESpMatParPvMarkoFast.h @@ -8,7 +8,7 @@ namespace MbD { // public: void preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) override; - void doPivoting(size_t p) override; + void doPivoting(int p) override; }; } diff --git a/MbDCode/GESpMatParPvPrecise.cpp b/MbDCode/GESpMatParPvPrecise.cpp index 9af64c1..c174ac5 100644 --- a/MbDCode/GESpMatParPvPrecise.cpp +++ b/MbDCode/GESpMatParPvPrecise.cpp @@ -4,7 +4,7 @@ using namespace MbD; -void MbD::GESpMatParPvPrecise::doPivoting(size_t p) +void MbD::GESpMatParPvPrecise::doPivoting(int p) { assert(false); } diff --git a/MbDCode/GESpMatParPvPrecise.h b/MbDCode/GESpMatParPvPrecise.h index 1958980..ede3a07 100644 --- a/MbDCode/GESpMatParPvPrecise.h +++ b/MbDCode/GESpMatParPvPrecise.h @@ -7,7 +7,7 @@ namespace MbD { { // public: - void doPivoting(size_t p) override; + void doPivoting(int p) override; void preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) override; }; diff --git a/MbDCode/ICKineIntegrator.cpp b/MbDCode/ICKineIntegrator.cpp new file mode 100644 index 0000000..c9fc019 --- /dev/null +++ b/MbDCode/ICKineIntegrator.cpp @@ -0,0 +1 @@ +#include "ICKineIntegrator.h" diff --git a/MbDCode/ICKineIntegrator.h b/MbDCode/ICKineIntegrator.h new file mode 100644 index 0000000..9557076 --- /dev/null +++ b/MbDCode/ICKineIntegrator.h @@ -0,0 +1,12 @@ +#pragma once +#include "QuasiIntegrator.h" + +namespace MbD { + class ICKineIntegrator : public QuasiIntegrator + { + // + public: + + }; +} + diff --git a/MbDCode/Integrator.h b/MbDCode/Integrator.h index 7c0e4d9..eee8258 100644 --- a/MbDCode/Integrator.h +++ b/MbDCode/Integrator.h @@ -1,13 +1,16 @@ #pragma once - +#include #include "Solver.h" namespace MbD { + class SystemSolver; + class Integrator : public Solver { - // + //system direction public: + double direction = 1; }; } diff --git a/MbDCode/IntegratorInterface.cpp b/MbDCode/IntegratorInterface.cpp index 3a2233c..4af6d4d 100644 --- a/MbDCode/IntegratorInterface.cpp +++ b/MbDCode/IntegratorInterface.cpp @@ -1,3 +1,46 @@ +#include + #include "IntegratorInterface.h" +#include "SystemSolver.h" +#include "BasicQuasiIntegrator.h" using namespace MbD; + +void MbD::IntegratorInterface::initializeGlobally() +{ +tstart = system->startTime(); +hout = system->outputStepSize(); +hmax = system->maxStepSize(); +hmin = system->minStepSize(); +tout = system->firstOutputTime(); +tend = system->endTime(); +direction = (tstart < tend) ? 1.0 : -1.0; +} + +void MbD::IntegratorInterface::setSystem(SystemSolver* sys) +{ + system = sys; +} + +void MbD::IntegratorInterface::logString(std::string& str) +{ + system->logString(str); +} + +void MbD::IntegratorInterface::run() +{ + this->preRun(); + this->initializeLocally(); + this->initializeGlobally(); + if (hout > (4 * std::numeric_limits::epsilon()) && (direction * tout < (direction * (tend + (0.1 * direction * hout))))) { + integrator->run(); + } + this->finalize(); + this->reportStats(); + this->postRun(); +} + +int MbD::IntegratorInterface::orderMax() +{ + return system->orderMax; +} diff --git a/MbDCode/IntegratorInterface.h b/MbDCode/IntegratorInterface.h index e1abf7c..342d794 100644 --- a/MbDCode/IntegratorInterface.h +++ b/MbDCode/IntegratorInterface.h @@ -1,13 +1,28 @@ #pragma once #include "Integrator.h" +//#include "BasicQuasiIntegrator.h" namespace MbD { + class BasicQuasiIntegrator; + class IntegratorInterface : public Integrator { - // + //tout hout hmin hmax tstart tend integrator public: + void initializeGlobally() override; + virtual void preRun() = 0; + void setSystem(SystemSolver* sys); + void logString(std::string& str) override; + void run() override; + int orderMax(); + virtual void preFirstStep() = 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 5b41df7..c8c4368 100644 --- a/MbDCode/Item.cpp +++ b/MbDCode/Item.cpp @@ -1,6 +1,8 @@ #include #include #include +#include + #include "Item.h" #include "System.h" @@ -45,7 +47,11 @@ void MbD::Item::calcPostDynCorrectorIteration() { } -void MbD::Item::removeRedundantConstraints(std::shared_ptr> redunEqnNos) +void MbD::Item::removeRedundantConstraints(std::shared_ptr> redunEqnNos) +{ +} + +void MbD::Item::reactivateRedundantConstraints() { } @@ -61,6 +67,10 @@ void MbD::Item::fillEssenConstraints(std::shared_ptr>> redunConstraints) +{ +} + void MbD::Item::fillqsu(FColDsptr col) { } @@ -77,11 +87,47 @@ void MbD::Item::setqsulam(FColDsptr col) { } +void MbD::Item::outputStates() +{ + std::stringstream ss; + ss << classname() << " " << name; + auto str = ss.str(); + this->logString(str); +} + +void MbD::Item::preDyn() +{ +} + +std::string MbD::Item::classname() +{ + std::string str = typeid(*this).name(); + auto answer = str.substr(11, str.size() - 11); + return answer; +} + +void MbD::Item::preDynFirstStep() +{ + //"Called before the start of the first step in the dynamic solution." + this->preDynStep(); +} + +void MbD::Item::preDynStep() +{ +} + +double MbD::Item::suggestSmallerOrAcceptDynFirstStepSize(double hnew) +{ + //"Default is return hnew." + //"Best to do nothing so as not to disrupt the starting algorithm." + return hnew; +} + void MbD::Item::constraintsReport() { } -void MbD::Item::setqsu(std::shared_ptr> qsuOld) +void MbD::Item::setqsu(FColDsptr qsuOld) { } @@ -89,6 +135,11 @@ void MbD::Item::useEquationNumbers() { } +void MbD::Item::logString(std::string& str) +{ + System::getInstance().logString(str); +} + void MbD::Item::prePosIC() { //"Called once before solving for position initial conditions." @@ -110,6 +161,10 @@ void MbD::Item::prePostICRestart() { } +void MbD::Item::postPosIC() +{ +} + void MbD::Item::postPosICIteration() { this->calcPostDynCorrectorIteration(); diff --git a/MbDCode/Item.h b/MbDCode/Item.h index 7f62402..3ad56da 100644 --- a/MbDCode/Item.h +++ b/MbDCode/Item.h @@ -23,8 +23,9 @@ namespace MbD { virtual void postInput(); virtual void calcPostDynCorrectorIteration(); virtual void constraintsReport(); - virtual void setqsu(std::shared_ptr> qsuOld); + virtual void setqsu(FColDsptr qsuOld); virtual void useEquationNumbers(); + virtual void logString(std::string& str); virtual void prePosIC(); virtual void fillPosICError(FColDsptr col); @@ -33,20 +34,30 @@ namespace MbD { virtual void prePostIC(); virtual void prePostICIteration(); virtual void prePostICRestart(); + virtual void postPosIC(); virtual void postPosICIteration(); - virtual void removeRedundantConstraints(std::shared_ptr> redundantEqnNos); + virtual void removeRedundantConstraints(std::shared_ptr> redundantEqnNos); + virtual void reactivateRedundantConstraints(); virtual void fillPosKineError(FColDsptr col); virtual void fillPosKineJacob(FMatDsptr mat); virtual void fillEssenConstraints(std::shared_ptr>> essenConstraints); + virtual void fillRedundantConstraints(std::shared_ptr>> redunConstraints); + virtual void fillqsu(FColDsptr col); virtual void fillqsuWeights(std::shared_ptr> diagMat); virtual void fillqsulam(FColDsptr col); virtual void setqsulam(FColDsptr col); + virtual void outputStates(); + virtual void preDyn(); + virtual std::string classname(); + virtual void preDynFirstStep(); + virtual void preDynStep(); + virtual double suggestSmallerOrAcceptDynFirstStepSize(double hnew); + void setName(std::string& str); const std::string& getName() const; - private: std::string name; }; } diff --git a/MbDCode/Joint.cpp b/MbDCode/Joint.cpp index 63259e5..f1d1ae2 100644 --- a/MbDCode/Joint.cpp +++ b/MbDCode/Joint.cpp @@ -1,9 +1,14 @@ #include +#include +#include #include "Joint.h" #include "Constraint.h" #include "EndFrameqc.h" #include "EndFrameqct.h" +#include "CREATE.h" +#include "RedundantConstraint.h" +#include "MarkerFrame.h" using namespace MbD; @@ -79,6 +84,11 @@ void MbD::Joint::fillPerpenConstraints(std::shared_ptr con) { con->fillPerpenConstraints(con, perpenConstraints); }); } +void MbD::Joint::fillRedundantConstraints(std::shared_ptr>> redunConstraints) +{ + constraintsDo([&](std::shared_ptr con) { con->fillRedundantConstraints(con, redunConstraints); }); +} + void MbD::Joint::fillqsulam(FColDsptr col) { constraintsDo([&](std::shared_ptr con) { con->fillqsulam(col); }); @@ -98,3 +108,77 @@ void MbD::Joint::postPosICIteration() { constraintsDo([](std::shared_ptr constraint) { constraint->postPosICIteration(); }); } + +void MbD::Joint::fillPosICError(FColDsptr col) +{ + constraintsDo([&](std::shared_ptr con) { con->fillPosICError(col); }); +} + +void MbD::Joint::fillPosICJacob(SpMatDsptr mat) +{ + constraintsDo([&](std::shared_ptr con) { con->fillPosICJacob(mat); }); +} + +void MbD::Joint::removeRedundantConstraints(std::shared_ptr> redundantEqnNos) +{ + for (size_t i = 0; i < constraints->size(); i++) + { + auto& constraint = constraints->at(i); + if (std::find(redundantEqnNos->begin(), redundantEqnNos->end(), constraint->iG) != redundantEqnNos->end()) { + auto redunCon = CREATE::With(); + redunCon->constraint = constraint; + constraints->at(i) = redunCon; + } + } +} + +void MbD::Joint::reactivateRedundantConstraints() +{ + for (size_t i = 0; i < constraints->size(); i++) + { + auto& con = constraints->at(i); + if (con->isRedundant()) { + constraints->at(i) = std::static_pointer_cast(con)->constraint; + } + } +} + +void MbD::Joint::constraintsReport() +{ + auto redunCons = std::make_shared>>(); + constraintsDo([&](std::shared_ptr con) { + if (con->isRedundant()) { + redunCons->push_back(con); + } + }); + if (redunCons->size() > 0) { + std::string str = "MbD: " + this->classname() + std::string(" ") + this->getName() + " has the following constraint(s) removed: "; + this->logString(str); + std::for_each(redunCons->begin(), redunCons->end(), [&](auto& con) { + str = "MbD: " + std::string(" ") + con->classname(); + this->logString(str); + }); + } +} + +void MbD::Joint::postPosIC() +{ + constraintsDo([](std::shared_ptr constraint) { constraint->postPosIC(); }); +} + +void MbD::Joint::outputStates() +{ + Item::outputStates(); + std::stringstream ss; + ss << "frmI = " << frmI->markerFrame->getName() << std::endl; + ss << "frmJ = " << frmJ->markerFrame->getName() << std::endl; + auto str = ss.str(); + this->logString(str); + + constraintsDo([](std::shared_ptr constraint) { constraint->outputStates(); }); +} + +void MbD::Joint::preDyn() +{ + constraintsDo([](std::shared_ptr constraint) { constraint->preDyn(); }); +} diff --git a/MbDCode/Joint.h b/MbDCode/Joint.h index f58fe76..a1f1ec7 100644 --- a/MbDCode/Joint.h +++ b/MbDCode/Joint.h @@ -26,10 +26,19 @@ namespace MbD { 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 fillqsulam(FColDsptr col) override; void useEquationNumbers() override; void setqsulam(FColDsptr col) override; void postPosICIteration() override; + void fillPosICError(FColDsptr col) override; + void fillPosICJacob(SpMatDsptr mat) override; + void removeRedundantConstraints(std::shared_ptr> redundantEqnNos) override; + void reactivateRedundantConstraints() override; + void constraintsReport() override; + void postPosIC() override; + void outputStates() override; + void preDyn() override; EndFrmcptr frmI; EndFrmcptr frmJ; diff --git a/MbDCode/KineIntegrator.cpp b/MbDCode/KineIntegrator.cpp index 6a36d4b..156939b 100644 --- a/MbDCode/KineIntegrator.cpp +++ b/MbDCode/KineIntegrator.cpp @@ -1,3 +1,13 @@ +#include + #include "KineIntegrator.h" +#include "SystemSolver.h" using namespace MbD; + +void MbD::KineIntegrator::preRun() +{ + std::string str = "MbD: Starting kinematic analysis."; + system->logString(str); + QuasiIntegrator::preRun(); +} diff --git a/MbDCode/KineIntegrator.h b/MbDCode/KineIntegrator.h index 90b70e7..e70426c 100644 --- a/MbDCode/KineIntegrator.h +++ b/MbDCode/KineIntegrator.h @@ -7,7 +7,7 @@ namespace MbD { { // public: - + void preRun() override; }; } diff --git a/MbDCode/KinematicIeJe.h b/MbDCode/KinematicIeJe.h index ffaf061..ec00315 100644 --- a/MbDCode/KinematicIeJe.h +++ b/MbDCode/KinematicIeJe.h @@ -10,6 +10,12 @@ namespace MbD { public: KinematicIeJe(); KinematicIeJe(EndFrmcptr frmi, EndFrmcptr frmj); + virtual FRowDsptr pvaluepXJ() = 0; + virtual FRowDsptr pvaluepEJ() = 0; + virtual FMatDsptr ppvaluepXJpEK() = 0; + virtual FMatDsptr ppvaluepEJpEK() = 0; + virtual FMatDsptr ppvaluepEJpEJ() = 0; + EndFrmcptr frmI, frmJ; }; diff --git a/MbDCode/LDUFullMat.cpp b/MbDCode/LDUFullMat.cpp new file mode 100644 index 0000000..9833bd4 --- /dev/null +++ b/MbDCode/LDUFullMat.cpp @@ -0,0 +1 @@ +#include "LDUFullMat.h" diff --git a/MbDCode/LDUFullMat.h b/MbDCode/LDUFullMat.h new file mode 100644 index 0000000..900cddd --- /dev/null +++ b/MbDCode/LDUFullMat.h @@ -0,0 +1,13 @@ +#pragma once + +#include "MatrixLDU.h" + +namespace MbD { + class LDUFullMat : public MatrixLDU + { + // + public: + + }; +} + diff --git a/MbDCode/LDUFullMatParPv.cpp b/MbDCode/LDUFullMatParPv.cpp new file mode 100644 index 0000000..057a5d4 --- /dev/null +++ b/MbDCode/LDUFullMatParPv.cpp @@ -0,0 +1,27 @@ +#include "LDUFullMatParPv.h" +#include "FullMatrix.h" + +using namespace MbD; + +FMatDsptr MbD::LDUFullMatParPv::inversesaveOriginal(FMatDsptr fullMat, bool saveOriginal) +{ + assert(false); + //"ForAndBackSub be optimized for the identity matrix." + + //| 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(); +} diff --git a/MbDCode/LDUFullMatParPv.h b/MbDCode/LDUFullMatParPv.h new file mode 100644 index 0000000..a99e98a --- /dev/null +++ b/MbDCode/LDUFullMatParPv.h @@ -0,0 +1,14 @@ +#pragma once + +#include "LDUFullMat.h" + +namespace MbD { + class LDUFullMatParPv : public LDUFullMat + { + // + public: + FMatDsptr inversesaveOriginal(FMatDsptr fullMat, bool saveOriginal); + + }; +} + diff --git a/MbDCode/LinearMultiStepMethod.cpp b/MbDCode/LinearMultiStepMethod.cpp new file mode 100644 index 0000000..cb543f4 --- /dev/null +++ b/MbDCode/LinearMultiStepMethod.cpp @@ -0,0 +1 @@ +#include "LinearMultiStepMethod.h" diff --git a/MbDCode/LinearMultiStepMethod.h b/MbDCode/LinearMultiStepMethod.h new file mode 100644 index 0000000..8b07f12 --- /dev/null +++ b/MbDCode/LinearMultiStepMethod.h @@ -0,0 +1,13 @@ +#pragma once + +#include "DifferenceOperator.h" + +namespace MbD { + class LinearMultiStepMethod : public DifferenceOperator + { + // + public: + + }; +} + diff --git a/MbDCode/MarkerFrame.cpp b/MbDCode/MarkerFrame.cpp index be9dfd6..b7c03d4 100644 --- a/MbDCode/MarkerFrame.cpp +++ b/MbDCode/MarkerFrame.cpp @@ -29,7 +29,7 @@ void MarkerFrame::initializeLocally() { pprOmOpEpE = EulerParameters::ppApEpEtimesColumn(rpmp); ppAOmpEpE = EulerParameters::ppApEpEtimesMatrix(aApm); - for (size_t i = 0; i < endFrames->size(); i++) + for (int i = 0; i < endFrames->size(); i++) { auto eFrmqc = std::dynamic_pointer_cast(endFrames->at(i)); if (eFrmqc) { @@ -56,10 +56,10 @@ void MbD::MarkerFrame::calcPostDynCorrectorIteration() { auto rOpO = partFrame->rOpO(); auto aAOp = partFrame->aAOp(); - auto rOmO = rOpO->plusFullColumn(aAOp->timesFullColumn(rpmp)); - auto aAOm = aAOp->timesFullMatrix(aApm); + rOmO = rOpO->plusFullColumn(aAOp->timesFullColumn(rpmp)); + aAOm = aAOp->timesFullMatrix(aApm); auto pAOppE = partFrame->pAOppE(); - for (size_t i = 0; i < 4; i++) + for (int i = 0; i < 4; i++) { auto& pAOppEi = pAOppE->at(i); prOmOpE->atijputFullColumn(1, i, pAOppEi->timesFullColumn(rpmp)); @@ -73,12 +73,12 @@ void MbD::MarkerFrame::prePosIC() endFramesDo([](std::shared_ptr endFrame) { endFrame->prePosIC(); }); } -size_t MbD::MarkerFrame::iqX() +int MbD::MarkerFrame::iqX() { return partFrame->iqX; } -size_t MbD::MarkerFrame::iqE() +int MbD::MarkerFrame::iqE() { return partFrame->iqE; } @@ -103,6 +103,11 @@ void MbD::MarkerFrame::fillqsulam(FColDsptr col) endFramesDo([&](const std::shared_ptr& endFrame) { endFrame->fillqsulam(col); }); } +void MbD::MarkerFrame::setqsu(FColDsptr col) +{ + endFramesDo([&](const std::shared_ptr& endFrame) { endFrame->setqsu(col); }); +} + void MbD::MarkerFrame::setqsulam(FColDsptr col) { endFramesDo([&](const std::shared_ptr& endFrame) { endFrame->setqsulam(col); }); @@ -113,6 +118,16 @@ void MbD::MarkerFrame::postPosICIteration() endFramesDo([](std::shared_ptr endFrame) { endFrame->postPosICIteration(); }); } +void MbD::MarkerFrame::postPosIC() +{ + endFramesDo([](std::shared_ptr endFrame) { endFrame->postPosIC(); }); +} + +void MbD::MarkerFrame::preDyn() +{ + endFramesDo([](std::shared_ptr endFrame) { endFrame->preDyn(); }); +} + void MarkerFrame::setPartFrame(PartFrame* partFrm) { partFrame = partFrm; diff --git a/MbDCode/MarkerFrame.h b/MbDCode/MarkerFrame.h index 587fc9b..306eec1 100644 --- a/MbDCode/MarkerFrame.h +++ b/MbDCode/MarkerFrame.h @@ -30,14 +30,17 @@ namespace MbD { void postInput() override; void calcPostDynCorrectorIteration() override; void prePosIC() override; - size_t iqX(); - size_t iqE(); + int iqX(); + int iqE(); void endFramesDo(const std::function )>& f); void fillqsu(FColDsptr col) override; void fillqsuWeights(std::shared_ptr> diagMat) override; void fillqsulam(FColDsptr col) override; + void setqsu(FColDsptr col) override; void setqsulam(FColDsptr col) override; void postPosICIteration() override; + void postPosIC() override; + void preDyn() 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 new file mode 100644 index 0000000..a9f2688 --- /dev/null +++ b/MbDCode/MatrixDecomposition.cpp @@ -0,0 +1 @@ +#include "MatrixDecomposition.h" diff --git a/MbDCode/MatrixDecomposition.h b/MbDCode/MatrixDecomposition.h new file mode 100644 index 0000000..d6fcf07 --- /dev/null +++ b/MbDCode/MatrixDecomposition.h @@ -0,0 +1,13 @@ +#pragma once + +#include "MatrixSolver.h" + +namespace MbD { + class MatrixDecomposition : public MatrixSolver + { + // + public: + + }; +} + diff --git a/MbDCode/MatrixGaussElimination.cpp b/MbDCode/MatrixGaussElimination.cpp index 6a5de77..d80091b 100644 --- a/MbDCode/MatrixGaussElimination.cpp +++ b/MbDCode/MatrixGaussElimination.cpp @@ -5,7 +5,7 @@ using namespace MbD; FColDsptr MbD::MatrixGaussElimination::basicSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) { this->preSolvewithsaveOriginal(spMat, fullCol, saveOriginal); - for (size_t p = 0; p < m; p++) + for (int p = 0; p < m; p++) { this->doPivoting(p); this->forwardEliminateWithPivot(p); diff --git a/MbDCode/MatrixGaussElimination.h b/MbDCode/MatrixGaussElimination.h index 5c49ef2..7bff644 100644 --- a/MbDCode/MatrixGaussElimination.h +++ b/MbDCode/MatrixGaussElimination.h @@ -9,7 +9,7 @@ namespace MbD { // public: FColDsptr basicSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) override; - virtual void forwardEliminateWithPivot(size_t p) = 0; + virtual void forwardEliminateWithPivot(int p) = 0; virtual void backSubstituteIntoDU() = 0; }; } diff --git a/MbDCode/MatrixLDU.cpp b/MbDCode/MatrixLDU.cpp new file mode 100644 index 0000000..2222847 --- /dev/null +++ b/MbDCode/MatrixLDU.cpp @@ -0,0 +1 @@ +#include "MatrixLDU.h" diff --git a/MbDCode/MatrixLDU.h b/MbDCode/MatrixLDU.h new file mode 100644 index 0000000..a921835 --- /dev/null +++ b/MbDCode/MatrixLDU.h @@ -0,0 +1,13 @@ +#pragma once + +#include "MatrixDecomposition.h" + +namespace MbD { + class MatrixLDU : public MatrixDecomposition + { + // + public: + + }; +} + diff --git a/MbDCode/MatrixSolver.h b/MbDCode/MatrixSolver.h index 25fbe28..ee8e594 100644 --- a/MbDCode/MatrixSolver.h +++ b/MbDCode/MatrixSolver.h @@ -3,6 +3,7 @@ #include "Solver.h" #include "RowTypeMatrix.h" #include "FullMatrix.h" +#include "FullColumn.h" #include "SparseMatrix.h" namespace MbD { @@ -18,12 +19,12 @@ namespace MbD { virtual FColDsptr timedSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal); virtual FColDsptr basicSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) = 0; virtual void preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) = 0; - virtual void doPivoting(size_t p) = 0; + virtual void doPivoting(int p) = 0; virtual void postSolve() = 0; - size_t m = 0, n = 0; + int m = 0, n = 0; FColDsptr answerX, rightHandSideB, rowScalings, pivotValues; - std::shared_ptr> rowOrder, colOrder; + std::shared_ptr> rowOrder, colOrder; double singularPivotTolerance = 0, millisecondsToRun = 0; }; } diff --git a/MbDCode/MbDCode.cpp b/MbDCode/MbDCode.cpp index d47f684..4c57ac1 100644 --- a/MbDCode/MbDCode.cpp +++ b/MbDCode/MbDCode.cpp @@ -72,22 +72,11 @@ int main() //omeOpO = std::make_shared>(ListD{ 0, 0, 0, 1 }); //assembly1->setqXdot(qXdot); //assembly1->setomeOpO(omeOpO); - std::cout << "assembly1->getqX() " << assembly1->getqX()->toString() << std::endl; - std::cout << "assembly1->getqE() " << assembly1->getqE()->toString() << std::endl; + std::cout << "assembly1->getqX() " << *assembly1->getqX() << std::endl; + std::cout << "assembly1->getqE() " << *assembly1->getqE() << std::endl; TheSystem.addPart(assembly1); { auto& partFrame = assembly1->partFrame; - auto marker1 = CREATE::With("/Assembly1/Marker1"); - rpmp = std::make_shared>(ListD{ 0.0, 2.8817526385684, 0.0 }); - marker1->setrpmp(rpmp); - aApm = std::make_shared>(ListListD{ - { 1, 0, 0 }, - { 0, 0, 1 }, - { 0, -1, 0 } - }); - marker1->setaApm(aApm); - partFrame->addMarkerFrame(marker1); - // auto marker2 = CREATE::With("/Assembly1/Marker2"); rpmp = std::make_shared>(ListD{ 0.0, 0.0, 0.0 }); marker2->setrpmp(rpmp); @@ -98,6 +87,17 @@ int main() }); marker2->setaApm(aApm); partFrame->addMarkerFrame(marker2); + // + auto marker1 = CREATE::With("/Assembly1/Marker1"); + rpmp = std::make_shared>(ListD{ 0.0, 2.8817526385684, 0.0 }); + marker1->setrpmp(rpmp); + aApm = std::make_shared>(ListListD{ + { 1, 0, 0 }, + { 0, 0, 1 }, + { 0, -1, 0 } + }); + marker1->setaApm(aApm); + partFrame->addMarkerFrame(marker1); } assembly1->asFixed(); // @@ -105,8 +105,8 @@ int main() std::cout << "crankPart1->getName() " << crankPart1->getName() << std::endl; crankPart1->m = 0.045210530089461; crankPart1->aJ = std::make_shared>(ListD{ 1.7381980042084e-4, 0.003511159968501, 0.0036154518487535 }); - qX = std::make_shared>(ListD{ 0.38423366582893, 6.8384291794733e-9, -0.048029210642807 }); - qE = std::make_shared>(ListD{ 0.0, 0.0, 1.4248456266393e-10, 1.0 }); + qX = std::make_shared>(ListD{ 0.38423368514246, 2.6661567755108e-17, -0.048029210642807 }); + qE = std::make_shared>(ListD{ 0.0, 0.0, 0.0, 1.0 }); crankPart1->setqX(qX); crankPart1->setqE(qE); TheSystem.parts->push_back(crankPart1); @@ -139,8 +139,8 @@ int main() std::cout << "conrodPart2->getName() " << conrodPart2->getName() << std::endl; conrodPart2->m = 0.067815795134192; conrodPart2->aJ = std::make_shared>(ListD{ 2.6072970063126e-4, 0.011784982468533, 0.011941420288912 }); - qX = std::make_shared>(ListD{ 0.38423366582893, 0.49215308269277, 0.048029210642807 }); - qE = std::make_shared>(ListD{ 0.0, 0.0, 0.89871701272344, 0.4385290538168 }); + qX = std::make_shared>(ListD{ 0.38423368514246, 0.49215295678475, 0.048029210642807 }); + qE = std::make_shared>(ListD{ 0.0, 0.0, 0.89871703427292, 0.43852900965351 }); conrodPart2->setqX(qX); conrodPart2->setqE(qE); TheSystem.parts->push_back(conrodPart2); @@ -173,7 +173,7 @@ int main() std::cout << "pistonPart3->getName() " << pistonPart3->getName() << std::endl; pistonPart3->m = 1.730132083368; pistonPart3->aJ = std::make_shared>(ListD{ 0.19449049546716, 0.23028116340971, 0.23028116340971 }); - qX = std::make_shared>(ListD{ -1.284772285311e-18, 1.4645982581368, -4.788228906425e-17 }); + qX = std::make_shared>(ListD{ -1.283972762056e-18, 1.4645980199976, -4.7652385308244e-17 }); qE = std::make_shared>(ListD{ 0.70710678118655, 0.70710678118655, 0.0, 0.0 }); pistonPart3->setqX(qX); pistonPart3->setqE(qE); diff --git a/MbDCode/MbDCode.vcxproj b/MbDCode/MbDCode.vcxproj index 84681b5..817b7fe 100644 --- a/MbDCode/MbDCode.vcxproj +++ b/MbDCode/MbDCode.vcxproj @@ -144,6 +144,7 @@ + @@ -152,6 +153,7 @@ + @@ -181,13 +183,14 @@ - + + @@ -195,13 +198,18 @@ + + + + + @@ -222,12 +230,16 @@ + + + + @@ -256,6 +268,7 @@ + @@ -264,6 +277,7 @@ + @@ -294,13 +308,14 @@ - + + @@ -308,13 +323,18 @@ + + + + + @@ -336,12 +356,16 @@ + + + + diff --git a/MbDCode/MbDCode.vcxproj.filters b/MbDCode/MbDCode.vcxproj.filters index 6f00643..72d480d 100644 --- a/MbDCode/MbDCode.vcxproj.filters +++ b/MbDCode/MbDCode.vcxproj.filters @@ -264,7 +264,7 @@ Source Files - + Source Files @@ -345,6 +345,42 @@ Source Files + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + @@ -599,7 +635,7 @@ Header Files - + Header Files @@ -686,6 +722,42 @@ Header Files + + Header Files + + + 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 d2505f1..a89c696 100644 --- a/MbDCode/NewtonRaphson.cpp +++ b/MbDCode/NewtonRaphson.cpp @@ -18,9 +18,9 @@ void NewtonRaphson::initialize() void NewtonRaphson::initializeLocally() { - iterNo = 0; - nDivergence = 0; - nBackTracking = 0; + iterNo = -1; + nDivergence = -1; + nBackTracking = -1; dxNorms->clear(); yNorms->clear(); yNormOld = std::numeric_limits::max(); @@ -28,6 +28,7 @@ void NewtonRaphson::initializeLocally() void MbD::NewtonRaphson::run() { + assert(false); //self preRun. //self initializeLocally. //self initializeGlobally. @@ -53,7 +54,7 @@ void MbD::NewtonRaphson::iterate() // zero. // " - iterNo = 0; + iterNo = -1; this->fillY(); this->calcyNorm(); yNorms->push_back(yNorm); @@ -70,7 +71,7 @@ void MbD::NewtonRaphson::iterate() void MbD::NewtonRaphson::incrementIterNo() { iterNo++; - if (iterNo > iterMax) { + if (iterNo >= iterMax) { this->reportStats(); throw MaximumIterationError(""); } @@ -92,7 +93,7 @@ bool MbD::NewtonRaphson::isConvergedToNumericalLimit() auto tooLargeTol = 1.0e-2; constexpr auto smallEnoughTol = std::numeric_limits::epsilon(); - auto nDecade = log(tooLargeTol / smallEnoughTol); + auto nDecade = log10(tooLargeTol / smallEnoughTol); auto nDivergenceMax = 3; auto dxNormIterNo = dxNorms->at(iterNo); if (iterNo > 0) { @@ -128,3 +129,8 @@ void MbD::NewtonRaphson::calcDXNormImproveRootCalcYNorm() yNorms->push_back(yNorm); yNormOld = yNorm; } + +void MbD::NewtonRaphson::postRun() +{ + system->postNewtonRaphson(); +} diff --git a/MbDCode/NewtonRaphson.h b/MbDCode/NewtonRaphson.h index 0359deb..fbea302 100644 --- a/MbDCode/NewtonRaphson.h +++ b/MbDCode/NewtonRaphson.h @@ -35,11 +35,12 @@ namespace MbD { virtual void passRootToSystem() = 0; bool isConvergedToNumericalLimit(); void calcDXNormImproveRootCalcYNorm(); + virtual void postRun(); SystemSolver* system; //Use raw pointer when pointing backwards. std::shared_ptr> dxNorms, yNorms; - double dxNorm, yNorm, yNormOld, yNormTol, dxTol, twoAlp, lam; - size_t iterNo = -1, iterMax = -1, nDivergence = -1, nBackTracking = -1; + double dxNorm = 0.0, yNorm = 0.0, yNormOld = 0.0, yNormTol = 0.0, dxTol = 0.0, twoAlp = 0.0, lam = 0.0; + int iterNo = -1, iterMax = -1, nDivergence = -1, nBackTracking = -1; }; } diff --git a/MbDCode/NotKinematicError.cpp b/MbDCode/NotKinematicError.cpp new file mode 100644 index 0000000..3bea61e --- /dev/null +++ b/MbDCode/NotKinematicError.cpp @@ -0,0 +1,7 @@ +#include "NotKinematicError.h" + +using namespace MbD; + +MbD::NotKinematicError::NotKinematicError(const std::string& msg) : std::runtime_error(msg) +{ +} diff --git a/MbDCode/NotKinematicError.h b/MbDCode/NotKinematicError.h new file mode 100644 index 0000000..31f3ada --- /dev/null +++ b/MbDCode/NotKinematicError.h @@ -0,0 +1,15 @@ +#pragma once +#include +#include +#include + +namespace MbD { + class NotKinematicError : virtual public std::runtime_error + { + + public: + //NotKinematicError(); + explicit NotKinematicError(const std::string& msg); + virtual ~NotKinematicError() noexcept {} + }; +} diff --git a/MbDCode/Part.cpp b/MbDCode/Part.cpp index c2b781f..d3b0ef2 100644 --- a/MbDCode/Part.cpp +++ b/MbDCode/Part.cpp @@ -91,12 +91,12 @@ void MbD::Part::prePosIC() partFrame->prePosIC(); } -void MbD::Part::iqX(size_t eqnNo) +void MbD::Part::iqX(int eqnNo) { partFrame->iqX = eqnNo; } -void MbD::Part::iqE(size_t eqnNo) +void MbD::Part::iqE(int eqnNo) { partFrame->iqE = eqnNo; @@ -107,6 +107,10 @@ void MbD::Part::fillEssenConstraints(std::shared_ptrfillEssenConstraints(essenConstraints); } +void MbD::Part::fillRedundantConstraints(std::shared_ptr>> redunConstraints) +{ +} + void MbD::Part::fillqsu(FColDsptr col) { partFrame->fillqsu(col); @@ -128,12 +132,12 @@ void MbD::Part::fillqsuWeights(std::shared_ptr> diagMat) auto wqX = std::make_shared>(3); auto wqE = std::make_shared>(4); if (mMax == 0) { mMax = 1.0; } - for (size_t i = 0; i < 3; i++) + for (int i = 0; i < 3; i++) { wqX->at(i) = (maxw * m / mMax) + minw; } if (aJiMax == 0) { aJiMax = 1.0; } - for (size_t i = 0; i < 3; i++) + for (int i = 0; i < 3; i++) { auto aJi = aJ->at(i); wqE->at(i) = (maxw * aJi / aJiMax) + minw; @@ -154,6 +158,11 @@ void MbD::Part::useEquationNumbers() partFrame->useEquationNumbers(); } +void MbD::Part::setqsu(FColDsptr col) +{ + partFrame->setqsu(col); +} + void MbD::Part::setqsulam(FColDsptr col) { partFrame->setqsulam(col); @@ -163,3 +172,45 @@ void MbD::Part::postPosICIteration() { partFrame->postPosICIteration(); } + +void MbD::Part::fillPosICError(FColDsptr col) +{ + partFrame->fillPosICError(col); +} + +void MbD::Part::fillPosICJacob(SpMatDsptr mat) +{ + partFrame->fillPosICJacob(mat); +} + +void MbD::Part::removeRedundantConstraints(std::shared_ptr> redundantEqnNos) +{ + partFrame->removeRedundantConstraints(redundantEqnNos); +} + +void MbD::Part::reactivateRedundantConstraints() +{ + partFrame->reactivateRedundantConstraints(); +} + +void MbD::Part::constraintsReport() +{ + partFrame->constraintsReport(); +} + +void MbD::Part::postPosIC() +{ + partFrame->postPosIC(); + //this->calcmE(); +} + +void MbD::Part::outputStates() +{ + Item::outputStates(); + partFrame->outputStates(); +} + +void MbD::Part::preDyn() +{ + partFrame->preDyn(); +} diff --git a/MbDCode/Part.h b/MbDCode/Part.h index 6e606f8..923416a 100644 --- a/MbDCode/Part.h +++ b/MbDCode/Part.h @@ -34,18 +34,28 @@ namespace MbD { void calcPostDynCorrectorIteration() override; void prePosIC() override; - void iqX(size_t eqnNo); - void iqE(size_t eqnNo); + void iqX(int eqnNo); + void iqE(int eqnNo); void fillEssenConstraints(std::shared_ptr>> essenConstraints) override; + void fillRedundantConstraints(std::shared_ptr>> redunConstraints) override; void fillqsu(FColDsptr col) override; void fillqsuWeights(std::shared_ptr> diagMat) override; void fillqsulam(FColDsptr col) override; void useEquationNumbers() override; + void setqsu(FColDsptr col) override; void setqsulam(FColDsptr col) override; void postPosICIteration() override; + void fillPosICError(FColDsptr col) override; + void fillPosICJacob(SpMatDsptr mat) override; + void removeRedundantConstraints(std::shared_ptr> redundantEqnNos) override; + void reactivateRedundantConstraints() override; + void constraintsReport() override; + void postPosIC() override; + void outputStates() override; + void preDyn() override; - size_t ipX = -1; - size_t ipE = -1; + int ipX = -1; + int ipE = -1; double m = 0.0; std::shared_ptr> aJ; std::shared_ptr partFrame; diff --git a/MbDCode/PartFrame.cpp b/MbDCode/PartFrame.cpp index 5481e48..3081b34 100644 --- a/MbDCode/PartFrame.cpp +++ b/MbDCode/PartFrame.cpp @@ -1,13 +1,14 @@ #include -#include "Part.h" #include "PartFrame.h" +#include "Part.h" #include "EulerConstraint.h" #include "AbsConstraint.h" #include "MarkerFrame.h" #include "EulerParameters.h" #include "EulerParametersDot.h" #include "CREATE.h" +#include "RedundantConstraint.h" using namespace MbD; @@ -21,7 +22,7 @@ void PartFrame::initialize() { aGeu = CREATE::With(); aGeu->setOwner(this); - aGabs = std::make_shared>>(); + aGabs = std::make_shared>>(); markerFrames = std::make_shared>>(); } @@ -90,7 +91,7 @@ EndFrmcptr PartFrame::endFrame(std::string name) return (*match)->endFrames->at(0); } -void MbD::PartFrame::aGabsDo(const std::function)>& f) +void MbD::PartFrame::aGabsDo(const std::function)>& f) { std::for_each(aGabs->begin(), aGabs->end(), f); } @@ -100,6 +101,55 @@ void MbD::PartFrame::markerFramesDo(const std::functionbegin(), markerFrames->end(), f); } +void MbD::PartFrame::removeRedundantConstraints(std::shared_ptr> redundantEqnNos) +{ + if (std::find(redundantEqnNos->begin(), redundantEqnNos->end(), aGeu->iG) != redundantEqnNos->end()) { + auto redunCon = CREATE::With(); + redunCon->constraint = aGeu; + aGeu = redunCon; + } + for (size_t i = 0; i < aGabs->size(); i++) + { + auto& constraint = aGabs->at(i); + if (std::find(redundantEqnNos->begin(), redundantEqnNos->end(), constraint->iG) != redundantEqnNos->end()) { + auto redunCon = CREATE::With(); + redunCon->constraint = constraint; + aGabs->at(i) = redunCon; + } + } +} + +void MbD::PartFrame::reactivateRedundantConstraints() +{ + if (aGeu->isRedundant()) aGeu = std::dynamic_pointer_cast(aGeu)->constraint; + for (size_t i = 0; i < aGabs->size(); i++) + { + auto& con = aGabs->at(i); + if (con->isRedundant()) { + aGabs->at(i) = std::static_pointer_cast(con)->constraint; + } + } +} + +void MbD::PartFrame::constraintsReport() +{ + auto redunCons = std::make_shared>>(); + aGabsDo([&](std::shared_ptr con) { + if (con->isRedundant()) { + redunCons->push_back(con); + } + }); + if (aGeu->isRedundant()) redunCons->push_back(aGeu); + if (redunCons->size() > 0) { + std::string str = "MbD: " + part->classname() + std::string(" ") + part->getName() + " has the following constraint(s) removed: "; + this->logString(str); + std::for_each(redunCons->begin(), redunCons->end(), [&](auto& con) { + str = "MbD: " + std::string(" ") + std::string(typeid(*con).name()); + this->logString(str); + }); + } +} + void MbD::PartFrame::prePosIC() { iqX = -1; @@ -131,6 +181,10 @@ void MbD::PartFrame::fillEssenConstraints(std::shared_ptr con) { con->fillEssenConstraints(con, essenConstraints); }); } +void MbD::PartFrame::fillRedundantConstraints(std::shared_ptr>> redunConstraints) +{ +} + void MbD::PartFrame::fillqsu(FColDsptr col) { col->atiputFullColumn(iqX, qX); @@ -159,6 +213,15 @@ void MbD::PartFrame::useEquationNumbers() aGabsDo([](std::shared_ptr con) { con->useEquationNumbers(); }); } +void MbD::PartFrame::setqsu(FColDsptr col) +{ + qX->equalFullColumnAt(col, iqX); + qE->equalFullColumnAt(col, iqE); + markerFramesDo([&](std::shared_ptr markerFrame) { markerFrame->setqsu(col); }); + aGeu->setqsu(col); + aGabsDo([&](std::shared_ptr con) { con->setqsu(col); }); +} + void MbD::PartFrame::setqsulam(FColDsptr col) { qX->equalFullColumnAt(col, iqX); @@ -176,10 +239,52 @@ void MbD::PartFrame::postPosICIteration() aGabsDo([](std::shared_ptr con) { con->postPosICIteration(); }); } +void MbD::PartFrame::fillPosICError(FColDsptr col) +{ + markerFramesDo([&](std::shared_ptr markerFrame) { markerFrame->fillPosICError(col); }); + aGeu->fillPosICError(col); + aGabsDo([&](std::shared_ptr con) { con->fillPosICError(col); }); +} + +void MbD::PartFrame::fillPosICJacob(SpMatDsptr mat) +{ + markerFramesDo([&](std::shared_ptr markerFrame) { markerFrame->fillPosICJacob(mat); }); + aGeu->fillPosICJacob(mat); + aGabsDo([&](std::shared_ptr con) { con->fillPosICJacob(mat); }); +} + +void MbD::PartFrame::postPosIC() +{ + markerFramesDo([](std::shared_ptr markerFrame) { markerFrame->postPosIC(); }); + aGeu->postPosIC(); + aGabsDo([](std::shared_ptr con) { con->postPosIC(); }); +} + +void MbD::PartFrame::outputStates() +{ + std::stringstream ss; + ss << "qX = "; + qX->printOn(ss); + ss << std::endl; + ss << "qE = "; + qE->printOn(ss); + auto str = ss.str(); + this->logString(str); + aGeu->outputStates(); + aGabsDo([](std::shared_ptr con) { con->outputStates(); }); +} + +void MbD::PartFrame::preDyn() +{ + markerFramesDo([](std::shared_ptr markerFrame) { markerFrame->preDyn(); }); + aGeu->preDyn(); + aGabsDo([](std::shared_ptr aGab) { aGab->preDyn(); }); +} + void PartFrame::asFixed() { - for (size_t i = 0; i < 6; i++) { - auto con = std::make_shared(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 9f008c8..b0874a5 100644 --- a/MbDCode/PartFrame.h +++ b/MbDCode/PartFrame.h @@ -43,32 +43,42 @@ namespace MbD { Part* getPart(); void addMarkerFrame(std::shared_ptr x); EndFrmcptr endFrame(std::string name); - void aGabsDo(const std::function )>& f); + void aGabsDo(const std::function )>& f); void markerFramesDo(const std::function )>& f); + void removeRedundantConstraints(std::shared_ptr> redundantEqnNos) override; + void reactivateRedundantConstraints() override; + void constraintsReport() override; void prePosIC() override; FColDsptr rOpO(); FMatDsptr aAOp(); FColFMatDsptr pAOppE(); void fillEssenConstraints(std::shared_ptr>> essenConstraints) override; + void fillRedundantConstraints(std::shared_ptr>> redunConstraints) override; void fillqsu(FColDsptr col) override; void fillqsuWeights(std::shared_ptr> diagMat) override; void fillqsulam(FColDsptr col) override; void useEquationNumbers() override; + void setqsu(FColDsptr col) override; void setqsulam(FColDsptr col) override; void postPosICIteration() override; + void fillPosICError(FColDsptr col) override; + void fillPosICJacob(SpMatDsptr mat) override; + void postPosIC() override; + void outputStates() override; + void preDyn() override; Part* part = nullptr; //Use raw pointer when pointing backwards. - size_t iqX = -1; - size_t iqE = -1; //Position index of frame variables qX and qE in system list of variables + 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); - std::shared_ptr aGeu; - std::shared_ptr>> aGabs; + std::shared_ptr aGeu; + std::shared_ptr>> aGabs; std::shared_ptr>> markerFrames; }; } diff --git a/MbDCode/PosICKineNewtonRaphson.cpp b/MbDCode/PosICKineNewtonRaphson.cpp index c8bae70..984234c 100644 --- a/MbDCode/PosICKineNewtonRaphson.cpp +++ b/MbDCode/PosICKineNewtonRaphson.cpp @@ -1,7 +1,15 @@ #include "PosICKineNewtonRaphson.h" +#include "SystemSolver.h" using namespace MbD; +void MbD::PosICKineNewtonRaphson::initializeGlobally() +{ + AnyPosICNewtonRaphson::initializeGlobally(); + iterMax = system->iterMaxPosKine; + dxTol = system->errorTolPosKine; +} + void MbD::PosICKineNewtonRaphson::assignEquationNumbers() { } diff --git a/MbDCode/PosICKineNewtonRaphson.h b/MbDCode/PosICKineNewtonRaphson.h index 0e62b52..dddfd80 100644 --- a/MbDCode/PosICKineNewtonRaphson.h +++ b/MbDCode/PosICKineNewtonRaphson.h @@ -7,6 +7,7 @@ namespace MbD { { // public: + void initializeGlobally() override; void assignEquationNumbers() override; }; diff --git a/MbDCode/PosICNewtonRaphson.cpp b/MbDCode/PosICNewtonRaphson.cpp index c68597b..54f6c8e 100644 --- a/MbDCode/PosICNewtonRaphson.cpp +++ b/MbDCode/PosICNewtonRaphson.cpp @@ -6,6 +6,9 @@ #include "SystemSolver.h" #include "Part.h" #include "Constraint.h" +#include "CREATE.h" +#include "GESpMatParPvPrecise.h" +#include "GESpMatFullPvPosIC.h" using namespace MbD; @@ -40,7 +43,7 @@ void MbD::PosICNewtonRaphson::assignEquationNumbers() auto essentialConstraints = system->essentialConstraints2(); auto displacementConstraints = system->displacementConstraints(); auto perpendicularConstraints = system->perpendicularConstraints2(); - size_t eqnNo = 0; + int eqnNo = 0; for (auto& part : *parts) { part->iqX(eqnNo); eqnNo = eqnNo + 3; @@ -74,8 +77,8 @@ void MbD::PosICNewtonRaphson::assignEquationNumbers() auto lastEqnNo = eqnNo - 1; nEqns = eqnNo; //C++ uses index 0. n = nEqns; - auto limits = { lastEssenConEqnNo, lastDispConEqnNo, lastEqnNo }; - pivotRowLimits = std::make_shared>(limits); + auto rangelimits = { lastEssenConEqnNo + 1, lastDispConEqnNo + 1, lastEqnNo + 1 }; + pivotRowLimits = std::make_shared>(rangelimits); } bool MbD::PosICNewtonRaphson::isConverged() @@ -85,5 +88,35 @@ bool MbD::PosICNewtonRaphson::isConverged() void MbD::PosICNewtonRaphson::handleSingularMatrix() { - assert(false); + nSingularMatrixError++; + if (nSingularMatrixError = 1){ + this->lookForRedundantConstraints(); + matrixSolver = this->matrixSolverClassNew(); + } + else { + 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->lookForRedundantConstraints(); + matrixSolver = this->matrixSolverClassNew(); + } + else { + assert(false); + } + } + } +} + +void MbD::PosICNewtonRaphson::lookForRedundantConstraints() +{ + std::string str("MbD: Checking for redundant constraints."); + system->logString(str); + auto posICsolver = CREATE::With(); + posICsolver->system = this; + dx = posICsolver->solvewithsaveOriginal(pypx, y->negated(), false); } diff --git a/MbDCode/PosICNewtonRaphson.h b/MbDCode/PosICNewtonRaphson.h index a0b3849..3ff3b54 100644 --- a/MbDCode/PosICNewtonRaphson.h +++ b/MbDCode/PosICNewtonRaphson.h @@ -14,8 +14,9 @@ namespace MbD { void assignEquationNumbers() override; bool isConverged() override; void handleSingularMatrix() override; + void lookForRedundantConstraints(); - std::shared_ptr> pivotRowLimits; + std::shared_ptr> pivotRowLimits; }; } diff --git a/MbDCode/PosKineNewtonRaphson.cpp b/MbDCode/PosKineNewtonRaphson.cpp index 665169c..62f5fbe 100644 --- a/MbDCode/PosKineNewtonRaphson.cpp +++ b/MbDCode/PosKineNewtonRaphson.cpp @@ -1,3 +1,12 @@ #include "PosKineNewtonRaphson.h" +#include "SystemSolver.h" using namespace MbD; + +void MbD::PosKineNewtonRaphson::initializeGlobally() +{ + SystemNewtonRaphson::initializeGlobally(); + system->partsJointsMotionsDo([&](std::shared_ptr item) { item->fillqsu(x); }); + iterMax = system->iterMaxPosKine; + dxTol = system->errorTolPosKine; +} diff --git a/MbDCode/PosKineNewtonRaphson.h b/MbDCode/PosKineNewtonRaphson.h index 28d9248..97adffc 100644 --- a/MbDCode/PosKineNewtonRaphson.h +++ b/MbDCode/PosKineNewtonRaphson.h @@ -7,7 +7,7 @@ namespace MbD { { // public: - + void initializeGlobally() override; }; } diff --git a/MbDCode/PosNewtonRaphson.cpp b/MbDCode/PosNewtonRaphson.cpp index af7aba0..9f2d608 100644 --- a/MbDCode/PosNewtonRaphson.cpp +++ b/MbDCode/PosNewtonRaphson.cpp @@ -38,3 +38,8 @@ void MbD::PosNewtonRaphson::askSystemToUpdate() { system->partsJointsMotionsDo([&](std::shared_ptr item) { item->postPosICIteration(); }); } + +void MbD::PosNewtonRaphson::postRun() +{ + system->partsJointsMotionsDo([&](std::shared_ptr item) { item->postPosIC(); }); +} diff --git a/MbDCode/PosNewtonRaphson.h b/MbDCode/PosNewtonRaphson.h index 8963a05..ed581ec 100644 --- a/MbDCode/PosNewtonRaphson.h +++ b/MbDCode/PosNewtonRaphson.h @@ -10,6 +10,7 @@ namespace MbD { void preRun() override; void incrementIterNo() override; void askSystemToUpdate() override; + virtual void postRun() override; }; } diff --git a/MbDCode/Product.cpp b/MbDCode/Product.cpp index 4026ee3..e27cb4c 100644 --- a/MbDCode/Product.cpp +++ b/MbDCode/Product.cpp @@ -19,7 +19,7 @@ Symsptr MbD::Product::differentiateWRT(Symsptr sptr, Symsptr var) [var](Symsptr term) { return term->differentiateWRT(term, var); } ); auto derivativeTerms = std::make_shared>(); - for (size_t i = 0; i < terms->size(); i++) + for (int i = 0; i < terms->size(); i++) { auto& derivative = derivatives->at(i); auto newTermFunctions = std::make_shared>(*terms); @@ -140,7 +140,7 @@ std::ostream& MbD::Product::printOn(std::ostream& s) const { s << "("; s << *(this->terms->at(0)); - for (size_t i = 1; i < this->terms->size(); i++) + for (int i = 1; i < this->terms->size(); i++) { s << "*" << *(this->terms->at(i)); } @@ -156,6 +156,6 @@ bool MbD::Product::isProduct() double Product::getValue() { double answer = 1.0; - for (size_t i = 0; i < terms->size(); i++) answer *= terms->at(i)->getValue(); + for (int i = 0; i < terms->size(); i++) answer *= terms->at(i)->getValue(); return answer; } diff --git a/MbDCode/QuasiIntegrator.cpp b/MbDCode/QuasiIntegrator.cpp index 12bb79a..2ca5a60 100644 --- a/MbDCode/QuasiIntegrator.cpp +++ b/MbDCode/QuasiIntegrator.cpp @@ -1,3 +1,90 @@ +#include + #include "QuasiIntegrator.h" +#include "Item.h" +#include "SystemSolver.h" +#include "CREATE.h" +#include "BasicQuasiIntegrator.h" +#include "SingularMatrixError.h" +#include "SimulationStoppingError.h" +#include "TooSmallStepSizeError.h" +#include "TooManyTriesError.h" +#include "SingularMatrixError.h" using namespace MbD; + +void MbD::QuasiIntegrator::preRun() +{ + system->partsJointsMotionsForcesTorquesDo([](std::shared_ptr item) { item->preDyn(); }); +} + +void MbD::QuasiIntegrator::initialize() +{ + Solver::initialize(); + integrator = CREATE::With(); + integrator->setSystem(this); +} + +void MbD::QuasiIntegrator::run() +{ + try { + try { + try { + IntegratorInterface::run(); + } + catch (SingularMatrixError ex) { + std::stringstream ss; + ss << "MbD: Solver has encountered a singular matrix." << std::endl; + ss << "MbD: Check to see if a massless or a very low mass part is under constrained." << std::endl; + ss << "MbD: Check to see if the system is in a locked position." << std::endl; + ss << "MbD: Check to see if the error tolerance is too demanding." << std::endl; + ss << "MbD: Check to see if a curve-curve is about to have multiple contact points." << std::endl; + auto str = ss.str(); + this->logString(str); + throw SimulationStoppingError(""); + } + } + catch (TooSmallStepSizeError ex) { + std::stringstream ss; + ss << "MbD: Step size is prevented from going below the user specified minimum." << std::endl; + ss << "MbD: Check to see if the system is in a locked position." << std::endl; + ss << "MbD: Check to see if a curve-curve is about to have multiple contact points." << std::endl; + ss << "MbD: If they are not, lower the permitted minimum step size." << std::endl; + auto str = ss.str(); + this->logString(str); + throw SimulationStoppingError(""); + } + } + catch (TooManyTriesError ex) { + std::stringstream ss; + ss << "MbD: Check to see if the error tolerance is too demanding." << std::endl; + auto str = ss.str(); + this->logString(str); + throw SimulationStoppingError(""); + } + +} + +void MbD::QuasiIntegrator::preFirstStep() +{ + system->partsJointsMotionsForcesTorquesDo([](std::shared_ptr item) { item->preDynFirstStep(); }); +} + +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."; + this->logString(str); + } + if (hnew2 < hmin) { + std::stringstream ss; + ss << "StM: Step size " << hnew2 << " < " << hmin << " user specified minimum."; + auto str = ss.str(); + this->logString(str); + throw TooSmallStepSizeError(""); + } + return hnew2; +} diff --git a/MbDCode/QuasiIntegrator.h b/MbDCode/QuasiIntegrator.h index cb221fa..4fcde8f 100644 --- a/MbDCode/QuasiIntegrator.h +++ b/MbDCode/QuasiIntegrator.h @@ -7,7 +7,11 @@ namespace MbD { { // public: - + void preRun() override; + void initialize() override; + void run() override; + void preFirstStep(); + double suggestSmallerOrAcceptFirstStepSize(double hnew) override; }; } diff --git a/MbDCode/RedundantConstraint.cpp b/MbDCode/RedundantConstraint.cpp index 111f097..1f2320f 100644 --- a/MbDCode/RedundantConstraint.cpp +++ b/MbDCode/RedundantConstraint.cpp @@ -1,3 +1,60 @@ #include "RedundantConstraint.h" using namespace MbD; + +void MbD::RedundantConstraint::removeRedundantConstraints(std::shared_ptr> redundantEqnNos) +{ +} + +bool MbD::RedundantConstraint::isRedundant() +{ + return true; +} + +std::string MbD::RedundantConstraint::classname() +{ + auto str = Item::classname() + "->" + constraint->classname(); + return str; +} + +MbD::ConstraintType MbD::RedundantConstraint::type() +{ + return MbD::redundant; +} + +void MbD::RedundantConstraint::fillqsulam(FColDsptr col) +{ +} + +void MbD::RedundantConstraint::postInput() +{ +} + +void MbD::RedundantConstraint::prePosIC() +{ +} + +void MbD::RedundantConstraint::fillEssenConstraints(std::shared_ptr sptr, std::shared_ptr>> essenConstraints) +{ +} + +void MbD::RedundantConstraint::fillDispConstraints(std::shared_ptr sptr, std::shared_ptr>> dispConstraints) +{ +} + +void MbD::RedundantConstraint::fillPerpenConstraints(std::shared_ptr sptr, std::shared_ptr>> perpenConstraints) +{ +} + +void MbD::RedundantConstraint::fillRedundantConstraints(std::shared_ptr sptr, std::shared_ptr>> redunConstraints) +{ + redunConstraints->push_back(sptr); +} + +void MbD::RedundantConstraint::setqsulam(FColDsptr col) +{ +} + +void MbD::RedundantConstraint::fillPosICError(FColDsptr col) +{ +} diff --git a/MbDCode/RedundantConstraint.h b/MbDCode/RedundantConstraint.h index 9c25ddf..e7c3b40 100644 --- a/MbDCode/RedundantConstraint.h +++ b/MbDCode/RedundantConstraint.h @@ -1,13 +1,27 @@ #pragma once -#include "Item.h" +#include "Constraint.h" namespace MbD { - class RedundantConstraint : public Item - { - // - public: - std::shared_ptr constraint; - }; + class RedundantConstraint : public Constraint + { + // + public: + void removeRedundantConstraints(std::shared_ptr> redundantEqnNos) override; + bool isRedundant() override; + std::string classname() override; + MbD::ConstraintType type() override; + void fillqsulam(FColDsptr col) override; + void postInput() override; + void prePosIC() override; + 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 setqsulam(FColDsptr col) override; + void fillPosICError(FColDsptr col) override; + + std::shared_ptr constraint; + }; } diff --git a/MbDCode/RowTypeMatrix.h b/MbDCode/RowTypeMatrix.h index 42c3be9..e700425 100644 --- a/MbDCode/RowTypeMatrix.h +++ b/MbDCode/RowTypeMatrix.h @@ -10,29 +10,28 @@ namespace MbD { { public: RowTypeMatrix() {} - RowTypeMatrix(size_t m) : Array(m) {} + RowTypeMatrix(int m) : Array(m) {} RowTypeMatrix(std::initializer_list list) : Array{ list } {} void copyFrom(std::shared_ptr> x); virtual void zeroSelf() = 0; - virtual void atijplusNumber(size_t i, size_t j, double value) = 0; - size_t nrow() { - return this->size(); + int nrow() { + return (int) this->size(); } - size_t ncol() { - return this->at(0)->size(); + int ncol() { + return this->at(0)->numberOfElements(); } - size_t numberOfElements() override; + int numberOfElements() override; }; template inline void RowTypeMatrix::copyFrom(std::shared_ptr> x) { - for (size_t i = 0; i < x->size(); i++) { + for (int i = 0; i < x->size(); i++) { this->at(i)->copyFrom(x->at(i)); } } template - inline size_t RowTypeMatrix::numberOfElements() + inline int RowTypeMatrix::numberOfElements() { return this->nrow() * this->ncol(); } diff --git a/MbDCode/ScalarNewtonRaphson.cpp b/MbDCode/ScalarNewtonRaphson.cpp index 4be5730..b0b5b2b 100644 --- a/MbDCode/ScalarNewtonRaphson.cpp +++ b/MbDCode/ScalarNewtonRaphson.cpp @@ -1,7 +1,14 @@ #include "ScalarNewtonRaphson.h" +#include "SystemSolver.h" using namespace MbD; +void MbD::ScalarNewtonRaphson::initializeGlobally() +{ + assert(false); + //x = system->x; +} + void MbD::ScalarNewtonRaphson::calcyNorm() { yNorm = 0.5 * y * y; diff --git a/MbDCode/ScalarNewtonRaphson.h b/MbDCode/ScalarNewtonRaphson.h index 121301b..604687b 100644 --- a/MbDCode/ScalarNewtonRaphson.h +++ b/MbDCode/ScalarNewtonRaphson.h @@ -7,6 +7,7 @@ namespace MbD { { // public: + void initializeGlobally() override; void calcyNorm() override; void solveEquations() override; void updatexold() override; diff --git a/MbDCode/SingularMatrixError.h b/MbDCode/SingularMatrixError.h index e6ca3f3..c4c70a5 100644 --- a/MbDCode/SingularMatrixError.h +++ b/MbDCode/SingularMatrixError.h @@ -3,16 +3,18 @@ #include #include +#include "FullColumn.h" + namespace MbD { class SingularMatrixError : virtual public std::runtime_error { protected: - std::shared_ptr> redundantEqnNos; + std::shared_ptr> redundantEqnNos; public: explicit - SingularMatrixError(const std::string& msg, std::shared_ptr> redunEqnNos) : + SingularMatrixError(const std::string& msg, std::shared_ptr> redunEqnNos) : std::runtime_error(msg), redundantEqnNos(redunEqnNos) { } @@ -22,7 +24,7 @@ namespace MbD { virtual ~SingularMatrixError() noexcept {} - virtual std::shared_ptr> getRedundantEqnNos() const noexcept { + virtual std::shared_ptr> getRedundantEqnNos() const noexcept { return redundantEqnNos; } }; diff --git a/MbDCode/Solver.cpp b/MbDCode/Solver.cpp index b1bcbcd..e0995f1 100644 --- a/MbDCode/Solver.cpp +++ b/MbDCode/Solver.cpp @@ -1,6 +1,7 @@ #include #include "Solver.h" +#include using namespace MbD; @@ -10,7 +11,6 @@ void MbD::Solver::initialize() void MbD::Solver::initializeLocally() { - assert(false); } void MbD::Solver::initializeGlobally() @@ -42,3 +42,8 @@ void MbD::Solver::postRun() { assert(false); } + +void MbD::Solver::logString(std::string& str) +{ + assert(false); +} diff --git a/MbDCode/Solver.h b/MbDCode/Solver.h index b5f7370..2e21f96 100644 --- a/MbDCode/Solver.h +++ b/MbDCode/Solver.h @@ -1,4 +1,7 @@ #pragma once + +#include + namespace MbD { class Solver { @@ -12,6 +15,7 @@ namespace MbD { virtual void finalize(); virtual void reportStats(); virtual void postRun(); + virtual void logString(std::string& str); }; } diff --git a/MbDCode/SparseColumn.h b/MbDCode/SparseColumn.h index c1be9ad..11e7b6c 100644 --- a/MbDCode/SparseColumn.h +++ b/MbDCode/SparseColumn.h @@ -7,7 +7,7 @@ namespace MbD { { public: SparseColumn() {} - SparseColumn(std::initializer_list> list) : SparseVector{ list } {} + SparseColumn(std::initializer_list> list) : SparseVector{ list } {} SparseColumn(std::initializer_list> list) : SparseVector{ list } {} }; } diff --git a/MbDCode/SparseMatrix.h b/MbDCode/SparseMatrix.h index 91f240a..9c795e8 100644 --- a/MbDCode/SparseMatrix.h +++ b/MbDCode/SparseMatrix.h @@ -1,4 +1,6 @@ #pragma once +#include + #include "RowTypeMatrix.h" #include "SparseRow.h" #include "DiagonalMatrix.h" @@ -8,11 +10,11 @@ namespace MbD { class SparseMatrix : public RowTypeMatrix>> { public: - SparseMatrix(size_t m) : RowTypeMatrix>>(m) + SparseMatrix(int m) : RowTypeMatrix>>(m) { } - SparseMatrix(size_t m, size_t n) { - for (size_t i = 0; i < m; i++) + SparseMatrix(int m, int n) { + for (int i = 0; i < m; i++) { auto row = std::make_shared>(n); this->push_back(row); @@ -25,42 +27,95 @@ namespace MbD { this->push_back(row); } } - void atijminusDiagonalMatrix(size_t i, size_t j, std::shared_ptr> diagMat); + void atijminusDiagonalMatrix(int i, int j, std::shared_ptr> diagMat); double sumOfSquares() override; - void atijplusNumber(size_t i, size_t j, double value); void zeroSelf() override; + void atijplusFullRow(int i, int j, std::shared_ptr> fullRow); + void atijplusFullColumn(int i, int j, std::shared_ptr> fullCol); + void atijplusFullMatrix(int i, int j, std::shared_ptr> fullMat); + void atijplusTransposeFullMatrix(int i, int j, std::shared_ptr> fullMat); + void atijplusFullMatrixtimes(int i, int j, std::shared_ptr> fullMat, T factor); + + virtual std::ostream& printOn(std::ostream& s) const; + friend std::ostream& operator<<(std::ostream& s, const SparseMatrix& spMat) + { + return spMat.printOn(s); + } }; using SpMatDsptr = std::shared_ptr>; template<> - inline void SparseMatrix::atijminusDiagonalMatrix(size_t i1, size_t j1, std::shared_ptr> diagMat) + inline void SparseMatrix::atijminusDiagonalMatrix(int i1, int j1, std::shared_ptr> diagMat) { auto n = diagMat->nrow(); - for (size_t ii = 0; ii < n; ii++) + for (int ii = 0; ii < n; ii++) { - this->at(i1 + ii)->atiminusNumber(j1 + ii, diagMat->at(ii)); + (*(this->at(i1 + ii)))[j1 + ii] -= diagMat->at(ii); } } template inline double SparseMatrix::sumOfSquares() { double sum = 0.0; - for (size_t i = 0; i < this->size(); i++) + for (int i = 0; i < this->size(); i++) { sum += this->at(i)->sumOfSquares(); } return sum; } template<> - inline void SparseMatrix::atijplusNumber(size_t i, size_t j, double value) - { - this->at(i)->atiplusNumber(j, value); - } - template<> inline void SparseMatrix::zeroSelf() { - for (size_t i = 0; i < this->size(); i++) { + for (int i = 0; i < this->size(); i++) { this->at(i)->zeroSelf(); } } + template + inline void SparseMatrix::atijplusFullRow(int i, int j, std::shared_ptr> fullRow) + { + this->at(i)->atiplusFullRow(j, fullRow); + } + template + inline void SparseMatrix::atijplusFullColumn(int i, int j, std::shared_ptr> fullCol) + { + for (int ii = 0; ii < fullCol->size(); ii++) + { + (*(this->at(i + ii)))[j] += fullCol->at(ii); + } + } + template + inline void SparseMatrix::atijplusFullMatrix(int i, int j, std::shared_ptr> fullMat) + { + for (int ii = 0; ii < fullMat->nrow(); ii++) + { + this->at(i + ii)->atiplusFullRow(j, fullMat->at(ii)); + } + } + template + inline void SparseMatrix::atijplusTransposeFullMatrix(int i, int j, std::shared_ptr> fullMat) + { + for (int ii = 0; ii < fullMat->nrow(); ii++) + { + this->atijplusFullColumn(i, j + ii, fullMat->at(ii)->transpose()); + } + } + template + inline void SparseMatrix::atijplusFullMatrixtimes(int i, int j, std::shared_ptr> fullMat, T factor) + { + for (int ii = 0; ii < fullMat->nrow(); ii++) + { + this->at(i + ii)->atiplusFullRowtimes(j, fullMat->at(ii), factor); + } + } + template + inline std::ostream& SparseMatrix::printOn(std::ostream& s) const + { + s << "SpMat[" << std::endl; + for (int i = 0; i < this->size(); i++) + { + s << *(this->at(i)) << std::endl; + } + s << "]" << std::endl; + return s; + } } \ No newline at end of file diff --git a/MbDCode/SparseRow.h b/MbDCode/SparseRow.h index 3d92800..e29d230 100644 --- a/MbDCode/SparseRow.h +++ b/MbDCode/SparseRow.h @@ -3,6 +3,7 @@ #include #include "SparseVector.h" +#include "FullRow.h" namespace MbD { template @@ -10,16 +11,19 @@ namespace MbD { { public: SparseRow(){} - SparseRow(size_t n) : SparseVector(n) {} - SparseRow(std::initializer_list> list) : SparseVector{ list } {} + SparseRow(int n) : SparseVector(n) {} + SparseRow(std::initializer_list> list) : SparseVector{ list } {} SparseRow(std::initializer_list> list) : SparseVector{ list } {} std::shared_ptr> timesconditionedWithTol(double scaling, double tol); + std::shared_ptr> conditionedWithTol(double tol); + void atiplusFullRow(int j, std::shared_ptr> fullRow); + void atiplusFullRowtimes(int j, std::shared_ptr> fullRow, double factor); }; using SpRowDsptr = std::shared_ptr>; template<> inline std::shared_ptr> SparseRow::timesconditionedWithTol(double scaling, double tol) { - auto answer = std::make_shared>(this->size()); + auto answer = std::make_shared>(this->numberOfElements()); for (auto const& keyValue : *this) { auto val = keyValue.second * scaling; @@ -27,5 +31,32 @@ namespace MbD { } return answer; } + template<> + inline std::shared_ptr> SparseRow::conditionedWithTol(double tol) + { + auto answer = std::make_shared>(this->numberOfElements()); + for (auto const& keyValue : *this) + { + auto val = keyValue.second; + if (std::abs(val) >= tol) (*answer)[keyValue.first] = val; + } + return answer; + } + template + inline void SparseRow::atiplusFullRow(int j, std::shared_ptr> fullRow) + { + for (int jj = 0; jj < fullRow->size(); jj++) + { + (*this)[j + jj] += fullRow->at(jj); + } + } + template + inline void SparseRow::atiplusFullRowtimes(int j, std::shared_ptr> fullRow, double factor) + { + for (int jj = 0; jj < fullRow->size(); jj++) + { + (*this)[j + jj] += fullRow->at(jj) * factor; + } + } } diff --git a/MbDCode/SparseVector.h b/MbDCode/SparseVector.h index c08182f..46dda49 100644 --- a/MbDCode/SparseVector.h +++ b/MbDCode/SparseVector.h @@ -1,51 +1,50 @@ #pragma once #include #include +#include namespace MbD { template - class SparseVector : public std::map + class SparseVector : public std::map { public: - size_t n; + int n; SparseVector() {} - SparseVector(size_t n) : std::map(), n(n) {} - SparseVector(std::initializer_list> list) : std::map{ list } {} + SparseVector(int n) : std::map(), n(n) {} + SparseVector(std::initializer_list> list) : std::map{ list } {} SparseVector(std::initializer_list> list) { for (auto& pair : list) { - size_t i = 0; - size_t index; + int i = 0; + int index; T value; for (auto& element : pair) { - if (i == 0) index = (size_t)std::round(element); ; + if (i == 0) index = (int)std::round(element); ; if (i == 1) value = element; i++; } - this->insert(std::pair(index, value)); + this->insert(std::pair(index, value)); } } - void atiminusNumber(size_t i, double value); double rootMeanSquare(); - size_t numberOfElements(); + int numberOfElements(); double sumOfSquares(); - void atiplusNumber(size_t i, double value); + void atiplusNumber(int i, double value); void zeroSelf(); double maxElement(); + + virtual std::ostream& printOn(std::ostream& s) const; + friend std::ostream& operator<<(std::ostream& s, const SparseVector& spVec) + { + return spVec.printOn(s); + } }; - template<> - inline void SparseVector::atiminusNumber(size_t i, double value) - { - //auto val = this->at(i); - auto val = (*this)[i]; - this->at(i) = val - value; - } template inline double SparseVector::rootMeanSquare() { return std::sqrt(this->sumOfSquares() / this->numberOfElements()); } template - inline size_t SparseVector::numberOfElements() + inline int SparseVector::numberOfElements() { return n; } @@ -60,7 +59,7 @@ namespace MbD { return sum; } template<> - inline void SparseVector::atiplusNumber(size_t i, double value) + inline void SparseVector::atiplusNumber(int i, double value) { this->at(i) += value; } @@ -80,5 +79,20 @@ namespace MbD { } return max; } + template + inline std::ostream& SparseVector::printOn(std::ostream& s) const + { + s << "{"; + auto index = 0; + for (const auto& keyValue : *this) { + if (index > 0) s << ", "; + s << keyValue.first; + s << "->"; + s << keyValue.second; + index++; + } + s << "}"; + return s; + } } diff --git a/MbDCode/StableBackwardDifference.cpp b/MbDCode/StableBackwardDifference.cpp new file mode 100644 index 0000000..efe3e78 --- /dev/null +++ b/MbDCode/StableBackwardDifference.cpp @@ -0,0 +1,28 @@ +#include "StableBackwardDifference.h" + +void MbD::StableBackwardDifference::formTaylorMatrix() +{ + //This form is numerically more stable and is prefered over the full Taylor Matrix. + //For method order 3: + //| (t1 - t) (t1 - t) ^ 2 / 2! (t1 - t) ^ 3 / 3!| |qd(t) | = | q(t1) - q(t) | + //| (t2 - t) (t2 - t) ^ 2 / 2! (t2 - t) ^ 3 / 3!| |qdd(t) | |q(t2) - q(t) | + //| (t3 - t) (t3 - t) ^ 2 / 2! (t3 - t) ^ 3 / 3!| |qddd(t)| |q(t3) - q(t) | + + this->instanciateTaylorMatrix(); + for (int i = 0; i < order; i++) + { + this->formTaylorRowwithTimeNodederivative(i, i, 0); + } +} + +void MbD::StableBackwardDifference::instanciateTaylorMatrix() +{ + if (taylorMatrix->empty() || (taylorMatrix->nrow() != (order))) { + taylorMatrix = std::make_shared>(order, order); + } +} + +void MbD::StableBackwardDifference::formTaylorRowwithTimeNodederivative(int i, int ii, int k) +{ + assert(false); +} diff --git a/MbDCode/StableBackwardDifference.h b/MbDCode/StableBackwardDifference.h new file mode 100644 index 0000000..e76069f --- /dev/null +++ b/MbDCode/StableBackwardDifference.h @@ -0,0 +1,15 @@ +#pragma once + +#include "LinearMultiStepMethod.h" + +namespace MbD { + class StableBackwardDifference : public LinearMultiStepMethod + { + // + public: + void formTaylorMatrix() override; + void instanciateTaylorMatrix() override; + void formTaylorRowwithTimeNodederivative(int i, int ii, int k) override; + }; +} + diff --git a/MbDCode/Sum.cpp b/MbDCode/Sum.cpp index 386b831..dc72d46 100644 --- a/MbDCode/Sum.cpp +++ b/MbDCode/Sum.cpp @@ -108,7 +108,7 @@ bool MbD::Sum::isSum() double Sum::getValue() { double answer = 0.0; - for (size_t i = 0; i < terms->size(); i++) answer += terms->at(i)->getValue(); + for (int i = 0; i < terms->size(); i++) answer += terms->at(i)->getValue(); return answer; } @@ -116,7 +116,7 @@ std::ostream& MbD::Sum::printOn(std::ostream& s) const { s << "("; s << *(this->terms->at(0)); - for (size_t i = 1; i < this->terms->size(); i++) + for (int i = 1; i < this->terms->size(); i++) { s << " + " << *(this->terms->at(i)); } diff --git a/MbDCode/Symbolic.cpp b/MbDCode/Symbolic.cpp index 7da7cee..ac6a951 100644 --- a/MbDCode/Symbolic.cpp +++ b/MbDCode/Symbolic.cpp @@ -25,13 +25,13 @@ Symsptr Symbolic::differentiateWRT(Symsptr sptr, Symsptr var) Symsptr Symbolic::simplified(Symsptr sptr) { - std::cout << "sptr " << *sptr << std::endl; + //std::cout << "sptr " << *sptr << std::endl; auto set = std::make_shared>(); auto expanded = sptr->expandUntil(sptr, set); - std::cout << "expanded " << *expanded << std::endl; + //std::cout << "expanded " << *expanded << std::endl; auto set1 = std::make_shared>(); auto simplified = expanded->simplifyUntil(expanded, set1); - std::cout << "simplified " << *simplified << std::endl; + //std::cout << "simplified " << *simplified << std::endl; return simplified; } diff --git a/MbDCode/System.cpp b/MbDCode/System.cpp index 6850ad3..280f39b 100644 --- a/MbDCode/System.cpp +++ b/MbDCode/System.cpp @@ -55,6 +55,9 @@ void MbD::System::outputInput() void MbD::System::outputInitialConditions() { + auto str = std::to_string(time->value); + this->logString(str); + partsJointsMotionsDo([](std::shared_ptr item) { item->outputStates(); }); } void MbD::System::outputTimeSeries() @@ -65,13 +68,13 @@ void System::initializeLocally() { hasChanged = false; time->value = systemSolver->tstart; - partsJointsMotionsDo([](std::shared_ptr item) { item->initializeLocally(); }); + partsJointsMotionsForcesTorquesDo([](std::shared_ptr item) { item->initializeLocally(); }); systemSolver->initializeLocally(); } void System::initializeGlobally() { - partsJointsMotionsDo([](std::shared_ptr item) { item->initializeGlobally(); }); + partsJointsMotionsForcesTorquesDo([](std::shared_ptr item) { item->initializeGlobally(); }); systemSolver->initializeGlobally(); } @@ -129,6 +132,13 @@ std::shared_ptr>> MbD::System::perpendic return perpenConstraints; } +std::shared_ptr>> MbD::System::allRedundantConstraints() +{ + auto redunConstraints = std::make_shared>>(); + this->partsJointsMotionsDo([&](std::shared_ptr item) { item->fillRedundantConstraints(redunConstraints); }); + return redunConstraints; +} + double MbD::System::maximumMass() { auto maxPart = std::max_element(parts->begin(), parts->end(), [](auto& a, auto& b) { return a->m < b->m; }); @@ -138,12 +148,12 @@ double MbD::System::maximumMass() double MbD::System::maximumMomentOfInertia() { double max = 0.0; - for (size_t i = 0; i < parts->size(); i++) + for (int i = 0; i < parts->size(); i++) { - auto part = parts->at(i); - for (size_t j = 0; j < 3; j++) + auto& part = parts->at(i); + for (int j = 0; j < 3; j++) { - auto aJ = part->aJ; + auto& aJ = part->aJ; auto aJi = aJ->at(j); if (max < aJi) max = aJi; } diff --git a/MbDCode/System.h b/MbDCode/System.h index 54ebbc9..bc5aba9 100644 --- a/MbDCode/System.h +++ b/MbDCode/System.h @@ -43,11 +43,13 @@ namespace MbD { void jointsMotionsDo(const std::function )>& f); void partsJointsMotionsDo(const std::function )>& f); void partsJointsMotionsForcesTorquesDo(const std::function )>& f); - void logString(std::string& str); + void logString(std::string& str) override; double mbdTimeValue(); std::shared_ptr>> essentialConstraints2(); std::shared_ptr>> displacementConstraints(); std::shared_ptr>> perpendicularConstraints2(); + std::shared_ptr>> allRedundantConstraints(); + double maximumMass(); double maximumMomentOfInertia(); diff --git a/MbDCode/SystemNewtonRaphson.cpp b/MbDCode/SystemNewtonRaphson.cpp index d011140..89919ea 100644 --- a/MbDCode/SystemNewtonRaphson.cpp +++ b/MbDCode/SystemNewtonRaphson.cpp @@ -4,14 +4,14 @@ #include "MatrixSolver.h" #include "GESpMatParPvMarkoFast.h" #include "CREATE.h" +#include "GESpMatParPvPrecise.h" using namespace MbD; void MbD::SystemNewtonRaphson::initializeGlobally() { this->assignEquationNumbers(); - system->partsJointsMotionsDo([&](std::shared_ptr item) { item->useEquationNumbers(); }); - + system->partsJointsMotionsForcesTorquesDo([&](std::shared_ptr item) { item->useEquationNumbers(); }); this->createVectorsAndMatrices(); matrixSolver = this->matrixSolverClassNew(); } @@ -40,3 +40,23 @@ void MbD::SystemNewtonRaphson::basicSolveEquations() { dx = matrixSolver->solvewithsaveOriginal(pypx, y->negated(), false); } + +void MbD::SystemNewtonRaphson::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") { + str = "MbD: Singular Matrix Error. "; + system->logString(str); + matrixSolver = this->matrixSolverClassNew(); + } + else { + assert(false); + } + } +} diff --git a/MbDCode/SystemNewtonRaphson.h b/MbDCode/SystemNewtonRaphson.h index 3bb7f8d..415a009 100644 --- a/MbDCode/SystemNewtonRaphson.h +++ b/MbDCode/SystemNewtonRaphson.h @@ -16,6 +16,7 @@ namespace MbD { std::shared_ptr matrixSolverClassNew() override; void calcdxNorm() override; void basicSolveEquations() override; + void handleSingularMatrix() override; std::shared_ptr> pypx; }; diff --git a/MbDCode/SystemSolver.cpp b/MbDCode/SystemSolver.cpp index 6bdb392..6579f19 100644 --- a/MbDCode/SystemSolver.cpp +++ b/MbDCode/SystemSolver.cpp @@ -1,9 +1,16 @@ #include +#include +#include #include "SystemSolver.h" #include "NewtonRaphson.h" #include "PosICNewtonRaphson.h" #include "CREATE.h" +#include "RedundantConstraint.h" +#include "NotKinematicError.h" +#include "ICKineIntegrator.h" +#include "KineIntegrator.h" +#include "DiscontinuityError.h" using namespace MbD; @@ -16,7 +23,7 @@ void MbD::SystemSolver::initialize() void SystemSolver::initializeLocally() { - setsOfRedundantConstraints = std::make_shared>>>(); + setsOfRedundantConstraints = std::make_shared>>>(); direction = (tstart < tend) ? 1.0 : -1.0; toutFirst = tstart + (direction * hout); } @@ -36,16 +43,17 @@ void SystemSolver::runAllIC() { runPosIC(); } - runVelIC(); - runAccIC(); - auto discontinuities = system->discontinuitiesAtIC(); - if (discontinuities->size() == 0) break; - if (std::find(discontinuities->begin(), discontinuities->end(), "REBOUND") != discontinuities->end()) - { - preCollision(); - runCollisionDerivativeIC(); - runBasicCollision(); - } + 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(); + //} } } @@ -66,7 +74,41 @@ void MbD::SystemSolver::runAccIC() bool MbD::SystemSolver::needToRedoPosIC() { - return false; + auto allRedunCons = this->allRedundantConstraints(); + auto newSet = std::make_shared>(); + for (auto& con : *allRedunCons) { + auto aaa = std::static_pointer_cast(con); + auto& bbb = aaa->constraint->getName(); + newSet->insert(bbb); + } + //std::transform(allRedunCons->begin(), allRedunCons->end(), newSet->begin(), [](auto con) { + // return std::static_pointer_cast(con)->constraint->getName(); + // }); + if (newSet->empty()) return false; + auto itr = std::find_if(setsOfRedundantConstraints->begin(), setsOfRedundantConstraints->end(), [&](auto& set) { + for (auto& name : *set) { + if (newSet->find(name) == newSet->end()) return false; + } + return true; + }); + if (itr != setsOfRedundantConstraints->end()) { + //"Same set of redundant constraints found." + setsOfRedundantConstraints->push_back(newSet); + return false; + } + if (setsOfRedundantConstraints->size() >= 2) { + auto it = std::find_if(setsOfRedundantConstraints->begin(), setsOfRedundantConstraints->end(), [&](auto set) { + return set->size() == newSet->size(); + }); + if (it != setsOfRedundantConstraints->end()) { + //"Equal number of redundant constraints found." + setsOfRedundantConstraints->push_back(newSet); + return false; + } + } + setsOfRedundantConstraints->push_back(newSet); + this->partsJointsMotionsDo([](auto item) { item->reactivateRedundantConstraints(); }); + return true; } void MbD::SystemSolver::preCollision() @@ -83,6 +125,26 @@ void MbD::SystemSolver::runBasicCollision() void SystemSolver::runBasicKinematic() { + try { + basicIntegrator = CREATE::With(); + basicIntegrator->setSystem(this); + basicIntegrator->run(); + } + catch (NotKinematicError ex) { + this->runQuasiKinematic(); + } +} + +void MbD::SystemSolver::runQuasiKinematic() +{ + try { + basicIntegrator = CREATE::With(); + basicIntegrator->setSystem(this); + basicIntegrator->run(); + } + catch (DiscontinuityError ex) { + this->discontinuityBlock(); + } } void MbD::SystemSolver::partsJointsMotionsDo(const std::function)>& f) @@ -114,3 +176,53 @@ std::shared_ptr>> MbD::SystemSolver::per { return system->perpendicularConstraints2(); } + +std::shared_ptr>> MbD::SystemSolver::allRedundantConstraints() +{ + return system->allRedundantConstraints(); +} + +void MbD::SystemSolver::postNewtonRaphson() +{ + assert(false); +} + +void MbD::SystemSolver::partsJointsMotionsForcesTorquesDo(const std::function)>& f) +{ + system->partsJointsMotionsForcesTorquesDo(f); +} + +void MbD::SystemSolver::discontinuityBlock() +{ + assert(false); +} + +double MbD::SystemSolver::startTime() +{ + return tstart; +} + +double MbD::SystemSolver::outputStepSize() +{ + return hout; +} + +double MbD::SystemSolver::maxStepSize() +{ + return hmax; +} + +double MbD::SystemSolver::minStepSize() +{ + return hmin; +} + +double MbD::SystemSolver::firstOutputTime() +{ + return toutFirst; +} + +double MbD::SystemSolver::endTime() +{ + return tend; +} diff --git a/MbDCode/SystemSolver.h b/MbDCode/SystemSolver.h index d214a1f..7a78cba 100644 --- a/MbDCode/SystemSolver.h +++ b/MbDCode/SystemSolver.h @@ -1,17 +1,20 @@ #pragma once #include #include -#include +#include +#include #include "Solver.h" #include "System.h" -#include "Constraint.h" +//#include "Constraint.h" //#include "NewtonRaphson.h" -#include "KineIntegrator.h" +//#include "QuasiIntegrator.h" namespace MbD { class System; + class Constraint; class NewtonRaphson; + class QuasiIntegrator; class SystemSolver : public Solver { @@ -34,6 +37,7 @@ namespace MbD { void runCollisionDerivativeIC(); void runBasicCollision(); void runBasicKinematic(); + void runQuasiKinematic(); void partsJointsMotionsDo(const std::function )>& f); void logString(std::string& str); std::shared_ptr>> parts(); @@ -42,16 +46,27 @@ namespace MbD { std::shared_ptr>> essentialConstraints2(); std::shared_ptr>> displacementConstraints(); std::shared_ptr>> perpendicularConstraints2(); + std::shared_ptr>> allRedundantConstraints(); + virtual void postNewtonRaphson(); + void partsJointsMotionsForcesTorquesDo(const std::function )>& f); + void discontinuityBlock(); + double startTime(); + double outputStepSize(); + double maxStepSize(); + double minStepSize(); + double firstOutputTime(); + double endTime(); + System* system; //Use raw pointer when pointing backwards. std::shared_ptr icTypeSolver; - std::shared_ptr>>> setsOfRedundantConstraints; - + std::shared_ptr>>> setsOfRedundantConstraints; + double errorTolPosKine = 1.0e-6; double errorTolAccKine = 1.0e-6; - size_t iterMaxPosKine = 25; - size_t iterMaxAccKine = 25; - std::shared_ptr basicIntegrator; + int iterMaxPosKine = 25; + int iterMaxAccKine = 25; + std::shared_ptr basicIntegrator; std::shared_ptr> tstartPasts; double tstart = 0; double tend = 25; @@ -64,8 +79,8 @@ namespace MbD { double corRelTol = 0; double intAbsTol = 0; double intRelTol = 0; - size_t iterMaxDyn = 0; - size_t orderMax = 0; + int iterMaxDyn = 0; + int orderMax = 0; double translationLimit = 0; double rotationLimit = 0; }; diff --git a/MbDCode/TooManyTriesError.cpp b/MbDCode/TooManyTriesError.cpp new file mode 100644 index 0000000..482dea9 --- /dev/null +++ b/MbDCode/TooManyTriesError.cpp @@ -0,0 +1,5 @@ +#include "TooManyTriesError.h" + +MbD::TooManyTriesError::TooManyTriesError(const std::string& msg) : std::runtime_error(msg) +{ +} diff --git a/MbDCode/TooManyTriesError.h b/MbDCode/TooManyTriesError.h new file mode 100644 index 0000000..24c0c26 --- /dev/null +++ b/MbDCode/TooManyTriesError.h @@ -0,0 +1,16 @@ +#pragma once + +#include +#include +#include + +namespace MbD { + class TooManyTriesError : virtual public std::runtime_error + { + + public: + //TooManyTriesError(); + explicit TooManyTriesError(const std::string& msg); + virtual ~TooManyTriesError() noexcept {} + }; +} diff --git a/MbDCode/TooSmallStepSizeError.cpp b/MbDCode/TooSmallStepSizeError.cpp new file mode 100644 index 0000000..ecd7749 --- /dev/null +++ b/MbDCode/TooSmallStepSizeError.cpp @@ -0,0 +1,5 @@ +#include "TooSmallStepSizeError.h" + +MbD::TooSmallStepSizeError::TooSmallStepSizeError(const std::string& msg) : std::runtime_error(msg) +{ +} diff --git a/MbDCode/TooSmallStepSizeError.h b/MbDCode/TooSmallStepSizeError.h new file mode 100644 index 0000000..915462e --- /dev/null +++ b/MbDCode/TooSmallStepSizeError.h @@ -0,0 +1,16 @@ +#pragma once + +#include +#include +#include + +namespace MbD { + class TooSmallStepSizeError : virtual public std::runtime_error + { + + public: + //TooSmallStepSizeError(); + explicit TooSmallStepSizeError(const std::string& msg); + virtual ~TooSmallStepSizeError() noexcept {} + }; +} diff --git a/MbDCode/TranslationConstraintIJ.cpp b/MbDCode/TranslationConstraintIJ.cpp index 3853633..2ff6d40 100644 --- a/MbDCode/TranslationConstraintIJ.cpp +++ b/MbDCode/TranslationConstraintIJ.cpp @@ -3,7 +3,7 @@ using namespace MbD; -TranslationConstraintIJ::TranslationConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj, size_t axisi) : +TranslationConstraintIJ::TranslationConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj, int axisi) : ConstraintIJ(frmi, frmj), axisI(axisi) { } diff --git a/MbDCode/TranslationConstraintIJ.h b/MbDCode/TranslationConstraintIJ.h index 9149558..59b674f 100644 --- a/MbDCode/TranslationConstraintIJ.h +++ b/MbDCode/TranslationConstraintIJ.h @@ -8,7 +8,7 @@ namespace MbD { { //riIeJeIe public: - TranslationConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj, size_t axisi); + TranslationConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj, int axisi); void initialize(); void initializeLocally() override; void initializeGlobally() override; @@ -19,7 +19,7 @@ namespace MbD { MbD::ConstraintType type() override; void postPosICIteration() override; - size_t axisI; + int axisI; std::shared_ptr riIeJeIe; }; } diff --git a/MbDCode/TranslationConstraintIqcJc.cpp b/MbDCode/TranslationConstraintIqcJc.cpp index 1ee2e04..a1faa97 100644 --- a/MbDCode/TranslationConstraintIqcJc.cpp +++ b/MbDCode/TranslationConstraintIqcJc.cpp @@ -5,7 +5,7 @@ using namespace MbD; -TranslationConstraintIqcJc::TranslationConstraintIqcJc(EndFrmcptr frmi, EndFrmcptr frmj, size_t axisi) : +TranslationConstraintIqcJc::TranslationConstraintIqcJc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi) : TranslationConstraintIJ(frmi, frmj, axisi) { } @@ -31,3 +31,22 @@ void MbD::TranslationConstraintIqcJc::useEquationNumbers() iqXI = std::static_pointer_cast(frmI)->iqX(); iqEI = std::static_pointer_cast(frmI)->iqE(); } + +void MbD::TranslationConstraintIqcJc::fillPosICError(FColDsptr col) +{ + Constraint::fillPosICError(col); + col->atiplusFullVectortimes(iqXI, pGpXI, lam); + col->atiplusFullVectortimes(iqEI, pGpEI, lam); +} + +void MbD::TranslationConstraintIqcJc::fillPosICJacob(SpMatDsptr mat) +{ + mat->atijplusFullRow(iG, iqXI, pGpXI); + mat->atijplusFullColumn(iqXI, iG, pGpXI->transpose()); + mat->atijplusFullRow(iG, iqEI, pGpEI); + mat->atijplusFullColumn(iqEI, iG, pGpEI->transpose()); + auto ppGpXIpEIlam = ppGpXIpEI->times(lam); + mat->atijplusFullMatrix(iqXI, iqEI, ppGpXIpEIlam); + mat->atijplusTransposeFullMatrix(iqEI, iqXI, ppGpXIpEIlam); + mat->atijplusFullMatrixtimes(iqEI, iqEI, ppGpEIpEI, lam); +} diff --git a/MbDCode/TranslationConstraintIqcJc.h b/MbDCode/TranslationConstraintIqcJc.h index 2257e5e..1f20055 100644 --- a/MbDCode/TranslationConstraintIqcJc.h +++ b/MbDCode/TranslationConstraintIqcJc.h @@ -7,16 +7,18 @@ namespace MbD { { //pGpXI pGpEI ppGpXIpEI ppGpEIpEI iqXI iqEI public: - TranslationConstraintIqcJc(EndFrmcptr frmi, EndFrmcptr frmj, size_t axisi); + TranslationConstraintIqcJc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi); void initriIeJeIe() override; void calcPostDynCorrectorIteration() override; void useEquationNumbers() override; + void fillPosICError(FColDsptr col) override; + void fillPosICJacob(SpMatDsptr mat) override; FRowDsptr pGpXI; FRowDsptr pGpEI; FMatDsptr ppGpXIpEI; FMatDsptr ppGpEIpEI; - size_t iqXI, iqEI; + int iqXI, iqEI; }; } diff --git a/MbDCode/TranslationConstraintIqcJqc.cpp b/MbDCode/TranslationConstraintIqcJqc.cpp index badb370..d1e02a9 100644 --- a/MbDCode/TranslationConstraintIqcJqc.cpp +++ b/MbDCode/TranslationConstraintIqcJqc.cpp @@ -5,7 +5,7 @@ using namespace MbD; -TranslationConstraintIqcJqc::TranslationConstraintIqcJqc(EndFrmcptr frmi, EndFrmcptr frmj, size_t axisi) : +TranslationConstraintIqcJqc::TranslationConstraintIqcJqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi) : TranslationConstraintIqcJc(frmi, frmj, axisi) { } @@ -17,6 +17,12 @@ void MbD::TranslationConstraintIqcJqc::initriIeJeIe() void MbD::TranslationConstraintIqcJqc::calcPostDynCorrectorIteration() { + TranslationConstraintIqcJc::calcPostDynCorrectorIteration(); + pGpXJ = riIeJeIe->pvaluepXJ(); + pGpEJ = riIeJeIe->pvaluepEJ(); + ppGpEIpXJ = riIeJeIe->ppvaluepXJpEK()->transpose(); + ppGpEIpEJ = riIeJeIe->ppvaluepEJpEK()->transpose(); + ppGpEJpEJ = riIeJeIe->ppvaluepEJpEJ(); } void MbD::TranslationConstraintIqcJqc::useEquationNumbers() @@ -25,3 +31,26 @@ void MbD::TranslationConstraintIqcJqc::useEquationNumbers() iqXJ = std::static_pointer_cast(frmJ)->iqX(); iqEJ = std::static_pointer_cast(frmJ)->iqE(); } + +void MbD::TranslationConstraintIqcJqc::fillPosICError(FColDsptr col) +{ + Constraint::fillPosICError(col); + col->atiplusFullVectortimes(iqXJ, pGpXJ, lam); + col->atiplusFullVectortimes(iqEJ, pGpEJ, lam); +} + +void MbD::TranslationConstraintIqcJqc::fillPosICJacob(SpMatDsptr mat) +{ + TranslationConstraintIqcJc::fillPosICJacob(mat); + mat->atijplusFullRow(iG, iqXJ, pGpXJ); + mat->atijplusFullColumn(iqXJ, iG, pGpXJ->transpose()); + mat->atijplusFullRow(iG, iqEJ, pGpEJ); + mat->atijplusFullColumn(iqEJ, iG, pGpEJ->transpose()); + auto ppGpEIpXJlam = ppGpEIpXJ->times(lam); + mat->atijplusFullMatrix(iqEI, iqXJ, ppGpEIpXJlam); + mat->atijplusTransposeFullMatrix(iqXJ, iqEI, ppGpEIpXJlam); + auto ppGpEIpEJlam = ppGpEIpEJ->times(lam); + mat->atijplusFullMatrix(iqEI, iqEJ, ppGpEIpEJlam); + mat->atijplusTransposeFullMatrix(iqEJ, iqEI, ppGpEIpEJlam); + mat->atijplusFullMatrixtimes(iqEJ, iqEJ, ppGpEJpEJ, lam); +} diff --git a/MbDCode/TranslationConstraintIqcJqc.h b/MbDCode/TranslationConstraintIqcJqc.h index 82bd1ef..f1ac363 100644 --- a/MbDCode/TranslationConstraintIqcJqc.h +++ b/MbDCode/TranslationConstraintIqcJqc.h @@ -7,17 +7,19 @@ namespace MbD { { //pGpXJ pGpEJ ppGpEIpXJ ppGpEIpEJ ppGpEJpEJ iqXJ iqEJ public: - TranslationConstraintIqcJqc(EndFrmcptr frmi, EndFrmcptr frmj, size_t axisi); + TranslationConstraintIqcJqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi); void initriIeJeIe() override; void calcPostDynCorrectorIteration() override; void useEquationNumbers() override; + void fillPosICError(FColDsptr col) override; + void fillPosICJacob(SpMatDsptr mat) override; FRowDsptr pGpXJ; FRowDsptr pGpEJ; FMatDsptr ppGpEIpXJ; FMatDsptr ppGpEIpEJ; FMatDsptr ppGpEJpEJ; - size_t iqXJ = -1, iqEJ = -1; + int iqXJ = -1, iqEJ = -1; }; } diff --git a/MbDCode/TranslationConstraintIqctJqc.cpp b/MbDCode/TranslationConstraintIqctJqc.cpp index c21662d..58f0f43 100644 --- a/MbDCode/TranslationConstraintIqctJqc.cpp +++ b/MbDCode/TranslationConstraintIqctJqc.cpp @@ -4,7 +4,7 @@ using namespace MbD; -TranslationConstraintIqctJqc::TranslationConstraintIqctJqc(EndFrmcptr frmi, EndFrmcptr frmj, size_t axisi) : +TranslationConstraintIqctJqc::TranslationConstraintIqctJqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi) : TranslationConstraintIqcJqc(frmi, frmj, axisi) { } diff --git a/MbDCode/TranslationConstraintIqctJqc.h b/MbDCode/TranslationConstraintIqctJqc.h index c9c505c..5e52f79 100644 --- a/MbDCode/TranslationConstraintIqctJqc.h +++ b/MbDCode/TranslationConstraintIqctJqc.h @@ -7,7 +7,7 @@ namespace MbD { { //pGpt ppGpXIpt ppGpEIpt ppGpXJpt ppGpEJpt ppGptpt public: - TranslationConstraintIqctJqc(EndFrmcptr frmi, EndFrmcptr frmj, size_t axisi); + TranslationConstraintIqctJqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi); void initriIeJeIe() override; MbD::ConstraintType type() override; diff --git a/MbDCode/VectorNewtonRaphson.cpp b/MbDCode/VectorNewtonRaphson.cpp index 0d7fc17..b3ff827 100644 --- a/MbDCode/VectorNewtonRaphson.cpp +++ b/MbDCode/VectorNewtonRaphson.cpp @@ -10,6 +10,12 @@ using namespace MbD; +void MbD::VectorNewtonRaphson::initializeGlobally() +{ + assert(false); + //system->fillVarVector(x); +} + void MbD::VectorNewtonRaphson::run() { this->preRun(); @@ -26,6 +32,7 @@ std::shared_ptr MbD::VectorNewtonRaphson::matrixSolverClassNew() void MbD::VectorNewtonRaphson::fillY() { + assert(false); } void MbD::VectorNewtonRaphson::calcyNorm() diff --git a/MbDCode/VectorNewtonRaphson.h b/MbDCode/VectorNewtonRaphson.h index 3985036..0bb2063 100644 --- a/MbDCode/VectorNewtonRaphson.h +++ b/MbDCode/VectorNewtonRaphson.h @@ -8,6 +8,7 @@ namespace MbD { { //matrixSolver n public: + void initializeGlobally() override; void run() override; virtual std::shared_ptr matrixSolverClassNew(); void fillY() override; @@ -21,7 +22,7 @@ namespace MbD { virtual void handleSingularMatrix(); std::shared_ptr matrixSolver; - size_t n; + int n; std::shared_ptr> xold, x, dx, y; //std::shared_ptr> pypx; }; diff --git a/MbDCode/enum.h b/MbDCode/enum.h index e867d9e..965fb34 100644 --- a/MbDCode/enum.h +++ b/MbDCode/enum.h @@ -1,4 +1,4 @@ #pragma once namespace MbD { - enum ConstraintType {essential, displacement, perpendicular}; + enum ConstraintType {essential, displacement, perpendicular, redundant}; } \ No newline at end of file