Merge pull request #1 from Ondsel-Development/working

First PR from working branch

.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/Array.h

@@ -12,6 +12,7 @@ namespace MbD {
         Array(size_t count, const T& value) : std::vector<T>(count, value) {}
         Array(std::initializer_list<T> list) : std::vector<T>{ list } {}
         void copy(std::shared_ptr<Array<T>> x);
+        void zeroSelf();
     };
     template<typename T>
     inline void Array<T>::copy(std::shared_ptr<Array<T>> x)
@@ -20,5 +21,14 @@ namespace MbD {
             this->at(i) = x->at(i);
         }
     }
+    template <>
+    inline void Array<double>::zeroSelf() {
+        for (int i = 0; i < this->size(); i++) {
+            this->at(i) = 0.0;;
+        }
+    }
+    using ListD = std::initializer_list<double>;
+    using ListListD = std::initializer_list<std::initializer_list<double>>;
+    using ListListPairD = std::initializer_list<std::initializer_list<std::initializer_list<double>>>;
 }
 

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/Constant.cpp

@@ -0,0 +1,16 @@
+#include "Constant.h"
+
+using namespace MbD;
+
+Constant::Constant()
+{
+}
+
+Constant::Constant(double val) : Variable(val)
+{
+}
+
+std::shared_ptr<Symbolic> MbD::Constant::differentiateWRT(std::shared_ptr<Symbolic> var)
+{
+	return std::make_shared<Constant>(0.0);
+}

MbDCode/Constant.h

@@ -0,0 +1,14 @@
+#pragma once
+
+#include "Variable.h"
+
+namespace MbD {
+    class Constant : public Variable
+    {
+    public:
+        Constant();
+        Constant(double val);
+        std::shared_ptr<Symbolic> differentiateWRT(std::shared_ptr<Symbolic> var) override;
+    };
+}
+

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(EndFrmcptr frmi, EndFrmcptr frmj) : frmI(frmi), frmJ(frmj)
+{
+	aConstant = 0.0;
+}

MbDCode/ConstraintIJ.h

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

MbDCode/DirectionCosineConstraintIJ.cpp

