diff --git a/MbDCode/AbsConstraint.cpp b/MbDCode/AbsConstraint.cpp index 417a0f1..de4f8be 100644 --- a/MbDCode/AbsConstraint.cpp +++ b/MbDCode/AbsConstraint.cpp @@ -12,11 +12,6 @@ AbsConstraint::AbsConstraint(int i) axis = i; } -void AbsConstraint::initialize() -{ - Constraint::initialize(); -} - void AbsConstraint::calcPostDynCorrectorIteration() { if (axis < 3) { diff --git a/MbDCode/AbsConstraint.h b/MbDCode/AbsConstraint.h index 3e1e544..a6febc9 100644 --- a/MbDCode/AbsConstraint.h +++ b/MbDCode/AbsConstraint.h @@ -1,24 +1,24 @@ #pragma once #include "Constraint.h" namespace MbD { - class AbsConstraint : public Constraint - { - //axis iqXminusOnePlusAxis - public: - //AbsConstraint(); - //AbsConstraint(const char* str); - AbsConstraint(int axis); - void initialize() override; - void calcPostDynCorrectorIteration() override; - void useEquationNumbers() override; - void fillPosICJacob(SpMatDsptr mat) override; - void fillPosICError(FColDsptr col) override; - void fillPosKineJacob(SpMatDsptr mat) override; - void fillVelICJacob(SpMatDsptr mat) override; - void fillAccICIterError(FColDsptr col) override; + class AbsConstraint : public Constraint + { + //axis iqXminusOnePlusAxis + public: + //AbsConstraint(); + //AbsConstraint(const char* str); + AbsConstraint(int axis); - int axis = -1; - int iqXminusOnePlusAxis = -1; - }; + void calcPostDynCorrectorIteration() override; + void fillAccICIterError(FColDsptr col) override; + void fillPosICError(FColDsptr col) override; + void fillPosICJacob(SpMatDsptr mat) override; + void fillPosKineJacob(SpMatDsptr mat) override; + void fillVelICJacob(SpMatDsptr mat) override; + void useEquationNumbers() override; + + int axis = -1; + int iqXminusOnePlusAxis = -1; + }; } diff --git a/MbDCode/AccNewtonRaphson.cpp b/MbDCode/AccNewtonRaphson.cpp index 92bb7c3..b7e6bec 100644 --- a/MbDCode/AccNewtonRaphson.cpp +++ b/MbDCode/AccNewtonRaphson.cpp @@ -56,7 +56,7 @@ void AccNewtonRaphson::fillY() y->zeroSelf(); system->partsJointsMotionsForcesTorquesDo([&](std::shared_ptr item) { item->fillAccICIterError(y); - //std::cout << item->getName() << *y << std::endl; + //std::cout << item->name << *y << std::endl; }); std::cout << *y << std::endl; } diff --git a/MbDCode/AngleJoint.cpp b/MbDCode/AngleJoint.cpp new file mode 100644 index 0000000..4d8a7f2 --- /dev/null +++ b/MbDCode/AngleJoint.cpp @@ -0,0 +1,30 @@ +#include "AngleJoint.h" +#include "CREATE.h" +#include "System.h" +#include "DirectionCosineConstraintIJ.h" + +using namespace MbD; + +MbD::AngleJoint::AngleJoint() +{ +} + +MbD::AngleJoint::AngleJoint(const char* str) : Joint(str) +{ +} + +void MbD::AngleJoint::initializeGlobally() +{ + if (constraints->empty()) + { + addConstraint(CREATE::ConstraintWith(frmI, frmJ, 0)); + addConstraint(CREATE::ConstraintWith(frmI, frmJ, 1)); + addConstraint(CREATE::ConstraintWith(frmI, frmJ, 2)); + addConstraint(CREATE::ConstraintWith(frmI, frmJ, 2, 0)); + addConstraint(CREATE::ConstraintWith(frmI, frmJ, 2, 1)); + this->root()->hasChanged = true; + } + else { + Joint::initializeGlobally(); + } +} diff --git a/MbDCode/AngleJoint.h b/MbDCode/AngleJoint.h new file mode 100644 index 0000000..d39184c --- /dev/null +++ b/MbDCode/AngleJoint.h @@ -0,0 +1,17 @@ +#pragma once + +#include "Joint.h" + +namespace MbD { + class AngleJoint : public Joint + { + //theIzJz + public: + AngleJoint(); + AngleJoint(const char* str); + void initializeGlobally() override; + + double theIzJz = 0.0; + }; +} + diff --git a/MbDCode/AngleZIecJec.cpp b/MbDCode/AngleZIecJec.cpp new file mode 100644 index 0000000..c0e9c0b --- /dev/null +++ b/MbDCode/AngleZIecJec.cpp @@ -0,0 +1,87 @@ +#include + +#include "AngleZIecJec.h" +#include "Numeric.h" + +using namespace MbD; + +void MbD::AngleZIecJec::calcPostDynCorrectorIteration() +{ + auto cthez = aA00IeJe->value(); + auto sthez = aA10IeJe->value(); + auto sumOfSquares = cthez * cthez + (sthez * sthez); + auto diffOfSquares = sthez * sthez - (cthez * cthez); + auto sumOfSquaresSquared = sumOfSquares * sumOfSquares; + auto thez0to2pi = Numeric::arcTan0to2piYoverX(sthez, cthez); + thez = std::round(thez - thez0to2pi / (2.0 * M_PI)) * (2.0 * M_PI) + thez0to2pi; + cosOverSSq = cthez / sumOfSquares; + sinOverSSq = sthez / sumOfSquares; + twoCosSinOverSSqSq = 2.0 * cthez * sthez / sumOfSquaresSquared; + dSqOverSSqSq = diffOfSquares / sumOfSquaresSquared; +} + +void MbD::AngleZIecJec::initialize() +{ + KinematicIeJe::initialize(); + this->init_aAijIeJe(); + +} + +void MbD::AngleZIecJec::initializeGlobally() +{ + aA00IeJe->initializeGlobally(); + aA10IeJe->initializeGlobally(); +} + +void MbD::AngleZIecJec::initializeLocally() +{ + aA00IeJe->initializeLocally(); + aA10IeJe->initializeLocally(); +} + +void MbD::AngleZIecJec::postInput() +{ + aA00IeJe->postInput(); + aA10IeJe->postInput(); + KinematicIeJe::postInput(); +} + +void MbD::AngleZIecJec::postPosICIteration() +{ + aA00IeJe->postPosICIteration(); + aA10IeJe->postPosICIteration(); + KinematicIeJe::postPosICIteration(); +} + +void MbD::AngleZIecJec::preAccIC() +{ + aA00IeJe->preAccIC(); + aA10IeJe->preAccIC(); + KinematicIeJe::preAccIC(); +} + +void MbD::AngleZIecJec::prePosIC() +{ + aA00IeJe->prePosIC(); + aA10IeJe->prePosIC(); + KinematicIeJe::prePosIC(); +} + +void MbD::AngleZIecJec::preVelIC() +{ + aA00IeJe->preVelIC(); + aA10IeJe->preVelIC(); + KinematicIeJe::preVelIC(); +} + +void MbD::AngleZIecJec::simUpdateAll() +{ + aA00IeJe->simUpdateAll(); + aA10IeJe->simUpdateAll(); + KinematicIeJe::simUpdateAll(); +} + +double MbD::AngleZIecJec::value() +{ + return thez; +} diff --git a/MbDCode/AngleZIecJec.h b/MbDCode/AngleZIecJec.h new file mode 100644 index 0000000..ef1fb5c --- /dev/null +++ b/MbDCode/AngleZIecJec.h @@ -0,0 +1,28 @@ +#pragma once + +#include "KinematicIeJe.h" +#include "DirectionCosineIeqcJec.h" + +namespace MbD { + class AngleZIecJec : public KinematicIeJe + { + //thez aA00IeJe aA10IeJe cosOverSSq sinOverSSq twoCosSinOverSSqSq dSqOverSSqSq + public: + void calcPostDynCorrectorIteration() override; + virtual void init_aAijIeJe() = 0; + void initialize() override; + void initializeGlobally() override; + void initializeLocally() override; + void postInput() override; + void postPosICIteration() override; + void preAccIC() override; + void prePosIC() override; + void preVelIC() override; + void simUpdateAll() override; + double value() override; + + double thez, cosOverSSq, sinOverSSq, twoCosSinOverSSqSq, dSqOverSSqSq; + std::shared_ptr aA00IeJe, aA10IeJe; + }; +} + diff --git a/MbDCode/AngleZIeqcJec.cpp b/MbDCode/AngleZIeqcJec.cpp new file mode 100644 index 0000000..08b81e1 --- /dev/null +++ b/MbDCode/AngleZIeqcJec.cpp @@ -0,0 +1,63 @@ +#include "AngleZIeqcJec.h" +#include "CREATE.h" + +using namespace MbD; + +void MbD::AngleZIeqcJec::calcPostDynCorrectorIteration() +{ + AngleZIecJec::calcPostDynCorrectorIteration(); + auto pcthezpEI = aA00IeJe->pvaluepEI(); + auto psthezpEI = aA10IeJe->pvaluepEI(); + auto ppcthezpEIpEI = aA00IeJe->ppvaluepEIpEI(); + auto ppsthezpEIpEI = aA10IeJe->ppvaluepEIpEI(); + for (int i = 0; i < 4; i++) + { + pthezpEI->atiput(i, (psthezpEI->at(i)) * cosOverSSq - ((pcthezpEI->at(i)) * sinOverSSq)); + } + for (int i = 0; i < 4; i++) + { + auto ppthezpEIpEIi = ppthezpEIpEI->at(i); + auto ppcthezpEIpEIi = ppcthezpEIpEI->at(i); + auto ppsthezpEIpEIi = ppsthezpEIpEI->at(i); + auto pcthezpEIi = pcthezpEI->at(i); + auto psthezpEIi = psthezpEI->at(i); + auto term1 = (pcthezpEIi * pcthezpEIi - (psthezpEIi * psthezpEIi)) * twoCosSinOverSSqSq; + auto term2 = ppsthezpEIpEIi->at(i) * cosOverSSq - (ppcthezpEIpEIi->at(i) * sinOverSSq); + auto term3 = (psthezpEIi * pcthezpEIi + (pcthezpEIi * psthezpEIi)) * dSqOverSSqSq; + ppthezpEIpEIi->atiput(i, term1 + term2 + term3); + for (int j = i + 1; j < 4; j++) + { + auto pcthezpEIj = pcthezpEI->at(j); + auto psthezpEIj = psthezpEI->at(j); + auto term1 = (pcthezpEIi * pcthezpEIj - (psthezpEIi * psthezpEIj)) * twoCosSinOverSSqSq; + auto term2 = ppsthezpEIpEIi->at(j) * cosOverSSq - (ppcthezpEIpEIi->at(j) * sinOverSSq); + auto term3 = (psthezpEIi * pcthezpEIj + (pcthezpEIi * psthezpEIj)) * dSqOverSSqSq; + auto ppthezpEIpEIij = term1 + term2 + term3; + ppthezpEIpEIi->atiput(j, ppthezpEIpEIij); + ppthezpEIpEI->atijput(j, i, ppthezpEIpEIij); + } + } +} + +void MbD::AngleZIeqcJec::init_aAijIeJe() +{ + aA00IeJe = CREATE::With(frmI, frmJ, 0, 0); + aA10IeJe = CREATE::With(frmI, frmJ, 1, 0); +} + +void MbD::AngleZIeqcJec::initialize() +{ + AngleZIecJec::initialize(); + pthezpEI = std::make_shared>(4); + ppthezpEIpEI = std::make_shared>(4, 4); +} + +FMatDsptr MbD::AngleZIeqcJec::ppvaluepEIpEI() +{ + return ppthezpEIpEI; +} + +FRowDsptr MbD::AngleZIeqcJec::pvaluepEI() +{ + return pthezpEI; +} diff --git a/MbDCode/AngleZIeqcJec.h b/MbDCode/AngleZIeqcJec.h new file mode 100644 index 0000000..c84c651 --- /dev/null +++ b/MbDCode/AngleZIeqcJec.h @@ -0,0 +1,20 @@ +#pragma once + +#include "AngleZIecJec.h" + +namespace MbD { + class AngleZIeqcJec : public AngleZIecJec + { + //pthezpEI ppthezpEIpEI pcthezpEI psthezpEI + public: + void calcPostDynCorrectorIteration() override; + void init_aAijIeJe() override; + void initialize() override; + FMatDsptr ppvaluepEIpEI() override; + FRowDsptr pvaluepEI() override; + + FRowDsptr pthezpEI, pcthezpEI, psthezpEI; + FMatDsptr ppthezpEIpEI; + }; +} + diff --git a/MbDCode/AngleZIeqcJeqc.cpp b/MbDCode/AngleZIeqcJeqc.cpp new file mode 100644 index 0000000..65246e4 --- /dev/null +++ b/MbDCode/AngleZIeqcJeqc.cpp @@ -0,0 +1,89 @@ +#include "AngleZIeqcJeqc.h" +#include "CREATE.h" +#include "DirectionCosineIeqcJeqc.h" + +using namespace MbD; + +void MbD::AngleZIeqcJeqc::calcPostDynCorrectorIteration() +{ + AngleZIeqcJec::calcPostDynCorrectorIteration(); + auto pcthezpEJ = aA00IeJe->pvaluepEJ(); + auto psthezpEJ = aA10IeJe->pvaluepEJ(); + auto ppcthezpEIpEJ = aA00IeJe->ppvaluepEIpEJ(); + auto ppsthezpEIpEJ = aA10IeJe->ppvaluepEIpEJ(); + auto ppcthezpEJpEJ = aA00IeJe->ppvaluepEJpEJ(); + auto ppsthezpEJpEJ = aA10IeJe->ppvaluepEJpEJ(); + for (int i = 0; i < 4; i++) + { + pthezpEJ->atiput(i, (psthezpEJ->at(i)) * cosOverSSq - ((pcthezpEJ->at(i)) * sinOverSSq)); + } + for (int i = 0; i < 4; i++) + { + auto ppthezpEIpEJi = ppthezpEIpEJ->at(i); + auto ppcthezpEIpEJi = ppcthezpEIpEJ->at(i); + auto ppsthezpEIpEJi = ppsthezpEIpEJ->at(i); + auto pcthezpEIi = pcthezpEI->at(i); + auto psthezpEIi = psthezpEI->at(i); + for (int j = 0; j < 4; j++) + { + auto pcthezpEJj = pcthezpEJ->at(j); + auto psthezpEJj = psthezpEJ->at(j); + auto term1 = (pcthezpEIi * pcthezpEJj - (psthezpEIi * psthezpEJj)) * twoCosSinOverSSqSq; + auto term2 = ppsthezpEIpEJi->at(j) * cosOverSSq - (ppcthezpEIpEJi->at(j) * sinOverSSq); + auto term3 = (psthezpEIi * pcthezpEJj + (pcthezpEIi * psthezpEJj)) * dSqOverSSqSq; + ppthezpEIpEJi->atiput(j, term1 + term2 + term3); + } + } + for (int i = 0; i < 4; i++) + { + auto ppthezpEJpEJi = ppthezpEJpEJ->at(i); + auto ppcthezpEJpEJi = ppcthezpEJpEJ->at(i); + auto ppsthezpEJpEJi = ppsthezpEJpEJ->at(i); + auto pcthezpEJi = pcthezpEJ->at(i); + auto psthezpEJi = psthezpEJ->at(i); + auto term1 = (pcthezpEJi * pcthezpEJi - (psthezpEJi * psthezpEJi)) * twoCosSinOverSSqSq; + auto term2 = ppsthezpEJpEJi->at(i) * cosOverSSq - (ppcthezpEJpEJi->at(i) * sinOverSSq); + auto term3 = (psthezpEJi * pcthezpEJi + (pcthezpEJi * psthezpEJi)) * dSqOverSSqSq; + ppthezpEJpEJi->atiput(i, term1 + term2 + term3); + for (int j = i + 1; j < 4; j++) + { + auto pcthezpEJj = pcthezpEJ->at(j); + auto psthezpEJj = psthezpEJ->at(j); + auto term1 = (pcthezpEJi * pcthezpEJj - (psthezpEJi * psthezpEJj)) * twoCosSinOverSSqSq; + auto term2 = ppsthezpEJpEJi->at(j) * cosOverSSq - (ppcthezpEJpEJi->at(j) * sinOverSSq); + auto term3 = (psthezpEJi * pcthezpEJj + (pcthezpEJi * psthezpEJj)) * dSqOverSSqSq; + auto ppthezpEJpEJij = term1 + term2 + term3; + ppthezpEJpEJi->atiput(j, ppthezpEJpEJij); + ppthezpEJpEJ->atijput(j, i, ppthezpEJpEJij); + } + } +} + +void MbD::AngleZIeqcJeqc::init_aAijIeJe() +{ + aA00IeJe = CREATE::With(frmI, frmJ, 0, 0); + aA10IeJe = CREATE::With(frmI, frmJ, 1, 0); +} + +void MbD::AngleZIeqcJeqc::initialize() +{ + AngleZIeqcJec::initialize(); + pthezpEJ = std::make_shared>(4); + ppthezpEIpEJ = std::make_shared>(4, 4); + ppthezpEJpEJ = std::make_shared>(4, 4); +} + +FMatDsptr MbD::AngleZIeqcJeqc::ppvaluepEIpEJ() +{ + return ppthezpEIpEJ; +} + +FMatDsptr MbD::AngleZIeqcJeqc::ppvaluepEJpEJ() +{ + return ppthezpEJpEJ; +} + +FRowDsptr MbD::AngleZIeqcJeqc::pvaluepEJ() +{ + return pthezpEJ; +} diff --git a/MbDCode/AngleZIeqcJeqc.h b/MbDCode/AngleZIeqcJeqc.h new file mode 100644 index 0000000..4f327b3 --- /dev/null +++ b/MbDCode/AngleZIeqcJeqc.h @@ -0,0 +1,21 @@ +#pragma once + +#include "AngleZIeqcJec.h" + +namespace MbD { + class AngleZIeqcJeqc : public AngleZIeqcJec + { + //pthezpEJ ppthezpEIpEJ ppthezpEJpEJ + public: + void calcPostDynCorrectorIteration() override; + void init_aAijIeJe() override; + void initialize() override; + FMatDsptr ppvaluepEIpEJ() override; + FMatDsptr ppvaluepEJpEJ() override; + FRowDsptr pvaluepEJ() override; + + FRowDsptr pthezpEJ; + FMatDsptr ppthezpEIpEJ, ppthezpEJpEJ; + }; +} + diff --git a/MbDCode/AnyPosICNewtonRaphson.h b/MbDCode/AnyPosICNewtonRaphson.h index 547b2af..46f8b84 100644 --- a/MbDCode/AnyPosICNewtonRaphson.h +++ b/MbDCode/AnyPosICNewtonRaphson.h @@ -15,6 +15,7 @@ namespace MbD { void fillY() override; void fillPyPx() override; void passRootToSystem() override; + void assignEquationNumbers() = 0; int nqsu = -1; std::shared_ptr> qsuOld; diff --git a/MbDCode/AtPointConstraintIJ.h b/MbDCode/AtPointConstraintIJ.h index c183e59..aa87708 100644 --- a/MbDCode/AtPointConstraintIJ.h +++ b/MbDCode/AtPointConstraintIJ.h @@ -10,17 +10,18 @@ namespace MbD { //axis riIeJeO public: AtPointConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj, int axisi); + + void calcPostDynCorrectorIteration() override; void initialize() override; - void initializeLocally() override; void initializeGlobally() override; + void initializeLocally() override; virtual void initriIeJeO(); void postInput() override; - void calcPostDynCorrectorIteration() override; - void prePosIC() override; - ConstraintType type() override; void postPosICIteration() override; - void preVelIC() override; void preAccIC() override; + void prePosIC() override; + void preVelIC() override; + ConstraintType type() override; int axis; diff --git a/MbDCode/AtPointConstraintIqcJc.cpp b/MbDCode/AtPointConstraintIqcJc.cpp index bb51fec..8599fdc 100644 --- a/MbDCode/AtPointConstraintIqcJc.cpp +++ b/MbDCode/AtPointConstraintIqcJc.cpp @@ -29,8 +29,9 @@ void AtPointConstraintIqcJc::calcPostDynCorrectorIteration() void AtPointConstraintIqcJc::useEquationNumbers() { - iqXIminusOnePlusAxis = std::static_pointer_cast(frmI)->iqX() + axis; - iqEI = std::static_pointer_cast(frmI)->iqE(); + auto frmIeqc = std::static_pointer_cast(frmI); + iqXIminusOnePlusAxis = frmIeqc->iqX() + axis; + iqEI = frmIeqc->iqE(); } void AtPointConstraintIqcJc::fillPosICError(FColDsptr col) diff --git a/MbDCode/AtPointConstraintIqcJc.h b/MbDCode/AtPointConstraintIqcJc.h index 3a471ad..8a92732 100644 --- a/MbDCode/AtPointConstraintIqcJc.h +++ b/MbDCode/AtPointConstraintIqcJc.h @@ -8,17 +8,18 @@ namespace MbD { //pGpEI ppGpEIpEI iqXIminusOnePlusAxis iqEI public: AtPointConstraintIqcJc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi); - void initializeGlobally() override; - void initriIeJeO() override; + + void addToJointForceI(FColDsptr col) override; + void addToJointTorqueI(FColDsptr col) override; void calcPostDynCorrectorIteration() override; - void useEquationNumbers() override; + void fillAccICIterError(FColDsptr col) override; void fillPosICError(FColDsptr col) override; void fillPosICJacob(SpMatDsptr mat) override; void fillPosKineJacob(SpMatDsptr mat) override; void fillVelICJacob(SpMatDsptr mat) override; - void fillAccICIterError(FColDsptr col) override; - void addToJointForceI(FColDsptr col) override; - void addToJointTorqueI(FColDsptr col) override; + void initializeGlobally() override; + void initriIeJeO() override; + void useEquationNumbers() override; FRowDsptr pGpEI; FMatDsptr ppGpEIpEI; diff --git a/MbDCode/AtPointConstraintIqcJqc.cpp b/MbDCode/AtPointConstraintIqcJqc.cpp index 7ec9a5b..ff0ab74 100644 --- a/MbDCode/AtPointConstraintIqcJqc.cpp +++ b/MbDCode/AtPointConstraintIqcJqc.cpp @@ -30,8 +30,9 @@ void AtPointConstraintIqcJqc::calcPostDynCorrectorIteration() void AtPointConstraintIqcJqc::useEquationNumbers() { AtPointConstraintIqcJc::useEquationNumbers(); - iqXJminusOnePlusAxis = std::static_pointer_cast(frmJ)->iqX() + axis; - iqEJ = std::static_pointer_cast(frmJ)->iqE(); + auto frmJeqc = std::static_pointer_cast(frmJ); + iqXJminusOnePlusAxis = frmJeqc->iqX() + axis; + iqEJ = frmJeqc->iqE(); } void AtPointConstraintIqcJqc::fillPosICError(FColDsptr col) diff --git a/MbDCode/AtPointConstraintIqcJqc.h b/MbDCode/AtPointConstraintIqcJqc.h index 103de33..6c8c01b 100644 --- a/MbDCode/AtPointConstraintIqcJqc.h +++ b/MbDCode/AtPointConstraintIqcJqc.h @@ -8,15 +8,16 @@ namespace MbD { //pGpEJ ppGpEJpEJ iqXJminusOnePlusAxis iqEJ public: AtPointConstraintIqcJqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi); + + void calcPostDynCorrectorIteration() override; void initializeGlobally() override; void initriIeJeO() override; - void calcPostDynCorrectorIteration() override; - void useEquationNumbers() override; + void fillAccICIterError(FColDsptr col) override; void fillPosICError(FColDsptr col) override; void fillPosICJacob(SpMatDsptr mat) override; void fillPosKineJacob(SpMatDsptr mat) override; void fillVelICJacob(SpMatDsptr mat) override; - void fillAccICIterError(FColDsptr col) override; + void useEquationNumbers() override; FRowDsptr pGpEJ; FMatDsptr ppGpEJpEJ; diff --git a/MbDCode/AtPointConstraintIqctJqc.cpp b/MbDCode/AtPointConstraintIqctJqc.cpp index 94b7127..41d21ca 100644 --- a/MbDCode/AtPointConstraintIqctJqc.cpp +++ b/MbDCode/AtPointConstraintIqctJqc.cpp @@ -35,7 +35,7 @@ ConstraintType AtPointConstraintIqctJqc::type() void AtPointConstraintIqctJqc::preVelIC() { - AtPointConstraintIJ::preVelIC(); + AtPointConstraintIqcJqc::preVelIC(); pGpt = std::static_pointer_cast(riIeJeO)->priIeJeOpt; } @@ -56,7 +56,7 @@ void AtPointConstraintIqctJqc::fillAccICIterError(FColDsptr col) void AtPointConstraintIqctJqc::preAccIC() { - AtPointConstraintIJ::preAccIC(); + AtPointConstraintIqcJqc::preAccIC(); ppGpEIpt = std::static_pointer_cast(riIeJeO)->ppriIeJeOpEIpt; ppGptpt = std::static_pointer_cast(riIeJeO)->ppriIeJeOptpt; } diff --git a/MbDCode/AtPointConstraintIqctJqc.h b/MbDCode/AtPointConstraintIqctJqc.h index ff4bf08..29c0bce 100644 --- a/MbDCode/AtPointConstraintIqctJqc.h +++ b/MbDCode/AtPointConstraintIqctJqc.h @@ -8,19 +8,19 @@ namespace MbD { //pGpt ppGpEIpt ppGptpt public: AtPointConstraintIqctJqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi); + + void calcPostDynCorrectorIteration() override; + void fillAccICIterError(FColDsptr col) override; + void fillVelICError(FColDsptr col) override; void initializeGlobally() override; void initriIeJeO() override; - void calcPostDynCorrectorIteration() override; - ConstraintType type() override; + void preAccIC() override; void preVelIC() override; - void fillVelICError(FColDsptr col) override; - void fillAccICIterError(FColDsptr col) override; + ConstraintType type() override; double pGpt; FRowDsptr ppGpEIpt; double ppGptpt; - void preAccIC() override; - }; } diff --git a/MbDCode/CADSystem.cpp b/MbDCode/CADSystem.cpp index 6f89500..3216561 100644 --- a/MbDCode/CADSystem.cpp +++ b/MbDCode/CADSystem.cpp @@ -26,7 +26,7 @@ void CADSystem::outputFor(AnalysisType type) this->logString(str); mbdSystem->partsJointsMotionsForcesTorquesDo([](std::shared_ptr item) { std::cout << std::endl; - std::cout << item->classname() << " " << item->getName() << std::endl; + std::cout << item->classname() << " " << item->name << std::endl; auto data = item->stateData(); std::cout << *data << std::endl; }); @@ -49,8 +49,8 @@ void CADSystem::runOndselPiston() auto& TheSystem = mbdSystem; TheSystem->clear(); std::string name = "TheSystem"; - TheSystem->setName(name); - std::cout << "TheSystem->getName() " << TheSystem->getName() << std::endl; + TheSystem->name = name; + std::cout << "TheSystem->name " << TheSystem->name << std::endl; auto& systemSolver = TheSystem->systemSolver; systemSolver->errorTolPosKine = 1.0e-6; systemSolver->errorTolAccKine = 1.0e-6; @@ -77,7 +77,7 @@ void CADSystem::runOndselPiston() FRowDsptr fullRow; // auto assembly1 = CREATE::With("/Assembly1"); - std::cout << "assembly1->getName() " << assembly1->getName() << std::endl; + std::cout << "assembly1->name " << assembly1->name << std::endl; assembly1->m = 0.0; assembly1->aJ = std::make_shared>(ListD{ 0, 0, 0 }); qX = std::make_shared>(ListD{ 0, 0, 0 }); @@ -122,7 +122,7 @@ void CADSystem::runOndselPiston() assembly1->asFixed(); // auto crankPart1 = CREATE::With("/Assembly1/Part1"); - std::cout << "crankPart1->getName() " << crankPart1->getName() << std::endl; + std::cout << "crankPart1->name " << crankPart1->name << 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 }); @@ -164,7 +164,7 @@ void CADSystem::runOndselPiston() } // auto conrodPart2 = CREATE::With("/Assembly1/Part2"); - std::cout << "conrodPart2->getName() " << conrodPart2->getName() << std::endl; + std::cout << "conrodPart2->name " << conrodPart2->name << 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 }); @@ -206,7 +206,7 @@ void CADSystem::runOndselPiston() } // auto pistonPart3 = CREATE::With("/Assembly1/Part3"); - std::cout << "pistonPart3->getName() " << pistonPart3->getName() << std::endl; + std::cout << "pistonPart3->name " << pistonPart3->name << 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 }); @@ -248,28 +248,28 @@ void CADSystem::runOndselPiston() } // auto revJoint1 = CREATE::With("/Assembly1/Joint1"); - std::cout << "revJoint1->getName() " << revJoint1->getName() << std::endl; + std::cout << "revJoint1->name " << revJoint1->name << 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; + std::cout << "revJoint2->name " << revJoint2->name << 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; + std::cout << "revJoint3->name " << revJoint3->name << 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; + std::cout << "cylJoint4->name " << cylJoint4->name << 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; + std::cout << "rotMotion1->name " << rotMotion1->name << std::endl; auto omega = std::make_shared(6.2831853071796); auto timeScale = std::make_shared(1.0); auto time = std::make_shared(timeScale, TheSystem->time); @@ -286,8 +286,8 @@ void CADSystem::runPiston() auto& TheSystem = mbdSystem; TheSystem->clear(); std::string name = "TheSystem"; - TheSystem->setName(name); - std::cout << "TheSystem->getName() " << TheSystem->getName() << std::endl; + TheSystem->name = name; + std::cout << "TheSystem->name " << TheSystem->name << std::endl; auto& systemSolver = TheSystem->systemSolver; systemSolver->errorTolPosKine = 1.0e-6; systemSolver->errorTolAccKine = 1.0e-6; @@ -314,7 +314,7 @@ void CADSystem::runPiston() FRowDsptr fullRow; // auto assembly1 = CREATE::With("/Assembly1"); - std::cout << "assembly1->getName() " << assembly1->getName() << std::endl; + std::cout << "assembly1->name " << assembly1->name << std::endl; assembly1->m = 0.0; assembly1->aJ = std::make_shared>(ListD{ 0, 0, 0 }); qX = std::make_shared>(ListD{ 0, 0, 0 }); @@ -359,7 +359,7 @@ void CADSystem::runPiston() assembly1->asFixed(); // auto crankPart1 = CREATE::With("/Assembly1/Part1"); - std::cout << "crankPart1->getName() " << crankPart1->getName() << std::endl; + std::cout << "crankPart1->name " << crankPart1->name << std::endl; crankPart1->m = 0.045210530089461; crankPart1->aJ = std::make_shared>(ListD{ 1.7381980042084e-4, 0.003511159968501, 0.0036154518487535 }); qX = std::make_shared>(ListD{ 0.38423368514246, 2.6661567755108e-17, -0.048029210642807 }); @@ -401,7 +401,7 @@ void CADSystem::runPiston() } // auto conrodPart2 = CREATE::With("/Assembly1/Part2"); - std::cout << "conrodPart2->getName() " << conrodPart2->getName() << std::endl; + std::cout << "conrodPart2->name " << conrodPart2->name << std::endl; conrodPart2->m = 0.067815795134192; conrodPart2->aJ = std::make_shared>(ListD{ 2.6072970063126e-4, 0.011784982468533, 0.011941420288912 }); qX = std::make_shared>(ListD{ 0.38423368514246, 0.49215295678475, 0.048029210642807 }); @@ -443,7 +443,7 @@ void CADSystem::runPiston() } // auto pistonPart3 = CREATE::With("/Assembly1/Part3"); - std::cout << "pistonPart3->getName() " << pistonPart3->getName() << std::endl; + std::cout << "pistonPart3->name " << pistonPart3->name << 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 }); @@ -485,28 +485,28 @@ void CADSystem::runPiston() } // auto revJoint1 = CREATE::With("/Assembly1/Joint1"); - std::cout << "revJoint1->getName() " << revJoint1->getName() << std::endl; + std::cout << "revJoint1->name " << revJoint1->name << 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; + std::cout << "revJoint2->name " << revJoint2->name << 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; + std::cout << "revJoint3->name " << revJoint3->name << 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; + std::cout << "cylJoint4->name " << cylJoint4->name << 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; + std::cout << "rotMotion1->name " << rotMotion1->name << std::endl; auto omega = std::make_shared(6.2831853071796); auto timeScale = std::make_shared(0.04); auto time = std::make_shared(timeScale, TheSystem->time); diff --git a/MbDCode/CREATE.h b/MbDCode/CREATE.h index daae3c3..6ed5f3e 100644 --- a/MbDCode/CREATE.h +++ b/MbDCode/CREATE.h @@ -39,6 +39,11 @@ namespace MbD { inst->initialize(); return inst; } + static std::shared_ptr With(EndFrmcptr frmi, EndFrmcptr frmj) { + auto inst = std::make_shared(frmi, frmj); + inst->initialize(); + return inst; + } static std::shared_ptr With(EndFrmcptr frmi, EndFrmcptr frmj, int axis) { auto inst = std::make_shared(frmi, frmj, axis); inst->initialize(); diff --git a/MbDCode/ConstVelConstraintIJ.cpp b/MbDCode/ConstVelConstraintIJ.cpp new file mode 100644 index 0000000..717cad4 --- /dev/null +++ b/MbDCode/ConstVelConstraintIJ.cpp @@ -0,0 +1,78 @@ +#include "ConstVelConstraintIJ.h" +#include "DirectionCosineIecJec.h" + +using namespace MbD; + +ConstVelConstraintIJ::ConstVelConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj) : ConstraintIJ(frmi, frmj) +{ +} + +void ConstVelConstraintIJ::calcPostDynCorrectorIteration() +{ + aG = aA01IeJe->aAijIeJe + aA10IeJe->aAijIeJe - aConstant; +} + +void ConstVelConstraintIJ::initialize() +{ + this->initA01IeJe(); + this->initA10IeJe(); +} + +void ConstVelConstraintIJ::initializeGlobally() +{ + aA01IeJe->initializeGlobally(); + aA10IeJe->initializeGlobally(); +} + +void ConstVelConstraintIJ::initializeLocally() +{ + aA01IeJe->initializeLocally(); + aA10IeJe->initializeLocally(); +} + +void ConstVelConstraintIJ::postInput() +{ + aA01IeJe->postInput(); + aA10IeJe->postInput(); + Constraint::postInput(); +} + +ConstraintType ConstVelConstraintIJ::type() +{ + return ConstraintType(); +} + +void ConstVelConstraintIJ::postPosICIteration() +{ + aA01IeJe->postPosICIteration(); + aA10IeJe->postPosICIteration(); + Item::postPosICIteration(); +} + +void ConstVelConstraintIJ::preAccIC() +{ + aA01IeJe->preAccIC(); + aA10IeJe->preAccIC(); + Constraint::preAccIC(); +} + +void ConstVelConstraintIJ::prePosIC() +{ + aA01IeJe->prePosIC(); + aA10IeJe->prePosIC(); + Constraint::prePosIC(); +} + +void ConstVelConstraintIJ::preVelIC() +{ + aA01IeJe->preVelIC(); + aA10IeJe->preVelIC(); + Item::preVelIC(); +} + +void ConstVelConstraintIJ::simUpdateAll() +{ + aA01IeJe->simUpdateAll(); + aA10IeJe->simUpdateAll(); + Item::simUpdateAll(); +} diff --git a/MbDCode/ConstVelConstraintIJ.h b/MbDCode/ConstVelConstraintIJ.h new file mode 100644 index 0000000..3251e10 --- /dev/null +++ b/MbDCode/ConstVelConstraintIJ.h @@ -0,0 +1,31 @@ +#pragma once + +#include "ConstraintIJ.h" + +namespace MbD { + class DirectionCosineIecJec; + + class ConstVelConstraintIJ : public ConstraintIJ + { + //aA01IeJe aA10IeJe + public: + ConstVelConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj); + + void calcPostDynCorrectorIteration() override; + virtual void initA01IeJe() = 0; + virtual void initA10IeJe() = 0; + void initialize() override; + void initializeGlobally() override; + void initializeLocally() override; + void postInput() override; + void postPosICIteration() override; + void preAccIC() override; + void prePosIC() override; + void preVelIC() override; + void simUpdateAll() override; + ConstraintType type() override; + + std::shared_ptr aA01IeJe, aA10IeJe; + }; +} + diff --git a/MbDCode/ConstVelConstraintIqcJc.cpp b/MbDCode/ConstVelConstraintIqcJc.cpp new file mode 100644 index 0000000..8841c19 --- /dev/null +++ b/MbDCode/ConstVelConstraintIqcJc.cpp @@ -0,0 +1,98 @@ +#include + +#include "ConstVelConstraintIqcJc.h" +#include "DirectionCosineIeqcJec.h" +#include "DirectionCosineIeqcJeqc.h" +#include "EndFrameqc.h" +#include "CREATE.h" + +using namespace MbD; + +MbD::ConstVelConstraintIqcJc::ConstVelConstraintIqcJc(EndFrmcptr frmi, EndFrmcptr frmj) : ConstVelConstraintIJ(frmi, frmj) +{ +} + +void MbD::ConstVelConstraintIqcJc::calcPostDynCorrectorIteration() +{ + ConstVelConstraintIJ::calcPostDynCorrectorIteration(); + auto aA01IeqcJec = std::dynamic_pointer_cast(aA01IeJe); + auto& pA01IeJepEI = aA01IeqcJec->pAijIeJepEI; + auto& ppA01IeJepEIpEI = aA01IeqcJec->ppAijIeJepEIpEI; + auto aA10IeqcJec = std::dynamic_pointer_cast(aA10IeJe); + auto& pA10IeJepEI = aA10IeqcJec->pAijIeJepEI; + auto& ppA10IeJepEIpEI = aA10IeqcJec->ppAijIeJepEIpEI; + for (int i = 0; i < 4; i++) + { + pGpEI->atiput(i, pA01IeJepEI->at(i) + pA10IeJepEI->at(i)); + } + for (int i = 0; i < 4; i++) + { + auto& ppGpEIpEIi = ppGpEIpEI->at(i); + auto& ppA01IeJepEIpEIi = ppA01IeJepEIpEI->at(i); + auto& ppA10IeJepEIpEIi = ppA10IeJepEIpEI->at(i); + ppGpEIpEIi->atiput(i, ppA01IeJepEIpEIi->at(i) + ppA10IeJepEIpEIi->at(i)); + for (int j = i + 1; j < 4; j++) + { + auto ppGpEIpEIij = ppA01IeJepEIpEIi->at(j) + ppA10IeJepEIpEIi->at(j); + ppGpEIpEIi->atiput(j, ppGpEIpEIij); + ppGpEIpEI->atijput(j, i, ppGpEIpEIij); + } + } +} + +void MbD::ConstVelConstraintIqcJc::fillAccICIterError(FColDsptr col) +{ + col->atiplusFullVectortimes(iqEI, pGpEI, lam); + auto efrmIqc = std::static_pointer_cast(frmI); + auto qEdotI = efrmIqc->qEdot(); + auto sum = 0.0; + sum += pGpEI->timesFullColumn(efrmIqc->qEddot()); + sum += qEdotI->transposeTimesFullColumn(ppGpEIpEI->timesFullColumn(qEdotI)); + col->atiplusNumber(iG, sum); +} + +void MbD::ConstVelConstraintIqcJc::fillPosICError(FColDsptr col) +{ + Constraint::fillPosICError(col); + col->atiplusFullVectortimes(iqEI, pGpEI, lam); +} + +void MbD::ConstVelConstraintIqcJc::fillPosICJacob(SpMatDsptr mat) +{ + mat->atijplusFullRow(iG, iqEI, pGpEI); + mat->atijplusFullColumn(iqEI, iG, pGpEI->transpose()); + mat->atijplusFullMatrixtimes(iqEI, iqEI, ppGpEIpEI, lam); +} + +void MbD::ConstVelConstraintIqcJc::fillPosKineJacob(SpMatDsptr mat) +{ + mat->atijplusFullRow(iG, iqEI, pGpEI); +} + +void MbD::ConstVelConstraintIqcJc::fillVelICJacob(SpMatDsptr mat) +{ + mat->atijplusFullRow(iG, iqEI, pGpEI); + mat->atijplusFullColumn(iqEI, iG, pGpEI->transpose()); +} + +void MbD::ConstVelConstraintIqcJc::initA01IeJe() +{ + aA01IeJe = CREATE::With(frmI, frmJ, 0, 1); +} + +void MbD::ConstVelConstraintIqcJc::initA10IeJe() +{ + aA10IeJe = CREATE::With(frmI, frmJ, 1, 0); +} + +void MbD::ConstVelConstraintIqcJc::initialize() +{ + ConstVelConstraintIJ::initialize(); + pGpEI = std::make_shared>(4); + ppGpEIpEI = std::make_shared>(4, 4); +} + +void MbD::ConstVelConstraintIqcJc::useEquationNumbers() +{ + iqEI = std::static_pointer_cast(frmI)->iqE(); +} diff --git a/MbDCode/ConstVelConstraintIqcJc.h b/MbDCode/ConstVelConstraintIqcJc.h new file mode 100644 index 0000000..a9c0b19 --- /dev/null +++ b/MbDCode/ConstVelConstraintIqcJc.h @@ -0,0 +1,27 @@ +#pragma once + +#include "ConstVelConstraintIJ.h" + +namespace MbD { + class ConstVelConstraintIqcJc : public ConstVelConstraintIJ + { + //pGpEI ppGpEIpEI iqEI + public: + ConstVelConstraintIqcJc(EndFrmcptr frmi, EndFrmcptr frmj); + + void calcPostDynCorrectorIteration() override; + void fillAccICIterError(FColDsptr col) override; + void fillPosICError(FColDsptr col) override; + void fillPosICJacob(SpMatDsptr mat) override; + void fillPosKineJacob(SpMatDsptr mat) override; + void fillVelICJacob(SpMatDsptr mat) override; + void initA01IeJe() override; + void initA10IeJe() override; + void initialize() override; + void useEquationNumbers() override; + + FRowDsptr pGpEI; + FMatDsptr ppGpEIpEI; + int iqEI = -1; + }; +} diff --git a/MbDCode/ConstVelConstraintIqcJqc.cpp b/MbDCode/ConstVelConstraintIqcJqc.cpp new file mode 100644 index 0000000..0d5fd40 --- /dev/null +++ b/MbDCode/ConstVelConstraintIqcJqc.cpp @@ -0,0 +1,121 @@ +#include "ConstVelConstraintIqcJqc.h" +#include "DirectionCosineIeqcJeqc.h" +#include "EndFrameqc.h" +#include "CREATE.h" + +using namespace MbD; + +MbD::ConstVelConstraintIqcJqc::ConstVelConstraintIqcJqc(EndFrmcptr frmi, EndFrmcptr frmj) : ConstVelConstraintIqcJc(frmi, frmj) +{ +} + +void MbD::ConstVelConstraintIqcJqc::calcPostDynCorrectorIteration() +{ + ConstVelConstraintIqcJc::calcPostDynCorrectorIteration(); + auto aA01IeqcJeqc = std::dynamic_pointer_cast(aA01IeJe); + auto& pA01IeJepEJ = aA01IeqcJeqc->pAijIeJepEJ; + auto& ppA01IeJepEIpEJ = aA01IeqcJeqc->ppAijIeJepEIpEJ; + auto& ppA01IeJepEJpEJ = aA01IeqcJeqc->ppAijIeJepEJpEJ; + auto aA10IeqcJeqc = std::dynamic_pointer_cast(aA10IeJe); + auto& pA10IeJepEJ = aA10IeqcJeqc->pAijIeJepEJ; + auto& ppA10IeJepEIpEJ = aA10IeqcJeqc->ppAijIeJepEIpEJ; + auto& ppA10IeJepEJpEJ = aA10IeqcJeqc->ppAijIeJepEJpEJ; + for (int i = 0; i < 4; i++) + { + pGpEJ->atiput(i, pA01IeJepEJ->at(i) + pA10IeJepEJ->at(i)); + } + for (int i = 0; i < 4; i++) + { + auto& ppGpEIpEJi = ppGpEIpEJ->at(i); + auto& ppA01IeJepEIpEJi = ppA01IeJepEIpEJ->at(i); + auto& ppA10IeJepEIpEJi = ppA10IeJepEIpEJ->at(i); + for (int j = 0; j < 4; j++) + { + auto ppGpEIpEJij = ppA01IeJepEIpEJi->at(j) + ppA10IeJepEIpEJi->at(j); + ppGpEIpEJi->atiput(j, ppGpEIpEJij); + } + } + for (int i = 0; i < 4; i++) + { + auto& ppGpEJpEJi = ppGpEJpEJ->at(i); + auto& ppA01IeJepEJpEJi = ppA01IeJepEJpEJ->at(i); + auto& ppA10IeJepEJpEJi = ppA10IeJepEJpEJ->at(i); + ppGpEJpEJi->atiput(i, ppA01IeJepEJpEJi->at(i) + ppA10IeJepEJpEJi->at(i)); + for (int j = i + 1; j < 4; j++) + { + auto ppGpEJpEJij = ppA01IeJepEJpEJi->at(j) + ppA10IeJepEJpEJi->at(j); + ppGpEJpEJi->atiput(j, ppGpEJpEJij); + ppGpEJpEJ->atijput(j, i, ppGpEJpEJij); + } + } + +} + +void MbD::ConstVelConstraintIqcJqc::fillAccICIterError(FColDsptr col) +{ + ConstVelConstraintIqcJc::fillAccICIterError(col); + col->atiplusFullVectortimes(iqEJ, pGpEJ, lam); + auto efrmIqc = std::static_pointer_cast(frmI); + auto qEdotI = efrmIqc->qEdot(); + auto efrmJqc = std::static_pointer_cast(frmJ); + auto qEdotJ = efrmJqc->qEdot(); + auto sum = 0.0; + sum += pGpEJ->timesFullColumn(efrmJqc->qEddot()); + sum += 2.0 * qEdotI->transposeTimesFullColumn(ppGpEIpEJ->timesFullColumn(qEdotJ)); + sum += qEdotJ->transposeTimesFullColumn(ppGpEJpEJ->timesFullColumn(qEdotJ)); + col->atiplusNumber(iG, sum); +} + +void MbD::ConstVelConstraintIqcJqc::fillPosICError(FColDsptr col) +{ + ConstVelConstraintIqcJc::fillPosICError(col); + col->atiplusFullVectortimes(iqEJ, pGpEJ, lam); +} + +void MbD::ConstVelConstraintIqcJqc::fillPosICJacob(SpMatDsptr mat) +{ + ConstVelConstraintIqcJc::fillPosICJacob(mat); + mat->atijplusFullRow(iG, iqEJ, pGpEJ); + mat->atijplusFullColumn(iqEJ, iG, pGpEJ->transpose()); + auto ppGpEIpEJlam = ppGpEIpEJ->times(lam); + mat->atijplusFullMatrix(iqEI, iqEJ, ppGpEIpEJlam); + mat->atijplusTransposeFullMatrix(iqEJ, iqEI, ppGpEIpEJlam); + mat->atijplusFullMatrixtimes(iqEJ, iqEJ, ppGpEJpEJ, lam); +} + +void MbD::ConstVelConstraintIqcJqc::fillPosKineJacob(SpMatDsptr mat) +{ + ConstVelConstraintIqcJc::fillPosKineJacob(mat); + mat->atijplusFullRow(iG, iqEJ, pGpEJ); +} + +void MbD::ConstVelConstraintIqcJqc::fillVelICJacob(SpMatDsptr mat) +{ + ConstVelConstraintIqcJc::fillVelICJacob(mat); + mat->atijplusFullRow(iG, iqEJ, pGpEJ); + mat->atijplusFullColumn(iqEJ, iG, pGpEJ->transpose()); +} + +void MbD::ConstVelConstraintIqcJqc::initA01IeJe() +{ + aA01IeJe = CREATE::With(frmI, frmJ, 0, 1); +} + +void MbD::ConstVelConstraintIqcJqc::initA10IeJe() +{ + aA10IeJe = CREATE::With(frmI, frmJ, 1, 0); +} + +void MbD::ConstVelConstraintIqcJqc::initialize() +{ + ConstVelConstraintIqcJc::initialize(); + pGpEJ = std::make_shared>(4); + ppGpEIpEJ = std::make_shared>(4, 4); + ppGpEJpEJ = std::make_shared>(4, 4); +} + +void MbD::ConstVelConstraintIqcJqc::useEquationNumbers() +{ + ConstVelConstraintIqcJc::useEquationNumbers(); + iqEJ = std::static_pointer_cast(frmJ)->iqE(); +} diff --git a/MbDCode/ConstVelConstraintIqcJqc.h b/MbDCode/ConstVelConstraintIqcJqc.h new file mode 100644 index 0000000..f98bf5a --- /dev/null +++ b/MbDCode/ConstVelConstraintIqcJqc.h @@ -0,0 +1,29 @@ +#pragma once + +#include "ConstVelConstraintIqcJc.h" + +namespace MbD { + class ConstVelConstraintIqcJqc : public ConstVelConstraintIqcJc + { + //pGpEJ ppGpEIpEJ ppGpEJpEJ iqEJ + public: + ConstVelConstraintIqcJqc(EndFrmcptr frmi, EndFrmcptr frmj); + + void calcPostDynCorrectorIteration() override; + void fillAccICIterError(FColDsptr col) override; + void fillPosICError(FColDsptr col) override; + void fillPosICJacob(SpMatDsptr mat) override; + void fillPosKineJacob(SpMatDsptr mat) override; + void fillVelICJacob(SpMatDsptr mat) override; + void initA01IeJe() override; + void initA10IeJe() override; + void initialize() override; + void useEquationNumbers() override; + + FRowDsptr pGpEJ; + FMatDsptr ppGpEIpEJ; + FMatDsptr ppGpEJpEJ; + int iqEJ = -1; + }; +} + diff --git a/MbDCode/Constraint.cpp b/MbDCode/Constraint.cpp index c4eba08..dcac26a 100644 --- a/MbDCode/Constraint.cpp +++ b/MbDCode/Constraint.cpp @@ -28,16 +28,6 @@ void Constraint::postInput() Item::postInput(); } -void Constraint::setOwner(Item* x) -{ - owner = x; -} - -Item* Constraint::getOwner() -{ - return owner; -} - void Constraint::prePosIC() { lam = 0.0; @@ -91,7 +81,7 @@ ConstraintType Constraint::type() void Constraint::fillqsulam(FColDsptr col) { - col->at(iG) = lam; + col->atiput(iG, lam); } void Constraint::setqsulam(FColDsptr col) @@ -126,17 +116,6 @@ bool Constraint::isRedundant() return false; } -void Constraint::outputStates() -{ - Item::outputStates(); - std::stringstream ss; - ss << "iG = " << iG << std::endl; - ss << "aG = " << aG << std::endl; - ss << "lam = " << lam << std::endl; - auto str = ss.str(); - this->logString(str); -} - void Constraint::preDyn() { mu = 0.0; @@ -147,6 +126,11 @@ void Constraint::fillPosKineError(FColDsptr col) col->atiplusNumber(iG, aG); } +void Constraint::fillqsuddotlam(FColDsptr col) +{ + col->atiput(iG, lam); +} + void Constraint::preAccIC() { lam = 0.0; diff --git a/MbDCode/Constraint.h b/MbDCode/Constraint.h index 115e776..1910e16 100644 --- a/MbDCode/Constraint.h +++ b/MbDCode/Constraint.h @@ -11,33 +11,32 @@ namespace MbD { public: Constraint(); Constraint(const char* str); - void initialize() override; - void postInput() override; - void setOwner(Item* x); - Item* getOwner(); - void prePosIC() override; - void prePosKine() override; - virtual void fillEssenConstraints(std::shared_ptr sptr, std::shared_ptr>> essenConstraints); - virtual void fillDispConstraints(std::shared_ptr sptr, std::shared_ptr>> dispConstraints); - virtual void fillPerpenConstraints(std::shared_ptr sptr, std::shared_ptr>> perpenConstraints); - virtual void fillRedundantConstraints(std::shared_ptr sptr, std::shared_ptr>> redunConstraints); - virtual void fillConstraints(std::shared_ptr sptr, std::shared_ptr>> allConstraints); - virtual ConstraintType type(); - void fillqsulam(FColDsptr col) override; - void setqsulam(FColDsptr col) override; - void setqsudotlam(FColDsptr col) override; - void fillPosICError(FColDsptr col) override; - void removeRedundantConstraints(std::shared_ptr> redundantEqnNos) override; - void reactivateRedundantConstraints() override; - virtual bool isRedundant(); - void outputStates() override; - void preDyn() override; - void fillPosKineError(FColDsptr col) override; - void preAccIC() override; - void fillAccICIterJacob(SpMatDsptr mat) override; - void setqsuddotlam(FColDsptr col) override; + virtual void addToJointForceI(FColDsptr col); virtual void addToJointTorqueI(FColDsptr col); + void fillAccICIterJacob(SpMatDsptr mat) override; + virtual void fillConstraints(std::shared_ptr sptr, std::shared_ptr>> allConstraints); + virtual void fillDispConstraints(std::shared_ptr sptr, std::shared_ptr>> dispConstraints); + virtual void fillEssenConstraints(std::shared_ptr sptr, std::shared_ptr>> essenConstraints); + virtual void fillPerpenConstraints(std::shared_ptr sptr, std::shared_ptr>> perpenConstraints); + void fillPosICError(FColDsptr col) override; + void fillPosKineError(FColDsptr col) override; + void fillqsuddotlam(FColDsptr col) override; + void fillqsulam(FColDsptr col) override; + virtual void fillRedundantConstraints(std::shared_ptr sptr, std::shared_ptr>> redunConstraints); + void initialize() override; + virtual bool isRedundant(); + void postInput() override; + void preAccIC() override; + void preDyn() override; + void prePosIC() override; + void prePosKine() override; + void reactivateRedundantConstraints() override; + void removeRedundantConstraints(std::shared_ptr> redundantEqnNos) override; + void setqsudotlam(FColDsptr col) override; + void setqsuddotlam(FColDsptr col) override; + void setqsulam(FColDsptr col) override; + virtual ConstraintType type(); int iG = -1; double aG = 0.0; //Constraint function diff --git a/MbDCode/ConstraintIJ.h b/MbDCode/ConstraintIJ.h index ae7c106..bbf2fa0 100644 --- a/MbDCode/ConstraintIJ.h +++ b/MbDCode/ConstraintIJ.h @@ -9,6 +9,7 @@ namespace MbD { //frmI frmJ aConstant public: ConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj); + void initialize() override; EndFrmcptr frmI, frmJ; diff --git a/MbDCode/DirectionCosineConstraintIJ.cpp b/MbDCode/DirectionCosineConstraintIJ.cpp index 94bff3e..1f3994e 100644 --- a/MbDCode/DirectionCosineConstraintIJ.cpp +++ b/MbDCode/DirectionCosineConstraintIJ.cpp @@ -34,7 +34,7 @@ void DirectionCosineConstraintIJ::initaAijIeJe() void DirectionCosineConstraintIJ::postInput() { aAijIeJe->postInput(); - Constraint::postInput(); + ConstraintIJ::postInput(); } void DirectionCosineConstraintIJ::calcPostDynCorrectorIteration() @@ -45,7 +45,7 @@ void DirectionCosineConstraintIJ::calcPostDynCorrectorIteration() void DirectionCosineConstraintIJ::prePosIC() { aAijIeJe->prePosIC(); - Constraint::prePosIC(); + ConstraintIJ::prePosIC(); } void DirectionCosineConstraintIJ::postPosICIteration() @@ -65,8 +65,14 @@ void DirectionCosineConstraintIJ::preVelIC() Item::preVelIC(); } +void MbD::DirectionCosineConstraintIJ::simUpdateAll() +{ + aAijIeJe->simUpdateAll(); + ConstraintIJ::simUpdateAll(); +} + void DirectionCosineConstraintIJ::preAccIC() { aAijIeJe->preAccIC(); - Constraint::preAccIC(); + ConstraintIJ::preAccIC(); } diff --git a/MbDCode/DirectionCosineConstraintIJ.h b/MbDCode/DirectionCosineConstraintIJ.h index 519dabe..4d8d51c 100644 --- a/MbDCode/DirectionCosineConstraintIJ.h +++ b/MbDCode/DirectionCosineConstraintIJ.h @@ -10,17 +10,19 @@ namespace MbD { //axisI axisJ aAijIeJe public: DirectionCosineConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj); - void initialize() override; - void initializeLocally() override; - void initializeGlobally() override; - virtual void initaAijIeJe(); - void postInput() override; + void calcPostDynCorrectorIteration() override; - void prePosIC() override; + virtual void initaAijIeJe(); + void initialize() override; + void initializeGlobally() override; + void initializeLocally() override; + void postInput() override; void postPosICIteration() override; - ConstraintType type() override; - void preVelIC() override; void preAccIC() override; + void prePosIC() override; + void preVelIC() override; + void simUpdateAll() override; + ConstraintType type() override; int axisI, axisJ; std::shared_ptr aAijIeJe; diff --git a/MbDCode/DirectionCosineConstraintIqcJc.cpp b/MbDCode/DirectionCosineConstraintIqcJc.cpp index a65cce2..6e0bf9b 100644 --- a/MbDCode/DirectionCosineConstraintIqcJc.cpp +++ b/MbDCode/DirectionCosineConstraintIqcJc.cpp @@ -62,10 +62,6 @@ void DirectionCosineConstraintIqcJc::fillAccICIterError(FColDsptr col) col->atiplusNumber(iG, sum); } -void DirectionCosineConstraintIqcJc::addToJointForceI(FColDsptr col) -{ -} - void DirectionCosineConstraintIqcJc::addToJointTorqueI(FColDsptr jointTorque) { auto aBOIp = frmI->aBOp(); diff --git a/MbDCode/DirectionCosineConstraintIqcJc.h b/MbDCode/DirectionCosineConstraintIqcJc.h index b55f7e6..7d08992 100644 --- a/MbDCode/DirectionCosineConstraintIqcJc.h +++ b/MbDCode/DirectionCosineConstraintIqcJc.h @@ -3,25 +3,25 @@ #include "DirectionCosineConstraintIJ.h" namespace MbD { - class DirectionCosineConstraintIqcJc : public DirectionCosineConstraintIJ - { - //pGpEI ppGpEIpEI iqEI - public: - DirectionCosineConstraintIqcJc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj); - void initaAijIeJe() override; - void calcPostDynCorrectorIteration() override; - void useEquationNumbers() override; - void fillPosICError(FColDsptr col) override; - void fillPosICJacob(SpMatDsptr mat) override; - 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; + class DirectionCosineConstraintIqcJc : public DirectionCosineConstraintIJ + { + //pGpEI ppGpEIpEI iqEI + public: + DirectionCosineConstraintIqcJc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj); - FRowDsptr pGpEI; - FMatDsptr ppGpEIpEI; - int iqEI = -1; - }; + void addToJointTorqueI(FColDsptr col) override; + void calcPostDynCorrectorIteration() override; + void fillAccICIterError(FColDsptr col) override; + void fillPosICError(FColDsptr col) override; + void fillPosICJacob(SpMatDsptr mat) override; + void fillPosKineJacob(SpMatDsptr mat) override; + void fillVelICJacob(SpMatDsptr mat) override; + void initaAijIeJe() override; + void useEquationNumbers() override; + + FRowDsptr pGpEI; + FMatDsptr ppGpEIpEI; + int iqEI = -1; + }; } diff --git a/MbDCode/DirectionCosineConstraintIqcJqc.h b/MbDCode/DirectionCosineConstraintIqcJqc.h index 48d93f8..e0ba092 100644 --- a/MbDCode/DirectionCosineConstraintIqcJqc.h +++ b/MbDCode/DirectionCosineConstraintIqcJqc.h @@ -8,14 +8,15 @@ namespace MbD { //pGpEJ ppGpEIpEJ ppGpEJpEJ iqEJ public: DirectionCosineConstraintIqcJqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj); - void initaAijIeJe() override; + void calcPostDynCorrectorIteration() override; - void useEquationNumbers() override; + void fillAccICIterError(FColDsptr col) override; void fillPosICError(FColDsptr col) override; void fillPosICJacob(SpMatDsptr mat) override; void fillPosKineJacob(SpMatDsptr mat) override; void fillVelICJacob(SpMatDsptr mat) override; - void fillAccICIterError(FColDsptr col) override; + void initaAijIeJe() override; + void useEquationNumbers() override; FRowDsptr pGpEJ; FMatDsptr ppGpEIpEJ; diff --git a/MbDCode/DirectionCosineConstraintIqctJqc.h b/MbDCode/DirectionCosineConstraintIqctJqc.h index e579114..41d9be6 100644 --- a/MbDCode/DirectionCosineConstraintIqctJqc.h +++ b/MbDCode/DirectionCosineConstraintIqctJqc.h @@ -8,12 +8,13 @@ namespace MbD { //pGpt ppGpEIpt ppGpEJpt ppGptpt public: DirectionCosineConstraintIqctJqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj); - void initaAijIeJe() override; - ConstraintType type() override; - void preVelIC() override; - void fillVelICError(FColDsptr col) override; - void preAccIC() override; + void fillAccICIterError(FColDsptr col) override; + void fillVelICError(FColDsptr col) override; + void initaAijIeJe() override; + void preAccIC() override; + void preVelIC() override; + ConstraintType type() override; double pGpt; FRowDsptr ppGpEIpt; diff --git a/MbDCode/DirectionCosineIecJec.cpp b/MbDCode/DirectionCosineIecJec.cpp index 1e166bb..5522ebe 100644 --- a/MbDCode/DirectionCosineIecJec.cpp +++ b/MbDCode/DirectionCosineIecJec.cpp @@ -22,32 +22,7 @@ void DirectionCosineIecJec::calcPostDynCorrectorIteration() aAijIeJe = aAjOIe->dot(aAjOJe); } -FRowDsptr DirectionCosineIecJec::pvaluepXJ() +double MbD::DirectionCosineIecJec::value() { - assert(false); - return FRowDsptr(); -} - -FRowDsptr DirectionCosineIecJec::pvaluepEJ() -{ - assert(false); - return FRowDsptr(); -} - -FMatDsptr DirectionCosineIecJec::ppvaluepXJpEK() -{ - assert(false); - return FMatDsptr(); -} - -FMatDsptr DirectionCosineIecJec::ppvaluepEJpEK() -{ - assert(false); - return FMatDsptr(); -} - -FMatDsptr DirectionCosineIecJec::ppvaluepEJpEJ() -{ - assert(false); - return FMatDsptr(); + return aAijIeJe; } diff --git a/MbDCode/DirectionCosineIecJec.h b/MbDCode/DirectionCosineIecJec.h index c2de495..4a5e747 100644 --- a/MbDCode/DirectionCosineIecJec.h +++ b/MbDCode/DirectionCosineIecJec.h @@ -13,12 +13,9 @@ namespace MbD { public: DirectionCosineIecJec(); DirectionCosineIecJec(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj); + void calcPostDynCorrectorIteration() override; - FRowDsptr pvaluepXJ() override; - FRowDsptr pvaluepEJ() override; - FMatDsptr ppvaluepXJpEK() override; - FMatDsptr ppvaluepEJpEK() override; - FMatDsptr ppvaluepEJpEJ() override; + double value() override; int axisI, axisJ; //0, 1, 2 = x, y, z double aAijIeJe; diff --git a/MbDCode/DirectionCosineIeqcJec.cpp b/MbDCode/DirectionCosineIeqcJec.cpp index c886412..40844ad 100644 --- a/MbDCode/DirectionCosineIeqcJec.cpp +++ b/MbDCode/DirectionCosineIeqcJec.cpp @@ -24,6 +24,16 @@ void DirectionCosineIeqcJec::initializeGlobally() ppAjOIepEIpEI = std::static_pointer_cast(frmI)->ppAjOepEpE(axisI); } +FMatDsptr MbD::DirectionCosineIeqcJec::ppvaluepEIpEI() +{ + return ppAijIeJepEIpEI; +} + +FRowDsptr MbD::DirectionCosineIeqcJec::pvaluepEI() +{ + return pAijIeJepEI; +} + void DirectionCosineIeqcJec::calcPostDynCorrectorIteration() { DirectionCosineIecJec::calcPostDynCorrectorIteration(); diff --git a/MbDCode/DirectionCosineIeqcJec.h b/MbDCode/DirectionCosineIeqcJec.h index de59393..3a3afdc 100644 --- a/MbDCode/DirectionCosineIeqcJec.h +++ b/MbDCode/DirectionCosineIeqcJec.h @@ -9,9 +9,12 @@ namespace MbD { public: DirectionCosineIeqcJec(); DirectionCosineIeqcJec(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj); - void initialize() override; - void initializeGlobally(); + void calcPostDynCorrectorIteration() override; + void initialize() override; + void initializeGlobally() override; + FMatDsptr ppvaluepEIpEI() override; + FRowDsptr pvaluepEI() override; FRowDsptr pAijIeJepEI; FMatDsptr ppAijIeJepEIpEI; diff --git a/MbDCode/DirectionCosineIeqcJeqc.cpp b/MbDCode/DirectionCosineIeqcJeqc.cpp index 6aeedb7..9ffc52a 100644 --- a/MbDCode/DirectionCosineIeqcJeqc.cpp +++ b/MbDCode/DirectionCosineIeqcJeqc.cpp @@ -26,6 +26,11 @@ void DirectionCosineIeqcJeqc::initializeGlobally() ppAjOJepEJpEJ = std::static_pointer_cast(frmJ)->ppAjOepEpE(axisJ); } +FMatDsptr MbD::DirectionCosineIeqcJeqc::ppvaluepEIpEJ() +{ + return ppAijIeJepEIpEJ; +} + void DirectionCosineIeqcJeqc::calcPostDynCorrectorIteration() { DirectionCosineIeqcJec::calcPostDynCorrectorIteration(); diff --git a/MbDCode/DirectionCosineIeqcJeqc.h b/MbDCode/DirectionCosineIeqcJeqc.h index dd6ed9b..5372cbd 100644 --- a/MbDCode/DirectionCosineIeqcJeqc.h +++ b/MbDCode/DirectionCosineIeqcJeqc.h @@ -9,11 +9,13 @@ namespace MbD { public: DirectionCosineIeqcJeqc(); DirectionCosineIeqcJeqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj); - void initialize() override; - void initializeGlobally(); + void calcPostDynCorrectorIteration() override; - FRowDsptr pvaluepEJ() override; + void initialize() override; + void initializeGlobally() override; + FMatDsptr ppvaluepEIpEJ() override; FMatDsptr ppvaluepEJpEJ() override; + FRowDsptr pvaluepEJ() override; FRowDsptr pAijIeJepEJ; FMatDsptr ppAijIeJepEIpEJ; diff --git a/MbDCode/DirectionCosineIeqctJeqc.cpp b/MbDCode/DirectionCosineIeqctJeqc.cpp index c64dc55..7255466 100644 --- a/MbDCode/DirectionCosineIeqctJeqc.cpp +++ b/MbDCode/DirectionCosineIeqctJeqc.cpp @@ -25,6 +25,21 @@ void DirectionCosineIeqctJeqc::initializeGlobally() ppAjOJepEJpEJ = std::static_pointer_cast(frmJ)->ppAjOepEpE(axisJ); } +FRowDsptr MbD::DirectionCosineIeqctJeqc::ppvaluepEIpt() +{ + return ppAijIeJepEIpt; +} + +FRowDsptr MbD::DirectionCosineIeqctJeqc::ppvaluepEJpt() +{ + return ppAijIeJepEJpt; +} + +double MbD::DirectionCosineIeqctJeqc::ppvalueptpt() +{ + return ppAijIeJeptpt; +} + void DirectionCosineIeqctJeqc::calcPostDynCorrectorIteration() { //"ppAjOIepEIpEI is not longer constant and must be set before any calculation." diff --git a/MbDCode/DirectionCosineIeqctJeqc.h b/MbDCode/DirectionCosineIeqctJeqc.h index 587bdec..efa85df 100644 --- a/MbDCode/DirectionCosineIeqctJeqc.h +++ b/MbDCode/DirectionCosineIeqctJeqc.h @@ -9,12 +9,16 @@ namespace MbD { public: DirectionCosineIeqctJeqc(); DirectionCosineIeqctJeqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj); - void initialize() override; - void initializeGlobally(); + void calcPostDynCorrectorIteration() override; - void preVelIC() override; - double pvaluept(); + void initialize() override; + void initializeGlobally() override; + FRowDsptr ppvaluepEIpt() override; + FRowDsptr ppvaluepEJpt() override; + double ppvalueptpt() override; void preAccIC() override; + void preVelIC() override; + double pvaluept() override; double pAijIeJept; FRowDsptr ppAijIeJepEIpt; diff --git a/MbDCode/DispCompIecJecIe.cpp b/MbDCode/DispCompIecJecIe.cpp new file mode 100644 index 0000000..67a2d60 --- /dev/null +++ b/MbDCode/DispCompIecJecIe.cpp @@ -0,0 +1,11 @@ +#include "DispCompIecJecIe.h" + +using namespace MbD; + +MbD::DispCompIecJecIe::DispCompIecJecIe() +{ +} + +MbD::DispCompIecJecIe::DispCompIecJecIe(EndFrmcptr frmi, EndFrmcptr frmj, int axis) : KinematicIeJe(frmi, frmj), axis(axis) +{ +} diff --git a/MbDCode/DispCompIecJecIe.h b/MbDCode/DispCompIecJecIe.h new file mode 100644 index 0000000..62f4da9 --- /dev/null +++ b/MbDCode/DispCompIecJecIe.h @@ -0,0 +1,18 @@ +#pragma once + +#include "KinematicIeJe.h" + +namespace MbD { + class DispCompIecJecIe : public KinematicIeJe + { + //axis riIeJeIe aAjOIe rIeJeO + public: + DispCompIecJecIe(); + DispCompIecJecIe(EndFrmcptr frmi, EndFrmcptr frmj, int axis); + + int axis; + double riIeJeIe; + FColDsptr aAjOIe, rIeJeO; + }; +} + diff --git a/MbDCode/DispCompIecJecKec.cpp b/MbDCode/DispCompIecJecKec.cpp index ddd45e5..ba0533f 100644 --- a/MbDCode/DispCompIecJecKec.cpp +++ b/MbDCode/DispCompIecJecKec.cpp @@ -10,36 +10,6 @@ DispCompIecJecKec::DispCompIecJecKec(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcpt { } -FRowDsptr DispCompIecJecKec::pvaluepXJ() -{ - assert(false); - return FRowDsptr(); -} - -FRowDsptr DispCompIecJecKec::pvaluepEJ() -{ - assert(false); - return FRowDsptr(); -} - -FMatDsptr DispCompIecJecKec::ppvaluepXJpEK() -{ - assert(false); - return FMatDsptr(); -} - -FMatDsptr DispCompIecJecKec::ppvaluepEJpEK() -{ - assert(false); - return FMatDsptr(); -} - -FMatDsptr DispCompIecJecKec::ppvaluepEJpEJ() -{ - assert(false); - return FMatDsptr(); -} - double DispCompIecJecKec::value() { return riIeJeKe; diff --git a/MbDCode/DispCompIecJecKec.h b/MbDCode/DispCompIecJecKec.h index 5e6826c..5d037ef 100644 --- a/MbDCode/DispCompIecJecKec.h +++ b/MbDCode/DispCompIecJecKec.h @@ -9,13 +9,9 @@ namespace MbD { public: DispCompIecJecKec(); DispCompIecJecKec(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk); - FRowDsptr pvaluepXJ() override; - FRowDsptr pvaluepEJ() override; - FMatDsptr ppvaluepXJpEK() override; - FMatDsptr ppvaluepEJpEK() override; - FMatDsptr ppvaluepEJpEJ() override; - virtual double value(); + double value() override; + EndFrmcptr efrmK; int axisK; double riIeJeKe; diff --git a/MbDCode/DispCompIecJecKeqc.cpp b/MbDCode/DispCompIecJecKeqc.cpp index 4bc45dc..5f8cdef 100644 --- a/MbDCode/DispCompIecJecKeqc.cpp +++ b/MbDCode/DispCompIecJecKeqc.cpp @@ -51,3 +51,8 @@ FMatDsptr DispCompIecJecKeqc::ppvaluepEKpEK() { return ppriIeJeKepEKpEK; } + +FRowDsptr MbD::DispCompIecJecKeqc::pvaluepEK() +{ + return priIeJeKepEK; +} diff --git a/MbDCode/DispCompIecJecKeqc.h b/MbDCode/DispCompIecJecKeqc.h index c559993..1d8ee3f 100644 --- a/MbDCode/DispCompIecJecKeqc.h +++ b/MbDCode/DispCompIecJecKeqc.h @@ -9,11 +9,13 @@ namespace MbD { public: DispCompIecJecKeqc(); DispCompIecJecKeqc(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk); + + void calcPostDynCorrectorIteration() override; void initialize() override; void initializeGlobally() override; - void calcPostDynCorrectorIteration() override; + FMatDsptr ppvaluepEKpEK() override; + FRowDsptr pvaluepEK() override; - FMatDsptr ppvaluepEKpEK(); FRowDsptr priIeJeKepEK; FMatDsptr ppriIeJeKepEKpEK; FMatDsptr pAjOKepEKT; diff --git a/MbDCode/DispCompIecJecO.cpp b/MbDCode/DispCompIecJecO.cpp index 9c62e3e..6a29266 100644 --- a/MbDCode/DispCompIecJecO.cpp +++ b/MbDCode/DispCompIecJecO.cpp @@ -15,32 +15,7 @@ void DispCompIecJecO::calcPostDynCorrectorIteration() riIeJeO = frmJ->riOeO(axis) - frmI->riOeO(axis); } -FRowDsptr DispCompIecJecO::pvaluepXJ() +double MbD::DispCompIecJecO::value() { - assert(false); - return FRowDsptr(); -} - -FRowDsptr DispCompIecJecO::pvaluepEJ() -{ - assert(false); - return FRowDsptr(); -} - -FMatDsptr DispCompIecJecO::ppvaluepXJpEK() -{ - assert(false); - return FMatDsptr(); -} - -FMatDsptr DispCompIecJecO::ppvaluepEJpEK() -{ - assert(false); - return FMatDsptr(); -} - -FMatDsptr DispCompIecJecO::ppvaluepEJpEJ() -{ - assert(false); - return FMatDsptr(); + return riIeJeO; } diff --git a/MbDCode/DispCompIecJecO.h b/MbDCode/DispCompIecJecO.h index 91fdeac..f8c99de 100644 --- a/MbDCode/DispCompIecJecO.h +++ b/MbDCode/DispCompIecJecO.h @@ -9,12 +9,9 @@ namespace MbD { public: DispCompIecJecO(); DispCompIecJecO(EndFrmcptr frmi, EndFrmcptr frmj, int axis); + void calcPostDynCorrectorIteration() override; - FRowDsptr pvaluepXJ() override; - FRowDsptr pvaluepEJ() override; - FMatDsptr ppvaluepXJpEK() override; - FMatDsptr ppvaluepEJpEK() override; - FMatDsptr ppvaluepEJpEJ() override; + double value() override; int axis = -1; double riIeJeO; diff --git a/MbDCode/DispCompIeqcJecIe.cpp b/MbDCode/DispCompIeqcJecIe.cpp new file mode 100644 index 0000000..8fc75c0 --- /dev/null +++ b/MbDCode/DispCompIeqcJecIe.cpp @@ -0,0 +1,9 @@ +#include "DispCompIeqcJecIe.h" + +MbD::DispCompIeqcJecIe::DispCompIeqcJecIe() +{ +} + +MbD::DispCompIeqcJecIe::DispCompIeqcJecIe(EndFrmcptr frmi, EndFrmcptr frmj, int axis) : DispCompIecJecIe(frmi, frmj, axis) +{ +} diff --git a/MbDCode/DispCompIeqcJecIe.h b/MbDCode/DispCompIeqcJecIe.h new file mode 100644 index 0000000..6c52bf6 --- /dev/null +++ b/MbDCode/DispCompIeqcJecIe.h @@ -0,0 +1,18 @@ +#pragma once + +#include "DispCompIecJecIe.h" + +namespace MbD { + class DispCompIeqcJecIe : public DispCompIecJecIe + { + //priIeJeIepXI priIeJeIepEI ppriIeJeIepXIpEI ppriIeJeIepEIpEI pAjOIepEIT ppAjOIepEIpEI + public: + DispCompIeqcJecIe(); + DispCompIeqcJecIe(EndFrmcptr frmi, EndFrmcptr frmj, int axis); + + FRowDsptr priIeJeIepXI, priIeJeIepEI; + FMatDsptr ppriIeJeIepXIpEI, ppriIeJeIepEIpEI, pAjOIepEIT; + FMatFColDsptr ppAjOIepEIpEI; + }; +} + diff --git a/MbDCode/DispCompIeqcJecKeqc.cpp b/MbDCode/DispCompIeqcJecKeqc.cpp index f1b8210..a44641d 100644 --- a/MbDCode/DispCompIeqcJecKeqc.cpp +++ b/MbDCode/DispCompIeqcJecKeqc.cpp @@ -26,7 +26,7 @@ void DispCompIeqcJecKeqc::calcPostDynCorrectorIteration() DispCompIecJecKeqc::calcPostDynCorrectorIteration(); auto frmIqc = std::static_pointer_cast(frmI); auto mprIeJeOpEIT = frmIqc->prOeOpE->transpose(); - auto mpprIeJeOpEIpEI = frmIqc->pprOeOpEpE; + auto& mpprIeJeOpEIpEI = frmIqc->pprOeOpEpE; for (int i = 0; i < 3; i++) { priIeJeKepXI->at(i) = 0.0 - (aAjOKe->at(i)); @@ -76,11 +76,6 @@ FRowDsptr DispCompIeqcJecKeqc::pvaluepEI() return priIeJeKepEI; } -FRowDsptr DispCompIeqcJecKeqc::pvaluepEK() -{ - return priIeJeKepEK; -} - FMatDsptr DispCompIeqcJecKeqc::ppvaluepXIpEK() { return ppriIeJeKepXIpEK; diff --git a/MbDCode/DispCompIeqcJecKeqc.h b/MbDCode/DispCompIeqcJecKeqc.h index 7e28a3a..e3e1945 100644 --- a/MbDCode/DispCompIeqcJecKeqc.h +++ b/MbDCode/DispCompIeqcJecKeqc.h @@ -9,15 +9,15 @@ namespace MbD { public: DispCompIeqcJecKeqc(); DispCompIeqcJecKeqc(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk); - void initialize() override; - void calcPostDynCorrectorIteration() override; - FRowDsptr pvaluepXI(); - FRowDsptr pvaluepEI(); - FRowDsptr pvaluepEK(); - FMatDsptr ppvaluepXIpEK(); - FMatDsptr ppvaluepEIpEK(); - FMatDsptr ppvaluepEIpEI(); + void calcPostDynCorrectorIteration() override; + void initialize() override; + FRowDsptr pvaluepXI() override; + FRowDsptr pvaluepEI() override; + FMatDsptr ppvaluepXIpEK() override; + FMatDsptr ppvaluepEIpEK() override; + FMatDsptr ppvaluepEIpEI() override; + FRowDsptr priIeJeKepXI; FRowDsptr priIeJeKepEI; FMatDsptr ppriIeJeKepXIpEK; diff --git a/MbDCode/DispCompIeqcJecO.cpp b/MbDCode/DispCompIeqcJecO.cpp index 20f883d..c305e70 100644 --- a/MbDCode/DispCompIeqcJecO.cpp +++ b/MbDCode/DispCompIeqcJecO.cpp @@ -18,6 +18,21 @@ void DispCompIeqcJecO::initializeGlobally() ppriIeJeOpEIpEI = std::static_pointer_cast(frmI)->ppriOeOpEpE(axis)->negated(); } +FMatDsptr MbD::DispCompIeqcJecO::ppvaluepEIpEI() +{ + return ppriIeJeOpEIpEI; +} + +FRowDsptr MbD::DispCompIeqcJecO::pvaluepEI() +{ + return priIeJeOpEI; +} + +FRowDsptr MbD::DispCompIeqcJecO::pvaluepXI() +{ + return priIeJeOpXI; +} + void DispCompIeqcJecO::calcPostDynCorrectorIteration() { DispCompIecJecO::calcPostDynCorrectorIteration(); diff --git a/MbDCode/DispCompIeqcJecO.h b/MbDCode/DispCompIeqcJecO.h index 862b51d..8cf2216 100644 --- a/MbDCode/DispCompIeqcJecO.h +++ b/MbDCode/DispCompIeqcJecO.h @@ -9,8 +9,12 @@ namespace MbD { public: DispCompIeqcJecO(); DispCompIeqcJecO(EndFrmcptr frmi, EndFrmcptr frmj, int axis); - void initializeGlobally() override; + void calcPostDynCorrectorIteration() override; + void initializeGlobally() override; + FMatDsptr ppvaluepEIpEI() override; + FRowDsptr pvaluepEI() override; + FRowDsptr pvaluepXI() override; FRowDsptr priIeJeOpXI; FRowDsptr priIeJeOpEI; diff --git a/MbDCode/DispCompIeqcJeqcIe.cpp b/MbDCode/DispCompIeqcJeqcIe.cpp new file mode 100644 index 0000000..85701dc --- /dev/null +++ b/MbDCode/DispCompIeqcJeqcIe.cpp @@ -0,0 +1,9 @@ +#include "DispCompIeqcJeqcIe.h" + +MbD::DispCompIeqcJeqcIe::DispCompIeqcJeqcIe() +{ +} + +MbD::DispCompIeqcJeqcIe::DispCompIeqcJeqcIe(EndFrmcptr frmi, EndFrmcptr frmj, int axis) : DispCompIeqcJecIe(frmi, frmj, axis) +{ +} diff --git a/MbDCode/DispCompIeqcJeqcIe.h b/MbDCode/DispCompIeqcJeqcIe.h new file mode 100644 index 0000000..aafbcf0 --- /dev/null +++ b/MbDCode/DispCompIeqcJeqcIe.h @@ -0,0 +1,17 @@ +#pragma once + +#include "DispCompIeqcJecIe.h" + +namespace MbD { + class DispCompIeqcJeqcIe : public DispCompIeqcJecIe + { + //priIeJeIepXJ priIeJeIepEJ ppriIeJeIepEIpXJ ppriIeJeIepEIpEJ ppriIeJeIepEJpEJ + public: + DispCompIeqcJeqcIe(); + DispCompIeqcJeqcIe(EndFrmcptr frmi, EndFrmcptr frmj, int axis); + + FRowDsptr priIeJeIepXJ, priIeJeIepEJ; + FMatDsptr ppriIeJeIepEIpXJ, ppriIeJeIepEIpEJ, ppriIeJeIepEJpEJ; + }; +} + diff --git a/MbDCode/DispCompIeqcJeqcKeqc.cpp b/MbDCode/DispCompIeqcJeqcKeqc.cpp index 525dba7..586bcb4 100644 --- a/MbDCode/DispCompIeqcJeqcKeqc.cpp +++ b/MbDCode/DispCompIeqcJeqcKeqc.cpp @@ -26,33 +26,33 @@ void DispCompIeqcJeqcKeqc::calcPostDynCorrectorIteration() DispCompIeqcJecKeqc::calcPostDynCorrectorIteration(); auto frmJqc = std::static_pointer_cast(frmJ); auto prIeJeOpEJT = frmJqc->prOeOpE->transpose(); - auto pprIeJeOpEJpEJ = frmJqc->pprOeOpEpE; + auto& pprIeJeOpEJpEJ = frmJqc->pprOeOpEpE; for (int i = 0; i < 3; i++) { - priIeJeKepXJ->at(i) = (aAjOKe->at(i)); + priIeJeKepXJ->atiput(i, aAjOKe->at(i)); } for (int i = 0; i < 4; i++) { - priIeJeKepEJ->at(i) = (aAjOKe->dot(prIeJeOpEJT->at(i))); + priIeJeKepEJ->atiput(i, aAjOKe->dot(prIeJeOpEJT->at(i))); } for (int i = 0; i < 3; i++) { auto& ppriIeJeKepXJipEK = ppriIeJeKepXJpEK->at(i); for (int j = 0; j < 4; j++) { - ppriIeJeKepXJipEK->at(j) = (pAjOKepEKT->at(j)->at(i)); + ppriIeJeKepXJipEK->atiput(j, pAjOKepEKT->at(j)->at(i)); } } for (int i = 0; i < 4; i++) { auto& pprIeJeOpEJipEJ = pprIeJeOpEJpEJ->at(i); auto& ppriIeJeKepEJipEJ = ppriIeJeKepEJpEJ->at(i); - ppriIeJeKepEJipEJ->at(i) = (aAjOKe->dot(pprIeJeOpEJipEJ->at(i))); + ppriIeJeKepEJipEJ->atiput(i, aAjOKe->dot(pprIeJeOpEJipEJ->at(i))); for (int j = 0; j < 4; j++) { auto ppriIeJeKepEJipEJj = (aAjOKe->dot(pprIeJeOpEJipEJ->at(j))); - ppriIeJeKepEJipEJ->at(j) = ppriIeJeKepEJipEJj; - ppriIeJeKepEJpEJ->at(j)->at(i) = ppriIeJeKepEJipEJj; + ppriIeJeKepEJipEJ->atiput(j, ppriIeJeKepEJipEJj); + ppriIeJeKepEJpEJ->atijput(j, i, ppriIeJeKepEJipEJj); } } for (int i = 0; i < 4; i++) @@ -61,7 +61,7 @@ void DispCompIeqcJeqcKeqc::calcPostDynCorrectorIteration() auto& ppriIeJeKepEJipEK = ppriIeJeKepEJpEK->at(i); for (int j = 0; j < 4; j++) { - ppriIeJeKepEJipEK->at(j) = (pAjOKepEKT->at(j)->dot(prIeJeOpEJTi)); + ppriIeJeKepEJipEK->atiput(j, pAjOKepEKT->at(j)->dot(prIeJeOpEJTi)); } } } diff --git a/MbDCode/DispCompIeqcJeqcKeqc.h b/MbDCode/DispCompIeqcJeqcKeqc.h index 8bc80c5..ec0efae 100644 --- a/MbDCode/DispCompIeqcJeqcKeqc.h +++ b/MbDCode/DispCompIeqcJeqcKeqc.h @@ -9,12 +9,13 @@ namespace MbD { public: DispCompIeqcJeqcKeqc(); DispCompIeqcJeqcKeqc(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk); - void initialize() override; + void calcPostDynCorrectorIteration() override; + void initialize() override; FRowDsptr pvaluepXJ() override; FRowDsptr pvaluepEJ() override; - FMatDsptr ppvaluepXJpEK(); - FMatDsptr ppvaluepEJpEK(); + FMatDsptr ppvaluepXJpEK() override; + FMatDsptr ppvaluepEJpEK() override; FMatDsptr ppvaluepEJpEJ() override; FRowDsptr priIeJeKepXJ; diff --git a/MbDCode/DispCompIeqcJeqcKeqct.h b/MbDCode/DispCompIeqcJeqcKeqct.h index 6c36671..e239e53 100644 --- a/MbDCode/DispCompIeqcJeqcKeqct.h +++ b/MbDCode/DispCompIeqcJeqcKeqct.h @@ -9,17 +9,18 @@ namespace MbD { public: DispCompIeqcJeqcKeqct(); DispCompIeqcJeqcKeqct(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk); - void initialize() override; + void calcPostDynCorrectorIteration() override; - void preVelIC() override; - double pvaluept(); + void initialize() override; FRowDsptr ppvaluepXIpt(); FRowDsptr ppvaluepEIpt(); FRowDsptr ppvaluepEKpt(); FRowDsptr ppvaluepXJpt(); FRowDsptr ppvaluepEJpt(); double ppvalueptpt(); + double pvaluept(); void preAccIC() override; + void preVelIC() override; double priIeJeKept; FRowDsptr ppriIeJeKepXIpt; diff --git a/MbDCode/DispCompIeqcJeqcO.h b/MbDCode/DispCompIeqcJeqcO.h index fa75b1f..2f1eded 100644 --- a/MbDCode/DispCompIeqcJeqcO.h +++ b/MbDCode/DispCompIeqcJeqcO.h @@ -9,11 +9,12 @@ namespace MbD { public: DispCompIeqcJeqcO(); DispCompIeqcJeqcO(EndFrmcptr frmi, EndFrmcptr frmj, int axis); - void initializeGlobally() override; + void calcPostDynCorrectorIteration() override; - FRowDsptr pvaluepXJ() override; - FRowDsptr pvaluepEJ() override; + void initializeGlobally() override; FMatDsptr ppvaluepEJpEJ() override; + FRowDsptr pvaluepEJ() override; + FRowDsptr pvaluepXJ() override; FRowDsptr priIeJeOpXJ; FRowDsptr priIeJeOpEJ; diff --git a/MbDCode/DispCompIeqctJeqcIe.cpp b/MbDCode/DispCompIeqctJeqcIe.cpp new file mode 100644 index 0000000..d6f6e8c --- /dev/null +++ b/MbDCode/DispCompIeqctJeqcIe.cpp @@ -0,0 +1,9 @@ +#include "DispCompIeqctJeqcIe.h" + +MbD::DispCompIeqctJeqcIe::DispCompIeqctJeqcIe() +{ +} + +MbD::DispCompIeqctJeqcIe::DispCompIeqctJeqcIe(EndFrmcptr frmi, EndFrmcptr frmj, int axis) : DispCompIeqcJeqcIe(frmi, frmj, axis) +{ +} diff --git a/MbDCode/DispCompIeqctJeqcIe.h b/MbDCode/DispCompIeqctJeqcIe.h new file mode 100644 index 0000000..9bd8384 --- /dev/null +++ b/MbDCode/DispCompIeqctJeqcIe.h @@ -0,0 +1,17 @@ +#pragma once + +#include "DispCompIeqcJeqcIe.h" + +namespace MbD { + class DispCompIeqctJeqcIe : public DispCompIeqcJeqcIe + { + //priIeJeIept ppriIeJeIepXIpt ppriIeJeIepEIpt ppriIeJeIepXJpt ppriIeJeIepEJpt ppriIeJeIeptpt + public: + DispCompIeqctJeqcIe(); + DispCompIeqctJeqcIe(EndFrmcptr frmi, EndFrmcptr frmj, int axis); + + double priIeJeIept, ppriIeJeIeptpt; + FRowDsptr ppriIeJeIepXIpt, ppriIeJeIepEIpt, ppriIeJeIepXJpt, ppriIeJeIepEJpt; + }; +} + diff --git a/MbDCode/DispCompIeqctJeqcKeqct.h b/MbDCode/DispCompIeqctJeqcKeqct.h index 76a589f..88114f7 100644 --- a/MbDCode/DispCompIeqctJeqcKeqct.h +++ b/MbDCode/DispCompIeqctJeqcKeqct.h @@ -9,8 +9,9 @@ namespace MbD { public: DispCompIeqctJeqcKeqct(); DispCompIeqctJeqcKeqct(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk); - void preVelIC() override; + void preAccIC() override; + void preVelIC() override; }; } diff --git a/MbDCode/DispCompIeqctJeqcO.cpp b/MbDCode/DispCompIeqctJeqcO.cpp index d15a451..6528d63 100644 --- a/MbDCode/DispCompIeqctJeqcO.cpp +++ b/MbDCode/DispCompIeqctJeqcO.cpp @@ -17,6 +17,16 @@ void DispCompIeqctJeqcO::initializeGlobally() ppriIeJeOpEJpEJ = std::static_pointer_cast(frmJ)->ppriOeOpEpE(axis); } +FRowDsptr MbD::DispCompIeqctJeqcO::ppvaluepEIpt() +{ + return ppriIeJeOpEIpt; +} + +double MbD::DispCompIeqctJeqcO::ppvalueptpt() +{ + return ppriIeJeOptpt; +} + void DispCompIeqctJeqcO::calcPostDynCorrectorIteration() { //"ppriIeJeOpEIpEI is not a constant now." diff --git a/MbDCode/DispCompIeqctJeqcO.h b/MbDCode/DispCompIeqctJeqcO.h index 7a9e9d4..3b23cb3 100644 --- a/MbDCode/DispCompIeqctJeqcO.h +++ b/MbDCode/DispCompIeqctJeqcO.h @@ -9,11 +9,14 @@ namespace MbD { public: DispCompIeqctJeqcO(); DispCompIeqctJeqcO(EndFrmcptr frmi, EndFrmcptr frmj, int axis); - void initializeGlobally() override; + void calcPostDynCorrectorIteration() override; + void initializeGlobally() override; + FRowDsptr ppvaluepEIpt() override; + double ppvalueptpt() override; + void preAccIC() override; void preVelIC() override; double pvaluept(); - void preAccIC() override; double priIeJeOpt; FRowDsptr ppriIeJeOpEIpt; diff --git a/MbDCode/DistIecJec.cpp b/MbDCode/DistIecJec.cpp new file mode 100644 index 0000000..3e27b16 --- /dev/null +++ b/MbDCode/DistIecJec.cpp @@ -0,0 +1,28 @@ +#include "DistIecJec.h" + +MbD::DistIecJec::DistIecJec() +{ +} + +MbD::DistIecJec::DistIecJec(EndFrmcptr frmi, EndFrmcptr frmj) : KinematicIeJe(frmi, frmj) +{ +} + +void MbD::DistIecJec::calcPostDynCorrectorIteration() +{ + rIeJeO = frmJ->rOeO->minusFullColumn(frmI->rOeO); + rIeJe = rIeJeO->length(); + this->calcPrivate(); +} + +void MbD::DistIecJec::calcPrivate() +{ + if (rIeJe == 0.0) return; + uIeJeO = rIeJeO->times(1.0 / rIeJe); + muIeJeO = uIeJeO->negated(); +} + +double MbD::DistIecJec::value() +{ + return rIeJe; +} diff --git a/MbDCode/DistIecJec.h b/MbDCode/DistIecJec.h new file mode 100644 index 0000000..0d892bc --- /dev/null +++ b/MbDCode/DistIecJec.h @@ -0,0 +1,21 @@ +#pragma once + +#include "KinematicIeJe.h" + +namespace MbD { + class DistIecJec : public KinematicIeJe + { + //rIeJe rIeJeO uIeJeO muIeJeO + public: + DistIecJec(); + DistIecJec(EndFrmcptr frmi, EndFrmcptr frmj); + + void calcPostDynCorrectorIteration() override; + virtual void calcPrivate(); + double value() override; + + double rIeJe; + FColDsptr rIeJeO, uIeJeO, muIeJeO; + }; +} + diff --git a/MbDCode/DistIeqcJec.cpp b/MbDCode/DistIeqcJec.cpp new file mode 100644 index 0000000..1feff10 --- /dev/null +++ b/MbDCode/DistIeqcJec.cpp @@ -0,0 +1,97 @@ +#include "DistIeqcJec.h" +#include "EndFrameqc.h" + +using namespace MbD; + +MbD::DistIeqcJec::DistIeqcJec() +{ +} + +MbD::DistIeqcJec::DistIeqcJec(EndFrmcptr frmi, EndFrmcptr frmj) : DistIecJec(frmi, frmj) +{ +} + +void MbD::DistIeqcJec::calcPrivate() +{ + DistIecJec::calcPrivate(); + if (rIeJe == 0.0) return; + auto frmIeqc = std::static_pointer_cast(frmI); + auto& mprIeJeOpEI = frmIeqc->prOeOpE; + auto mprIeJeOpEIT = mprIeJeOpEI->transpose(); + auto& mpprIeJeOpEIpEI = frmIeqc->pprOeOpEpE; + auto muIeJeOT = muIeJeO->transpose(); + prIeJepXI = muIeJeOT; + prIeJepEI = muIeJeOT->timesFullMatrix(mprIeJeOpEI); + for (int i = 0; i < 3; i++) + { + auto& pprIeJepXIipXI = pprIeJepXIpXI->at(i); + auto& prIeJepXIi = prIeJepXI->at(i); + for (int j = 0; j < 3; j++) + { + auto element = (i == j) ? 1.0 : 0.0; + element -= prIeJepXIi * prIeJepXI->at(j); + pprIeJepXIipXI->atiput(j, element / rIeJe); + } + } + pprIeJepXIpEI = std::make_shared>(3, 4); + for (int i = 0; i < 3; i++) + { + auto& pprIeJepXIipEI = pprIeJepXIpEI->at(i); + auto& prIeJepXIi = prIeJepXI->at(i); + auto& mprIeJeOipEI = mprIeJeOpEI->at(i); + for (int j = 0; j < 4; j++) + { + auto element = mprIeJeOipEI->at(j) - prIeJepXIi * prIeJepEI->at(j); + pprIeJepXIipEI->atiput(j, element / rIeJe); + } + } + pprIeJepEIpEI = std::make_shared>(4, 4); + for (int i = 0; i < 4; i++) + { + auto& pprIeJepEIipEI = pprIeJepEIpEI->at(i); + auto& prIeJepEIi = prIeJepEI->at(i); + auto& mpprIeJeOpEIipEI = mpprIeJeOpEIpEI->at(i); + auto& mprIeJeOpEIiT = mprIeJeOpEIT->at(i); + for (int j = 0; j < 4; j++) + { + auto element = mprIeJeOpEIiT->dot(mprIeJeOpEIT->at(j)) + - mpprIeJeOpEIipEI->at(j)->dot(rIeJeO) - prIeJepEIi * prIeJepEI->at(j); + pprIeJepEIipEI->atiput(j, element / rIeJe); + } + } +} + +void MbD::DistIeqcJec::initialize() +{ + DistIecJec::initialize(); + prIeJepXI = std::make_shared>(3); + prIeJepEI = std::make_shared>(4); + pprIeJepXIpXI = std::make_shared>(3, 3); + pprIeJepXIpEI = std::make_shared>(3, 4); + pprIeJepEIpEI = std::make_shared>(4, 4); +} + +FMatDsptr MbD::DistIeqcJec::ppvaluepEIpEI() +{ + return pprIeJepEIpEI; +} + +FMatDsptr MbD::DistIeqcJec::ppvaluepXIpEI() +{ + return pprIeJepXIpEI; +} + +FMatDsptr MbD::DistIeqcJec::ppvaluepXIpXI() +{ + return pprIeJepXIpXI; +} + +FRowDsptr MbD::DistIeqcJec::pvaluepEI() +{ + return prIeJepEI; +} + +FRowDsptr MbD::DistIeqcJec::pvaluepXI() +{ + return prIeJepXI; +} diff --git a/MbDCode/DistIeqcJec.h b/MbDCode/DistIeqcJec.h new file mode 100644 index 0000000..f85163f --- /dev/null +++ b/MbDCode/DistIeqcJec.h @@ -0,0 +1,25 @@ +#pragma once + +#include "DistIecJec.h" + +namespace MbD { + class DistIeqcJec : public DistIecJec + { + //prIeJepXI prIeJepEI pprIeJepXIpXI pprIeJepXIpEI pprIeJepEIpEI mprIeJeOpEIT + public: + DistIeqcJec(); + DistIeqcJec(EndFrmcptr frmi, EndFrmcptr frmj); + + void calcPrivate() override; + void initialize() override; + FMatDsptr ppvaluepEIpEI(); + FMatDsptr ppvaluepXIpEI(); + FMatDsptr ppvaluepXIpXI(); + FRowDsptr pvaluepEI(); + FRowDsptr pvaluepXI(); + + FRowDsptr prIeJepXI, prIeJepEI; + FMatDsptr pprIeJepXIpXI, pprIeJepXIpEI, pprIeJepEIpEI, mprIeJeOpEIT; + }; +} + diff --git a/MbDCode/DistIeqcJeqc.cpp b/MbDCode/DistIeqcJeqc.cpp new file mode 100644 index 0000000..5810c7d --- /dev/null +++ b/MbDCode/DistIeqcJeqc.cpp @@ -0,0 +1,169 @@ +#include "DistIeqcJeqc.h" +#include "EndFrameqc.h" + +using namespace MbD; + +MbD::DistIeqcJeqc::DistIeqcJeqc() +{ +} + +MbD::DistIeqcJeqc::DistIeqcJeqc(EndFrmcptr frmi, EndFrmcptr frmj) : DistIeqcJec(frmi, frmj) +{ +} + +void MbD::DistIeqcJeqc::calcPrivate() +{ + DistIeqcJec::calcPrivate(); + if (rIeJe == 0.0) return; + auto frmJeqc = std::static_pointer_cast(frmJ); + auto& prIeJeOpEJ = frmJeqc->prOeOpE; + auto prIeJeOpEJT = prIeJeOpEJ->transpose(); + auto& pprIeJeOpEJpEJ = frmJeqc->pprOeOpEpE; + auto uIeJeOT = uIeJeO->transpose(); + prIeJepXJ = uIeJeOT; + prIeJepEJ = uIeJeOT->timesFullMatrix(prIeJeOpEJ); + for (int i = 0; i < 3; i++) + { + auto& pprIeJepXIipXJ = pprIeJepXIpXJ->at(i); + auto& prIeJepXIi = prIeJepXI->at(i); + for (int j = 0; j < 3; j++) + { + auto element = (i == j) ? -1.0 : 0.0; + element -= prIeJepXIi * prIeJepXJ->at(j); + pprIeJepXIipXJ->atiput(j, element / rIeJe); + } + } + + for (int i = 0; i < 4; i++) + { + auto& pprIeJepEIipXJ = pprIeJepEIpXJ->at(i); + auto& prIeJepEIi = prIeJepEI->at(i); + auto& mprIeJeOpEIiT = mprIeJeOpEIT->at(i); + for (int j = 0; j < 3; j++) + { + auto element = 0.0 - mprIeJeOpEIiT->at(j) - prIeJepEIi * prIeJepXJ->at(j); + pprIeJepEIipXJ->atiput(j, element / rIeJe); + } + } + + for (int i = 0; i < 3; i++) + { + auto& pprIeJepXJipXJ = pprIeJepXJpXJ->at(i); + auto& prIeJepXJi = prIeJepXJ->at(i); + for (int j = 0; j < 3; j++) + { + auto element = (i == j) ? 1.0 : 0.0; + element -= prIeJepXJi * prIeJepXJ->at(j); + pprIeJepXJipXJ->atiput(j, element / rIeJe); + } + } + + for (int i = 0; i < 3; i++) + { + auto& pprIeJepXIipEJ = pprIeJepXIpEJ->at(i); + auto& prIeJepXIi = prIeJepXI->at(i); + auto& prIeJeOipEJ = prIeJeOpEJ->at(i); + for (int j = 0; j < 4; j++) + { + auto element = 0.0 - prIeJeOipEJ->at(j) - prIeJepXIi * prIeJepEJ->at(j); + pprIeJepXIipEJ->atiput(j, element / rIeJe); + } + } + + for (int i = 0; i < 4; i++) + { + auto& pprIeJepEIipEJ = pprIeJepEIpEJ->at(i); + auto& prIeJepEIi = prIeJepEI->at(i); + auto& mprIeJeOpEIiT = mprIeJeOpEIT->at(i); + for (int j = 0; j < 4; j++) + { + auto element = 0.0 - mprIeJeOpEIiT->dot(prIeJeOpEJT->at(j)) - prIeJepEIi * prIeJepEJ->at(j); + pprIeJepEIipEJ->atiput(j, element / rIeJe); + } + } + + for (int i = 0; i < 3; i++) + { + auto& pprIeJepXJipEJ = pprIeJepXJpEJ->at(i); + auto& prIeJepXJi = prIeJepXJ->at(i); + auto& prIeJeOipEJ = prIeJeOpEJ->at(i); + for (int j = 0; j < 4; j++) + { + auto element = prIeJeOipEJ->at(j) - prIeJepXJi * prIeJepEJ->at(j); + pprIeJepXJipEJ->atiput(j, element / rIeJe); + } + } + + for (int i = 0; i < 4; i++) + { + auto& pprIeJepEJipEJ = pprIeJepEJpEJ->at(i); + auto& prIeJepEJi = prIeJepEJ->at(i); + auto& pprIeJeOpEJipEJ = pprIeJeOpEJpEJ->at(i); + auto& prIeJeOpEJiT = prIeJeOpEJT->at(i); + for (int j = 0; j < 4; j++) + { + auto element = prIeJeOpEJiT->dot(prIeJeOpEJT->at(j)) + + pprIeJeOpEJipEJ->at(j)->dot(rIeJeO) - prIeJepEJi * prIeJepEJ->at(j); + pprIeJepEJipEJ->atiput(j, element / rIeJe); + } + } +} + +void MbD::DistIeqcJeqc::initialize() +{ + DistIeqcJec::initialize(); + prIeJepXJ = std::make_shared>(3); + prIeJepEJ = std::make_shared>(4); + pprIeJepXIpXJ = std::make_shared>(3, 3); + pprIeJepEIpXJ = std::make_shared>(4, 3); + pprIeJepXJpXJ = std::make_shared>(3, 3); + pprIeJepXIpEJ = std::make_shared>(3, 4); + pprIeJepEIpEJ = std::make_shared>(4, 4); + pprIeJepXJpEJ = std::make_shared>(3, 4); + pprIeJepEJpEJ = std::make_shared>(4, 4); +} + +FMatDsptr MbD::DistIeqcJeqc::ppvaluepEIpEJ() +{ + return pprIeJepEIpEJ; +} + +FMatDsptr MbD::DistIeqcJeqc::ppvaluepEIpXJ() +{ + return pprIeJepEIpXJ; +} + +FMatDsptr MbD::DistIeqcJeqc::ppvaluepEJpEJ() +{ + return pprIeJepEJpEJ; +} + +FMatDsptr MbD::DistIeqcJeqc::ppvaluepXIpEJ() +{ + return pprIeJepXIpEJ; +} + +FMatDsptr MbD::DistIeqcJeqc::ppvaluepXIpXJ() +{ + return pprIeJepXIpXJ; +} + +FMatDsptr MbD::DistIeqcJeqc::ppvaluepXJpEJ() +{ + return pprIeJepXJpEJ; +} + +FMatDsptr MbD::DistIeqcJeqc::ppvaluepXJpXJ() +{ + return pprIeJepXJpXJ; +} + +FRowDsptr MbD::DistIeqcJeqc::pvaluepEJ() +{ + return prIeJepEJ; +} + +FRowDsptr MbD::DistIeqcJeqc::pvaluepXJ() +{ + return prIeJepXJ; +} diff --git a/MbDCode/DistIeqcJeqc.h b/MbDCode/DistIeqcJeqc.h new file mode 100644 index 0000000..26e0585 --- /dev/null +++ b/MbDCode/DistIeqcJeqc.h @@ -0,0 +1,29 @@ +#pragma once + +#include "DistIeqcJec.h" + +namespace MbD { + class DistIeqcJeqc : public DistIeqcJec + { + //prIeJepXJ prIeJepEJ pprIeJepXIpXJ pprIeJepEIpXJ pprIeJepXJpXJ pprIeJepXIpEJ pprIeJepEIpEJ pprIeJepXJpEJ pprIeJepEJpEJ prIeJeOpEJT + public: + DistIeqcJeqc(); + DistIeqcJeqc(EndFrmcptr frmi, EndFrmcptr frmj); + + void calcPrivate() override; + void initialize() override; + FMatDsptr ppvaluepEIpEJ(); + FMatDsptr ppvaluepEIpXJ(); + FMatDsptr ppvaluepEJpEJ(); + FMatDsptr ppvaluepXIpEJ(); + FMatDsptr ppvaluepXIpXJ(); + FMatDsptr ppvaluepXJpEJ(); + FMatDsptr ppvaluepXJpXJ(); + FRowDsptr pvaluepEJ(); + FRowDsptr pvaluepXJ(); + + FRowDsptr prIeJepXJ, prIeJepEJ; + FMatDsptr pprIeJepXIpXJ, pprIeJepEIpXJ, pprIeJepXJpXJ, pprIeJepXIpEJ, pprIeJepEIpEJ, pprIeJepXJpEJ, pprIeJepEJpEJ, prIeJeOpEJT; + }; +} + diff --git a/MbDCode/DistIeqctJeqc.cpp b/MbDCode/DistIeqctJeqc.cpp new file mode 100644 index 0000000..ad5ed2e --- /dev/null +++ b/MbDCode/DistIeqctJeqc.cpp @@ -0,0 +1 @@ +#include "DistIeqctJeqc.h" diff --git a/MbDCode/DistIeqctJeqc.h b/MbDCode/DistIeqctJeqc.h new file mode 100644 index 0000000..8be73fb --- /dev/null +++ b/MbDCode/DistIeqctJeqc.h @@ -0,0 +1,14 @@ +#pragma once + +#include "DistIeqcJeqc.h" + +namespace MbD { + class DistIeqctJeqc : public DistIeqcJeqc + { + //prIeJept pprIeJepXIpt pprIeJepEIpt pprIeJepXJpt pprIeJepEJpt pprIeJeptpt + public: + //ToDo: add member functions. + + }; +} + diff --git a/MbDCode/DistanceConstraintIJ.cpp b/MbDCode/DistanceConstraintIJ.cpp new file mode 100644 index 0000000..4bd0467 --- /dev/null +++ b/MbDCode/DistanceConstraintIJ.cpp @@ -0,0 +1,69 @@ +#include "DistanceConstraintIJ.h" + +using namespace MbD; + +MbD::DistanceConstraintIJ::DistanceConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj) : ConstraintIJ(frmi, frmj) +{ +} + +void MbD::DistanceConstraintIJ::calcPostDynCorrectorIteration() +{ +aG = distIeJe->value() - aConstant; +} + +void MbD::DistanceConstraintIJ::initialize() +{ + ConstraintIJ::initialize(); + this->init_distIeJe(); +} + +void MbD::DistanceConstraintIJ::initializeGlobally() +{ + distIeJe->initializeGlobally(); +} + +void MbD::DistanceConstraintIJ::initializeLocally() +{ + distIeJe->initializeLocally(); +} + +void MbD::DistanceConstraintIJ::postInput() +{ + distIeJe->postInput(); + ConstraintIJ::postInput(); +} + +void MbD::DistanceConstraintIJ::postPosICIteration() +{ + distIeJe->postPosICIteration(); + ConstraintIJ::postPosICIteration(); +} + +void MbD::DistanceConstraintIJ::preAccIC() +{ + distIeJe->preAccIC(); + ConstraintIJ::preAccIC(); +} + +void MbD::DistanceConstraintIJ::prePosIC() +{ + distIeJe->prePosIC(); + ConstraintIJ::prePosIC(); +} + +void MbD::DistanceConstraintIJ::preVelIC() +{ + distIeJe->preVelIC(); + ConstraintIJ::preVelIC(); +} + +void MbD::DistanceConstraintIJ::simUpdateAll() +{ + distIeJe->simUpdateAll(); + ConstraintIJ::simUpdateAll(); +} + +ConstraintType MbD::DistanceConstraintIJ::type() +{ + return ConstraintType::displacement; +} diff --git a/MbDCode/DistanceConstraintIJ.h b/MbDCode/DistanceConstraintIJ.h new file mode 100644 index 0000000..2705d23 --- /dev/null +++ b/MbDCode/DistanceConstraintIJ.h @@ -0,0 +1,30 @@ +#pragma once + +#include "ConstraintIJ.h" +#include "DistIecJec.h" + +namespace MbD { + class DistanceConstraintIJ : public ConstraintIJ + { + //distIeJe + public: + DistanceConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj); + + void calcPostDynCorrectorIteration() override; + virtual void init_distIeJe() = 0; + void initialize() override; + void initializeGlobally() override; + void initializeLocally() override; + void postInput() override; + void postPosICIteration() override; + void preAccIC() override; + void prePosIC() override; + void preVelIC() override; + void simUpdateAll() override; + ConstraintType type() override; + + std::shared_ptr distIeJe; + + }; +} + diff --git a/MbDCode/DistanceConstraintIqcJc.cpp b/MbDCode/DistanceConstraintIqcJc.cpp new file mode 100644 index 0000000..582caf5 --- /dev/null +++ b/MbDCode/DistanceConstraintIqcJc.cpp @@ -0,0 +1,102 @@ +#include "DistanceConstraintIqcJc.h" +#include "EndFrameqc.h" +#include "CREATE.h" +#include "DistIeqcJec.h" + +MbD::DistanceConstraintIqcJc::DistanceConstraintIqcJc(EndFrmcptr frmi, EndFrmcptr frmj) : DistanceConstraintIJ(frmi, frmj) +{ +} + +void MbD::DistanceConstraintIqcJc::addToJointForceI(FColDsptr col) +{ + col->equalSelfPlusFullVectortimes(pGpXI, lam); +} + +void MbD::DistanceConstraintIqcJc::addToJointTorqueI(FColDsptr jointTorque) +{ + auto cForceT = pGpXI->times(lam); + auto frmIeqc = std::static_pointer_cast(frmI); + auto rIpIeIp = frmIeqc->rpep(); + auto pAOIppEI = frmIeqc->pAOppE(); + auto aBOIp = frmIeqc->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); +} + +void MbD::DistanceConstraintIqcJc::calcPostDynCorrectorIteration() +{ + DistanceConstraintIJ::calcPostDynCorrectorIteration(); + pGpXI = distIeJe->pvaluepXI(); + pGpEI = distIeJe->pvaluepEI(); + ppGpXIpXI = distIeJe->ppvaluepXIpXI(); + ppGpXIpEI = distIeJe->ppvaluepXIpEI(); + ppGpEIpEI = distIeJe->ppvaluepEIpEI(); +} + +void MbD::DistanceConstraintIqcJc::fillAccICIterError(FColDsptr col) +{ + col->atiplusFullVectortimes(iqXI, pGpXI, lam); + col->atiplusFullVectortimes(iqEI, pGpEI, lam); + auto efrmIqc = std::static_pointer_cast(frmI); + auto qXdotI = efrmIqc->qXdot(); + auto qEdotI = efrmIqc->qEdot(); + auto sum = pGpXI->timesFullColumn(efrmIqc->qXddot()); + sum += pGpEI->timesFullColumn(efrmIqc->qEddot()); + sum += qXdotI->transposeTimesFullColumn(ppGpXIpXI->timesFullColumn(qXdotI)); + sum += 2.0 * (qXdotI->transposeTimesFullColumn(ppGpXIpEI->timesFullColumn(qEdotI))); + sum += qEdotI->transposeTimesFullColumn(ppGpEIpEI->timesFullColumn(qEdotI)); + col->atiplusNumber(iG, sum); +} + +void MbD::DistanceConstraintIqcJc::fillPosICError(FColDsptr col) +{ + DistanceConstraintIJ::fillPosICError(col); + col->atiplusFullVectortimes(iqXI, pGpXI, lam); + col->atiplusFullVectortimes(iqEI, pGpEI, lam); +} + +void MbD::DistanceConstraintIqcJc::fillPosICJacob(SpMatDsptr mat) +{ + mat->atijplusFullRow(iG, iqXI, pGpXI); + mat->atijplusFullColumn(iqXI, iG, pGpXI->transpose()); + mat->atijplusFullRow(iG, iqEI, pGpEI); + mat->atijplusFullColumn(iqEI, iG, pGpEI->transpose()); + mat->atijplusFullMatrixtimes(iqXI, iqXI, ppGpXIpXI, lam); + auto ppGpXIpEIlam = ppGpXIpEI->times(lam); + mat->atijplusFullMatrix(iqXI, iqEI, ppGpXIpEIlam); + mat->atijplusTransposeFullMatrix(iqEI, iqXI, ppGpXIpEIlam); + mat->atijplusFullMatrixtimes(iqEI, iqEI, ppGpEIpEI, lam); +} + +void MbD::DistanceConstraintIqcJc::fillPosKineJacob(SpMatDsptr mat) +{ + mat->atijplusFullRow(iG, iqXI, pGpXI); + mat->atijplusFullRow(iG, iqEI, pGpEI); +} + +void MbD::DistanceConstraintIqcJc::fillVelICJacob(SpMatDsptr mat) +{ + mat->atijplusFullRow(iG, iqXI, pGpXI); + mat->atijplusFullColumn(iqXI, iG, pGpXI->transpose()); + mat->atijplusFullRow(iG, iqEI, pGpEI); + mat->atijplusFullColumn(iqEI, iG, pGpEI->transpose()); +} + +void MbD::DistanceConstraintIqcJc::init_distIeJe() +{ + distIeJe = CREATE::With(frmI, frmJ); +} + +void MbD::DistanceConstraintIqcJc::useEquationNumbers() +{ + auto frmIeqc = std::static_pointer_cast(frmI); + iqXI = frmIeqc->iqX(); + iqEI = frmIeqc->iqE(); +} diff --git a/MbDCode/DistanceConstraintIqcJc.h b/MbDCode/DistanceConstraintIqcJc.h new file mode 100644 index 0000000..3a6ecac --- /dev/null +++ b/MbDCode/DistanceConstraintIqcJc.h @@ -0,0 +1,28 @@ +#pragma once + +#include "DistanceConstraintIJ.h" + +namespace MbD { + class DistanceConstraintIqcJc : public DistanceConstraintIJ + { + //pGpXI pGpEI ppGpXIpXI ppGpXIpEI ppGpEIpEI iqXI iqEI + public: + DistanceConstraintIqcJc(EndFrmcptr frmi, EndFrmcptr frmj); + + void addToJointForceI(FColDsptr col) override; + void addToJointTorqueI(FColDsptr col) override; + void calcPostDynCorrectorIteration() override; + void fillAccICIterError(FColDsptr col) override; + void fillPosICError(FColDsptr col) override; + void fillPosICJacob(SpMatDsptr mat) override; + void fillPosKineJacob(SpMatDsptr mat) override; + void fillVelICJacob(SpMatDsptr mat) override; + void init_distIeJe() override; + void useEquationNumbers() override; + + FRowDsptr pGpXI, pGpEI; + FMatDsptr ppGpXIpXI, ppGpXIpEI, ppGpEIpEI; + int iqXI, iqEI; + }; +} + diff --git a/MbDCode/DistanceConstraintIqcJqc.cpp b/MbDCode/DistanceConstraintIqcJqc.cpp new file mode 100644 index 0000000..4a1f5b6 --- /dev/null +++ b/MbDCode/DistanceConstraintIqcJqc.cpp @@ -0,0 +1,108 @@ +#include "DistanceConstraintIqcJqc.h" +#include "EndFrameqc.h" +#include "CREATE.h" +#include "DistIeqcJeqc.h" + +MbD::DistanceConstraintIqcJqc::DistanceConstraintIqcJqc(EndFrmcptr frmi, EndFrmcptr frmj) : DistanceConstraintIqcJc(frmi, frmj) +{ +} + +void MbD::DistanceConstraintIqcJqc::calcPostDynCorrectorIteration() +{ + DistanceConstraintIqcJc::calcPostDynCorrectorIteration(); + pGpXJ = distIeJe->pvaluepXJ(); + pGpEJ = distIeJe->pvaluepEJ(); + ppGpXIpXJ = distIeJe->ppvaluepXIpXJ(); + ppGpEIpXJ = distIeJe->ppvaluepEIpXJ(); + ppGpXJpXJ = distIeJe->ppvaluepXJpXJ(); + ppGpXIpEJ = distIeJe->ppvaluepXIpEJ(); + ppGpEIpEJ = distIeJe->ppvaluepEIpEJ(); + ppGpXJpEJ = distIeJe->ppvaluepXJpEJ(); + ppGpEJpEJ = distIeJe->ppvaluepEJpEJ(); +} + +void MbD::DistanceConstraintIqcJqc::fillAccICIterError(FColDsptr col) +{ + DistanceConstraintIqcJc::fillAccICIterError(col); + col->atiplusFullVectortimes(iqXJ, pGpXJ, lam); + col->atiplusFullVectortimes(iqEJ, pGpEJ, lam); + auto frmIeqc = std::static_pointer_cast(frmI); + auto frmJeqc = std::static_pointer_cast(frmJ); + auto qXdotI = frmIeqc->qXdot(); + auto qEdotI = frmIeqc->qEdot(); + auto qXdotJ = frmJeqc->qXdot(); + auto qEdotJ = frmJeqc->qEdot(); + double sum = 0.0; + sum += pGpXJ->timesFullColumn(frmJeqc->qXddot()); + sum += pGpEJ->timesFullColumn(frmJeqc->qEddot()); + sum += 2.0 * (qXdotI->transposeTimesFullColumn(ppGpXIpXJ->timesFullColumn(qXdotJ))); + sum += 2.0 * (qEdotI->transposeTimesFullColumn(ppGpEIpXJ->timesFullColumn(qXdotJ))); + sum += qXdotJ->transposeTimesFullColumn(ppGpXJpXJ->timesFullColumn(qXdotJ)); + sum += 2.0 * (qXdotI->transposeTimesFullColumn(ppGpXIpEJ->timesFullColumn(qEdotJ))); + sum += 2.0 * (qEdotI->transposeTimesFullColumn(ppGpEIpEJ->timesFullColumn(qEdotJ))); + sum += 2.0 * (qXdotJ->transposeTimesFullColumn(ppGpXJpEJ->timesFullColumn(qEdotJ))); + sum += qEdotJ->transposeTimesFullColumn(ppGpEJpEJ->timesFullColumn(qEdotJ)); + col->atiplusNumber(iG, sum); +} + +void MbD::DistanceConstraintIqcJqc::fillPosICError(FColDsptr col) +{ + DistanceConstraintIqcJc::fillPosICError(col); + col->atiplusFullVectortimes(iqXJ, pGpXJ, lam); + col->atiplusFullVectortimes(iqEJ, pGpEJ, lam); +} + +void MbD::DistanceConstraintIqcJqc::fillPosICJacob(SpMatDsptr mat) +{ + DistanceConstraintIqcJc::fillPosICJacob(mat); + mat->atijplusFullRow(iG, iqXJ, pGpXJ); + mat->atijplusFullColumn(iqXJ, iG, pGpXJ->transpose()); + mat->atijplusFullRow(iG, iqEJ, pGpEJ); + mat->atijplusFullColumn(iqEJ, iG, pGpEJ->transpose()); + auto ppGpXIpXJlam = ppGpXIpXJ->times(lam); + mat->atijplusFullMatrix(iqXI, iqXJ, ppGpXIpXJlam); + mat->atijplusTransposeFullMatrix(iqXJ, iqXI, ppGpXIpXJlam); + auto ppGpEIpXJlam = ppGpEIpXJ->times(lam); + mat->atijplusFullMatrix(iqEI, iqXJ, ppGpEIpXJlam); + mat->atijplusTransposeFullMatrix(iqXJ, iqEI, ppGpEIpXJlam); + mat->atijplusFullMatrixtimes(iqXJ, iqXJ, ppGpXJpXJ, lam); + auto ppGpXIpEJlam = ppGpXIpEJ->times(lam); + mat->atijplusFullMatrix(iqXI, iqEJ, ppGpXIpEJlam); + mat->atijplusTransposeFullMatrix(iqEJ, iqXI, ppGpXIpEJlam); + auto ppGpEIpEJlam = ppGpEIpEJ->times(lam); + mat->atijplusFullMatrix(iqEI, iqEJ, ppGpEIpEJlam); + mat->atijplusTransposeFullMatrix(iqEJ, iqEI, ppGpEIpEJlam); + auto ppGpXJpEJlam = ppGpXJpEJ->times(lam); + mat->atijplusFullMatrix(iqXJ, iqEJ, ppGpXJpEJlam); + mat->atijplusTransposeFullMatrix(iqEJ, iqXJ, ppGpXJpEJlam); + mat->atijplusFullMatrixtimes(iqEJ, iqEJ, ppGpEJpEJ, lam); +} + +void MbD::DistanceConstraintIqcJqc::fillPosKineJacob(SpMatDsptr mat) +{ + DistanceConstraintIqcJc::fillPosKineJacob(mat); + mat->atijplusFullRow(iG, iqXJ, pGpXJ); + mat->atijplusFullRow(iG, iqEJ, pGpEJ); +} + +void MbD::DistanceConstraintIqcJqc::fillVelICJacob(SpMatDsptr mat) +{ + DistanceConstraintIqcJc::fillVelICJacob(mat); + mat->atijplusFullRow(iG, iqXJ, pGpXJ); + mat->atijplusFullColumn(iqXJ, iG, pGpXJ->transpose()); + mat->atijplusFullRow(iG, iqEJ, pGpEJ); + mat->atijplusFullColumn(iqEJ, iG, pGpEJ->transpose()); +} + +void MbD::DistanceConstraintIqcJqc::init_distIeJe() +{ + distIeJe = CREATE::With(frmI, frmJ); +} + +void MbD::DistanceConstraintIqcJqc::useEquationNumbers() +{ + DistanceConstraintIqcJc::useEquationNumbers(); + auto frmJeqc = std::static_pointer_cast(frmJ); + iqXJ = frmJeqc->iqX(); + iqEJ = frmJeqc->iqE(); +} diff --git a/MbDCode/DistanceConstraintIqcJqc.h b/MbDCode/DistanceConstraintIqcJqc.h new file mode 100644 index 0000000..105cdcf --- /dev/null +++ b/MbDCode/DistanceConstraintIqcJqc.h @@ -0,0 +1,26 @@ +#pragma once + +#include "DistanceConstraintIqcJc.h" + +namespace MbD { + class DistanceConstraintIqcJqc : public DistanceConstraintIqcJc + { + //pGpXJ pGpEJ ppGpXIpXJ ppGpEIpXJ ppGpXJpXJ ppGpXIpEJ ppGpEIpEJ ppGpXJpEJ ppGpEJpEJ iqXJ iqEJ + public: + DistanceConstraintIqcJqc(EndFrmcptr frmi, EndFrmcptr frmj); + + void calcPostDynCorrectorIteration() override; + void fillAccICIterError(FColDsptr col) override; + void fillPosICError(FColDsptr col) override; + void fillPosICJacob(SpMatDsptr mat) override; + void fillPosKineJacob(SpMatDsptr mat) override; + void fillVelICJacob(SpMatDsptr mat) override; + void init_distIeJe() override; + void useEquationNumbers() override; + + FRowDsptr pGpXJ, pGpEJ; + FMatDsptr ppGpXIpXJ, ppGpEIpXJ, ppGpXJpXJ, ppGpXIpEJ, ppGpEIpEJ, ppGpXJpEJ, ppGpEJpEJ; + int iqXJ, iqEJ; + }; +} + diff --git a/MbDCode/DistanceConstraintIqctJqc.cpp b/MbDCode/DistanceConstraintIqctJqc.cpp new file mode 100644 index 0000000..84941b9 --- /dev/null +++ b/MbDCode/DistanceConstraintIqctJqc.cpp @@ -0,0 +1,6 @@ +#include "DistanceConstraintIqctJqc.h" + +MbD::DistanceConstraintIqctJqc::DistanceConstraintIqctJqc(EndFrmcptr frmi, EndFrmcptr frmj) : DistanceConstraintIqcJqc(frmi, frmj) +{ + assert(false); +} diff --git a/MbDCode/DistanceConstraintIqctJqc.h b/MbDCode/DistanceConstraintIqctJqc.h new file mode 100644 index 0000000..a747d04 --- /dev/null +++ b/MbDCode/DistanceConstraintIqctJqc.h @@ -0,0 +1,17 @@ +#pragma once + +#include "DistanceConstraintIqcJqc.h" + +namespace MbD { + class DistanceConstraintIqctJqc : public DistanceConstraintIqcJqc + { + //pGpt ppGpXIpt ppGpEIpt ppGpXJpt ppGpEJpt ppGptpt + public: + DistanceConstraintIqctJqc(EndFrmcptr frmi, EndFrmcptr frmj); + + double pGpt, ppGptpt; + FRowDsptr ppGpXIpt, ppGpEIpt, ppGpXJpt, ppGpEJpt; + + }; +} + diff --git a/MbDCode/DistancexyConstraintIJ.cpp b/MbDCode/DistancexyConstraintIJ.cpp new file mode 100644 index 0000000..0a45f2a --- /dev/null +++ b/MbDCode/DistancexyConstraintIJ.cpp @@ -0,0 +1,79 @@ +#include "DistancexyConstraintIJ.h" + +using namespace MbD; + +MbD::DistancexyConstraintIJ::DistancexyConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj) : ConstraintIJ(frmi, frmj) +{ +} + +void MbD::DistancexyConstraintIJ::calcPostDynCorrectorIteration() +{ + auto x = xIeJeIe->value(); + auto y = yIeJeIe->value(); + aG = x * x + (y * y) - (aConstant * aConstant); +} + +void MbD::DistancexyConstraintIJ::initialize() +{ + ConstraintIJ::initialize(); + this->init_xyIeJeIe(); +} + +void MbD::DistancexyConstraintIJ::initializeGlobally() +{ + xIeJeIe->initializeGlobally(); + yIeJeIe->initializeGlobally(); +} + +void MbD::DistancexyConstraintIJ::initializeLocally() +{ + xIeJeIe->initializeLocally(); + yIeJeIe->initializeLocally(); +} + +void MbD::DistancexyConstraintIJ::postInput() +{ + xIeJeIe->postInput(); + yIeJeIe->postInput(); + ConstraintIJ::postInput(); +} + +void MbD::DistancexyConstraintIJ::postPosICIteration() +{ + xIeJeIe->postPosICIteration(); + yIeJeIe->postPosICIteration(); + ConstraintIJ::postPosICIteration(); +} + +void MbD::DistancexyConstraintIJ::preAccIC() +{ + xIeJeIe->preAccIC(); + yIeJeIe->preAccIC(); + ConstraintIJ::preAccIC(); +} + +void MbD::DistancexyConstraintIJ::prePosIC() +{ + xIeJeIe->prePosIC(); + yIeJeIe->prePosIC(); + ConstraintIJ::prePosIC(); +} + +void MbD::DistancexyConstraintIJ::preVelIC() +{ + xIeJeIe->preVelIC(); + yIeJeIe->preVelIC(); + ConstraintIJ::preVelIC(); +} + +void MbD::DistancexyConstraintIJ::simUpdateAll() +{ + xIeJeIe->simUpdateAll(); + yIeJeIe->simUpdateAll(); + ConstraintIJ::simUpdateAll(); +} + +ConstraintType MbD::DistancexyConstraintIJ::type() +{ + return ConstraintType::displacement; +} diff --git a/MbDCode/DistancexyConstraintIJ.h b/MbDCode/DistancexyConstraintIJ.h new file mode 100644 index 0000000..73f6f79 --- /dev/null +++ b/MbDCode/DistancexyConstraintIJ.h @@ -0,0 +1,32 @@ +#pragma once + +#include "ConstraintIJ.h" +#include "DispCompIecJecIe.h" + +namespace MbD { + class DistancexyConstraintIJ : public ConstraintIJ + { + //xIeJeIe yIeJeIe + public: + DistancexyConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj); + + void calcPostDynCorrectorIteration() override; + virtual void init_xyIeJeIe() = 0; + void initialize() override; + void initializeGlobally() override; + void initializeLocally() override; + void postInput() override; + void postPosICIteration() override; + void preAccIC() override; + void prePosIC() override; + void preVelIC() override; + void simUpdateAll() override; + ConstraintType type() override; + + + + std::shared_ptr xIeJeIe, yIeJeIe; + + }; +} + diff --git a/MbDCode/DistancexyConstraintIqcJc.cpp b/MbDCode/DistancexyConstraintIqcJc.cpp new file mode 100644 index 0000000..9fa988a --- /dev/null +++ b/MbDCode/DistancexyConstraintIqcJc.cpp @@ -0,0 +1,144 @@ +#include "DistancexyConstraintIqcJc.h" +#include "EndFramec.h" +#include "CREATE.h" +#include "DispCompIeqcJecIe.h" + +using namespace MbD; + +MbD::DistancexyConstraintIqcJc::DistancexyConstraintIqcJc(EndFrmcptr frmi, EndFrmcptr frmj) : DistancexyConstraintIJ(frmi, frmj) +{ +} + +void MbD::DistancexyConstraintIqcJc::addToJointForceI(FColDsptr col) +{ + col->equalSelfPlusFullVectortimes(pGpXI, lam); +} + +void MbD::DistancexyConstraintIqcJc::addToJointTorqueI(FColDsptr jointTorque) +{ + auto cForceT = pGpXI->times(lam); + auto frmIeqc = std::static_pointer_cast(frmI); + auto rIpIeIp = frmIeqc->rpep(); + auto pAOIppEI = frmIeqc->pAOppE(); + auto aBOIp = frmIeqc->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); +} + +void MbD::DistancexyConstraintIqcJc::calcPostDynCorrectorIteration() +{ + DistancexyConstraintIJ::calcPostDynCorrectorIteration(); + this->calc_pGpXI(); + this->calc_pGpEI(); + this->calc_ppGpXIpXI(); + this->calc_ppGpXIpEI(); + this->calc_ppGpEIpEI(); +} + +void MbD::DistancexyConstraintIqcJc::calc_pGpEI() +{ + pGpEI = (xIeJeIe->pvaluepEI()->times(xIeJeIe->value())->plusFullRow(yIeJeIe->pvaluepEI()->times(yIeJeIe->value()))); + pGpEI->magnifySelf(2.0); +} + +void MbD::DistancexyConstraintIqcJc::calc_pGpXI() +{ + pGpXI = (xIeJeIe->pvaluepXI()->times(xIeJeIe->value())->plusFullRow(yIeJeIe->pvaluepXI()->times(yIeJeIe->value()))); + pGpXI->magnifySelf(2.0); +} + +void MbD::DistancexyConstraintIqcJc::calc_ppGpEIpEI() +{ + ppGpEIpEI = (xIeJeIe->pvaluepEI()->transposeTimesFullRow(xIeJeIe->pvaluepEI())); + ppGpEIpEI = ppGpEIpEI->plusFullMatrix(xIeJeIe->ppvaluepEIpEI()->times(xIeJeIe->value())); + ppGpEIpEI = ppGpEIpEI->plusFullMatrix(yIeJeIe->pvaluepEI()->transposeTimesFullRow(yIeJeIe->pvaluepEI())); + ppGpEIpEI = ppGpEIpEI->plusFullMatrix(yIeJeIe->ppvaluepEIpEI()->times(yIeJeIe->value())); + ppGpEIpEI->magnifySelf(2.0); +} + +void MbD::DistancexyConstraintIqcJc::calc_ppGpXIpEI() +{ + ppGpXIpEI = (xIeJeIe->pvaluepXI()->transposeTimesFullRow(xIeJeIe->pvaluepEI())); + ppGpXIpEI = ppGpXIpEI->plusFullMatrix(xIeJeIe->ppvaluepXIpEI()->times(xIeJeIe->value())); + ppGpXIpEI = ppGpXIpEI->plusFullMatrix(yIeJeIe->pvaluepXI()->transposeTimesFullRow(yIeJeIe->pvaluepEI())); + ppGpXIpEI = ppGpXIpEI->plusFullMatrix(yIeJeIe->ppvaluepXIpEI()->times(yIeJeIe->value())); + ppGpXIpEI->magnifySelf(2.0); +} + +void MbD::DistancexyConstraintIqcJc::calc_ppGpXIpXI() +{ + //xIeJeIe ppvaluepXIpXI = 0 + //yIeJeIe ppvaluepXIpXI = 0 + ppGpXIpXI = (xIeJeIe->pvaluepXI()->transposeTimesFullRow(xIeJeIe->pvaluepXI())); + ppGpXIpXI = ppGpXIpXI->plusFullMatrix(yIeJeIe->pvaluepXI()->transposeTimesFullRow(yIeJeIe->pvaluepXI())); + ppGpXIpXI->magnifySelf(2.0); +} + +void MbD::DistancexyConstraintIqcJc::init_xyIeJeIe() +{ + xIeJeIe = CREATE::With(frmI, frmJ, 0); + yIeJeIe = CREATE::With(frmI, frmJ, 1); +} + +void MbD::DistancexyConstraintIqcJc::fillAccICIterError(FColDsptr col) +{ + col->atiplusFullVectortimes(iqXI, pGpXI, lam); + col->atiplusFullVectortimes(iqEI, pGpEI, lam); + auto efrmIqc = std::static_pointer_cast(frmI); + auto qXdotI = efrmIqc->qXdot(); + auto qEdotI = efrmIqc->qEdot(); + auto sum = pGpXI->timesFullColumn(efrmIqc->qXddot()); + sum += pGpEI->timesFullColumn(efrmIqc->qEddot()); + sum += qXdotI->transposeTimesFullColumn(ppGpXIpXI->timesFullColumn(qXdotI)); + sum += 2.0 * (qXdotI->transposeTimesFullColumn(ppGpXIpEI->timesFullColumn(qEdotI))); + sum += qEdotI->transposeTimesFullColumn(ppGpEIpEI->timesFullColumn(qEdotI)); + col->atiplusNumber(iG, sum); +} + +void MbD::DistancexyConstraintIqcJc::fillPosICError(FColDsptr col) +{ + DistancexyConstraintIJ::fillPosICError(col); + col->atiplusFullVectortimes(iqXI, pGpXI, lam); + col->atiplusFullVectortimes(iqEI, pGpEI, lam); +} + +void MbD::DistancexyConstraintIqcJc::fillPosICJacob(SpMatDsptr mat) +{ + mat->atijplusFullRow(iG, iqXI, pGpXI); + mat->atijplusFullColumn(iqXI, iG, pGpXI->transpose()); + mat->atijplusFullRow(iG, iqEI, pGpEI); + mat->atijplusFullColumn(iqEI, iG, pGpEI->transpose()); + mat->atijplusFullMatrixtimes(iqXI, iqXI, ppGpXIpXI, lam); + auto ppGpXIpEIlam = ppGpXIpEI->times(lam); + mat->atijplusFullMatrix(iqXI, iqEI, ppGpXIpEIlam); + mat->atijplusTransposeFullMatrix(iqEI, iqXI, ppGpXIpEIlam); + mat->atijplusFullMatrixtimes(iqEI, iqEI, ppGpEIpEI, lam); +} + +void MbD::DistancexyConstraintIqcJc::fillPosKineJacob(SpMatDsptr mat) +{ + mat->atijplusFullRow(iG, iqXI, pGpXI); + mat->atijplusFullRow(iG, iqEI, pGpEI); +} + +void MbD::DistancexyConstraintIqcJc::fillVelICJacob(SpMatDsptr mat) +{ + mat->atijplusFullRow(iG, iqXI, pGpXI); + mat->atijplusFullColumn(iqXI, iG, pGpXI->transpose()); + mat->atijplusFullRow(iG, iqEI, pGpEI); + mat->atijplusFullColumn(iqEI, iG, pGpEI->transpose()); +} + +void MbD::DistancexyConstraintIqcJc::useEquationNumbers() +{ + auto frmIeqc = std::static_pointer_cast(frmI); + iqXI = frmIeqc->iqX(); + iqEI = frmIeqc->iqE(); +} diff --git a/MbDCode/DistancexyConstraintIqcJc.h b/MbDCode/DistancexyConstraintIqcJc.h new file mode 100644 index 0000000..9f09342 --- /dev/null +++ b/MbDCode/DistancexyConstraintIqcJc.h @@ -0,0 +1,34 @@ +#pragma once + +#include "DistancexyConstraintIJ.h" + +namespace MbD { + class DistancexyConstraintIqcJc : public DistancexyConstraintIJ + { + //pGpXI pGpEI ppGpXIpXI ppGpXIpEI ppGpEIpEI iqXI iqEI + public: + DistancexyConstraintIqcJc(EndFrmcptr frmi, EndFrmcptr frmj); + + void addToJointForceI(FColDsptr col) override; + void addToJointTorqueI(FColDsptr col) override; + void calc_pGpEI(); + void calc_pGpXI(); + void calc_ppGpEIpEI(); + void calc_ppGpXIpEI(); + void calc_ppGpXIpXI(); + void calcPostDynCorrectorIteration() override; + void init_xyIeJeIe() override; + void fillAccICIterError(FColDsptr col) override; + void fillPosICError(FColDsptr col) override; + void fillPosICJacob(SpMatDsptr mat) override; + void fillPosKineJacob(SpMatDsptr mat) override; + void fillVelICJacob(SpMatDsptr mat) override; + void useEquationNumbers() override; + + + FRowDsptr pGpXI, pGpEI; + FMatDsptr ppGpXIpXI, ppGpXIpEI, ppGpEIpEI; + int iqXI, iqEI; + }; +} + diff --git a/MbDCode/DistancexyConstraintIqcJqc.cpp b/MbDCode/DistancexyConstraintIqcJqc.cpp new file mode 100644 index 0000000..128133a --- /dev/null +++ b/MbDCode/DistancexyConstraintIqcJqc.cpp @@ -0,0 +1,182 @@ +#include "DistancexyConstraintIqcJqc.h" +#include "EndFrameqc.h" +#include "CREATE.h" +#include "DispCompIeqcJeqcIe.h" + +using namespace MbD; + +void MbD::DistancexyConstraintIqcJqc::calc_pGpXJ() +{ + pGpXJ = (xIeJeIe->pvaluepXJ()->times(xIeJeIe->value())->plusFullRow(yIeJeIe->pvaluepXJ()->times(yIeJeIe->value()))); + pGpXJ->magnifySelf(2.0); +} + +void MbD::DistancexyConstraintIqcJqc::calc_pGpEJ() +{ + pGpEJ = (xIeJeIe->pvaluepEJ()->times(xIeJeIe->value())->plusFullRow(yIeJeIe->pvaluepEJ()->times(yIeJeIe->value()))); + pGpEJ->magnifySelf(2.0); +} + +void MbD::DistancexyConstraintIqcJqc::calc_ppGpXIpXJ() +{ + //xIeJeIe ppvaluepXIpXJ = 0 + //yIeJeIe ppvaluepXIpXJ = 0 + ppGpXIpXJ = (xIeJeIe->pvaluepXI()->transposeTimesFullRow(xIeJeIe->pvaluepXJ())); + ppGpXIpXJ = ppGpXIpXJ->plusFullMatrix(yIeJeIe->pvaluepXI()->transposeTimesFullRow(yIeJeIe->pvaluepXJ())); + ppGpXIpXJ->magnifySelf(2.0); +} + +void MbD::DistancexyConstraintIqcJqc::calc_ppGpEIpXJ() +{ + ppGpEIpXJ = (xIeJeIe->pvaluepEI()->transposeTimesFullRow(xIeJeIe->pvaluepXJ())); + ppGpEIpXJ = ppGpEIpXJ->plusFullMatrix(xIeJeIe->ppvaluepEIpXJ()->times(xIeJeIe->value())); + ppGpEIpXJ = ppGpEIpXJ->plusFullMatrix(yIeJeIe->pvaluepEI()->transposeTimesFullRow(yIeJeIe->pvaluepXJ())); + ppGpEIpXJ = ppGpEIpXJ->plusFullMatrix(yIeJeIe->ppvaluepEIpXJ()->times(yIeJeIe->value())); + ppGpEIpXJ->magnifySelf(2.0); +} + +void MbD::DistancexyConstraintIqcJqc::calc_ppGpXJpXJ() +{ + //xIeJeIe ppvaluepXJpXJ = 0 + //yIeJeIe ppvaluepXJpXJ = 0 + ppGpXJpXJ = (xIeJeIe->pvaluepXJ()->transposeTimesFullRow(xIeJeIe->pvaluepXJ())); + ppGpXJpXJ = ppGpXJpXJ->plusFullMatrix(yIeJeIe->pvaluepXJ()->transposeTimesFullRow(yIeJeIe->pvaluepXJ())); + ppGpXJpXJ->magnifySelf(2.0); +} + +void MbD::DistancexyConstraintIqcJqc::calc_ppGpXIpEJ() +{ + //xIeJeIe ppvaluepXIpEJ = 0 + //yIeJeIe ppvaluepXIpEJ = 0 + ppGpXIpEJ = (xIeJeIe->pvaluepXI()->transposeTimesFullRow(xIeJeIe->pvaluepEJ())); + ppGpXIpEJ = ppGpXIpEJ->plusFullMatrix(yIeJeIe->pvaluepXI()->transposeTimesFullRow(yIeJeIe->pvaluepEJ())); + ppGpXIpEJ->magnifySelf(2.0); +} + +void MbD::DistancexyConstraintIqcJqc::calc_ppGpEIpEJ() +{ + ppGpEIpEJ = (xIeJeIe->pvaluepEI()->transposeTimesFullRow(xIeJeIe->pvaluepEJ())); + ppGpEIpEJ = ppGpEIpEJ->plusFullMatrix(xIeJeIe->ppvaluepEIpEJ()->times(xIeJeIe->value())); + ppGpEIpEJ = ppGpEIpEJ->plusFullMatrix(yIeJeIe->pvaluepEI()->transposeTimesFullRow(yIeJeIe->pvaluepEJ())); + ppGpEIpEJ = ppGpEIpEJ->plusFullMatrix(yIeJeIe->ppvaluepEIpEJ()->times(yIeJeIe->value())); + ppGpEIpEJ->magnifySelf(2.0); +} + +void MbD::DistancexyConstraintIqcJqc::calc_ppGpXJpEJ() +{ + //xIeJeIe ppvaluepXJpEJ = 0 + //yIeJeIe ppvaluepXJpEJ = 0 + ppGpXJpEJ = (xIeJeIe->pvaluepXJ()->transposeTimesFullRow(xIeJeIe->pvaluepEJ())); + ppGpXJpEJ = ppGpXJpEJ->plusFullMatrix(yIeJeIe->pvaluepXJ()->transposeTimesFullRow(yIeJeIe->pvaluepEJ())); + ppGpXJpEJ->magnifySelf(2.0); +} + +void MbD::DistancexyConstraintIqcJqc::calc_ppGpEJpEJ() +{ + ppGpEJpEJ = (xIeJeIe->pvaluepEJ()->transposeTimesFullRow(xIeJeIe->pvaluepEJ())); + ppGpEJpEJ = ppGpEJpEJ->plusFullMatrix(xIeJeIe->ppvaluepEJpEJ()->times(xIeJeIe->value())); + ppGpEJpEJ = ppGpEJpEJ->plusFullMatrix(yIeJeIe->pvaluepEJ()->transposeTimesFullRow(yIeJeIe->pvaluepEJ())); + ppGpEJpEJ = ppGpEJpEJ->plusFullMatrix(yIeJeIe->ppvaluepEJpEJ()->times(yIeJeIe->value())); + ppGpEJpEJ->magnifySelf(2.0); +} + +void MbD::DistancexyConstraintIqcJqc::calcPostDynCorrectorIteration() +{ + DistancexyConstraintIqcJc::calcPostDynCorrectorIteration(); + this->calc_pGpXJ(); + this->calc_pGpEJ(); + this->calc_ppGpXIpXJ(); + this->calc_ppGpEIpXJ(); + this->calc_ppGpXJpXJ(); + this->calc_ppGpXIpEJ(); + this->calc_ppGpEIpEJ(); + this->calc_ppGpXJpEJ(); + this->calc_ppGpEJpEJ(); +} + +void MbD::DistancexyConstraintIqcJqc::fillAccICIterError(FColDsptr col) +{ + DistancexyConstraintIqcJc::fillAccICIterError(col); + col->atiplusFullVectortimes(iqXJ, pGpXJ, lam); + col->atiplusFullVectortimes(iqEJ, pGpEJ, lam); + auto frmIeqc = std::static_pointer_cast(frmI); + auto frmJeqc = std::static_pointer_cast(frmJ); + auto qXdotI = frmIeqc->qXdot(); + auto qEdotI = frmIeqc->qEdot(); + auto qXdotJ = frmJeqc->qXdot(); + auto qEdotJ = frmJeqc->qEdot(); + double sum = 0.0; + sum += pGpXJ->timesFullColumn(frmJeqc->qXddot()); + sum += pGpEJ->timesFullColumn(frmJeqc->qEddot()); + sum += 2.0 * (qXdotI->transposeTimesFullColumn(ppGpXIpXJ->timesFullColumn(qXdotJ))); + sum += 2.0 * (qEdotI->transposeTimesFullColumn(ppGpEIpXJ->timesFullColumn(qXdotJ))); + sum += qXdotJ->transposeTimesFullColumn(ppGpXJpXJ->timesFullColumn(qXdotJ)); + sum += 2.0 * (qXdotI->transposeTimesFullColumn(ppGpXIpEJ->timesFullColumn(qEdotJ))); + sum += 2.0 * (qEdotI->transposeTimesFullColumn(ppGpEIpEJ->timesFullColumn(qEdotJ))); + sum += 2.0 * (qXdotJ->transposeTimesFullColumn(ppGpXJpEJ->timesFullColumn(qEdotJ))); + sum += qEdotJ->transposeTimesFullColumn(ppGpEJpEJ->timesFullColumn(qEdotJ)); + col->atiplusNumber(iG, sum); +} + +void MbD::DistancexyConstraintIqcJqc::fillPosICError(FColDsptr col) +{ + DistancexyConstraintIqcJc::fillPosICError(col); + col->atiplusFullVectortimes(iqXJ, pGpXJ, lam); + col->atiplusFullVectortimes(iqEJ, pGpEJ, lam); +} + +void MbD::DistancexyConstraintIqcJqc::fillPosICJacob(SpMatDsptr mat) +{ + DistancexyConstraintIqcJc::fillPosICJacob(mat); + mat->atijplusFullRow(iG, iqXJ, pGpXJ); + mat->atijplusFullColumn(iqXJ, iG, pGpXJ->transpose()); + mat->atijplusFullRow(iG, iqEJ, pGpEJ); + mat->atijplusFullColumn(iqEJ, iG, pGpEJ->transpose()); + auto ppGpXIpXJlam = ppGpXIpXJ->times(lam); + mat->atijplusFullMatrix(iqXI, iqXJ, ppGpXIpXJlam); + mat->atijplusTransposeFullMatrix(iqXJ, iqXI, ppGpXIpXJlam); + auto ppGpEIpXJlam = ppGpEIpXJ->times(lam); + mat->atijplusFullMatrix(iqEI, iqXJ, ppGpEIpXJlam); + mat->atijplusTransposeFullMatrix(iqXJ, iqEI, ppGpEIpXJlam); + mat->atijplusFullMatrixtimes(iqXJ, iqXJ, ppGpXJpXJ, lam); + auto ppGpXIpEJlam = ppGpXIpEJ->times(lam); + mat->atijplusFullMatrix(iqXI, iqEJ, ppGpXIpEJlam); + mat->atijplusTransposeFullMatrix(iqEJ, iqXI, ppGpXIpEJlam); + auto ppGpEIpEJlam = ppGpEIpEJ->times(lam); + mat->atijplusFullMatrix(iqEI, iqEJ, ppGpEIpEJlam); + mat->atijplusTransposeFullMatrix(iqEJ, iqEI, ppGpEIpEJlam); + auto ppGpXJpEJlam = ppGpXJpEJ->times(lam); + mat->atijplusFullMatrix(iqXJ, iqEJ, ppGpXJpEJlam); + mat->atijplusTransposeFullMatrix(iqEJ, iqXJ, ppGpXJpEJlam); + mat->atijplusFullMatrixtimes(iqEJ, iqEJ, ppGpEJpEJ, lam); +} + +void MbD::DistancexyConstraintIqcJqc::fillPosKineJacob(SpMatDsptr mat) +{ + DistancexyConstraintIqcJc::fillPosKineJacob(mat); + mat->atijplusFullRow(iG, iqXJ, pGpXJ); + mat->atijplusFullRow(iG, iqEJ, pGpEJ); +} + +void MbD::DistancexyConstraintIqcJqc::fillVelICJacob(SpMatDsptr mat) +{ + DistancexyConstraintIqcJc::fillVelICJacob(mat); + mat->atijplusFullRow(iG, iqXJ, pGpXJ); + mat->atijplusFullColumn(iqXJ, iG, pGpXJ->transpose()); + mat->atijplusFullRow(iG, iqEJ, pGpEJ); + mat->atijplusFullColumn(iqEJ, iG, pGpEJ->transpose()); +} + +void MbD::DistancexyConstraintIqcJqc::init_xyIeJeIe() +{ + xIeJeIe = CREATE::With(frmI, frmJ, 0); + yIeJeIe = CREATE::With(frmI, frmJ, 1); +} + +void MbD::DistancexyConstraintIqcJqc::useEquationNumbers() +{ + DistancexyConstraintIqcJc::useEquationNumbers(); + auto frmJeqc = std::static_pointer_cast(frmJ); + iqXJ = frmJeqc->iqX(); + iqEJ = frmJeqc->iqE(); +} diff --git a/MbDCode/DistancexyConstraintIqcJqc.h b/MbDCode/DistancexyConstraintIqcJqc.h new file mode 100644 index 0000000..09b97e7 --- /dev/null +++ b/MbDCode/DistancexyConstraintIqcJqc.h @@ -0,0 +1,33 @@ +#pragma once + +#include "DistancexyConstraintIqcJc.h" + +namespace MbD { + class DistancexyConstraintIqcJqc : public DistancexyConstraintIqcJc + { + //pGpXJ pGpEJ ppGpXIpXJ ppGpEIpXJ ppGpXJpXJ ppGpXIpEJ ppGpEIpEJ ppGpXJpEJ ppGpEJpEJ iqXJ iqEJ + public: + void calc_pGpXJ(); + void calc_pGpEJ(); + void calc_ppGpXIpXJ(); + void calc_ppGpEIpXJ(); + void calc_ppGpXJpXJ(); + void calc_ppGpXIpEJ(); + void calc_ppGpEIpEJ(); + void calc_ppGpXJpEJ(); + void calc_ppGpEJpEJ(); + void calcPostDynCorrectorIteration() override; + void fillAccICIterError(FColDsptr col) override; + void fillPosICError(FColDsptr col) override; + void fillPosICJacob(SpMatDsptr mat) override; + void fillPosKineJacob(SpMatDsptr mat) override; + void fillVelICJacob(SpMatDsptr mat) override; + void init_xyIeJeIe() override; + void useEquationNumbers() override; + + FRowDsptr pGpXJ, pGpEJ; + FMatDsptr ppGpXIpXJ, ppGpEIpXJ, ppGpXJpXJ, ppGpXIpEJ, ppGpEIpEJ, ppGpXJpEJ, ppGpEJpEJ; + int iqXJ, iqEJ; + }; +} + diff --git a/MbDCode/EndFramec.cpp b/MbDCode/EndFramec.cpp index fc00dbe..2131fcb 100644 --- a/MbDCode/EndFramec.cpp +++ b/MbDCode/EndFramec.cpp @@ -12,6 +12,11 @@ EndFramec::EndFramec() { EndFramec::EndFramec(const char* str) : CartesianFrame(str) { } +FMatDsptr MbD::EndFramec::aAeO() +{ + return aAOe->transpose(); +} + System* EndFramec::root() { return markerFrame->root(); diff --git a/MbDCode/EndFramec.h b/MbDCode/EndFramec.h index 38015bd..cc89fb4 100644 --- a/MbDCode/EndFramec.h +++ b/MbDCode/EndFramec.h @@ -19,6 +19,8 @@ namespace MbD { public: EndFramec(); EndFramec(const char* str); + + FMatDsptr aAeO(); System* root() override; void initialize() override; void setMarkerFrame(MarkerFrame* markerFrm); diff --git a/MbDCode/EndFrameqc.cpp b/MbDCode/EndFrameqc.cpp index ffe1868..ea07454 100644 --- a/MbDCode/EndFrameqc.cpp +++ b/MbDCode/EndFrameqc.cpp @@ -30,7 +30,7 @@ void EndFrameqc::initializeGlobally() void EndFrameqc::initEndFrameqct() { - endFrameqct = CREATE::With(this->getName().data()); + endFrameqct = CREATE::With(this->name.data()); endFrameqct->prOeOpE = prOeOpE; endFrameqct->pprOeOpEpE = pprOeOpEpE; endFrameqct->pAOepE = pAOepE; diff --git a/MbDCode/EulerParameters.h b/MbDCode/EulerParameters.h index a2ca2a9..0cb7e27 100644 --- a/MbDCode/EulerParameters.h +++ b/MbDCode/EulerParameters.h @@ -35,43 +35,43 @@ namespace MbD { template<> inline std::shared_ptr>>> EulerParameters::ppApEpEtimesColumn(FColDsptr col) { - double a2c1 = 2 * col->at(0); - double a2c2 = 2 * col->at(1); - double a2c3 = 2 * col->at(2); + double a2c0 = 2 * col->at(0); + double a2c1 = 2 * col->at(1); + double a2c2 = 2 * col->at(2); + double m2c0 = 0 - a2c0; double m2c1 = 0 - a2c1; double m2c2 = 0 - a2c2; - double m2c3 = 0 - a2c3; - auto col11 = std::make_shared>(ListD{ a2c1, m2c2, m2c3 }); - auto col12 = std::make_shared>(ListD{ a2c2, a2c1, 0 }); - auto col13 = std::make_shared>(ListD{ a2c3, 0, a2c1 }); - auto col14 = std::make_shared>(ListD{ 0, m2c3, a2c2 }); - auto col22 = std::make_shared>(ListD{ m2c1, a2c2, m2c3 }); - auto col23 = std::make_shared>(ListD{ 0, a2c3, a2c2 }); - auto col24 = std::make_shared>(ListD{ a2c3, 0, m2c1 }); - auto col33 = std::make_shared>(ListD{ m2c1, m2c2, a2c3 }); - auto col34 = std::make_shared>(ListD{ m2c2, a2c1, 0 }); - auto col44 = std::make_shared>(ListD{ a2c1, a2c2, a2c3 }); + auto col00 = std::make_shared>(ListD{ a2c0, m2c1, m2c2 }); + auto col01 = std::make_shared>(ListD{ a2c1, a2c0, 0 }); + auto col02 = std::make_shared>(ListD{ a2c2, 0, a2c0 }); + auto col03 = std::make_shared>(ListD{ 0, m2c2, a2c1 }); + auto col11 = std::make_shared>(ListD{ m2c0, a2c1, m2c2 }); + auto col12 = std::make_shared>(ListD{ 0, a2c2, a2c1 }); + auto col13 = std::make_shared>(ListD{ a2c2, 0, m2c0 }); + auto col22 = std::make_shared>(ListD{ m2c0, m2c1, a2c2 }); + auto col23 = std::make_shared>(ListD{ m2c1, a2c0, 0 }); + auto col33 = std::make_shared>(ListD{ a2c0, a2c1, a2c2 }); auto answer = std::make_shared>>>(4, 4); - auto& row1 = answer->at(0); - row1->at(0) = col11; - row1->at(1) = col12; - row1->at(2) = col13; - row1->at(3) = col14; - auto& row2 = answer->at(1); - row2->at(0) = col12; - row2->at(1) = col22; - row2->at(2) = col23; - row2->at(3) = col24; - auto& row3 = answer->at(2); - row3->at(0) = col13; - row3->at(1) = col23; - row3->at(2) = col33; - row3->at(3) = col34; - auto& row4 = answer->at(3); - row4->at(0) = col14; - row4->at(1) = col24; - row4->at(2) = col34; - row4->at(3) = col44; + auto& row0 = answer->at(0); + row0->at(0) = col00; + row0->at(1) = col01; + row0->at(2) = col02; + row0->at(3) = col03; + auto& row1 = answer->at(1); + row1->at(0) = col01; + row1->at(1) = col11; + row1->at(2) = col12; + row1->at(3) = col13; + auto& row2 = answer->at(2); + row2->at(0) = col02; + row2->at(1) = col12; + row2->at(2) = col22; + row2->at(3) = col23; + auto& row3 = answer->at(3); + row3->at(0) = col03; + row3->at(1) = col13; + row3->at(2) = col23; + row3->at(3) = col33; return answer; } @@ -142,44 +142,44 @@ namespace MbD { template<> inline std::shared_ptr>>> EulerParameters::ppApEpEtimesMatrix(FMatDsptr mat) { - FRowDsptr a2m1 = mat->at(0)->times(2.0); - FRowDsptr a2m2 = mat->at(1)->times(2.0); - FRowDsptr a2m3 = mat->at(2)->times(2.0); + FRowDsptr a2m0 = mat->at(0)->times(2.0); + FRowDsptr a2m1 = mat->at(1)->times(2.0); + FRowDsptr a2m2 = mat->at(2)->times(2.0); + FRowDsptr m2m0 = a2m0->negated(); FRowDsptr m2m1 = a2m1->negated(); FRowDsptr m2m2 = a2m2->negated(); - FRowDsptr m2m3 = a2m3->negated(); FRowDsptr zero = std::make_shared>(3, 0.0); - auto mat11 = std::make_shared>(ListFRD{ a2m1, m2m2, m2m3 }); - auto mat12 = std::make_shared>(ListFRD{ a2m2, a2m1, zero }); - auto mat13 = std::make_shared>(ListFRD{ a2m3, zero, a2m1 }); - auto mat14 = std::make_shared>(ListFRD{ zero, m2m3, a2m2 }); - auto mat22 = std::make_shared>(ListFRD{ m2m1, a2m2, m2m3 }); - auto mat23 = std::make_shared>(ListFRD{ zero, a2m3, a2m2 }); - auto mat24 = std::make_shared>(ListFRD{ a2m3, zero, m2m1 }); - auto mat33 = std::make_shared>(ListFRD{ m2m1, m2m2, a2m3 }); - auto mat34 = std::make_shared>(ListFRD{ m2m2, a2m1, zero }); - auto mat44 = std::make_shared>(ListFRD{ a2m1, a2m2, a2m3 }); + auto mat00 = std::make_shared>(ListFRD{ a2m0, m2m1, m2m2 }); + auto mat01 = std::make_shared>(ListFRD{ a2m1, a2m0, zero }); + auto mat02 = std::make_shared>(ListFRD{ a2m2, zero, a2m0 }); + auto mat03 = std::make_shared>(ListFRD{ zero, m2m2, a2m1 }); + auto mat11 = std::make_shared>(ListFRD{ m2m0, a2m1, m2m2 }); + auto mat12 = std::make_shared>(ListFRD{ zero, a2m2, a2m1 }); + auto mat13 = std::make_shared>(ListFRD{ a2m2, zero, m2m0 }); + auto mat22 = std::make_shared>(ListFRD{ m2m0, m2m1, a2m2 }); + auto mat23 = std::make_shared>(ListFRD{ m2m1, a2m0, zero }); + auto mat33 = std::make_shared>(ListFRD{ a2m0, a2m1, a2m2 }); auto answer = std::make_shared>>>(4, 4); - auto& row1 = answer->at(0); - row1->at(0) = mat11; - row1->at(1) = mat12; - row1->at(2) = mat13; - row1->at(3) = mat14; - auto& row2 = answer->at(1); - row2->at(0) = mat12; - row2->at(1) = mat22; - row2->at(2) = mat23; - row2->at(3) = mat24; - auto& row3 = answer->at(2); - row3->at(0) = mat13; - row3->at(1) = mat23; - row3->at(2) = mat33; - row3->at(3) = mat34; - auto& row4 = answer->at(3); - row4->at(0) = mat14; - row4->at(1) = mat24; - row4->at(2) = mat34; - row4->at(3) = mat44; + auto& row0 = answer->at(0); + row0->at(0) = mat00; + row0->at(1) = mat01; + row0->at(2) = mat02; + row0->at(3) = mat03; + auto& row1 = answer->at(1); + row1->at(0) = mat01; + row1->at(1) = mat11; + row1->at(2) = mat12; + row1->at(3) = mat13; + auto& row2 = answer->at(2); + row2->at(0) = mat02; + row2->at(1) = mat12; + row2->at(2) = mat22; + row2->at(3) = mat23; + auto& row3 = answer->at(3); + row3->at(0) = mat03; + row3->at(1) = mat13; + row3->at(2) = mat23; + row3->at(3) = mat33; return answer; } diff --git a/MbDCode/FullMatrix.h b/MbDCode/FullMatrix.h index e3f0ad2..a5d58a5 100644 --- a/MbDCode/FullMatrix.h +++ b/MbDCode/FullMatrix.h @@ -59,6 +59,7 @@ namespace MbD { std::shared_ptr> copy(); FullMatrix operator+(const FullMatrix fullMat); std::shared_ptr> transposeTimesFullColumn(const std::shared_ptr> fullCol); + void magnifySelf(T factor); std::ostream& printOn(std::ostream& s) const override; }; @@ -243,6 +244,13 @@ namespace MbD { return fullCol->transpose()->timesFullMatrix(sptr)->transpose(); } template + inline void FullMatrix::magnifySelf(T factor) + { + for (int i = 0; i < this->size(); i++) { + this->at(i)->magnifySelf(factor); + } + } + template inline std::ostream& FullMatrix::printOn(std::ostream& s) const { s << "FullMat[" << std::endl; diff --git a/MbDCode/FullRow.h b/MbDCode/FullRow.h index d32a871..3542e39 100644 --- a/MbDCode/FullRow.h +++ b/MbDCode/FullRow.h @@ -29,6 +29,7 @@ namespace MbD { std::shared_ptr> transpose(); std::shared_ptr> copy(); void atiplusFullRow(int j, std::shared_ptr> fullRow); + std::shared_ptr> transposeTimesFullRow(std::shared_ptr> fullRow); std::ostream& printOn(std::ostream& s) const override; }; @@ -125,6 +126,18 @@ namespace MbD { } } template + inline std::shared_ptr> FullRow::transposeTimesFullRow(std::shared_ptr> fullRow) + { + //"a*b = a(i)b(j)" + auto nrow = (int)this->size(); + auto answer = std::make_shared>(nrow); + for (int i = 0; i < nrow; i++) + { + answer->atiput(i, fullRow->times(this->at(i))); + } + return answer; + } + template inline std::ostream& FullRow::printOn(std::ostream& s) const { s << "FullRow{"; diff --git a/MbDCode/FullVector.h b/MbDCode/FullVector.h index d18767a..8904205 100644 --- a/MbDCode/FullVector.h +++ b/MbDCode/FullVector.h @@ -27,6 +27,7 @@ namespace MbD { double maxMagnitude(); double length(); void equalSelfPlusFullVectortimes(std::shared_ptr> fullVec, T factor); + void magnifySelf(T factor); std::ostream& printOn(std::ostream& s) const override; }; @@ -143,6 +144,14 @@ namespace MbD { } } template + inline void FullVector::magnifySelf(T factor) + { + for (int i = 0; i < this->size(); i++) + { + this->atitimes(i, factor); + } + } + template inline std::ostream& FullVector::printOn(std::ostream& s) const { s << "FullVec{"; diff --git a/MbDCode/GearConstraintIJ.cpp b/MbDCode/GearConstraintIJ.cpp new file mode 100644 index 0000000..9956005 --- /dev/null +++ b/MbDCode/GearConstraintIJ.cpp @@ -0,0 +1 @@ +#include "GearConstraintIJ.h" diff --git a/MbDCode/GearConstraintIJ.h b/MbDCode/GearConstraintIJ.h new file mode 100644 index 0000000..a969444 --- /dev/null +++ b/MbDCode/GearConstraintIJ.h @@ -0,0 +1,14 @@ +#pragma once + +#include "ConstraintIJ.h" + +namespace MbD { + class GearConstraintIJ : public ConstraintIJ + { + //orbitIeJe orbitJeIe radiusI radiusJ + public: + + + }; +} + diff --git a/MbDCode/Item.cpp b/MbDCode/Item.cpp index 5110fd9..f7e6fab 100644 --- a/MbDCode/Item.cpp +++ b/MbDCode/Item.cpp @@ -24,16 +24,6 @@ void Item::initialize() { } -void Item::setName(std::string& str) -{ - name = str; -} - -const std::string& Item::getName() const -{ - return name; -} - std::ostream& Item::printOn(std::ostream& s) const { std::string str = typeid(*this).name(); @@ -46,6 +36,30 @@ void Item::initializeLocally() { } +bool MbD::Item::isJointForce() +{ + assert(false); + return false; +} + +bool MbD::Item::isJointTorque() +{ + assert(false); + return false; +} + +bool MbD::Item::isKinedotIJ() +{ + assert(false); + return false; +} + +bool MbD::Item::isKineIJ() +{ + assert(false); + return false; +} + void Item::initializeGlobally() { } @@ -60,14 +74,39 @@ void Item::calcPostDynCorrectorIteration() { } +void MbD::Item::checkForCollisionDiscontinuityBetweenand(double impulsePrevious, double impulse) +{ + assert(false); +} + void Item::removeRedundantConstraints(std::shared_ptr> redunEqnNos) { } +void MbD::Item::setpqsumu(FColDsptr col) +{ + assert(false); +} + +void MbD::Item::setpqsumuddot(FColDsptr col) +{ + assert(false); +} + +void MbD::Item::setpqsumudot(FColDsptr col) +{ + assert(false); +} + void Item::reactivateRedundantConstraints() { } +void MbD::Item::registerName() +{ + assert(false); +} + void Item::fillPosKineError(FColDsptr col) { } @@ -76,19 +115,65 @@ void Item::fillPosKineJacob(SpMatDsptr mat) { } +void MbD::Item::fillpqsumu(FColDsptr col) +{ + assert(false); +} + +void MbD::Item::fillpqsumudot(FColDsptr col) +{ + assert(false); +} + void Item::fillEssenConstraints(std::shared_ptr>> essenConstraints) { + assert(false); +} + +void MbD::Item::fillPerpenConstraints(std::shared_ptr>> perpenConstraints) +{ + assert(false); +} + +void MbD::Item::fillpFpy(SpMatDsptr mat) +{ + assert(false); +} + +void MbD::Item::fillpFpydot(SpMatDsptr mat) +{ + assert(false); } void Item::fillRedundantConstraints(std::shared_ptr>> redunConstraints) { } +void MbD::Item::fillStaticError(FColDsptr col) +{ + assert(false); +} + +void MbD::Item::fillStaticJacob(FMatDsptr mat) +{ + assert(false); +} + void Item::fillConstraints(std::shared_ptr>> allConstraints) { assert(false); } +void MbD::Item::fillDispConstraints(std::shared_ptr>> dispConstraints) +{ + assert(false); +} + +void MbD::Item::fillDynError(FColDsptr col) +{ + assert(false); +} + void Item::fillqsu(FColDsptr col) { } @@ -97,6 +182,11 @@ void Item::fillqsuWeights(std::shared_ptr> diagMat) { } +void MbD::Item::fillqsuWeightsSmall(FColDsptr col) +{ + assert(false); +} + void Item::fillqsulam(FColDsptr col) { } @@ -105,12 +195,9 @@ void Item::setqsulam(FColDsptr col) { } -void Item::outputStates() +void MbD::Item::simUpdateAll() { - std::stringstream ss; - ss << classname() << " " << name; - auto str = ss.str(); - this->logString(str); + assert(false); } void Item::preDyn() @@ -123,6 +210,16 @@ void Item::preDyn() //"Default is do nothing." } +void MbD::Item::preDynCorrector() +{ + assert(false); +} + +void MbD::Item::preDynCorrectorIteration() +{ + assert(false); +} + void Item::postDyn() { //"Assume runDYNAMICS ended successfully." @@ -133,6 +230,16 @@ void Item::postDyn() //"Default is do nothing." } +void MbD::Item::postDynCorrector() +{ + assert(false); +} + +void MbD::Item::postDynCorrectorIteration() +{ + assert(false); +} + std::string Item::classname() { std::string str = typeid(*this).name(); @@ -146,15 +253,40 @@ void Item::preDynFirstStep() this->preDynStep(); } +void MbD::Item::preDynOutput() +{ + assert(false); +} + +void MbD::Item::preDynPredictor() +{ + assert(false); +} + void Item::postDynFirstStep() { this->postDynStep(); } +void MbD::Item::postDynOutput() +{ + assert(false); +} + +void MbD::Item::postDynPredictor() +{ + assert(false); +} + void Item::preDynStep() { } +void MbD::Item::preICRestart() +{ + assert(false); +} + void Item::postDynStep() { //"Called after the end of a complete step in the dynamic solution." @@ -166,6 +298,23 @@ void Item::storeDynState() { } +void MbD::Item::submitToSystem() +{ + assert(false); +} + +double MbD::Item::suggestSmallerOrAcceptCollisionFirstStepSize(double hnew) +{ + assert(false); + return 0.0; +} + +double MbD::Item::suggestSmallerOrAcceptCollisionStepSize(double hnew) +{ + assert(false); + return 0.0; +} + double Item::suggestSmallerOrAcceptDynFirstStepSize(double hnew) { //"Default is return hnew." @@ -198,6 +347,16 @@ void Item::fillqsudot(FColDsptr col) { } +void MbD::Item::fillqsudotPlam(FColDsptr col) +{ + assert(false); +} + +void MbD::Item::fillqsudotPlamDeriv(FColDsptr col) +{ + assert(false); +} + void Item::fillqsudotWeights(std::shared_ptr> diagMat) { } @@ -210,15 +369,60 @@ void Item::fillVelICJacob(SpMatDsptr mat) { } +void MbD::Item::getString(std::string str) +{ + assert(false); +} + void Item::setqsudotlam(FColDsptr col) { } +void MbD::Item::setqsudotPlam(FColDsptr col) +{ + assert(false); +} + +void MbD::Item::setqsudotPlamDeriv(FColDsptr col) +{ + assert(false); +} + void Item::preAccIC() { this->calcPostDynCorrectorIteration(); } +void MbD::Item::preCollision() +{ + assert(false); +} + +void MbD::Item::preCollisionCorrector() +{ + assert(false); +} + +void MbD::Item::preCollisionCorrectorIteration() +{ + assert(false); +} + +void MbD::Item::preCollisionDerivativeIC() +{ + assert(false); +} + +void MbD::Item::preCollisionPredictor() +{ + assert(false); +} + +void MbD::Item::preCollisionStep() +{ + assert(false); +} + void Item::postAccIC() { } @@ -227,6 +431,31 @@ void Item::postAccICIteration() { } +void MbD::Item::postCollisionCorrector() +{ + assert(false); +} + +void MbD::Item::postCollisionCorrectorIteration() +{ + assert(false); +} + +void MbD::Item::postCollisionDerivativeIC() +{ + assert(false); +} + +void MbD::Item::postCollisionPredictor() +{ + assert(false); +} + +void MbD::Item::postCollisionStep() +{ + assert(false); +} + void Item::fillqsuddotlam(FColDsptr col) { } @@ -239,6 +468,31 @@ void Item::fillAccICIterJacob(SpMatDsptr mat) { } +void MbD::Item::fillCollisionDerivativeICError(FColDsptr col) +{ + assert(false); +} + +void MbD::Item::fillCollisionDerivativeICJacob(SpMatDsptr mat) +{ + assert(false); +} + +void MbD::Item::fillCollisionError(FColDsptr col) +{ + assert(false); +} + +void MbD::Item::fillCollisionpFpy(SpMatDsptr mat) +{ + assert(false); +} + +void MbD::Item::fillCollisionpFpydot(SpMatDsptr mat) +{ + assert(false); +} + void Item::setqsudot(FColDsptr col) { } @@ -253,11 +507,21 @@ std::shared_ptr Item::stateData() return std::shared_ptr(); } +void MbD::Item::storeCollisionState() +{ + assert(false); +} + void Item::discontinuityAtaddTypeTo(double t, std::shared_ptr> disconTypes) { } -double Item::checkForDynDiscontinuityBetween(double tprev, double t) +void MbD::Item::discontinuityAtICAddTo(std::shared_ptr> disconTypes) +{ + assert(false); +} + +double Item::checkForDynDiscontinuityBetweenand(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." @@ -288,6 +552,21 @@ void Item::logString(const char* chars) this->logString(str); } +void MbD::Item::logStringwithArgument(const char* chars, const char* chars1) +{ + assert(false); +} + +void MbD::Item::logStringwithArguments(const char* chars, std::shared_ptr> arrayOfChars) +{ + assert(false); +} + +void MbD::Item::normalImpulse(double imp) +{ + assert(false); +} + void Item::prePosIC() { //"Called once before solving for position initial conditions." @@ -302,6 +581,11 @@ void Item::prePosKine() this->prePosIC(); } +void MbD::Item::preStatic() +{ + assert(false); +} + void Item::postPosIC() { } @@ -311,6 +595,16 @@ void Item::postPosICIteration() this->calcPostDynCorrectorIteration(); } +void MbD::Item::postStatic() +{ + assert(false); +} + +void MbD::Item::postStaticIteration() +{ + assert(false); +} + void Item::fillPosICError(FColDsptr col) { } diff --git a/MbDCode/Item.h b/MbDCode/Item.h index 2b02a10..c502282 100644 --- a/MbDCode/Item.h +++ b/MbDCode/Item.h @@ -20,70 +20,124 @@ namespace MbD { Item(); Item(const char* str); virtual System* root(); - virtual void initialize(); - virtual void initializeLocally(); - virtual void initializeGlobally(); - virtual void postInput(); + virtual void calcPostDynCorrectorIteration(); + virtual void checkForCollisionDiscontinuityBetweenand(double impulsePrevious, double impulse); + virtual double checkForDynDiscontinuityBetweenand(double tprev, double t); + virtual std::string classname(); virtual void constraintsReport(); - 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(); - + virtual void discontinuityAtaddTypeTo(double t, std::shared_ptr> disconTypes); + virtual void discontinuityAtICAddTo(std::shared_ptr> disconTypes); + virtual void fillAccICIterError(FColDsptr col); + virtual void fillAccICIterJacob(SpMatDsptr mat); + virtual void fillCollisionDerivativeICError(FColDsptr col); + virtual void fillCollisionDerivativeICJacob(SpMatDsptr mat); + virtual void fillCollisionError(FColDsptr col); + virtual void fillCollisionpFpy(SpMatDsptr mat); + virtual void fillCollisionpFpydot(SpMatDsptr mat); + virtual void fillConstraints(std::shared_ptr>> allConstraints); + virtual void fillDispConstraints(std::shared_ptr>> dispConstraints); + virtual void fillDynError(FColDsptr col); + virtual void fillEssenConstraints(std::shared_ptr>> essenConstraints); + virtual void fillPerpenConstraints(std::shared_ptr>> perpenConstraints); + virtual void fillpFpy(SpMatDsptr mat); + virtual void fillpFpydot(SpMatDsptr mat); virtual void fillPosICError(FColDsptr col); virtual void fillPosICJacob(FMatDsptr mat); virtual void fillPosICJacob(SpMatDsptr mat); - virtual void postPosIC(); - virtual void postPosICIteration(); - virtual void removeRedundantConstraints(std::shared_ptr> redundantEqnNos); - virtual void reactivateRedundantConstraints(); virtual void fillPosKineError(FColDsptr col); virtual void fillPosKineJacob(SpMatDsptr mat); - virtual void fillEssenConstraints(std::shared_ptr>> essenConstraints); - virtual void fillRedundantConstraints(std::shared_ptr>> redunConstraints); - virtual void fillConstraints(std::shared_ptr>> allConstraints); - + virtual void fillpqsumu(FColDsptr col); + virtual void fillpqsumudot(FColDsptr col); virtual void fillqsu(FColDsptr col); - virtual void fillqsuWeights(std::shared_ptr> diagMat); - virtual void fillqsulam(FColDsptr col); - virtual void setqsulam(FColDsptr col); - virtual void outputStates(); - virtual void preDyn(); - virtual 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 fillqsuddotlam(FColDsptr col); virtual void fillqsudot(FColDsptr col); + virtual void fillqsudotPlam(FColDsptr col); + virtual void fillqsudotPlamDeriv(FColDsptr col); virtual void fillqsudotWeights(std::shared_ptr> diagMat); + virtual void fillqsulam(FColDsptr col); + virtual void fillqsuWeights(std::shared_ptr> diagMat); + virtual void fillqsuWeightsSmall(FColDsptr col); + virtual void fillRedundantConstraints(std::shared_ptr>> redunConstraints); + virtual void fillStaticError(FColDsptr col); + virtual void fillStaticJacob(FMatDsptr mat); virtual void fillVelICError(FColDsptr col); virtual void fillVelICJacob(SpMatDsptr mat); - virtual void setqsudotlam(FColDsptr col); - virtual void setqsudot(FColDsptr col); - virtual void setqsuddotlam(FColDsptr col); - - virtual void preAccIC(); + virtual void getString(std::string str); + virtual void initialize(); + virtual void initializeGlobally(); + virtual void initializeLocally(); + virtual bool isJointForce(); + virtual bool isJointTorque(); + virtual bool isKinedotIJ(); + virtual bool isKineIJ(); + virtual void logString(std::string& str); + void logString(const char* chars); + virtual void logStringwithArgument(const char* chars, const char* chars1); + virtual void logStringwithArguments(const char* chars, std::shared_ptr> arrayOfChars); + virtual void normalImpulse(double imp); virtual void postAccIC(); virtual void postAccICIteration(); - virtual void fillqsuddotlam(FColDsptr col); - virtual void fillAccICIterError(FColDsptr col); - virtual void fillAccICIterJacob(SpMatDsptr mat); + virtual void postCollisionCorrector(); + virtual void postCollisionCorrectorIteration(); + virtual void postCollisionDerivativeIC(); + virtual void postCollisionPredictor(); + virtual void postCollisionStep(); + virtual void postDyn(); + virtual void postDynCorrector(); + virtual void postDynCorrectorIteration(); + virtual void postDynFirstStep(); + virtual void postDynOutput(); + virtual void postDynPredictor(); + virtual void postDynStep(); + virtual void postInput(); + virtual void postPosIC(); + virtual void postPosICIteration(); + virtual void postStatic(); + virtual void postStaticIteration(); + virtual void postVelIC(); + virtual void preAccIC(); + virtual void preCollision(); + virtual void preCollisionCorrector(); + virtual void preCollisionCorrectorIteration(); + virtual void preCollisionDerivativeIC(); + virtual void preCollisionPredictor(); + virtual void preCollisionStep(); + virtual void preDyn(); + virtual void preDynCorrector(); + virtual void preDynCorrectorIteration(); + virtual void preDynFirstStep(); + virtual void preDynOutput(); + virtual void preDynPredictor(); + virtual void preDynStep(); + virtual void preICRestart(); + virtual void prePosIC(); + virtual void prePosKine(); + virtual void preStatic(); + virtual void preVelIC(); + virtual void reactivateRedundantConstraints(); + virtual void registerName(); + virtual void removeRedundantConstraints(std::shared_ptr> redundantEqnNos); + virtual void setpqsumu(FColDsptr col); + virtual void setpqsumuddot(FColDsptr col); + virtual void setpqsumudot(FColDsptr col); + virtual void setqsu(FColDsptr qsuOld); + virtual void setqsuddotlam(FColDsptr col); + virtual void setqsudot(FColDsptr col); + virtual void setqsudotlam(FColDsptr col); + virtual void setqsudotPlam(FColDsptr col); + virtual void setqsudotPlamDeriv(FColDsptr col); + virtual void setqsulam(FColDsptr col); + virtual void simUpdateAll(); 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 void storeCollisionState(); + virtual void storeDynState(); + virtual void submitToSystem(); + virtual double suggestSmallerOrAcceptCollisionFirstStepSize(double hnew); + virtual double suggestSmallerOrAcceptCollisionStepSize(double hnew); + virtual double suggestSmallerOrAcceptDynFirstStepSize(double hnew); + virtual double suggestSmallerOrAcceptDynStepSize(double hnew); + virtual void useEquationNumbers(); virtual std::ostream& printOn(std::ostream& s) const; friend std::ostream& operator<<(std::ostream& s, const Item& item) diff --git a/MbDCode/Joint.cpp b/MbDCode/Joint.cpp index c50c752..1e8064e 100644 --- a/MbDCode/Joint.cpp +++ b/MbDCode/Joint.cpp @@ -61,10 +61,25 @@ void Joint::postInput() void Joint::addConstraint(std::shared_ptr con) { - con->setOwner(this); + con->owner = this; constraints->push_back(con); } +FColDsptr MbD::Joint::aFIeJtIe() +{ + //"aFIeJtIe is joint force on end frame Ie expresses in Ie components." + auto frmIqc = std::dynamic_pointer_cast(frmI); + return frmIqc->aAeO()->timesFullColumn(this->aFIeJtO()); +} + +FColDsptr MbD::Joint::aFIeJtO() +{ + //"aFIeJtO is joint force on end frame Ie expresses in O components." + auto aFIeJtO = std::make_shared >(3); + constraintsDo([&](std::shared_ptr con) { con->addToJointForceI(aFIeJtO); }); + return aFIeJtO; +} + void Joint::prePosIC() { constraintsDo([](std::shared_ptr constraint) { constraint->prePosIC(); }); @@ -177,7 +192,7 @@ void Joint::constraintsReport() } }); if (redunCons->size() > 0) { - std::string str = "MbD: " + this->classname() + std::string(" ") + this->getName() + " has the following constraint(s) removed: "; + std::string str = "MbD: " + this->classname() + std::string(" ") + this->name + " has the following constraint(s) removed: "; this->logString(str); std::for_each(redunCons->begin(), redunCons->end(), [&](auto& con) { str = "MbD: " + std::string(" ") + con->classname(); @@ -191,18 +206,6 @@ void Joint::postPosIC() constraintsDo([](std::shared_ptr constraint) { constraint->postPosIC(); }); } -void Joint::outputStates() -{ - Item::outputStates(); - std::stringstream ss; - ss << "frmI = " << frmI->markerFrame->getName() << std::endl; - ss << "frmJ = " << frmJ->markerFrame->getName() << std::endl; - auto str = ss.str(); - this->logString(str); - - constraintsDo([](std::shared_ptr constraint) { constraint->outputStates(); }); -} - void Joint::preDyn() { constraintsDo([](std::shared_ptr constraint) { constraint->preDyn(); }); @@ -218,6 +221,11 @@ void Joint::fillPosKineJacob(SpMatDsptr mat) constraintsDo([&](std::shared_ptr constraint) { constraint->fillPosKineJacob(mat); }); } +void MbD::Joint::fillqsuddotlam(FColDsptr col) +{ + constraintsDo([&](std::shared_ptr constraint) { constraint->fillqsuddotlam(col); }); +} + void Joint::preVelIC() { constraintsDo([](std::shared_ptr constraint) { constraint->preVelIC(); }); @@ -277,6 +285,20 @@ FColDsptr Joint::aFX() return this->jointForceI(); } +FColDsptr MbD::Joint::aTIeJtIe() +{ + //"aTIeJtIe is torque on part containing end frame Ie expressed in Ie components." + return frmI->aAeO()->timesFullColumn(this->aTIeJtO()); +} + +FColDsptr MbD::Joint::aTIeJtO() +{ + //"aTIeJtO is torque on part containing end frame Ie expressed in O components." + auto aTIeJtO = std::make_shared >(3); + constraintsDo([&](std::shared_ptr con) { con->addToJointTorqueI(aTIeJtO); }); + return aTIeJtO; +} + FColDsptr Joint::jointForceI() { //"jointForceI is force on MbD marker I." diff --git a/MbDCode/Joint.h b/MbDCode/Joint.h index bd90c1f..1d912b9 100644 --- a/MbDCode/Joint.h +++ b/MbDCode/Joint.h @@ -7,63 +7,68 @@ #include "EndFramec.h" namespace MbD { - class Constraint; + class Constraint; - class Joint : public Item - { - //frmI frmJ constraints friction - public: - Joint(); - Joint(const char* str); - void initialize() override; - virtual void connectsItoJ(EndFrmcptr frmI, EndFrmcptr frmJ); - void initializeLocally() override; - void initializeGlobally() override; - void constraintsDo(const std::function )>& f); - void postInput() override; - void addConstraint(std::shared_ptr con); - void prePosIC() override; - void prePosKine() override; - void fillEssenConstraints(std::shared_ptr>> essenConstraints) override; - virtual void fillDispConstraints(std::shared_ptr>> dispConstraints); - virtual void fillPerpenConstraints(std::shared_ptr>> perpenConstraints); - void fillRedundantConstraints(std::shared_ptr>> redunConstraints) override; - void fillConstraints(std::shared_ptr>> allConstraints) override; - void fillqsulam(FColDsptr col) override; - void fillqsudot(FColDsptr col) override; - void fillqsudotWeights(std::shared_ptr> diagMat) override; - void useEquationNumbers() override; - void setqsulam(FColDsptr col) override; - void setqsudotlam(FColDsptr col) override; - void postPosICIteration() override; - void fillPosICError(FColDsptr col) override; - void fillPosICJacob(SpMatDsptr mat) override; - void removeRedundantConstraints(std::shared_ptr> redundantEqnNos) override; - void reactivateRedundantConstraints() override; - void constraintsReport() override; - void postPosIC() override; - void outputStates() override; - void preDyn() override; - void fillPosKineError(FColDsptr col) override; - void fillPosKineJacob(SpMatDsptr mat) override; - void preVelIC() override; - void fillVelICError(FColDsptr col) override; - void fillVelICJacob(SpMatDsptr mat) override; - void preAccIC() override; - void fillAccICIterError(FColDsptr col) override; - void fillAccICIterJacob(SpMatDsptr mat) override; - void setqsuddotlam(FColDsptr col) override; - std::shared_ptr stateData() override; - FColDsptr aFX(); - FColDsptr jointForceI(); - FColDsptr aTX(); - FColDsptr jointTorqueI(); - void postDynStep() override; + class Joint : public Item + { + //frmI frmJ constraints friction + public: + Joint(); + Joint(const char* str); - EndFrmcptr frmI; - EndFrmcptr frmJ; - std::shared_ptr>> constraints; + void addConstraint(std::shared_ptr con); + FColDsptr aFIeJtIe(); + FColDsptr aFIeJtO(); + FColDsptr aFX(); + FColDsptr aTIeJtIe(); + FColDsptr aTIeJtO(); + FColDsptr aTX(); + virtual void connectsItoJ(EndFrmcptr frmI, EndFrmcptr frmJ); + void constraintsDo(const std::function )>& f); + void constraintsReport() override; + void fillAccICIterError(FColDsptr col) override; + void fillAccICIterJacob(SpMatDsptr mat) override; + void fillConstraints(std::shared_ptr>> allConstraints) override; + void fillDispConstraints(std::shared_ptr>> dispConstraints) override; + void fillEssenConstraints(std::shared_ptr>> essenConstraints) override; + void fillPerpenConstraints(std::shared_ptr>> perpenConstraints) override; + void fillPosICError(FColDsptr col) override; + void fillPosICJacob(SpMatDsptr mat) override; + void fillPosKineError(FColDsptr col) override; + void fillPosKineJacob(SpMatDsptr mat) override; + void fillqsuddotlam(FColDsptr col) override; + void fillqsulam(FColDsptr col) override; + void fillqsudot(FColDsptr col) override; + void fillqsudotWeights(std::shared_ptr> diagMat) override; + void fillRedundantConstraints(std::shared_ptr>> redunConstraints) override; + void fillVelICError(FColDsptr col) override; + void fillVelICJacob(SpMatDsptr mat) override; + void initialize() override; + void initializeGlobally() override; + void initializeLocally() override; + FColDsptr jointForceI(); + FColDsptr jointTorqueI(); + void postDynStep() override; + void postInput() override; + void postPosIC() override; + void postPosICIteration() override; + void preAccIC() override; + void preDyn() override; + void prePosIC() override; + void prePosKine() override; + void preVelIC() override; + void reactivateRedundantConstraints() override; + void removeRedundantConstraints(std::shared_ptr> redundantEqnNos) override; + void setqsuddotlam(FColDsptr col) override; + void setqsudotlam(FColDsptr col) override; + void setqsulam(FColDsptr col) override; + std::shared_ptr stateData() override; + void useEquationNumbers() override; - }; + EndFrmcptr frmI; + EndFrmcptr frmJ; + std::shared_ptr>> constraints; + + }; } diff --git a/MbDCode/KinematicIeJe.cpp b/MbDCode/KinematicIeJe.cpp index b8cceb6..f8f9215 100644 --- a/MbDCode/KinematicIeJe.cpp +++ b/MbDCode/KinematicIeJe.cpp @@ -9,3 +9,188 @@ KinematicIeJe::KinematicIeJe() KinematicIeJe::KinematicIeJe(EndFrmcptr frmi, EndFrmcptr frmj) : frmI(frmi), frmJ(frmj) { } + +bool MbD::KinematicIeJe::isKineIJ() +{ + return true; +} + +FRowDsptr MbD::KinematicIeJe::pvaluepXI() +{ + assert(false); + return FRowDsptr(); +} + +FRowDsptr MbD::KinematicIeJe::pvaluepEI() +{ + assert(false); + return FRowDsptr(); +} + +FMatDsptr MbD::KinematicIeJe::ppvaluepXIpXI() +{ + assert(false); + return FMatDsptr(); +} + +FMatDsptr MbD::KinematicIeJe::ppvaluepXIpEI() +{ + assert(false); + return FMatDsptr(); +} + +FMatDsptr MbD::KinematicIeJe::ppvaluepEIpEI() +{ + assert(false); + return FMatDsptr(); +} + +FRowDsptr MbD::KinematicIeJe::pvaluepXJ() +{ + assert(false); + return FRowDsptr(); +} + +FRowDsptr MbD::KinematicIeJe::pvaluepEJ() +{ + assert(false); + return FRowDsptr(); +} + +FMatDsptr MbD::KinematicIeJe::ppvaluepXIpXJ() +{ + assert(false); + return FMatDsptr(); +} + +FMatDsptr MbD::KinematicIeJe::ppvaluepXIpEJ() +{ + assert(false); + return FMatDsptr(); +} + +FMatDsptr MbD::KinematicIeJe::ppvaluepEIpXJ() +{ + assert(false); + return FMatDsptr(); +} + +FMatDsptr MbD::KinematicIeJe::ppvaluepEIpEJ() +{ + assert(false); + return FMatDsptr(); +} + +FMatDsptr MbD::KinematicIeJe::ppvaluepXJpXJ() +{ + assert(false); + return FMatDsptr(); +} + +FMatDsptr MbD::KinematicIeJe::ppvaluepXJpEJ() +{ + assert(false); + return FMatDsptr(); +} + +FMatDsptr MbD::KinematicIeJe::ppvaluepEJpEJ() +{ + assert(false); + return FMatDsptr(); +} + +FRowDsptr MbD::KinematicIeJe::pvaluepXK() +{ + assert(false); + return FRowDsptr(); +} + +FRowDsptr MbD::KinematicIeJe::pvaluepEK() +{ + assert(false); + return FRowDsptr(); +} + +FMatDsptr MbD::KinematicIeJe::ppvaluepXIpEK() +{ + assert(false); + return FMatDsptr(); +} + +FMatDsptr MbD::KinematicIeJe::ppvaluepEIpEK() +{ + assert(false); + return FMatDsptr(); +} + +FMatDsptr MbD::KinematicIeJe::ppvaluepXJpEK() +{ + assert(false); + return FMatDsptr(); +} + +FMatDsptr MbD::KinematicIeJe::ppvaluepEJpEK() +{ + assert(false); + return FMatDsptr(); +} + +FMatDsptr MbD::KinematicIeJe::ppvaluepEKpEK() +{ + assert(false); + return FMatDsptr(); +} + +double MbD::KinematicIeJe::pvaluept() +{ + assert(false); + return 0.0; +} + +double MbD::KinematicIeJe::ppvalueptpt() +{ + assert(false); + return 0.0; +} + +FRowDsptr MbD::KinematicIeJe::ppvaluepXIpt() +{ + assert(false); + return FRowDsptr(); +} + +FRowDsptr MbD::KinematicIeJe::ppvaluepEIpt() +{ + assert(false); + return FRowDsptr(); +} + +FRowDsptr MbD::KinematicIeJe::ppvaluepXJpt() +{ + assert(false); + return FRowDsptr(); +} + +FRowDsptr MbD::KinematicIeJe::ppvaluepEJpt() +{ + assert(false); + return FRowDsptr(); +} + +FRowDsptr MbD::KinematicIeJe::ppvaluepXKpt() +{ + assert(false); + return FRowDsptr(); +} + +FRowDsptr MbD::KinematicIeJe::ppvaluepEKpt() +{ + assert(false); + return FRowDsptr(); +} + +double MbD::KinematicIeJe::value() +{ + assert(false); + return 0.0; +} diff --git a/MbDCode/KinematicIeJe.h b/MbDCode/KinematicIeJe.h index ec00315..ce8718b 100644 --- a/MbDCode/KinematicIeJe.h +++ b/MbDCode/KinematicIeJe.h @@ -10,12 +10,38 @@ namespace MbD { public: KinematicIeJe(); KinematicIeJe(EndFrmcptr frmi, EndFrmcptr frmj); - virtual FRowDsptr pvaluepXJ() = 0; - virtual FRowDsptr pvaluepEJ() = 0; - virtual FMatDsptr ppvaluepXJpEK() = 0; - virtual FMatDsptr ppvaluepEJpEK() = 0; - virtual FMatDsptr ppvaluepEJpEJ() = 0; + bool isKineIJ() override; + virtual FRowDsptr pvaluepXI(); + virtual FRowDsptr pvaluepEI(); + virtual FMatDsptr ppvaluepXIpXI(); + virtual FMatDsptr ppvaluepXIpEI(); + virtual FMatDsptr ppvaluepEIpEI(); + virtual FRowDsptr pvaluepXJ(); + virtual FRowDsptr pvaluepEJ(); + virtual FMatDsptr ppvaluepXIpXJ(); + virtual FMatDsptr ppvaluepXIpEJ(); + virtual FMatDsptr ppvaluepEIpXJ(); + virtual FMatDsptr ppvaluepEIpEJ(); + virtual FMatDsptr ppvaluepXJpXJ(); + virtual FMatDsptr ppvaluepXJpEJ(); + virtual FMatDsptr ppvaluepEJpEJ(); + virtual FRowDsptr pvaluepXK(); + virtual FRowDsptr pvaluepEK(); + virtual FMatDsptr ppvaluepXIpEK(); + virtual FMatDsptr ppvaluepEIpEK(); + virtual FMatDsptr ppvaluepXJpEK(); + virtual FMatDsptr ppvaluepEJpEK(); + virtual FMatDsptr ppvaluepEKpEK(); + virtual double pvaluept(); + virtual double ppvalueptpt(); + virtual FRowDsptr ppvaluepXIpt(); + virtual FRowDsptr ppvaluepEIpt(); + virtual FRowDsptr ppvaluepXJpt(); + virtual FRowDsptr ppvaluepEJpt(); + virtual FRowDsptr ppvaluepXKpt(); + virtual FRowDsptr ppvaluepEKpt(); + virtual double value(); EndFrmcptr frmI, frmJ; }; diff --git a/MbDCode/MarkerFrame.cpp b/MbDCode/MarkerFrame.cpp index 08675f8..8a0e127 100644 --- a/MbDCode/MarkerFrame.cpp +++ b/MbDCode/MarkerFrame.cpp @@ -103,6 +103,11 @@ void MarkerFrame::fillqsuWeights(std::shared_ptr> diagMat endFramesDo([&](const std::shared_ptr& endFrame) { endFrame->fillqsuWeights(diagMat); }); } +void MbD::MarkerFrame::fillqsuddotlam(FColDsptr col) +{ + endFramesDo([&](const std::shared_ptr& endFrame) { endFrame->fillqsuddotlam(col); }); +} + void MarkerFrame::fillqsulam(FColDsptr col) { endFramesDo([&](const std::shared_ptr& endFrame) { endFrame->fillqsulam(col); }); diff --git a/MbDCode/MarkerFrame.h b/MbDCode/MarkerFrame.h index 8b3c990..b5e0615 100644 --- a/MbDCode/MarkerFrame.h +++ b/MbDCode/MarkerFrame.h @@ -38,6 +38,7 @@ namespace MbD { void endFramesDo(const std::function )>& f); void fillqsu(FColDsptr col) override; void fillqsuWeights(std::shared_ptr> diagMat) override; + void fillqsuddotlam(FColDsptr col) override; void fillqsulam(FColDsptr col) override; void fillqsudot(FColDsptr col) override; void fillqsudotWeights(std::shared_ptr> diagMat) override; diff --git a/MbDCode/Math.cpp b/MbDCode/Math.cpp new file mode 100644 index 0000000..b00989a --- /dev/null +++ b/MbDCode/Math.cpp @@ -0,0 +1 @@ +#include "Math.h" diff --git a/MbDCode/Math.h b/MbDCode/Math.h new file mode 100644 index 0000000..9d1d918 --- /dev/null +++ b/MbDCode/Math.h @@ -0,0 +1,8 @@ +#pragma once + +namespace MbD { + class Math + { + }; +} + diff --git a/MbDCode/MbDCode.vcxproj b/MbDCode/MbDCode.vcxproj index a8e086d..1f60cc8 100644 --- a/MbDCode/MbDCode.vcxproj +++ b/MbDCode/MbDCode.vcxproj @@ -132,6 +132,10 @@ + + + + @@ -146,6 +150,9 @@ + + + @@ -160,16 +167,31 @@ + + + + + + + + + + + + + + + @@ -190,6 +212,7 @@ + @@ -215,6 +238,7 @@ + @@ -224,6 +248,10 @@ + + + + @@ -275,6 +303,10 @@ + + + + @@ -289,6 +321,9 @@ + + + @@ -303,16 +338,31 @@ + + + + + + + + + + + + + + + @@ -334,6 +384,7 @@ + @@ -359,6 +410,7 @@ + @@ -368,6 +420,10 @@ + + + + diff --git a/MbDCode/MbDCode.vcxproj.filters b/MbDCode/MbDCode.vcxproj.filters index 28b21fe..0de51a4 100644 --- a/MbDCode/MbDCode.vcxproj.filters +++ b/MbDCode/MbDCode.vcxproj.filters @@ -438,6 +438,90 @@ Source Files + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + @@ -872,6 +956,90 @@ Header Files + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + diff --git a/MbDCode/Numeric.cpp b/MbDCode/Numeric.cpp new file mode 100644 index 0000000..15403df --- /dev/null +++ b/MbDCode/Numeric.cpp @@ -0,0 +1,44 @@ +#include +#include +#include + +#include "Numeric.h" + +using namespace MbD; + +double MbD::Numeric::arcTan0to2piYoverX(double y, double x) +{ + //"(y/x) arcTan in the range 0 to 2*pi." + //"Double arcTan0to2piY: 1.0d overX: 1.0d." + + if (x > 0.0) { + if (y >= 0) { + //"First quadrant." + return std::atan2(y, x); + } + else { + //"Forth quadrant." + return 2.0 * M_PI + std::atan2(y, x); + } + } + else { + if (x < 0.0) { + //"Second and Third quadrants." + return M_PI + std::atan2(y, x); + } + else { + //"x = 0" + if (y > 0.0) { + return M_PI / 2.0; + } + else { + if (y < 0.0) { + return 1.5 * M_PI; + } + else { + throw std::invalid_argument("atan2(0, 0) is not defined."); + } + } + } + } +} diff --git a/MbDCode/Numeric.h b/MbDCode/Numeric.h new file mode 100644 index 0000000..b384477 --- /dev/null +++ b/MbDCode/Numeric.h @@ -0,0 +1,15 @@ +#pragma once + +#include "Math.h" + +namespace MbD { + class Numeric : public Math + { + // + public: + static double arcTan0to2piYoverX(double y, double x); + + + }; +} + diff --git a/MbDCode/OrbitAnglezIecJec.cpp b/MbDCode/OrbitAnglezIecJec.cpp new file mode 100644 index 0000000..44fcd90 --- /dev/null +++ b/MbDCode/OrbitAnglezIecJec.cpp @@ -0,0 +1,103 @@ +#include + +#include "OrbitAnglezIecJec.h" +#include "Numeric.h" + +using namespace MbD; + +void MbD::OrbitAnglezIecJec::calcPostDynCorrectorIteration() +{ + auto x = xIeJeIe->value(); + auto y = yIeJeIe->value(); + auto sumOfSquares = x * x + (y * y); + auto diffOfSquares = y * y - (x * x); + auto sumOfSquaresSquared = sumOfSquares * sumOfSquares; + auto thez0to2pi = Numeric::arcTan0to2piYoverX(y, x); + thez = std::round((thez - thez0to2pi) / (2.0 * M_PI)) * (2.0 * M_PI) + thez0to2pi; + auto cosOverSSq = x / sumOfSquares; + auto sinOverSSq = y / sumOfSquares; + twoCosSinOverSSqSq = 2.0 * x * y / sumOfSquaresSquared; + dSqOverSSqSq = diffOfSquares / sumOfSquaresSquared; +} + +void MbD::OrbitAnglezIecJec::initialize() +{ + KinematicIeJe::initialize(); + this->init_xyIeJeIe(); +} + +void MbD::OrbitAnglezIecJec::initializeGlobally() +{ + xIeJeIe->initializeGlobally(); + yIeJeIe->initializeGlobally(); +} + +void MbD::OrbitAnglezIecJec::initializeLocally() +{ + xIeJeIe->initializeLocally(); + yIeJeIe->initializeLocally(); +} + +void MbD::OrbitAnglezIecJec::postInput() +{ + xIeJeIe->postInput(); + yIeJeIe->postInput(); + auto x = xIeJeIe->value(); + auto y = yIeJeIe->value(); + if (x > 0.0) { + thez = std::atan2(y, x); + } + else { + thez = Numeric::arcTan0to2piYoverX(y, x); + } + KinematicIeJe::postInput(); +} + +void MbD::OrbitAnglezIecJec::postPosICIteration() +{ + xIeJeIe->postPosICIteration(); + yIeJeIe->postPosICIteration(); + KinematicIeJe::postPosICIteration(); +} + +void MbD::OrbitAnglezIecJec::preAccIC() +{ + if (thez == 0.0) this->prePosIC(); + xIeJeIe->preAccIC(); + yIeJeIe->preAccIC(); + KinematicIeJe::preAccIC(); +} + +void MbD::OrbitAnglezIecJec::prePosIC() +{ + xIeJeIe->prePosIC(); + yIeJeIe->prePosIC(); + auto x = xIeJeIe->value(); + auto y = yIeJeIe->value(); + if (x > 0.0) { + thez = std::atan2(y, x); + } + else { + thez = Numeric::arcTan0to2piYoverX(y, x); + } + KinematicIeJe::prePosIC(); +} + +void MbD::OrbitAnglezIecJec::preVelIC() +{ + xIeJeIe->preVelIC(); + yIeJeIe->preVelIC(); + KinematicIeJe::preVelIC(); +} + +void MbD::OrbitAnglezIecJec::simUpdateAll() +{ + xIeJeIe->simUpdateAll(); + yIeJeIe->simUpdateAll(); + KinematicIeJe::simUpdateAll(); +} + +double MbD::OrbitAnglezIecJec::value() +{ + return thez; +} diff --git a/MbDCode/OrbitAnglezIecJec.h b/MbDCode/OrbitAnglezIecJec.h new file mode 100644 index 0000000..9759769 --- /dev/null +++ b/MbDCode/OrbitAnglezIecJec.h @@ -0,0 +1,28 @@ +#pragma once + +#include "KinematicIeJe.h" +#include "DispCompIecJecIe.h" + +namespace MbD { + class OrbitAnglezIecJec : public KinematicIeJe + { + //thez xIeJeIe yIeJeIe cosOverSSq sinOverSSq twoCosSinOverSSqSq dSqOverSSqSq + public: + void calcPostDynCorrectorIteration() override; + virtual void init_xyIeJeIe() = 0; + void initialize() override; + void initializeGlobally() override; + void initializeLocally() override; + void postInput() override; + void postPosICIteration() override; + void preAccIC() override; + void prePosIC() override; + void preVelIC() override; + void simUpdateAll() override; + double value() override; + + double thez, cosOverSSq, sinOverSSq, twoCosSinOverSSqSq, dSqOverSSqSq; + std::shared_ptr xIeJeIe, yIeJeIe; + }; +} + diff --git a/MbDCode/OrbitAnglezIeqcJec.cpp b/MbDCode/OrbitAnglezIeqcJec.cpp new file mode 100644 index 0000000..ecabaef --- /dev/null +++ b/MbDCode/OrbitAnglezIeqcJec.cpp @@ -0,0 +1,145 @@ +#include "OrbitAnglezIeqcJec.h" + +using namespace MbD; + +void MbD::OrbitAnglezIeqcJec::calc_ppthezpEIpEI() +{ + auto pxpEI = xIeJeIe->pvaluepEI(); + auto pypEI = yIeJeIe->pvaluepEI(); + auto ppxpEIpEI = xIeJeIe->ppvaluepEIpEI(); + auto ppypEIpEI = yIeJeIe->ppvaluepEIpEI(); + for (int i = 0; i < 4; i++) + { + auto ppthezpEIpEIi = ppthezpEIpEI->at(i); + auto ppxpEIpEIi = ppxpEIpEI->at(i); + auto ppypEIpEIi = ppypEIpEI->at(i); + auto pxpEIi = pxpEI->at(i); + auto pypEIi = pypEI->at(i); + for (int j = i; j < 4; j++) + { + auto pxpEIj = pxpEI->at(j); + auto pypEIj = pypEI->at(j); + auto term1 = (pxpEIi * pxpEIj - (pypEIi * pypEIj)) * twoCosSinOverSSqSq; + auto term2 = ppypEIpEIi->at(j) * cosOverSSq - (ppxpEIpEIi->at(j) * sinOverSSq); + auto term3 = (pypEIi * pxpEIj + (pxpEIi * pypEIj)) * dSqOverSSqSq; + auto ppthezpEIpEIij = term1 + term2 + term3; + ppthezpEIpEIi->atiput(j, ppthezpEIpEIij); + if (i < j) ppthezpEIpEI->atijput(j, i, ppthezpEIpEIij); + } + } +} + +void MbD::OrbitAnglezIeqcJec::calc_ppthezpXIpEI() +{ + auto pxpXI = xIeJeIe->pvaluepXI(); + auto pypXI = yIeJeIe->pvaluepXI(); + auto pxpEI = xIeJeIe->pvaluepEI(); + auto pypEI = yIeJeIe->pvaluepEI(); + auto ppxpXIpEI = xIeJeIe->ppvaluepXIpEI(); + auto ppypXIpEI = yIeJeIe->ppvaluepXIpEI(); + for (int i = 0; i < 3; i++) + { + auto ppthezpXIpEIi = ppthezpXIpEI->at(i); + auto ppxpXIpEIi = ppxpXIpEI->at(i); + auto ppypXIpEIi = ppypXIpEI->at(i); + auto pxpXIi = pxpXI->at(i); + auto pypXIi = pypXI->at(i); + for (int j = 0; j < 4; j++) + { + auto pxpEIj = pxpEI->at(j); + auto pypEIj = pypEI->at(j); + auto term1 = (pxpXIi * pxpEIj - (pypXIi * pypEIj)) * twoCosSinOverSSqSq; + auto term2 = ppypXIpEIi->at(j) * cosOverSSq - (ppxpXIpEIi->at(j) * sinOverSSq); + auto term3 = (pypXIi * pxpEIj + (pxpXIi * pypEIj)) * dSqOverSSqSq; + ppthezpXIpEIi->atiput(j, term1 + term2 + term3); + } + } +} + +void MbD::OrbitAnglezIeqcJec::calc_ppthezpXIpXI() +{ + //ppxpXIpXI = 0 + //ppypXIpXI = 0 + + auto pxpXI = xIeJeIe->pvaluepXI(); + auto pypXI = yIeJeIe->pvaluepXI(); + for (int i = 0; i < 3; i++) + { + auto ppthezpXIpXIi = ppthezpXIpXI->at(i); + auto pxpXIi = pxpXI->at(i); + auto pypXIi = pypXI->at(i); + for (int j = 0; j < 3; j++) + { + auto pxpXIj = pxpXI->at(j); + auto pypXIj = pypXI->at(j); + auto term1 = (pxpXIi * pxpXIj - (pypXIi * pypXIj)) * twoCosSinOverSSqSq; + auto term3 = (pypXIi * pxpXIj + (pxpXIi * pypXIj)) * dSqOverSSqSq; + ppthezpXIpXIi->atiput(j, term1 + term3); + } + } +} + +void MbD::OrbitAnglezIeqcJec::calc_pthezpEI() +{ + auto pxpEI = xIeJeIe->pvaluepEI(); + auto pypEI = yIeJeIe->pvaluepEI(); + for (int i = 0; i < 4; i++) + { + pthezpEI->atiput(i, pypEI->at(i) * cosOverSSq - (pxpEI->at(i) * sinOverSSq)); + } +} + +void MbD::OrbitAnglezIeqcJec::calc_pthezpXI() +{ + auto pxpXI = xIeJeIe->pvaluepXI(); + auto pypXI = yIeJeIe->pvaluepXI(); + for (int i = 0; i < 3; i++) + { + pthezpXI->atiput(i, pypXI->at(i) * cosOverSSq - (pxpXI->at(i) * sinOverSSq)); + } +} + +void MbD::OrbitAnglezIeqcJec::calcPostDynCorrectorIteration() +{ + OrbitAnglezIecJec::calcPostDynCorrectorIteration(); + this->calc_pthezpXI(); + this->calc_pthezpEI(); + this->calc_ppthezpXIpXI(); + this->calc_ppthezpXIpEI(); + this->calc_ppthezpEIpEI(); +} + +void MbD::OrbitAnglezIeqcJec::initialize() +{ + OrbitAnglezIecJec::initialize(); + pthezpXI = std::make_shared>(3); + pthezpEI = std::make_shared>(4); + ppthezpXIpXI = std::make_shared>(3, 3); + ppthezpXIpEI = std::make_shared>(3, 4); + ppthezpEIpEI = std::make_shared>(4, 4); +} + +FMatDsptr MbD::OrbitAnglezIeqcJec::ppvaluepEIpEI() +{ + return ppthezpEIpEI; +} + +FMatDsptr MbD::OrbitAnglezIeqcJec::ppvaluepXIpEI() +{ + return ppthezpXIpEI; +} + +FMatDsptr MbD::OrbitAnglezIeqcJec::ppvaluepXIpXI() +{ + return ppthezpXIpXI; +} + +FRowDsptr MbD::OrbitAnglezIeqcJec::pvaluepEI() +{ + return pthezpEI; +} + +FRowDsptr MbD::OrbitAnglezIeqcJec::pvaluepXI() +{ + return pthezpXI; +} diff --git a/MbDCode/OrbitAnglezIeqcJec.h b/MbDCode/OrbitAnglezIeqcJec.h new file mode 100644 index 0000000..7009d9e --- /dev/null +++ b/MbDCode/OrbitAnglezIeqcJec.h @@ -0,0 +1,27 @@ +#pragma once + +#include "OrbitAnglezIecJec.h" + +namespace MbD { + class OrbitAnglezIeqcJec : public OrbitAnglezIecJec + { + //pthezpXI pthezpEI ppthezpXIpXI ppthezpXIpEI ppthezpEIpEI + public: + void calc_ppthezpEIpEI(); + void calc_ppthezpXIpEI(); + void calc_ppthezpXIpXI(); + void calc_pthezpEI(); + void calc_pthezpXI(); + void calcPostDynCorrectorIteration() override; + void initialize() override; + FMatDsptr ppvaluepEIpEI() override; + FMatDsptr ppvaluepXIpEI() override; + FMatDsptr ppvaluepXIpXI() override; + FRowDsptr pvaluepEI() override; + FRowDsptr pvaluepXI() override; + + FRowDsptr pthezpXI, pthezpEI; + FMatDsptr ppthezpXIpXI, ppthezpXIpEI, ppthezpEIpEI; + }; +} + diff --git a/MbDCode/OrbitAnglezIeqcJeqc.cpp b/MbDCode/OrbitAnglezIeqcJeqc.cpp new file mode 100644 index 0000000..242c30c --- /dev/null +++ b/MbDCode/OrbitAnglezIeqcJeqc.cpp @@ -0,0 +1,273 @@ +#include "OrbitAnglezIeqcJeqc.h" + +using namespace MbD; + +void MbD::OrbitAnglezIeqcJeqc::calc_ppthezpEIpEJ() +{ + auto pxpEI = xIeJeIe->pvaluepEI(); + auto pypEI = yIeJeIe->pvaluepEI(); + auto pxpEJ = xIeJeIe->pvaluepEJ(); + auto pypEJ = yIeJeIe->pvaluepEJ(); + auto ppxpEIpEJ = xIeJeIe->ppvaluepEIpEJ(); + auto ppypEIpEJ = yIeJeIe->ppvaluepEIpEJ(); + for (int i = 0; i < 4; i++) + { + auto ppthezpEIpEJi = ppthezpEIpEJ->at(i); + auto ppxpEIpEJi = ppxpEIpEJ->at(i); + auto ppypEIpEJi = ppypEIpEJ->at(i); + auto pxpEIi = pxpEI->at(i); + auto pypEIi = pypEI->at(i); + for (int j = 0; j < 4; j++) + { + auto pxpEJj = pxpEJ->at(j); + auto pypEJj = pypEJ->at(j); + auto term1 = (pxpEIi * pxpEJj - (pypEIi * pypEJj)) * twoCosSinOverSSqSq; + auto term2 = ppypEIpEJi->at(j) * cosOverSSq - (ppxpEIpEJi->at(j) * sinOverSSq); + auto term3 = (pypEIi * pxpEJj + (pxpEIi * pypEJj)) * dSqOverSSqSq; + ppthezpEIpEJi->atiput(j, term1 + term2 + term3); + } + } +} + +void MbD::OrbitAnglezIeqcJeqc::calc_ppthezpEIpXJ() +{ + //ppxpEIpXJ = 0 + //ppypEIpXJ = 0 + + auto pxpEI = xIeJeIe->pvaluepEI(); + auto pypEI = yIeJeIe->pvaluepEI(); + auto pxpXJ = xIeJeIe->pvaluepXJ(); + auto pypXJ = yIeJeIe->pvaluepXJ(); + for (int i = 0; i < 4; i++) + { + auto ppthezpEIpXJi = ppthezpEIpXJ->at(i); + auto pxpEIi = pxpEI->at(i); + auto pypEIi = pypEI->at(i); + for (int j = 0; j < 3; j++) + { + auto pxpXJj = pxpXJ->at(j); + auto pypXJj = pypXJ->at(j); + auto term1 = (pxpEIi * pxpXJj - (pypEIi * pypXJj)) * twoCosSinOverSSqSq; + auto term3 = (pypEIi * pxpXJj + (pxpEIi * pypXJj)) * dSqOverSSqSq; + ppthezpEIpXJi->atiput(j, term1 + term3); + } + } +} + +void MbD::OrbitAnglezIeqcJeqc::calc_ppthezpEJpEJ() +{ + auto pxpEJ = xIeJeIe->pvaluepEJ(); + auto pypEJ = yIeJeIe->pvaluepEJ(); + auto ppxpEJpEJ = xIeJeIe->ppvaluepEJpEJ(); + auto ppypEJpEJ = yIeJeIe->ppvaluepEJpEJ(); + for (int i = 0; i < 4; i++) + { + auto ppthezpEJpEJi = ppthezpEJpEJ->at(i); + auto ppxpEJpEJi = ppxpEJpEJ->at(i); + auto ppypEJpEJi = ppypEJpEJ->at(i); + auto pxpEJi = pxpEJ->at(i); + auto pypEJi = pypEJ->at(i); + for (int j = i; j < 4; j++) + { + auto pxpEJj = pxpEJ->at(j); + auto pypEJj = pypEJ->at(j); + auto term1 = (pxpEJi * pxpEJj - (pypEJi * pypEJj)) * twoCosSinOverSSqSq; + auto term2 = ppypEJpEJi->at(j) * cosOverSSq - (ppxpEJpEJi->at(j) * sinOverSSq); + auto term3 = (pypEJi * pxpEJj + (pxpEJi * pypEJj)) * dSqOverSSqSq; + auto ppthezpEJpEJij = term1 + term2 + term3; + ppthezpEJpEJi->atiput(j, ppthezpEJpEJij); + if (i < j) ppthezpEJpEJ->atijput(j, i, ppthezpEJpEJij); + } + } +} + +void MbD::OrbitAnglezIeqcJeqc::calc_ppthezpXIpEJ() +{ + //ppxpXIpEJ = 0 + //ppypXIpEJ = 0 + + auto pxpXI = xIeJeIe->pvaluepXI(); + auto pypXI = yIeJeIe->pvaluepXI(); + auto pxpEJ = xIeJeIe->pvaluepEJ(); + auto pypEJ = yIeJeIe->pvaluepEJ(); + for (int i = 0; i < 3; i++) + { + auto ppthezpXIpEJi = ppthezpXIpEJ->at(i); + auto pxpXIi = pxpXI->at(i); + auto pypXIi = pypXI->at(i); + for (int j = 0; j < 4; j++) + { + auto pxpEJj = pxpEJ->at(j); + auto pypEJj = pypEJ->at(j); + auto term1 = (pxpXIi * pxpEJj - (pypXIi * pypEJj)) * twoCosSinOverSSqSq; + auto term3 = (pypXIi * pxpEJj + (pxpXIi * pypEJj)) * dSqOverSSqSq; + ppthezpXIpEJi->atiput(j, term1 + term3); + } + } +} + +void MbD::OrbitAnglezIeqcJeqc::calc_ppthezpXIpXJ() +{ + //ppxpXIpXJ = 0 + //ppypXIpXJ = 0 + + auto pxpXI = xIeJeIe->pvaluepXI(); + auto pypXI = yIeJeIe->pvaluepXI(); + auto pxpXJ = xIeJeIe->pvaluepXJ(); + auto pypXJ = yIeJeIe->pvaluepXJ(); + for (int i = 0; i < 3; i++) + { + auto ppthezpXIpXJi = ppthezpXIpXJ->at(i); + auto pxpXIi = pxpXI->at(i); + auto pypXIi = pypXI->at(i); + for (int j = 0; j < 3; j++) + { + auto pxpXJj = pxpXJ->at(j); + auto pypXJj = pypXJ->at(j); + auto term1 = (pxpXIi * pxpXJj - (pypXIi * pypXJj)) * twoCosSinOverSSqSq; + auto term3 = (pypXIi * pxpXJj + (pxpXIi * pypXJj)) * dSqOverSSqSq; + ppthezpXIpXJi->atiput(j, term1 + term3); + } + } +} + +void MbD::OrbitAnglezIeqcJeqc::calc_ppthezpXJpEJ() +{ + //ppxpXJpEJ = 0 + //ppypXJpEJ = 0 + + auto pxpXJ = xIeJeIe->pvaluepXJ(); + auto pypXJ = yIeJeIe->pvaluepXJ(); + auto pxpEJ = xIeJeIe->pvaluepEJ(); + auto pypEJ = yIeJeIe->pvaluepEJ(); + for (int i = 0; i < 3; i++) + { + auto ppthezpXJpEJi = ppthezpXJpEJ->at(i); + auto pxpXJi = pxpXJ->at(i); + auto pypXJi = pypXJ->at(i); + for (int j = 0; j < 4; j++) + { + auto pxpEJj = pxpEJ->at(j); + auto pypEJj = pypEJ->at(j); + auto term1 = (pxpXJi * pxpEJj - (pypXJi * pypEJj)) * twoCosSinOverSSqSq; + auto term3 = (pypXJi * pxpEJj + (pxpXJi * pypEJj)) * dSqOverSSqSq; + ppthezpXJpEJi->atiput(j, term1 + term3); + } + } +} + +void MbD::OrbitAnglezIeqcJeqc::calc_ppthezpXJpXJ() +{ + //ppxpXJpXJ = 0 + //ppypXJpXJ = 0 + + auto pxpXJ = xIeJeIe->pvaluepXJ(); + auto pypXJ = yIeJeIe->pvaluepXJ(); + for (int i = 0; i < 3; i++) + { + auto ppthezpXJpXJi = ppthezpXJpXJ->at(i); + auto pxpXJi = pxpXJ->at(i); + auto pypXJi = pypXJ->at(i); + for (int j = 0; j < 3; j++) + { + auto pxpXJj = pxpXJ->at(j); + auto pypXJj = pypXJ->at(j); + auto term1 = (pxpXJi * pxpXJj - (pypXJi * pypXJj)) * twoCosSinOverSSqSq; + auto term3 = (pypXJi * pxpXJj + (pxpXJi * pypXJj)) * dSqOverSSqSq; + ppthezpXJpXJi->atiput(j, term1 + term3); + } + } +} + +void MbD::OrbitAnglezIeqcJeqc::calc_pthezpEJ() +{ + auto pxpEJ = xIeJeIe->pvaluepEJ(); + auto pypEJ = yIeJeIe->pvaluepEJ(); + for (int i = 0; i < 4; i++) + { + pthezpEJ->atiput(i, pypEJ->at(i) * cosOverSSq - (pxpEJ->at(i) * sinOverSSq)); + } +} + +void MbD::OrbitAnglezIeqcJeqc::calc_pthezpXJ() +{ + auto pxpXJ = xIeJeIe->pvaluepXJ(); + auto pypXJ = yIeJeIe->pvaluepXJ(); + for (int i = 0; i < 3; i++) + { + pthezpXJ->atiput(i, pypXJ->at(i) * cosOverSSq - (pxpXJ->at(i) * sinOverSSq)); + } +} + +void MbD::OrbitAnglezIeqcJeqc::calcPostDynCorrectorIteration() +{ + OrbitAnglezIeqcJec::calcPostDynCorrectorIteration(); + this->calc_pthezpXJ(); + this->calc_pthezpEJ(); + this->calc_ppthezpXIpXJ(); + this->calc_ppthezpXIpEJ(); + this->calc_ppthezpEIpXJ(); + this->calc_ppthezpEIpEJ(); + this->calc_ppthezpXJpXJ(); + this->calc_ppthezpXJpEJ(); + this->calc_ppthezpEJpEJ(); +} + +void MbD::OrbitAnglezIeqcJeqc::initialize() +{ + OrbitAnglezIeqcJec::initialize(); + pthezpXJ = std::make_shared>(3); + pthezpEJ = std::make_shared>(4); + ppthezpXIpXJ = std::make_shared>(3, 3); + ppthezpXIpEJ = std::make_shared>(3, 4); + ppthezpEIpXJ = std::make_shared>(4, 3); + ppthezpEIpEJ = std::make_shared>(4, 4); + ppthezpXJpXJ = std::make_shared>(3, 3); + ppthezpXJpEJ = std::make_shared>(3, 4); + ppthezpEJpEJ = std::make_shared>(4, 4); +} + +FMatDsptr MbD::OrbitAnglezIeqcJeqc::ppvaluepEIpEJ() +{ + return ppthezpEIpEJ; +} + +FMatDsptr MbD::OrbitAnglezIeqcJeqc::ppvaluepEIpXJ() +{ + return ppthezpEIpXJ; +} + +FMatDsptr MbD::OrbitAnglezIeqcJeqc::ppvaluepEJpEJ() +{ + return ppthezpEJpEJ; +} + +FMatDsptr MbD::OrbitAnglezIeqcJeqc::ppvaluepXIpEJ() +{ + return ppthezpXIpEJ; +} + +FMatDsptr MbD::OrbitAnglezIeqcJeqc::ppvaluepXIpXJ() +{ + return ppthezpXIpXJ; +} + +FMatDsptr MbD::OrbitAnglezIeqcJeqc::ppvaluepXJpEJ() +{ + return ppthezpXJpEJ; +} + +FMatDsptr MbD::OrbitAnglezIeqcJeqc::ppvaluepXJpXJ() +{ + return ppthezpXJpXJ; +} + +FRowDsptr MbD::OrbitAnglezIeqcJeqc::pvaluepEJ() +{ + return pthezpEJ; +} + +FRowDsptr MbD::OrbitAnglezIeqcJeqc::pvaluepXJ() +{ + return pthezpXJ; +} diff --git a/MbDCode/OrbitAnglezIeqcJeqc.h b/MbDCode/OrbitAnglezIeqcJeqc.h new file mode 100644 index 0000000..2a4dcce --- /dev/null +++ b/MbDCode/OrbitAnglezIeqcJeqc.h @@ -0,0 +1,36 @@ +#pragma once + +#include "OrbitAnglezIeqcJec.h" + +namespace MbD { + class OrbitAnglezIeqcJeqc : public OrbitAnglezIeqcJec + { + //pthezpXJ pthezpEJ ppthezpXIpXJ ppthezpXIpEJ ppthezpEIpXJ ppthezpEIpEJ ppthezpXJpXJ ppthezpXJpEJ ppthezpEJpEJ + public: + void calc_ppthezpEIpEJ(); + void calc_ppthezpEIpXJ(); + void calc_ppthezpEJpEJ(); + void calc_ppthezpXIpEJ(); + void calc_ppthezpXIpXJ(); + void calc_ppthezpXJpEJ(); + void calc_ppthezpXJpXJ(); + void calc_pthezpEJ(); + void calc_pthezpXJ(); + void calcPostDynCorrectorIteration() override; + void initialize() override; + FMatDsptr ppvaluepEIpEJ() override; + FMatDsptr ppvaluepEIpXJ() override; + FMatDsptr ppvaluepEJpEJ() override; + FMatDsptr ppvaluepXIpEJ() override; + FMatDsptr ppvaluepXIpXJ() override; + FMatDsptr ppvaluepXJpEJ() override; + FMatDsptr ppvaluepXJpXJ() override; + FRowDsptr pvaluepEJ() override; + FRowDsptr pvaluepXJ() override; + + + FRowDsptr pthezpXJ, pthezpEJ; + FMatDsptr ppthezpXIpXJ, ppthezpXIpEJ, ppthezpEIpXJ, ppthezpEIpEJ, ppthezpXJpXJ, ppthezpXJpEJ, ppthezpEJpEJ; + }; +} + diff --git a/MbDCode/Part.cpp b/MbDCode/Part.cpp index fe3784b..5b018ee 100644 --- a/MbDCode/Part.cpp +++ b/MbDCode/Part.cpp @@ -266,6 +266,11 @@ void Part::fillqsuWeights(std::shared_ptr> diagMat) partFrame->fillqsuWeights(diagMat); } +void MbD::Part::fillqsuddotlam(FColDsptr col) +{ + partFrame->fillqsuddotlam(col); +} + void Part::fillqsulam(FColDsptr col) { partFrame->fillqsulam(col); @@ -365,12 +370,6 @@ void Part::postPosIC() this->calcmE(); } -void Part::outputStates() -{ - Item::outputStates(); - partFrame->outputStates(); -} - void Part::preDyn() { partFrame->preDyn(); diff --git a/MbDCode/Part.h b/MbDCode/Part.h index 1e1066f..2f98d7e 100644 --- a/MbDCode/Part.h +++ b/MbDCode/Part.h @@ -62,6 +62,7 @@ namespace MbD { void fillConstraints(std::shared_ptr>> allConstraints) override; void fillqsu(FColDsptr col) override; void fillqsuWeights(std::shared_ptr> diagMat) override; + void fillqsuddotlam(FColDsptr col) override; void fillqsulam(FColDsptr col) override; void fillqsudot(FColDsptr col) override; void fillqsudotWeights(std::shared_ptr> diagMat) override; @@ -77,7 +78,6 @@ namespace MbD { void reactivateRedundantConstraints() override; void constraintsReport() override; void postPosIC() override; - void outputStates() override; void preDyn() override; void storeDynState() override; void fillPosKineError(FColDsptr col) override; diff --git a/MbDCode/PartFrame.cpp b/MbDCode/PartFrame.cpp index 938daeb..14d5277 100644 --- a/MbDCode/PartFrame.cpp +++ b/MbDCode/PartFrame.cpp @@ -26,7 +26,7 @@ System* PartFrame::root() void PartFrame::initialize() { aGeu = CREATE::With(); - aGeu->setOwner(this); + aGeu->owner = this; aGabs = std::make_shared>>(); markerFrames = std::make_shared>>(); } @@ -116,7 +116,7 @@ void PartFrame::addMarkerFrame(std::shared_ptr markerFrame) EndFrmcptr PartFrame::endFrame(std::string name) { - auto match = std::find_if(markerFrames->begin(), markerFrames->end(), [&](auto& mkr) {return mkr->getName() == name; }); + auto match = std::find_if(markerFrames->begin(), markerFrames->end(), [&](auto& mkr) {return mkr->name == name; }); return (*match)->endFrames->at(0); } @@ -170,7 +170,7 @@ void PartFrame::constraintsReport() }); if (aGeu->isRedundant()) redunCons->push_back(aGeu); if (redunCons->size() > 0) { - std::string str = "MbD: " + part->classname() + std::string(" ") + part->getName() + " has the following constraint(s) removed: "; + std::string str = "MbD: " + part->classname() + std::string(" ") + part->name + " has the following constraint(s) removed: "; this->logString(str); std::for_each(redunCons->begin(), redunCons->end(), [&](auto& con) { str = "MbD: " + std::string(" ") + std::string(typeid(*con).name()); @@ -259,6 +259,15 @@ void PartFrame::fillqsuWeights(std::shared_ptr> diagMat) markerFramesDo([&](std::shared_ptr markerFrame) { markerFrame->fillqsuWeights(diagMat); }); } +void MbD::PartFrame::fillqsuddotlam(FColDsptr col) +{ + col->atiputFullColumn(iqX, qXddot); + col->atiputFullColumn(iqE, qEddot); + markerFramesDo([&](std::shared_ptr markerFrame) { markerFrame->fillqsuddotlam(col); }); + aGeu->fillqsuddotlam(col); + aGabsDo([&](std::shared_ptr con) { con->fillqsuddotlam(col); }); +} + void PartFrame::fillqsulam(FColDsptr col) { col->atiputFullColumn(iqX, qX); @@ -350,20 +359,6 @@ void PartFrame::postPosIC() aGabsDo([](std::shared_ptr con) { con->postPosIC(); }); } -void PartFrame::outputStates() -{ - std::stringstream ss; - ss << "qX = "; - qX->printOn(ss); - ss << std::endl; - ss << "qE = "; - qE->printOn(ss); - auto str = ss.str(); - this->logString(str); - aGeu->outputStates(); - aGabsDo([](std::shared_ptr con) { con->outputStates(); }); -} - void PartFrame::preDyn() { markerFramesDo([](std::shared_ptr markerFrame) { markerFrame->preDyn(); }); @@ -505,7 +500,7 @@ void PartFrame::asFixed() { for (int i = 0; i < 6; i++) { auto con = CREATE::With(i); - con->setOwner(this); + con->owner = this; aGabs->push_back(con); } } diff --git a/MbDCode/PartFrame.h b/MbDCode/PartFrame.h index 5be6afb..551222e 100644 --- a/MbDCode/PartFrame.h +++ b/MbDCode/PartFrame.h @@ -69,6 +69,7 @@ namespace MbD { void fillConstraints(std::shared_ptr>> allConstraints) override; void fillqsu(FColDsptr col) override; void fillqsuWeights(std::shared_ptr> diagMat) override; + void fillqsuddotlam(FColDsptr col) override; void fillqsulam(FColDsptr col) override; void fillqsudot(FColDsptr col) override; void fillqsudotWeights(std::shared_ptr> diagMat) override; @@ -82,7 +83,6 @@ namespace MbD { void fillPosICError(FColDsptr col) override; void fillPosICJacob(SpMatDsptr mat) override; void postPosIC() override; - void outputStates() override; void preDyn() override; void storeDynState() override; void fillPosKineError(FColDsptr col) override; diff --git a/MbDCode/PosICKineNewtonRaphson.cpp b/MbDCode/PosICKineNewtonRaphson.cpp index a698733..253fde0 100644 --- a/MbDCode/PosICKineNewtonRaphson.cpp +++ b/MbDCode/PosICKineNewtonRaphson.cpp @@ -43,3 +43,9 @@ void PosICKineNewtonRaphson::assignEquationNumbers() nEqns = eqnNo; //C++ uses index 0. n = nEqns; } + +void MbD::PosICKineNewtonRaphson::preRun() +{ + system->Solver::logString("MbD: Solving for quasi kinematic position."); + PosNewtonRaphson::preRun(); +} diff --git a/MbDCode/PosICKineNewtonRaphson.h b/MbDCode/PosICKineNewtonRaphson.h index dddfd80..f69884d 100644 --- a/MbDCode/PosICKineNewtonRaphson.h +++ b/MbDCode/PosICKineNewtonRaphson.h @@ -9,6 +9,7 @@ namespace MbD { public: void initializeGlobally() override; void assignEquationNumbers() override; + void preRun() override; }; } diff --git a/MbDCode/QuasiIntegrator.cpp b/MbDCode/QuasiIntegrator.cpp index 14e5dd4..857fdf1 100644 --- a/MbDCode/QuasiIntegrator.cpp +++ b/MbDCode/QuasiIntegrator.cpp @@ -102,7 +102,7 @@ void QuasiIntegrator::checkForDiscontinuity() epsilon = std::abs(t) * epsilon; tstartNew = ((direction * t) + epsilon) / direction; } - system->partsJointsMotionsForcesTorquesDo([&](std::shared_ptr item) { tstartNew = item->checkForDynDiscontinuityBetween(tprevious, tstartNew); }); + system->partsJointsMotionsForcesTorquesDo([&](std::shared_ptr item) { tstartNew = item->checkForDynDiscontinuityBetweenand(tprevious, tstartNew); }); if ((direction * tstartNew) > (direction * t)) { //"No discontinuity in step" return; diff --git a/MbDCode/RedundantConstraint.cpp b/MbDCode/RedundantConstraint.cpp index 6f5e4c9..07e33fd 100644 --- a/MbDCode/RedundantConstraint.cpp +++ b/MbDCode/RedundantConstraint.cpp @@ -22,6 +22,10 @@ ConstraintType RedundantConstraint::type() return redundant; } +void MbD::RedundantConstraint::fillqsuddotlam(FColDsptr col) +{ +} + void RedundantConstraint::fillqsulam(FColDsptr col) { } diff --git a/MbDCode/RedundantConstraint.h b/MbDCode/RedundantConstraint.h index 6dc2a65..71673e1 100644 --- a/MbDCode/RedundantConstraint.h +++ b/MbDCode/RedundantConstraint.h @@ -11,14 +11,15 @@ namespace MbD { bool isRedundant() override; std::string classname() override; ConstraintType type() override; + void fillqsuddotlam(FColDsptr col) override; void fillqsulam(FColDsptr col) override; void postInput() override; void prePosIC() override; - void fillEssenConstraints(std::shared_ptr sptr, std::shared_ptr>> essenConstraints); - void fillDispConstraints(std::shared_ptr sptr, std::shared_ptr>> dispConstraints); - void fillPerpenConstraints(std::shared_ptr sptr, std::shared_ptr>> perpenConstraints); - void fillConstraints(std::shared_ptr sptr, std::shared_ptr>> redunConstraints); - void fillRedundantConstraints(std::shared_ptr sptr, std::shared_ptr>> allConstraints); + void fillEssenConstraints(std::shared_ptr sptr, std::shared_ptr>> essenConstraints) override; + void fillDispConstraints(std::shared_ptr sptr, std::shared_ptr>> dispConstraints) override; + void fillPerpenConstraints(std::shared_ptr sptr, std::shared_ptr>> perpenConstraints) override; + void fillConstraints(std::shared_ptr sptr, std::shared_ptr>> redunConstraints) override; + void fillRedundantConstraints(std::shared_ptr sptr, std::shared_ptr>> allConstraints) override; void setqsulam(FColDsptr col) override; void setqsudotlam(FColDsptr col) override; void fillPosICError(FColDsptr col) override; diff --git a/MbDCode/SparseVector.h b/MbDCode/SparseVector.h index b634e30..bf08b53 100644 --- a/MbDCode/SparseVector.h +++ b/MbDCode/SparseVector.h @@ -18,7 +18,7 @@ namespace MbD { int index; T value; for (auto& element : pair) { - if (i == 0) index = (int)std::round(element); ; + if (i == 0) index = (int)std::round(element); if (i == 1) value = element; i++; } diff --git a/MbDCode/Symbolic.h b/MbDCode/Symbolic.h index cda65b3..1e09e23 100644 --- a/MbDCode/Symbolic.h +++ b/MbDCode/Symbolic.h @@ -4,9 +4,11 @@ #include #include +#include "Math.h" + namespace MbD { - class Symbolic + class Symbolic : public Math { public: Symbolic(); diff --git a/MbDCode/SystemSolver.cpp b/MbDCode/SystemSolver.cpp index 91b0ee3..d26bd25 100644 --- a/MbDCode/SystemSolver.cpp +++ b/MbDCode/SystemSolver.cpp @@ -96,11 +96,11 @@ bool SystemSolver::needToRedoPosIC() auto newSet = std::make_shared>(); for (auto& con : *allRedunCons) { auto aaa = std::static_pointer_cast(con); - auto& bbb = aaa->constraint->getName(); + auto& bbb = aaa->constraint->name; newSet->insert(bbb); } //std::transform(allRedunCons->begin(), allRedunCons->end(), newSet->begin(), [](auto con) { - // return std::static_pointer_cast(con)->constraint->getName(); + // return std::static_pointer_cast(con)->constraint->name; // }); if (newSet->empty()) return false; auto itr = std::find_if(setsOfRedundantConstraints->begin(), setsOfRedundantConstraints->end(), [&](auto& set) { diff --git a/MbDCode/TranslationConstraintIqcJc.cpp b/MbDCode/TranslationConstraintIqcJc.cpp index b349d35..5f34d08 100644 --- a/MbDCode/TranslationConstraintIqcJc.cpp +++ b/MbDCode/TranslationConstraintIqcJc.cpp @@ -28,8 +28,9 @@ void TranslationConstraintIqcJc::calcPostDynCorrectorIteration() void TranslationConstraintIqcJc::useEquationNumbers() { - iqXI = std::static_pointer_cast(frmI)->iqX(); - iqEI = std::static_pointer_cast(frmI)->iqE(); + auto frmIeqc = std::static_pointer_cast(frmI); + iqXI = frmIeqc->iqX(); + iqEI = frmIeqc->iqE(); } void TranslationConstraintIqcJc::fillPosICError(FColDsptr col) diff --git a/MbDCode/TranslationConstraintIqcJqc.cpp b/MbDCode/TranslationConstraintIqcJqc.cpp index 5d8089a..4d06875 100644 --- a/MbDCode/TranslationConstraintIqcJqc.cpp +++ b/MbDCode/TranslationConstraintIqcJqc.cpp @@ -28,8 +28,9 @@ void TranslationConstraintIqcJqc::calcPostDynCorrectorIteration() void TranslationConstraintIqcJqc::useEquationNumbers() { TranslationConstraintIqcJc::useEquationNumbers(); - iqXJ = std::static_pointer_cast(frmJ)->iqX(); - iqEJ = std::static_pointer_cast(frmJ)->iqE(); + auto frmJeqc = std::static_pointer_cast(frmJ); + iqXJ = frmJeqc->iqX(); + iqEJ = frmJeqc->iqE(); } void TranslationConstraintIqcJqc::fillPosICError(FColDsptr col)