runPosIC, initializeGlobally

MbDCode/MarkerFrame.cpp

@@ -5,6 +5,7 @@
 #include "EndFramec.h"
 #include "EndFrameqc.h"
 #include "EulerParameters.h"
+#include "CREATE.h"
 
 using namespace MbD;
 
@@ -20,7 +21,7 @@ void MarkerFrame::initialize()
 	prOmOpE = std::make_shared<FullMatrix<double>>(3, 4);
 	pAOmpE = std::make_shared<FullColumn<std::shared_ptr<FullMatrix<double>>>>(4);
 	endFrames = std::make_shared<std::vector<EndFrmcptr>>();
-	auto endFrm = std::make_shared<EndFrameqc>("EndFrame1");
+	auto endFrm = CREATE<EndFrameqc>::With();
 	this->addEndFrame(endFrm);
 }
 
@@ -28,6 +29,15 @@ void MarkerFrame::initializeLocally()
 {
 	pprOmOpEpE = EulerParameters<double>::ppApEpEtimesColumn(rpmp);
 	ppAOmpEpE = EulerParameters<double>::ppApEpEtimesMatrix(aApm);
+	for (size_t i = 0; i < endFrames->size(); i++)
+	{
+		auto eFrmqc = std::dynamic_pointer_cast<EndFrameqc>(endFrames->at(i));
+		if (eFrmqc) {
+			if (eFrmqc->endFrameqct) {
+				endFrames->at(i) = eFrmqc->endFrameqct;
+			}
+		}
+	}
 	std::for_each(endFrames->begin(), endFrames->end(), [](const auto& endFrame) { endFrame->initializeLocally(); });
 }
 
@@ -42,6 +52,33 @@ void MbD::MarkerFrame::postInput()
 
 void MbD::MarkerFrame::calcPostDynCorrectorIteration()
 {
+	auto rOpO = partFrame->rOpO();
+	auto aAOp = partFrame->aAOp();
+	auto rOmO = rOpO->plusFullColumn(aAOp->timesFullColumn(rpmp));
+	auto aAOm = aAOp->timesFullMatrix(aApm);
+	auto pAOppE = partFrame->pAOppE();
+	for (size_t i = 0; i < 4; i++)
+	{
+		auto& pAOppEi = pAOppE->at(i);
+		prOmOpE->atijputFullColumn(1, i, pAOppEi->timesFullColumn(rpmp));
+		pAOmpE->at(i) = pAOppEi->timesFullMatrix(aApm);
+	}
+}
+
+void MbD::MarkerFrame::prePosIC()
+{
+	Item::prePosIC();
+	std::for_each(endFrames->begin(), endFrames->end(), [](const auto& endFrame) { endFrame->prePosIC(); });
+}
+
+int MbD::MarkerFrame::iqX()
+{
+	return partFrame->iqX;
+}
+
+int MbD::MarkerFrame::iqE()
+{
+	return partFrame->iqE;
 }
 
 void MarkerFrame::setPartFrame(PartFrame* partFrm)
@@ -55,12 +92,12 @@ PartFrame* MarkerFrame::getPartFrame() {
 
 void MarkerFrame::setrpmp(FColDsptr x)
 {
-	rpmp->copy(x);
+	rpmp->copyFrom(x);
 }
 
 void MarkerFrame::setaApm(FMatDsptr x)
 {
-	aApm->copy(x);
+	aApm->copyFrom(x);
 }
 void MarkerFrame::addEndFrame(EndFrmcptr endFrm)
 {