runPosIC, initializeGlobally

MbDCode/PartFrame.cpp

@@ -7,16 +7,10 @@
 #include "MarkerFrame.h"
 #include "EulerParameters.h"
 #include "EulerParametersDot.h"
+#include "CREATE.h"
 
 using namespace MbD;
 
-std::shared_ptr<PartFrame> MbD::PartFrame::Create()
-{
-	auto item = std::make_shared<PartFrame>();
-	item->initialize();
-	return item;
-}
-
 PartFrame::PartFrame()
 {
 }
@@ -25,7 +19,7 @@ PartFrame::PartFrame(const char* str) : CartesianFrame(str)
 }
 void PartFrame::initialize()
 {
-	aGeu = EulerConstraint::Create();
+	aGeu = CREATE<EulerConstraint>::With();
 	aGeu->setOwner(this);
 	aGabs = std::make_shared<std::vector<std::shared_ptr<AbsConstraint>>>();
 	markerFrames = std::make_shared<std::vector<std::shared_ptr<MarkerFrame>>>();
@@ -45,7 +39,7 @@ void PartFrame::initializeGlobally()
 	std::for_each(aGabs->begin(), aGabs->end(), [](const auto& aGab) { aGab->initializeGlobally(); });
 }
 void PartFrame::setqX(FColDsptr x) {
-	qX->copy(x);
+	qX->copyFrom(x);
 }
 
 FColDsptr PartFrame::getqX() {
@@ -53,23 +47,23 @@ FColDsptr PartFrame::getqX() {
 }
 
 void PartFrame::setqE(FColDsptr x) {
-	qE->copy(x);
+	qE->copyFrom(x);
 }
 
 FColDsptr PartFrame::getqE() {
 	return qE;
 }
 void PartFrame::setqXdot(FColDsptr x) {
-	qXdot->copy(x);
+	//qXdot->copy(x);
 }
 
 FColDsptr PartFrame::getqXdot() {
-	return qXdot;
+	//return qXdot;
+	return std::make_shared<FullColumn<double>>(3);
 }
 
 void PartFrame::setomeOpO(FColDsptr omeOpO) {
-	//qEdot := MbDEulerParametersDot fromqEOp: qE andOmegaOpO: omeOpO
-	qEdot = EulerParametersDot<double>::FromqEOpAndOmegaOpO(qE, omeOpO);
+	//qEdot = EulerParametersDot<double>::FromqEOpAndOmegaOpO(qE, omeOpO);
 }
 
 FColDsptr PartFrame::getomeOpO() {
@@ -92,13 +86,38 @@ void PartFrame::addMarkerFrame(std::shared_ptr<MarkerFrame> markerFrame)
 
 EndFrmcptr PartFrame::endFrame(std::string name)
 {
-	auto match = std::find_if(markerFrames->begin(), markerFrames->end(), [&](auto mkr) {return mkr->getName() == name; });
+	auto match = std::find_if(markerFrames->begin(), markerFrames->end(), [&](auto& mkr) {return mkr->getName() == name; });
 	return (*match)->endFrames->at(0);
 }
 
+void MbD::PartFrame::prePosIC()
+{
+//iqX = -1;
+//iqE  = -1;
+//super prePosIC.
+//markerFrames do : [:mkr | mkr prePosIC] .
+//aGeu prePosIC.
+//aGabs do : [:con | con prePosIC]
+}
+
+FColDsptr MbD::PartFrame::rOpO()
+{
+	return qX;
+}
+
+FMatDsptr MbD::PartFrame::aAOp()
+{
+	return qE->aA;
+}
+
+FColFMatDsptr MbD::PartFrame::pAOppE()
+{
+	return qE->pApE;
+}
+
 void PartFrame::asFixed()
 {
-	for (int i = 0; i < 6; i++) {
+	for (size_t i = 0; i < 6; i++) {
 		auto con = std::make_shared<AbsConstraint>(i);
 		con->setOwner(this);
 		aGabs->push_back(con);
@@ -111,4 +130,8 @@ void MbD::PartFrame::postInput()
 
 void MbD::PartFrame::calcPostDynCorrectorIteration()
 {
+	qE->calcABC();
+	qE->calcpApE();
+	//qEdot->calcAdotBdotCdot();
+	//qEdot->calcpAdotpE();
 }