kindred/solver
1
0
Fork 0
Code Issues 10 Pull Requests 1 Actions Packages Projects Releases Wiki Activity

add DistanceConstraintIJ and AngleConstraintIJ

Browse Source
This commit is contained in:
Aik-Siong Koh
2023-07-08 23:20:27 -06:00
parent cb27f2344b
commit 718b17a2cf
134 changed files with 4218 additions and 556 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
MbDCode/AbsConstraint.cpp
View File

@@ -12,11 +12,6 @@ AbsConstraint::AbsConstraint(int i)
axis = i;
}
void AbsConstraint::initialize()
{
Constraint::initialize();
}
void AbsConstraint::calcPostDynCorrectorIteration()
{
if (axis < 3) {

36
MbDCode/AbsConstraint.h
View File

@@ -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;
};
}

2
MbDCode/AccNewtonRaphson.cpp
View File

@@ -56,7 +56,7 @@ void AccNewtonRaphson::fillY()
y->zeroSelf();
system->partsJointsMotionsForcesTorquesDo([&](std::shared_ptr<Item> item) {
item->fillAccICIterError(y);
//std::cout << item->getName() << *y << std::endl;
//std::cout << item->name << *y << std::endl;
});
std::cout << *y << std::endl;
}

30
MbDCode/AngleJoint.cpp Normal file
View File

@@ -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<AtPointConstraintIJ>::ConstraintWith(frmI, frmJ, 0));
addConstraint(CREATE<AtPointConstraintIJ>::ConstraintWith(frmI, frmJ, 1));
addConstraint(CREATE<AtPointConstraintIJ>::ConstraintWith(frmI, frmJ, 2));
addConstraint(CREATE<DirectionCosineConstraintIJ>::ConstraintWith(frmI, frmJ, 2, 0));
addConstraint(CREATE<DirectionCosineConstraintIJ>::ConstraintWith(frmI, frmJ, 2, 1));
this->root()->hasChanged = true;
}
else {
Joint::initializeGlobally();
}
}

17
MbDCode/AngleJoint.h Normal file
View File

@@ -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;
};
}

87
MbDCode/AngleZIecJec.cpp Normal file
View File

@@ -0,0 +1,87 @@
#include <corecrt_math_defines.h>
#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;
}

28
MbDCode/AngleZIecJec.h Normal file
View File

@@ -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<DirectionCosineIeqcJec> aA00IeJe, aA10IeJe;
};
}

63
MbDCode/AngleZIeqcJec.cpp Normal file
View File

@@ -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<DirectionCosineIeqcJec>::With(frmI, frmJ, 0, 0);
aA10IeJe = CREATE<DirectionCosineIeqcJec>::With(frmI, frmJ, 1, 0);
}
void MbD::AngleZIeqcJec::initialize()
{
AngleZIecJec::initialize();
pthezpEI = std::make_shared<FullRow<double>>(4);
ppthezpEIpEI = std::make_shared<FullMatrix<double>>(4, 4);
}
FMatDsptr MbD::AngleZIeqcJec::ppvaluepEIpEI()
{
return ppthezpEIpEI;
}
FRowDsptr MbD::AngleZIeqcJec::pvaluepEI()
{
return pthezpEI;
}

20
MbDCode/AngleZIeqcJec.h Normal file
View File

@@ -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;
};
}

89
MbDCode/AngleZIeqcJeqc.cpp Normal file
View File

@@ -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<DirectionCosineIeqcJeqc>::With(frmI, frmJ, 0, 0);
aA10IeJe = CREATE<DirectionCosineIeqcJeqc>::With(frmI, frmJ, 1, 0);
}
void MbD::AngleZIeqcJeqc::initialize()
{
AngleZIeqcJec::initialize();
pthezpEJ = std::make_shared<FullRow<double>>(4);
ppthezpEIpEJ = std::make_shared<FullMatrix<double>>(4, 4);
ppthezpEJpEJ = std::make_shared<FullMatrix<double>>(4, 4);
}
FMatDsptr MbD::AngleZIeqcJeqc::ppvaluepEIpEJ()
{
return ppthezpEIpEJ;
}
FMatDsptr MbD::AngleZIeqcJeqc::ppvaluepEJpEJ()
{
return ppthezpEJpEJ;
}
FRowDsptr MbD::AngleZIeqcJeqc::pvaluepEJ()
{
return pthezpEJ;
}

21
MbDCode/AngleZIeqcJeqc.h Normal file
View File

@@ -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;
};
}

1
MbDCode/AnyPosICNewtonRaphson.h
View File

