new branch: working

.gitignore

@@ -32,4 +32,5 @@
 *.app
 
 .vs
-x64/
+x64/
+*.bak

MbDCode/AbsConstraint.cpp

@@ -12,6 +12,11 @@ AbsConstraint::AbsConstraint(const char* str) : Constraint(str)
     initialize();
 }
 
+MbD::AbsConstraint::AbsConstraint(int i)
+{
+    axis = i;
+}
+
 void AbsConstraint::initialize()
 {
     axis = 0;

MbDCode/AbsConstraint.h

@@ -7,6 +7,7 @@ namespace MbD {
     public:
         AbsConstraint();
         AbsConstraint(const char* str);
+        AbsConstraint(int axis);
         void initialize();
         int axis;
         int iqXminusOnePlusAxis;

MbDCode/BasicUserFunction.cpp

@@ -0,0 +1 @@
+#include "BasicUserFunction.h"

MbDCode/BasicUserFunction.h

@@ -0,0 +1,15 @@
+#pragma once
+#include <string>
+
+#include "UserFunction.h"
+
+namespace MbD {
+    class BasicUserFunction : public UserFunction
+    {
+        //funcText myUnit units 
+    public:
+        std::string funcText;
+        double myUnit;
+    };
+}
+

MbDCode/CartesianFrame.cpp

@@ -12,4 +12,5 @@ CartesianFrame::CartesianFrame(const char* str) : Item(str)
 
 void CartesianFrame::initialize()
 {
+	Item::initialize();
 }

MbDCode/Constant.cpp

@@ -0,0 +1,11 @@
+#include "Constant.h"
+
+using namespace MbD;
+
+Constant::Constant()
+{
+}
+
+Constant::Constant(double val) : Variable(val)
+{
+}

MbDCode/Constant.h

@@ -0,0 +1,13 @@
+#pragma once
+
+#include "Variable.h"
+
+namespace MbD {
+    class Constant : public Variable
+    {
+    public:
+        Constant();
+        Constant(double val);
+    };
+}
+

MbDCode/Constraint.h

@@ -15,9 +15,9 @@ namespace MbD {
 		Item* getOwner();
 		
 		int iG;
-		double aG;  //Constraint function
-		double lam; //Lambda is Lagrange Multiplier
-		Item* owner; //A Joint or PartFrame owns the constraint
+		double aG;		//Constraint function
+		double lam;		//Lambda is Lagrange Multiplier
+		Item* owner;	//A Joint or PartFrame owns the constraint
 	};
 }
 

MbDCode/ConstraintIJ.cpp

@@ -0,0 +1,9 @@
+#include "ConstraintIJ.h"
+#include "EndFramec.h"
+
+using namespace MbD;
+
+MbD::ConstraintIJ::ConstraintIJ(std::shared_ptr<EndFramec> frmi, std::shared_ptr<EndFramec> frmj) : frmI(frmi), frmJ(frmj)
+{
+	aConstant = 0.0;
+}

MbDCode/ConstraintIJ.h

@@ -0,0 +1,17 @@
+#pragma once
+
+#include "Constraint.h"
+#include "EndFramec.h"
+
+namespace MbD {
+    class ConstraintIJ : public Constraint
+    {
+        //frmI frmJ aConstant 
+    public:
+        ConstraintIJ(std::shared_ptr<EndFramec> frmi, std::shared_ptr<EndFramec> frmj);
+
+        std::shared_ptr<EndFramec> frmI, frmJ;
+        double aConstant;
+    };
+}
+

MbDCode/DirectionCosineConstraintIJ.cpp

@@ -0,0 +1,10 @@
+#include "DirectionCosineConstraintIJ.h"
+#include "EndFramec.h"
+
+using namespace MbD;
+
+DirectionCosineConstraintIJ::DirectionCosineConstraintIJ(std::shared_ptr<EndFramec> frmi, std::shared_ptr<EndFramec> frmj, int axisi, int axisj) : 
+	ConstraintIJ(frmI, frmJ), axisI(axisi), axisJ(axisj)
+{
+	aAijIeJe = std::make_shared<DirectionCosineIecJec>(frmI, frmJ, axisI, axisJ);
+}

MbDCode/DirectionCosineConstraintIJ.h

@@ -0,0 +1,18 @@
+#pragma once
+
+#include "ConstraintIJ.h"
+#include "DirectionCosineIecJec.h"
+
+namespace MbD {
+    class DirectionCosineConstraintIJ : public ConstraintIJ
+    {
+        //axisI axisJ aAijIeJe 
+    public:
+        //	self owns : (MbDDirectionCosineConstraintIJ withFrmI : frmI frmJ : frmJ axisI : 2 axisJ : 1).
+        DirectionCosineConstraintIJ(std::shared_ptr<EndFramec> frmI, std::shared_ptr<EndFramec> frmJ, int axisI, int axisJ);
+
+        int axisI, axisJ;
+        std::shared_ptr<DirectionCosineIecJec> aAijIeJe;
+    };
+}
+

MbDCode/DirectionCosineIecJec.cpp

@@ -0,0 +1,18 @@
+#include <memory>
+
+#include "DirectionCosineIecJec.h"
+#include "FullColumn.h"
+
+using namespace MbD;
+
+MbD::DirectionCosineIecJec::DirectionCosineIecJec()
+{
+}
+
+MbD::DirectionCosineIecJec::DirectionCosineIecJec(std::shared_ptr<EndFramec> frmi, std::shared_ptr<EndFramec> frmj, int axisi, int axisj) :
+	KinematicIeJe(frmi, frmj), axisI(axisi), axisJ(axisj)
+{
+	aAijIeJe = 0.0;
+	aAjOIe = std::make_shared<FullColumn<double>>(3);
+	aAjOJe = std::make_shared<FullColumn<double>>(3);
+}

MbDCode/DirectionCosineIecJec.h

@@ -0,0 +1,21 @@
+#pragma once
+#include <memory>
+
+#include "KinematicIeJe.h"
+//#include "EndFramec.h"
+#include "FullColumn.h"
+
+namespace MbD {
+    class DirectionCosineIecJec : public KinematicIeJe
+    {
+        //aAijIeJe axisI axisJ aAjOIe aAjOJe 
+    public:
+        DirectionCosineIecJec();
+        DirectionCosineIecJec(std::shared_ptr<EndFramec> frmI, std::shared_ptr<EndFramec> frmJ, int axisI, int axisJ);
+
+        int axisI, axisJ;
+        double aAijIeJe;
+        std::shared_ptr<FullColumn<double>> aAjOIe, aAjOJe;
+    };
+}
+

MbDCode/DirectionCosineIeqcJec.cpp

@@ -0,0 +1,14 @@
+#include "DirectionCosineIeqcJec.h"
+
+using namespace MbD;
+
+MbD::DirectionCosineIeqcJec::DirectionCosineIeqcJec()
+{
+	initialize();
+}
+
+void MbD::DirectionCosineIeqcJec::initialize()
+{
+	pAijIeJepEI = std::make_shared<FullRow<double>>(4);
+	ppAijIeJepEIpEI = std::make_shared<FullMatrix<double>>(4, 4);
+}

MbDCode/DirectionCosineIeqcJec.h

@@ -0,0 +1,19 @@
+#pragma once
+
+#include "DirectionCosineIecJec.h"
+
+namespace MbD {
+    class DirectionCosineIeqcJec : public DirectionCosineIecJec
+    {
+        //pAijIeJepEI ppAijIeJepEIpEI pAjOIepEIT ppAjOIepEIpEI 
+    public:
+        DirectionCosineIeqcJec();
+        void initialize();
+
+        FRowDsptr pAijIeJepEI;
+        FMatDsptr ppAijIeJepEIpEI;
+        FMatDsptr pAjOIepEIT;
+        std::shared_ptr<FullMatrix<FullColumn<double>>> ppAjOIepEIpEI;
+    };
+}
+

MbDCode/DirectionCosineIeqcJeqc.cpp

@@ -0,0 +1,15 @@
+#include "DirectionCosineIeqcJeqc.h"
+
+using namespace MbD;
+
+MbD::DirectionCosineIeqcJeqc::DirectionCosineIeqcJeqc()
+{
+	initialize();
+}
+
+void MbD::DirectionCosineIeqcJeqc::initialize()
+{
+	pAijIeJepEJ = std::make_shared<FullRow<double>>(4);
+	ppAijIeJepEIpEJ = std::make_shared<FullMatrix<double>>(4, 4);
+	ppAijIeJepEJpEJ = std::make_shared<FullMatrix<double>>(4, 4);
+}

MbDCode/DirectionCosineIeqcJeqc.h

@@ -0,0 +1,21 @@
+#pragma once
+
+#include "DirectionCosineIeqcJec.h"
+
+namespace MbD {
+    class DirectionCosineIeqcJeqc : public DirectionCosineIeqcJec
+    {
+        //pAijIeJepEJ ppAijIeJepEIpEJ ppAijIeJepEJpEJ pAjOJepEJT ppAjOJepEJpEJ 
+    public:
+        DirectionCosineIeqcJeqc();
+        void initialize();
+
+        FRowDsptr pAijIeJepEJ;
+        FMatDsptr ppAijIeJepEIpEJ;
+        FMatDsptr ppAijIeJepEJpEJ;
+        FMatDsptr pAjOJepEJT;
+        std::shared_ptr<FullMatrix<FullColumn<double>>> ppAjOJepEJpEJ;
+
+    };
+}
+

MbDCode/DirectionCosineIeqctJeqc.cpp

@@ -0,0 +1,14 @@
+#include "DirectionCosineIeqctJeqc.h"
+
+using namespace MbD;
+
+MbD::DirectionCosineIeqctJeqc::DirectionCosineIeqctJeqc()
+{
+	initialize();
+}
+
+void MbD::DirectionCosineIeqctJeqc::initialize()
+{
+	ppAijIeJepEIpt = std::make_shared<FullRow<double>>(4);
+	ppAijIeJepEJpt = std::make_shared<FullRow<double>>(4);
+}

MbDCode/DirectionCosineIeqctJeqc.h

@@ -0,0 +1,19 @@
+#pragma once
+
+#include "DirectionCosineIeqcJeqc.h"
+
+namespace MbD {
+    class DirectionCosineIeqctJeqc : public DirectionCosineIeqcJeqc
+    {
+        //pAijIeJept ppAijIeJepEIpt ppAijIeJepEJpt ppAijIeJeptpt 
+    public:
+        DirectionCosineIeqctJeqc();
+        void initialize();
+
+        double pAijIeJept;
+        FRowDsptr ppAijIeJepEIpt;
+        FRowDsptr ppAijIeJepEJpt;
+        double ppAijIeJeptpt;
+    };
+}
+

MbDCode/EndFramec.cpp

@@ -1,3 +1,5 @@
+#include <memory>
+
 #include "EndFramec.h"
 
 using namespace MbD;
@@ -12,9 +14,27 @@ EndFramec::EndFramec(const char* str) : CartesianFrame(str) {
 
 void EndFramec::initialize()
 {
+	CartesianFrame::initialize();
 }
 
 void EndFramec::setMarkerFrame(MarkerFrame* markerFrm)
 {
 	markerFrame = markerFrm;
 }
+
+MarkerFrame* EndFramec::getMarkerFrame()
+{
+	return markerFrame;
+}
+
+void EndFramec::initializeLocally()
+{
+}
+
+void EndFramec::initializeGlobally()
+{
+}
+
+void MbD::EndFramec::EndFrameqctFrom(std::shared_ptr<EndFramec>& newFrmI)
+{
+}

MbDCode/EndFramec.h

@@ -15,10 +15,14 @@ namespace MbD {
         EndFramec(const char* str);
         void initialize();
         void setMarkerFrame(MarkerFrame* markerFrm);
+        MarkerFrame* getMarkerFrame();
+        void initializeLocally() override;
+        void initializeGlobally() override;
+        virtual void EndFrameqctFrom(std::shared_ptr<EndFramec>& frm);
 
         MarkerFrame* markerFrame;
-        FullColDptr rOeO = std::make_shared<FullColumn<double>>(3);
-        FullMatDptr aAOe = std::make_shared<FullMatrix<double>>(3, 3);
+        FColDsptr rOeO = std::make_shared<FullColumn<double>>(3);
+        FMatDsptr aAOe = std::make_shared<FullMatrix<double>>(3, 3);
     };
 }
 

MbDCode/EndFrameqc.cpp

@@ -1,4 +1,7 @@
+#include <memory>
+
 #include "EndFrameqc.h"
+#include "EndFrameqct.h"
 
 using namespace MbD;
 
@@ -12,4 +15,26 @@ EndFrameqc::EndFrameqc(const char* str) : EndFramec(str) {
 
 void EndFrameqc::initialize()
 {
+    EndFramec::initialize();
+    prOeOpE = std::make_unique<FullMatrix<double>>(3, 4);
+    pprOeOpEpE = std::make_unique<FullMatrix<std::shared_ptr<FullColumn<double>>>>(4, 4);
+    pAOepE = std::make_unique<FullColumn<std::shared_ptr<FullMatrix<double>>>>(4);
+    ppAOepEpE = std::make_unique<FullMatrix<std::shared_ptr<FullMatrix<double>>>>(4, 4);
+}
+
+void EndFrameqc::initializeLocally()
+{
+}
+
+void EndFrameqc::initializeGlobally()
+{
+}
+
+void MbD::EndFrameqc::EndFrameqctFrom(std::shared_ptr<EndFramec>& frm)
+{
+	std::shared_ptr<EndFramec> newFrm;
+	newFrm = std::make_shared<EndFrameqct>(frm->getName().c_str());
+	newFrm->setMarkerFrame(frm->getMarkerFrame());
+	//frm.swap(newFrm);
+	std::swap(*(frm.get()), *(newFrm.get()));
 }

MbDCode/EndFrameqc.h

@@ -13,13 +13,14 @@ namespace MbD {
         EndFrameqc();
         EndFrameqc(const char* str);
         void initialize();
-        FullMatDptr prOeOpE = std::make_shared<FullMatrix<double>>(3, 4);
-        //std::shared_ptr<FullMatrix<std::shared_ptr<FullColumn<double>>>> pprOeOpEpE = std::make_shared<FullMatrix<std::shared_ptr<FullColumn<double>>>>(3, 4);
-        std::shared_ptr<FullColumn<std::shared_ptr<FullMatrix<double>>>> pAOepE = std::make_shared<FullColumn<std::shared_ptr<FullMatrix<double>>>>(4);
-        //std::shared_ptr<FullMatrix<std::shared_ptr<FullMatrix<double>>>> ppAOepEpE = std::make_shared<FullMatrix<std::shared_ptr<FullMatrix<double>>>>(4, 4);
-        //FullMatrix<FullColumn<double>>* pprOeOpEpE1 = new FullMatrix<FullColumn<double>>(3, 4);
-        //FullColumn<FullMatrix<double>>* pAOepE1 = new FullColumn<FullMatrix<double>>(4);
-        //FullMatrix<FullMatrix<double>>* ppAOepEpE1 = new FullMatrix<FullMatrix<double>>(4, 4);
+        void initializeLocally() override;
+        void initializeGlobally() override;
+        void EndFrameqctFrom(std::shared_ptr<EndFramec>& frm) override;
+
+        FMatDuptr prOeOpE;
+        std::unique_ptr<FullMatrix<std::shared_ptr<FullColumn<double>>>> pprOeOpEpE;
+        std::unique_ptr<FullColumn<std::shared_ptr<FullMatrix<double>>>> pAOepE;
+        FMatFMatDuptr ppAOepEpE;
     };
 }
 

MbDCode/EndFrameqct.cpp

@@ -1 +1,45 @@
 #include "EndFrameqct.h"
+
+using namespace MbD;
+
+EndFrameqct::EndFrameqct() {
+	initialize();
+}
+
+EndFrameqct::EndFrameqct(const char* str) : EndFrameqc(str) {
+	initialize();
+}
+
+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);
+}
+
+void EndFrameqct::initializeLocally()
+{
+	//rmemBlks == nil
+	//	ifTrue :
+	//[rmem zeroSelf.
+	//	prmempt zeroSelf.
+	//	pprmemptpt zeroSelf] .
+	//	phiThePsiBlks == nil
+	//	ifTrue :
+	//[aAme identity.
+	//	pAmept zeroSelf.
+	//	ppAmeptpt zeroSelf]
+}
+
+void EndFrameqct::initializeGlobally()
+{
+
+}

