DiagonalMatrix is now double only also.
This commit is contained in:
John Dupuy
@@ -9,7 +9,6 @@
|
||||
#include "EndFrameqct.h"
|
||||
#include "MarkerFrame.h"
|
||||
#include "System.h"
|
||||
#include "Symbolic.h"
|
||||
#include "Time.h"
|
||||
#include "EulerParameters.h"
|
||||
#include "CREATE.h"
|
||||
@@ -17,347 +16,309 @@
|
||||
#include "EulerAngleszxzDot.h"
|
||||
#include "EulerAngleszxzDDot.h"
|
||||
|
||||
using namespace MbD;
|
||||
namespace MbD {
|
||||
template class EulerParameters<double>;
|
||||
|
||||
EndFrameqct::EndFrameqct() {
|
||||
EndFrameqct::EndFrameqct() {
|
||||
}
|
||||
|
||||
EndFrameqct::EndFrameqct(const char *str) : EndFrameqc(str) {
|
||||
}
|
||||
|
||||
void EndFrameqct::initialize() {
|
||||
EndFrameqc::initialize();
|
||||
rmem = std::make_shared<FullColumn<double>>(3);
|
||||
prmempt = std::make_shared<FullColumn<double>>(3);
|
||||
pprmemptpt = std::make_shared<FullColumn<double>>(3);
|
||||
aAme = FullMatrixDouble::identitysptr(3);
|
||||
pAmept = std::make_shared<FullMatrixDouble>(3, 3);
|
||||
ppAmeptpt = std::make_shared<FullMatrixDouble>(3, 3);
|
||||
pprOeOpEpt = std::make_shared<FullMatrixDouble>(3, 4);
|
||||
pprOeOptpt = std::make_shared<FullColumn<double>>(3);
|
||||
ppAOepEpt = std::make_shared<FullColumn<FMatDsptr>>(4);
|
||||
ppAOeptpt = std::make_shared<FullMatrixDouble>(3, 3);
|
||||
}
|
||||
|
||||
void EndFrameqct::initializeLocally() {
|
||||
if (!rmemBlks) {
|
||||
rmem->zeroSelf();
|
||||
prmempt->zeroSelf();
|
||||
pprmemptpt->zeroSelf();
|
||||
}
|
||||
if (!phiThePsiBlks) {
|
||||
aAme->identity();
|
||||
pAmept->zeroSelf();
|
||||
ppAmeptpt->zeroSelf();
|
||||
}
|
||||
}
|
||||
|
||||
void EndFrameqct::initializeGlobally() {
|
||||
if (rmemBlks) {
|
||||
initprmemptBlks();
|
||||
initpprmemptptBlks();
|
||||
}
|
||||
if (phiThePsiBlks) {
|
||||
initpPhiThePsiptBlks();
|
||||
initppPhiThePsiptptBlks();
|
||||
}
|
||||
}
|
||||
|
||||
void EndFrameqct::initprmemptBlks() {
|
||||
auto &mbdTime = this->root()->time;
|
||||
prmemptBlks = std::make_shared<FullColumn<Symsptr>>(3);
|
||||
for (int i = 0; i < 3; i++) {
|
||||
auto &disp = rmemBlks->at(i);
|
||||
auto var = disp->differentiateWRT(mbdTime);
|
||||
auto vel = var->simplified(var);
|
||||
prmemptBlks->at(i) = vel;
|
||||
}
|
||||
}
|
||||
|
||||
void EndFrameqct::initpprmemptptBlks() {
|
||||
auto &mbdTime = this->root()->time;
|
||||
pprmemptptBlks = std::make_shared<FullColumn<Symsptr>>(3);
|
||||
for (int i = 0; i < 3; i++) {
|
||||
auto &vel = prmemptBlks->at(i);
|
||||
auto var = vel->differentiateWRT(mbdTime);
|
||||
auto acc = var->simplified(var);
|
||||
pprmemptptBlks->at(i) = acc;
|
||||
}
|
||||
}
|
||||
|
||||
void EndFrameqct::initpPhiThePsiptBlks() {
|
||||
auto &mbdTime = this->root()->time;
|
||||
pPhiThePsiptBlks = std::make_shared<FullColumn<Symsptr>>(3);
|
||||
for (int i = 0; i < 3; i++) {
|
||||
auto &angle = phiThePsiBlks->at(i);
|
||||
auto var = angle->differentiateWRT(mbdTime);
|
||||
//std::cout << "var " << *var << std::endl;
|
||||
auto vel = var->simplified(var);
|
||||
//std::cout << "vel " << *vel << std::endl;
|
||||
pPhiThePsiptBlks->at(i) = vel;
|
||||
}
|
||||
}
|
||||
|
||||
void EndFrameqct::initppPhiThePsiptptBlks() {
|
||||
auto &mbdTime = this->root()->time;
|
||||
ppPhiThePsiptptBlks = std::make_shared<FullColumn<Symsptr>>(3);
|
||||
for (int i = 0; i < 3; i++) {
|
||||
auto &angleVel = pPhiThePsiptBlks->at(i);
|
||||
auto var = angleVel->differentiateWRT(mbdTime);
|
||||
auto angleAcc = var->simplified(var);
|
||||
ppPhiThePsiptptBlks->at(i) = angleAcc;
|
||||
}
|
||||
}
|
||||
|
||||
void EndFrameqct::postInput() {
|
||||
this->evalrmem();
|
||||
this->evalAme();
|
||||
Item::postInput();
|
||||
}
|
||||
|
||||
void EndFrameqct::calcPostDynCorrectorIteration() {
|
||||
auto &rOmO = markerFrame->rOmO;
|
||||
auto &aAOm = markerFrame->aAOm;
|
||||
rOeO = rOmO->plusFullColumn(aAOm->timesFullColumn(rmem));
|
||||
auto &prOmOpE = markerFrame->prOmOpE;
|
||||
auto &pAOmpE = markerFrame->pAOmpE;
|
||||
for (int i = 0; i < 3; i++) {
|
||||
auto &prOmOpEi = prOmOpE->at(i);
|
||||
auto &prOeOpEi = prOeOpE->at(i);
|
||||
for (int j = 0; j < 4; j++) {
|
||||
auto prOeOpEij = prOmOpEi->at(j) + pAOmpE->at(j)->at(i)->timesFullColumn(rmem);
|
||||
prOeOpEi->at(j) = prOeOpEij;
|
||||
}
|
||||
}
|
||||
auto rpep = markerFrame->rpmp->plusFullColumn(markerFrame->aApm->timesFullColumn(rmem));
|
||||
pprOeOpEpE = EulerParameters<double>::ppApEpEtimesColumn(rpep);
|
||||
aAOe = aAOm->timesFullMatrix(aAme);
|
||||
for (int i = 0; i < 4; i++) {
|
||||
pAOepE->at(i) = pAOmpE->at(i)->timesFullMatrix(aAme);
|
||||
}
|
||||
auto aApe = markerFrame->aApm->timesFullMatrix(aAme);
|
||||
ppAOepEpE = EulerParameters<double>::ppApEpEtimesMatrix(aApe);
|
||||
}
|
||||
|
||||
void EndFrameqct::prePosIC() {
|
||||
time = this->root()->mbdTimeValue();
|
||||
this->evalrmem();
|
||||
this->evalAme();
|
||||
EndFrameqc::prePosIC();
|
||||
}
|
||||
|
||||
void EndFrameqct::evalrmem() {
|
||||
if (rmemBlks) {
|
||||
for (int i = 0; i < 3; i++) {
|
||||
auto &expression = rmemBlks->at(i);
|
||||
double value = expression->getValue();
|
||||
rmem->at(i) = value;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void EndFrameqct::evalAme() {
|
||||
if (phiThePsiBlks) {
|
||||
auto phiThePsi = CREATE<EulerAngleszxz<double>>::With();
|
||||
for (int i = 0; i < 3; i++) {
|
||||
auto &expression = phiThePsiBlks->at(i);
|
||||
auto value = expression->getValue();
|
||||
phiThePsi->at(i) = value;
|
||||
}
|
||||
phiThePsi->calc();
|
||||
aAme = phiThePsi->aA;
|
||||
}
|
||||
}
|
||||
|
||||
void EndFrameqct::preVelIC() {
|
||||
time = this->root()->mbdTimeValue();
|
||||
this->evalrmem();
|
||||
this->evalAme();
|
||||
Item::preVelIC();
|
||||
this->evalprmempt();
|
||||
this->evalpAmept();
|
||||
auto &aAOm = markerFrame->aAOm;
|
||||
prOeOpt = aAOm->timesFullColumn(prmempt);
|
||||
pAOept = aAOm->timesFullMatrix(pAmept);
|
||||
}
|
||||
|
||||
void EndFrameqct::postVelIC() {
|
||||
auto &pAOmpE = markerFrame->pAOmpE;
|
||||
for (int i = 0; i < 3; i++) {
|
||||
auto &pprOeOpEpti = pprOeOpEpt->at(i);
|
||||
for (int j = 0; j < 4; j++) {
|
||||
auto pprOeOpEptij = pAOmpE->at(j)->at(i)->dot(prmempt);
|
||||
pprOeOpEpti->atiput(j, pprOeOpEptij);
|
||||
}
|
||||
}
|
||||
for (int i = 0; i < 4; i++) {
|
||||
ppAOepEpt->atiput(i, pAOmpE->at(i)->timesFullMatrix(pAmept));
|
||||
}
|
||||
}
|
||||
|
||||
FColDsptr EndFrameqct::pAjOept(int j) {
|
||||
return pAOept->column(j);
|
||||
}
|
||||
|
||||
FMatDsptr EndFrameqct::ppAjOepETpt(int jj) {
|
||||
auto answer = std::make_shared<FullMatrixDouble>(4, 3);
|
||||
for (int i = 0; i < 4; i++) {
|
||||
auto &answeri = answer->at(i);
|
||||
auto &ppAOepEipt = ppAOepEpt->at(i);
|
||||
for (int j = 0; j < 3; j++) {
|
||||
auto &answerij = ppAOepEipt->at(j)->at(jj);
|
||||
answeri->atiput(j, answerij);
|
||||
}
|
||||
}
|
||||
return answer;
|
||||
}
|
||||
|
||||
FColDsptr EndFrameqct::ppAjOeptpt(int j) {
|
||||
return ppAOeptpt->column(j);
|
||||
}
|
||||
|
||||
double EndFrameqct::priOeOpt(int i) {
|
||||
return prOeOpt->at(i);
|
||||
}
|
||||
|
||||
FRowDsptr EndFrameqct::ppriOeOpEpt(int i) {
|
||||
return pprOeOpEpt->at(i);
|
||||
}
|
||||
|
||||
double EndFrameqct::ppriOeOptpt(int i) {
|
||||
return pprOeOptpt->at(i);
|
||||
}
|
||||
|
||||
void EndFrameqct::evalprmempt() {
|
||||
if (rmemBlks) {
|
||||
for (int i = 0; i < 3; i++) {
|
||||
auto &derivative = prmemptBlks->at(i);
|
||||
auto value = derivative->getValue();
|
||||
prmempt->at(i) = value;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void EndFrameqct::evalpAmept() {
|
||||
if (phiThePsiBlks) {
|
||||
auto phiThePsi = CREATE<EulerAngleszxz<double>>::With();
|
||||
auto phiThePsiDot = CREATE<EulerAngleszxzDot<double>>::With();
|
||||
phiThePsiDot->phiThePsi = phiThePsi;
|
||||
for (int i = 0; i < 3; i++) {
|
||||
auto &expression = phiThePsiBlks->at(i);
|
||||
auto &derivative = pPhiThePsiptBlks->at(i);
|
||||
auto value = expression->getValue();
|
||||
auto valueDot = derivative->getValue();
|
||||
phiThePsi->at(i) = value;
|
||||
phiThePsiDot->at(i) = valueDot;
|
||||
}
|
||||
phiThePsi->calc();
|
||||
phiThePsiDot->calc();
|
||||
pAmept = phiThePsiDot->aAdot;
|
||||
}
|
||||
}
|
||||
|
||||
void EndFrameqct::evalpprmemptpt() {
|
||||
if (rmemBlks) {
|
||||
for (int i = 0; i < 3; i++) {
|
||||
auto &secondDerivative = pprmemptptBlks->at(i);
|
||||
auto value = secondDerivative->getValue();
|
||||
pprmemptpt->atiput(i, value);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void EndFrameqct::evalppAmeptpt() {
|
||||
if (phiThePsiBlks) {
|
||||
auto phiThePsi = CREATE<EulerAngleszxz<double>>::With();
|
||||
auto phiThePsiDot = CREATE<EulerAngleszxzDot<double>>::With();
|
||||
phiThePsiDot->phiThePsi = phiThePsi;
|
||||
auto phiThePsiDDot = CREATE<EulerAngleszxzDDot<double>>::With();
|
||||
phiThePsiDDot->phiThePsiDot = phiThePsiDot;
|
||||
for (int i = 0; i < 3; i++) {
|
||||
auto &expression = phiThePsiBlks->at(i);
|
||||
auto &derivative = pPhiThePsiptBlks->at(i);
|
||||
auto &secondDerivative = ppPhiThePsiptptBlks->at(i);
|
||||
auto value = expression->getValue();
|
||||
auto valueDot = derivative->getValue();
|
||||
auto valueDDot = secondDerivative->getValue();
|
||||
phiThePsi->atiput(i, value);
|
||||
phiThePsiDot->atiput(i, valueDot);
|
||||
phiThePsiDDot->atiput(i, valueDDot);
|
||||
}
|
||||
phiThePsi->calc();
|
||||
phiThePsiDot->calc();
|
||||
phiThePsiDDot->calc();
|
||||
ppAmeptpt = phiThePsiDDot->aAddot;
|
||||
}
|
||||
}
|
||||
|
||||
FColDsptr EndFrameqct::rmeO() {
|
||||
return markerFrame->aAOm->timesFullColumn(rmem);
|
||||
}
|
||||
|
||||
FColDsptr EndFrameqct::rpep() {
|
||||
auto &rpmp = markerFrame->rpmp;
|
||||
auto &aApm = markerFrame->aApm;
|
||||
auto rpep = rpmp->plusFullColumn(aApm->timesFullColumn(rmem));
|
||||
return rpep;
|
||||
}
|
||||
|
||||
void EndFrameqct::preAccIC() {
|
||||
time = this->root()->mbdTimeValue();
|
||||
this->evalrmem();
|
||||
this->evalAme();
|
||||
Item::preVelIC();
|
||||
this->evalprmempt();
|
||||
this->evalpAmept();
|
||||
auto &aAOm = markerFrame->aAOm;
|
||||
prOeOpt = aAOm->timesFullColumn(prmempt);
|
||||
pAOept = aAOm->timesFullMatrix(pAmept);
|
||||
Item::preAccIC();
|
||||
this->evalpprmemptpt();
|
||||
this->evalppAmeptpt();
|
||||
aAOm = markerFrame->aAOm;
|
||||
pprOeOptpt = aAOm->timesFullColumn(pprmemptpt);
|
||||
ppAOeptpt = aAOm->timesFullMatrix(ppAmeptpt);
|
||||
}
|
||||
}
|
||||
|
||||
EndFrameqct::EndFrameqct(const char* str) : EndFrameqc(str) {
|
||||
}
|
||||
|
||||
void EndFrameqct::initialize()
|
||||
{
|
||||
EndFrameqc::initialize();
|
||||
rmem = std::make_shared<FullColumn<double>>(3);
|
||||
prmempt = std::make_shared<FullColumn<double>>(3);
|
||||
pprmemptpt = std::make_shared<FullColumn<double>>(3);
|
||||
aAme = FullMatrixDouble::identitysptr(3);
|
||||
pAmept = std::make_shared<FullMatrixDouble>(3, 3);
|
||||
ppAmeptpt = std::make_shared<FullMatrixDouble>(3, 3);
|
||||
pprOeOpEpt = std::make_shared<FullMatrixDouble>(3, 4);
|
||||
pprOeOptpt = std::make_shared<FullColumn<double>>(3);
|
||||
ppAOepEpt = std::make_shared<FullColumn<FMatDsptr>>(4);
|
||||
ppAOeptpt = std::make_shared<FullMatrixDouble>(3, 3);
|
||||
}
|
||||
|
||||
void EndFrameqct::initializeLocally()
|
||||
{
|
||||
if (!rmemBlks) {
|
||||
rmem->zeroSelf();
|
||||
prmempt->zeroSelf();
|
||||
pprmemptpt->zeroSelf();
|
||||
}
|
||||
if (!phiThePsiBlks) {
|
||||
aAme->identity();
|
||||
pAmept->zeroSelf();
|
||||
ppAmeptpt->zeroSelf();
|
||||
}
|
||||
}
|
||||
|
||||
void EndFrameqct::initializeGlobally()
|
||||
{
|
||||
if (rmemBlks) {
|
||||
initprmemptBlks();
|
||||
initpprmemptptBlks();
|
||||
}
|
||||
if (phiThePsiBlks) {
|
||||
initpPhiThePsiptBlks();
|
||||
initppPhiThePsiptptBlks();
|
||||
}
|
||||
}
|
||||
|
||||
void EndFrameqct::initprmemptBlks()
|
||||
{
|
||||
auto& mbdTime = this->root()->time;
|
||||
prmemptBlks = std::make_shared< FullColumn<Symsptr>>(3);
|
||||
for (int i = 0; i < 3; i++) {
|
||||
auto& disp = rmemBlks->at(i);
|
||||
auto var = disp->differentiateWRT(mbdTime);
|
||||
auto vel = var->simplified(var);
|
||||
prmemptBlks->at(i) = vel;
|
||||
}
|
||||
}
|
||||
|
||||
void EndFrameqct::initpprmemptptBlks()
|
||||
{
|
||||
auto& mbdTime = this->root()->time;
|
||||
pprmemptptBlks = std::make_shared< FullColumn<Symsptr>>(3);
|
||||
for (int i = 0; i < 3; i++) {
|
||||
auto& vel = prmemptBlks->at(i);
|
||||
auto var = vel->differentiateWRT(mbdTime);
|
||||
auto acc = var->simplified(var);
|
||||
pprmemptptBlks->at(i) = acc;
|
||||
}
|
||||
}
|
||||
|
||||
void EndFrameqct::initpPhiThePsiptBlks()
|
||||
{
|
||||
auto& mbdTime = this->root()->time;
|
||||
pPhiThePsiptBlks = std::make_shared< FullColumn<Symsptr>>(3);
|
||||
for (int i = 0; i < 3; i++) {
|
||||
auto& angle = phiThePsiBlks->at(i);
|
||||
auto var = angle->differentiateWRT(mbdTime);
|
||||
//std::cout << "var " << *var << std::endl;
|
||||
auto vel = var->simplified(var);
|
||||
//std::cout << "vel " << *vel << std::endl;
|
||||
pPhiThePsiptBlks->at(i) = vel;
|
||||
}
|
||||
}
|
||||
|
||||
void EndFrameqct::initppPhiThePsiptptBlks()
|
||||
{
|
||||
auto& mbdTime = this->root()->time;
|
||||
ppPhiThePsiptptBlks = std::make_shared< FullColumn<Symsptr>>(3);
|
||||
for (int i = 0; i < 3; i++) {
|
||||
auto& angleVel = pPhiThePsiptBlks->at(i);
|
||||
auto var = angleVel->differentiateWRT(mbdTime);
|
||||
auto angleAcc = var->simplified(var);
|
||||
ppPhiThePsiptptBlks->at(i) = angleAcc;
|
||||
}
|
||||
}
|
||||
|
||||
void EndFrameqct::postInput()
|
||||
{
|
||||
this->evalrmem();
|
||||
this->evalAme();
|
||||
Item::postInput();
|
||||
}
|
||||
|
||||
void EndFrameqct::calcPostDynCorrectorIteration()
|
||||
{
|
||||
auto& rOmO = markerFrame->rOmO;
|
||||
auto& aAOm = markerFrame->aAOm;
|
||||
rOeO = rOmO->plusFullColumn(aAOm->timesFullColumn(rmem));
|
||||
auto& prOmOpE = markerFrame->prOmOpE;
|
||||
auto& pAOmpE = markerFrame->pAOmpE;
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
auto& prOmOpEi = prOmOpE->at(i);
|
||||
auto& prOeOpEi = prOeOpE->at(i);
|
||||
for (int j = 0; j < 4; j++)
|
||||
{
|
||||
auto prOeOpEij = prOmOpEi->at(j) + pAOmpE->at(j)->at(i)->timesFullColumn(rmem);
|
||||
prOeOpEi->at(j) = prOeOpEij;
|
||||
}
|
||||
}
|
||||
auto rpep = markerFrame->rpmp->plusFullColumn(markerFrame->aApm->timesFullColumn(rmem));
|
||||
pprOeOpEpE = EulerParameters<double>::ppApEpEtimesColumn(rpep);
|
||||
aAOe = aAOm->timesFullMatrix(aAme);
|
||||
for (int i = 0; i < 4; i++)
|
||||
{
|
||||
pAOepE->at(i) = pAOmpE->at(i)->timesFullMatrix(aAme);
|
||||
}
|
||||
auto aApe = markerFrame->aApm->timesFullMatrix(aAme);
|
||||
ppAOepEpE = EulerParameters<double>::ppApEpEtimesMatrix(aApe);
|
||||
}
|
||||
|
||||
void EndFrameqct::prePosIC()
|
||||
{
|
||||
time = this->root()->mbdTimeValue();
|
||||
this->evalrmem();
|
||||
this->evalAme();
|
||||
EndFrameqc::prePosIC();
|
||||
}
|
||||
|
||||
void EndFrameqct::evalrmem()
|
||||
{
|
||||
if (rmemBlks) {
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
auto& expression = rmemBlks->at(i);
|
||||
double value = expression->getValue();
|
||||
rmem->at(i) = value;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void EndFrameqct::evalAme()
|
||||
{
|
||||
if (phiThePsiBlks) {
|
||||
auto phiThePsi = CREATE<EulerAngleszxz<double>>::With();
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
auto& expression = phiThePsiBlks->at(i);
|
||||
auto value = expression->getValue();
|
||||
phiThePsi->at(i) = value;
|
||||
}
|
||||
phiThePsi->calc();
|
||||
aAme = phiThePsi->aA;
|
||||
}
|
||||
}
|
||||
|
||||
void EndFrameqct::preVelIC()
|
||||
{
|
||||
time = this->root()->mbdTimeValue();
|
||||
this->evalrmem();
|
||||
this->evalAme();
|
||||
Item::preVelIC();
|
||||
this->evalprmempt();
|
||||
this->evalpAmept();
|
||||
auto& aAOm = markerFrame->aAOm;
|
||||
prOeOpt = aAOm->timesFullColumn(prmempt);
|
||||
pAOept = aAOm->timesFullMatrix(pAmept);
|
||||
}
|
||||
|
||||
void EndFrameqct::postVelIC()
|
||||
{
|
||||
auto& pAOmpE = markerFrame->pAOmpE;
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
auto& pprOeOpEpti = pprOeOpEpt->at(i);
|
||||
for (int j = 0; j < 4; j++)
|
||||
{
|
||||
auto pprOeOpEptij = pAOmpE->at(j)->at(i)->dot(prmempt);
|
||||
pprOeOpEpti->atiput(j, pprOeOpEptij);
|
||||
}
|
||||
}
|
||||
for (int i = 0; i < 4; i++)
|
||||
{
|
||||
ppAOepEpt->atiput(i, pAOmpE->at(i)->timesFullMatrix(pAmept));
|
||||
}
|
||||
}
|
||||
|
||||
FColDsptr EndFrameqct::pAjOept(int j)
|
||||
{
|
||||
return pAOept->column(j);
|
||||
}
|
||||
|
||||
FMatDsptr EndFrameqct::ppAjOepETpt(int jj)
|
||||
{
|
||||
auto answer = std::make_shared<FullMatrixDouble>(4, 3);
|
||||
for (int i = 0; i < 4; i++)
|
||||
{
|
||||
auto& answeri = answer->at(i);
|
||||
auto& ppAOepEipt = ppAOepEpt->at(i);
|
||||
for (int j = 0; j < 3; j++)
|
||||
{
|
||||
auto& answerij = ppAOepEipt->at(j)->at(jj);
|
||||
answeri->atiput(j, answerij);
|
||||
}
|
||||
}
|
||||
return answer;
|
||||
}
|
||||
|
||||
FColDsptr EndFrameqct::ppAjOeptpt(int j)
|
||||
{
|
||||
return ppAOeptpt->column(j);
|
||||
}
|
||||
|
||||
double EndFrameqct::priOeOpt(int i)
|
||||
{
|
||||
return prOeOpt->at(i);
|
||||
}
|
||||
|
||||
FRowDsptr EndFrameqct::ppriOeOpEpt(int i)
|
||||
{
|
||||
return pprOeOpEpt->at(i);
|
||||
}
|
||||
|
||||
double EndFrameqct::ppriOeOptpt(int i)
|
||||
{
|
||||
return pprOeOptpt->at(i);
|
||||
}
|
||||
|
||||
void EndFrameqct::evalprmempt()
|
||||
{
|
||||
if (rmemBlks) {
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
auto& derivative = prmemptBlks->at(i);
|
||||
auto value = derivative->getValue();
|
||||
prmempt->at(i) = value;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void EndFrameqct::evalpAmept()
|
||||
{
|
||||
if (phiThePsiBlks) {
|
||||
auto phiThePsi = CREATE<EulerAngleszxz<double>>::With();
|
||||
auto phiThePsiDot = CREATE<EulerAngleszxzDot<double>>::With();
|
||||
phiThePsiDot->phiThePsi = phiThePsi;
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
auto& expression = phiThePsiBlks->at(i);
|
||||
auto& derivative = pPhiThePsiptBlks->at(i);
|
||||
auto value = expression->getValue();
|
||||
auto valueDot = derivative->getValue();
|
||||
phiThePsi->at(i) = value;
|
||||
phiThePsiDot->at(i) = valueDot;
|
||||
}
|
||||
phiThePsi->calc();
|
||||
phiThePsiDot->calc();
|
||||
pAmept = phiThePsiDot->aAdot;
|
||||
}
|
||||
}
|
||||
|
||||
void EndFrameqct::evalpprmemptpt()
|
||||
{
|
||||
if (rmemBlks) {
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
auto& secondDerivative = pprmemptptBlks->at(i);
|
||||
auto value = secondDerivative->getValue();
|
||||
pprmemptpt->atiput(i, value);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void EndFrameqct::evalppAmeptpt()
|
||||
{
|
||||
if (phiThePsiBlks) {
|
||||
auto phiThePsi = CREATE<EulerAngleszxz<double>>::With();
|
||||
auto phiThePsiDot = CREATE<EulerAngleszxzDot<double>>::With();
|
||||
phiThePsiDot->phiThePsi = phiThePsi;
|
||||
auto phiThePsiDDot = CREATE<EulerAngleszxzDDot<double>>::With();
|
||||
phiThePsiDDot->phiThePsiDot = phiThePsiDot;
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
auto& expression = phiThePsiBlks->at(i);
|
||||
auto& derivative = pPhiThePsiptBlks->at(i);
|
||||
auto& secondDerivative = ppPhiThePsiptptBlks->at(i);
|
||||
auto value = expression->getValue();
|
||||
auto valueDot = derivative->getValue();
|
||||
auto valueDDot = secondDerivative->getValue();
|
||||
phiThePsi->atiput(i, value);
|
||||
phiThePsiDot->atiput(i, valueDot);
|
||||
phiThePsiDDot->atiput(i, valueDDot);
|
||||
}
|
||||
phiThePsi->calc();
|
||||
phiThePsiDot->calc();
|
||||
phiThePsiDDot->calc();
|
||||
ppAmeptpt = phiThePsiDDot->aAddot;
|
||||
}
|
||||
}
|
||||
|
||||
FColDsptr EndFrameqct::rmeO()
|
||||
{
|
||||
return markerFrame->aAOm->timesFullColumn(rmem);
|
||||
}
|
||||
|
||||
FColDsptr EndFrameqct::rpep()
|
||||
{
|
||||
auto& rpmp = markerFrame->rpmp;
|
||||
auto& aApm = markerFrame->aApm;
|
||||
auto rpep = rpmp->plusFullColumn(aApm->timesFullColumn(rmem));
|
||||
return rpep;
|
||||
}
|
||||
|
||||
void EndFrameqct::preAccIC()
|
||||
{
|
||||
time = this->root()->mbdTimeValue();
|
||||
this->evalrmem();
|
||||
this->evalAme();
|
||||
Item::preVelIC();
|
||||
this->evalprmempt();
|
||||
this->evalpAmept();
|
||||
auto& aAOm = markerFrame->aAOm;
|
||||
prOeOpt = aAOm->timesFullColumn(prmempt);
|
||||
pAOept = aAOm->timesFullMatrix(pAmept);
|
||||
Item::preAccIC();
|
||||
this->evalpprmemptpt();
|
||||
this->evalppAmeptpt();
|
||||
aAOm = markerFrame->aAOm;
|
||||
pprOeOptpt = aAOm->timesFullColumn(pprmemptpt);
|
||||
ppAOeptpt = aAOm->timesFullMatrix(ppAmeptpt);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user