106 lines
2.9 KiB
C++
106 lines
2.9 KiB
C++
/***************************************************************************
|
|
* Copyright (c) 2023 Ondsel, Inc. *
|
|
* *
|
|
* This file is part of OndselSolver. *
|
|
* *
|
|
* See LICENSE file for details about copyright. *
|
|
***************************************************************************/
|
|
|
|
#include "DistIeqcJec.h"
|
|
#include "EndFrameqc.h"
|
|
|
|
using namespace MbD;
|
|
|
|
MbD::DistIeqcJec::DistIeqcJec()
|
|
{
|
|
}
|
|
|
|
MbD::DistIeqcJec::DistIeqcJec(EndFrmsptr frmi, EndFrmsptr 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;
|
|
}
|