@@ -15,6 +15,7 @@ namespace MbD {
void fillY() override;
void fillPyPx() override;
void passRootToSystem() override;
void assignEquationNumbers() = 0;
int nqsu = -1;
std::shared_ptr<FullColumn<double>> qsuOld;

11
MbDCode/AtPointConstraintIJ.h
View File

@@ -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;

5
MbDCode/AtPointConstraintIqcJc.cpp
View File

@@ -29,8 +29,9 @@ void AtPointConstraintIqcJc::calcPostDynCorrectorIteration()
void AtPointConstraintIqcJc::useEquationNumbers()
{
iqXIminusOnePlusAxis = std::static_pointer_cast<EndFrameqc>(frmI)->iqX() + axis;
iqEI = std::static_pointer_cast<EndFrameqc>(frmI)->iqE();
auto frmIeqc = std::static_pointer_cast<EndFrameqc>(frmI);
iqXIminusOnePlusAxis = frmIeqc->iqX() + axis;
iqEI = frmIeqc->iqE();
}
void AtPointConstraintIqcJc::fillPosICError(FColDsptr col)

13
MbDCode/AtPointConstraintIqcJc.h
View File

@@ -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;

5
MbDCode/AtPointConstraintIqcJqc.cpp
View File

@@ -30,8 +30,9 @@ void AtPointConstraintIqcJqc::calcPostDynCorrectorIteration()
void AtPointConstraintIqcJqc::useEquationNumbers()
{
AtPointConstraintIqcJc::useEquationNumbers();
iqXJminusOnePlusAxis = std::static_pointer_cast<EndFrameqc>(frmJ)->iqX() + axis;
iqEJ = std::static_pointer_cast<EndFrameqc>(frmJ)->iqE();
auto frmJeqc = std::static_pointer_cast<EndFrameqc>(frmJ);
iqXJminusOnePlusAxis = frmJeqc->iqX() + axis;
iqEJ = frmJeqc->iqE();
}
void AtPointConstraintIqcJqc::fillPosICError(FColDsptr col)

7
MbDCode/AtPointConstraintIqcJqc.h
View File

@@ -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;

4
MbDCode/AtPointConstraintIqctJqc.cpp
View File

@@ -35,7 +35,7 @@ ConstraintType AtPointConstraintIqctJqc::type()
void AtPointConstraintIqctJqc::preVelIC()
{
AtPointConstraintIJ::preVelIC();
AtPointConstraintIqcJqc::preVelIC();
pGpt = std::static_pointer_cast<DispCompIeqctJeqcO>(riIeJeO)->priIeJeOpt;
}
@@ -56,7 +56,7 @@ void AtPointConstraintIqctJqc::fillAccICIterError(FColDsptr col)
void AtPointConstraintIqctJqc::preAccIC()
{
AtPointConstraintIJ::preAccIC();
AtPointConstraintIqcJqc::preAccIC();
ppGpEIpt = std::static_pointer_cast<DispCompIeqctJeqcO>(riIeJeO)->ppriIeJeOpEIpt;
ppGptpt = std::static_pointer_cast<DispCompIeqctJeqcO>(riIeJeO)->ppriIeJeOptpt;
}

12
MbDCode/AtPointConstraintIqctJqc.h
View File

@@ -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;
};
}

46
MbDCode/CADSystem.cpp
View File

@@ -26,7 +26,7 @@ void CADSystem::outputFor(AnalysisType type)
this->logString(str);
mbdSystem->partsJointsMotionsForcesTorquesDo([](std::shared_ptr<Item> 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<Part>::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<DiagonalMatrix<double>>(ListD{ 0, 0, 0 });
qX = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0 });
@@ -122,7 +122,7 @@ void CADSystem::runOndselPiston()
assembly1->asFixed();
//
auto crankPart1 = CREATE<Part>::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<DiagonalMatrix<double>>(ListD{ 1, 1, 1 });
qX = std::make_shared<FullColumn<double>>(ListD{ 0.4, 0.0, -0.05 });
@@ -164,7 +164,7 @@ void CADSystem::runOndselPiston()
}
//
auto conrodPart2 = CREATE<Part>::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<DiagonalMatrix<double>>(ListD{ 1, 1, 1 });
qX = std::make_shared<FullColumn<double>>(ListD{ 0.15, 0.1, 0.05 });
@@ -206,7 +206,7 @@ void CADSystem::runOndselPiston()
}
//
auto pistonPart3 = CREATE<Part>::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<DiagonalMatrix<double>>(ListD{ 1, 1, 1 });
qX = std::make_shared<FullColumn<double>>(ListD{ -0.0, 1.5, 0.0 });
@@ -248,28 +248,28 @@ void CADSystem::runOndselPiston()
}
//
auto revJoint1 = CREATE<RevoluteJoint>::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<RevoluteJoint>::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<RevoluteJoint>::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<CylindricalJoint>::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<ZRotation>::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<Constant>(6.2831853071796);
auto timeScale = std::make_shared<Constant>(1.0);
auto time = std::make_shared<Product>(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<Part>::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<DiagonalMatrix<double>>(ListD{ 0, 0, 0 });
qX = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0 });
@@ -359,7 +359,7 @@ void CADSystem::runPiston()
assembly1->asFixed();
//
auto crankPart1 = CREATE<Part>::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<DiagonalMatrix<double>>(ListD{ 1.7381980042084e-4, 0.003511159968501, 0.0036154518487535 });
qX = std::make_shared<FullColumn<double>>(ListD{ 0.38423368514246, 2.6661567755108e-17, -0.048029210642807 });
@@ -401,7 +401,7 @@ void CADSystem::runPiston()
}
//
auto conrodPart2 = CREATE<Part>::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<DiagonalMatrix<double>>(ListD{ 2.6072970063126e-4, 0.011784982468533, 0.011941420288912 });
qX = std::make_shared<FullColumn<double>>(ListD{ 0.38423368514246, 0.49215295678475, 0.048029210642807 });
@@ -443,7 +443,7 @@ void CADSystem::runPiston()
}
//
auto pistonPart3 = CREATE<Part>::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<DiagonalMatrix<double>>(ListD{ 0.19449049546716, 0.23028116340971, 0.23028116340971 });
qX = std::make_shared<FullColumn<double>>(ListD{ -1.283972762056e-18, 1.4645980199976, -4.7652385308244e-17 });
@@ -485,28 +485,28 @@ void CADSystem::runPiston()
}
//
auto revJoint1 = CREATE<RevoluteJoint>::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<RevoluteJoint>::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<RevoluteJoint>::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<CylindricalJoint>::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<ZRotation>::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<Constant>(6.2831853071796);
auto timeScale = std::make_shared<Constant>(0.04);
auto time = std::make_shared<Product>(timeScale, TheSystem->time);

