Merge pull request #2 from Ondsel-Development/working

MatrixSolver NewtonRaphson

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()
+{
+}

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;
     };

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;
+    };
+}
+

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()
+{
+}

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;
+    };
+}
+

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()
+{
+}

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;
+    };
+}
+

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()
+{
+}

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;
+    };
+}
+

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()
+{
+}

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;
+    };
+}
+

MbDCode/BasicIntegrator.cpp

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

MbDCode/BasicIntegrator.h

@@ -0,0 +1,13 @@
+#pragma once
+
+#include "Integrator.h"
+
+namespace MbD {
+    class BasicIntegrator : public Integrator
+    {
+        //
+    public:
+
+    };
+}
+

MbDCode/BasicQuasiIntegrator.cpp

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

MbDCode/BasicQuasiIntegrator.h

@@ -0,0 +1,13 @@
+#pragma once
+
+#include "BasicIntegrator.h"
+
+namespace MbD {
+    class BasicQuasiIntegrator : public BasicIntegrator
+    {
+        //
+    public:
+
+    };
+}
+

MbDCode/CREATE.cpp

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

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;
+        }
+    };
+}
+

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);
 }

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;

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();
 		

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;
 }

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;

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();
+	}
 }

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();
     };
 }
 

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 } {}
     };
 }
 

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);
 }

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;

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()
+{
+}

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;
+    };
+}
+

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()
+{
+}

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;
+    };
+}
+

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()
+{
+}

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;
+	};
+}

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()
+{
+}

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;

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()
+{
+}

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;

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()
+{
+}

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;

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()
+{
+}

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;

MbDCode/DispCompIecJecKec.cpp

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

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;
+    };
+}
+

MbDCode/DispCompIecJecKeqc.cpp

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

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;
+    };
+}
+

MbDCode/DispCompIecJecO.cpp

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

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;
+	};
+}
+

MbDCode/DispCompIeqcJecKeqc.cpp

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

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;
+    };
+}
+

MbDCode/DispCompIeqcJecO.cpp

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

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;
+    };
+}
+

MbDCode/DispCompIeqcJeqcKeqc.cpp

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

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;
+    };
+}
+

MbDCode/DispCompIeqcJeqcKeqct.cpp

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

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;
+    };
+}
+

MbDCode/DispCompIeqcJeqcO.cpp

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

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;
+    };
+}
+

MbDCode/DispCompIeqctJeqcKeqct.cpp

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

MbDCode/DispCompIeqctJeqcKeqct.h

@@ -0,0 +1,13 @@
+#pragma once
+
+#include "DispCompIeqcJeqcKeqct.h"
+
+namespace MbD {
+    class DispCompIeqctJeqcKeqct : public DispCompIeqcJeqcKeqct
+    {
+        //
+    public:
+
+    };
+}
+

MbDCode/DispCompIeqctJeqcO.cpp

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

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;
+    };
+}
+

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()
 {
 }

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);

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()
+{
+}

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;

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()
 {
 }

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;

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 } {}
+
     };
 }
 

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()
+{
+}

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;
 	};
 }

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;
-}

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;
+	}
 }
 

MbDCode/EulerParametersDot.cpp

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

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>>();
+    }
+
+}
+

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>>>>;
 }
 

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>>>>;
 }
 

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));

MbDCode/FunctionWithManyArgs.h

@@ -3,6 +3,8 @@
 #include "Function.h"
 
 namespace MbD {
+    class Symbolic;
+
     class FunctionWithManyArgs : public Function
     {
         //terms

MbDCode/GESpMat3.cpp

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

MbDCode/GESpMat3.h

@@ -0,0 +1,13 @@
+#pragma once
+
+#include "MatrixGaussElimination.h"
+
+namespace MbD {
+    class GESpMat : public MatrixGaussElimination
+    {
+        //
+    public:
+
+    };
+}
+

MbDCode/GESpMatFullPv.cpp

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

MbDCode/GESpMatFullPv.h

@@ -0,0 +1,13 @@
+#pragma once
+
+#include "GESpMat3.h"
+
+namespace MbD {
+    class GESpMatFullPv : public GESpMat
+    {
+        //
+    public:
+
+    };
+}
+

MbDCode/GESpMatFullPvPosIC.cpp

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

MbDCode/GESpMatFullPvPosIC.h

@@ -0,0 +1,13 @@
+#pragma once
+
+#include "GESpMatFullPv.h"
+
+namespace MbD {
+    class GESpMatFullPvPosIC : public GESpMatFullPv
+    {
+        //
+    public:
+
+    };
+}
+

MbDCode/GESpMatParPv.cpp

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

MbDCode/GESpMatParPv.h

@@ -0,0 +1,13 @@
+#pragma once
+
+#include "GESpMat3.h"
+
+namespace MbD {
+    class GESpMatParPv : public GESpMat
+    {
+        //
+    public:
+
+    };
+}
+

MbDCode/GESpMatParPvMarko.cpp

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

MbDCode/GESpMatParPvMarko.h

@@ -0,0 +1,13 @@
+#pragma once
+
+#include "GESpMatParPv.h"
+
+namespace MbD {
+    class GESpMatParPvMarko : public GESpMatParPv
+    {
+        //
+    public:
+
+    };
+}
+

MbDCode/GESpMatParPvMarkoFast.cpp

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

MbDCode/GESpMatParPvMarkoFast.h

@@ -0,0 +1,13 @@
+#pragma once
+
+#include "GESpMatParPvMarko.h"
+
+namespace MbD {
+    class GESpMatParPvMarkoFast : public GESpMatParPvMarko
+    {
+        //
+    public:
+
+    };
+}
+

MbDCode/GESpMatParPvPrecise.cpp

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

MbDCode/GESpMatParPvPrecise.h

@@ -0,0 +1,13 @@
+#pragma once
+
+#include "GESpMatParPv.h"
+
+namespace MbD {
+    class GESpMatParPvPrecise : public GESpMatParPv
+    {
+        //
+    public:
+
+    };
+}
+

MbDCode/IndependentVariable.cpp

@@ -1 +1,7 @@
 #include "IndependentVariable.h"
+
+using namespace MbD;
+
+MbD::IndependentVariable::IndependentVariable()
+{
+}

MbDCode/IndependentVariable.h

@@ -1,9 +1,12 @@
 #pragma once
+
 #include "Variable.h"
+
 namespace MbD {
-    class IndependentVariable :
-        public Variable
+    class IndependentVariable : public Variable
     {
+    public:
+        IndependentVariable();
     };
 }
 

MbDCode/Integrator.cpp

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

MbDCode/Integrator.h

@@ -0,0 +1,13 @@
+#pragma once
+
+#include "Solver.h"
+
+namespace MbD {
+    class Integrator : public Solver
+    {
+        //
+    public:
+
+    };
+}
+

MbDCode/IntegratorInterface.cpp

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

MbDCode/IntegratorInterface.h

@@ -0,0 +1,13 @@
+#pragma once
+
+#include "Integrator.h"
+
+namespace MbD {
+    class IntegratorInterface : public Integrator
+    {
+        //
+    public:
+
+    };
+}
+

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)
 {
 }