EndFrameqct done

MbDCode/EndFrameqct.cpp

@@ -1,4 +1,5 @@
 #include "EndFrameqct.h"
+#include "System.h"
 
 using namespace MbD;
 
@@ -12,34 +13,63 @@ EndFrameqct::EndFrameqct(const char* str) : EndFrameqc(str) {
 
 void EndFrameqct::initialize()
 {
-	EndFrameqc::initialize();
-	rmem = std::make_unique<FullColumn<double>>(3);	
-	prmempt = std::make_unique<FullColumn<double>>(3);	
-	pprmemptpt = std::make_unique<FullColumn<double>>(3);	
-	aAme = std::make_unique<FullMatrix<double>>(3, 3);
-	pAmept = std::make_unique<FullMatrix<double>>(3, 3);
-	ppAmeptpt = std::make_unique<FullMatrix<double>>(3, 3);
-	pprOeOpEpt = std::make_unique<FullMatrix<double>>(3, 4);
-	pprOeOptpt = std::make_unique<FullColumn<double>>(3);	
-	ppAOepEpt = std::make_unique<FullColumn<FullMatrix<double>>>(4);
-	ppAOeptpt = std::make_unique<FullMatrix<double>>(3, 3);
+	rmem = std::make_shared<FullColumn<double>>(3);	
+	prmempt = std::make_shared<FullColumn<double>>(3);	
+	pprmemptpt = std::make_shared<FullColumn<double>>(3);	
+	aAme = std::make_shared<FullMatrix<double>>(3, 3);
+	pAmept = std::make_shared<FullMatrix<double>>(3, 3);
+	ppAmeptpt = std::make_shared<FullMatrix<double>>(3, 3);
+	pprOeOpEpt = std::make_shared<FullMatrix<double>>(3, 4);
+	pprOeOptpt = std::make_shared<FullColumn<double>>(3);	
+	ppAOepEpt = std::make_shared<FullColumn<std::shared_ptr<FullMatrix<double>>>>(4);
+	ppAOeptpt = std::make_shared<FullMatrix<double>>(3, 3);
 }
 
 void EndFrameqct::initializeLocally()
 {
-	//rmemBlks == nil
-	//	ifTrue :
-	//[rmem zeroSelf.
-	//	prmempt zeroSelf.
-	//	pprmemptpt zeroSelf] .
-	//	phiThePsiBlks == nil
-	//	ifTrue :
-	//[aAme identity.
-	//	pAmept zeroSelf.
-	//	ppAmeptpt zeroSelf]
+	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 MbD::EndFrameqct::initprmemptBlks()
+{
+	auto time = System::getInstance().time;
+	prmemptBlks = std::make_shared< FullColumn<std::shared_ptr<Symbolic>>>(3);
+	for (int i = 0; i < 3; i++) {
+		auto disp = rmemBlks->at(i);
+		auto vel = (disp->differentiateWRT(time))->simplified();
+		prmemptBlks->at(i) = vel;
+	}
+}
+
+void MbD::EndFrameqct::initpprmemptptBlks()
+{
+}
+
+void MbD::EndFrameqct::initpPhiThePsiptBlks()
+{
+}
+
+void MbD::EndFrameqct::initppPhiThePsiptptBlks()
+{
 }