@@ -0,0 +1,10 @@
+#include "DirectionCosineConstraintIJ.h"
+#include "EndFramec.h"
+
+using namespace MbD;
+
+DirectionCosineConstraintIJ::DirectionCosineConstraintIJ(EndFrmcptr frmi, EndFrmcptr 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(EndFrmcptr frmI, EndFrmcptr 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(EndFrmcptr frmi, EndFrmcptr 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(EndFrmcptr frmI, EndFrmcptr 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<std::shared_ptr<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<std::shared_ptr<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;
@@ -18,3 +20,20 @@ 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

@@ -1,6 +1,9 @@
 #pragma once
+
+#include <memory>
+
 #include "CartesianFrame.h"
-#include "MarkerFrame.h"
+//#include "MarkerFrame.h"
 #include "FullColumn.h"
 #include "FullMatrix.h"
 
@@ -15,10 +18,15 @@ 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);
     };
+    using EndFrmcptr = std::shared_ptr<EndFramec>;
 }
 

MbDCode/EndFrameqc.cpp

@@ -1,4 +1,9 @@
+#include <memory>
+
 #include "EndFrameqc.h"
+#include "EndFrameqct.h"
+#include "Variable.h"
+#include "MarkerFrame.h"
 
 using namespace MbD;
 
@@ -12,4 +17,41 @@ EndFrameqc::EndFrameqc(const char* str) : EndFramec(str) {
 
 void EndFrameqc::initialize()
 {
+	prOeOpE = std::make_shared<FullMatrix<double>>(3, 4);
+	pprOeOpEpE = std::make_shared<FullMatrix<std::shared_ptr<FullColumn<double>>>>(4, 4);
+	pAOepE = std::make_shared<FullColumn<std::shared_ptr<FullMatrix<double>>>>(4);
+	ppAOepEpE = std::make_shared<FullMatrix<std::shared_ptr<FullMatrix<double>>>>(4, 4);
+}
+
+void EndFrameqc::initializeLocally()
+{
+	if (endFrameqct) {
+		endFrameqct->initializeLocally();
+	}
+}
+
+void EndFrameqc::initializeGlobally()
+{
+	if (endFrameqct) {
+		endFrameqct->initializeGlobally();
+	}
+	else {
+		pprOeOpEpE = markerFrame->pprOmOpEpE;
+		ppAOepEpE = markerFrame->ppAOmpEpE;
+	}
+}
+
+void MbD::EndFrameqc::EndFrameqctFrom(EndFrmcptr& frm)
+{
+	endFrameqct = std::make_shared<EndFrameqct>();
+}
+
+void MbD::EndFrameqc::setrmemBlks(std::shared_ptr<FullColumn<std::shared_ptr<Symbolic>>> xyzBlks)
+{
+	std::static_pointer_cast<EndFrameqct>(endFrameqct)->rmemBlks = xyzBlks;
+}
+
+void MbD::EndFrameqc::setphiThePsiBlks(std::shared_ptr<FullColumn<std::shared_ptr<Symbolic>>> xyzRotBlks)
+{
+	std::static_pointer_cast<EndFrameqct>(endFrameqct)->phiThePsiBlks = xyzRotBlks;
 }

MbDCode/EndFrameqc.h

@@ -1,10 +1,9 @@
 #pragma once
+
 #include "EndFramec.h"
-#include "FullColumn.h"
-#include "FullMatrix.h"
 
 namespace MbD {
-    class EndFramec;
+    class Symbolic;
 
     class EndFrameqc : public EndFramec
     {
@@ -13,13 +12,17 @@ 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(EndFrmcptr& frm) override;
+        void setrmemBlks(std::shared_ptr<FullColumn<std::shared_ptr<Symbolic>>> xyzBlks);
+        void setphiThePsiBlks(std::shared_ptr<FullColumn<std::shared_ptr<Symbolic>>> xyzRotBlks);
+
+        FMatDsptr prOeOpE;
+        std::shared_ptr<FullMatrix<std::shared_ptr<FullColumn<double>>>> pprOeOpEpE;
+        std::shared_ptr<FullColumn<std::shared_ptr<FullMatrix<double>>>> pAOepE;
+        FMatFMatDsptr ppAOepEpE;
+        std::shared_ptr<EndFramec> endFrameqct;
     };
 }
 

MbDCode/EndFrameqct.cpp

@@ -1 +1,75 @@
 #include "EndFrameqct.h"
+#include "System.h"
+
+using namespace MbD;
+
+EndFrameqct::EndFrameqct() {
+	initialize();
+}
+
+EndFrameqct::EndFrameqct(const char* str) : EndFrameqc(str) {
+	initialize();
+}
+
+void EndFrameqct::initialize()
+{
+	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()
+{
+	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()
+{
+}

MbDCode/EndFrameqct.h

@@ -2,14 +2,32 @@
 #include "EndFrameqc.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 Symbolic;
+
+	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;
+		void initprmemptBlks();
+		void initpprmemptptBlks();
+		void initpPhiThePsiptBlks();
+		void initppPhiThePsiptptBlks();
+
+		double time;
+		std::shared_ptr<FullColumn<std::shared_ptr<Symbolic>>> rmemBlks, prmemptBlks, pprmemptptBlks;
+		std::shared_ptr<FullColumn<std::shared_ptr<Symbolic>>> phiThePsiBlks, pPhiThePsiptBlks, ppPhiThePsiptptBlks;
+		FColDsptr rmem, prmempt, pprmemptpt, pprOeOptpt;
+		FMatDsptr aAme, pAmept, ppAmeptpt, ppAOeptpt;
+		FMatDsptr pprOeOpEpt;
+		std::shared_ptr<FullColumn<std::shared_ptr<FullMatrix<double>>>> ppAOepEpt;
+	};
+	using EndFrmqctptr = std::shared_ptr<EndFrameqct>;
 }
 

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::shared_ptr<FullMatrix<std::shared_ptr<FullColumn<double>>>> EulerParameters::ppApEpEtimesColumn(FColDsptr col)
 {
 	double a2c1 = 2 * col->at(0);
 	double a2c2 = 2 * col->at(1);
@@ -32,17 +30,17 @@ std::shared_ptr<FullMatrix<std::shared_ptr<FullColumn<double>>>> EulerParameters
 	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::shared_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 });

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::shared_ptr<FullMatrix<std::shared_ptr<FullColumn<double>>>> ppApEpEtimesColumn(FColDsptr col);
+		static std::shared_ptr<FullMatrix<std::shared_ptr<FullMatrix<double>>>> ppApEpEtimesMatrix(FMatDsptr col);
 		
 	};
-
-	//typedef std::shared_ptr<FullMatrix<std::shared_ptr<FullColumn<double>>>> FMatFColDptr;
 }
 

MbDCode/ExpressionX.cpp

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

MbDCode/ExpressionX.h

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

MbDCode/FullColumn.h

@@ -25,8 +25,7 @@ namespace MbD {
 			return ss.str();
 		}
 	};
-
-	typedef std::shared_ptr<FullColumn<double>> FullColDptr;
-	//typedef std::shared_ptr<FullColumn<std::shared_ptr<FullMatrix<double>>>> FullColFMptr;
+	using FColDsptr = std::shared_ptr<FullColumn<double>>;
+	//using FColFMatDsptr = std::shared_ptr<FullColumn<std::shared_ptr<FullMatrix<double>>>>;
 }
 

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>>>
@@ -32,13 +31,18 @@ namespace MbD {
 				this->push_back(row);
 			}
 		}
