diff --git a/MbDCode/AbsConstraint.cpp b/MbDCode/AbsConstraint.cpp
index 030db5f..94ad287 100644
--- a/MbDCode/AbsConstraint.cpp
+++ b/MbDCode/AbsConstraint.cpp
@@ -2,17 +2,18 @@
 
 using namespace MbD;
 
-AbsConstraint::AbsConstraint()
+std::shared_ptr<AbsConstraint> MbD::AbsConstraint::Create(const char* name)
 {
-    initialize();
+	auto item = std::make_shared<AbsConstraint>(name);
+	item->initialize();
+	return item;
 }
 
-AbsConstraint::AbsConstraint(const char* str) : Constraint(str)
-{
-    initialize();
-}
+AbsConstraint::AbsConstraint() {}
 
-MbD::AbsConstraint::AbsConstraint(int i)
+AbsConstraint::AbsConstraint(const char* str) : Constraint(str) {}
+
+AbsConstraint::AbsConstraint(int i)
 {
     axis = i;
 }
@@ -22,3 +23,7 @@ void AbsConstraint::initialize()
     axis = 0;
     iqXminusOnePlusAxis = 0;
 }
+
+void MbD::AbsConstraint::calcPostDynCorrectorIteration()
+{
+}
diff --git a/MbDCode/AbsConstraint.h b/MbDCode/AbsConstraint.h
index 29551c7..f2923ee 100644
--- a/MbDCode/AbsConstraint.h
+++ b/MbDCode/AbsConstraint.h
@@ -5,10 +5,13 @@ namespace MbD {
     {
         //axis iqXminusOnePlusAxis 
     public:
+        static std::shared_ptr<AbsConstraint> Create(const char* name);
         AbsConstraint();
         AbsConstraint(const char* str);
         AbsConstraint(int axis);
         void initialize();
+        void calcPostDynCorrectorIteration() override;
+
         int axis;
         int iqXminusOnePlusAxis;
     };
diff --git a/MbDCode/AnyPosICNewtonRaphson.cpp b/MbDCode/AnyPosICNewtonRaphson.cpp
new file mode 100644
index 0000000..e69de29
diff --git a/MbDCode/AnyPosICNewtonRaphson.h b/MbDCode/AnyPosICNewtonRaphson.h
new file mode 100644
index 0000000..2f91791
--- /dev/null
+++ b/MbDCode/AnyPosICNewtonRaphson.h
@@ -0,0 +1,15 @@
+#pragma once
+
+#include "PosNewtonRaphson.h"
+
+namespace MbD {
+    class AnyPosICNewtonRaphson : public PosNewtonRaphson
+    {
+        //nqsu qsuOld qsuWeights nSingularMatrixError 
+    public:
+        int nqsu;
+        std::shared_ptr<FullColumn<double>> qsuOld, qsuWeights;
+        int nSingularMatrixError;
+    };
+}
+
diff --git a/MbDCode/AtPointConstraintIJ.cpp b/MbDCode/AtPointConstraintIJ.cpp
new file mode 100644
index 0000000..c4758b1
--- /dev/null
+++ b/MbDCode/AtPointConstraintIJ.cpp
@@ -0,0 +1,43 @@
+#include "AtPointConstraintIJ.h"
+#include "DispCompIecJecO.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)
+{
+}
+
+void AtPointConstraintIJ::initialize()
+{
+}
+
+void MbD::AtPointConstraintIJ::initializeLocally()
+{
+	riIeJeO->initializeLocally();
+}
+
+void MbD::AtPointConstraintIJ::initializeGlobally()
+{
+	riIeJeO->initializeGlobally();
+}
+
+void AtPointConstraintIJ::initriIeJeO()
+{
+	riIeJeO = std::make_shared<DispCompIecJecO>();
+}
+
+void MbD::AtPointConstraintIJ::postInput()
+{
+}
+
+void MbD::AtPointConstraintIJ::calcPostDynCorrectorIteration()
+{
+}
diff --git a/MbDCode/AtPointConstraintIJ.h b/MbDCode/AtPointConstraintIJ.h
new file mode 100644
index 0000000..bc05668
--- /dev/null
+++ b/MbDCode/AtPointConstraintIJ.h
@@ -0,0 +1,25 @@
+#pragma once
+
+#include "ConstraintIJ.h"
+
+namespace MbD {
+    class DispCompIecJecO;
+
+    class AtPointConstraintIJ : public ConstraintIJ
+    {
+        //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;
+        void initializeGlobally() override;
+        virtual void initriIeJeO();
+        void postInput() override;
+        void calcPostDynCorrectorIteration() override;
+
+        int axis;
+        std::shared_ptr<DispCompIecJecO> riIeJeO;
+    };
+}
+
diff --git a/MbDCode/AtPointConstraintIqcJc.cpp b/MbDCode/AtPointConstraintIqcJc.cpp
new file mode 100644
index 0000000..0578a64
--- /dev/null
+++ b/MbDCode/AtPointConstraintIqcJc.cpp
@@ -0,0 +1,22 @@
+#include "AtPointConstraintIqcJc.h"
+#include "DispCompIeqcJecO.h"
+
+using namespace MbD;
+
+AtPointConstraintIqcJc::AtPointConstraintIqcJc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi) :
+	AtPointConstraintIJ(frmi, frmj, axisi)
+{
+}
+
+void AtPointConstraintIqcJc::initialize()
+{
+}
+
+void AtPointConstraintIqcJc::initriIeJeO()
+{
+	riIeJeO = std::make_shared<DispCompIeqcJecO>();
+}
+
+void MbD::AtPointConstraintIqcJc::calcPostDynCorrectorIteration()
+{
+}
diff --git a/MbDCode/AtPointConstraintIqcJc.h b/MbDCode/AtPointConstraintIqcJc.h
new file mode 100644
index 0000000..0c9fe5b
--- /dev/null
+++ b/MbDCode/AtPointConstraintIqcJc.h
@@ -0,0 +1,20 @@
+#pragma once
+
+#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;
+
+        FRowDsptr pGpEI;
+        FMatDsptr ppGpEIpEI;
+        int iqXIminusOnePlusAxis, iqEI;
+    };
+}
+
diff --git a/MbDCode/AtPointConstraintIqcJqc.cpp b/MbDCode/AtPointConstraintIqcJqc.cpp
new file mode 100644
index 0000000..a9467c8
--- /dev/null
+++ b/MbDCode/AtPointConstraintIqcJqc.cpp
@@ -0,0 +1,22 @@
+#include "AtPointConstraintIqcJqc.h"
+#include "DispCompIeqcJeqcO.h"
+
+using namespace MbD;
+
+AtPointConstraintIqcJqc::AtPointConstraintIqcJqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi) :
+	AtPointConstraintIqcJc(frmi, frmj, axisi)
+{
+}
+
+void AtPointConstraintIqcJqc::initialize()
+{
+}
+
+void AtPointConstraintIqcJqc::initriIeJeO()
+{
+	riIeJeO = std::make_shared<DispCompIeqcJeqcO>();
+}
+
+void MbD::AtPointConstraintIqcJqc::calcPostDynCorrectorIteration()
+{
+}
diff --git a/MbDCode/AtPointConstraintIqcJqc.h b/MbDCode/AtPointConstraintIqcJqc.h
new file mode 100644
index 0000000..6f4a79b
--- /dev/null
+++ b/MbDCode/AtPointConstraintIqcJqc.h
@@ -0,0 +1,20 @@
+#pragma once
+
+#include "AtPointConstraintIqcJc.h"
+
+namespace MbD {
+    class AtPointConstraintIqcJqc : public AtPointConstraintIqcJc
+    {
+        //pGpEJ ppGpEJpEJ iqXJminusOnePlusAxis iqEJ 
+    public:
+        AtPointConstraintIqcJqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi);
+        void initialize();
+        void initriIeJeO() override;
+        void calcPostDynCorrectorIteration() override;
+
+        FRowDsptr pGpEJ;
+        FMatDsptr ppGpEJpEJ;
+        int iqXJminusOnePlusAxis, iqEJ;
+    };
+}
+
diff --git a/MbDCode/AtPointConstraintIqctJqc.cpp b/MbDCode/AtPointConstraintIqctJqc.cpp
new file mode 100644
index 0000000..d5ef975
--- /dev/null
+++ b/MbDCode/AtPointConstraintIqctJqc.cpp
@@ -0,0 +1,22 @@
+#include "AtPointConstraintIqctJqc.h"
+#include "DispCompIeqctJeqcO.h"
+
+using namespace MbD;
+
+AtPointConstraintIqctJqc::AtPointConstraintIqctJqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi) :
+	AtPointConstraintIqcJqc(frmi, frmj, axisi)
+{
+}
+
+void AtPointConstraintIqctJqc::initialize()
+{
+}
+
+void AtPointConstraintIqctJqc::initriIeJeO()
+{
+	riIeJeO = std::make_shared<DispCompIeqctJeqcO>();
+}
+
+void MbD::AtPointConstraintIqctJqc::calcPostDynCorrectorIteration()
+{
+}
diff --git a/MbDCode/AtPointConstraintIqctJqc.h b/MbDCode/AtPointConstraintIqctJqc.h
new file mode 100644
index 0000000..9412e4f
--- /dev/null
+++ b/MbDCode/AtPointConstraintIqctJqc.h
@@ -0,0 +1,20 @@
+#pragma once
+
+#include "AtPointConstraintIqcJqc.h"
+
+namespace MbD {
+    class AtPointConstraintIqctJqc : public AtPointConstraintIqcJqc
+    {
+        //pGpt ppGpEIpt ppGptpt 
+    public:
+        AtPointConstraintIqctJqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi);
+        void initialize();
+        void initriIeJeO() override;
+        void calcPostDynCorrectorIteration() override;
+
+        double pGpt;
+        FRowDsptr ppGpEIpt;
+        double ppGptpt;
+    };
+}
+
diff --git a/MbDCode/BasicIntegrator.cpp b/MbDCode/BasicIntegrator.cpp
new file mode 100644
index 0000000..26b6e87
--- /dev/null
+++ b/MbDCode/BasicIntegrator.cpp
@@ -0,0 +1 @@
+#include "BasicIntegrator.h"
diff --git a/MbDCode/BasicIntegrator.h b/MbDCode/BasicIntegrator.h
new file mode 100644
index 0000000..3914a6b
--- /dev/null
+++ b/MbDCode/BasicIntegrator.h
@@ -0,0 +1,13 @@
+#pragma once
+
+#include "Integrator.h"
+
+namespace MbD {
+    class BasicIntegrator : public Integrator
+    {
+        //
+    public:
+
+    };
+}
+
diff --git a/MbDCode/BasicQuasiIntegrator.cpp b/MbDCode/BasicQuasiIntegrator.cpp
new file mode 100644
index 0000000..5d408ea
--- /dev/null
+++ b/MbDCode/BasicQuasiIntegrator.cpp
@@ -0,0 +1 @@
+#include "BasicQuasiIntegrator.h"
diff --git a/MbDCode/BasicQuasiIntegrator.h b/MbDCode/BasicQuasiIntegrator.h
new file mode 100644
index 0000000..6b8f0d8
--- /dev/null
+++ b/MbDCode/BasicQuasiIntegrator.h
@@ -0,0 +1,13 @@
+#pragma once
+
+#include "BasicIntegrator.h"
+
+namespace MbD {
+    class BasicQuasiIntegrator : public BasicIntegrator
+    {
+        //
+    public:
+
+    };
+}
+
diff --git a/MbDCode/CREATE.cpp b/MbDCode/CREATE.cpp
new file mode 100644
index 0000000..5a4110e
--- /dev/null
+++ b/MbDCode/CREATE.cpp
@@ -0,0 +1 @@
+#include "CREATE.h"
diff --git a/MbDCode/CREATE.h b/MbDCode/CREATE.h
new file mode 100644
index 0000000..2166532
--- /dev/null
+++ b/MbDCode/CREATE.h
@@ -0,0 +1,21 @@
+#pragma once
+#include <memory>
+
+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;
+        }
+    };
+}
+
diff --git a/MbDCode/Constant.cpp b/MbDCode/Constant.cpp
index 8f7cbc5..a25769a 100644
--- a/MbDCode/Constant.cpp
+++ b/MbDCode/Constant.cpp
@@ -10,7 +10,7 @@ Constant::Constant(double val) : Variable(val)
 {
 }
 
-std::shared_ptr<Symbolic> MbD::Constant::differentiateWRT(std::shared_ptr<Symbolic> var)
+std::shared_ptr<Symbolic> Constant::differentiateWRT(std::shared_ptr<Symbolic> var)
 {
 	return std::make_shared<Constant>(0.0);
 }
diff --git a/MbDCode/Constraint.cpp b/MbDCode/Constraint.cpp
index 377fd05..403d87f 100644
--- a/MbDCode/Constraint.cpp
+++ b/MbDCode/Constraint.cpp
@@ -5,12 +5,10 @@ using namespace MbD;
 
 Constraint::Constraint()
 {
-	initialize();
 }
 
 Constraint::Constraint(const char* str) : Item(str)
 {
-	initialize();
 }
 
 void Constraint::initialize()
@@ -20,6 +18,10 @@ void Constraint::initialize()
 	lam = 0.0;
 }
 
+void MbD::Constraint::postInput()
+{
+}
+
 void Constraint::setOwner(Item* x)
 {
 	owner = x;
diff --git a/MbDCode/Constraint.h b/MbDCode/Constraint.h
index eef8945..23c6f95 100644
--- a/MbDCode/Constraint.h
+++ b/MbDCode/Constraint.h
@@ -11,6 +11,7 @@ namespace MbD {
 		Constraint();
 		Constraint(const char* str);
 		void initialize();
+		void postInput() override;
 		void setOwner(Item* x);
 		Item* getOwner();
 		
diff --git a/MbDCode/ConstraintIJ.cpp b/MbDCode/ConstraintIJ.cpp
index e065d91..ad2fae6 100644
--- a/MbDCode/ConstraintIJ.cpp
+++ b/MbDCode/ConstraintIJ.cpp
@@ -3,7 +3,12 @@
 
 using namespace MbD;
 
-MbD::ConstraintIJ::ConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj) : frmI(frmi), frmJ(frmj)
+ConstraintIJ::ConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj) : frmI(frmi), frmJ(frmj)
+{
+	aConstant = 0.0;
+}
+
+void ConstraintIJ::initialize()
 {
 	aConstant = 0.0;
 }
diff --git a/MbDCode/ConstraintIJ.h b/MbDCode/ConstraintIJ.h
index 12ffeff..77059b1 100644
--- a/MbDCode/ConstraintIJ.h
+++ b/MbDCode/ConstraintIJ.h
@@ -1,8 +1,7 @@
 #pragma once
 
-//#include "typedef.h"
 #include "Constraint.h"
-#include "EndFramec.h"
+#include "EndFramec.h"  //EndFrmcptr is defined
 
 namespace MbD {
     class ConstraintIJ : public Constraint
@@ -10,6 +9,7 @@ namespace MbD {
         //frmI frmJ aConstant 
     public:
         ConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj);
+        void initialize();
 
         EndFrmcptr frmI, frmJ;
         double aConstant;
diff --git a/MbDCode/CylindricalJoint.cpp b/MbDCode/CylindricalJoint.cpp
index 80af383..5e90d42 100644
--- a/MbDCode/CylindricalJoint.cpp
+++ b/MbDCode/CylindricalJoint.cpp
@@ -1,15 +1,27 @@
 #include "CylindricalJoint.h"
+#include "System.h"
+#include "DirectionCosineConstraintIJ.h"
+#include "TranslationConstraintIJ.h"
 
 using namespace MbD;
 
 CylindricalJoint::CylindricalJoint() {
-	initialize();
 }
 
 CylindricalJoint::CylindricalJoint(const char* str) : Joint(str) {
-	initialize();
 }
 
-void CylindricalJoint::initialize()
+void MbD::CylindricalJoint::initializeGlobally()
 {
+	if (!constraints)
+	{
+		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));
+		System::getInstance().hasChanged = true;
+	}
+	else {
+		Joint::initializeGlobally();
+	}
 }
diff --git a/MbDCode/CylindricalJoint.h b/MbDCode/CylindricalJoint.h
index 5901fe5..5811e28 100644
--- a/MbDCode/CylindricalJoint.h
+++ b/MbDCode/CylindricalJoint.h
@@ -6,9 +6,10 @@ namespace MbD {
     {
         //frmI frmJ constraints friction 
     public:
+        static std::shared_ptr<CylindricalJoint> Create(const char* name);
         CylindricalJoint();
         CylindricalJoint(const char* str);
-        void initialize();
+        void initializeGlobally();
     };
 }
 
diff --git a/MbDCode/DiagonalMatrix.h b/MbDCode/DiagonalMatrix.h
index b2b5bc8..f25ec35 100644
--- a/MbDCode/DiagonalMatrix.h
+++ b/MbDCode/DiagonalMatrix.h
@@ -4,6 +4,10 @@ namespace MbD {
     template <typename T>
     class DiagonalMatrix : public Array<T>
     {
+        //
+    public:
+        DiagonalMatrix(size_t count) : Array<T>(count) {}
+        DiagonalMatrix(std::initializer_list<T> list) : Array<T>{ list } {}
     };
 }
 
diff --git a/MbDCode/DirectionCosineConstraintIJ.cpp b/MbDCode/DirectionCosineConstraintIJ.cpp
index bb544bb..daa24ce 100644
--- a/MbDCode/DirectionCosineConstraintIJ.cpp
+++ b/MbDCode/DirectionCosineConstraintIJ.cpp
@@ -1,10 +1,44 @@
 #include "DirectionCosineConstraintIJ.h"
+#include "DirectionCosineIecJec.h"
 #include "EndFramec.h"
 
 using namespace MbD;
 
-DirectionCosineConstraintIJ::DirectionCosineConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj) : 
-	ConstraintIJ(frmI, frmJ), axisI(axisi), axisJ(axisj)
+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)
+{
+}
+
+void DirectionCosineConstraintIJ::initialize()
+{
+}
+
+void MbD::DirectionCosineConstraintIJ::initializeLocally()
+{
+	aAijIeJe->initializeLocally();
+}
+
+void MbD::DirectionCosineConstraintIJ::initializeGlobally()
+{
+	aAijIeJe->initializeGlobally();
+}
+
+void DirectionCosineConstraintIJ::initaAijIeJe()
+{
+	aAijIeJe = std::make_shared<DirectionCosineIecJec>();
+}
+
+void MbD::DirectionCosineConstraintIJ::postInput()
+{
+}
+
+void MbD::DirectionCosineConstraintIJ::calcPostDynCorrectorIteration()
 {
-	aAijIeJe = std::make_shared<DirectionCosineIecJec>(frmI, frmJ, axisI, axisJ);
 }
diff --git a/MbDCode/DirectionCosineConstraintIJ.h b/MbDCode/DirectionCosineConstraintIJ.h
index 0a0f330..62c805d 100644
--- a/MbDCode/DirectionCosineConstraintIJ.h
+++ b/MbDCode/DirectionCosineConstraintIJ.h
@@ -1,15 +1,22 @@
 #pragma once
 
 #include "ConstraintIJ.h"
