120 lines
3.2 KiB
C++
120 lines
3.2 KiB
C++
/***************************************************************************
|
|
* Copyright (c) 2023 Ondsel, Inc. *
|
|
* *
|
|
* This file is part of OndselSolver. *
|
|
* *
|
|
* See LICENSE file for details about copyright. *
|
|
***************************************************************************/
|
|
|
|
#include "DispCompIeqcJeqcIe.h"
|
|
#include "EndFrameqc.h"
|
|
|
|
using namespace MbD;
|
|
|
|
MbD::DispCompIeqcJeqcIe::DispCompIeqcJeqcIe()
|
|
{
|
|
}
|
|
|
|
MbD::DispCompIeqcJeqcIe::DispCompIeqcJeqcIe(EndFrmsptr frmi, EndFrmsptr frmj, size_t axis) : DispCompIeqcJecIe(frmi, frmj, axis)
|
|
{
|
|
priIeJeIepXJ = std::make_shared<FullRow<double>>(3);
|
|
priIeJeIepEJ = std::make_shared<FullRow<double>>(4);
|
|
ppriIeJeIepEIpXJ = std::make_shared<FullMatrix<double>>(4, 3);
|
|
ppriIeJeIepEIpEJ = std::make_shared<FullMatrix<double>>(4, 4);
|
|
ppriIeJeIepEJpEJ = std::make_shared<FullMatrix<double>>(4, 4);
|
|
}
|
|
|
|
void MbD::DispCompIeqcJeqcIe::calc_ppvaluepEIpEJ()
|
|
{
|
|
auto frmJeqc = std::static_pointer_cast<EndFrameqc>(frmJ);
|
|
auto& prIeJeOpEJ = frmJeqc->prOeOpE;
|
|
ppriIeJeIepEIpEJ = pAjOIepEIT->timesFullMatrix(prIeJeOpEJ);
|
|
}
|
|
|
|
void MbD::DispCompIeqcJeqcIe::calc_ppvaluepEIpXJ()
|
|
{
|
|
ppriIeJeIepEIpXJ = pAjOIepEIT;
|
|
}
|
|
|
|
void MbD::DispCompIeqcJeqcIe::calc_ppvaluepEJpEJ()
|
|
{
|
|
auto frmJeqc = std::static_pointer_cast<EndFrameqc>(frmJ);
|
|
auto pprIeJeOpEJpEJ = frmJeqc->pprOeOpEpE;
|
|
for (size_t i = 0; i < 4; i++)
|
|
{
|
|
auto pprIeJeOpEJipEJ = pprIeJeOpEJpEJ->at(i);
|
|
auto ppriIeJeIepEJipEJ = ppriIeJeIepEJpEJ->at(i);
|
|
for (size_t j = i; j < 4; j++)
|
|
{
|
|
auto term1 = aAjOIe->dot(pprIeJeOpEJipEJ->at(j));
|
|
ppriIeJeIepEJipEJ->atiput(j, term1);
|
|
}
|
|
}
|
|
ppriIeJeIepEJpEJ->symLowerWithUpper();
|
|
}
|
|
|
|
void MbD::DispCompIeqcJeqcIe::calc_pvaluepEJ()
|
|
{
|
|
auto frmJeqc = std::static_pointer_cast<EndFrameqc>(frmJ);
|
|
auto prIeJeOpEJT = frmJeqc->prOeOpE->transpose();
|
|
for (size_t i = 0; i < 4; i++)
|
|
{
|
|
priIeJeIepEJ->atiput(i, aAjOIe->dot(prIeJeOpEJT->at(i)));
|
|
}
|
|
}
|
|
|
|
void MbD::DispCompIeqcJeqcIe::calc_pvaluepXJ()
|
|
{
|
|
for (size_t i = 0; i < 3; i++)
|
|
{
|
|
priIeJeIepXJ->atiput(i, aAjOIe->at(i));
|
|
}
|
|
}
|
|
|
|
void MbD::DispCompIeqcJeqcIe::calcPostDynCorrectorIteration()
|
|
{
|
|
//Must maintain order of calc_xxx.
|
|
DispCompIeqcJecIe::calcPostDynCorrectorIteration();
|
|
calc_pvaluepXJ();
|
|
calc_pvaluepEJ();
|
|
calc_ppvaluepEIpXJ();
|
|
calc_ppvaluepEIpEJ();
|
|
calc_ppvaluepEJpEJ();
|
|
|
|
}
|
|
|
|
void MbD::DispCompIeqcJeqcIe::initialize()
|
|
{
|
|
DispCompIeqcJecIe::initialize();
|
|
priIeJeIepXJ = std::make_shared<FullRow<double>>(3);
|
|
priIeJeIepEJ = std::make_shared<FullRow<double>>(4);
|
|
ppriIeJeIepEIpXJ = std::make_shared<FullMatrix<double>>(4, 3);
|
|
ppriIeJeIepEIpEJ = std::make_shared<FullMatrix<double>>(4, 4);
|
|
ppriIeJeIepEJpEJ = std::make_shared<FullMatrix<double>>(4, 4);
|
|
}
|
|
|
|
FMatDsptr MbD::DispCompIeqcJeqcIe::ppvaluepEIpEJ()
|
|
{
|
|
return ppriIeJeIepEIpEJ;
|
|
}
|
|
|
|
FMatDsptr MbD::DispCompIeqcJeqcIe::ppvaluepEIpXJ()
|
|
{
|
|
return ppriIeJeIepEIpXJ;
|
|
}
|
|
|
|
FMatDsptr MbD::DispCompIeqcJeqcIe::ppvaluepEJpEJ()
|
|
{
|
|
return ppriIeJeIepEJpEJ;
|
|
}
|
|
|
|
FRowDsptr MbD::DispCompIeqcJeqcIe::pvaluepEJ()
|
|
{
|
|
return priIeJeIepEJ;
|
|
}
|
|
|
|
FRowDsptr MbD::DispCompIeqcJeqcIe::pvaluepXJ()
|
|
{
|
|
return priIeJeIepXJ;
|
|
}
|