+		void identity();
 	};
-
-	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;
-
+	template <>
+	inline void FullMatrix<double>::identity() {
+		this->zeroSelf();
+		for (int i = 0; i < this->size(); i++) {
+			this->at(i)->at(i) = 1.0;
+		}
+	}
+	using FMatDsptr = std::shared_ptr<FullMatrix<double>>;
+	using FMatDsptr = std::shared_ptr<FullMatrix<double>>;
+	//using FMatFColDsptr = std::shared_ptr<FullMatrix<std::shared_ptr<FullColumn<double>>>>;
+	using FMatFMatDsptr = std::shared_ptr<FullMatrix<std::shared_ptr<FullMatrix<double>>>>;
 }
 

MbDCode/FullRow.h

@@ -13,7 +13,6 @@ namespace MbD {
 		std::shared_ptr<FullRow<T>> negated();
 
 	};
-	typedef std::shared_ptr<FullRow<double>> FullRowDptr;
 
 	template<typename T>
 	inline std::shared_ptr<FullRow<T>> FullRow<T>::times(double a)
@@ -30,5 +29,7 @@ namespace MbD {
 	{
 		return this->times(-1.0);
 	}
+	using ListFRD = std::initializer_list<std::shared_ptr<FullRow<double>>>;
+	using FRowDsptr = std::shared_ptr<FullRow<double>>;
 }
 

MbDCode/Function.cpp

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

MbDCode/Function.h

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

MbDCode/FunctionWithManyArgs.cpp

@@ -0,0 +1,27 @@
+#include "FunctionWithManyArgs.h"
+#include "Symbolic.h"
+
+using namespace MbD;
+
+MbD::FunctionWithManyArgs::FunctionWithManyArgs(std::shared_ptr<Symbolic> term)
+{
+	terms = std::make_shared<std::vector<std::shared_ptr<Symbolic>>>();
+	terms->push_back(term);
+}
+
+MbD::FunctionWithManyArgs::FunctionWithManyArgs(std::shared_ptr<Symbolic> term, std::shared_ptr<Symbolic> term1) : FunctionWithManyArgs(term)
+{
+	terms->push_back(term1);
+}
+
+MbD::FunctionWithManyArgs::FunctionWithManyArgs(std::shared_ptr<Symbolic> term, std::shared_ptr<Symbolic> term1, std::shared_ptr<Symbolic> term2) : FunctionWithManyArgs(term, term1)
+{
+	terms->push_back(term2);
+}
+
+MbD::FunctionWithManyArgs::FunctionWithManyArgs(std::shared_ptr<std::vector<std::shared_ptr<Symbolic>>> _terms) {
+	terms = std::make_shared<std::vector<std::shared_ptr<Symbolic>>>();
+	for (int i = 0; i < _terms->size(); i++)
+		terms->push_back(_terms->at(i));
+}
+

MbDCode/FunctionWithManyArgs.h

@@ -0,0 +1,18 @@
+#pragma once
+
+#include "Function.h"
+
+namespace MbD {
+    class FunctionWithManyArgs : public Function
+    {
+        //terms
+    public:
+        FunctionWithManyArgs(std::shared_ptr<Symbolic> term);
+        FunctionWithManyArgs(std::shared_ptr<Symbolic> term, std::shared_ptr<Symbolic> term1);
+        FunctionWithManyArgs(std::shared_ptr<Symbolic> term, std::shared_ptr<Symbolic> term1, std::shared_ptr<Symbolic> term2);
+        FunctionWithManyArgs(std::shared_ptr<std::vector<std::shared_ptr<Symbolic>>> _terms);
+
+        std::shared_ptr<std::vector<std::shared_ptr<Symbolic>>> terms;
+    };
+}
+

MbDCode/FunctionX.cpp

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