MbDCode/EndFrameqct.h

@@ -1,15 +1,26 @@
 #pragma once
 #include "EndFrameqc.h"
+#include "Variable.h"
 
 namespace MbD {
-    class EndFrameqct : public EndFrameqc
-    {
-        //time rmemBlks prmemptBlks pprmemptptBlks phiThePsiBlks pPhiThePsiptBlks ppPhiThePsiptptBlks 
-        //rmem prmempt pprmemptpt aAme pAmept ppAmeptpt prOeOpt pprOeOpEpt pprOeOptpt pAOept ppAOepEpt ppAOeptpt 
-    public:
-        double time;
-        FullColDptr rmem, prmempt, pprmemptpt;
-        FullMatDptr aAme, pAmept, ppAmeptpt;
-    };
+	class EndFrameqct : public EndFrameqc
+	{
+		//time rmemBlks prmemptBlks pprmemptptBlks phiThePsiBlks pPhiThePsiptBlks ppPhiThePsiptptBlks 
+		//rmem prmempt pprmemptpt aAme pAmept ppAmeptpt prOeOpt pprOeOpEpt pprOeOptpt pAOept ppAOepEpt ppAOeptpt 
+	public:
+		EndFrameqct();
+		EndFrameqct(const char* str);
+		void initialize();
+		void initializeLocally() override;
+		void initializeGlobally() override;
+
+		double time;
+		std::shared_ptr<FullColumn<std::shared_ptr<Variable>>> rmemBlks;
+		std::shared_ptr<FullColumn<std::shared_ptr<Variable>>> phiThePsiBlks;
+		FColDuptr rmem, prmempt, pprmemptpt, pprOeOptpt;
+		FMatDuptr aAme, pAmept, ppAmeptpt, ppAOeptpt;
+		FMatDuptr pprOeOpEpt;
+		std::unique_ptr<FullColumn<FullMatrix<double>>> ppAOepEpt;
+	};
 }
 