-#include "DirectionCosineIecJec.h"
 
 namespace MbD {
+    class DirectionCosineIecJec;
+
     class DirectionCosineConstraintIJ : public ConstraintIJ
     {
         //axisI axisJ aAijIeJe 
     public:
-        //	self owns : (MbDDirectionCosineConstraintIJ withFrmI : frmI frmJ : frmJ axisI : 2 axisJ : 1).
-        DirectionCosineConstraintIJ(EndFrmcptr frmI, EndFrmcptr frmJ, int axisI, int axisJ);
+        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;
+        void initializeGlobally() override;
+        virtual void initaAijIeJe();
+        void postInput() override;
+        void calcPostDynCorrectorIteration() override;
 
         int axisI, axisJ;
         std::shared_ptr<DirectionCosineIecJec> aAijIeJe;
diff --git a/MbDCode/DirectionCosineConstraintIqcJc.cpp b/MbDCode/DirectionCosineConstraintIqcJc.cpp
new file mode 100644
index 0000000..ca2caea
--- /dev/null
+++ b/MbDCode/DirectionCosineConstraintIqcJc.cpp
@@ -0,0 +1,22 @@
+#include "DirectionCosineConstraintIqcJc.h"
+#include "DirectionCosineIeqcJec.h"
+
+using namespace MbD;
+
+DirectionCosineConstraintIqcJc::DirectionCosineConstraintIqcJc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj) :
+	DirectionCosineConstraintIJ(frmi, frmj, axisi, axisj)
+{
+}
+
+void DirectionCosineConstraintIqcJc::initialize()
+{
+}
+
+void DirectionCosineConstraintIqcJc::initaAijIeJe()
+{
+	aAijIeJe = std::make_shared<DirectionCosineIeqcJec>();
+}
+
+void MbD::DirectionCosineConstraintIqcJc::calcPostDynCorrectorIteration()
+{
+}
diff --git a/MbDCode/DirectionCosineConstraintIqcJc.h b/MbDCode/DirectionCosineConstraintIqcJc.h
new file mode 100644
index 0000000..28f1783
--- /dev/null
+++ b/MbDCode/DirectionCosineConstraintIqcJc.h
@@ -0,0 +1,20 @@
+#pragma once
+
+#include "DirectionCosineConstraintIJ.h"
+
+namespace MbD {
+    class DirectionCosineConstraintIqcJc : public DirectionCosineConstraintIJ
+    {
+        //pGpEI ppGpEIpEI iqEI 
+    public:
+        DirectionCosineConstraintIqcJc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj);
+        void initialize();
+        void initaAijIeJe() override;
+        void calcPostDynCorrectorIteration() override;
+
+        FRowDsptr pGpEI;
+        FMatDsptr ppGpEIpEI;
+        int iqEI;
+    };
+}
+
diff --git a/MbDCode/DirectionCosineConstraintIqcJqc.cpp b/MbDCode/DirectionCosineConstraintIqcJqc.cpp
new file mode 100644
index 0000000..7391e0e
--- /dev/null
+++ b/MbDCode/DirectionCosineConstraintIqcJqc.cpp
@@ -0,0 +1,22 @@
+#include "DirectionCosineConstraintIqcJqc.h"
+#include "DirectionCosineIeqcJeqc.h"
+
+using namespace MbD;
+
+DirectionCosineConstraintIqcJqc::DirectionCosineConstraintIqcJqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj) :
+	DirectionCosineConstraintIqcJc(frmi, frmj, axisi, axisj)
+{
+}
+
+void DirectionCosineConstraintIqcJqc::initialize()
+{
+}
+
+void DirectionCosineConstraintIqcJqc::initaAijIeJe()
+{
+	aAijIeJe = std::make_shared<DirectionCosineIeqcJeqc>();
+}
+
+void MbD::DirectionCosineConstraintIqcJqc::calcPostDynCorrectorIteration()
+{
+}
diff --git a/MbDCode/DirectionCosineConstraintIqcJqc.h b/MbDCode/DirectionCosineConstraintIqcJqc.h
new file mode 100644
index 0000000..57f1eb5
--- /dev/null
+++ b/MbDCode/DirectionCosineConstraintIqcJqc.h
@@ -0,0 +1,21 @@
+#pragma once
+
+#include "DirectionCosineConstraintIqcJc.h"
+
+namespace MbD {
+    class DirectionCosineConstraintIqcJqc : public DirectionCosineConstraintIqcJc
+    {
+        //pGpEJ ppGpEIpEJ ppGpEJpEJ iqEJ 
+    public:
+        DirectionCosineConstraintIqcJqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj);
+        void initialize();
+        void initaAijIeJe() override;
+        void calcPostDynCorrectorIteration() override;
+
+        FRowDsptr pGpEJ;
+        FMatDsptr ppGpEIpEJ;
+        FMatDsptr ppGpEJpEJ;
+        int iqEJ;
+    };
+}
+
diff --git a/MbDCode/DirectionCosineConstraintIqctJqc.cpp b/MbDCode/DirectionCosineConstraintIqctJqc.cpp
new file mode 100644
index 0000000..57fb084
--- /dev/null
+++ b/MbDCode/DirectionCosineConstraintIqctJqc.cpp
@@ -0,0 +1,22 @@
+#include "DirectionCosineConstraintIqctJqc.h"
+#include "DirectionCosineIeqctJeqc.h"
+
+using namespace MbD;
+
+DirectionCosineConstraintIqctJqc::DirectionCosineConstraintIqctJqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj) :
+	DirectionCosineConstraintIqcJqc(frmi, frmj, axisi, axisj)
+{
+}
+
+void DirectionCosineConstraintIqctJqc::initialize()
+{
+}
+
+void DirectionCosineConstraintIqctJqc::initaAijIeJe()
+{
+	aAijIeJe = std::make_shared<DirectionCosineIeqctJeqc>();
+}
+
+void MbD::DirectionCosineConstraintIqctJqc::calcPostDynCorrectorIteration()
+{
+}
diff --git a/MbDCode/DirectionCosineConstraintIqctJqc.h b/MbDCode/DirectionCosineConstraintIqctJqc.h
new file mode 100644
index 0000000..3283486
--- /dev/null
+++ b/MbDCode/DirectionCosineConstraintIqctJqc.h
@@ -0,0 +1,20 @@
+#pragma once
+
+#include "DirectionCosineConstraintIqcJqc.h"
+
+namespace MbD {
+	class DirectionCosineConstraintIqctJqc : public DirectionCosineConstraintIqcJqc
+	{
+		//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;
+		FRowDsptr ppGpEJpt;
+		double ppGptpt;
+	};
+}
diff --git a/MbDCode/DirectionCosineIecJec.cpp b/MbDCode/DirectionCosineIecJec.cpp
index b84a0c6..16f2623 100644
--- a/MbDCode/DirectionCosineIecJec.cpp
+++ b/MbDCode/DirectionCosineIecJec.cpp
@@ -5,14 +5,23 @@
 
 using namespace MbD;
 
-MbD::DirectionCosineIecJec::DirectionCosineIecJec()
+DirectionCosineIecJec::DirectionCosineIecJec()
 {
 }
 
-MbD::DirectionCosineIecJec::DirectionCosineIecJec(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj) :
+DirectionCosineIecJec::DirectionCosineIecJec(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj) :
 	KinematicIeJe(frmi, frmj), axisI(axisi), axisJ(axisj)
+{
+
+}
+
+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()
+{
+}
diff --git a/MbDCode/DirectionCosineIecJec.h b/MbDCode/DirectionCosineIecJec.h
index d76a874..25a044b 100644
--- a/MbDCode/DirectionCosineIecJec.h
+++ b/MbDCode/DirectionCosineIecJec.h
@@ -2,16 +2,19 @@
 #include <memory>
 
 #include "KinematicIeJe.h"