5
MbDCode/CREATE.h
View File

@@ -39,6 +39,11 @@ namespace MbD {
inst->initialize();
return inst;
}
static std::shared_ptr<T> With(EndFrmcptr frmi, EndFrmcptr frmj) {
auto inst = std::make_shared<T>(frmi, frmj);
inst->initialize();
return inst;
}
static std::shared_ptr<T> With(EndFrmcptr frmi, EndFrmcptr frmj, int axis) {
auto inst = std::make_shared<T>(frmi, frmj, axis);
inst->initialize();

78
MbDCode/ConstVelConstraintIJ.cpp Normal file
View File

@@ -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();
}

31
MbDCode/ConstVelConstraintIJ.h Normal file
View File

@@ -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<DirectionCosineIecJec> aA01IeJe, aA10IeJe;
};
}

98
MbDCode/ConstVelConstraintIqcJc.cpp Normal file
View File

@@ -0,0 +1,98 @@
#include <memory>
#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<DirectionCosineIeqcJec>(aA01IeJe);
auto& pA01IeJepEI = aA01IeqcJec->pAijIeJepEI;
auto& ppA01IeJepEIpEI = aA01IeqcJec->ppAijIeJepEIpEI;
auto aA10IeqcJec = std::dynamic_pointer_cast<DirectionCosineIeqcJec>(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<EndFrameqc>(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<DirectionCosineIeqcJec>::With(frmI, frmJ, 0, 1);
}
void MbD::ConstVelConstraintIqcJc::initA10IeJe()
{
aA10IeJe = CREATE<DirectionCosineIeqcJec>::With(frmI, frmJ, 1, 0);
}
void MbD::ConstVelConstraintIqcJc::initialize()
{
ConstVelConstraintIJ::initialize();
pGpEI = std::make_shared<FullRow<double>>(4);
ppGpEIpEI = std::make_shared<FullMatrix<double>>(4, 4);
}
void MbD::ConstVelConstraintIqcJc::useEquationNumbers()
{
iqEI = std::static_pointer_cast<EndFrameqc>(frmI)->iqE();
}

27
MbDCode/ConstVelConstraintIqcJc.h Normal file
View File

@@ -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;
};
}

121
MbDCode/ConstVelConstraintIqcJqc.cpp Normal file
View File

@@ -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<DirectionCosineIeqcJeqc>(aA01IeJe);
auto& pA01IeJepEJ = aA01IeqcJeqc->pAijIeJepEJ;
auto& ppA01IeJepEIpEJ = aA01IeqcJeqc->ppAijIeJepEIpEJ;
auto& ppA01IeJepEJpEJ = aA01IeqcJeqc->ppAijIeJepEJpEJ;
auto aA10IeqcJeqc = std::dynamic_pointer_cast<DirectionCosineIeqcJeqc>(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<EndFrameqc>(frmI);
auto qEdotI = efrmIqc->qEdot();
auto efrmJqc = std::static_pointer_cast<EndFrameqc>(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<DirectionCosineIeqcJeqc>::With(frmI, frmJ, 0, 1);
}
void MbD::ConstVelConstraintIqcJqc::initA10IeJe()
{
aA10IeJe = CREATE<DirectionCosineIeqcJeqc>::With(frmI, frmJ, 1, 0);
}
void MbD::ConstVelConstraintIqcJqc::initialize()
{
ConstVelConstraintIqcJc::initialize();
pGpEJ = std::make_shared<FullRow<double>>(4);
ppGpEIpEJ = std::make_shared<FullMatrix<double>>(4, 4);
ppGpEJpEJ = std::make_shared<FullMatrix<double>>(4, 4);
}
void MbD::ConstVelConstraintIqcJqc::useEquationNumbers()
{
ConstVelConstraintIqcJc::useEquationNumbers();
iqEJ = std::static_pointer_cast<EndFrameqc>(frmJ)->iqE();
}

29
MbDCode/ConstVelConstraintIqcJqc.h Normal file
View File

@@ -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;
};
}

