diff --git a/MbDCode/AbsConstraint.cpp b/MbDCode/AbsConstraint.cpp index b8ba416..417a0f1 100644 --- a/MbDCode/AbsConstraint.cpp +++ b/MbDCode/AbsConstraint.cpp @@ -17,7 +17,7 @@ void AbsConstraint::initialize() Constraint::initialize(); } -void MbD::AbsConstraint::calcPostDynCorrectorIteration() +void AbsConstraint::calcPostDynCorrectorIteration() { if (axis < 3) { aG = static_cast(owner)->qX->at(axis); @@ -27,34 +27,34 @@ void MbD::AbsConstraint::calcPostDynCorrectorIteration() } } -void MbD::AbsConstraint::useEquationNumbers() +void AbsConstraint::useEquationNumbers() { iqXminusOnePlusAxis = static_cast(owner)->iqX + axis; } -void MbD::AbsConstraint::fillPosICJacob(SpMatDsptr mat) +void AbsConstraint::fillPosICJacob(SpMatDsptr mat) { mat->atijplusNumber(iG, iqXminusOnePlusAxis, 1.0); mat->atijplusNumber(iqXminusOnePlusAxis, iG, 1.0); } -void MbD::AbsConstraint::fillPosICError(FColDsptr col) +void AbsConstraint::fillPosICError(FColDsptr col) { Constraint::fillPosICError(col); col->at(iqXminusOnePlusAxis) += lam; } -void MbD::AbsConstraint::fillPosKineJacob(SpMatDsptr mat) +void AbsConstraint::fillPosKineJacob(SpMatDsptr mat) { mat->atijplusNumber(iG, iqXminusOnePlusAxis, 1.0); } -void MbD::AbsConstraint::fillVelICJacob(SpMatDsptr mat) +void AbsConstraint::fillVelICJacob(SpMatDsptr mat) { this->fillPosICJacob(mat); } -void MbD::AbsConstraint::fillAccICIterError(FColDsptr col) +void AbsConstraint::fillAccICIterError(FColDsptr col) { col->atiplusNumber(iqXminusOnePlusAxis, lam); auto partFrame = static_cast(owner); diff --git a/MbDCode/AccICKineNewtonRaphson.cpp b/MbDCode/AccICKineNewtonRaphson.cpp index 12c4c71..911983b 100644 --- a/MbDCode/AccICKineNewtonRaphson.cpp +++ b/MbDCode/AccICKineNewtonRaphson.cpp @@ -1,14 +1,16 @@ #include "AccICKineNewtonRaphson.h" #include "SystemSolver.h" -void MbD::AccICKineNewtonRaphson::initializeGlobally() +using namespace MbD; + +void AccICKineNewtonRaphson::initializeGlobally() { AccNewtonRaphson::initializeGlobally(); iterMax = system->iterMaxAccKine; dxTol = system->errorTolAccKine; } -void MbD::AccICKineNewtonRaphson::preRun() +void AccICKineNewtonRaphson::preRun() { std::string str("MbD: Solving for quasi kinematic acceleration."); system->logString(str); diff --git a/MbDCode/AccICNewtonRaphson.cpp b/MbDCode/AccICNewtonRaphson.cpp index 39fe3e1..368311a 100644 --- a/MbDCode/AccICNewtonRaphson.cpp +++ b/MbDCode/AccICNewtonRaphson.cpp @@ -1,12 +1,14 @@ #include "AccICNewtonRaphson.h" #include "SystemSolver.h" -bool MbD::AccICNewtonRaphson::isConverged() +using namespace MbD; + +bool AccICNewtonRaphson::isConverged() { return this->isConvergedToNumericalLimit(); } -void MbD::AccICNewtonRaphson::preRun() +void AccICNewtonRaphson::preRun() { std::string str("MbD: Solving for quasi kinematic acceleration."); system->logString(str); diff --git a/MbDCode/AccKineNewtonRaphson.cpp b/MbDCode/AccKineNewtonRaphson.cpp index 4b0d4f6..42830c3 100644 --- a/MbDCode/AccKineNewtonRaphson.cpp +++ b/MbDCode/AccKineNewtonRaphson.cpp @@ -1,14 +1,16 @@ #include "AccKineNewtonRaphson.h" #include "SystemSolver.h" -void MbD::AccKineNewtonRaphson::initializeGlobally() +using namespace MbD; + +void AccKineNewtonRaphson::initializeGlobally() { AccNewtonRaphson::initializeGlobally(); iterMax = system->iterMaxAccKine; dxTol = system->errorTolAccKine; } -void MbD::AccKineNewtonRaphson::preRun() +void AccKineNewtonRaphson::preRun() { std::string str("MbD: Solving for kinematic acceleration."); system->logString(str); diff --git a/MbDCode/AccNewtonRaphson.cpp b/MbDCode/AccNewtonRaphson.cpp index 57f270d..92bb7c3 100644 --- a/MbDCode/AccNewtonRaphson.cpp +++ b/MbDCode/AccNewtonRaphson.cpp @@ -5,12 +5,14 @@ #include "SimulationStoppingError.h" #include -void MbD::AccNewtonRaphson::askSystemToUpdate() +using namespace MbD; + +void AccNewtonRaphson::askSystemToUpdate() { system->partsJointsMotionsForcesTorquesDo([&](std::shared_ptr item) { item->postAccICIteration(); }); } -void MbD::AccNewtonRaphson::assignEquationNumbers() +void AccNewtonRaphson::assignEquationNumbers() { auto parts = system->parts(); //auto contactEndFrames = system->contactEndFrames(); @@ -41,7 +43,7 @@ void MbD::AccNewtonRaphson::assignEquationNumbers() n = nEqns; } -void MbD::AccNewtonRaphson::fillPyPx() +void AccNewtonRaphson::fillPyPx() { pypx->zeroSelf(); system->partsJointsMotionsForcesTorquesDo([&](std::shared_ptr item) { @@ -49,17 +51,17 @@ void MbD::AccNewtonRaphson::fillPyPx() }); } -void MbD::AccNewtonRaphson::fillY() +void AccNewtonRaphson::fillY() { y->zeroSelf(); system->partsJointsMotionsForcesTorquesDo([&](std::shared_ptr item) { item->fillAccICIterError(y); //std::cout << item->getName() << *y << std::endl; }); - //std::cout << *y << std::endl; + std::cout << *y << std::endl; } -void MbD::AccNewtonRaphson::incrementIterNo() +void AccNewtonRaphson::incrementIterNo() { if (iterNo >= iterMax) { @@ -82,13 +84,13 @@ void MbD::AccNewtonRaphson::incrementIterNo() iterNo++; } -void MbD::AccNewtonRaphson::initializeGlobally() +void AccNewtonRaphson::initializeGlobally() { SystemNewtonRaphson::initializeGlobally(); system->partsJointsMotionsDo([&](std::shared_ptr item) { item->fillqsuddotlam(x); }); } -void MbD::AccNewtonRaphson::logSingularMatrixMessage() +void AccNewtonRaphson::logSingularMatrixMessage() { std::string str = "MbD: Some parts with zero masses or moment of inertias have infinite accelerations."; system->logString(str); @@ -96,17 +98,17 @@ void MbD::AccNewtonRaphson::logSingularMatrixMessage() system->logString(str); } -void MbD::AccNewtonRaphson::passRootToSystem() +void AccNewtonRaphson::passRootToSystem() { system->partsJointsMotionsDo([&](std::shared_ptr item) { item->setqsuddotlam(x); }); } -void MbD::AccNewtonRaphson::postRun() +void AccNewtonRaphson::postRun() { system->partsJointsMotionsForcesTorquesDo([&](std::shared_ptr item) { item->postAccIC(); }); } -void MbD::AccNewtonRaphson::preRun() +void AccNewtonRaphson::preRun() { system->partsJointsMotionsForcesTorquesDo([&](std::shared_ptr item) { item->preAccIC(); }); } diff --git a/MbDCode/AnyPosICNewtonRaphson.cpp b/MbDCode/AnyPosICNewtonRaphson.cpp index 1f75321..5b509fe 100644 --- a/MbDCode/AnyPosICNewtonRaphson.cpp +++ b/MbDCode/AnyPosICNewtonRaphson.cpp @@ -5,13 +5,13 @@ using namespace MbD; -void MbD::AnyPosICNewtonRaphson::initialize() +void AnyPosICNewtonRaphson::initialize() { NewtonRaphson::initialize(); nSingularMatrixError = 0; } -void MbD::AnyPosICNewtonRaphson::initializeGlobally() +void AnyPosICNewtonRaphson::initializeGlobally() { SystemNewtonRaphson::initializeGlobally(); system->partsJointsMotionsDo([&](std::shared_ptr item) { @@ -21,14 +21,14 @@ void MbD::AnyPosICNewtonRaphson::initializeGlobally() }); } -void MbD::AnyPosICNewtonRaphson::createVectorsAndMatrices() +void AnyPosICNewtonRaphson::createVectorsAndMatrices() { qsuOld = std::make_shared>(nqsu); qsuWeights = std::make_shared>(nqsu); SystemNewtonRaphson::createVectorsAndMatrices(); } -void MbD::AnyPosICNewtonRaphson::fillY() +void AnyPosICNewtonRaphson::fillY() { auto newMinusOld = qsuOld->negated(); newMinusOld->equalSelfPlusFullColumnAt(x, 0); @@ -41,7 +41,7 @@ void MbD::AnyPosICNewtonRaphson::fillY() //std::cout << *y << std::endl; } -void MbD::AnyPosICNewtonRaphson::fillPyPx() +void AnyPosICNewtonRaphson::fillPyPx() { pypx->zeroSelf(); pypx->atijminusDiagonalMatrix(0, 0, qsuWeights); @@ -52,7 +52,7 @@ void MbD::AnyPosICNewtonRaphson::fillPyPx() //std::cout << *pypx << std::endl; } -void MbD::AnyPosICNewtonRaphson::passRootToSystem() +void AnyPosICNewtonRaphson::passRootToSystem() { system->partsJointsMotionsDo([&](std::shared_ptr item) { item->setqsulam(x); }); } diff --git a/MbDCode/Array.h b/MbDCode/Array.h index b6ec565..e7547ca 100644 --- a/MbDCode/Array.h +++ b/MbDCode/Array.h @@ -1,7 +1,7 @@ #pragma once +#include #include #include -//#include #include #include @@ -28,6 +28,17 @@ namespace MbD { double maxMagnitudeOfVector(); void equalArrayAt(std::shared_ptr> array, int i); + virtual std::ostream& printOn(std::ostream& s) const { + std::string str = typeid(*this).name(); + auto classname = str.substr(11, str.size() - 11); + s << classname << std::endl; + return s; + } + friend std::ostream& operator<<(std::ostream& s, const Array& array) + { + return array.printOn(s); + } + }; template inline void Array::initialize() @@ -99,7 +110,7 @@ namespace MbD { this->at(ii) = array->at(i + ii); } } - using ListD = std::initializer_list; - using ListListD = std::initializer_list>; - using ListListPairD = std::initializer_list>>; + using ListD = std::initializer_list; + using ListListD = std::initializer_list>; + using ListListPairD = std::initializer_list>>; } \ No newline at end of file diff --git a/MbDCode/AtPointConstraintIJ.cpp b/MbDCode/AtPointConstraintIJ.cpp index 2f68031..439d976 100644 --- a/MbDCode/AtPointConstraintIJ.cpp +++ b/MbDCode/AtPointConstraintIJ.cpp @@ -15,12 +15,12 @@ void AtPointConstraintIJ::initialize() initriIeJeO(); } -void MbD::AtPointConstraintIJ::initializeLocally() +void AtPointConstraintIJ::initializeLocally() { riIeJeO->initializeLocally(); } -void MbD::AtPointConstraintIJ::initializeGlobally() +void AtPointConstraintIJ::initializeGlobally() { riIeJeO->initializeGlobally(); } @@ -30,41 +30,41 @@ void AtPointConstraintIJ::initriIeJeO() riIeJeO = CREATE::With(frmI, frmJ, axis); } -void MbD::AtPointConstraintIJ::postInput() +void AtPointConstraintIJ::postInput() { riIeJeO->postInput(); Constraint::postInput(); } -void MbD::AtPointConstraintIJ::calcPostDynCorrectorIteration() +void AtPointConstraintIJ::calcPostDynCorrectorIteration() { aG = riIeJeO->riIeJeO - aConstant; } -void MbD::AtPointConstraintIJ::prePosIC() +void AtPointConstraintIJ::prePosIC() { riIeJeO->prePosIC(); Constraint::prePosIC(); } -MbD::ConstraintType MbD::AtPointConstraintIJ::type() +ConstraintType AtPointConstraintIJ::type() { - return MbD::displacement; + return displacement; } -void MbD::AtPointConstraintIJ::postPosICIteration() +void AtPointConstraintIJ::postPosICIteration() { riIeJeO->postPosICIteration(); Item::postPosICIteration(); } -void MbD::AtPointConstraintIJ::preVelIC() +void AtPointConstraintIJ::preVelIC() { riIeJeO->preVelIC(); Item::preVelIC(); } -void MbD::AtPointConstraintIJ::preAccIC() +void AtPointConstraintIJ::preAccIC() { riIeJeO->preAccIC(); Constraint::preAccIC(); diff --git a/MbDCode/AtPointConstraintIJ.h b/MbDCode/AtPointConstraintIJ.h index 4ef5252..c183e59 100644 --- a/MbDCode/AtPointConstraintIJ.h +++ b/MbDCode/AtPointConstraintIJ.h @@ -17,7 +17,7 @@ namespace MbD { void postInput() override; void calcPostDynCorrectorIteration() override; void prePosIC() override; - MbD::ConstraintType type() override; + ConstraintType type() override; void postPosICIteration() override; void preVelIC() override; void preAccIC() override; diff --git a/MbDCode/AtPointConstraintIqcJc.cpp b/MbDCode/AtPointConstraintIqcJc.cpp index fd4f8af..bb51fec 100644 --- a/MbDCode/AtPointConstraintIqcJc.cpp +++ b/MbDCode/AtPointConstraintIqcJc.cpp @@ -10,7 +10,7 @@ AtPointConstraintIqcJc::AtPointConstraintIqcJc(EndFrmcptr frmi, EndFrmcptr frmj, { } -void MbD::AtPointConstraintIqcJc::initializeGlobally() +void AtPointConstraintIqcJc::initializeGlobally() { AtPointConstraintIJ::initializeGlobally(); ppGpEIpEI = (std::static_pointer_cast(riIeJeO))->ppriIeJeOpEIpEI; @@ -21,26 +21,26 @@ void AtPointConstraintIqcJc::initriIeJeO() riIeJeO = CREATE::With(frmI, frmJ, axis); } -void MbD::AtPointConstraintIqcJc::calcPostDynCorrectorIteration() +void AtPointConstraintIqcJc::calcPostDynCorrectorIteration() { AtPointConstraintIJ::calcPostDynCorrectorIteration(); pGpEI = std::static_pointer_cast(riIeJeO)->priIeJeOpEI; } -void MbD::AtPointConstraintIqcJc::useEquationNumbers() +void AtPointConstraintIqcJc::useEquationNumbers() { iqXIminusOnePlusAxis = std::static_pointer_cast(frmI)->iqX() + axis; iqEI = std::static_pointer_cast(frmI)->iqE(); } -void MbD::AtPointConstraintIqcJc::fillPosICError(FColDsptr col) +void AtPointConstraintIqcJc::fillPosICError(FColDsptr col) { Constraint::fillPosICError(col); col->at(iqXIminusOnePlusAxis) -= lam; col->atiplusFullVectortimes(iqEI, pGpEI, lam); } -void MbD::AtPointConstraintIqcJc::fillPosICJacob(SpMatDsptr mat) +void AtPointConstraintIqcJc::fillPosICJacob(SpMatDsptr mat) { mat->atijplusNumber(iG, iqXIminusOnePlusAxis, -1.0); mat->atijplusNumber(iqXIminusOnePlusAxis, iG, -1.0); @@ -49,13 +49,13 @@ void MbD::AtPointConstraintIqcJc::fillPosICJacob(SpMatDsptr mat) mat->atijplusFullMatrixtimes(iqEI, iqEI, ppGpEIpEI, lam); } -void MbD::AtPointConstraintIqcJc::fillPosKineJacob(SpMatDsptr mat) +void AtPointConstraintIqcJc::fillPosKineJacob(SpMatDsptr mat) { mat->atijplusNumber(iG, iqXIminusOnePlusAxis, -1.0); mat->atijplusFullRow(iG, iqEI, pGpEI); } -void MbD::AtPointConstraintIqcJc::fillVelICJacob(SpMatDsptr mat) +void AtPointConstraintIqcJc::fillVelICJacob(SpMatDsptr mat) { mat->atijplusNumber(iG, iqXIminusOnePlusAxis, -1.0); mat->atijplusNumber(iqXIminusOnePlusAxis, iG, -1.0); @@ -63,7 +63,7 @@ void MbD::AtPointConstraintIqcJc::fillVelICJacob(SpMatDsptr mat) mat->atijplusFullColumn(iqEI, iG, pGpEI->transpose()); } -void MbD::AtPointConstraintIqcJc::fillAccICIterError(FColDsptr col) +void AtPointConstraintIqcJc::fillAccICIterError(FColDsptr col) { col->atiminusNumber(iqXIminusOnePlusAxis, lam); col->atiplusFullVectortimes(iqEI, pGpEI, lam); @@ -74,3 +74,26 @@ void MbD::AtPointConstraintIqcJc::fillAccICIterError(FColDsptr col) sum += qEdotI->transposeTimesFullColumn(ppGpEIpEI->timesFullColumn(qEdotI)); col->atiplusNumber(iG, sum); } + +void AtPointConstraintIqcJc::addToJointForceI(FColDsptr col) +{ + col->atiminusNumber(axis, lam); +} + +void AtPointConstraintIqcJc::addToJointTorqueI(FColDsptr jointTorque) +{ + auto cForceT = std::make_shared>(3, 0.0); + cForceT->atiput(axis, -lam); + auto rIpIeIp = frmI->rpep(); + auto pAOIppEI = frmI->pAOppE(); + auto aBOIp = frmI->aBOp(); + auto fpAOIppEIrIpIeIp = std::make_shared>(4, 0.0); + for (int i = 0; i < 4; i++) + { + auto dum = cForceT->timesFullColumn(pAOIppEI->at(i)->timesFullColumn(rIpIeIp)); + fpAOIppEIrIpIeIp->atiput(i, dum); + } + auto lampGpE = pGpEI->transpose()->times(lam); + auto c2Torque = aBOIp->timesFullColumn(lampGpE->minusFullColumn(fpAOIppEIrIpIeIp)); + jointTorque->equalSelfPlusFullColumntimes(c2Torque, 0.5); +} diff --git a/MbDCode/AtPointConstraintIqcJc.h b/MbDCode/AtPointConstraintIqcJc.h index 913255a..3a471ad 100644 --- a/MbDCode/AtPointConstraintIqcJc.h +++ b/MbDCode/AtPointConstraintIqcJc.h @@ -17,6 +17,8 @@ namespace MbD { void fillPosKineJacob(SpMatDsptr mat) override; void fillVelICJacob(SpMatDsptr mat) override; void fillAccICIterError(FColDsptr col) override; + void addToJointForceI(FColDsptr col) override; + void addToJointTorqueI(FColDsptr col) override; FRowDsptr pGpEI; FMatDsptr ppGpEIpEI; diff --git a/MbDCode/AtPointConstraintIqcJqc.cpp b/MbDCode/AtPointConstraintIqcJqc.cpp index 7efb21b..7ec9a5b 100644 --- a/MbDCode/AtPointConstraintIqcJqc.cpp +++ b/MbDCode/AtPointConstraintIqcJqc.cpp @@ -10,7 +10,7 @@ AtPointConstraintIqcJqc::AtPointConstraintIqcJqc(EndFrmcptr frmi, EndFrmcptr frm { } -void MbD::AtPointConstraintIqcJqc::initializeGlobally() +void AtPointConstraintIqcJqc::initializeGlobally() { AtPointConstraintIqcJc::initializeGlobally(); ppGpEJpEJ = (std::static_pointer_cast(riIeJeO))->ppriIeJeOpEJpEJ; @@ -21,27 +21,27 @@ void AtPointConstraintIqcJqc::initriIeJeO() riIeJeO = CREATE::With(frmI, frmJ, axis); } -void MbD::AtPointConstraintIqcJqc::calcPostDynCorrectorIteration() +void AtPointConstraintIqcJqc::calcPostDynCorrectorIteration() { AtPointConstraintIqcJc::calcPostDynCorrectorIteration(); pGpEJ = std::static_pointer_cast(riIeJeO)->priIeJeOpEJ; } -void MbD::AtPointConstraintIqcJqc::useEquationNumbers() +void AtPointConstraintIqcJqc::useEquationNumbers() { AtPointConstraintIqcJc::useEquationNumbers(); iqXJminusOnePlusAxis = std::static_pointer_cast(frmJ)->iqX() + axis; iqEJ = std::static_pointer_cast(frmJ)->iqE(); } -void MbD::AtPointConstraintIqcJqc::fillPosICError(FColDsptr col) +void AtPointConstraintIqcJqc::fillPosICError(FColDsptr col) { AtPointConstraintIqcJc::fillPosICError(col); col->at(iqXJminusOnePlusAxis) += lam; col->atiplusFullVectortimes(iqEJ, pGpEJ, lam); } -void MbD::AtPointConstraintIqcJqc::fillPosICJacob(SpMatDsptr mat) +void AtPointConstraintIqcJqc::fillPosICJacob(SpMatDsptr mat) { AtPointConstraintIqcJc::fillPosICJacob(mat); mat->atijplusNumber(iG, iqXJminusOnePlusAxis, 1.0); @@ -51,14 +51,14 @@ void MbD::AtPointConstraintIqcJqc::fillPosICJacob(SpMatDsptr mat) mat->atijplusFullMatrixtimes(iqEJ, iqEJ, ppGpEJpEJ, lam); } -void MbD::AtPointConstraintIqcJqc::fillPosKineJacob(SpMatDsptr mat) +void AtPointConstraintIqcJqc::fillPosKineJacob(SpMatDsptr mat) { AtPointConstraintIqcJc::fillPosKineJacob(mat); mat->atijplusNumber(iG, iqXJminusOnePlusAxis, 1.0); mat->atijplusFullRow(iG, iqEJ, pGpEJ); } -void MbD::AtPointConstraintIqcJqc::fillVelICJacob(SpMatDsptr mat) +void AtPointConstraintIqcJqc::fillVelICJacob(SpMatDsptr mat) { AtPointConstraintIqcJc::fillVelICJacob(mat); mat->atijplusNumber(iG, iqXJminusOnePlusAxis, 1.0); @@ -67,7 +67,7 @@ void MbD::AtPointConstraintIqcJqc::fillVelICJacob(SpMatDsptr mat) mat->atijplusFullColumn(iqEJ, iG, pGpEJ->transpose()); } -void MbD::AtPointConstraintIqcJqc::fillAccICIterError(FColDsptr col) +void AtPointConstraintIqcJqc::fillAccICIterError(FColDsptr col) { AtPointConstraintIqcJc::fillAccICIterError(col); col->atiplusNumber(iqXJminusOnePlusAxis, lam); diff --git a/MbDCode/AtPointConstraintIqctJqc.cpp b/MbDCode/AtPointConstraintIqctJqc.cpp index 13a36b7..94b7127 100644 --- a/MbDCode/AtPointConstraintIqctJqc.cpp +++ b/MbDCode/AtPointConstraintIqctJqc.cpp @@ -9,7 +9,7 @@ AtPointConstraintIqctJqc::AtPointConstraintIqctJqc(EndFrmcptr frmi, EndFrmcptr f { } -void MbD::AtPointConstraintIqctJqc::initializeGlobally() +void AtPointConstraintIqctJqc::initializeGlobally() { riIeJeO->initializeGlobally(); ppGpEJpEJ = std::static_pointer_cast(riIeJeO)->ppriIeJeOpEJpEJ; @@ -20,7 +20,7 @@ void AtPointConstraintIqctJqc::initriIeJeO() riIeJeO = CREATE::With(frmI, frmJ, axis); } -void MbD::AtPointConstraintIqctJqc::calcPostDynCorrectorIteration() +void AtPointConstraintIqctJqc::calcPostDynCorrectorIteration() { //"ppGpEIpEI is no longer constant." @@ -28,23 +28,23 @@ void MbD::AtPointConstraintIqctJqc::calcPostDynCorrectorIteration() AtPointConstraintIqcJqc::calcPostDynCorrectorIteration(); } -MbD::ConstraintType MbD::AtPointConstraintIqctJqc::type() +ConstraintType AtPointConstraintIqctJqc::type() { - return MbD::essential; + return essential; } -void MbD::AtPointConstraintIqctJqc::preVelIC() +void AtPointConstraintIqctJqc::preVelIC() { AtPointConstraintIJ::preVelIC(); pGpt = std::static_pointer_cast(riIeJeO)->priIeJeOpt; } -void MbD::AtPointConstraintIqctJqc::fillVelICError(FColDsptr col) +void AtPointConstraintIqctJqc::fillVelICError(FColDsptr col) { col->atiminusNumber(iG, pGpt); } -void MbD::AtPointConstraintIqctJqc::fillAccICIterError(FColDsptr col) +void AtPointConstraintIqctJqc::fillAccICIterError(FColDsptr col) { AtPointConstraintIqcJqc::fillAccICIterError(col); auto efrmIqc = std::static_pointer_cast(frmI); @@ -54,7 +54,7 @@ void MbD::AtPointConstraintIqctJqc::fillAccICIterError(FColDsptr col) col->atiplusNumber(iG, sum); } -void MbD::AtPointConstraintIqctJqc::preAccIC() +void AtPointConstraintIqctJqc::preAccIC() { AtPointConstraintIJ::preAccIC(); ppGpEIpt = std::static_pointer_cast(riIeJeO)->ppriIeJeOpEIpt; diff --git a/MbDCode/AtPointConstraintIqctJqc.h b/MbDCode/AtPointConstraintIqctJqc.h index 5f9778a..ff4bf08 100644 --- a/MbDCode/AtPointConstraintIqctJqc.h +++ b/MbDCode/AtPointConstraintIqctJqc.h @@ -11,7 +11,7 @@ namespace MbD { void initializeGlobally() override; void initriIeJeO() override; void calcPostDynCorrectorIteration() override; - MbD::ConstraintType type() override; + ConstraintType type() override; void preVelIC() override; void fillVelICError(FColDsptr col) override; void fillAccICIterError(FColDsptr col) override; diff --git a/MbDCode/BasicIntegrator.cpp b/MbDCode/BasicIntegrator.cpp index 856e4a4..7691746 100644 --- a/MbDCode/BasicIntegrator.cpp +++ b/MbDCode/BasicIntegrator.cpp @@ -5,30 +5,34 @@ using namespace MbD; -void MbD::BasicIntegrator::initializeLocally() +void BasicIntegrator::initializeLocally() { _continue = true; } -void MbD::BasicIntegrator::iStep(int integer) +void BasicIntegrator::iStep(int integer) { istep = integer; opBDF->setiStep(integer); } -void MbD::BasicIntegrator::postFirstStep() +void BasicIntegrator::postFirstStep() +{ + t = tnew; + system->postFirstStep(); +} + +void BasicIntegrator::postRun() { } -void MbD::BasicIntegrator::postRun() +void BasicIntegrator::postStep() { + t = tnew; + system->postStep(); } -void MbD::BasicIntegrator::postStep() -{ -} - -void MbD::BasicIntegrator::initializeGlobally() +void BasicIntegrator::initializeGlobally() { //"Get info from system and prepare for start of simulation." //"Integrator asks system for info. Not system setting integrator." @@ -38,17 +42,17 @@ void MbD::BasicIntegrator::initializeGlobally() this->orderMax = system->orderMax(); } -void MbD::BasicIntegrator::setSystem(Solver* sys) +void BasicIntegrator::setSystem(Solver* sys) { system = static_cast(sys); } -void MbD::BasicIntegrator::calcOperatorMatrix() +void BasicIntegrator::calcOperatorMatrix() { opBDF->calcOperatorMatrix(); } -void MbD::BasicIntegrator::incrementTime() +void BasicIntegrator::incrementTime() { tpast->insert(tpast->begin(), t); @@ -62,12 +66,12 @@ void MbD::BasicIntegrator::incrementTime() system->incrementTime(tnew); } -void MbD::BasicIntegrator::incrementTry() +void BasicIntegrator::incrementTry() { assert(false); } -void MbD::BasicIntegrator::initialize() +void BasicIntegrator::initialize() { Solver::initialize(); //statistics = IdentityDictionary new. @@ -76,12 +80,12 @@ void MbD::BasicIntegrator::initialize() opBDF->timeNodes = tpast; } -void MbD::BasicIntegrator::logString(std::string& str) +void BasicIntegrator::logString(std::string& str) { system->logString(str); } -void MbD::BasicIntegrator::run() +void BasicIntegrator::run() { this->preRun(); this->initializeLocally(); @@ -93,59 +97,59 @@ void MbD::BasicIntegrator::run() this->postRun(); } -void MbD::BasicIntegrator::selectOrder() +void BasicIntegrator::selectOrder() { - assert(false); + //"Increase order consecutively with step." + if (iTry == 1) orderNew = std::min(istep + 1, orderMax); } -void MbD::BasicIntegrator::preFirstStep() +void BasicIntegrator::preFirstStep() { system->preFirstStep(); } -void MbD::BasicIntegrator::preRun() +void BasicIntegrator::preRun() { } -void MbD::BasicIntegrator::preStep() +void BasicIntegrator::preStep() { system->preStep(); } -void MbD::BasicIntegrator::reportStats() +void BasicIntegrator::reportStats() { - assert(false); } -void MbD::BasicIntegrator::firstStep() -{ - assert(false); -} - -void MbD::BasicIntegrator::setorder(int o) +void BasicIntegrator::setorder(int o) { order = o; opBDF->setorder(o); } -void MbD::BasicIntegrator::settnew(double t) +void BasicIntegrator::settnew(double t) { tnew = t; this->settime(t); } -void MbD::BasicIntegrator::sett(double tt) +void BasicIntegrator::sett(double tt) { t = tt; opBDF->settime(tt); } -void MbD::BasicIntegrator::settime(double tt) +void BasicIntegrator::settime(double tt) { opBDF->settime(tt); } -void MbD::BasicIntegrator::subsequentSteps() +double BasicIntegrator::tprevious() +{ + return tpast->at(0); +} + +void BasicIntegrator::subsequentSteps() { while (_continue) { this->nextStep(); } } diff --git a/MbDCode/BasicIntegrator.h b/MbDCode/BasicIntegrator.h index 16acc4e..4d7fcb7 100644 --- a/MbDCode/BasicIntegrator.h +++ b/MbDCode/BasicIntegrator.h @@ -17,26 +17,25 @@ namespace MbD { void initialize() override; void initializeGlobally() override; void initializeLocally() override; - virtual void iStep(int i); - virtual void postFirstStep(); + void iStep(int i) override; + void postFirstStep() override; + void postStep() override; void postRun() override; - virtual void postStep(); - virtual void preFirstStep(); - virtual void preRun() override; - virtual void preStep(); - virtual void reportStats() override; + void preFirstStep() override; + void preRun() override; + void preStep() override; + void reportStats() override; void run() override; - virtual void selectOrder(); - virtual void subsequentSteps(); + void selectOrder() override; + void subsequentSteps() override; void setSystem(Solver* sys) override; void logString(std::string& str) override; - virtual void firstStep(); - virtual void nextStep() = 0; virtual void setorder(int o); virtual void settnew(double t); virtual void sett(double t); void settime(double t); + double tprevious(); IntegratorInterface* system; int istep = 0, iTry = 0, maxTry = 0; diff --git a/MbDCode/BasicQuasiIntegrator.cpp b/MbDCode/BasicQuasiIntegrator.cpp index bcc7e0a..e0629b3 100644 --- a/MbDCode/BasicQuasiIntegrator.cpp +++ b/MbDCode/BasicQuasiIntegrator.cpp @@ -3,7 +3,7 @@ using namespace MbD; -void MbD::BasicQuasiIntegrator::firstStep() +void BasicQuasiIntegrator::firstStep() { istep = 0; this->preFirstStep(); @@ -13,47 +13,34 @@ void MbD::BasicQuasiIntegrator::firstStep() this->incrementTime(); this->runInitialConditionTypeSolution(); //this->reportTrialStepStats(); - - //while (this->isRedoingFirstStep()) - //{ - // this->incrementTry(); - // orderNew = 1; - // this->selectFirstStepSize(); - // this->changeTime(); - // this->runInitialConditionTypeSolution(); - // this->reportTrialStepStats(); - //} - //this->postFirstStep(); + this->postFirstStep(); //this->reportStepStats(); } -void MbD::BasicQuasiIntegrator::nextStep() +bool BasicQuasiIntegrator::isRedoingFirstStep() +{ + return false; +} + +void BasicQuasiIntegrator::nextStep() { this->preStep(); iTry = 1; this->selectOrder(); - //this->selectStepSize(); - //this->incrementTime(); - //this->runInitialConditionTypeSolution(); + 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->postStep(); //this->reportStepStats(); } -void MbD::BasicQuasiIntegrator::runInitialConditionTypeSolution() +void BasicQuasiIntegrator::runInitialConditionTypeSolution() { system->runInitialConditionTypeSolution(); } -void MbD::BasicQuasiIntegrator::selectFirstStepSize() +void BasicQuasiIntegrator::selectFirstStepSize() { if (iTry == 1) { hnew = direction * (system->tout - t); @@ -63,3 +50,14 @@ void MbD::BasicQuasiIntegrator::selectFirstStepSize() } hnew = system->suggestSmallerOrAcceptFirstStepSize(hnew); } + +void BasicQuasiIntegrator::selectStepSize() +{ + if (iTry == 1) { + hnew = direction * (system->tout - t); + } + else { + hnew = 0.25 * h; + } + hnew = system->suggestSmallerOrAcceptStepSize(hnew); +} diff --git a/MbDCode/BasicQuasiIntegrator.h b/MbDCode/BasicQuasiIntegrator.h index 0918edf..b458aae 100644 --- a/MbDCode/BasicQuasiIntegrator.h +++ b/MbDCode/BasicQuasiIntegrator.h @@ -8,14 +8,13 @@ namespace MbD { // public: void firstStep(); - //bool isRedoingFirstStep(); - //bool isRedoingStep(); + bool isRedoingFirstStep(); void nextStep(); //void reportStepStats(); //void reportTrialStepStats(); void runInitialConditionTypeSolution() override; void selectFirstStepSize(); - //void selectStepSize(); + void selectStepSize(); }; } diff --git a/MbDCode/CADSystem.cpp b/MbDCode/CADSystem.cpp new file mode 100644 index 0000000..6f89500 --- /dev/null +++ b/MbDCode/CADSystem.cpp @@ -0,0 +1,522 @@ +#include +#include + +#include "CADSystem.h" +#include "CREATE.h" +#include "System.h" +#include "Item.h" +#include "Product.h" +#include "Constant.h" +#include "ZRotation.h" +#include "RevoluteJoint.h" +#include "CylindricalJoint.h" +#include "SystemSolver.h" +#include "Part.h" +#include "MarkerFrame.h" +#include "PartFrame.h" +#include "Time.h" +#include "StateData.h" + +using namespace MbD; +//using namespace CAD; + +void CADSystem::outputFor(AnalysisType type) +{ + auto str = std::to_string(mbdSystem->mbdTimeValue()); + this->logString(str); + mbdSystem->partsJointsMotionsForcesTorquesDo([](std::shared_ptr item) { + std::cout << std::endl; + std::cout << item->classname() << " " << item->getName() << std::endl; + auto data = item->stateData(); + std::cout << *data << std::endl; + }); +} + +void CADSystem::logString(std::string& str) +{ + std::cout << str << std::endl; +} + +void CADSystem::logString(double value) +{ +} + +void CADSystem::runOndselPiston() +{ + //Piston with easy input numbers for exact port from Smalltalk + //GEOAssembly calcCharacteristicDimensions must set mbdUnits to unity. + std::cout << "runOndselPiston" << std::endl; + auto& TheSystem = mbdSystem; + TheSystem->clear(); + std::string name = "TheSystem"; + TheSystem->setName(name); + std::cout << "TheSystem->getName() " << TheSystem->getName() << std::endl; + auto& systemSolver = TheSystem->systemSolver; + systemSolver->errorTolPosKine = 1.0e-6; + systemSolver->errorTolAccKine = 1.0e-6; + systemSolver->iterMaxPosKine = 25; + systemSolver->iterMaxAccKine = 25; + systemSolver->tstart = 0.0; + systemSolver->tend = 1.0; + systemSolver->hmin = 1.0e-9; + systemSolver->hmax = 1.0; + systemSolver->hout = 0.04; + systemSolver->corAbsTol = 1.0e-6; + systemSolver->corRelTol = 1.0e-6; + systemSolver->intAbsTol = 1.0e-6; + systemSolver->intRelTol = 1.0e-6; + systemSolver->iterMaxDyn = 4; + systemSolver->orderMax = 5; + systemSolver->translationLimit = 1.0e10; + systemSolver->rotationLimit = 0.5; + + std::string str; + FColDsptr qX, qE, qXdot, omeOpO, qXddot, qEddot; + FColDsptr rpmp; + FMatDsptr aApm; + FRowDsptr fullRow; + // + auto assembly1 = CREATE::With("/Assembly1"); + std::cout << "assembly1->getName() " << assembly1->getName() << std::endl; + assembly1->m = 0.0; + assembly1->aJ = std::make_shared>(ListD{ 0, 0, 0 }); + qX = std::make_shared>(ListD{ 0, 0, 0 }); + qE = std::make_shared>(ListD{ 0, 0, 0, 1 }); + assembly1->setqX(qX); + assembly1->setqE(qE); + qXdot = std::make_shared>(ListD{ 0, 0, 0 }); + omeOpO = std::make_shared>(ListD{ 0, 0, 0 }); + assembly1->setqXdot(qXdot); + assembly1->setomeOpO(omeOpO); + qXddot = std::make_shared>(ListD{ 0, 0, 0 }); + qEddot = std::make_shared>(ListD{ 0, 0, 0, 0 }); + assembly1->setqXddot(qXddot); + assembly1->setqEddot(qEddot); + std::cout << "assembly1->getqX() " << *assembly1->getqX() << std::endl; + std::cout << "assembly1->getqE() " << *assembly1->getqE() << std::endl; + TheSystem->addPart(assembly1); + { + auto& partFrame = assembly1->partFrame; + auto marker2 = CREATE::With("/Assembly1/Marker2"); + rpmp = std::make_shared>(ListD{ 0.0, 0.0, 0.0 }); + marker2->setrpmp(rpmp); + aApm = std::make_shared>(ListListD{ + { 1, 0, 0 }, + { 0, 1, 0 }, + { 0, 0, 1 } + }); + marker2->setaApm(aApm); + partFrame->addMarkerFrame(marker2); + // + auto marker1 = CREATE::With("/Assembly1/Marker1"); + rpmp = std::make_shared>(ListD{ 0.0, 3.0, 0.0 }); + marker1->setrpmp(rpmp); + aApm = std::make_shared>(ListListD{ + { 1, 0, 0 }, + { 0, 0, 1 }, + { 0, -1, 0 } + }); + marker1->setaApm(aApm); + partFrame->addMarkerFrame(marker1); + } + assembly1->asFixed(); + // + auto crankPart1 = CREATE::With("/Assembly1/Part1"); + std::cout << "crankPart1->getName() " << crankPart1->getName() << std::endl; + crankPart1->m = 1.0; + crankPart1->aJ = std::make_shared>(ListD{ 1, 1, 1 }); + qX = std::make_shared>(ListD{ 0.4, 0.0, -0.05 }); + qE = std::make_shared>(ListD{ 0.0, 0.0, 0.0, 1.0 }); + crankPart1->setqX(qX); + crankPart1->setqE(qE); + qXdot = std::make_shared>(ListD{ 0, 0, 0 }); + omeOpO = std::make_shared>(ListD{ 0, 0, 0 }); + crankPart1->setqXdot(qXdot); + crankPart1->setomeOpO(omeOpO); + qXddot = std::make_shared>(ListD{ 0, 0, 0 }); + qEddot = std::make_shared>(ListD{ 0, 0, 0, 0 }); + crankPart1->setqXddot(qXddot); + crankPart1->setqEddot(qEddot); + TheSystem->addPart(crankPart1); + { + auto& partFrame = crankPart1->partFrame; + auto marker1 = CREATE::With("/Assembly1/Part1/Marker1"); + rpmp = std::make_shared>(ListD{ -0.4, 0.0, 0.05 }); + marker1->setrpmp(rpmp); + aApm = std::make_shared>(ListListD{ + { 1, 0, 0 }, + { 0, 1, 0 }, + { 0, 0, 1 } + }); + marker1->setaApm(aApm); + partFrame->addMarkerFrame(marker1); + // + auto marker2 = CREATE::With("/Assembly1/Part1/Marker2"); + rpmp = std::make_shared>(ListD{ 0.4, 0.0, 0.05 }); + marker2->setrpmp(rpmp); + aApm = std::make_shared>(ListListD{ + { 1, 0, 0 }, + { 0, 1, 0 }, + { 0, 0, 1 } + }); + marker2->setaApm(aApm); + partFrame->addMarkerFrame(marker2); + } + // + auto conrodPart2 = CREATE::With("/Assembly1/Part2"); + std::cout << "conrodPart2->getName() " << conrodPart2->getName() << std::endl; + conrodPart2->m = 1.0; + conrodPart2->aJ = std::make_shared>(ListD{ 1, 1, 1 }); + qX = std::make_shared>(ListD{ 0.15, 0.1, 0.05 }); + qE = std::make_shared>(ListD{ 0.0, 0.0, 1.0, 0.0 }); + conrodPart2->setqX(qX); + conrodPart2->setqE(qE); + qXdot = std::make_shared>(ListD{ 0, 0, 0 }); + omeOpO = std::make_shared>(ListD{ 0, 0, 0 }); + conrodPart2->setqXdot(qXdot); + conrodPart2->setomeOpO(omeOpO); + qXddot = std::make_shared>(ListD{ 0, 0, 0 }); + qEddot = std::make_shared>(ListD{ 0, 0, 0, 0 }); + conrodPart2->setqXddot(qXddot); + conrodPart2->setqEddot(qEddot); + TheSystem->addPart(conrodPart2); + { + auto& partFrame = conrodPart2->partFrame; + auto marker1 = CREATE::With("/Assembly1/Part2/Marker1"); + rpmp = std::make_shared>(ListD{ -0.65, 0.0, -0.05 }); + marker1->setrpmp(rpmp); + aApm = std::make_shared>(ListListD{ + {1.0, 0.0, 0.0}, + {0.0, 1.0, 0.0}, + {0.0, 0.0, 1.0} + }); + marker1->setaApm(aApm); + partFrame->addMarkerFrame(marker1); + // + auto marker2 = CREATE::With("/Assembly1/Part2/Marker2"); + rpmp = std::make_shared>(ListD{ 0.65, 0.0, -0.05 }); + marker2->setrpmp(rpmp); + aApm = std::make_shared>(ListListD{ + {1.0, 0.0, 0.0}, + {0.0, 1.0, 0.0}, + {0.0, 0.0, 1.0} + }); + marker2->setaApm(aApm); + partFrame->addMarkerFrame(marker2); + } + // + auto pistonPart3 = CREATE::With("/Assembly1/Part3"); + std::cout << "pistonPart3->getName() " << pistonPart3->getName() << std::endl; + pistonPart3->m = 1.0; + pistonPart3->aJ = std::make_shared>(ListD{ 1, 1, 1 }); + qX = std::make_shared>(ListD{ -0.0, 1.5, 0.0 }); + qE = std::make_shared>(ListD{ 0.70710678118655, 0.70710678118655, 0.0, 0.0 }); + pistonPart3->setqX(qX); + pistonPart3->setqE(qE); + qXdot = std::make_shared>(ListD{ 0, 0, 0 }); + omeOpO = std::make_shared>(ListD{ 0, 0, 0 }); + pistonPart3->setqXdot(qXdot); + pistonPart3->setomeOpO(omeOpO); + qXddot = std::make_shared>(ListD{ 0, 0, 0 }); + qEddot = std::make_shared>(ListD{ 0, 0, 0, 0 }); + pistonPart3->setqXddot(qXddot); + pistonPart3->setqEddot(qEddot); + TheSystem->addPart(pistonPart3); + { + auto& partFrame = pistonPart3->partFrame; + auto marker1 = CREATE::With("/Assembly1/Part3/Marker1"); + rpmp = std::make_shared>(ListD{ -0.5, 0.0, 0.0 }); + marker1->setrpmp(rpmp); + aApm = std::make_shared>(ListListD{ + {0.0, 1.0, 0.0}, + {1.0, 0.0, 0.0}, + {0.0, 0.0, -1.0} + }); + marker1->setaApm(aApm); + partFrame->addMarkerFrame(marker1); + // + auto marker2 = CREATE::With("/Assembly1/Part3/Marker2"); + rpmp = std::make_shared>(ListD{ 0.5, 0.0, 0.0 }); + marker2->setrpmp(rpmp); + aApm = std::make_shared>(ListListD{ + {0.0, 0.0, 1.0}, + {1.0, 0.0, 0.0}, + {0.0, 1.0, 0.0} + }); + marker2->setaApm(aApm); + partFrame->addMarkerFrame(marker2); + } + // + auto revJoint1 = CREATE::With("/Assembly1/Joint1"); + std::cout << "revJoint1->getName() " << revJoint1->getName() << std::endl; + revJoint1->connectsItoJ(assembly1->partFrame->endFrame("/Assembly1/Marker2"), crankPart1->partFrame->endFrame("/Assembly1/Part1/Marker1")); + TheSystem->addJoint(revJoint1); + + auto revJoint2 = CREATE::With("/Assembly1/Joint2"); + std::cout << "revJoint2->getName() " << revJoint2->getName() << std::endl; + revJoint2->connectsItoJ(crankPart1->partFrame->endFrame("/Assembly1/Part1/Marker2"), conrodPart2->partFrame->endFrame("/Assembly1/Part2/Marker1")); + TheSystem->addJoint(revJoint2); + + auto revJoint3 = CREATE::With("/Assembly1/Joint3"); + std::cout << "revJoint3->getName() " << revJoint3->getName() << std::endl; + revJoint3->connectsItoJ(conrodPart2->partFrame->endFrame("/Assembly1/Part2/Marker2"), pistonPart3->partFrame->endFrame("/Assembly1/Part3/Marker1")); + TheSystem->addJoint(revJoint3); + + auto cylJoint4 = CREATE::With("/Assembly1/Joint4"); + std::cout << "cylJoint4->getName() " << cylJoint4->getName() << std::endl; + cylJoint4->connectsItoJ(pistonPart3->partFrame->endFrame("/Assembly1/Part3/Marker2"), assembly1->partFrame->endFrame("/Assembly1/Marker1")); + TheSystem->addJoint(cylJoint4); + + auto rotMotion1 = CREATE::With("/Assembly1/Motion1"); + rotMotion1->connectsItoJ(assembly1->partFrame->endFrame("/Assembly1/Marker2"), crankPart1->partFrame->endFrame("/Assembly1/Part1/Marker1")); + std::cout << "rotMotion1->getName() " << rotMotion1->getName() << std::endl; + auto omega = std::make_shared(6.2831853071796); + auto timeScale = std::make_shared(1.0); + auto time = std::make_shared(timeScale, TheSystem->time); + rotMotion1->phiBlk = std::make_shared(omega, time); + std::cout << "rotMotion1->phiBlk " << *(rotMotion1->phiBlk) << std::endl; + TheSystem->addJoint(rotMotion1); + // + TheSystem->runKINEMATIC(); +} + +void CADSystem::runPiston() +{ + std::cout << "runPiston" << std::endl; + auto& TheSystem = mbdSystem; + TheSystem->clear(); + std::string name = "TheSystem"; + TheSystem->setName(name); + std::cout << "TheSystem->getName() " << TheSystem->getName() << std::endl; + auto& systemSolver = TheSystem->systemSolver; + systemSolver->errorTolPosKine = 1.0e-6; + systemSolver->errorTolAccKine = 1.0e-6; + systemSolver->iterMaxPosKine = 25; + systemSolver->iterMaxAccKine = 25; + systemSolver->tstart = 0; + systemSolver->tend = 25.0; + systemSolver->hmin = 2.5e-8; + systemSolver->hmax = 25.0; + systemSolver->hout = 1.0; + systemSolver->corAbsTol = 1.0e-6; + systemSolver->corRelTol = 1.0e-6; + systemSolver->intAbsTol = 1.0e-6; + systemSolver->intRelTol = 1.0e-6; + systemSolver->iterMaxDyn = 4; + systemSolver->orderMax = 5; + systemSolver->translationLimit = 9.6058421285615e9; + systemSolver->rotationLimit = 0.5; + + std::string str; + FColDsptr qX, qE, qXdot, omeOpO, qXddot, qEddot; + FColDsptr rpmp; + FMatDsptr aApm; + FRowDsptr fullRow; + // + auto assembly1 = CREATE::With("/Assembly1"); + std::cout << "assembly1->getName() " << assembly1->getName() << std::endl; + assembly1->m = 0.0; + assembly1->aJ = std::make_shared>(ListD{ 0, 0, 0 }); + qX = std::make_shared>(ListD{ 0, 0, 0 }); + qE = std::make_shared>(ListD{ 0, 0, 0, 1 }); + assembly1->setqX(qX); + assembly1->setqE(qE); + qXdot = std::make_shared>(ListD{ 0, 0, 0 }); + omeOpO = std::make_shared>(ListD{ 0, 0, 0 }); + assembly1->setqXdot(qXdot); + assembly1->setomeOpO(omeOpO); + qXddot = std::make_shared>(ListD{ 0, 0, 0 }); + qEddot = std::make_shared>(ListD{ 0, 0, 0, 0 }); + assembly1->setqXddot(qXddot); + assembly1->setqEddot(qEddot); + std::cout << "assembly1->getqX() " << *assembly1->getqX() << std::endl; + std::cout << "assembly1->getqE() " << *assembly1->getqE() << std::endl; + TheSystem->addPart(assembly1); + { + auto& partFrame = assembly1->partFrame; + auto marker2 = CREATE::With("/Assembly1/Marker2"); + rpmp = std::make_shared>(ListD{ 0.0, 0.0, 0.0 }); + marker2->setrpmp(rpmp); + aApm = std::make_shared>(ListListD{ + { 1, 0, 0 }, + { 0, 1, 0 }, + { 0, 0, 1 } + }); + marker2->setaApm(aApm); + partFrame->addMarkerFrame(marker2); + // + auto marker1 = CREATE::With("/Assembly1/Marker1"); + rpmp = std::make_shared>(ListD{ 0.0, 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(); + // + auto crankPart1 = CREATE::With("/Assembly1/Part1"); + 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.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); + qXdot = std::make_shared>(ListD{ 0, 0.096568457800423, 0 }); + omeOpO = std::make_shared>(ListD{ 0, 0, 0.25132741228718 }); + crankPart1->setqXdot(qXdot); + crankPart1->setomeOpO(omeOpO); + qXddot = std::make_shared>(ListD{ 0, 0, 0 }); + qEddot = std::make_shared>(ListD{ 0, 0, 0, 0 }); + crankPart1->setqXddot(qXddot); + crankPart1->setqEddot(qEddot); + TheSystem->addPart(crankPart1); + { + auto& partFrame = crankPart1->partFrame; + auto marker1 = CREATE::With("/Assembly1/Part1/Marker1"); + rpmp = std::make_shared>(ListD{ -0.38423368514246, -2.6661567755108e-17, 0.048029210642807 }); + marker1->setrpmp(rpmp); + aApm = std::make_shared>(ListListD{ + { 1, 0, 0 }, + { 0, 1, 0 }, + { 0, 0, 1 } + }); + marker1->setaApm(aApm); + partFrame->addMarkerFrame(marker1); + // + auto marker2 = CREATE::With("/Assembly1/Part1/Marker2"); + rpmp = std::make_shared>(ListD{ 0.38423368514246, -2.6661567755108e-17, 0.048029210642807 }); + marker2->setrpmp(rpmp); + aApm = std::make_shared>(ListListD{ + { 1, 0, 0 }, + { 0, 1, 0 }, + { 0, 0, 1 } + }); + marker2->setaApm(aApm); + partFrame->addMarkerFrame(marker2); + } + // + auto conrodPart2 = CREATE::With("/Assembly1/Part2"); + std::cout << "conrodPart2->getName() " << conrodPart2->getName() << std::endl; + conrodPart2->m = 0.067815795134192; + conrodPart2->aJ = std::make_shared>(ListD{ 2.6072970063126e-4, 0.011784982468533, 0.011941420288912 }); + qX = std::make_shared>(ListD{ 0.38423368514246, 0.49215295678475, 0.048029210642807 }); + qE = std::make_shared>(ListD{ 0.0, 0.0, 0.89871703427292, 0.43852900965351 }); + conrodPart2->setqX(qX); + conrodPart2->setqE(qE); + qXdot = std::make_shared>(ListD{ 0, 0.19313691560085, 0 }); + omeOpO = std::make_shared>(ListD{ 1.670970041317e-34, 1.3045598281729e-34, -1.2731200314796e-35 }); + conrodPart2->setqXdot(qXdot); + conrodPart2->setomeOpO(omeOpO); + qXddot = std::make_shared>(ListD{ 0, 0, 0 }); + qEddot = std::make_shared>(ListD{ 0, 0, 0, 0 }); + conrodPart2->setqXddot(qXddot); + conrodPart2->setqEddot(qEddot); + TheSystem->addPart(conrodPart2); + { + auto& partFrame = conrodPart2->partFrame; + auto marker1 = CREATE::With("/Assembly1/Part2/Marker1"); + rpmp = std::make_shared>(ListD{ -0.6243797383565, 1.1997705489799e-16, -0.048029210642807 }); + marker1->setrpmp(rpmp); + aApm = std::make_shared>(ListListD{ + {1.0, 2.7755575615629e-16, 0.0}, + {-2.7755575615629e-16, 1.0, 0.0}, + {0.0, 0.0, 1.0} + }); + marker1->setaApm(aApm); + partFrame->addMarkerFrame(marker1); + // + auto marker2 = CREATE::With("/Assembly1/Part2/Marker2"); + rpmp = std::make_shared>(ListD{ 0.6243797383565, -2.1329254204087e-16, -0.048029210642807 }); + marker2->setrpmp(rpmp); + aApm = std::make_shared>(ListListD{ + {1.0, 2.4980018054066e-16, 2.2204460492503e-16}, + {-2.4980018054066e-16, 1.0, 4.1633363423443e-17}, + {-2.2204460492503e-16, -4.1633363423443e-17, 1.0} + }); + marker2->setaApm(aApm); + partFrame->addMarkerFrame(marker2); + } + // + auto pistonPart3 = CREATE::With("/Assembly1/Part3"); + 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.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); + qXdot = std::make_shared>(ListD{ -6.3364526821409e-32, 0.19313691560085, -1.933731897626e-34 }); + omeOpO = std::make_shared>(ListD{ 1.670970041317e-34, 1.3045598281729e-34, 1.8896472173894e-50 }); + pistonPart3->setqXdot(qXdot); + pistonPart3->setomeOpO(omeOpO); + qXddot = std::make_shared>(ListD{ 0, 0, 0 }); + qEddot = std::make_shared>(ListD{ 0, 0, 0, 0 }); + pistonPart3->setqXddot(qXddot); + pistonPart3->setqEddot(qEddot); + TheSystem->addPart(pistonPart3); + { + auto& partFrame = pistonPart3->partFrame; + auto marker1 = CREATE::With("/Assembly1/Part3/Marker1"); + rpmp = std::make_shared>(ListD{ -0.48029210642807, 7.6201599718927e-18, -2.816737703896e-17 }); + marker1->setrpmp(rpmp); + aApm = std::make_shared>(ListListD{ + {9.2444637330587e-33, 1.0, 2.2204460492503e-16}, + {1.0, -9.2444637330587e-33, -1.0785207688569e-32}, + {-1.0785207688569e-32, 2.2204460492503e-16, -1.0} + }); + marker1->setaApm(aApm); + partFrame->addMarkerFrame(marker1); + // + auto marker2 = CREATE::With("/Assembly1/Part3/Marker2"); + rpmp = std::make_shared>(ListD{ 0.48029210642807, 1.7618247880058e-17, 2.5155758471256e-17 }); + marker2->setrpmp(rpmp); + aApm = std::make_shared>(ListListD{ + {6.9388939039072e-18, -6.4146353042213e-50, 1.0}, + {1.0, -6.9388939039072e-18, 6.9388939039072e-18}, + {-6.9388939039072e-18, 1.0, -7.4837411882581e-50} + }); + marker2->setaApm(aApm); + partFrame->addMarkerFrame(marker2); + } + // + auto revJoint1 = CREATE::With("/Assembly1/Joint1"); + std::cout << "revJoint1->getName() " << revJoint1->getName() << std::endl; + revJoint1->connectsItoJ(assembly1->partFrame->endFrame("/Assembly1/Marker2"), crankPart1->partFrame->endFrame("/Assembly1/Part1/Marker1")); + TheSystem->addJoint(revJoint1); + + auto revJoint2 = CREATE::With("/Assembly1/Joint2"); + std::cout << "revJoint2->getName() " << revJoint2->getName() << std::endl; + revJoint2->connectsItoJ(crankPart1->partFrame->endFrame("/Assembly1/Part1/Marker2"), conrodPart2->partFrame->endFrame("/Assembly1/Part2/Marker1")); + TheSystem->addJoint(revJoint2); + + auto revJoint3 = CREATE::With("/Assembly1/Joint3"); + std::cout << "revJoint3->getName() " << revJoint3->getName() << std::endl; + revJoint3->connectsItoJ(conrodPart2->partFrame->endFrame("/Assembly1/Part2/Marker2"), pistonPart3->partFrame->endFrame("/Assembly1/Part3/Marker1")); + TheSystem->addJoint(revJoint3); + + auto cylJoint4 = CREATE::With("/Assembly1/Joint4"); + std::cout << "cylJoint4->getName() " << cylJoint4->getName() << std::endl; + cylJoint4->connectsItoJ(pistonPart3->partFrame->endFrame("/Assembly1/Part3/Marker2"), assembly1->partFrame->endFrame("/Assembly1/Marker1")); + TheSystem->addJoint(cylJoint4); + + auto rotMotion1 = CREATE::With("/Assembly1/Motion1"); + rotMotion1->connectsItoJ(assembly1->partFrame->endFrame("/Assembly1/Marker2"), crankPart1->partFrame->endFrame("/Assembly1/Part1/Marker1")); + std::cout << "rotMotion1->getName() " << rotMotion1->getName() << std::endl; + auto omega = std::make_shared(6.2831853071796); + auto timeScale = std::make_shared(0.04); + auto time = std::make_shared(timeScale, TheSystem->time); + rotMotion1->phiBlk = std::make_shared(omega, time); + std::cout << "rotMotion1->phiBlk " << *(rotMotion1->phiBlk) << std::endl; + TheSystem->addJoint(rotMotion1); + // + TheSystem->runKINEMATIC(); +} + +void CADSystem::postMbDrun() +{ +} diff --git a/MbDCode/CADSystem.h b/MbDCode/CADSystem.h new file mode 100644 index 0000000..90618fe --- /dev/null +++ b/MbDCode/CADSystem.h @@ -0,0 +1,31 @@ +#pragma once +#include + +#include "System.h" + +//using namespace MbD; + +//namespace CAD { +namespace MbD { + //class System; + + class CADSystem + { + // + public: + CADSystem() { + mbdSystem->externalSystem = this; + } + + void outputFor(AnalysisType type); + void logString(std::string& str); + void logString(double value); + void runOndselPiston(); + void runPiston(); + void postMbDrun(); + + std::shared_ptr mbdSystem = std::make_shared(); + + }; +} + diff --git a/MbDCode/Constant.cpp b/MbDCode/Constant.cpp index 6648041..d11c445 100644 --- a/MbDCode/Constant.cpp +++ b/MbDCode/Constant.cpp @@ -15,12 +15,12 @@ Symsptr Constant::differentiateWRT(Symsptr sptr, Symsptr var) return std::make_shared(0.0); } -bool MbD::Constant::isConstant() +bool Constant::isConstant() { return true; } -std::ostream& MbD::Constant::printOn(std::ostream& s) const +std::ostream& Constant::printOn(std::ostream& s) const { return s << this->value; } diff --git a/MbDCode/Constraint.cpp b/MbDCode/Constraint.cpp index 06d4351a..c4eba08 100644 --- a/MbDCode/Constraint.cpp +++ b/MbDCode/Constraint.cpp @@ -22,7 +22,7 @@ void Constraint::initialize() name = std::to_string(nanoseconds); } -void MbD::Constraint::postInput() +void Constraint::postInput() { lam = 0.0; Item::postInput(); @@ -38,14 +38,14 @@ Item* Constraint::getOwner() return owner; } -void MbD::Constraint::prePosIC() +void Constraint::prePosIC() { lam = 0.0; iG = -1; Item::prePosIC(); } -void MbD::Constraint::prePosKine() +void Constraint::prePosKine() { //"Preserve lam calculated from AccIC for possible use by DynIntegrator if system is not kinematic." auto lamOld = lam; @@ -53,80 +53,80 @@ void MbD::Constraint::prePosKine() lam = lamOld; } -void MbD::Constraint::fillEssenConstraints(std::shared_ptr sptr, std::shared_ptr>> essenConstraints) +void Constraint::fillEssenConstraints(std::shared_ptr sptr, std::shared_ptr>> essenConstraints) { - if (this->type() == MbD::essential) { + if (this->type() == essential) { essenConstraints->push_back(sptr); } } -void MbD::Constraint::fillDispConstraints(std::shared_ptr sptr, std::shared_ptr>> dispConstraints) +void Constraint::fillDispConstraints(std::shared_ptr sptr, std::shared_ptr>> dispConstraints) { - if (this->type() == MbD::displacement) { + if (this->type() == displacement) { dispConstraints->push_back(sptr); } } -void MbD::Constraint::fillPerpenConstraints(std::shared_ptr sptr, std::shared_ptr>> perpenConstraints) +void Constraint::fillPerpenConstraints(std::shared_ptr sptr, std::shared_ptr>> perpenConstraints) { - if (this->type() == MbD::perpendicular) { + if (this->type() == perpendicular) { perpenConstraints->push_back(sptr); } } -void MbD::Constraint::fillRedundantConstraints(std::shared_ptr sptr, std::shared_ptr>> redunConstraints) +void Constraint::fillRedundantConstraints(std::shared_ptr sptr, std::shared_ptr>> redunConstraints) { - if (this->type() == MbD::redundant) { + if (this->type() == redundant) { redunConstraints->push_back(sptr); } } -void MbD::Constraint::fillConstraints(std::shared_ptr sptr, std::shared_ptr>> allConstraints) +void Constraint::fillConstraints(std::shared_ptr sptr, std::shared_ptr>> allConstraints) { allConstraints->push_back(sptr); } -MbD::ConstraintType MbD::Constraint::type() +ConstraintType Constraint::type() { - return MbD::essential; + return essential; } -void MbD::Constraint::fillqsulam(FColDsptr col) +void Constraint::fillqsulam(FColDsptr col) { col->at(iG) = lam; } -void MbD::Constraint::setqsulam(FColDsptr col) +void Constraint::setqsulam(FColDsptr col) { lam = col->at(iG); } -void MbD::Constraint::setqsudotlam(FColDsptr col) +void Constraint::setqsudotlam(FColDsptr col) { lam = col->at(iG); } -void MbD::Constraint::fillPosICError(FColDsptr col) +void Constraint::fillPosICError(FColDsptr col) { col->at(iG) += aG; } -void MbD::Constraint::removeRedundantConstraints(std::shared_ptr> redundantEqnNos) +void Constraint::removeRedundantConstraints(std::shared_ptr> redundantEqnNos) { //My owner should handle this. assert(false); } -void MbD::Constraint::reactivateRedundantConstraints() +void Constraint::reactivateRedundantConstraints() { //My owner should handle this. assert(false); } -bool MbD::Constraint::isRedundant() +bool Constraint::isRedundant() { return false; } -void MbD::Constraint::outputStates() +void Constraint::outputStates() { Item::outputStates(); std::stringstream ss; @@ -137,29 +137,37 @@ void MbD::Constraint::outputStates() this->logString(str); } -void MbD::Constraint::preDyn() +void Constraint::preDyn() { mu = 0.0; } -void MbD::Constraint::fillPosKineError(FColDsptr col) +void Constraint::fillPosKineError(FColDsptr col) { col->atiplusNumber(iG, aG); } -void MbD::Constraint::preAccIC() +void Constraint::preAccIC() { lam = 0.0; Item::preAccIC(); } -void MbD::Constraint::fillAccICIterJacob(SpMatDsptr mat) +void Constraint::fillAccICIterJacob(SpMatDsptr mat) { //"Same as velIC." this->fillVelICJacob(mat); } -void MbD::Constraint::setqsuddotlam(FColDsptr qsudotlam) +void Constraint::setqsuddotlam(FColDsptr col) +{ + lam = col->at(iG); +} + +void Constraint::addToJointForceI(FColDsptr col) +{ +} + +void Constraint::addToJointTorqueI(FColDsptr col) { - lam = qsudotlam->at(iG); } diff --git a/MbDCode/Constraint.h b/MbDCode/Constraint.h index 87c61d4..115e776 100644 --- a/MbDCode/Constraint.h +++ b/MbDCode/Constraint.h @@ -22,7 +22,7 @@ namespace MbD { virtual void fillPerpenConstraints(std::shared_ptr sptr, std::shared_ptr>> perpenConstraints); virtual void fillRedundantConstraints(std::shared_ptr sptr, std::shared_ptr>> redunConstraints); virtual void fillConstraints(std::shared_ptr sptr, std::shared_ptr>> allConstraints); - virtual MbD::ConstraintType type(); + virtual ConstraintType type(); void fillqsulam(FColDsptr col) override; void setqsulam(FColDsptr col) override; void setqsudotlam(FColDsptr col) override; @@ -35,13 +35,14 @@ namespace MbD { void fillPosKineError(FColDsptr col) override; void preAccIC() override; void fillAccICIterJacob(SpMatDsptr mat) override; - void setqsuddotlam(FColDsptr qsudotlam) override; - + void setqsuddotlam(FColDsptr col) override; + virtual void addToJointForceI(FColDsptr col); + virtual void addToJointTorqueI(FColDsptr col); + 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/CylindricalJoint.cpp b/MbDCode/CylindricalJoint.cpp index 96de886..3eb317a 100644 --- a/MbDCode/CylindricalJoint.cpp +++ b/MbDCode/CylindricalJoint.cpp @@ -12,7 +12,7 @@ CylindricalJoint::CylindricalJoint() { CylindricalJoint::CylindricalJoint(const char* str) : Joint(str) { } -void MbD::CylindricalJoint::initializeGlobally() +void CylindricalJoint::initializeGlobally() { if (constraints->empty()) { @@ -20,7 +20,7 @@ void MbD::CylindricalJoint::initializeGlobally() addConstraint(CREATE::ConstraintWith(frmI, frmJ, 1)); addConstraint(CREATE::ConstraintWith(frmI, frmJ, 2, 0)); addConstraint(CREATE::ConstraintWith(frmI, frmJ, 2, 1)); - System::getInstance().hasChanged = true; + this->root()->hasChanged = true; } else { Joint::initializeGlobally(); diff --git a/MbDCode/DataPosVelAcc.cpp b/MbDCode/DataPosVelAcc.cpp new file mode 100644 index 0000000..7fda007 --- /dev/null +++ b/MbDCode/DataPosVelAcc.cpp @@ -0,0 +1,17 @@ +#include + +#include "DataPosVelAcc.h" + +using namespace MbD; + +std::ostream& DataPosVelAcc::printOn(std::ostream& s) const +{ + s << "refData = " << *refData << std::endl; + s << "rFfF = " << *rFfF << std::endl; + s << "vFfF = " << *vFfF << std::endl; + s << "omeFfF = " << *omeFfF << std::endl; + s << "aFfF = " << *aFfF << std::endl; + s << "alpFfF = " << *alpFfF << std::endl; + s << "aAFf = " << *aAFf; + return s; +} diff --git a/MbDCode/DataPosVelAcc.h b/MbDCode/DataPosVelAcc.h new file mode 100644 index 0000000..b60dab0 --- /dev/null +++ b/MbDCode/DataPosVelAcc.h @@ -0,0 +1,17 @@ +#pragma once + +#include "StateData.h" + +namespace MbD { + class DataPosVelAcc : public StateData + { + //refData rFfF aAFf vFfF omeFfF aFfF alpFfF + public: + std::ostream& printOn(std::ostream& s) const override; + + std::shared_ptr refData; + FColDsptr rFfF, vFfF, omeFfF, aFfF, alpFfF; + FMatDsptr aAFf; + }; +} + diff --git a/MbDCode/DiagonalMatrix.h b/MbDCode/DiagonalMatrix.h index 52569a8..563e82b 100644 --- a/MbDCode/DiagonalMatrix.h +++ b/MbDCode/DiagonalMatrix.h @@ -26,6 +26,8 @@ namespace MbD { double sumOfSquares() override; int numberOfElements() override; void zeroSelf() override; + std::ostream& printOn(std::ostream& s) const override; + }; template inline void DiagonalMatrix::atiputDiagonalMatrix(int i, std::shared_ptr> diagMat) @@ -94,5 +96,17 @@ namespace MbD { this->at(i) = 0.0; } } + template + inline std::ostream& DiagonalMatrix::printOn(std::ostream& s) const + { + s << "DiagMat["; + s << this->at(0); + for (int i = 1; i < this->size(); i++) + { + s << ", " << this->at(i); + } + s << "]"; + return s; + } } diff --git a/MbDCode/DifferenceOperator.cpp b/MbDCode/DifferenceOperator.cpp index 84a49c3..3a4a343 100644 --- a/MbDCode/DifferenceOperator.cpp +++ b/MbDCode/DifferenceOperator.cpp @@ -17,7 +17,7 @@ std::shared_ptr> DifferenceOperator::OneOverFactorials = []() { return oneOverFactorials; }(); -void MbD::DifferenceOperator::calcOperatorMatrix() +void DifferenceOperator::calcOperatorMatrix() { //Compute operatorMatrix such that //value(time) : = (operatorMatrix at : 1) timesColumn : series. @@ -32,28 +32,28 @@ void MbD::DifferenceOperator::calcOperatorMatrix() } } -void MbD::DifferenceOperator::initialize() +void DifferenceOperator::initialize() { } -void MbD::DifferenceOperator::setiStep(int i) +void DifferenceOperator::setiStep(int i) { iStep = i; } -void MbD::DifferenceOperator::setorder(int o) +void DifferenceOperator::setorder(int o) { order = o; } -void MbD::DifferenceOperator::instantiateTaylorMatrix() +void DifferenceOperator::instantiateTaylorMatrix() { if (taylorMatrix == nullptr || (taylorMatrix->nrow() != (order + 1))) { taylorMatrix = std::make_shared>(order + 1, order + 1); } } -void MbD::DifferenceOperator::formTaylorRowwithTimeNodederivative(int i, int ii, int k) +void DifferenceOperator::formTaylorRowwithTimeNodederivative(int i, int ii, int k) { //| rowi hi hipower aij | auto& rowi = taylorMatrix->at(i); @@ -73,7 +73,7 @@ void MbD::DifferenceOperator::formTaylorRowwithTimeNodederivative(int i, int ii, } } -void MbD::DifferenceOperator::settime(double t) +void DifferenceOperator::settime(double t) { time = t; } diff --git a/MbDCode/DirectionCosineConstraintIJ.cpp b/MbDCode/DirectionCosineConstraintIJ.cpp index 0fcf760..94bff3e 100644 --- a/MbDCode/DirectionCosineConstraintIJ.cpp +++ b/MbDCode/DirectionCosineConstraintIJ.cpp @@ -16,12 +16,12 @@ void DirectionCosineConstraintIJ::initialize() initaAijIeJe(); } -void MbD::DirectionCosineConstraintIJ::initializeLocally() +void DirectionCosineConstraintIJ::initializeLocally() { aAijIeJe->initializeLocally(); } -void MbD::DirectionCosineConstraintIJ::initializeGlobally() +void DirectionCosineConstraintIJ::initializeGlobally() { aAijIeJe->initializeGlobally(); } @@ -31,41 +31,41 @@ void DirectionCosineConstraintIJ::initaAijIeJe() aAijIeJe = CREATE::With(frmI, frmJ, axisI, axisJ); } -void MbD::DirectionCosineConstraintIJ::postInput() +void DirectionCosineConstraintIJ::postInput() { aAijIeJe->postInput(); Constraint::postInput(); } -void MbD::DirectionCosineConstraintIJ::calcPostDynCorrectorIteration() +void DirectionCosineConstraintIJ::calcPostDynCorrectorIteration() { aG = aAijIeJe->aAijIeJe - aConstant; } -void MbD::DirectionCosineConstraintIJ::prePosIC() +void DirectionCosineConstraintIJ::prePosIC() { aAijIeJe->prePosIC(); Constraint::prePosIC(); } -void MbD::DirectionCosineConstraintIJ::postPosICIteration() +void DirectionCosineConstraintIJ::postPosICIteration() { aAijIeJe->postPosICIteration(); Item::postPosICIteration(); } -MbD::ConstraintType MbD::DirectionCosineConstraintIJ::type() +ConstraintType DirectionCosineConstraintIJ::type() { - return MbD::perpendicular; + return perpendicular; } -void MbD::DirectionCosineConstraintIJ::preVelIC() +void DirectionCosineConstraintIJ::preVelIC() { aAijIeJe->preVelIC(); Item::preVelIC(); } -void MbD::DirectionCosineConstraintIJ::preAccIC() +void DirectionCosineConstraintIJ::preAccIC() { aAijIeJe->preAccIC(); Constraint::preAccIC(); diff --git a/MbDCode/DirectionCosineConstraintIJ.h b/MbDCode/DirectionCosineConstraintIJ.h index fe71902..519dabe 100644 --- a/MbDCode/DirectionCosineConstraintIJ.h +++ b/MbDCode/DirectionCosineConstraintIJ.h @@ -18,7 +18,7 @@ namespace MbD { void calcPostDynCorrectorIteration() override; void prePosIC() override; void postPosICIteration() override; - MbD::ConstraintType type() override; + ConstraintType type() override; void preVelIC() override; void preAccIC() override; diff --git a/MbDCode/DirectionCosineConstraintIqcJc.cpp b/MbDCode/DirectionCosineConstraintIqcJc.cpp index 7bf95d3..a65cce2 100644 --- a/MbDCode/DirectionCosineConstraintIqcJc.cpp +++ b/MbDCode/DirectionCosineConstraintIqcJc.cpp @@ -15,7 +15,7 @@ void DirectionCosineConstraintIqcJc::initaAijIeJe() aAijIeJe = CREATE::With(frmI, frmJ, axisI, axisJ); } -void MbD::DirectionCosineConstraintIqcJc::calcPostDynCorrectorIteration() +void DirectionCosineConstraintIqcJc::calcPostDynCorrectorIteration() { DirectionCosineConstraintIJ::calcPostDynCorrectorIteration(); auto aAijIeqJe = std::static_pointer_cast(aAijIeJe); @@ -23,36 +23,36 @@ void MbD::DirectionCosineConstraintIqcJc::calcPostDynCorrectorIteration() ppGpEIpEI = aAijIeqJe->ppAijIeJepEIpEI; } -void MbD::DirectionCosineConstraintIqcJc::useEquationNumbers() +void DirectionCosineConstraintIqcJc::useEquationNumbers() { iqEI = std::static_pointer_cast(frmI)->iqE(); } -void MbD::DirectionCosineConstraintIqcJc::fillPosICError(FColDsptr col) +void DirectionCosineConstraintIqcJc::fillPosICError(FColDsptr col) { Constraint::fillPosICError(col); col->atiplusFullVectortimes(iqEI, pGpEI, lam); } -void MbD::DirectionCosineConstraintIqcJc::fillPosICJacob(SpMatDsptr mat) +void DirectionCosineConstraintIqcJc::fillPosICJacob(SpMatDsptr mat) { mat->atijplusFullRow(iG, iqEI, pGpEI); mat->atijplusFullColumn(iqEI, iG, pGpEI->transpose()); mat->atijplusFullMatrixtimes(iqEI, iqEI, ppGpEIpEI, lam); } -void MbD::DirectionCosineConstraintIqcJc::fillPosKineJacob(SpMatDsptr mat) +void DirectionCosineConstraintIqcJc::fillPosKineJacob(SpMatDsptr mat) { mat->atijplusFullRow(iG, iqEI, pGpEI); } -void MbD::DirectionCosineConstraintIqcJc::fillVelICJacob(SpMatDsptr mat) +void DirectionCosineConstraintIqcJc::fillVelICJacob(SpMatDsptr mat) { mat->atijplusFullRow(iG, iqEI, pGpEI); mat->atijplusFullColumn(iqEI, iG, pGpEI->transpose()); } -void MbD::DirectionCosineConstraintIqcJc::fillAccICIterError(FColDsptr col) +void DirectionCosineConstraintIqcJc::fillAccICIterError(FColDsptr col) { col->atiplusFullVector(iqEI, pGpEI->times(lam)); auto efrmIqc = std::static_pointer_cast(frmI); @@ -61,3 +61,15 @@ void MbD::DirectionCosineConstraintIqcJc::fillAccICIterError(FColDsptr col) sum += qEdotI->transposeTimesFullColumn(ppGpEIpEI->timesFullColumn(qEdotI)); col->atiplusNumber(iG, sum); } + +void DirectionCosineConstraintIqcJc::addToJointForceI(FColDsptr col) +{ +} + +void DirectionCosineConstraintIqcJc::addToJointTorqueI(FColDsptr jointTorque) +{ + auto aBOIp = frmI->aBOp(); + auto lampGpE = pGpEI->transpose()->times(lam); + auto c2Torque = aBOIp->timesFullColumn(lampGpE); + jointTorque->equalSelfPlusFullColumntimes(c2Torque, 0.5); +} diff --git a/MbDCode/DirectionCosineConstraintIqcJc.h b/MbDCode/DirectionCosineConstraintIqcJc.h index b2d2f88..b55f7e6 100644 --- a/MbDCode/DirectionCosineConstraintIqcJc.h +++ b/MbDCode/DirectionCosineConstraintIqcJc.h @@ -16,6 +16,8 @@ namespace MbD { void fillPosKineJacob(SpMatDsptr mat) override; void fillVelICJacob(SpMatDsptr mat) override; void fillAccICIterError(FColDsptr col) override; + void addToJointForceI(FColDsptr col) override; + void addToJointTorqueI(FColDsptr col) override; FRowDsptr pGpEI; FMatDsptr ppGpEIpEI; diff --git a/MbDCode/DirectionCosineConstraintIqcJqc.cpp b/MbDCode/DirectionCosineConstraintIqcJqc.cpp index 6381d57..0135e33 100644 --- a/MbDCode/DirectionCosineConstraintIqcJqc.cpp +++ b/MbDCode/DirectionCosineConstraintIqcJqc.cpp @@ -15,7 +15,7 @@ void DirectionCosineConstraintIqcJqc::initaAijIeJe() aAijIeJe = CREATE::With(frmI, frmJ, axisI, axisJ); } -void MbD::DirectionCosineConstraintIqcJqc::calcPostDynCorrectorIteration() +void DirectionCosineConstraintIqcJqc::calcPostDynCorrectorIteration() { DirectionCosineConstraintIqcJc::calcPostDynCorrectorIteration(); auto aAijIeqJqe = std::static_pointer_cast(aAijIeJe); @@ -24,19 +24,19 @@ void MbD::DirectionCosineConstraintIqcJqc::calcPostDynCorrectorIteration() ppGpEJpEJ = aAijIeqJqe->ppAijIeJepEJpEJ; } -void MbD::DirectionCosineConstraintIqcJqc::useEquationNumbers() +void DirectionCosineConstraintIqcJqc::useEquationNumbers() { DirectionCosineConstraintIqcJc::useEquationNumbers(); iqEJ = std::static_pointer_cast(frmJ)->iqE(); } -void MbD::DirectionCosineConstraintIqcJqc::fillPosICError(FColDsptr col) +void DirectionCosineConstraintIqcJqc::fillPosICError(FColDsptr col) { DirectionCosineConstraintIqcJc::fillPosICError(col); col->atiplusFullVectortimes(iqEJ, pGpEJ, lam); } -void MbD::DirectionCosineConstraintIqcJqc::fillPosICJacob(SpMatDsptr mat) +void DirectionCosineConstraintIqcJqc::fillPosICJacob(SpMatDsptr mat) { DirectionCosineConstraintIqcJc::fillPosICJacob(mat); mat->atijplusFullRow(iG, iqEJ, pGpEJ); @@ -47,20 +47,20 @@ void MbD::DirectionCosineConstraintIqcJqc::fillPosICJacob(SpMatDsptr mat) mat->atijplusFullMatrixtimes(iqEJ, iqEJ, ppGpEJpEJ, lam); } -void MbD::DirectionCosineConstraintIqcJqc::fillPosKineJacob(SpMatDsptr mat) +void DirectionCosineConstraintIqcJqc::fillPosKineJacob(SpMatDsptr mat) { DirectionCosineConstraintIqcJc::fillPosKineJacob(mat); mat->atijplusFullRow(iG, iqEJ, pGpEJ); } -void MbD::DirectionCosineConstraintIqcJqc::fillVelICJacob(SpMatDsptr mat) +void DirectionCosineConstraintIqcJqc::fillVelICJacob(SpMatDsptr mat) { DirectionCosineConstraintIqcJc::fillVelICJacob(mat); mat->atijplusFullRow(iG, iqEJ, pGpEJ); mat->atijplusFullColumn(iqEJ, iG, pGpEJ->transpose()); } -void MbD::DirectionCosineConstraintIqcJqc::fillAccICIterError(FColDsptr col) +void DirectionCosineConstraintIqcJqc::fillAccICIterError(FColDsptr col) { DirectionCosineConstraintIqcJc::fillAccICIterError(col); col->atiplusFullVectortimes(iqEJ, pGpEJ, lam); diff --git a/MbDCode/DirectionCosineConstraintIqctJqc.cpp b/MbDCode/DirectionCosineConstraintIqctJqc.cpp index 5f3b251..07f15a7 100644 --- a/MbDCode/DirectionCosineConstraintIqctJqc.cpp +++ b/MbDCode/DirectionCosineConstraintIqctJqc.cpp @@ -14,23 +14,23 @@ void DirectionCosineConstraintIqctJqc::initaAijIeJe() aAijIeJe = CREATE::With(frmI, frmJ, axisI, axisJ); } -MbD::ConstraintType MbD::DirectionCosineConstraintIqctJqc::type() +ConstraintType DirectionCosineConstraintIqctJqc::type() { - return MbD::essential; + return essential; } -void MbD::DirectionCosineConstraintIqctJqc::preVelIC() +void DirectionCosineConstraintIqctJqc::preVelIC() { DirectionCosineConstraintIJ::preVelIC(); pGpt = std::static_pointer_cast(aAijIeJe)->pAijIeJept; } -void MbD::DirectionCosineConstraintIqctJqc::fillVelICError(FColDsptr col) +void DirectionCosineConstraintIqctJqc::fillVelICError(FColDsptr col) { col->atiminusNumber(iG, pGpt); } -void MbD::DirectionCosineConstraintIqctJqc::preAccIC() +void DirectionCosineConstraintIqctJqc::preAccIC() { DirectionCosineConstraintIJ::preAccIC(); ppGpEIpt = std::static_pointer_cast(aAijIeJe)->ppAijIeJepEIpt; @@ -38,7 +38,7 @@ void MbD::DirectionCosineConstraintIqctJqc::preAccIC() ppGptpt = std::static_pointer_cast(aAijIeJe)->ppAijIeJeptpt; } -void MbD::DirectionCosineConstraintIqctJqc::fillAccICIterError(FColDsptr col) +void DirectionCosineConstraintIqctJqc::fillAccICIterError(FColDsptr col) { DirectionCosineConstraintIqcJqc::fillAccICIterError(col); auto efrmIqc = std::static_pointer_cast(frmI); diff --git a/MbDCode/DirectionCosineConstraintIqctJqc.h b/MbDCode/DirectionCosineConstraintIqctJqc.h index 458fa78..e579114 100644 --- a/MbDCode/DirectionCosineConstraintIqctJqc.h +++ b/MbDCode/DirectionCosineConstraintIqctJqc.h @@ -9,7 +9,7 @@ namespace MbD { public: DirectionCosineConstraintIqctJqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj); void initaAijIeJe() override; - MbD::ConstraintType type() override; + ConstraintType type() override; void preVelIC() override; void fillVelICError(FColDsptr col) override; void preAccIC() override; diff --git a/MbDCode/DirectionCosineIecJec.cpp b/MbDCode/DirectionCosineIecJec.cpp index 3f40ab4..1e166bb 100644 --- a/MbDCode/DirectionCosineIecJec.cpp +++ b/MbDCode/DirectionCosineIecJec.cpp @@ -15,38 +15,38 @@ DirectionCosineIecJec::DirectionCosineIecJec(EndFrmcptr frmi, EndFrmcptr frmj, i } -void MbD::DirectionCosineIecJec::calcPostDynCorrectorIteration() +void DirectionCosineIecJec::calcPostDynCorrectorIteration() { aAjOIe = frmI->aAjOe(axisI); aAjOJe = frmJ->aAjOe(axisJ); aAijIeJe = aAjOIe->dot(aAjOJe); } -FRowDsptr MbD::DirectionCosineIecJec::pvaluepXJ() +FRowDsptr DirectionCosineIecJec::pvaluepXJ() { assert(false); return FRowDsptr(); } -FRowDsptr MbD::DirectionCosineIecJec::pvaluepEJ() +FRowDsptr DirectionCosineIecJec::pvaluepEJ() { assert(false); return FRowDsptr(); } -FMatDsptr MbD::DirectionCosineIecJec::ppvaluepXJpEK() +FMatDsptr DirectionCosineIecJec::ppvaluepXJpEK() { assert(false); return FMatDsptr(); } -FMatDsptr MbD::DirectionCosineIecJec::ppvaluepEJpEK() +FMatDsptr DirectionCosineIecJec::ppvaluepEJpEK() { assert(false); return FMatDsptr(); } -FMatDsptr MbD::DirectionCosineIecJec::ppvaluepEJpEJ() +FMatDsptr DirectionCosineIecJec::ppvaluepEJpEJ() { assert(false); return FMatDsptr(); diff --git a/MbDCode/DirectionCosineIeqcJec.cpp b/MbDCode/DirectionCosineIeqcJec.cpp index 3975ae7..c886412 100644 --- a/MbDCode/DirectionCosineIeqcJec.cpp +++ b/MbDCode/DirectionCosineIeqcJec.cpp @@ -19,12 +19,12 @@ void DirectionCosineIeqcJec::initialize() ppAijIeJepEIpEI = std::make_shared>(4, 4); } -void MbD::DirectionCosineIeqcJec::initializeGlobally() +void DirectionCosineIeqcJec::initializeGlobally() { ppAjOIepEIpEI = std::static_pointer_cast(frmI)->ppAjOepEpE(axisI); } -void MbD::DirectionCosineIeqcJec::calcPostDynCorrectorIteration() +void DirectionCosineIeqcJec::calcPostDynCorrectorIteration() { DirectionCosineIecJec::calcPostDynCorrectorIteration(); pAjOIepEIT = std::static_pointer_cast(frmI)->pAjOepET(axisI); diff --git a/MbDCode/DirectionCosineIeqcJeqc.cpp b/MbDCode/DirectionCosineIeqcJeqc.cpp index 902b114..6aeedb7 100644 --- a/MbDCode/DirectionCosineIeqcJeqc.cpp +++ b/MbDCode/DirectionCosineIeqcJeqc.cpp @@ -20,13 +20,13 @@ void DirectionCosineIeqcJeqc::initialize() ppAijIeJepEJpEJ = std::make_shared>(4, 4); } -void MbD::DirectionCosineIeqcJeqc::initializeGlobally() +void DirectionCosineIeqcJeqc::initializeGlobally() { DirectionCosineIeqcJec::initializeGlobally(); ppAjOJepEJpEJ = std::static_pointer_cast(frmJ)->ppAjOepEpE(axisJ); } -void MbD::DirectionCosineIeqcJeqc::calcPostDynCorrectorIteration() +void DirectionCosineIeqcJeqc::calcPostDynCorrectorIteration() { DirectionCosineIeqcJec::calcPostDynCorrectorIteration(); pAjOJepEJT = std::static_pointer_cast(frmJ)->pAjOepET(axisJ); @@ -54,12 +54,12 @@ void MbD::DirectionCosineIeqcJeqc::calcPostDynCorrectorIteration() ppAijIeJepEJpEJ->symLowerWithUpper(); } -FRowDsptr MbD::DirectionCosineIeqcJeqc::pvaluepEJ() +FRowDsptr DirectionCosineIeqcJeqc::pvaluepEJ() { return pAijIeJepEJ; } -FMatDsptr MbD::DirectionCosineIeqcJeqc::ppvaluepEJpEJ() +FMatDsptr DirectionCosineIeqcJeqc::ppvaluepEJpEJ() { return ppAijIeJepEJpEJ; } diff --git a/MbDCode/DirectionCosineIeqctJeqc.cpp b/MbDCode/DirectionCosineIeqctJeqc.cpp index 92592ef..c64dc55 100644 --- a/MbDCode/DirectionCosineIeqctJeqc.cpp +++ b/MbDCode/DirectionCosineIeqctJeqc.cpp @@ -20,12 +20,12 @@ void DirectionCosineIeqctJeqc::initialize() ppAijIeJepEJpt = std::make_shared>(4); } -void MbD::DirectionCosineIeqctJeqc::initializeGlobally() +void DirectionCosineIeqctJeqc::initializeGlobally() { ppAjOJepEJpEJ = std::static_pointer_cast(frmJ)->ppAjOepEpE(axisJ); } -void MbD::DirectionCosineIeqctJeqc::calcPostDynCorrectorIteration() +void DirectionCosineIeqctJeqc::calcPostDynCorrectorIteration() { //"ppAjOIepEIpEI is not longer constant and must be set before any calculation." @@ -33,19 +33,19 @@ void MbD::DirectionCosineIeqctJeqc::calcPostDynCorrectorIteration() DirectionCosineIeqcJeqc::calcPostDynCorrectorIteration(); } -void MbD::DirectionCosineIeqctJeqc::preVelIC() +void DirectionCosineIeqctJeqc::preVelIC() { Item::preVelIC(); auto pAjOIept = std::static_pointer_cast(frmI)->pAjOept(axisI); pAijIeJept = pAjOIept->dot(aAjOJe); } -double MbD::DirectionCosineIeqctJeqc::pvaluept() +double DirectionCosineIeqctJeqc::pvaluept() { return pAijIeJept; } -void MbD::DirectionCosineIeqctJeqc::preAccIC() +void DirectionCosineIeqctJeqc::preAccIC() { //| ppAjOIepEITpt ppAjOIeptpt ppAjOIepEITpti pAjOIept | Item::preAccIC(); diff --git a/MbDCode/DiscontinuityError.cpp b/MbDCode/DiscontinuityError.cpp index 3b71e36..ece43f2 100644 --- a/MbDCode/DiscontinuityError.cpp +++ b/MbDCode/DiscontinuityError.cpp @@ -1,7 +1,3 @@ #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 index 850f6f8..19c00d0 100644 --- a/MbDCode/DiscontinuityError.h +++ b/MbDCode/DiscontinuityError.h @@ -2,14 +2,25 @@ #include #include #include +#include "enum.h" namespace MbD { class DiscontinuityError : virtual public std::runtime_error { + protected: + std::shared_ptr> discontinuityTypes; public: //DiscontinuityError(); - explicit DiscontinuityError(const std::string& msg); + explicit + DiscontinuityError(const std::string& msg, std::shared_ptr> disconTypes) : + std::runtime_error(msg), discontinuityTypes(disconTypes) + { + } + explicit DiscontinuityError(const std::string& msg) : std::runtime_error(msg) + { + } + virtual ~DiscontinuityError() noexcept {} }; } diff --git a/MbDCode/DispCompIecJecKec.cpp b/MbDCode/DispCompIecJecKec.cpp index 713419e..ddd45e5 100644 --- a/MbDCode/DispCompIecJecKec.cpp +++ b/MbDCode/DispCompIecJecKec.cpp @@ -2,45 +2,45 @@ using namespace MbD; -MbD::DispCompIecJecKec::DispCompIecJecKec() +DispCompIecJecKec::DispCompIecJecKec() { } -MbD::DispCompIecJecKec::DispCompIecJecKec(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk): KinematicIeJe(frmi, frmj), efrmK(frmk), axisK(axisk) +DispCompIecJecKec::DispCompIecJecKec(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk): KinematicIeJe(frmi, frmj), efrmK(frmk), axisK(axisk) { } -FRowDsptr MbD::DispCompIecJecKec::pvaluepXJ() +FRowDsptr DispCompIecJecKec::pvaluepXJ() { assert(false); return FRowDsptr(); } -FRowDsptr MbD::DispCompIecJecKec::pvaluepEJ() +FRowDsptr DispCompIecJecKec::pvaluepEJ() { assert(false); return FRowDsptr(); } -FMatDsptr MbD::DispCompIecJecKec::ppvaluepXJpEK() +FMatDsptr DispCompIecJecKec::ppvaluepXJpEK() { assert(false); return FMatDsptr(); } -FMatDsptr MbD::DispCompIecJecKec::ppvaluepEJpEK() +FMatDsptr DispCompIecJecKec::ppvaluepEJpEK() { assert(false); return FMatDsptr(); } -FMatDsptr MbD::DispCompIecJecKec::ppvaluepEJpEJ() +FMatDsptr DispCompIecJecKec::ppvaluepEJpEJ() { assert(false); return FMatDsptr(); } -double MbD::DispCompIecJecKec::value() +double DispCompIecJecKec::value() { return riIeJeKe; } diff --git a/MbDCode/DispCompIecJecKeqc.cpp b/MbDCode/DispCompIecJecKeqc.cpp index 788b069..4bc45dc 100644 --- a/MbDCode/DispCompIecJecKeqc.cpp +++ b/MbDCode/DispCompIecJecKeqc.cpp @@ -3,26 +3,26 @@ using namespace MbD; -MbD::DispCompIecJecKeqc::DispCompIecJecKeqc() +DispCompIecJecKeqc::DispCompIecJecKeqc() { } -MbD::DispCompIecJecKeqc::DispCompIecJecKeqc(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk) : DispCompIecJecKec(frmi, frmj, frmk, axisk) +DispCompIecJecKeqc::DispCompIecJecKeqc(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk) : DispCompIecJecKec(frmi, frmj, frmk, axisk) { } -void MbD::DispCompIecJecKeqc::initialize() +void DispCompIecJecKeqc::initialize() { priIeJeKepEK = std::make_shared>(4); ppriIeJeKepEKpEK = std::make_shared>(4, 4); } -void MbD::DispCompIecJecKeqc::initializeGlobally() +void DispCompIecJecKeqc::initializeGlobally() { ppAjOKepEKpEK = std::static_pointer_cast(efrmK)->ppAjOepEpE(axisK); } -void MbD::DispCompIecJecKeqc::calcPostDynCorrectorIteration() +void DispCompIecJecKeqc::calcPostDynCorrectorIteration() { auto frmIqc = std::static_pointer_cast(frmI); auto frmJqc = std::static_pointer_cast(frmJ); @@ -47,7 +47,7 @@ void MbD::DispCompIecJecKeqc::calcPostDynCorrectorIteration() } } -FMatDsptr MbD::DispCompIecJecKeqc::ppvaluepEKpEK() +FMatDsptr DispCompIecJecKeqc::ppvaluepEKpEK() { return ppriIeJeKepEKpEK; } diff --git a/MbDCode/DispCompIecJecO.cpp b/MbDCode/DispCompIecJecO.cpp index acc8958..9c62e3e 100644 --- a/MbDCode/DispCompIecJecO.cpp +++ b/MbDCode/DispCompIecJecO.cpp @@ -2,44 +2,44 @@ using namespace MbD; -MbD::DispCompIecJecO::DispCompIecJecO() +DispCompIecJecO::DispCompIecJecO() { } -MbD::DispCompIecJecO::DispCompIecJecO(EndFrmcptr frmi, EndFrmcptr frmj, int axis) : KinematicIeJe(frmi, frmj), axis(axis) +DispCompIecJecO::DispCompIecJecO(EndFrmcptr frmi, EndFrmcptr frmj, int axis) : KinematicIeJe(frmi, frmj), axis(axis) { } -void MbD::DispCompIecJecO::calcPostDynCorrectorIteration() +void DispCompIecJecO::calcPostDynCorrectorIteration() { riIeJeO = frmJ->riOeO(axis) - frmI->riOeO(axis); } -FRowDsptr MbD::DispCompIecJecO::pvaluepXJ() +FRowDsptr DispCompIecJecO::pvaluepXJ() { assert(false); return FRowDsptr(); } -FRowDsptr MbD::DispCompIecJecO::pvaluepEJ() +FRowDsptr DispCompIecJecO::pvaluepEJ() { assert(false); return FRowDsptr(); } -FMatDsptr MbD::DispCompIecJecO::ppvaluepXJpEK() +FMatDsptr DispCompIecJecO::ppvaluepXJpEK() { assert(false); return FMatDsptr(); } -FMatDsptr MbD::DispCompIecJecO::ppvaluepEJpEK() +FMatDsptr DispCompIecJecO::ppvaluepEJpEK() { assert(false); return FMatDsptr(); } -FMatDsptr MbD::DispCompIecJecO::ppvaluepEJpEJ() +FMatDsptr DispCompIecJecO::ppvaluepEJpEJ() { assert(false); return FMatDsptr(); diff --git a/MbDCode/DispCompIeqcJecKeqc.cpp b/MbDCode/DispCompIeqcJecKeqc.cpp index ef615b5..f1b8210 100644 --- a/MbDCode/DispCompIeqcJecKeqc.cpp +++ b/MbDCode/DispCompIeqcJecKeqc.cpp @@ -3,15 +3,15 @@ using namespace MbD; -MbD::DispCompIeqcJecKeqc::DispCompIeqcJecKeqc() +DispCompIeqcJecKeqc::DispCompIeqcJecKeqc() { } -MbD::DispCompIeqcJecKeqc::DispCompIeqcJecKeqc(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk) : DispCompIecJecKeqc(frmi, frmj, frmk, axisk) +DispCompIeqcJecKeqc::DispCompIeqcJecKeqc(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk) : DispCompIecJecKeqc(frmi, frmj, frmk, axisk) { } -void MbD::DispCompIeqcJecKeqc::initialize() +void DispCompIeqcJecKeqc::initialize() { DispCompIecJecKeqc::initialize(); priIeJeKepXI = std::make_shared>(3); @@ -21,7 +21,7 @@ void MbD::DispCompIeqcJecKeqc::initialize() ppriIeJeKepEIpEK = std::make_shared>(4, 4); } -void MbD::DispCompIeqcJecKeqc::calcPostDynCorrectorIteration() +void DispCompIeqcJecKeqc::calcPostDynCorrectorIteration() { DispCompIecJecKeqc::calcPostDynCorrectorIteration(); auto frmIqc = std::static_pointer_cast(frmI); @@ -66,32 +66,32 @@ void MbD::DispCompIeqcJecKeqc::calcPostDynCorrectorIteration() } } -FRowDsptr MbD::DispCompIeqcJecKeqc::pvaluepXI() +FRowDsptr DispCompIeqcJecKeqc::pvaluepXI() { return priIeJeKepXI; } -FRowDsptr MbD::DispCompIeqcJecKeqc::pvaluepEI() +FRowDsptr DispCompIeqcJecKeqc::pvaluepEI() { return priIeJeKepEI; } -FRowDsptr MbD::DispCompIeqcJecKeqc::pvaluepEK() +FRowDsptr DispCompIeqcJecKeqc::pvaluepEK() { return priIeJeKepEK; } -FMatDsptr MbD::DispCompIeqcJecKeqc::ppvaluepXIpEK() +FMatDsptr DispCompIeqcJecKeqc::ppvaluepXIpEK() { return ppriIeJeKepXIpEK; } -FMatDsptr MbD::DispCompIeqcJecKeqc::ppvaluepEIpEK() +FMatDsptr DispCompIeqcJecKeqc::ppvaluepEIpEK() { return ppriIeJeKepEIpEK; } -FMatDsptr MbD::DispCompIeqcJecKeqc::ppvaluepEIpEI() +FMatDsptr DispCompIeqcJecKeqc::ppvaluepEIpEI() { return ppriIeJeKepEIpEI; } diff --git a/MbDCode/DispCompIeqcJecO.cpp b/MbDCode/DispCompIeqcJecO.cpp index 253cd95..20f883d 100644 --- a/MbDCode/DispCompIeqcJecO.cpp +++ b/MbDCode/DispCompIeqcJecO.cpp @@ -3,22 +3,22 @@ using namespace MbD; -MbD::DispCompIeqcJecO::DispCompIeqcJecO() +DispCompIeqcJecO::DispCompIeqcJecO() { } -MbD::DispCompIeqcJecO::DispCompIeqcJecO(EndFrmcptr frmi, EndFrmcptr frmj, int axis) : DispCompIecJecO(frmi, frmj, axis) +DispCompIeqcJecO::DispCompIeqcJecO(EndFrmcptr frmi, EndFrmcptr frmj, int axis) : DispCompIecJecO(frmi, frmj, axis) { } -void MbD::DispCompIeqcJecO::initializeGlobally() +void DispCompIeqcJecO::initializeGlobally() { priIeJeOpXI = std::make_shared>(3, 0.0); priIeJeOpXI->at(axis) = -1.0; ppriIeJeOpEIpEI = std::static_pointer_cast(frmI)->ppriOeOpEpE(axis)->negated(); } -void MbD::DispCompIeqcJecO::calcPostDynCorrectorIteration() +void DispCompIeqcJecO::calcPostDynCorrectorIteration() { DispCompIecJecO::calcPostDynCorrectorIteration(); priIeJeOpEI = std::static_pointer_cast(frmI)->priOeOpE(axis)->negated(); diff --git a/MbDCode/DispCompIeqcJeqcKeqc.cpp b/MbDCode/DispCompIeqcJeqcKeqc.cpp index a381b45..525dba7 100644 --- a/MbDCode/DispCompIeqcJeqcKeqc.cpp +++ b/MbDCode/DispCompIeqcJeqcKeqc.cpp @@ -3,15 +3,15 @@ using namespace MbD; -MbD::DispCompIeqcJeqcKeqc::DispCompIeqcJeqcKeqc() +DispCompIeqcJeqcKeqc::DispCompIeqcJeqcKeqc() { } -MbD::DispCompIeqcJeqcKeqc::DispCompIeqcJeqcKeqc(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk) : DispCompIeqcJecKeqc(frmi, frmj, frmk, axisk) +DispCompIeqcJeqcKeqc::DispCompIeqcJeqcKeqc(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk) : DispCompIeqcJecKeqc(frmi, frmj, frmk, axisk) { } -void MbD::DispCompIeqcJeqcKeqc::initialize() +void DispCompIeqcJeqcKeqc::initialize() { DispCompIeqcJecKeqc::initialize(); priIeJeKepXJ = std::make_shared>(3); @@ -21,7 +21,7 @@ void MbD::DispCompIeqcJeqcKeqc::initialize() ppriIeJeKepEJpEK = std::make_shared>(4, 4); } -void MbD::DispCompIeqcJeqcKeqc::calcPostDynCorrectorIteration() +void DispCompIeqcJeqcKeqc::calcPostDynCorrectorIteration() { DispCompIeqcJecKeqc::calcPostDynCorrectorIteration(); auto frmJqc = std::static_pointer_cast(frmJ); @@ -66,27 +66,27 @@ void MbD::DispCompIeqcJeqcKeqc::calcPostDynCorrectorIteration() } } -FRowDsptr MbD::DispCompIeqcJeqcKeqc::pvaluepXJ() +FRowDsptr DispCompIeqcJeqcKeqc::pvaluepXJ() { return priIeJeKepXJ; } -FRowDsptr MbD::DispCompIeqcJeqcKeqc::pvaluepEJ() +FRowDsptr DispCompIeqcJeqcKeqc::pvaluepEJ() { return priIeJeKepEJ; } -FMatDsptr MbD::DispCompIeqcJeqcKeqc::ppvaluepXJpEK() +FMatDsptr DispCompIeqcJeqcKeqc::ppvaluepXJpEK() { return ppriIeJeKepXJpEK; } -FMatDsptr MbD::DispCompIeqcJeqcKeqc::ppvaluepEJpEK() +FMatDsptr DispCompIeqcJeqcKeqc::ppvaluepEJpEK() { return ppriIeJeKepEJpEK; } -FMatDsptr MbD::DispCompIeqcJeqcKeqc::ppvaluepEJpEJ() +FMatDsptr DispCompIeqcJeqcKeqc::ppvaluepEJpEJ() { return ppriIeJeKepEJpEJ; } diff --git a/MbDCode/DispCompIeqcJeqcKeqct.cpp b/MbDCode/DispCompIeqcJeqcKeqct.cpp index a93fdeb..13e0e8f 100644 --- a/MbDCode/DispCompIeqcJeqcKeqct.cpp +++ b/MbDCode/DispCompIeqcJeqcKeqct.cpp @@ -4,15 +4,15 @@ using namespace MbD; -MbD::DispCompIeqcJeqcKeqct::DispCompIeqcJeqcKeqct() +DispCompIeqcJeqcKeqct::DispCompIeqcJeqcKeqct() { } -MbD::DispCompIeqcJeqcKeqct::DispCompIeqcJeqcKeqct(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk) : DispCompIeqcJeqcKeqc(frmi, frmj, frmk, axisk) +DispCompIeqcJeqcKeqct::DispCompIeqcJeqcKeqct(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk) : DispCompIeqcJeqcKeqc(frmi, frmj, frmk, axisk) { } -void MbD::DispCompIeqcJeqcKeqct::initialize() +void DispCompIeqcJeqcKeqct::initialize() { DispCompIeqcJeqcKeqc::initialize(); ppriIeJeKepXIpt = std::make_shared>(3); @@ -22,7 +22,7 @@ void MbD::DispCompIeqcJeqcKeqct::initialize() ppriIeJeKepEKpt = std::make_shared>(4); } -void MbD::DispCompIeqcJeqcKeqct::calcPostDynCorrectorIteration() +void DispCompIeqcJeqcKeqct::calcPostDynCorrectorIteration() { //"ppAjOIepEKpEK is not longer constant and must be set before any calculation." auto efrmKqc = std::static_pointer_cast(efrmK); @@ -30,49 +30,49 @@ void MbD::DispCompIeqcJeqcKeqct::calcPostDynCorrectorIteration() DispCompIeqcJeqcKeqc::calcPostDynCorrectorIteration(); } -void MbD::DispCompIeqcJeqcKeqct::preVelIC() +void DispCompIeqcJeqcKeqct::preVelIC() { Item::preVelIC(); auto pAjOKept = std::static_pointer_cast(efrmK)->pAjOept(axisK); priIeJeKept = pAjOKept->dot(rIeJeO); } -double MbD::DispCompIeqcJeqcKeqct::pvaluept() +double DispCompIeqcJeqcKeqct::pvaluept() { return priIeJeKept; } -FRowDsptr MbD::DispCompIeqcJeqcKeqct::ppvaluepXIpt() +FRowDsptr DispCompIeqcJeqcKeqct::ppvaluepXIpt() { return ppriIeJeKepXIpt; } -FRowDsptr MbD::DispCompIeqcJeqcKeqct::ppvaluepEIpt() +FRowDsptr DispCompIeqcJeqcKeqct::ppvaluepEIpt() { return ppriIeJeKepEIpt; } -FRowDsptr MbD::DispCompIeqcJeqcKeqct::ppvaluepEKpt() +FRowDsptr DispCompIeqcJeqcKeqct::ppvaluepEKpt() { return ppriIeJeKepEKpt; } -FRowDsptr MbD::DispCompIeqcJeqcKeqct::ppvaluepXJpt() +FRowDsptr DispCompIeqcJeqcKeqct::ppvaluepXJpt() { return ppriIeJeKepXJpt; } -FRowDsptr MbD::DispCompIeqcJeqcKeqct::ppvaluepEJpt() +FRowDsptr DispCompIeqcJeqcKeqct::ppvaluepEJpt() { return ppriIeJeKepEJpt; } -double MbD::DispCompIeqcJeqcKeqct::ppvalueptpt() +double DispCompIeqcJeqcKeqct::ppvalueptpt() { return ppriIeJeKeptpt; } -void MbD::DispCompIeqcJeqcKeqct::preAccIC() +void DispCompIeqcJeqcKeqct::preAccIC() { Item::preAccIC(); auto pAjOKept = std::static_pointer_cast(efrmK)->pAjOept(axisK); diff --git a/MbDCode/DispCompIeqcJeqcO.cpp b/MbDCode/DispCompIeqcJeqcO.cpp index e1e3bf2..997c9b8 100644 --- a/MbDCode/DispCompIeqcJeqcO.cpp +++ b/MbDCode/DispCompIeqcJeqcO.cpp @@ -3,15 +3,15 @@ using namespace MbD; -MbD::DispCompIeqcJeqcO::DispCompIeqcJeqcO() +DispCompIeqcJeqcO::DispCompIeqcJeqcO() { } -MbD::DispCompIeqcJeqcO::DispCompIeqcJeqcO(EndFrmcptr frmi, EndFrmcptr frmj, int axis) : DispCompIeqcJecO(frmi, frmj, axis) +DispCompIeqcJeqcO::DispCompIeqcJeqcO(EndFrmcptr frmi, EndFrmcptr frmj, int axis) : DispCompIeqcJecO(frmi, frmj, axis) { } -void MbD::DispCompIeqcJeqcO::initializeGlobally() +void DispCompIeqcJeqcO::initializeGlobally() { DispCompIeqcJecO::initializeGlobally(); priIeJeOpXJ = std::make_shared>(3, 0.0); @@ -19,23 +19,23 @@ void MbD::DispCompIeqcJeqcO::initializeGlobally() ppriIeJeOpEJpEJ = std::static_pointer_cast(frmJ)->ppriOeOpEpE(axis); } -void MbD::DispCompIeqcJeqcO::calcPostDynCorrectorIteration() +void DispCompIeqcJeqcO::calcPostDynCorrectorIteration() { DispCompIeqcJecO::calcPostDynCorrectorIteration(); priIeJeOpEJ = std::static_pointer_cast(frmJ)->priOeOpE(axis); } -FRowDsptr MbD::DispCompIeqcJeqcO::pvaluepXJ() +FRowDsptr DispCompIeqcJeqcO::pvaluepXJ() { return priIeJeOpXJ; } -FRowDsptr MbD::DispCompIeqcJeqcO::pvaluepEJ() +FRowDsptr DispCompIeqcJeqcO::pvaluepEJ() { return priIeJeOpEJ; } -FMatDsptr MbD::DispCompIeqcJeqcO::ppvaluepEJpEJ() +FMatDsptr DispCompIeqcJeqcO::ppvaluepEJpEJ() { return ppriIeJeOpEJpEJ; } diff --git a/MbDCode/DispCompIeqctJeqcKeqct.cpp b/MbDCode/DispCompIeqctJeqcKeqct.cpp index 1658fa6..563e1a3 100644 --- a/MbDCode/DispCompIeqctJeqcKeqct.cpp +++ b/MbDCode/DispCompIeqctJeqcKeqct.cpp @@ -3,22 +3,22 @@ using namespace MbD; -MbD::DispCompIeqctJeqcKeqct::DispCompIeqctJeqcKeqct() +DispCompIeqctJeqcKeqct::DispCompIeqctJeqcKeqct() { } -MbD::DispCompIeqctJeqcKeqct::DispCompIeqctJeqcKeqct(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk) : DispCompIeqcJeqcKeqct(frmi, frmj, frmk, axisk) +DispCompIeqctJeqcKeqct::DispCompIeqctJeqcKeqct(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk) : DispCompIeqcJeqcKeqct(frmi, frmj, frmk, axisk) { } -void MbD::DispCompIeqctJeqcKeqct::preVelIC() +void DispCompIeqctJeqcKeqct::preVelIC() { DispCompIeqcJeqcKeqct::preVelIC(); auto& mprIeJeOpt = std::static_pointer_cast(frmI)->prOeOpt; priIeJeKept -= aAjOKe->dot(mprIeJeOpt); } -void MbD::DispCompIeqctJeqcKeqct::preAccIC() +void DispCompIeqctJeqcKeqct::preAccIC() { DispCompIeqcJeqcKeqct::preAccIC(); auto pAjOKept = std::static_pointer_cast(efrmK)->pAjOept(axisK); diff --git a/MbDCode/DispCompIeqctJeqcO.cpp b/MbDCode/DispCompIeqctJeqcO.cpp index 2dda312..d15a451 100644 --- a/MbDCode/DispCompIeqctJeqcO.cpp +++ b/MbDCode/DispCompIeqctJeqcO.cpp @@ -3,39 +3,39 @@ using namespace MbD; -MbD::DispCompIeqctJeqcO::DispCompIeqctJeqcO() +DispCompIeqctJeqcO::DispCompIeqctJeqcO() { } -MbD::DispCompIeqctJeqcO::DispCompIeqctJeqcO(EndFrmcptr frmi, EndFrmcptr frmj, int axis) : DispCompIeqcJeqcO(frmi, frmj, axis) +DispCompIeqctJeqcO::DispCompIeqctJeqcO(EndFrmcptr frmi, EndFrmcptr frmj, int axis) : DispCompIeqcJeqcO(frmi, frmj, axis) { } -void MbD::DispCompIeqctJeqcO::initializeGlobally() +void DispCompIeqctJeqcO::initializeGlobally() { //ToDo: Check why not using super classes. ppriIeJeOpEJpEJ = std::static_pointer_cast(frmJ)->ppriOeOpEpE(axis); } -void MbD::DispCompIeqctJeqcO::calcPostDynCorrectorIteration() +void DispCompIeqctJeqcO::calcPostDynCorrectorIteration() { //"ppriIeJeOpEIpEI is not a constant now." DispCompIeqcJeqcO::calcPostDynCorrectorIteration(); ppriIeJeOpEIpEI = std::static_pointer_cast(frmI)->ppriOeOpEpE(axis)->negated(); } -void MbD::DispCompIeqctJeqcO::preVelIC() +void DispCompIeqctJeqcO::preVelIC() { Item::preVelIC(); priIeJeOpt = -(std::static_pointer_cast(frmI)->priOeOpt(axis)); } -double MbD::DispCompIeqctJeqcO::pvaluept() +double DispCompIeqctJeqcO::pvaluept() { return priIeJeOpt; } -void MbD::DispCompIeqctJeqcO::preAccIC() +void DispCompIeqctJeqcO::preAccIC() { Item::preAccIC(); ppriIeJeOpEIpt = (std::static_pointer_cast(frmI)->ppriOeOpEpt(axis))->negated(); diff --git a/MbDCode/EndFramec.cpp b/MbDCode/EndFramec.cpp index 5f2d8fd..fc00dbe 100644 --- a/MbDCode/EndFramec.cpp +++ b/MbDCode/EndFramec.cpp @@ -12,6 +12,11 @@ EndFramec::EndFramec() { EndFramec::EndFramec(const char* str) : CartesianFrame(str) { } +System* EndFramec::root() +{ + return markerFrame->root(); +} + void EndFramec::initialize() { } @@ -35,18 +40,38 @@ void EndFramec::initEndFrameqct() assert(false); } -void MbD::EndFramec::calcPostDynCorrectorIteration() +void EndFramec::calcPostDynCorrectorIteration() { rOeO = markerFrame->rOmO; aAOe = markerFrame->aAOm; } -FColDsptr MbD::EndFramec::aAjOe(int j) +FColDsptr EndFramec::aAjOe(int j) { return aAOe->column(j); } -double MbD::EndFramec::riOeO(int i) +double EndFramec::riOeO(int i) { return rOeO->at(i); } + +FColDsptr EndFramec::rmeO() +{ + return rOeO->minusFullColumn(markerFrame->rOmO); +} + +FColDsptr EndFramec::rpep() +{ + return FColDsptr(); +} + +FColFMatDsptr EndFramec::pAOppE() +{ + return FColFMatDsptr(); +} + +FMatDsptr EndFramec::aBOp() +{ + return FMatDsptr(); +} diff --git a/MbDCode/EndFramec.h b/MbDCode/EndFramec.h index ae17b8c..38015bd 100644 --- a/MbDCode/EndFramec.h +++ b/MbDCode/EndFramec.h @@ -19,6 +19,7 @@ namespace MbD { public: EndFramec(); EndFramec(const char* str); + System* root() override; void initialize() override; void setMarkerFrame(MarkerFrame* markerFrm); MarkerFrame* getMarkerFrame(); @@ -27,6 +28,10 @@ namespace MbD { void calcPostDynCorrectorIteration() override; FColDsptr aAjOe(int j); double riOeO(int i); + virtual FColDsptr rmeO(); + virtual FColDsptr rpep(); + virtual FColFMatDsptr pAOppE(); + virtual FMatDsptr aBOp(); 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 2f486ac..ffe1868 100644 --- a/MbDCode/EndFrameqc.cpp +++ b/MbDCode/EndFrameqc.cpp @@ -38,7 +38,7 @@ void EndFrameqc::initEndFrameqct() endFrameqct->setMarkerFrame(markerFrame); } -FMatFColDsptr MbD::EndFrameqc::ppAjOepEpE(int jj) +FMatFColDsptr EndFrameqc::ppAjOepEpE(int jj) { auto answer = std::make_shared>>>(4, 4); for (int i = 0; i < 4; i++) { @@ -52,14 +52,14 @@ FMatFColDsptr MbD::EndFrameqc::ppAjOepEpE(int jj) return answer; } -void MbD::EndFrameqc::calcPostDynCorrectorIteration() +void EndFrameqc::calcPostDynCorrectorIteration() { EndFramec::calcPostDynCorrectorIteration(); prOeOpE = markerFrame->prOmOpE; pAOepE = markerFrame->pAOmpE; } -FMatDsptr MbD::EndFrameqc::pAjOepET(int axis) +FMatDsptr EndFrameqc::pAjOepET(int axis) { auto answer = std::make_shared>(4, 3); for (int i = 0; i < 4; i++) { @@ -73,7 +73,7 @@ FMatDsptr MbD::EndFrameqc::pAjOepET(int axis) return answer; } -FMatDsptr MbD::EndFrameqc::ppriOeOpEpE(int ii) +FMatDsptr EndFrameqc::ppriOeOpEpE(int ii) { auto answer = std::make_shared>(4, 4); for (int i = 0; i < 4; i++) { @@ -87,37 +87,52 @@ FMatDsptr MbD::EndFrameqc::ppriOeOpEpE(int ii) return answer; } -int MbD::EndFrameqc::iqX() +int EndFrameqc::iqX() { return markerFrame->iqX(); } -int MbD::EndFrameqc::iqE() +int EndFrameqc::iqE() { return markerFrame->iqE(); } -FRowDsptr MbD::EndFrameqc::priOeOpE(int i) +FRowDsptr EndFrameqc::priOeOpE(int i) { return prOeOpE->at(i); } -FColDsptr MbD::EndFrameqc::qXdot() +FColDsptr EndFrameqc::qXdot() { return markerFrame->qXdot(); } -std::shared_ptr> MbD::EndFrameqc::qEdot() +std::shared_ptr> EndFrameqc::qEdot() { return markerFrame->qEdot(); } -FColDsptr MbD::EndFrameqc::qXddot() +FColDsptr EndFrameqc::qXddot() { return markerFrame->qXddot(); } -FColDsptr MbD::EndFrameqc::qEddot() +FColDsptr EndFrameqc::qEddot() { return markerFrame->qEddot(); } + +FColDsptr EndFrameqc::rpep() +{ + return markerFrame->rpmp; +} + +FColFMatDsptr EndFrameqc::pAOppE() +{ + return markerFrame->pAOppE(); +} + +FMatDsptr EndFrameqc::aBOp() +{ + return markerFrame->aBOp(); +} diff --git a/MbDCode/EndFrameqc.h b/MbDCode/EndFrameqc.h index ced0e69..acd8776 100644 --- a/MbDCode/EndFrameqc.h +++ b/MbDCode/EndFrameqc.h @@ -28,6 +28,9 @@ namespace MbD { std::shared_ptr> qEdot(); FColDsptr qXddot(); FColDsptr qEddot(); + FColDsptr rpep() override; + FColFMatDsptr pAOppE() override; + FMatDsptr aBOp() override; FMatDsptr prOeOpE; std::shared_ptr>>> pprOeOpEpE; diff --git a/MbDCode/EndFrameqct.cpp b/MbDCode/EndFrameqct.cpp index ddcc67c..3cfc402 100644 --- a/MbDCode/EndFrameqct.cpp +++ b/MbDCode/EndFrameqct.cpp @@ -60,7 +60,7 @@ void EndFrameqct::initializeGlobally() void EndFrameqct::initprmemptBlks() { - auto& mbdTime = System::getInstance().time; + auto& mbdTime = this->root()->time; prmemptBlks = std::make_shared< FullColumn>(3); for (int i = 0; i < 3; i++) { auto& disp = rmemBlks->at(i); @@ -72,7 +72,7 @@ void EndFrameqct::initprmemptBlks() void EndFrameqct::initpprmemptptBlks() { - auto& mbdTime = System::getInstance().time; + auto& mbdTime = this->root()->time; pprmemptptBlks = std::make_shared< FullColumn>(3); for (int i = 0; i < 3; i++) { auto& vel = prmemptBlks->at(i); @@ -84,7 +84,7 @@ void EndFrameqct::initpprmemptptBlks() void EndFrameqct::initpPhiThePsiptBlks() { - auto& mbdTime = System::getInstance().time; + auto& mbdTime = this->root()->time; pPhiThePsiptBlks = std::make_shared< FullColumn>(3); for (int i = 0; i < 3; i++) { auto& angle = phiThePsiBlks->at(i); @@ -96,9 +96,9 @@ void EndFrameqct::initpPhiThePsiptBlks() } } -void MbD::EndFrameqct::initppPhiThePsiptptBlks() +void EndFrameqct::initppPhiThePsiptptBlks() { - auto& mbdTime = System::getInstance().time; + auto& mbdTime = this->root()->time; ppPhiThePsiptptBlks = std::make_shared< FullColumn>(3); for (int i = 0; i < 3; i++) { auto& angleVel = pPhiThePsiptBlks->at(i); @@ -108,14 +108,14 @@ void MbD::EndFrameqct::initppPhiThePsiptptBlks() } } -void MbD::EndFrameqct::postInput() +void EndFrameqct::postInput() { this->evalrmem(); this->evalAme(); Item::postInput(); } -void MbD::EndFrameqct::calcPostDynCorrectorIteration() +void EndFrameqct::calcPostDynCorrectorIteration() { auto& rOmO = markerFrame->rOmO; auto& aAOm = markerFrame->aAOm; @@ -143,15 +143,15 @@ void MbD::EndFrameqct::calcPostDynCorrectorIteration() ppAOepEpE = EulerParameters::ppApEpEtimesMatrix(aApe); } -void MbD::EndFrameqct::prePosIC() +void EndFrameqct::prePosIC() { - time = System::getInstance().mbdTimeValue(); + time = this->root()->mbdTimeValue(); this->evalrmem(); this->evalAme(); EndFrameqc::prePosIC(); } -void MbD::EndFrameqct::evalrmem() +void EndFrameqct::evalrmem() { if (rmemBlks) { for (int i = 0; i < 3; i++) @@ -163,7 +163,7 @@ void MbD::EndFrameqct::evalrmem() } } -void MbD::EndFrameqct::evalAme() +void EndFrameqct::evalAme() { if (phiThePsiBlks) { auto phiThePsi = CREATE>::With(); @@ -178,9 +178,9 @@ void MbD::EndFrameqct::evalAme() } } -void MbD::EndFrameqct::preVelIC() +void EndFrameqct::preVelIC() { - time = System::getInstance().mbdTimeValue(); + time = this->root()->mbdTimeValue(); this->evalrmem(); this->evalAme(); Item::preVelIC(); @@ -191,16 +191,16 @@ void MbD::EndFrameqct::preVelIC() pAOept = aAOm->timesFullMatrix(pAmept); } -void MbD::EndFrameqct::postVelIC() +void EndFrameqct::postVelIC() { auto& pAOmpE = markerFrame->pAOmpE; for (int i = 0; i < 3; i++) { auto& pprOeOpEpti = pprOeOpEpt->at(i); - for (int j = 0; j < 4; j++) + for (int j = 0; j < 4; j++) { - auto pprOeOpEptij = pAOmpE->at(j)->at(i)->dot(prmempt); - pprOeOpEpti->atiput(j, pprOeOpEptij); + auto pprOeOpEptij = pAOmpE->at(j)->at(i)->dot(prmempt); + pprOeOpEpti->atiput(j, pprOeOpEptij); } } for (int i = 0; i < 4; i++) @@ -209,12 +209,12 @@ void MbD::EndFrameqct::postVelIC() } } -FColDsptr MbD::EndFrameqct::pAjOept(int j) +FColDsptr EndFrameqct::pAjOept(int j) { return pAOept->column(j); } -FMatDsptr MbD::EndFrameqct::ppAjOepETpt(int jj) +FMatDsptr EndFrameqct::ppAjOepETpt(int jj) { auto answer = std::make_shared>(4, 3); for (int i = 0; i < 4; i++) @@ -230,27 +230,27 @@ FMatDsptr MbD::EndFrameqct::ppAjOepETpt(int jj) return answer; } -FColDsptr MbD::EndFrameqct::ppAjOeptpt(int j) +FColDsptr EndFrameqct::ppAjOeptpt(int j) { return ppAOeptpt->column(j); } -double MbD::EndFrameqct::priOeOpt(int i) +double EndFrameqct::priOeOpt(int i) { return prOeOpt->at(i); } -FRowDsptr MbD::EndFrameqct::ppriOeOpEpt(int i) +FRowDsptr EndFrameqct::ppriOeOpEpt(int i) { return pprOeOpEpt->at(i); } -double MbD::EndFrameqct::ppriOeOptpt(int i) +double EndFrameqct::ppriOeOptpt(int i) { return pprOeOptpt->at(i); } -void MbD::EndFrameqct::evalprmempt() +void EndFrameqct::evalprmempt() { if (rmemBlks) { for (int i = 0; i < 3; i++) @@ -262,7 +262,7 @@ void MbD::EndFrameqct::evalprmempt() } } -void MbD::EndFrameqct::evalpAmept() +void EndFrameqct::evalpAmept() { if (phiThePsiBlks) { auto phiThePsi = CREATE>::With(); @@ -283,7 +283,7 @@ void MbD::EndFrameqct::evalpAmept() } } -void MbD::EndFrameqct::evalpprmemptpt() +void EndFrameqct::evalpprmemptpt() { if (rmemBlks) { for (int i = 0; i < 3; i++) @@ -295,7 +295,7 @@ void MbD::EndFrameqct::evalpprmemptpt() } } -void MbD::EndFrameqct::evalppAmeptpt() +void EndFrameqct::evalppAmeptpt() { if (phiThePsiBlks) { auto phiThePsi = CREATE>::With(); @@ -322,9 +322,22 @@ void MbD::EndFrameqct::evalppAmeptpt() } } -void MbD::EndFrameqct::preAccIC() +FColDsptr EndFrameqct::rmeO() { - time = System::getInstance().mbdTimeValue(); + return markerFrame->aAOm->timesFullColumn(rmem); +} + +FColDsptr EndFrameqct::rpep() +{ + auto& rpmp = markerFrame->rpmp; + auto& aApm = markerFrame->aApm; + auto rpep = rpmp->plusFullColumn(aApm->timesFullColumn(rmem)); + return rpep; +} + +void EndFrameqct::preAccIC() +{ + time = this->root()->mbdTimeValue(); this->evalrmem(); this->evalAme(); Item::preVelIC(); diff --git a/MbDCode/EndFrameqct.h b/MbDCode/EndFrameqct.h index 26fa6f3..2349033 100644 --- a/MbDCode/EndFrameqct.h +++ b/MbDCode/EndFrameqct.h @@ -37,7 +37,9 @@ namespace MbD { void evalpAmept(); void evalpprmemptpt(); void evalppAmeptpt(); - + FColDsptr rmeO() override; + FColDsptr rpep() override; + void preAccIC() override; std::shared_ptr> rmemBlks, prmemptBlks, pprmemptptBlks; diff --git a/MbDCode/EulerConstraint.cpp b/MbDCode/EulerConstraint.cpp index 7e54efe..71c2b89 100644 --- a/MbDCode/EulerConstraint.cpp +++ b/MbDCode/EulerConstraint.cpp @@ -19,7 +19,7 @@ void EulerConstraint::initialize() pGpE = std::make_shared>(4); } -void MbD::EulerConstraint::calcPostDynCorrectorIteration() +void EulerConstraint::calcPostDynCorrectorIteration() { auto& qE = static_cast(owner)->qE; aG = qE->sumOfSquares() - 1.0; @@ -29,18 +29,18 @@ void MbD::EulerConstraint::calcPostDynCorrectorIteration() } } -void MbD::EulerConstraint::useEquationNumbers() +void EulerConstraint::useEquationNumbers() { iqE = static_cast(owner)->iqE; } -void MbD::EulerConstraint::fillPosICError(FColDsptr col) +void EulerConstraint::fillPosICError(FColDsptr col) { Constraint::fillPosICError(col); col->atiplusFullVectortimes(iqE, pGpE, lam); } -void MbD::EulerConstraint::fillPosICJacob(SpMatDsptr mat) +void EulerConstraint::fillPosICJacob(SpMatDsptr mat) { //"ppGpEpE is a diag(2,2,2,2)." mat->atijplusFullRow(iG, iqE, pGpE); @@ -53,18 +53,18 @@ void MbD::EulerConstraint::fillPosICJacob(SpMatDsptr mat) } } -void MbD::EulerConstraint::fillPosKineJacob(SpMatDsptr mat) +void EulerConstraint::fillPosKineJacob(SpMatDsptr mat) { mat->atijplusFullRow(iG, iqE, pGpE); } -void MbD::EulerConstraint::fillVelICJacob(SpMatDsptr mat) +void EulerConstraint::fillVelICJacob(SpMatDsptr mat) { mat->atijplusFullRow(iG, iqE, pGpE); mat->atijplusFullColumn(iqE, iG, pGpE->transpose()); } -void MbD::EulerConstraint::fillAccICIterError(FColDsptr col) +void EulerConstraint::fillAccICIterError(FColDsptr col) { //"qdotT[ppGpqpq]*qdot." //"qdotT[2 2 2 2 diag]*qdot." diff --git a/MbDCode/EulerParametersDot.h b/MbDCode/EulerParametersDot.h index 52cd3a1..3499c74 100644 --- a/MbDCode/EulerParametersDot.h +++ b/MbDCode/EulerParametersDot.h @@ -20,6 +20,7 @@ namespace MbD { void calcAdotBdotCdot(); void calcpAdotpE(); FMatDsptr aB(); + FColDsptr omeOpO(); std::shared_ptr> qE; FMatDsptr aAdot, aBdot, aCdot; @@ -170,6 +171,16 @@ namespace MbD { return qE->aB; } + template + inline FColDsptr EulerParametersDot::omeOpO() + { + auto aaa = this->aB(); + auto bbb = aaa->timesFullColumn(this); + auto ccc = bbb->times(2.0); + return ccc; + //return this->aB->timesFullColumn(this)->times(2.0); + } + template inline void EulerParametersDot::calc() { diff --git a/MbDCode/ForceTorqueData.cpp b/MbDCode/ForceTorqueData.cpp new file mode 100644 index 0000000..89d5498 --- /dev/null +++ b/MbDCode/ForceTorqueData.cpp @@ -0,0 +1,10 @@ +#include "ForceTorqueData.h" + +using namespace MbD; + +std::ostream& ForceTorqueData::printOn(std::ostream& s) const +{ + s << "aFIO = " << *aFIO << std::endl; + s << "aTIO = " << *aTIO << std::endl; + return s; +} diff --git a/MbDCode/ForceTorqueData.h b/MbDCode/ForceTorqueData.h new file mode 100644 index 0000000..965c3c7 --- /dev/null +++ b/MbDCode/ForceTorqueData.h @@ -0,0 +1,15 @@ +#pragma once + +#include "StateData.h" + +namespace MbD { + class ForceTorqueData : public StateData + { + //aFIO aTIO + public: + std::ostream& printOn(std::ostream& s) const override; + + FColDsptr aFIO, aTIO; + }; +} + diff --git a/MbDCode/FullColumn.h b/MbDCode/FullColumn.h index 0407679..947ce84 100644 --- a/MbDCode/FullColumn.h +++ b/MbDCode/FullColumn.h @@ -30,13 +30,11 @@ namespace MbD { std::shared_ptr> copy(); std::shared_ptr> transpose(); void atiplusFullColumntimes(int i, std::shared_ptr> fullCol, T factor); - T transposeTimesFullColumn(const std::shared_ptr> fullCol); + T transposeTimesFullColumn(const std::shared_ptr> fullCol); + void equalSelfPlusFullColumntimes(std::shared_ptr> fullCol, T factor); + std::shared_ptr> cross(std::shared_ptr> fullCol); - virtual std::ostream& printOn(std::ostream& s) const; - friend std::ostream& operator<<(std::ostream& s, const FullColumn& fullCol) - { - return fullCol.printOn(s); - } + std::ostream& printOn(std::ostream& s) const override; }; using FColDsptr = std::shared_ptr>; template @@ -63,7 +61,7 @@ namespace MbD { inline std::shared_ptr> FullColumn::times(double a) { int n = (int) this->size(); - auto answer = std::make_shared(n); + auto answer = std::make_shared>(n); for (int i = 0; i < n; i++) { answer->at(i) = this->at(i) * a; } @@ -148,9 +146,29 @@ namespace MbD { return this->dot(fullCol); } template + inline void FullColumn::equalSelfPlusFullColumntimes(std::shared_ptr> fullCol, T factor) + { + this->equalSelfPlusFullVectortimes(fullCol, factor); + } + template + inline std::shared_ptr> FullColumn::cross(std::shared_ptr> fullCol) + { + auto a0 = this->at(0); + auto a1 = this->at(1); + auto a2 = this->at(2); + auto b0 = fullCol->at(0); + auto b1 = fullCol->at(1); + auto b2 = fullCol->at(2); + auto answer = std::make_shared>(3); + answer->atiput(0, a1 * b2 - (a2 * b1)); + answer->atiput(1, a2 * b0 - (a0 * b2)); + answer->atiput(2, a0 * b1 - (a1 * b0)); + return answer; + } + template inline std::ostream& FullColumn::printOn(std::ostream& s) const { - s << "{"; + s << "FullCol{"; s << this->at(0); for (int i = 1; i < this->size(); i++) { diff --git a/MbDCode/FullMatrix.h b/MbDCode/FullMatrix.h index 0a29bd0..e3f0ad2 100644 --- a/MbDCode/FullMatrix.h +++ b/MbDCode/FullMatrix.h @@ -39,6 +39,7 @@ namespace MbD { void identity(); std::shared_ptr> column(int j); std::shared_ptr> timesFullColumn(std::shared_ptr> fullCol); + std::shared_ptr> timesFullColumn(FullColumn* fullCol); std::shared_ptr> timesFullMatrix(std::shared_ptr> fullMat); std::shared_ptr> timesTransposeFullMatrix(std::shared_ptr> fullMat); std::shared_ptr> times(double a); @@ -58,6 +59,7 @@ namespace MbD { std::shared_ptr> copy(); FullMatrix operator+(const FullMatrix fullMat); std::shared_ptr> transposeTimesFullColumn(const std::shared_ptr> fullCol); + std::ostream& printOn(std::ostream& s) const override; }; template<> @@ -241,7 +243,23 @@ namespace MbD { return fullCol->transpose()->timesFullMatrix(sptr)->transpose(); } template + inline std::ostream& FullMatrix::printOn(std::ostream& s) const + { + s << "FullMat[" << std::endl; + for (int i = 0; i < this->size(); i++) + { + s << *(this->at(i)) << std::endl; + } + s << "]"; + return s; + } + template inline std::shared_ptr> FullMatrix::timesFullColumn(std::shared_ptr> fullCol) + { + return this->timesFullColumn(fullCol.get()); + } + template + inline std::shared_ptr> FullMatrix::timesFullColumn(FullColumn* fullCol) { //"a*b = a(i,j)b(j) sum j." auto nrow = this->nrow(); diff --git a/MbDCode/FullRow.h b/MbDCode/FullRow.h index f100c9e..d32a871 100644 --- a/MbDCode/FullRow.h +++ b/MbDCode/FullRow.h @@ -22,19 +22,22 @@ namespace MbD { std::shared_ptr> plusFullRow(std::shared_ptr> fullRow); std::shared_ptr> minusFullRow(std::shared_ptr> fullRow); T timesFullColumn(std::shared_ptr> fullCol); + T timesFullColumn(FullColumn* fullCol); 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(); std::shared_ptr> copy(); void atiplusFullRow(int j, std::shared_ptr> fullRow); + std::ostream& printOn(std::ostream& s) const override; + }; template inline std::shared_ptr> FullRow::times(double a) { int n = (int) this->size(); - auto answer = std::make_shared(n); + auto answer = std::make_shared>(n); for (int i = 0; i < n; i++) { answer->at(i) = this->at(i) * a; } @@ -67,6 +70,11 @@ namespace MbD { } template inline T FullRow::timesFullColumn(std::shared_ptr> fullCol) + { + return this->timesFullColumn(fullCol.get()); + } + template + inline T FullRow::timesFullColumn(FullColumn* fullCol) { auto answer = this->at(0) * fullCol->at(0); for (int i = 1; i < this->size(); i++) @@ -89,10 +97,7 @@ namespace MbD { template inline void FullRow::equalSelfPlusFullRowTimes(std::shared_ptr> fullRow, double factor) { - for (int i = 0; i < this->size(); i++) - { - this->at(i) += fullRow->at(i) * factor; - } + this->equalSelfPlusFullVectortimes(fullRow, factor); } template inline std::shared_ptr> FullRow::transpose() @@ -120,6 +125,18 @@ namespace MbD { } } template + inline std::ostream& FullRow::printOn(std::ostream& s) const + { + s << "FullRow{"; + s << this->at(0); + for (int i = 1; i < this->size(); i++) + { + s << ", " << this->at(i); + } + s << "}"; + return s; + } + template inline std::shared_ptr> FullRow::timesFullMatrix(std::shared_ptr> fullMat) { std::shared_ptr> answer = fullMat->at(0)->times(this->at(0)); diff --git a/MbDCode/FullVector.h b/MbDCode/FullVector.h index 577ef2c..d18767a 100644 --- a/MbDCode/FullVector.h +++ b/MbDCode/FullVector.h @@ -1,121 +1,157 @@ #pragma once +#include + #include "Array.h" namespace MbD { - template - class FullVector : public Array - { - public: - FullVector() {} - 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 atiput(int i, T value); - void atiplusNumber(int i, T value); - void atiminusNumber(int i, T value); - double sumOfSquares() override; - int numberOfElements() override; - void zeroSelf() override; - void atitimes(int i, double factor); - void atiplusFullVector(int i, std::shared_ptr fullVec); - void atiplusFullVectortimes(int i, std::shared_ptr fullVec, double factor); - double maxMagnitude(); + template + class FullVector : public Array + { + public: + FullVector() {} + 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 atiput(int i, T value); + void atiplusNumber(int i, T value); + void atiminusNumber(int i, T value); + double sumOfSquares() override; + int numberOfElements() override; + void zeroSelf() override; + void atitimes(int i, double factor); + void atiplusFullVector(int i, std::shared_ptr> fullVec); + void atiplusFullVectortimes(int i, std::shared_ptr> fullVec, T factor); + double maxMagnitude(); + double length(); + void equalSelfPlusFullVectortimes(std::shared_ptr> fullVec, T factor); + std::ostream& printOn(std::ostream& s) const override; - }; - template - inline double FullVector::dot(std::shared_ptr> vec) - { - int n = (int) this->size(); - double answer = 0.0; - for (int i = 0; i < n; i++) { - answer += this->at(i) * vec->at(i); - } - return answer; - } - template - inline void FullVector::atiput(int i, T value) - { - this->at(i) = value; - } - template - inline void FullVector::atiplusNumber(int i, T value) - { - this->at(i) += value; - } - template - inline void FullVector::atiminusNumber(int i, T value) - { - this->at(i) -= value; - } - template<> - inline double FullVector::sumOfSquares() - { - double sum = 0.0; - for (int i = 0; i < this->size(); i++) - { - double element = this->at(i); - sum += element * element; - } - return sum; - } - template - inline double FullVector::sumOfSquares() - { - assert(false); - return 0.0; - } - template - inline int FullVector::numberOfElements() - { - return (int) this->size(); - } - template<> - inline void FullVector::zeroSelf() - { - for (int i = 0; i < this->size(); i++) { - this->at(i) = 0.0; - } - } - template - inline void FullVector::zeroSelf() - { - assert(false); - } - template - inline void FullVector::atitimes(int i, double factor) - { - this->at(i) *= factor; - } - template - inline void FullVector::atiplusFullVector(int i1, std::shared_ptr fullVec) - { - for (int ii = 0; ii < fullVec->size(); ii++) - { - auto i = i1 + ii; - this->at(i) += fullVec->at(ii); - } - } - template - inline void FullVector::atiplusFullVectortimes(int i1, std::shared_ptr fullVec, double factor) - { - for (int ii = 0; ii < fullVec->size(); ii++) - { - auto i = i1 + ii; - this->at(i) += fullVec->at(ii) * factor; - } - } - template<> - inline double FullVector::maxMagnitude() - { - auto answer = 0.0; - for (int i = 0; i < this->size(); i++) - { - auto mag = std::abs(this->at(i)); - if (answer < mag) answer = mag; - } - return answer; - } + }; + template + inline double FullVector::dot(std::shared_ptr> vec) + { + int n = (int)this->size(); + double answer = 0.0; + for (int i = 0; i < n; i++) { + answer += this->at(i) * vec->at(i); + } + return answer; + } + template + inline void FullVector::atiput(int i, T value) + { + this->at(i) = value; + } + template + inline void FullVector::atiplusNumber(int i, T value) + { + this->at(i) += value; + } + template + inline void FullVector::atiminusNumber(int i, T value) + { + this->at(i) -= value; + } + template<> + inline double FullVector::sumOfSquares() + { + double sum = 0.0; + for (int i = 0; i < this->size(); i++) + { + double element = this->at(i); + sum += element * element; + } + return sum; + } + template + inline double FullVector::sumOfSquares() + { + assert(false); + return 0.0; + } + template + inline int FullVector::numberOfElements() + { + return (int)this->size(); + } + template<> + inline void FullVector::zeroSelf() + { + for (int i = 0; i < this->size(); i++) { + this->at(i) = 0.0; + } + } + template + inline void FullVector::zeroSelf() + { + assert(false); + } + template + inline void FullVector::atitimes(int i, double factor) + { + this->at(i) *= factor; + } + template + inline void FullVector::atiplusFullVector(int i1, std::shared_ptr> fullVec) + { + for (int ii = 0; ii < fullVec->size(); ii++) + { + auto i = i1 + ii; + this->at(i) += fullVec->at(ii); + } + } + template + inline void FullVector::atiplusFullVectortimes(int i1, std::shared_ptr> fullVec, T factor) + { + for (int ii = 0; ii < fullVec->size(); ii++) + { + auto i = i1 + ii; + this->at(i) += fullVec->at(ii) * factor; + } + } + template<> + inline double FullVector::maxMagnitude() + { + auto answer = 0.0; + for (int i = 0; i < this->size(); i++) + { + auto mag = std::abs(this->at(i)); + if (answer < mag) answer = mag; + } + return answer; + } + template<> + inline double FullVector::length() + { + auto ssq = 0.0; + for (int i = 0; i < this->size(); i++) + { + auto elem = this->at(i); + ssq += elem * elem; + } + return std::sqrt(ssq); + } + template + inline void FullVector::equalSelfPlusFullVectortimes(std::shared_ptr> fullVec, T factor) + { + for (int i = 0; i < this->size(); i++) + { + this->atiplusNumber(i, fullVec->at(i) * factor); + } + } + template + inline std::ostream& FullVector::printOn(std::ostream& s) const + { + s << "FullVec{"; + s << this->at(0); + for (int i = 1; i < this->size(); i++) + { + s << ", " << this->at(i); + } + s << "}"; + return s; + } } diff --git a/MbDCode/FunctionWithManyArgs.cpp b/MbDCode/FunctionWithManyArgs.cpp index 48831ee..1044375 100644 --- a/MbDCode/FunctionWithManyArgs.cpp +++ b/MbDCode/FunctionWithManyArgs.cpp @@ -3,7 +3,7 @@ using namespace MbD; -MbD::FunctionWithManyArgs::FunctionWithManyArgs() +FunctionWithManyArgs::FunctionWithManyArgs() { terms = std::make_shared>(); } @@ -29,7 +29,7 @@ FunctionWithManyArgs::FunctionWithManyArgs(std::shared_ptr> terms->push_back(_terms->at(i)); } -std::shared_ptr> MbD::FunctionWithManyArgs::getTerms() +std::shared_ptr> FunctionWithManyArgs::getTerms() { return terms; } diff --git a/MbDCode/GEFullMat.cpp b/MbDCode/GEFullMat.cpp index 9ebb885..bf25f05 100644 --- a/MbDCode/GEFullMat.cpp +++ b/MbDCode/GEFullMat.cpp @@ -4,12 +4,12 @@ using namespace MbD; -void MbD::GEFullMat::forwardEliminateWithPivot(int p) +void GEFullMat::forwardEliminateWithPivot(int p) { assert(false); } -void MbD::GEFullMat::backSubstituteIntoDU() +void GEFullMat::backSubstituteIntoDU() { answerX = std::make_shared>(n); answerX->at(n - 1) = rightHandSideB->at(m - 1) / matrixA->at(m - 1)->at(n - 1); @@ -25,27 +25,27 @@ void MbD::GEFullMat::backSubstituteIntoDU() } } -void MbD::GEFullMat::postSolve() +void GEFullMat::postSolve() { assert(false); } -void MbD::GEFullMat::preSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal) +void GEFullMat::preSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal) { assert(false); } -void MbD::GEFullMat::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) +void GEFullMat::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) { assert(false); } -double MbD::GEFullMat::getmatrixArowimaxMagnitude(int i) +double GEFullMat::getmatrixArowimaxMagnitude(int i) { return matrixA->at(i)->maxMagnitude(); } -FColDsptr MbD::GEFullMat::basicSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal) +FColDsptr GEFullMat::basicSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal) { this->preSolvewithsaveOriginal(fullMat, fullCol, saveOriginal); for (int p = 0; p < m; p++) @@ -58,7 +58,7 @@ FColDsptr MbD::GEFullMat::basicSolvewithsaveOriginal(FMatDsptr fullMat, FColDspt return answerX; } -FColDsptr MbD::GEFullMat::basicSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) +FColDsptr GEFullMat::basicSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) { assert(false); return FColDsptr(); diff --git a/MbDCode/GEFullMatFullPv.cpp b/MbDCode/GEFullMatFullPv.cpp index b6e2642..06fa6b0 100644 --- a/MbDCode/GEFullMatFullPv.cpp +++ b/MbDCode/GEFullMatFullPv.cpp @@ -5,7 +5,7 @@ using namespace MbD; -void MbD::GEFullMatFullPv::doPivoting(int p) +void GEFullMatFullPv::doPivoting(int p) { //"Do full pivoting." @@ -45,7 +45,7 @@ void MbD::GEFullMatFullPv::doPivoting(int p) if (max < singularPivotTolerance) throwSingularMatrixError(""); } -void MbD::GEFullMatFullPv::postSolve() +void GEFullMatFullPv::postSolve() { assert(false); } diff --git a/MbDCode/GEFullMatParPv.cpp b/MbDCode/GEFullMatParPv.cpp index 80373cd..8d15e2e 100644 --- a/MbDCode/GEFullMatParPv.cpp +++ b/MbDCode/GEFullMatParPv.cpp @@ -5,7 +5,7 @@ using namespace MbD; -void MbD::GEFullMatParPv::doPivoting(int p) +void GEFullMatParPv::doPivoting(int p) { //"Use scalings. Do row pivoting." @@ -36,12 +36,12 @@ void MbD::GEFullMatParPv::doPivoting(int p) if (max < singularPivotTolerance) throwSingularMatrixError(""); } -void MbD::GEFullMatParPv::postSolve() +void GEFullMatParPv::postSolve() { assert(false); } -void MbD::GEFullMatParPv::preSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal) +void GEFullMatParPv::preSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal) { assert(false); } diff --git a/MbDCode/GESpMat.cpp b/MbDCode/GESpMat.cpp index ebd211e..0e94e35 100644 --- a/MbDCode/GESpMat.cpp +++ b/MbDCode/GESpMat.cpp @@ -4,13 +4,13 @@ using namespace MbD; -FColDsptr MbD::GESpMat::solvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) +FColDsptr GESpMat::solvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) { this->timedSolvewithsaveOriginal(spMat, fullCol, saveOriginal); return answerX; } -FColDsptr MbD::GESpMat::basicSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) +FColDsptr GESpMat::basicSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) { this->preSolvewithsaveOriginal(spMat, fullCol, saveOriginal); for (int p = 0; p < m; p++) @@ -23,23 +23,23 @@ FColDsptr MbD::GESpMat::basicSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr f return answerX; } -FColDsptr MbD::GESpMat::basicSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal) +FColDsptr GESpMat::basicSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal) { assert(false); return FColDsptr(); } -void MbD::GESpMat::preSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal) +void GESpMat::preSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal) { assert(false); } -void MbD::GESpMat::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) +void GESpMat::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) { assert(false); } -double MbD::GESpMat::getmatrixArowimaxMagnitude(int i) +double GESpMat::getmatrixArowimaxMagnitude(int i) { return matrixA->at(i)->maxMagnitude(); } diff --git a/MbDCode/GESpMatFullPv.cpp b/MbDCode/GESpMatFullPv.cpp index 7cc5293..3ba10b2 100644 --- a/MbDCode/GESpMatFullPv.cpp +++ b/MbDCode/GESpMatFullPv.cpp @@ -5,7 +5,7 @@ using namespace MbD; -void MbD::GESpMatFullPv::doPivoting(int p) +void GESpMatFullPv::doPivoting(int p) { //"Used by Gauss Elimination only." //"Do full pivoting." @@ -53,7 +53,7 @@ void MbD::GESpMatFullPv::doPivoting(int p) } markowitzPivotColCount = (int)rowPositionsOfNonZerosInPivotColumn->size(); } -void MbD::GESpMatFullPv::forwardEliminateWithPivot(int p) +void GESpMatFullPv::forwardEliminateWithPivot(int p) { //app is pivot. //i > p, j > p @@ -92,7 +92,7 @@ void MbD::GESpMatFullPv::forwardEliminateWithPivot(int p) } } -void MbD::GESpMatFullPv::backSubstituteIntoDU() +void GESpMatFullPv::backSubstituteIntoDU() { //"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." @@ -124,12 +124,12 @@ void MbD::GESpMatFullPv::backSubstituteIntoDU() } } -void MbD::GESpMatFullPv::postSolve() +void GESpMatFullPv::postSolve() { assert(false); } -void MbD::GESpMatFullPv::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) +void GESpMatFullPv::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) { //"A conditioned copy of spMat is solved." if (m != spMat->nrow() || n != spMat->ncol()) { diff --git a/MbDCode/GESpMatFullPvPosIC.cpp b/MbDCode/GESpMatFullPvPosIC.cpp index 63bf58c..e7ddad0 100644 --- a/MbDCode/GESpMatFullPvPosIC.cpp +++ b/MbDCode/GESpMatFullPvPosIC.cpp @@ -7,7 +7,7 @@ using namespace MbD; -void MbD::GESpMatFullPvPosIC::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) +void GESpMatFullPvPosIC::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) { GESpMatFullPv::preSolvewithsaveOriginal(spMat, fullCol, saveOriginal); if (system == nullptr) { @@ -19,7 +19,7 @@ void MbD::GESpMatFullPvPosIC::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsp pivotRowLimit = -1; } -void MbD::GESpMatFullPvPosIC::doPivoting(int p) +void GESpMatFullPvPosIC::doPivoting(int p) { //"Used by Gauss Elimination only." //"Swap rows but keep columns in place." diff --git a/MbDCode/GESpMatParPv.cpp b/MbDCode/GESpMatParPv.cpp index b6c687b..3445c51 100644 --- a/MbDCode/GESpMatParPv.cpp +++ b/MbDCode/GESpMatParPv.cpp @@ -4,7 +4,7 @@ using namespace MbD; -void MbD::GESpMatParPv::forwardEliminateWithPivot(int p) +void GESpMatParPv::forwardEliminateWithPivot(int p) { //"rightHandSideB may be multidimensional." @@ -36,7 +36,7 @@ void MbD::GESpMatParPv::forwardEliminateWithPivot(int p) } } -void MbD::GESpMatParPv::backSubstituteIntoDU() +void GESpMatParPv::backSubstituteIntoDU() { //"DU is upper triangular with nonzero diagonals." @@ -64,6 +64,6 @@ void MbD::GESpMatParPv::backSubstituteIntoDU() } } -void MbD::GESpMatParPv::postSolve() +void GESpMatParPv::postSolve() { } diff --git a/MbDCode/GESpMatParPvMarko.cpp b/MbDCode/GESpMatParPvMarko.cpp index 5bbbbbf..5a33c55 100644 --- a/MbDCode/GESpMatParPvMarko.cpp +++ b/MbDCode/GESpMatParPvMarko.cpp @@ -5,7 +5,7 @@ using namespace MbD; -void MbD::GESpMatParPvMarko::doPivoting(int p) +void GESpMatParPvMarko::doPivoting(int p) { //"Search from bottom to top." //"Check for singular pivot." @@ -67,7 +67,7 @@ void MbD::GESpMatParPvMarko::doPivoting(int p) if (max < singularPivotTolerance) throwSingularMatrixError(""); } -void MbD::GESpMatParPvMarko::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) +void GESpMatParPvMarko::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) { //"Optimized for speed." if (m != spMat->nrow() || n != spMat->ncol()) { diff --git a/MbDCode/GESpMatParPvMarkoFast.cpp b/MbDCode/GESpMatParPvMarkoFast.cpp index 19cce65..efbb05c 100644 --- a/MbDCode/GESpMatParPvMarkoFast.cpp +++ b/MbDCode/GESpMatParPvMarkoFast.cpp @@ -6,7 +6,7 @@ using namespace MbD; -void MbD::GESpMatParPvMarkoFast::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) +void GESpMatParPvMarkoFast::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) { //assert(false); //"Optimized for speed." @@ -33,7 +33,7 @@ void MbD::GESpMatParPvMarkoFast::preSolvewithsaveOriginal(SpMatDsptr spMat, FCol } } -void MbD::GESpMatParPvMarkoFast::doPivoting(int p) +void GESpMatParPvMarkoFast::doPivoting(int p) { //"Search from bottom to top." //"Optimized for speed. No check for singular pivot." diff --git a/MbDCode/GESpMatParPvPrecise.cpp b/MbDCode/GESpMatParPvPrecise.cpp index d1bfd29..6aea527 100644 --- a/MbDCode/GESpMatParPvPrecise.cpp +++ b/MbDCode/GESpMatParPvPrecise.cpp @@ -5,7 +5,7 @@ using namespace MbD; -void MbD::GESpMatParPvPrecise::doPivoting(int p) +void GESpMatParPvPrecise::doPivoting(int p) { //"Search from bottom to top." //"Use scaling vector and partial pivoting with actual swapping of rows." @@ -66,7 +66,7 @@ void MbD::GESpMatParPvPrecise::doPivoting(int p) if (max < singularPivotTolerance) throwSingularMatrixError(""); } -void MbD::GESpMatParPvPrecise::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) +void GESpMatParPvPrecise::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) { //assert(false); //"A conditioned copy of aMatrix is solved." diff --git a/MbDCode/ICKineIntegrator.cpp b/MbDCode/ICKineIntegrator.cpp index 7964a74..83351fc 100644 --- a/MbDCode/ICKineIntegrator.cpp +++ b/MbDCode/ICKineIntegrator.cpp @@ -1,9 +1,42 @@ #include "ICKineIntegrator.h" #include "SystemSolver.h" -void MbD::ICKineIntegrator::runInitialConditionTypeSolution() +using namespace MbD; + +void ICKineIntegrator::runInitialConditionTypeSolution() { system->runPosICKine(); system->runVelICKine(); system->runAccICKine(); } + +void ICKineIntegrator::iStep(int i) +{ + assert(false); +} + +void ICKineIntegrator::selectOrder() +{ + assert(false); +} + +void ICKineIntegrator::preRun() +{ + system->Solver::logString("MbD: Starting quasi kinematic analysis."); + QuasiIntegrator::preRun(); +} + +void ICKineIntegrator::firstStep() +{ + assert(false); +} + +void ICKineIntegrator::subsequentSteps() +{ + assert(false); +} + +void MbD::ICKineIntegrator::nextStep() +{ + assert(false); +} diff --git a/MbDCode/ICKineIntegrator.h b/MbDCode/ICKineIntegrator.h index c196525..f7c1c4b 100644 --- a/MbDCode/ICKineIntegrator.h +++ b/MbDCode/ICKineIntegrator.h @@ -6,7 +6,13 @@ namespace MbD { { // public: + void preRun() override; + void firstStep() override; + void subsequentSteps() override; + void nextStep() override; void runInitialConditionTypeSolution() override; + void iStep(int i) override; + void selectOrder() override; }; } diff --git a/MbDCode/IndependentVariable.cpp b/MbDCode/IndependentVariable.cpp index 6eaa572..2b200b9 100644 --- a/MbDCode/IndependentVariable.cpp +++ b/MbDCode/IndependentVariable.cpp @@ -3,11 +3,11 @@ using namespace MbD; -MbD::IndependentVariable::IndependentVariable() +IndependentVariable::IndependentVariable() { } -Symsptr MbD::IndependentVariable::differentiateWRT(Symsptr sptr, Symsptr var) +Symsptr IndependentVariable::differentiateWRT(Symsptr sptr, Symsptr var) { if (this == var.get()) { return std::make_shared(1.0); diff --git a/MbDCode/Integrator.cpp b/MbDCode/Integrator.cpp index d67e5f4..7666aad 100644 --- a/MbDCode/Integrator.cpp +++ b/MbDCode/Integrator.cpp @@ -2,6 +2,6 @@ using namespace MbD; -void MbD::Integrator::setSystem(Solver* sys) +void Integrator::setSystem(Solver* sys) { } diff --git a/MbDCode/Integrator.h b/MbDCode/Integrator.h index d2ee323..d96b40b 100644 --- a/MbDCode/Integrator.h +++ b/MbDCode/Integrator.h @@ -10,7 +10,16 @@ namespace MbD { //system direction public: void setSystem(Solver* sys) override; + virtual void firstStep() = 0; + virtual void preFirstStep() = 0; + virtual void postFirstStep() = 0; + virtual void subsequentSteps() = 0; + virtual void nextStep() = 0; + virtual void preStep() = 0; + virtual void postStep() = 0; virtual void runInitialConditionTypeSolution() = 0; + virtual void iStep(int i) = 0; + virtual void selectOrder() = 0; double direction = 1; }; diff --git a/MbDCode/IntegratorInterface.cpp b/MbDCode/IntegratorInterface.cpp index 6979264..5bef921 100644 --- a/MbDCode/IntegratorInterface.cpp +++ b/MbDCode/IntegratorInterface.cpp @@ -6,28 +6,28 @@ using namespace MbD; -void MbD::IntegratorInterface::initializeGlobally() +void 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; + 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(Solver* sys) +void IntegratorInterface::setSystem(Solver* sys) { system = static_cast(sys); } -void MbD::IntegratorInterface::logString(std::string& str) +void IntegratorInterface::logString(std::string& str) { system->logString(str); } -void MbD::IntegratorInterface::run() +void IntegratorInterface::run() { this->preRun(); this->initializeLocally(); @@ -40,12 +40,37 @@ void MbD::IntegratorInterface::run() this->postRun(); } -int MbD::IntegratorInterface::orderMax() +int IntegratorInterface::orderMax() { return system->orderMax; } -void MbD::IntegratorInterface::incrementTime(double tnew) +void IntegratorInterface::incrementTime(double tnew) { system->settime(tnew); } + +void IntegratorInterface::postFirstStep() +{ + assert(false); //Not used. + //system->postFirstStep(); + //if (integrator->istep > 0) { + // //"Noise make checking at the start unreliable." + // this->checkForDiscontinuity(); + //} + //this->checkForOutputThrough(integrator->t); +} + +void IntegratorInterface::interpolateAt(double tArg) +{ + //"Interpolate for system state at tArg and leave system in that state." + assert(false); + //auto yout = integrator->yDerivat(0, tArg); + //auto ydotout = integrator->yDerivat(1, tArg); + //auto yddotout = integrator->yDerivat(2, tArg); + //system->time(tArg); + //system->y(yout); + //system->ydot(ydotout); + //system->yddot(yddotout); + //system->simUpdateAll(); +} diff --git a/MbDCode/IntegratorInterface.h b/MbDCode/IntegratorInterface.h index 049d90b..9f23abd 100644 --- a/MbDCode/IntegratorInterface.h +++ b/MbDCode/IntegratorInterface.h @@ -13,15 +13,20 @@ namespace MbD { void initializeGlobally() override; virtual void preRun() = 0; + virtual void checkForDiscontinuity() = 0; + void setSystem(Solver* sys) override; void logString(std::string& str) override; void run() override; int orderMax(); virtual void incrementTime(double tnew); - virtual void preFirstStep() = 0; - virtual void preStep() = 0; + void postFirstStep() override; virtual double suggestSmallerOrAcceptFirstStepSize(double hnew) = 0; + virtual double suggestSmallerOrAcceptStepSize(double hnew) = 0; + virtual void checkForOutputThrough(double t) = 0; + virtual void interpolateAt(double t); + 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 3afcd6a..5110fd9 100644 --- a/MbDCode/Item.cpp +++ b/MbDCode/Item.cpp @@ -11,10 +11,15 @@ using namespace MbD; Item::Item() { } -Item::Item(const char* str) : name(str) +Item::Item(const char* str) : name(str) { } +System* Item::root() +{ + return owner->root(); +} + void Item::initialize() { } @@ -29,6 +34,14 @@ const std::string& Item::getName() const return name; } +std::ostream& Item::printOn(std::ostream& s) const +{ + std::string str = typeid(*this).name(); + auto classname = str.substr(11, str.size() - 11); + s << classname << std::endl; + return s; +} + void Item::initializeLocally() { } @@ -37,62 +50,62 @@ void Item::initializeGlobally() { } -void MbD::Item::postInput() +void Item::postInput() { //Called once after input calcPostDynCorrectorIteration(); } -void MbD::Item::calcPostDynCorrectorIteration() +void Item::calcPostDynCorrectorIteration() { } -void MbD::Item::removeRedundantConstraints(std::shared_ptr> redunEqnNos) +void Item::removeRedundantConstraints(std::shared_ptr> redunEqnNos) { } -void MbD::Item::reactivateRedundantConstraints() +void Item::reactivateRedundantConstraints() { } -void MbD::Item::fillPosKineError(FColDsptr col) +void Item::fillPosKineError(FColDsptr col) { } -void MbD::Item::fillPosKineJacob(SpMatDsptr mat) +void Item::fillPosKineJacob(SpMatDsptr mat) { } -void MbD::Item::fillEssenConstraints(std::shared_ptr>> essenConstraints) +void Item::fillEssenConstraints(std::shared_ptr>> essenConstraints) { } -void MbD::Item::fillRedundantConstraints(std::shared_ptr>> redunConstraints) +void Item::fillRedundantConstraints(std::shared_ptr>> redunConstraints) { } -void MbD::Item::fillConstraints(std::shared_ptr>> allConstraints) +void Item::fillConstraints(std::shared_ptr>> allConstraints) { assert(false); } -void MbD::Item::fillqsu(FColDsptr col) +void Item::fillqsu(FColDsptr col) { } -void MbD::Item::fillqsuWeights(std::shared_ptr> diagMat) +void Item::fillqsuWeights(std::shared_ptr> diagMat) { } -void MbD::Item::fillqsulam(FColDsptr col) +void Item::fillqsulam(FColDsptr col) { } -void MbD::Item::setqsulam(FColDsptr col) +void Item::setqsulam(FColDsptr col) { } -void MbD::Item::outputStates() +void Item::outputStates() { std::stringstream ss; ss << classname() << " " << name; @@ -100,39 +113,73 @@ void MbD::Item::outputStates() this->logString(str); } -void MbD::Item::preDyn() +void Item::preDyn() { + //"Assume positions, velocities and accelerations are valid." + //"Called once before solving for dynamic solution." + //"Update all variable dependent instance variables needed for runDYNAMICS even if they + //have been calculated in postPosIC, postVelIC and postAccIC." + //"Calculate p, pdot." + //"Default is do nothing." } -std::string MbD::Item::classname() +void Item::postDyn() +{ + //"Assume runDYNAMICS ended successfully." + //"Called once at the end of runDYNAMICS." + //"Update all instance variables dependent on p,q,s,u,mu,pdot,qdot,sdot,udot,mudot (lam) + //regardless of whether they are needed." + //"This is a subset of update." + //"Default is do nothing." +} + +std::string Item::classname() { std::string str = typeid(*this).name(); auto answer = str.substr(11, str.size() - 11); return answer; } -void MbD::Item::preDynFirstStep() +void Item::preDynFirstStep() { //"Called before the start of the first step in the dynamic solution." this->preDynStep(); } -void MbD::Item::preDynStep() +void Item::postDynFirstStep() +{ + this->postDynStep(); +} + +void Item::preDynStep() { } -void MbD::Item::storeDynState() +void Item::postDynStep() +{ + //"Called after the end of a complete step in the dynamic solution." + //"Update info before checking for discontinuities." + //"Default is do nothing." +} + +void Item::storeDynState() { } -double MbD::Item::suggestSmallerOrAcceptDynFirstStepSize(double hnew) +double 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::preVelIC() +double Item::suggestSmallerOrAcceptDynStepSize(double hnew) +{ + //"Default is return hnew." + return hnew; +} + +void Item::preVelIC() { //"Assume positions are valid." //"Called once before solving for velocity initial conditions." @@ -143,77 +190,105 @@ void MbD::Item::preVelIC() this->calcPostDynCorrectorIteration(); } -void MbD::Item::postVelIC() +void Item::postVelIC() { } -void MbD::Item::fillqsudot(FColDsptr col) +void Item::fillqsudot(FColDsptr col) { } -void MbD::Item::fillqsudotWeights(std::shared_ptr> diagMat) +void Item::fillqsudotWeights(std::shared_ptr> diagMat) { } -void MbD::Item::fillVelICError(FColDsptr col) +void Item::fillVelICError(FColDsptr col) { } -void MbD::Item::fillVelICJacob(SpMatDsptr mat) +void Item::fillVelICJacob(SpMatDsptr mat) { } -void MbD::Item::setqsudotlam(FColDsptr qsudotlam) +void Item::setqsudotlam(FColDsptr col) { } -void MbD::Item::preAccIC() +void Item::preAccIC() { this->calcPostDynCorrectorIteration(); } -void MbD::Item::postAccIC() +void Item::postAccIC() { } -void MbD::Item::postAccICIteration() +void Item::postAccICIteration() { } -void MbD::Item::fillqsuddotlam(FColDsptr col) +void Item::fillqsuddotlam(FColDsptr col) { } -void MbD::Item::fillAccICIterError(FColDsptr col) +void Item::fillAccICIterError(FColDsptr col) { } -void MbD::Item::fillAccICIterJacob(SpMatDsptr mat) +void Item::fillAccICIterJacob(SpMatDsptr mat) { } -void MbD::Item::setqsuddotlam(FColDsptr qsudotlam) +void Item::setqsudot(FColDsptr col) { } -void MbD::Item::constraintsReport() +void Item::setqsuddotlam(FColDsptr col) { } -void MbD::Item::setqsu(FColDsptr qsuOld) +std::shared_ptr Item::stateData() +{ + assert(false); + return std::shared_ptr(); +} + +void Item::discontinuityAtaddTypeTo(double t, std::shared_ptr> disconTypes) { } -void MbD::Item::useEquationNumbers() +double Item::checkForDynDiscontinuityBetween(double tprev, double t) +{ + //"Check for discontinuity in the last step defined by the interval (tprevious,t]." + //"Default is assume no discontinuity and return t." + + return t; +} + +void Item::constraintsReport() { } -void MbD::Item::logString(std::string& str) +void Item::setqsu(FColDsptr qsuOld) { - System::getInstance().logString(str); } -void MbD::Item::prePosIC() +void Item::useEquationNumbers() +{ +} + +void Item::logString(std::string& str) +{ + this->root()->logString(str); +} + +void Item::logString(const char* chars) +{ + std::string str = chars; + this->logString(str); +} + +void Item::prePosIC() { //"Called once before solving for position initial conditions." //"Update all variable dependent instance variables needed for posIC." @@ -222,28 +297,28 @@ void MbD::Item::prePosIC() calcPostDynCorrectorIteration(); } -void MbD::Item::prePosKine() +void Item::prePosKine() { this->prePosIC(); } -void MbD::Item::postPosIC() +void Item::postPosIC() { } -void MbD::Item::postPosICIteration() +void Item::postPosICIteration() { this->calcPostDynCorrectorIteration(); } -void MbD::Item::fillPosICError(FColDsptr col) +void Item::fillPosICError(FColDsptr col) { } -void MbD::Item::fillPosICJacob(FMatDsptr mat) +void Item::fillPosICJacob(FMatDsptr mat) { } -void MbD::Item::fillPosICJacob(SpMatDsptr mat) +void Item::fillPosICJacob(SpMatDsptr mat) { } diff --git a/MbDCode/Item.h b/MbDCode/Item.h index 7203a78..2b02a10 100644 --- a/MbDCode/Item.h +++ b/MbDCode/Item.h @@ -6,10 +6,12 @@ #include "FullMatrix.h" #include "DiagonalMatrix.h" #include "SparseMatrix.h" +#include "enum.h" namespace MbD { - + class System; class Constraint; + class StateData; class Item { @@ -17,6 +19,7 @@ namespace MbD { public: Item(); Item(const char* str); + virtual System* root(); virtual void initialize(); virtual void initializeLocally(); virtual void initializeGlobally(); @@ -26,6 +29,7 @@ namespace MbD { virtual void setqsu(FColDsptr qsuOld); virtual void useEquationNumbers(); virtual void logString(std::string& str); + void logString(const char* chars); virtual void prePosIC(); virtual void prePosKine(); @@ -49,18 +53,24 @@ namespace MbD { virtual void setqsulam(FColDsptr col); virtual void outputStates(); virtual void preDyn(); + virtual void postDyn(); virtual std::string classname(); virtual void preDynFirstStep(); + virtual void postDynFirstStep(); virtual void preDynStep(); + virtual void postDynStep(); virtual void storeDynState(); virtual double suggestSmallerOrAcceptDynFirstStepSize(double hnew); + virtual double suggestSmallerOrAcceptDynStepSize(double hnew); virtual void preVelIC(); virtual void postVelIC(); virtual void fillqsudot(FColDsptr col); virtual void fillqsudotWeights(std::shared_ptr> diagMat); virtual void fillVelICError(FColDsptr col); virtual void fillVelICJacob(SpMatDsptr mat); - virtual void setqsudotlam(FColDsptr qsudotlam); + virtual void setqsudotlam(FColDsptr col); + virtual void setqsudot(FColDsptr col); + virtual void setqsuddotlam(FColDsptr col); virtual void preAccIC(); virtual void postAccIC(); @@ -68,12 +78,27 @@ namespace MbD { virtual void fillqsuddotlam(FColDsptr col); virtual void fillAccICIterError(FColDsptr col); virtual void fillAccICIterJacob(SpMatDsptr mat); - virtual void setqsuddotlam(FColDsptr qsudotlam); + virtual std::shared_ptr stateData(); + virtual void discontinuityAtaddTypeTo(double t, std::shared_ptr> disconTypes); + virtual double checkForDynDiscontinuityBetween(double tprev, double t); void setName(std::string& str); const std::string& getName() const; + virtual std::ostream& printOn(std::ostream& s) const; + friend std::ostream& operator<<(std::ostream& s, const Item& item) + { + if (&item) { + return item.printOn(s); + } + else { + s << "NULL"; + } + return s; + } + std::string name; + Item* owner = nullptr; //Use raw pointer when pointing backwards. }; } diff --git a/MbDCode/Joint.cpp b/MbDCode/Joint.cpp index 98f8f44..c50c752 100644 --- a/MbDCode/Joint.cpp +++ b/MbDCode/Joint.cpp @@ -9,6 +9,7 @@ #include "CREATE.h" #include "RedundantConstraint.h" #include "MarkerFrame.h" +#include "ForceTorqueData.h" using namespace MbD; @@ -47,103 +48,103 @@ void Joint::initializeGlobally() constraintsDo([](std::shared_ptr constraint) { constraint->initializeGlobally(); }); } -void MbD::Joint::constraintsDo(const std::function)>& f) +void Joint::constraintsDo(const std::function)>& f) { std::for_each(constraints->begin(), constraints->end(), f); } -void MbD::Joint::postInput() +void Joint::postInput() { constraintsDo([](std::shared_ptr constraint) { constraint->postInput(); }); } -void MbD::Joint::addConstraint(std::shared_ptr con) +void Joint::addConstraint(std::shared_ptr con) { con->setOwner(this); constraints->push_back(con); } -void MbD::Joint::prePosIC() +void Joint::prePosIC() { constraintsDo([](std::shared_ptr constraint) { constraint->prePosIC(); }); } -void MbD::Joint::prePosKine() +void Joint::prePosKine() { constraintsDo([](std::shared_ptr constraint) { constraint->prePosKine(); }); } -void MbD::Joint::fillEssenConstraints(std::shared_ptr>> essenConstraints) +void Joint::fillEssenConstraints(std::shared_ptr>> essenConstraints) { constraintsDo([&](std::shared_ptr con) { con->fillEssenConstraints(con, essenConstraints); }); } -void MbD::Joint::fillDispConstraints(std::shared_ptr>> dispConstraints) +void Joint::fillDispConstraints(std::shared_ptr>> dispConstraints) { constraintsDo([&](std::shared_ptr con) { con->fillDispConstraints(con, dispConstraints); }); } -void MbD::Joint::fillPerpenConstraints(std::shared_ptr>> perpenConstraints) +void Joint::fillPerpenConstraints(std::shared_ptr>> perpenConstraints) { constraintsDo([&](std::shared_ptr con) { con->fillPerpenConstraints(con, perpenConstraints); }); } -void MbD::Joint::fillRedundantConstraints(std::shared_ptr>> redunConstraints) +void Joint::fillRedundantConstraints(std::shared_ptr>> redunConstraints) { constraintsDo([&](std::shared_ptr con) { con->fillRedundantConstraints(con, redunConstraints); }); } -void MbD::Joint::fillConstraints(std::shared_ptr>> allConstraints) +void Joint::fillConstraints(std::shared_ptr>> allConstraints) { constraintsDo([&](std::shared_ptr con) { con->fillConstraints(con, allConstraints); }); } -void MbD::Joint::fillqsulam(FColDsptr col) +void Joint::fillqsulam(FColDsptr col) { constraintsDo([&](std::shared_ptr con) { con->fillqsulam(col); }); } -void MbD::Joint::fillqsudot(FColDsptr col) +void Joint::fillqsudot(FColDsptr col) { constraintsDo([&](std::shared_ptr con) { con->fillqsudot(col); }); } -void MbD::Joint::fillqsudotWeights(std::shared_ptr> diagMat) +void Joint::fillqsudotWeights(std::shared_ptr> diagMat) { } -void MbD::Joint::useEquationNumbers() +void Joint::useEquationNumbers() { constraintsDo([](std::shared_ptr constraint) { constraint->useEquationNumbers(); }); } -void MbD::Joint::setqsulam(FColDsptr col) +void Joint::setqsulam(FColDsptr col) { constraintsDo([&](std::shared_ptr con) { con->setqsulam(col); }); } -void MbD::Joint::setqsudotlam(FColDsptr col) +void Joint::setqsudotlam(FColDsptr col) { constraintsDo([&](std::shared_ptr con) { con->setqsudotlam(col); }); } -void MbD::Joint::postPosICIteration() +void Joint::postPosICIteration() { constraintsDo([](std::shared_ptr constraint) { constraint->postPosICIteration(); }); } -void MbD::Joint::fillPosICError(FColDsptr col) +void Joint::fillPosICError(FColDsptr col) { constraintsDo([&](std::shared_ptr con) { con->fillPosICError(col); }); } -void MbD::Joint::fillPosICJacob(SpMatDsptr mat) +void Joint::fillPosICJacob(SpMatDsptr mat) { constraintsDo([&](std::shared_ptr con) { con->fillPosICJacob(mat); }); } -void MbD::Joint::removeRedundantConstraints(std::shared_ptr> redundantEqnNos) +void Joint::removeRedundantConstraints(std::shared_ptr> redundantEqnNos) { for (size_t i = 0; i < constraints->size(); i++) { @@ -156,7 +157,7 @@ void MbD::Joint::removeRedundantConstraints(std::shared_ptr> re } } -void MbD::Joint::reactivateRedundantConstraints() +void Joint::reactivateRedundantConstraints() { for (size_t i = 0; i < constraints->size(); i++) { @@ -167,7 +168,7 @@ void MbD::Joint::reactivateRedundantConstraints() } } -void MbD::Joint::constraintsReport() +void Joint::constraintsReport() { auto redunCons = std::make_shared>>(); constraintsDo([&](std::shared_ptr con) { @@ -185,12 +186,12 @@ void MbD::Joint::constraintsReport() } } -void MbD::Joint::postPosIC() +void Joint::postPosIC() { constraintsDo([](std::shared_ptr constraint) { constraint->postPosIC(); }); } -void MbD::Joint::outputStates() +void Joint::outputStates() { Item::outputStates(); std::stringstream ss; @@ -202,52 +203,102 @@ void MbD::Joint::outputStates() constraintsDo([](std::shared_ptr constraint) { constraint->outputStates(); }); } -void MbD::Joint::preDyn() +void Joint::preDyn() { constraintsDo([](std::shared_ptr constraint) { constraint->preDyn(); }); } -void MbD::Joint::fillPosKineError(FColDsptr col) +void Joint::fillPosKineError(FColDsptr col) { constraintsDo([&](std::shared_ptr con) { con->fillPosKineError(col); }); } -void MbD::Joint::fillPosKineJacob(SpMatDsptr mat) +void Joint::fillPosKineJacob(SpMatDsptr mat) { constraintsDo([&](std::shared_ptr constraint) { constraint->fillPosKineJacob(mat); }); } -void MbD::Joint::preVelIC() +void Joint::preVelIC() { constraintsDo([](std::shared_ptr constraint) { constraint->preVelIC(); }); } -void MbD::Joint::fillVelICError(FColDsptr col) +void Joint::fillVelICError(FColDsptr col) { constraintsDo([&](std::shared_ptr con) { con->fillVelICError(col); }); } -void MbD::Joint::fillVelICJacob(SpMatDsptr mat) +void Joint::fillVelICJacob(SpMatDsptr mat) { constraintsDo([&](std::shared_ptr constraint) { constraint->fillVelICJacob(mat); }); } -void MbD::Joint::preAccIC() +void Joint::preAccIC() { constraintsDo([](std::shared_ptr constraint) { constraint->preAccIC(); }); } -void MbD::Joint::fillAccICIterError(FColDsptr col) +void Joint::fillAccICIterError(FColDsptr col) { constraintsDo([&](std::shared_ptr con) { con->fillAccICIterError(col); }); } -void MbD::Joint::fillAccICIterJacob(SpMatDsptr mat) +void Joint::fillAccICIterJacob(SpMatDsptr mat) { constraintsDo([&](std::shared_ptr con) { con->fillAccICIterJacob(mat); }); } -void MbD::Joint::setqsuddotlam(FColDsptr qsudotlam) +void Joint::setqsuddotlam(FColDsptr col) { - constraintsDo([&](std::shared_ptr con) { con->setqsuddotlam(qsudotlam); }); + constraintsDo([&](std::shared_ptr con) { con->setqsuddotlam(col); }); +} + +std::shared_ptr Joint::stateData() +{ + //" + //MbD returns aFIeO and aTIeO. + //GEO needs aFImO and aTImO. + //For Motion rImIeO is not zero and is changing. + //aFImO : = aFIeO. + //aTImO : = aTIeO + (rImIeO cross : aFIeO). + //" + + auto answer = std::make_shared(); + auto aFIeO = this->aFX(); + auto aTIeO = this->aTX(); + auto rImIeO = this->frmI->rmeO(); + answer->aFIO = aFIeO; + answer->aTIO = aTIeO->plusFullColumn(rImIeO->cross(aFIeO)); + return answer; +} + +FColDsptr Joint::aFX() +{ + return this->jointForceI(); +} + +FColDsptr Joint::jointForceI() +{ + //"jointForceI is force on MbD marker I." + auto jointForce = std::make_shared >(3); + constraintsDo([&](std::shared_ptr con) { con->addToJointForceI(jointForce); }); + return jointForce; +} + +FColDsptr Joint::aTX() +{ + return this->jointTorqueI(); +} + +FColDsptr Joint::jointTorqueI() +{ + //"jointTorqueI is torque on MbD marker I." + auto jointTorque = std::make_shared >(3); + constraintsDo([&](std::shared_ptr con) { con->addToJointTorqueI(jointTorque); }); + return jointTorque; +} + +void Joint::postDynStep() +{ + constraintsDo([](std::shared_ptr constraint) { constraint->postDynStep(); }); } diff --git a/MbDCode/Joint.h b/MbDCode/Joint.h index 5748ff7..bd90c1f 100644 --- a/MbDCode/Joint.h +++ b/MbDCode/Joint.h @@ -52,7 +52,13 @@ namespace MbD { void preAccIC() override; void fillAccICIterError(FColDsptr col) override; void fillAccICIterJacob(SpMatDsptr mat) override; - void setqsuddotlam(FColDsptr qsudotlam) override; + void setqsuddotlam(FColDsptr col) override; + std::shared_ptr stateData() override; + FColDsptr aFX(); + FColDsptr jointForceI(); + FColDsptr aTX(); + FColDsptr jointTorqueI(); + void postDynStep() override; EndFrmcptr frmI; EndFrmcptr frmJ; diff --git a/MbDCode/KineIntegrator.cpp b/MbDCode/KineIntegrator.cpp index 029793b..46ce769 100644 --- a/MbDCode/KineIntegrator.cpp +++ b/MbDCode/KineIntegrator.cpp @@ -2,19 +2,44 @@ #include "KineIntegrator.h" #include "SystemSolver.h" +#include "Solver.h" using namespace MbD; -void MbD::KineIntegrator::preRun() +void KineIntegrator::preRun() { - std::string str = "MbD: Starting kinematic analysis."; - system->logString(str); + system->Solver::logString("MbD: Starting kinematic analysis."); QuasiIntegrator::preRun(); } -void MbD::KineIntegrator::runInitialConditionTypeSolution() +void KineIntegrator::firstStep() +{ + assert(false); +} + +void KineIntegrator::subsequentSteps() +{ + assert(false); +} + +void KineIntegrator::nextStep() +{ + assert(false); +} + +void KineIntegrator::runInitialConditionTypeSolution() { system->runPosKine(); system->runVelKine(); system->runAccKine(); } + +void KineIntegrator::iStep(int i) +{ + assert(false); +} + +void KineIntegrator::selectOrder() +{ + assert(false); +} diff --git a/MbDCode/KineIntegrator.h b/MbDCode/KineIntegrator.h index c5bc2db..a2f3f50 100644 --- a/MbDCode/KineIntegrator.h +++ b/MbDCode/KineIntegrator.h @@ -8,7 +8,12 @@ namespace MbD { // public: void preRun() override; + void firstStep() override; + void subsequentSteps() override; + void nextStep() override; void runInitialConditionTypeSolution() override; + void iStep(int i) override; + void selectOrder() override; }; } diff --git a/MbDCode/LDUFullMat.cpp b/MbDCode/LDUFullMat.cpp index 1346bfe..4d9feba 100644 --- a/MbDCode/LDUFullMat.cpp +++ b/MbDCode/LDUFullMat.cpp @@ -2,30 +2,30 @@ using namespace MbD; -FColDsptr MbD::LDUFullMat::basicSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal) +FColDsptr LDUFullMat::basicSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal) { this->decomposesaveOriginal(fullMat, saveOriginal); FColDsptr answer = this->forAndBackSubsaveOriginal(fullCol, saveOriginal); return answer; } -FColDsptr MbD::LDUFullMat::basicSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) +FColDsptr LDUFullMat::basicSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) { assert(false); return FColDsptr(); } -void MbD::LDUFullMat::preSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal) +void LDUFullMat::preSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal) { assert(false); } -void MbD::LDUFullMat::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) +void LDUFullMat::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) { assert(false); } -void MbD::LDUFullMat::forwardEliminateWithPivot(int p) +void LDUFullMat::forwardEliminateWithPivot(int p) { //"Save factors in lower triangle for LU decomposition." @@ -47,12 +47,12 @@ void MbD::LDUFullMat::forwardEliminateWithPivot(int p) } } -void MbD::LDUFullMat::postSolve() +void LDUFullMat::postSolve() { assert(false); } -void MbD::LDUFullMat::preSolvesaveOriginal(FMatDsptr fullMat, bool saveOriginal) +void LDUFullMat::preSolvesaveOriginal(FMatDsptr fullMat, bool saveOriginal) { if (saveOriginal) { matrixA = fullMat->copy(); @@ -87,7 +87,7 @@ void MbD::LDUFullMat::preSolvesaveOriginal(FMatDsptr fullMat, bool saveOriginal) this->findScalingsForRowRange(0, m); } -void MbD::LDUFullMat::decomposesaveOriginal(FMatDsptr fullMat, bool saveOriginal) +void LDUFullMat::decomposesaveOriginal(FMatDsptr fullMat, bool saveOriginal) { this->preSolvesaveOriginal(fullMat, saveOriginal); for (int p = 0; p < m; p++) @@ -97,12 +97,12 @@ void MbD::LDUFullMat::decomposesaveOriginal(FMatDsptr fullMat, bool saveOriginal } } -void MbD::LDUFullMat::decomposesaveOriginal(SpMatDsptr spMat, bool saveOriginal) +void LDUFullMat::decomposesaveOriginal(SpMatDsptr spMat, bool saveOriginal) { assert(false); } -FMatDsptr MbD::LDUFullMat::inversesaveOriginal(FMatDsptr fullMat, bool saveOriginal) +FMatDsptr LDUFullMat::inversesaveOriginal(FMatDsptr fullMat, bool saveOriginal) { //"ForAndBackSub be optimized for the identity matrix." @@ -119,12 +119,12 @@ FMatDsptr MbD::LDUFullMat::inversesaveOriginal(FMatDsptr fullMat, bool saveOrigi return matrixAinverse; } -double MbD::LDUFullMat::getmatrixArowimaxMagnitude(int i) +double LDUFullMat::getmatrixArowimaxMagnitude(int i) { return matrixA->at(i)->maxMagnitude(); } -void MbD::LDUFullMat::forwardSubstituteIntoL() +void LDUFullMat::forwardSubstituteIntoL() { //"L is lower triangular with nonzero and ones in diagonal." auto vectorc = std::make_shared>(n); @@ -141,17 +141,17 @@ void MbD::LDUFullMat::forwardSubstituteIntoL() rightHandSideB = vectorc; } -void MbD::LDUFullMat::backSubstituteIntoDU() +void LDUFullMat::backSubstituteIntoDU() { //"DU is upper triangular with nonzero and arbitrary diagonals." //| rowi sum | answerX = std::make_shared>(n); answerX->at(n - 1) = rightHandSideB->at(m - 1) / matrixA->at(m - 1)->at(n - 1); - for (int i = n-2; i >= 0; i--) + for (int i = n - 2; i >= 0; i--) { auto rowi = matrixA->at(i); - double sum = answerX->at(n) * rowi->at(n); + double sum = answerX->at(n - 1) * rowi->at(n - 1); for (int j = i + 1; j < n - 1; j++) { sum += answerX->at(j) * rowi->at(j); diff --git a/MbDCode/LDUFullMatParPv.cpp b/MbDCode/LDUFullMatParPv.cpp index 3710ded..cd3c089 100644 --- a/MbDCode/LDUFullMatParPv.cpp +++ b/MbDCode/LDUFullMatParPv.cpp @@ -4,7 +4,7 @@ using namespace MbD; -void MbD::LDUFullMatParPv::doPivoting(int p) +void LDUFullMatParPv::doPivoting(int p) { //"Use scalings. Do row pivoting." diff --git a/MbDCode/LDUSpMat.cpp b/MbDCode/LDUSpMat.cpp index 2d7eedb..6965922 100644 --- a/MbDCode/LDUSpMat.cpp +++ b/MbDCode/LDUSpMat.cpp @@ -3,35 +3,35 @@ using namespace MbD; -FColDsptr MbD::LDUSpMat::basicSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) +FColDsptr LDUSpMat::basicSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) { this->decomposesaveOriginal(spMat, saveOriginal); FColDsptr answer = this->forAndBackSubsaveOriginal(fullCol, saveOriginal); return answer; } -void MbD::LDUSpMat::decomposesaveOriginal(FMatDsptr fullMat, bool saveOriginal) +void LDUSpMat::decomposesaveOriginal(FMatDsptr fullMat, bool saveOriginal) { assert(false); } -void MbD::LDUSpMat::decomposesaveOriginal(SpMatDsptr spMat, bool saveOriginal) +void LDUSpMat::decomposesaveOriginal(SpMatDsptr spMat, bool saveOriginal) { assert(false); } -FColDsptr MbD::LDUSpMat::forAndBackSubsaveOriginal(FColDsptr fullCol, bool saveOriginal) +FColDsptr LDUSpMat::forAndBackSubsaveOriginal(FColDsptr fullCol, bool saveOriginal) { assert(false); return FColDsptr(); } -double MbD::LDUSpMat::getmatrixArowimaxMagnitude(int i) +double LDUSpMat::getmatrixArowimaxMagnitude(int i) { return matrixA->at(i)->maxMagnitude(); } -void MbD::LDUSpMat::forwardSubstituteIntoL() +void LDUSpMat::forwardSubstituteIntoL() { //"L is lower triangular with nonzero and ones in diagonal." auto vectorc = std::make_shared>(n); @@ -50,7 +50,7 @@ void MbD::LDUSpMat::forwardSubstituteIntoL() rightHandSideB = vectorc; } -void MbD::LDUSpMat::backSubstituteIntoDU() +void LDUSpMat::backSubstituteIntoDU() { //"DU is upper triangular with nonzero diagonals." diff --git a/MbDCode/LDUSpMatParPvMarko.cpp b/MbDCode/LDUSpMatParPvMarko.cpp index 94e5311..fb2734a 100644 --- a/MbDCode/LDUSpMatParPvMarko.cpp +++ b/MbDCode/LDUSpMatParPvMarko.cpp @@ -1,7 +1,9 @@ #include "LDUSpMatParPvMarko.h" #include "SingularMatrixError.h" -void MbD::LDUSpMatParPvMarko::doPivoting(int p) +using namespace MbD; + +void LDUSpMatParPvMarko::doPivoting(int p) { //"Search from bottom to top." //"Check for singular pivot." diff --git a/MbDCode/LDUSpMatParPvPrecise.cpp b/MbDCode/LDUSpMatParPvPrecise.cpp index dec0961..8c1fa7f 100644 --- a/MbDCode/LDUSpMatParPvPrecise.cpp +++ b/MbDCode/LDUSpMatParPvPrecise.cpp @@ -1,7 +1,9 @@ #include "LDUSpMatParPvPrecise.h" #include "SingularMatrixError.h" -void MbD::LDUSpMatParPvPrecise::doPivoting(int p) +using namespace MbD; + +void LDUSpMatParPvPrecise::doPivoting(int p) { //"Search from bottom to top." //"Use scaling vector and partial pivoting with actual swapping of rows." diff --git a/MbDCode/MarkerFrame.cpp b/MbDCode/MarkerFrame.cpp index 5618e8a..08675f8 100644 --- a/MbDCode/MarkerFrame.cpp +++ b/MbDCode/MarkerFrame.cpp @@ -16,6 +16,11 @@ MarkerFrame::MarkerFrame() MarkerFrame::MarkerFrame(const char* str) : CartesianFrame(str) { } +System* MarkerFrame::root() +{ + return partFrame->root(); +} + void MarkerFrame::initialize() { prOmOpE = std::make_shared>(3, 4); @@ -46,13 +51,13 @@ void MarkerFrame::initializeGlobally() endFramesDo([](std::shared_ptr endFrame) { endFrame->initializeGlobally(); }); } -void MbD::MarkerFrame::postInput() +void MarkerFrame::postInput() { Item::postInput(); endFramesDo([](std::shared_ptr endFrame) { endFrame->postInput(); }); } -void MbD::MarkerFrame::calcPostDynCorrectorIteration() +void MarkerFrame::calcPostDynCorrectorIteration() { auto rOpO = partFrame->rOpO(); auto aAOp = partFrame->aAOp(); @@ -67,128 +72,148 @@ void MbD::MarkerFrame::calcPostDynCorrectorIteration() } } -void MbD::MarkerFrame::prePosIC() +void MarkerFrame::prePosIC() { Item::prePosIC(); endFramesDo([](std::shared_ptr endFrame) { endFrame->prePosIC(); }); } -int MbD::MarkerFrame::iqX() +int MarkerFrame::iqX() { return partFrame->iqX; } -int MbD::MarkerFrame::iqE() +int MarkerFrame::iqE() { return partFrame->iqE; } -void MbD::MarkerFrame::endFramesDo(const std::function)>& f) +void MarkerFrame::endFramesDo(const std::function)>& f) { std::for_each(endFrames->begin(), endFrames->end(), f); } -void MbD::MarkerFrame::fillqsu(FColDsptr col) +void MarkerFrame::fillqsu(FColDsptr col) { endFramesDo([&](const std::shared_ptr& endFrame) { endFrame->fillqsu(col); }); } -void MbD::MarkerFrame::fillqsuWeights(std::shared_ptr> diagMat) +void MarkerFrame::fillqsuWeights(std::shared_ptr> diagMat) { endFramesDo([&](const std::shared_ptr& endFrame) { endFrame->fillqsuWeights(diagMat); }); } -void MbD::MarkerFrame::fillqsulam(FColDsptr col) +void MarkerFrame::fillqsulam(FColDsptr col) { endFramesDo([&](const std::shared_ptr& endFrame) { endFrame->fillqsulam(col); }); } -void MbD::MarkerFrame::fillqsudot(FColDsptr col) +void MarkerFrame::fillqsudot(FColDsptr col) { endFramesDo([&](const std::shared_ptr& endFrame) { endFrame->fillqsudot(col); }); } -void MbD::MarkerFrame::fillqsudotWeights(std::shared_ptr> diagMat) +void MarkerFrame::fillqsudotWeights(std::shared_ptr> diagMat) { endFramesDo([&](const std::shared_ptr& endFrame) { endFrame->fillqsudotWeights(diagMat); }); } -void MbD::MarkerFrame::setqsu(FColDsptr col) +void MarkerFrame::setqsu(FColDsptr col) { endFramesDo([&](const std::shared_ptr& endFrame) { endFrame->setqsu(col); }); } -void MbD::MarkerFrame::setqsulam(FColDsptr col) +void MarkerFrame::setqsulam(FColDsptr col) { endFramesDo([&](const std::shared_ptr& endFrame) { endFrame->setqsulam(col); }); } -void MbD::MarkerFrame::setqsudotlam(FColDsptr col) +void MarkerFrame::setqsudot(FColDsptr col) +{ + endFramesDo([&](const std::shared_ptr& endFrame) { endFrame->setqsudot(col); }); +} + +void MarkerFrame::setqsudotlam(FColDsptr col) { endFramesDo([&](const std::shared_ptr& endFrame) { endFrame->setqsudotlam(col); }); } -void MbD::MarkerFrame::postPosICIteration() +void MarkerFrame::postPosICIteration() { Item::postPosICIteration(); endFramesDo([](std::shared_ptr endFrame) { endFrame->postPosICIteration(); }); } -void MbD::MarkerFrame::postPosIC() +void MarkerFrame::postPosIC() { endFramesDo([](std::shared_ptr endFrame) { endFrame->postPosIC(); }); } -void MbD::MarkerFrame::preDyn() +void MarkerFrame::preDyn() { endFramesDo([](std::shared_ptr endFrame) { endFrame->preDyn(); }); } -void MbD::MarkerFrame::storeDynState() +void MarkerFrame::storeDynState() { endFramesDo([](std::shared_ptr endFrame) { endFrame->storeDynState(); }); } -void MbD::MarkerFrame::preVelIC() +void MarkerFrame::preVelIC() { Item::preVelIC(); endFramesDo([](std::shared_ptr endFrame) { endFrame->preVelIC(); }); } -void MbD::MarkerFrame::postVelIC() +void MarkerFrame::postVelIC() { endFramesDo([](std::shared_ptr endFrame) { endFrame->postVelIC(); }); } -void MbD::MarkerFrame::preAccIC() +void MarkerFrame::preAccIC() { Item::preAccIC(); endFramesDo([](std::shared_ptr endFrame) { endFrame->preAccIC(); }); } -FColDsptr MbD::MarkerFrame::qXdot() +FColDsptr MarkerFrame::qXdot() { return partFrame->qXdot; } -std::shared_ptr> MbD::MarkerFrame::qEdot() +std::shared_ptr> MarkerFrame::qEdot() { return partFrame->qEdot; } -FColDsptr MbD::MarkerFrame::qXddot() +FColDsptr MarkerFrame::qXddot() { return partFrame->qXddot; } -FColDsptr MbD::MarkerFrame::qEddot() +FColDsptr MarkerFrame::qEddot() { return partFrame->qEddot; } -void MbD::MarkerFrame::setqsuddotlam(FColDsptr qsudotlam) +void MarkerFrame::setqsuddotlam(FColDsptr col) { - endFramesDo([&](const std::shared_ptr& endFrame) { endFrame->setqsuddotlam(qsudotlam); }); + endFramesDo([&](const std::shared_ptr& endFrame) { endFrame->setqsuddotlam(col); }); +} + +FColFMatDsptr MarkerFrame::pAOppE() +{ + return partFrame->pAOppE(); +} + +FMatDsptr MarkerFrame::aBOp() +{ + return partFrame->aBOp(); +} + +void MarkerFrame::postDynStep() +{ + endFramesDo([](std::shared_ptr endFrame) { endFrame->postDynStep(); }); } void MarkerFrame::setPartFrame(PartFrame* partFrm) diff --git a/MbDCode/MarkerFrame.h b/MbDCode/MarkerFrame.h index 64c7f43..8b3c990 100644 --- a/MbDCode/MarkerFrame.h +++ b/MbDCode/MarkerFrame.h @@ -21,7 +21,8 @@ namespace MbD { public: MarkerFrame(); MarkerFrame(const char* str); - void initialize() override; + System* root() override; + void initialize() override; void setPartFrame(PartFrame* partFrm); PartFrame* getPartFrame(); void setrpmp(FColDsptr x); @@ -42,7 +43,9 @@ namespace MbD { void fillqsudotWeights(std::shared_ptr> diagMat) override; void setqsu(FColDsptr col) override; void setqsulam(FColDsptr col) override; + void setqsudot(FColDsptr col) override; void setqsudotlam(FColDsptr col) override; + void setqsuddotlam(FColDsptr col) override; void postPosICIteration() override; void postPosIC() override; void preDyn() override; @@ -54,7 +57,9 @@ namespace MbD { std::shared_ptr> qEdot(); FColDsptr qXddot(); FColDsptr qEddot(); - void setqsuddotlam(FColDsptr qsudotlam) override; + FColFMatDsptr pAOppE(); + FMatDsptr aBOp(); + void postDynStep() override; PartFrame* partFrame; //Use raw pointer when pointing backwards. FColDsptr rpmp = std::make_shared>(3); diff --git a/MbDCode/MatrixDecomposition.cpp b/MbDCode/MatrixDecomposition.cpp index 60f4de3..d1ddbb8 100644 --- a/MbDCode/MatrixDecomposition.cpp +++ b/MbDCode/MatrixDecomposition.cpp @@ -2,7 +2,7 @@ using namespace MbD; -void MbD::MatrixDecomposition::applyRowOrderOnRightHandSideB() +void MatrixDecomposition::applyRowOrderOnRightHandSideB() { FColDsptr answer = std::make_shared>(m); for (int i = 0; i < m; i++) diff --git a/MbDCode/MatrixLDU.cpp b/MbDCode/MatrixLDU.cpp index 7c55bc6..4722327 100644 --- a/MbDCode/MatrixLDU.cpp +++ b/MbDCode/MatrixLDU.cpp @@ -2,7 +2,7 @@ using namespace MbD; -FColDsptr MbD::MatrixLDU::forAndBackSubsaveOriginal(FColDsptr fullCol, bool saveOriginal) +FColDsptr MatrixLDU::forAndBackSubsaveOriginal(FColDsptr fullCol, bool saveOriginal) { if (saveOriginal) { rightHandSideB = fullCol->copy(); diff --git a/MbDCode/MatrixSolver.cpp b/MbDCode/MatrixSolver.cpp index 48b91a3..075866e 100644 --- a/MbDCode/MatrixSolver.cpp +++ b/MbDCode/MatrixSolver.cpp @@ -10,29 +10,29 @@ using namespace MbD; -void MbD::MatrixSolver::initialize() +void MatrixSolver::initialize() { Solver::initialize(); singularPivotTolerance = 4 * std::numeric_limits::epsilon(); } -void MbD::MatrixSolver::setSystem(Solver* sys) +void MatrixSolver::setSystem(Solver* sys) { } -FColDsptr MbD::MatrixSolver::solvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) +FColDsptr MatrixSolver::solvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) { this->timedSolvewithsaveOriginal(spMat, fullCol, saveOriginal); return answerX; } -FColDsptr MbD::MatrixSolver::solvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal) +FColDsptr MatrixSolver::solvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal) { this->timedSolvewithsaveOriginal(fullMat, fullCol, saveOriginal); return answerX; } -FColDsptr MbD::MatrixSolver::timedSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) +FColDsptr MatrixSolver::timedSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) { auto start = std::chrono::steady_clock::now(); @@ -45,12 +45,12 @@ FColDsptr MbD::MatrixSolver::timedSolvewithsaveOriginal(SpMatDsptr spMat, FColDs return answerX; } -FColDsptr MbD::MatrixSolver::timedSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal) +FColDsptr MatrixSolver::timedSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal) { return FColDsptr(); } -void MbD::MatrixSolver::findScalingsForRowRange(int begin, int end) +void MatrixSolver::findScalingsForRowRange(int begin, int end) { //"Row element * scaling <= 1.0." rowScalings = std::make_shared>(m); @@ -62,12 +62,12 @@ void MbD::MatrixSolver::findScalingsForRowRange(int begin, int end) } } -void MbD::MatrixSolver::throwSingularMatrixError(const char* chars) +void MatrixSolver::throwSingularMatrixError(const char* chars) { throw SingularMatrixError(chars); } -void MbD::MatrixSolver::throwSingularMatrixError(const char* chars, std::shared_ptr> redunEqnNos) +void MatrixSolver::throwSingularMatrixError(const char* chars, std::shared_ptr> redunEqnNos) { throw SingularMatrixError(chars, redunEqnNos); } diff --git a/MbDCode/MaximumIterationError.cpp b/MbDCode/MaximumIterationError.cpp index ec915b5..f01e63f 100644 --- a/MbDCode/MaximumIterationError.cpp +++ b/MbDCode/MaximumIterationError.cpp @@ -2,6 +2,6 @@ using namespace MbD; -MbD::MaximumIterationError::MaximumIterationError(const std::string& msg) : std::runtime_error(msg) +MaximumIterationError::MaximumIterationError(const std::string& msg) : std::runtime_error(msg) { } diff --git a/MbDCode/MbDCode.cpp b/MbDCode/MbDCode.cpp index 485e16d..37dbc0d 100644 --- a/MbDCode/MbDCode.cpp +++ b/MbDCode/MbDCode.cpp @@ -27,490 +27,18 @@ #include "CREATE.h" #include "GESpMatParPvMarkoFast.h" #include "GESpMatParPvPrecise.h" +#include "CADSystem.h" using namespace MbD; +//using namespace CAD; void runSpMat(); -void runPiston(); -void runOndselPiston(); int main() { + auto externalSys = std::make_shared(); + externalSys->runOndselPiston(); + externalSys->runPiston(); //runSpMat(); - //runPiston(); - runOndselPiston(); -} - -void runOndselPiston() -{ - //Piston with easy input numbers for exact port from Smalltalk - std::cout << "Hello World!\n"; - System& TheSystem = System::getInstance(); - std::string name = "TheSystem"; - TheSystem.setName(name); - std::cout << "TheSystem.getName() " << TheSystem.getName() << std::endl; - auto& systemSolver = TheSystem.systemSolver; - systemSolver->errorTolPosKine = 1.0e-6; - systemSolver->errorTolAccKine = 1.0e-6; - systemSolver->iterMaxPosKine = 25; - systemSolver->iterMaxAccKine = 25; - systemSolver->tstart = 0; - systemSolver->tend = 25.0; - systemSolver->hmin = 2.5e-8; - systemSolver->hmax = 25.0; - systemSolver->hout = 1.0; - systemSolver->corAbsTol = 1.0e-6; - systemSolver->corRelTol = 1.0e-6; - systemSolver->intAbsTol = 1.0e-6; - systemSolver->intRelTol = 1.0e-6; - systemSolver->iterMaxDyn = 4; - systemSolver->orderMax = 5; - systemSolver->translationLimit = 9.6058421285615e9; - systemSolver->rotationLimit = 0.5; - - std::string str; - FColDsptr qX, qE, qXdot, omeOpO, qXddot, qEddot; - FColDsptr rpmp; - FMatDsptr aApm; - FRowDsptr fullRow; - // - auto assembly1 = CREATE::With("/Assembly1"); - std::cout << "assembly1->getName() " << assembly1->getName() << std::endl; - assembly1->m = 0.0; - assembly1->aJ = std::make_shared>(ListD{ 0, 0, 0 }); - qX = std::make_shared>(ListD{ 0, 0, 0 }); - qE = std::make_shared>(ListD{ 0, 0, 0, 1 }); - assembly1->setqX(qX); - assembly1->setqE(qE); - qXdot = std::make_shared>(ListD{ 0, 0, 0 }); - omeOpO = std::make_shared>(ListD{ 0, 0, 0 }); - assembly1->setqXdot(qXdot); - assembly1->setomeOpO(omeOpO); - qXddot = std::make_shared>(ListD{ 0, 0, 0 }); - qEddot = std::make_shared>(ListD{ 0, 0, 0, 0 }); - assembly1->setqXddot(qXddot); - assembly1->setqEddot(qEddot); - std::cout << "assembly1->getqX() " << *assembly1->getqX() << std::endl; - std::cout << "assembly1->getqE() " << *assembly1->getqE() << std::endl; - TheSystem.addPart(assembly1); - { - auto& partFrame = assembly1->partFrame; - auto marker2 = CREATE::With("/Assembly1/Marker2"); - rpmp = std::make_shared>(ListD{ 0.0, 0.0, 0.0 }); - marker2->setrpmp(rpmp); - aApm = std::make_shared>(ListListD{ - { 1, 0, 0 }, - { 0, 1, 0 }, - { 0, 0, 1 } - }); - marker2->setaApm(aApm); - partFrame->addMarkerFrame(marker2); - // - auto marker1 = CREATE::With("/Assembly1/Marker1"); - rpmp = std::make_shared>(ListD{ 0.0, 3.0, 0.0 }); - marker1->setrpmp(rpmp); - aApm = std::make_shared>(ListListD{ - { 1, 0, 0 }, - { 0, 0, 1 }, - { 0, -1, 0 } - }); - marker1->setaApm(aApm); - partFrame->addMarkerFrame(marker1); - } - assembly1->asFixed(); - // - auto crankPart1 = CREATE::With("/Assembly1/Part1"); - std::cout << "crankPart1->getName() " << crankPart1->getName() << std::endl; - crankPart1->m = 1.0; - crankPart1->aJ = std::make_shared>(ListD{ 1, 1, 1 }); - qX = std::make_shared>(ListD{ 0.4, 0.0, -0.05 }); - qE = std::make_shared>(ListD{ 0.0, 0.0, 0.0, 1.0 }); - crankPart1->setqX(qX); - crankPart1->setqE(qE); - qXdot = std::make_shared>(ListD{ 0, 0, 0 }); - omeOpO = std::make_shared>(ListD{ 0, 0, 0 }); - crankPart1->setqXdot(qXdot); - crankPart1->setomeOpO(omeOpO); - qXddot = std::make_shared>(ListD{ 0, 0, 0 }); - qEddot = std::make_shared>(ListD{ 0, 0, 0, 0 }); - crankPart1->setqXddot(qXddot); - crankPart1->setqEddot(qEddot); - TheSystem.parts->push_back(crankPart1); - { - auto& partFrame = crankPart1->partFrame; - auto marker1 = CREATE::With("/Assembly1/Part1/Marker1"); - rpmp = std::make_shared>(ListD{ -0.4, 0.0, 0.05 }); - marker1->setrpmp(rpmp); - aApm = std::make_shared>(ListListD{ - { 1, 0, 0 }, - { 0, 1, 0 }, - { 0, 0, 1 } - }); - marker1->setaApm(aApm); - partFrame->addMarkerFrame(marker1); - // - auto marker2 = CREATE::With("/Assembly1/Part1/Marker2"); - rpmp = std::make_shared>(ListD{ 0.4, 0.0, 0.05 }); - marker2->setrpmp(rpmp); - aApm = std::make_shared>(ListListD{ - { 1, 0, 0 }, - { 0, 1, 0 }, - { 0, 0, 1 } - }); - marker2->setaApm(aApm); - partFrame->addMarkerFrame(marker2); - } - // - auto conrodPart2 = CREATE::With("/Assembly1/Part2"); - std::cout << "conrodPart2->getName() " << conrodPart2->getName() << std::endl; - conrodPart2->m = 1.0; - conrodPart2->aJ = std::make_shared>(ListD{ 1, 1, 1 }); - qX = std::make_shared>(ListD{ 0.15, 0.1, 0.05 }); - qE = std::make_shared>(ListD{ 0.0, 0.0, 1.0, 0.0 }); - conrodPart2->setqX(qX); - conrodPart2->setqE(qE); - qXdot = std::make_shared>(ListD{ 0, 0, 0 }); - omeOpO = std::make_shared>(ListD{ 0, 0, 0 }); - conrodPart2->setqXdot(qXdot); - conrodPart2->setomeOpO(omeOpO); - qXddot = std::make_shared>(ListD{ 0, 0, 0 }); - qEddot = std::make_shared>(ListD{ 0, 0, 0, 0 }); - conrodPart2->setqXddot(qXddot); - conrodPart2->setqEddot(qEddot); - TheSystem.parts->push_back(conrodPart2); - { - auto& partFrame = conrodPart2->partFrame; - auto marker1 = CREATE::With("/Assembly1/Part2/Marker1"); - rpmp = std::make_shared>(ListD{ -0.65, 0.0, -0.05 }); - marker1->setrpmp(rpmp); - aApm = std::make_shared>(ListListD{ - {1.0, 0.0, 0.0}, - {0.0, 1.0, 0.0}, - {0.0, 0.0, 1.0} - }); - marker1->setaApm(aApm); - partFrame->addMarkerFrame(marker1); - // - auto marker2 = CREATE::With("/Assembly1/Part2/Marker2"); - rpmp = std::make_shared>(ListD{ 0.65, 0.0, -0.05 }); - marker2->setrpmp(rpmp); - aApm = std::make_shared>(ListListD{ - {1.0, 0.0, 0.0}, - {0.0, 1.0, 0.0}, - {0.0, 0.0, 1.0} - }); - marker2->setaApm(aApm); - partFrame->addMarkerFrame(marker2); - } - // - auto pistonPart3 = CREATE::With("/Assembly1/Part3"); - std::cout << "pistonPart3->getName() " << pistonPart3->getName() << std::endl; - pistonPart3->m = 1.0; - pistonPart3->aJ = std::make_shared>(ListD{ 1, 1, 1 }); - qX = std::make_shared>(ListD{ -0.0, 1.5, 0.0 }); - qE = std::make_shared>(ListD{ 0.70710678118655, 0.70710678118655, 0.0, 0.0 }); - pistonPart3->setqX(qX); - pistonPart3->setqE(qE); - qXdot = std::make_shared>(ListD{ 0, 0, 0 }); - omeOpO = std::make_shared>(ListD{ 0, 0, 0 }); - pistonPart3->setqXdot(qXdot); - pistonPart3->setomeOpO(omeOpO); - qXddot = std::make_shared>(ListD{ 0, 0, 0 }); - qEddot = std::make_shared>(ListD{ 0, 0, 0, 0 }); - pistonPart3->setqXddot(qXddot); - pistonPart3->setqEddot(qEddot); - TheSystem.parts->push_back(pistonPart3); - { - auto& partFrame = pistonPart3->partFrame; - auto marker1 = CREATE::With("/Assembly1/Part3/Marker1"); - rpmp = std::make_shared>(ListD{ -0.5, 0.0, 0.0 }); - marker1->setrpmp(rpmp); - aApm = std::make_shared>(ListListD{ - {0.0, 1.0, 0.0}, - {1.0, 0.0, 0.0}, - {0.0, 0.0, -1.0} - }); - marker1->setaApm(aApm); - partFrame->addMarkerFrame(marker1); - // - auto marker2 = CREATE::With("/Assembly1/Part3/Marker2"); - rpmp = std::make_shared>(ListD{ 0.5, 0.0, 0.0 }); - marker2->setrpmp(rpmp); - aApm = std::make_shared>(ListListD{ - {0.0, 0.0, 1.0}, - {1.0, 0.0, 0.0}, - {0.0, 1.0, 0.0} - }); - marker2->setaApm(aApm); - partFrame->addMarkerFrame(marker2); - } - // - auto revJoint1 = CREATE::With("/Assembly1/Joint1"); - std::cout << "revJoint1->getName() " << revJoint1->getName() << std::endl; - revJoint1->connectsItoJ(assembly1->partFrame->endFrame("/Assembly1/Marker2"), crankPart1->partFrame->endFrame("/Assembly1/Part1/Marker1")); - TheSystem.jointsMotions->push_back(revJoint1); - - auto revJoint2 = CREATE::With("/Assembly1/Joint2"); - std::cout << "revJoint2->getName() " << revJoint2->getName() << std::endl; - revJoint2->connectsItoJ(crankPart1->partFrame->endFrame("/Assembly1/Part1/Marker2"), conrodPart2->partFrame->endFrame("/Assembly1/Part2/Marker1")); - TheSystem.jointsMotions->push_back(revJoint2); - - auto revJoint3 = CREATE::With("/Assembly1/Joint3"); - std::cout << "revJoint3->getName() " << revJoint3->getName() << std::endl; - revJoint3->connectsItoJ(conrodPart2->partFrame->endFrame("/Assembly1/Part2/Marker2"), pistonPart3->partFrame->endFrame("/Assembly1/Part3/Marker1")); - TheSystem.jointsMotions->push_back(revJoint3); - - auto cylJoint4 = CREATE::With("/Assembly1/Joint4"); - std::cout << "cylJoint4->getName() " << cylJoint4->getName() << std::endl; - cylJoint4->connectsItoJ(pistonPart3->partFrame->endFrame("/Assembly1/Part3/Marker2"), assembly1->partFrame->endFrame("/Assembly1/Marker1")); - TheSystem.jointsMotions->push_back(cylJoint4); - - auto rotMotion1 = CREATE::With("/Assembly1/Motion1"); - rotMotion1->connectsItoJ(assembly1->partFrame->endFrame("/Assembly1/Marker2"), crankPart1->partFrame->endFrame("/Assembly1/Part1/Marker1")); - std::cout << "rotMotion1->getName() " << rotMotion1->getName() << std::endl; - auto omega = std::make_shared(6.2831853071796); - auto timeScale = std::make_shared(1.0); - auto time = std::make_shared(timeScale, TheSystem.time); - rotMotion1->phiBlk = std::make_shared(omega, time); - std::cout << "rotMotion1->phiBlk " << *(rotMotion1->phiBlk) << std::endl; - TheSystem.jointsMotions->push_back(rotMotion1); - // - TheSystem.runKINEMATICS(); -} - -void runPiston() -{ - std::cout << "Hello World!\n"; - System& TheSystem = System::getInstance(); - std::string name = "TheSystem"; - TheSystem.setName(name); - std::cout << "TheSystem.getName() " << TheSystem.getName() << std::endl; - auto& systemSolver = TheSystem.systemSolver; - systemSolver->errorTolPosKine = 1.0e-6; - systemSolver->errorTolAccKine = 1.0e-6; - systemSolver->iterMaxPosKine = 25; - systemSolver->iterMaxAccKine = 25; - systemSolver->tstart = 0; - systemSolver->tend = 25.0; - systemSolver->hmin = 2.5e-8; - systemSolver->hmax = 25.0; - systemSolver->hout = 1.0; - systemSolver->corAbsTol = 1.0e-6; - systemSolver->corRelTol = 1.0e-6; - systemSolver->intAbsTol = 1.0e-6; - systemSolver->intRelTol = 1.0e-6; - systemSolver->iterMaxDyn = 4; - systemSolver->orderMax = 5; - systemSolver->translationLimit = 9.6058421285615e9; - systemSolver->rotationLimit = 0.5; - - std::string str; - FColDsptr qX, qE, qXdot, omeOpO, qXddot, qEddot; - FColDsptr rpmp; - FMatDsptr aApm; - FRowDsptr fullRow; - // - auto assembly1 = CREATE::With("/Assembly1"); - std::cout << "assembly1->getName() " << assembly1->getName() << std::endl; - assembly1->m = 0.0; - assembly1->aJ = std::make_shared>(ListD{ 0, 0, 0 }); - qX = std::make_shared>(ListD{ 0, 0, 0 }); - qE = std::make_shared>(ListD{ 0, 0, 0, 1 }); - assembly1->setqX(qX); - assembly1->setqE(qE); - qXdot = std::make_shared>(ListD{ 0, 0, 0 }); - omeOpO = std::make_shared>(ListD{ 0, 0, 0 }); - assembly1->setqXdot(qXdot); - assembly1->setomeOpO(omeOpO); - qXddot = std::make_shared>(ListD{ 0, 0, 0 }); - qEddot = std::make_shared>(ListD{ 0, 0, 0, 0 }); - assembly1->setqXddot(qXddot); - assembly1->setqEddot(qEddot); - std::cout << "assembly1->getqX() " << *assembly1->getqX() << std::endl; - std::cout << "assembly1->getqE() " << *assembly1->getqE() << std::endl; - TheSystem.addPart(assembly1); - { - auto& partFrame = assembly1->partFrame; - auto marker2 = CREATE::With("/Assembly1/Marker2"); - rpmp = std::make_shared>(ListD{ 0.0, 0.0, 0.0 }); - marker2->setrpmp(rpmp); - aApm = std::make_shared>(ListListD{ - { 1, 0, 0 }, - { 0, 1, 0 }, - { 0, 0, 1 } - }); - marker2->setaApm(aApm); - partFrame->addMarkerFrame(marker2); - // - auto marker1 = CREATE::With("/Assembly1/Marker1"); - rpmp = std::make_shared>(ListD{ 0.0, 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(); - // - auto crankPart1 = CREATE::With("/Assembly1/Part1"); - 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.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); - qXdot = std::make_shared>(ListD{ 0, 0.096568457800423, 0 }); - omeOpO = std::make_shared>(ListD{ 0, 0, 0.25132741228718 }); - crankPart1->setqXdot(qXdot); - crankPart1->setomeOpO(omeOpO); - qXddot = std::make_shared>(ListD{ 0, 0, 0 }); - qEddot = std::make_shared>(ListD{ 0, 0, 0, 0 }); - crankPart1->setqXddot(qXddot); - crankPart1->setqEddot(qEddot); - TheSystem.parts->push_back(crankPart1); - { - auto& partFrame = crankPart1->partFrame; - auto marker1 = CREATE::With("/Assembly1/Part1/Marker1"); - rpmp = std::make_shared>(ListD{ -0.38423368514246, -2.6661567755108e-17, 0.048029210642807 }); - marker1->setrpmp(rpmp); - aApm = std::make_shared>(ListListD{ - { 1, 0, 0 }, - { 0, 1, 0 }, - { 0, 0, 1 } - }); - marker1->setaApm(aApm); - partFrame->addMarkerFrame(marker1); - // - auto marker2 = CREATE::With("/Assembly1/Part1/Marker2"); - rpmp = std::make_shared>(ListD{ 0.38423368514246, -2.6661567755108e-17, 0.048029210642807 }); - marker2->setrpmp(rpmp); - aApm = std::make_shared>(ListListD{ - { 1, 0, 0 }, - { 0, 1, 0 }, - { 0, 0, 1 } - }); - marker2->setaApm(aApm); - partFrame->addMarkerFrame(marker2); - } - // - auto conrodPart2 = CREATE::With("/Assembly1/Part2"); - std::cout << "conrodPart2->getName() " << conrodPart2->getName() << std::endl; - conrodPart2->m = 0.067815795134192; - conrodPart2->aJ = std::make_shared>(ListD{ 2.6072970063126e-4, 0.011784982468533, 0.011941420288912 }); - qX = std::make_shared>(ListD{ 0.38423368514246, 0.49215295678475, 0.048029210642807 }); - qE = std::make_shared>(ListD{ 0.0, 0.0, 0.89871703427292, 0.43852900965351 }); - conrodPart2->setqX(qX); - conrodPart2->setqE(qE); - qXdot = std::make_shared>(ListD{ 0, 0.19313691560085, 0 }); - omeOpO = std::make_shared>(ListD{ 1.670970041317e-34, 1.3045598281729e-34, -1.2731200314796e-35 }); - conrodPart2->setqXdot(qXdot); - conrodPart2->setomeOpO(omeOpO); - qXddot = std::make_shared>(ListD{ 0, 0, 0 }); - qEddot = std::make_shared>(ListD{ 0, 0, 0, 0 }); - conrodPart2->setqXddot(qXddot); - conrodPart2->setqEddot(qEddot); - TheSystem.parts->push_back(conrodPart2); - { - auto& partFrame = conrodPart2->partFrame; - auto marker1 = CREATE::With("/Assembly1/Part2/Marker1"); - rpmp = std::make_shared>(ListD{ -0.6243797383565, 1.1997705489799e-16, -0.048029210642807 }); - marker1->setrpmp(rpmp); - aApm = std::make_shared>(ListListD{ - {1.0, 2.7755575615629e-16, 0.0}, - {-2.7755575615629e-16, 1.0, 0.0}, - {0.0, 0.0, 1.0} - }); - marker1->setaApm(aApm); - partFrame->addMarkerFrame(marker1); - // - auto marker2 = CREATE::With("/Assembly1/Part2/Marker2"); - rpmp = std::make_shared>(ListD{ 0.6243797383565, -2.1329254204087e-16, -0.048029210642807 }); - marker2->setrpmp(rpmp); - aApm = std::make_shared>(ListListD{ - {1.0, 2.4980018054066e-16, 2.2204460492503e-16}, - {-2.4980018054066e-16, 1.0, 4.1633363423443e-17}, - {-2.2204460492503e-16, -4.1633363423443e-17, 1.0} - }); - marker2->setaApm(aApm); - partFrame->addMarkerFrame(marker2); - } - // - auto pistonPart3 = CREATE::With("/Assembly1/Part3"); - std::cout << "pistonPart3->getName() " << pistonPart3->getName() << std::endl; - pistonPart3->m = 1.730132083368; - pistonPart3->aJ = std::make_shared>(ListD{ 0.19449049546716, 0.23028116340971, 0.23028116340971 }); - qX = std::make_shared>(ListD{ -1.283972762056e-18, 1.4645980199976, -4.7652385308244e-17 }); - qE = std::make_shared>(ListD{ 0.70710678118655, 0.70710678118655, 0.0, 0.0 }); - pistonPart3->setqX(qX); - pistonPart3->setqE(qE); - qXdot = std::make_shared>(ListD{ -6.3364526821409e-32, 0.19313691560085, -1.933731897626e-34 }); - omeOpO = std::make_shared>(ListD{ 1.670970041317e-34, 1.3045598281729e-34, 1.8896472173894e-50 }); - pistonPart3->setqXdot(qXdot); - pistonPart3->setomeOpO(omeOpO); - qXddot = std::make_shared>(ListD{ 0, 0, 0 }); - qEddot = std::make_shared>(ListD{ 0, 0, 0, 0 }); - pistonPart3->setqXddot(qXddot); - pistonPart3->setqEddot(qEddot); - TheSystem.parts->push_back(pistonPart3); - { - auto& partFrame = pistonPart3->partFrame; - auto marker1 = CREATE::With("/Assembly1/Part3/Marker1"); - rpmp = std::make_shared>(ListD{ -0.48029210642807, 7.6201599718927e-18, -2.816737703896e-17 }); - marker1->setrpmp(rpmp); - aApm = std::make_shared>(ListListD{ - {9.2444637330587e-33, 1.0, 2.2204460492503e-16}, - {1.0, -9.2444637330587e-33, -1.0785207688569e-32}, - {-1.0785207688569e-32, 2.2204460492503e-16, -1.0} - }); - marker1->setaApm(aApm); - partFrame->addMarkerFrame(marker1); - // - auto marker2 = CREATE::With("/Assembly1/Part3/Marker2"); - rpmp = std::make_shared>(ListD{ 0.48029210642807, 1.7618247880058e-17, 2.5155758471256e-17 }); - marker2->setrpmp(rpmp); - aApm = std::make_shared>(ListListD{ - {6.9388939039072e-18, -6.4146353042213e-50, 1.0}, - {1.0, -6.9388939039072e-18, 6.9388939039072e-18}, - {-6.9388939039072e-18, 1.0, -7.4837411882581e-50} - }); - marker2->setaApm(aApm); - partFrame->addMarkerFrame(marker2); - } - // - auto revJoint1 = CREATE::With("/Assembly1/Joint1"); - std::cout << "revJoint1->getName() " << revJoint1->getName() << std::endl; - revJoint1->connectsItoJ(assembly1->partFrame->endFrame("/Assembly1/Marker2"), crankPart1->partFrame->endFrame("/Assembly1/Part1/Marker1")); - TheSystem.jointsMotions->push_back(revJoint1); - - auto revJoint2 = CREATE::With("/Assembly1/Joint2"); - std::cout << "revJoint2->getName() " << revJoint2->getName() << std::endl; - revJoint2->connectsItoJ(crankPart1->partFrame->endFrame("/Assembly1/Part1/Marker2"), conrodPart2->partFrame->endFrame("/Assembly1/Part2/Marker1")); - TheSystem.jointsMotions->push_back(revJoint2); - - auto revJoint3 = CREATE::With("/Assembly1/Joint3"); - std::cout << "revJoint3->getName() " << revJoint3->getName() << std::endl; - revJoint3->connectsItoJ(conrodPart2->partFrame->endFrame("/Assembly1/Part2/Marker2"), pistonPart3->partFrame->endFrame("/Assembly1/Part3/Marker1")); - TheSystem.jointsMotions->push_back(revJoint3); - - auto cylJoint4 = CREATE::With("/Assembly1/Joint4"); - std::cout << "cylJoint4->getName() " << cylJoint4->getName() << std::endl; - cylJoint4->connectsItoJ(pistonPart3->partFrame->endFrame("/Assembly1/Part3/Marker2"), assembly1->partFrame->endFrame("/Assembly1/Marker1")); - TheSystem.jointsMotions->push_back(cylJoint4); - - auto rotMotion1 = CREATE::With("/Assembly1/Motion1"); - rotMotion1->connectsItoJ(assembly1->partFrame->endFrame("/Assembly1/Marker2"), crankPart1->partFrame->endFrame("/Assembly1/Part1/Marker1")); - std::cout << "rotMotion1->getName() " << rotMotion1->getName() << std::endl; - auto omega = std::make_shared(6.2831853071796); - auto timeScale = std::make_shared(0.04); - auto time = std::make_shared(timeScale, TheSystem.time); - rotMotion1->phiBlk = std::make_shared(omega, time); - std::cout << "rotMotion1->phiBlk " << *(rotMotion1->phiBlk) << std::endl; - TheSystem.jointsMotions->push_back(rotMotion1); - // - TheSystem.runKINEMATICS(); } void runSpMat() { diff --git a/MbDCode/MbDCode.vcxproj b/MbDCode/MbDCode.vcxproj index b3cca92..a8e086d 100644 --- a/MbDCode/MbDCode.vcxproj +++ b/MbDCode/MbDCode.vcxproj @@ -141,12 +141,14 @@ + + @@ -180,6 +182,7 @@ + @@ -243,6 +246,7 @@ + @@ -280,12 +284,14 @@ + + @@ -320,6 +326,7 @@ + @@ -384,6 +391,7 @@ + diff --git a/MbDCode/MbDCode.vcxproj.filters b/MbDCode/MbDCode.vcxproj.filters index 616b719..28b21fe 100644 --- a/MbDCode/MbDCode.vcxproj.filters +++ b/MbDCode/MbDCode.vcxproj.filters @@ -426,6 +426,18 @@ Source Files + + Source Files + + + Source Files + + + Source Files + + + Source Files + @@ -848,6 +860,18 @@ Header Files + + Header Files + + + Header Files + + + Header Files + + + Header Files + diff --git a/MbDCode/NewtonRaphson.cpp b/MbDCode/NewtonRaphson.cpp index 56285a2..b158994 100644 --- a/MbDCode/NewtonRaphson.cpp +++ b/MbDCode/NewtonRaphson.cpp @@ -26,7 +26,7 @@ void NewtonRaphson::initializeLocally() yNormOld = std::numeric_limits::max(); } -void MbD::NewtonRaphson::run() +void NewtonRaphson::run() { assert(false); //self preRun. @@ -38,12 +38,12 @@ void MbD::NewtonRaphson::run() //self postRun. } -void MbD::NewtonRaphson::setSystem(Solver* sys) +void NewtonRaphson::setSystem(Solver* sys) { system = static_cast(sys); } -void MbD::NewtonRaphson::iterate() +void NewtonRaphson::iterate() { //" // Do not skip matrix solution even when yNorm is very small. @@ -68,7 +68,7 @@ void MbD::NewtonRaphson::iterate() } } -void MbD::NewtonRaphson::incrementIterNo() +void NewtonRaphson::incrementIterNo() { iterNo++; if (iterNo >= iterMax) { @@ -77,16 +77,16 @@ void MbD::NewtonRaphson::incrementIterNo() } } -bool MbD::NewtonRaphson::isConverged() +bool NewtonRaphson::isConverged() { return this->isConvergedToNumericalLimit(); } -void MbD::NewtonRaphson::askSystemToUpdate() +void NewtonRaphson::askSystemToUpdate() { } -bool MbD::NewtonRaphson::isConvergedToNumericalLimit() +bool NewtonRaphson::isConvergedToNumericalLimit() { //"worthIterating is less stringent with IterNo." //"nDivergenceMax is the number of small divergences allowed." @@ -116,7 +116,7 @@ bool MbD::NewtonRaphson::isConvergedToNumericalLimit() } } -void MbD::NewtonRaphson::calcDXNormImproveRootCalcYNorm() +void NewtonRaphson::calcDXNormImproveRootCalcYNorm() { this->calcdxNorm(); dxNorms->push_back(dxNorm); @@ -130,7 +130,7 @@ void MbD::NewtonRaphson::calcDXNormImproveRootCalcYNorm() yNormOld = yNorm; } -void MbD::NewtonRaphson::postRun() +void NewtonRaphson::postRun() { system->postNewtonRaphson(); } diff --git a/MbDCode/NewtonRaphsonError.cpp b/MbDCode/NewtonRaphsonError.cpp index edcb073..301c295 100644 --- a/MbDCode/NewtonRaphsonError.cpp +++ b/MbDCode/NewtonRaphsonError.cpp @@ -2,10 +2,10 @@ using namespace MbD; -//MbD::NewtonRaphsonError::NewtonRaphsonError() +//NewtonRaphsonError::NewtonRaphsonError() //{ //} -MbD::NewtonRaphsonError::NewtonRaphsonError(const std::string& msg) : std::runtime_error(msg) +NewtonRaphsonError::NewtonRaphsonError(const std::string& msg) : std::runtime_error(msg) { } diff --git a/MbDCode/NotKinematicError.cpp b/MbDCode/NotKinematicError.cpp index 3bea61e..1fa90de 100644 --- a/MbDCode/NotKinematicError.cpp +++ b/MbDCode/NotKinematicError.cpp @@ -2,6 +2,6 @@ using namespace MbD; -MbD::NotKinematicError::NotKinematicError(const std::string& msg) : std::runtime_error(msg) +NotKinematicError::NotKinematicError(const std::string& msg) : std::runtime_error(msg) { } diff --git a/MbDCode/Part.cpp b/MbDCode/Part.cpp index 95cb5b4..fe3784b 100644 --- a/MbDCode/Part.cpp +++ b/MbDCode/Part.cpp @@ -4,6 +4,7 @@ #include "CREATE.h" #include "DiagonalMatrix.h" #include "EulerParameters.h" +#include "DataPosVelAcc.h" using namespace MbD; @@ -14,6 +15,11 @@ Part::Part() { Part::Part(const char* str) : Item(str) { } +System* MbD::Part::root() +{ + return system; +} + void Part::initialize() { partFrame = CREATE::With(); @@ -71,92 +77,101 @@ FColDsptr Part::getomeOpO() { return partFrame->getomeOpO(); } -void MbD::Part::setqXddot(FColDsptr x) +void Part::setqXddot(FColDsptr x) { partFrame->setqXddot(x); } -FColDsptr MbD::Part::getqXddot() +FColDsptr Part::getqXddot() { return partFrame->getqXddot(); } -void MbD::Part::setqEddot(FColDsptr x) +void Part::setqEddot(FColDsptr x) { partFrame->setqEddot(x); } -FColDsptr MbD::Part::getqEddot() +FColDsptr Part::getqEddot() { return partFrame->getqEddot(); } -void MbD::Part::qX(FColDsptr x) +void Part::qX(FColDsptr x) { partFrame->qX = x; } -FColDsptr MbD::Part::qX() +FColDsptr Part::qX() { return partFrame->qX; } -void MbD::Part::qE(std::shared_ptr> x) +void Part::qE(std::shared_ptr> x) { partFrame->qE = x; } -std::shared_ptr> MbD::Part::qE() +std::shared_ptr> Part::qE() { return partFrame->qE; } -void MbD::Part::qXdot(FColDsptr x) +void Part::qXdot(FColDsptr x) { partFrame->qXdot = x; } -FColDsptr MbD::Part::qXdot() +FColDsptr Part::qXdot() { return partFrame->qXdot; } -void MbD::Part::omeOpO(FColDsptr x) +void Part::omeOpO(FColDsptr x) { partFrame->setomeOpO(x); } -FColDsptr MbD::Part::omeOpO() +FColDsptr Part::omeOpO() { - assert(false); - return FColDsptr(); + return partFrame->omeOpO(); } -void MbD::Part::qXddot(FColDsptr x) +void Part::qXddot(FColDsptr x) { partFrame->qXddot = x; } -FColDsptr MbD::Part::qXddot() +FColDsptr Part::qXddot() { return partFrame->qXddot; } -void MbD::Part::qEddot(FColDsptr x) +void Part::qEddot(FColDsptr x) { //ToDo: Should store EulerParametersDDot //ToDo: Need alpOpO too partFrame->qXddot = x; } -FColDsptr MbD::Part::qEddot() +FColDsptr Part::qEddot() { return partFrame->qEddot; } -void Part::setSystem(System& sys) +FMatDsptr Part::aAOp() { - //May be needed in the future + return partFrame->aAOp(); +} + +FColDsptr Part::alpOpO() +{ + return partFrame->alpOpO(); +} + +void Part::setSystem(System* sys) +{ + system = sys; } void Part::asFixed() @@ -164,13 +179,13 @@ void Part::asFixed() partFrame->asFixed(); } -void MbD::Part::postInput() +void Part::postInput() { partFrame->postInput(); Item::postInput(); } -void MbD::Part::calcPostDynCorrectorIteration() +void Part::calcPostDynCorrectorIteration() { this->calcmE(); this->calcmEdot(); @@ -179,47 +194,47 @@ void MbD::Part::calcPostDynCorrectorIteration() this->calcppTpEpEdot(); } -void MbD::Part::prePosIC() +void Part::prePosIC() { partFrame->prePosIC(); } -void MbD::Part::prePosKine() +void Part::prePosKine() { partFrame->prePosKine(); } -void MbD::Part::iqX(int eqnNo) +void Part::iqX(int eqnNo) { partFrame->iqX = eqnNo; } -void MbD::Part::iqE(int eqnNo) +void Part::iqE(int eqnNo) { partFrame->iqE = eqnNo; } -void MbD::Part::fillEssenConstraints(std::shared_ptr>> essenConstraints) +void Part::fillEssenConstraints(std::shared_ptr>> essenConstraints) { partFrame->fillEssenConstraints(essenConstraints); } -void MbD::Part::fillRedundantConstraints(std::shared_ptr>> redunConstraints) +void Part::fillRedundantConstraints(std::shared_ptr>> redunConstraints) { } -void MbD::Part::fillConstraints(std::shared_ptr>> allConstraints) +void Part::fillConstraints(std::shared_ptr>> allConstraints) { partFrame->fillConstraints(allConstraints); } -void MbD::Part::fillqsu(FColDsptr col) +void Part::fillqsu(FColDsptr col) { partFrame->fillqsu(col); } -void MbD::Part::fillqsuWeights(std::shared_ptr> diagMat) +void Part::fillqsuWeights(std::shared_ptr> diagMat) { //"Map wqX and wqE according to inertias. (0 to maximum inertia) map to (minw to maxw)" //"When the inertias are zero, they are set to a small number for positive definiteness." @@ -228,8 +243,8 @@ void MbD::Part::fillqsuWeights(std::shared_ptr> diagMat) //"Redundant constraint removal likes equal weights." //"wqE(4) = 0.0d is ok because there is always the euler parameter constraint." - auto mMax = System::getInstance().maximumMass(); - auto aJiMax = System::getInstance().maximumMomentOfInertia(); + auto mMax = this->root()->maximumMass(); + auto aJiMax = this->root()->maximumMomentOfInertia(); auto minw = 1.0e3; auto maxw = 1.0e6; auto wqX = std::make_shared>(3); @@ -251,17 +266,17 @@ void MbD::Part::fillqsuWeights(std::shared_ptr> diagMat) partFrame->fillqsuWeights(diagMat); } -void MbD::Part::fillqsulam(FColDsptr col) +void Part::fillqsulam(FColDsptr col) { partFrame->fillqsulam(col); } -void MbD::Part::fillqsudot(FColDsptr col) +void Part::fillqsudot(FColDsptr col) { partFrame->fillqsudot(col); } -void MbD::Part::fillqsudotWeights(std::shared_ptr> diagMat) +void Part::fillqsudotWeights(std::shared_ptr> diagMat) { //"wqXdot and wqEdot are set to their respective inertias." //"When the inertias are zero, they are set to a small number for positive definiteness." @@ -269,8 +284,8 @@ void MbD::Part::fillqsudotWeights(std::shared_ptr> diagMa //"wqEdot(4) = 0.0d is ok because there is always the euler parameter constraint." //| mMax aJiMax maxInertia minw maxw aJi wqXdot wqEdot | - auto mMax = System::getInstance().maximumMass(); - auto aJiMax = System::getInstance().maximumMomentOfInertia(); + auto mMax = this->root()->maximumMass(); + auto aJiMax = this->root()->maximumMomentOfInertia(); auto maxInertia = std::max(mMax, aJiMax); if (maxInertia == 0) maxInertia = 1.0; auto minw = 1.0e-12 * maxInertia; @@ -289,94 +304,99 @@ void MbD::Part::fillqsudotWeights(std::shared_ptr> diagMa partFrame->fillqsudotWeights(diagMat); } -void MbD::Part::useEquationNumbers() +void Part::useEquationNumbers() { partFrame->useEquationNumbers(); } -void MbD::Part::setqsu(FColDsptr col) +void Part::setqsu(FColDsptr col) { partFrame->setqsu(col); } -void MbD::Part::setqsulam(FColDsptr col) +void Part::setqsulam(FColDsptr col) { partFrame->setqsulam(col); } -void MbD::Part::setqsudotlam(FColDsptr col) +void Part::setqsudot(FColDsptr col) +{ + partFrame->setqsudot(col); +} + +void Part::setqsudotlam(FColDsptr col) { partFrame->setqsudotlam(col); } -void MbD::Part::postPosICIteration() +void Part::postPosICIteration() { partFrame->postPosICIteration(); } -void MbD::Part::fillPosICError(FColDsptr col) +void Part::fillPosICError(FColDsptr col) { partFrame->fillPosICError(col); } -void MbD::Part::fillPosICJacob(SpMatDsptr mat) +void Part::fillPosICJacob(SpMatDsptr mat) { partFrame->fillPosICJacob(mat); } -void MbD::Part::removeRedundantConstraints(std::shared_ptr> redundantEqnNos) +void Part::removeRedundantConstraints(std::shared_ptr> redundantEqnNos) { partFrame->removeRedundantConstraints(redundantEqnNos); } -void MbD::Part::reactivateRedundantConstraints() +void Part::reactivateRedundantConstraints() { partFrame->reactivateRedundantConstraints(); } -void MbD::Part::constraintsReport() +void Part::constraintsReport() { partFrame->constraintsReport(); } -void MbD::Part::postPosIC() +void Part::postPosIC() { partFrame->postPosIC(); this->calcmE(); } -void MbD::Part::outputStates() +void Part::outputStates() { Item::outputStates(); partFrame->outputStates(); } -void MbD::Part::preDyn() +void Part::preDyn() { partFrame->preDyn(); } -void MbD::Part::storeDynState() +void Part::storeDynState() { partFrame->storeDynState(); } -void MbD::Part::fillPosKineError(FColDsptr col) +void Part::fillPosKineError(FColDsptr col) { partFrame->fillPosKineError(col); } -void MbD::Part::fillPosKineJacob(SpMatDsptr mat) +void Part::fillPosKineJacob(SpMatDsptr mat) { partFrame->fillPosKineJacob(mat); } -void MbD::Part::preVelIC() +void Part::preVelIC() { partFrame->preVelIC(); } -void MbD::Part::postVelIC() +void Part::postVelIC() { partFrame->postVelIC(); this->calcp(); @@ -386,35 +406,35 @@ void MbD::Part::postVelIC() this->calcppTpEpEdot(); } -void MbD::Part::fillVelICError(FColDsptr col) +void Part::fillVelICError(FColDsptr col) { partFrame->fillVelICError(col); } -void MbD::Part::fillVelICJacob(SpMatDsptr mat) +void Part::fillVelICJacob(SpMatDsptr mat) { partFrame->fillVelICJacob(mat); } -void MbD::Part::preAccIC() +void Part::preAccIC() { partFrame->preAccIC(); Item::preAccIC(); } -void MbD::Part::calcp() +void Part::calcp() { pX = mX->timesFullColumn(partFrame->qXdot); pE = mE->timesFullColumn(partFrame->qEdot); } -void MbD::Part::calcpdot() +void Part::calcpdot() { pXdot = mX->timesFullColumn(partFrame->qXddot); pEdot = mEdot->timesFullColumn(partFrame->qEdot)->plusFullColumn(mE->timesFullColumn(partFrame->qEddot)); } -void MbD::Part::calcmEdot() +void Part::calcmEdot() { auto aC = partFrame->aC(); auto aCdot = partFrame->aCdot(); @@ -424,7 +444,7 @@ void MbD::Part::calcmEdot() mEdot = term1->plusFullMatrix(term2); } -void MbD::Part::calcpTpE() +void Part::calcpTpE() { //"pTpE is a column vector." auto& qEdot = partFrame->qEdot; @@ -433,7 +453,7 @@ void MbD::Part::calcpTpE() pTpE = (pCpEtimesqEdot->transposeTimesFullColumn(aJ->timesFullColumn(aC->timesFullColumn(qEdot))))->times(4.0); } -void MbD::Part::calcppTpEpE() +void Part::calcppTpEpE() { auto& qEdot = partFrame->qEdot; auto pCpEtimesqEdot = EulerParameters::pCpEtimesColumn(qEdot); @@ -441,7 +461,7 @@ void MbD::Part::calcppTpEpE() ppTpEpE = pCpEtimesqEdot->transposeTimesFullMatrix(a4J->timesFullMatrix(pCpEtimesqEdot)); } -void MbD::Part::calcppTpEpEdot() +void Part::calcppTpEpEdot() { //| qEdot aC a4J term1 pCpEtimesqEdot term2 | auto& qEdot = partFrame->qEdot; @@ -453,13 +473,13 @@ void MbD::Part::calcppTpEpEdot() ppTpEpEdot = term1->plusFullMatrix(term2)->transpose(); } -void MbD::Part::calcmE() +void Part::calcmE() { auto aC = partFrame->aC(); mE = aC->transposeTimesFullMatrix(aJ->timesFullMatrix(aC))->times(4.0); } -void MbD::Part::fillAccICIterError(FColDsptr col) +void Part::fillAccICIterError(FColDsptr col) { auto iqX = partFrame->iqX; auto iqE = partFrame->iqE; @@ -470,7 +490,7 @@ void MbD::Part::fillAccICIterError(FColDsptr col) partFrame->fillAccICIterError(col); } -void MbD::Part::fillAccICIterJacob(SpMatDsptr mat) +void Part::fillAccICIterJacob(SpMatDsptr mat) { auto iqX = partFrame->iqX; auto iqE = partFrame->iqE; @@ -479,12 +499,64 @@ void MbD::Part::fillAccICIterJacob(SpMatDsptr mat) partFrame->fillAccICIterJacob(mat); } -std::shared_ptr> MbD::Part::qEdot() +std::shared_ptr> Part::qEdot() { return partFrame->qEdot; } -void MbD::Part::setqsuddotlam(FColDsptr qsudotlam) +void Part::setqsuddotlam(FColDsptr col) { - partFrame->setqsuddotlam(qsudotlam); + partFrame->setqsuddotlam(col); +} + +std::shared_ptr Part::stateData() +{ + //" + //P : = part frame. + //p : = principal frame at cm. + //rOcmO : = rOPO + aAOP * rPcmP. + //aAOp : = aAOP * aAPp. + //vOcmO : = vOPO + aAdotOP * rPcmP + //: = vOPO + (omeOPO cross : aAOP * rPcmP). + //omeOpO : = omeOPO. + //aOcmO : = aOPO + aAddotOP * rPcmP + //: = aOPO + (alpOPO cross : aAOP * rPcmP) + (omeOPO cross : (omeOPO cross : aAOP * rPcmP)). + //alpOpO : = alpOPO. + + //Therefore + //aAOP : = aAOp * aAPpT + //rOPO : = rOcmO - aAOP * rPcmP. + //omeOPO : = omeOpO. + //vOPO : = vOcmO - (omeOPO cross : aAOP * rPcmP). + //alpOPO : = alpOpO. + //aOPO : = aOcmO - (alpOPO cross : aAOP * rPcmP) - (omeOPO cross : (omeOPO cross : + //aAOP * rPcmP)). + //" + + auto rOpO = this->qX(); + auto aAOp = this->aAOp(); + auto vOpO = this->qXdot(); + auto omeOpO = this->omeOpO(); + auto aOpO = this->qXddot(); + auto alpOpO = this->alpOpO(); + auto answer = std::make_shared(); + answer->rFfF = rOpO; + answer->aAFf = aAOp; + answer->vFfF = vOpO; + answer->omeFfF = omeOpO; + answer->aFfF = aOpO; + answer->alpFfF = alpOpO; + return answer; +} + +double Part::suggestSmallerOrAcceptDynStepSize(double hnew) +{ + auto hnew2 = hnew; + hnew2 = partFrame->suggestSmallerOrAcceptDynStepSize(hnew2); + return hnew2; +} + +void Part::postDynStep() +{ + partFrame->postDynStep(); } diff --git a/MbDCode/Part.h b/MbDCode/Part.h index 66c3479..1e1066f 100644 --- a/MbDCode/Part.h +++ b/MbDCode/Part.h @@ -9,8 +9,7 @@ namespace MbD { class System; class PartFrame; - template - class DiagonalMatrix; + template class DiagonalMatrix; class Part : public Item { @@ -18,6 +17,7 @@ namespace MbD { public: Part(); Part(const char* str); + System* root() override; void initialize() override; void initializeLocally() override; void initializeGlobally() override; @@ -45,7 +45,10 @@ namespace MbD { FColDsptr qXddot(); void qEddot(FColDsptr x); FColDsptr qEddot(); - void setSystem(System& sys); + FMatDsptr aAOp(); + FColDsptr alpOpO(); + + void setSystem(System* sys); void asFixed(); void postInput() override; void calcPostDynCorrectorIteration() override; @@ -65,6 +68,7 @@ namespace MbD { void useEquationNumbers() override; void setqsu(FColDsptr col) override; void setqsulam(FColDsptr col) override; + void setqsudot(FColDsptr col) override; void setqsudotlam(FColDsptr col) override; void postPosICIteration() override; void fillPosICError(FColDsptr col) override; @@ -93,8 +97,12 @@ namespace MbD { void fillAccICIterError(FColDsptr col) override; void fillAccICIterJacob(SpMatDsptr mat) override; std::shared_ptr> qEdot(); - void setqsuddotlam(FColDsptr qsudotlam) override; + void setqsuddotlam(FColDsptr col) override; + std::shared_ptr stateData() override; + double suggestSmallerOrAcceptDynStepSize(double hnew) override; + void postDynStep() override; + System* system; //Use raw pointer when pointing backwards. int ipX = -1; int ipE = -1; double m = 0.0; diff --git a/MbDCode/PartFrame.cpp b/MbDCode/PartFrame.cpp index c1ab959..938daeb 100644 --- a/MbDCode/PartFrame.cpp +++ b/MbDCode/PartFrame.cpp @@ -9,6 +9,7 @@ #include "EulerParametersDot.h" #include "CREATE.h" #include "RedundantConstraint.h" +#include "System.h" using namespace MbD; @@ -18,6 +19,10 @@ PartFrame::PartFrame() PartFrame::PartFrame(const char* str) : CartesianFrame(str) { } +System* PartFrame::root() +{ + return part->root(); +} void PartFrame::initialize() { aGeu = CREATE::With(); @@ -67,29 +72,34 @@ void PartFrame::setomeOpO(FColDsptr omeOpO) { } FColDsptr PartFrame::getomeOpO() { - return qE; + return qEdot->omeOpO(); } -void MbD::PartFrame::setqXddot(FColDsptr x) +void PartFrame::setqXddot(FColDsptr x) { qXddot = x; } -FColDsptr MbD::PartFrame::getqXddot() +FColDsptr PartFrame::getqXddot() { return qXddot; } -void MbD::PartFrame::setqEddot(FColDsptr x) +void PartFrame::setqEddot(FColDsptr x) { qEddot = x; } -FColDsptr MbD::PartFrame::getqEddot() +FColDsptr PartFrame::getqEddot() { return qEddot; } +FColDsptr PartFrame::omeOpO() +{ + return qEdot->omeOpO(); +} + void PartFrame::setPart(Part* x) { part = x; } @@ -110,17 +120,17 @@ EndFrmcptr PartFrame::endFrame(std::string name) return (*match)->endFrames->at(0); } -void MbD::PartFrame::aGabsDo(const std::function)>& f) +void PartFrame::aGabsDo(const std::function)>& f) { std::for_each(aGabs->begin(), aGabs->end(), f); } -void MbD::PartFrame::markerFramesDo(const std::function)>& f) +void PartFrame::markerFramesDo(const std::function)>& f) { std::for_each(markerFrames->begin(), markerFrames->end(), f); } -void MbD::PartFrame::removeRedundantConstraints(std::shared_ptr> redundantEqnNos) +void PartFrame::removeRedundantConstraints(std::shared_ptr> redundantEqnNos) { if (std::find(redundantEqnNos->begin(), redundantEqnNos->end(), aGeu->iG) != redundantEqnNos->end()) { auto redunCon = CREATE::With(); @@ -138,7 +148,7 @@ void MbD::PartFrame::removeRedundantConstraints(std::shared_ptr } } -void MbD::PartFrame::reactivateRedundantConstraints() +void PartFrame::reactivateRedundantConstraints() { if (aGeu->isRedundant()) aGeu = std::dynamic_pointer_cast(aGeu)->constraint; for (size_t i = 0; i < aGabs->size(); i++) @@ -150,7 +160,7 @@ void MbD::PartFrame::reactivateRedundantConstraints() } } -void MbD::PartFrame::constraintsReport() +void PartFrame::constraintsReport() { auto redunCons = std::make_shared>>(); aGabsDo([&](std::shared_ptr con) { @@ -169,7 +179,7 @@ void MbD::PartFrame::constraintsReport() } } -void MbD::PartFrame::prePosIC() +void PartFrame::prePosIC() { iqX = -1; iqE = -1; @@ -179,7 +189,7 @@ void MbD::PartFrame::prePosIC() aGabsDo([](std::shared_ptr aGab) { aGab->prePosIC(); }); } -void MbD::PartFrame::prePosKine() +void PartFrame::prePosKine() { iqX = -1; iqE = -1; @@ -189,60 +199,67 @@ void MbD::PartFrame::prePosKine() aGabsDo([](std::shared_ptr aGab) { aGab->prePosKine(); }); } -FColDsptr MbD::PartFrame::rOpO() +FColDsptr PartFrame::rOpO() { return qX; } -FMatDsptr MbD::PartFrame::aAOp() +FMatDsptr PartFrame::aAOp() { return qE->aA; } -FMatDsptr MbD::PartFrame::aC() +FMatDsptr PartFrame::aC() { return qE->aC; } -FMatDsptr MbD::PartFrame::aCdot() +FMatDsptr PartFrame::aCdot() { return qEdot->aCdot; } -FColFMatDsptr MbD::PartFrame::pAOppE() +FColDsptr PartFrame::alpOpO() +{ + auto& aB = qE->aB; + auto& aBdot = qEdot->aBdot; + return aBdot->timesFullColumn(qEdot)->plusFullColumn(aB->timesFullColumn(qEddot))->times(2.0); +} + +FColFMatDsptr PartFrame::pAOppE() { return qE->pApE; } -void MbD::PartFrame::fillEssenConstraints(std::shared_ptr>> essenConstraints) +void PartFrame::fillEssenConstraints(std::shared_ptr>> essenConstraints) { aGeu->fillEssenConstraints(aGeu, essenConstraints); aGabsDo([&](std::shared_ptr con) { con->fillEssenConstraints(con, essenConstraints); }); } -void MbD::PartFrame::fillRedundantConstraints(std::shared_ptr>> redunConstraints) +void PartFrame::fillRedundantConstraints(std::shared_ptr>> redunConstraints) { } -void MbD::PartFrame::fillConstraints(std::shared_ptr>> allConstraints) +void PartFrame::fillConstraints(std::shared_ptr>> allConstraints) { aGeu->fillConstraints(aGeu, allConstraints); aGabsDo([&](std::shared_ptr con) { con->fillConstraints(con, allConstraints); }); } -void MbD::PartFrame::fillqsu(FColDsptr col) +void PartFrame::fillqsu(FColDsptr col) { col->atiputFullColumn(iqX, qX); col->atiputFullColumn(iqE, qE); markerFramesDo([&](std::shared_ptr markerFrame) { markerFrame->fillqsu(col); }); } -void MbD::PartFrame::fillqsuWeights(std::shared_ptr> diagMat) +void PartFrame::fillqsuWeights(std::shared_ptr> diagMat) { markerFramesDo([&](std::shared_ptr markerFrame) { markerFrame->fillqsuWeights(diagMat); }); } -void MbD::PartFrame::fillqsulam(FColDsptr col) +void PartFrame::fillqsulam(FColDsptr col) { col->atiputFullColumn(iqX, qX); col->atiputFullColumn(iqE, qE); @@ -251,26 +268,26 @@ void MbD::PartFrame::fillqsulam(FColDsptr col) aGabsDo([&](std::shared_ptr con) { con->fillqsulam(col); }); } -void MbD::PartFrame::fillqsudot(FColDsptr col) +void PartFrame::fillqsudot(FColDsptr col) { col->atiputFullColumn(iqX, qXdot); col->atiputFullColumn(iqE, qEdot); markerFramesDo([&](std::shared_ptr markerFrame) { markerFrame->fillqsudot(col); }); } -void MbD::PartFrame::fillqsudotWeights(std::shared_ptr> diagMat) +void PartFrame::fillqsudotWeights(std::shared_ptr> diagMat) { markerFramesDo([&](std::shared_ptr markerFrame) { markerFrame->fillqsudotWeights(diagMat); }); } -void MbD::PartFrame::useEquationNumbers() +void PartFrame::useEquationNumbers() { markerFramesDo([](std::shared_ptr markerFrame) { markerFrame->useEquationNumbers(); }); aGeu->useEquationNumbers(); aGabsDo([](std::shared_ptr con) { con->useEquationNumbers(); }); } -void MbD::PartFrame::setqsu(FColDsptr col) +void PartFrame::setqsu(FColDsptr col) { qX->equalFullColumnAt(col, iqX); qE->equalFullColumnAt(col, iqE); @@ -279,7 +296,7 @@ void MbD::PartFrame::setqsu(FColDsptr col) aGabsDo([&](std::shared_ptr con) { con->setqsu(col); }); } -void MbD::PartFrame::setqsulam(FColDsptr col) +void PartFrame::setqsulam(FColDsptr col) { qX->equalFullColumnAt(col, iqX); qE->equalFullColumnAt(col, iqE); @@ -288,7 +305,7 @@ void MbD::PartFrame::setqsulam(FColDsptr col) aGabsDo([&](std::shared_ptr con) { con->setqsulam(col); }); } -void MbD::PartFrame::setqsudotlam(FColDsptr col) +void PartFrame::setqsudotlam(FColDsptr col) { qXdot->equalFullColumnAt(col, iqX); qEdot->equalFullColumnAt(col, iqE); @@ -297,7 +314,14 @@ void MbD::PartFrame::setqsudotlam(FColDsptr col) aGabsDo([&](std::shared_ptr con) { con->setqsudotlam(col); }); } -void MbD::PartFrame::postPosICIteration() +void PartFrame::setqsudot(FColDsptr col) +{ + qXdot->equalFullColumnAt(col, iqX); + qEdot->equalFullColumnAt(col, iqE); + markerFramesDo([&](std::shared_ptr markerFrame) { markerFrame->setqsudot(col); }); +} + +void PartFrame::postPosICIteration() { Item::postPosICIteration(); markerFramesDo([](std::shared_ptr markerFrame) { markerFrame->postPosICIteration(); }); @@ -305,28 +329,28 @@ void MbD::PartFrame::postPosICIteration() aGabsDo([](std::shared_ptr con) { con->postPosICIteration(); }); } -void MbD::PartFrame::fillPosICError(FColDsptr col) +void 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) +void 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() +void PartFrame::postPosIC() { markerFramesDo([](std::shared_ptr markerFrame) { markerFrame->postPosIC(); }); aGeu->postPosIC(); aGabsDo([](std::shared_ptr con) { con->postPosIC(); }); } -void MbD::PartFrame::outputStates() +void PartFrame::outputStates() { std::stringstream ss; ss << "qX = "; @@ -340,28 +364,28 @@ void MbD::PartFrame::outputStates() aGabsDo([](std::shared_ptr con) { con->outputStates(); }); } -void MbD::PartFrame::preDyn() +void PartFrame::preDyn() { markerFramesDo([](std::shared_ptr markerFrame) { markerFrame->preDyn(); }); aGeu->preDyn(); aGabsDo([](std::shared_ptr aGab) { aGab->preDyn(); }); } -void MbD::PartFrame::storeDynState() +void PartFrame::storeDynState() { markerFramesDo([](std::shared_ptr markerFrame) { markerFrame->storeDynState(); }); aGeu->storeDynState(); aGabsDo([](std::shared_ptr aGab) { aGab->storeDynState(); }); } -void MbD::PartFrame::fillPosKineError(FColDsptr col) +void PartFrame::fillPosKineError(FColDsptr col) { markerFramesDo([&](std::shared_ptr markerFrame) { markerFrame->fillPosKineError(col); }); aGeu->fillPosKineError(col); aGabsDo([&](std::shared_ptr con) { con->fillPosKineError(col); }); } -void MbD::PartFrame::preVelIC() +void PartFrame::preVelIC() { Item::preVelIC(); markerFramesDo([](std::shared_ptr markerFrame) { markerFrame->preVelIC(); }); @@ -369,7 +393,7 @@ void MbD::PartFrame::preVelIC() aGabsDo([](std::shared_ptr aGab) { aGab->preVelIC(); }); } -void MbD::PartFrame::postVelIC() +void PartFrame::postVelIC() { qEdot->calcAdotBdotCdot(); qEdot->calcpAdotpE(); @@ -378,21 +402,21 @@ void MbD::PartFrame::postVelIC() aGabsDo([](std::shared_ptr aGab) { aGab->postVelIC(); }); } -void MbD::PartFrame::fillVelICError(FColDsptr col) +void PartFrame::fillVelICError(FColDsptr col) { markerFramesDo([&](std::shared_ptr markerFrame) { markerFrame->fillVelICError(col); }); aGeu->fillVelICError(col); aGabsDo([&](std::shared_ptr con) { con->fillVelICError(col); }); } -void MbD::PartFrame::fillVelICJacob(SpMatDsptr mat) +void PartFrame::fillVelICJacob(SpMatDsptr mat) { markerFramesDo([&](std::shared_ptr markerFrame) { markerFrame->fillVelICJacob(mat); }); aGeu->fillVelICJacob(mat); aGabsDo([&](std::shared_ptr con) { con->fillVelICJacob(mat); }); } -void MbD::PartFrame::preAccIC() +void PartFrame::preAccIC() { qXddot = std::make_shared>(3, 0.0); qEddot = std::make_shared>(4, 0.0); @@ -402,27 +426,79 @@ void MbD::PartFrame::preAccIC() aGabsDo([](std::shared_ptr aGab) { aGab->preAccIC(); }); } -void MbD::PartFrame::fillAccICIterError(FColDsptr col) +void PartFrame::fillAccICIterError(FColDsptr col) { markerFramesDo([&](std::shared_ptr markerFrame) { markerFrame->fillAccICIterError(col); }); aGeu->fillAccICIterError(col); aGabsDo([&](std::shared_ptr con) { con->fillAccICIterError(col); }); } -void MbD::PartFrame::fillAccICIterJacob(SpMatDsptr mat) +void PartFrame::fillAccICIterJacob(SpMatDsptr mat) { markerFramesDo([&](std::shared_ptr markerFrame) { markerFrame->fillAccICIterJacob(mat); }); aGeu->fillAccICIterJacob(mat); aGabsDo([&](std::shared_ptr con) { con->fillAccICIterJacob(mat); }); } -void MbD::PartFrame::setqsuddotlam(FColDsptr qsudotlam) +void PartFrame::setqsuddotlam(FColDsptr col) { - qXddot->equalFullColumnAt(qsudotlam, iqX); - qEddot->equalFullColumnAt(qsudotlam, iqE); - markerFramesDo([&](std::shared_ptr markerFrame) { markerFrame->setqsuddotlam(qsudotlam); }); - aGeu->setqsuddotlam(qsudotlam); - aGabsDo([&](std::shared_ptr con) { con->setqsuddotlam(qsudotlam); }); + qXddot->equalFullColumnAt(col, iqX); + qEddot->equalFullColumnAt(col, iqE); + markerFramesDo([&](std::shared_ptr markerFrame) { markerFrame->setqsuddotlam(col); }); + aGeu->setqsuddotlam(col); + aGabsDo([&](std::shared_ptr con) { con->setqsuddotlam(col); }); +} + +FMatDsptr PartFrame::aBOp() +{ + return qE->aB; +} + +void PartFrame::fillPosKineJacob(SpMatDsptr mat) +{ + markerFramesDo([&](std::shared_ptr markerFrame) { markerFrame->fillPosKineJacob(mat); }); + aGeu->fillPosKineJacob(mat); + aGabsDo([&](std::shared_ptr con) { con->fillPosKineJacob(mat); }); +} + +double PartFrame::suggestSmallerOrAcceptDynStepSize(double hnew) +{ + auto hnew2 = hnew; + auto speed = qXdot->length(); + double htran; + if (speed < 1.0e-15) { + htran = 1.0e99; + } + else { + htran = this->root()->translationLimit() / speed; + } + if (hnew2 > htran) { + this->logString("MbD: Time step limited by translation limit per step."); + hnew2 = htran; + } + auto omegaMagnitude = qEdot->omeOpO()->length(); + double hrot; + if (omegaMagnitude < 1.0e-15) { + hrot = 1.0e99; + } + else { + hrot = this->root()->rotationLimit() / omegaMagnitude; + } + if (hnew2 > hrot) { + this->logString("MbD: Time step limited by rotation limit per step."); + hnew2 = hrot; + } + markerFramesDo([&](std::shared_ptr markerFrame) { hnew2 = markerFrame->suggestSmallerOrAcceptDynStepSize(hnew2); }); + hnew2 = aGeu->suggestSmallerOrAcceptDynStepSize(hnew2); + aGabsDo([&](std::shared_ptr aGab) { hnew2 = aGab->suggestSmallerOrAcceptDynStepSize(hnew2); }); + return hnew2; +} + +void PartFrame::postDynStep() +{ + markerFramesDo([](std::shared_ptr markerFrame) { markerFrame->postDynStep(); }); + aGeu->postDynStep(); + aGabsDo([](std::shared_ptr aGab) { aGab->postDynStep(); }); } void PartFrame::asFixed() @@ -434,7 +510,7 @@ void PartFrame::asFixed() } } -void MbD::PartFrame::postInput() +void PartFrame::postInput() { qXddot = std::make_shared>(3, 0.0); qEddot = std::make_shared>(4, 0.0); @@ -444,7 +520,7 @@ void MbD::PartFrame::postInput() aGabsDo([](std::shared_ptr aGab) { aGab->postInput(); }); } -void MbD::PartFrame::calcPostDynCorrectorIteration() +void PartFrame::calcPostDynCorrectorIteration() { qE->calcABC(); qE->calcpApE(); diff --git a/MbDCode/PartFrame.h b/MbDCode/PartFrame.h index cbab5f5..5be6afb 100644 --- a/MbDCode/PartFrame.h +++ b/MbDCode/PartFrame.h @@ -24,6 +24,7 @@ namespace MbD { public: PartFrame(); PartFrame(const char* str); + System* root() override; void initialize() override; void initializeLocally() override; void initializeGlobally() override; @@ -43,6 +44,8 @@ namespace MbD { FColDsptr getqXddot(); void setqEddot(FColDsptr x); FColDsptr getqEddot(); + FColDsptr omeOpO(); + void setPart(Part* x); Part* getPart(); void addMarkerFrame(std::shared_ptr x); @@ -59,6 +62,7 @@ namespace MbD { FMatDsptr aAOp(); FMatDsptr aC(); FMatDsptr aCdot(); + FColDsptr alpOpO(); FColFMatDsptr pAOppE(); void fillEssenConstraints(std::shared_ptr>> essenConstraints) override; void fillRedundantConstraints(std::shared_ptr>> redunConstraints) override; @@ -72,6 +76,8 @@ namespace MbD { void setqsu(FColDsptr col) override; void setqsulam(FColDsptr col) override; void setqsudotlam(FColDsptr col) override; + void setqsudot(FColDsptr col) override; + void setqsuddotlam(FColDsptr col) override; void postPosICIteration() override; void fillPosICError(FColDsptr col) override; void fillPosICJacob(SpMatDsptr mat) override; @@ -87,7 +93,10 @@ namespace MbD { void preAccIC() override; void fillAccICIterError(FColDsptr col) override; void fillAccICIterJacob(SpMatDsptr mat) override; - void setqsuddotlam(FColDsptr qsudotlam) override; + FMatDsptr aBOp(); + void fillPosKineJacob(SpMatDsptr mat) override; + double suggestSmallerOrAcceptDynStepSize(double hnew) override; + void postDynStep() override; Part* part = nullptr; //Use raw pointer when pointing backwards. int iqX = -1; diff --git a/MbDCode/PosICKineNewtonRaphson.cpp b/MbDCode/PosICKineNewtonRaphson.cpp index e09a2fb..a698733 100644 --- a/MbDCode/PosICKineNewtonRaphson.cpp +++ b/MbDCode/PosICKineNewtonRaphson.cpp @@ -5,14 +5,14 @@ using namespace MbD; -void MbD::PosICKineNewtonRaphson::initializeGlobally() +void PosICKineNewtonRaphson::initializeGlobally() { AnyPosICNewtonRaphson::initializeGlobally(); iterMax = system->iterMaxPosKine; dxTol = system->errorTolPosKine; } -void MbD::PosICKineNewtonRaphson::assignEquationNumbers() +void PosICKineNewtonRaphson::assignEquationNumbers() { auto parts = system->parts(); //auto contactEndFrames = system->contactEndFrames(); diff --git a/MbDCode/PosICNewtonRaphson.cpp b/MbDCode/PosICNewtonRaphson.cpp index da6fafa..9c32b06 100644 --- a/MbDCode/PosICNewtonRaphson.cpp +++ b/MbDCode/PosICNewtonRaphson.cpp @@ -12,7 +12,7 @@ using namespace MbD; -void MbD::PosICNewtonRaphson::run() +void PosICNewtonRaphson::run() { while (true) { try { @@ -28,14 +28,14 @@ void MbD::PosICNewtonRaphson::run() } } -void MbD::PosICNewtonRaphson::preRun() +void PosICNewtonRaphson::preRun() { std::string str("MbD: Assembling system. "); system->logString(str); PosNewtonRaphson::preRun(); } -void MbD::PosICNewtonRaphson::assignEquationNumbers() +void PosICNewtonRaphson::assignEquationNumbers() { auto parts = system->parts(); //auto contactEndFrames = system->contactEndFrames(); @@ -81,12 +81,12 @@ void MbD::PosICNewtonRaphson::assignEquationNumbers() pivotRowLimits = std::make_shared>(rangelimits); } -bool MbD::PosICNewtonRaphson::isConverged() +bool PosICNewtonRaphson::isConverged() { return this->isConvergedToNumericalLimit(); } -void MbD::PosICNewtonRaphson::handleSingularMatrix() +void PosICNewtonRaphson::handleSingularMatrix() { nSingularMatrixError++; if (nSingularMatrixError = 1){ @@ -95,13 +95,13 @@ void MbD::PosICNewtonRaphson::handleSingularMatrix() } else { std::string str = typeid(*matrixSolver).name(); - if (str == "class MbD::GESpMatParPvMarkoFast") { + if (str == "class GESpMatParPvMarkoFast") { matrixSolver = CREATE::With(); this->solveEquations(); } else { str = typeid(*matrixSolver).name(); - if (str == "class MbD::GESpMatParPvPrecise") { + if (str == "class GESpMatParPvPrecise") { this->lookForRedundantConstraints(); matrixSolver = this->matrixSolverClassNew(); } @@ -112,7 +112,7 @@ void MbD::PosICNewtonRaphson::handleSingularMatrix() } } -void MbD::PosICNewtonRaphson::lookForRedundantConstraints() +void PosICNewtonRaphson::lookForRedundantConstraints() { std::string str("MbD: Checking for redundant constraints."); system->logString(str); diff --git a/MbDCode/PosKineNewtonRaphson.cpp b/MbDCode/PosKineNewtonRaphson.cpp index 74084b3..7c7a3da 100644 --- a/MbDCode/PosKineNewtonRaphson.cpp +++ b/MbDCode/PosKineNewtonRaphson.cpp @@ -6,7 +6,7 @@ using namespace MbD; -void MbD::PosKineNewtonRaphson::initializeGlobally() +void PosKineNewtonRaphson::initializeGlobally() { SystemNewtonRaphson::initializeGlobally(); system->partsJointsMotionsDo([&](std::shared_ptr item) { item->fillqsu(x); }); @@ -14,7 +14,7 @@ void MbD::PosKineNewtonRaphson::initializeGlobally() dxTol = system->errorTolPosKine; } -void MbD::PosKineNewtonRaphson::fillPyPx() +void PosKineNewtonRaphson::fillPyPx() { pypx->zeroSelf(); system->partsJointsMotionsDo([&](std::shared_ptr item) { @@ -22,12 +22,12 @@ void MbD::PosKineNewtonRaphson::fillPyPx() }); } -void MbD::PosKineNewtonRaphson::passRootToSystem() +void PosKineNewtonRaphson::passRootToSystem() { system->partsJointsMotionsDo([&](std::shared_ptr item) { item->setqsu(x); }); } -void MbD::PosKineNewtonRaphson::assignEquationNumbers() +void PosKineNewtonRaphson::assignEquationNumbers() { //"Equation order is q,s,u." auto parts = system->parts(); @@ -62,14 +62,14 @@ void MbD::PosKineNewtonRaphson::assignEquationNumbers() } } -void MbD::PosKineNewtonRaphson::preRun() +void PosKineNewtonRaphson::preRun() { std::string str = "MbD: Solving for kinematic position."; system->logString(str); system->partsJointsMotionsDo([](std::shared_ptr item) { item->prePosKine(); }); } -void MbD::PosKineNewtonRaphson::fillY() +void PosKineNewtonRaphson::fillY() { y->zeroSelf(); system->partsJointsMotionsDo([&](std::shared_ptr item) { item->fillPosKineError(y); }); diff --git a/MbDCode/PosNewtonRaphson.cpp b/MbDCode/PosNewtonRaphson.cpp index 9f2d608..e4a33b6 100644 --- a/MbDCode/PosNewtonRaphson.cpp +++ b/MbDCode/PosNewtonRaphson.cpp @@ -6,12 +6,12 @@ using namespace MbD; -void MbD::PosNewtonRaphson::preRun() +void PosNewtonRaphson::preRun() { system->partsJointsMotionsDo([&](std::shared_ptr item) { item->prePosIC(); }); } -void MbD::PosNewtonRaphson::incrementIterNo() +void PosNewtonRaphson::incrementIterNo() { if (iterNo >= iterMax) { @@ -34,12 +34,12 @@ void MbD::PosNewtonRaphson::incrementIterNo() iterNo++; } -void MbD::PosNewtonRaphson::askSystemToUpdate() +void PosNewtonRaphson::askSystemToUpdate() { system->partsJointsMotionsDo([&](std::shared_ptr item) { item->postPosICIteration(); }); } -void MbD::PosNewtonRaphson::postRun() +void PosNewtonRaphson::postRun() { system->partsJointsMotionsDo([&](std::shared_ptr item) { item->postPosIC(); }); } diff --git a/MbDCode/Product.cpp b/MbDCode/Product.cpp index e27cb4c..3de8546 100644 --- a/MbDCode/Product.cpp +++ b/MbDCode/Product.cpp @@ -7,7 +7,7 @@ using namespace MbD; -Symsptr MbD::Product::differentiateWRT(Symsptr sptr, Symsptr var) +Symsptr Product::differentiateWRT(Symsptr sptr, Symsptr var) { //"Apply chain rule of differentiation." // "(xyz)' := x'yz + xy'z + xyz'." @@ -33,7 +33,7 @@ Symsptr MbD::Product::differentiateWRT(Symsptr sptr, Symsptr var) return answer; } -Symsptr MbD::Product::expandUntil(Symsptr sptr, std::shared_ptr> set) +Symsptr Product::expandUntil(Symsptr sptr, std::shared_ptr> set) { auto itr = std::find_if(set->begin(), set->end(), [sptr](Symsptr sym) {return sptr.get() == sym.get(); }); if (itr != set->end()) { @@ -64,7 +64,7 @@ Symsptr MbD::Product::expandUntil(Symsptr sptr, std::shared_ptrtimesSum(factor, sumOfProductsOfSums); } -Symsptr MbD::Product::simplifyUntil(Symsptr sptr, std::shared_ptr> set) +Symsptr Product::simplifyUntil(Symsptr sptr, std::shared_ptr> set) { auto itr = std::find_if(set->begin(), set->end(), [this](Symsptr sym) {return this == (sym.get()); }); if (itr != set->end()) { @@ -103,7 +103,7 @@ Symsptr MbD::Product::simplifyUntil(Symsptr sptr, std::shared_ptr(); auto sumTERMs = aSum->getTerms(); @@ -120,7 +120,7 @@ Symsptr MbD::Product::timesSum(Symsptr sptr, Symsptr aSum) return answer; } -Symsptr MbD::Product::timesProduct(Symsptr sptr, Symsptr product) +Symsptr Product::timesProduct(Symsptr sptr, Symsptr product) { auto answer = std::make_shared(sptr->getTerms()); auto& answerTerms = answer->terms; @@ -129,14 +129,14 @@ Symsptr MbD::Product::timesProduct(Symsptr sptr, Symsptr product) return answer; } -Symsptr MbD::Product::timesFunction(Symsptr sptr, Symsptr function) +Symsptr Product::timesFunction(Symsptr sptr, Symsptr function) { auto answer = std::make_shared(sptr->getTerms()); answer->terms->push_back(function); return answer; } -std::ostream& MbD::Product::printOn(std::ostream& s) const +std::ostream& Product::printOn(std::ostream& s) const { s << "("; s << *(this->terms->at(0)); @@ -148,7 +148,7 @@ std::ostream& MbD::Product::printOn(std::ostream& s) const return s; } -bool MbD::Product::isProduct() +bool Product::isProduct() { return true; } diff --git a/MbDCode/QuasiIntegrator.cpp b/MbDCode/QuasiIntegrator.cpp index 04c3669..14e5dd4 100644 --- a/MbDCode/QuasiIntegrator.cpp +++ b/MbDCode/QuasiIntegrator.cpp @@ -10,22 +10,23 @@ #include "TooSmallStepSizeError.h" #include "TooManyTriesError.h" #include "SingularMatrixError.h" +#include "DiscontinuityError.h" using namespace MbD; -void MbD::QuasiIntegrator::preRun() +void QuasiIntegrator::preRun() { system->partsJointsMotionsForcesTorquesDo([](std::shared_ptr item) { item->preDyn(); }); } -void MbD::QuasiIntegrator::initialize() +void QuasiIntegrator::initialize() { Solver::initialize(); integrator = CREATE::With(); integrator->setSystem(this); } -void MbD::QuasiIntegrator::run() +void QuasiIntegrator::run() { try { try { @@ -65,17 +66,60 @@ void MbD::QuasiIntegrator::run() } -void MbD::QuasiIntegrator::preFirstStep() +void QuasiIntegrator::preFirstStep() { system->partsJointsMotionsForcesTorquesDo([](std::shared_ptr item) { item->preDynFirstStep(); }); } -void MbD::QuasiIntegrator::preStep() +void QuasiIntegrator::postFirstStep() +{ + system->partsJointsMotionsForcesTorquesDo([](std::shared_ptr item) { item->postDynFirstStep(); }); + if (integrator->istep > 0) { + //"Noise make checking at the start unreliable." + this->checkForDiscontinuity(); + } + this->checkForOutputThrough(integrator->t); +} + +void QuasiIntegrator::preStep() { system->partsJointsMotionsForcesTorquesDo([](std::shared_ptr item) { item->preDynStep(); }); } -double MbD::QuasiIntegrator::suggestSmallerOrAcceptFirstStepSize(double hnew) +void QuasiIntegrator::checkForDiscontinuity() +{ + //"Check for discontinuity in (tpast,t] or [t,tpast) if integrating + //backward." + + auto t = integrator->t; + auto tprevious = integrator->tprevious(); + auto epsilon = std::numeric_limits::epsilon(); + double tstartNew; + if (direction == 0) { + tstartNew = epsilon; + } + else { + epsilon = std::abs(t) * epsilon; + tstartNew = ((direction * t) + epsilon) / direction; + } + system->partsJointsMotionsForcesTorquesDo([&](std::shared_ptr item) { tstartNew = item->checkForDynDiscontinuityBetween(tprevious, tstartNew); }); + if ((direction * tstartNew) > (direction * t)) { + //"No discontinuity in step" + return; + } + else { + this->checkForOutputThrough(tstartNew); + this->interpolateAt(tstartNew); + system->tstartPastsAddFirst(tstart); + system->tstart = tstartNew; + system->toutFirst = tout; + auto discontinuityTypes = std::make_shared>(); + system->partsJointsMotionsForcesTorquesDo([&](std::shared_ptr item) { item->discontinuityAtaddTypeTo(tstartNew, discontinuityTypes); }); + this->throwDiscontinuityError("", discontinuityTypes); + } +} + +double QuasiIntegrator::suggestSmallerOrAcceptFirstStepSize(double hnew) { auto hnew2 = hnew; system->partsJointsMotionsForcesTorquesDo([&](std::shared_ptr item) { hnew2 = item->suggestSmallerOrAcceptDynFirstStepSize(hnew2); }); @@ -94,8 +138,68 @@ double MbD::QuasiIntegrator::suggestSmallerOrAcceptFirstStepSize(double hnew) return hnew2; } -void MbD::QuasiIntegrator::incrementTime(double tnew) +double QuasiIntegrator::suggestSmallerOrAcceptStepSize(double hnew) +{ + auto hnew2 = hnew; + system->partsJointsMotionsForcesTorquesDo([&](std::shared_ptr item) { hnew2 = item->suggestSmallerOrAcceptDynStepSize(hnew2); }); + if (hnew2 > hmax) { + hnew2 = hmax; + this->Solver::logString("StM: Step size is at user specified maximum."); + } + if (hnew2 < hmin) { + std::stringstream ss; + ss << "StM: Step size " << hnew2 << " < " << hmin << " user specified minimum."; + auto str = ss.str(); + system->logString(str); + throw TooSmallStepSizeError(""); + } + return hnew2; +} + +void QuasiIntegrator::incrementTime(double tnew) { system->partsJointsMotionsForcesTorquesDo([](std::shared_ptr item) { item->storeDynState(); }); IntegratorInterface::incrementTime(tnew); } + +void QuasiIntegrator::throwDiscontinuityError(const char* chars, std::shared_ptr> discontinuityTypes) +{ + throw DiscontinuityError(chars, discontinuityTypes); +} + +void QuasiIntegrator::checkForOutputThrough(double t) +{ + //"Kinematic analysis is done at every tout." + if (direction * t <= (direction * (tend + (0.1 * direction * hout)))) { + if (std::abs(tout - t) < 1.0e-12) { + system->output(); + tout += direction * hout; + } + } + else { + integrator->_continue = false; + } +} + +void QuasiIntegrator::interpolateAt(double tArg) +{ + //"Interpolate for system state at tArg and leave system in that state." + system->time(tArg); + this->runInitialConditionTypeSolution(); +} + +void QuasiIntegrator::postStep() +{ + system->partsJointsMotionsForcesTorquesDo([](std::shared_ptr item) { item->postDynStep(); }); + + if (integrator->istep > 0) { + //"Noise make checking at the start unreliable." + this->checkForDiscontinuity(); + } + this->checkForOutputThrough(integrator->t); +} + +void QuasiIntegrator::postRun() +{ + system->partsJointsMotionsForcesTorquesDo([](std::shared_ptr item) { item->postDyn(); }); +} diff --git a/MbDCode/QuasiIntegrator.h b/MbDCode/QuasiIntegrator.h index 560cb40..835651b 100644 --- a/MbDCode/QuasiIntegrator.h +++ b/MbDCode/QuasiIntegrator.h @@ -1,6 +1,8 @@ #pragma once +#include #include "IntegratorInterface.h" +#include "enum.h" namespace MbD { class QuasiIntegrator : public IntegratorInterface @@ -10,10 +12,19 @@ namespace MbD { void preRun() override; void initialize() override; void run() override; - void preFirstStep(); - void preStep(); + void preFirstStep() override; + void postFirstStep() override; + void preStep() override; + void checkForDiscontinuity() override; double suggestSmallerOrAcceptFirstStepSize(double hnew) override; + double suggestSmallerOrAcceptStepSize(double hnew) override; void incrementTime(double tnew) override; + void throwDiscontinuityError(const char* chars, std::shared_ptr> discontinuityTypes); + void checkForOutputThrough(double t) override; + void interpolateAt(double t) override; + void postStep() override; + void postRun() override; + }; } diff --git a/MbDCode/RedundantConstraint.cpp b/MbDCode/RedundantConstraint.cpp index 06836e2..6f5e4c9 100644 --- a/MbDCode/RedundantConstraint.cpp +++ b/MbDCode/RedundantConstraint.cpp @@ -2,91 +2,101 @@ using namespace MbD; -void MbD::RedundantConstraint::removeRedundantConstraints(std::shared_ptr> redundantEqnNos) +void RedundantConstraint::removeRedundantConstraints(std::shared_ptr> redundantEqnNos) { } -bool MbD::RedundantConstraint::isRedundant() +bool RedundantConstraint::isRedundant() { return true; } -std::string MbD::RedundantConstraint::classname() +std::string RedundantConstraint::classname() { auto str = Item::classname() + "->" + constraint->classname(); return str; } -MbD::ConstraintType MbD::RedundantConstraint::type() +ConstraintType RedundantConstraint::type() { - return MbD::redundant; + return redundant; } -void MbD::RedundantConstraint::fillqsulam(FColDsptr col) +void RedundantConstraint::fillqsulam(FColDsptr col) { } -void MbD::RedundantConstraint::postInput() +void RedundantConstraint::postInput() { } -void MbD::RedundantConstraint::prePosIC() +void RedundantConstraint::prePosIC() { } -void MbD::RedundantConstraint::fillEssenConstraints(std::shared_ptr sptr, std::shared_ptr>> essenConstraints) +void RedundantConstraint::fillEssenConstraints(std::shared_ptr sptr, std::shared_ptr>> essenConstraints) { } -void MbD::RedundantConstraint::fillDispConstraints(std::shared_ptr sptr, std::shared_ptr>> dispConstraints) +void RedundantConstraint::fillDispConstraints(std::shared_ptr sptr, std::shared_ptr>> dispConstraints) { } -void MbD::RedundantConstraint::fillPerpenConstraints(std::shared_ptr sptr, std::shared_ptr>> perpenConstraints) +void RedundantConstraint::fillPerpenConstraints(std::shared_ptr sptr, std::shared_ptr>> perpenConstraints) { } -void MbD::RedundantConstraint::fillConstraints(std::shared_ptr sptr, std::shared_ptr>> redunConstraints) +void RedundantConstraint::fillConstraints(std::shared_ptr sptr, std::shared_ptr>> redunConstraints) { } -void MbD::RedundantConstraint::fillRedundantConstraints(std::shared_ptr sptr, std::shared_ptr>> redunConstraints) +void RedundantConstraint::fillRedundantConstraints(std::shared_ptr sptr, std::shared_ptr>> redunConstraints) { redunConstraints->push_back(sptr); } -void MbD::RedundantConstraint::setqsulam(FColDsptr col) +void RedundantConstraint::setqsulam(FColDsptr col) { } -void MbD::RedundantConstraint::setqsudotlam(FColDsptr col) +void RedundantConstraint::setqsudotlam(FColDsptr col) { } -void MbD::RedundantConstraint::fillPosICError(FColDsptr col) +void RedundantConstraint::fillPosICError(FColDsptr col) { } -void MbD::RedundantConstraint::fillPosKineError(FColDsptr col) +void RedundantConstraint::fillPosKineError(FColDsptr col) { } -void MbD::RedundantConstraint::fillPosKineJacob(SpMatDsptr mat) +void RedundantConstraint::fillPosKineJacob(SpMatDsptr mat) { } -void MbD::RedundantConstraint::preVelIC() +void RedundantConstraint::preVelIC() { } -void MbD::RedundantConstraint::preAccIC() +void RedundantConstraint::preAccIC() { } -void MbD::RedundantConstraint::fillAccICIterError(FColDsptr col) +void RedundantConstraint::fillAccICIterError(FColDsptr col) { } -void MbD::RedundantConstraint::setqsuddotlam(FColDsptr qsudotlam) +void RedundantConstraint::setqsuddotlam(FColDsptr col) { } + +void RedundantConstraint::discontinuityAtaddTypeTo(double t, std::shared_ptr> disconTypes) +{ + //"Reactivate all contraints." + assert(false); + //| newSelf | + //newSelf : = self constraint. + //newSelf discontinuityAt : tstartNew addTypeTo : collection. + //self become : newSelf +} diff --git a/MbDCode/RedundantConstraint.h b/MbDCode/RedundantConstraint.h index 8e51e41..6dc2a65 100644 --- a/MbDCode/RedundantConstraint.h +++ b/MbDCode/RedundantConstraint.h @@ -10,7 +10,7 @@ namespace MbD { void removeRedundantConstraints(std::shared_ptr> redundantEqnNos) override; bool isRedundant() override; std::string classname() override; - MbD::ConstraintType type() override; + ConstraintType type() override; void fillqsulam(FColDsptr col) override; void postInput() override; void prePosIC() override; @@ -27,7 +27,8 @@ namespace MbD { void preVelIC() override; void preAccIC() override; void fillAccICIterError(FColDsptr col) override; - void setqsuddotlam(FColDsptr qsudotlam) override; + void setqsuddotlam(FColDsptr col) override; + void discontinuityAtaddTypeTo(double t, std::shared_ptr> disconTypes) override; std::shared_ptr constraint; }; diff --git a/MbDCode/RevoluteJoint.cpp b/MbDCode/RevoluteJoint.cpp index adaf971..97ec2d6 100644 --- a/MbDCode/RevoluteJoint.cpp +++ b/MbDCode/RevoluteJoint.cpp @@ -14,7 +14,7 @@ RevoluteJoint::RevoluteJoint(const char* str) : Joint(str) { } -void MbD::RevoluteJoint::initializeGlobally() +void RevoluteJoint::initializeGlobally() { if (constraints->empty()) { @@ -23,7 +23,7 @@ void MbD::RevoluteJoint::initializeGlobally() addConstraint(CREATE::ConstraintWith(frmI, frmJ, 2)); addConstraint(CREATE::ConstraintWith(frmI, frmJ, 2, 0)); addConstraint(CREATE::ConstraintWith(frmI, frmJ, 2, 1)); - System::getInstance().hasChanged = true; + this->root()->hasChanged = true; } else { Joint::initializeGlobally(); diff --git a/MbDCode/ScalarNewtonRaphson.cpp b/MbDCode/ScalarNewtonRaphson.cpp index b0b5b2b..6007cf4 100644 --- a/MbDCode/ScalarNewtonRaphson.cpp +++ b/MbDCode/ScalarNewtonRaphson.cpp @@ -3,33 +3,33 @@ using namespace MbD; -void MbD::ScalarNewtonRaphson::initializeGlobally() +void ScalarNewtonRaphson::initializeGlobally() { assert(false); //x = system->x; } -void MbD::ScalarNewtonRaphson::calcyNorm() +void ScalarNewtonRaphson::calcyNorm() { yNorm = 0.5 * y * y; } -void MbD::ScalarNewtonRaphson::solveEquations() +void ScalarNewtonRaphson::solveEquations() { dx = -y / pypx; } -void MbD::ScalarNewtonRaphson::updatexold() +void ScalarNewtonRaphson::updatexold() { xold = x; } -void MbD::ScalarNewtonRaphson::calcdxNorm() +void ScalarNewtonRaphson::calcdxNorm() { dxNorm = std::abs(dx); } -void MbD::ScalarNewtonRaphson::xEqualxoldPlusdx() +void ScalarNewtonRaphson::xEqualxoldPlusdx() { x = xold + dx; } diff --git a/MbDCode/SimulationStoppingError.cpp b/MbDCode/SimulationStoppingError.cpp index da1125a..f0a2537 100644 --- a/MbDCode/SimulationStoppingError.cpp +++ b/MbDCode/SimulationStoppingError.cpp @@ -2,6 +2,6 @@ using namespace MbD; -MbD::SimulationStoppingError::SimulationStoppingError(const std::string& msg) : std::runtime_error(msg) +SimulationStoppingError::SimulationStoppingError(const std::string& msg) : std::runtime_error(msg) { } diff --git a/MbDCode/Solver.cpp b/MbDCode/Solver.cpp index d120641..f9415a3 100644 --- a/MbDCode/Solver.cpp +++ b/MbDCode/Solver.cpp @@ -5,50 +5,54 @@ using namespace MbD; -void MbD::Solver::initialize() +void Solver::initialize() { } -void MbD::Solver::initializeLocally() +void Solver::initializeLocally() { } -void MbD::Solver::initializeGlobally() +void Solver::initializeGlobally() { assert(false); } -void MbD::Solver::assignEquationNumbers() +void Solver::assignEquationNumbers() { assert(false); } -void MbD::Solver::run() +void Solver::run() { assert(false); } -void MbD::Solver::preRun() +void Solver::preRun() { assert(false); } -void MbD::Solver::finalize() +void Solver::finalize() +{ +} + +void Solver::reportStats() +{ +} + +void Solver::postRun() { assert(false); } -void MbD::Solver::reportStats() +void Solver::logString(std::string& str) { assert(false); } -void MbD::Solver::postRun() +void Solver::logString(const char* chars) { - assert(false); -} - -void MbD::Solver::logString(std::string& str) -{ - assert(false); + std::string str = chars; + this->logString(str); } diff --git a/MbDCode/Solver.h b/MbDCode/Solver.h index 937e38f..971e075 100644 --- a/MbDCode/Solver.h +++ b/MbDCode/Solver.h @@ -17,6 +17,7 @@ namespace MbD { virtual void reportStats(); virtual void postRun(); virtual void logString(std::string& str); + void logString(const char* chars); virtual void setSystem(Solver* sys) = 0; }; diff --git a/MbDCode/SparseMatrix.h b/MbDCode/SparseMatrix.h index a766587..77ecd7d 100644 --- a/MbDCode/SparseMatrix.h +++ b/MbDCode/SparseMatrix.h @@ -42,11 +42,7 @@ namespace MbD { void atijminusNumber(int i, int j, double value); void atijput(int i, int j, T value); - virtual std::ostream& printOn(std::ostream& s) const; - friend std::ostream& operator<<(std::ostream& s, const SparseMatrix& spMat) - { - return spMat.printOn(s); - } + std::ostream& printOn(std::ostream& s) const override; std::shared_ptr> timesFullColumn(std::shared_ptr> fullCol); }; diff --git a/MbDCode/StableBackwardDifference.cpp b/MbDCode/StableBackwardDifference.cpp index 4a0c504..0fdfd46 100644 --- a/MbDCode/StableBackwardDifference.cpp +++ b/MbDCode/StableBackwardDifference.cpp @@ -2,7 +2,7 @@ using namespace MbD; -void MbD::StableBackwardDifference::formTaylorMatrix() +void StableBackwardDifference::formTaylorMatrix() { //This form is numerically more stable and is prefered over the full Taylor Matrix. //For method order 3: @@ -17,14 +17,14 @@ void MbD::StableBackwardDifference::formTaylorMatrix() } } -void MbD::StableBackwardDifference::instantiateTaylorMatrix() +void StableBackwardDifference::instantiateTaylorMatrix() { if (taylorMatrix == nullptr || (taylorMatrix->nrow() != (order))) { taylorMatrix = std::make_shared>(order, order); } } -void MbD::StableBackwardDifference::formTaylorRowwithTimeNodederivative(int i, int ii, int k) +void StableBackwardDifference::formTaylorRowwithTimeNodederivative(int i, int ii, int k) { //| rowi hi hipower aij | auto& rowi = taylorMatrix->at(i); diff --git a/MbDCode/StateData.cpp b/MbDCode/StateData.cpp new file mode 100644 index 0000000..39544dd --- /dev/null +++ b/MbDCode/StateData.cpp @@ -0,0 +1,11 @@ +#include "StateData.h" + +using namespace MbD; + +StateData::StateData() +{ +} + +void StateData::initialize() +{ +} diff --git a/MbDCode/StateData.h b/MbDCode/StateData.h new file mode 100644 index 0000000..b63f38d --- /dev/null +++ b/MbDCode/StateData.h @@ -0,0 +1,16 @@ +#pragma once + +#include "Item.h" + +namespace MbD { + class StateData : public Item + { + // + public: + StateData(); + void initialize() override; + + + }; +} + diff --git a/MbDCode/Sum.cpp b/MbDCode/Sum.cpp index dc72d46..7933ab3 100644 --- a/MbDCode/Sum.cpp +++ b/MbDCode/Sum.cpp @@ -3,7 +3,7 @@ using namespace MbD; -Symsptr MbD::Sum::timesSum(Symsptr sptr, Symsptr aSum) +Symsptr Sum::timesSum(Symsptr sptr, Symsptr aSum) { auto answer = std::make_shared(); auto sumTERMs = aSum->getTerms(); @@ -22,17 +22,17 @@ Symsptr MbD::Sum::timesSum(Symsptr sptr, Symsptr aSum) return answer; } -Symsptr MbD::Sum::timesProduct(Symsptr sptr, Symsptr product) +Symsptr Sum::timesProduct(Symsptr sptr, Symsptr product) { return product->timesSum(product, sptr); } -Symsptr MbD::Sum::timesFunction(Symsptr sptr, Symsptr function) +Symsptr Sum::timesFunction(Symsptr sptr, Symsptr function) { return function->timesSum(function, sptr); } -Symsptr MbD::Sum::expandUntil(Symsptr sptr, std::shared_ptr> set) +Symsptr Sum::expandUntil(Symsptr sptr, std::shared_ptr> set) { auto itr = std::find_if(set->begin(), set->end(), [sptr](Symsptr sym) {return sptr.get() == sym.get(); }); if (itr != set->end()) { @@ -64,7 +64,7 @@ Symsptr MbD::Sum::expandUntil(Symsptr sptr, std::shared_ptr> set) +Symsptr Sum::simplifyUntil(Symsptr sptr, std::shared_ptr> set) { auto itr = std::find_if(set->begin(), set->end(), [this](Symsptr sym) {return this == (sym.get()); }); if (itr != set->end()) { @@ -100,7 +100,7 @@ Symsptr MbD::Sum::simplifyUntil(Symsptr sptr, std::shared_ptrterms->at(0)); diff --git a/MbDCode/Symbolic.cpp b/MbDCode/Symbolic.cpp index ac6a951..f862d85 100644 --- a/MbDCode/Symbolic.cpp +++ b/MbDCode/Symbolic.cpp @@ -45,13 +45,13 @@ Symsptr Symbolic::simplifyUntil(Symsptr sptr, std::shared_ptr(sptr); return product->timesSum(product, sum); } -Symsptr MbD::Symbolic::timesProduct(Symsptr sptr, Symsptr product) +Symsptr Symbolic::timesProduct(Symsptr sptr, Symsptr product) { auto answer = std::make_shared(product->getTerms()); auto& answerTerms = answer->terms; @@ -59,44 +59,39 @@ Symsptr MbD::Symbolic::timesProduct(Symsptr sptr, Symsptr product) return answer; } -Symsptr MbD::Symbolic::timesFunction(Symsptr sptr, Symsptr function) +Symsptr Symbolic::timesFunction(Symsptr sptr, Symsptr function) { auto answer = std::make_shared(sptr, function); return answer; } -bool MbD::Symbolic::isSum() +bool Symbolic::isSum() { return false; } -bool MbD::Symbolic::isProduct() +bool Symbolic::isProduct() { return false; } -bool MbD::Symbolic::isConstant() +bool Symbolic::isConstant() { return false; } -std::ostream& MbD::Symbolic::printOn(std::ostream& s) const +std::ostream& Symbolic::printOn(std::ostream& s) const { return s << "(" << typeid(*this).name() << ")"; } -std::shared_ptr> MbD::Symbolic::getTerms() +std::shared_ptr> Symbolic::getTerms() { return std::make_shared>(); } -double MbD::Symbolic::getValue() +double Symbolic::getValue() { assert(false); return 0.0; } - -std::ostream& MbD::operator<<(std::ostream& s, const Symbolic& sym) -{ - return sym.printOn(s); -} diff --git a/MbDCode/Symbolic.h b/MbDCode/Symbolic.h index b3b53ab..cda65b3 100644 --- a/MbDCode/Symbolic.h +++ b/MbDCode/Symbolic.h @@ -26,7 +26,10 @@ namespace MbD { virtual std::shared_ptr>> getTerms(); virtual double getValue(); - friend std::ostream& operator<<(std::ostream& s, const Symbolic& sym); + friend std::ostream& operator<<(std::ostream& s, const Symbolic& sym) + { + return sym.printOn(s); + } }; using Symsptr = std::shared_ptr; } diff --git a/MbDCode/System.cpp b/MbDCode/System.cpp index 826acba..b94fb1e 100644 --- a/MbDCode/System.cpp +++ b/MbDCode/System.cpp @@ -7,6 +7,7 @@ #include "SystemSolver.h" #include "Time.h" #include "CREATE.h" +#include "CADSystem.h" using namespace MbD; @@ -18,7 +19,12 @@ System::System(const char* str) : Item(str) { initialize(); } -void MbD::System::initialize() +System* MbD::System::root() +{ + return this; +} + +void System::initialize() { time = CREATE