MbDCode/FunctionX.h

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

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;
@@ -12,10 +14,10 @@ Joint::Joint(const char* str) : Item(str) {
 
 void Joint::initialize()
 {
-	constraints = std::make_unique<std::vector<std::shared_ptr<Constraint>>>();
+	constraints = std::make_shared<std::vector<std::shared_ptr<Constraint>>>();
 }
 
-void Joint::connectsItoJ(std::shared_ptr<EndFramec> frmi, std::shared_ptr<EndFramec> frmj)
+void Joint::connectsItoJ(EndFrmcptr frmi, EndFrmcptr frmj)
 {
 	frmI = frmi;
 	frmJ = frmj;
@@ -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

@@ -2,13 +2,15 @@
 #include <memory>
 #include <vector>
 
+//#include "typedef.h"
 #include "Item.h"
 #include "EndFramec.h"
 #include "Constraint.h"
 
 namespace MbD {
-    class EndFramec;
+    //class EndFramec;
     class Constraint;
+    //using EndFrmcptr = std::shared_ptr<EndFramec>;
 
     class Joint : public Item
     {
@@ -17,13 +19,13 @@ namespace MbD {
         Joint();
         Joint(const char* str);
         void initialize();
-        virtual void connectsItoJ(std::shared_ptr<EndFramec> frmI, std::shared_ptr<EndFramec> frmJ);
-        void initializeLocally();
-        void initializeGlobally();
+        virtual void connectsItoJ(EndFrmcptr frmI, EndFrmcptr frmJ);
+        void initializeLocally() override;
+        void initializeGlobally() override;
 
-        std::shared_ptr<EndFramec> frmI;
-        std::shared_ptr<EndFramec> frmJ;
-        std::unique_ptr<std::vector<std::shared_ptr<Constraint>>> constraints;
+        EndFrmcptr frmI;
+        EndFrmcptr frmJ;
+        std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> constraints;
 
     };
 }

MbDCode/KinematicIeJe.cpp

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

MbDCode/KinematicIeJe.h

@@ -0,0 +1,18 @@
+#pragma once
+
+//#include "typedef.h"
+#include "Item.h"
+#include "EndFramec.h"
+
+namespace MbD {
+    class KinematicIeJe : public Item
+    {
+        //frmI frmJ 
+    public:
+        KinematicIeJe();
+        KinematicIeJe(EndFrmcptr frmi, EndFrmcptr frmj);
+
+        EndFrmcptr 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,14 +13,15 @@ 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;
-	endFrames = std::make_unique<std::vector<std::shared_ptr<EndFramec>>>();
+	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");
 	this->addEndFrame(endFrm);
 }
@@ -31,17 +35,29 @@ 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);
 }
-void MarkerFrame::addEndFrame(std::shared_ptr<EndFramec> endFrm)
+void MarkerFrame::addEndFrame(EndFrmcptr 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()
+{
+	std::for_each(endFrames->begin(), endFrames->end(), [](const auto& endFrame) { endFrame->initializeGlobally(); });
+}

MbDCode/MarkerFrame.h

@@ -10,6 +10,7 @@
 namespace MbD {
 	class PartFrame;
 	class EndFramec;
+	using EndFrmcptr = std::shared_ptr<EndFramec>;
 
 	class MarkerFrame : public CartesianFrame
 	{
@@ -20,18 +21,22 @@ namespace MbD {
         void initialize();
 		void setPartFrame(PartFrame* partFrm);
 		PartFrame* getPartFrame();
-		void setrpmp(FullColDptr x);
-		void setaApm(FullMatDptr x);
-		void addEndFrame(std::shared_ptr<EndFramec> x);
+		void setrpmp(FColDsptr x);
+		void setaApm(FMatDsptr x);
+		void addEndFrame(EndFrmcptr 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);
-		std::unique_ptr<std::vector<std::shared_ptr<EndFramec>>> endFrames;
+		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::shared_ptr<FullColumn<std::shared_ptr<FullMatrix<double>>>> pAOmpE;
+		std::shared_ptr<FullMatrix<std::shared_ptr<FullColumn<double>>>> pprOmOpEpE;
+		FMatFMatDsptr ppAOmpEpE;
+		std::shared_ptr<std::vector<EndFrmcptr>> endFrames;
 
 	};
 }

MbDCode/MbDCode.cpp

@@ -6,6 +6,7 @@
 
 #include <iostream>	
 #include "System.h"
+#include "Constant.h"
 #include "FullColumn.h"
 #include "FullMatrix.h"
 #include "Part.h"
@@ -14,6 +15,8 @@
 #include "RevoluteJoint.h"
 #include "ZRotation.h"
 #include "EndFrameqc.h"
+#include "EndFrameqct.h"
+#include "Product.h"
 #include "MbDCode.h"
 
 using namespace MbD;
@@ -21,17 +24,32 @@ using namespace MbD;
 int main()
 {
 	std::cout << "Hello World!\n";
-	//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 +84,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 +117,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 +148,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 +180,26 @@ 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"));
+	auto omega = std::make_shared<Constant>(6.2831853071796);
+	auto timeScale = std::make_shared<Constant>(0.04);
+	rotMotion1->phiBlk = std::make_shared<Product>(omega, timeScale, TheSystem.time);
 	TheSystem.jointsMotions->push_back(rotMotion1);
 	//
 	TheSystem.runKINEMATICS();

MbDCode/MbDCode.vcxproj

@@ -129,28 +129,42 @@
   <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" />
     <ClCompile Include="EulerArray.cpp" />
     <ClCompile Include="EulerConstraint.cpp" />
     <ClCompile Include="EulerParameters.cpp" />
+    <ClCompile Include="ExpressionX.cpp" />
     <ClCompile Include="FullColumn.cpp" />
     <ClCompile Include="FullMatrix.cpp" />
     <ClCompile Include="FullRow.cpp" />
+    <ClCompile Include="Function.cpp" />
+    <ClCompile Include="FunctionWithManyArgs.cpp" />
+    <ClCompile Include="FunctionX.cpp" />
     <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" />
     <ClCompile Include="Part.cpp" />
     <ClCompile Include="PartFrame.cpp" />
     <ClCompile Include="PrescribedMotion.cpp" />
+    <ClCompile Include="Product.cpp" />
     <ClCompile Include="RevoluteJoint.cpp" />
     <ClCompile Include="RowTypeMatrix.cpp" />
     <ClCompile Include="Solver.cpp" />
@@ -158,11 +172,12 @@
     <ClCompile Include="SparseMatrix.cpp" />
     <ClCompile Include="SparseRow.cpp" />
     <ClCompile Include="SparseVector.cpp" />
+    <ClCompile Include="Sum.cpp" />
     <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,28 +185,42 @@
   <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" />
     <ClInclude Include="EulerArray.h" />
     <ClInclude Include="EulerConstraint.h" />
     <ClInclude Include="EulerParameters.h" />
+    <ClInclude Include="ExpressionX.h" />
     <ClInclude Include="FullColumn.h" />
     <ClInclude Include="FullMatrix.h" />
     <ClInclude Include="FullRow.h" />
+    <ClInclude Include="Function.h" />
+    <ClInclude Include="FunctionWithManyArgs.h" />
+    <ClInclude Include="FunctionX.h" />
     <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" />
     <ClInclude Include="Part.h" />
     <ClInclude Include="PartFrame.h" />
     <ClInclude Include="PrescribedMotion.h" />
+    <ClInclude Include="Product.h" />
     <ClInclude Include="RevoluteJoint.h" />
     <ClInclude Include="RowTypeMatrix.h" />
     <ClInclude Include="Solver.h" />
@@ -199,11 +228,13 @@
     <ClInclude Include="SparseMatrix.h" />
     <ClInclude Include="SparseRow.h" />
     <ClInclude Include="SparseVector.h" />
+    <ClInclude Include="Sum.h" />
     <ClInclude Include="Symbolic.h" />
     <ClInclude Include="System.h" />
     <ClInclude Include="SystemSolver.h" />
-    <ClInclude Include="Temp.h" />
     <ClInclude Include="Time.h" />
+    <ClInclude Include="typedef.h" />
+    <ClInclude Include="UserFunction.h" />
     <ClInclude Include="Variable.h" />
     <ClInclude Include="Vector.h" />
     <ClInclude Include="ZRotation.h" />

MbDCode/MbDCode.vcxproj.filters

@@ -129,7 +129,52 @@
     <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>
+    <ClCompile Include="Function.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="FunctionWithManyArgs.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="Product.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="Sum.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="FunctionX.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="ExpressionX.cpp">
       <Filter>Source Files</Filter>
     </ClCompile>
   </ItemGroup>
@@ -248,7 +293,55 @@
     <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>
+    <ClInclude Include="typedef.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="Function.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="FunctionWithManyArgs.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="Product.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="Sum.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="FunctionX.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="ExpressionX.h">
       <Filter>Header Files</Filter>
     </ClInclude>
   </ItemGroup>

MbDCode/NewtonRaphson.h

@@ -1,8 +1,18 @@
 #pragma once
+
+#include <memory>
+
 #include "Solver.h"
+
+#include "FullColumn.h"
+
 namespace MbD {
     class NewtonRaphson : public Solver
     {
+        //system xold x dx dxNorm dxNorms dxTol y yNorm yNormOld yNorms yNormTol pypx iterNo iterMax nDivergence nBackTracking twoAlp lam 
+    public:
+        std::shared_ptr<FullColumn<double>> xold, x, dx, dxTol, y;
+        double dxNorm, yNorm, yNormOld, yNormTol, twoAlp, lam;
     };
 }
 

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,10 +42,17 @@ 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()
 {
+	partFrame->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::shared_ptr<DiagonalMatrix<double>> aJ;
 		std::shared_ptr<PartFrame> partFrame;
+		FColDsptr pX;
+		FColDsptr pXdot;
+		FColDsptr pE;
+		FColDsptr pEdot;
+		std::shared_ptr<DiagonalMatrix<double>> mX;
+		std::shared_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>>>();
+	aGabs = std::make_shared<std::vector<std::shared_ptr<AbsConstraint>>>();
+	markerFrames = std::make_shared<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) {
@@ -44,8 +48,31 @@ void PartFrame::addMarkerFrame(std::shared_ptr<MarkerFrame> markerFrame)
 	markerFrames->push_back(markerFrame);
 }
 
-std::shared_ptr<EndFramec> PartFrame::endFrame(std::string name)
+EndFrmcptr 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()
+{
+	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(); });
+}

MbDCode/PartFrame.h

@@ -14,6 +14,7 @@ namespace MbD {
 	class Part;
 	class MarkerFrame;
 	class EndFramec;
+	using EndFrmcptr = std::shared_ptr<EndFramec>;
 
 	class PartFrame : public CartesianFrame
 	{
@@ -22,23 +23,30 @@ 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);
-		std::shared_ptr<EndFramec> endFrame(std::string name);
+		EndFrmcptr endFrame(std::string name);
 
 		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;
+		std::shared_ptr<std::vector<std::shared_ptr<AbsConstraint>>> aGabs;
+		std::shared_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,16 @@ 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)
+void PrescribedMotion::connectsItoJ(EndFrmcptr frmi, EndFrmcptr 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);
 }

MbDCode/PrescribedMotion.h

@@ -2,6 +2,9 @@
 #include "Joint.h"
 
 namespace MbD {
+
+    class Symbolic;
+
     class PrescribedMotion : public Joint
     {
         //xBlk yBlk zBlk phiBlk theBlk psiBlk 
@@ -9,7 +12,14 @@ namespace MbD {
         PrescribedMotion();
         PrescribedMotion(const char* str);
         void initialize();
-        void connectsItoJ(std::shared_ptr<EndFramec> frmI, std::shared_ptr<EndFramec> frmJ) override;
+        void connectsItoJ(EndFrmcptr frmI, EndFrmcptr frmJ) override;
+
+        std::shared_ptr<Symbolic> xBlk;
+        std::shared_ptr<Symbolic> yBlk;
+        std::shared_ptr<Symbolic> zBlk;
+        std::shared_ptr<Symbolic> phiBlk;
+        std::shared_ptr<Symbolic> theBlk;
+        std::shared_ptr<Symbolic> psiBlk;
     };
 }
 

MbDCode/Product.cpp

@@ -0,0 +1,10 @@
+#include "Product.h"
+
+using namespace MbD;
+
+double MbD::Product::getValue()
+{
+	double answer = 1.0;
+	for (int i = 0; i < terms->size(); i++) answer *= terms->at(i)->getValue();
+	return answer;
+}

MbDCode/Product.h

@@ -0,0 +1,16 @@
+#pragma once
+
+#include "FunctionWithManyArgs.h"
+
+namespace MbD {
+    class Product : public FunctionWithManyArgs
+    {
+    public:
+        Product(std::shared_ptr<Symbolic> term) : FunctionWithManyArgs(term) {}
+        Product(std::shared_ptr<Symbolic> term, std::shared_ptr<Symbolic> term1) : FunctionWithManyArgs(term, term1) {}
+        Product(std::shared_ptr<Symbolic> term, std::shared_ptr<Symbolic> term1, std::shared_ptr<Symbolic> term2) : FunctionWithManyArgs(term, term1, term2) {}
+        Product(std::shared_ptr<std::vector<std::shared_ptr<Symbolic>>> _terms) : FunctionWithManyArgs(_terms) {}
+        double getValue() override;
+    };
+}
+

MbDCode/RowTypeMatrix.h

@@ -1,6 +1,10 @@
 #pragma once
+
 #include "Array.h"
+#include "FullRow.h"
+
 namespace MbD {
+
 	template <typename T>
 	class RowTypeMatrix : public Array<T>
 	{
@@ -8,6 +12,7 @@ namespace MbD {
 		RowTypeMatrix() {}
 		RowTypeMatrix(std::initializer_list<T> list) : Array<T>{ list } {}
 		void copy(std::shared_ptr<RowTypeMatrix<T>> x);
+		void zeroSelf();
 	};
 
 	template<typename T>
@@ -17,5 +22,11 @@ namespace MbD {
 			this->at(i)->copy(x->at(i));
 		}
 	}
+	template <>
+	inline void RowTypeMatrix< std::shared_ptr<FullRow<double>>>::zeroSelf() {
+		for (int i = 0; i < this->size(); i++) {
+			this->at(i)->zeroSelf();
+		}
+	}
 }
 

MbDCode/Solver.h

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

MbDCode/SparseMatrix.h

@@ -15,8 +15,5 @@ namespace MbD {
 			}
 		}
 	};
-	typedef std::shared_ptr<SparseMatrix<double>> SpMatDptr;
-	typedef std::initializer_list<std::initializer_list<std::initializer_list<double>>> ListListPairD;
-}
-
-
+	using SpMatDptr = std::shared_ptr<SparseMatrix<double>>;
+}

