107 lines
3.4 KiB
C++
107 lines
3.4 KiB
C++
/***************************************************************************
|
|
* Copyright (c) 2023 Ondsel, Inc. *
|
|
* *
|
|
* This file is part of OndselSolver. *
|
|
* *
|
|
* See LICENSE file for details about copyright. *
|
|
***************************************************************************/
|
|
|
|
#include <memory>
|
|
|
|
#include "ConstVelConstraintIqcJc.h"
|
|
#include "DirectionCosineIeqcJec.h"
|
|
#include "DirectionCosineIeqcJeqc.h"
|
|
#include "EndFrameqc.h"
|
|
#include "CREATE.h"
|
|
|
|
using namespace MbD;
|
|
|
|
MbD::ConstVelConstraintIqcJc::ConstVelConstraintIqcJc(EndFrmsptr frmi, EndFrmsptr 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();
|
|
double 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<FullMatrixDouble>(4, 4);
|
|
}
|
|
|
|
void MbD::ConstVelConstraintIqcJc::useEquationNumbers()
|
|
{
|
|
iqEI = std::static_pointer_cast<EndFrameqc>(frmI)->iqE();
|
|
}
|