-//#include "EndFramec.h"
-#include "FullColumn.h"
 
 namespace MbD {
+    template<typename T>
+    class FullColumn;
+
     class DirectionCosineIecJec : public KinematicIeJe
     {
         //aAijIeJe axisI axisJ aAjOIe aAjOJe 
     public:
         DirectionCosineIecJec();
-        DirectionCosineIecJec(EndFrmcptr frmI, EndFrmcptr frmJ, int axisI, int axisJ);
+        DirectionCosineIecJec(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj);
+        void initialize();
+        void calcPostDynCorrectorIteration() override;
 
         int axisI, axisJ;
         double aAijIeJe;
diff --git a/MbDCode/DirectionCosineIeqcJec.cpp b/MbDCode/DirectionCosineIeqcJec.cpp
index ead28b8..ff6048e 100644
--- a/MbDCode/DirectionCosineIeqcJec.cpp
+++ b/MbDCode/DirectionCosineIeqcJec.cpp
@@ -1,14 +1,28 @@
 #include "DirectionCosineIeqcJec.h"
+#include "EndFrameqc.h"
 
 using namespace MbD;
 
-MbD::DirectionCosineIeqcJec::DirectionCosineIeqcJec()
+DirectionCosineIeqcJec::DirectionCosineIeqcJec()
 {
-	initialize();
 }
 
-void MbD::DirectionCosineIeqcJec::initialize()
+DirectionCosineIeqcJec::DirectionCosineIeqcJec(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj) :
+	DirectionCosineIecJec(frmi, frmj, axisi, axisj)
+{
+}
+
+void DirectionCosineIeqcJec::initialize()
 {
 	pAijIeJepEI = std::make_shared<FullRow<double>>(4);
 	ppAijIeJepEIpEI = std::make_shared<FullMatrix<double>>(4, 4);
 }
+
+void MbD::DirectionCosineIeqcJec::initializeGlobally()
+{
+	ppAjOIepEIpEI = std::static_pointer_cast<EndFrameqc>(frmI)->ppAjOepEpE(axisI);
+}
+
+void MbD::DirectionCosineIeqcJec::calcPostDynCorrectorIteration()
+{
+}
diff --git a/MbDCode/DirectionCosineIeqcJec.h b/MbDCode/DirectionCosineIeqcJec.h
index 59f1446..3eb5e21 100644
--- a/MbDCode/DirectionCosineIeqcJec.h
+++ b/MbDCode/DirectionCosineIeqcJec.h
@@ -8,7 +8,10 @@ namespace MbD {
         //pAijIeJepEI ppAijIeJepEIpEI pAjOIepEIT ppAjOIepEIpEI 
     public:
         DirectionCosineIeqcJec();
+        DirectionCosineIeqcJec(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj);
         void initialize();
+        void initializeGlobally();
+        void calcPostDynCorrectorIteration() override;
 
         FRowDsptr pAijIeJepEI;
         FMatDsptr ppAijIeJepEIpEI;
diff --git a/MbDCode/DirectionCosineIeqcJeqc.cpp b/MbDCode/DirectionCosineIeqcJeqc.cpp
index ba59a9c..656ce0a 100644
--- a/MbDCode/DirectionCosineIeqcJeqc.cpp
+++ b/MbDCode/DirectionCosineIeqcJeqc.cpp
@@ -1,15 +1,30 @@
 #include "DirectionCosineIeqcJeqc.h"
+#include "EndFrameqc.h"
 
 using namespace MbD;
 
-MbD::DirectionCosineIeqcJeqc::DirectionCosineIeqcJeqc()
+DirectionCosineIeqcJeqc::DirectionCosineIeqcJeqc()
 {
-	initialize();
 }
 
-void MbD::DirectionCosineIeqcJeqc::initialize()
+DirectionCosineIeqcJeqc::DirectionCosineIeqcJeqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj) :
+	DirectionCosineIeqcJec(frmi, frmj, axisi, axisj)
+{
+}
+
+void DirectionCosineIeqcJeqc::initialize()
 {
 	pAijIeJepEJ = std::make_shared<FullRow<double>>(4);
 	ppAijIeJepEIpEJ = std::make_shared<FullMatrix<double>>(4, 4);
 	ppAijIeJepEJpEJ = std::make_shared<FullMatrix<double>>(4, 4);
 }
+
+void MbD::DirectionCosineIeqcJeqc::initializeGlobally()
+{
+	DirectionCosineIeqcJec::initializeGlobally();
+	ppAjOJepEJpEJ = std::static_pointer_cast<EndFrameqc>(frmJ)->ppAjOepEpE(axisJ);
+}
+
+void MbD::DirectionCosineIeqcJeqc::calcPostDynCorrectorIteration()
+{
+}
diff --git a/MbDCode/DirectionCosineIeqcJeqc.h b/MbDCode/DirectionCosineIeqcJeqc.h
index 239a3fa..f3c6546 100644
--- a/MbDCode/DirectionCosineIeqcJeqc.h
+++ b/MbDCode/DirectionCosineIeqcJeqc.h
@@ -8,7 +8,10 @@ namespace MbD {
         //pAijIeJepEJ ppAijIeJepEIpEJ ppAijIeJepEJpEJ pAjOJepEJT ppAjOJepEJpEJ 
     public:
         DirectionCosineIeqcJeqc();
+        DirectionCosineIeqcJeqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj);
         void initialize();
+        void initializeGlobally();
+        void calcPostDynCorrectorIteration() override;
 
         FRowDsptr pAijIeJepEJ;
         FMatDsptr ppAijIeJepEIpEJ;
diff --git a/MbDCode/DirectionCosineIeqctJeqc.cpp b/MbDCode/DirectionCosineIeqctJeqc.cpp
index de992ec..ae39837 100644
--- a/MbDCode/DirectionCosineIeqctJeqc.cpp
+++ b/MbDCode/DirectionCosineIeqctJeqc.cpp
@@ -1,14 +1,30 @@
 #include "DirectionCosineIeqctJeqc.h"
+#include "EndFrameqc.h"
 
 using namespace MbD;
 
-MbD::DirectionCosineIeqctJeqc::DirectionCosineIeqctJeqc()
+DirectionCosineIeqctJeqc::DirectionCosineIeqctJeqc()
 {
-	initialize();
+
 }
 
-void MbD::DirectionCosineIeqctJeqc::initialize()
+DirectionCosineIeqctJeqc::DirectionCosineIeqctJeqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj) :
+	DirectionCosineIeqcJeqc(frmi, frmj, axisi, axisj)
+{
+
+}
+
+void DirectionCosineIeqctJeqc::initialize()
 {
 	ppAijIeJepEIpt = std::make_shared<FullRow<double>>(4);
 	ppAijIeJepEJpt = std::make_shared<FullRow<double>>(4);
 }
+
+void MbD::DirectionCosineIeqctJeqc::initializeGlobally()
+{
+	ppAjOJepEJpEJ = std::static_pointer_cast<EndFrameqc>(frmJ)->ppAjOepEpE(axisJ);
+}
+
+void MbD::DirectionCosineIeqctJeqc::calcPostDynCorrectorIteration()
+{
+}
diff --git a/MbDCode/DirectionCosineIeqctJeqc.h b/MbDCode/DirectionCosineIeqctJeqc.h
index c730035..5327c45 100644
--- a/MbDCode/DirectionCosineIeqctJeqc.h
+++ b/MbDCode/DirectionCosineIeqctJeqc.h
@@ -8,7 +8,10 @@ namespace MbD {
         //pAijIeJept ppAijIeJepEIpt ppAijIeJepEJpt ppAijIeJeptpt 
     public:
         DirectionCosineIeqctJeqc();
+        DirectionCosineIeqctJeqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj);
         void initialize();
+        void initializeGlobally();
+        void calcPostDynCorrectorIteration() override;
 
         double pAijIeJept;
         FRowDsptr ppAijIeJepEIpt;
diff --git a/MbDCode/DispCompIecJecKec.cpp b/MbDCode/DispCompIecJecKec.cpp
new file mode 100644
index 0000000..8b94427
--- /dev/null
+++ b/MbDCode/DispCompIecJecKec.cpp
@@ -0,0 +1 @@
+#include "DispCompIecJecKec.h"
diff --git a/MbDCode/DispCompIecJecKec.h b/MbDCode/DispCompIecJecKec.h
new file mode 100644
index 0000000..3ac0318
--- /dev/null
+++ b/MbDCode/DispCompIecJecKec.h
@@ -0,0 +1,18 @@
+#pragma once
+
+#include "KinematicIeJe.h"
+
+namespace MbD {
+    class DispCompIecJecKec : public KinematicIeJe
+    {
+        //efrmK axisK riIeJeKe aAjOKe rIeJeO 
+    public:
+
+        EndFrmcptr efrmK;
+        int axisK;
+        double riIeJeKe;
+        FColDsptr aAjOKe;
+        FColDsptr rIeJeO;
+    };
+}
+
diff --git a/MbDCode/DispCompIecJecKeqc.cpp b/MbDCode/DispCompIecJecKeqc.cpp
new file mode 100644
index 0000000..4b9eac9
--- /dev/null
+++ b/MbDCode/DispCompIecJecKeqc.cpp
@@ -0,0 +1 @@
+#include "DispCompIecJecKeqc.h"
diff --git a/MbDCode/DispCompIecJecKeqc.h b/MbDCode/DispCompIecJecKeqc.h
new file mode 100644
index 0000000..aea94a4
--- /dev/null
+++ b/MbDCode/DispCompIecJecKeqc.h
@@ -0,0 +1,17 @@
+#pragma once
+
+#include "DispCompIecJecKec.h"
+
+namespace MbD {
+    class DispCompIecJecKeqc : public DispCompIecJecKec
+    {
+        //priIeJeKepEK ppriIeJeKepEKpEK pAjOKepEKT ppAjOKepEKpEK 
+    public:
+
+        FRowDsptr priIeJeKepEK;
+        FMatDsptr ppriIeJeKepEKpEK;
+        FMatDsptr pAjOKepEKT;
+        FMatFColDsptr ppAjOKepEKpEK;
+    };
+}
+
diff --git a/MbDCode/DispCompIecJecO.cpp b/MbDCode/DispCompIecJecO.cpp
new file mode 100644
index 0000000..1786e5c
--- /dev/null
+++ b/MbDCode/DispCompIecJecO.cpp
@@ -0,0 +1 @@
+#include "DispCompIecJecO.h"
diff --git a/MbDCode/DispCompIecJecO.h b/MbDCode/DispCompIecJecO.h
new file mode 100644
index 0000000..f7aeda3
--- /dev/null
+++ b/MbDCode/DispCompIecJecO.h
@@ -0,0 +1,15 @@
+#pragma once
+
+#include "KinematicIeJe.h"
+
+namespace MbD {
+	class DispCompIecJecO : public KinematicIeJe
+	{
+		//axis riIeJeO 
+	public:
+
+		int axis;
+		double riIeJeO;
+	};
+}
+
diff --git a/MbDCode/DispCompIeqcJecKeqc.cpp b/MbDCode/DispCompIeqcJecKeqc.cpp
new file mode 100644
index 0000000..1aa90b9
--- /dev/null
+++ b/MbDCode/DispCompIeqcJecKeqc.cpp
@@ -0,0 +1 @@
+#include "DispCompIeqcJecKeqc.h"
diff --git a/MbDCode/DispCompIeqcJecKeqc.h b/MbDCode/DispCompIeqcJecKeqc.h
new file mode 100644
index 0000000..740ea95
--- /dev/null
+++ b/MbDCode/DispCompIeqcJecKeqc.h
@@ -0,0 +1,18 @@
+#pragma once
+
+#include "DispCompIecJecKeqc.h"
+
+namespace MbD {
+    class DispCompIeqcJecKeqc : public DispCompIecJecKeqc
+    {
+        //priIeJeKepXI priIeJeKepEI ppriIeJeKepXIpEK ppriIeJeKepEIpEI ppriIeJeKepEIpEK 
+    public:
+
+        FRowDsptr priIeJeKepXI;
+        FRowDsptr priIeJeKepEI;
+        FMatDsptr ppriIeJeKepXIpEK;
+        FMatDsptr ppriIeJeKepEIpEI;
+        FMatDsptr ppriIeJeKepEIpEK;
+    };
+}
+
diff --git a/MbDCode/DispCompIeqcJecO.cpp b/MbDCode/DispCompIeqcJecO.cpp
new file mode 100644
index 0000000..8eb7ab5
--- /dev/null
+++ b/MbDCode/DispCompIeqcJecO.cpp
@@ -0,0 +1 @@
+#include "DispCompIeqcJecO.h"
diff --git a/MbDCode/DispCompIeqcJecO.h b/MbDCode/DispCompIeqcJecO.h
new file mode 100644
index 0000000..78741fd
--- /dev/null
+++ b/MbDCode/DispCompIeqcJecO.h
@@ -0,0 +1,16 @@
+#pragma once
+
+#include "DispCompIecJecO.h"
+
+namespace MbD {
+    class DispCompIeqcJecO : public DispCompIecJecO
+    {
+        //priIeJeOpXI priIeJeOpEI ppriIeJeOpEIpEI 
+    public:
+
+        FRowDsptr priIeJeOpXI;
+        FRowDsptr priIeJeOpEI;
+        FMatDsptr ppriIeJeOpEIpEI;
+    };
+}
+
diff --git a/MbDCode/DispCompIeqcJeqcKeqc.cpp b/MbDCode/DispCompIeqcJeqcKeqc.cpp
new file mode 100644
index 0000000..2686a3c
--- /dev/null
+++ b/MbDCode/DispCompIeqcJeqcKeqc.cpp
@@ -0,0 +1 @@
+#include "DispCompIeqcJeqcKeqc.h"
diff --git a/MbDCode/DispCompIeqcJeqcKeqc.h b/MbDCode/DispCompIeqcJeqcKeqc.h
new file mode 100644
index 0000000..34e26f9
--- /dev/null
+++ b/MbDCode/DispCompIeqcJeqcKeqc.h
@@ -0,0 +1,18 @@
+#pragma once
+
+#include "DispCompIeqcJecKeqc.h"
+
+namespace MbD {
+    class DispCompIeqcJeqcKeqc : public DispCompIeqcJecKeqc
+    {
+        //priIeJeKepXJ priIeJeKepEJ ppriIeJeKepXJpEK ppriIeJeKepEJpEJ ppriIeJeKepEJpEK 
+    public:
+
+        FRowDsptr priIeJeKepXJ;
+        FRowDsptr priIeJeKepEJ;
+        FMatDsptr ppriIeJeKepXJpEK;
+        FMatDsptr ppriIeJeKepEJpEJ;
+        FMatDsptr ppriIeJeKepEJpEK;
+    };
+}
+
diff --git a/MbDCode/DispCompIeqcJeqcKeqct.cpp b/MbDCode/DispCompIeqcJeqcKeqct.cpp
new file mode 100644
index 0000000..aa1aa54
--- /dev/null
+++ b/MbDCode/DispCompIeqcJeqcKeqct.cpp
@@ -0,0 +1 @@
+#include "DispCompIeqcJeqcKeqct.h"
diff --git a/MbDCode/DispCompIeqcJeqcKeqct.h b/MbDCode/DispCompIeqcJeqcKeqct.h
new file mode 100644
index 0000000..4f1ced2
--- /dev/null
+++ b/MbDCode/DispCompIeqcJeqcKeqct.h
@@ -0,0 +1,20 @@
+#pragma once
+
+#include "DispCompIeqcJeqcKeqc.h"
+
+namespace MbD {
+    class DispCompIeqcJeqcKeqct : public DispCompIeqcJeqcKeqc
+    {
+        //priIeJeKept ppriIeJeKepXIpt ppriIeJeKepEIpt ppriIeJeKepXJpt ppriIeJeKepEJpt ppriIeJeKepEKpt ppriIeJeKeptpt 
+    public:
+
+        double priIeJeKept;
+        FRowDsptr ppriIeJeKepXIpt;
+        FRowDsptr ppriIeJeKepEIpt;
+        FRowDsptr ppriIeJeKepXJpt;
+        FRowDsptr ppriIeJeKepEJpt;
+        FRowDsptr ppriIeJeKepEKpt;
+        double ppriIeJeKeptpt;
+    };
+}
+
diff --git a/MbDCode/DispCompIeqcJeqcO.cpp b/MbDCode/DispCompIeqcJeqcO.cpp
new file mode 100644
index 0000000..05ac82c
--- /dev/null
+++ b/MbDCode/DispCompIeqcJeqcO.cpp
@@ -0,0 +1 @@
+#include "DispCompIeqcJeqcO.h"
diff --git a/MbDCode/DispCompIeqcJeqcO.h b/MbDCode/DispCompIeqcJeqcO.h
new file mode 100644
index 0000000..f272f14
--- /dev/null
+++ b/MbDCode/DispCompIeqcJeqcO.h
@@ -0,0 +1,16 @@
+#pragma once
+
+#include "DispCompIeqcJecO.h"
+
+namespace MbD {
+    class DispCompIeqcJeqcO : public DispCompIeqcJecO
+    {
+        //priIeJeOpXJ priIeJeOpEJ ppriIeJeOpEJpEJ 
+    public:
+
+        FRowDsptr priIeJeOpXJ;
+        FRowDsptr priIeJeOpEJ;
+        FMatDsptr ppriIeJeOpEJpEJ;
+    };
+}
+
diff --git a/MbDCode/DispCompIeqctJeqcKeqct.cpp b/MbDCode/DispCompIeqctJeqcKeqct.cpp
new file mode 100644
index 0000000..689a1d1
--- /dev/null
+++ b/MbDCode/DispCompIeqctJeqcKeqct.cpp
@@ -0,0 +1 @@
+#include "DispCompIeqctJeqcKeqct.h"
diff --git a/MbDCode/DispCompIeqctJeqcKeqct.h b/MbDCode/DispCompIeqctJeqcKeqct.h
new file mode 100644
index 0000000..e2eea94
--- /dev/null
+++ b/MbDCode/DispCompIeqctJeqcKeqct.h
@@ -0,0 +1,13 @@
+#pragma once
+
+#include "DispCompIeqcJeqcKeqct.h"
+
+namespace MbD {
+    class DispCompIeqctJeqcKeqct : public DispCompIeqcJeqcKeqct
+    {
+        //
+    public:
+
+    };
+}
+
diff --git a/MbDCode/DispCompIeqctJeqcO.cpp b/MbDCode/DispCompIeqctJeqcO.cpp
new file mode 100644
index 0000000..3552ffb
--- /dev/null
+++ b/MbDCode/DispCompIeqctJeqcO.cpp
@@ -0,0 +1 @@
+#include "DispCompIeqctJeqcO.h"
diff --git a/MbDCode/DispCompIeqctJeqcO.h b/MbDCode/DispCompIeqctJeqcO.h
new file mode 100644
index 0000000..e140ce8
--- /dev/null
+++ b/MbDCode/DispCompIeqctJeqcO.h
@@ -0,0 +1,16 @@
+#pragma once
+
+#include "DispCompIeqcJeqcO.h"
+
+namespace MbD {
+    class DispCompIeqctJeqcO : public DispCompIeqcJeqcO
+    {
+        //priIeJeOpt ppriIeJeOpEIpt ppriIeJeOptpt 
+    public:
+
+        double priIeJeOpt;
+        FRowDsptr ppriIeJeOpEIpt;
+        double ppriIeJeOptpt;
+    };
+}
+
diff --git a/MbDCode/EndFramec.cpp b/MbDCode/EndFramec.cpp
index d583628..7d6d253 100644
--- a/MbDCode/EndFramec.cpp
+++ b/MbDCode/EndFramec.cpp
@@ -1,3 +1,4 @@
+#include <assert.h>
 #include <memory>
 
 #include "EndFramec.h"
@@ -5,11 +6,9 @@
 using namespace MbD;
 
 EndFramec::EndFramec() {
-	initialize();
 }
 
 EndFramec::EndFramec(const char* str) : CartesianFrame(str) {
-	initialize();
 }
 
 void EndFramec::initialize()
@@ -34,6 +33,11 @@ void EndFramec::initializeGlobally()
 {
 }
 
-void MbD::EndFramec::EndFrameqctFrom(std::shared_ptr<EndFramec>& newFrmI)
+void EndFramec::initEndFrameqct()
+{
+	assert(false);
+}
+
+void MbD::EndFramec::calcPostDynCorrectorIteration()
 {
 }
diff --git a/MbDCode/EndFramec.h b/MbDCode/EndFramec.h
index dcbbdcf..6f9ddab 100644
--- a/MbDCode/EndFramec.h
+++ b/MbDCode/EndFramec.h
@@ -3,12 +3,15 @@
 #include <memory>
 
 #include "CartesianFrame.h"
-//#include "MarkerFrame.h"
-#include "FullColumn.h"
-#include "FullMatrix.h"
+#include "FullColumn.h"     //FColDsptr is defined
+#include "FullMatrix.h"     //FMatDsptr is defined
 
 namespace MbD {
     class MarkerFrame;
+    //template <typename T>
+    //class FullColumn;
+    //template <typename T>
+    //class FullMatrix;
 
     class EndFramec : public CartesianFrame
     {
@@ -21,7 +24,8 @@ namespace MbD {
         MarkerFrame* getMarkerFrame();
         void initializeLocally() override;
         void initializeGlobally() override;
-        virtual void EndFrameqctFrom(std::shared_ptr<EndFramec>& frm);
+        virtual void initEndFrameqct();
+        void calcPostDynCorrectorIteration() override;
 
         MarkerFrame* markerFrame;
         FColDsptr rOeO = std::make_shared<FullColumn<double>>(3);
diff --git a/MbDCode/EndFrameqc.cpp b/MbDCode/EndFrameqc.cpp
index 1a82319..95d1c54 100644
--- a/MbDCode/EndFrameqc.cpp
+++ b/MbDCode/EndFrameqc.cpp
@@ -7,12 +7,17 @@
 
 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() {
-	initialize();
 }
 
 EndFrameqc::EndFrameqc(const char* str) : EndFramec(str) {
-	initialize();
 }
 
 void EndFrameqc::initialize()
@@ -41,17 +46,35 @@ void EndFrameqc::initializeGlobally()
 	}
 }
 
-void MbD::EndFrameqc::EndFrameqctFrom(EndFrmcptr& frm)
+void EndFrameqc::initEndFrameqct()
 {
-	endFrameqct = std::make_shared<EndFrameqct>();
+	endFrameqct = EndFrameqct::Create(this->getName().data());
 }
 
-void MbD::EndFrameqc::setrmemBlks(std::shared_ptr<FullColumn<std::shared_ptr<Symbolic>>> xyzBlks)
+void EndFrameqc::setrmemBlks(std::shared_ptr<FullColumn<std::shared_ptr<Symbolic>>> xyzBlks)
 {
 	std::static_pointer_cast<EndFrameqct>(endFrameqct)->rmemBlks = xyzBlks;
 }
 
-void MbD::EndFrameqc::setphiThePsiBlks(std::shared_ptr<FullColumn<std::shared_ptr<Symbolic>>> xyzRotBlks)
+void EndFrameqc::setphiThePsiBlks(std::shared_ptr<FullColumn<std::shared_ptr<Symbolic>>> xyzRotBlks)
 {
 	std::static_pointer_cast<EndFrameqct>(endFrameqct)->phiThePsiBlks = xyzRotBlks;
 }
+
+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++) {
+			answeri->at(j) = ppAOepEipE->at(j)->column(jj);
+		}
+	}
+	answer->symLowerWithUpper();
+	return answer;
+}
+
+void MbD::EndFrameqc::calcPostDynCorrectorIteration()
+{
+}
diff --git a/MbDCode/EndFrameqc.h b/MbDCode/EndFrameqc.h
index f962584..748e813 100644
--- a/MbDCode/EndFrameqc.h
+++ b/MbDCode/EndFrameqc.h
@@ -9,14 +9,17 @@ namespace MbD {
     {
         //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 EndFrameqctFrom(EndFrmcptr& frm) 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);
+        void calcPostDynCorrectorIteration() override;
 
         FMatDsptr prOeOpE;
         std::shared_ptr<FullMatrix<std::shared_ptr<FullColumn<double>>>> pprOeOpEpE;
diff --git a/MbDCode/EndFrameqct.cpp b/MbDCode/EndFrameqct.cpp
index 889a6d6..4ae861c 100644
--- a/MbDCode/EndFrameqct.cpp
+++ b/MbDCode/EndFrameqct.cpp
@@ -1,14 +1,23 @@
 #include "EndFrameqct.h"
 #include "System.h"
+#include "Symbolic.h"
+#include "Time.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() {
-	initialize();
 }
 
 EndFrameqct::EndFrameqct(const char* str) : EndFrameqc(str) {
-	initialize();
 }
 
 void EndFrameqct::initialize()
@@ -41,19 +50,19 @@ void EndFrameqct::initializeLocally()
 
 void EndFrameqct::initializeGlobally()
 {
-	if (!rmemBlks) {
+	if (rmemBlks) {
 		initprmemptBlks();
 		initpprmemptptBlks();
 	}
-	if (!phiThePsiBlks) {
+	if (phiThePsiBlks) {
 		initpPhiThePsiptBlks();
 		initppPhiThePsiptptBlks();
 	}
 }
 
-void MbD::EndFrameqct::initprmemptBlks()
+void EndFrameqct::initprmemptBlks()
 {
-	auto time = System::getInstance().time;
+	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);
@@ -62,14 +71,22 @@ void MbD::EndFrameqct::initprmemptBlks()
 	}
 }
 
-void MbD::EndFrameqct::initpprmemptptBlks()
+void EndFrameqct::initpprmemptptBlks()
 {
 }
 
-void MbD::EndFrameqct::initpPhiThePsiptBlks()
+void EndFrameqct::initpPhiThePsiptBlks()
 {
 }
 
-void MbD::EndFrameqct::initppPhiThePsiptptBlks()
+void EndFrameqct::initppPhiThePsiptptBlks()
+{
+}
+
+void MbD::EndFrameqct::postInput()
+{
+}
+
+void MbD::EndFrameqct::calcPostDynCorrectorIteration()
 {
 }
diff --git a/MbDCode/EndFrameqct.h b/MbDCode/EndFrameqct.h
index 1fd80a0..b4658a0 100644
--- a/MbDCode/EndFrameqct.h
+++ b/MbDCode/EndFrameqct.h
@@ -4,12 +4,14 @@
 namespace MbD {
 
 	class Symbolic;
+	class Time;
 
 	class EndFrameqct : public EndFrameqc
 	{
 		//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();
@@ -19,8 +21,10 @@ namespace MbD {
 		void initpprmemptptBlks();
 		void initpPhiThePsiptBlks();
 		void initppPhiThePsiptptBlks();
+		void postInput() override;
+		void calcPostDynCorrectorIteration() override;
 
-		double time;
+		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;
 		FColDsptr rmem, prmempt, pprmemptpt, pprOeOptpt;
diff --git a/MbDCode/EulerArray.h b/MbDCode/EulerArray.h
index 05302c7..95041b3 100644
--- a/MbDCode/EulerArray.h
+++ b/MbDCode/EulerArray.h
@@ -2,8 +2,16 @@
 #include "FullColumn.h"
 
 namespace MbD {
-    class EulerArray : public FullColumn<double>
+
+    template <typename T>
+    class EulerArray : public FullColumn<T>
     {
+        //
+    public:
+        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 } {}
+
     };
 }
 
diff --git a/MbDCode/EulerConstraint.cpp b/MbDCode/EulerConstraint.cpp
index bbe18af..6af46c2 100644
--- a/MbDCode/EulerConstraint.cpp
+++ b/MbDCode/EulerConstraint.cpp
@@ -3,9 +3,16 @@
 
 using namespace MbD;
 
+std::shared_ptr<EulerConstraint> MbD::EulerConstraint::Create()
+{
+	auto item = std::make_shared<EulerConstraint>();
+	item->initialize();
+	return item;
+}
+
 EulerConstraint::EulerConstraint()
 {
-	initialize();
+
 }
 
 EulerConstraint::EulerConstraint(const char* str) : Constraint(str)
@@ -16,3 +23,7 @@ void EulerConstraint::initialize()
 {
 	pGpE = std::make_shared<FullRow<double>>(4);
 }
+
+void MbD::EulerConstraint::calcPostDynCorrectorIteration()
+{
+}
diff --git a/MbDCode/EulerConstraint.h b/MbDCode/EulerConstraint.h
index 5e3d167..17787dc 100644
--- a/MbDCode/EulerConstraint.h
+++ b/MbDCode/EulerConstraint.h
@@ -3,18 +3,20 @@
 #include <vector>
 
 #include "Constraint.h"
-#include "FullRow.h"
+#include "FullRow.h"	//FRowDsptr is defined
 
 namespace MbD {
 	class EulerConstraint : public Constraint
 	{
 		//pGpE iqE 
 	public:
+		static std::shared_ptr<EulerConstraint> Create();
 		EulerConstraint();
 		EulerConstraint(const char* str);
 		void initialize();
+		void calcPostDynCorrectorIteration() override;
 
-		FRowDsptr pGpE;//partial derivative of G wrt pE
+		FRowDsptr pGpE;		//partial derivative of G wrt pE
 		int iqE = -1;
 	};
 }
diff --git a/MbDCode/EulerParameters.cpp b/MbDCode/EulerParameters.cpp
index cfac1cf..8e58f9e 100644
--- a/MbDCode/EulerParameters.cpp
+++ b/MbDCode/EulerParameters.cpp
@@ -1,93 +1,3 @@
-#include <memory>
-
 #include "EulerParameters.h"
-#include "FullColumn.h"
-#include "FullMatrix.h"
 
 using namespace MbD;
-
-std::shared_ptr<FullMatrix<std::shared_ptr<FullColumn<double>>>> EulerParameters::ppApEpEtimesColumn(FColDsptr col)
-{
-	double a2c1 = 2 * col->at(0);
-	double a2c2 = 2 * col->at(1);
-	double a2c3 = 2 * col->at(2);
-	double m2c1 = 0 - a2c1;
-	double m2c2 = 0 - a2c2;
-	double m2c3 = 0 - a2c3;
-	auto col11 = std::make_shared<FullColumn<double>>(ListD{ a2c1, m2c2, m2c3 });
-	auto col12 = std::make_shared<FullColumn<double>>(ListD{ a2c2, a2c1, 0 });
-	auto col13 = std::make_shared<FullColumn<double>>(ListD{ a2c3, 0, a2c1 });
-	auto col14 = std::make_shared<FullColumn<double>>(ListD{ 0, m2c3, a2c2 });
-	auto col22 = std::make_shared<FullColumn<double>>(ListD{ m2c1, a2c2, m2c3 });
-	auto col23 = std::make_shared<FullColumn<double>>(ListD{ 0, a2c3, a2c2 });
-	auto col24 = std::make_shared<FullColumn<double>>(ListD{ a2c3, 0, m2c1 });
-	auto col33 = std::make_shared<FullColumn<double>>(ListD{ m2c1, m2c2, a2c3 });
-	auto col34 = std::make_shared<FullColumn<double>>(ListD{ m2c2, a2c1, 0 });
-	auto col44 = std::make_shared<FullColumn<double>>(ListD{ a2c1, a2c2, a2c3 });
-	auto answer = std::make_shared<FullMatrix<std::shared_ptr<FullColumn<double>>>>(4, 4);
-	auto 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);
-	row2->at(0) = col12;
-	row2->at(1) = col22;
-	row2->at(2) = col23;
-	row2->at(3) = col24;
-	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);
-	row4->at(0) = col14;
-	row4->at(1) = col24;
-	row4->at(2) = col34;
-	row4->at(3) = col44;
-	return answer;
-}
-
-std::shared_ptr<FullMatrix<std::shared_ptr<FullMatrix<double>>>> EulerParameters::ppApEpEtimesMatrix(FMatDsptr mat)
-{
-	FRowDsptr a2m1 = mat->at(0)->times(2.0);
-	FRowDsptr a2m2 = mat->at(1)->times(2.0);
-	FRowDsptr a2m3 = mat->at(2)->times(2.0);
-	FRowDsptr m2m1 = a2m1->negated();
-	FRowDsptr m2m2 = a2m2->negated();
-	FRowDsptr m2m3 = a2m3->negated();
-	auto aaaa = std::make_shared<std::vector<double>>(3, 0.0);
-	FRowDsptr zero = std::make_shared<FullRow<double>>(3, 0.0);
-	auto mat11 = std::make_shared<FullMatrix<double>>(ListFRD{ a2m1, m2m2, m2m3 });
-	auto mat12 = std::make_shared<FullMatrix<double>>(ListFRD{ a2m2, a2m1, zero });
-	auto mat13 = std::make_shared<FullMatrix<double>>(ListFRD{ a2m3, zero, a2m1 });
-	auto mat14 = std::make_shared<FullMatrix<double>>(ListFRD{ zero, m2m3, a2m2 });
-	auto mat22 = std::make_shared<FullMatrix<double>>(ListFRD{ m2m1, a2m2, m2m3 });
-	auto mat23 = std::make_shared<FullMatrix<double>>(ListFRD{ zero, a2m3, a2m2 });
-	auto mat24 = std::make_shared<FullMatrix<double>>(ListFRD{ a2m3, zero, m2m1 });
-	auto mat33 = std::make_shared<FullMatrix<double>>(ListFRD{ m2m1, m2m2, a2m3 });
-	auto mat34 = std::make_shared<FullMatrix<double>>(ListFRD{ m2m2, a2m1, zero });
-	auto mat44 = std::make_shared<FullMatrix<double>>(ListFRD{ a2m1, a2m2, a2m3 });
-	auto answer = std::make_shared<FullMatrix<std::shared_ptr<FullMatrix<double>>>>(4, 4);
-	auto 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);
-	row2->at(0) = mat12;
-	row2->at(1) = mat22;
-	row2->at(2) = mat23;
-	row2->at(3) = mat24;
-	auto row3 = answer->at(0);
-	row3->at(0) = mat13;
-	row3->at(1) = mat23;
-	row3->at(2) = mat33;
-	row3->at(3) = mat34;
-	auto row4 = answer->at(0);
-	row4->at(0) = mat14;
-	row4->at(1) = mat24;
-	row4->at(2) = mat34;
-	row4->at(3) = mat44;
-	return answer;
-}
diff --git a/MbDCode/EulerParameters.h b/MbDCode/EulerParameters.h
index 30eee56..9b02d77 100644
--- a/MbDCode/EulerParameters.h
+++ b/MbDCode/EulerParameters.h
@@ -4,13 +4,107 @@
 #include "FullMatrix.h"
 
 namespace MbD {
-	class EulerParameters : public EulerArray
+
+	template <typename T>
+	class EulerParameters : public EulerArray<T>
 	{
 		//aA aB aC pApE
 	public:
-		static std::shared_ptr<FullMatrix<std::shared_ptr<FullColumn<double>>>> ppApEpEtimesColumn(FColDsptr col);
-		static std::shared_ptr<FullMatrix<std::shared_ptr<FullMatrix<double>>>> ppApEpEtimesMatrix(FMatDsptr col);
+		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 } {}
+		
+		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);
 		
 	};
