diff --git a/MbDCode/AbsConstraint.cpp b/MbDCode/AbsConstraint.cpp
index 94ad287..2b7827c 100644
--- a/MbDCode/AbsConstraint.cpp
+++ b/MbDCode/AbsConstraint.cpp
@@ -1,14 +1,8 @@
 #include "AbsConstraint.h"
+#include "PartFrame.h"
 
 using namespace MbD;
 
-std::shared_ptr<AbsConstraint> MbD::AbsConstraint::Create(const char* name)
-{
-	auto item = std::make_shared<AbsConstraint>(name);
-	item->initialize();
-	return item;
-}
-
 AbsConstraint::AbsConstraint() {}
 
 AbsConstraint::AbsConstraint(const char* str) : Constraint(str) {}
@@ -26,4 +20,10 @@ void AbsConstraint::initialize()
 
 void MbD::AbsConstraint::calcPostDynCorrectorIteration()
 {
+    if (axis < 3) {
+        aG = static_cast<PartFrame*>(owner)->qX->at(axis);
+    }
+    else {
+        aG = static_cast<PartFrame*>(owner)->qE->at(axis - 3);
+    }
 }
diff --git a/MbDCode/AbsConstraint.h b/MbDCode/AbsConstraint.h
index f2923ee..14f3510 100644
--- a/MbDCode/AbsConstraint.h
+++ b/MbDCode/AbsConstraint.h
@@ -5,7 +5,6 @@ namespace MbD {
     {
         //axis iqXminusOnePlusAxis 
     public:
-        static std::shared_ptr<AbsConstraint> Create(const char* name);
         AbsConstraint();
         AbsConstraint(const char* str);
         AbsConstraint(int axis);
diff --git a/MbDCode/AnyPosICNewtonRaphson.cpp b/MbDCode/AnyPosICNewtonRaphson.cpp
index e69de29..1bd666d 100644
--- a/MbDCode/AnyPosICNewtonRaphson.cpp
+++ b/MbDCode/AnyPosICNewtonRaphson.cpp
@@ -0,0 +1,7 @@
+#include "AnyPosICNewtonRaphson.h"
+
+void MbD::AnyPosICNewtonRaphson::initialize()
+{
+	NewtonRaphson::initialize();
+	nSingularMatrixError = 0;
+}
diff --git a/MbDCode/AnyPosICNewtonRaphson.h b/MbDCode/AnyPosICNewtonRaphson.h
index 2f91791..6a19bbf 100644
--- a/MbDCode/AnyPosICNewtonRaphson.h
+++ b/MbDCode/AnyPosICNewtonRaphson.h
@@ -7,6 +7,8 @@ namespace MbD {
     {
         //nqsu qsuOld qsuWeights nSingularMatrixError 
     public:
+        void initialize() override;
+
         int nqsu;
         std::shared_ptr<FullColumn<double>> qsuOld, qsuWeights;
         int nSingularMatrixError;
diff --git a/MbDCode/Array.h b/MbDCode/Array.h
index c0b8ce2..146c9ad 100644
--- a/MbDCode/Array.h
+++ b/MbDCode/Array.h
@@ -1,34 +1,63 @@
 #pragma once
 #include <vector>
 #include <memory>
+#include <type_traits>
 
 namespace MbD {
 	template <typename T>
 	class Array : public std::vector<T>
 	{
 	public:
-		Array(){}
-        Array(size_t count) : std::vector<T>(count) {}
-        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)
-    {
-        for (int i = 0; i < x->size(); i++) {
-            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>>>;
+		Array() {}
+		Array(size_t count) : std::vector<T>(count) {}
+		Array(size_t count, const T& value) : std::vector<T>(count, value) {}
+		Array(std::initializer_list<T> list) : std::vector<T>{ list } {}
+		void copyFrom(std::shared_ptr<Array<T>> x);
+		void zeroSelf();
+		double sumOfSquares();
+		double sumOfSquaresOfVector();
+	};
+	template<typename T>
+	inline void Array<T>::copyFrom(std::shared_ptr<Array<T>> x)
+	{
+		for (size_t i = 0; i < x->size(); i++) {
+			this->at(i) = x->at(i);
+		}
+	}
+	template <>
+	inline void Array<double>::zeroSelf() {
+		for (size_t i = 0; i < this->size(); i++) {
+			this->at(i) = 0.0;;
+		}
+	}
+	template<typename T>
+	inline double Array<T>::sumOfSquares()
+	{
+		if (std::is_arithmetic<T>) {
+			return this->sumOfSquaresOfVector();
+		}
+		else {
+			double sum = 0.0;
+			for (size_t i = 0; i < this->size(); i++)
+			{
+				sum += this->at(i)->sumOfSquares();
+			}
+			return sum;
+		}
+	}
+	template<typename T>
+	inline double Array<T>::sumOfSquaresOfVector()
+	{
+		double sum = 0.0;
+		for (size_t i = 0; i < this->size(); i++)
+		{
+			double element = this->at(i);
+			sum += element * element;
+		}
+		return sum;
+	}
+	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>>>;
 }
 
diff --git a/MbDCode/AtPointConstraintIJ.cpp b/MbDCode/AtPointConstraintIJ.cpp
index c4758b1..9398f3b 100644
--- a/MbDCode/AtPointConstraintIJ.cpp
+++ b/MbDCode/AtPointConstraintIJ.cpp
@@ -1,15 +1,9 @@
 #include "AtPointConstraintIJ.h"
 #include "DispCompIecJecO.h"
+#include "CREATE.h"
 
 using namespace MbD;
 
-std::shared_ptr<AtPointConstraintIJ> MbD::AtPointConstraintIJ::Create(EndFrmcptr frmi, EndFrmcptr frmj, int axisi)
-{
-	auto item = std::make_shared<AtPointConstraintIJ>(frmi, frmj, axisi);
-	item->initialize();
-	return item;
-}
-
 AtPointConstraintIJ::AtPointConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj, int axisi) :
 	ConstraintIJ(frmi, frmj), axis(axisi)
 {
@@ -17,6 +11,7 @@ AtPointConstraintIJ::AtPointConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj, int a
 
 void AtPointConstraintIJ::initialize()
 {
+	initriIeJeO();
 }
 
 void MbD::AtPointConstraintIJ::initializeLocally()
@@ -31,7 +26,7 @@ void MbD::AtPointConstraintIJ::initializeGlobally()
 
 void AtPointConstraintIJ::initriIeJeO()
 {
-	riIeJeO = std::make_shared<DispCompIecJecO>();
+	riIeJeO = CREATE<DispCompIecJecO>::With(frmI, frmJ, axis);
 }
 
 void MbD::AtPointConstraintIJ::postInput()
@@ -40,4 +35,11 @@ void MbD::AtPointConstraintIJ::postInput()
 
 void MbD::AtPointConstraintIJ::calcPostDynCorrectorIteration()
 {
+	aG = riIeJeO->riIeJeO - aConstant;
+}
+
+void MbD::AtPointConstraintIJ::prePosIC()
+{
+	riIeJeO->prePosIC();
+	Constraint::prePosIC();
 }
diff --git a/MbDCode/AtPointConstraintIJ.h b/MbDCode/AtPointConstraintIJ.h
index bc05668..1228a58 100644
--- a/MbDCode/AtPointConstraintIJ.h
+++ b/MbDCode/AtPointConstraintIJ.h
@@ -9,7 +9,6 @@ namespace MbD {
     {
         //axis riIeJeO 
     public:
-        static std::shared_ptr<AtPointConstraintIJ> Create(EndFrmcptr frmi, EndFrmcptr frmj, int axisi);
         AtPointConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj, int axisi);
         void initialize();
         void initializeLocally() override;
@@ -17,6 +16,7 @@ namespace MbD {
         virtual void initriIeJeO();
         void postInput() override;
         void calcPostDynCorrectorIteration() override;
+        void prePosIC() override;
 
         int axis;
         std::shared_ptr<DispCompIecJecO> riIeJeO;
diff --git a/MbDCode/AtPointConstraintIqcJc.cpp b/MbDCode/AtPointConstraintIqcJc.cpp
index 0578a64..858aeee 100644
--- a/MbDCode/AtPointConstraintIqcJc.cpp
+++ b/MbDCode/AtPointConstraintIqcJc.cpp
@@ -1,5 +1,7 @@
 #include "AtPointConstraintIqcJc.h"
 #include "DispCompIeqcJecO.h"
+#include "CREATE.h"
+#include "EndFrameqc.h"
 
 using namespace MbD;
 
@@ -8,15 +10,25 @@ AtPointConstraintIqcJc::AtPointConstraintIqcJc(EndFrmcptr frmi, EndFrmcptr frmj,
 {
 }
 
-void AtPointConstraintIqcJc::initialize()
+void MbD::AtPointConstraintIqcJc::initializeGlobally()
 {
+	AtPointConstraintIJ::initializeGlobally();
+	ppGpEIpEI = (std::static_pointer_cast<DispCompIeqcJecO>(riIeJeO))->ppriIeJeOpEIpEI;
 }
 
 void AtPointConstraintIqcJc::initriIeJeO()
 {
-	riIeJeO = std::make_shared<DispCompIeqcJecO>();
+	riIeJeO = CREATE<DispCompIeqcJecO>::With(frmI, frmJ, axis);
 }
 
 void MbD::AtPointConstraintIqcJc::calcPostDynCorrectorIteration()
 {
+	AtPointConstraintIJ::calcPostDynCorrectorIteration();
+	pGpEI = std::static_pointer_cast<DispCompIeqcJecO>(riIeJeO)->priIeJeOpEI;
+}
+
+void MbD::AtPointConstraintIqcJc::useEquationNumbers()
+{
+	iqXIminusOnePlusAxis = std::static_pointer_cast<EndFrameqc>(frmI)->iqX() - 1 + axis;
+	iqEI = std::static_pointer_cast<EndFrameqc>(frmI)->iqE();
 }
diff --git a/MbDCode/AtPointConstraintIqcJc.h b/MbDCode/AtPointConstraintIqcJc.h
index 0c9fe5b..aa1132a 100644
--- a/MbDCode/AtPointConstraintIqcJc.h
+++ b/MbDCode/AtPointConstraintIqcJc.h
@@ -3,18 +3,20 @@
 #include "AtPointConstraintIJ.h"
 
 namespace MbD {
-    class AtPointConstraintIqcJc : public AtPointConstraintIJ
-    {
-        //pGpEI ppGpEIpEI iqXIminusOnePlusAxis iqEI 
-    public:
-        AtPointConstraintIqcJc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi);
-        void initialize();
-        void initriIeJeO() override;
-        void calcPostDynCorrectorIteration() override;
+	class AtPointConstraintIqcJc : public AtPointConstraintIJ
+	{
+		//pGpEI ppGpEIpEI iqXIminusOnePlusAxis iqEI 
+	public:
+		AtPointConstraintIqcJc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi);
+		void initializeGlobally() override;
+		void initriIeJeO() override;
+		void calcPostDynCorrectorIteration() override;
+		void useEquationNumbers() override;
 
-        FRowDsptr pGpEI;
-        FMatDsptr ppGpEIpEI;
-        int iqXIminusOnePlusAxis, iqEI;
-    };
+		FRowDsptr pGpEI;
+		FMatDsptr ppGpEIpEI;
+		int iqXIminusOnePlusAxis = -1;
+		int iqEI = -1;
+	};
 }
 
diff --git a/MbDCode/AtPointConstraintIqcJqc.cpp b/MbDCode/AtPointConstraintIqcJqc.cpp
index a9467c8..e8f96d5 100644
--- a/MbDCode/AtPointConstraintIqcJqc.cpp
+++ b/MbDCode/AtPointConstraintIqcJqc.cpp
@@ -1,5 +1,7 @@
 #include "AtPointConstraintIqcJqc.h"
 #include "DispCompIeqcJeqcO.h"
+#include "CREATE.h"
+#include "EndFrameqc.h"
 
 using namespace MbD;
 
@@ -8,15 +10,26 @@ AtPointConstraintIqcJqc::AtPointConstraintIqcJqc(EndFrmcptr frmi, EndFrmcptr frm
 {
 }
 
-void AtPointConstraintIqcJqc::initialize()
+void MbD::AtPointConstraintIqcJqc::initializeGlobally()
 {
+	AtPointConstraintIqcJc::initializeGlobally();
+	ppGpEJpEJ = (std::static_pointer_cast<DispCompIeqcJeqcO>(riIeJeO))->ppriIeJeOpEJpEJ;
 }
 
 void AtPointConstraintIqcJqc::initriIeJeO()
 {
-	riIeJeO = std::make_shared<DispCompIeqcJeqcO>();
+	riIeJeO = CREATE<DispCompIeqcJeqcO>::With(frmI, frmJ, axis);
 }
 
 void MbD::AtPointConstraintIqcJqc::calcPostDynCorrectorIteration()
 {
+	AtPointConstraintIqcJc::calcPostDynCorrectorIteration();
+	pGpEJ = std::static_pointer_cast<DispCompIeqcJeqcO>(riIeJeO)->priIeJeOpEJ;
+}
+
+void MbD::AtPointConstraintIqcJqc::useEquationNumbers()
+{
+	AtPointConstraintIqcJc::useEquationNumbers();
+	iqXJminusOnePlusAxis = std::static_pointer_cast<EndFrameqc>(frmJ)->iqX() - 1 + axis;
+	iqEJ = std::static_pointer_cast<EndFrameqc>(frmJ)->iqE();
 }
diff --git a/MbDCode/AtPointConstraintIqcJqc.h b/MbDCode/AtPointConstraintIqcJqc.h
index 6f4a79b..ed5b531 100644
--- a/MbDCode/AtPointConstraintIqcJqc.h
+++ b/MbDCode/AtPointConstraintIqcJqc.h
@@ -8,9 +8,10 @@ namespace MbD {
         //pGpEJ ppGpEJpEJ iqXJminusOnePlusAxis iqEJ 
     public:
         AtPointConstraintIqcJqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi);
-        void initialize();
+        void initializeGlobally() override;
         void initriIeJeO() override;
         void calcPostDynCorrectorIteration() override;
+        void useEquationNumbers() override;
 
         FRowDsptr pGpEJ;
         FMatDsptr ppGpEJpEJ;
diff --git a/MbDCode/AtPointConstraintIqctJqc.cpp b/MbDCode/AtPointConstraintIqctJqc.cpp
index d5ef975..6387e99 100644
--- a/MbDCode/AtPointConstraintIqctJqc.cpp
+++ b/MbDCode/AtPointConstraintIqctJqc.cpp
@@ -1,5 +1,6 @@
 #include "AtPointConstraintIqctJqc.h"
 #include "DispCompIeqctJeqcO.h"
+#include "CREATE.h"
 
 using namespace MbD;
 
@@ -8,15 +9,21 @@ AtPointConstraintIqctJqc::AtPointConstraintIqctJqc(EndFrmcptr frmi, EndFrmcptr f
 {
 }
 
-void AtPointConstraintIqctJqc::initialize()
+void MbD::AtPointConstraintIqctJqc::initializeGlobally()
 {
+	riIeJeO->initializeGlobally();
+	ppGpEJpEJ = (std::static_pointer_cast<DispCompIeqctJeqcO>(riIeJeO))->ppriIeJeOpEJpEJ;
 }
 
 void AtPointConstraintIqctJqc::initriIeJeO()
 {
-	riIeJeO = std::make_shared<DispCompIeqctJeqcO>();
+	riIeJeO = CREATE<DispCompIeqctJeqcO>::With(frmI, frmJ, axis);
 }
 
 void MbD::AtPointConstraintIqctJqc::calcPostDynCorrectorIteration()
 {
+	//"ppGpEIpEI is no longer constant."
+
+	ppGpEIpEI = std::static_pointer_cast<DispCompIeqctJeqcO>(riIeJeO)->ppriIeJeOpEIpEI;
+	AtPointConstraintIqcJqc::calcPostDynCorrectorIteration();
 }
diff --git a/MbDCode/AtPointConstraintIqctJqc.h b/MbDCode/AtPointConstraintIqctJqc.h
index 9412e4f..b8432aa 100644
--- a/MbDCode/AtPointConstraintIqctJqc.h
+++ b/MbDCode/AtPointConstraintIqctJqc.h
@@ -8,7 +8,7 @@ namespace MbD {
         //pGpt ppGpEIpt ppGptpt 
     public:
         AtPointConstraintIqctJqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi);
-        void initialize();
+        void initializeGlobally() override;
         void initriIeJeO() override;
         void calcPostDynCorrectorIteration() override;
 
diff --git a/MbDCode/CREATE.h b/MbDCode/CREATE.h
index 2166532..58a1c0c 100644
--- a/MbDCode/CREATE.h
+++ b/MbDCode/CREATE.h
@@ -1,21 +1,89 @@
 #pragma once
 #include <memory>
+//#include "EndFramec.h"
+#include "EndFrameqct.h"
+#include "AtPointConstraintIqctJqc.h"
+#include "DirectionCosineConstraintIqctJqc.h"
+#include "TranslationConstraintIqctJqc.h"
+#include "DispCompIeqctJeqcKeqct.h"
+#include "DispCompIeqctJeqcO.h"
 
 namespace MbD {
 
-    template<typename T>
-    class CREATE {
-    public:
-        static std::shared_ptr<T> With(const char* name) {
-            auto inst = std::make_shared<T>(name);
-            inst->initialize();
-            return inst;
-        }
-        static std::shared_ptr<T> With() {
-            auto inst = std::make_shared<T>();
-            inst->initialize();
-            return inst;
-        }
-    };
+	template<typename T>
+	class CREATE {
+	public:
+		static std::shared_ptr<T> With(const char* name) {
+			auto inst = std::make_shared<T>(name);
+			inst->initialize();
+			return inst;
+		}
+		static std::shared_ptr<T> With() {
+			auto inst = std::make_shared<T>();
+			inst->initialize();
+			return inst;
+		}
+		static std::shared_ptr<T> With(int n) {
+			auto inst = std::make_shared<T>(n);
+			inst->initialize();
+			return inst;
+		}
+		static std::shared_ptr<T> With(std::initializer_list<double> listD) {
+			auto inst = std::make_shared<T>(listD);
+			inst->initialize();
+			return inst;
+		}
+		static std::shared_ptr<T> With(EndFrmcptr frmi, EndFrmcptr frmj, int axis) {
+			auto inst = std::make_shared<T>(frmi, frmj, axis);
+			inst->initialize();
+			return inst;
+		}
+		static std::shared_ptr<T> With(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk) {
+			auto inst = std::make_shared<T>(frmi, frmj, frmk, axisk);
+			inst->initialize();
+			return inst;
+		}
+		static std::shared_ptr<Constraint> ConstraintWith(EndFrmcptr frmi, EndFrmcptr frmj, int axis) {
+			std::shared_ptr<Constraint> inst;
+			std::string str = typeid(T(frmi, frmj, axis)).name();
+			if (str == "class MbD::AtPointConstraintIJ") {
+				if (std::dynamic_pointer_cast<EndFrameqct>(frmi)) {
+					inst = std::make_shared<AtPointConstraintIqctJqc>(frmi, frmj, axis);
+				}
+				else {
+					inst = std::make_shared<AtPointConstraintIqcJqc>(frmi, frmj, axis);
+				}
+			}
+			else if(str == "class MbD::TranslationConstraintIJ") {
+				if (std::dynamic_pointer_cast<EndFrameqct>(frmi)) {
+					inst = std::make_shared<TranslationConstraintIqctJqc>(frmi, frmj, axis);
+				}
+				else {
+					inst = std::make_shared<TranslationConstraintIqcJqc>(frmi, frmj, axis);
+				}
+			}
+			inst->initialize();
+			return inst;
+		}
+		static std::shared_ptr<T> With(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj) {
+			auto inst = std::make_shared<T>(frmi, frmj, axisi, axisj);
+			inst->initialize();
+			return inst;
+		}
+		static std::shared_ptr<Constraint> ConstraintWith(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj) {
+			std::shared_ptr<Constraint> inst;
+			std::string str = typeid(T(frmi, frmj, axisi, axisj)).name();
+			if (str == "class MbD::DirectionCosineConstraintIJ") {
+				if (std::dynamic_pointer_cast<EndFrameqct>(frmi)) {
+					inst = std::make_shared<DirectionCosineConstraintIqctJqc>(frmi, frmj, axisi, axisj);
+				}
+				else {
+					inst = std::make_shared<DirectionCosineConstraintIqcJqc>(frmi, frmj, axisi, axisj);
+				}
+			}
+			inst->initialize();
+			return inst;
+		}
+	};
 }
 
diff --git a/MbDCode/Constant.cpp b/MbDCode/Constant.cpp
index a25769a..6648041 100644
--- a/MbDCode/Constant.cpp
+++ b/MbDCode/Constant.cpp
@@ -10,7 +10,17 @@ Constant::Constant(double val) : Variable(val)
 {
 }
 
-std::shared_ptr<Symbolic> Constant::differentiateWRT(std::shared_ptr<Symbolic> var)
+Symsptr Constant::differentiateWRT(Symsptr sptr, Symsptr var)
 {
 	return std::make_shared<Constant>(0.0);
 }
+
+bool MbD::Constant::isConstant()
+{
+	return true;
+}
+
+std::ostream& MbD::Constant::printOn(std::ostream& s) const
+{
+	return s << this->value;
+}
diff --git a/MbDCode/Constant.h b/MbDCode/Constant.h
index 5ac5a89..bd007ba 100644
--- a/MbDCode/Constant.h
+++ b/MbDCode/Constant.h
@@ -8,7 +8,9 @@ namespace MbD {
     public:
         Constant();
         Constant(double val);
-        std::shared_ptr<Symbolic> differentiateWRT(std::shared_ptr<Symbolic> var) override;
+        Symsptr differentiateWRT(Symsptr sptr, Symsptr var) override;
+        bool isConstant() override;
+        std::ostream& printOn(std::ostream& s) const override;
     };
 }
 
diff --git a/MbDCode/Constraint.cpp b/MbDCode/Constraint.cpp
index 403d87f..790c8f5 100644
--- a/MbDCode/Constraint.cpp
+++ b/MbDCode/Constraint.cpp
@@ -31,3 +31,10 @@ Item* Constraint::getOwner()
 {
 	return owner;
 }
+
+void MbD::Constraint::prePosIC()
+{
+	lam = 0.0;
+	iG = -1;
+	Item::prePosIC();
+}
diff --git a/MbDCode/Constraint.h b/MbDCode/Constraint.h
index 23c6f95..c6ed8af 100644
--- a/MbDCode/Constraint.h
+++ b/MbDCode/Constraint.h
@@ -14,6 +14,8 @@ namespace MbD {
 		void postInput() override;
 		void setOwner(Item* x);
 		Item* getOwner();
+		void prePosIC() override;
+
 		
 		int iG;
 		double aG;		//Constraint function
diff --git a/MbDCode/ConstraintIJ.cpp b/MbDCode/ConstraintIJ.cpp
index ad2fae6..a0261b6 100644
--- a/MbDCode/ConstraintIJ.cpp
+++ b/MbDCode/ConstraintIJ.cpp
@@ -5,10 +5,10 @@ using namespace MbD;
 
 ConstraintIJ::ConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj) : frmI(frmi), frmJ(frmj)
 {
-	aConstant = 0.0;
 }
 
 void ConstraintIJ::initialize()
 {
+	Constraint::initialize();
 	aConstant = 0.0;
 }
diff --git a/MbDCode/CylindricalJoint.cpp b/MbDCode/CylindricalJoint.cpp
index 5e90d42..96de886 100644
--- a/MbDCode/CylindricalJoint.cpp
+++ b/MbDCode/CylindricalJoint.cpp
@@ -2,6 +2,7 @@
 #include "System.h"
 #include "DirectionCosineConstraintIJ.h"
 #include "TranslationConstraintIJ.h"
+#include "CREATE.h"
 
 using namespace MbD;
 
@@ -13,12 +14,12 @@ CylindricalJoint::CylindricalJoint(const char* str) : Joint(str) {
 
 void MbD::CylindricalJoint::initializeGlobally()
 {
-	if (!constraints)
+	if (constraints->empty())
 	{
-		addConstraint(std::make_shared <TranslationConstraintIJ>(frmI, frmJ, 1));
-		addConstraint(std::make_shared <TranslationConstraintIJ>(frmI, frmJ, 2));
-		addConstraint(std::make_shared<DirectionCosineConstraintIJ>(frmI, frmJ, 3, 1));
-		addConstraint(std::make_shared<DirectionCosineConstraintIJ>(frmI, frmJ, 3, 2));
+		addConstraint(CREATE<TranslationConstraintIJ>::ConstraintWith(frmI, frmJ, 0));
+		addConstraint(CREATE<TranslationConstraintIJ>::ConstraintWith(frmI, frmJ, 1));
+		addConstraint(CREATE<DirectionCosineConstraintIJ>::ConstraintWith(frmI, frmJ, 2, 0));
+		addConstraint(CREATE<DirectionCosineConstraintIJ>::ConstraintWith(frmI, frmJ, 2, 1));
 		System::getInstance().hasChanged = true;
 	}
 	else {
diff --git a/MbDCode/CylindricalJoint.h b/MbDCode/CylindricalJoint.h
index 5811e28..02ab376 100644
--- a/MbDCode/CylindricalJoint.h
+++ b/MbDCode/CylindricalJoint.h
@@ -6,10 +6,9 @@ namespace MbD {
     {
         //frmI frmJ constraints friction 
     public:
-        static std::shared_ptr<CylindricalJoint> Create(const char* name);
         CylindricalJoint();
         CylindricalJoint(const char* str);
-        void initializeGlobally();
+        void initializeGlobally() override;
     };
 }
 
diff --git a/MbDCode/DirectionCosineConstraintIJ.cpp b/MbDCode/DirectionCosineConstraintIJ.cpp
index daa24ce..9ea51c7 100644
--- a/MbDCode/DirectionCosineConstraintIJ.cpp
+++ b/MbDCode/DirectionCosineConstraintIJ.cpp
@@ -1,16 +1,10 @@
 #include "DirectionCosineConstraintIJ.h"
 #include "DirectionCosineIecJec.h"
 #include "EndFramec.h"
+#include "CREATE.h"
 
 using namespace MbD;
 
-std::shared_ptr<DirectionCosineConstraintIJ> MbD::DirectionCosineConstraintIJ::Create(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj)
-{
-	auto item = std::make_shared<DirectionCosineConstraintIJ>(frmi, frmj, axisi, axisj);
-	item->initialize();
-	return item;
-}
-
 DirectionCosineConstraintIJ::DirectionCosineConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj) :
 	ConstraintIJ(frmi, frmj), axisI(axisi), axisJ(axisj)
 {
@@ -18,6 +12,7 @@ DirectionCosineConstraintIJ::DirectionCosineConstraintIJ(EndFrmcptr frmi, EndFrm
 
 void DirectionCosineConstraintIJ::initialize()
 {
+	initaAijIeJe();
 }
 
 void MbD::DirectionCosineConstraintIJ::initializeLocally()
@@ -32,7 +27,7 @@ void MbD::DirectionCosineConstraintIJ::initializeGlobally()
 
 void DirectionCosineConstraintIJ::initaAijIeJe()
 {
-	aAijIeJe = std::make_shared<DirectionCosineIecJec>();
+	aAijIeJe = CREATE<DirectionCosineIecJec>::With(frmI, frmJ, axisI, axisJ);
 }
 
 void MbD::DirectionCosineConstraintIJ::postInput()
@@ -41,4 +36,11 @@ void MbD::DirectionCosineConstraintIJ::postInput()
 
 void MbD::DirectionCosineConstraintIJ::calcPostDynCorrectorIteration()
 {
+	aG = aAijIeJe->aAijIeJe - aConstant;
+}
+
+void MbD::DirectionCosineConstraintIJ::prePosIC()
+{
+	aAijIeJe->prePosIC();
+	Constraint::prePosIC();
 }
diff --git a/MbDCode/DirectionCosineConstraintIJ.h b/MbDCode/DirectionCosineConstraintIJ.h
index 62c805d..12470ec 100644
--- a/MbDCode/DirectionCosineConstraintIJ.h
+++ b/MbDCode/DirectionCosineConstraintIJ.h
@@ -9,7 +9,6 @@ namespace MbD {
     {
         //axisI axisJ aAijIeJe 
     public:
-        static std::shared_ptr<DirectionCosineConstraintIJ> Create(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj);
         DirectionCosineConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj);
         void initialize();
         void initializeLocally() override;
@@ -17,6 +16,7 @@ namespace MbD {
         virtual void initaAijIeJe();
         void postInput() override;
         void calcPostDynCorrectorIteration() override;
+        void prePosIC() override;
 
         int axisI, axisJ;
         std::shared_ptr<DirectionCosineIecJec> aAijIeJe;
diff --git a/MbDCode/DirectionCosineConstraintIqcJc.cpp b/MbDCode/DirectionCosineConstraintIqcJc.cpp
index ca2caea..6e4431b 100644
--- a/MbDCode/DirectionCosineConstraintIqcJc.cpp
+++ b/MbDCode/DirectionCosineConstraintIqcJc.cpp
@@ -1,5 +1,7 @@
 #include "DirectionCosineConstraintIqcJc.h"
 #include "DirectionCosineIeqcJec.h"
+#include "EndFrameqc.h"
+#include "CREATE.h"
 
 using namespace MbD;
 
@@ -8,15 +10,20 @@ DirectionCosineConstraintIqcJc::DirectionCosineConstraintIqcJc(EndFrmcptr frmi,
 {
 }
 
-void DirectionCosineConstraintIqcJc::initialize()
-{
-}
-
 void DirectionCosineConstraintIqcJc::initaAijIeJe()
 {
-	aAijIeJe = std::make_shared<DirectionCosineIeqcJec>();
+	aAijIeJe = CREATE<DirectionCosineIeqcJec>::With(frmI, frmJ, axisI, axisJ);
 }
 
 void MbD::DirectionCosineConstraintIqcJc::calcPostDynCorrectorIteration()
 {
+	DirectionCosineConstraintIJ::calcPostDynCorrectorIteration();
+	auto aAijIeqJe = std::static_pointer_cast<DirectionCosineIeqcJec>(aAijIeJe);
+	pGpEI = aAijIeqJe->pAijIeJepEI;
+	ppGpEIpEI = aAijIeqJe->ppAijIeJepEIpEI;
+}
+
+void MbD::DirectionCosineConstraintIqcJc::useEquationNumbers()
+{
+	iqEI = std::static_pointer_cast<EndFrameqc>(frmI)->iqE();
 }
diff --git a/MbDCode/DirectionCosineConstraintIqcJc.h b/MbDCode/DirectionCosineConstraintIqcJc.h
index 28f1783..336176e 100644
--- a/MbDCode/DirectionCosineConstraintIqcJc.h
+++ b/MbDCode/DirectionCosineConstraintIqcJc.h
@@ -8,9 +8,9 @@ namespace MbD {
         //pGpEI ppGpEIpEI iqEI 
     public:
         DirectionCosineConstraintIqcJc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj);
-        void initialize();
         void initaAijIeJe() override;
         void calcPostDynCorrectorIteration() override;
+        void useEquationNumbers() override;
 
         FRowDsptr pGpEI;
         FMatDsptr ppGpEIpEI;
diff --git a/MbDCode/DirectionCosineConstraintIqcJqc.cpp b/MbDCode/DirectionCosineConstraintIqcJqc.cpp
index 7391e0e..726d4dd 100644
--- a/MbDCode/DirectionCosineConstraintIqcJqc.cpp
+++ b/MbDCode/DirectionCosineConstraintIqcJqc.cpp
@@ -1,5 +1,7 @@
 #include "DirectionCosineConstraintIqcJqc.h"
 #include "DirectionCosineIeqcJeqc.h"
+#include "EndFrameqc.h"
+#include "CREATE.h"
 
 using namespace MbD;
 
@@ -8,15 +10,22 @@ DirectionCosineConstraintIqcJqc::DirectionCosineConstraintIqcJqc(EndFrmcptr frmi
 {
 }
 
-void DirectionCosineConstraintIqcJqc::initialize()
-{
-}
-
 void DirectionCosineConstraintIqcJqc::initaAijIeJe()
 {
-	aAijIeJe = std::make_shared<DirectionCosineIeqcJeqc>();
+	aAijIeJe = CREATE<DirectionCosineIeqcJeqc>::With(frmI, frmJ, axisI, axisJ);
 }
 
 void MbD::DirectionCosineConstraintIqcJqc::calcPostDynCorrectorIteration()
 {
+	DirectionCosineConstraintIqcJc::calcPostDynCorrectorIteration();
+	auto aAijIeqJqe = std::static_pointer_cast<DirectionCosineIeqcJeqc>(aAijIeJe);
+	pGpEJ = aAijIeqJqe->pAijIeJepEJ;
+	ppGpEIpEJ = aAijIeqJqe->ppAijIeJepEIpEJ;
+	ppGpEJpEJ = aAijIeqJqe->ppAijIeJepEJpEJ;
+}
+
+void MbD::DirectionCosineConstraintIqcJqc::useEquationNumbers()
+{
+	DirectionCosineConstraintIqcJc::useEquationNumbers();
+	iqEJ = std::static_pointer_cast<EndFrameqc>(frmJ)->iqE();
 }
diff --git a/MbDCode/DirectionCosineConstraintIqcJqc.h b/MbDCode/DirectionCosineConstraintIqcJqc.h
index 57f1eb5..8952e2b 100644
--- a/MbDCode/DirectionCosineConstraintIqcJqc.h
+++ b/MbDCode/DirectionCosineConstraintIqcJqc.h
@@ -8,9 +8,9 @@ namespace MbD {
         //pGpEJ ppGpEIpEJ ppGpEJpEJ iqEJ 
     public:
         DirectionCosineConstraintIqcJqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj);
-        void initialize();
         void initaAijIeJe() override;
         void calcPostDynCorrectorIteration() override;
+        void useEquationNumbers() override;
 
         FRowDsptr pGpEJ;
         FMatDsptr ppGpEIpEJ;
diff --git a/MbDCode/DirectionCosineConstraintIqctJqc.cpp b/MbDCode/DirectionCosineConstraintIqctJqc.cpp
index 57fb084..a15cdbf 100644
--- a/MbDCode/DirectionCosineConstraintIqctJqc.cpp
+++ b/MbDCode/DirectionCosineConstraintIqctJqc.cpp
@@ -1,5 +1,6 @@
 #include "DirectionCosineConstraintIqctJqc.h"
 #include "DirectionCosineIeqctJeqc.h"
+#include "CREATE.h"
 
 using namespace MbD;
 
@@ -8,15 +9,7 @@ DirectionCosineConstraintIqctJqc::DirectionCosineConstraintIqctJqc(EndFrmcptr fr
 {
 }
 
-void DirectionCosineConstraintIqctJqc::initialize()
-{
-}
-
 void DirectionCosineConstraintIqctJqc::initaAijIeJe()
 {
-	aAijIeJe = std::make_shared<DirectionCosineIeqctJeqc>();
-}
-
-void MbD::DirectionCosineConstraintIqctJqc::calcPostDynCorrectorIteration()
-{
+	aAijIeJe = CREATE<DirectionCosineIeqctJeqc>::With(frmI, frmJ, axisI, axisJ);
 }
diff --git a/MbDCode/DirectionCosineConstraintIqctJqc.h b/MbDCode/DirectionCosineConstraintIqctJqc.h
index 3283486..9ed5c7b 100644
--- a/MbDCode/DirectionCosineConstraintIqctJqc.h
+++ b/MbDCode/DirectionCosineConstraintIqctJqc.h
@@ -8,9 +8,7 @@ namespace MbD {
 		//pGpt ppGpEIpt ppGpEJpt ppGptpt 
 	public:
 		DirectionCosineConstraintIqctJqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj);
-		void initialize();
 		void initaAijIeJe() override;
-		void calcPostDynCorrectorIteration() override;
 
 		double pGpt;
 		FRowDsptr ppGpEIpt;
diff --git a/MbDCode/DirectionCosineIecJec.cpp b/MbDCode/DirectionCosineIecJec.cpp
index 16f2623..fa9a966 100644
--- a/MbDCode/DirectionCosineIecJec.cpp
+++ b/MbDCode/DirectionCosineIecJec.cpp
@@ -15,13 +15,9 @@ DirectionCosineIecJec::DirectionCosineIecJec(EndFrmcptr frmi, EndFrmcptr frmj, i
 
 }
 
-void MbD::DirectionCosineIecJec::initialize()
-{
-	aAijIeJe = 0.0;
-	aAjOIe = std::make_shared<FullColumn<double>>(3);
-	aAjOJe = std::make_shared<FullColumn<double>>(3);
-}
-
 void MbD::DirectionCosineIecJec::calcPostDynCorrectorIteration()
 {
+	aAjOIe = frmI->aAjOe(axisI);
+	aAjOJe = frmJ->aAjOe(axisJ);
+	aAijIeJe = aAjOIe->dot(aAjOJe);
 }
diff --git a/MbDCode/DirectionCosineIecJec.h b/MbDCode/DirectionCosineIecJec.h
index 25a044b..205f22c 100644
--- a/MbDCode/DirectionCosineIecJec.h
+++ b/MbDCode/DirectionCosineIecJec.h
@@ -13,10 +13,9 @@ namespace MbD {
     public:
         DirectionCosineIecJec();
         DirectionCosineIecJec(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj);
-        void initialize();
         void calcPostDynCorrectorIteration() override;
 
-        int axisI, axisJ;
+        int axisI, axisJ;   //0, 1, 2 = x, y, z
         double aAijIeJe;
         std::shared_ptr<FullColumn<double>> aAjOIe, aAjOJe;
     };
diff --git a/MbDCode/DirectionCosineIeqcJec.cpp b/MbDCode/DirectionCosineIeqcJec.cpp
index ff6048e..022aab0 100644
--- a/MbDCode/DirectionCosineIeqcJec.cpp
+++ b/MbDCode/DirectionCosineIeqcJec.cpp
@@ -14,6 +14,7 @@ DirectionCosineIeqcJec::DirectionCosineIeqcJec(EndFrmcptr frmi, EndFrmcptr frmj,
 
 void DirectionCosineIeqcJec::initialize()
 {
+	DirectionCosineIecJec::initialize();
 	pAijIeJepEI = std::make_shared<FullRow<double>>(4);
 	ppAijIeJepEIpEI = std::make_shared<FullMatrix<double>>(4, 4);
 }
@@ -25,4 +26,20 @@ void MbD::DirectionCosineIeqcJec::initializeGlobally()
 
 void MbD::DirectionCosineIeqcJec::calcPostDynCorrectorIteration()
 {
+	DirectionCosineIecJec::calcPostDynCorrectorIteration();
+	pAjOIepEIT = std::static_pointer_cast<EndFrameqc>(frmI)->pAjOepET(axisI);
+	for (size_t i = 0; i < 4; i++)
+	{
+		pAijIeJepEI->at(i) = pAjOIepEIT->at(i)->dot(aAjOJe);
+	}
+	for (size_t i = 0; i < 4; i++)
+	{
+		auto& ppAijIeJepEIipEI = ppAijIeJepEIpEI->at(i);
+		auto& ppAjOIepEIipEI = ppAjOIepEIpEI->at(i);
+		for (size_t j = 0; j < 4; j++)
+		{
+			ppAijIeJepEIipEI->at(j) = ppAjOIepEIipEI->at(j)->dot(aAjOJe);
+		}
+	}	
+	ppAijIeJepEIpEI->symLowerWithUpper();
 }
diff --git a/MbDCode/DirectionCosineIeqcJeqc.cpp b/MbDCode/DirectionCosineIeqcJeqc.cpp
index 656ce0a..f60a1b9 100644
--- a/MbDCode/DirectionCosineIeqcJeqc.cpp
+++ b/MbDCode/DirectionCosineIeqcJeqc.cpp
@@ -14,6 +14,7 @@ DirectionCosineIeqcJeqc::DirectionCosineIeqcJeqc(EndFrmcptr frmi, EndFrmcptr frm
 
 void DirectionCosineIeqcJeqc::initialize()
 {
+	DirectionCosineIeqcJec::initialize();
 	pAijIeJepEJ = std::make_shared<FullRow<double>>(4);
 	ppAijIeJepEIpEJ = std::make_shared<FullMatrix<double>>(4, 4);
 	ppAijIeJepEJpEJ = std::make_shared<FullMatrix<double>>(4, 4);
@@ -27,4 +28,28 @@ void MbD::DirectionCosineIeqcJeqc::initializeGlobally()
 
 void MbD::DirectionCosineIeqcJeqc::calcPostDynCorrectorIteration()
 {
+	DirectionCosineIeqcJec::calcPostDynCorrectorIteration();
+	pAjOJepEJT = std::static_pointer_cast<EndFrameqc>(frmJ)->pAjOepET(axisJ);
+	for (size_t i = 0; i < 4; i++)
+	{
+		pAijIeJepEJ->at(i) = aAjOIe->dot(pAjOJepEJT->at(i));
+	}
+	for (size_t i = 0; i < 4; i++)
+	{
+		auto& ppAijIeJepEIipEJ = ppAijIeJepEIpEJ->at(i);
+		for (size_t j = 0; j < 4; j++)
+		{
+			ppAijIeJepEIipEJ->at(j) = pAjOIepEIT->at(i)->dot(pAjOJepEJT->at(j));
+		}
+	}
+	for (size_t i = 0; i < 4; i++)
+	{
+		auto& ppAijIeJepEJipEJ = ppAijIeJepEJpEJ->at(i);
+		auto& ppAjOJepEJipEJ = ppAjOJepEJpEJ->at(i);
+		for (size_t j = 0; j < 4; j++)
+		{
+			ppAijIeJepEJipEJ->at(j) = aAjOIe->dot(ppAjOJepEJipEJ->at(j));
+		}
+	}
+	ppAijIeJepEJpEJ->symLowerWithUpper();
 }
diff --git a/MbDCode/DirectionCosineIeqctJeqc.cpp b/MbDCode/DirectionCosineIeqctJeqc.cpp
index ae39837..b184775 100644
--- a/MbDCode/DirectionCosineIeqctJeqc.cpp
+++ b/MbDCode/DirectionCosineIeqctJeqc.cpp
@@ -5,17 +5,16 @@ using namespace MbD;
 
 DirectionCosineIeqctJeqc::DirectionCosineIeqctJeqc()
 {
-
 }
 
 DirectionCosineIeqctJeqc::DirectionCosineIeqctJeqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj) :
 	DirectionCosineIeqcJeqc(frmi, frmj, axisi, axisj)
 {
-
 }
 
 void DirectionCosineIeqctJeqc::initialize()
 {
+	DirectionCosineIeqcJeqc::initialize();
 	ppAijIeJepEIpt = std::make_shared<FullRow<double>>(4);
 	ppAijIeJepEJpt = std::make_shared<FullRow<double>>(4);
 }
@@ -27,4 +26,8 @@ void MbD::DirectionCosineIeqctJeqc::initializeGlobally()
 
 void MbD::DirectionCosineIeqctJeqc::calcPostDynCorrectorIteration()
 {
+	//"ppAjOIepEIpEI is not longer constant and must be set before any calculation."
+
+	ppAjOIepEIpEI = std::static_pointer_cast<EndFrameqc>(frmI)->ppAjOepEpE(axisI);
+	DirectionCosineIeqcJeqc::calcPostDynCorrectorIteration();
 }
diff --git a/MbDCode/DispCompIecJecKec.cpp b/MbDCode/DispCompIecJecKec.cpp
index 8b94427..3ee1f38 100644
--- a/MbDCode/DispCompIecJecKec.cpp
+++ b/MbDCode/DispCompIecJecKec.cpp
@@ -1 +1,14 @@
 #include "DispCompIecJecKec.h"
+
+MbD::DispCompIecJecKec::DispCompIecJecKec()
+{
+}
+
+MbD::DispCompIecJecKec::DispCompIecJecKec(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk): KinematicIeJe(frmi, frmj), efrmK(frmk), axisK(axisk)
+{
+}
+
+double MbD::DispCompIecJecKec::value()
+{
+    return riIeJeKe;
+}
diff --git a/MbDCode/DispCompIecJecKec.h b/MbDCode/DispCompIecJecKec.h
index 3ac0318..b088c30 100644
--- a/MbDCode/DispCompIecJecKec.h
+++ b/MbDCode/DispCompIecJecKec.h
@@ -7,7 +7,10 @@ namespace MbD {
     {
         //efrmK axisK riIeJeKe aAjOKe rIeJeO 
     public:
-
+        DispCompIecJecKec();
+        DispCompIecJecKec(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk);
+        
+        virtual double value();
         EndFrmcptr efrmK;
         int axisK;
         double riIeJeKe;
diff --git a/MbDCode/DispCompIecJecKeqc.cpp b/MbDCode/DispCompIecJecKeqc.cpp
index 4b9eac9..676c0ae 100644
--- a/MbDCode/DispCompIecJecKeqc.cpp
+++ b/MbDCode/DispCompIecJecKeqc.cpp
@@ -1 +1,53 @@
 #include "DispCompIecJecKeqc.h"
+#include "EndFrameqc.h"
+
+using namespace MbD;
+
+MbD::DispCompIecJecKeqc::DispCompIecJecKeqc()
+{
+}
+
+MbD::DispCompIecJecKeqc::DispCompIecJecKeqc(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk) : DispCompIecJecKec(frmi, frmj, frmk, axisk)
+{
+}
+
+void MbD::DispCompIecJecKeqc::initialize()
+{
+    priIeJeKepEK = std::make_shared<FullRow<double>>(4);
+    ppriIeJeKepEKpEK = std::make_shared<FullMatrix<double>>(4, 4);
+}
+
+void MbD::DispCompIecJecKeqc::initializeGlobally()
+{
+    ppAjOKepEKpEK = std::static_pointer_cast<EndFrameqc>(efrmK)->ppAjOepEpE(axisK);
+}
+
+void MbD::DispCompIecJecKeqc::calcPostDynCorrectorIteration()
+{
+	auto frmIqc = std::static_pointer_cast<EndFrameqc>(frmI);
+	auto frmJqc = std::static_pointer_cast<EndFrameqc>(frmJ);
+	auto efrmKqc = std::static_pointer_cast<EndFrameqc>(efrmK);
+	aAjOKe = efrmKqc->aAjOe(axisK);
+	rIeJeO = frmJqc->rOeO->minusFullColumn(frmIqc->rOeO);
+	riIeJeKe = aAjOKe->dot(rIeJeO);
+	pAjOKepEKT = efrmKqc->pAjOepET(axisK);
+	ppAjOKepEKpEK = efrmKqc->ppAjOepEpE(axisK);
+	for (size_t i = 0; i < 4; i++)
+	{
+		priIeJeKepEK->at(i) = ((pAjOKepEKT->at(i))->dot(rIeJeO));
+		auto& ppAjOKepEKipEK = ppAjOKepEKpEK->at(i);
+		auto& ppriIeJeKepEKipEK = ppriIeJeKepEKpEK->at(i);
+		ppriIeJeKepEKipEK->at(i) = ((ppAjOKepEKipEK->at(i))->dot(rIeJeO));
+		for (size_t j = i + 1; j < 4; j++)
+		{
+			auto ppriIeJeKepEKipEKj = (ppAjOKepEKipEK->at(i))->dot(rIeJeO);
+			ppriIeJeKepEKipEK->at(j) = ppriIeJeKepEKipEKj;
+			ppriIeJeKepEKpEK->at(j)->at(i) = ppriIeJeKepEKipEKj;
+		}
+	}
+}
+
+FMatDsptr MbD::DispCompIecJecKeqc::ppvaluepEKpEK()
+{
+    return ppriIeJeKepEKpEK;
+}
diff --git a/MbDCode/DispCompIecJecKeqc.h b/MbDCode/DispCompIecJecKeqc.h
index aea94a4..c559993 100644
--- a/MbDCode/DispCompIecJecKeqc.h
+++ b/MbDCode/DispCompIecJecKeqc.h
@@ -7,7 +7,13 @@ namespace MbD {
     {
         //priIeJeKepEK ppriIeJeKepEKpEK pAjOKepEKT ppAjOKepEKpEK 
     public:
+        DispCompIecJecKeqc();
+        DispCompIecJecKeqc(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk);
+        void initialize() override;
+        void initializeGlobally() override;
+        void calcPostDynCorrectorIteration() override;
 
+        FMatDsptr ppvaluepEKpEK();
         FRowDsptr priIeJeKepEK;
         FMatDsptr ppriIeJeKepEKpEK;
         FMatDsptr pAjOKepEKT;
diff --git a/MbDCode/DispCompIecJecO.cpp b/MbDCode/DispCompIecJecO.cpp
index 1786e5c..ceb2467 100644
--- a/MbDCode/DispCompIecJecO.cpp
+++ b/MbDCode/DispCompIecJecO.cpp
@@ -1 +1,14 @@
 #include "DispCompIecJecO.h"
+
+MbD::DispCompIecJecO::DispCompIecJecO()
+{
+}
+
+MbD::DispCompIecJecO::DispCompIecJecO(EndFrmcptr frmi, EndFrmcptr frmj, int axis) : KinematicIeJe(frmi, frmj), axis(axis)
+{
+}
+
+void MbD::DispCompIecJecO::calcPostDynCorrectorIteration()
+{
+	riIeJeO = frmJ->riOeO(axis) - frmI->riOeO(axis);
+}
diff --git a/MbDCode/DispCompIecJecO.h b/MbDCode/DispCompIecJecO.h
index f7aeda3..78656f1 100644
--- a/MbDCode/DispCompIecJecO.h
+++ b/MbDCode/DispCompIecJecO.h
@@ -7,6 +7,9 @@ namespace MbD {
 	{
 		//axis riIeJeO 
 	public:
+		DispCompIecJecO();
+		DispCompIecJecO(EndFrmcptr frmi, EndFrmcptr frmj, int axis);
+		void calcPostDynCorrectorIteration() override;
 
 		int axis;
 		double riIeJeO;
diff --git a/MbDCode/DispCompIeqcJecKeqc.cpp b/MbDCode/DispCompIeqcJecKeqc.cpp
index 1aa90b9..cb49af5 100644
--- a/MbDCode/DispCompIeqcJecKeqc.cpp
+++ b/MbDCode/DispCompIeqcJecKeqc.cpp
@@ -1 +1,97 @@
 #include "DispCompIeqcJecKeqc.h"
+#include "EndFrameqc.h"
+
+using namespace MbD;
+
+MbD::DispCompIeqcJecKeqc::DispCompIeqcJecKeqc()
+{
+}
+
+MbD::DispCompIeqcJecKeqc::DispCompIeqcJecKeqc(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk) : DispCompIecJecKeqc(frmi, frmj, frmk, axisk)
+{
+}
+
+void MbD::DispCompIeqcJecKeqc::initialize()
+{
+	DispCompIecJecKeqc::initialize();
+	priIeJeKepXI = std::make_shared<FullRow<double>>(3);
+	priIeJeKepEI = std::make_shared<FullRow<double>>(4);
+	ppriIeJeKepEIpEI = std::make_shared<FullMatrix<double>>(4, 4);
+	ppriIeJeKepXIpEK = std::make_shared<FullMatrix<double>>(3, 4);
+	ppriIeJeKepEIpEK = std::make_shared<FullMatrix<double>>(4, 4);
+}
+
+void MbD::DispCompIeqcJecKeqc::calcPostDynCorrectorIteration()
+{
+	DispCompIecJecKeqc::calcPostDynCorrectorIteration();
+	auto frmIqc = std::static_pointer_cast<EndFrameqc>(frmI);
+	auto mprIeJeOpEIT = frmIqc->prOeOpE->transpose();
+	auto mpprIeJeOpEIpEI = frmIqc->pprOeOpEpE;
+	for (size_t i = 0; i < 3; i++)
+	{
+		priIeJeKepXI->at(i) = 0.0 - (aAjOKe->at(i));
+	}
+	for (size_t i = 0; i < 4; i++)
+	{
+		priIeJeKepEI->at(i) = 0.0 - (aAjOKe->dot(mprIeJeOpEIT->at(i)));
+	}
+	for (size_t i = 0; i < 3; i++)
+	{
+		auto& ppriIeJeKepXIipEK = ppriIeJeKepXIpEK->at(i);
+		for (size_t j = 0; j < 4; j++)
+		{
+			ppriIeJeKepXIipEK->at(j) = 0.0 - (pAjOKepEKT->at(j)->at(i));
+		}
+	}
+	for (size_t i = 0; i < 4; i++)
+	{
+		auto& mpprIeJeOpEIipEI = mpprIeJeOpEIpEI->at(i);
+		auto& ppriIeJeKepEIipEI = ppriIeJeKepEIpEI->at(i);
+		ppriIeJeKepEIipEI->at(i) = 0.0 - (aAjOKe->dot(mpprIeJeOpEIipEI->at(i)));
+		for (size_t j = 0; j < 4; j++)
+		{
+			auto ppriIeJeKepEIipEIj = 0.0 - (aAjOKe->dot(mpprIeJeOpEIipEI->at(j)));
+			ppriIeJeKepEIipEI->at(j) = ppriIeJeKepEIipEIj;
+			ppriIeJeKepEIpEI->at(j)->at(i) = ppriIeJeKepEIipEIj;
+		}
+	}
+	for (size_t i = 0; i < 4; i++)
+	{
+		auto& mprIeJeOpEITi = mprIeJeOpEIT->at(i);
+		auto& ppriIeJeKepEIipEK = ppriIeJeKepEIpEK->at(i);
+		for (size_t j = 0; j < 4; j++)
+		{
+			ppriIeJeKepEIipEK->at(j) = 0.0 - (pAjOKepEKT->at(j)->dot(mprIeJeOpEITi));
+		}
+	}
+}
+
+FRowDsptr MbD::DispCompIeqcJecKeqc::pvaluepXI()
+{
+	return priIeJeKepXI;
+}
+
+FRowDsptr MbD::DispCompIeqcJecKeqc::pvaluepEI()
+{
+	return priIeJeKepEI;
+}
+
+FRowDsptr MbD::DispCompIeqcJecKeqc::pvaluepEK()
+{
+	return priIeJeKepEK;
+}
+
+FMatDsptr MbD::DispCompIeqcJecKeqc::ppvaluepXIpEK()
+{
+	return ppriIeJeKepXIpEK;
+}
+
+FMatDsptr MbD::DispCompIeqcJecKeqc::ppvaluepEIpEK()
+{
+	return ppriIeJeKepEIpEK;
+}
+
+FMatDsptr MbD::DispCompIeqcJecKeqc::ppvaluepEIpEI()
+{
+	return ppriIeJeKepEIpEI;
+}
diff --git a/MbDCode/DispCompIeqcJecKeqc.h b/MbDCode/DispCompIeqcJecKeqc.h
index 740ea95..7e28a3a 100644
--- a/MbDCode/DispCompIeqcJecKeqc.h
+++ b/MbDCode/DispCompIeqcJecKeqc.h
@@ -7,7 +7,17 @@ namespace MbD {
     {
         //priIeJeKepXI priIeJeKepEI ppriIeJeKepXIpEK ppriIeJeKepEIpEI ppriIeJeKepEIpEK 
     public:
+        DispCompIeqcJecKeqc();
+        DispCompIeqcJecKeqc(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk);
+        void initialize() override;
+        void calcPostDynCorrectorIteration() override;
 
+        FRowDsptr pvaluepXI();
+        FRowDsptr pvaluepEI();
+        FRowDsptr pvaluepEK();
+        FMatDsptr ppvaluepXIpEK();
+        FMatDsptr ppvaluepEIpEK();
+        FMatDsptr ppvaluepEIpEI();
         FRowDsptr priIeJeKepXI;
         FRowDsptr priIeJeKepEI;
         FMatDsptr ppriIeJeKepXIpEK;
diff --git a/MbDCode/DispCompIeqcJecO.cpp b/MbDCode/DispCompIeqcJecO.cpp
index 8eb7ab5..d894db2 100644
--- a/MbDCode/DispCompIeqcJecO.cpp
+++ b/MbDCode/DispCompIeqcJecO.cpp
@@ -1 +1,23 @@
 #include "DispCompIeqcJecO.h"
+#include "EndFrameqc.h"
+
+MbD::DispCompIeqcJecO::DispCompIeqcJecO()
+{
+}
+
+MbD::DispCompIeqcJecO::DispCompIeqcJecO(EndFrmcptr frmi, EndFrmcptr frmj, int axis) : DispCompIecJecO(frmi, frmj, axis)
+{
+}
+
+void MbD::DispCompIeqcJecO::initializeGlobally()
+{
+	priIeJeOpXI = std::make_shared<FullRow<double>>(3, 0.0);
+	priIeJeOpXI->at(axis) = -1.0;
+	ppriIeJeOpEIpEI = std::static_pointer_cast<EndFrameqc>(frmI)->ppriOeOpEpE(axis)->negated();
+}
+
+void MbD::DispCompIeqcJecO::calcPostDynCorrectorIteration()
+{
+	DispCompIecJecO::calcPostDynCorrectorIteration();
+	priIeJeOpEI = std::static_pointer_cast<EndFrameqc>(frmI)->priOeOpE(axis)->negated();
+}
diff --git a/MbDCode/DispCompIeqcJecO.h b/MbDCode/DispCompIeqcJecO.h
index 78741fd..862b51d 100644
--- a/MbDCode/DispCompIeqcJecO.h
+++ b/MbDCode/DispCompIeqcJecO.h
@@ -7,6 +7,10 @@ namespace MbD {
     {
         //priIeJeOpXI priIeJeOpEI ppriIeJeOpEIpEI 
     public:
+        DispCompIeqcJecO();
+        DispCompIeqcJecO(EndFrmcptr frmi, EndFrmcptr frmj, int axis);
+        void initializeGlobally() override;
+        void calcPostDynCorrectorIteration() override;
 
         FRowDsptr priIeJeOpXI;
         FRowDsptr priIeJeOpEI;
diff --git a/MbDCode/DispCompIeqcJeqcKeqc.cpp b/MbDCode/DispCompIeqcJeqcKeqc.cpp
index 2686a3c..ae1ccb0 100644
--- a/MbDCode/DispCompIeqcJeqcKeqc.cpp
+++ b/MbDCode/DispCompIeqcJeqcKeqc.cpp
@@ -1 +1,65 @@
 #include "DispCompIeqcJeqcKeqc.h"
+#include "EndFrameqc.h"
+
+MbD::DispCompIeqcJeqcKeqc::DispCompIeqcJeqcKeqc()
+{
+}
+
+MbD::DispCompIeqcJeqcKeqc::DispCompIeqcJeqcKeqc(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk) : DispCompIeqcJecKeqc(frmi, frmj, frmk, axisk)
+{
+}
+
+void MbD::DispCompIeqcJeqcKeqc::initialize()
+{
+	DispCompIeqcJecKeqc::initialize();
+	priIeJeKepXJ = std::make_shared<FullRow<double>>(3);
+	priIeJeKepEJ = std::make_shared<FullRow<double>>(4);
+	ppriIeJeKepEJpEJ = std::make_shared<FullMatrix<double>>(4, 4);
+	ppriIeJeKepXJpEK = std::make_shared<FullMatrix<double>>(3, 4);
+	ppriIeJeKepEJpEK = std::make_shared<FullMatrix<double>>(4, 4);
+}
+
+void MbD::DispCompIeqcJeqcKeqc::calcPostDynCorrectorIteration()
+{
+	DispCompIeqcJecKeqc::calcPostDynCorrectorIteration();
+	auto frmJqc = std::static_pointer_cast<EndFrameqc>(frmJ);
+	auto prIeJeOpEJT = frmJqc->prOeOpE->transpose();
+	auto pprIeJeOpEJpEJ = frmJqc->pprOeOpEpE;
+	for (size_t i = 0; i < 3; i++)
+	{
+		priIeJeKepXJ->at(i) = (aAjOKe->at(i));
+	}
+	for (size_t i = 0; i < 4; i++)
+	{
+		priIeJeKepEJ->at(i) = (aAjOKe->dot(prIeJeOpEJT->at(i)));
+	}
+	for (size_t i = 0; i < 3; i++)
+	{
+		auto& ppriIeJeKepXJipEK = ppriIeJeKepXJpEK->at(i);
+		for (size_t j = 0; j < 4; j++)
+		{
+			ppriIeJeKepXJipEK->at(j) = (pAjOKepEKT->at(j)->at(i));
+		}
+	}
+	for (size_t i = 0; i < 4; i++)
+	{
+		auto& pprIeJeOpEJipEJ = pprIeJeOpEJpEJ->at(i);
+		auto& ppriIeJeKepEJipEJ = ppriIeJeKepEJpEJ->at(i);
+		ppriIeJeKepEJipEJ->at(i) = (aAjOKe->dot(pprIeJeOpEJipEJ->at(i)));
+		for (size_t j = 0; j < 4; j++)
+		{
+			auto ppriIeJeKepEJipEJj = (aAjOKe->dot(pprIeJeOpEJipEJ->at(j)));
+			ppriIeJeKepEJipEJ->at(j) = ppriIeJeKepEJipEJj;
+			ppriIeJeKepEJpEJ->at(j)->at(i) = ppriIeJeKepEJipEJj;
+		}
+	}
+	for (size_t i = 0; i < 4; i++)
+	{
+		auto& prIeJeOpEJTi = prIeJeOpEJT->at(i);
+		auto& ppriIeJeKepEJipEK = ppriIeJeKepEJpEK->at(i);
+		for (size_t j = 0; j < 4; j++)
+		{
+			ppriIeJeKepEJipEK->at(j) = (pAjOKepEKT->at(j)->dot(prIeJeOpEJTi));
+		}
+	}
+}
diff --git a/MbDCode/DispCompIeqcJeqcKeqc.h b/MbDCode/DispCompIeqcJeqcKeqc.h
index 34e26f9..c6752c0 100644
--- a/MbDCode/DispCompIeqcJeqcKeqc.h
+++ b/MbDCode/DispCompIeqcJeqcKeqc.h
@@ -7,6 +7,10 @@ namespace MbD {
     {
         //priIeJeKepXJ priIeJeKepEJ ppriIeJeKepXJpEK ppriIeJeKepEJpEJ ppriIeJeKepEJpEK 
     public:
+        DispCompIeqcJeqcKeqc();
+        DispCompIeqcJeqcKeqc(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk);
+        void initialize() override;
+        void calcPostDynCorrectorIteration() override;
 
         FRowDsptr priIeJeKepXJ;
         FRowDsptr priIeJeKepEJ;
diff --git a/MbDCode/DispCompIeqcJeqcKeqct.cpp b/MbDCode/DispCompIeqcJeqcKeqct.cpp
index aa1aa54..e4eb5b2 100644
--- a/MbDCode/DispCompIeqcJeqcKeqct.cpp
+++ b/MbDCode/DispCompIeqcJeqcKeqct.cpp
@@ -1 +1,28 @@
 #include "DispCompIeqcJeqcKeqct.h"
+#include "EndFrameqc.h"
+
+MbD::DispCompIeqcJeqcKeqct::DispCompIeqcJeqcKeqct()
+{
+}
+
+MbD::DispCompIeqcJeqcKeqct::DispCompIeqcJeqcKeqct(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk) : DispCompIeqcJeqcKeqc(frmi, frmj, frmk, axisk)
+{
+}
+
+void MbD::DispCompIeqcJeqcKeqct::initialize()
+{
+	DispCompIeqcJeqcKeqc::initialize();
+	ppriIeJeKepXIpt = std::make_shared<FullRow<double>>(3);
+	ppriIeJeKepEIpt = std::make_shared<FullRow<double>>(4);
+	ppriIeJeKepXJpt = std::make_shared<FullRow<double>>(3);
+	ppriIeJeKepEJpt = std::make_shared<FullRow<double>>(4);
+	ppriIeJeKepEKpt = std::make_shared<FullRow<double>>(4);
+}
+
+void MbD::DispCompIeqcJeqcKeqct::calcPostDynCorrectorIteration()
+{
+	//"ppAjOIepEKpEK is not longer constant and must be set before any calculation."
+	auto efrmKqc = std::static_pointer_cast<EndFrameqc>(efrmK);
+	ppAjOKepEKpEK = efrmKqc->ppAjOepEpE(axisK);
+	DispCompIeqcJeqcKeqc::calcPostDynCorrectorIteration();
+}
diff --git a/MbDCode/DispCompIeqcJeqcKeqct.h b/MbDCode/DispCompIeqcJeqcKeqct.h
index 4f1ced2..0ebb021 100644
--- a/MbDCode/DispCompIeqcJeqcKeqct.h
+++ b/MbDCode/DispCompIeqcJeqcKeqct.h
@@ -7,6 +7,10 @@ namespace MbD {
     {
         //priIeJeKept ppriIeJeKepXIpt ppriIeJeKepEIpt ppriIeJeKepXJpt ppriIeJeKepEJpt ppriIeJeKepEKpt ppriIeJeKeptpt 
     public:
+        DispCompIeqcJeqcKeqct();
+        DispCompIeqcJeqcKeqct(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk);
+        void initialize() override;
+        void calcPostDynCorrectorIteration() override;
 
         double priIeJeKept;
         FRowDsptr ppriIeJeKepXIpt;
diff --git a/MbDCode/DispCompIeqcJeqcO.cpp b/MbDCode/DispCompIeqcJeqcO.cpp
index 05ac82c..684f91e 100644
--- a/MbDCode/DispCompIeqcJeqcO.cpp
+++ b/MbDCode/DispCompIeqcJeqcO.cpp
@@ -1 +1,24 @@
 #include "DispCompIeqcJeqcO.h"
+#include "EndFrameqc.h"
+
+MbD::DispCompIeqcJeqcO::DispCompIeqcJeqcO()
+{
+}
+
+MbD::DispCompIeqcJeqcO::DispCompIeqcJeqcO(EndFrmcptr frmi, EndFrmcptr frmj, int axis) : DispCompIeqcJecO(frmi, frmj, axis)
+{
+}
+
+void MbD::DispCompIeqcJeqcO::initializeGlobally()
+{
+	DispCompIeqcJecO::initializeGlobally();
+	priIeJeOpXJ = std::make_shared<FullRow<double>>(3, 0.0);
+	priIeJeOpXJ->at(axis) = 1.0;
+	ppriIeJeOpEJpEJ = std::static_pointer_cast<EndFrameqc>(frmJ)->ppriOeOpEpE(axis);
+}
+
+void MbD::DispCompIeqcJeqcO::calcPostDynCorrectorIteration()
+{
+	DispCompIeqcJecO::calcPostDynCorrectorIteration();
+	priIeJeOpEJ = std::static_pointer_cast<EndFrameqc>(frmJ)->priOeOpE(axis);
+}
diff --git a/MbDCode/DispCompIeqcJeqcO.h b/MbDCode/DispCompIeqcJeqcO.h
index f272f14..b4dc305 100644
--- a/MbDCode/DispCompIeqcJeqcO.h
+++ b/MbDCode/DispCompIeqcJeqcO.h
@@ -7,6 +7,10 @@ namespace MbD {
     {
         //priIeJeOpXJ priIeJeOpEJ ppriIeJeOpEJpEJ 
     public:
+        DispCompIeqcJeqcO();
+        DispCompIeqcJeqcO(EndFrmcptr frmi, EndFrmcptr frmj, int axis);
+        void initializeGlobally() override;
+        void calcPostDynCorrectorIteration() override;
 
         FRowDsptr priIeJeOpXJ;
         FRowDsptr priIeJeOpEJ;
diff --git a/MbDCode/DispCompIeqctJeqcKeqct.cpp b/MbDCode/DispCompIeqctJeqcKeqct.cpp
index 689a1d1..6f998ba 100644
--- a/MbDCode/DispCompIeqctJeqcKeqct.cpp
+++ b/MbDCode/DispCompIeqctJeqcKeqct.cpp
@@ -1 +1,9 @@
 #include "DispCompIeqctJeqcKeqct.h"
+
+MbD::DispCompIeqctJeqcKeqct::DispCompIeqctJeqcKeqct()
+{
+}
+
+MbD::DispCompIeqctJeqcKeqct::DispCompIeqctJeqcKeqct(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk) : DispCompIeqcJeqcKeqct(frmi, frmj, frmk, axisk)
+{
+}
diff --git a/MbDCode/DispCompIeqctJeqcKeqct.h b/MbDCode/DispCompIeqctJeqcKeqct.h
index e2eea94..55a9926 100644
--- a/MbDCode/DispCompIeqctJeqcKeqct.h
+++ b/MbDCode/DispCompIeqctJeqcKeqct.h
@@ -7,6 +7,8 @@ namespace MbD {
     {
         //
     public:
+        DispCompIeqctJeqcKeqct();
+        DispCompIeqctJeqcKeqct(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk);
 
     };
 }
diff --git a/MbDCode/DispCompIeqctJeqcO.cpp b/MbDCode/DispCompIeqctJeqcO.cpp
index 3552ffb..024ed44 100644
--- a/MbDCode/DispCompIeqctJeqcO.cpp
+++ b/MbDCode/DispCompIeqctJeqcO.cpp
@@ -1 +1,23 @@
 #include "DispCompIeqctJeqcO.h"
+#include "EndFrameqct.h"
+
+MbD::DispCompIeqctJeqcO::DispCompIeqctJeqcO()
+{
+}
+
+MbD::DispCompIeqctJeqcO::DispCompIeqctJeqcO(EndFrmcptr frmi, EndFrmcptr frmj, int axis) : DispCompIeqcJeqcO(frmi, frmj, axis)
+{
+}
+
+void MbD::DispCompIeqctJeqcO::initializeGlobally()
+{
+	//ToDo: Check why not using super classes.
+	ppriIeJeOpEJpEJ = std::static_pointer_cast<EndFrameqc>(frmJ)->ppriOeOpEpE(axis);
+}
+
+void MbD::DispCompIeqctJeqcO::calcPostDynCorrectorIteration()
+{
+	//"ppriIeJeOpEIpEI is not a constant now."
+	DispCompIeqcJeqcO::calcPostDynCorrectorIteration();
+	ppriIeJeOpEIpEI = std::static_pointer_cast<EndFrameqc>(frmI)->ppriOeOpEpE(axis)->negated();
+}
diff --git a/MbDCode/DispCompIeqctJeqcO.h b/MbDCode/DispCompIeqctJeqcO.h
index e140ce8..eb45cb4 100644
--- a/MbDCode/DispCompIeqctJeqcO.h
+++ b/MbDCode/DispCompIeqctJeqcO.h
@@ -7,6 +7,10 @@ namespace MbD {
     {
         //priIeJeOpt ppriIeJeOpEIpt ppriIeJeOptpt 
     public:
+        DispCompIeqctJeqcO();
+        DispCompIeqctJeqcO(EndFrmcptr frmi, EndFrmcptr frmj, int axis);
+        void initializeGlobally() override;
+        void calcPostDynCorrectorIteration() override;
 
         double priIeJeOpt;
         FRowDsptr ppriIeJeOpEIpt;
diff --git a/MbDCode/EndFramec.cpp b/MbDCode/EndFramec.cpp
index 7d6d253..b49837a 100644
--- a/MbDCode/EndFramec.cpp
+++ b/MbDCode/EndFramec.cpp
@@ -2,6 +2,7 @@
 #include <memory>
 
 #include "EndFramec.h"
+#include "MarkerFrame.h"
 
 using namespace MbD;
 
@@ -40,4 +41,16 @@ void EndFramec::initEndFrameqct()
 
 void MbD::EndFramec::calcPostDynCorrectorIteration()
 {
+	rOeO = markerFrame->rOmO;
+	aAOe = markerFrame->aAOm;
+}
+
+FColDsptr MbD::EndFramec::aAjOe(int j)
+{
+	return aAOe->column(j);
+}
+
+double MbD::EndFramec::riOeO(int i)
+{
+	return rOeO->at(i);
 }
diff --git a/MbDCode/EndFramec.h b/MbDCode/EndFramec.h
index 6f9ddab..510764b 100644
--- a/MbDCode/EndFramec.h
+++ b/MbDCode/EndFramec.h
@@ -26,6 +26,8 @@ namespace MbD {
         void initializeGlobally() override;
         virtual void initEndFrameqct();
         void calcPostDynCorrectorIteration() override;
+        FColDsptr aAjOe(int j);
+        double riOeO(int i);
 
         MarkerFrame* markerFrame;
         FColDsptr rOeO = std::make_shared<FullColumn<double>>(3);
diff --git a/MbDCode/EndFrameqc.cpp b/MbDCode/EndFrameqc.cpp
index 95d1c54..b1cdc48 100644
--- a/MbDCode/EndFrameqc.cpp
+++ b/MbDCode/EndFrameqc.cpp
@@ -4,16 +4,10 @@
 #include "EndFrameqct.h"
 #include "Variable.h"
 #include "MarkerFrame.h"
+#include "CREATE.h"
 
 using namespace MbD;
 
-std::shared_ptr<EndFrameqc> MbD::EndFrameqc::Create(const char* name)
-{
-	auto item = std::make_shared<EndFrameqc>(name);
-	item->initialize();
-	return item;
-}
-
 EndFrameqc::EndFrameqc() {
 }
 
@@ -28,46 +22,28 @@ void EndFrameqc::initialize()
 	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;
-	}
+	pprOeOpEpE = markerFrame->pprOmOpEpE;
+	ppAOepEpE = markerFrame->ppAOmpEpE;
 }
 
 void EndFrameqc::initEndFrameqct()
 {
-	endFrameqct = EndFrameqct::Create(this->getName().data());
-}
-
-void EndFrameqc::setrmemBlks(std::shared_ptr<FullColumn<std::shared_ptr<Symbolic>>> xyzBlks)
-{
-	std::static_pointer_cast<EndFrameqct>(endFrameqct)->rmemBlks = xyzBlks;
-}
-
-void EndFrameqc::setphiThePsiBlks(std::shared_ptr<FullColumn<std::shared_ptr<Symbolic>>> xyzRotBlks)
-{
-	std::static_pointer_cast<EndFrameqct>(endFrameqct)->phiThePsiBlks = xyzRotBlks;
+	endFrameqct = CREATE<EndFrameqct>::With(this->getName().data());
+	endFrameqct->prOeOpE = prOeOpE;
+	endFrameqct->pprOeOpEpE = pprOeOpEpE;
+	endFrameqct->pAOepE = pAOepE;
+	endFrameqct->ppAOepEpE = ppAOepEpE;
 }
 
 FMatFColDsptr MbD::EndFrameqc::ppAjOepEpE(int jj)
 {
 	auto answer = std::make_shared<FullMatrix<std::shared_ptr<FullColumn<double>>>>(4, 4);
-	for (int i = 0; i < 4; i++) {
-		auto answeri = answer->at(i);
-		auto ppAOepEipE = ppAOepEpE->at(i);
-		for (int j = i; j < 4; j++) {
+	for (size_t i = 0; i < 4; i++) {
+		auto& answeri = answer->at(i);
+		auto& ppAOepEipE = ppAOepEpE->at(i);
+		for (size_t j = i; j < 4; j++) {
 			answeri->at(j) = ppAOepEipE->at(j)->column(jj);
 		}
 	}
@@ -77,4 +53,50 @@ FMatFColDsptr MbD::EndFrameqc::ppAjOepEpE(int jj)
 
 void MbD::EndFrameqc::calcPostDynCorrectorIteration()
 {
+	EndFramec::calcPostDynCorrectorIteration();
+	prOeOpE = markerFrame->prOmOpE;
+	pAOepE = markerFrame->pAOmpE;
+}
+
+FMatDsptr MbD::EndFrameqc::pAjOepET(int axis)
+{
+	auto answer = std::make_shared<FullMatrix<double>>(4, 3);
+	for (size_t i = 0; i < 4; i++) {
+		auto& answeri = answer->at(i);
+		auto& pAOepEi = pAOepE->at(i);
+		for (size_t j = 0; j < 3; j++) {
+			auto& answerij = pAOepEi->at(j)->at(axis);
+			answeri->at(j) = answerij;
+		}
+	}
+	return answer;
+}
+
+FMatDsptr MbD::EndFrameqc::ppriOeOpEpE(int ii)
+{
+	auto answer = std::make_shared<FullMatrix<double>>(4, 4);
+	for (size_t i = 0; i < 4; i++) {
+		auto& answeri = answer->at(i);
+		auto& pprOeOpEipE = pprOeOpEpE->at(i);
+		for (size_t j = 0; j < 4; j++) {
+			auto& answerij = pprOeOpEipE->at(j)->at(ii);
+			answeri->at(j) = answerij;
+		}
+	}
+	return answer;
+}
+
+int MbD::EndFrameqc::iqX()
+{
+	return markerFrame->iqX();
+}
+
+int MbD::EndFrameqc::iqE()
+{
+	return markerFrame->iqE();
+}
+
+FRowDsptr MbD::EndFrameqc::priOeOpE(int i)
+{
+	return prOeOpE->at(i);
 }
diff --git a/MbDCode/EndFrameqc.h b/MbDCode/EndFrameqc.h
index 748e813..042604f 100644
--- a/MbDCode/EndFrameqc.h
+++ b/MbDCode/EndFrameqc.h
@@ -1,31 +1,33 @@
 #pragma once
 
 #include "EndFramec.h"
+#include "Symbolic.h"
 
 namespace MbD {
-    class Symbolic;
+    class EndFrameqct;
 
     class EndFrameqc : public EndFramec
     {
         //prOeOpE pprOeOpEpE pAOepE ppAOepEpE
     public:
-        static std::shared_ptr<EndFrameqc> Create(const char* name);
         EndFrameqc();
         EndFrameqc(const char* str);
         void initialize();
-        void initializeLocally() override;
         void initializeGlobally() override;
         void initEndFrameqct() override;
-        void setrmemBlks(std::shared_ptr<FullColumn<std::shared_ptr<Symbolic>>> xyzBlks);
-        void setphiThePsiBlks(std::shared_ptr<FullColumn<std::shared_ptr<Symbolic>>> xyzRotBlks);
-        FMatFColDsptr ppAjOepEpE(int axis);
+        FMatFColDsptr ppAjOepEpE(int j);
         void calcPostDynCorrectorIteration() override;
+        FMatDsptr pAjOepET(int j);
+        FMatDsptr ppriOeOpEpE(int i);
+        int iqX();
+        int iqE();
+        FRowDsptr priOeOpE(int i);
 
         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;
+        std::shared_ptr<EndFrameqct> endFrameqct;
     };
 }
 
diff --git a/MbDCode/EndFrameqct.cpp b/MbDCode/EndFrameqct.cpp
index 4ae861c..3ae7cd5 100644
--- a/MbDCode/EndFrameqct.cpp
+++ b/MbDCode/EndFrameqct.cpp
@@ -1,19 +1,14 @@
 #include "EndFrameqct.h"
+#include "MarkerFrame.h"
 #include "System.h"
 #include "Symbolic.h"
 #include "Time.h"
+#include "EulerParameters.h"
+#include "CREATE.h"
+#include "EulerAngleszxz.h"
 
 using namespace MbD;
 
-//class Symbolic;
-
-std::shared_ptr<EndFrameqct> MbD::EndFrameqct::Create(const char* name)
-{
-	auto item = std::make_shared<EndFrameqct>(name);
-	item->initialize();
-	return item;
-}
-
 EndFrameqct::EndFrameqct() {
 }
 
@@ -22,14 +17,15 @@ EndFrameqct::EndFrameqct(const char* str) : EndFrameqc(str) {
 
 void EndFrameqct::initialize()
 {
-	rmem = std::make_shared<FullColumn<double>>(3);	
-	prmempt = std::make_shared<FullColumn<double>>(3);	
-	pprmemptpt = std::make_shared<FullColumn<double>>(3);	
+	EndFrameqc::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);	
+	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);
 }
@@ -62,11 +58,12 @@ void EndFrameqct::initializeGlobally()
 
 void EndFrameqct::initprmemptBlks()
 {
-	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();
+	auto& mbdTime = System::getInstance().time;
+	prmemptBlks = std::make_shared< FullColumn<Symsptr>>(3);
+	for (size_t i = 0; i < 3; i++) {
+		auto& disp = rmemBlks->at(i);
+		auto var = disp->differentiateWRT(disp, mbdTime);
+		auto vel = var->simplified(var);
 		prmemptBlks->at(i) = vel;
 	}
 }
@@ -77,9 +74,19 @@ void EndFrameqct::initpprmemptptBlks()
 
 void EndFrameqct::initpPhiThePsiptBlks()
 {
+	auto& mbdTime = System::getInstance().time;
+	pPhiThePsiptBlks = std::make_shared< FullColumn<Symsptr>>(3);
+	for (size_t i = 0; i < 3; i++) {
+		auto& angle = phiThePsiBlks->at(i);
+		auto var = angle->differentiateWRT(angle, mbdTime);
+		std::cout << "var " << *var << std::endl;
+		auto vel = var->simplified(var);
+		std::cout << "vel " << *vel << std::endl;
+		pPhiThePsiptBlks->at(i) = vel;
+	}
 }
 
-void EndFrameqct::initppPhiThePsiptptBlks()
+void MbD::EndFrameqct::initppPhiThePsiptptBlks()
 {
 }
 
@@ -89,4 +96,63 @@ void MbD::EndFrameqct::postInput()
 
 void MbD::EndFrameqct::calcPostDynCorrectorIteration()
 {
+	auto& rOmO = markerFrame->rOmO;
+	auto& aAOm = markerFrame->aAOm;
+	rOeO = rOmO->plusFullColumn(aAOm->timesFullColumn(rmem));
+	auto& prOmOpE = markerFrame->prOmOpE;
+	auto& pAOmpE = markerFrame->pAOmpE;
+	for (size_t i = 0; i < 3; i++)
+	{
+		auto& prOmOpEi = prOmOpE->at(i);
+		auto& prOeOpEi = prOeOpE->at(i);
+		for (size_t j = 0; j < 4; j++)
+		{
+			auto prOeOpEij = prOmOpEi->at(j) + pAOmpE->at(j)->at(i)->timesFullColumn(rmem);
+			prOeOpEi->at(j) = prOeOpEij;
+		}
+	}
+	auto rpep = markerFrame->rpmp->plusFullColumn(markerFrame->aApm->timesFullColumn(rmem));
+	pprOeOpEpE = EulerParameters<double>::ppApEpEtimesColumn(rpep);
+	aAOe = aAOm->timesFullMatrix(aAme);
+	for (size_t i = 0; i < 4; i++)
+	{
+		pAOepE->at(i) = pAOmpE->at(i)->timesFullMatrix(aAme);
+	}
+	auto aApe = markerFrame->aApm->timesFullMatrix(aAme);
+	ppAOepEpE = EulerParameters<double>::ppApEpEtimesMatrix(aApe);
+}
+
+void MbD::EndFrameqct::prePosIC()
+{
+	time = System::getInstance().mbdTimeValue();
+	this->evalrmem();
+	this->evalAme();
+	EndFrameqc::prePosIC();
+}
+
+void MbD::EndFrameqct::evalrmem()
+{
+	if (rmemBlks) {
+		for (size_t i = 0; i < 3; i++)
+		{
+			auto& expression = rmemBlks->at(i);
+			double value = expression->getValue();
+			rmem->at(i) = value;
+		}
+	}
+}
+
+void MbD::EndFrameqct::evalAme()
+{
+	if (phiThePsiBlks) {
+		auto phiThePsi = CREATE<EulerAngleszxz<double>>::With();
+		for (size_t i = 0; i < 3; i++)
+		{
+			auto& expression = phiThePsiBlks->at(i);
+			auto value = expression->getValue();
+			phiThePsi->at(i) = value;
+		}
+		phiThePsi->calc();
+		aAme = phiThePsi->aA;
+	}
 }
diff --git a/MbDCode/EndFrameqct.h b/MbDCode/EndFrameqct.h
index b4658a0..4deb721 100644
--- a/MbDCode/EndFrameqct.h
+++ b/MbDCode/EndFrameqct.h
@@ -1,9 +1,8 @@
 #pragma once
 #include "EndFrameqc.h"
+#include "Symbolic.h"
 
 namespace MbD {
-
-	class Symbolic;
 	class Time;
 
 	class EndFrameqct : public EndFrameqc
@@ -11,7 +10,6 @@ namespace MbD {
 		//time rmemBlks prmemptBlks pprmemptptBlks phiThePsiBlks pPhiThePsiptBlks ppPhiThePsiptptBlks 
 		//rmem prmempt pprmemptpt aAme pAmept ppAmeptpt prOeOpt pprOeOpEpt pprOeOptpt pAOept ppAOepEpt ppAOeptpt 
 	public:
-		static std::shared_ptr<EndFrameqct> Create(const char* name);
 		EndFrameqct();
 		EndFrameqct(const char* str);
 		void initialize();
@@ -23,10 +21,13 @@ namespace MbD {
 		void initppPhiThePsiptptBlks();
 		void postInput() override;
 		void calcPostDynCorrectorIteration() override;
+		void prePosIC() override;
+		void evalrmem();
+		void evalAme();
 
-		std::shared_ptr<Time> time;
-		std::shared_ptr<FullColumn<std::shared_ptr<Symbolic>>> rmemBlks, prmemptBlks, pprmemptptBlks;
-		std::shared_ptr<FullColumn<std::shared_ptr<Symbolic>>> phiThePsiBlks, pPhiThePsiptBlks, ppPhiThePsiptptBlks;
+		double time = 0.0;
+		std::shared_ptr<FullColumn<Symsptr>> rmemBlks, prmemptBlks, pprmemptptBlks;
+		std::shared_ptr<FullColumn<Symsptr>> phiThePsiBlks, pPhiThePsiptBlks, ppPhiThePsiptptBlks;
 		FColDsptr rmem, prmempt, pprmemptpt, pprOeOptpt;
 		FMatDsptr aAme, pAmept, ppAmeptpt, ppAOeptpt;
 		FMatDsptr pprOeOpEpt;
diff --git a/MbDCode/EulerAngleszxz.cpp b/MbDCode/EulerAngleszxz.cpp
new file mode 100644
index 0000000..de386cc
--- /dev/null
+++ b/MbDCode/EulerAngleszxz.cpp
@@ -0,0 +1 @@
+#include "EulerAngleszxz.h"
diff --git a/MbDCode/EulerAngleszxz.h b/MbDCode/EulerAngleszxz.h
new file mode 100644
index 0000000..00dfa57
--- /dev/null
+++ b/MbDCode/EulerAngleszxz.h
@@ -0,0 +1,67 @@
+#pragma once
+
+#include "EulerArray.h"
+#include "FullColumn.h"
+#include "FullMatrix.h"
+
+namespace MbD {
+
+	template <typename T>
+	class EulerAngleszxz : public EulerArray<T>
+	{
+		//phiA theA psiA aA 
+	public:
+		EulerAngleszxz() : EulerArray<T>(3) {}
+		void initialize() override;
+		void calc();
+
+		FMatDsptr phiA, theA, psiA, aA;
+	};
+	template<typename T>
+	inline void EulerAngleszxz<T>::initialize()
+	{
+		phiA = std::make_shared<FullMatrix<double>>(3, 3);
+		phiA->identity();
+		theA = std::make_shared<FullMatrix<double>>(3, 3);
+		theA->identity();
+		psiA = std::make_shared<FullMatrix<double>>(3, 3);
+		psiA->identity();
+	}
+	template<typename T>
+	inline void EulerAngleszxz<T>::calc()
+	{
+		double zero = 0.0;
+		double phi = this->at(0);
+		double sphi = sin(phi);
+		double cphi = cos(phi);
+		double the = this->at(1);
+		double sthe = sin(the);
+		double cthe = cos(the);
+		double psi = this->at(2);
+		double spsi = sin(psi);
+		double cpsi = cos(psi);
+		FRowDsptr phiAi;
+		phiAi = phiA->at(0);
+		phiAi->at(0) = cphi;
+		phiAi->at(1) = -sphi;
+		phiAi = phiA->at(1);
+		phiAi->at(0) = sphi;
+		phiAi->at(1) = cphi;
+		FRowDsptr theAi;
+		theAi = theA->at(1);
+		theAi->at(1) = cthe;
+		theAi->at(2) = -sthe;
+		theAi = theA->at(2);
+		theAi->at(1) = sthe;
+		theAi->at(2) = cthe;
+		FRowDsptr psiAi;
+		psiAi = psiA->at(0);
+		psiAi->at(0) = cpsi;
+		psiAi->at(1) = -spsi;
+		psiAi = psiA->at(1);
+		psiAi->at(0) = spsi;
+		psiAi->at(1) = cpsi;
+		aA = phiA->timesFullMatrix(theA->timesFullMatrix(psiA));
+	}
+}
+
diff --git a/MbDCode/EulerArray.h b/MbDCode/EulerArray.h
index 95041b3..dd2e25b 100644
--- a/MbDCode/EulerArray.h
+++ b/MbDCode/EulerArray.h
@@ -11,7 +11,11 @@ namespace MbD {
         EulerArray(size_t count) : FullColumn<T>(count) {}
         EulerArray(size_t count, const T& value) : FullColumn<T>(count, value) {}
         EulerArray(std::initializer_list<T> list) : FullColumn<T>{ list } {}
-
+        virtual void initialize();
     };
+    template<typename T>
+    inline void EulerArray<T>::initialize()
+    {
+    }
 }
 
diff --git a/MbDCode/EulerConstraint.cpp b/MbDCode/EulerConstraint.cpp
index 6af46c2..132e21f 100644
--- a/MbDCode/EulerConstraint.cpp
+++ b/MbDCode/EulerConstraint.cpp
@@ -1,15 +1,9 @@
 #include "EulerConstraint.h"
 #include "Item.h"
+#include "PartFrame.h"
 
 using namespace MbD;
 
-std::shared_ptr<EulerConstraint> MbD::EulerConstraint::Create()
-{
-	auto item = std::make_shared<EulerConstraint>();
-	item->initialize();
-	return item;
-}
-
 EulerConstraint::EulerConstraint()
 {
 
@@ -26,4 +20,10 @@ void EulerConstraint::initialize()
 
 void MbD::EulerConstraint::calcPostDynCorrectorIteration()
 {
-}
+	auto qE = static_cast<PartFrame*>(owner)->qE;
+	aG = qE->sumOfSquares() - 1.0;
+	for (size_t i = 0; i < 4; i++)
+	{
+		pGpE->at(i) = 2.0 * qE->at(i);
+	}
+}
\ No newline at end of file
diff --git a/MbDCode/EulerConstraint.h b/MbDCode/EulerConstraint.h
index 17787dc..0877743 100644
--- a/MbDCode/EulerConstraint.h
+++ b/MbDCode/EulerConstraint.h
@@ -10,7 +10,6 @@ namespace MbD {
 	{
 		//pGpE iqE 
 	public:
-		static std::shared_ptr<EulerConstraint> Create();
 		EulerConstraint();
 		EulerConstraint(const char* str);
 		void initialize();
diff --git a/MbDCode/EulerParameters.h b/MbDCode/EulerParameters.h
index 9b02d77..de43862 100644
--- a/MbDCode/EulerParameters.h
+++ b/MbDCode/EulerParameters.h
@@ -10,15 +10,31 @@ namespace MbD {
 	{
 		//aA aB aC pApE
 	public:
+		EulerParameters() : EulerArray<T>(4) {}
 		EulerParameters(size_t count) : EulerArray<T>(count) {}
 		EulerParameters(size_t count, const T& value) : EulerArray<T>(count, value) {}
 		EulerParameters(std::initializer_list<T> list) : EulerArray<T>{ list } {}
-		
+		double sumOfSquares();
+
 		static std::shared_ptr<FullMatrix<std::shared_ptr<FullColumn<T>>>> ppApEpEtimesColumn(FColDsptr col);
 		static std::shared_ptr<FullMatrix<std::shared_ptr<FullMatrix<T>>>> ppApEpEtimesMatrix(FMatDsptr mat);
-		
+
+		void initialize();
+		void calcABC();
+		void calcpApE();
+
+		FMatDsptr aA;
+		FMatDsptr aB;
+		FMatDsptr aC;
+		FColFMatDsptr pApE;
 	};
 
+	template<typename T>
+	inline double EulerParameters<T>::sumOfSquares()
+	{
+		return 0.0;
+	}
+
 	template<>
 	inline std::shared_ptr<FullMatrix<std::shared_ptr<FullColumn<double>>>> EulerParameters<double>::ppApEpEtimesColumn(FColDsptr col)
 	{
@@ -39,22 +55,22 @@ namespace MbD {
 		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 row1 = answer->at(0);
+		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(1);
+		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(2);
+		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(3);
+		auto& row4 = answer->at(3);
 		row4->at(0) = col14;
 		row4->at(1) = col24;
 		row4->at(2) = col34;
@@ -71,7 +87,6 @@ namespace MbD {
 		FRowDsptr m2m1 = a2m1->negated();
 		FRowDsptr m2m2 = a2m2->negated();
 		FRowDsptr m2m3 = a2m3->negated();
-		auto aaaa = std::make_shared<std::vector<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 });
@@ -84,27 +99,154 @@ namespace MbD {
 		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 row1 = answer->at(0);
+		auto& row1 = answer->at(0);
 		row1->at(0) = mat11;
 		row1->at(1) = mat12;
 		row1->at(2) = mat13;
 		row1->at(3) = mat14;
-		auto row2 = answer->at(0);
+		auto& row2 = answer->at(1);
 		row2->at(0) = mat12;
 		row2->at(1) = mat22;
 		row2->at(2) = mat23;
 		row2->at(3) = mat24;
-		auto row3 = answer->at(0);
+		auto& row3 = answer->at(2);
 		row3->at(0) = mat13;
 		row3->at(1) = mat23;
 		row3->at(2) = mat33;
 		row3->at(3) = mat34;
-		auto row4 = answer->at(0);
+		auto& row4 = answer->at(3);
 		row4->at(0) = mat14;
 		row4->at(1) = mat24;
 		row4->at(2) = mat34;
 		row4->at(3) = mat44;
 		return answer;
 	}
+
+	template<>
+	inline void EulerParameters<double>::initialize()
+	{
+		aA = std::make_shared<FullMatrix<double>>(3, 3);
+		aB = std::make_shared<FullMatrix<double>>(3, 4);
+		aC = std::make_shared<FullMatrix<double>>(3, 4);
+		pApE = std::make_shared<FullColumn<std::shared_ptr<FullMatrix<double>>>>(4);
+		for (size_t i = 0; i < 4; i++)
+		{
+			pApE->at(i) = std::make_shared<FullMatrix<double>>(3, 3);
+		}
+	}
+	template<>
+	inline void EulerParameters<double>::calcABC()
+	{
+		double aE0 = this->at(0);
+		double aE1 = this->at(1);
+		double aE2 = this->at(2);
+		double aE3 = this->at(3);
+		double mE0 = -aE0;
+		double mE1 = -aE1;
+		double mE2 = -aE2;
+		FRowDsptr aBi;
+		aBi = aB->at(0);
+		aBi->at(0) = aE3;
+		aBi->at(1) = mE2;
+		aBi->at(2) = aE1;
+		aBi->at(3) = mE0;
+		aBi = aB->at(1);
+		aBi->at(0) = aE2;
+		aBi->at(1) = aE3;
+		aBi->at(2) = mE0;
+		aBi->at(3) = mE1;
+		aBi = aB->at(2);
+		aBi->at(0) = mE1;
+		aBi->at(1) = aE0;
+		aBi->at(2) = aE3;
+		aBi->at(3) = mE2;
+		FRowDsptr aCi;
+		aCi = aC->at(0);
+		aCi->at(0) = aE3;
+		aCi->at(1) = aE2;
+		aCi->at(2) = mE1;
+		aCi->at(3) = mE0;
+		aCi = aC->at(1);
+		aCi->at(0) = mE2;
+		aCi->at(1) = aE3;
+		aCi->at(2) = aE0;
+		aCi->at(3) = mE1;
+		aCi = aC->at(2);
+		aCi->at(0) = aE1;
+		aCi->at(1) = mE0;
+		aCi->at(2) = aE3;
+		aCi->at(3) = mE2;
+
+		aA = aB->timesTransposeFullMatrix(aC);
+	}
+	template<>
+	inline void EulerParameters<double>::calcpApE()
+	{
+		double a2E0 = 2.0*(this->at(0));
+		double a2E1  = 2.0*(this->at(1));
+		double a2E2  = 2.0*(this->at(2));
+		double a2E3  = 2.0*(this->at(3));
+		double m2E0 = -a2E0;
+		double m2E1 = -a2E1;
+		double m2E2 = -a2E2;
+		double m2E3 = -a2E3;
+		FMatDsptr pApEk;
+		pApEk = pApE->at(0);
+		FRowDsptr pAipEk;
+		pAipEk = pApEk->at(0);
+		pAipEk->at(0) = a2E0;
+		pAipEk->at(1) = a2E1;
+		pAipEk->at(2) = a2E2;
+		pAipEk = pApEk->at(1);
+		pAipEk->at(0) = a2E1;
+		pAipEk->at(1) = m2E0;
+		pAipEk->at(2) = m2E3;
+		pAipEk = pApEk->at(2);
+		pAipEk->at(0) = a2E2;
+		pAipEk->at(1) = a2E3;
+		pAipEk->at(2) = m2E0;
+		//
+		pApEk = pApE->at(1);
+		pAipEk = pApEk->at(0);
+		pAipEk->at(0) = m2E1;
+		pAipEk->at(1) = a2E0;
+		pAipEk->at(2) = a2E3;
+		pAipEk = pApEk->at(1);
+		pAipEk->at(0) = a2E0;
+		pAipEk->at(1) = a2E1;
+		pAipEk->at(2) = a2E2;
+		pAipEk = pApEk->at(2);
+		pAipEk->at(0) = m2E3;
+		pAipEk->at(1) = a2E2;
+		pAipEk->at(2) = m2E1;
+		//
+		pApEk = pApE->at(2);
+		pAipEk = pApEk->at(0);
+		pAipEk->at(0) = m2E2;
+		pAipEk->at(1) = m2E3;
+		pAipEk->at(2) = a2E0;
+		pAipEk = pApEk->at(1);
+		pAipEk->at(0) = a2E3;
+		pAipEk->at(1) = m2E2;
+		pAipEk->at(2) = a2E1;
+		pAipEk = pApEk->at(2);
+		pAipEk->at(0) = a2E0;
+		pAipEk->at(1) = a2E1;
+		pAipEk->at(2) = a2E2;
+		//
+		pApEk = pApE->at(3);
+		pAipEk = pApEk->at(0);
+		pAipEk->at(0) = a2E3;
+		pAipEk->at(1) = m2E2;
+		pAipEk->at(2) = a2E1;
+		pAipEk = pApEk->at(1);
+		pAipEk->at(0) = a2E2;
+		pAipEk->at(1) = a2E3;
+		pAipEk->at(2) = m2E0;
+		pAipEk = pApEk->at(2);
+		pAipEk->at(0) = m2E1;
+		pAipEk->at(1) = a2E0;
+		pAipEk->at(2) = a2E3;
+	}
 }
 
diff --git a/MbDCode/FullColumn.h b/MbDCode/FullColumn.h
index b3f14e6..197c799 100644
--- a/MbDCode/FullColumn.h
+++ b/MbDCode/FullColumn.h
@@ -13,12 +13,16 @@ namespace MbD {
 		FullColumn(size_t count) : Vector<T>(count) {}
 		FullColumn(size_t count, const T& value) : Vector<T>(count, value) {}
 		FullColumn(std::initializer_list<T> list) : Vector<T>{ list } {}
+		std::shared_ptr<FullColumn<T>> plusFullColumn(std::shared_ptr<FullColumn<T>> fullCol);
+		std::shared_ptr<FullColumn<T>> minusFullColumn(std::shared_ptr<FullColumn<T>> fullCol);
+		std::shared_ptr<FullColumn<T>> times(double a);
+		std::shared_ptr<FullColumn<T>> negated();
 		std::string toString()
 		{
 			std::stringstream ss;
 
 			ss << "FullColumn { ";
-			for (int i = 0; i < this->size() - 1; i++) {
+			for (size_t i = 0; i < this->size() - 1; i++) {
 				ss << this->at(i) << ", ";
 			}
 			ss << this->back() << " }";
@@ -26,5 +30,40 @@ namespace MbD {
 		}
 	};
 	using FColDsptr = std::shared_ptr<FullColumn<double>>;
+	template<typename T>
+	inline std::shared_ptr<FullColumn<T>> FullColumn<T>::plusFullColumn(std::shared_ptr<FullColumn<T>> fullCol)
+	{
+		size_t n = this->size();
+		auto answer = std::make_shared<FullColumn<T>>(n);
+		for (size_t i = 0; i < n; i++) {
+			answer->at(i) = this->at(i) + fullCol->at(i);
+		}
+		return answer;
+	}
+	template<typename T>
+	inline std::shared_ptr<FullColumn<T>> FullColumn<T>::minusFullColumn(std::shared_ptr<FullColumn<T>> fullCol)
+	{
+		size_t n = this->size();
+		auto answer = std::make_shared<FullColumn<T>>(n);
+		for (size_t i = 0; i < n; i++) {
+			answer->at(i) = this->at(i) - fullCol->at(i);
+		}
+		return answer;
+	}
+	template<typename T>
+	inline std::shared_ptr<FullColumn<T>> FullColumn<T>::times(double a)
+	{
+		size_t n = this->size();
+		auto answer = std::make_shared<FullColumn>(n);
+		for (size_t i = 0; i < n; i++) {
+			answer->at(i) = this->at(i) * a;
+		}
+		return answer;
+	}
+	template<typename T>
+	inline std::shared_ptr<FullColumn<T>> FullColumn<T>::negated()
+	{
+		return this->times(-1.0);
+	}
 }
 
diff --git a/MbDCode/FullMatrix.h b/MbDCode/FullMatrix.h
index 4b19555..428a307 100644
--- a/MbDCode/FullMatrix.h
+++ b/MbDCode/FullMatrix.h
@@ -14,20 +14,26 @@ namespace MbD {
 	{
 	public:
 		FullMatrix() {}
+		FullMatrix(int m) {
+			for (size_t i = 0; i < m; i++) {
+				auto row = std::make_shared<FullRow<T>>();
+				this->push_back(row);
+			}
+		}
 		FullMatrix(int m, int n) {
-			for (int i = 0; i < m; i++) {
+			for (size_t i = 0; i < m; i++) {
 				auto row = std::make_shared<FullRow<T>>(n);
 				this->push_back(row);
 			}
 		}
 		FullMatrix(std::initializer_list<std::shared_ptr<FullRow<T>>> listOfRows) {
-			for (auto row : listOfRows)
+			for (auto& row : listOfRows)
 			{
 				this->push_back(row);
 			}
 		}
 		FullMatrix(std::initializer_list<std::initializer_list<T>> list2D) {
-			for (auto rowList : list2D)
+			for (auto& rowList : list2D)
 			{
 				auto row = std::make_shared<FullRow<T>>(rowList);
 				this->push_back(row);
@@ -35,13 +41,20 @@ namespace MbD {
 		}
 		void identity();
 		std::shared_ptr<FullColumn<T>> column(int j);
+		std::shared_ptr<FullColumn<T>> timesFullColumn(std::shared_ptr<FullColumn<T>> fullCol);
+		std::shared_ptr<FullMatrix<T>> timesFullMatrix(std::shared_ptr<FullMatrix<T>> fullMat);
+		std::shared_ptr<FullMatrix<T>> timesTransposeFullMatrix(std::shared_ptr<FullMatrix<T>> fullMat);
 		std::shared_ptr<FullMatrix<T>> times(double a);
+		std::shared_ptr<FullMatrix<T>> plusFullMatrix(std::shared_ptr<FullMatrix<T>> fullMat);
+		std::shared_ptr<FullMatrix<T>> transpose();
+		std::shared_ptr<FullMatrix<T>> negated();
 		void symLowerWithUpper();
+		void atijputFullColumn(size_t i, size_t j, std::shared_ptr<FullColumn<T>> fullCol);
 	};
 	template <>
 	inline void FullMatrix<double>::identity() {
 		this->zeroSelf();
-		for (int i = 0; i < this->size(); i++) {
+		for (size_t i = 0; i < this->size(); i++) {
 			this->at(i)->at(i) = 1.0;
 		}
 	}
@@ -49,31 +62,101 @@ namespace MbD {
 	inline std::shared_ptr<FullColumn<T>> FullMatrix<T>::column(int j) {
 		size_t n = this->size();
 		auto answer = std::make_shared<FullColumn<T>>(n);
-		for (int i = 0; i < n; i++) {
+		for (size_t i = 0; i < n; i++) {
 			answer->at(i) = this->at(i)->at(j);
 		}
 		return answer;
 	}
 	template<typename T>
+	inline std::shared_ptr<FullColumn<T>> FullMatrix<T>::timesFullColumn(std::shared_ptr<FullColumn<T>> fullCol)
+	{
+		//"a*b = a(i,j)b(j) sum j."
+		size_t n = this->size();
+		auto answer = std::make_shared<FullColumn<T>>(n);
+		for (size_t i = 0; i < n; i++)
+		{
+			answer->at(i) = this->at(i)->timesFullColumn(fullCol);
+		}
+		return answer;
+	}
+	template<typename T>
+	inline std::shared_ptr<FullMatrix<T>> FullMatrix<T>::timesFullMatrix(std::shared_ptr<FullMatrix<T>> fullMat)
+	{
+		size_t m = this->nRow();
+		auto answer = std::make_shared<FullMatrix<T>>(m);
+		for (size_t i = 0; i < m; i++) {
+			answer->at(i) = this->at(i)->timesFullMatrix(fullMat);
+		}
+		return answer;
+	}
+	template<typename T>
+	inline std::shared_ptr<FullMatrix<T>> FullMatrix<T>::timesTransposeFullMatrix(std::shared_ptr<FullMatrix<T>> fullMat)
+	{
+		size_t nrow = this->nRow();
+		auto answer = std::make_shared<FullMatrix<T>>(nrow);
+		for (size_t i = 0; i < nrow; i++) {
+			answer->at(i) = this->at(i)->timesTransposeFullMatrix(fullMat);
+		}
+		return answer;
+	}
+	template<typename T>
 	inline std::shared_ptr<FullMatrix<T>> FullMatrix<T>::times(double a)
 	{
-		int m = this->nRow();
+		size_t m = this->nRow();
 		auto answer = std::make_shared<FullMatrix<T>>(m);
-		for (int i = 0; i < m; i++) {
+		for (size_t i = 0; i < m; i++) {
 			answer->at(i) = this->at(i)->times(a);
 		}
 		return answer;
 	}
 	template<typename T>
+	inline std::shared_ptr<FullMatrix<T>> FullMatrix<T>::plusFullMatrix(std::shared_ptr<FullMatrix<T>> fullMat)
+	{
+		size_t n = this->size();
+		auto answer = std::make_shared<FullMatrix<T>>(n);
+		for (size_t i = 0; i < n; i++) {
+			answer->at(i) = this->at(i)->plusFullRow(fullMat->at(i));
+		}
+		return answer;
+	}
+	template<typename T>
+	inline std::shared_ptr<FullMatrix<T>> FullMatrix<T>::transpose()
+	{
+		size_t nrow = this->nRow();
+		auto ncol = this->nCol();
+		auto answer = std::make_shared<FullMatrix<T>>(ncol, nrow);
+		for (size_t i = 0; i < nrow; i++) {
+			auto& row = this->at(i);
+			for (size_t j = 0; j < ncol; j++) {
+				answer->at(j)->at(i) = row->at(j);
+			}
+		}
+		return answer;
+	}
+	template<typename T>
+	inline std::shared_ptr<FullMatrix<T>> FullMatrix<T>::negated()
+	{
+		return this->times(-1.0);
+	}
+	template<typename T>
 	inline void FullMatrix<T>::symLowerWithUpper()
 	{
 		size_t n = this->size();
-		for (int i = 0; i < n; i++) {
-			for (int j = i+1; j < n; j++) {
+		for (size_t i = 0; i < n; i++) {
+			for (size_t j = i+1; j < n; j++) {
 				this->at(j)->at(i) = this->at(i)->at(j);
 			}
 		}
 	}
+	template<typename T>
+	inline void FullMatrix<T>::atijputFullColumn(size_t i1, size_t j1, std::shared_ptr<FullColumn<T>> fullCol)
+	{
+		auto iOffset = i1 - 1;
+		for (size_t ii = 0; ii < fullCol->size(); ii++)
+		{
+			this->at(iOffset + ii)->at(j1) = fullCol->at(ii);
+		}
+	}
 	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>>>>;
diff --git a/MbDCode/FullRow.h b/MbDCode/FullRow.h
index 65979e3..8bb0cb2 100644
--- a/MbDCode/FullRow.h
+++ b/MbDCode/FullRow.h
@@ -1,6 +1,11 @@
 #pragma once
 #include "Vector.h"
+#include "FullColumn.h"
+
 namespace MbD {
+	template <typename T>
+	class FullMatrix;
+
 	template <typename T>
 	class FullRow : public Vector<T>
 	{
@@ -11,7 +16,12 @@ namespace MbD {
 		FullRow(std::initializer_list<T> list) : Vector<T>{ list } {}
 		std::shared_ptr<FullRow<T>> times(double a);
 		std::shared_ptr<FullRow<T>> negated();
-
+		std::shared_ptr<FullRow<T>> plusFullRow(std::shared_ptr<FullRow<T>> fullRow);
+		std::shared_ptr<FullRow<T>> minusFullRow(std::shared_ptr<FullRow<T>> fullRow);
+		T timesFullColumn(std::shared_ptr<FullColumn<T>> fullCol);
+		std::shared_ptr<FullRow<T>> timesFullMatrix(std::shared_ptr<FullMatrix<T>> fullMat);
+		std::shared_ptr<FullRow<T>> timesTransposeFullMatrix(std::shared_ptr<FullMatrix<T>> fullMat);
+		void equalSelfPlusFullRowTimes(std::shared_ptr<FullRow<T>> fullRow, double factor);
 	};
 
 	template<typename T>
@@ -19,7 +29,7 @@ namespace MbD {
 	{
 		size_t n = this->size();
 		auto answer = std::make_shared<FullRow>(n);
-		for (int i = 0; i < n; i++) {
+		for (size_t i = 0; i < n; i++) {
 			answer->at(i) = this->at(i) * a;
 		}
 		return answer;
@@ -29,6 +39,60 @@ namespace MbD {
 	{
 		return this->times(-1.0);
 	}
+	template<typename T>
+	inline std::shared_ptr<FullRow<T>> FullRow<T>::plusFullRow(std::shared_ptr<FullRow<T>> fullRow)
+	{
+		size_t n = this->size();
+		auto answer = std::make_shared<FullRow<T>>(n);
+		for (size_t i = 0; i < n; i++) {
+			answer->at(i) = this->at(i) + fullRow->at(i);
+		}
+		return answer;
+	}
+	template<typename T>
+	inline std::shared_ptr<FullRow<T>> FullRow<T>::minusFullRow(std::shared_ptr<FullRow<T>> fullRow)
+	{
+		size_t n = this->size();
+		auto answer = std::make_shared<FullRow<T>>(n);
+		for (size_t i = 0; i < n; i++) {
+			answer->at(i) = this->at(i) - fullRow->at(i);
+		}
+		return answer;
+	}
+	template<typename T>
+	inline T FullRow<T>::timesFullColumn(std::shared_ptr<FullColumn<T>> fullCol)
+	{
+		auto answer = this->at(0) * fullCol->at(0);
+		for (size_t i = 1; i < this->size(); i++)
+		{
+			answer += this->at(i) * fullCol->at(i);
+		}
+		return answer;
+	}
+	template<typename T>
+	inline std::shared_ptr<FullRow<T>> FullRow<T>::timesTransposeFullMatrix(std::shared_ptr<FullMatrix<T>> fullMat)
+	{
+		return std::shared_ptr<FullRow<T>>();
+	}
+	template<typename T>
+	inline void FullRow<T>::equalSelfPlusFullRowTimes(std::shared_ptr<FullRow<T>> fullRow, double factor)
+	{
+		for (size_t i = 0; i < this->size(); i++)
+		{
+			this->at(i) += fullRow->at(i) * factor;
+		}
+	}
+	template<typename T>
+	inline std::shared_ptr<FullRow<T>> FullRow<T>::timesFullMatrix(std::shared_ptr<FullMatrix<T>> fullMat)
+	{
+		std::shared_ptr<FullRow<T>> answer = fullMat->at(0)->times(this->at(0));
+		for (size_t j = 1; j < this->size(); j++)
+		{
+			answer->equalSelfPlusFullRowTimes(fullMat->at(j), this->at(j));
+		}
+		return answer;
+			//return std::shared_ptr<FullRow<T>>();
+	}
 	using ListFRD = std::initializer_list<std::shared_ptr<FullRow<double>>>;
 	using FRowDsptr = std::shared_ptr<FullRow<double>>;
 }
diff --git a/MbDCode/FunctionWithManyArgs.cpp b/MbDCode/FunctionWithManyArgs.cpp
index 4240c70..d3589e3 100644
--- a/MbDCode/FunctionWithManyArgs.cpp
+++ b/MbDCode/FunctionWithManyArgs.cpp
@@ -3,25 +3,34 @@
 
 using namespace MbD;
 
-FunctionWithManyArgs::FunctionWithManyArgs(std::shared_ptr<Symbolic> term)
+MbD::FunctionWithManyArgs::FunctionWithManyArgs()
+{
+	terms = std::make_shared<std::vector<Symsptr>>();
+}
+
+FunctionWithManyArgs::FunctionWithManyArgs(Symsptr term) : FunctionWithManyArgs()
 {
-	terms = std::make_shared<std::vector<std::shared_ptr<Symbolic>>>();
 	terms->push_back(term);
 }
 
-FunctionWithManyArgs::FunctionWithManyArgs(std::shared_ptr<Symbolic> term, std::shared_ptr<Symbolic> term1) : FunctionWithManyArgs(term)
+FunctionWithManyArgs::FunctionWithManyArgs(Symsptr term, Symsptr term1) : FunctionWithManyArgs(term)
 {
 	terms->push_back(term1);
 }
 
-FunctionWithManyArgs::FunctionWithManyArgs(std::shared_ptr<Symbolic> term, std::shared_ptr<Symbolic> term1, std::shared_ptr<Symbolic> term2) : FunctionWithManyArgs(term, term1)
+FunctionWithManyArgs::FunctionWithManyArgs(Symsptr term, Symsptr term1, Symsptr term2) : FunctionWithManyArgs(term, term1)
 {
 	terms->push_back(term2);
 }
 
-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++)
+FunctionWithManyArgs::FunctionWithManyArgs(std::shared_ptr<std::vector<Symsptr>> _terms) {
+	terms = std::make_shared<std::vector<Symsptr>>();
+	for (size_t i = 0; i < _terms->size(); i++)
 		terms->push_back(_terms->at(i));
 }
 
+std::shared_ptr<std::vector<Symsptr>> MbD::FunctionWithManyArgs::getTerms()
+{
+	return terms;
+}
+
diff --git a/MbDCode/FunctionWithManyArgs.h b/MbDCode/FunctionWithManyArgs.h
index fe43eb6..cf214c7 100644
--- a/MbDCode/FunctionWithManyArgs.h
+++ b/MbDCode/FunctionWithManyArgs.h
@@ -1,20 +1,22 @@
 #pragma once
 
 #include "Function.h"
+#include "Symbolic.h"
 
 namespace MbD {
-    class Symbolic;
 
     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);
+        FunctionWithManyArgs();
+        FunctionWithManyArgs(Symsptr term);
+        FunctionWithManyArgs(Symsptr term, Symsptr term1);
+        FunctionWithManyArgs(Symsptr term, Symsptr term1, Symsptr term2);
+        FunctionWithManyArgs(std::shared_ptr<std::vector<Symsptr>> _terms);
+        std::shared_ptr<std::vector<Symsptr>> getTerms() override;
 
-        std::shared_ptr<std::vector<std::shared_ptr<Symbolic>>> terms;
+        std::shared_ptr<std::vector<Symsptr>> terms;
     };
 }
 
diff --git a/MbDCode/GEFullMat.cpp b/MbDCode/GEFullMat.cpp
new file mode 100644
index 0000000..176d93c
--- /dev/null
+++ b/MbDCode/GEFullMat.cpp
@@ -0,0 +1 @@
+#include "GEFullMat.h"
diff --git a/MbDCode/GEFullMat.h b/MbDCode/GEFullMat.h
new file mode 100644
index 0000000..8e484ad
--- /dev/null
+++ b/MbDCode/GEFullMat.h
@@ -0,0 +1,13 @@
+#pragma once
+
+#include "MatrixGaussElimination.h"
+
+namespace MbD {
+    class GEFullMat : public MatrixGaussElimination
+    {
+        //
+    public:
+
+    };
+}
+
diff --git a/MbDCode/GEFullMatFullPv.cpp b/MbDCode/GEFullMatFullPv.cpp
new file mode 100644
index 0000000..21ec4b2
--- /dev/null
+++ b/MbDCode/GEFullMatFullPv.cpp
@@ -0,0 +1 @@
+#include "GEFullMatFullPv.h"
diff --git a/MbDCode/GEFullMatFullPv.h b/MbDCode/GEFullMatFullPv.h
new file mode 100644
index 0000000..39e9543
--- /dev/null
+++ b/MbDCode/GEFullMatFullPv.h
@@ -0,0 +1,13 @@
+#pragma once
+
+#include "GEFullMat.h"
+
+namespace MbD {
+    class GEFullMatFullPv : public GEFullMat
+    {
+        //
+    public:
+
+    };
+}
+
diff --git a/MbDCode/GEFullMatParPv.cpp b/MbDCode/GEFullMatParPv.cpp
new file mode 100644
index 0000000..f931cdb
--- /dev/null
+++ b/MbDCode/GEFullMatParPv.cpp
@@ -0,0 +1 @@
+#include "GEFullMatParPv.h"
diff --git a/MbDCode/GEFullMatParPv.h b/MbDCode/GEFullMatParPv.h
new file mode 100644
index 0000000..d6a7f6f
--- /dev/null
+++ b/MbDCode/GEFullMatParPv.h
@@ -0,0 +1,13 @@
+#pragma once
+
+#include "GEFullMat.h"
+
+namespace MbD {
+    class GEFullMatParPv : public GEFullMat
+    {
+        //
+    public:
+
+    };
+}
+
diff --git a/MbDCode/IndependentVariable.cpp b/MbDCode/IndependentVariable.cpp
index 84afc06..6eaa572 100644
--- a/MbDCode/IndependentVariable.cpp
+++ b/MbDCode/IndependentVariable.cpp
@@ -1,7 +1,18 @@
 #include "IndependentVariable.h"
+#include "Constant.h"
 
 using namespace MbD;
 
 MbD::IndependentVariable::IndependentVariable()
 {
 }
+
+Symsptr MbD::IndependentVariable::differentiateWRT(Symsptr sptr, Symsptr var)
+{
+	if (this == var.get()) {
+		return std::make_shared<Constant>(1.0);
+	}
+	else {
+		return std::make_shared<Constant>(0.0);
+	}
+}
diff --git a/MbDCode/IndependentVariable.h b/MbDCode/IndependentVariable.h
index cc6e985..64e6f66 100644
--- a/MbDCode/IndependentVariable.h
+++ b/MbDCode/IndependentVariable.h
@@ -7,6 +7,7 @@ namespace MbD {
     {
     public:
         IndependentVariable();
+        Symsptr differentiateWRT(Symsptr sptr, Symsptr var) override;
     };
 }
 
diff --git a/MbDCode/Item.cpp b/MbDCode/Item.cpp
index 39cd1bd..885d821 100644
--- a/MbDCode/Item.cpp
+++ b/MbDCode/Item.cpp
@@ -15,7 +15,6 @@ Item::Item(const char* str) : name(str)
 
 void Item::initialize()
 {
-	assert(false);
 }
 
 void Item::setName(std::string& str)
@@ -30,12 +29,10 @@ const std::string& Item::getName() const
 
 void Item::initializeLocally()
 {
-	//DebugBreak();
 }
 
 void Item::initializeGlobally()
 {
-	//DebugBreak();
 }
 
 void MbD::Item::postInput()
@@ -46,13 +43,20 @@ void MbD::Item::postInput()
 
 void MbD::Item::calcPostDynCorrectorIteration()
 {
-	//DebugBreak();
 }
 
 void MbD::Item::removeRedundantConstraints(std::shared_ptr<std::vector<int>> redunEqnNos)
 {
 }
 
+void MbD::Item::fillPosKineError(FColDsptr col)
+{
+}
+
+void MbD::Item::fillPosKineJacob(FMatDsptr mat)
+{
+}
+
 void MbD::Item::constraintsReport()
 {
 }
@@ -60,3 +64,36 @@ void MbD::Item::constraintsReport()
 void MbD::Item::setqsu(std::shared_ptr<FullColumn<double>> qsuOld)
 {
 }
+
+void MbD::Item::useEquationNumbers()
+{
+}
+
+void MbD::Item::prePosIC()
+{
+	//"Called once before solving for position initial conditions."
+	//"Update all variable dependent instance variables needed for posIC."
+	//"This is a subset of update."
+
+	calcPostDynCorrectorIteration();
+}
+
+void MbD::Item::prePostIC()
+{
+}
+
+void MbD::Item::prePostICIteration()
+{
+}
+
+void MbD::Item::prePostICRestart()
+{
+}
+
+void MbD::Item::fillPosICError(FColDsptr col)
+{
+}
+
+void MbD::Item::fillPosICJacob(FMatDsptr mat)
+{
+}
diff --git a/MbDCode/Item.h b/MbDCode/Item.h
index 6ff8dc9..1e48da2 100644
--- a/MbDCode/Item.h
+++ b/MbDCode/Item.h
@@ -3,6 +3,7 @@
 #include <vector>
 
 #include "FullColumn.h"
+#include "FullMatrix.h"
 
 namespace MbD {
 
@@ -17,9 +18,20 @@ namespace MbD {
 		virtual void initializeGlobally();
 		virtual void postInput();
 		virtual void calcPostDynCorrectorIteration();
-		virtual void removeRedundantConstraints(std::shared_ptr<std::vector<int>> redunEqnNos);
 		virtual void constraintsReport();
 		virtual void setqsu(std::shared_ptr<FullColumn<double>> qsuOld);
+		virtual void useEquationNumbers();
+
+		virtual void prePosIC();
+		virtual void fillPosICError(FColDsptr col);
+		virtual void fillPosICJacob(FMatDsptr mat);
+		virtual void prePostIC();
+		virtual void prePostICIteration();
+		virtual void prePostICRestart();
+		virtual void removeRedundantConstraints(std::shared_ptr<std::vector<int>> redundantEqnNos);
+		virtual void fillPosKineError(FColDsptr col);
+		virtual void fillPosKineJacob(FMatDsptr mat);
+
 		void setName(std::string& str);
 		const std::string& getName() const;
 
diff --git a/MbDCode/Joint.cpp b/MbDCode/Joint.cpp
index c98fc30..5511abd 100644
--- a/MbDCode/Joint.cpp
+++ b/MbDCode/Joint.cpp
@@ -2,6 +2,8 @@
 
 #include "Joint.h"
 #include "Constraint.h"
+#include "EndFrameqc.h"
+#include "EndFrameqct.h"
 
 using namespace MbD;
 
@@ -26,6 +28,12 @@ void Joint::connectsItoJ(EndFrmcptr frmi, EndFrmcptr frmj)
 
 void Joint::initializeLocally()
 {
+	auto frmIqc = std::dynamic_pointer_cast<EndFrameqc>(frmI);
+	if (frmIqc) {
+		if (frmIqc->endFrameqct) {
+			frmI = frmIqc->endFrameqct;
+		}
+	}
 	std::for_each(constraints->begin(), constraints->end(), [](const auto& constraint) { constraint->initializeLocally(); });
 }
 
@@ -43,3 +51,8 @@ void MbD::Joint::addConstraint(std::shared_ptr<Constraint> con)
 	con->setOwner(this);
 	constraints->push_back(con);
 }
+
+void MbD::Joint::prePosIC()
+{
+	std::for_each(constraints->begin(), constraints->end(), [](const auto& constraint) { constraint->prePosIC(); });
+}
diff --git a/MbDCode/Joint.h b/MbDCode/Joint.h
index 33c5f8e..7143c66 100644
--- a/MbDCode/Joint.h
+++ b/MbDCode/Joint.h
@@ -20,6 +20,7 @@ namespace MbD {
         void initializeGlobally() override;
         void postInput() override;
         void addConstraint(std::shared_ptr<Constraint> con);
+        void prePosIC() override;
 
         EndFrmcptr frmI;
         EndFrmcptr frmJ;
diff --git a/MbDCode/MarkerFrame.cpp b/MbDCode/MarkerFrame.cpp
index 1579d85..dfcfda6 100644
--- a/MbDCode/MarkerFrame.cpp
+++ b/MbDCode/MarkerFrame.cpp
@@ -5,6 +5,7 @@
 #include "EndFramec.h"
 #include "EndFrameqc.h"
 #include "EulerParameters.h"
+#include "CREATE.h"
 
 using namespace MbD;
 
@@ -20,7 +21,7 @@ void MarkerFrame::initialize()
 	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");
+	auto endFrm = CREATE<EndFrameqc>::With();
 	this->addEndFrame(endFrm);
 }
 
@@ -28,6 +29,15 @@ void MarkerFrame::initializeLocally()
 {
 	pprOmOpEpE = EulerParameters<double>::ppApEpEtimesColumn(rpmp);
 	ppAOmpEpE = EulerParameters<double>::ppApEpEtimesMatrix(aApm);
+	for (size_t i = 0; i < endFrames->size(); i++)
+	{
+		auto eFrmqc = std::dynamic_pointer_cast<EndFrameqc>(endFrames->at(i));
+		if (eFrmqc) {
+			if (eFrmqc->endFrameqct) {
+				endFrames->at(i) = eFrmqc->endFrameqct;
+			}
+		}
+	}
 	std::for_each(endFrames->begin(), endFrames->end(), [](const auto& endFrame) { endFrame->initializeLocally(); });
 }
 
@@ -42,6 +52,33 @@ void MbD::MarkerFrame::postInput()
 
 void MbD::MarkerFrame::calcPostDynCorrectorIteration()
 {
+	auto rOpO = partFrame->rOpO();
+	auto aAOp = partFrame->aAOp();
+	auto rOmO = rOpO->plusFullColumn(aAOp->timesFullColumn(rpmp));
+	auto aAOm = aAOp->timesFullMatrix(aApm);
+	auto pAOppE = partFrame->pAOppE();
+	for (size_t i = 0; i < 4; i++)
+	{
+		auto& pAOppEi = pAOppE->at(i);
+		prOmOpE->atijputFullColumn(1, i, pAOppEi->timesFullColumn(rpmp));
+		pAOmpE->at(i) = pAOppEi->timesFullMatrix(aApm);
+	}
+}
+
+void MbD::MarkerFrame::prePosIC()
+{
+	Item::prePosIC();
+	std::for_each(endFrames->begin(), endFrames->end(), [](const auto& endFrame) { endFrame->prePosIC(); });
+}
+
+int MbD::MarkerFrame::iqX()
+{
+	return partFrame->iqX;
+}
+
+int MbD::MarkerFrame::iqE()
+{
+	return partFrame->iqE;
 }
 
 void MarkerFrame::setPartFrame(PartFrame* partFrm)
@@ -55,12 +92,12 @@ PartFrame* MarkerFrame::getPartFrame() {
 
 void MarkerFrame::setrpmp(FColDsptr x)
 {
-	rpmp->copy(x);
+	rpmp->copyFrom(x);
 }
 
 void MarkerFrame::setaApm(FMatDsptr x)
 {
-	aApm->copy(x);
+	aApm->copyFrom(x);
 }
 void MarkerFrame::addEndFrame(EndFrmcptr endFrm)
 {
diff --git a/MbDCode/MarkerFrame.h b/MbDCode/MarkerFrame.h
index 4626308..cf866e7 100644
--- a/MbDCode/MarkerFrame.h
+++ b/MbDCode/MarkerFrame.h
@@ -16,7 +16,6 @@ namespace MbD {
 	{
 		//partFrame rpmp aApm rOmO aAOm prOmOpE pAOmpE pprOmOpEpE ppAOmpEpE endFrames 
 	public:
-		static std::shared_ptr<MarkerFrame> Create(const char* name);
 		MarkerFrame();
         MarkerFrame(const char* str);
         void initialize();
@@ -29,6 +28,9 @@ namespace MbD {
 		void initializeGlobally() override;
 		void postInput() override;
 		void calcPostDynCorrectorIteration() override;
+		void prePosIC() override;
+		int iqX();
+		int iqE();
 
 		PartFrame* partFrame;
 		FColDsptr rpmp = std::make_shared<FullColumn<double>>(3);
diff --git a/MbDCode/MatrixSolver.h b/MbDCode/MatrixSolver.h
index b768832..dd206f4 100644
--- a/MbDCode/MatrixSolver.h
+++ b/MbDCode/MatrixSolver.h
@@ -7,7 +7,7 @@ namespace MbD {
     {
         //
     public:
-
+        MatrixSolver(){}
     };
 }
 
diff --git a/MbDCode/MbDCode.cpp b/MbDCode/MbDCode.cpp
index 4cd9ac6..b161b5d 100644
--- a/MbDCode/MbDCode.cpp
+++ b/MbDCode/MbDCode.cpp
@@ -31,9 +31,11 @@ using namespace MbD;
 int main()
 {
 	std::cout << "Hello World!\n";
-	System& TheSystem = System::getInstance("TheSystem");
+	System& TheSystem = System::getInstance();
+	std::string name = "TheSystem";
+	TheSystem.setName(name);
 	std::cout << "TheSystem.getName() " << TheSystem.getName() << std::endl;
-	auto systemSolver = TheSystem.systemSolver;
+	auto& systemSolver = TheSystem.systemSolver;
 	systemSolver->errorTolPosKine = 1.0e-6;
 	systemSolver->errorTolAccKine = 1.0e-6;
 	systemSolver->iterMaxPosKine = 25;
@@ -66,17 +68,17 @@ int main()
 	qE = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0, 1 });
 	assembly1->setqX(qX);
 	assembly1->setqE(qE);
-	qXdot = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0 });
-	omeOpO = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0, 1 });
-	assembly1->setqXdot(qXdot);
-	assembly1->setomeOpO(omeOpO);
+	//qXdot = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0 });
+	//omeOpO = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0, 1 });
+	//assembly1->setqXdot(qXdot);
+	//assembly1->setomeOpO(omeOpO);
 	std::cout << "assembly1->getqX() " << assembly1->getqX()->toString() << std::endl;
 	std::cout << "assembly1->getqE() " << assembly1->getqE()->toString() << std::endl;
 	TheSystem.addPart(assembly1);
 	{
-		auto partFrame = assembly1->partFrame;
+		auto& partFrame = assembly1->partFrame;
 		auto marker1 = CREATE<MarkerFrame>::With("/Assembly1/Marker1");
-		rpmp = std::make_shared<FullColumn<double>>(ListD{ 0.38423366582893, 6.8384291794733e-9, -0.048029210642807 });
+		rpmp = std::make_shared<FullColumn<double>>(ListD{ 0.0, 2.8817526385684, 0.0 });
 		marker1->setrpmp(rpmp);
 		aApm = std::make_shared<FullMatrix<double>>(ListListD{
 			{ 1, 0, 0 },
@@ -100,13 +102,14 @@ int main()
 	assembly1->asFixed();
 	//
 	auto crankPart1 = CREATE<Part>::With("/Assembly1/Part1");
+	std::cout << "crankPart1->getName() " << crankPart1->getName() << std::endl;
 	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 });
 	crankPart1->setqX(qX);
 	crankPart1->setqE(qE);
 	TheSystem.parts->push_back(crankPart1);
 	{
-		auto partFrame = crankPart1->partFrame;
+		auto& partFrame = crankPart1->partFrame;
 		auto marker1 = CREATE<MarkerFrame>::With("/Assembly1/Part1/Marker1");
 		rpmp = std::make_shared<FullColumn<double>>(ListD{ -0.38423368514246, -2.6661567755108e-17, 0.048029210642807 });
 		marker1->setrpmp(rpmp);
@@ -131,13 +134,14 @@ int main()
 	}
 	//
 	auto conrodPart2 = CREATE<Part>::With("/Assembly1/Part2");
+	std::cout << "conrodPart2->getName() " << conrodPart2->getName() << std::endl;
 	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 });
 	conrodPart2->setqX(qX);
 	conrodPart2->setqE(qE);
 	TheSystem.parts->push_back(conrodPart2);
 	{
-		auto partFrame = conrodPart2->partFrame;
+		auto& partFrame = conrodPart2->partFrame;
 		auto marker1 = CREATE<MarkerFrame>::With("/Assembly1/Part2/Marker1");
 		rpmp = std::make_shared<FullColumn<double>>(ListD{ -0.6243797383565, 1.1997705489799e-16, -0.048029210642807 });
 		marker1->setrpmp(rpmp);
@@ -162,13 +166,14 @@ int main()
 	}
 	//
 	auto pistonPart3 = CREATE<Part>::With("/Assembly1/Part3");
+	std::cout << "pistonPart3->getName() " << pistonPart3->getName() << std::endl;
 	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 });
 	pistonPart3->setqX(qX);
 	pistonPart3->setqE(qE);
 	TheSystem.parts->push_back(pistonPart3);
 	{
-		auto partFrame = pistonPart3->partFrame;
+		auto& partFrame = pistonPart3->partFrame;
 		auto marker1 = CREATE<MarkerFrame>::With("/Assembly1/Part3/Marker1");
 		rpmp = std::make_shared<FullColumn<double>>(ListD{ -0.48029210642807, 7.6201599718927e-18, -2.816737703896e-17 });
 		marker1->setrpmp(rpmp);
@@ -192,27 +197,34 @@ int main()
 		partFrame->addMarkerFrame(marker2);
 	}
 	//
-	auto cylJoint4 = CREATE<CylindricalJoint>::With("/Assembly1/Joint4");
-	cylJoint4->connectsItoJ(pistonPart3->partFrame->endFrame("/Assembly1/Part3/Marker2"), assembly1->partFrame->endFrame("/Assembly1/Marker1"));
-	TheSystem.jointsMotions->push_back(cylJoint4);
+	auto revJoint1 = CREATE<RevoluteJoint>::With("/Assembly1/Joint1");
+	std::cout << "revJoint1->getName() " << revJoint1->getName() << std::endl;
+	revJoint1->connectsItoJ(assembly1->partFrame->endFrame("/Assembly1/Marker2"), crankPart1->partFrame->endFrame("/Assembly1/Part1/Marker1"));
+	TheSystem.jointsMotions->push_back(revJoint1);
 
-	auto revJoint3 = CREATE<RevoluteJoint>::With("/Assembly1/Joint3");
-	revJoint3->connectsItoJ(conrodPart2->partFrame->endFrame("/Assembly1/Part2/Marker2"), pistonPart3->partFrame->endFrame("/Assembly1/Part3/Marker1"));
-	TheSystem.jointsMotions->push_back(revJoint3);
-	
 	auto revJoint2 = CREATE<RevoluteJoint>::With("/Assembly1/Joint2");
+	std::cout << "revJoint2->getName() " << revJoint2->getName() << std::endl;
 	revJoint2->connectsItoJ(crankPart1->partFrame->endFrame("/Assembly1/Part1/Marker2"), conrodPart2->partFrame->endFrame("/Assembly1/Part2/Marker1"));
 	TheSystem.jointsMotions->push_back(revJoint2);
 	
-	auto revJoint1 = CREATE<RevoluteJoint>::With("/Assembly1/Joint1");
-	revJoint1->connectsItoJ(assembly1->partFrame->endFrame("/Assembly1/Marker2"), crankPart1->partFrame->endFrame("/Assembly1/Part1/Marker1"));
-	TheSystem.jointsMotions->push_back(revJoint1);
-	
+	auto revJoint3 = CREATE<RevoluteJoint>::With("/Assembly1/Joint3");
+	std::cout << "revJoint3->getName() " << revJoint3->getName() << std::endl;
+	revJoint3->connectsItoJ(conrodPart2->partFrame->endFrame("/Assembly1/Part2/Marker2"), pistonPart3->partFrame->endFrame("/Assembly1/Part3/Marker1"));
+	TheSystem.jointsMotions->push_back(revJoint3);
+
+	auto cylJoint4 = CREATE<CylindricalJoint>::With("/Assembly1/Joint4");
+	std::cout << "cylJoint4->getName() " << cylJoint4->getName() << std::endl;
+	cylJoint4->connectsItoJ(pistonPart3->partFrame->endFrame("/Assembly1/Part3/Marker2"), assembly1->partFrame->endFrame("/Assembly1/Marker1"));
+	TheSystem.jointsMotions->push_back(cylJoint4);
+
 	auto rotMotion1 = CREATE<ZRotation>::With("/Assembly1/Motion1");
 	rotMotion1->connectsItoJ(assembly1->partFrame->endFrame("/Assembly1/Marker2"), crankPart1->partFrame->endFrame("/Assembly1/Part1/Marker1"));
+	std::cout << "rotMotion1->getName() " << rotMotion1->getName() << std::endl;
 	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);
+	auto time = std::make_shared<Product>(timeScale, TheSystem.time);
+	rotMotion1->phiBlk = std::make_shared<Product>(omega, time);
+	std::cout << "rotMotion1->phiBlk " << *(rotMotion1->phiBlk) << std::endl;
 	TheSystem.jointsMotions->push_back(rotMotion1);
 	//
 	TheSystem.runKINEMATICS();
diff --git a/MbDCode/MbDCode.vcxproj b/MbDCode/MbDCode.vcxproj
index 3b9f24d..4623cae 100644
--- a/MbDCode/MbDCode.vcxproj
+++ b/MbDCode/MbDCode.vcxproj
@@ -165,6 +165,7 @@
     <ClCompile Include="EndFramec.cpp" />
     <ClCompile Include="EndFrameqc.cpp" />
     <ClCompile Include="EndFrameqct.cpp" />
+    <ClCompile Include="EulerAngleszxz.cpp" />
     <ClCompile Include="EulerArray.cpp" />
     <ClCompile Include="EulerConstraint.cpp" />
     <ClCompile Include="EulerParameters.cpp" />
@@ -176,6 +177,9 @@
     <ClCompile Include="Function.cpp" />
     <ClCompile Include="FunctionWithManyArgs.cpp" />
     <ClCompile Include="FunctionX.cpp" />
+    <ClCompile Include="GEFullMat.cpp" />
+    <ClCompile Include="GEFullMatFullPv.cpp" />
+    <ClCompile Include="GEFullMatParPv.cpp" />
     <ClCompile Include="GESpMat3.cpp" />
     <ClCompile Include="GESpMatFullPv.cpp" />
     <ClCompile Include="GESpMatFullPvPosIC.cpp" />
@@ -268,6 +272,7 @@
     <ClInclude Include="EndFramec.h" />
     <ClInclude Include="EndFrameqc.h" />
     <ClInclude Include="EndFrameqct.h" />
+    <ClInclude Include="EulerAngleszxz.h" />
     <ClInclude Include="EulerArray.h" />
     <ClInclude Include="EulerConstraint.h" />
     <ClInclude Include="EulerParameters.h" />
@@ -279,6 +284,9 @@
     <ClInclude Include="Function.h" />
     <ClInclude Include="FunctionWithManyArgs.h" />
     <ClInclude Include="FunctionX.h" />
+    <ClInclude Include="GEFullMat.h" />
+    <ClInclude Include="GEFullMatFullPv.h" />
+    <ClInclude Include="GEFullMatParPv.h" />
     <ClInclude Include="GESpMat3.h" />
     <ClInclude Include="GESpMatFullPv.h" />
     <ClInclude Include="GESpMatFullPvPosIC.h" />
diff --git a/MbDCode/MbDCode.vcxproj.filters b/MbDCode/MbDCode.vcxproj.filters
index f6efdea..c9cd1a1 100644
--- a/MbDCode/MbDCode.vcxproj.filters
+++ b/MbDCode/MbDCode.vcxproj.filters
@@ -318,6 +318,18 @@
     <ClCompile Include="SingularMatrixError.cpp">
       <Filter>Source Files</Filter>
     </ClCompile>
+    <ClCompile Include="EulerAngleszxz.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="GEFullMat.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="GEFullMatFullPv.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="GEFullMatParPv.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
   </ItemGroup>
   <ItemGroup>
     <ClInclude Include="Array.h">
@@ -626,5 +638,17 @@
     <ClInclude Include="SingularMatrixError.h">
       <Filter>Header Files</Filter>
     </ClInclude>
+    <ClInclude Include="EulerAngleszxz.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="GEFullMat.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="GEFullMatFullPv.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="GEFullMatParPv.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
   </ItemGroup>
 </Project>
\ No newline at end of file
diff --git a/MbDCode/NewtonRaphson.cpp b/MbDCode/NewtonRaphson.cpp
index e157679..c6a96a2 100644
--- a/MbDCode/NewtonRaphson.cpp
+++ b/MbDCode/NewtonRaphson.cpp
@@ -40,3 +40,26 @@ void MbD::NewtonRaphson::setSystem(SystemSolver* sys)
 {
 	system = sys;
 }
+
+void MbD::NewtonRaphson::iterate()
+{
+	//"
+	//	Do not skip matrix solution even when yNorm is very small.
+	//	This avoids unexpected behaviors when convergence is still
+	//	possible.
+
+	//	Do not skip redundant constraint removal even when yNorm is
+	//	zero.
+	//	"
+
+	//	iterNo : = 0.
+	//	self fillY.
+	//	self calcyNorm.
+	//	yNorms add : yNorm.
+
+	//	[self incrementIterNo.
+	//	self fillPyPx.
+	//	self solveEquations.
+	//	self calcDXNormImproveRootCalcYNorm.
+	//	self isConverged] whileFalse
+}
diff --git a/MbDCode/NewtonRaphson.h b/MbDCode/NewtonRaphson.h
index c83a165..a576d8e 100644
--- a/MbDCode/NewtonRaphson.h
+++ b/MbDCode/NewtonRaphson.h
@@ -4,12 +4,12 @@
 #include <vector>
 
 #include "Solver.h"
-//#include "FullColumn.h"
-//#include "SystemSolver.h"
+//#include "RowTypeMatrix.h"
 
 namespace MbD {
     template <typename T>
     class FullColumn;
+    //class RowTypeMatrix;
     class SystemSolver;
 
     class NewtonRaphson : public Solver
@@ -20,9 +20,11 @@ namespace MbD {
         void initializeLocally() override;
         void run() override;
         void setSystem(SystemSolver* sys);
+        void iterate();
 
         SystemSolver* system;
         std::shared_ptr<FullColumn<double>> xold, x, dx, y;
+        //std::shared_ptr<RowTypeMatrix<double>> pypx;
         std::shared_ptr<std::vector<double>> dxNorms, yNorms;
         double dxNorm, yNorm, yNormOld, yNormTol, dxTol, twoAlp, lam;
         int iterNo, iterMax, nDivergence, nBackTracking;
diff --git a/MbDCode/Part.cpp b/MbDCode/Part.cpp
index 122e66c..0723082 100644
--- a/MbDCode/Part.cpp
+++ b/MbDCode/Part.cpp
@@ -1,5 +1,6 @@
 #include "Part.h"
 #include "PartFrame.h"
+#include "CREATE.h"
 
 using namespace MbD;
 
@@ -11,7 +12,7 @@ Part::Part(const char* str) : Item(str) {
 
 void Part::initialize()
 {
-	partFrame = PartFrame::Create();
+	partFrame = CREATE<PartFrame>::With();
 	partFrame->setPart(this);
 	pTpE = std::make_shared<FullColumn<double>>(4);
 	ppTpEpE = std::make_shared<FullMatrix<double>>(4, 4);
@@ -21,6 +22,9 @@ void Part::initialize()
 void Part::initializeLocally()
 {
 	partFrame->initializeLocally();
+	//mX: = m > 0
+	//ifTrue: [StMDiagonalMatrix new:3 withAll : m]
+	//ifFalse : [StMDiagonalMatrix new:3 withAll : 0.0d]
 }
 
 void Part::initializeGlobally()
@@ -77,3 +81,19 @@ void MbD::Part::postInput()
 void MbD::Part::calcPostDynCorrectorIteration()
 {
 }
+
+void MbD::Part::prePosIC()
+{
+	partFrame->prePosIC();
+}
+
+void MbD::Part::iqX(int eqnNo)
+{
+	partFrame->iqX = eqnNo;
+}
+
+void MbD::Part::iqE(int eqnNo)
+{
+	partFrame->iqE = eqnNo;
+
+}
diff --git a/MbDCode/Part.h b/MbDCode/Part.h
index 3a3b0a1..5620e49 100644
--- a/MbDCode/Part.h
+++ b/MbDCode/Part.h
@@ -15,7 +15,6 @@ namespace MbD {
 	{
 		//ToDo: ipX ipE m aJ partFrame pX pXdot pE pEdot mX mE mEdot pTpE ppTpEpE ppTpEpEdot 
 	public:
-		static std::shared_ptr<Part> Create(const char* name);
 		Part();
 		Part(const char* str);
 		void initialize();
@@ -34,6 +33,10 @@ namespace MbD {
 		void postInput() override;
 		void calcPostDynCorrectorIteration() override;
 
+		void prePosIC() override;
+		void iqX(int eqnNo);
+		void iqE(int eqnNo);
+
 		int ipX = -1; 
 		int ipE = -1; 
 		double m = 0.0; 
diff --git a/MbDCode/PartFrame.cpp b/MbDCode/PartFrame.cpp
index aaef221..365039b 100644
--- a/MbDCode/PartFrame.cpp
+++ b/MbDCode/PartFrame.cpp
@@ -7,16 +7,10 @@
 #include "MarkerFrame.h"
 #include "EulerParameters.h"
 #include "EulerParametersDot.h"
+#include "CREATE.h"
 
 using namespace MbD;
 
-std::shared_ptr<PartFrame> MbD::PartFrame::Create()
-{
-	auto item = std::make_shared<PartFrame>();
-	item->initialize();
-	return item;
-}
-
 PartFrame::PartFrame()
 {
 }
@@ -25,7 +19,7 @@ PartFrame::PartFrame(const char* str) : CartesianFrame(str)
 }
 void PartFrame::initialize()
 {
-	aGeu = EulerConstraint::Create();
+	aGeu = CREATE<EulerConstraint>::With();
 	aGeu->setOwner(this);
 	aGabs = std::make_shared<std::vector<std::shared_ptr<AbsConstraint>>>();
 	markerFrames = std::make_shared<std::vector<std::shared_ptr<MarkerFrame>>>();
@@ -45,7 +39,7 @@ void PartFrame::initializeGlobally()
 	std::for_each(aGabs->begin(), aGabs->end(), [](const auto& aGab) { aGab->initializeGlobally(); });
 }
 void PartFrame::setqX(FColDsptr x) {
-	qX->copy(x);
+	qX->copyFrom(x);
 }
 
 FColDsptr PartFrame::getqX() {
@@ -53,23 +47,23 @@ FColDsptr PartFrame::getqX() {
 }
 
 void PartFrame::setqE(FColDsptr x) {
-	qE->copy(x);
+	qE->copyFrom(x);
 }
 
 FColDsptr PartFrame::getqE() {
 	return qE;
 }
 void PartFrame::setqXdot(FColDsptr x) {
-	qXdot->copy(x);
+	//qXdot->copy(x);
 }
 
 FColDsptr PartFrame::getqXdot() {
-	return qXdot;
+	//return qXdot;
+	return std::make_shared<FullColumn<double>>(3);
 }
 
 void PartFrame::setomeOpO(FColDsptr omeOpO) {
-	//qEdot := MbDEulerParametersDot fromqEOp: qE andOmegaOpO: omeOpO
-	qEdot = EulerParametersDot<double>::FromqEOpAndOmegaOpO(qE, omeOpO);
+	//qEdot = EulerParametersDot<double>::FromqEOpAndOmegaOpO(qE, omeOpO);
 }
 
 FColDsptr PartFrame::getomeOpO() {
@@ -92,13 +86,38 @@ void PartFrame::addMarkerFrame(std::shared_ptr<MarkerFrame> markerFrame)
 
 EndFrmcptr PartFrame::endFrame(std::string name)
 {
-	auto match = std::find_if(markerFrames->begin(), markerFrames->end(), [&](auto mkr) {return mkr->getName() == name; });
+	auto match = std::find_if(markerFrames->begin(), markerFrames->end(), [&](auto& mkr) {return mkr->getName() == name; });
 	return (*match)->endFrames->at(0);
 }
 
+void MbD::PartFrame::prePosIC()
+{
+//iqX = -1;
+//iqE  = -1;
+//super prePosIC.
+//markerFrames do : [:mkr | mkr prePosIC] .
+//aGeu prePosIC.
+//aGabs do : [:con | con prePosIC]
+}
+
+FColDsptr MbD::PartFrame::rOpO()
+{
+	return qX;
+}
+
+FMatDsptr MbD::PartFrame::aAOp()
+{
+	return qE->aA;
+}
+
+FColFMatDsptr MbD::PartFrame::pAOppE()
+{
+	return qE->pApE;
+}
+
 void PartFrame::asFixed()
 {
-	for (int i = 0; i < 6; i++) {
+	for (size_t i = 0; i < 6; i++) {
 		auto con = std::make_shared<AbsConstraint>(i);
 		con->setOwner(this);
 		aGabs->push_back(con);
@@ -111,4 +130,8 @@ void MbD::PartFrame::postInput()
 
 void MbD::PartFrame::calcPostDynCorrectorIteration()
 {
+	qE->calcABC();
+	qE->calcpApE();
+	//qEdot->calcAdotBdotCdot();
+	//qEdot->calcpAdotpE();
 }
diff --git a/MbDCode/PartFrame.h b/MbDCode/PartFrame.h
index 5e35351..b751e39 100644
--- a/MbDCode/PartFrame.h
+++ b/MbDCode/PartFrame.h
@@ -7,6 +7,7 @@
 #include "FullColumn.h"
 #include "EulerParameters.h"
 #include "EulerParametersDot.h"
+#include "CREATE.h"
 
 namespace MbD {
 	class Part;
@@ -20,7 +21,6 @@ namespace MbD {
 	{
 		//ToDo: part iqX iqE qX qE qXdot qEdot qXddot qEddot aGeu aGabs markerFrames 
 	public:
-		static std::shared_ptr<PartFrame> Create();
 		PartFrame();
 		PartFrame(const char* str);
 		void initialize();
@@ -43,15 +43,20 @@ namespace MbD {
 		void addMarkerFrame(std::shared_ptr<MarkerFrame> x);
 		EndFrmcptr endFrame(std::string name);
 
+		void prePosIC() override;
+		FColDsptr rOpO();
+		FMatDsptr aAOp();
+		FColFMatDsptr pAOppE();
+
 		Part* part = nullptr;
 		int iqX = -1;
 		int iqE = -1;	//Position index of frame variables qX and qE in system list of variables
 		FColDsptr qX = std::make_shared<FullColumn<double>>(3);
-		std::shared_ptr<EulerParameters<double>> qE = std::make_shared<EulerParameters<double>>(4);
-		FColDsptr qXdot = std::make_shared<FullColumn<double>>(3);
-		std::shared_ptr<EulerParametersDot<double>> qEdot = std::make_shared<EulerParametersDot<double>>(4);
-		FColDsptr qXddot = std::make_shared<FullColumn<double>>(3);
-		FColDsptr qEddot = std::make_shared<FullColumn<double>>(4);
+		std::shared_ptr<EulerParameters<double>> qE = CREATE<EulerParameters<double>>::With(4);
+		//FColDsptr qXdot = std::make_shared<FullColumn<double>>(3);
+		//std::shared_ptr<EulerParametersDot<double>> qEdot = std::make_shared<EulerParametersDot<double>>(4);
+		//FColDsptr qXddot = std::make_shared<FullColumn<double>>(3);
+		//FColDsptr qEddot = std::make_shared<FullColumn<double>>(4);
 		std::shared_ptr<EulerConstraint> aGeu;
 		std::shared_ptr<std::vector<std::shared_ptr<AbsConstraint>>> aGabs;
 		std::shared_ptr<std::vector<std::shared_ptr<MarkerFrame>>> markerFrames;
diff --git a/MbDCode/PosICKineNewtonRaphson.cpp b/MbDCode/PosICKineNewtonRaphson.cpp
index 8d54dbd..c8bae70 100644
--- a/MbDCode/PosICKineNewtonRaphson.cpp
+++ b/MbDCode/PosICKineNewtonRaphson.cpp
@@ -1 +1,7 @@
 #include "PosICKineNewtonRaphson.h"
+
+using namespace MbD;
+
+void MbD::PosICKineNewtonRaphson::assignEquationNumbers()
+{
+}
diff --git a/MbDCode/PosICKineNewtonRaphson.h b/MbDCode/PosICKineNewtonRaphson.h
index 1a734ec..0e62b52 100644
--- a/MbDCode/PosICKineNewtonRaphson.h
+++ b/MbDCode/PosICKineNewtonRaphson.h
@@ -7,6 +7,7 @@ namespace MbD {
     {
         //
     public:
+        void assignEquationNumbers() override;
 
     };
 }
diff --git a/MbDCode/PosICNewtonRaphson.cpp b/MbDCode/PosICNewtonRaphson.cpp
index 77af4aa..73854c7 100644
--- a/MbDCode/PosICNewtonRaphson.cpp
+++ b/MbDCode/PosICNewtonRaphson.cpp
@@ -4,6 +4,10 @@
 #include "PosICNewtonRaphson.h"
 #include "SingularMatrixError.h"
 #include "SystemSolver.h"
+#include "Part.h"
+#include "Constraint.h"
+
+using namespace MbD;
 
 void MbD::PosICNewtonRaphson::run()
 {
@@ -20,3 +24,53 @@ void MbD::PosICNewtonRaphson::run()
 		}
 	}
 }
+
+void MbD::PosICNewtonRaphson::preRun()
+{
+	std::string str("MbD: Assembling system. ");
+	system->logString(str);
+	PosNewtonRaphson::preRun();
+}
+
+void MbD::PosICNewtonRaphson::assignEquationNumbers()
+{
+	auto parts = system->parts();
+	//auto contactEndFrames = system->contactEndFrames();
+	//auto uHolders = system->uHolders();
+	auto essentialConstraints = system->essentialConstraints2();
+	auto displacementConstraints = system->displacementConstraints();
+	auto perpendicularConstraints = system->perpendicularConstraints2();
+	auto eqnNo = 0;
+	for (auto& part : *parts) {
+		part->iqX(eqnNo);
+		eqnNo = eqnNo + 3;
+		part->iqE(eqnNo);
+		eqnNo = eqnNo + 4;
+	}
+	//for (auto& endFrm : *contactEndFrames) {
+	//	endFrm->is(eqnNo);
+	//	eqnNo = eqnNo + endFrm->sSize();
+	//}
+	//for (auto& uHolder : *uHolders) {
+	//	uHolder->iu(eqnNo);
+	//	eqnNo = eqnNo + 1;
+	//}
+	//nqsu = eqnNo - 1;
+	for (auto& con : *essentialConstraints) {
+		con->iG = eqnNo;
+		eqnNo = eqnNo + 1;
+	}
+	auto lastEssenConEqnNo = eqnNo - 1;
+	for (auto& con : *displacementConstraints) {
+		con->iG = eqnNo;
+		eqnNo = eqnNo + 1;
+	}
+	auto lastDispConEqnNo = eqnNo - 1;
+	for (auto& con : *perpendicularConstraints) {
+		con->iG = eqnNo;
+		eqnNo = eqnNo + 1;
+	}
+	auto n = eqnNo;
+	auto limits = { lastEssenConEqnNo, lastDispConEqnNo, n };
+	pivotRowLimits = std::make_shared<Array<int>>(limits);
+}
diff --git a/MbDCode/PosICNewtonRaphson.h b/MbDCode/PosICNewtonRaphson.h
index d608638..cc5eac4 100644
--- a/MbDCode/PosICNewtonRaphson.h
+++ b/MbDCode/PosICNewtonRaphson.h
@@ -10,6 +10,8 @@ namespace MbD {
         PosICNewtonRaphson(){}
 
         void run() override;
+        void preRun() override;
+        void assignEquationNumbers() override;
 
         std::shared_ptr<std::vector<int>> pivotRowLimits;
     };
diff --git a/MbDCode/PosNewtonRaphson.cpp b/MbDCode/PosNewtonRaphson.cpp
index a336d53..cc4eab0 100644
--- a/MbDCode/PosNewtonRaphson.cpp
+++ b/MbDCode/PosNewtonRaphson.cpp
@@ -1 +1,9 @@
 #include "PosNewtonRaphson.h"
+#include "SystemSolver.h"
+
+using namespace MbD;
+
+void MbD::PosNewtonRaphson::preRun()
+{
+	system->partsJointsMotionsDo([&](std::shared_ptr<Item> item) { item->prePosIC(); });
+}
diff --git a/MbDCode/PosNewtonRaphson.h b/MbDCode/PosNewtonRaphson.h
index 73e2169..afea2b7 100644
--- a/MbDCode/PosNewtonRaphson.h
+++ b/MbDCode/PosNewtonRaphson.h
@@ -7,6 +7,7 @@ namespace MbD {
     {
         //
     public:
+        void preRun() override;
 
     };
 }
diff --git a/MbDCode/PrescribedMotion.h b/MbDCode/PrescribedMotion.h
index b5d23a1..5653f95 100644
--- a/MbDCode/PrescribedMotion.h
+++ b/MbDCode/PrescribedMotion.h
@@ -1,8 +1,8 @@
 #pragma once
 #include "Joint.h"
+#include "Symbolic.h"
 
 namespace MbD {
-    class Symbolic;
 
     class PrescribedMotion : public Joint
     {
@@ -13,12 +13,12 @@ namespace MbD {
         void initialize();
         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;
+        Symsptr xBlk;
+        Symsptr yBlk;
+        Symsptr zBlk;
+        Symsptr phiBlk;
+        Symsptr theBlk;
+        Symsptr psiBlk;
     };
 }
 
diff --git a/MbDCode/Product.cpp b/MbDCode/Product.cpp
index abc43a9..4026ee3 100644
--- a/MbDCode/Product.cpp
+++ b/MbDCode/Product.cpp
@@ -1,10 +1,161 @@
+#include <algorithm>
+#include <iterator>
+
 #include "Product.h"
+#include "Sum.h"
+#include "Constant.h"
 
 using namespace MbD;
 
+Symsptr MbD::Product::differentiateWRT(Symsptr sptr, Symsptr var)
+{
+	//"Apply chain rule of differentiation."
+	//	"(xyz)' := x'yz + xy'z + xyz'."
+
+	auto derivatives = std::make_shared<std::vector<Symsptr>>();
+	std::transform(terms->begin(),
+		terms->end(),
+		std::back_inserter(*derivatives),
+		[var](Symsptr term) { return term->differentiateWRT(term, var); }
+	);
+	auto derivativeTerms = std::make_shared<std::vector<Symsptr>>();
+	for (size_t i = 0; i < terms->size(); i++)
+	{
+		auto& derivative = derivatives->at(i);
+		auto newTermFunctions = std::make_shared<std::vector<Symsptr>>(*terms);
+		newTermFunctions->at(i) = derivative;
+		auto newTerm = std::make_shared<Product>();
+		newTerm->terms = newTermFunctions;
+		derivativeTerms->push_back(newTerm);
+	}
+	auto answer = std::make_shared<Sum>();
+	answer->terms = derivativeTerms;
+	return answer;
+}
+
+Symsptr MbD::Product::expandUntil(Symsptr sptr, std::shared_ptr<std::unordered_set<Symsptr>> set)
+{
+	auto itr = std::find_if(set->begin(), set->end(), [sptr](Symsptr sym) {return sptr.get() == sym.get(); });
+	if (itr != set->end()) {
+		auto answer = std::make_shared<Sum>();
+		answer->terms = terms;
+		return answer;
+	}
+	auto sumTerms = std::make_shared<std::vector<Symsptr>>();
+	auto productTerms = std::make_shared<std::vector<Symsptr>>();
+	for (const auto& term : *terms) {
+		auto newTerm = term->expandUntil(term, set);
+		if (newTerm->isSum()) {
+			sumTerms->push_back(newTerm);
+		}
+		else if (newTerm->isProduct()) {
+			productTerms->insert(productTerms->end(), newTerm->getTerms()->begin(), newTerm->getTerms()->end());
+		}
+		else {
+			productTerms->push_back(newTerm);
+		}
+	}
+	auto factor = std::make_shared<Product>();
+	factor->terms = productTerms;
+	auto sumOfProductsOfSums = std::make_shared<Sum>(std::make_shared<Constant>(1));
+	for (const auto& term : *sumTerms) {
+		sumOfProductsOfSums = std::static_pointer_cast<Sum>(sumOfProductsOfSums->timesSum(sumOfProductsOfSums, term));
+	}
+	return factor->timesSum(factor, sumOfProductsOfSums);
+}
+
+Symsptr MbD::Product::simplifyUntil(Symsptr sptr, std::shared_ptr<std::unordered_set<Symsptr>> set)
+{
+	auto itr = std::find_if(set->begin(), set->end(), [this](Symsptr sym) {return this == (sym.get()); });
+	if (itr != set->end()) {
+		auto answer = std::make_shared<Sum>();
+		answer->terms = terms;
+		return answer;
+	}
+	auto newTerms = std::make_shared<std::vector<Symsptr>>();
+	double factor = 1.0;
+	for (const auto& term : *terms) {
+		auto newTerm = term->simplifyUntil(term, set);
+		if (newTerm->isConstant()) {
+			factor *= term->getValue();
+		}
+		else {
+			newTerms->push_back(newTerm);
+		}
+	}
+	if (factor == 0.0) {
+		return std::make_shared<Constant>(0.0);
+	}
+	if (factor != 1.0) {
+		newTerms->insert(newTerms->begin(), std::make_shared<Constant>(factor));
+	}
+	auto newSize = newTerms->size();
+	if (newSize == 0) {
+		return std::make_shared<Constant>(1.0);
+	}
+	else if (newSize == 1) {
+		return newTerms->at(0);
+	}
+	else {
+		auto answer = std::make_shared<Product>();
+		answer->terms = newTerms;
+		return answer;
+	}
+}
+
+Symsptr MbD::Product::timesSum(Symsptr sptr, Symsptr aSum)
+{
+	auto answer = std::make_shared<Sum>();
+	auto sumTERMs = aSum->getTerms();
+	for (const auto& sumTERM : *sumTERMs) {
+		Symsptr termTERM;
+		if (sumTERM->isProduct()) {
+			termTERM = sptr->timesProduct(sptr, sumTERM);
+		}
+		else {
+			termTERM = sptr->timesFunction(sptr, sumTERM);
+		}
+		answer->terms->push_back(termTERM);
+	}
+	return answer;
+}
+
+Symsptr MbD::Product::timesProduct(Symsptr sptr, Symsptr product)
+{
+	auto answer = std::make_shared<Product>(sptr->getTerms());
+	auto& answerTerms = answer->terms;
+	auto productTerms = product->getTerms();
+	answerTerms->insert(answerTerms->end(), productTerms->begin(), productTerms->end());
+	return answer;
+}
+
+Symsptr MbD::Product::timesFunction(Symsptr sptr, Symsptr function)
+{
+	auto answer = std::make_shared<Product>(sptr->getTerms());
+	answer->terms->push_back(function);
+	return answer;
+}
+
+std::ostream& MbD::Product::printOn(std::ostream& s) const
+{
+	s << "(";
+	s << *(this->terms->at(0));
+	for (size_t i = 1; i < this->terms->size(); i++)
+	{
+		s << "*" << *(this->terms->at(i));
+	}
+	s << ")";
+	return s;
+}
+
+bool MbD::Product::isProduct()
+{
+	return true;
+}
+
 double Product::getValue()
 {
 	double answer = 1.0;
-	for (int i = 0; i < terms->size(); i++) answer *= terms->at(i)->getValue();
+	for (size_t i = 0; i < terms->size(); i++) answer *= terms->at(i)->getValue();
 	return answer;
 }
diff --git a/MbDCode/Product.h b/MbDCode/Product.h
index e041b6d..12a3806 100644
--- a/MbDCode/Product.h
+++ b/MbDCode/Product.h
@@ -1,17 +1,26 @@
 #pragma once
 
 #include "FunctionWithManyArgs.h"
+#include "Symbolic.h"
 
 namespace MbD {
-    class Symbolic;
 
     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) {}
+        Product() : FunctionWithManyArgs() {}
+        Product(Symsptr term) : FunctionWithManyArgs(term) {}
+        Product(Symsptr term, Symsptr term1) : FunctionWithManyArgs(term, term1) {}
+        Product(Symsptr term, Symsptr term1, Symsptr term2) : FunctionWithManyArgs(term, term1, term2) {}
+        Product(std::shared_ptr<std::vector<Symsptr>> _terms) : FunctionWithManyArgs(_terms) {}
+        Symsptr differentiateWRT(Symsptr sptr, Symsptr var) override;
+        Symsptr expandUntil(Symsptr sptr, std::shared_ptr<std::unordered_set<Symsptr>> set) override;
+        Symsptr simplifyUntil(Symsptr sptr, std::shared_ptr<std::unordered_set<Symsptr>> set) override;
+        Symsptr timesSum(Symsptr sptr, Symsptr sum) override;
+        Symsptr timesProduct(Symsptr sptr, Symsptr product) override;
+        Symsptr timesFunction(Symsptr sptr, Symsptr function) override;
+        std::ostream& printOn(std::ostream& s) const override;
+        bool isProduct() override;
         double getValue() override;
     };
 }
diff --git a/MbDCode/RevoluteJoint.cpp b/MbDCode/RevoluteJoint.cpp
index c870a90..adaf971 100644
--- a/MbDCode/RevoluteJoint.cpp
+++ b/MbDCode/RevoluteJoint.cpp
@@ -1,4 +1,8 @@
 #include "RevoluteJoint.h"
+#include "System.h"
+#include "AtPointConstraintIJ.h"
+#include "DirectionCosineConstraintIJ.h"
+#include "CREATE.h"
 
 using namespace MbD;
 
@@ -9,3 +13,19 @@ RevoluteJoint::RevoluteJoint() {
 RevoluteJoint::RevoluteJoint(const char* str) : Joint(str) {
 
 }
+
+void MbD::RevoluteJoint::initializeGlobally()
+{
+	if (constraints->empty())
+	{
+		addConstraint(CREATE<AtPointConstraintIJ>::ConstraintWith(frmI, frmJ, 0));
+		addConstraint(CREATE<AtPointConstraintIJ>::ConstraintWith(frmI, frmJ, 1));
+		addConstraint(CREATE<AtPointConstraintIJ>::ConstraintWith(frmI, frmJ, 2));
+		addConstraint(CREATE<DirectionCosineConstraintIJ>::ConstraintWith(frmI, frmJ, 2, 0));
+		addConstraint(CREATE<DirectionCosineConstraintIJ>::ConstraintWith(frmI, frmJ, 2, 1));
+		System::getInstance().hasChanged = true;
+	}
+	else {
+		Joint::initializeGlobally();
+	}
+}
diff --git a/MbDCode/RevoluteJoint.h b/MbDCode/RevoluteJoint.h
index 1653497..fe9c71e 100644
--- a/MbDCode/RevoluteJoint.h
+++ b/MbDCode/RevoluteJoint.h
@@ -5,9 +5,9 @@ namespace MbD {
     {
         //
     public:
-        static std::shared_ptr<RevoluteJoint> Create(const char* name);
         RevoluteJoint();
         RevoluteJoint(const char* str);
+        void initializeGlobally() override;
     };
 }
 
diff --git a/MbDCode/RowTypeMatrix.h b/MbDCode/RowTypeMatrix.h
index 71da531..2fbca3b 100644
--- a/MbDCode/RowTypeMatrix.h
+++ b/MbDCode/RowTypeMatrix.h
@@ -11,26 +11,26 @@ namespace MbD {
 	public:
 		RowTypeMatrix() {}
 		RowTypeMatrix(std::initializer_list<T> list) : Array<T>{ list } {}
-		void copy(std::shared_ptr<RowTypeMatrix<T>> x);
+		void copyFrom(std::shared_ptr<RowTypeMatrix<T>> x);
 		void zeroSelf();
-		int nRow() {
+		size_t nRow() {
 			return this->size();
 		}
-		int nCol() {
+		size_t nCol() {
 			return this->at(0)->size();
 		}
 	};
 
 	template<typename T>
-	inline void RowTypeMatrix<T>::copy(std::shared_ptr<RowTypeMatrix<T>> x)
+	inline void RowTypeMatrix<T>::copyFrom(std::shared_ptr<RowTypeMatrix<T>> x)
 	{
-		for (int i = 0; i < x->size(); i++) {
-			this->at(i)->copy(x->at(i));
+		for (size_t i = 0; i < x->size(); i++) {
+			this->at(i)->copyFrom(x->at(i));
 		}
 	}
 	template <>
 	inline void RowTypeMatrix< std::shared_ptr<FullRow<double>>>::zeroSelf() {
-		for (int i = 0; i < this->size(); i++) {
+		for (size_t i = 0; i < this->size(); i++) {
 			this->at(i)->zeroSelf();
 		}
 	}
diff --git a/MbDCode/Solver.cpp b/MbDCode/Solver.cpp
index 43ac519..94d8023 100644
--- a/MbDCode/Solver.cpp
+++ b/MbDCode/Solver.cpp
@@ -21,3 +21,23 @@ void MbD::Solver::run()
 {
 	assert(false);
 }
+
+void MbD::Solver::preRun()
+{
+	assert(false);
+}
+
+void MbD::Solver::finalize()
+{
+	assert(false);
+}
+
+void MbD::Solver::reportStats()
+{
+	assert(false);
+}
+
+void MbD::Solver::postRun()
+{
+	assert(false);
+}
diff --git a/MbDCode/Solver.h b/MbDCode/Solver.h
index 9a6cc9d..cc2cf02 100644
--- a/MbDCode/Solver.h
+++ b/MbDCode/Solver.h
@@ -7,6 +7,10 @@ namespace MbD {
 		virtual void initializeLocally();
 		virtual void initializeGlobally();
 		virtual void run();
+		virtual void preRun();
+		virtual void finalize();
+		virtual void reportStats();
+		virtual void postRun();
 	};
 }
 
diff --git a/MbDCode/SparseMatrix.h b/MbDCode/SparseMatrix.h
index bffe854..a90a433 100644
--- a/MbDCode/SparseMatrix.h
+++ b/MbDCode/SparseMatrix.h
@@ -7,8 +7,15 @@ namespace MbD {
 	class SparseMatrix : public RowTypeMatrix<std::shared_ptr<SparseRow<T>>>
 	{
 	public:
+		SparseMatrix(int m, int n) {
+			for (size_t i = 0; i < m; i++)
+			{
+				auto row = std::make_shared<SparseRow<T>>(n);
+				this->push_back(row);
+			}
+		}
 		SparseMatrix(std::initializer_list<std::initializer_list<std::initializer_list<double>>> list2D) {
-			for (auto rowList : list2D)
+			for (auto& rowList : list2D)
 			{
 				auto row = std::make_shared<SparseRow<T>>(rowList);
 				this->push_back(row);
diff --git a/MbDCode/SparseRow.h b/MbDCode/SparseRow.h
index 4a632de..2af547d 100644
--- a/MbDCode/SparseRow.h
+++ b/MbDCode/SparseRow.h
@@ -9,7 +9,8 @@ namespace MbD {
 	class SparseRow : public SparseVector<T>
 	{
 	public:
-		SparseRow() {}
+		SparseRow(){}
+		SparseRow(int n) : SparseVector<T>(n) {}
 		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 } {}
 	};
diff --git a/MbDCode/SparseVector.h b/MbDCode/SparseVector.h
index 43674fa..16fb45a 100644
--- a/MbDCode/SparseVector.h
+++ b/MbDCode/SparseVector.h
@@ -7,14 +7,16 @@ namespace MbD {
 	class SparseVector : public std::map<int, T>
 	{
 	public:
+		int n;
 		SparseVector() {}
+		SparseVector(int n) : std::map<int, T>(), n(n) {}
 		SparseVector(std::initializer_list<std::pair<const int, T>> list) : std::map<int, T>{ list } {}
 		SparseVector(std::initializer_list<std::initializer_list<T>> list) {
-			for (auto pair : list) {
+			for (auto& pair : list) {
 				int i = 0;
 				int index;
 				T value;
-				for (auto element : pair) {
+				for (auto& element : pair) {
 					if (i == 0) index = (int)std::round(element); ;
 					if (i == 1) value = element;
 					i++;
diff --git a/MbDCode/Sum.cpp b/MbDCode/Sum.cpp
index ef611fe..386b831 100644
--- a/MbDCode/Sum.cpp
+++ b/MbDCode/Sum.cpp
@@ -1,10 +1,125 @@
 #include "Sum.h"
+#include "Constant.h"
 
 using namespace MbD;
 
+Symsptr MbD::Sum::timesSum(Symsptr sptr, Symsptr aSum)
+{
+	auto answer = std::make_shared<Sum>();
+	auto sumTERMs = aSum->getTerms();
+	for (const auto& term : *terms) {
+		for (const auto& sumTERM : *sumTERMs) {
+			Symsptr termTERM;
+			if (sumTERM->isProduct()) {
+				termTERM = term->timesProduct(term, sumTERM);
+			}
+			else {
+				termTERM = term->timesFunction(term, sumTERM);
+			}
+			answer->terms->push_back(termTERM);
+		}
+	}
+	return answer;
+}
+
+Symsptr MbD::Sum::timesProduct(Symsptr sptr, Symsptr product)
+{
+	return product->timesSum(product, sptr);
+}
+
+Symsptr MbD::Sum::timesFunction(Symsptr sptr, Symsptr function)
+{
+	return function->timesSum(function, sptr);
+}
+
+Symsptr MbD::Sum::expandUntil(Symsptr sptr, std::shared_ptr<std::unordered_set<Symsptr>> set)
+{
+	auto itr = std::find_if(set->begin(), set->end(), [sptr](Symsptr sym) {return sptr.get() == sym.get(); });
+	if (itr != set->end()) {
+		auto answer = std::make_shared<Sum>();
+		answer->terms = terms;
+		return answer;
+	}
+	auto newTerms = std::make_shared<std::vector<Symsptr>>();
+	for (const auto& term : *terms) {
+		auto newTerm = term->expandUntil(term, set);
+		if (newTerm->isSum()) {
+			newTerms->insert(newTerms->end(), newTerm->getTerms()->begin(), newTerm->getTerms()->end());
+		}
+		else {
+			newTerms->push_back(newTerm);
+		}
+	}
+	auto newSize = newTerms->size();
+	if (newSize == 0) {
+		return std::make_shared<Constant>(0.0);
+	}
+	else if (newSize == 1) {
+		return newTerms->at(0);
+	}
+	else {
+		auto answer = std::make_shared<Sum>();
+		answer->terms = newTerms;
+		return answer;
+	}
+}
+
+Symsptr MbD::Sum::simplifyUntil(Symsptr sptr, std::shared_ptr<std::unordered_set<Symsptr>> set)
+{
+	auto itr = std::find_if(set->begin(), set->end(), [this](Symsptr sym) {return this == (sym.get()); });
+	if (itr != set->end()) {
+		auto answer = std::make_shared<Sum>();
+		answer->terms = terms;
+		return answer;
+	}
+	auto newTerms = std::make_shared<std::vector<Symsptr>>();
+	double constant = 0.0;
+	for (const auto& term : *terms) {
+		auto newTerm = term->simplifyUntil(term, set);
+		if (newTerm->isConstant()) {
+			constant += term->getValue();
+		}
+		else {
+			newTerms->push_back(newTerm);
+		}
+	}
+	if (constant != 0.0) {
+		newTerms->insert(newTerms->begin(), std::make_shared<Constant>(constant));
+	}
+	auto newSize = newTerms->size();
+	if (newSize == 0) {
+		return std::make_shared<Constant>(0.0);
+	}
+	else if (newSize == 1) {
+		return newTerms->at(0);
+	}
+	else {
+		auto answer = std::make_shared<Sum>();
+		answer->terms = newTerms;
+		return answer;
+	}
+}
+
+bool MbD::Sum::isSum()
+{
+	return true;
+}
+
 double Sum::getValue()
 {
 	double answer = 0.0;
-	for (int i = 0; i < terms->size(); i++) answer += terms->at(i)->getValue();
+	for (size_t i = 0; i < terms->size(); i++) answer += terms->at(i)->getValue();
 	return answer;
 }
+
+std::ostream& MbD::Sum::printOn(std::ostream& s) const
+{
+	s << "(";
+	s << *(this->terms->at(0));
+	for (size_t i = 1; i < this->terms->size(); i++)
+	{
+		s << " + " << *(this->terms->at(i));
+	}
+	s << ")";
+	return s;
+}
diff --git a/MbDCode/Sum.h b/MbDCode/Sum.h
index 3ce07f4..8c0e4a7 100644
--- a/MbDCode/Sum.h
+++ b/MbDCode/Sum.h
@@ -6,11 +6,19 @@ 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) {}
+        Sum() : FunctionWithManyArgs() {}
+        Sum(Symsptr term) : FunctionWithManyArgs(term) {}
+        Sum(Symsptr term, Symsptr term1) : FunctionWithManyArgs(term, term1) {}
+        Sum(Symsptr term, Symsptr term1, Symsptr term2) : FunctionWithManyArgs(term, term1, term2) {}
+        Sum(std::shared_ptr<std::vector<Symsptr>> _terms) : FunctionWithManyArgs(_terms) {}
+        Symsptr timesSum(Symsptr sptr, Symsptr sum) override;
+        Symsptr timesProduct(Symsptr sptr, Symsptr product) override;
+        Symsptr timesFunction(Symsptr sptr, Symsptr function) override;
+        Symsptr expandUntil(Symsptr sptr, std::shared_ptr<std::unordered_set<Symsptr>> set) override;
+        Symsptr simplifyUntil(Symsptr sptr, std::shared_ptr<std::unordered_set<Symsptr>> set) override;
+        bool isSum() override;
         double getValue() override;
+        std::ostream& printOn(std::ostream& s) const override;
     };
 }
 
diff --git a/MbDCode/Symbolic.cpp b/MbDCode/Symbolic.cpp
index 03e7c40..7da7cee 100644
--- a/MbDCode/Symbolic.cpp
+++ b/MbDCode/Symbolic.cpp
@@ -4,6 +4,8 @@
 
 #include "Symbolic.h"
 #include "Constant.h"
+#include "Product.h"
+#include "Sum.h"
 
 using namespace MbD;
 
@@ -15,32 +17,77 @@ void Symbolic::initialize()
 {
 }
 
-std::shared_ptr<Symbolic> Symbolic::differentiateWRT(std::shared_ptr<Symbolic> var)
+Symsptr Symbolic::differentiateWRT(Symsptr sptr, Symsptr var)
 {
 	assert(false);
-	return std::shared_ptr<Symbolic>();
+	return Symsptr();
 }
 
-std::shared_ptr<Symbolic> Symbolic::simplified()
+Symsptr Symbolic::simplified(Symsptr sptr)
 {
-	//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::cout << "sptr " << *sptr << std::endl;
+	auto set = std::make_shared<std::unordered_set<Symsptr>>();
+	auto expanded = sptr->expandUntil(sptr, set);
+	std::cout << "expanded " << *expanded << std::endl;
+	auto set1 = std::make_shared<std::unordered_set<Symsptr>>();
+	auto simplified = expanded->simplifyUntil(expanded, set1);
+	std::cout << "simplified " << *simplified << std::endl;
+	return simplified;
 }
 
-std::shared_ptr<Symbolic> Symbolic::expandUntil(std::shared_ptr<std::unordered_set<Symbolic>> set)
+Symsptr Symbolic::expandUntil(Symsptr sptr, std::shared_ptr<std::unordered_set<Symsptr>> set)
 {
-	assert(false);
-	return std::make_shared<Constant>(0.0);
+	return sptr;
 }
 
-std::shared_ptr<Symbolic> Symbolic::simplifyUntil(std::shared_ptr<std::unordered_set<Symbolic>> set)
+Symsptr Symbolic::simplifyUntil(Symsptr sptr, std::shared_ptr<std::unordered_set<Symsptr>> set)
 {
-	assert(false);
-	return std::make_shared<Constant>(0.0);
+	return sptr;
+}
+
+Symsptr MbD::Symbolic::timesSum(Symsptr sptr, Symsptr sum)
+{
+	auto product = std::make_shared<Product>(sptr);
+	return product->timesSum(product, sum);
+}
+
+Symsptr MbD::Symbolic::timesProduct(Symsptr sptr, Symsptr product)
+{
+	auto answer = std::make_shared<Product>(product->getTerms());
+	auto& answerTerms = answer->terms;
+	answerTerms->insert(answerTerms->begin(), sptr);
+	return answer;
+}
+
+Symsptr MbD::Symbolic::timesFunction(Symsptr sptr, Symsptr function)
+{
+	auto answer = std::make_shared<Product>(sptr, function);
+	return answer;
+}
+
+bool MbD::Symbolic::isSum()
+{
+	return false;
+}
+
+bool MbD::Symbolic::isProduct()
+{
+	return false;
+}
+
+bool MbD::Symbolic::isConstant()
+{
+	return false;
+}
+
+std::ostream& MbD::Symbolic::printOn(std::ostream& s) const
+{
+	return s << "(" << typeid(*this).name() << ")";
+}
+
+std::shared_ptr<std::vector<Symsptr>> MbD::Symbolic::getTerms()
+{
+	return std::make_shared<std::vector<Symsptr>>();
 }
 
 double MbD::Symbolic::getValue()
@@ -48,3 +95,8 @@ double MbD::Symbolic::getValue()
 	assert(false);
 	return 0.0;
 }
+
+std::ostream& MbD::operator<<(std::ostream& s, const Symbolic& sym)
+{
+	return sym.printOn(s);
+}
diff --git a/MbDCode/Symbolic.h b/MbDCode/Symbolic.h
index ac34424..b3b53ab 100644
--- a/MbDCode/Symbolic.h
+++ b/MbDCode/Symbolic.h
@@ -1,7 +1,8 @@
 #pragma once
-
+#include <iostream>
 #include <unordered_set>
 #include <memory>
+#include <ostream>
 
 namespace MbD {
 
@@ -9,12 +10,24 @@ namespace MbD {
 	{
 	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 void initialize();
+		virtual std::shared_ptr<Symbolic> differentiateWRT(std::shared_ptr<Symbolic> sptr, std::shared_ptr<Symbolic> var);
+		virtual std::shared_ptr<Symbolic> simplified(std::shared_ptr<Symbolic> sptr);
+		virtual std::shared_ptr<Symbolic> expandUntil(std::shared_ptr<Symbolic> sptr, std::shared_ptr<std::unordered_set<std::shared_ptr<Symbolic>>> set);
+		virtual std::shared_ptr<Symbolic> simplifyUntil(std::shared_ptr<Symbolic> sptr, std::shared_ptr<std::unordered_set<std::shared_ptr<Symbolic>>> set);
+		virtual std::shared_ptr<Symbolic> timesSum(std::shared_ptr<Symbolic> sptr, std::shared_ptr<Symbolic> sum);
+		virtual std::shared_ptr<Symbolic> timesProduct(std::shared_ptr<Symbolic> sptr, std::shared_ptr<Symbolic> product);
+		virtual std::shared_ptr<Symbolic> timesFunction(std::shared_ptr<Symbolic> sptr, std::shared_ptr<Symbolic> function);
+		virtual bool isSum();
+		virtual bool isProduct();
+		virtual bool isConstant();
+		virtual std::ostream& printOn(std::ostream& s) const;
+
+		virtual std::shared_ptr<std::vector<std::shared_ptr<Symbolic>>> getTerms();
 		virtual double getValue();
+
+		friend std::ostream& operator<<(std::ostream& s, const Symbolic& sym);
 	};
+	using Symsptr = std::shared_ptr<Symbolic>;
 }
 
diff --git a/MbDCode/System.cpp b/MbDCode/System.cpp
index 58c5ee6..b9157b9 100644
--- a/MbDCode/System.cpp
+++ b/MbDCode/System.cpp
@@ -5,6 +5,7 @@
 #include "Joint.h"
 #include "SystemSolver.h"
 #include "Time.h"
+#include "CREATE.h"
 
 using namespace MbD;
 
@@ -18,7 +19,7 @@ System::System(const char* str) : Item(str) {
 
 void MbD::System::initialize()
 {
-	time = std::make_shared<Time>();
+	time = CREATE<Time>::With();
 	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);
@@ -84,3 +85,13 @@ void MbD::System::partsJointsMotionsDo(const std::function<void(std::shared_ptr<
 	std::for_each(parts->begin(), parts->end(), f);
 	std::for_each(jointsMotions->begin(), jointsMotions->end(), f);
 }
+
+void MbD::System::logString(std::string& str)
+{
+	std::cout << str << std::endl;
+}
+
+double MbD::System::mbdTimeValue()
+{
+	return time->getValue();
+}
diff --git a/MbDCode/System.h b/MbDCode/System.h
index ae24cfa..11d759e 100644
--- a/MbDCode/System.h
+++ b/MbDCode/System.h
@@ -29,10 +29,6 @@ namespace MbD {
 			static System singleInstance; // Block-scoped static Singleton instance
 			return singleInstance;
 		};
-		static System& getInstance(const char* str) {
-			static System singleInstance(str); // Block-scoped static Singleton instance
-			return singleInstance;
-		};
 
 		void initialize() override;
 		void initializeLocally() override;
@@ -43,6 +39,8 @@ namespace MbD {
 		void outputTimeSeries();
 		std::shared_ptr<std::vector<std::string>> discontinuitiesAtIC();
 		void partsJointsMotionsDo(const std::function <void(std::shared_ptr<Item>)>& f);
+		void logString(std::string& str);
+		double mbdTimeValue();
 
 
 		std::shared_ptr<std::vector<std::shared_ptr<Part>>> parts;
diff --git a/MbDCode/SystemNewtonRaphson.cpp b/MbDCode/SystemNewtonRaphson.cpp
index 9e4c97d..91a10dd 100644
--- a/MbDCode/SystemNewtonRaphson.cpp
+++ b/MbDCode/SystemNewtonRaphson.cpp
@@ -1 +1,19 @@
 #include "SystemNewtonRaphson.h"
+#include "SystemSolver.h"
+#include "SparseMatrix.h"
+
+void MbD::SystemNewtonRaphson::initializeGlobally()
+{
+	this->assignEquationNumbers();
+	system->partsJointsMotionsDo([&](std::shared_ptr<Item> item) { item->useEquationNumbers(); });
+
+	this->createVectorsAndMatrices();
+	matrixSolver = this->matrixSolverClassNew();
+}
+
+void MbD::SystemNewtonRaphson::createVectorsAndMatrices()
+{
+	x = std::make_shared<FullColumn<double>>(n);
+	y = std::make_shared<FullColumn<double>>(n);
+	pypx = std::make_shared <SparseMatrix<double>>(n, n);
+}
diff --git a/MbDCode/SystemNewtonRaphson.h b/MbDCode/SystemNewtonRaphson.h
index f1775fb..2860b40 100644
--- a/MbDCode/SystemNewtonRaphson.h
+++ b/MbDCode/SystemNewtonRaphson.h
@@ -1,13 +1,19 @@
 #pragma once
 
 #include "VectorNewtonRaphson.h"
+#include "SparseMatrix.h"
 
 namespace MbD {
+    //class SparseMatrix;
+
     class SystemNewtonRaphson : public VectorNewtonRaphson
     {
         //
     public:
-
+        void initializeGlobally() override;
+        virtual void assignEquationNumbers() = 0;
+        virtual void createVectorsAndMatrices();
+        std::shared_ptr<SparseMatrix<double>> pypx;
     };
 }
 
diff --git a/MbDCode/SystemSolver.cpp b/MbDCode/SystemSolver.cpp
index 6aa2029..0b5158e 100644
--- a/MbDCode/SystemSolver.cpp
+++ b/MbDCode/SystemSolver.cpp
@@ -3,18 +3,12 @@
 #include "SystemSolver.h"
 #include "NewtonRaphson.h"
 #include "PosICNewtonRaphson.h"
+#include "CREATE.h"
 
 using namespace MbD;
 
 //class PosICNewtonRaphson;
 
-std::shared_ptr<SystemSolver> MbD::SystemSolver::Create(System* x)
-{
-	auto item = std::make_shared<SystemSolver>(x);
-	item->initialize();
-	return item;
-}
-
 void MbD::SystemSolver::initialize()
 {
 	tstartPasts = std::make_shared<std::vector<double>>();
@@ -57,7 +51,7 @@ void SystemSolver::runAllIC()
 
 void MbD::SystemSolver::runPosIC()
 {
-	icTypeSolver = std::make_shared<PosICNewtonRaphson>();
+	icTypeSolver = CREATE<PosICNewtonRaphson>::With();
 	icTypeSolver->setSystem(this);
 	icTypeSolver->run();
 }
@@ -95,3 +89,28 @@ void MbD::SystemSolver::partsJointsMotionsDo(const std::function<void(std::share
 {
 	system->partsJointsMotionsDo(f);
 }
+
+void MbD::SystemSolver::logString(std::string& str)
+{
+	system->logString(str);
+}
+
+std::shared_ptr<std::vector<std::shared_ptr<Part>>> MbD::SystemSolver::parts()
+{
+	return std::shared_ptr<std::vector<std::shared_ptr<Part>>>();
+}
+
+std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> MbD::SystemSolver::essentialConstraints2()
+{
+	return std::shared_ptr<std::vector<std::shared_ptr<Constraint>>>();
+}
+
+std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> MbD::SystemSolver::displacementConstraints()
+{
+	return std::shared_ptr<std::vector<std::shared_ptr<Constraint>>>();
+}
+
+std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> MbD::SystemSolver::perpendicularConstraints2()
+{
+	return std::shared_ptr<std::vector<std::shared_ptr<Constraint>>>();
+}
diff --git a/MbDCode/SystemSolver.h b/MbDCode/SystemSolver.h
index e6dd8a2..5bda846 100644
--- a/MbDCode/SystemSolver.h
+++ b/MbDCode/SystemSolver.h
@@ -19,8 +19,8 @@ namespace MbD {
 		//iterMaxPosKine iterMaxAccKine basicIntegrator tstartPasts tstart hmin hmax tend toutFirst hout direction corAbsTol corRelTol 
 		//intAbsTol intRelTol iterMaxDyn orderMax translationLimit rotationLimit 
 	public:
-		static std::shared_ptr<SystemSolver> Create(System* x);
 		SystemSolver(System* x) : system(x) {
+			initialize();
 		}
 		void initialize() override;
 		void initializeLocally() override;
@@ -35,6 +35,13 @@ namespace MbD {
 		void runBasicCollision();
 		void runBasicKinematic();
 		void partsJointsMotionsDo(const std::function <void(std::shared_ptr<Item>)>& f);
+		void logString(std::string& str);
+		std::shared_ptr<std::vector<std::shared_ptr<Part>>> parts();
+		//std::shared_ptr<std::vector<ContactEndFrame>> contactEndFrames();
+		//std::shared_ptr<std::vector<UHolder>> uHolders();
+		std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> essentialConstraints2();
+		std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> displacementConstraints();
+		std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> perpendicularConstraints2();
 
 		System* system;
 		std::shared_ptr<NewtonRaphson> icTypeSolver;
diff --git a/MbDCode/Time.cpp b/MbDCode/Time.cpp
index 829b064..47bdc0d 100644
--- a/MbDCode/Time.cpp
+++ b/MbDCode/Time.cpp
@@ -3,6 +3,10 @@
 using namespace MbD;
 
 Time::Time()
+{
+}
+
+void MbD::Time::initialize()
 {
 	std::string str = "t";
 	this->setName(str);
diff --git a/MbDCode/Time.h b/MbDCode/Time.h
index 5d0685d..a3b17ef 100644
--- a/MbDCode/Time.h
+++ b/MbDCode/Time.h
@@ -7,6 +7,7 @@ namespace MbD {
     {
     public:
         Time();
+        void initialize() override;
     };
 }
 
diff --git a/MbDCode/TranslationConstraintIJ.cpp b/MbDCode/TranslationConstraintIJ.cpp
index a6b023e..303db62 100644
--- a/MbDCode/TranslationConstraintIJ.cpp
+++ b/MbDCode/TranslationConstraintIJ.cpp
@@ -1,21 +1,17 @@
 #include "TranslationConstraintIJ.h"
+#include "CREATE.h"
 
 using namespace MbD;
 
-std::shared_ptr<TranslationConstraintIJ> MbD::TranslationConstraintIJ::Create(EndFrmcptr frmi, EndFrmcptr frmj, int axisk)
-{
-    auto item = std::make_shared<TranslationConstraintIJ>(frmi, frmj, axisk);
-    item->initialize();
-    return item;
-}
-
-TranslationConstraintIJ::TranslationConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj, int axisk) :
-    ConstraintIJ(frmi, frmj), axisK(axisk)
+TranslationConstraintIJ::TranslationConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj, int axisi) :
+    ConstraintIJ(frmi, frmj), axisI(axisi)
 {
 }
 
 void TranslationConstraintIJ::initialize()
 {
+    ConstraintIJ::initialize();
+    initriIeJeIe();
 }
 
 void MbD::TranslationConstraintIJ::initializeLocally()
@@ -30,9 +26,20 @@ void MbD::TranslationConstraintIJ::initializeGlobally()
 
 void MbD::TranslationConstraintIJ::initriIeJeIe()
 {
-    riIeJeIe = std::make_shared<DispCompIecJecKec>();
+    riIeJeIe = CREATE<DispCompIecJecKec>::With(frmI, frmJ, frmI, axisI);
 }
 
 void MbD::TranslationConstraintIJ::postInput()
 {
 }
+
+void MbD::TranslationConstraintIJ::calcPostDynCorrectorIteration()
+{
+    aG = riIeJeIe->value() - aConstant;
+}
+
+void MbD::TranslationConstraintIJ::prePosIC()
+{
+    riIeJeIe->prePosIC();
+    Constraint::prePosIC();
+}
diff --git a/MbDCode/TranslationConstraintIJ.h b/MbDCode/TranslationConstraintIJ.h
index 7b02334..b0f9528 100644
--- a/MbDCode/TranslationConstraintIJ.h
+++ b/MbDCode/TranslationConstraintIJ.h
@@ -8,15 +8,16 @@ namespace MbD {
     {
         //riIeJeIe
     public:
-        static std::shared_ptr<TranslationConstraintIJ> Create(EndFrmcptr frmi, EndFrmcptr frmj, int axisk);
-        TranslationConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj, int axisk);
+        TranslationConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj, int axisi);
         void initialize();
         void initializeLocally() override;
         void initializeGlobally() override;
         virtual void initriIeJeIe();
         void postInput() override;
+        void calcPostDynCorrectorIteration() override;
+        void prePosIC()override;
 
-        int axisK;
+        int axisI;
         std::shared_ptr<DispCompIecJecKec> riIeJeIe;
     };
 }
diff --git a/MbDCode/TranslationConstraintIqcJc.cpp b/MbDCode/TranslationConstraintIqcJc.cpp
index fc92462..7281d0c 100644
--- a/MbDCode/TranslationConstraintIqcJc.cpp
+++ b/MbDCode/TranslationConstraintIqcJc.cpp
@@ -1,18 +1,32 @@
 #include "TranslationConstraintIqcJc.h"
 #include "DispCompIeqcJecKeqc.h"
+#include "EndFrameqc.h"
+#include "CREATE.h"
 
 using namespace MbD;
 
-TranslationConstraintIqcJc::TranslationConstraintIqcJc(EndFrmcptr frmi, EndFrmcptr frmj, int axisk) :
-	TranslationConstraintIJ(frmi, frmj, axisk)
-{
-}
-
-void TranslationConstraintIqcJc::initialize()
+TranslationConstraintIqcJc::TranslationConstraintIqcJc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi) :
+	TranslationConstraintIJ(frmi, frmj, axisi)
 {
 }
 
 void MbD::TranslationConstraintIqcJc::initriIeJeIe()
 {
-    riIeJeIe = std::make_shared<DispCompIeqcJecKeqc>();
+    riIeJeIe = CREATE<DispCompIeqcJecKeqc>::With(frmI, frmJ, frmI, axisI);
+}
+
+void MbD::TranslationConstraintIqcJc::calcPostDynCorrectorIteration()
+{
+	TranslationConstraintIJ::calcPostDynCorrectorIteration();
+	auto riIeqJeIeq = std::static_pointer_cast<DispCompIeqcJecKeqc>(riIeJeIe);
+	pGpXI = riIeqJeIeq->pvaluepXI();
+	pGpEI = (riIeqJeIeq->pvaluepEI())->plusFullRow(riIeqJeIeq->pvaluepEK());
+	ppGpXIpEI = riIeqJeIeq->ppvaluepXIpEK();
+	ppGpEIpEI = riIeqJeIeq->ppvaluepEIpEI()->plusFullMatrix(riIeqJeIeq->ppvaluepEIpEK())
+		->plusFullMatrix((riIeqJeIeq->ppvaluepEIpEK()->transpose()->plusFullMatrix(riIeqJeIeq->ppvaluepEKpEK())));
+}
+
+void MbD::TranslationConstraintIqcJc::useEquationNumbers()
+{
+	iqEI = std::static_pointer_cast<EndFrameqc>(frmI)->iqE();
 }
diff --git a/MbDCode/TranslationConstraintIqcJc.h b/MbDCode/TranslationConstraintIqcJc.h
index 973a81b..1083c9b 100644
--- a/MbDCode/TranslationConstraintIqcJc.h
+++ b/MbDCode/TranslationConstraintIqcJc.h
@@ -7,9 +7,10 @@ namespace MbD {
     {
         //pGpXI pGpEI ppGpXIpEI ppGpEIpEI iqXI iqEI 
     public:
-        TranslationConstraintIqcJc(EndFrmcptr frmi, EndFrmcptr frmj, int axisk);
-        void initialize();
+        TranslationConstraintIqcJc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi);
         void initriIeJeIe() override;
+        void calcPostDynCorrectorIteration() override;
+        void useEquationNumbers() override;
 
         FRowDsptr pGpXI;
         FRowDsptr pGpEI;
diff --git a/MbDCode/TranslationConstraintIqcJqc.cpp b/MbDCode/TranslationConstraintIqcJqc.cpp
index a5acc52..47e0b9c 100644
--- a/MbDCode/TranslationConstraintIqcJqc.cpp
+++ b/MbDCode/TranslationConstraintIqcJqc.cpp
@@ -1,18 +1,26 @@
 #include "TranslationConstraintIqcJqc.h"
 #include "DispCompIeqcJeqcKeqc.h"
+#include "EndFrameqc.h"
+#include "CREATE.h"
 
 using namespace MbD;
 
-TranslationConstraintIqcJqc::TranslationConstraintIqcJqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisk) :
-	TranslationConstraintIqcJc(frmi, frmj, axisk)
-{
-}
-
-void TranslationConstraintIqcJqc::initialize()
+TranslationConstraintIqcJqc::TranslationConstraintIqcJqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi) :
+	TranslationConstraintIqcJc(frmi, frmj, axisi)
 {
 }
 
 void MbD::TranslationConstraintIqcJqc::initriIeJeIe()
 {
-	riIeJeIe = std::make_shared<DispCompIeqcJeqcKeqc>();
+	riIeJeIe = CREATE<DispCompIeqcJeqcKeqc>::With(frmI, frmJ, frmI, axisI);
+}
+
+void MbD::TranslationConstraintIqcJqc::calcPostDynCorrectorIteration()
+{
+}
+
+void MbD::TranslationConstraintIqcJqc::useEquationNumbers()
+{
+	TranslationConstraintIqcJc::useEquationNumbers();
+	iqEJ = std::static_pointer_cast<EndFrameqc>(frmJ)->iqE();
 }
diff --git a/MbDCode/TranslationConstraintIqcJqc.h b/MbDCode/TranslationConstraintIqcJqc.h
index 61c8526..18f9d3a 100644
--- a/MbDCode/TranslationConstraintIqcJqc.h
+++ b/MbDCode/TranslationConstraintIqcJqc.h
@@ -7,9 +7,10 @@ namespace MbD {
     {
         //pGpXJ pGpEJ ppGpEIpXJ ppGpEIpEJ ppGpEJpEJ iqXJ iqEJ 
     public:
-        TranslationConstraintIqcJqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisk);
-        void initialize();
+        TranslationConstraintIqcJqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi);
         void initriIeJeIe() override;
+        void calcPostDynCorrectorIteration() override;
+        void useEquationNumbers() override;
 
         FRowDsptr pGpXJ;
         FRowDsptr pGpEJ;
diff --git a/MbDCode/TranslationConstraintIqctJqc.cpp b/MbDCode/TranslationConstraintIqctJqc.cpp
index 26cad0f..dd7a9e1 100644
--- a/MbDCode/TranslationConstraintIqctJqc.cpp
+++ b/MbDCode/TranslationConstraintIqctJqc.cpp
@@ -1,18 +1,15 @@
 #include "TranslationConstraintIqctJqc.h"
 #include "DispCompIeqctJeqcKeqct.h"
+#include "CREATE.h"
 
 using namespace MbD;
 
-TranslationConstraintIqctJqc::TranslationConstraintIqctJqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisk) :
-	TranslationConstraintIqcJqc(frmi, frmj, axisk)
-{
-}
-
-void TranslationConstraintIqctJqc::initialize()
+TranslationConstraintIqctJqc::TranslationConstraintIqctJqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi) :
+	TranslationConstraintIqcJqc(frmi, frmj, axisi)
 {
 }
 
 void MbD::TranslationConstraintIqctJqc::initriIeJeIe()
 {
-	riIeJeIe = std::make_shared<DispCompIeqctJeqcKeqct>();
+	riIeJeIe = CREATE<DispCompIeqctJeqcKeqct>::With(frmI, frmJ, frmI, axisI);
 }
diff --git a/MbDCode/TranslationConstraintIqctJqc.h b/MbDCode/TranslationConstraintIqctJqc.h
index 0695918..8385d30 100644
--- a/MbDCode/TranslationConstraintIqctJqc.h
+++ b/MbDCode/TranslationConstraintIqctJqc.h
@@ -7,8 +7,7 @@ namespace MbD {
     {
         //pGpt ppGpXIpt ppGpEIpt ppGpXJpt ppGpEJpt ppGptpt 
     public:
-        TranslationConstraintIqctJqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisk);
-        void initialize();
+        TranslationConstraintIqctJqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi);
         void initriIeJeIe() override;
 
         double pGpt;
diff --git a/MbDCode/Variable.cpp b/MbDCode/Variable.cpp
index 0730a76..e38eaf1 100644
--- a/MbDCode/Variable.cpp
+++ b/MbDCode/Variable.cpp
@@ -34,3 +34,8 @@ double Variable::getValue()
 {
 	return value;
 }
+
+std::ostream& MbD::Variable::printOn(std::ostream& s) const
+{
+	return s << this->name;
+}
diff --git a/MbDCode/Variable.h b/MbDCode/Variable.h
index ac434c9..2ad47e7 100644
--- a/MbDCode/Variable.h
+++ b/MbDCode/Variable.h
@@ -14,6 +14,7 @@ namespace MbD {
 		void setName(std::string& str);
 		const std::string& getName() const;
 		double getValue() override;
+		std::ostream& printOn(std::ostream& s) const override;
 
 		std::string name;
 		double value;
diff --git a/MbDCode/Vector.h b/MbDCode/Vector.h
index 17eef28..18b3edc 100644
--- a/MbDCode/Vector.h
+++ b/MbDCode/Vector.h
@@ -10,5 +10,16 @@ namespace MbD {
         Vector(size_t count) : Array<T>(count) {}
         Vector(size_t count, const T& value) : Array<T>(count, value) {}
         Vector(std::initializer_list<T> list) : Array<T>{ list } {}
+        double dot(std::shared_ptr<Vector<T>> vec);
     };
+    template<typename T>
+    inline double Vector<T>::dot(std::shared_ptr<Vector<T>> vec)
+    {
+        size_t n = this->size();
+        double answer = 0.0;
+        for (size_t i = 0; i < n; i++) {
+            answer += this->at(i) * vec->at(i);
+        }
+        return answer;
+    }
 }
diff --git a/MbDCode/VectorNewtonRaphson.cpp b/MbDCode/VectorNewtonRaphson.cpp
index b453e01..44de876 100644
--- a/MbDCode/VectorNewtonRaphson.cpp
+++ b/MbDCode/VectorNewtonRaphson.cpp
@@ -1,10 +1,20 @@
-#include "VectorNewtonRaphson.h"
+#include <memory>
 
+#include "VectorNewtonRaphson.h"
+#include "MatrixSolver.h"
+#include "GEFullMatParPv.h"
+
+using namespace MbD;
 void MbD::VectorNewtonRaphson::run()
 {
-		//self preRun.
-		//self initializeLocally.
-		//self initializeGlobally.
-		//self iterate.
-		//self postRun.
+		this->preRun();
+		this->initializeLocally();
+		this->initializeGlobally();
+		this->iterate();
+		this->postRun();
+}
+
+std::shared_ptr<MatrixSolver> MbD::VectorNewtonRaphson::matrixSolverClassNew()
+{
+	return std::make_shared<GEFullMatParPv>();
 }
diff --git a/MbDCode/VectorNewtonRaphson.h b/MbDCode/VectorNewtonRaphson.h
index a406b8c..2c52ddf 100644
--- a/MbDCode/VectorNewtonRaphson.h
+++ b/MbDCode/VectorNewtonRaphson.h
@@ -9,7 +9,7 @@ namespace MbD {
         //matrixSolver n
     public:
         void run() override;
-
+        virtual std::shared_ptr<MatrixSolver> matrixSolverClassNew();
         std::shared_ptr<MatrixSolver> matrixSolver;
         int n;
     };
diff --git a/MbDCode/ZRotation.cpp b/MbDCode/ZRotation.cpp
index 5fb2e60..98d8069 100644
--- a/MbDCode/ZRotation.cpp
+++ b/MbDCode/ZRotation.cpp
@@ -4,6 +4,7 @@
 #include "DirectionCosineConstraintIJ.h"
 #include "EndFrameqc.h"
 #include "EndFrameqct.h"
+#include "CREATE.h"
 
 using namespace MbD;
 
@@ -17,15 +18,9 @@ ZRotation::ZRotation(const char* str) : PrescribedMotion(str) {
 
 void 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);
+		auto dirCosCon = CREATE<DirectionCosineConstraintIJ>::ConstraintWith(frmI, frmJ, 1, 0);
 		addConstraint(dirCosCon);
 		System::getInstance().hasChanged = true;
 	}
@@ -36,12 +31,8 @@ void ZRotation::initializeGlobally()
 
 void 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 ZRotation::addConstraint(std::shared_ptr<Constraint> con)
-{
+	auto xyzBlkList = std::initializer_list<Symsptr>{ xBlk, yBlk, zBlk };
+	std::static_pointer_cast<EndFrameqct>(frmI)->rmemBlks = (std::make_shared<FullColumn<Symsptr>>(xyzBlkList));
+	auto xyzRotBlkList = std::initializer_list<Symsptr>{ phiBlk, theBlk, psiBlk };
+	std::static_pointer_cast<EndFrameqct>(frmI)->phiThePsiBlks = (std::make_shared<FullColumn<Symsptr>>(xyzRotBlkList));
 }
diff --git a/MbDCode/ZRotation.h b/MbDCode/ZRotation.h
index a47be28..e52252c 100644
--- a/MbDCode/ZRotation.h
+++ b/MbDCode/ZRotation.h
@@ -6,12 +6,10 @@ namespace MbD {
 	{
 		//
 	public:
-		static std::shared_ptr<ZRotation> Create(const char* name);
 		ZRotation();
 		ZRotation(const char* str);
 		void initializeGlobally() override;
 		void initMotions();
-		void addConstraint(std::shared_ptr<Constraint> con);
 	};
 }