MbDCode/SparseRow.h

@@ -13,6 +13,6 @@ namespace MbD {
 		SparseRow(std::initializer_list<std::pair<const int, T>> list) : SparseVector<T>{ list } {}
 		SparseRow(std::initializer_list<std::initializer_list<T>> list) : SparseVector<T>{ list } {}
 	};
-	typedef std::shared_ptr<SparseRow<double>> SpRowDptr;
+	using SpRowDptr = std::shared_ptr<SparseRow<double>>;
 }
 

MbDCode/Sum.cpp

@@ -0,0 +1,10 @@
+#include "Sum.h"
+
+using namespace MbD;
+
+double MbD::Sum::getValue()
+{
+	double answer = 0.0;
+	for (int i = 0; i < terms->size(); i++) answer += terms->at(i)->getValue();
+	return answer;
+}

MbDCode/Sum.h

@@ -0,0 +1,16 @@
+#pragma once
+
+#include "FunctionWithManyArgs.h"
+
+namespace MbD {
+    class Sum : public FunctionWithManyArgs
+    {
+    public:
+        Sum(std::shared_ptr<Symbolic> term) : FunctionWithManyArgs(term) {}
+        Sum(std::shared_ptr<Symbolic> term, std::shared_ptr<Symbolic> term1) : FunctionWithManyArgs(term, term1) {}
+        Sum(std::shared_ptr<Symbolic> term, std::shared_ptr<Symbolic> term1, std::shared_ptr<Symbolic> term2) : FunctionWithManyArgs(term, term1, term2) {}
+        Sum(std::shared_ptr<std::vector<std::shared_ptr<Symbolic>>> _terms) : FunctionWithManyArgs(_terms) {}
+        double getValue() override;
+    };
+}
+