28
MbDCode/Constraint.cpp
View File

@@ -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;

49
MbDCode/Constraint.h
View File

@@ -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<Constraint> sptr, std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> essenConstraints);
virtual void fillDispConstraints(std::shared_ptr<Constraint> sptr, std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> dispConstraints);
virtual void fillPerpenConstraints(std::shared_ptr<Constraint> sptr, std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> perpenConstraints);
virtual void fillRedundantConstraints(std::shared_ptr<Constraint> sptr, std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> redunConstraints);
virtual void fillConstraints(std::shared_ptr<Constraint> sptr, std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> 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<std::vector<int>> 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<Constraint> sptr, std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> allConstraints);
virtual void fillDispConstraints(std::shared_ptr<Constraint> sptr, std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> dispConstraints);
virtual void fillEssenConstraints(std::shared_ptr<Constraint> sptr, std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> essenConstraints);
virtual void fillPerpenConstraints(std::shared_ptr<Constraint> sptr, std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> 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<Constraint> sptr, std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> 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<std::vector<int>> 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

1
MbDCode/ConstraintIJ.h
View File

@@ -9,6 +9,7 @@ namespace MbD {
//frmI frmJ aConstant
public:
ConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj);
void initialize() override;
EndFrmcptr frmI, frmJ;

12
MbDCode/DirectionCosineConstraintIJ.cpp
View File

@@ -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();
}

18
MbDCode/DirectionCosineConstraintIJ.h
View File

@@ -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<DirectionCosineIecJec> aAijIeJe;

4
MbDCode/DirectionCosineConstraintIqcJc.cpp
View File

@@ -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();

38
MbDCode/DirectionCosineConstraintIqcJc.h
View File

@@ -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;
};
}

7
MbDCode/DirectionCosineConstraintIqcJqc.h
View File

@@ -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;

11
MbDCode/DirectionCosineConstraintIqctJqc.h
View File

@@ -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;

29
MbDCode/DirectionCosineIecJec.cpp
View File

@@ -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;
}

7
MbDCode/DirectionCosineIecJec.h
View File

@@ -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;

10
MbDCode/DirectionCosineIeqcJec.cpp
View File

@@ -24,6 +24,16 @@ void DirectionCosineIeqcJec::initializeGlobally()
ppAjOIepEIpEI = std::static_pointer_cast<EndFrameqc>(frmI)->ppAjOepEpE(axisI);
}
FMatDsptr MbD::DirectionCosineIeqcJec::ppvaluepEIpEI()
{
return ppAijIeJepEIpEI;
}
FRowDsptr MbD::DirectionCosineIeqcJec::pvaluepEI()
{
return pAijIeJepEI;
}
void DirectionCosineIeqcJec::calcPostDynCorrectorIteration()
{
DirectionCosineIecJec::calcPostDynCorrectorIteration();

7
MbDCode/DirectionCosineIeqcJec.h
View File

@@ -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;

5
MbDCode/DirectionCosineIeqcJeqc.cpp
View File

@@ -26,6 +26,11 @@ void DirectionCosineIeqcJeqc::initializeGlobally()
ppAjOJepEJpEJ = std::static_pointer_cast<EndFrameqc>(frmJ)->ppAjOepEpE(axisJ);
}
FMatDsptr MbD::DirectionCosineIeqcJeqc::ppvaluepEIpEJ()
{
return ppAijIeJepEIpEJ;
}
void DirectionCosineIeqcJeqc::calcPostDynCorrectorIteration()
{
DirectionCosineIeqcJec::calcPostDynCorrectorIteration();

8
MbDCode/DirectionCosineIeqcJeqc.h
View File

@@ -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;

15
MbDCode/DirectionCosineIeqctJeqc.cpp
View File

@@ -25,6 +25,21 @@ void DirectionCosineIeqctJeqc::initializeGlobally()
ppAjOJepEJpEJ = std::static_pointer_cast<EndFrameqc>(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."

12
MbDCode/DirectionCosineIeqctJeqc.h
View File

@@ -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;

11
MbDCode/DispCompIecJecIe.cpp Normal file
View File

@@ -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)
{
}

18
MbDCode/DispCompIecJecIe.h Normal file
View File

@@ -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;
};
}

30
MbDCode/DispCompIecJecKec.cpp
View File

@@ -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;

8
MbDCode/DispCompIecJecKec.h
View File

@@ -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;

5
MbDCode/DispCompIecJecKeqc.cpp
View File

@@ -51,3 +51,8 @@ FMatDsptr DispCompIecJecKeqc::ppvaluepEKpEK()
{
return ppriIeJeKepEKpEK;
}
FRowDsptr MbD::DispCompIecJecKeqc::pvaluepEK()
{
return priIeJeKepEK;
}

