diff --git a/OndselSolver/EndFrameqct.cpp b/OndselSolver/EndFrameqct.cpp index a55201a..bd9be1b 100644 --- a/OndselSolver/EndFrameqct.cpp +++ b/OndselSolver/EndFrameqct.cpp @@ -321,347 +321,3 @@ namespace MbD { ppAOeptpt = aAOm->timesFullMatrix(ppAmeptpt); } } - -EndFrameqct::EndFrameqct(const char* str) : EndFrameqc(str) { -} - -void EndFrameqct::initialize() -{ - EndFrameqc::initialize(); - rmem = std::make_shared>(3); - prmempt = std::make_shared>(3); - pprmemptpt = std::make_shared>(3); - aAme = FullMatrix::identitysptr(3); - pAmept = std::make_shared>(3, 3); - ppAmeptpt = std::make_shared>(3, 3); - pprOeOpEpt = std::make_shared>(3, 4); - pprOeOptpt = std::make_shared>(3); - ppAOepEpt = std::make_shared>(4); - ppAOeptpt = std::make_shared>(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>(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>(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>(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; - //std::cout << *angle << std::endl; - //std::cout << *vel << std::endl; - } -} - -void EndFrameqct::initppPhiThePsiptptBlks() -{ - auto& mbdTime = this->root()->time; - ppPhiThePsiptptBlks = std::make_shared< FullColumn>(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; - //std::cout << *angleVel << std::endl; - //std::cout << *angleAcc << std::endl; - } -} - -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::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::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>::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>(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>::With(); - auto phiThePsiDot = CREATE>::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>::With(); - auto phiThePsiDot = CREATE>::With(); - phiThePsiDot->phiThePsi = phiThePsi; - auto phiThePsiDDot = CREATE>::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); -} diff --git a/OndselSolver/FullRow.h b/OndselSolver/FullRow.h index d031bd9..85712bf 100644 --- a/OndselSolver/FullRow.h +++ b/OndselSolver/FullRow.h @@ -172,37 +172,6 @@ namespace MbD { return answer; } template - inline std::shared_ptr> FullRow::clonesptr() - { - return std::make_shared>(*this); - } - template - inline double FullRow::dot(std::shared_ptr> vec) - { - int n = (int)this->size(); - double answer = 0.0; - for (int i = 0; i < n; i++) { - answer += this->at(i) * vec->at(i); - } - return answer; - } - template - inline std::shared_ptr> FullRow::dot(std::shared_ptr>>> vecvec) - { - auto ncol = (int)this->size(); - auto nelem = vecvec->at(0)->size(); - auto answer = std::make_shared>(nelem); - for (int k = 0; k < nelem; k++) { - auto sum = 0.0; - for (int i = 0; i < ncol; i++) - { - sum += this->at(i) * vecvec->at(i)->at(k); - } - answer->at(k) = sum; - } - return answer; - } - template inline std::ostream& FullRow::printOn(std::ostream& s) const { s << "FullRow{"; diff --git a/OndselSolver/FullVector.h b/OndselSolver/FullVector.h index 02b0eb6..60a3bb7 100644 --- a/OndselSolver/FullVector.h +++ b/OndselSolver/FullVector.h @@ -228,46 +228,6 @@ namespace MbD { return true; } template - inline std::shared_ptr> FullVector::clonesptr() - { - //Return shallow copy of *this wrapped in shared_ptr - assert(false); - return std::make_shared>(*this); - } - template - inline bool FullVector::isIncreasing() - { - return isIncreasingIfExceptionsAreLessThan(0.0); - } - template - inline bool FullVector::isIncreasingIfExceptionsAreLessThan(double tol) - { - //"Test if elements are increasing." - //"Ok if spoilers are less than tol." - auto next = this->at(0); - for (int i = 1; i < this->size(); i++) - { - auto previous = next; - next = this->at(i); - if (previous > next && (previous - tol > next)) return false; - } - return true; - } - template - inline bool FullVector::isDecreasingIfExceptionsAreLessThan(double tol) - { - //"Test if elements are increasing." - //"Ok if spoilers are less than tol." - auto next = this->at(0); - for (int i = 1; i < this->size(); i++) - { - auto previous = next; - next = this->at(i); - if (previous < next && (previous + tol < next)) return false; - } - return true; - } - template inline std::ostream& FullVector::printOn(std::ostream& s) const { s << "FullVec{";