MbDCode/Symbolic.cpp

@@ -1 +1,42 @@
+#include <memory>
+#include <unordered_set>
+#include <assert.h>
+
 #include "Symbolic.h"
+#include "Constant.h"
+
+using namespace MbD;
+
+Symbolic::Symbolic()
+{
+	initialize();
+}
+
+void Symbolic::initialize()
+{
+}
+
+std::shared_ptr<Symbolic> MbD::Symbolic::differentiateWRT(std::shared_ptr<Symbolic> var)
+{
+	return std::shared_ptr<Symbolic>();
+}
+
+std::shared_ptr<Symbolic> MbD::Symbolic::simplified()
+{
+	//Debug 
+	auto set = nullptr;
+	//Debug auto set = std::make_shared<std::unordered_set<Symbolic>>();
+	auto answer = expandUntil(set);
+	auto set1 = nullptr; //std::make_shared<std::unordered_set<Symbolic>>();
+	return answer->simplifyUntil(set1);
+}
+
+std::shared_ptr<Symbolic> MbD::Symbolic::expandUntil(std::shared_ptr<std::unordered_set<Symbolic>> set)
+{
+	return std::make_shared<Constant>(0.0);
+}
+
+std::shared_ptr<Symbolic> MbD::Symbolic::simplifyUntil(std::shared_ptr<std::unordered_set<Symbolic>> set)
+{
+	return std::make_shared<Constant>(0.0);
+}