6
MbDCode/DispCompIecJecKeqc.h
View File

@@ -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;

29
MbDCode/DispCompIecJecO.cpp
View File

@@ -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;
}

7
MbDCode/DispCompIecJecO.h
View File

@@ -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;

9
MbDCode/DispCompIeqcJecIe.cpp Normal file
View File

@@ -0,0 +1,9 @@
#include "DispCompIeqcJecIe.h"
MbD::DispCompIeqcJecIe::DispCompIeqcJecIe()
{
}
MbD::DispCompIeqcJecIe::DispCompIeqcJecIe(EndFrmcptr frmi, EndFrmcptr frmj, int axis) : DispCompIecJecIe(frmi, frmj, axis)
{
}

18
MbDCode/DispCompIeqcJecIe.h Normal file
View File

@@ -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;
};
}

7
MbDCode/DispCompIeqcJecKeqc.cpp
View File

@@ -26,7 +26,7 @@ void DispCompIeqcJecKeqc::calcPostDynCorrectorIteration()
DispCompIecJecKeqc::calcPostDynCorrectorIteration();
auto frmIqc = std::static_pointer_cast<EndFrameqc>(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;

16
MbDCode/DispCompIeqcJecKeqc.h
View File

@@ -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;

15
MbDCode/DispCompIeqcJecO.cpp
View File

@@ -18,6 +18,21 @@ void DispCompIeqcJecO::initializeGlobally()
ppriIeJeOpEIpEI = std::static_pointer_cast<EndFrameqc>(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();

6
MbDCode/DispCompIeqcJecO.h
View File

@@ -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;

9
MbDCode/DispCompIeqcJeqcIe.cpp Normal file
View File

@@ -0,0 +1,9 @@
#include "DispCompIeqcJeqcIe.h"
MbD::DispCompIeqcJeqcIe::DispCompIeqcJeqcIe()
{
}
MbD::DispCompIeqcJeqcIe::DispCompIeqcJeqcIe(EndFrmcptr frmi, EndFrmcptr frmj, int axis) : DispCompIeqcJecIe(frmi, frmj, axis)
{
}

17
MbDCode/DispCompIeqcJeqcIe.h Normal file
View File

@@ -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;
};
}

16
MbDCode/DispCompIeqcJeqcKeqc.cpp
View File

@@ -26,33 +26,33 @@ void DispCompIeqcJeqcKeqc::calcPostDynCorrectorIteration()
DispCompIeqcJecKeqc::calcPostDynCorrectorIteration();
auto frmJqc = std::static_pointer_cast<EndFrameqc>(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));
}
}
}

7
MbDCode/DispCompIeqcJeqcKeqc.h
View File

@@ -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;

7
MbDCode/DispCompIeqcJeqcKeqct.h
View File

@@ -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;

7
MbDCode/DispCompIeqcJeqcO.h
View File

@@ -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;

9
MbDCode/DispCompIeqctJeqcIe.cpp Normal file
View File

@@ -0,0 +1,9 @@
#include "DispCompIeqctJeqcIe.h"
MbD::DispCompIeqctJeqcIe::DispCompIeqctJeqcIe()
{
}
MbD::DispCompIeqctJeqcIe::DispCompIeqctJeqcIe(EndFrmcptr frmi, EndFrmcptr frmj, int axis) : DispCompIeqcJeqcIe(frmi, frmj, axis)
{
}

17
MbDCode/DispCompIeqctJeqcIe.h Normal file
View File

@@ -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;
};
}

3
MbDCode/DispCompIeqctJeqcKeqct.h
View File

@@ -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;
};
}

10
MbDCode/DispCompIeqctJeqcO.cpp
View File

@@ -17,6 +17,16 @@ void DispCompIeqctJeqcO::initializeGlobally()
ppriIeJeOpEJpEJ = std::static_pointer_cast<EndFrameqct>(frmJ)->ppriOeOpEpE(axis);
}
FRowDsptr MbD::DispCompIeqctJeqcO::ppvaluepEIpt()
{
return ppriIeJeOpEIpt;
}
double MbD::DispCompIeqctJeqcO::ppvalueptpt()
{
return ppriIeJeOptpt;
}
void DispCompIeqctJeqcO::calcPostDynCorrectorIteration()
{
//"ppriIeJeOpEIpEI is not a constant now."

7
MbDCode/DispCompIeqctJeqcO.h
View File

@@ -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;

28
MbDCode/DistIecJec.cpp Normal file
View File

@@ -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;
}

21
MbDCode/DistIecJec.h Normal file
View File

@@ -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;
};
}

97
MbDCode/DistIeqcJec.cpp Normal file
View File

