MatrixSolver NewtonRaphson

MbDCode/PartFrame.cpp

@@ -5,39 +5,81 @@
 #include "EulerConstraint.h"
 #include "AbsConstraint.h"
 #include "MarkerFrame.h"
+#include "EulerParameters.h"
+#include "EulerParametersDot.h"
 
 using namespace MbD;
 
+std::shared_ptr<PartFrame> MbD::PartFrame::Create()
+{
+	auto item = std::make_shared<PartFrame>();
+	item->initialize();
+	return item;
+}
+
 PartFrame::PartFrame()
 {
-	initialize();
 }
 PartFrame::PartFrame(const char* str) : CartesianFrame(str)
 {
-	initialize();
 }
 void PartFrame::initialize()
 {
-	aGeu = std::make_shared<EulerConstraint>("EulerCon");
+	aGeu = EulerConstraint::Create();
 	aGeu->setOwner(this);
 	aGabs = std::make_shared<std::vector<std::shared_ptr<AbsConstraint>>>();
 	markerFrames = std::make_shared<std::vector<std::shared_ptr<MarkerFrame>>>();
 }
+
+void PartFrame::initializeLocally()
+{
+	std::for_each(markerFrames->begin(), markerFrames->end(), [](const auto& markerFrame) { markerFrame->initializeLocally(); });
+	aGeu->initializeLocally();
+	std::for_each(aGabs->begin(), aGabs->end(), [](const auto& aGab) { aGab->initializeLocally(); });
+}
+
+void PartFrame::initializeGlobally()
+{
+	std::for_each(markerFrames->begin(), markerFrames->end(), [](const auto& markerFrame) { markerFrame->initializeGlobally(); });
+	aGeu->initializeGlobally();
+	std::for_each(aGabs->begin(), aGabs->end(), [](const auto& aGab) { aGab->initializeGlobally(); });
+}
 void PartFrame::setqX(FColDsptr x) {
 	qX->copy(x);
 }
+
 FColDsptr PartFrame::getqX() {
 	return qX;
 }
+
 void PartFrame::setqE(FColDsptr x) {
 	qE->copy(x);
 }
+
 FColDsptr PartFrame::getqE() {
 	return qE;
 }
+void PartFrame::setqXdot(FColDsptr x) {
+	qXdot->copy(x);
+}
+
+FColDsptr PartFrame::getqXdot() {
+	return qXdot;
+}
+
+void PartFrame::setomeOpO(FColDsptr omeOpO) {
+	//qEdot := MbDEulerParametersDot fromqEOp: qE andOmegaOpO: omeOpO
+	qEdot = EulerParametersDot<double>::FromqEOpAndOmegaOpO(qE, omeOpO);
+}
+
+FColDsptr PartFrame::getomeOpO() {
+	return qE;
+}
+
 void PartFrame::setPart(Part* x) {
 	part = x;
 }
+
 Part* PartFrame::getPart() {
 	return part;
 }
@@ -54,7 +96,7 @@ EndFrmcptr PartFrame::endFrame(std::string name)
 	return (*match)->endFrames->at(0);
 }
 
-void MbD::PartFrame::asFixed()
+void PartFrame::asFixed()
 {
 	for (int i = 0; i < 6; i++) {
 		auto con = std::make_shared<AbsConstraint>(i);
@@ -63,16 +105,10 @@ void MbD::PartFrame::asFixed()
 	}
 }
 
-void PartFrame::initializeLocally()
+void MbD::PartFrame::postInput()
 {
-	std::for_each(markerFrames->begin(), markerFrames->end(), [](const auto& markerFrame) { markerFrame->initializeLocally(); });
-	aGeu->initializeLocally();
-	std::for_each(aGabs->begin(), aGabs->end(), [](const auto& aGab) { aGab->initializeLocally(); });
 }
 
-void PartFrame::initializeGlobally()
+void MbD::PartFrame::calcPostDynCorrectorIteration()
 {
-	std::for_each(markerFrames->begin(), markerFrames->end(), [](const auto& markerFrame) { markerFrame->initializeGlobally(); });
-	aGeu->initializeGlobally();
-	std::for_each(aGabs->begin(), aGabs->end(), [](const auto& aGab) { aGab->initializeGlobally(); });
 }