MbDCode/Symbolic.h

@@ -1,7 +1,20 @@
 #pragma once
+
+#include <unordered_set>
+#include <memory>
+
 namespace MbD {
+
 	class Symbolic
 	{
+	public:
+		Symbolic();
+		void initialize();
+		virtual std::shared_ptr<Symbolic> differentiateWRT(std::shared_ptr<Symbolic> var);
+		virtual std::shared_ptr<Symbolic> simplified();
+		virtual std::shared_ptr<Symbolic> expandUntil(std::shared_ptr<std::unordered_set<Symbolic>> set);
+		virtual std::shared_ptr<Symbolic> simplifyUntil(std::shared_ptr<std::unordered_set<Symbolic>> set);
+		virtual double getValue() = 0;
 	};
 }
 

MbDCode/System.cpp

@@ -5,17 +5,17 @@
 using namespace MbD;
 
 System::System() {
-	time = std::make_unique<Time>();
-	parts = std::make_unique<std::vector<std::shared_ptr<Part>>>();
-	jointsMotions = std::make_unique<std::vector<std::shared_ptr<Joint>>>();
-	systemSolver = std::make_unique<SystemSolver>(this);
+	time = std::make_shared<Time>();
+	parts = std::make_shared<std::vector<std::shared_ptr<Part>>>();
+	jointsMotions = std::make_shared<std::vector<std::shared_ptr<Joint>>>();
+	systemSolver = std::make_shared<SystemSolver>(this);
 }
 
 System::System(const char* str) : Item(str) {
-	time = std::make_unique<Time>();
-	parts = std::make_unique<std::vector<std::shared_ptr<Part>>>();
-	jointsMotions = std::make_unique<std::vector<std::shared_ptr<Joint>>>();
-	systemSolver = std::make_unique<SystemSolver>(this);
+	time = std::make_shared<Time>();
+	parts = std::make_shared<std::vector<std::shared_ptr<Part>>>();
+	jointsMotions = std::make_shared<std::vector<std::shared_ptr<Joint>>>();
+	systemSolver = std::make_shared<SystemSolver>(this);
 }
 
 void System::addPart(std::shared_ptr<Part> part)