@@ -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<EndFrameqc>(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<FullMatrix<double>>(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<FullMatrix<double>>(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<FullRow<double>>(3);
prIeJepEI = std::make_shared<FullRow<double>>(4);
pprIeJepXIpXI = std::make_shared<FullMatrix<double>>(3, 3);
pprIeJepXIpEI = std::make_shared<FullMatrix<double>>(3, 4);
pprIeJepEIpEI = std::make_shared<FullMatrix<double>>(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;
}

25
MbDCode/DistIeqcJec.h Normal file
View File

@@ -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;
};
}

169
MbDCode/DistIeqcJeqc.cpp Normal file
View File

@@ -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<EndFrameqc>(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<FullRow<double>>(3);
prIeJepEJ = std::make_shared<FullRow<double>>(4);
pprIeJepXIpXJ = std::make_shared<FullMatrix<double>>(3, 3);
pprIeJepEIpXJ = std::make_shared<FullMatrix<double>>(4, 3);
pprIeJepXJpXJ = std::make_shared<FullMatrix<double>>(3, 3);
pprIeJepXIpEJ = std::make_shared<FullMatrix<double>>(3, 4);
pprIeJepEIpEJ = std::make_shared<FullMatrix<double>>(4, 4);
pprIeJepXJpEJ = std::make_shared<FullMatrix<double>>(3, 4);
pprIeJepEJpEJ = std::make_shared<FullMatrix<double>>(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;
}

29
MbDCode/DistIeqcJeqc.h Normal file
View File

@@ -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;
};
}

1
MbDCode/DistIeqctJeqc.cpp Normal file
View File

@@ -0,0 +1 @@
#include "DistIeqctJeqc.h"

14
MbDCode/DistIeqctJeqc.h Normal file
View File

@@ -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.
};
}

69
MbDCode/DistanceConstraintIJ.cpp Normal file
View File

@@ -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;
}

30
MbDCode/DistanceConstraintIJ.h Normal file
View File

@@ -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<DistIecJec> distIeJe;
};
}

102
MbDCode/DistanceConstraintIqcJc.cpp Normal file
View File

@@ -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<EndFrameqc>(frmI);
auto rIpIeIp = frmIeqc->rpep();
auto pAOIppEI = frmIeqc->pAOppE();
auto aBOIp = frmIeqc->aBOp();
auto fpAOIppEIrIpIeIp = std::make_shared<FullColumn<double>>(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<EndFrameqc>(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<DistIeqcJec>::With(frmI, frmJ);
}
void MbD::DistanceConstraintIqcJc::useEquationNumbers()
{
auto frmIeqc = std::static_pointer_cast<EndFrameqc>(frmI);
iqXI = frmIeqc->iqX();
iqEI = frmIeqc->iqE();
}

28
MbDCode/DistanceConstraintIqcJc.h Normal file
View File

@@ -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;
};
}

108
MbDCode/DistanceConstraintIqcJqc.cpp Normal file
View File

@@ -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<EndFrameqc>(frmI);
auto frmJeqc = std::static_pointer_cast<EndFrameqc>(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<DistIeqcJeqc>::With(frmI, frmJ);
}
void MbD::DistanceConstraintIqcJqc::useEquationNumbers()
{
DistanceConstraintIqcJc::useEquationNumbers();
auto frmJeqc = std::static_pointer_cast<EndFrameqc>(frmJ);
iqXJ = frmJeqc->iqX();
iqEJ = frmJeqc->iqE();
}

26
MbDCode/DistanceConstraintIqcJqc.h Normal file
View File

@@ -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;
};
}

6
MbDCode/DistanceConstraintIqctJqc.cpp Normal file
View File

@@ -0,0 +1,6 @@
#include "DistanceConstraintIqctJqc.h"
MbD::DistanceConstraintIqctJqc::DistanceConstraintIqctJqc(EndFrmcptr frmi, EndFrmcptr frmj) : DistanceConstraintIqcJqc(frmi, frmj)
{
assert(false);
}

17
MbDCode/DistanceConstraintIqctJqc.h Normal file
View File

@@ -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;
};
}

79
MbDCode/DistancexyConstraintIJ.cpp Normal file
View File

@@ -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;
}

32
MbDCode/DistancexyConstraintIJ.h Normal file
View File

@@ -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<DispCompIecJecIe> xIeJeIe, yIeJeIe;
};
}

144
MbDCode/DistancexyConstraintIqcJc.cpp Normal file
View File

@@ -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<EndFrameqc>(frmI);
auto rIpIeIp = frmIeqc->rpep();
auto pAOIppEI = frmIeqc->pAOppE();
auto aBOIp = frmIeqc->aBOp();
auto fpAOIppEIrIpIeIp = std::make_shared<FullColumn<double>>(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<DispCompIeqcJecIe>::With(frmI, frmJ, 0);
yIeJeIe = CREATE<DispCompIeqcJecIe>::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<EndFrameqc>(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<EndFrameqc>(frmI);
iqXI = frmIeqc->iqX();
iqEI = frmIeqc->iqE();
}

34
MbDCode/DistancexyConstraintIqcJc.h Normal file
View File

@@ -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;
};
}