MbDCode/EulerConstraint.cpp

@@ -5,12 +5,14 @@ using namespace MbD;
 
 EulerConstraint::EulerConstraint()
 {
+	initialize();
 }
 
-MbD::EulerConstraint::EulerConstraint(const char* str) : Constraint(str)
+EulerConstraint::EulerConstraint(const char* str) : Constraint(str)
 {
 }
 
-void MbD::EulerConstraint::initialize()
+void EulerConstraint::initialize()
 {
+	pGpE = std::make_shared<FullRow<double>>(4);
 }

MbDCode/EulerConstraint.h

@@ -14,7 +14,7 @@ namespace MbD {
 		EulerConstraint(const char* str);
 		void initialize();
 
-		FullRowDptr pGpE = std::make_shared<FullRow<double>>(4);    //partial derivative of G wrt pE
+		FRowDsptr pGpE;//partial derivative of G wrt pE
 		int iqE = -1;
 	};
 }

MbDCode/EulerParameters.cpp

@@ -6,9 +6,7 @@
 
 using namespace MbD;
 
-//typedef std::shared_ptr<FullMatrix<std::shared_ptr<FullColumn<double>>>> FMatFColDptr;
-
-std::shared_ptr<FullMatrix<std::shared_ptr<FullColumn<double>>>> EulerParameters::ppApEpEtimesColumn(FullColDptr col)
+std::unique_ptr<FullMatrix<std::shared_ptr<FullColumn<double>>>> EulerParameters::ppApEpEtimesColumn(FColDsptr col)
 {
 	double a2c1 = 2 * col->at(0);
 	double a2c2 = 2 * col->at(1);
@@ -26,23 +24,23 @@ std::shared_ptr<FullMatrix<std::shared_ptr<FullColumn<double>>>> EulerParameters
 	auto col33 = std::make_shared<FullColumn<double>>(ListD{ m2c1, m2c2, a2c3 });
 	auto col34 = std::make_shared<FullColumn<double>>(ListD{ m2c2, a2c1, 0 });
 	auto col44 = std::make_shared<FullColumn<double>>(ListD{ a2c1, a2c2, a2c3 });
-	auto answer = std::make_shared<FullMatrix<std::shared_ptr<FullColumn<double>>>>(4, 4);
+	auto answer = std::make_unique<FullMatrix<std::shared_ptr<FullColumn<double>>>>(4, 4);
 	auto row1 = answer->at(0);
 	row1->at(0) = col11;
 	row1->at(1) = col12;
 	row1->at(2) = col13;
 	row1->at(3) = col14;
-	auto row2 = answer->at(0);
+	auto row2 = answer->at(1);
 	row2->at(0) = col12;
 	row2->at(1) = col22;
 	row2->at(2) = col23;
 	row2->at(3) = col24;
-	auto row3 = answer->at(0);
+	auto row3 = answer->at(2);
 	row3->at(0) = col13;
 	row3->at(1) = col23;
 	row3->at(2) = col33;
 	row3->at(3) = col34;
-	auto row4 = answer->at(0);
+	auto row4 = answer->at(3);
 	row4->at(0) = col14;
 	row4->at(1) = col24;
 	row4->at(2) = col34;
@@ -50,16 +48,16 @@ std::shared_ptr<FullMatrix<std::shared_ptr<FullColumn<double>>>> EulerParameters
 	return answer;
 }
 
-std::shared_ptr<FullMatrix<std::shared_ptr<FullMatrix<double>>>> EulerParameters::ppApEpEtimesMatrix(FullMatDptr mat)
+std::unique_ptr<FullMatrix<std::shared_ptr<FullMatrix<double>>>> EulerParameters::ppApEpEtimesMatrix(FMatDsptr mat)
 {
-	FullRowDptr a2m1 = mat->at(0)->times(2.0);
-	FullRowDptr a2m2 = mat->at(1)->times(2.0);
-	FullRowDptr a2m3 = mat->at(2)->times(2.0);
-	FullRowDptr m2m1 = a2m1->negated();
-	FullRowDptr m2m2 = a2m2->negated();
-	FullRowDptr m2m3 = a2m3->negated();
+	FRowDsptr a2m1 = mat->at(0)->times(2.0);
+	FRowDsptr a2m2 = mat->at(1)->times(2.0);
+	FRowDsptr a2m3 = mat->at(2)->times(2.0);
+	FRowDsptr m2m1 = a2m1->negated();
+	FRowDsptr m2m2 = a2m2->negated();
+	FRowDsptr m2m3 = a2m3->negated();
 	auto aaaa = std::make_shared<std::vector<double>>(3, 0.0);
-	FullRowDptr zero = std::make_shared<FullRow<double>>(3, 0.0);
+	FRowDsptr zero = std::make_shared<FullRow<double>>(3, 0.0);
 	auto mat11 = std::make_shared<FullMatrix<double>>(ListFRD{ a2m1, m2m2, m2m3 });
 	auto mat12 = std::make_shared<FullMatrix<double>>(ListFRD{ a2m2, a2m1, zero });
 	auto mat13 = std::make_shared<FullMatrix<double>>(ListFRD{ a2m3, zero, a2m1 });
@@ -70,7 +68,7 @@ std::shared_ptr<FullMatrix<std::shared_ptr<FullMatrix<double>>>> EulerParameters
 	auto mat33 = std::make_shared<FullMatrix<double>>(ListFRD{ m2m1, m2m2, a2m3 });
 	auto mat34 = std::make_shared<FullMatrix<double>>(ListFRD{ m2m2, a2m1, zero });
 	auto mat44 = std::make_shared<FullMatrix<double>>(ListFRD{ a2m1, a2m2, a2m3 });
-	auto answer = std::make_shared<FullMatrix<std::shared_ptr<FullMatrix<double>>>>(4, 4);
+	auto answer = std::make_unique<FullMatrix<std::shared_ptr<FullMatrix<double>>>>(4, 4);
 	auto row1 = answer->at(0);
 	row1->at(0) = mat11;
 	row1->at(1) = mat12;

MbDCode/EulerParameters.h

@@ -8,11 +8,9 @@ namespace MbD {
 	{
 		//aA aB aC pApE
 	public:
-		static std::shared_ptr<FullMatrix<std::shared_ptr<FullColumn<double>>>> ppApEpEtimesColumn(FullColDptr col);
-		static std::shared_ptr<FullMatrix<std::shared_ptr<FullMatrix<double>>>> ppApEpEtimesMatrix(FullMatDptr col);
+		static std::unique_ptr<FullMatrix<std::shared_ptr<FullColumn<double>>>> ppApEpEtimesColumn(FColDsptr col);
+		static std::unique_ptr<FullMatrix<std::shared_ptr<FullMatrix<double>>>> ppApEpEtimesMatrix(FMatDsptr col);
 		
 	};
-
-	//typedef std::shared_ptr<FullMatrix<std::shared_ptr<FullColumn<double>>>> FMatFColDptr;
 }
 

MbDCode/FullColumn.h

@@ -26,7 +26,9 @@ namespace MbD {
 		}
 	};
 