@@ -50,15 +50,18 @@ 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();
 }
 
 void System::initializeGlobally()
 {
+	std::for_each(parts->begin(), parts->end(), [](const auto& part) { part->initializeGlobally(); });
+	std::for_each(jointsMotions->begin(), jointsMotions->end(), [](const auto& joint) { joint->initializeGlobally();	});
+	systemSolver->initializeGlobally();
 }

MbDCode/System.h

@@ -35,17 +35,18 @@ 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;
+
+
+		std::shared_ptr<std::vector<std::shared_ptr<Part>>> parts;
+		std::shared_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;
+		std::shared_ptr<Time> time;
 	private:
 		System();
 		System(const char* str);

MbDCode/SystemSolver.cpp

@@ -4,6 +4,9 @@ using namespace MbD;
 
 void SystemSolver::initializeLocally()
 {
+	setsOfRedundantConstraints = std::make_shared<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::shared_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;
@@ -11,3 +29,8 @@ const std::string& Variable::getName() const
 {
 	return name;
 }
+
+double MbD::Variable::getValue()
+{
+	return value;
+}

MbDCode/Variable.h

@@ -3,17 +3,20 @@
 
 #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:
+		double getValue() override;
+
 		std::string name;
+		double value;
 	};
 }
 

MbDCode/Vector.h

@@ -12,5 +12,3 @@ namespace MbD {
         Vector(std::initializer_list<T> list) : Array<T>{ list } {}
     };
 }
-
-typedef std::initializer_list<double> ListD;

MbDCode/ZRotation.cpp

@@ -1,4 +1,9 @@
+#include "System.h"
 #include "ZRotation.h"
+#include "FullColumn.h"
+#include "DirectionCosineConstraintIJ.h"
+#include "EndFrameqc.h"
+#include "EndFrameqct.h"
 
 using namespace MbD;
 
@@ -13,3 +18,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<Symbolic>>{ xBlk, yBlk, zBlk };
+	std::static_pointer_cast<EndFrameqc>(frmI)->setrmemBlks(std::make_shared<FullColumn<std::shared_ptr<Symbolic>>>(xyzBlks));
+	auto xyzRotBlks = std::initializer_list<std::shared_ptr<Symbolic>>{ phiBlk, theBlk, psiBlk };
+	std::static_pointer_cast<EndFrameqc>(frmI)->setphiThePsiBlks(std::make_shared<FullColumn<std::shared_ptr<Symbolic>>>(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);
+	};
 }
 

MbDCode/typedef.h

@@ -0,0 +1,30 @@
+#pragma once
+#include <memory>
+
+namespace MbD {
+
+	//class EndFramec;
+	//class SparseRow;
+	//class SparseMatrix;
+
+
+
+	//typedef std::initializer_list<double> ListD;
+	//typedef std::initializer_list<std::initializer_list<double>> ListListD;
+	//typedef std::initializer_list<std::initializer_list<std::initializer_list<double>>> ListListPairD;
+	//typedef std::initializer_list<std::shared_ptr<FullRow<double>>> ListFRD;
+	//typedef std::shared_ptr<FullColumn<double>> FColDsptr;
+	//typedef std::shared_ptr<FullColumn<std::shared_ptr<FullMatrix<double>>>> FColFMatDsptr;
+	//typedef std::shared_ptr<FullRow<double>> FRowDsptr;
+	//typedef std::shared_ptr<FullMatrix<double>> FMatDsptr;
+	//typedef std::shared_ptr<FullMatrix<double>> FMatDsptr;
+	//typedef std::shared_ptr<FullMatrix<std::shared_ptr<FullMatrix<double>>>> FMatFMatDsptr;
+	//typedef std::shared_ptr<FullMatrix<std::shared_ptr<FullMatrix<double>>>> FMatFMatDsptr;
+	//typedef std::shared_ptr<FullMatrix<std::shared_ptr<FullColumn<double>>>> FMatFColDsptr;
+	//typedef std::shared_ptr<SparseRow<double>> SpRowDptr;
+	//typedef std::shared_ptr<SparseMatrix<double>> SpMatDptr;
+	//typedef std::shared_ptr<EndFramec> EndFrmcptr;
+	//typedef std::shared_ptr<EndFrameqct> EndFrmqctptr;
+
+
+}