182
MbDCode/DistancexyConstraintIqcJqc.cpp Normal file
View File

@@ -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<EndFrameqc>(frmI);
auto frmJeqc = std::static_pointer_cast<EndFrameqc>(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<DispCompIeqcJeqcIe>::With(frmI, frmJ, 0);
yIeJeIe = CREATE<DispCompIeqcJeqcIe>::With(frmI, frmJ, 1);
}
void MbD::DistancexyConstraintIqcJqc::useEquationNumbers()
{
DistancexyConstraintIqcJc::useEquationNumbers();
auto frmJeqc = std::static_pointer_cast<EndFrameqc>(frmJ);
iqXJ = frmJeqc->iqX();
iqEJ = frmJeqc->iqE();
}

33
MbDCode/DistancexyConstraintIqcJqc.h Normal file
View File

@@ -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;
};
}

5
MbDCode/EndFramec.cpp
View File

@@ -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();

2
MbDCode/EndFramec.h
View File

@@ -19,6 +19,8 @@ namespace MbD {
public:
EndFramec();
EndFramec(const char* str);
FMatDsptr aAeO();
System* root() override;
void initialize() override;
void setMarkerFrame(MarkerFrame* markerFrm);

2
MbDCode/EndFrameqc.cpp
View File

@@ -30,7 +30,7 @@ void EndFrameqc::initializeGlobally()
void EndFrameqc::initEndFrameqct()
{
endFrameqct = CREATE<EndFrameqct>::With(this->getName().data());
endFrameqct = CREATE<EndFrameqct>::With(this->name.data());
endFrameqct->prOeOpE = prOeOpE;
endFrameqct->pprOeOpEpE = pprOeOpEpE;
endFrameqct->pAOepE = pAOepE;

136
MbDCode/EulerParameters.h
View File

@@ -35,43 +35,43 @@ namespace MbD {
template<>
inline std::shared_ptr<FullMatrix<std::shared_ptr<FullColumn<double>>>> EulerParameters<double>::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<FullColumn<double>>(ListD{ a2c1, m2c2, m2c3 });
auto col12 = std::make_shared<FullColumn<double>>(ListD{ a2c2, a2c1, 0 });
auto col13 = std::make_shared<FullColumn<double>>(ListD{ a2c3, 0, a2c1 });
auto col14 = std::make_shared<FullColumn<double>>(ListD{ 0, m2c3, a2c2 });
auto col22 = std::make_shared<FullColumn<double>>(ListD{ m2c1, a2c2, m2c3 });
auto col23 = std::make_shared<FullColumn<double>>(ListD{ 0, a2c3, a2c2 });
auto col24 = std::make_shared<FullColumn<double>>(ListD{ a2c3, 0, m2c1 });
auto col33 = std::make_shared<FullColumn<double>>(ListD{ m2c1, m2c2, a2c3 });
auto col34 = std::make_shared<FullColumn<double>>(ListD{ m2c2, a2c1, 0 });
auto col44 = std::make_shared<FullColumn<double>>(ListD{ a2c1, a2c2, a2c3 });
auto col00 = std::make_shared<FullColumn<double>>(ListD{ a2c0, m2c1, m2c2 });
auto col01 = std::make_shared<FullColumn<double>>(ListD{ a2c1, a2c0, 0 });
auto col02 = std::make_shared<FullColumn<double>>(ListD{ a2c2, 0, a2c0 });
auto col03 = std::make_shared<FullColumn<double>>(ListD{ 0, m2c2, a2c1 });
auto col11 = std::make_shared<FullColumn<double>>(ListD{ m2c0, a2c1, m2c2 });
auto col12 = std::make_shared<FullColumn<double>>(ListD{ 0, a2c2, a2c1 });
auto col13 = std::make_shared<FullColumn<double>>(ListD{ a2c2, 0, m2c0 });
auto col22 = std::make_shared<FullColumn<double>>(ListD{ m2c0, m2c1, a2c2 });
auto col23 = std::make_shared<FullColumn<double>>(ListD{ m2c1, a2c0, 0 });
auto col33 = std::make_shared<FullColumn<double>>(ListD{ a2c0, a2c1, a2c2 });
auto answer = std::make_shared<FullMatrix<std::shared_ptr<FullColumn<double>>>>(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<FullMatrix<std::shared_ptr<FullMatrix<double>>>> EulerParameters<double>::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<FullRow<double>>(3, 0.0);
auto mat11 = std::make_shared<FullMatrix<double>>(ListFRD{ a2m1, m2m2, m2m3 });
auto mat12 = std::make_shared<FullMatrix<double>>(ListFRD{ a2m2, a2m1, zero });
auto mat13 = std::make_shared<FullMatrix<double>>(ListFRD{ a2m3, zero, a2m1 });
auto mat14 = std::make_shared<FullMatrix<double>>(ListFRD{ zero, m2m3, a2m2 });
auto mat22 = std::make_shared<FullMatrix<double>>(ListFRD{ m2m1, a2m2, m2m3 });
auto mat23 = std::make_shared<FullMatrix<double>>(ListFRD{ zero, a2m3, a2m2 });
auto mat24 = std::make_shared<FullMatrix<double>>(ListFRD{ a2m3, zero, m2m1 });
auto mat33 = std::make_shared<FullMatrix<double>>(ListFRD{ m2m1, m2m2, a2m3 });
auto mat34 = std::make_shared<FullMatrix<double>>(ListFRD{ m2m2, a2m1, zero });
auto mat44 = std::make_shared<FullMatrix<double>>(ListFRD{ a2m1, a2m2, a2m3 });
auto mat00 = std::make_shared<FullMatrix<double>>(ListFRD{ a2m0, m2m1, m2m2 });
auto mat01 = std::make_shared<FullMatrix<double>>(ListFRD{ a2m1, a2m0, zero });
auto mat02 = std::make_shared<FullMatrix<double>>(ListFRD{ a2m2, zero, a2m0 });
auto mat03 = std::make_shared<FullMatrix<double>>(ListFRD{ zero, m2m2, a2m1 });
auto mat11 = std::make_shared<FullMatrix<double>>(ListFRD{ m2m0, a2m1, m2m2 });
auto mat12 = std::make_shared<FullMatrix<double>>(ListFRD{ zero, a2m2, a2m1 });
auto mat13 = std::make_shared<FullMatrix<double>>(ListFRD{ a2m2, zero, m2m0 });
auto mat22 = std::make_shared<FullMatrix<double>>(ListFRD{ m2m0, m2m1, a2m2 });
auto mat23 = std::make_shared<FullMatrix<double>>(ListFRD{ m2m1, a2m0, zero });
auto mat33 = std::make_shared<FullMatrix<double>>(ListFRD{ a2m0, a2m1, a2m2 });
auto answer = std::make_shared<FullMatrix<std::shared_ptr<FullMatrix<double>>>>(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;
}

8
MbDCode/FullMatrix.h
View File

@@ -59,6 +59,7 @@ namespace MbD {
std::shared_ptr<FullMatrix<T>> copy();
FullMatrix<T> operator+(const FullMatrix<T> fullMat);
std::shared_ptr<FullColumn<T>> transposeTimesFullColumn(const std::shared_ptr<FullColumn<T>> 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<typename T>
inline void FullMatrix<T>::magnifySelf(T factor)
{
for (int i = 0; i < this->size(); i++) {
this->at(i)->magnifySelf(factor);
}
}
template<typename T>
inline std::ostream& FullMatrix<T>::printOn(std::ostream& s) const
{
s << "FullMat[" << std::endl;

13
MbDCode/FullRow.h
View File

@@ -29,6 +29,7 @@ namespace MbD {
std::shared_ptr<FullColumn<T>> transpose();
std::shared_ptr<FullRow<T>> copy();
void atiplusFullRow(int j, std::shared_ptr<FullRow<T>> fullRow);
std::shared_ptr<FullMatrix<T>> transposeTimesFullRow(std::shared_ptr<FullRow<T>> fullRow);
std::ostream& printOn(std::ostream& s) const override;
};
@@ -125,6 +126,18 @@ namespace MbD {
}
}
template<typename T>
inline std::shared_ptr<FullMatrix<T>> FullRow<T>::transposeTimesFullRow(std::shared_ptr<FullRow<T>> fullRow)
{
//"a*b = a(i)b(j)"
auto nrow = (int)this->size();
auto answer = std::make_shared<FullMatrix<double>>(nrow);
for (int i = 0; i < nrow; i++)
{
answer->atiput(i, fullRow->times(this->at(i)));
}
return answer;
}
template<typename T>
inline std::ostream& FullRow<T>::printOn(std::ostream& s) const
{
s << "FullRow{";

9
MbDCode/FullVector.h
View File

@@ -27,6 +27,7 @@ namespace MbD {
double maxMagnitude();
double length();
void equalSelfPlusFullVectortimes(std::shared_ptr<FullVector<T>> fullVec, T factor);
void magnifySelf(T factor);
std::ostream& printOn(std::ostream& s) const override;
};
@@ -143,6 +144,14 @@ namespace MbD {
}
}
template<typename T>
inline void FullVector<T>::magnifySelf(T factor)
{
for (int i = 0; i < this->size(); i++)
{
this->atitimes(i, factor);
}
}
template<typename T>
inline std::ostream& FullVector<T>::printOn(std::ostream& s) const
{
s << "FullVec{";

1
MbDCode/GearConstraintIJ.cpp Normal file
View File

@@ -0,0 +1 @@
#include "GearConstraintIJ.h"

14
MbDCode/GearConstraintIJ.h Normal file
View File

@@ -0,0 +1,14 @@
#pragma once
#include "ConstraintIJ.h"
namespace MbD {
class GearConstraintIJ : public ConstraintIJ
{
//orbitIeJe orbitJeIe radiusI radiusJ
public:
};
}

Some files were not shown because too many files have changed in this diff Show More