+
+	template<>
+	inline std::shared_ptr<FullMatrix<std::shared_ptr<FullColumn<double>>>> EulerParameters<double>::ppApEpEtimesColumn(FColDsptr col)
+	{
+		double a2c1 = 2 * col->at(0);
+		double a2c2 = 2 * col->at(1);
+		double a2c3 = 2 * col->at(2);
+		double m2c1 = 0 - a2c1;
+		double m2c2 = 0 - a2c2;
+		double m2c3 = 0 - a2c3;
+		auto col11 = std::make_shared<FullColumn<double>>(ListD{ a2c1, m2c2, m2c3 });
+		auto col12 = std::make_shared<FullColumn<double>>(ListD{ a2c2, a2c1, 0 });
+		auto col13 = std::make_shared<FullColumn<double>>(ListD{ a2c3, 0, a2c1 });
+		auto col14 = std::make_shared<FullColumn<double>>(ListD{ 0, m2c3, a2c2 });
+		auto col22 = std::make_shared<FullColumn<double>>(ListD{ m2c1, a2c2, m2c3 });
+		auto col23 = std::make_shared<FullColumn<double>>(ListD{ 0, a2c3, a2c2 });
+		auto col24 = std::make_shared<FullColumn<double>>(ListD{ a2c3, 0, m2c1 });
+		auto col33 = std::make_shared<FullColumn<double>>(ListD{ m2c1, m2c2, a2c3 });
+		auto col34 = std::make_shared<FullColumn<double>>(ListD{ m2c2, a2c1, 0 });
+		auto col44 = std::make_shared<FullColumn<double>>(ListD{ a2c1, a2c2, a2c3 });
+		auto answer = std::make_shared<FullMatrix<std::shared_ptr<FullColumn<double>>>>(4, 4);
+		auto 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);
+		row2->at(0) = col12;
+		row2->at(1) = col22;
+		row2->at(2) = col23;
+		row2->at(3) = col24;
+		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);
+		row4->at(0) = col14;
+		row4->at(1) = col24;
+		row4->at(2) = col34;
+		row4->at(3) = col44;
+		return answer;
+	}
+
+	template<>
+	inline std::shared_ptr<FullMatrix<std::shared_ptr<FullMatrix<double>>>> EulerParameters<double>::ppApEpEtimesMatrix(FMatDsptr mat)
+	{
+		FRowDsptr a2m1 = mat->at(0)->times(2.0);
+		FRowDsptr a2m2 = mat->at(1)->times(2.0);
+		FRowDsptr a2m3 = mat->at(2)->times(2.0);
+		FRowDsptr m2m1 = a2m1->negated();
+		FRowDsptr m2m2 = a2m2->negated();
+		FRowDsptr m2m3 = a2m3->negated();
+		auto aaaa = std::make_shared<std::vector<double>>(3, 0.0);
+		FRowDsptr zero = std::make_shared<FullRow<double>>(3, 0.0);
+		auto mat11 = std::make_shared<FullMatrix<double>>(ListFRD{ a2m1, m2m2, m2m3 });
+		auto mat12 = std::make_shared<FullMatrix<double>>(ListFRD{ a2m2, a2m1, zero });
+		auto mat13 = std::make_shared<FullMatrix<double>>(ListFRD{ a2m3, zero, a2m1 });
+		auto mat14 = std::make_shared<FullMatrix<double>>(ListFRD{ zero, m2m3, a2m2 });
+		auto mat22 = std::make_shared<FullMatrix<double>>(ListFRD{ m2m1, a2m2, m2m3 });
+		auto mat23 = std::make_shared<FullMatrix<double>>(ListFRD{ zero, a2m3, a2m2 });
+		auto mat24 = std::make_shared<FullMatrix<double>>(ListFRD{ a2m3, zero, m2m1 });
+		auto mat33 = std::make_shared<FullMatrix<double>>(ListFRD{ m2m1, m2m2, a2m3 });
+		auto mat34 = std::make_shared<FullMatrix<double>>(ListFRD{ m2m2, a2m1, zero });
+		auto mat44 = std::make_shared<FullMatrix<double>>(ListFRD{ a2m1, a2m2, a2m3 });
+		auto answer = std::make_shared<FullMatrix<std::shared_ptr<FullMatrix<double>>>>(4, 4);
+		auto 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);
+		row2->at(0) = mat12;
+		row2->at(1) = mat22;
+		row2->at(2) = mat23;
+		row2->at(3) = mat24;
+		auto row3 = answer->at(0);
+		row3->at(0) = mat13;
+		row3->at(1) = mat23;
+		row3->at(2) = mat33;
+		row3->at(3) = mat34;
+		auto row4 = answer->at(0);
+		row4->at(0) = mat14;
+		row4->at(1) = mat24;
+		row4->at(2) = mat34;
+		row4->at(3) = mat44;
+		return answer;
+	}
 }
 
diff --git a/MbDCode/EulerParametersDot.cpp b/MbDCode/EulerParametersDot.cpp
new file mode 100644
index 0000000..c1b063a
--- /dev/null
+++ b/MbDCode/EulerParametersDot.cpp
@@ -0,0 +1 @@
+#include "EulerParametersDot.h"
diff --git a/MbDCode/EulerParametersDot.h b/MbDCode/EulerParametersDot.h
new file mode 100644
index 0000000..74b2928
--- /dev/null
+++ b/MbDCode/EulerParametersDot.h
@@ -0,0 +1,31 @@
+#pragma once
+
+#include "EulerArray.h"
+#include "FullColumn.h"
+#include "FullMatrix.h"
+#include "EulerParameters.h"
+
+namespace MbD {
+
+    template <typename T>
+    class EulerParametersDot : public EulerArray<T>
+    {
+        //qE aAdot aBdot aCdot pAdotpE
+    public:
+        EulerParametersDot(size_t count) : EulerArray<T>(count) {}
+        EulerParametersDot(size_t count, const T& value) : EulerArray<T>(count, value) {}
+        EulerParametersDot(std::initializer_list<T> list) : EulerArray<T>{ list } {}
+        static std::shared_ptr<EulerParametersDot<T>> FromqEOpAndOmegaOpO(std::shared_ptr<EulerParameters<T>> qe, FColDsptr omeOpO);
+        std::shared_ptr<EulerParameters<T>> qE;
+        FMatDsptr aAdot, aBdot, aCdot;
+        FColFMatDsptr pAdotpE;
+    };
+
+    template<typename T>
+    inline std::shared_ptr<EulerParametersDot<T>> EulerParametersDot<T>::FromqEOpAndOmegaOpO(std::shared_ptr<EulerParameters<T>> qe, FColDsptr omeOpO)
+    {
+        return std::shared_ptr<EulerParametersDot<T>>();
+    }
+
+}
+
diff --git a/MbDCode/FullColumn.h b/MbDCode/FullColumn.h
index bb938b4..b3f14e6 100644
--- a/MbDCode/FullColumn.h
+++ b/MbDCode/FullColumn.h
@@ -3,9 +3,9 @@
 #include <sstream> 
 
 #include "Vector.h"
-#include "FullMatrix.h"
 
 namespace MbD {
+
 	template <typename T>
 	class FullColumn : public Vector<T>
 	{
@@ -26,6 +26,5 @@ namespace MbD {
 		}
 	};
 	using FColDsptr = std::shared_ptr<FullColumn<double>>;
-	//using FColFMatDsptr = std::shared_ptr<FullColumn<std::shared_ptr<FullMatrix<double>>>>;
 }
 
diff --git a/MbDCode/FullMatrix.h b/MbDCode/FullMatrix.h
index 51b6627..4b19555 100644
--- a/MbDCode/FullMatrix.h
+++ b/MbDCode/FullMatrix.h
@@ -1,11 +1,13 @@
 #pragma once
 #include <memory>
 
-#include "FullColumn.h"
 #include "RowTypeMatrix.h"
-#include "FullRow.h"
 
 namespace MbD {
+	template <typename T>
+	class FullColumn;
+	template <typename T>
+	class FullRow;
 
 	template <typename T>
 	class FullMatrix : public RowTypeMatrix<std::shared_ptr<FullRow<T>>>
@@ -32,6 +34,9 @@ namespace MbD {
 			}
 		}
 		void identity();
+		std::shared_ptr<FullColumn<T>> column(int j);
+		std::shared_ptr<FullMatrix<T>> times(double a);
+		void symLowerWithUpper();
 	};
 	template <>
 	inline void FullMatrix<double>::identity() {
@@ -40,9 +45,39 @@ namespace MbD {
 			this->at(i)->at(i) = 1.0;
 		}
 	}
+	template<typename T>
+	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++) {
+			answer->at(i) = this->at(i)->at(j);
+		}
+		return answer;
+	}
+	template<typename T>
+	inline std::shared_ptr<FullMatrix<T>> FullMatrix<T>::times(double a)
+	{
+		int m = this->nRow();
+		auto answer = std::make_shared<FullMatrix<T>>(m);
+		for (int i = 0; i < m; i++) {
+			answer->at(i) = this->at(i)->times(a);
+		}
+		return answer;
+	}
+	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++) {
+				this->at(j)->at(i) = this->at(i)->at(j);
+			}
+		}
+	}
 	using FMatDsptr = std::shared_ptr<FullMatrix<double>>;
 	using FMatDsptr = std::shared_ptr<FullMatrix<double>>;
-	//using FMatFColDsptr = std::shared_ptr<FullMatrix<std::shared_ptr<FullColumn<double>>>>;
+	using FMatFColDsptr = std::shared_ptr<FullMatrix<std::shared_ptr<FullColumn<double>>>>;
 	using FMatFMatDsptr = std::shared_ptr<FullMatrix<std::shared_ptr<FullMatrix<double>>>>;
+	using FColFMatDsptr = std::shared_ptr<FullColumn<std::shared_ptr<FullMatrix<double>>>>;
 }
 
diff --git a/MbDCode/FunctionWithManyArgs.cpp b/MbDCode/FunctionWithManyArgs.cpp
index 0b1df70..4240c70 100644
--- a/MbDCode/FunctionWithManyArgs.cpp
+++ b/MbDCode/FunctionWithManyArgs.cpp
@@ -3,23 +3,23 @@
 
 using namespace MbD;
 
-MbD::FunctionWithManyArgs::FunctionWithManyArgs(std::shared_ptr<Symbolic> term)
+FunctionWithManyArgs::FunctionWithManyArgs(std::shared_ptr<Symbolic> term)
 {
 	terms = std::make_shared<std::vector<std::shared_ptr<Symbolic>>>();
 	terms->push_back(term);
 }
 
-MbD::FunctionWithManyArgs::FunctionWithManyArgs(std::shared_ptr<Symbolic> term, std::shared_ptr<Symbolic> term1) : FunctionWithManyArgs(term)
+FunctionWithManyArgs::FunctionWithManyArgs(std::shared_ptr<Symbolic> term, std::shared_ptr<Symbolic> term1) : FunctionWithManyArgs(term)
 {
 	terms->push_back(term1);
 }
 
-MbD::FunctionWithManyArgs::FunctionWithManyArgs(std::shared_ptr<Symbolic> term, std::shared_ptr<Symbolic> term1, std::shared_ptr<Symbolic> term2) : FunctionWithManyArgs(term, term1)
+FunctionWithManyArgs::FunctionWithManyArgs(std::shared_ptr<Symbolic> term, std::shared_ptr<Symbolic> term1, std::shared_ptr<Symbolic> term2) : FunctionWithManyArgs(term, term1)
 {
 	terms->push_back(term2);
 }
 
