44 lines
1.6 KiB
C++
44 lines
1.6 KiB
C++
/***************************************************************************
|
|
* Copyright (c) 2023 Ondsel, Inc. *
|
|
* *
|
|
* This file is part of OndselSolver. *
|
|
* *
|
|
* See LICENSE file for details about copyright. *
|
|
***************************************************************************/
|
|
|
|
#include "DispCompIeqctJeqcKeqct.h"
|
|
#include "EndFrameqct.h"
|
|
|
|
using namespace MbD;
|
|
|
|
DispCompIeqctJeqcKeqct::DispCompIeqctJeqcKeqct()
|
|
{
|
|
}
|
|
|
|
DispCompIeqctJeqcKeqct::DispCompIeqctJeqcKeqct(EndFrmsptr frmi, EndFrmsptr frmj, EndFrmsptr frmk, size_t axisk) : DispCompIeqcJeqcKeqct(frmi, frmj, frmk, axisk)
|
|
{
|
|
}
|
|
|
|
void DispCompIeqctJeqcKeqct::preVelIC()
|
|
{
|
|
DispCompIeqcJeqcKeqct::preVelIC();
|
|
auto& mprIeJeOpt = std::static_pointer_cast<EndFrameqct>(frmI)->prOeOpt;
|
|
priIeJeKept -= aAjOKe->dot(mprIeJeOpt);
|
|
}
|
|
|
|
void DispCompIeqctJeqcKeqct::preAccIC()
|
|
{
|
|
DispCompIeqcJeqcKeqct::preAccIC();
|
|
auto pAjOKept = std::static_pointer_cast<EndFrameqct>(efrmK)->pAjOept(axisK);
|
|
auto efrmIqct = std::static_pointer_cast<EndFrameqct>(frmI);
|
|
auto& mprIeJeOpt = efrmIqct->prOeOpt;
|
|
auto mpprIeJeOpEITpt = efrmIqct->pprOeOpEpt->transpose();
|
|
auto& mpprIeJeOptpt = efrmIqct->pprOeOptpt;
|
|
for (size_t i = 0; i < 4; i++)
|
|
{
|
|
ppriIeJeKepEIpt->atiminusNumber(i, aAjOKe->dot(mpprIeJeOpEITpt->at(i)));
|
|
ppriIeJeKepEKpt->atiminusNumber(i, pAjOKepEKT->at(i)->dot(mprIeJeOpt));
|
|
}
|
|
ppriIeJeKeptpt += -(2.0 * pAjOKept->dot(mprIeJeOpt)) - aAjOKe->dot(mpprIeJeOptpt);
|
|
}
|