-	typedef std::shared_ptr<FullColumn<double>> FullColDptr;
-	//typedef std::shared_ptr<FullColumn<std::shared_ptr<FullMatrix<double>>>> FullColFMptr;
+	typedef std::shared_ptr<FullColumn<double>> FColDsptr;
+	typedef std::unique_ptr<FullColumn<double>> FColDuptr;
+	//typedef std::shared_ptr<FullColumn<std::shared_ptr<FullMatrix<double>>>> FColFMatDsptr;
+	//typedef std::unique_ptr<FullColumn<std::shared_ptr<FullMatrix<double>>>> FColFMatDuptr;
 }
 

MbDCode/FullMatrix.h

@@ -1,12 +1,11 @@
 #pragma once
 #include <memory>
 
-#include "RowTypeMatrix.h"
 #include "FullColumn.h"
+#include "RowTypeMatrix.h"
 #include "FullRow.h"
 
 namespace MbD {
-	//class FullColumn<double>;
 
 	template <typename T>
 	class FullMatrix : public RowTypeMatrix<std::shared_ptr<FullRow<T>>>
@@ -35,10 +34,13 @@ namespace MbD {
 	};
 
 	typedef std::initializer_list<std::initializer_list<double>> ListListD;
-	typedef std::initializer_list<FullRowDptr> ListFRD;
-	typedef std::shared_ptr<FullMatrix<double>> FullMatDptr;
-	//typedef std::shared_ptr<FullMatrix<std::shared_ptr<FullColumn<double>>>> FMatFColDptr;
-	typedef std::shared_ptr<FullMatrix<std::shared_ptr<FullMatrix<double>>>> FMatFMatDptr;
+	typedef std::initializer_list<FRowDsptr> ListFRD;
+	typedef std::shared_ptr<FullMatrix<double>> FMatDsptr;
+	typedef std::unique_ptr<FullMatrix<double>> FMatDuptr;
+	//typedef std::shared_ptr<FullMatrix<std::shared_ptr<FullColumn<double>>>> FMatFColDsptr;
+	//typedef std::unique_ptr<FullMatrix<std::shared_ptr<FullColumn<double>>>> FMatFColDuptr;
+	typedef std::shared_ptr<FullMatrix<std::shared_ptr<FullMatrix<double>>>> FMatFMatDsptr;
+	typedef std::unique_ptr<FullMatrix<std::shared_ptr<FullMatrix<double>>>> FMatFMatDuptr;
 
 }
 

MbDCode/FullRow.h

@@ -13,7 +13,7 @@ namespace MbD {
 		std::shared_ptr<FullRow<T>> negated();
 
 	};
-	typedef std::shared_ptr<FullRow<double>> FullRowDptr;
+	typedef std::shared_ptr<FullRow<double>> FRowDsptr;
 
 	template<typename T>
 	inline std::shared_ptr<FullRow<T>> FullRow<T>::times(double a)

MbDCode/Item.cpp

@@ -2,6 +2,19 @@
 
 using namespace MbD;
 