-MbD::FunctionWithManyArgs::FunctionWithManyArgs(std::shared_ptr<std::vector<std::shared_ptr<Symbolic>>> _terms) {
+FunctionWithManyArgs::FunctionWithManyArgs(std::shared_ptr<std::vector<std::shared_ptr<Symbolic>>> _terms) {
 	terms = std::make_shared<std::vector<std::shared_ptr<Symbolic>>>();
 	for (int i = 0; i < _terms->size(); i++)
 		terms->push_back(_terms->at(i));
diff --git a/MbDCode/FunctionWithManyArgs.h b/MbDCode/FunctionWithManyArgs.h
index 0fb6b74..fe43eb6 100644
--- a/MbDCode/FunctionWithManyArgs.h
+++ b/MbDCode/FunctionWithManyArgs.h
@@ -3,6 +3,8 @@
 #include "Function.h"
 
 namespace MbD {
+    class Symbolic;
+
     class FunctionWithManyArgs : public Function
     {
         //terms
diff --git a/MbDCode/GESpMat3.cpp b/MbDCode/GESpMat3.cpp
new file mode 100644
index 0000000..0907e43
--- /dev/null
+++ b/MbDCode/GESpMat3.cpp
@@ -0,0 +1 @@
+#include "GESpMat3.h"
diff --git a/MbDCode/GESpMat3.h b/MbDCode/GESpMat3.h
new file mode 100644
index 0000000..463e8b6
--- /dev/null
+++ b/MbDCode/GESpMat3.h
@@ -0,0 +1,13 @@
+#pragma once
+
+#include "MatrixGaussElimination.h"
+
+namespace MbD {
+    class GESpMat : public MatrixGaussElimination
+    {
+        //
+    public:
+
+    };
+}
+
diff --git a/MbDCode/GESpMatFullPv.cpp b/MbDCode/GESpMatFullPv.cpp
new file mode 100644
index 0000000..c01ef14
--- /dev/null
+++ b/MbDCode/GESpMatFullPv.cpp
@@ -0,0 +1 @@
+#include "GESpMatFullPv.h"
diff --git a/MbDCode/GESpMatFullPv.h b/MbDCode/GESpMatFullPv.h
new file mode 100644
index 0000000..3e306f3
--- /dev/null
+++ b/MbDCode/GESpMatFullPv.h
@@ -0,0 +1,13 @@
+#pragma once
+
+#include "GESpMat3.h"
+
+namespace MbD {
+    class GESpMatFullPv : public GESpMat
+    {
+        //
+    public:
+
+    };
+}
+
diff --git a/MbDCode/GESpMatFullPvPosIC.cpp b/MbDCode/GESpMatFullPvPosIC.cpp
new file mode 100644
index 0000000..e7f8ec8
--- /dev/null
+++ b/MbDCode/GESpMatFullPvPosIC.cpp
@@ -0,0 +1 @@
+#include "GESpMatFullPvPosIC.h"
diff --git a/MbDCode/GESpMatFullPvPosIC.h b/MbDCode/GESpMatFullPvPosIC.h
new file mode 100644
index 0000000..f20fc6d
--- /dev/null
+++ b/MbDCode/GESpMatFullPvPosIC.h
@@ -0,0 +1,13 @@
+#pragma once
+
+#include "GESpMatFullPv.h"
+
+namespace MbD {
+    class GESpMatFullPvPosIC : public GESpMatFullPv
+    {
+        //
+    public:
+
+    };
+}
+
diff --git a/MbDCode/GESpMatParPv.cpp b/MbDCode/GESpMatParPv.cpp
new file mode 100644
index 0000000..26f8898
--- /dev/null
+++ b/MbDCode/GESpMatParPv.cpp
@@ -0,0 +1 @@
+#include "GESpMatParPv.h"
diff --git a/MbDCode/GESpMatParPv.h b/MbDCode/GESpMatParPv.h
new file mode 100644
index 0000000..b9b59a0
--- /dev/null
+++ b/MbDCode/GESpMatParPv.h
@@ -0,0 +1,13 @@
+#pragma once
+
+#include "GESpMat3.h"
+
+namespace MbD {
+    class GESpMatParPv : public GESpMat
+    {
+        //
+    public:
+
+    };
+}
+
diff --git a/MbDCode/GESpMatParPvMarko.cpp b/MbDCode/GESpMatParPvMarko.cpp
new file mode 100644
index 0000000..5f60743
--- /dev/null
+++ b/MbDCode/GESpMatParPvMarko.cpp
@@ -0,0 +1 @@
+#include "GESpMatParPvMarko.h"
diff --git a/MbDCode/GESpMatParPvMarko.h b/MbDCode/GESpMatParPvMarko.h
new file mode 100644
index 0000000..7faa5e2
--- /dev/null
+++ b/MbDCode/GESpMatParPvMarko.h
@@ -0,0 +1,13 @@
+#pragma once
+
+#include "GESpMatParPv.h"
+
+namespace MbD {
+    class GESpMatParPvMarko : public GESpMatParPv
+    {
+        //
+    public:
+
+    };
+}
+
diff --git a/MbDCode/GESpMatParPvMarkoFast.cpp b/MbDCode/GESpMatParPvMarkoFast.cpp
new file mode 100644
index 0000000..b4877d3
--- /dev/null
+++ b/MbDCode/GESpMatParPvMarkoFast.cpp
@@ -0,0 +1 @@
+#include "GESpMatParPvMarkoFast.h"
diff --git a/MbDCode/GESpMatParPvMarkoFast.h b/MbDCode/GESpMatParPvMarkoFast.h
new file mode 100644
index 0000000..4c95abf
--- /dev/null
+++ b/MbDCode/GESpMatParPvMarkoFast.h
@@ -0,0 +1,13 @@
+#pragma once
+
+#include "GESpMatParPvMarko.h"
+
+namespace MbD {
+    class GESpMatParPvMarkoFast : public GESpMatParPvMarko
+    {
+        //
+    public:
+
+    };
+}
+
diff --git a/MbDCode/GESpMatParPvPrecise.cpp b/MbDCode/GESpMatParPvPrecise.cpp
new file mode 100644
index 0000000..4b798fa
--- /dev/null
+++ b/MbDCode/GESpMatParPvPrecise.cpp
@@ -0,0 +1 @@
+#include "GESpMatParPvPrecise.h"
diff --git a/MbDCode/GESpMatParPvPrecise.h b/MbDCode/GESpMatParPvPrecise.h
new file mode 100644
index 0000000..61f5e01
--- /dev/null
+++ b/MbDCode/GESpMatParPvPrecise.h
@@ -0,0 +1,13 @@
+#pragma once
+
+#include "GESpMatParPv.h"
+
+namespace MbD {
+    class GESpMatParPvPrecise : public GESpMatParPv
+    {
+        //
+    public:
+
+    };
+}
+
diff --git a/MbDCode/IndependentVariable.cpp b/MbDCode/IndependentVariable.cpp
index 01b48f8..84afc06 100644
--- a/MbDCode/IndependentVariable.cpp
+++ b/MbDCode/IndependentVariable.cpp
@@ -1 +1,7 @@
 #include "IndependentVariable.h"
+
+using namespace MbD;
+
+MbD::IndependentVariable::IndependentVariable()
+{
+}
diff --git a/MbDCode/IndependentVariable.h b/MbDCode/IndependentVariable.h
index c5f6f2a..cc6e985 100644
--- a/MbDCode/IndependentVariable.h
+++ b/MbDCode/IndependentVariable.h
@@ -1,9 +1,12 @@
 #pragma once
+
 #include "Variable.h"
+
 namespace MbD {
-    class IndependentVariable :
-        public Variable
+    class IndependentVariable : public Variable
     {
+    public:
+        IndependentVariable();
     };
 }
 
diff --git a/MbDCode/Integrator.cpp b/MbDCode/Integrator.cpp
new file mode 100644
index 0000000..6908343
--- /dev/null
+++ b/MbDCode/Integrator.cpp
@@ -0,0 +1 @@
+#include "Integrator.h"
diff --git a/MbDCode/Integrator.h b/MbDCode/Integrator.h
new file mode 100644
index 0000000..7c0e4d9
--- /dev/null
+++ b/MbDCode/Integrator.h
@@ -0,0 +1,13 @@
+#pragma once
+
+#include "Solver.h"
+
+namespace MbD {
+    class Integrator : public Solver
+    {
+        //
+    public:
+
+    };
+}
+
diff --git a/MbDCode/IntegratorInterface.cpp b/MbDCode/IntegratorInterface.cpp
new file mode 100644
index 0000000..b291db6
--- /dev/null
+++ b/MbDCode/IntegratorInterface.cpp
@@ -0,0 +1 @@
+#include "IntegratorInterface.h"
diff --git a/MbDCode/IntegratorInterface.h b/MbDCode/IntegratorInterface.h
new file mode 100644
index 0000000..e1abf7c
--- /dev/null
+++ b/MbDCode/IntegratorInterface.h
@@ -0,0 +1,13 @@
+#pragma once
+
+#include "Integrator.h"
+
+namespace MbD {
+    class IntegratorInterface : public Integrator
+    {
+        //
+    public:
+
+    };
+}
+
diff --git a/MbDCode/Item.cpp b/MbDCode/Item.cpp
index 79534ef..39cd1bd 100644
--- a/MbDCode/Item.cpp
+++ b/MbDCode/Item.cpp
@@ -1,18 +1,21 @@
+#include <windows.h>
+#include <assert.h>
+#include <debugapi.h>
 #include "Item.h"
+#include "System.h"
 
 using namespace MbD;
 
 Item::Item() {
-	initialize();
 }
 
 Item::Item(const char* str) : name(str) 
 {
-	initialize();
 }
 
 void Item::initialize()
 {
+	assert(false);
 }
 
 void Item::setName(std::string& str)
@@ -27,8 +30,33 @@ const std::string& Item::getName() const
 
 void Item::initializeLocally()
 {
+	//DebugBreak();
 }
 
 void Item::initializeGlobally()
+{
+	//DebugBreak();
+}
+
+void MbD::Item::postInput()
+{
+	//Called once after input
+	calcPostDynCorrectorIteration();
+}
+
+void MbD::Item::calcPostDynCorrectorIteration()
+{
+	//DebugBreak();
+}
+
+void MbD::Item::removeRedundantConstraints(std::shared_ptr<std::vector<int>> redunEqnNos)
+{
+}
+
+void MbD::Item::constraintsReport()
+{
+}
+
+void MbD::Item::setqsu(std::shared_ptr<FullColumn<double>> qsuOld)
 {
 }
diff --git a/MbDCode/Item.h b/MbDCode/Item.h
index 9533f9c..6ff8dc9 100644
--- a/MbDCode/Item.h
+++ b/MbDCode/Item.h
@@ -1,7 +1,11 @@
 #pragma once
 #include <string>
+#include <vector>
+
+#include "FullColumn.h"
 
 namespace MbD {
+
 	class Item
 	{
 		//name
@@ -11,6 +15,11 @@ namespace MbD {
 		virtual void initialize();
 		virtual void initializeLocally();
 		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);
 		void setName(std::string& str);
 		const std::string& getName() const;
 
diff --git a/MbDCode/Joint.cpp b/MbDCode/Joint.cpp
index e6ddd9e..c98fc30 100644
--- a/MbDCode/Joint.cpp
+++ b/MbDCode/Joint.cpp
@@ -1,15 +1,16 @@
 #include<algorithm>
 
 #include "Joint.h"
+#include "Constraint.h"
 
 using namespace MbD;
 
 Joint::Joint() {
-	initialize();
+
 }
 
 Joint::Joint(const char* str) : Item(str) {
-	initialize();
+
 }
 
 void Joint::initialize()
@@ -32,3 +33,13 @@ void Joint::initializeGlobally()
 {
 	std::for_each(constraints->begin(), constraints->end(), [](const auto& constraint) { constraint->initializeGlobally(); });
 }
+
+void MbD::Joint::postInput()
+{
+}
+
+void MbD::Joint::addConstraint(std::shared_ptr<Constraint> con)
+{
+	con->setOwner(this);
+	constraints->push_back(con);
+}
diff --git a/MbDCode/Joint.h b/MbDCode/Joint.h
index f769805..33c5f8e 100644
--- a/MbDCode/Joint.h
+++ b/MbDCode/Joint.h
@@ -2,15 +2,11 @@
 #include <memory>
 #include <vector>
 
-//#include "typedef.h"
 #include "Item.h"
 #include "EndFramec.h"
-#include "Constraint.h"
 
 namespace MbD {
-    //class EndFramec;
     class Constraint;
-    //using EndFrmcptr = std::shared_ptr<EndFramec>;
 
     class Joint : public Item
     {
@@ -22,6 +18,8 @@ namespace MbD {
         virtual void connectsItoJ(EndFrmcptr frmI, EndFrmcptr frmJ);
         void initializeLocally() override;
         void initializeGlobally() override;
+        void postInput() override;
+        void addConstraint(std::shared_ptr<Constraint> con);
 
         EndFrmcptr frmI;
         EndFrmcptr frmJ;
diff --git a/MbDCode/KineIntegrator.cpp b/MbDCode/KineIntegrator.cpp
new file mode 100644
index 0000000..285499e
--- /dev/null
+++ b/MbDCode/KineIntegrator.cpp
@@ -0,0 +1 @@
+#include "KineIntegrator.h"
diff --git a/MbDCode/KineIntegrator.h b/MbDCode/KineIntegrator.h
new file mode 100644
index 0000000..90b70e7
--- /dev/null
+++ b/MbDCode/KineIntegrator.h
@@ -0,0 +1,13 @@
+#pragma once
+
+#include "QuasiIntegrator.h"
+
+namespace MbD {
+    class KineIntegrator : public QuasiIntegrator
+    {
+        //
+    public:
+
+    };
+}
+
diff --git a/MbDCode/KinematicIeJe.cpp b/MbDCode/KinematicIeJe.cpp
index 481ec42..b8cceb6 100644
--- a/MbDCode/KinematicIeJe.cpp
+++ b/MbDCode/KinematicIeJe.cpp
@@ -2,10 +2,10 @@
 
 using namespace MbD;
 
-MbD::KinematicIeJe::KinematicIeJe()
+KinematicIeJe::KinematicIeJe()
 {
 }
 
-MbD::KinematicIeJe::KinematicIeJe(EndFrmcptr frmi, EndFrmcptr frmj) : frmI(frmi), frmJ(frmj)
+KinematicIeJe::KinematicIeJe(EndFrmcptr frmi, EndFrmcptr frmj) : frmI(frmi), frmJ(frmj)
 {
 }
diff --git a/MbDCode/KinematicIeJe.h b/MbDCode/KinematicIeJe.h
index 6964104..ffaf061 100644
--- a/MbDCode/KinematicIeJe.h
+++ b/MbDCode/KinematicIeJe.h
@@ -1,6 +1,5 @@
 #pragma once
 
-//#include "typedef.h"
 #include "Item.h"
 #include "EndFramec.h"
 
diff --git a/MbDCode/MarkerFrame.cpp b/MbDCode/MarkerFrame.cpp
index 763aa69..1579d85 100644
--- a/MbDCode/MarkerFrame.cpp
+++ b/MbDCode/MarkerFrame.cpp
@@ -10,11 +10,9 @@ using namespace MbD;
 
 MarkerFrame::MarkerFrame()
 {
-	initialize();
 }
 
 MarkerFrame::MarkerFrame(const char* str) : CartesianFrame(str) {
-	initialize();
 }
 
 void MarkerFrame::initialize()
@@ -26,6 +24,26 @@ void MarkerFrame::initialize()
 	this->addEndFrame(endFrm);
 }
 
+void MarkerFrame::initializeLocally()
+{
+	pprOmOpEpE = EulerParameters<double>::ppApEpEtimesColumn(rpmp);
+	ppAOmpEpE = EulerParameters<double>::ppApEpEtimesMatrix(aApm);
+	std::for_each(endFrames->begin(), endFrames->end(), [](const auto& endFrame) { endFrame->initializeLocally(); });
+}
+
+void MarkerFrame::initializeGlobally()
+{
+	std::for_each(endFrames->begin(), endFrames->end(), [](const auto& endFrame) { endFrame->initializeGlobally(); });
+}
+
+void MbD::MarkerFrame::postInput()
+{
+}
+
+void MbD::MarkerFrame::calcPostDynCorrectorIteration()
+{
+}
+
 void MarkerFrame::setPartFrame(PartFrame* partFrm)
 {
 	partFrame = partFrm;
@@ -49,15 +67,3 @@ void MarkerFrame::addEndFrame(EndFrmcptr endFrm)
 	endFrm->setMarkerFrame(this);
 	endFrames->push_back(endFrm);
 }
-
-void MarkerFrame::initializeLocally()
-{
-	pprOmOpEpE = EulerParameters::ppApEpEtimesColumn(rpmp);
-	ppAOmpEpE = EulerParameters::ppApEpEtimesMatrix(aApm);
-	std::for_each(endFrames->begin(), endFrames->end(), [](const auto& endFrame) { endFrame->initializeLocally(); });
-}
-
-void MarkerFrame::initializeGlobally()
-{
-	std::for_each(endFrames->begin(), endFrames->end(), [](const auto& endFrame) { endFrame->initializeGlobally(); });
-}
diff --git a/MbDCode/MarkerFrame.h b/MbDCode/MarkerFrame.h
index af22c4e..4626308 100644
--- a/MbDCode/MarkerFrame.h
+++ b/MbDCode/MarkerFrame.h
@@ -2,20 +2,21 @@
 #include <memory>
 
 #include "CartesianFrame.h"
-#include "PartFrame.h"
+//#include "PartFrame.h"
 #include "FullColumn.h"
 #include "FullMatrix.h"
 #include "EndFramec.h"
 
 namespace MbD {
 	class PartFrame;
-	class EndFramec;
-	using EndFrmcptr = std::shared_ptr<EndFramec>;
+	//class EndFramec;
+	//using EndFrmcptr = std::shared_ptr<EndFramec>;
 
 	class MarkerFrame : public CartesianFrame
 	{
 		//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();
@@ -26,6 +27,8 @@ namespace MbD {
 		void addEndFrame(EndFrmcptr x);
 		void initializeLocally() override;
 		void initializeGlobally() override;
+		void postInput() override;
+		void calcPostDynCorrectorIteration() override;
 
 		PartFrame* partFrame;
 		FColDsptr rpmp = std::make_shared<FullColumn<double>>(3);
diff --git a/MbDCode/MatrixGaussElimination.cpp b/MbDCode/MatrixGaussElimination.cpp
new file mode 100644
index 0000000..23d1bdf
--- /dev/null
+++ b/MbDCode/MatrixGaussElimination.cpp
@@ -0,0 +1 @@
+#include "MatrixGaussElimination.h"
diff --git a/MbDCode/MatrixGaussElimination.h b/MbDCode/MatrixGaussElimination.h
new file mode 100644
index 0000000..5d0f016
--- /dev/null
+++ b/MbDCode/MatrixGaussElimination.h
@@ -0,0 +1,13 @@
+#pragma once
+
+#include "MatrixSolver.h"
+
+namespace MbD {
+    class MatrixGaussElimination : public MatrixSolver
+    {
+        //
+    public:
+
+    };
+}
+
diff --git a/MbDCode/MatrixSolver.cpp b/MbDCode/MatrixSolver.cpp
new file mode 100644
index 0000000..aabe56e
--- /dev/null
+++ b/MbDCode/MatrixSolver.cpp
@@ -0,0 +1 @@
+#include "MatrixSolver.h"
diff --git a/MbDCode/MatrixSolver.h b/MbDCode/MatrixSolver.h
new file mode 100644
index 0000000..b768832
--- /dev/null
+++ b/MbDCode/MatrixSolver.h
@@ -0,0 +1,13 @@
+#pragma once
+
+#include "Solver.h"
+
+namespace MbD {
+    class MatrixSolver : public Solver
+    {
+        //
+    public:
+
+    };
+}
+
diff --git a/MbDCode/MbDCode.cpp b/MbDCode/MbDCode.cpp
index eaa6abe..4cd9ac6 100644
--- a/MbDCode/MbDCode.cpp
+++ b/MbDCode/MbDCode.cpp
@@ -9,15 +9,22 @@
 #include "Constant.h"
 #include "FullColumn.h"
 #include "FullMatrix.h"
+#include "DiagonalMatrix.h"
 #include "Part.h"
 #include "Joint.h"
 #include "CylindricalJoint.h"
 #include "RevoluteJoint.h"
 #include "ZRotation.h"
+#include "PartFrame.h"
+#include "MarkerFrame.h"
 #include "EndFrameqc.h"
 #include "EndFrameqct.h"
 #include "Product.h"
+#include "Symbolic.h"
+#include "SystemSolver.h"
 #include "MbDCode.h"
+#include "Time.h"
+#include "CREATE.h"
 
 using namespace MbD;
 
@@ -46,23 +53,29 @@ int main()
 	systemSolver->rotationLimit = 0.5;
 
 	std::string str;
-	FColDsptr qX, qE;
+	FColDsptr qX, qE, qXdot, omeOpO;
 	FColDsptr rpmp;
 	FMatDsptr aApm;
 	FRowDsptr fullRow;
 	//
-	auto assembly1 = std::make_shared<Part>("Assembly1");
+	auto assembly1 = CREATE<Part>::With("/Assembly1");
 	std::cout << "assembly1->getName() " << assembly1->getName() << std::endl;
+	assembly1->m = 0.0;
+	assembly1->aJ = std::make_shared<DiagonalMatrix<double>>(ListD{ 0, 0, 0 });
 	qX = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0 });
 	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);
 	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 marker1 = std::make_shared<MarkerFrame>("Marker1");
+		auto marker1 = CREATE<MarkerFrame>::With("/Assembly1/Marker1");
 		rpmp = std::make_shared<FullColumn<double>>(ListD{ 0.38423366582893, 6.8384291794733e-9, -0.048029210642807 });
 		marker1->setrpmp(rpmp);
 		aApm = std::make_shared<FullMatrix<double>>(ListListD{
@@ -73,7 +86,7 @@ int main()
 		marker1->setaApm(aApm);
 		partFrame->addMarkerFrame(marker1);
 		//
-		auto marker2 = std::make_shared<MarkerFrame>("Marker2");
+		auto marker2 = CREATE<MarkerFrame>::With("/Assembly1/Marker2");
 		rpmp = std::make_shared<FullColumn<double>>(ListD{ 0.0, 0.0, 0.0 });
 		marker2->setrpmp(rpmp);
 		aApm = std::make_shared<FullMatrix<double>>(ListListD{
@@ -86,7 +99,7 @@ int main()
 	}
 	assembly1->asFixed();
 	//
-	auto crankPart1 = std::make_shared<Part>("Part1");
+	auto crankPart1 = CREATE<Part>::With("/Assembly1/Part1");
 	qX = std::make_shared<FullColumn<double>>(ListD{ 0.38423366582893, 6.8384291794733e-9, -0.048029210642807 });
 	qE = std::make_shared<FullColumn<double>>(ListD{ 0.0, 0.0, 1.4248456266393e-10, 1.0 });
 	crankPart1->setqX(qX);
@@ -94,7 +107,7 @@ int main()
 	TheSystem.parts->push_back(crankPart1);
 	{
 		auto partFrame = crankPart1->partFrame;
-		auto marker1 = std::make_shared<MarkerFrame>("Marker1");
+		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);
 		aApm = std::make_shared<FullMatrix<double>>(ListListD{
@@ -105,7 +118,7 @@ int main()
 		marker1->setaApm(aApm);
 		partFrame->addMarkerFrame(marker1);
 		//
-		auto marker2 = std::make_shared<MarkerFrame>("Marker2");
+		auto marker2 = CREATE<MarkerFrame>::With("/Assembly1/Part1/Marker2");
 		rpmp = std::make_shared<FullColumn<double>>(ListD{ 0.38423368514246, -2.6661567755108e-17, 0.048029210642807 });
 		marker2->setrpmp(rpmp);
 		aApm = std::make_shared<FullMatrix<double>>(ListListD{
@@ -117,7 +130,7 @@ int main()
 		partFrame->addMarkerFrame(marker2);
 	}
 	//
-	auto conrodPart2 = std::make_shared<Part>("Part2");
+	auto conrodPart2 = CREATE<Part>::With("/Assembly1/Part2");
 	qX = std::make_shared<FullColumn<double>>(ListD{ 0.38423366582893,  0.49215308269277,  0.048029210642807 });
 	qE = std::make_shared<FullColumn<double>>(ListD{ 0.0, 0.0, 0.89871701272344, 0.4385290538168 });
 	conrodPart2->setqX(qX);
@@ -125,7 +138,7 @@ int main()
 	TheSystem.parts->push_back(conrodPart2);
 	{
 		auto partFrame = conrodPart2->partFrame;
-		auto marker1 = std::make_shared<MarkerFrame>("Marker1");
+		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);
 		aApm = std::make_shared<FullMatrix<double>>(ListListD{
@@ -136,7 +149,7 @@ int main()
 		marker1->setaApm(aApm);
 		partFrame->addMarkerFrame(marker1);
 		//
-		auto marker2 = std::make_shared<MarkerFrame>("Marker2");
+		auto marker2 = CREATE<MarkerFrame>::With("/Assembly1/Part2/Marker2");
 		rpmp = std::make_shared<FullColumn<double>>(ListD{ 0.6243797383565, -2.1329254204087e-16, -0.048029210642807 });
 		marker2->setrpmp(rpmp);
 		aApm = std::make_shared<FullMatrix<double>>(ListListD{
@@ -148,7 +161,7 @@ int main()
 		partFrame->addMarkerFrame(marker2);
 	}
 	//
-	auto pistonPart3 = std::make_shared<Part>("Part3");
+	auto pistonPart3 = CREATE<Part>::With("/Assembly1/Part3");
 	qX = std::make_shared<FullColumn<double>>(ListD{ -1.284772285311e-18, 1.4645982581368, -4.788228906425e-17 });
 	qE = std::make_shared<FullColumn<double>>(ListD{ 0.70710678118655, 0.70710678118655, 0.0, 0.0 });
 	pistonPart3->setqX(qX);
@@ -156,7 +169,7 @@ int main()
 	TheSystem.parts->push_back(pistonPart3);
 	{
 		auto partFrame = pistonPart3->partFrame;
-		auto marker1 = std::make_shared<MarkerFrame>("Marker1");
+		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);
 		aApm = std::make_shared<FullMatrix<double>>(ListListD{
@@ -167,7 +180,7 @@ int main()
 		marker1->setaApm(aApm);
 		partFrame->addMarkerFrame(marker1);
 		//
-		auto marker2 = std::make_shared<MarkerFrame>("Marker2");
+		auto marker2 = CREATE<MarkerFrame>::With("/Assembly1/Part3/Marker2");
 		rpmp = std::make_shared<FullColumn<double>>(ListD{ 0.48029210642807, 1.7618247880058e-17, 2.5155758471256e-17 });
 		marker2->setrpmp(rpmp);
 		aApm = std::make_shared<FullMatrix<double>>(ListListD{
@@ -179,24 +192,24 @@ int main()
 		partFrame->addMarkerFrame(marker2);
 	}
 	//
-	auto cylJoint4 = std::make_shared<CylindricalJoint>("CylJoint4");
-	cylJoint4->connectsItoJ(pistonPart3->partFrame->endFrame("Marker2"), assembly1->partFrame->endFrame("Marker1"));
+	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 revJoint3 = std::make_shared<RevoluteJoint>("RevJoint3");
-	revJoint3->connectsItoJ(conrodPart2->partFrame->endFrame("Marker2"), pistonPart3->partFrame->endFrame("Marker1"));
+	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 = std::make_shared<RevoluteJoint>("RevJoint2");
-	revJoint2->connectsItoJ(crankPart1->partFrame->endFrame("Marker2"), conrodPart2->partFrame->endFrame("Marker1"));
+	auto revJoint2 = CREATE<RevoluteJoint>::With("/Assembly1/Joint2");
+	revJoint2->connectsItoJ(crankPart1->partFrame->endFrame("/Assembly1/Part1/Marker2"), conrodPart2->partFrame->endFrame("/Assembly1/Part2/Marker1"));
 	TheSystem.jointsMotions->push_back(revJoint2);
 	
-	auto revJoint1 = std::make_shared<RevoluteJoint>("RevJoint1");
-	revJoint1->connectsItoJ(assembly1->partFrame->endFrame("Marker2"), crankPart1->partFrame->endFrame("Marker1"));
+	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 rotMotion1 = std::make_shared<ZRotation>("RotMotion1");
-	rotMotion1->connectsItoJ(assembly1->partFrame->endFrame("Marker2"), crankPart1->partFrame->endFrame("Marker1"));
+	auto rotMotion1 = CREATE<ZRotation>::With("/Assembly1/Motion1");
+	rotMotion1->connectsItoJ(assembly1->partFrame->endFrame("/Assembly1/Marker2"), crankPart1->partFrame->endFrame("/Assembly1/Part1/Marker1"));
 	auto omega = std::make_shared<Constant>(6.2831853071796);
 	auto timeScale = std::make_shared<Constant>(0.04);
 	rotMotion1->phiBlk = std::make_shared<Product>(omega, timeScale, TheSystem.time);
diff --git a/MbDCode/MbDCode.vcxproj b/MbDCode/MbDCode.vcxproj
index e90b03a..3b9f24d 100644
--- a/MbDCode/MbDCode.vcxproj
+++ b/MbDCode/MbDCode.vcxproj
@@ -1,4 +1,4 @@
-<?xml version="1.0" encoding="utf-8"?>
+<?xml version="1.0" encoding="utf-8"?>
 <Project DefaultTargets="Build" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
   <ItemGroup Label="ProjectConfigurations">
     <ProjectConfiguration Include="Debug|Win32">
@@ -128,25 +128,47 @@
   </ItemDefinitionGroup>
   <ItemGroup>
     <ClCompile Include="AbsConstraint.cpp" />
+    <ClCompile Include="AnyPosICNewtonRaphson.cpp" />
     <ClCompile Include="Array.cpp" />
+    <ClCompile Include="AtPointConstraintIJ.cpp" />
+    <ClCompile Include="AtPointConstraintIqcJc.cpp" />
+    <ClCompile Include="AtPointConstraintIqcJqc.cpp" />
+    <ClCompile Include="AtPointConstraintIqctJqc.cpp" />
+    <ClCompile Include="BasicIntegrator.cpp" />
+    <ClCompile Include="BasicQuasiIntegrator.cpp" />
     <ClCompile Include="BasicUserFunction.cpp" />
     <ClCompile Include="CartesianFrame.cpp" />
     <ClCompile Include="Constant.cpp" />
     <ClCompile Include="Constraint.cpp" />
     <ClCompile Include="ConstraintIJ.cpp" />
+    <ClCompile Include="CREATE.cpp" />
     <ClCompile Include="CylindricalJoint.cpp" />
     <ClCompile Include="DiagonalMatrix.cpp" />
     <ClCompile Include="DirectionCosineConstraintIJ.cpp" />
+    <ClCompile Include="DirectionCosineConstraintIqcJc.cpp" />
+    <ClCompile Include="DirectionCosineConstraintIqcJqc.cpp" />
+    <ClCompile Include="DirectionCosineConstraintIqctJqc.cpp" />
     <ClCompile Include="DirectionCosineIecJec.cpp" />
     <ClCompile Include="DirectionCosineIeqcJec.cpp" />
     <ClCompile Include="DirectionCosineIeqcJeqc.cpp" />
     <ClCompile Include="DirectionCosineIeqctJeqc.cpp" />
+    <ClCompile Include="DispCompIecJecKec.cpp" />
+    <ClCompile Include="DispCompIecJecKeqc.cpp" />
+    <ClCompile Include="DispCompIecJecO.cpp" />
+    <ClCompile Include="DispCompIeqcJecKeqc.cpp" />
+    <ClCompile Include="DispCompIeqcJecO.cpp" />
+    <ClCompile Include="DispCompIeqcJeqcKeqc.cpp" />
+    <ClCompile Include="DispCompIeqcJeqcKeqct.cpp" />
+    <ClCompile Include="DispCompIeqcJeqcO.cpp" />
+    <ClCompile Include="DispCompIeqctJeqcKeqct.cpp" />
+    <ClCompile Include="DispCompIeqctJeqcO.cpp" />
     <ClCompile Include="EndFramec.cpp" />
     <ClCompile Include="EndFrameqc.cpp" />
     <ClCompile Include="EndFrameqct.cpp" />
     <ClCompile Include="EulerArray.cpp" />
     <ClCompile Include="EulerConstraint.cpp" />
     <ClCompile Include="EulerParameters.cpp" />
+    <ClCompile Include="EulerParametersDot.cpp" />
     <ClCompile Include="ExpressionX.cpp" />
     <ClCompile Include="FullColumn.cpp" />
     <ClCompile Include="FullMatrix.cpp" />
@@ -154,19 +176,38 @@
     <ClCompile Include="Function.cpp" />
     <ClCompile Include="FunctionWithManyArgs.cpp" />
     <ClCompile Include="FunctionX.cpp" />
+    <ClCompile Include="GESpMat3.cpp" />
+    <ClCompile Include="GESpMatFullPv.cpp" />
+    <ClCompile Include="GESpMatFullPvPosIC.cpp" />
+    <ClCompile Include="GESpMatParPv.cpp" />
+    <ClCompile Include="GESpMatParPvMarko.cpp" />
+    <ClCompile Include="GESpMatParPvMarkoFast.cpp" />
+    <ClCompile Include="GESpMatParPvPrecise.cpp" />
     <ClCompile Include="IndependentVariable.cpp" />
+    <ClCompile Include="Integrator.cpp" />
+    <ClCompile Include="IntegratorInterface.cpp" />
     <ClCompile Include="Item.cpp" />
     <ClCompile Include="Joint.cpp" />
+    <ClCompile Include="KineIntegrator.cpp" />
     <ClCompile Include="KinematicIeJe.cpp" />
     <ClCompile Include="MarkerFrame.cpp" />
+    <ClCompile Include="MatrixGaussElimination.cpp" />
+    <ClCompile Include="MatrixSolver.cpp" />
     <ClCompile Include="MbDCode.cpp" />
     <ClCompile Include="NewtonRaphson.cpp" />
     <ClCompile Include="Part.cpp" />
     <ClCompile Include="PartFrame.cpp" />
+    <ClCompile Include="PosICKineNewtonRaphson.cpp" />
+    <ClCompile Include="PosICNewtonRaphson.cpp" />
+    <ClCompile Include="PosKineNewtonRaphson.cpp" />
+    <ClCompile Include="PosNewtonRaphson.cpp" />
     <ClCompile Include="PrescribedMotion.cpp" />
     <ClCompile Include="Product.cpp" />
+    <ClCompile Include="QuasiIntegrator.cpp" />
     <ClCompile Include="RevoluteJoint.cpp" />
     <ClCompile Include="RowTypeMatrix.cpp" />
+    <ClCompile Include="ScalarNewtonRaphson.cpp" />
+    <ClCompile Include="SingularMatrixError.cpp" />
     <ClCompile Include="Solver.cpp" />
     <ClCompile Include="SparseColumn.cpp" />
     <ClCompile Include="SparseMatrix.cpp" />
@@ -175,34 +216,62 @@
     <ClCompile Include="Sum.cpp" />
     <ClCompile Include="Symbolic.cpp" />
     <ClCompile Include="System.cpp" />
+    <ClCompile Include="SystemNewtonRaphson.cpp" />
     <ClCompile Include="SystemSolver.cpp" />
     <ClCompile Include="Time.cpp" />
+    <ClCompile Include="TranslationConstraintIJ.cpp" />
+    <ClCompile Include="TranslationConstraintIqcJc.cpp" />
+    <ClCompile Include="TranslationConstraintIqcJqc.cpp" />
+    <ClCompile Include="TranslationConstraintIqctJqc.cpp" />
     <ClCompile Include="UserFunction.cpp" />
     <ClCompile Include="Variable.cpp" />
     <ClCompile Include="Vector.cpp" />
+    <ClCompile Include="VectorNewtonRaphson.cpp" />
     <ClCompile Include="ZRotation.cpp" />
   </ItemGroup>
   <ItemGroup>
     <ClInclude Include="AbsConstraint.h" />
+    <ClInclude Include="AnyPosICNewtonRaphson.h" />
     <ClInclude Include="Array.h" />
+    <ClInclude Include="AtPointConstraintIJ.h" />
+    <ClInclude Include="AtPointConstraintIqcJc.h" />
+    <ClInclude Include="AtPointConstraintIqcJqc.h" />
+    <ClInclude Include="AtPointConstraintIqctJqc.h" />
+    <ClInclude Include="BasicIntegrator.h" />
+    <ClInclude Include="BasicQuasiIntegrator.h" />
     <ClInclude Include="BasicUserFunction.h" />
     <ClInclude Include="CartesianFrame.h" />
     <ClInclude Include="Constant.h" />
     <ClInclude Include="Constraint.h" />
     <ClInclude Include="ConstraintIJ.h" />
+    <ClInclude Include="CREATE.h" />
     <ClInclude Include="CylindricalJoint.h" />
     <ClInclude Include="DiagonalMatrix.h" />
     <ClInclude Include="DirectionCosineConstraintIJ.h" />
+    <ClInclude Include="DirectionCosineConstraintIqcJc.h" />
+    <ClInclude Include="DirectionCosineConstraintIqcJqc.h" />
+    <ClInclude Include="DirectionCosineConstraintIqctJqc.h" />
     <ClInclude Include="DirectionCosineIecJec.h" />
     <ClInclude Include="DirectionCosineIeqcJec.h" />
     <ClInclude Include="DirectionCosineIeqcJeqc.h" />
     <ClInclude Include="DirectionCosineIeqctJeqc.h" />
+    <ClInclude Include="DispCompIecJecKec.h" />
+    <ClInclude Include="DispCompIecJecKeqc.h" />
+    <ClInclude Include="DispCompIecJecO.h" />
+    <ClInclude Include="DispCompIeqcJecKeqc.h" />
+    <ClInclude Include="DispCompIeqcJecO.h" />
+    <ClInclude Include="DispCompIeqcJeqcKeqc.h" />
+    <ClInclude Include="DispCompIeqcJeqcKeqct.h" />
+    <ClInclude Include="DispCompIeqcJeqcO.h" />
+    <ClInclude Include="DispCompIeqctJeqcKeqct.h" />
+    <ClInclude Include="DispCompIeqctJeqcO.h" />
     <ClInclude Include="EndFramec.h" />
     <ClInclude Include="EndFrameqc.h" />
     <ClInclude Include="EndFrameqct.h" />
     <ClInclude Include="EulerArray.h" />
     <ClInclude Include="EulerConstraint.h" />
     <ClInclude Include="EulerParameters.h" />
+    <ClInclude Include="EulerParametersDot.h" />
     <ClInclude Include="ExpressionX.h" />
     <ClInclude Include="FullColumn.h" />
     <ClInclude Include="FullMatrix.h" />
@@ -210,19 +279,38 @@
     <ClInclude Include="Function.h" />
     <ClInclude Include="FunctionWithManyArgs.h" />
     <ClInclude Include="FunctionX.h" />
+    <ClInclude Include="GESpMat3.h" />
+    <ClInclude Include="GESpMatFullPv.h" />
+    <ClInclude Include="GESpMatFullPvPosIC.h" />
+    <ClInclude Include="GESpMatParPv.h" />
+    <ClInclude Include="GESpMatParPvMarko.h" />
+    <ClInclude Include="GESpMatParPvMarkoFast.h" />
+    <ClInclude Include="GESpMatParPvPrecise.h" />
     <ClInclude Include="IndependentVariable.h" />
+    <ClInclude Include="Integrator.h" />
+    <ClInclude Include="IntegratorInterface.h" />
     <ClInclude Include="Item.h" />
     <ClInclude Include="Joint.h" />
+    <ClInclude Include="KineIntegrator.h" />
     <ClInclude Include="KinematicIeJe.h" />
     <ClInclude Include="MarkerFrame.h" />
+    <ClInclude Include="MatrixGaussElimination.h" />
+    <ClInclude Include="MatrixSolver.h" />
     <ClInclude Include="MbDCode.h" />
     <ClInclude Include="NewtonRaphson.h" />
     <ClInclude Include="Part.h" />
     <ClInclude Include="PartFrame.h" />
+    <ClInclude Include="PosICKineNewtonRaphson.h" />
+    <ClInclude Include="PosICNewtonRaphson.h" />
+    <ClInclude Include="PosKineNewtonRaphson.h" />
+    <ClInclude Include="PosNewtonRaphson.h" />
     <ClInclude Include="PrescribedMotion.h" />
     <ClInclude Include="Product.h" />
+    <ClInclude Include="QuasiIntegrator.h" />
     <ClInclude Include="RevoluteJoint.h" />
     <ClInclude Include="RowTypeMatrix.h" />
+    <ClInclude Include="ScalarNewtonRaphson.h" />
+    <ClInclude Include="SingularMatrixError.h" />
     <ClInclude Include="Solver.h" />
     <ClInclude Include="SparseColumn.h" />
     <ClInclude Include="SparseMatrix.h" />
@@ -231,12 +319,18 @@
     <ClInclude Include="Sum.h" />
     <ClInclude Include="Symbolic.h" />
     <ClInclude Include="System.h" />
+    <ClInclude Include="SystemNewtonRaphson.h" />
     <ClInclude Include="SystemSolver.h" />
     <ClInclude Include="Time.h" />
+    <ClInclude Include="TranslationConstraintIJ.h" />
+    <ClInclude Include="TranslationConstraintIqcJc.h" />
+    <ClInclude Include="TranslationConstraintIqcJqc.h" />
+    <ClInclude Include="TranslationConstraintIqctJqc.h" />
     <ClInclude Include="typedef.h" />
     <ClInclude Include="UserFunction.h" />
     <ClInclude Include="Variable.h" />
     <ClInclude Include="Vector.h" />
+    <ClInclude Include="VectorNewtonRaphson.h" />
     <ClInclude Include="ZRotation.h" />
   </ItemGroup>
   <Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />
diff --git a/MbDCode/MbDCode.vcxproj.filters b/MbDCode/MbDCode.vcxproj.filters
index e0b657c..f6efdea 100644
--- a/MbDCode/MbDCode.vcxproj.filters
+++ b/MbDCode/MbDCode.vcxproj.filters
@@ -177,6 +177,147 @@
     <ClCompile Include="ExpressionX.cpp">
       <Filter>Source Files</Filter>
     </ClCompile>
+    <ClCompile Include="DirectionCosineConstraintIqcJc.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="DirectionCosineConstraintIqcJqc.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="DirectionCosineConstraintIqctJqc.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="TranslationConstraintIJ.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="TranslationConstraintIqcJc.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="TranslationConstraintIqcJqc.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="TranslationConstraintIqctJqc.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="DispCompIecJecKec.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="DispCompIecJecKeqc.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="DispCompIeqcJecKeqc.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="DispCompIeqcJeqcKeqc.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="DispCompIeqcJeqcKeqct.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="DispCompIeqctJeqcKeqct.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="AtPointConstraintIJ.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="AtPointConstraintIqcJc.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="AtPointConstraintIqcJqc.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="AtPointConstraintIqctJqc.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="DispCompIecJecO.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="DispCompIeqcJecO.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="DispCompIeqcJeqcO.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="DispCompIeqctJeqcO.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="Integrator.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="IntegratorInterface.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="BasicIntegrator.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="QuasiIntegrator.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="KineIntegrator.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="BasicQuasiIntegrator.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="MatrixSolver.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="MatrixGaussElimination.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="GESpMat3.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="GESpMatFullPv.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="GESpMatFullPvPosIC.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="GESpMatParPv.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="GESpMatParPvMarko.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="GESpMatParPvMarkoFast.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="GESpMatParPvPrecise.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="AnyPosICNewtonRaphson.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="PosICKineNewtonRaphson.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="PosICNewtonRaphson.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="PosKineNewtonRaphson.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="PosNewtonRaphson.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="ScalarNewtonRaphson.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="SystemNewtonRaphson.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="VectorNewtonRaphson.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="EulerParametersDot.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="CREATE.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="SingularMatrixError.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
   </ItemGroup>
   <ItemGroup>
     <ClInclude Include="Array.h">
@@ -344,5 +485,146 @@
     <ClInclude Include="ExpressionX.h">
       <Filter>Header Files</Filter>
     </ClInclude>
+    <ClInclude Include="DirectionCosineConstraintIqcJc.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="DirectionCosineConstraintIqcJqc.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="DirectionCosineConstraintIqctJqc.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="TranslationConstraintIJ.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="TranslationConstraintIqcJc.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="TranslationConstraintIqcJqc.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="TranslationConstraintIqctJqc.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="DispCompIecJecKec.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="DispCompIecJecKeqc.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="DispCompIeqcJecKeqc.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="DispCompIeqcJeqcKeqc.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="DispCompIeqcJeqcKeqct.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="DispCompIeqctJeqcKeqct.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="AtPointConstraintIJ.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="AtPointConstraintIqcJc.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="AtPointConstraintIqcJqc.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="AtPointConstraintIqctJqc.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="DispCompIecJecO.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="DispCompIeqcJecO.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="DispCompIeqcJeqcO.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="DispCompIeqctJeqcO.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="Integrator.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="IntegratorInterface.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="BasicIntegrator.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="QuasiIntegrator.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="KineIntegrator.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="BasicQuasiIntegrator.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="MatrixSolver.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="MatrixGaussElimination.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="GESpMat3.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="GESpMatFullPv.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="GESpMatFullPvPosIC.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="GESpMatParPv.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="GESpMatParPvMarko.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="GESpMatParPvMarkoFast.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="GESpMatParPvPrecise.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="AnyPosICNewtonRaphson.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="PosICKineNewtonRaphson.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="PosICNewtonRaphson.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="PosKineNewtonRaphson.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="PosNewtonRaphson.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="ScalarNewtonRaphson.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="SystemNewtonRaphson.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="VectorNewtonRaphson.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="EulerParametersDot.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="CREATE.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="SingularMatrixError.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 860afa9..e157679 100644
--- a/MbDCode/NewtonRaphson.cpp
+++ b/MbDCode/NewtonRaphson.cpp
@@ -1 +1,42 @@
+#include <limits>
+
 #include "NewtonRaphson.h"
+#include "SystemSolver.h"
+
+using namespace MbD;
+
+void NewtonRaphson::initialize()
+{
+	dxNorms = std::shared_ptr<std::vector<double>>();
+	dxTol = 4 * std::numeric_limits<double>::epsilon();
+	yNorms = std::shared_ptr<std::vector<double>>();
+	yNormTol = 1.0e-30;
+	iterMax = 100;
+	twoAlp = 2.0e-4;
+}
+
+void NewtonRaphson::initializeLocally()
+{
+	iterNo = 0;
+	nDivergence = 0;
+	nBackTracking = 0;
+	dxNorms->clear();
+	yNorms->clear();
+	yNormOld = std::numeric_limits<double>::max();
+}
+
+void MbD::NewtonRaphson::run()
+{
+		//self preRun.
+		//self initializeLocally.
+		//self initializeGlobally.
+		//self iterate.
+		//self finalize.
+		//self reportStats.
+		//self postRun.
+}
+
+void MbD::NewtonRaphson::setSystem(SystemSolver* sys)
+{
+	system = sys;
+}
diff --git a/MbDCode/NewtonRaphson.h b/MbDCode/NewtonRaphson.h
index e1402fc..c83a165 100644
--- a/MbDCode/NewtonRaphson.h
+++ b/MbDCode/NewtonRaphson.h
@@ -1,18 +1,31 @@
 #pragma once
 
 #include <memory>
+#include <vector>
 
 #include "Solver.h"
-
-#include "FullColumn.h"
+//#include "FullColumn.h"
+//#include "SystemSolver.h"
 
 namespace MbD {
+    template <typename T>
+    class FullColumn;
+    class SystemSolver;
+
     class NewtonRaphson : public Solver
     {
         //system xold x dx dxNorm dxNorms dxTol y yNorm yNormOld yNorms yNormTol pypx iterNo iterMax nDivergence nBackTracking twoAlp lam 
     public:
-        std::shared_ptr<FullColumn<double>> xold, x, dx, dxTol, y;
-        double dxNorm, yNorm, yNormOld, yNormTol, twoAlp, lam;
+        void initialize();
+        void initializeLocally() override;
+        void run() override;
+        void setSystem(SystemSolver* sys);
+
+        SystemSolver* system;
+        std::shared_ptr<FullColumn<double>> xold, x, dx, y;
+        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 3c409b4..122e66c 100644
--- a/MbDCode/Part.cpp
+++ b/MbDCode/Part.cpp
@@ -1,26 +1,33 @@
 #include "Part.h"
 #include "PartFrame.h"
-#include "FullColumn.h"
 
 using namespace MbD;
 
 Part::Part() {
-	initialize();
 }
 
 Part::Part(const char* str) : Item(str) {
-	initialize();
 }
 
 void Part::initialize()
 {
-	partFrame = std::make_shared<PartFrame>();
+	partFrame = PartFrame::Create();
 	partFrame->setPart(this);
 	pTpE = std::make_shared<FullColumn<double>>(4);
 	ppTpEpE = std::make_shared<FullMatrix<double>>(4, 4);
 	ppTpEpEdot = std::make_shared<FullMatrix<double>>(4, 4);
 }
 
+void Part::initializeLocally()
+{
+	partFrame->initializeLocally();
+}
+
+void Part::initializeGlobally()
+{
+	partFrame->initializeGlobally();
+}
+
 void Part::setqX(FColDsptr x) {
 	partFrame->setqX(x);
 }
@@ -37,22 +44,36 @@ FColDsptr Part::getqE() {
 	return partFrame->getqE();
 }
 
+void Part::setqXdot(FColDsptr x) {
+	partFrame->setqXdot(x);
+}
+
+FColDsptr Part::getqXdot() {
+	return partFrame->getqXdot();
+}
+
+void Part::setomeOpO(FColDsptr x) {
+	partFrame->setomeOpO(x);
+}
+
+FColDsptr Part::getomeOpO() {
+	return partFrame->getomeOpO();
+}
+
 void Part::setSystem(System& sys)
 {
 	//May be needed in the future
 }
 
-void MbD::Part::asFixed()
+void Part::asFixed()
 {
 	partFrame->asFixed();
 }
 
-void Part::initializeLocally()
+void MbD::Part::postInput()
 {
-	partFrame->initializeLocally();
 }
 
-void Part::initializeGlobally()
+void MbD::Part::calcPostDynCorrectorIteration()
 {
-	partFrame->initializeGlobally();
 }
diff --git a/MbDCode/Part.h b/MbDCode/Part.h
index d02a199..3a3b0a1 100644
--- a/MbDCode/Part.h
+++ b/MbDCode/Part.h
@@ -2,19 +2,20 @@
 #include <memory>
 
 #include "Item.h"
-#include "System.h"
-#include "PartFrame.h"
 #include "FullColumn.h"
-#include "DiagonalMatrix.h"
+#include "FullMatrix.h"
 
 namespace MbD {
 	class System;
 	class PartFrame;
+	template <typename T>
+	class DiagonalMatrix;
 
 	class Part : public Item
 	{
 		//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();
@@ -24,12 +25,18 @@ namespace MbD {
 		FColDsptr getqX();
 		void setqE(FColDsptr x);
 		FColDsptr getqE();
+		void setqXdot(FColDsptr x);
+		FColDsptr getqXdot();
+		void setomeOpO(FColDsptr x);
+		FColDsptr getomeOpO();
 		void setSystem(System& sys);
 		void asFixed();
+		void postInput() override;
+		void calcPostDynCorrectorIteration() override;
 
-		int ipX; 
-		int ipE; 
-		double m; 
+		int ipX = -1; 
+		int ipE = -1; 
+		double m = 0.0; 
 		std::shared_ptr<DiagonalMatrix<double>> aJ;
 		std::shared_ptr<PartFrame> partFrame;
 		FColDsptr pX;
diff --git a/MbDCode/PartFrame.cpp b/MbDCode/PartFrame.cpp
index 950d2b1..aaef221 100644
--- a/MbDCode/PartFrame.cpp
+++ b/MbDCode/PartFrame.cpp
@@ -5,39 +5,81 @@
 #include "EulerConstraint.h"
 #include "AbsConstraint.h"
 #include "MarkerFrame.h"
+#include "EulerParameters.h"
+#include "EulerParametersDot.h"
 
 using namespace MbD;
 
+std::shared_ptr<PartFrame> MbD::PartFrame::Create()
+{
+	auto item = std::make_shared<PartFrame>();
+	item->initialize();
+	return item;
+}
+
 PartFrame::PartFrame()
 {
-	initialize();
 }
 PartFrame::PartFrame(const char* str) : CartesianFrame(str)
 {
-	initialize();
 }
 void PartFrame::initialize()
 {
-	aGeu = std::make_shared<EulerConstraint>("EulerCon");
+	aGeu = EulerConstraint::Create();
 	aGeu->setOwner(this);
 	aGabs = std::make_shared<std::vector<std::shared_ptr<AbsConstraint>>>();
 	markerFrames = std::make_shared<std::vector<std::shared_ptr<MarkerFrame>>>();
 }
+
+void PartFrame::initializeLocally()
+{
+	std::for_each(markerFrames->begin(), markerFrames->end(), [](const auto& markerFrame) { markerFrame->initializeLocally(); });
+	aGeu->initializeLocally();
+	std::for_each(aGabs->begin(), aGabs->end(), [](const auto& aGab) { aGab->initializeLocally(); });
+}
+
+void PartFrame::initializeGlobally()
+{
+	std::for_each(markerFrames->begin(), markerFrames->end(), [](const auto& markerFrame) { markerFrame->initializeGlobally(); });
+	aGeu->initializeGlobally();
+	std::for_each(aGabs->begin(), aGabs->end(), [](const auto& aGab) { aGab->initializeGlobally(); });
+}
 void PartFrame::setqX(FColDsptr x) {
 	qX->copy(x);
 }
+
 FColDsptr PartFrame::getqX() {
 	return qX;
 }
+
 void PartFrame::setqE(FColDsptr x) {
 	qE->copy(x);
 }
+
 FColDsptr PartFrame::getqE() {
 	return qE;
 }
+void PartFrame::setqXdot(FColDsptr x) {
+	qXdot->copy(x);
+}
+
+FColDsptr PartFrame::getqXdot() {
+	return qXdot;
+}
+
+void PartFrame::setomeOpO(FColDsptr omeOpO) {
+	//qEdot := MbDEulerParametersDot fromqEOp: qE andOmegaOpO: omeOpO
+	qEdot = EulerParametersDot<double>::FromqEOpAndOmegaOpO(qE, omeOpO);
+}
+
+FColDsptr PartFrame::getomeOpO() {
+	return qE;
+}
+
 void PartFrame::setPart(Part* x) {
 	part = x;
 }
+
 Part* PartFrame::getPart() {
 	return part;
 }
@@ -54,7 +96,7 @@ EndFrmcptr PartFrame::endFrame(std::string name)
 	return (*match)->endFrames->at(0);
 }
 
-void MbD::PartFrame::asFixed()
+void PartFrame::asFixed()
 {
 	for (int i = 0; i < 6; i++) {
 		auto con = std::make_shared<AbsConstraint>(i);
@@ -63,16 +105,10 @@ void MbD::PartFrame::asFixed()
 	}
 }
 
-void PartFrame::initializeLocally()
+void MbD::PartFrame::postInput()
 {
-	std::for_each(markerFrames->begin(), markerFrames->end(), [](const auto& markerFrame) { markerFrame->initializeLocally(); });
-	aGeu->initializeLocally();
-	std::for_each(aGabs->begin(), aGabs->end(), [](const auto& aGab) { aGab->initializeLocally(); });
 }
 
-void PartFrame::initializeGlobally()
+void MbD::PartFrame::calcPostDynCorrectorIteration()
 {
-	std::for_each(markerFrames->begin(), markerFrames->end(), [](const auto& markerFrame) { markerFrame->initializeGlobally(); });
-	aGeu->initializeGlobally();
-	std::for_each(aGabs->begin(), aGabs->end(), [](const auto& aGab) { aGab->initializeGlobally(); });
 }
diff --git a/MbDCode/PartFrame.h b/MbDCode/PartFrame.h
index 4008c58..5e35351 100644
--- a/MbDCode/PartFrame.h
+++ b/MbDCode/PartFrame.h
@@ -3,45 +3,53 @@
 #include <vector>
 
 #include "CartesianFrame.h"
-#include "Part.h"
-#include "MarkerFrame.h"
 #include "EndFramec.h"
-#include "EulerConstraint.h"
-#include "AbsConstraint.h"
 #include "FullColumn.h"
+#include "EulerParameters.h"
+#include "EulerParametersDot.h"
 
 namespace MbD {
 	class Part;
 	class MarkerFrame;
-	class EndFramec;
-	using EndFrmcptr = std::shared_ptr<EndFramec>;
+	class EulerConstraint;
+	class AbsConstraint;
+	//class EulerParameters;
+	//class EulerParametersDot;
 
 	class PartFrame : public CartesianFrame
 	{
 		//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();
 		void initializeLocally() override;
 		void initializeGlobally() override;
 		void asFixed();
+		void postInput() override;
+		void calcPostDynCorrectorIteration() override;
 
 		void setqX(FColDsptr x);
 		FColDsptr getqX();
 		void setqE(FColDsptr x);
 		FColDsptr getqE();
+		void setqXdot(FColDsptr x);
+		FColDsptr getqXdot();
+		void setomeOpO(FColDsptr x);
+		FColDsptr getomeOpO();
 		void setPart(Part* x);
 		Part* getPart();
 		void addMarkerFrame(std::shared_ptr<MarkerFrame> x);
 		EndFrmcptr endFrame(std::string name);
 
-		Part* part;
-		int iqX, iqE;	//Position index of frame variables qX and qE in system list of variables
+		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);
-		FColDsptr qE = std::make_shared<FullColumn<double>>(4);
+		std::shared_ptr<EulerParameters<double>> qE = std::make_shared<EulerParameters<double>>(4);
 		FColDsptr qXdot = std::make_shared<FullColumn<double>>(3);
-		FColDsptr qEdot = std::make_shared<FullColumn<double>>(4);
+		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;
diff --git a/MbDCode/PosICKineNewtonRaphson.cpp b/MbDCode/PosICKineNewtonRaphson.cpp
new file mode 100644
index 0000000..8d54dbd
--- /dev/null
+++ b/MbDCode/PosICKineNewtonRaphson.cpp
@@ -0,0 +1 @@
+#include "PosICKineNewtonRaphson.h"
diff --git a/MbDCode/PosICKineNewtonRaphson.h b/MbDCode/PosICKineNewtonRaphson.h
new file mode 100644
index 0000000..1a734ec
--- /dev/null
+++ b/MbDCode/PosICKineNewtonRaphson.h
@@ -0,0 +1,13 @@
+#pragma once
+
+#include "AnyPosICNewtonRaphson.h"
+
+namespace MbD {
+    class PosICKineNewtonRaphson : public AnyPosICNewtonRaphson
+    {
+        //
+    public:
+
+    };
+}
+
diff --git a/MbDCode/PosICNewtonRaphson.cpp b/MbDCode/PosICNewtonRaphson.cpp
new file mode 100644
index 0000000..77af4aa
--- /dev/null
+++ b/MbDCode/PosICNewtonRaphson.cpp
@@ -0,0 +1,22 @@
+#include <assert.h>
+#include <exception>
+
+#include "PosICNewtonRaphson.h"
+#include "SingularMatrixError.h"
+#include "SystemSolver.h"
+
+void MbD::PosICNewtonRaphson::run()
+{
+	while (true) {
+		try {
+			VectorNewtonRaphson::run();
+			break;
+		}
+		catch (SingularMatrixError ex) {
+			auto redundantEqnNos = ex.getRedundantEqnNos();
+			system->partsJointsMotionsDo([&](std::shared_ptr<Item> item) { item->removeRedundantConstraints(redundantEqnNos); });
+			system->partsJointsMotionsDo([&](std::shared_ptr<Item> item) { item->constraintsReport(); });
+			system->partsJointsMotionsDo([&](std::shared_ptr<Item> item) { item->setqsu(qsuOld); });
+		}
+	}
+}
diff --git a/MbDCode/PosICNewtonRaphson.h b/MbDCode/PosICNewtonRaphson.h
new file mode 100644
index 0000000..d608638
--- /dev/null
+++ b/MbDCode/PosICNewtonRaphson.h
@@ -0,0 +1,17 @@
+#pragma once
+
+#include "AnyPosICNewtonRaphson.h"
+
+namespace MbD {
+    class PosICNewtonRaphson : public AnyPosICNewtonRaphson
+    {
+        //pivotRowLimits
+    public:
+        PosICNewtonRaphson(){}
+
+        void run() override;
+
+        std::shared_ptr<std::vector<int>> pivotRowLimits;
+    };
+}
+
diff --git a/MbDCode/PosKineNewtonRaphson.cpp b/MbDCode/PosKineNewtonRaphson.cpp
new file mode 100644
index 0000000..222ffd3
--- /dev/null
+++ b/MbDCode/PosKineNewtonRaphson.cpp
@@ -0,0 +1 @@
+#include "PosKineNewtonRaphson.h"
diff --git a/MbDCode/PosKineNewtonRaphson.h b/MbDCode/PosKineNewtonRaphson.h
new file mode 100644
index 0000000..28d9248
--- /dev/null
+++ b/MbDCode/PosKineNewtonRaphson.h
@@ -0,0 +1,13 @@
+#pragma once
+
+#include "PosNewtonRaphson.h"
+
+namespace MbD {
+    class PosKineNewtonRaphson : public PosNewtonRaphson
+    {
+        //
+    public:
+
+    };
+}
+
diff --git a/MbDCode/PosNewtonRaphson.cpp b/MbDCode/PosNewtonRaphson.cpp
new file mode 100644
index 0000000..a336d53
--- /dev/null
+++ b/MbDCode/PosNewtonRaphson.cpp
@@ -0,0 +1 @@
+#include "PosNewtonRaphson.h"
diff --git a/MbDCode/PosNewtonRaphson.h b/MbDCode/PosNewtonRaphson.h
new file mode 100644
index 0000000..73e2169
--- /dev/null
+++ b/MbDCode/PosNewtonRaphson.h
@@ -0,0 +1,13 @@
+#pragma once
+
+#include "SystemNewtonRaphson.h"
+
+namespace MbD {
+    class PosNewtonRaphson : public SystemNewtonRaphson
+    {
+        //
+    public:
+
+    };
+}
+
diff --git a/MbDCode/PrescribedMotion.cpp b/MbDCode/PrescribedMotion.cpp
index c06fb98..527c4d3 100644
--- a/MbDCode/PrescribedMotion.cpp
+++ b/MbDCode/PrescribedMotion.cpp
@@ -10,15 +10,16 @@
 using namespace MbD;
 
 PrescribedMotion::PrescribedMotion() {
-	initialize();
+
 }
 
 PrescribedMotion::PrescribedMotion(const char* str) : Joint(str) {
-	initialize();
+
 }
 
 void PrescribedMotion::initialize()
 {
+	Joint::initialize();
 	xBlk = std::make_shared<Constant>(0.0);
 	yBlk = std::make_shared<Constant>(0.0);
 	zBlk = std::make_shared<Constant>(0.0);
@@ -30,5 +31,5 @@ void PrescribedMotion::initialize()
 void PrescribedMotion::connectsItoJ(EndFrmcptr frmi, EndFrmcptr frmj)
 {
 	Joint::connectsItoJ(frmi, frmj);
-	frmI->EndFrameqctFrom(frmI);
+	frmI->initEndFrameqct();
 }
diff --git a/MbDCode/PrescribedMotion.h b/MbDCode/PrescribedMotion.h
index 6236c96..b5d23a1 100644
--- a/MbDCode/PrescribedMotion.h
+++ b/MbDCode/PrescribedMotion.h
@@ -2,7 +2,6 @@
 #include "Joint.h"
 
 namespace MbD {
-
     class Symbolic;
 
     class PrescribedMotion : public Joint
diff --git a/MbDCode/Product.cpp b/MbDCode/Product.cpp
index 149c119..abc43a9 100644
--- a/MbDCode/Product.cpp
+++ b/MbDCode/Product.cpp
@@ -2,7 +2,7 @@
 
 using namespace MbD;
 
-double MbD::Product::getValue()
+double Product::getValue()
 {
 	double answer = 1.0;
 	for (int i = 0; i < terms->size(); i++) answer *= terms->at(i)->getValue();
diff --git a/MbDCode/Product.h b/MbDCode/Product.h
index 2b9ca7a..e041b6d 100644
--- a/MbDCode/Product.h
+++ b/MbDCode/Product.h
@@ -3,6 +3,8 @@
 #include "FunctionWithManyArgs.h"
 
 namespace MbD {
+    class Symbolic;
+
     class Product : public FunctionWithManyArgs
     {
     public:
diff --git a/MbDCode/QuasiIntegrator.cpp b/MbDCode/QuasiIntegrator.cpp
new file mode 100644
index 0000000..4d34449
--- /dev/null
+++ b/MbDCode/QuasiIntegrator.cpp
@@ -0,0 +1 @@
+#include "QuasiIntegrator.h"
diff --git a/MbDCode/QuasiIntegrator.h b/MbDCode/QuasiIntegrator.h
new file mode 100644
index 0000000..cb221fa
--- /dev/null
+++ b/MbDCode/QuasiIntegrator.h
@@ -0,0 +1,13 @@
+#pragma once
+
+#include "IntegratorInterface.h"
+
+namespace MbD {
+    class QuasiIntegrator : public IntegratorInterface
+    {
+        //
+    public:
+
+    };
+}
+
diff --git a/MbDCode/RevoluteJoint.cpp b/MbDCode/RevoluteJoint.cpp
index 8812ea4..c870a90 100644
--- a/MbDCode/RevoluteJoint.cpp
+++ b/MbDCode/RevoluteJoint.cpp
@@ -3,13 +3,9 @@
 using namespace MbD;
 
 RevoluteJoint::RevoluteJoint() {
-	initialize();
+
 }
 
 RevoluteJoint::RevoluteJoint(const char* str) : Joint(str) {
-	initialize();
-}
 
-void RevoluteJoint::initialize()
-{
 }
diff --git a/MbDCode/RevoluteJoint.h b/MbDCode/RevoluteJoint.h
index 7a9705a..1653497 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 initialize();
     };
 }
 
diff --git a/MbDCode/RowTypeMatrix.h b/MbDCode/RowTypeMatrix.h
index 39b8598..71da531 100644
--- a/MbDCode/RowTypeMatrix.h
+++ b/MbDCode/RowTypeMatrix.h
@@ -13,6 +13,12 @@ namespace MbD {
 		RowTypeMatrix(std::initializer_list<T> list) : Array<T>{ list } {}
 		void copy(std::shared_ptr<RowTypeMatrix<T>> x);
 		void zeroSelf();
+		int nRow() {
+			return this->size();
+		}
+		int nCol() {
+			return this->at(0)->size();
+		}
 	};
 
 	template<typename T>
diff --git a/MbDCode/ScalarNewtonRaphson.cpp b/MbDCode/ScalarNewtonRaphson.cpp
new file mode 100644
index 0000000..3a44b35
--- /dev/null
+++ b/MbDCode/ScalarNewtonRaphson.cpp
@@ -0,0 +1 @@
+#include "ScalarNewtonRaphson.h"
diff --git a/MbDCode/ScalarNewtonRaphson.h b/MbDCode/ScalarNewtonRaphson.h
new file mode 100644
index 0000000..c309970
--- /dev/null
+++ b/MbDCode/ScalarNewtonRaphson.h
@@ -0,0 +1,13 @@
+#pragma once
+
+#include "NewtonRaphson.h"
+
+namespace MbD {
+    class ScalarNewtonRaphson : public NewtonRaphson
+    {
+        //
+    public:
+
+    };
+}
+
diff --git a/MbDCode/SingularMatrixError.cpp b/MbDCode/SingularMatrixError.cpp
new file mode 100644
index 0000000..16b7578
--- /dev/null
+++ b/MbDCode/SingularMatrixError.cpp
@@ -0,0 +1 @@
+#include "SingularMatrixError.h"
diff --git a/MbDCode/SingularMatrixError.h b/MbDCode/SingularMatrixError.h
new file mode 100644
index 0000000..d8a3bb7
--- /dev/null
+++ b/MbDCode/SingularMatrixError.h
@@ -0,0 +1,27 @@
+#pragma once
+#include <stdexcept>
+#include <memory>
+#include <vector>
+
+namespace MbD {
+	class SingularMatrixError : virtual public std::runtime_error
+	{
+	protected:
+
+		std::shared_ptr<std::vector<int>> redundantEqnNos;
+
+	public:
+		explicit
+			SingularMatrixError(const std::string& msg, std::shared_ptr<std::vector<int>> redunEqnNos) :
+			std::runtime_error(msg), redundantEqnNos(redunEqnNos)
+		{
+		}
+
+		virtual ~SingularMatrixError() noexcept {}
+
+		virtual std::shared_ptr<std::vector<int>> getRedundantEqnNos() const noexcept {
+			return redundantEqnNos;
+		}
+	};
+}
+
diff --git a/MbDCode/Solver.cpp b/MbDCode/Solver.cpp
index ef27f99..43ac519 100644
--- a/MbDCode/Solver.cpp
+++ b/MbDCode/Solver.cpp
@@ -1 +1,23 @@
+#include <assert.h>
+
 #include "Solver.h"
+
+void MbD::Solver::initialize()
+{
+	assert(false);
+}
+
+void MbD::Solver::initializeLocally()
+{
+	assert(false);
+}
+
+void MbD::Solver::initializeGlobally()
+{
+	assert(false);
+}
+
+void MbD::Solver::run()
+{
+	assert(false);
+}
diff --git a/MbDCode/Solver.h b/MbDCode/Solver.h
index 6e1ae32..9a6cc9d 100644
--- a/MbDCode/Solver.h
+++ b/MbDCode/Solver.h
@@ -3,8 +3,10 @@ namespace MbD {
 	class Solver
 	{
 	public:
-		virtual void initializeLocally() = 0;
-		virtual void initializeGlobally() = 0;
+		virtual void initialize();
+		virtual void initializeLocally();
+		virtual void initializeGlobally();
+		virtual void run();
 	};
 }
 
diff --git a/MbDCode/Sum.cpp b/MbDCode/Sum.cpp
index b39a51f..ef611fe 100644
--- a/MbDCode/Sum.cpp
+++ b/MbDCode/Sum.cpp
@@ -2,7 +2,7 @@
 
 using namespace MbD;
 
-double MbD::Sum::getValue()
+double Sum::getValue()
 {
 	double answer = 0.0;
 	for (int i = 0; i < terms->size(); i++) answer += terms->at(i)->getValue();
diff --git a/MbDCode/Symbolic.cpp b/MbDCode/Symbolic.cpp
index fc72fa9..03e7c40 100644
--- a/MbDCode/Symbolic.cpp
+++ b/MbDCode/Symbolic.cpp
@@ -9,19 +9,19 @@ using namespace MbD;
 
 Symbolic::Symbolic()
 {
-	initialize();
 }
 
 void Symbolic::initialize()
 {
 }
 
-std::shared_ptr<Symbolic> MbD::Symbolic::differentiateWRT(std::shared_ptr<Symbolic> var)
+std::shared_ptr<Symbolic> Symbolic::differentiateWRT(std::shared_ptr<Symbolic> var)
 {
+	assert(false);
 	return std::shared_ptr<Symbolic>();
 }
 
-std::shared_ptr<Symbolic> MbD::Symbolic::simplified()
+std::shared_ptr<Symbolic> Symbolic::simplified()
 {
 	//Debug 
 	auto set = nullptr;
@@ -31,12 +31,20 @@ std::shared_ptr<Symbolic> MbD::Symbolic::simplified()
 	return answer->simplifyUntil(set1);
 }
 
-std::shared_ptr<Symbolic> MbD::Symbolic::expandUntil(std::shared_ptr<std::unordered_set<Symbolic>> set)
+std::shared_ptr<Symbolic> Symbolic::expandUntil(std::shared_ptr<std::unordered_set<Symbolic>> set)
 {
+	assert(false);
 	return std::make_shared<Constant>(0.0);
 }
 
-std::shared_ptr<Symbolic> MbD::Symbolic::simplifyUntil(std::shared_ptr<std::unordered_set<Symbolic>> set)
+std::shared_ptr<Symbolic> Symbolic::simplifyUntil(std::shared_ptr<std::unordered_set<Symbolic>> set)
 {
+	assert(false);
 	return std::make_shared<Constant>(0.0);
 }
+
+double MbD::Symbolic::getValue()
+{
+	assert(false);
+	return 0.0;
+}
diff --git a/MbDCode/Symbolic.h b/MbDCode/Symbolic.h
index f48b118..ac34424 100644
--- a/MbDCode/Symbolic.h
+++ b/MbDCode/Symbolic.h
@@ -14,7 +14,7 @@ namespace MbD {
 		virtual std::shared_ptr<Symbolic> simplified();
 		virtual std::shared_ptr<Symbolic> expandUntil(std::shared_ptr<std::unordered_set<Symbolic>> set);
 		virtual std::shared_ptr<Symbolic> simplifyUntil(std::shared_ptr<std::unordered_set<Symbolic>> set);
-		virtual double getValue() = 0;
+		virtual double getValue();
 	};
 }
 
diff --git a/MbDCode/System.cpp b/MbDCode/System.cpp
index 423d2b3..58c5ee6 100644
--- a/MbDCode/System.cpp
+++ b/MbDCode/System.cpp
@@ -1,17 +1,23 @@
 #include<algorithm>
 
 #include "System.h"
+#include "Part.h"
+#include "Joint.h"
+#include "SystemSolver.h"
+#include "Time.h"
 
 using namespace MbD;
 
 System::System() {
-	time = std::make_shared<Time>();
-	parts = std::make_shared<std::vector<std::shared_ptr<Part>>>();
-	jointsMotions = std::make_shared<std::vector<std::shared_ptr<Joint>>>();
-	systemSolver = std::make_shared<SystemSolver>(this);
+	initialize();
 }
 
 System::System(const char* str) : Item(str) {
+	initialize();
+}
+
+void MbD::System::initialize()
+{
 	time = std::make_shared<Time>();
 	parts = std::make_shared<std::vector<std::shared_ptr<Part>>>();
 	jointsMotions = std::make_shared<std::vector<std::shared_ptr<Joint>>>();
@@ -26,31 +32,33 @@ void System::addPart(std::shared_ptr<Part> part)
 
 void System::runKINEMATICS()
 {
-	//Smalltalk code
-	//admSystem   preMbDrun.
-
-	//	[self initializeLocally.
-	//	self initializeGlobally.
-	//	self hasChanged]
-	//				whileTrue.
-	//	self partsJointsMotionsForcesTorquesDo : [:item | item postInput] .
-	//	admSystem outputFor : #INPUT.
-	//	mbdSystemSolver runAllIC.
-	//	admSystem outputFor : #'INITIAL CONDITIONS'.
-	//	mbdSystemSolver runBasicKinematic.
-	//	admSystem postMbDrun
-
 	while (true)
 	{
 		initializeLocally();
 		initializeGlobally();
 		if (!hasChanged) break;
 	}
+	postInput();
+	outputInput();
 	systemSolver->runAllIC();
+	outputInitialConditions();
 	systemSolver->runBasicKinematic();
+	outputTimeSeries();
 }
 
-void System::initializeLocally() 
+void MbD::System::outputInput()
+{
+}
+
+void MbD::System::outputInitialConditions()
+{
+}
+
+void MbD::System::outputTimeSeries()
+{
+}
+
+void System::initializeLocally()
 {
 	hasChanged = false;
 	time->value = systemSolver->tstart;
@@ -65,3 +73,14 @@ void System::initializeGlobally()
 	std::for_each(jointsMotions->begin(), jointsMotions->end(), [](const auto& joint) { joint->initializeGlobally();	});
 	systemSolver->initializeGlobally();
 }
+
+std::shared_ptr<std::vector<std::string>> MbD::System::discontinuitiesAtIC()
+{
+	return std::shared_ptr<std::vector<std::string>>();
+}
+
+void MbD::System::partsJointsMotionsDo(const std::function<void(std::shared_ptr<Item>)>& f)
+{
+	std::for_each(parts->begin(), parts->end(), f);
+	std::for_each(jointsMotions->begin(), jointsMotions->end(), f);
+}
diff --git a/MbDCode/System.h b/MbDCode/System.h
index bb2f2e1..ae24cfa 100644
--- a/MbDCode/System.h
+++ b/MbDCode/System.h
@@ -9,17 +9,16 @@
 #pragma once
 #include <memory>
 #include <vector>
+#include <functional>
 
 #include "Item.h"
-#include "Part.h"
-#include "Joint.h"
-#include "SystemSolver.h"
-#include "Time.h"
+//#include "Time.h"
 
 namespace MbD {
 	class Part;
 	class Joint;
 	class SystemSolver;
+	class Time;
 
 	class System : public Item
 	{
@@ -31,20 +30,26 @@ namespace MbD {
 			return singleInstance;
 		};
 		static System& getInstance(const char* str) {
-			//https://medium.com/@caglayandokme/further-enhancing-the-singleton-pattern-in-c-8278b02b1ac7
 			static System singleInstance(str); // Block-scoped static Singleton instance
 			return singleInstance;
 		};
 
+		void initialize() override;
+		void initializeLocally() override;
+		void initializeGlobally() override;
+		void runKINEMATICS();
+		void outputInput();
+		void outputInitialConditions();
+		void outputTimeSeries();
+		std::shared_ptr<std::vector<std::string>> discontinuitiesAtIC();
+		void partsJointsMotionsDo(const std::function <void(std::shared_ptr<Item>)>& f);
+
 
 		std::shared_ptr<std::vector<std::shared_ptr<Part>>> parts;
 		std::shared_ptr<std::vector<std::shared_ptr<Joint>>> jointsMotions;
 		bool hasChanged = false;
 		std::shared_ptr<SystemSolver> systemSolver;
 		void addPart(std::shared_ptr<Part> part);
-		void runKINEMATICS();
-		void initializeLocally() override;
-		void initializeGlobally() override;
 
 		std::shared_ptr<Time> time;
 	private:
diff --git a/MbDCode/SystemNewtonRaphson.cpp b/MbDCode/SystemNewtonRaphson.cpp
new file mode 100644
index 0000000..9e4c97d
--- /dev/null
+++ b/MbDCode/SystemNewtonRaphson.cpp
@@ -0,0 +1 @@
+#include "SystemNewtonRaphson.h"
diff --git a/MbDCode/SystemNewtonRaphson.h b/MbDCode/SystemNewtonRaphson.h
new file mode 100644
index 0000000..f1775fb
--- /dev/null
+++ b/MbDCode/SystemNewtonRaphson.h
@@ -0,0 +1,13 @@
+#pragma once
+
+#include "VectorNewtonRaphson.h"
+
+namespace MbD {
+    class SystemNewtonRaphson : public VectorNewtonRaphson
+    {
+        //
+    public:
+
+    };
+}
+
diff --git a/MbDCode/SystemSolver.cpp b/MbDCode/SystemSolver.cpp
index 5b810ee..6aa2029 100644
--- a/MbDCode/SystemSolver.cpp
+++ b/MbDCode/SystemSolver.cpp
@@ -1,7 +1,25 @@
+#include <vector>
+
 #include "SystemSolver.h"
+#include "NewtonRaphson.h"
+#include "PosICNewtonRaphson.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>>();
+}
+
 void SystemSolver::initializeLocally()
 {
 	setsOfRedundantConstraints = std::make_shared<std::vector<std::vector<std::shared_ptr<Constraint>>>>();
@@ -14,9 +32,66 @@ void SystemSolver::initializeGlobally()
 }
 
 void SystemSolver::runAllIC()
+{
+	while (true)
+	{
+		initializeLocally();
+		initializeGlobally();
+		runPosIC();
+		while (needToRedoPosIC())
+		{
+			runPosIC();
+		}
+		runVelIC();
+		runAccIC();
+		auto discontinuities = system->discontinuitiesAtIC();
+		if (discontinuities->size() == 0) break;
+		if (std::find(discontinuities->begin(), discontinuities->end(), "REBOUND") != discontinuities->end())
+		{
+			preCollision();
+			runCollisionDerivativeIC();
+			runBasicCollision();
+		}
+	}
+}
+
+void MbD::SystemSolver::runPosIC()
+{
+	icTypeSolver = std::make_shared<PosICNewtonRaphson>();
+	icTypeSolver->setSystem(this);
+	icTypeSolver->run();
+}
+
+void MbD::SystemSolver::runVelIC()
+{
+}
+
+void MbD::SystemSolver::runAccIC()
+{
+}
+
+bool MbD::SystemSolver::needToRedoPosIC()
+{
+	return false;
+}
+
+void MbD::SystemSolver::preCollision()
+{
+}
+
+void MbD::SystemSolver::runCollisionDerivativeIC()
+{
+}
+
+void MbD::SystemSolver::runBasicCollision()
 {
 }
 
 void SystemSolver::runBasicKinematic()
 {
 }
+
+void MbD::SystemSolver::partsJointsMotionsDo(const std::function<void(std::shared_ptr<Item>)>& f)
+{
+	system->partsJointsMotionsDo(f);
+}
diff --git a/MbDCode/SystemSolver.h b/MbDCode/SystemSolver.h
index d3e1ba7..e6dd8a2 100644
--- a/MbDCode/SystemSolver.h
+++ b/MbDCode/SystemSolver.h
@@ -1,37 +1,54 @@
 #pragma once
 #include <memory>
+#include <vector>
+#include<functional> 
 
 #include "Solver.h"
 #include "System.h"
 #include "Constraint.h"
-#include "NewtonRaphson.h"
+//#include "NewtonRaphson.h"
+#include "KineIntegrator.h"
 
 namespace MbD {
 	class System;
+	class NewtonRaphson;
+
 	class SystemSolver : public Solver
 	{
 		//system parts jointsMotions forcesTorques sensors variables icTypeSolver setsOfRedundantConstraints errorTolPosKine errorTolAccKine 
 		//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) {
 		}
+		void initialize() override;
 		void initializeLocally() override;
 		void initializeGlobally() override;
 		void runAllIC();
+		void runPosIC();
+		void runVelIC();
+		void runAccIC();
+		bool needToRedoPosIC();
+		void preCollision();
+		void runCollisionDerivativeIC();
+		void runBasicCollision();
 		void runBasicKinematic();
+		void partsJointsMotionsDo(const std::function <void(std::shared_ptr<Item>)>& f);
 
-		std::shared_ptr<NewtonRaphson> icTypeSolver;
 		System* system;
+		std::shared_ptr<NewtonRaphson> icTypeSolver;
 		std::shared_ptr<std::vector<std::vector<std::shared_ptr<Constraint>>>> setsOfRedundantConstraints;
 
-		double tstart = 0;
-		double tend = 25;
-		double toutFirst = 0.0;
 		double errorTolPosKine = 1.0e-6;
 		double errorTolAccKine = 1.0e-6;
 		int iterMaxPosKine = 25;
 		int iterMaxAccKine = 25;
+		std::shared_ptr <KineIntegrator> basicIntegrator;
+		std::shared_ptr<std::vector<double>> tstartPasts;
+		double tstart = 0;
+		double tend = 25;
+		double toutFirst = 0.0;
 		double hmin = 1.0e-9;
 		double hmax = 1.0;
 		double hout = 1.0e-1;
diff --git a/MbDCode/Time.h b/MbDCode/Time.h
index b5c4053..5d0685d 100644
--- a/MbDCode/Time.h
+++ b/MbDCode/Time.h
@@ -1,4 +1,5 @@
 #pragma once
+
 #include "IndependentVariable.h"
 
 namespace MbD {
diff --git a/MbDCode/TranslationConstraintIJ.cpp b/MbDCode/TranslationConstraintIJ.cpp
new file mode 100644
index 0000000..a6b023e
--- /dev/null
+++ b/MbDCode/TranslationConstraintIJ.cpp
@@ -0,0 +1,38 @@
+#include "TranslationConstraintIJ.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)
+{
+}
+
+void TranslationConstraintIJ::initialize()
+{
+}
+
+void MbD::TranslationConstraintIJ::initializeLocally()
+{
+    riIeJeIe->initializeLocally();
+}
+
+void MbD::TranslationConstraintIJ::initializeGlobally()
+{
+    riIeJeIe->initializeGlobally();
+}
+
+void MbD::TranslationConstraintIJ::initriIeJeIe()
+{
+    riIeJeIe = std::make_shared<DispCompIecJecKec>();
+}
+
+void MbD::TranslationConstraintIJ::postInput()
+{
+}
diff --git a/MbDCode/TranslationConstraintIJ.h b/MbDCode/TranslationConstraintIJ.h
new file mode 100644
index 0000000..7b02334
--- /dev/null
+++ b/MbDCode/TranslationConstraintIJ.h
@@ -0,0 +1,23 @@
+#pragma once
+
+#include "ConstraintIJ.h"
+#include "DispCompIecJecKec.h"
+
+namespace MbD {
+    class TranslationConstraintIJ : public ConstraintIJ
+    {
+        //riIeJeIe
+    public:
+        static std::shared_ptr<TranslationConstraintIJ> Create(EndFrmcptr frmi, EndFrmcptr frmj, int axisk);
+        TranslationConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj, int axisk);
+        void initialize();
+        void initializeLocally() override;
+        void initializeGlobally() override;
+        virtual void initriIeJeIe();
+        void postInput() override;
+
+        int axisK;
+        std::shared_ptr<DispCompIecJecKec> riIeJeIe;
+    };
+}
+
diff --git a/MbDCode/TranslationConstraintIqcJc.cpp b/MbDCode/TranslationConstraintIqcJc.cpp
new file mode 100644
index 0000000..fc92462
--- /dev/null
+++ b/MbDCode/TranslationConstraintIqcJc.cpp
@@ -0,0 +1,18 @@
+#include "TranslationConstraintIqcJc.h"
+#include "DispCompIeqcJecKeqc.h"
+
+using namespace MbD;
+
+TranslationConstraintIqcJc::TranslationConstraintIqcJc(EndFrmcptr frmi, EndFrmcptr frmj, int axisk) :
+	TranslationConstraintIJ(frmi, frmj, axisk)
+{
+}
+
+void TranslationConstraintIqcJc::initialize()
+{
+}
+
+void MbD::TranslationConstraintIqcJc::initriIeJeIe()
+{
+    riIeJeIe = std::make_shared<DispCompIeqcJecKeqc>();
+}
diff --git a/MbDCode/TranslationConstraintIqcJc.h b/MbDCode/TranslationConstraintIqcJc.h
new file mode 100644
index 0000000..973a81b
--- /dev/null
+++ b/MbDCode/TranslationConstraintIqcJc.h
@@ -0,0 +1,21 @@
+#pragma once
+
+#include "TranslationConstraintIJ.h"
+
+namespace MbD {
+    class TranslationConstraintIqcJc : public TranslationConstraintIJ
+    {
+        //pGpXI pGpEI ppGpXIpEI ppGpEIpEI iqXI iqEI 
+    public:
+        TranslationConstraintIqcJc(EndFrmcptr frmi, EndFrmcptr frmj, int axisk);
+        void initialize();
+        void initriIeJeIe() override;
+
+        FRowDsptr pGpXI;
+        FRowDsptr pGpEI;
+        FMatDsptr ppGpXIpEI;
+        FMatDsptr ppGpEIpEI;
+        int iqXI, iqEI;
+    };
+}
+
diff --git a/MbDCode/TranslationConstraintIqcJqc.cpp b/MbDCode/TranslationConstraintIqcJqc.cpp
new file mode 100644
index 0000000..a5acc52
--- /dev/null
+++ b/MbDCode/TranslationConstraintIqcJqc.cpp
@@ -0,0 +1,18 @@
+#include "TranslationConstraintIqcJqc.h"
+#include "DispCompIeqcJeqcKeqc.h"
+
+using namespace MbD;
+
+TranslationConstraintIqcJqc::TranslationConstraintIqcJqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisk) :
+	TranslationConstraintIqcJc(frmi, frmj, axisk)
+{
+}
+
+void TranslationConstraintIqcJqc::initialize()
+{
+}
+
+void MbD::TranslationConstraintIqcJqc::initriIeJeIe()
+{
+	riIeJeIe = std::make_shared<DispCompIeqcJeqcKeqc>();
+}
diff --git a/MbDCode/TranslationConstraintIqcJqc.h b/MbDCode/TranslationConstraintIqcJqc.h
new file mode 100644
index 0000000..61c8526
--- /dev/null
+++ b/MbDCode/TranslationConstraintIqcJqc.h
@@ -0,0 +1,22 @@
+#pragma once
+
+#include "TranslationConstraintIqcJc.h"
+
+namespace MbD {
+    class TranslationConstraintIqcJqc : public TranslationConstraintIqcJc
+    {
+        //pGpXJ pGpEJ ppGpEIpXJ ppGpEIpEJ ppGpEJpEJ iqXJ iqEJ 
+    public:
+        TranslationConstraintIqcJqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisk);
+        void initialize();
+        void initriIeJeIe() override;
+
+        FRowDsptr pGpXJ;
+        FRowDsptr pGpEJ;
+        FMatDsptr ppGpEIpXJ;
+        FMatDsptr ppGpEIpEJ;
+        FMatDsptr ppGpEJpEJ;
+        int iqXJ, iqEJ;
+    };
+}
+
diff --git a/MbDCode/TranslationConstraintIqctJqc.cpp b/MbDCode/TranslationConstraintIqctJqc.cpp
new file mode 100644
index 0000000..26cad0f
--- /dev/null
+++ b/MbDCode/TranslationConstraintIqctJqc.cpp
@@ -0,0 +1,18 @@
+#include "TranslationConstraintIqctJqc.h"
+#include "DispCompIeqctJeqcKeqct.h"
+
+using namespace MbD;
+
+TranslationConstraintIqctJqc::TranslationConstraintIqctJqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisk) :
+	TranslationConstraintIqcJqc(frmi, frmj, axisk)
+{
+}
+
+void TranslationConstraintIqctJqc::initialize()
+{
+}
+
+void MbD::TranslationConstraintIqctJqc::initriIeJeIe()
+{
+	riIeJeIe = std::make_shared<DispCompIeqctJeqcKeqct>();
+}
diff --git a/MbDCode/TranslationConstraintIqctJqc.h b/MbDCode/TranslationConstraintIqctJqc.h
new file mode 100644
index 0000000..0695918
--- /dev/null
+++ b/MbDCode/TranslationConstraintIqctJqc.h
@@ -0,0 +1,22 @@
+#pragma once
+
+#include "TranslationConstraintIqcJqc.h"
+
+namespace MbD {
+    class TranslationConstraintIqctJqc : public TranslationConstraintIqcJqc
+    {
+        //pGpt ppGpXIpt ppGpEIpt ppGpXJpt ppGpEJpt ppGptpt 
+    public:
+        TranslationConstraintIqctJqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisk);
+        void initialize();
+        void initriIeJeIe() override;
+
+        double pGpt;
+        FRowDsptr ppGpXIpt;
+        FRowDsptr ppGpEIpt;
+        FRowDsptr ppGpXJpt;
+        FRowDsptr ppGpEJpt;
+        double ppGptpt;
+    };
+}
+
diff --git a/MbDCode/Variable.cpp b/MbDCode/Variable.cpp
index cdc41e8..0730a76 100644
--- a/MbDCode/Variable.cpp
+++ b/MbDCode/Variable.cpp
@@ -30,7 +30,7 @@ const std::string& Variable::getName() const
 	return name;
 }
 
-double MbD::Variable::getValue()
+double Variable::getValue()
 {
 	return value;
 }
diff --git a/MbDCode/VectorNewtonRaphson.cpp b/MbDCode/VectorNewtonRaphson.cpp
new file mode 100644
index 0000000..b453e01
--- /dev/null
+++ b/MbDCode/VectorNewtonRaphson.cpp
@@ -0,0 +1,10 @@
+#include "VectorNewtonRaphson.h"
+
+void MbD::VectorNewtonRaphson::run()
+{
+		//self preRun.
+		//self initializeLocally.
+		//self initializeGlobally.
+		//self iterate.
+		//self postRun.
+}
diff --git a/MbDCode/VectorNewtonRaphson.h b/MbDCode/VectorNewtonRaphson.h
new file mode 100644
index 0000000..a406b8c
--- /dev/null
+++ b/MbDCode/VectorNewtonRaphson.h
@@ -0,0 +1,17 @@
+#pragma once
+
+#include "NewtonRaphson.h"
+#include "MatrixSolver.h"
+
+namespace MbD {
+    class VectorNewtonRaphson : public NewtonRaphson
+    {
+        //matrixSolver n
+    public:
+        void run() override;
+
+        std::shared_ptr<MatrixSolver> matrixSolver;
+        int n;
+    };
+}
+
diff --git a/MbDCode/ZRotation.cpp b/MbDCode/ZRotation.cpp
index 09087ad..5fb2e60 100644
--- a/MbDCode/ZRotation.cpp
+++ b/MbDCode/ZRotation.cpp
@@ -8,18 +8,14 @@
 using namespace MbD;
 
 ZRotation::ZRotation() {
-	initialize();
+
 }
 
 ZRotation::ZRotation(const char* str) : PrescribedMotion(str) {
-	initialize();
+
 }
 
-void ZRotation::initialize()
-{
-}
-
-void MbD::ZRotation::initializeGlobally()
+void ZRotation::initializeGlobally()
 {
 	//constraints isEmpty
 	//	ifTrue :
@@ -38,7 +34,7 @@ void MbD::ZRotation::initializeGlobally()
 	}
 }
 
-void MbD::ZRotation::initMotions()
+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));
@@ -46,6 +42,6 @@ void MbD::ZRotation::initMotions()
 	std::static_pointer_cast<EndFrameqc>(frmI)->setphiThePsiBlks(std::make_shared<FullColumn<std::shared_ptr<Symbolic>>>(xyzRotBlks));
 }
 
-void MbD::ZRotation::addConstraint(std::shared_ptr<Constraint> con)
+void ZRotation::addConstraint(std::shared_ptr<Constraint> con)
 {
 }
diff --git a/MbDCode/ZRotation.h b/MbDCode/ZRotation.h
index 27e92ec..a47be28 100644
--- a/MbDCode/ZRotation.h
+++ b/MbDCode/ZRotation.h
@@ -6,9 +6,9 @@ namespace MbD {
 	{
 		//
 	public:
+		static std::shared_ptr<ZRotation> Create(const char* name);
 		ZRotation();
 		ZRotation(const char* str);
-		void initialize();
 		void initializeGlobally() override;
 		void initMotions();
 		void addConstraint(std::shared_ptr<Constraint> con);