56 lines
1.5 KiB
C++
56 lines
1.5 KiB
C++
#include "DirectionCosineIeqcJeqc.h"
|
|
#include "EndFrameqc.h"
|
|
|
|
using namespace MbD;
|
|
|
|
DirectionCosineIeqcJeqc::DirectionCosineIeqcJeqc()
|
|
{
|
|
}
|
|
|
|
DirectionCosineIeqcJeqc::DirectionCosineIeqcJeqc(EndFrmcptr frmi, EndFrmcptr frmj, size_t axisi, size_t axisj) :
|
|
DirectionCosineIeqcJec(frmi, frmj, axisi, axisj)
|
|
{
|
|
}
|
|
|
|
void DirectionCosineIeqcJeqc::initialize()
|
|
{
|
|
DirectionCosineIeqcJec::initialize();
|
|
pAijIeJepEJ = std::make_shared<FullRow<double>>(4);
|
|
ppAijIeJepEIpEJ = std::make_shared<FullMatrix<double>>(4, 4);
|
|
ppAijIeJepEJpEJ = std::make_shared<FullMatrix<double>>(4, 4);
|
|
}
|
|
|
|
void MbD::DirectionCosineIeqcJeqc::initializeGlobally()
|
|
{
|
|
DirectionCosineIeqcJec::initializeGlobally();
|
|
ppAjOJepEJpEJ = std::static_pointer_cast<EndFrameqc>(frmJ)->ppAjOepEpE(axisJ);
|
|
}
|
|
|
|
void MbD::DirectionCosineIeqcJeqc::calcPostDynCorrectorIteration()
|
|
{
|
|
DirectionCosineIeqcJec::calcPostDynCorrectorIteration();
|
|
pAjOJepEJT = std::static_pointer_cast<EndFrameqc>(frmJ)->pAjOepET(axisJ);
|
|
for (size_t i = 0; i < 4; i++)
|
|
{
|
|
pAijIeJepEJ->at(i) = aAjOIe->dot(pAjOJepEJT->at(i));
|
|
}
|
|
for (size_t i = 0; i < 4; i++)
|
|
{
|
|
auto& ppAijIeJepEIipEJ = ppAijIeJepEIpEJ->at(i);
|
|
for (size_t j = 0; j < 4; j++)
|
|
{
|
|
ppAijIeJepEIipEJ->at(j) = pAjOIepEIT->at(i)->dot(pAjOJepEJT->at(j));
|
|
}
|
|
}
|
|
for (size_t i = 0; i < 4; i++)
|
|
{
|
|
auto& ppAijIeJepEJipEJ = ppAijIeJepEJpEJ->at(i);
|
|
auto& ppAjOJepEJipEJ = ppAjOJepEJpEJ->at(i);
|
|
for (size_t j = 0; j < 4; j++)
|
|
{
|
|
ppAijIeJepEJipEJ->at(j) = aAjOIe->dot(ppAjOJepEJipEJ->at(j));
|
|
}
|
|
}
|
|
ppAijIeJepEJpEJ->symLowerWithUpper();
|
|
}
|