+Item::Item() {
+	initialize();
+}
+
+Item::Item(const char* str) : name(str) 
+{
+	initialize();
+}
+
+void Item::initialize()
+{
+}
+
 void Item::setName(std::string& str)
 {
 	name = str;
@@ -11,3 +24,11 @@ const std::string& Item::getName() const
 {
 	return name;
 }
+
+void Item::initializeLocally()
+{
+}
+
+void Item::initializeGlobally()
+{
+}

MbDCode/Item.h

@@ -6,10 +6,14 @@ namespace MbD {
 	{
 		//name
 	public:
-		Item() {}
-		Item(const char* str) : name(str) {}
+		Item();
+		Item(const char* str);
+		virtual void initialize();
+		virtual void initializeLocally();
+		virtual void initializeGlobally();
 		void setName(std::string& str);
 		const std::string& getName() const;
+
 	private:
 		std::string name;
 	};

MbDCode/Joint.cpp

@@ -1,3 +1,5 @@
+#include<algorithm>
+
 #include "Joint.h"
 
 using namespace MbD;
@@ -23,8 +25,10 @@ void Joint::connectsItoJ(std::shared_ptr<EndFramec> frmi, std::shared_ptr<EndFra
 
 void Joint::initializeLocally()
 {
+	std::for_each(constraints->begin(), constraints->end(), [](const auto& constraint) { constraint->initializeLocally(); });
 }
 
 void Joint::initializeGlobally()
 {
+	std::for_each(constraints->begin(), constraints->end(), [](const auto& constraint) { constraint->initializeGlobally(); });
 }

MbDCode/Joint.h

@@ -18,8 +18,8 @@ namespace MbD {
         Joint(const char* str);
         void initialize();
         virtual void connectsItoJ(std::shared_ptr<EndFramec> frmI, std::shared_ptr<EndFramec> frmJ);
-        void initializeLocally();
-        void initializeGlobally();
+        void initializeLocally() override;
+        void initializeGlobally() override;
 
         std::shared_ptr<EndFramec> frmI;
         std::shared_ptr<EndFramec> frmJ;

MbDCode/KinematicIeJe.cpp

@@ -0,0 +1,11 @@
+#include "KinematicIeJe.h"
+
+using namespace MbD;
+
+MbD::KinematicIeJe::KinematicIeJe()
+{
+}
+
+MbD::KinematicIeJe::KinematicIeJe(std::shared_ptr<EndFramec> frmi, std::shared_ptr<EndFramec> frmj) : frmI(frmi), frmJ(frmj)
+{
+}

MbDCode/KinematicIeJe.h

@@ -0,0 +1,17 @@
+#pragma once
+
+#include "Item.h"
+#include "EndFramec.h"
+
+namespace MbD {
+    class KinematicIeJe : public Item
+    {
+        //frmI frmJ 
+    public:
+        KinematicIeJe();
+        KinematicIeJe(std::shared_ptr<EndFramec> frmi, std::shared_ptr<EndFramec> frmj);
+
+        std::shared_ptr<EndFramec> frmI, frmJ;
+    };
+}
+

MbDCode/MarkerFrame.cpp

@@ -1,7 +1,10 @@
+#include<algorithm>
+
 #include "PartFrame.h"
 #include "MarkerFrame.h"
 #include "EndFramec.h"
 #include "EndFrameqc.h"
+#include "EulerParameters.h"
 
 using namespace MbD;
 
@@ -10,13 +13,17 @@ MarkerFrame::MarkerFrame()
 	initialize();
 }
 
-MbD::MarkerFrame::MarkerFrame(const char* str) : CartesianFrame(str) {
+MarkerFrame::MarkerFrame(const char* str) : CartesianFrame(str) {
 	initialize();
 }
 
-void MbD::MarkerFrame::initialize()
+void MarkerFrame::initialize()
 {
-	partFrame = nullptr;
+	//prOmOpE: = StMFullMatrix new : 3 by : 4.
+	//pAOmpE : = StMFullColumn new : 4.
+	//endFrames : = OrderedCollection new
+	prOmOpE = std::make_shared<FullMatrix<double>>(3, 4);
+	pAOmpE = std::make_unique<FullColumn<FullMatrix<double>>>(4);
 	endFrames = std::make_unique<std::vector<std::shared_ptr<EndFramec>>>();
 	auto endFrm = std::make_shared<EndFrameqc>("EndFrame1");
 	this->addEndFrame(endFrm);
@@ -31,12 +38,12 @@ PartFrame* MarkerFrame::getPartFrame() {
 	return partFrame;
 }
 
-void MarkerFrame::setrpmp(FullColDptr x)
+void MarkerFrame::setrpmp(FColDsptr x)
 {
 	rpmp->copy(x);
 }
 
-void MarkerFrame::setaApm(FullMatDptr x)
+void MarkerFrame::setaApm(FMatDsptr x)
 {
 	aApm->copy(x);
 }
@@ -45,3 +52,14 @@ void MarkerFrame::addEndFrame(std::shared_ptr<EndFramec> endFrm)
 	endFrm->setMarkerFrame(this);
 	endFrames->push_back(endFrm);
 }
+
+void MarkerFrame::initializeLocally()
+{
+	pprOmOpEpE = EulerParameters::ppApEpEtimesColumn(rpmp);
+	ppAOmpEpE = EulerParameters::ppApEpEtimesMatrix(aApm);
+	std::for_each(endFrames->begin(), endFrames->end(), [](const auto& endFrame) { endFrame->initializeLocally(); });
+}
+
+void MarkerFrame::initializeGlobally()
+{
+}

MbDCode/MarkerFrame.h

@@ -20,17 +20,21 @@ namespace MbD {
         void initialize();
 		void setPartFrame(PartFrame* partFrm);
 		PartFrame* getPartFrame();
-		void setrpmp(FullColDptr x);
-		void setaApm(FullMatDptr x);
+		void setrpmp(FColDsptr x);
+		void setaApm(FMatDsptr x);
 		void addEndFrame(std::shared_ptr<EndFramec> x);
+		void initializeLocally() override;
+		void initializeGlobally() override;
 
 		PartFrame* partFrame;
-		FullColDptr rpmp = std::make_shared<FullColumn<double>>(3);
-		FullMatDptr aApm = std::make_shared<FullMatrix<double>>(3, 3);
-		FullColDptr rOmO = std::make_shared<FullColumn<double>>(3);
-		FullMatDptr aAOm = std::make_shared<FullMatrix<double>>(3, 3);
-		FullMatDptr prOmOpE = std::make_shared<FullMatrix<double>>(3, 4);
-		FullColumn<FullMatrix<double>>* pAOmpE = new FullColumn<FullMatrix<double>>(4);
+		FColDsptr rpmp = std::make_shared<FullColumn<double>>(3);
+		FMatDsptr aApm = std::make_shared<FullMatrix<double>>(3, 3);
+		FColDsptr rOmO = std::make_shared<FullColumn<double>>(3);
+		FMatDsptr aAOm = std::make_shared<FullMatrix<double>>(3, 3);
+		FMatDsptr prOmOpE;
+		std::unique_ptr<FullColumn<FullMatrix<double>>> pAOmpE;
+		std::unique_ptr<FullMatrix<std::shared_ptr<FullColumn<double>>>> pprOmOpEpE;
+		FMatFMatDuptr ppAOmpEpE;
 		std::unique_ptr<std::vector<std::shared_ptr<EndFramec>>> endFrames;
 
 	};

MbDCode/MbDCode.cpp

@@ -24,14 +24,30 @@ int main()
 	//System& TheSystem = System::getInstance();
 	System& TheSystem = System::getInstance("TheSystem");
 	std::cout << "TheSystem.getName() " << TheSystem.getName() << std::endl;
+	auto systemSolver = TheSystem.systemSolver;
+	systemSolver->errorTolPosKine = 1.0e-6;
+	systemSolver->errorTolAccKine = 1.0e-6;
+	systemSolver->iterMaxPosKine = 25;
+	systemSolver->iterMaxAccKine = 25;
+	systemSolver->tstart = 0;
+	systemSolver->tend = 25.0;
+	systemSolver->hmin = 2.5e-8;
+	systemSolver->hmax = 25.0;
+	systemSolver->hout = 1.0;
+	systemSolver->corAbsTol = 1.0e-6;
+	systemSolver->corRelTol = 1.0e-6;
+	systemSolver->intAbsTol = 1.0e-6;
+	systemSolver->intRelTol = 1.0e-6;
+	systemSolver->iterMaxDyn = 4;
+	systemSolver->orderMax = 5;
+	systemSolver->translationLimit = 9.6058421285615e9;
+	systemSolver->rotationLimit = 0.5;
+
 	std::string str;
-	FullColDptr qX, qE;
-	FullColDptr rpmp;
-	FullMatDptr aApm;
-	FullRowDptr fullRow;
-	auto row = std::make_shared<FullRow<double>>(ListD{ 0.0, 0.0, 0.0, 1.0 });
-	fullRow = std::make_shared<FullRow<double>>(4);
-	fullRow->copy(row);
+	FColDsptr qX, qE;
+	FColDsptr rpmp;
+	FMatDsptr aApm;
+	FRowDsptr fullRow;
 	//
 	auto assembly1 = std::make_shared<Part>("Assembly1");
 	std::cout << "assembly1->getName() " << assembly1->getName() << std::endl;
@@ -66,15 +82,16 @@ int main()
 		marker2->setaApm(aApm);
 		partFrame->addMarkerFrame(marker2);
 	}
+	assembly1->asFixed();
 	//
-	auto part1 = std::make_shared<Part>("Part1");
+	auto crankPart1 = std::make_shared<Part>("Part1");
 	qX = std::make_shared<FullColumn<double>>(ListD{ 0.38423366582893, 6.8384291794733e-9, -0.048029210642807 });
 	qE = std::make_shared<FullColumn<double>>(ListD{ 0.0, 0.0, 1.4248456266393e-10, 1.0 });
-	part1->setqX(qX);
-	part1->setqE(qE);
-	TheSystem.parts->push_back(part1);
+	crankPart1->setqX(qX);
+	crankPart1->setqE(qE);
+	TheSystem.parts->push_back(crankPart1);
 	{
-		auto partFrame = part1->partFrame;
+		auto partFrame = crankPart1->partFrame;
 		auto marker1 = std::make_shared<MarkerFrame>("Marker1");
 		rpmp = std::make_shared<FullColumn<double>>(ListD{ -0.38423368514246, -2.6661567755108e-17, 0.048029210642807 });
 		marker1->setrpmp(rpmp);
@@ -98,14 +115,14 @@ int main()
 		partFrame->addMarkerFrame(marker2);
 	}
 	//
-	auto part2 = std::make_shared<Part>("Part2");
+	auto conrodPart2 = std::make_shared<Part>("Part2");
 	qX = std::make_shared<FullColumn<double>>(ListD{ 0.38423366582893,  0.49215308269277,  0.048029210642807 });
 	qE = std::make_shared<FullColumn<double>>(ListD{ 0.0, 0.0, 0.89871701272344, 0.4385290538168 });
-	part2->setqX(qX);
-	part2->setqE(qE);
-	TheSystem.parts->push_back(part2);
+	conrodPart2->setqX(qX);
+	conrodPart2->setqE(qE);
+	TheSystem.parts->push_back(conrodPart2);
 	{
-		auto partFrame = part2->partFrame;
+		auto partFrame = conrodPart2->partFrame;
 		auto marker1 = std::make_shared<MarkerFrame>("Marker1");
 		rpmp = std::make_shared<FullColumn<double>>(ListD{ -0.6243797383565, 1.1997705489799e-16, -0.048029210642807 });
 		marker1->setrpmp(rpmp);
@@ -129,14 +146,14 @@ int main()
 		partFrame->addMarkerFrame(marker2);
 	}
 	//
-	auto part3 = std::make_shared<Part>("Part3");
+	auto pistonPart3 = std::make_shared<Part>("Part3");
 	qX = std::make_shared<FullColumn<double>>(ListD{ -1.284772285311e-18, 1.4645982581368, -4.788228906425e-17 });
 	qE = std::make_shared<FullColumn<double>>(ListD{ 0.70710678118655, 0.70710678118655, 0.0, 0.0 });
-	part3->setqX(qX);
-	part3->setqE(qE);
-	TheSystem.parts->push_back(part3);
+	pistonPart3->setqX(qX);
+	pistonPart3->setqE(qE);
+	TheSystem.parts->push_back(pistonPart3);
 	{
-		auto partFrame = part3->partFrame;
+		auto partFrame = pistonPart3->partFrame;
 		auto marker1 = std::make_shared<MarkerFrame>("Marker1");
 		rpmp = std::make_shared<FullColumn<double>>(ListD{ -0.48029210642807, 7.6201599718927e-18, -2.816737703896e-17 });
 		marker1->setrpmp(rpmp);
@@ -161,23 +178,23 @@ int main()
 	}
 	//
 	auto cylJoint4 = std::make_shared<CylindricalJoint>("CylJoint4");
-	cylJoint4->connectsItoJ(part3->partFrame->endFrame("Marker2"), assembly1->partFrame->endFrame("Marker1"));
+	cylJoint4->connectsItoJ(pistonPart3->partFrame->endFrame("Marker2"), assembly1->partFrame->endFrame("Marker1"));
 	TheSystem.jointsMotions->push_back(cylJoint4);
 
 	auto revJoint3 = std::make_shared<RevoluteJoint>("RevJoint3");
-	revJoint3->connectsItoJ(part2->partFrame->endFrame("Marker2"), part3->partFrame->endFrame("Marker1"));
+	revJoint3->connectsItoJ(conrodPart2->partFrame->endFrame("Marker2"), pistonPart3->partFrame->endFrame("Marker1"));
 	TheSystem.jointsMotions->push_back(revJoint3);
 	
 	auto revJoint2 = std::make_shared<RevoluteJoint>("RevJoint2");
-	revJoint2->connectsItoJ(part1->partFrame->endFrame("Marker2"), part2->partFrame->endFrame("Marker1"));
+	revJoint2->connectsItoJ(crankPart1->partFrame->endFrame("Marker2"), conrodPart2->partFrame->endFrame("Marker1"));
 	TheSystem.jointsMotions->push_back(revJoint2);
 	
 	auto revJoint1 = std::make_shared<RevoluteJoint>("RevJoint1");
-	revJoint1->connectsItoJ(assembly1->partFrame->endFrame("Marker2"), part1->partFrame->endFrame("Marker1"));
+	revJoint1->connectsItoJ(assembly1->partFrame->endFrame("Marker2"), crankPart1->partFrame->endFrame("Marker1"));
 	TheSystem.jointsMotions->push_back(revJoint1);
 	
 	auto rotMotion1 = std::make_shared<ZRotation>("RotMotion1");
-	rotMotion1->connectsItoJ(assembly1->partFrame->endFrame("Marker2"), part1->partFrame->endFrame("Marker1"));
+	rotMotion1->connectsItoJ(assembly1->partFrame->endFrame("Marker2"), crankPart1->partFrame->endFrame("Marker1"));
 	TheSystem.jointsMotions->push_back(rotMotion1);
 	//
 	TheSystem.runKINEMATICS();

MbDCode/MbDCode.vcxproj

@@ -129,10 +129,18 @@
   <ItemGroup>
     <ClCompile Include="AbsConstraint.cpp" />
     <ClCompile Include="Array.cpp" />
+    <ClCompile Include="BasicUserFunction.cpp" />
     <ClCompile Include="CartesianFrame.cpp" />
+    <ClCompile Include="Constant.cpp" />
     <ClCompile Include="Constraint.cpp" />
+    <ClCompile Include="ConstraintIJ.cpp" />
     <ClCompile Include="CylindricalJoint.cpp" />
     <ClCompile Include="DiagonalMatrix.cpp" />
+    <ClCompile Include="DirectionCosineConstraintIJ.cpp" />
+    <ClCompile Include="DirectionCosineIecJec.cpp" />
+    <ClCompile Include="DirectionCosineIeqcJec.cpp" />
+    <ClCompile Include="DirectionCosineIeqcJeqc.cpp" />
+    <ClCompile Include="DirectionCosineIeqctJeqc.cpp" />
     <ClCompile Include="EndFramec.cpp" />
     <ClCompile Include="EndFrameqc.cpp" />
     <ClCompile Include="EndFrameqct.cpp" />
@@ -145,6 +153,7 @@
     <ClCompile Include="IndependentVariable.cpp" />
     <ClCompile Include="Item.cpp" />
     <ClCompile Include="Joint.cpp" />
+    <ClCompile Include="KinematicIeJe.cpp" />
     <ClCompile Include="MarkerFrame.cpp" />
     <ClCompile Include="MbDCode.cpp" />
     <ClCompile Include="NewtonRaphson.cpp" />
@@ -161,8 +170,8 @@
     <ClCompile Include="Symbolic.cpp" />
     <ClCompile Include="System.cpp" />
     <ClCompile Include="SystemSolver.cpp" />
-    <ClCompile Include="Temp.cpp" />
     <ClCompile Include="Time.cpp" />
+    <ClCompile Include="UserFunction.cpp" />
     <ClCompile Include="Variable.cpp" />
     <ClCompile Include="Vector.cpp" />
     <ClCompile Include="ZRotation.cpp" />
@@ -170,10 +179,18 @@
   <ItemGroup>
     <ClInclude Include="AbsConstraint.h" />
     <ClInclude Include="Array.h" />
+    <ClInclude Include="BasicUserFunction.h" />
     <ClInclude Include="CartesianFrame.h" />
+    <ClInclude Include="Constant.h" />
     <ClInclude Include="Constraint.h" />
+    <ClInclude Include="ConstraintIJ.h" />
     <ClInclude Include="CylindricalJoint.h" />
     <ClInclude Include="DiagonalMatrix.h" />
+    <ClInclude Include="DirectionCosineConstraintIJ.h" />
+    <ClInclude Include="DirectionCosineIecJec.h" />
+    <ClInclude Include="DirectionCosineIeqcJec.h" />
+    <ClInclude Include="DirectionCosineIeqcJeqc.h" />
+    <ClInclude Include="DirectionCosineIeqctJeqc.h" />
     <ClInclude Include="EndFramec.h" />
     <ClInclude Include="EndFrameqc.h" />
     <ClInclude Include="EndFrameqct.h" />
@@ -186,6 +203,7 @@
     <ClInclude Include="IndependentVariable.h" />
     <ClInclude Include="Item.h" />
     <ClInclude Include="Joint.h" />
+    <ClInclude Include="KinematicIeJe.h" />
     <ClInclude Include="MarkerFrame.h" />
     <ClInclude Include="MbDCode.h" />
     <ClInclude Include="NewtonRaphson.h" />
@@ -202,8 +220,8 @@
     <ClInclude Include="Symbolic.h" />
     <ClInclude Include="System.h" />
     <ClInclude Include="SystemSolver.h" />
-    <ClInclude Include="Temp.h" />
     <ClInclude Include="Time.h" />
+    <ClInclude Include="UserFunction.h" />
     <ClInclude Include="Variable.h" />
     <ClInclude Include="Vector.h" />
     <ClInclude Include="ZRotation.h" />

MbDCode/MbDCode.vcxproj.filters

@@ -129,7 +129,34 @@
     <ClCompile Include="Time.cpp">
       <Filter>Source Files</Filter>
     </ClCompile>
-    <ClCompile Include="Temp.cpp">
+    <ClCompile Include="UserFunction.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="BasicUserFunction.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="Constant.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="ConstraintIJ.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="DirectionCosineConstraintIJ.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="KinematicIeJe.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="DirectionCosineIecJec.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="DirectionCosineIeqcJec.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="DirectionCosineIeqcJeqc.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="DirectionCosineIeqctJeqc.cpp">
       <Filter>Source Files</Filter>
     </ClCompile>
   </ItemGroup>
@@ -248,7 +275,34 @@
     <ClInclude Include="Time.h">
       <Filter>Header Files</Filter>
     </ClInclude>
-    <ClInclude Include="Temp.h">
+    <ClInclude Include="UserFunction.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="BasicUserFunction.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="Constant.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="ConstraintIJ.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="DirectionCosineConstraintIJ.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="KinematicIeJe.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="DirectionCosineIecJec.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="DirectionCosineIeqcJec.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="DirectionCosineIeqcJeqc.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="DirectionCosineIeqctJeqc.h">
       <Filter>Header Files</Filter>
     </ClInclude>
   </ItemGroup>

MbDCode/Part.cpp

@@ -16,21 +16,24 @@ void Part::initialize()
 {
 	partFrame = std::make_shared<PartFrame>();
 	partFrame->setPart(this);
+	pTpE = std::make_shared<FullColumn<double>>(4);
+	ppTpEpE = std::make_shared<FullMatrix<double>>(4, 4);
+	ppTpEpEdot = std::make_shared<FullMatrix<double>>(4, 4);
 }
 
-void Part::setqX(FullColDptr x) {
+void Part::setqX(FColDsptr x) {
 	partFrame->setqX(x);
 }
 
-FullColDptr Part::getqX() {
+FColDsptr Part::getqX() {
 	return partFrame->getqX();
 }
 
-void Part::setqE(FullColDptr x) {
+void Part::setqE(FColDsptr x) {
 	partFrame->setqE(x);
 }
 
-FullColDptr Part::getqE() {
+FColDsptr Part::getqE() {
 	return partFrame->getqE();
 }
 
@@ -39,8 +42,14 @@ void Part::setSystem(System& sys)
 	//May be needed in the future
 }
 
+void MbD::Part::asFixed()
+{
+	partFrame->asFixed();
+}
+
 void Part::initializeLocally()
 {
+	partFrame->initializeLocally();
 }
 
 void Part::initializeGlobally()

MbDCode/Part.h

@@ -5,6 +5,7 @@
 #include "System.h"
 #include "PartFrame.h"
 #include "FullColumn.h"
+#include "DiagonalMatrix.h"
 
 namespace MbD {
 	class System;
@@ -17,15 +18,30 @@ namespace MbD {
 		Part();
 		Part(const char* str);
 		void initialize();
-		void setqX(FullColDptr x);
-		FullColDptr getqX();
-		void setqE(FullColDptr x);
-		FullColDptr getqE();
+		void initializeLocally() override;
+		void initializeGlobally() override;
+		void setqX(FColDsptr x);
+		FColDsptr getqX();
+		void setqE(FColDsptr x);
+		FColDsptr getqE();
 		void setSystem(System& sys);
-		void initializeLocally();
-		void initializeGlobally();
+		void asFixed();
 
+		int ipX; 
+		int ipE; 
+		double m; 
+		std::unique_ptr<DiagonalMatrix<double>> aJ;
 		std::shared_ptr<PartFrame> partFrame;
+		FColDsptr pX;
+		FColDsptr pXdot;
+		FColDsptr pE;
+		FColDsptr pEdot;
+		std::unique_ptr<DiagonalMatrix<double>> mX;
+		std::unique_ptr<DiagonalMatrix<double>> mE;
+		FMatDsptr mEdot;
+		FColDsptr pTpE;
+		FMatDsptr ppTpEpE;
+		FMatDsptr ppTpEpEdot;
 	};
 }
 

MbDCode/PartFrame.cpp

@@ -1,3 +1,5 @@
+#include<algorithm>
+
 #include "Part.h"
 #include "PartFrame.h"
 #include "EulerConstraint.h"
@@ -7,28 +9,30 @@
 using namespace MbD;
 
 PartFrame::PartFrame()
+{
+	initialize();
+}
+PartFrame::PartFrame(const char* str) : CartesianFrame(str)
+{
+	initialize();
+}
+void PartFrame::initialize()
 {
 	aGeu = std::make_shared<EulerConstraint>("EulerCon");
 	aGeu->setOwner(this);
 	aGabs = std::make_unique<std::vector<std::shared_ptr<AbsConstraint>>>();
 	markerFrames = std::make_unique<std::vector<std::shared_ptr<MarkerFrame>>>();
 }
-MbD::PartFrame::PartFrame(const char* str) : CartesianFrame(str)
-{
-}
-void MbD::PartFrame::initialize()
-{
-}
-void PartFrame::setqX(FullColDptr x) {
+void PartFrame::setqX(FColDsptr x) {
 	qX->copy(x);
 }
-FullColDptr PartFrame::getqX() {
+FColDsptr PartFrame::getqX() {
 	return qX;
 }
-void PartFrame::setqE(FullColDptr x) {
+void PartFrame::setqE(FColDsptr x) {
 	qE->copy(x);
 }
-FullColDptr PartFrame::getqE() {
+FColDsptr PartFrame::getqE() {
 	return qE;
 }
 void PartFrame::setPart(Part* x) {
@@ -49,3 +53,23 @@ std::shared_ptr<EndFramec> PartFrame::endFrame(std::string name)
 	auto match = std::find_if(markerFrames->begin(), markerFrames->end(), [&](auto mkr) {return mkr->getName() == name; });
 	return (*match)->endFrames->at(0);
 }
+
+void MbD::PartFrame::asFixed()
+{
+	for (int i = 0; i < 6; i++) {
+		auto con = std::make_shared<AbsConstraint>(i);
+		con->setOwner(this);
+		aGabs->push_back(con);
+	}
+}
+
+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()
+{
+}

MbDCode/PartFrame.h

@@ -22,11 +22,14 @@ namespace MbD {
 		PartFrame();
 		PartFrame(const char* str);
 		void initialize();
+		void initializeLocally() override;
+		void initializeGlobally() override;
+		void asFixed();
 
-		void setqX(FullColDptr x);
-		FullColDptr getqX();
-		void setqE(FullColDptr x);
-		FullColDptr getqE();
+		void setqX(FColDsptr x);
+		FColDsptr getqX();
+		void setqE(FColDsptr x);
+		FColDsptr getqE();
 		void setPart(Part* x);
 		Part* getPart();
 		void addMarkerFrame(std::shared_ptr<MarkerFrame> x);
@@ -34,8 +37,12 @@ namespace MbD {
 
 		Part* part;
 		int iqX, iqE;	//Position index of frame variables qX and qE in system list of variables
-		FullColDptr qX = std::make_shared<FullColumn<double>>(3);
-		FullColDptr qE = std::make_shared<FullColumn<double>>(4);
+		FColDsptr qX = std::make_shared<FullColumn<double>>(3);
+		FColDsptr qE = std::make_shared<FullColumn<double>>(4);
+		FColDsptr qXdot = std::make_shared<FullColumn<double>>(3);
+		FColDsptr qEdot = std::make_shared<FullColumn<double>>(4);
+		FColDsptr qXddot = std::make_shared<FullColumn<double>>(3);
+		FColDsptr qEddot = std::make_shared<FullColumn<double>>(4);
 		std::shared_ptr<EulerConstraint> aGeu;
 		std::unique_ptr<std::vector<std::shared_ptr<AbsConstraint>>> aGabs;
 		std::unique_ptr<std::vector<std::shared_ptr<MarkerFrame>>> markerFrames;

MbDCode/PrescribedMotion.cpp

@@ -1,9 +1,11 @@
+#include <iostream>	
 #include <memory>
 #include <typeinfo>
 #include <assert.h>
 
 #include "PrescribedMotion.h"
 #include "EndFrameqct.h"
+#include "Constant.h"
 
 using namespace MbD;
 
@@ -17,15 +19,36 @@ PrescribedMotion::PrescribedMotion(const char* str) : Joint(str) {
 
 void PrescribedMotion::initialize()
 {
+	xBlk = std::make_shared<Constant>(0.0);
+	yBlk = std::make_shared<Constant>(0.0);
+	zBlk = std::make_shared<Constant>(0.0);
+	phiBlk = std::make_shared<Constant>(0.0);
+	theBlk = std::make_shared<Constant>(0.0);
+	psiBlk = std::make_shared<Constant>(0.0);
 }
 
 void PrescribedMotion::connectsItoJ(std::shared_ptr<EndFramec> frmi, std::shared_ptr<EndFramec> frmj)
 {
 	Joint::connectsItoJ(frmi, frmj);
-	if (typeid(frmI).name() != "EndFrameqct") {
-		std::shared_ptr<EndFramec> newFrmI;
-		newFrmI = std::make_shared<EndFrameqct>();
-		std::swap(frmI, newFrmI);
-		assert(typeid(frmI).name() != "EndFrameqct");
-	}
+	frmI->EndFrameqctFrom(frmI);
 }
+
+//void PrescribedMotion::connectsItoJ(std::shared_ptr<EndFramec> frmi, std::shared_ptr<EndFramec> frmj)
+//{
+//	Joint::connectsItoJ(frmi, frmj);
+//	frmI->EndFrameqctFrom(frmI);
+//	decltype(frmi) dddd;
+//	std::cout << "typeid(dddd).name() " << typeid(dddd).name() << std::endl;
+//	std::cout << "typeid(frmI).name() " << typeid(frmI).name() << std::endl;
+//	if (typeid(frmI).name() != "EndFrameqct") {
+//		std::shared_ptr<EndFramec> newFrmI;
+//		newFrmI = std::make_shared<EndFrameqct>(frmI->getName().c_str());
+//		decltype(newFrmI) ffff;
+//		std::cout << "typeid(ffff).name() " << typeid(ffff).name() << std::endl;
+//		auto gggg = std::make_shared<EndFrameqct>(frmI->getName().c_str());
+//		std::cout << "typeid(gggg).name() " << typeid(gggg).name() << std::endl;
+//		std::cout << "typeid(newFrmI).name() " << typeid(newFrmI).name() << std::endl;
+//		std::swap(frmI, newFrmI);
+//		assert(typeid(frmI).name() != "EndFrameqct");
+//	}
+//}

MbDCode/PrescribedMotion.h

@@ -10,6 +10,13 @@ namespace MbD {
         PrescribedMotion(const char* str);
         void initialize();
         void connectsItoJ(std::shared_ptr<EndFramec> frmI, std::shared_ptr<EndFramec> frmJ) override;
+
+        std::shared_ptr<Variable> xBlk;
+        std::shared_ptr<Variable> yBlk;
+        std::shared_ptr<Variable> zBlk;
+        std::shared_ptr<Variable> phiBlk;
+        std::shared_ptr<Variable> theBlk;
+        std::shared_ptr<Variable> psiBlk;
     };
 }
 

MbDCode/Solver.h

@@ -2,6 +2,9 @@
 namespace MbD {
 	class Solver
 	{
+	public:
+		virtual void initializeLocally() = 0;
+		virtual void initializeGlobally() = 0;
 	};
 }
 

MbDCode/Symbolic.cpp

@@ -1 +1,12 @@
 #include "Symbolic.h"
+
+using namespace MbD;
+
+Symbolic::Symbolic()
+{
+	initialize();
+}
+
+void Symbolic::initialize()
+{
+}

MbDCode/Symbolic.h

@@ -2,6 +2,9 @@
 namespace MbD {
 	class Symbolic
 	{
+	public:
+		Symbolic();
+		void initialize();
 	};
 }
 

MbDCode/System.cpp

@@ -50,11 +50,11 @@ void System::runKINEMATICS()
 	systemSolver->runBasicKinematic();
 }
 
-void System::initializeLocally()
+void System::initializeLocally() 
 {
 	hasChanged = false;
-	timeValue = systemSolver->tstart;
-	std::for_each(parts->begin(), parts->end(), [](const auto& part) { part->initializeLocally();	});
+	time->value = systemSolver->tstart;
+	std::for_each(parts->begin(), parts->end(), [](const auto& part) { part->initializeLocally(); });
 	std::for_each(jointsMotions->begin(), jointsMotions->end(), [](const auto& joint) { joint->initializeLocally();	});
 	systemSolver->initializeLocally();
 }

MbDCode/System.h

@@ -35,16 +35,17 @@ namespace MbD {
 			static System singleInstance(str); // Block-scoped static Singleton instance
 			return singleInstance;
 		};
+
+
 		std::unique_ptr<std::vector<std::shared_ptr<Part>>> parts;
 		std::unique_ptr<std::vector<std::shared_ptr<Joint>>> jointsMotions;
 		bool hasChanged = false;
 		std::shared_ptr<SystemSolver> systemSolver;
 		void addPart(std::shared_ptr<Part> part);
 		void runKINEMATICS();
-		void initializeLocally();
-		void initializeGlobally();
+		void initializeLocally() override;
+		void initializeGlobally() override;
 
-		double timeValue;
 		std::unique_ptr<Time> time;
 	private:
 		System();

MbDCode/SystemSolver.cpp

@@ -4,6 +4,9 @@ using namespace MbD;
 
 void SystemSolver::initializeLocally()
 {
+	setsOfRedundantConstraints = std::make_unique<std::vector<std::vector<std::shared_ptr<Constraint>>>>();
+	direction = (tstart < tend) ? 1.0 : -1.0;
+	toutFirst = tstart + (direction * hout);
 }
 
 void SystemSolver::initializeGlobally()

MbDCode/SystemSolver.h

@@ -3,6 +3,7 @@
 
 #include "Solver.h"
 #include "System.h"
+#include "Constraint.h"
 #include "NewtonRaphson.h"
 
 namespace MbD {
@@ -15,23 +16,34 @@ namespace MbD {
 	public:
 		SystemSolver(System* x) : system(x) {
 		}
-		void initializeLocally();
-		void initializeGlobally();
+		void initializeLocally() override;
+		void initializeGlobally() override;
 		void runAllIC();
 		void runBasicKinematic();
 
 		std::shared_ptr<NewtonRaphson> icTypeSolver;
 		System* system;
+		std::unique_ptr<std::vector<std::vector<std::shared_ptr<Constraint>>>> setsOfRedundantConstraints;
 
 		double tstart = 0;
-		double tend = 10;
+		double tend = 25;
 		double toutFirst = 0.0;
 		double errorTolPosKine = 1.0e-6;
-		int iterMaxPosKine = 100;
+		double errorTolAccKine = 1.0e-6;
+		int iterMaxPosKine = 25;
+		int iterMaxAccKine = 25;
 		double hmin = 1.0e-9;
 		double hmax = 1.0;
 		double hout = 1.0e-1;
 		double direction = 1;
+		double corAbsTol = 0;
+		double corRelTol = 0;
+		double intAbsTol = 0;
+		double intRelTol = 0;
+		int iterMaxDyn = 0;
+		int orderMax = 0;
+		double translationLimit = 0;
+		double rotationLimit = 0;
 	};
 }
 

MbDCode/Temp.cpp

@@ -1,11 +0,0 @@
-#include "Temp.h"
-
-using namespace MbD;
-
-//Temp::Temp() //: Constraint()
-//{
-//}
-//
-//Temp::Temp(Item* item) //: Constraint(item)
-//{
-//}

MbDCode/Temp.h

@@ -1,20 +0,0 @@
-#pragma once
-#include <memory>
-#include <vector>
-
-#include "Constraint.h"
-#include "FullRow.h"
-
-namespace MbD {
-    class Temp : public Constraint
-    {
-		//pGpE iqE 
-	public:
-		Temp() {};
-		Temp(Item* item) {};
-
-		FullRowDptr pGpE = std::make_shared<FullRow<double>>(4);    //partial derivative of G wrt pE
-		int iqE = -1;
-	};
-}
-

MbDCode/Time.h

@@ -1,8 +1,8 @@
 #pragma once
 #include "IndependentVariable.h"
+
 namespace MbD {
-    class Time :
-        public IndependentVariable
+    class Time : public IndependentVariable
     {
     public:
         Time();

MbDCode/UserFunction.cpp

@@ -0,0 +1 @@
+#include "UserFunction.h"

MbDCode/UserFunction.h

@@ -0,0 +1,10 @@
+#pragma once
+
+#include "Symbolic.h"
+
+namespace MbD {
+    class UserFunction : public Symbolic
+    {
+    };
+}
+

MbDCode/Variable.cpp

@@ -2,6 +2,24 @@
 
 using namespace MbD;
 
+Variable::Variable()
+{
+	value = 0.0;
+}
+
+Variable::Variable(const char* str) : name(str) 
+{
+	value = 0.0;
+}
+
+Variable::Variable(double val) : value(val)
+{
+}
+
+void Variable::initialize()
+{
+}
+
 void Variable::setName(std::string& str)
 {
 	name = str;

MbDCode/Variable.h

@@ -3,17 +3,19 @@
 
 #include "Symbolic.h"
 namespace MbD {
-    class Variable :
-        public Symbolic
+    class Variable : public Symbolic
     {
         //name value 
-    public:
 	public:
-		Variable() {}
+		Variable();
+		Variable(const char* str);
+		Variable(double val);
+		void initialize();
 		void setName(std::string& str);
 		const std::string& getName() const;
-	private:
+
 		std::string name;
+		double value;
 	};
 }
 

MbDCode/ZRotation.cpp

@@ -1,4 +1,8 @@
 #include "ZRotation.h"
+#include "FullColumn.h"
+#include "DirectionCosineConstraintIJ.h"
+#include "EndFrameqc.h"
+#include "EndFrameqct.h"
 
 using namespace MbD;
 
@@ -13,3 +17,34 @@ ZRotation::ZRotation(const char* str) : PrescribedMotion(str) {
 void ZRotation::initialize()
 {
 }
+
+void MbD::ZRotation::initializeGlobally()
+{
+	//constraints isEmpty
+	//	ifTrue :
+	//[self initMotions.
+	//	self owns : (MbDDirectionCosineConstraintIJ withFrmI : frmI frmJ : frmJ axisI : 2 axisJ : 1).
+	//	TheMbDSystem hasChanged : true]
+	//ifFalse : [super initializeGlobally]
+	if (constraints->empty()) {
+		initMotions();
+		auto dirCosCon = std::make_shared<DirectionCosineConstraintIJ>(frmI, frmJ, 2, 1);
+		addConstraint(dirCosCon);
+		System::getInstance().hasChanged = true;
+	}
+	else {
+		Joint::initializeGlobally();
+	}
+}
+
+void MbD::ZRotation::initMotions()
+{
+	auto xyzBlks = std::initializer_list<std::shared_ptr<Variable>>{ xBlk, yBlk, zBlk };
+	std::static_pointer_cast<EndFrameqct>(frmI)->rmemBlks = std::make_shared<FullColumn<std::shared_ptr<Variable>>>(xyzBlks);
+	auto xyzRotBlks = std::initializer_list<std::shared_ptr<Variable>>{ xBlk, yBlk, zBlk };
+	std::static_pointer_cast<EndFrameqct>(frmI)->phiThePsiBlks = std::make_shared<FullColumn<std::shared_ptr<Variable>>>(xyzRotBlks);
+}
+
+void MbD::ZRotation::addConstraint(std::shared_ptr<Constraint> con)
+{
+}

MbDCode/ZRotation.h

@@ -2,13 +2,16 @@
 #include "PrescribedMotion.h"
 
 namespace MbD {
-    class ZRotation : public PrescribedMotion
-    {
-        //
-    public:
-        ZRotation();
-        ZRotation(const char* str);
-        void initialize();
-    };
+	class ZRotation : public PrescribedMotion
+	{
+		//
+	public:
+		ZRotation();
+		ZRotation(const char* str);
+		void initialize();
+		void initializeGlobally() override;
+		void initMotions();
+		void addConstraint(std::shared_ptr<Constraint> con);
+	};
 }