runPosIC, VelIC, AccIC numerically correct

MbDCode/AbsConstraint.cpp

@@ -48,3 +48,22 @@ void MbD::AbsConstraint::fillPosKineJacob(SpMatDsptr mat)
 {
     mat->atijplusNumber(iG, iqXminusOnePlusAxis, 1.0);
 }
+
+void MbD::AbsConstraint::fillVelICJacob(SpMatDsptr mat)
+{
+    this->fillPosICJacob(mat);
+}
+
+void MbD::AbsConstraint::fillAccICIterError(FColDsptr col)
+{
+    col->atiplusNumber(iqXminusOnePlusAxis, lam);
+    auto partFrame = static_cast<PartFrame*>(owner);
+        double sum;
+        if (axis < 3) {
+            sum = partFrame->qXddot->at(axis);
+        }
+        else {
+            sum = partFrame->qEddot->at(axis - 3);
+        }
+        col->atiplusNumber(iG, sum);
+}

MbDCode/AbsConstraint.h

@@ -8,12 +8,14 @@ namespace MbD {
         //AbsConstraint();
         //AbsConstraint(const char* str);
         AbsConstraint(int axis);
-        void initialize();
+        void initialize() override;
         void calcPostDynCorrectorIteration() override;
         void useEquationNumbers() override;
         void fillPosICJacob(SpMatDsptr mat) override;
         void fillPosICError(FColDsptr col) override;
         void fillPosKineJacob(SpMatDsptr mat) override;
+        void fillVelICJacob(SpMatDsptr mat) override;
+        void fillAccICIterError(FColDsptr col) override;
 
         int axis = -1;
         int iqXminusOnePlusAxis = -1;

MbDCode/AccICKineNewtonRaphson.cpp

@@ -0,0 +1,16 @@
+#include "AccICKineNewtonRaphson.h"
+#include "SystemSolver.h"
+
+void MbD::AccICKineNewtonRaphson::initializeGlobally()
+{
+	AccNewtonRaphson::initializeGlobally();
+	iterMax = system->iterMaxAccKine;
+	dxTol = system->errorTolAccKine;
+}
+
+void MbD::AccICKineNewtonRaphson::preRun()
+{
+	std::string str("MbD: Solving for quasi kinematic acceleration.");
+	system->logString(str);
+	AccNewtonRaphson::preRun();
+}

MbDCode/AccICKineNewtonRaphson.h

@@ -0,0 +1,16 @@
+#pragma once
+
+#include "AccNewtonRaphson.h"
+
+namespace MbD {
+    class AccICKineNewtonRaphson : public AccNewtonRaphson
+    {
+        //
+    public:
+        void initializeGlobally() override;
+        void preRun() override;
+
+
+    };
+}
+

MbDCode/AccICNewtonRaphson.cpp

@@ -0,0 +1,14 @@
+#include "AccICNewtonRaphson.h"
+#include "SystemSolver.h"
+
+bool MbD::AccICNewtonRaphson::isConverged()
+{
+	return this->isConvergedToNumericalLimit();
+}
+
+void MbD::AccICNewtonRaphson::preRun()
+{
+	std::string str("MbD: Solving for quasi kinematic acceleration.");
+	system->logString(str);
+	AccNewtonRaphson::preRun();
+}

MbDCode/AccICNewtonRaphson.h

@@ -0,0 +1,16 @@
+#pragma once
+
+#include "AccNewtonRaphson.h"
+
+namespace MbD {
+    class AccICNewtonRaphson : public AccNewtonRaphson
+    {
+        //
+    public:
+        bool isConverged() override;
+        void preRun() override;
+
+
+    };
+}
+

MbDCode/AccKineNewtonRaphson.cpp

@@ -0,0 +1,16 @@
+#include "AccKineNewtonRaphson.h"
+#include "SystemSolver.h"
+
+void MbD::AccKineNewtonRaphson::initializeGlobally()
+{
+	AccNewtonRaphson::initializeGlobally();
+	iterMax = system->iterMaxAccKine;
+	dxTol = system->errorTolAccKine;
+}
+
+void MbD::AccKineNewtonRaphson::preRun()
+{
+	std::string str("MbD: Solving for kinematic acceleration.");
+	system->logString(str);
+	AccNewtonRaphson::preRun();
+}

MbDCode/AccKineNewtonRaphson.h

@@ -0,0 +1,16 @@
+#pragma once
+
+#include "AccNewtonRaphson.h"
+
+namespace MbD {
+    class AccKineNewtonRaphson : public AccNewtonRaphson
+    {
+        //
+    public:
+        void initializeGlobally() override;
+        void preRun() override;
+
+
+    };
+}
+

MbDCode/AccNewtonRaphson.cpp

@@ -0,0 +1,112 @@
+#include "AccNewtonRaphson.h"
+#include "SystemSolver.h"
+#include "Part.h"
+#include "Constraint.h"
+#include "SimulationStoppingError.h"
+#include <iostream>
+
+void MbD::AccNewtonRaphson::askSystemToUpdate()
+{
+	system->partsJointsMotionsForcesTorquesDo([&](std::shared_ptr<Item> item) { item->postAccICIteration(); });
+}
+
+void MbD::AccNewtonRaphson::assignEquationNumbers()
+{
+	auto parts = system->parts();
+	//auto contactEndFrames = system->contactEndFrames();
+	//auto uHolders = system->uHolders();
+	auto constraints = system->allConstraints();
+	int eqnNo = 0;
+	for (auto& part : *parts) {
+		part->iqX(eqnNo);
+		eqnNo = eqnNo + 3;
+		part->iqE(eqnNo);
+		eqnNo = eqnNo + 4;
+	}
+	//for (auto& endFrm : *contactEndFrames) {
+	//	endFrm->is(eqnNo);
+	//	eqnNo = eqnNo + endFrm->sSize();
+	//}
+	//for (auto& uHolder : *uHolders) {
+	//	uHolder->iu(eqnNo);
+	//	eqnNo += 1;
+	//}
+	auto nEqns = eqnNo;	//C++ uses index 0.
+	for (auto& con : *constraints) {
+		con->iG = eqnNo;
+		eqnNo += 1;
+	}
+	auto lastEqnNo = eqnNo - 1;
+	nEqns = eqnNo;	//C++ uses index 0.
+	n = nEqns;
+}
+
+void MbD::AccNewtonRaphson::fillPyPx()
+{
+	pypx->zeroSelf();
+	system->partsJointsMotionsForcesTorquesDo([&](std::shared_ptr<Item> item) {
+		item->fillAccICIterJacob(pypx);
+		});
+}
+
+void MbD::AccNewtonRaphson::fillY()
+{
+	y->zeroSelf();
+	system->partsJointsMotionsForcesTorquesDo([&](std::shared_ptr<Item> item) {
+		item->fillAccICIterError(y);
+		//std::cout << item->getName() << *y << std::endl;
+		});
+	//std::cout << *y << std::endl;
+}
+
+void MbD::AccNewtonRaphson::incrementIterNo()
+{
+	if (iterNo >= iterMax)
+	{
+		std::stringstream ss;
+		ss << "MbD: No convergence after " << iterNo << " iterations.";
+		auto str = ss.str();
+		system->logString(str);
+		ss.str("");
+		ss << "A force function of joint reactions can cause this problem";
+		str = ss.str();
+		system->logString(str);
+		ss.str("");
+		ss << "if the function returns large values.";
+		str = ss.str();
+		system->logString(str);
+
+		throw SimulationStoppingError("");
+	}
+
+	iterNo++;
+}
+
+void MbD::AccNewtonRaphson::initializeGlobally()
+{
+	SystemNewtonRaphson::initializeGlobally();
+	system->partsJointsMotionsDo([&](std::shared_ptr<Item> item) { item->fillqsuddotlam(x); });
+}
+
+void MbD::AccNewtonRaphson::logSingularMatrixMessage()
+{
+	std::string str = "MbD: Some parts with zero masses or moment of inertias have infinite accelerations.";
+	system->logString(str);
+	str = "Add masses and inertias to or properly constrain those parts.";
+	system->logString(str);
+}
+
+void MbD::AccNewtonRaphson::passRootToSystem()
+{
+	system->partsJointsMotionsDo([&](std::shared_ptr<Item> item) { item->setqsuddotlam(x); });
+}
+
+void MbD::AccNewtonRaphson::postRun()
+{
+	system->partsJointsMotionsForcesTorquesDo([&](std::shared_ptr<Item> item) { item->postAccIC(); });
+}
+
+void MbD::AccNewtonRaphson::preRun()
+{
+	system->partsJointsMotionsForcesTorquesDo([&](std::shared_ptr<Item> item) { item->preAccIC(); });
+}

MbDCode/AccNewtonRaphson.h

@@ -0,0 +1,24 @@
+#pragma once
+
+#include "SystemNewtonRaphson.h"
+
+namespace MbD {
+    class AccNewtonRaphson : public SystemNewtonRaphson
+    {
+        //
+    public:
+        void askSystemToUpdate() override;
+        void assignEquationNumbers() override;
+        void fillPyPx() override;
+        void fillY() override;
+        void incrementIterNo() override;
+        void initializeGlobally() override;
+        void logSingularMatrixMessage();
+        void passRootToSystem();
+        void postRun() override;
+        void preRun() override;
+
+
+    };
+}
+

MbDCode/Array.h

@@ -17,6 +17,7 @@ namespace MbD {
 		Array(int count, const T& value) : std::vector<T>(count, value) {}
 		Array(std::vector<T>::iterator begin, std::vector<T>::iterator end) : std::vector<T>(begin, end) {}
 		Array(std::initializer_list<T> list) : std::vector<T>{ list } {}
+		virtual void initialize();
 		void copyFrom(std::shared_ptr<Array<T>> x);
 		virtual void zeroSelf();
 		virtual double sumOfSquares() = 0;
@@ -29,6 +30,10 @@ namespace MbD {
 
 	};
 	template<typename T>
+	inline void Array<T>::initialize()
+	{
+	}
+	template<typename T>
 	inline void Array<T>::copyFrom(std::shared_ptr<Array<T>> x)
 	{
 		for (int i = 0; i < x->size(); i++) {

MbDCode/AtPointConstraintIJ.cpp

@@ -63,3 +63,9 @@ void MbD::AtPointConstraintIJ::preVelIC()
 	riIeJeO->preVelIC();
 	Item::preVelIC();
 }
+
+void MbD::AtPointConstraintIJ::preAccIC()
+{
+	riIeJeO->preAccIC();
+	Constraint::preAccIC();
+}

MbDCode/AtPointConstraintIJ.h

@@ -10,7 +10,7 @@ namespace MbD {
         //axis riIeJeO 
     public:
         AtPointConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj, int axisi);
-        void initialize();
+        void initialize() override;
         void initializeLocally() override;
         void initializeGlobally() override;
         virtual void initriIeJeO();
@@ -20,6 +20,8 @@ namespace MbD {
         MbD::ConstraintType type() override;
         void postPosICIteration() override;
         void preVelIC() override;
+        void preAccIC() override;
+        
 
         int axis;
         std::shared_ptr<DispCompIecJecO> riIeJeO;

MbDCode/AtPointConstraintIqcJc.cpp

@@ -54,3 +54,23 @@ void MbD::AtPointConstraintIqcJc::fillPosKineJacob(SpMatDsptr mat)
 	mat->atijplusNumber(iG, iqXIminusOnePlusAxis, -1.0);
 	mat->atijplusFullRow(iG, iqEI, pGpEI);
 }
+
+void MbD::AtPointConstraintIqcJc::fillVelICJacob(SpMatDsptr mat)
+{
+	mat->atijplusNumber(iG, iqXIminusOnePlusAxis, -1.0);
+	mat->atijplusNumber(iqXIminusOnePlusAxis, iG, -1.0);
+	mat->atijplusFullRow(iG, iqEI, pGpEI);
+	mat->atijplusFullColumn(iqEI, iG, pGpEI->transpose());
+}
+
+void MbD::AtPointConstraintIqcJc::fillAccICIterError(FColDsptr col)
+{
+	col->atiminusNumber(iqXIminusOnePlusAxis, lam);
+	col->atiplusFullVectortimes(iqEI, pGpEI, lam);
+	auto efrmIqc = std::static_pointer_cast<EndFrameqc>(frmI);
+	auto qEdotI = efrmIqc->qEdot();
+	auto sum = -efrmIqc->qXddot()->at(axis);
+	sum += pGpEI->timesFullColumn(efrmIqc->qEddot());
+	sum += qEdotI->transposeTimesFullColumn(ppGpEIpEI->timesFullColumn(qEdotI));
+	col->atiplusNumber(iG, sum);
+}

MbDCode/AtPointConstraintIqcJc.h

@@ -15,6 +15,8 @@ namespace MbD {
 		void fillPosICError(FColDsptr col) override;
 		void fillPosICJacob(SpMatDsptr mat) override;
 		void fillPosKineJacob(SpMatDsptr mat) override;
+		void fillVelICJacob(SpMatDsptr mat) override;
+		void fillAccICIterError(FColDsptr col) override;
 
 		FRowDsptr pGpEI;
 		FMatDsptr ppGpEIpEI;

MbDCode/AtPointConstraintIqcJqc.cpp

@@ -57,3 +57,25 @@ void MbD::AtPointConstraintIqcJqc::fillPosKineJacob(SpMatDsptr mat)
 	mat->atijplusNumber(iG, iqXJminusOnePlusAxis, 1.0);
 	mat->atijplusFullRow(iG, iqEJ, pGpEJ);
 }
+
+void MbD::AtPointConstraintIqcJqc::fillVelICJacob(SpMatDsptr mat)
+{
+	AtPointConstraintIqcJc::fillVelICJacob(mat);
+	mat->atijplusNumber(iG, iqXJminusOnePlusAxis, 1.0);
+	mat->atijplusNumber(iqXJminusOnePlusAxis, iG, 1.0);
+	mat->atijplusFullRow(iG, iqEJ, pGpEJ);
+	mat->atijplusFullColumn(iqEJ, iG, pGpEJ->transpose());
+}
+
+void MbD::AtPointConstraintIqcJqc::fillAccICIterError(FColDsptr col)
+{
+	AtPointConstraintIqcJc::fillAccICIterError(col);
+	col->atiplusNumber(iqXJminusOnePlusAxis, lam);
+	col->atiplusFullVectortimes(iqEJ, pGpEJ, lam);
+	auto efrmJqc = std::static_pointer_cast<EndFrameqc>(frmJ);
+	auto qEdotJ = efrmJqc->qEdot();
+	auto sum = efrmJqc->qXddot()->at(axis);
+	sum += pGpEJ->timesFullColumn(efrmJqc->qEddot());
+	sum += qEdotJ->transposeTimesFullColumn(ppGpEJpEJ->timesFullColumn(qEdotJ));
+	col->atiplusNumber(iG, sum);
+}

MbDCode/AtPointConstraintIqcJqc.h

@@ -15,6 +15,8 @@ namespace MbD {
         void fillPosICError(FColDsptr col) override;
         void fillPosICJacob(SpMatDsptr mat) override;
         void fillPosKineJacob(SpMatDsptr mat) override;
+        void fillVelICJacob(SpMatDsptr mat) override;
+        void fillAccICIterError(FColDsptr col) override;
 
         FRowDsptr pGpEJ;
         FMatDsptr ppGpEJpEJ;

MbDCode/AtPointConstraintIqctJqc.cpp

@@ -38,3 +38,25 @@ void MbD::AtPointConstraintIqctJqc::preVelIC()
 	AtPointConstraintIJ::preVelIC();
 	pGpt = std::static_pointer_cast<DispCompIeqctJeqcO>(riIeJeO)->priIeJeOpt;
 }
+
+void MbD::AtPointConstraintIqctJqc::fillVelICError(FColDsptr col)
+{
+	col->atiminusNumber(iG, pGpt);
+}
+
+void MbD::AtPointConstraintIqctJqc::fillAccICIterError(FColDsptr col)
+{
+	AtPointConstraintIqcJqc::fillAccICIterError(col);
+	auto efrmIqc = std::static_pointer_cast<EndFrameqc>(frmI);
+	auto qEdotI = efrmIqc->qEdot();
+	double sum = (ppGpEIpt->timesFullColumn(qEdotI)) * 2.0;
+	sum += ppGptpt;
+	col->atiplusNumber(iG, sum);
+}
+
+void MbD::AtPointConstraintIqctJqc::preAccIC()
+{
+	AtPointConstraintIJ::preAccIC();
+	ppGpEIpt = std::static_pointer_cast<DispCompIeqctJeqcO>(riIeJeO)->ppriIeJeOpEIpt;
+	ppGptpt = std::static_pointer_cast<DispCompIeqctJeqcO>(riIeJeO)->ppriIeJeOptpt;
+}

MbDCode/AtPointConstraintIqctJqc.h

@@ -13,10 +13,15 @@ namespace MbD {
         void calcPostDynCorrectorIteration() override;
         MbD::ConstraintType type() override;
         void preVelIC() override;
+        void fillVelICError(FColDsptr col) override;
+        void fillAccICIterError(FColDsptr col) override;
 
         double pGpt;
         FRowDsptr ppGpEIpt;
         double ppGptpt;
+        void preAccIC() override;
+
+
     };
 }
 

MbDCode/BasicIntegrator.h

@@ -19,7 +19,7 @@ namespace MbD {
         void initializeLocally() override;
         virtual void iStep(int i);
         virtual void postFirstStep();
-        virtual void postRun() override;
+        void postRun() override;
         virtual void postStep();
         virtual void preFirstStep();
         virtual void preRun() override;

MbDCode/CREATE.h

@@ -1,6 +1,7 @@
+//This header file causes wierd problems in Visual Studio when included in subclasses of std::vector or std::map.
+
 #pragma once
 #include <memory>
-//#include "EndFramec.h"
 #include "EndFrameqct.h"
 #include "AtPointConstraintIqctJqc.h"
 #include "DirectionCosineConstraintIqctJqc.h"
@@ -28,6 +29,11 @@ namespace MbD {
 			inst->initialize();
 			return inst;
 		}
+		static std::shared_ptr<T> With(int m, int n) {
+			auto inst = std::make_shared<T>(m, n);
+			inst->initialize();
+			return inst;
+		}
 		static std::shared_ptr<T> With(std::initializer_list<double> listD) {
 			auto inst = std::make_shared<T>(listD);
 			inst->initialize();

MbDCode/CartesianFrame.h

@@ -7,7 +7,7 @@ namespace MbD {
 	public:
 		CartesianFrame();
 		CartesianFrame(const char* str);
-		void initialize();
+		void initialize() override;
 	};
 }
 

MbDCode/Constraint.cpp

@@ -99,6 +99,11 @@ void MbD::Constraint::setqsulam(FColDsptr col)
 	lam = col->at(iG);
 }
 
+void MbD::Constraint::setqsudotlam(FColDsptr col)
+{
+	lam = col->at(iG);
+}
+
 void MbD::Constraint::fillPosICError(FColDsptr col)
 {
 	col->at(iG) += aG;
@@ -141,3 +146,20 @@ void MbD::Constraint::fillPosKineError(FColDsptr col)
 {
 	col->atiplusNumber(iG, aG);
 }
+
+void MbD::Constraint::preAccIC()
+{
+	lam = 0.0;
+	Item::preAccIC();
+}
+
+void MbD::Constraint::fillAccICIterJacob(SpMatDsptr mat)
+{
+	//"Same as velIC."
+	this->fillVelICJacob(mat);
+}
+
+void MbD::Constraint::setqsuddotlam(FColDsptr qsudotlam)
+{
+	lam = qsudotlam->at(iG);
+}

MbDCode/Constraint.h

@@ -11,7 +11,7 @@ namespace MbD {
 	public:
 		Constraint();
 		Constraint(const char* str);
-		void initialize();
+		void initialize() override;
 		void postInput() override;
 		void setOwner(Item* x);
 		Item* getOwner();
@@ -25,6 +25,7 @@ namespace MbD {
 		virtual MbD::ConstraintType type();
 		void fillqsulam(FColDsptr col) override;
 		void setqsulam(FColDsptr col) override;
+		void setqsudotlam(FColDsptr col) override;
 		void fillPosICError(FColDsptr col) override;
 		void removeRedundantConstraints(std::shared_ptr<std::vector<int>> redundantEqnNos) override;
 		void reactivateRedundantConstraints() override;
@@ -32,6 +33,9 @@ namespace MbD {
 		void outputStates() override;
 		void preDyn() override;
 		void fillPosKineError(FColDsptr col) override;
+		void preAccIC() override;
+		void fillAccICIterJacob(SpMatDsptr mat) override;
+		void setqsuddotlam(FColDsptr qsudotlam) override;
 
 		int iG = -1;
 		double aG = 0.0;		//Constraint function

MbDCode/ConstraintIJ.h

@@ -9,7 +9,7 @@ namespace MbD {
         //frmI frmJ aConstant 
     public:
         ConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj);
-        void initialize();
+        void initialize() override;
 
         EndFrmcptr frmI, frmJ;
         double aConstant;

MbDCode/DiagonalMatrix.h

@@ -2,6 +2,7 @@
 
 #include "Array.h"
 #include "FullColumn.h"
+#include "FullMatrix.h"
 
 namespace MbD {
 	template<typename T>
@@ -10,9 +11,12 @@ namespace MbD {
 		//
 	public:
 		DiagonalMatrix(int count) : Array<T>(count) {}
+		DiagonalMatrix(int count, const T& value) : Array<T>(count, value) {}
 		DiagonalMatrix(std::initializer_list<T> list) : Array<T>{ list } {}
 		void atiputDiagonalMatrix(int i, std::shared_ptr < DiagonalMatrix<T>> diagMat);
+		std::shared_ptr<DiagonalMatrix<T>> times(T factor);
 		std::shared_ptr<FullColumn<T>> timesFullColumn(std::shared_ptr<FullColumn<T>> fullCol);
+		std::shared_ptr<FullMatrix<T>> timesFullMatrix(std::shared_ptr<FullMatrix<T>> fullMat);
 		int nrow() {
 			return (int) this->size();
 		}
@@ -32,6 +36,17 @@ namespace MbD {
 		}
 	}
 	template<typename T>
+	inline std::shared_ptr<DiagonalMatrix<T>> DiagonalMatrix<T>::times(T factor)
+	{
+		auto nrow = (int)this->size();
+		auto answer = std::make_shared<DiagonalMatrix<T>>(nrow);
+		for (int i = 0; i < nrow; i++)
+		{
+			answer->at(i) = this->at(i) * factor;
+		}
+		return answer;
+	}
+	template<typename T>
 	inline std::shared_ptr<FullColumn<T>> DiagonalMatrix<T>::timesFullColumn(std::shared_ptr<FullColumn<T>> fullCol)
 	{
 		//"a*b = a(i,j)b(j) sum j."
@@ -44,6 +59,17 @@ namespace MbD {
 		}
 		return answer;
 	}
+	template<typename T>
+	inline std::shared_ptr<FullMatrix<T>> DiagonalMatrix<T>::timesFullMatrix(std::shared_ptr<FullMatrix<T>> fullMat)
+	{
+		auto nrow = (int)this->size();
+		auto answer = std::make_shared<FullMatrix<T>>(nrow);
+		for (int i = 0; i < nrow; i++)
+		{
+			answer->at(i) = fullMat->at(i)->times(this->at(i));
+		}
+		return answer;
+	}
 	template<>
 	inline double DiagonalMatrix<double>::sumOfSquares()
 	{

MbDCode/DifferenceOperator.h

@@ -10,7 +10,7 @@ namespace MbD {
 		//iStep order taylorMatrix operatorMatrix time timeNodes 
 	public:
 		void calcOperatorMatrix();
-		void initialize();
+		virtual void initialize();
 		virtual void setiStep(int i);
 		virtual void setorder(int o);
 		virtual void formTaylorMatrix() = 0;

MbDCode/DirectionCosineConstraintIJ.cpp

@@ -64,3 +64,9 @@ void MbD::DirectionCosineConstraintIJ::preVelIC()
 	aAijIeJe->preVelIC();
 	Item::preVelIC();
 }
+
+void MbD::DirectionCosineConstraintIJ::preAccIC()
+{
+	aAijIeJe->preAccIC();
+	Constraint::preAccIC();
+}

MbDCode/DirectionCosineConstraintIJ.h

@@ -10,7 +10,7 @@ namespace MbD {
         //axisI axisJ aAijIeJe 
     public:
         DirectionCosineConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj);
-        void initialize();
+        void initialize() override;
         void initializeLocally() override;
         void initializeGlobally() override;
         virtual void initaAijIeJe();
@@ -20,6 +20,7 @@ namespace MbD {
         void postPosICIteration() override;
         MbD::ConstraintType type() override;
         void preVelIC() override;
+        void preAccIC() override;
 
         int axisI, axisJ;
         std::shared_ptr<DirectionCosineIecJec> aAijIeJe;

MbDCode/DirectionCosineConstraintIqcJc.cpp

@@ -45,3 +45,19 @@ void MbD::DirectionCosineConstraintIqcJc::fillPosKineJacob(SpMatDsptr mat)
 {
 	mat->atijplusFullRow(iG, iqEI, pGpEI);
 }
+
+void MbD::DirectionCosineConstraintIqcJc::fillVelICJacob(SpMatDsptr mat)
+{
+	mat->atijplusFullRow(iG, iqEI, pGpEI);
+	mat->atijplusFullColumn(iqEI, iG, pGpEI->transpose());
+}
+
+void MbD::DirectionCosineConstraintIqcJc::fillAccICIterError(FColDsptr col)
+{
+	col->atiplusFullVector(iqEI, pGpEI->times(lam));
+	auto efrmIqc = std::static_pointer_cast<EndFrameqc>(frmI);
+	auto qEdotI = efrmIqc->qEdot();
+	auto sum = pGpEI->timesFullColumn(efrmIqc->qEddot());
+	sum += qEdotI->transposeTimesFullColumn(ppGpEIpEI->timesFullColumn(qEdotI));
+	col->atiplusNumber(iG, sum);
+}

MbDCode/DirectionCosineConstraintIqcJc.h

@@ -14,6 +14,8 @@ namespace MbD {
         void fillPosICError(FColDsptr col) override;
         void fillPosICJacob(SpMatDsptr mat) override;
         void fillPosKineJacob(SpMatDsptr mat) override;
+        void fillVelICJacob(SpMatDsptr mat) override;
+        void fillAccICIterError(FColDsptr col) override;
 
         FRowDsptr pGpEI;
         FMatDsptr ppGpEIpEI;

MbDCode/DirectionCosineConstraintIqcJqc.cpp

@@ -52,3 +52,24 @@ void MbD::DirectionCosineConstraintIqcJqc::fillPosKineJacob(SpMatDsptr mat)
 	DirectionCosineConstraintIqcJc::fillPosKineJacob(mat);
 	mat->atijplusFullRow(iG, iqEJ, pGpEJ);
 }
+
+void MbD::DirectionCosineConstraintIqcJqc::fillVelICJacob(SpMatDsptr mat)
+{
+	DirectionCosineConstraintIqcJc::fillVelICJacob(mat);
+	mat->atijplusFullRow(iG, iqEJ, pGpEJ);
+	mat->atijplusFullColumn(iqEJ, iG, pGpEJ->transpose());
+}
+
+void MbD::DirectionCosineConstraintIqcJqc::fillAccICIterError(FColDsptr col)
+{
+	DirectionCosineConstraintIqcJc::fillAccICIterError(col);
+	col->atiplusFullVectortimes(iqEJ, pGpEJ, lam);
+	auto efrmIqc = std::static_pointer_cast<EndFrameqc>(frmI);
+	auto efrmJqc = std::static_pointer_cast<EndFrameqc>(frmJ);
+	auto qEdotI = efrmIqc->qEdot();
+	auto qEdotJ = efrmJqc->qEdot();
+	double sum = pGpEJ->timesFullColumn(efrmJqc->qEddot());
+	sum += (qEdotI->transposeTimesFullColumn(ppGpEIpEJ->timesFullColumn(qEdotJ))) * 2.0;
+	sum += qEdotJ->transposeTimesFullColumn(ppGpEJpEJ->timesFullColumn(qEdotJ));
+	col->atiplusNumber(iG, sum);
+}

MbDCode/DirectionCosineConstraintIqcJqc.h

@@ -14,6 +14,8 @@ namespace MbD {
         void fillPosICError(FColDsptr col) override;
         void fillPosICJacob(SpMatDsptr mat) override;
         void fillPosKineJacob(SpMatDsptr mat) override;
+        void fillVelICJacob(SpMatDsptr mat) override;
+        void fillAccICIterError(FColDsptr col) override;
 
         FRowDsptr pGpEJ;
         FMatDsptr ppGpEIpEJ;

MbDCode/DirectionCosineConstraintIqctJqc.cpp

@@ -24,3 +24,29 @@ void MbD::DirectionCosineConstraintIqctJqc::preVelIC()
 	DirectionCosineConstraintIJ::preVelIC();
 	pGpt = std::static_pointer_cast<DirectionCosineIeqctJeqc>(aAijIeJe)->pAijIeJept;
 }
+
+void MbD::DirectionCosineConstraintIqctJqc::fillVelICError(FColDsptr col)
+{
+	col->atiminusNumber(iG, pGpt);
+}
+
+void MbD::DirectionCosineConstraintIqctJqc::preAccIC()
+{
+	DirectionCosineConstraintIJ::preAccIC();
+	ppGpEIpt = std::static_pointer_cast<DirectionCosineIeqctJeqc>(aAijIeJe)->ppAijIeJepEIpt;
+	ppGpEJpt = std::static_pointer_cast<DirectionCosineIeqctJeqc>(aAijIeJe)->ppAijIeJepEJpt;
+	ppGptpt = std::static_pointer_cast<DirectionCosineIeqctJeqc>(aAijIeJe)->ppAijIeJeptpt;
+}
+
+void MbD::DirectionCosineConstraintIqctJqc::fillAccICIterError(FColDsptr col)
+{
+	DirectionCosineConstraintIqcJqc::fillAccICIterError(col);
+	auto efrmIqc = std::static_pointer_cast<EndFrameqc>(frmI);
+	auto efrmJqc = std::static_pointer_cast<EndFrameqc>(frmJ);
+	auto qEdotI = efrmIqc->qEdot();
+	auto qEdotJ = efrmJqc->qEdot();
+	double sum = (ppGpEIpt->timesFullColumn(qEdotI)) * 2.0;
+	sum += (ppGpEJpt->timesFullColumn(qEdotJ)) * 2.0;
+	sum += ppGptpt;
+	col->atiplusNumber(iG, sum);
+}

MbDCode/DirectionCosineConstraintIqctJqc.h

@@ -11,6 +11,9 @@ namespace MbD {
 		void initaAijIeJe() override;
 		MbD::ConstraintType type() override;
 		void preVelIC() override;
+		void fillVelICError(FColDsptr col) override;
+		void preAccIC() override;
+		void fillAccICIterError(FColDsptr col) override;
 
 		double pGpt;
 		FRowDsptr ppGpEIpt;

MbDCode/DirectionCosineIeqcJec.h

@@ -9,7 +9,7 @@ namespace MbD {
     public:
         DirectionCosineIeqcJec();
         DirectionCosineIeqcJec(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj);
-        void initialize();
+        void initialize() override;
         void initializeGlobally();
         void calcPostDynCorrectorIteration() override;
 

MbDCode/DirectionCosineIeqcJeqc.h

@@ -9,7 +9,7 @@ namespace MbD {
     public:
         DirectionCosineIeqcJeqc();
         DirectionCosineIeqcJeqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj);
-        void initialize();
+        void initialize() override;
         void initializeGlobally();
         void calcPostDynCorrectorIteration() override;
         FRowDsptr pvaluepEJ() override;

MbDCode/DirectionCosineIeqctJeqc.cpp

@@ -44,3 +44,22 @@ double MbD::DirectionCosineIeqctJeqc::pvaluept()
 {
 	return pAijIeJept;
 }
+
+void MbD::DirectionCosineIeqctJeqc::preAccIC()
+{
+	//| ppAjOIepEITpt ppAjOIeptpt ppAjOIepEITpti pAjOIept |
+	Item::preAccIC();
+	auto pAjOIept = std::static_pointer_cast<EndFrameqct>(frmI)->pAjOept(axisI);
+	auto ppAjOIepEITpt = std::static_pointer_cast<EndFrameqct>(frmI)->ppAjOepETpt(axisI);
+	auto ppAjOIeptpt = std::static_pointer_cast<EndFrameqct>(frmI)->ppAjOeptpt(axisI);
+	for (int i = 0; i < 4; i++)
+	{
+		auto& ppAjOIepEITpti = ppAjOIepEITpt->at(i);
+		ppAijIeJepEIpt->atiput(i, ppAjOIepEITpti->dot(aAjOJe));
+	}
+	for (int i = 0; i < 4; i++)
+	{
+		ppAijIeJepEJpt->atiput(i, pAjOIept->dot(pAjOJepEJT->at(i)));
+	}
+	ppAijIeJeptpt = ppAjOIeptpt->dot(aAjOJe);
+}

MbDCode/DirectionCosineIeqctJeqc.h

@@ -9,11 +9,12 @@ namespace MbD {
     public:
         DirectionCosineIeqctJeqc();
         DirectionCosineIeqctJeqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj);
-        void initialize();
+        void initialize() override;
         void initializeGlobally();
         void calcPostDynCorrectorIteration() override;
         void preVelIC() override;
         double pvaluept();
+        void preAccIC() override;
 
         double pAijIeJept;
         FRowDsptr ppAijIeJepEIpt;

MbDCode/DispCompIeqcJeqcKeqct.cpp

@@ -41,3 +41,55 @@ double MbD::DispCompIeqcJeqcKeqct::pvaluept()
 {
 	return priIeJeKept;
 }
+
+FRowDsptr MbD::DispCompIeqcJeqcKeqct::ppvaluepXIpt()
+{
+	return ppriIeJeKepXIpt;
+}
+
+FRowDsptr MbD::DispCompIeqcJeqcKeqct::ppvaluepEIpt()
+{
+	return ppriIeJeKepEIpt;
+}
+
+FRowDsptr MbD::DispCompIeqcJeqcKeqct::ppvaluepEKpt()
+{
+	return ppriIeJeKepEKpt;
+}
+
+FRowDsptr MbD::DispCompIeqcJeqcKeqct::ppvaluepXJpt()
+{
+	return ppriIeJeKepXJpt;
+}
+
+FRowDsptr MbD::DispCompIeqcJeqcKeqct::ppvaluepEJpt()
+{
+	return ppriIeJeKepEJpt;
+}
+
+double MbD::DispCompIeqcJeqcKeqct::ppvalueptpt()
+{
+	return ppriIeJeKeptpt;
+}
+
+void MbD::DispCompIeqcJeqcKeqct::preAccIC()
+{
+	Item::preAccIC();
+	auto pAjOKept = std::static_pointer_cast<EndFrameqct>(efrmK)->pAjOept(axisK);
+	auto ppAjOKepEKTpt = std::static_pointer_cast<EndFrameqct>(efrmK)->ppAjOepETpt(axisK);
+	auto ppAjOKeptpt = std::static_pointer_cast<EndFrameqct>(efrmK)->ppAjOeptpt(axisK);
+	auto prIeJeOpEIT = std::static_pointer_cast<EndFrameqc>(frmI)->prOeOpE->transpose()->negated();
+	auto prIeJeOpEJT = std::static_pointer_cast<EndFrameqc>(frmJ)->prOeOpE->transpose();
+	for (int i = 0; i < 3; i++)
+	{
+		ppriIeJeKepXIpt->atiput(i, -(pAjOKept->at(i)));
+		ppriIeJeKepXJpt->atiput(i, pAjOKept->at(i));
+	}
+	for (int i = 0; i < 4; i++)
+	{
+		ppriIeJeKepEIpt->atiput(i, pAjOKept->dot(prIeJeOpEIT->at(i)));
+		ppriIeJeKepEJpt->atiput(i, pAjOKept->dot(prIeJeOpEJT->at(i)));
+		ppriIeJeKepEKpt->atiput(i, ppAjOKepEKTpt->at(i)->dot(rIeJeO));
+	}
+	ppriIeJeKeptpt = ppAjOKeptpt->dot(rIeJeO);
+}

MbDCode/DispCompIeqcJeqcKeqct.h

@@ -13,6 +13,13 @@ namespace MbD {
         void calcPostDynCorrectorIteration() override;
         void preVelIC() override;
         double pvaluept();
+        FRowDsptr ppvaluepXIpt();
+        FRowDsptr ppvaluepEIpt();
+        FRowDsptr ppvaluepEKpt();
+        FRowDsptr ppvaluepXJpt();
+        FRowDsptr ppvaluepEJpt();
+        double ppvalueptpt();
+        void preAccIC() override;
 
         double priIeJeKept;
         FRowDsptr ppriIeJeKepXIpt;

MbDCode/DispCompIeqctJeqcKeqct.cpp

@@ -14,6 +14,22 @@ MbD::DispCompIeqctJeqcKeqct::DispCompIeqctJeqcKeqct(EndFrmcptr frmi, EndFrmcptr
 void MbD::DispCompIeqctJeqcKeqct::preVelIC()
 {
 	DispCompIeqcJeqcKeqct::preVelIC();
-	auto mprIeJeOpt = std::static_pointer_cast<EndFrameqct>(frmI)->prOeOpt;
+	auto& mprIeJeOpt = std::static_pointer_cast<EndFrameqct>(frmI)->prOeOpt;
 	priIeJeKept -= aAjOKe->dot(mprIeJeOpt);
 }
+
+void MbD::DispCompIeqctJeqcKeqct::preAccIC()
+{
+	DispCompIeqcJeqcKeqct::preAccIC();
+	auto pAjOKept = std::static_pointer_cast<EndFrameqct>(efrmK)->pAjOept(axisK);
+	auto efrmIqct = std::static_pointer_cast<EndFrameqct>(frmI);
+	auto& mprIeJeOpt = efrmIqct->prOeOpt;
+	auto mpprIeJeOpEITpt = efrmIqct->pprOeOpEpt->transpose();
+	auto& mpprIeJeOptpt = efrmIqct->pprOeOptpt;
+	for (int i = 0; i < 4; i++)
+	{
+		ppriIeJeKepEIpt->atiminusNumber(i, aAjOKe->dot(mpprIeJeOpEITpt->at(i)));
+		ppriIeJeKepEKpt->atiminusNumber(i, pAjOKepEKT->at(i)->dot(mprIeJeOpt));
+	}
+	ppriIeJeKeptpt +=  -(2.0 * pAjOKept->dot(mprIeJeOpt)) - aAjOKe->dot(mpprIeJeOptpt);
+}

MbDCode/DispCompIeqctJeqcKeqct.h

@@ -10,6 +10,7 @@ namespace MbD {
         DispCompIeqctJeqcKeqct();
         DispCompIeqctJeqcKeqct(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk);
         void preVelIC() override;
+        void preAccIC() override;
 
     };
 }

MbDCode/DispCompIeqctJeqcO.cpp

@@ -34,3 +34,10 @@ double MbD::DispCompIeqctJeqcO::pvaluept()
 {
 	return priIeJeOpt;
 }
+
+void MbD::DispCompIeqctJeqcO::preAccIC()
+{
+	Item::preAccIC();
+	ppriIeJeOpEIpt = (std::static_pointer_cast<EndFrameqct>(frmI)->ppriOeOpEpt(axis))->negated();
+	ppriIeJeOptpt = -(std::static_pointer_cast<EndFrameqct>(frmI)->ppriOeOptpt(axis));
+}

MbDCode/DispCompIeqctJeqcO.h

@@ -13,6 +13,7 @@ namespace MbD {
         void calcPostDynCorrectorIteration() override;
         void preVelIC() override;
         double pvaluept();
+        void preAccIC() override;
 
         double priIeJeOpt;
         FRowDsptr ppriIeJeOpEIpt;

MbDCode/EndFramec.h

@@ -19,7 +19,7 @@ namespace MbD {
     public:
         EndFramec();
         EndFramec(const char* str);
-        void initialize();
+        void initialize() override;
         void setMarkerFrame(MarkerFrame* markerFrm);
         MarkerFrame* getMarkerFrame();
         void initializeLocally() override;

MbDCode/EndFrameqc.cpp

@@ -101,3 +101,23 @@ FRowDsptr MbD::EndFrameqc::priOeOpE(int i)
 {
 	return prOeOpE->at(i);
 }
+
+FColDsptr MbD::EndFrameqc::qXdot()
+{
+	return markerFrame->qXdot();
+}
+
+std::shared_ptr<EulerParametersDot<double>> MbD::EndFrameqc::qEdot()
+{
+	return markerFrame->qEdot();
+}
+
+FColDsptr MbD::EndFrameqc::qXddot()
+{
+	return markerFrame->qXddot();
+}
+
+FColDsptr MbD::EndFrameqc::qEddot()
+{
+	return markerFrame->qEddot();
+}

MbDCode/EndFrameqc.h

@@ -2,6 +2,8 @@
 
 #include "EndFramec.h"
 #include "Symbolic.h"
+#include "EulerParametersDot.h"
+#include "EulerParametersDDot.h"
 
 namespace MbD {
     class EndFrameqct;
@@ -12,7 +14,7 @@ namespace MbD {
     public:
         EndFrameqc();
         EndFrameqc(const char* str);
-        void initialize();
+        void initialize() override;
         void initializeGlobally() override;
         void initEndFrameqct() override;
         FMatFColDsptr ppAjOepEpE(int j);
@@ -22,6 +24,10 @@ namespace MbD {
         int iqX();
         int iqE();
         FRowDsptr priOeOpE(int i);
+        FColDsptr qXdot();
+        std::shared_ptr<EulerParametersDot<double>> qEdot();
+        FColDsptr qXddot();
+        FColDsptr qEddot();
 
         FMatDsptr prOeOpE;
         std::shared_ptr<FullMatrix<std::shared_ptr<FullColumn<double>>>> pprOeOpEpE;

MbDCode/EndFrameqct.cpp

@@ -7,6 +7,7 @@
 #include "CREATE.h"
 #include "EulerAngleszxz.h"
 #include "EulerAngleszxzDot.h"
+#include "EulerAngleszxzDDot.h"
 
 using namespace MbD;
 
@@ -71,6 +72,14 @@ void EndFrameqct::initprmemptBlks()
 
 void EndFrameqct::initpprmemptptBlks()
 {
+	auto& mbdTime = System::getInstance().time;
+	pprmemptptBlks = std::make_shared< FullColumn<Symsptr>>(3);
+	for (int i = 0; i < 3; i++) {
+		auto& vel = prmemptBlks->at(i);
+		auto var = vel->differentiateWRT(vel, mbdTime);
+		auto acc = var->simplified(var);
+		pprmemptptBlks->at(i) = acc;
+	}
 }
 
 void EndFrameqct::initpPhiThePsiptBlks()
@@ -89,6 +98,14 @@ void EndFrameqct::initpPhiThePsiptBlks()
 
 void MbD::EndFrameqct::initppPhiThePsiptptBlks()
 {
+	auto& mbdTime = System::getInstance().time;
+	ppPhiThePsiptptBlks = std::make_shared< FullColumn<Symsptr>>(3);
+	for (int i = 0; i < 3; i++) {
+		auto& angleVel = pPhiThePsiptBlks->at(i);
+		auto var = angleVel->differentiateWRT(angleVel, mbdTime);
+		auto angleAcc = var->simplified(var);
+		ppPhiThePsiptptBlks->at(i) = angleAcc;
+	}
 }
 
 void MbD::EndFrameqct::postInput()
@@ -169,21 +186,70 @@ void MbD::EndFrameqct::preVelIC()
 	Item::preVelIC();
 	this->evalprmempt();
 	this->evalpAmept();
-	auto aAOm = markerFrame->aAOm;
+	auto& aAOm = markerFrame->aAOm;
 	prOeOpt = aAOm->timesFullColumn(prmempt);
 	pAOept = aAOm->timesFullMatrix(pAmept);
 }
 
+void MbD::EndFrameqct::postVelIC()
+{
+	auto& pAOmpE = markerFrame->pAOmpE;
+	for (int i = 0; i < 3; i++)
+	{
+		auto& pprOeOpEpti = pprOeOpEpt->at(i);
+			for (int j = 0; j < 4; j++)
+		{
+				auto pprOeOpEptij = pAOmpE->at(j)->at(i)->dot(prmempt);
+				pprOeOpEpti->atiput(j, pprOeOpEptij);
+		}
+	}
+	for (int i = 0; i < 4; i++)
+	{
+		ppAOepEpt->atiput(i, pAOmpE->at(i)->timesFullMatrix(pAmept));
+	}
+}
+
 FColDsptr MbD::EndFrameqct::pAjOept(int j)
 {
 	return pAOept->column(j);
 }
 
+FMatDsptr MbD::EndFrameqct::ppAjOepETpt(int jj)
+{
+	auto answer = std::make_shared<FullMatrix<double>>(4, 3);
+	for (int i = 0; i < 4; i++)
+	{
+		auto& answeri = answer->at(i);
+		auto& ppAOepEipt = ppAOepEpt->at(i);
+		for (int j = 0; j < 3; j++)
+		{
+			auto& answerij = ppAOepEipt->at(j)->at(jj);
+			answeri->atiput(j, answerij);
+		}
+	}
+	return answer;
+}
+
+FColDsptr MbD::EndFrameqct::ppAjOeptpt(int j)
+{
+	return ppAOeptpt->column(j);
+}
+
 double MbD::EndFrameqct::priOeOpt(int i)
 {
 	return prOeOpt->at(i);
 }
 
+FRowDsptr MbD::EndFrameqct::ppriOeOpEpt(int i)
+{
+	return pprOeOpEpt->at(i);
+}
+
+double MbD::EndFrameqct::ppriOeOptpt(int i)
+{
+	return pprOeOptpt->at(i);
+}
+
 void MbD::EndFrameqct::evalprmempt()
 {
 	if (rmemBlks) {
@@ -216,3 +282,61 @@ void MbD::EndFrameqct::evalpAmept()
 		pAmept = phiThePsiDot->aAdot;
 	}
 }
+
+void MbD::EndFrameqct::evalpprmemptpt()
+{
+	if (rmemBlks) {
+		for (int i = 0; i < 3; i++)
+		{
+			auto& secondDerivative = pprmemptptBlks->at(i);
+			auto value = secondDerivative->getValue();
+			pprmemptpt->atiput(i, value);
+		}
+	}
+}
+
+void MbD::EndFrameqct::evalppAmeptpt()
+{
+	if (phiThePsiBlks) {
+		auto phiThePsi = CREATE<EulerAngleszxz<double>>::With();
+		auto phiThePsiDot = CREATE<EulerAngleszxzDot<double>>::With();
+		phiThePsiDot->phiThePsi = phiThePsi;
+		auto phiThePsiDDot = CREATE<EulerAngleszxzDDot<double>>::With();
+		phiThePsiDDot->phiThePsiDot = phiThePsiDot;
+		for (int i = 0; i < 3; i++)
+		{
+			auto& expression = phiThePsiBlks->at(i);
+			auto& derivative = pPhiThePsiptBlks->at(i);
+			auto& secondDerivative = ppPhiThePsiptptBlks->at(i);
+			auto value = expression->getValue();
+			auto valueDot = derivative->getValue();
+			auto valueDDot = secondDerivative->getValue();
+			phiThePsi->atiput(i, value);
+			phiThePsiDot->atiput(i, valueDot);
+			phiThePsiDDot->atiput(i, valueDDot);
+		}
+		phiThePsi->calc();
+		phiThePsiDot->calc();
+		phiThePsiDDot->calc();
+		ppAmeptpt = phiThePsiDDot->aAddot;
+	}
+}
+
+void MbD::EndFrameqct::preAccIC()
+{
+	time = System::getInstance().mbdTimeValue();
+	this->evalrmem();
+	this->evalAme();
+	Item::preVelIC();
+	this->evalprmempt();
+	this->evalpAmept();
+	auto& aAOm = markerFrame->aAOm;
+	prOeOpt = aAOm->timesFullColumn(prmempt);
+	pAOept = aAOm->timesFullMatrix(pAmept);
+	Item::preAccIC();
+	this->evalpprmemptpt();
+	this->evalppAmeptpt();
+	aAOm = markerFrame->aAOm;
+	pprOeOptpt = aAOm->timesFullColumn(pprmemptpt);
+	ppAOeptpt = aAOm->timesFullMatrix(ppAmeptpt);
+}

MbDCode/EndFrameqct.h

@@ -12,7 +12,7 @@ namespace MbD {
 	public:
 		EndFrameqct();
 		EndFrameqct(const char* str);
-		void initialize();
+		void initialize() override;
 		void initializeLocally() override;
 		void initializeGlobally() override;
 		void initprmemptBlks();
@@ -25,11 +25,20 @@ namespace MbD {
 		void evalrmem();
 		void evalAme();
 		void preVelIC() override;
+		void postVelIC() override;
 		FColDsptr pAjOept(int j);
+		FMatDsptr ppAjOepETpt(int j);
+		FColDsptr ppAjOeptpt(int j);
 		double time = 0.0;
 		double priOeOpt(int i);
+		FRowDsptr ppriOeOpEpt(int i);
+		double ppriOeOptpt(int i);
 		void evalprmempt();
 		void evalpAmept();
+		void evalpprmemptpt();
+		void evalppAmeptpt();
+		
+		void preAccIC() override;
 
 		std::shared_ptr<FullColumn<Symsptr>> rmemBlks, prmemptBlks, pprmemptptBlks;
 		std::shared_ptr<FullColumn<Symsptr>> phiThePsiBlks, pPhiThePsiptBlks, ppPhiThePsiptptBlks;

MbDCode/EulerAngleszxz.h

@@ -13,7 +13,7 @@ namespace MbD {
 	public:
 		EulerAngleszxz() : EulerArray<T>(3) {}
 		void initialize() override;
-		void calc();
+		void calc() override;
 
 		FMatDsptr phiA, theA, psiA, aA;
 	};

MbDCode/EulerAngleszxzDDot.h

@@ -10,9 +10,79 @@ namespace MbD {
     {
         //phiThePsiDot phiAddot theAddot psiAddot aAddot 
     public:
+        EulerAngleszxzDDot() : EulerArray<T>(3) {}
+        void initialize() override;
+        void calc() override;
 
         std::shared_ptr<EulerAngleszxzDot<double>> phiThePsiDot;
         FMatDsptr phiAddot, theAddot, psiAddot, aAddot;
     };
+    template<typename T>
+    inline void EulerAngleszxzDDot<T>::initialize()
+    {
+        phiAddot = std::make_shared<FullMatrix<double>>(3, 3);
+        phiAddot->zeroSelf();
+        theAddot = std::make_shared<FullMatrix<double>>(3, 3);
+        theAddot->zeroSelf();
+        psiAddot = std::make_shared<FullMatrix<double>>(3, 3);
+        psiAddot->zeroSelf();
+    }
+    template<typename T>
+    inline void EulerAngleszxzDDot<T>::calc()
+    {
+		//| zero phiThePsi phi sphi cphi phidot phiddot cphiddot sphiddot the sthe cthe thedot theddot ctheddot stheddot 
+		// psi spsi cpsi psidot psiddot cpsiddot spsiddot phiA theA psiA phiAdot theAdot psiAdot |
+		auto zero = 0.0;
+		auto& phiThePsi = phiThePsiDot->phiThePsi;
+		auto& phi = phiThePsi->at(0);
+		auto sphi = std::sin(phi);
+		auto cphi = std::cos(phi);
+		auto& phidot = phiThePsiDot->at(0);
+		auto& phiddot = this->at(0);
+		auto cphiddot = zero - (sphi * phiddot) - (cphi * phidot * phidot);
+		auto sphiddot = cphi * phiddot - (sphi * phidot * phidot);
+		auto& the = phiThePsi->at(1);
+		auto sthe = std::sin(the);
+		auto cthe = std::cos(the);
+		auto& thedot = phiThePsiDot->at(1);
+		auto& theddot = this->at(1);
+		auto ctheddot = zero - (sthe * theddot) - (cthe * thedot * thedot);
+		auto stheddot = cthe * theddot - (sthe * thedot * thedot);
+		auto& psi = phiThePsi->at(2);
+		auto spsi = std::sin(psi);
+		auto cpsi = std::cos(psi);
+		auto& psidot = phiThePsiDot->at(2);
+		auto& psiddot = this->at(2);
+		auto cpsiddot = zero - (spsi * psiddot) - (cpsi * psidot * psidot);
+		auto spsiddot = cpsi * psiddot - (spsi * psidot * psidot);
+		phiAddot->at(0)->atiput(0, cphiddot);
+		phiAddot->at(0)->atiput(1, zero - sphiddot);
+		phiAddot->at(1)->atiput(0, sphiddot);
+		phiAddot->at(1)->atiput(1, cphiddot);
+		theAddot->at(1)->atiput(1, ctheddot);
+		theAddot->at(1)->atiput(2, zero - stheddot);
+		theAddot->at(2)->atiput(1, stheddot);
+		theAddot->at(2)->atiput(2, ctheddot);
+		psiAddot->at(0)->atiput(0, cpsiddot);
+		psiAddot->at(0)->atiput(1, zero - spsiddot);
+		psiAddot->at(1)->atiput(0, spsiddot);
+		psiAddot->at(1)->atiput(1, cpsiddot);
+		auto& phiA = phiThePsi->phiA;
+		auto& theA = phiThePsi->theA;
+		auto& psiA = phiThePsi->psiA;
+		auto& phiAdot = phiThePsiDot->phiAdot;
+		auto& theAdot = phiThePsiDot->theAdot;
+		auto& psiAdot = phiThePsiDot->psiAdot;
+		auto mat = *(phiAddot->timesFullMatrix(theA->timesFullMatrix(psiA)))
+			+ *(phiAdot->timesFullMatrix(theAdot->timesFullMatrix(psiA)))
+			+ *(phiAdot->timesFullMatrix(theA->timesFullMatrix(psiAdot)))
+			+ *(phiAdot->timesFullMatrix(theAdot->timesFullMatrix(psiA)))
+			+ *(phiA->timesFullMatrix(theAddot->timesFullMatrix(psiA)))
+			+ *(phiA->timesFullMatrix(theAdot->timesFullMatrix(psiAdot)))
+			+ *(phiAdot->timesFullMatrix(theA->timesFullMatrix(psiAdot)))
+			+ *(phiA->timesFullMatrix(theAdot->timesFullMatrix(psiAdot)))
+			+ *(phiA->timesFullMatrix(theA->timesFullMatrix(psiAddot)));
+		aAddot = std::make_shared<FullMatrix<double>>(mat);
+	}
 }
 

MbDCode/EulerAngleszxzDot.h

@@ -12,7 +12,7 @@ namespace MbD {
 	public:
 		EulerAngleszxzDot() : EulerArray<T>(3) {}
 		void initialize() override;
-		void calc();
+		void calc() override;
 
 		std::shared_ptr<EulerAngleszxz<T>> phiThePsi;
 		FMatDsptr phiAdot, theAdot, psiAdot, aAdot;
@@ -30,7 +30,6 @@ namespace MbD {
 	template<typename T>
 	inline void EulerAngleszxzDot<T>::calc()
 	{
-		//| zero phi sphi cphi phidot minussphiTimesphidot cphiTimesphidot the sthe cthe thedot minusstheTimesthedot ctheTimesthedot psi spsi cpsi psidot minusspsiTimespsidot cpsiTimespsidot phiA theA psiA |
 		auto phi = phiThePsi->at(0);
 		auto sphi = std::sin(phi);
 		auto cphi = std::cos(phi);

MbDCode/EulerArray.h

@@ -11,9 +11,10 @@ namespace MbD {
         EulerArray(int count) : FullColumn<T>(count) {}
         EulerArray(int count, const T& value) : FullColumn<T>(count, value) {}
         EulerArray(std::initializer_list<T> list) : FullColumn<T>{ list } {}
-        virtual void initialize();
+        void initialize() override;
         void equalFullColumn(std::shared_ptr<FullColumn<T>> fullCol);
         void equalFullColumnAt(std::shared_ptr<FullColumn<T>> fullCol, int i);
+        virtual void calc() = 0;
 
     };
     template<typename T>

MbDCode/EulerConstraint.cpp

@@ -57,3 +57,21 @@ void MbD::EulerConstraint::fillPosKineJacob(SpMatDsptr mat)
 {
 	mat->atijplusFullRow(iG, iqE, pGpE);
 }
+
+void MbD::EulerConstraint::fillVelICJacob(SpMatDsptr mat)
+{
+	mat->atijplusFullRow(iG, iqE, pGpE);
+	mat->atijplusFullColumn(iqE, iG, pGpE->transpose());
+}
+
+void MbD::EulerConstraint::fillAccICIterError(FColDsptr col)
+{
+	//"qdotT[ppGpqpq]*qdot."
+	//"qdotT[2 2 2 2 diag]*qdot."
+
+	col->atiplusFullVectortimes(iqE, pGpE, lam);
+	auto partFrame = static_cast<PartFrame*>(owner);
+	double sum = pGpE->timesFullColumn(partFrame->qEddot);
+	sum += 2.0 * partFrame->qEdot->sumOfSquares();
+	col->atiplusNumber(iG, sum);
+}

MbDCode/EulerConstraint.h

@@ -12,12 +12,14 @@ namespace MbD {
 	public:
 		EulerConstraint();
 		EulerConstraint(const char* str);
-		void initialize();
+		void initialize() override;
 		void calcPostDynCorrectorIteration() override;
 		void useEquationNumbers() override;
 		void fillPosICError(FColDsptr col) override;
 		void fillPosICJacob(SpMatDsptr mat) override;
 		void fillPosKineJacob(SpMatDsptr mat) override;
+		void fillVelICJacob(SpMatDsptr mat) override;
+		void fillAccICIterError(FColDsptr col) override;
 
 		FRowDsptr pGpE;		//partial derivative of G wrt pE
 		int iqE = -1;

MbDCode/EulerParameters.h

@@ -16,10 +16,13 @@ namespace MbD {
 		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<double>> pCpEtimesColumn(FColDsptr col);
+		static std::shared_ptr<FullMatrix<double>> pCTpEtimesColumn(FColDsptr col);
 		static std::shared_ptr<FullMatrix<std::shared_ptr<FullMatrix<T>>>> ppApEpEtimesMatrix(FMatDsptr mat);
 
-		void initialize();
-		void calc();
+
+		void initialize() override;
+		void calc() override;
 		void calcABC();
 		void calcpApE();
 
@@ -72,6 +75,70 @@ namespace MbD {
 		return answer;
 	}
 
+	template<>
+	inline std::shared_ptr<FullMatrix<double>> EulerParameters<double>::pCpEtimesColumn(FColDsptr col)
+	{
+		//"col size = 4."
+		auto c0 = col->at(0);
+		auto c1 = col->at(1);
+		auto c2 = col->at(2);
+		auto mc0 = -c0;
+		auto mc1 = -c1;
+		auto mc2 = -c2;
+		auto mc3 = -col->at(3);
+		auto answer = std::make_shared<FullMatrix<double>>(3, 4);
+		auto& row0 = answer->at(0);
+		auto& row1 = answer->at(1);
+		auto& row2 = answer->at(2);
+		row0->atiput(0, mc3);
+		row0->atiput(1, mc2);
+		row0->atiput(2, c1);
+		row0->atiput(3, c0);
+		row1->atiput(0, c2);
+		row1->atiput(1, mc3);
+		row1->atiput(2, mc0);
+		row1->atiput(3, c1);
+		row2->atiput(0, mc1);
+		row2->atiput(1, c0);
+		row2->atiput(2, mc3);
+		row2->atiput(3, c2);
+		return answer;
+	}
+
+	template<typename T>
+	inline std::shared_ptr<FullMatrix<double>> EulerParameters<T>::pCTpEtimesColumn(FColDsptr col)
+	{
+		//"col size = 3."
+		auto c0 = col->at(0);
+		auto c1 = col->at(1);
+		auto c2 = col->at(2);
+		auto mc0 = -c0;
+		auto mc1 = -c1;
+		auto mc2 = -c2;
+		auto answer = std::make_shared<FullMatrix<double>>(4, 4);
+		auto& row0 = answer->at(0);
+		auto& row1 = answer->at(1);
+		auto& row2 = answer->at(2);
+		auto& row3 = answer->at(3);
+		row0->atiput(0, 0.0);
+		row0->atiput(1, c2);
+		row0->atiput(2, mc1);
+		row0->atiput(3, c0);
+		row1->atiput(0, mc2);
+		row1->atiput(1, 0.0);
+		row1->atiput(2, c0);
+		row1->atiput(3, c1);
+		row2->atiput(0, c1);
+		row2->atiput(1, mc0);
+		row2->atiput(2, 0.0);
+		row2->atiput(3, c2);
+		row3->atiput(0, mc0);
+		row3->atiput(1, mc1);
+		row3->atiput(2, mc2);
+		row3->atiput(3, 0.0);
+		return answer;
+	}
+
 	template<>
 	inline std::shared_ptr<FullMatrix<std::shared_ptr<FullMatrix<double>>>> EulerParameters<double>::ppApEpEtimesMatrix(FMatDsptr mat)
 	{
@@ -182,10 +249,10 @@ namespace MbD {
 	template<>
 	inline void EulerParameters<double>::calcpApE()
 	{
-		double a2E0 = 2.0*(this->at(0));
-		double a2E1  = 2.0*(this->at(1));
-		double a2E2  = 2.0*(this->at(2));
-		double a2E3  = 2.0*(this->at(3));
+		double a2E0 = 2.0 * (this->at(0));
+		double a2E1 = 2.0 * (this->at(1));
+		double a2E2 = 2.0 * (this->at(2));
+		double a2E3 = 2.0 * (this->at(3));
 		double m2E0 = -a2E0;
 		double m2E1 = -a2E1;
 		double m2E2 = -a2E2;

MbDCode/EulerParametersDDot.cpp

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

MbDCode/EulerParametersDDot.h

@@ -0,0 +1,21 @@
+#pragma once
+
+#include "EulerArray.h"
+#include "EulerParametersDot.h"
+
+namespace MbD {
+
+    template<typename T>
+    class EulerParametersDDot : public EulerArray<T>
+    {
+        //qEdot aAddot aBddot aCddot 
+    public:
+        EulerParametersDDot(int count) : EulerArray<T>(count) {}
+        EulerParametersDDot(int count, const T& value) : EulerArray<T>(count, value) {}
+        EulerParametersDDot(std::initializer_list<T> list) : EulerArray<T>{ list } {}
+
+        //std::shared_ptr<EulerParametersDot<T>> qEdot;
+        FMatDsptr aAddot, aBddot, aCddot;
+    };
+}
+

MbDCode/EulerParametersDot.h

@@ -4,7 +4,6 @@
 #include "FullColumn.h"
 #include "FullMatrix.h"
 #include "EulerParameters.h"
-#include "CREATE.h"
 
 namespace MbD {
 
@@ -17,20 +16,23 @@ namespace MbD {
 		EulerParametersDot(int 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);
+		void initialize() override;
 		void calcAdotBdotCdot();
-		void calcpAdotpE();		
+		void calcpAdotpE();
+		FMatDsptr aB();
 
 		std::shared_ptr<EulerParameters<T>> qE;
 		FMatDsptr aAdot, aBdot, aCdot;
 		FColFMatDsptr pAdotpE;
-		void calc();
+		void calc() override;
 
 	};
 
 	template<typename T>
 	inline std::shared_ptr<EulerParametersDot<T>> EulerParametersDot<T>::FromqEOpAndOmegaOpO(std::shared_ptr<EulerParameters<T>> qEOp, FColDsptr omeOpO)
 	{
-		auto answer = CREATE<EulerParametersDot<T>>::With(4);
+		auto answer = std::make_shared<EulerParametersDot<T>>(4);
+		answer->initialize();	//Cannot use CREATE.h in subclasses of std::vector. Why?
 		qEOp->calcABC();
 		auto aB = qEOp->aB;
 		answer->equalFullColumn(aB->transposeTimesFullColumn(omeOpO->times(0.5)));
@@ -39,16 +41,133 @@ namespace MbD {
 		return answer;
 	}
 
+	template<typename T>
+	inline void EulerParametersDot<T>::initialize()
+	{
+		aAdot = std::make_shared<FullMatrix<double>>(3, 3);
+		aBdot = std::make_shared<FullMatrix<double>>(3, 4);
+		aCdot = std::make_shared<FullMatrix<double>>(3, 4);
+		pAdotpE = std::make_shared<FullColumn<FMatDsptr>>(4);
+		for (int i = 0; i < 4; i++)
+		{
+			pAdotpE->at(i) = std::make_shared<FullMatrix<double>>(3, 3);
+		}
+	}
+
 	template<typename T>
 	inline void EulerParametersDot<T>::calcAdotBdotCdot()
 	{
-		assert(false);
+		//"aAdot, aBdot and aCdot are all calculated together and only here."
+		auto aE0dot = this->at(0);
+		auto aE1dot = this->at(1);
+		auto aE2dot = this->at(2);
+		auto aE3dot = this->at(3);
+		auto mE0dot = -aE0dot;
+		auto mE1dot = -aE1dot;
+		auto mE2dot = -aE2dot;
+		aBdot->at(0)->at(0) = aE3dot;
+		aBdot->at(0)->at(1) = mE2dot;
+		aBdot->at(0)->at(2) = aE1dot;
+		aBdot->at(0)->at(3) = mE0dot;
+		aBdot->at(1)->at(0) = aE2dot;
+		aBdot->at(1)->at(1) = aE3dot;
+		aBdot->at(1)->at(2) = mE0dot;
+		aBdot->at(1)->at(3) = mE1dot;
+		aBdot->at(2)->at(0) = mE1dot;
+		aBdot->at(2)->at(1) = aE0dot;
+		aBdot->at(2)->at(2) = aE3dot;
+		aBdot->at(2)->at(3) = mE2dot;
+		aCdot->at(0)->at(0) = aE3dot;
+		aCdot->at(0)->at(1) = aE2dot;
+		aCdot->at(0)->at(2) = mE1dot;
+		aCdot->at(0)->at(3) = mE0dot;
+		aCdot->at(1)->at(0) = mE2dot;
+		aCdot->at(1)->at(1) = aE3dot;
+		aCdot->at(1)->at(2) = aE0dot;
+		aCdot->at(1)->at(3) = mE1dot;
+		aCdot->at(2)->at(0) = aE1dot;
+		aCdot->at(2)->at(1) = mE0dot;
+		aCdot->at(2)->at(2) = aE3dot;
+		aCdot->at(2)->at(3) = mE2dot;
+		aAdot = this->aB()->timesTransposeFullMatrix(aCdot)->times(2.0);
 	}
 
 	template<typename T>
 	inline void EulerParametersDot<T>::calcpAdotpE()
 	{
-		assert(false);
+		//"Mimic calcpApE"
+		//"All aE's are actually aEdot's."
+		double a2E0 = 2.0 * (this->at(0));
+		double a2E1 = 2.0 * (this->at(1));
+		double a2E2 = 2.0 * (this->at(2));
+		double a2E3 = 2.0 * (this->at(3));
+		double m2E0 = -a2E0;
+		double m2E1 = -a2E1;
+		double m2E2 = -a2E2;
+		double m2E3 = -a2E3;
+		FMatDsptr pApEk;
+		pApEk = pAdotpE->at(0);
+		FRowDsptr pAipEk;
+		pAipEk = pApEk->at(0);
+		pAipEk->at(0) = a2E0;
+		pAipEk->at(1) = a2E1;
+		pAipEk->at(2) = a2E2;
+		pAipEk = pApEk->at(1);
+		pAipEk->at(0) = a2E1;
+		pAipEk->at(1) = m2E0;
+		pAipEk->at(2) = m2E3;
+		pAipEk = pApEk->at(2);
+		pAipEk->at(0) = a2E2;
+		pAipEk->at(1) = a2E3;
+		pAipEk->at(2) = m2E0;
+		//
+		pApEk = pAdotpE->at(1);
+		pAipEk = pApEk->at(0);
+		pAipEk->at(0) = m2E1;
+		pAipEk->at(1) = a2E0;
+		pAipEk->at(2) = a2E3;
+		pAipEk = pApEk->at(1);
+		pAipEk->at(0) = a2E0;
+		pAipEk->at(1) = a2E1;
+		pAipEk->at(2) = a2E2;
+		pAipEk = pApEk->at(2);
+		pAipEk->at(0) = m2E3;
+		pAipEk->at(1) = a2E2;
+		pAipEk->at(2) = m2E1;
+		//
+		pApEk = pAdotpE->at(2);
+		pAipEk = pApEk->at(0);
+		pAipEk->at(0) = m2E2;
+		pAipEk->at(1) = m2E3;
+		pAipEk->at(2) = a2E0;
+		pAipEk = pApEk->at(1);
+		pAipEk->at(0) = a2E3;
+		pAipEk->at(1) = m2E2;
+		pAipEk->at(2) = a2E1;
+		pAipEk = pApEk->at(2);
+		pAipEk->at(0) = a2E0;
+		pAipEk->at(1) = a2E1;
+		pAipEk->at(2) = a2E2;
+		//
+		pApEk = pAdotpE->at(3);
+		pAipEk = pApEk->at(0);
+		pAipEk->at(0) = a2E3;
+		pAipEk->at(1) = m2E2;
+		pAipEk->at(2) = a2E1;
+		pAipEk = pApEk->at(1);
+		pAipEk->at(0) = a2E2;
+		pAipEk->at(1) = a2E3;
+		pAipEk->at(2) = m2E0;
+		pAipEk = pApEk->at(2);
+		pAipEk->at(0) = m2E1;
+		pAipEk->at(1) = a2E0;
+		pAipEk->at(2) = a2E3;
+	}
+
+	template<typename T>
+	inline FMatDsptr EulerParametersDot<T>::aB()
+	{
+		return qE->aB;
 	}
 
 	template<typename T>

MbDCode/FullColumn.h

@@ -29,6 +29,8 @@ namespace MbD {
 		void equalFullColumnAt(std::shared_ptr<FullColumn<T>> fullCol, int i);
 		std::shared_ptr<FullColumn<T>> copy();
 		std::shared_ptr<FullRow<T>> transpose();
+		void atiplusFullColumntimes(int i, std::shared_ptr<FullColumn<T>> fullCol, T factor);
+		T transposeTimesFullColumn(const std::shared_ptr<FullColumn<T>> fullCol);
 
 		virtual std::ostream& printOn(std::ostream& s) const;
 		friend std::ostream& operator<<(std::ostream& s, const FullColumn& fullCol)
@@ -132,6 +134,20 @@ namespace MbD {
 		return std::make_shared<FullRow<T>>(*this);
 	}
 	template<typename T>
+	inline void FullColumn<T>::atiplusFullColumntimes(int i1, std::shared_ptr<FullColumn<T>> fullCol, T factor)
+	{
+		for (int ii = 0; ii < fullCol->size(); ii++)
+		{
+			int i = i1 + ii;
+			this->at(i) += fullCol->at(ii) * factor;
+		}
+	}
+	template<typename T>
+	inline T FullColumn<T>::transposeTimesFullColumn(const std::shared_ptr<FullColumn<T>> fullCol)
+	{
+		return this->dot(fullCol);
+	}
+	template<typename T>
 	inline std::ostream& FullColumn<T>::printOn(std::ostream& s) const
 	{
 		s << "{";

MbDCode/FullMatrix.h

@@ -42,10 +42,13 @@ namespace MbD {
 		std::shared_ptr<FullMatrix<T>> timesFullMatrix(std::shared_ptr<FullMatrix<T>> fullMat);
 		std::shared_ptr<FullMatrix<T>> timesTransposeFullMatrix(std::shared_ptr<FullMatrix<T>> fullMat);
 		std::shared_ptr<FullMatrix<T>> times(double a);
+		std::shared_ptr<FullMatrix<T>> transposeTimesFullMatrix(std::shared_ptr<FullMatrix<T>> fullMat);
 		std::shared_ptr<FullMatrix<T>> plusFullMatrix(std::shared_ptr<FullMatrix<T>> fullMat);
 		std::shared_ptr<FullMatrix<T>> transpose();
 		std::shared_ptr<FullMatrix<T>> negated();
 		void symLowerWithUpper();
+		void atiput(int i, std::shared_ptr<FullRow<T>> fullRow);
+		void atijput(int i, int j, T value);
 		void atijputFullColumn(int i, int j, std::shared_ptr<FullColumn<T>> fullCol);
 		void atijplusFullRow(int i, int j, std::shared_ptr<FullRow<T>> fullRow);
 		void atijplusNumber(int i, int j, double value);
@@ -53,7 +56,7 @@ namespace MbD {
 		double sumOfSquares() override;
 		void zeroSelf() override;
 		std::shared_ptr<FullMatrix<T>> copy();
-		std::shared_ptr<FullMatrix<T>> operator+(const std::shared_ptr<FullMatrix<T>> fullMat);
+		FullMatrix<T> operator+(const FullMatrix<T> fullMat);
 		std::shared_ptr<FullColumn<T>> transposeTimesFullColumn(const std::shared_ptr<FullColumn<T>> fullCol);
 
 	};
@@ -74,18 +77,6 @@ namespace MbD {
 		return answer;
 	}
 	template<typename T>
-	inline std::shared_ptr<FullColumn<T>> FullMatrix<T>::timesFullColumn(std::shared_ptr<FullColumn<T>> fullCol)
-	{
-		//"a*b = a(i,j)b(j) sum j."
-		int n = (int)this->size();
-		auto answer = std::make_shared<FullColumn<T>>(n);
-		for (int i = 0; i < n; i++)
-		{
-			answer->at(i) = this->at(i)->timesFullColumn(fullCol);
-		}
-		return answer;
-	}
-	template<typename T>
 	inline std::shared_ptr<FullMatrix<T>> FullMatrix<T>::timesFullMatrix(std::shared_ptr<FullMatrix<T>> fullMat)
 	{
 		int m = this->nrow();
@@ -116,6 +107,11 @@ namespace MbD {
 		return answer;
 	}
 	template<typename T>
+	inline std::shared_ptr<FullMatrix<T>> FullMatrix<T>::transposeTimesFullMatrix(std::shared_ptr<FullMatrix<T>> fullMat)
+	{
+		return this->transpose()->timesFullMatrix(fullMat);
+	}
+	template<typename T>
 	inline std::shared_ptr<FullMatrix<T>> FullMatrix<T>::plusFullMatrix(std::shared_ptr<FullMatrix<T>> fullMat)
 	{
 		int n = (int)this->size();
@@ -155,6 +151,16 @@ namespace MbD {
 		}
 	}
 	template<typename T>
+	inline void FullMatrix<T>::atiput(int i, std::shared_ptr<FullRow<T>> fullRow)
+	{
+		this->at(i) = fullRow;
+	}
+	template<typename T>
+	inline void FullMatrix<T>::atijput(int i, int j, T value)
+	{
+		this->at(i)->atiput(j, value);
+	}
+	template<typename T>
 	inline void FullMatrix<T>::atijputFullColumn(int i1, int j1, std::shared_ptr<FullColumn<T>> fullCol)
 	{
 		for (int ii = 0; ii < fullCol->size(); ii++)
@@ -219,19 +225,33 @@ namespace MbD {
 		return answer;
 	}
 	template<typename T>
-	inline std::shared_ptr<FullMatrix<T>> FullMatrix<T>::operator+(const std::shared_ptr<FullMatrix<T>> fullMat)
+	inline FullMatrix<T> FullMatrix<T>::operator+(const FullMatrix<T> fullMat)
 	{
-		return this->plusFullMatrix(fullMat);
+		int n = (int)this->size();
+		auto answer = FullMatrix<T>(n);
+		for (int i = 0; i < n; i++) {
+			answer.at(i) = this->at(i)->plusFullRow(fullMat.at(i));
+		}
+		return answer;
 	}
 	template<typename T>
 	inline std::shared_ptr<FullColumn<T>> FullMatrix<T>::transposeTimesFullColumn(std::shared_ptr<FullColumn<T>> fullCol)
 	{
 		auto sptr = std::make_shared<FullMatrix<T>>(*this);
-		//auto aaa = fullCol->transpose();
-		//auto bbb = aaa->timesFullMatrix(sptr);
-		//auto ccc = bbb->transpose();
 		return fullCol->transpose()->timesFullMatrix(sptr)->transpose();
 	}
+	template<typename T>
+	inline std::shared_ptr<FullColumn<T>> FullMatrix<T>::timesFullColumn(std::shared_ptr<FullColumn<T>> fullCol)
+	{
+		//"a*b = a(i,j)b(j) sum j."
+		auto nrow = this->nrow();
+		auto answer = std::make_shared<FullColumn<T>>(nrow);
+		for (int i = 0; i < nrow; i++)
+		{
+			answer->at(i) = this->at(i)->timesFullColumn(fullCol);
+		}
+		return answer;
+	}
 	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>>>>;

MbDCode/FullVector.h

@@ -13,12 +13,14 @@ namespace MbD {
         FullVector(std::vector<T>::iterator begin, std::vector<T>::iterator end) : Array<T>(begin, end) {}
         FullVector(std::initializer_list<T> list) : Array<T>{ list } {}
         double dot(std::shared_ptr<FullVector<T>> vec);
+        void atiput(int i, T value);
         void atiplusNumber(int i, T value);
         void atiminusNumber(int i, T value);
         double sumOfSquares() override;
         int numberOfElements() override;
         void zeroSelf() override;
         void atitimes(int i, double factor);
+        void atiplusFullVector(int i, std::shared_ptr<FullVector> fullVec);
         void atiplusFullVectortimes(int i, std::shared_ptr<FullVector> fullVec, double factor);
         double maxMagnitude();
 
@@ -34,6 +36,11 @@ namespace MbD {
         return answer;
     }
     template<typename T>
+    inline void FullVector<T>::atiput(int i, T value)
+    {
+        this->at(i) = value;
+    }
+    template<typename T>
     inline void FullVector<T>::atiplusNumber(int i, T value)
     {
         this->at(i) += value;
@@ -83,6 +90,15 @@ namespace MbD {
         this->at(i) *= factor;
     }
     template<typename T>
+    inline void FullVector<T>::atiplusFullVector(int i1, std::shared_ptr<FullVector> fullVec)
+    {
+        for (int ii = 0; ii < fullVec->size(); ii++)
+        {
+            auto i = i1 + ii;
+            this->at(i) += fullVec->at(ii);
+        }
+    }
+    template<typename T>
     inline void FullVector<T>::atiplusFullVectortimes(int i1, std::shared_ptr<FullVector> fullVec, double factor)
     {
         for (int ii = 0; ii < fullVec->size(); ii++)

MbDCode/GEFullMatFullPv.cpp

@@ -42,7 +42,7 @@ void MbD::GEFullMatFullPv::doPivoting(int p)
 		colOrder->swapElems(p, pivotCol);
 	}
 	pivotValues->at(p) = max;
-	if (max < singularPivotTolerance) throw SingularMatrixError("");
+	if (max < singularPivotTolerance) throwSingularMatrixError("");
 }
 
 void MbD::GEFullMatFullPv::postSolve()

MbDCode/GEFullMatParPv.cpp

@@ -33,7 +33,7 @@ void MbD::GEFullMatParPv::doPivoting(int p)
 		rowOrder->swapElems(p, rowPivot);
 	}
 	pivotValues->at(p) = max;
-	if (max < singularPivotTolerance) throw SingularMatrixError("");
+	if (max < singularPivotTolerance) throwSingularMatrixError("");
 }
 
 void MbD::GEFullMatParPv::postSolve()

MbDCode/GESpMatFullPv.cpp

@@ -45,7 +45,7 @@ void MbD::GESpMatFullPv::doPivoting(int p)
 		positionsOfOriginalCols->at(colOrder->at(pivotCol)) = pivotCol;
 	}
 	pivotValues->at(p) = max;
-	if (max < singularPivotTolerance) throw SingularMatrixError("");
+	if (max < singularPivotTolerance) throwSingularMatrixError("");
 	auto jp = colOrder->at(p);
 	rowPositionsOfNonZerosInPivotColumn = rowPositionsOfNonZerosInColumns->at(jp);
 	if (rowPositionsOfNonZerosInPivotColumn->front() == p) {
@@ -120,7 +120,7 @@ void MbD::GESpMatFullPv::backSubstituteIntoDU()
 			}
 		}
 		auto ji = colOrder->at(i);
-		answerX->at(ji) = rightHandSideB->at(i) - (sum / duii);
+		answerX->at(ji) = (rightHandSideB->at(i) - sum) / duii;
 	}
 }
 
@@ -157,7 +157,7 @@ void MbD::GESpMatFullPv::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fu
 		auto& spRowi = spMat->at(i);
 		auto maxRowMagnitude = spRowi->maxMagnitude();
 		if (maxRowMagnitude == 0) {
-			throw SingularMatrixError("");
+			throwSingularMatrixError("");
 		}
 		matrixA->at(i) = spRowi->conditionedWithTol(singularPivotTolerance * maxRowMagnitude);
 		rowOrder->at(i) = i;

MbDCode/GESpMatFullPvPosIC.cpp

@@ -71,7 +71,7 @@ void MbD::GESpMatFullPvPosIC::doPivoting(int p)
 			auto begin = rowOrder->begin() + p;
 			auto end = rowOrder->begin() + pivotRowLimit;
 			auto redundantEqnNos = std::make_shared<FullColumn<int>>(begin, end);
-			throw SingularMatrixError("", redundantEqnNos);
+			throwSingularMatrixError("", redundantEqnNos);
 		}
 		else {
 			pivotRowLimit = *itr;

MbDCode/GESpMatParPv.cpp

@@ -60,7 +60,7 @@ void MbD::GESpMatParPv::backSubstituteIntoDU()
 				duii = keyValue.second;
 			}
 		}
-		answerX->at(i) = rightHandSideB->at(i) - (sum / duii);
+		answerX->at(i) = (rightHandSideB->at(i) - sum) / duii;
 	}
 }
 

MbDCode/GESpMatParPvMarko.cpp

@@ -21,7 +21,7 @@ void MbD::GESpMatParPvMarko::doPivoting(int p)
 	while (lookForFirstNonZeroInPivotCol) {
 		spRowi = matrixA->at(i);
 		if (spRowi->find(p) == spRowi->end()) {
-			if (i <= p) throw SingularMatrixError("");
+			if (i <= p) throwSingularMatrixError("");
 		}
 		else {
 			markowitzPivotColCount = 0;
@@ -64,10 +64,33 @@ void MbD::GESpMatParPvMarko::doPivoting(int p)
 		rowScalings->swapElems(p, rowPivoti);
 		if (aip != std::numeric_limits<double>::min()) rowPositionsOfNonZerosInPivotColumn->at(markowitzPivotColCount - 1) = rowPivoti;
 	}
-	if (max < singularPivotTolerance) throw SingularMatrixError("");
+	if (max < singularPivotTolerance) throwSingularMatrixError("");
 }
 
 void MbD::GESpMatParPvMarko::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal)
 {
-	assert(false);
+	//"Optimized for speed."
+	if (m != spMat->nrow() || n != spMat->ncol()) {
+		m = spMat->nrow();
+		n = spMat->ncol();
+		matrixA = std::make_shared<SparseMatrix<double>>(m);
+		rowScalings = std::make_shared<FullColumn<double>>(m);
+		rowPositionsOfNonZerosInPivotColumn = std::make_shared<std::vector<int>>();
+	}
+	if (saveOriginal) {
+		rightHandSideB = fullCol->copy();
+	}
+	else {
+		rightHandSideB = fullCol;
+	}
+	for (int i = 0; i < m; i++)
+	{
+		auto& spRowi = spMat->at(i);
+		auto maxRowMagnitude = spRowi->maxMagnitude();
+		if (maxRowMagnitude == 0) {
+			throwSingularMatrixError("");
+		}
+		rowScalings->atiput(i, 1.0 / maxRowMagnitude);
+		matrixA->atiput(i, spRowi->conditionedWithTol(singularPivotTolerance * maxRowMagnitude));
+	}
 }

MbDCode/GESpMatParPvMarkoFast.cpp

@@ -26,9 +26,7 @@ void MbD::GESpMatParPvMarkoFast::preSolvewithsaveOriginal(SpMatDsptr spMat, FCol
 	{
 		auto& spRowi = spMat->at(i);
 		auto maxRowMagnitude = spRowi->maxMagnitude();
-		if (maxRowMagnitude == 0) {
-			throw SingularMatrixError("");
-		}
+		if (maxRowMagnitude == 0) throwSingularMatrixError("");
 		auto scaling = 1.0 / maxRowMagnitude;
 		matrixA->at(i) = spRowi->timesconditionedWithTol(scaling, singularPivotTolerance);
 		rightHandSideB->atitimes(i, scaling);
@@ -54,7 +52,7 @@ void MbD::GESpMatParPvMarkoFast::doPivoting(int p)
 	while (lookForFirstNonZeroInPivotCol) {
 		spRowi = matrixA->at(i);
 		if (spRowi->find(p) == spRowi->end()) {
-			if (i <= p) throw SingularMatrixError("");
+			if (i <= p) throwSingularMatrixError("");
 		}
 		else {
 			markowitzPivotColCount = 0;
@@ -94,5 +92,5 @@ void MbD::GESpMatParPvMarkoFast::doPivoting(int p)
 		rightHandSideB->swapElems(p, rowPivoti);
 		if (aip != std::numeric_limits<double>::min()) rowPositionsOfNonZerosInPivotColumn->at(markowitzPivotColCount - 1) = rowPivoti;
 	}
-	if (max < singularPivotTolerance) throw SingularMatrixError("");
+	if (max < singularPivotTolerance) throwSingularMatrixError("");
 }

MbDCode/GESpMatParPvPrecise.cpp

@@ -21,7 +21,7 @@ void MbD::GESpMatParPvPrecise::doPivoting(int p)
 	while (lookForFirstNonZeroInPivotCol) {
 		spRowi = matrixA->at(i);
 		if (spRowi->find(p) == spRowi->end()) {
-			if (i <= p) throw SingularMatrixError("");
+			if (i <= p) throwSingularMatrixError("");
 		}
 		else {
 			markowitzPivotColCount = 0;
@@ -63,7 +63,7 @@ void MbD::GESpMatParPvPrecise::doPivoting(int p)
 		if (aip != std::numeric_limits<double>::min()) rowPositionsOfNonZerosInPivotColumn->at(markowitzPivotColCount - 1) = rowPivoti;
 	}
 	pivotValues->at(p) = max;
-	if (max < singularPivotTolerance) throw SingularMatrixError("");
+	if (max < singularPivotTolerance) throwSingularMatrixError("");
 }
 
 void MbD::GESpMatParPvPrecise::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal)
@@ -89,7 +89,7 @@ void MbD::GESpMatParPvPrecise::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDs
 	{
 		auto& spRowi = spMat->at(i);
 		auto maxRowMagnitude = spRowi->maxMagnitude();
-		if (maxRowMagnitude == 0) throw SingularMatrixError("");
+		if (maxRowMagnitude == 0) throwSingularMatrixError("");
 		rowScalings->at(i) = 1.0 / maxRowMagnitude;
 		matrixA->at(i) = spRowi->conditionedWithTol(singularPivotTolerance * maxRowMagnitude);
 		rowOrder->at(i) = i;

MbDCode/Item.cpp

@@ -155,11 +155,11 @@ void MbD::Item::fillqsudotWeights(std::shared_ptr<DiagonalMatrix<double>> diagMa
 {
 }
 
-void MbD::Item::fillVelICError(FColDsptr error)
+void MbD::Item::fillVelICError(FColDsptr col)
 {
 }
 
-void MbD::Item::fillVelICJacob(SpMatDsptr jacob)
+void MbD::Item::fillVelICJacob(SpMatDsptr mat)
 {
 }
 
@@ -167,6 +167,35 @@ void MbD::Item::setqsudotlam(FColDsptr qsudotlam)
 {
 }
 
+void MbD::Item::preAccIC()
+{
+	this->calcPostDynCorrectorIteration();
+}
+
+void MbD::Item::postAccIC()
+{
+}
+
+void MbD::Item::postAccICIteration()
+{
+}
+
+void MbD::Item::fillqsuddotlam(FColDsptr col)
+{
+}
+
+void MbD::Item::fillAccICIterError(FColDsptr col)
+{
+}
+
+void MbD::Item::fillAccICIterJacob(SpMatDsptr mat)
+{
+}
+
+void MbD::Item::setqsuddotlam(FColDsptr qsudotlam)
+{
+}
+
 void MbD::Item::constraintsReport()
 {
 }

MbDCode/Item.h

@@ -58,10 +58,18 @@ namespace MbD {
 		virtual void postVelIC();
 		virtual void fillqsudot(FColDsptr col);
 		virtual void fillqsudotWeights(std::shared_ptr<DiagonalMatrix<double>> diagMat);
-		virtual void fillVelICError(FColDsptr error);
-		virtual void fillVelICJacob(SpMatDsptr jacob);
+		virtual void fillVelICError(FColDsptr col);
+		virtual void fillVelICJacob(SpMatDsptr mat);
 		virtual void setqsudotlam(FColDsptr qsudotlam);
 
+		virtual void preAccIC();
+		virtual void postAccIC();
+		virtual void postAccICIteration();
+		virtual void fillqsuddotlam(FColDsptr col);
+		virtual void fillAccICIterError(FColDsptr col);
+		virtual void fillAccICIterJacob(SpMatDsptr mat);
+		virtual void setqsuddotlam(FColDsptr qsudotlam);
+
 		void setName(std::string& str);
 		const std::string& getName() const;
 

MbDCode/Joint.cpp

@@ -123,6 +123,11 @@ void MbD::Joint::setqsulam(FColDsptr col)
 	constraintsDo([&](std::shared_ptr<Constraint> con) { con->setqsulam(col); });
 }
 
+void MbD::Joint::setqsudotlam(FColDsptr col)
+{
+	constraintsDo([&](std::shared_ptr<Constraint> con) { con->setqsudotlam(col); });
+}
+
 void MbD::Joint::postPosICIteration()
 {
 	constraintsDo([](std::shared_ptr<Constraint> constraint) { constraint->postPosICIteration(); });
@@ -216,3 +221,33 @@ void MbD::Joint::preVelIC()
 {
 	constraintsDo([](std::shared_ptr<Constraint> constraint) { constraint->preVelIC(); });
 }
+
+void MbD::Joint::fillVelICError(FColDsptr col)
+{
+	constraintsDo([&](std::shared_ptr<Constraint> con) { con->fillVelICError(col); });
+}
+
+void MbD::Joint::fillVelICJacob(SpMatDsptr mat)
+{
+	constraintsDo([&](std::shared_ptr<Constraint> constraint) { constraint->fillVelICJacob(mat); });
+}
+
+void MbD::Joint::preAccIC()
+{
+	constraintsDo([](std::shared_ptr<Constraint> constraint) { constraint->preAccIC(); });
+}
+
+void MbD::Joint::fillAccICIterError(FColDsptr col)
+{
+	constraintsDo([&](std::shared_ptr<Constraint> con) { con->fillAccICIterError(col); });
+}
+
+void MbD::Joint::fillAccICIterJacob(SpMatDsptr mat)
+{
+	constraintsDo([&](std::shared_ptr<Constraint> con) { con->fillAccICIterJacob(mat); });
+}
+
+void MbD::Joint::setqsuddotlam(FColDsptr qsudotlam)
+{
+	constraintsDo([&](std::shared_ptr<Constraint> con) { con->setqsuddotlam(qsudotlam); });
+}

MbDCode/Joint.h

@@ -15,7 +15,7 @@ namespace MbD {
     public:
         Joint();
         Joint(const char* str);
-        void initialize();
+        void initialize() override;
         virtual void connectsItoJ(EndFrmcptr frmI, EndFrmcptr frmJ);
         void initializeLocally() override;
         void initializeGlobally() override;
@@ -34,6 +34,7 @@ namespace MbD {
         void fillqsudotWeights(std::shared_ptr<DiagonalMatrix<double>> diagMat) override;
         void useEquationNumbers() override;
         void setqsulam(FColDsptr col) override;
+        void setqsudotlam(FColDsptr col) override;
         void postPosICIteration() override;
         void fillPosICError(FColDsptr col) override;
         void fillPosICJacob(SpMatDsptr mat) override;
@@ -46,6 +47,12 @@ namespace MbD {
         void fillPosKineError(FColDsptr col) override;
         void fillPosKineJacob(SpMatDsptr mat) override;
         void preVelIC() override;
+        void fillVelICError(FColDsptr col) override;
+        void fillVelICJacob(SpMatDsptr mat) override;
+        void preAccIC() override;
+        void fillAccICIterError(FColDsptr col) override;
+        void fillAccICIterJacob(SpMatDsptr mat) override;
+        void setqsuddotlam(FColDsptr qsudotlam) override;
 
         EndFrmcptr frmI;
         EndFrmcptr frmJ;

MbDCode/LDUFullMatParPv.cpp

@@ -31,5 +31,5 @@ void MbD::LDUFullMatParPv::doPivoting(int p)
 		rowOrder->swapElems(p, rowPivot);
 	}
 	pivotValues->at(p) = max;
-	if (max < singularPivotTolerance) throw SingularMatrixError("");
+	if (max < singularPivotTolerance) throwSingularMatrixError("");
 }

MbDCode/LDUSpMatParPvMarko.cpp

@@ -17,7 +17,7 @@ void MbD::LDUSpMatParPvMarko::doPivoting(int p)
 	while (lookForFirstNonZeroInPivotCol) {
 		spRowi = matrixA->at(i);
 		if (spRowi->find(p) == spRowi->end()) {
-			if (i <= p) throw SingularMatrixError("");
+			if (i <= p) throwSingularMatrixError("");
 		}
 		else {
 			markowitzPivotColCount = 0;
@@ -61,5 +61,5 @@ void MbD::LDUSpMatParPvMarko::doPivoting(int p)
 		matrixL->swapElems(p, rowPivoti);
 		if (aip != std::numeric_limits<double>::min()) rowPositionsOfNonZerosInPivotColumn->at(markowitzPivotColCount - 1) = rowPivoti;
 	}
-	if (max < singularPivotTolerance) throw SingularMatrixError("");
+	if (max < singularPivotTolerance) throwSingularMatrixError("");
 }

MbDCode/LDUSpMatParPvPrecise.cpp

@@ -17,7 +17,7 @@ void MbD::LDUSpMatParPvPrecise::doPivoting(int p)
 	while (lookForFirstNonZeroInPivotCol) {
 		spRowi = matrixA->at(i);
 		if (spRowi->find(p) == spRowi->end()) {
-			if (i <= p) throw SingularMatrixError("");
+			if (i <= p) throwSingularMatrixError("");
 		}
 		else {
 			markowitzPivotColCount = 0;
@@ -59,5 +59,5 @@ void MbD::LDUSpMatParPvPrecise::doPivoting(int p)
 		if (aip != std::numeric_limits<double>::min()) rowPositionsOfNonZerosInPivotColumn->at(markowitzPivotColCount - 1) = rowPivoti;
 	}
 	pivotValues->at(p) = max;
-	if (max < singularPivotTolerance) throw SingularMatrixError("");
+	if (max < singularPivotTolerance) throwSingularMatrixError("");
 }

MbDCode/MarkerFrame.cpp

@@ -123,8 +123,14 @@ void MbD::MarkerFrame::setqsulam(FColDsptr col)
 	endFramesDo([&](const std::shared_ptr<EndFramec>& endFrame) { endFrame->setqsulam(col); });
 }
 
+void MbD::MarkerFrame::setqsudotlam(FColDsptr col)
+{
+	endFramesDo([&](const std::shared_ptr<EndFramec>& endFrame) { endFrame->setqsudotlam(col); });
+}
+
 void MbD::MarkerFrame::postPosICIteration()
 {
+	Item::postPosICIteration();
 	endFramesDo([](std::shared_ptr<EndFramec> endFrame) { endFrame->postPosICIteration(); });
 }
 
@@ -149,6 +155,42 @@ void MbD::MarkerFrame::preVelIC()
 	endFramesDo([](std::shared_ptr<EndFramec> endFrame) { endFrame->preVelIC(); });
 }
 
+void MbD::MarkerFrame::postVelIC()
+{
+	endFramesDo([](std::shared_ptr<EndFramec> endFrame) { endFrame->postVelIC(); });
+}
+
+void MbD::MarkerFrame::preAccIC()
+{
+	Item::preAccIC();
+	endFramesDo([](std::shared_ptr<EndFramec> endFrame) { endFrame->preAccIC(); });
+}
+
+FColDsptr MbD::MarkerFrame::qXdot()
+{
+	return partFrame->qXdot;
+}
+
+std::shared_ptr<EulerParametersDot<double>> MbD::MarkerFrame::qEdot()
+{
+	return partFrame->qEdot;
+}
+
+FColDsptr MbD::MarkerFrame::qXddot()
+{
+	return partFrame->qXddot;
+}
+
+FColDsptr MbD::MarkerFrame::qEddot()
+{
+	return partFrame->qEddot;
+}
+
+void MbD::MarkerFrame::setqsuddotlam(FColDsptr qsudotlam)
+{
+	endFramesDo([&](const std::shared_ptr<EndFramec>& endFrame) { endFrame->setqsuddotlam(qsudotlam); });
+}
+
 void MarkerFrame::setPartFrame(PartFrame* partFrm)
 {
 	partFrame = partFrm;

MbDCode/MarkerFrame.h

@@ -7,6 +7,8 @@
 #include "FullColumn.h"
 #include "FullMatrix.h"
 #include "EndFramec.h"
+#include "EulerParametersDot.h"
+#include "EulerParametersDDot.h"
 
 namespace MbD {
 	class PartFrame;
@@ -19,7 +21,7 @@ namespace MbD {
 	public:
 		MarkerFrame();
         MarkerFrame(const char* str);
-        void initialize();
+        void initialize() override;
 		void setPartFrame(PartFrame* partFrm);
 		PartFrame* getPartFrame();
 		void setrpmp(FColDsptr x);
@@ -40,11 +42,19 @@ namespace MbD {
 		void fillqsudotWeights(std::shared_ptr<DiagonalMatrix<double>> diagMat) override;
 		void setqsu(FColDsptr col) override;
 		void setqsulam(FColDsptr col) override;
+		void setqsudotlam(FColDsptr col) override;
 		void postPosICIteration() override;
 		void postPosIC() override;
 		void preDyn() override;
 		void storeDynState() override;
 		void preVelIC() override;
+		void postVelIC() override;
+		void preAccIC() override;
+		FColDsptr qXdot();
+		std::shared_ptr<EulerParametersDot<double>> qEdot();
+		FColDsptr qXddot();
+		FColDsptr qEddot();
+		void setqsuddotlam(FColDsptr qsudotlam) override;
 
 		PartFrame* partFrame; //Use raw pointer when pointing backwards.
 		FColDsptr rpmp = std::make_shared<FullColumn<double>>(3);

MbDCode/MatrixSolver.cpp

@@ -57,7 +57,17 @@ void MbD::MatrixSolver::findScalingsForRowRange(int begin, int end)
 	for (int i = begin; i < end; i++)
 	{
 		auto maxRowMagnitude = this->getmatrixArowimaxMagnitude(i);
-		if (maxRowMagnitude == 0.0) throw SingularMatrixError("");
+		if (maxRowMagnitude == 0.0) throwSingularMatrixError("");
 		rowScalings->at(i) = 1.0 / maxRowMagnitude;
 	}
 }
+
+void MbD::MatrixSolver::throwSingularMatrixError(const char* chars)
+{
+	throw SingularMatrixError(chars);
+}
+
+void MbD::MatrixSolver::throwSingularMatrixError(const char* chars, std::shared_ptr<FullColumn<int>> redunEqnNos)
+{
+	throw SingularMatrixError(chars, redunEqnNos);
+}

MbDCode/MatrixSolver.h

@@ -29,6 +29,8 @@ namespace MbD {
         virtual void postSolve() = 0;
         virtual void findScalingsForRowRange(int begin, int end);
         virtual double getmatrixArowimaxMagnitude(int i) = 0;
+        void throwSingularMatrixError(const char* chars);
+        void throwSingularMatrixError(const char* chars, std::shared_ptr<FullColumn<int>> redunEqnNos);
 
         int m = 0, n = 0;
         FColDsptr answerX, rightHandSideB, rowScalings, pivotValues;

MbDCode/MbDCode.cpp

@@ -25,10 +25,259 @@
 #include "MbDCode.h"
 #include "Time.h"
 #include "CREATE.h"
+#include "GESpMatParPvMarkoFast.h"
+#include "GESpMatParPvPrecise.h"
 
 using namespace MbD;
+void runSpMat();
+void runPiston();
+void runOndselPiston();
 
 int main()
+{
+	//runSpMat();
+	//runPiston();
+	runOndselPiston();
+}
+
+void runOndselPiston()
+{
+	//Piston with easy input numbers for exact port from Smalltalk
+	std::cout << "Hello World!\n";
+	System& TheSystem = System::getInstance();
+	std::string name = "TheSystem";
+	TheSystem.setName(name);
+	std::cout << "TheSystem.getName() " << TheSystem.getName() << std::endl;
+	auto& systemSolver = TheSystem.systemSolver;
+	systemSolver->errorTolPosKine = 1.0e-6;
+	systemSolver->errorTolAccKine = 1.0e-6;
+	systemSolver->iterMaxPosKine = 25;
+	systemSolver->iterMaxAccKine = 25;
+	systemSolver->tstart = 0;
+	systemSolver->tend = 25.0;
+	systemSolver->hmin = 2.5e-8;
+	systemSolver->hmax = 25.0;
+	systemSolver->hout = 1.0;
+	systemSolver->corAbsTol = 1.0e-6;
+	systemSolver->corRelTol = 1.0e-6;
+	systemSolver->intAbsTol = 1.0e-6;
+	systemSolver->intRelTol = 1.0e-6;
+	systemSolver->iterMaxDyn = 4;
+	systemSolver->orderMax = 5;
+	systemSolver->translationLimit = 9.6058421285615e9;
+	systemSolver->rotationLimit = 0.5;
+
+	std::string str;
+	FColDsptr qX, qE, qXdot, omeOpO, qXddot, qEddot;
+	FColDsptr rpmp;
+	FMatDsptr aApm;
+	FRowDsptr fullRow;
+	//
+	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 });
+	assembly1->setqXdot(qXdot);
+	assembly1->setomeOpO(omeOpO);
+	qXddot = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0 });
+	qEddot = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0, 0 });
+	assembly1->setqXddot(qXddot);
+	assembly1->setqEddot(qEddot);
+	std::cout << "assembly1->getqX() " << *assembly1->getqX() << std::endl;
+	std::cout << "assembly1->getqE() " << *assembly1->getqE() << std::endl;
+	TheSystem.addPart(assembly1);
+	{
+		auto& partFrame = assembly1->partFrame;
+		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{
+			{ 1, 0, 0 },
+			{ 0, 1, 0 },
+			{ 0, 0, 1 }
+			});
+		marker2->setaApm(aApm);
+		partFrame->addMarkerFrame(marker2);
+		//
+		auto marker1 = CREATE<MarkerFrame>::With("/Assembly1/Marker1");
+		rpmp = std::make_shared<FullColumn<double>>(ListD{ 0.0, 3.0, 0.0 });
+		marker1->setrpmp(rpmp);
+		aApm = std::make_shared<FullMatrix<double>>(ListListD{
+			{ 1, 0, 0 },
+			{ 0, 0, 1 },
+			{ 0, -1, 0 }
+			});
+		marker1->setaApm(aApm);
+		partFrame->addMarkerFrame(marker1);
+	}
+	assembly1->asFixed();
+	//
+	auto crankPart1 = CREATE<Part>::With("/Assembly1/Part1");
+	std::cout << "crankPart1->getName() " << crankPart1->getName() << std::endl;
+	crankPart1->m = 1.0;
+	crankPart1->aJ = std::make_shared<DiagonalMatrix<double>>(ListD{ 1, 1, 1 });
+	qX = std::make_shared<FullColumn<double>>(ListD{ 0.4, 0.0, -0.05 });
+	qE = std::make_shared<FullColumn<double>>(ListD{ 0.0, 0.0, 0.0, 1.0 });
+	crankPart1->setqX(qX);
+	crankPart1->setqE(qE);
+	qXdot = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0 });
+	omeOpO = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0 });
+	crankPart1->setqXdot(qXdot);
+	crankPart1->setomeOpO(omeOpO);
+	qXddot = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0 });
+	qEddot = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0, 0 });
+	crankPart1->setqXddot(qXddot);
+	crankPart1->setqEddot(qEddot);
+	TheSystem.parts->push_back(crankPart1);
+	{
+		auto& partFrame = crankPart1->partFrame;
+		auto marker1 = CREATE<MarkerFrame>::With("/Assembly1/Part1/Marker1");
+		rpmp = std::make_shared<FullColumn<double>>(ListD{ -0.4, 0.0, 0.05 });
+		marker1->setrpmp(rpmp);
+		aApm = std::make_shared<FullMatrix<double>>(ListListD{
+			{ 1, 0, 0 },
+			{ 0, 1, 0 },
+			{ 0, 0, 1 }
+			});
+		marker1->setaApm(aApm);
+		partFrame->addMarkerFrame(marker1);
+		//
+		auto marker2 = CREATE<MarkerFrame>::With("/Assembly1/Part1/Marker2");
+		rpmp = std::make_shared<FullColumn<double>>(ListD{ 0.4, 0.0, 0.05 });
+		marker2->setrpmp(rpmp);
+		aApm = std::make_shared<FullMatrix<double>>(ListListD{
+			{ 1, 0, 0 },
+			{ 0, 1, 0 },
+			{ 0, 0, 1 }
+			});
+		marker2->setaApm(aApm);
+		partFrame->addMarkerFrame(marker2);
+	}
+	//
+	auto conrodPart2 = CREATE<Part>::With("/Assembly1/Part2");
+	std::cout << "conrodPart2->getName() " << conrodPart2->getName() << std::endl;
+	conrodPart2->m = 1.0;
+	conrodPart2->aJ = std::make_shared<DiagonalMatrix<double>>(ListD{ 1, 1, 1 });
+	qX = std::make_shared<FullColumn<double>>(ListD{ 0.15, 0.1, 0.05 });
+	qE = std::make_shared<FullColumn<double>>(ListD{ 0.0, 0.0, 1.0, 0.0 });
+	conrodPart2->setqX(qX);
+	conrodPart2->setqE(qE);
+	qXdot = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0 });
+	omeOpO = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0 });
+	conrodPart2->setqXdot(qXdot);
+	conrodPart2->setomeOpO(omeOpO);
+	qXddot = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0 });
+	qEddot = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0, 0 });
+	conrodPart2->setqXddot(qXddot);
+	conrodPart2->setqEddot(qEddot);
+	TheSystem.parts->push_back(conrodPart2);
+	{
+		auto& partFrame = conrodPart2->partFrame;
+		auto marker1 = CREATE<MarkerFrame>::With("/Assembly1/Part2/Marker1");
+		rpmp = std::make_shared<FullColumn<double>>(ListD{ -0.65, 0.0, -0.05 });
+		marker1->setrpmp(rpmp);
+		aApm = std::make_shared<FullMatrix<double>>(ListListD{
+			{1.0, 0.0, 0.0},
+			{0.0, 1.0, 0.0},
+			{0.0, 0.0, 1.0}
+			});
+		marker1->setaApm(aApm);
+		partFrame->addMarkerFrame(marker1);
+		//
+		auto marker2 = CREATE<MarkerFrame>::With("/Assembly1/Part2/Marker2");
+		rpmp = std::make_shared<FullColumn<double>>(ListD{ 0.65, 0.0, -0.05 });
+		marker2->setrpmp(rpmp);
+		aApm = std::make_shared<FullMatrix<double>>(ListListD{
+			{1.0, 0.0, 0.0},
+			{0.0, 1.0, 0.0},
+			{0.0, 0.0, 1.0}
+			});
+		marker2->setaApm(aApm);
+		partFrame->addMarkerFrame(marker2);
+	}
+	//
+	auto pistonPart3 = CREATE<Part>::With("/Assembly1/Part3");
+	std::cout << "pistonPart3->getName() " << pistonPart3->getName() << std::endl;
+	pistonPart3->m = 1.0;
+	pistonPart3->aJ = std::make_shared<DiagonalMatrix<double>>(ListD{ 1, 1, 1 });
+	qX = std::make_shared<FullColumn<double>>(ListD{ -0.0, 1.5, 0.0 });
+	qE = std::make_shared<FullColumn<double>>(ListD{ 0.70710678118655, 0.70710678118655, 0.0, 0.0 });
+	pistonPart3->setqX(qX);
+	pistonPart3->setqE(qE);
+	qXdot = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0 });
+	omeOpO = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0 });
+	pistonPart3->setqXdot(qXdot);
+	pistonPart3->setomeOpO(omeOpO);
+	qXddot = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0 });
+	qEddot = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0, 0 });
+	pistonPart3->setqXddot(qXddot);
+	pistonPart3->setqEddot(qEddot);
+	TheSystem.parts->push_back(pistonPart3);
+	{
+		auto& partFrame = pistonPart3->partFrame;
+		auto marker1 = CREATE<MarkerFrame>::With("/Assembly1/Part3/Marker1");
+		rpmp = std::make_shared<FullColumn<double>>(ListD{ -0.5, 0.0, 0.0 });
+		marker1->setrpmp(rpmp);
+		aApm = std::make_shared<FullMatrix<double>>(ListListD{
+			{0.0, 1.0, 0.0},
+			{1.0, 0.0, 0.0},
+			{0.0, 0.0, -1.0}
+			});
+		marker1->setaApm(aApm);
+		partFrame->addMarkerFrame(marker1);
+		//
+		auto marker2 = CREATE<MarkerFrame>::With("/Assembly1/Part3/Marker2");
+		rpmp = std::make_shared<FullColumn<double>>(ListD{ 0.5, 0.0, 0.0 });
+		marker2->setrpmp(rpmp);
+		aApm = std::make_shared<FullMatrix<double>>(ListListD{
+			{0.0, 0.0, 1.0},
+			{1.0, 0.0, 0.0},
+			{0.0, 1.0, 0.0}
+			});
+		marker2->setaApm(aApm);
+		partFrame->addMarkerFrame(marker2);
+	}
+	//
+	auto revJoint1 = CREATE<RevoluteJoint>::With("/Assembly1/Joint1");
+	std::cout << "revJoint1->getName() " << revJoint1->getName() << std::endl;
+	revJoint1->connectsItoJ(assembly1->partFrame->endFrame("/Assembly1/Marker2"), crankPart1->partFrame->endFrame("/Assembly1/Part1/Marker1"));
+	TheSystem.jointsMotions->push_back(revJoint1);
+
+	auto revJoint2 = CREATE<RevoluteJoint>::With("/Assembly1/Joint2");
+	std::cout << "revJoint2->getName() " << revJoint2->getName() << std::endl;
+	revJoint2->connectsItoJ(crankPart1->partFrame->endFrame("/Assembly1/Part1/Marker2"), conrodPart2->partFrame->endFrame("/Assembly1/Part2/Marker1"));
+	TheSystem.jointsMotions->push_back(revJoint2);
+
+	auto revJoint3 = CREATE<RevoluteJoint>::With("/Assembly1/Joint3");
+	std::cout << "revJoint3->getName() " << revJoint3->getName() << std::endl;
+	revJoint3->connectsItoJ(conrodPart2->partFrame->endFrame("/Assembly1/Part2/Marker2"), pistonPart3->partFrame->endFrame("/Assembly1/Part3/Marker1"));
+	TheSystem.jointsMotions->push_back(revJoint3);
+
+	auto cylJoint4 = CREATE<CylindricalJoint>::With("/Assembly1/Joint4");
+	std::cout << "cylJoint4->getName() " << cylJoint4->getName() << std::endl;
+	cylJoint4->connectsItoJ(pistonPart3->partFrame->endFrame("/Assembly1/Part3/Marker2"), assembly1->partFrame->endFrame("/Assembly1/Marker1"));
+	TheSystem.jointsMotions->push_back(cylJoint4);
+
+	auto rotMotion1 = CREATE<ZRotation>::With("/Assembly1/Motion1");
+	rotMotion1->connectsItoJ(assembly1->partFrame->endFrame("/Assembly1/Marker2"), crankPart1->partFrame->endFrame("/Assembly1/Part1/Marker1"));
+	std::cout << "rotMotion1->getName() " << rotMotion1->getName() << std::endl;
+	auto omega = std::make_shared<Constant>(6.2831853071796);
+	auto timeScale = std::make_shared<Constant>(1.0);
+	auto time = std::make_shared<Product>(timeScale, TheSystem.time);
+	rotMotion1->phiBlk = std::make_shared<Product>(omega, time);
+	std::cout << "rotMotion1->phiBlk " << *(rotMotion1->phiBlk) << std::endl;
+	TheSystem.jointsMotions->push_back(rotMotion1);
+	//
+	TheSystem.runKINEMATICS();
+}
+
+void runPiston()
 {
 	std::cout << "Hello World!\n";
 	System& TheSystem = System::getInstance();
@@ -73,9 +322,9 @@ int main()
 	assembly1->setqXdot(qXdot);
 	assembly1->setomeOpO(omeOpO);
 	qXddot = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0 });
-	qEddot = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0 });
-	assembly1->setqXddot(qXdot);
-	assembly1->setqEddot(omeOpO);
+	qEddot = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0, 0 });
+	assembly1->setqXddot(qXddot);
+	assembly1->setqEddot(qEddot);
 	std::cout << "assembly1->getqX() " << *assembly1->getqX() << std::endl;
 	std::cout << "assembly1->getqE() " << *assembly1->getqE() << std::endl;
 	TheSystem.addPart(assembly1);
@@ -118,9 +367,9 @@ int main()
 	crankPart1->setqXdot(qXdot);
 	crankPart1->setomeOpO(omeOpO);
 	qXddot = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0 });
-	qEddot = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0 });
-	crankPart1->setqXddot(qXdot);
-	crankPart1->setqEddot(omeOpO);
+	qEddot = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0, 0 });
+	crankPart1->setqXddot(qXddot);
+	crankPart1->setqEddot(qEddot);
 	TheSystem.parts->push_back(crankPart1);
 	{
 		auto& partFrame = crankPart1->partFrame;
@@ -157,12 +406,12 @@ int main()
 	conrodPart2->setqE(qE);
 	qXdot = std::make_shared<FullColumn<double>>(ListD{ 0, 0.19313691560085, 0 });
 	omeOpO = std::make_shared<FullColumn<double>>(ListD{ 1.670970041317e-34, 1.3045598281729e-34, -1.2731200314796e-35 });
-	crankPart1->setqXdot(qXdot);
-	crankPart1->setomeOpO(omeOpO);
+	conrodPart2->setqXdot(qXdot);
+	conrodPart2->setomeOpO(omeOpO);
 	qXddot = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0 });
-	qEddot = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0 });
-	crankPart1->setqXddot(qXdot);
-	crankPart1->setqEddot(omeOpO);
+	qEddot = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0, 0 });
+	conrodPart2->setqXddot(qXddot);
+	conrodPart2->setqEddot(qEddot);
 	TheSystem.parts->push_back(conrodPart2);
 	{
 		auto& partFrame = conrodPart2->partFrame;
@@ -172,7 +421,7 @@ int main()
 		aApm = std::make_shared<FullMatrix<double>>(ListListD{
 			{1.0, 2.7755575615629e-16, 0.0},
 			{-2.7755575615629e-16, 1.0, 0.0},
-			{0.0, 0.0, 1.0} 
+			{0.0, 0.0, 1.0}
 			});
 		marker1->setaApm(aApm);
 		partFrame->addMarkerFrame(marker1);
@@ -183,7 +432,7 @@ int main()
 		aApm = std::make_shared<FullMatrix<double>>(ListListD{
 			{1.0, 2.4980018054066e-16, 2.2204460492503e-16},
 			{-2.4980018054066e-16, 1.0, 4.1633363423443e-17},
-			{-2.2204460492503e-16, -4.1633363423443e-17, 1.0} 
+			{-2.2204460492503e-16, -4.1633363423443e-17, 1.0}
 			});
 		marker2->setaApm(aApm);
 		partFrame->addMarkerFrame(marker2);
@@ -199,12 +448,12 @@ int main()
 	pistonPart3->setqE(qE);
 	qXdot = std::make_shared<FullColumn<double>>(ListD{ -6.3364526821409e-32, 0.19313691560085, -1.933731897626e-34 });
 	omeOpO = std::make_shared<FullColumn<double>>(ListD{ 1.670970041317e-34, 1.3045598281729e-34, 1.8896472173894e-50 });
-	crankPart1->setqXdot(qXdot);
-	crankPart1->setomeOpO(omeOpO);
+	pistonPart3->setqXdot(qXdot);
+	pistonPart3->setomeOpO(omeOpO);
 	qXddot = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0 });
-	qEddot = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0 });
-	crankPart1->setqXddot(qXdot);
-	crankPart1->setqEddot(omeOpO);
+	qEddot = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0, 0 });
+	pistonPart3->setqXddot(qXddot);
+	pistonPart3->setqEddot(qEddot);
 	TheSystem.parts->push_back(pistonPart3);
 	{
 		auto& partFrame = pistonPart3->partFrame;
@@ -214,7 +463,7 @@ int main()
 		aApm = std::make_shared<FullMatrix<double>>(ListListD{
 			{9.2444637330587e-33, 1.0, 2.2204460492503e-16},
 			{1.0, -9.2444637330587e-33, -1.0785207688569e-32},
-			{-1.0785207688569e-32, 2.2204460492503e-16, -1.0} 
+			{-1.0785207688569e-32, 2.2204460492503e-16, -1.0}
 			});
 		marker1->setaApm(aApm);
 		partFrame->addMarkerFrame(marker1);
@@ -225,7 +474,7 @@ int main()
 		aApm = std::make_shared<FullMatrix<double>>(ListListD{
 			{6.9388939039072e-18, -6.4146353042213e-50, 1.0},
 			{1.0, -6.9388939039072e-18, 6.9388939039072e-18},
-			{-6.9388939039072e-18, 1.0, -7.4837411882581e-50} 
+			{-6.9388939039072e-18, 1.0, -7.4837411882581e-50}
 			});
 		marker2->setaApm(aApm);
 		partFrame->addMarkerFrame(marker2);
@@ -240,7 +489,7 @@ int main()
 	std::cout << "revJoint2->getName() " << revJoint2->getName() << std::endl;
 	revJoint2->connectsItoJ(crankPart1->partFrame->endFrame("/Assembly1/Part1/Marker2"), conrodPart2->partFrame->endFrame("/Assembly1/Part2/Marker1"));
 	TheSystem.jointsMotions->push_back(revJoint2);
-	
+
 	auto revJoint3 = CREATE<RevoluteJoint>::With("/Assembly1/Joint3");
 	std::cout << "revJoint3->getName() " << revJoint3->getName() << std::endl;
 	revJoint3->connectsItoJ(conrodPart2->partFrame->endFrame("/Assembly1/Part2/Marker2"), pistonPart3->partFrame->endFrame("/Assembly1/Part3/Marker1"));
@@ -262,4 +511,22 @@ int main()
 	TheSystem.jointsMotions->push_back(rotMotion1);
 	//
 	TheSystem.runKINEMATICS();
+}
+
+void runSpMat() {
+	auto spMat = std::make_shared<SparseMatrix<double>>(3, 3);
+	spMat->atijput(0, 0, 1.0);
+	spMat->atijput(0, 1, 1.0);
+	spMat->atijput(1, 0, 1.0);
+	spMat->atijput(1, 1, 1.0);
+	spMat->atijput(1, 2, 1.0);
+	spMat->atijput(2, 1, 1.0);
+	spMat->atijput(2, 2, 1.0);
+	auto fullCol = std::make_shared<FullColumn<double>>(3);
+	fullCol->atiput(0, 1.0);
+	fullCol->atiput(1, 2.0);
+	fullCol->atiput(2, 3.0);
+	auto matSolver = CREATE<GESpMatParPvPrecise>::With();
+	auto answer = matSolver->solvewithsaveOriginal(spMat, fullCol, true);
+	auto aAx = spMat->timesFullColumn(answer);
 }

MbDCode/MbDCode.vcxproj

@@ -128,6 +128,10 @@
   </ItemDefinitionGroup>
   <ItemGroup>
     <ClCompile Include="AbsConstraint.cpp" />
+    <ClCompile Include="AccICKineNewtonRaphson.cpp" />
+    <ClCompile Include="AccICNewtonRaphson.cpp" />
+    <ClCompile Include="AccKineNewtonRaphson.cpp" />
+    <ClCompile Include="AccNewtonRaphson.cpp" />
     <ClCompile Include="AnyPosICNewtonRaphson.cpp" />
     <ClCompile Include="Array.cpp" />
     <ClCompile Include="AtPointConstraintIJ.cpp" />
@@ -173,6 +177,7 @@
     <ClCompile Include="EulerArray.cpp" />
     <ClCompile Include="EulerConstraint.cpp" />
     <ClCompile Include="EulerParameters.cpp" />
+    <ClCompile Include="EulerParametersDDot.cpp" />
     <ClCompile Include="EulerParametersDot.cpp" />
     <ClCompile Include="ExpressionX.cpp" />
     <ClCompile Include="ForceTorqueItem.cpp" />
@@ -262,6 +267,10 @@
   </ItemGroup>
   <ItemGroup>
     <ClInclude Include="AbsConstraint.h" />
+    <ClInclude Include="AccICKineNewtonRaphson.h" />
+    <ClInclude Include="AccICNewtonRaphson.h" />
+    <ClInclude Include="AccKineNewtonRaphson.h" />
+    <ClInclude Include="AccNewtonRaphson.h" />
     <ClInclude Include="AnyPosICNewtonRaphson.h" />
     <ClInclude Include="Array.h" />
     <ClInclude Include="AtPointConstraintIJ.h" />
@@ -308,6 +317,7 @@
     <ClInclude Include="EulerArray.h" />
     <ClInclude Include="EulerConstraint.h" />
     <ClInclude Include="EulerParameters.h" />
+    <ClInclude Include="EulerParametersDDot.h" />
     <ClInclude Include="EulerParametersDot.h" />
     <ClInclude Include="ExpressionX.h" />
     <ClInclude Include="ForceTorqueItem.h" />

MbDCode/MbDCode.vcxproj.filters

@@ -411,6 +411,21 @@
     <ClCompile Include="EulerAngleszxzDDot.cpp">
       <Filter>Source Files</Filter>
     </ClCompile>
+    <ClCompile Include="AccNewtonRaphson.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="AccICNewtonRaphson.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="AccKineNewtonRaphson.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="AccICKineNewtonRaphson.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="EulerParametersDDot.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
   </ItemGroup>
   <ItemGroup>
     <ClInclude Include="Array.h">
@@ -818,6 +833,21 @@
     <ClInclude Include="EulerAngleszxzDDot.h">
       <Filter>Header Files</Filter>
     </ClInclude>
+    <ClInclude Include="AccNewtonRaphson.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="AccICNewtonRaphson.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="AccKineNewtonRaphson.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="AccICKineNewtonRaphson.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="EulerParametersDDot.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
   </ItemGroup>
   <ItemGroup>
     <ResourceCompile Include="MbDCode.rc">

MbDCode/NewtonRaphson.h

@@ -16,7 +16,7 @@ namespace MbD {
     {
         //system xold x dx dxNorm dxNorms dxTol y yNorm yNormOld yNorms yNormTol pypx iterNo iterMax nDivergence nBackTracking twoAlp lam 
     public:
-        void initialize();
+        void initialize() override;
         void initializeLocally() override;
         void run() override;
         void setSystem(Solver* sys) override;
@@ -35,7 +35,7 @@ namespace MbD {
         virtual void passRootToSystem() = 0;
         bool isConvergedToNumericalLimit();
         void calcDXNormImproveRootCalcYNorm();
-        virtual void postRun();
+        void postRun() override;
         
         SystemSolver* system; //Use raw pointer when pointing backwards.
         std::shared_ptr<std::vector<double>> dxNorms, yNorms;

MbDCode/Part.cpp

@@ -3,6 +3,8 @@
 #include "System.h"
 #include "CREATE.h"
 #include "DiagonalMatrix.h"
+#include "EulerParameters.h"
+
 
 using namespace MbD;
 
@@ -24,9 +26,12 @@ void Part::initialize()
 void Part::initializeLocally()
 {
 	partFrame->initializeLocally();
-	//mX: = m > 0
-	//ifTrue: [StMDiagonalMatrix new:3 withAll : m]
-	//ifFalse : [StMDiagonalMatrix new:3 withAll : 0.0d]
+	if (m > 0) {
+		mX = std::make_shared<DiagonalMatrix<double>>(3, m);
+	}
+	else {
+		mX = std::make_shared<DiagonalMatrix<double>>(3, 0.0);
+	}
 }
 
 void Part::initializeGlobally()
@@ -86,6 +91,69 @@ FColDsptr MbD::Part::getqEddot()
 	return partFrame->getqEddot();
 }
 
+void MbD::Part::qX(FColDsptr x)
+{
+	partFrame->qX = x;
+}
+
+FColDsptr MbD::Part::qX()
+{
+	return partFrame->qX;
+}
+
+void MbD::Part::qE(std::shared_ptr<EulerParameters<double>> x)
+{
+	partFrame->qE = x;
+}
+
+std::shared_ptr<EulerParameters<double>> MbD::Part::qE()
+{
+	return partFrame->qE;
+}
+
+void MbD::Part::qXdot(FColDsptr x)
+{
+	partFrame->qXdot = x;
+}
+
+FColDsptr MbD::Part::qXdot()
+{
+	return partFrame->qXdot;
+}
+
+void MbD::Part::omeOpO(FColDsptr x)
+{
+	partFrame->setomeOpO(x);
+}
+
+FColDsptr MbD::Part::omeOpO()
+{
+	assert(false);
+	return FColDsptr();
+}
+
+void MbD::Part::qXddot(FColDsptr x)
+{
+	partFrame->qXddot = x;
+}
+
+FColDsptr MbD::Part::qXddot()
+{
+	return partFrame->qXddot;
+}
+
+void MbD::Part::qEddot(FColDsptr x)
+{
+	//ToDo: Should store EulerParametersDDot
+	//ToDo: Need alpOpO too
+	partFrame->qXddot = x;
+}
+
+FColDsptr MbD::Part::qEddot()
+{
+	return partFrame->qEddot;
+}
+
 void Part::setSystem(System& sys)
 {
 	//May be needed in the future
@@ -104,6 +172,11 @@ void MbD::Part::postInput()
 
 void MbD::Part::calcPostDynCorrectorIteration()
 {
+	this->calcmE();
+	this->calcmEdot();
+	this->calcpTpE();
+	this->calcppTpEpE();
+	this->calcppTpEpEdot();
 }
 
 void MbD::Part::prePosIC()
@@ -171,7 +244,7 @@ void MbD::Part::fillqsuWeights(std::shared_ptr<DiagonalMatrix<double>> diagMat)
 	{
 		auto aJi = aJ->at(i);
 		wqE->at(i) = (maxw * aJi / aJiMax) + minw;
-	}		
+	}
 	wqE->at(3) = minw;
 	diagMat->atiputDiagonalMatrix(partFrame->iqX, wqX);
 	diagMat->atiputDiagonalMatrix(partFrame->iqE, wqE);
@@ -231,6 +304,11 @@ void MbD::Part::setqsulam(FColDsptr col)
 	partFrame->setqsulam(col);
 }
 
+void MbD::Part::setqsudotlam(FColDsptr col)
+{
+	partFrame->setqsudotlam(col);
+}
+
 void MbD::Part::postPosICIteration()
 {
 	partFrame->postPosICIteration();
@@ -264,7 +342,7 @@ void MbD::Part::constraintsReport()
 void MbD::Part::postPosIC()
 {
 	partFrame->postPosIC();
-	//this->calcmE();
+	this->calcmE();
 }
 
 void MbD::Part::outputStates()
@@ -297,3 +375,116 @@ void MbD::Part::preVelIC()
 {
 	partFrame->preVelIC();
 }
+
+void MbD::Part::postVelIC()
+{
+	partFrame->postVelIC();
+	this->calcp();
+	this->calcmEdot();
+	this->calcpTpE();
+	this->calcppTpEpE();
+	this->calcppTpEpEdot();
+}
+
+void MbD::Part::fillVelICError(FColDsptr col)
+{
+	partFrame->fillVelICError(col);
+}
+
+void MbD::Part::fillVelICJacob(SpMatDsptr mat)
+{
+	partFrame->fillVelICJacob(mat);
+}
+
+void MbD::Part::preAccIC()
+{
+	partFrame->preAccIC();
+	Item::preAccIC();
+}
+
+void MbD::Part::calcp()
+{
+	pX = mX->timesFullColumn(partFrame->qXdot);
+	pE = mE->timesFullColumn(partFrame->qEdot);
+}
+
+void MbD::Part::calcpdot()
+{
+	pXdot = mX->timesFullColumn(partFrame->qXddot);
+	pEdot = mEdot->timesFullColumn(partFrame->qEdot)->plusFullColumn(mE->timesFullColumn(partFrame->qEddot));
+}
+
+void MbD::Part::calcmEdot()
+{
+	auto aC = partFrame->aC();
+	auto aCdot = partFrame->aCdot();
+	auto a4J = aJ->times(4.0);
+	auto term1 = aC->transposeTimesFullMatrix(a4J->timesFullMatrix(aCdot));
+	auto term2 = term1->transpose();
+	mEdot = term1->plusFullMatrix(term2);
+}
+
+void MbD::Part::calcpTpE()
+{
+	//"pTpE is a column vector."
+	auto& qEdot = partFrame->qEdot;
+	auto aC = partFrame->aC();
+	auto pCpEtimesqEdot = EulerParameters<double>::pCpEtimesColumn(qEdot);
+	pTpE = (pCpEtimesqEdot->transposeTimesFullColumn(aJ->timesFullColumn(aC->timesFullColumn(qEdot))))->times(4.0);
+}
+
+void MbD::Part::calcppTpEpE()
+{
+	auto& qEdot = partFrame->qEdot;
+	auto pCpEtimesqEdot = EulerParameters<double>::pCpEtimesColumn(qEdot);
+	auto a4J = aJ->times(4.0);
+	ppTpEpE = pCpEtimesqEdot->transposeTimesFullMatrix(a4J->timesFullMatrix(pCpEtimesqEdot));
+}
+
+void MbD::Part::calcppTpEpEdot()
+{
+	//| qEdot aC a4J term1 pCpEtimesqEdot term2 |
+	auto& qEdot = partFrame->qEdot;
+	auto aC = partFrame->aC();
+	auto a4J = aJ->times(4.0);
+	auto term1 = EulerParameters<double>::pCTpEtimesColumn(a4J->timesFullColumn(aC->timesFullColumn(qEdot)));
+	auto pCpEtimesqEdot = EulerParameters<double>::pCpEtimesColumn(qEdot);
+	auto term2 = aC->transposeTimesFullMatrix(a4J->timesFullMatrix(pCpEtimesqEdot));
+	ppTpEpEdot = term1->plusFullMatrix(term2)->transpose();
+}
+
+void MbD::Part::calcmE()
+{
+	auto aC = partFrame->aC();
+	mE = aC->transposeTimesFullMatrix(aJ->timesFullMatrix(aC))->times(4.0);
+}
+
+void MbD::Part::fillAccICIterError(FColDsptr col)
+{
+	auto iqX = partFrame->iqX;
+	auto iqE = partFrame->iqE;
+	col->atiminusFullColumn(iqX, mX->timesFullColumn(partFrame->qXddot));
+	col->atiminusFullColumn(iqE, mEdot->timesFullColumn(partFrame->qEdot));
+	col->atiminusFullColumn(iqE, mE->timesFullColumn(partFrame->qEddot));
+	col->atiplusFullColumn(iqE, pTpE);
+	partFrame->fillAccICIterError(col);
+}
+
+void MbD::Part::fillAccICIterJacob(SpMatDsptr mat)
+{
+	auto iqX = partFrame->iqX;
+	auto iqE = partFrame->iqE;
+	mat->atijminusDiagonalMatrix(iqX, iqX, mX);
+	mat->atijminusFullMatrix(iqE, iqE, mE);
+	partFrame->fillAccICIterJacob(mat);
+}
+
+std::shared_ptr<EulerParametersDot<double>> MbD::Part::qEdot()
+{
+	return partFrame->qEdot;
+}
+
+void MbD::Part::setqsuddotlam(FColDsptr qsudotlam)
+{
+	partFrame->setqsuddotlam(qsudotlam);
+}

MbDCode/Part.h

@@ -4,6 +4,7 @@
 #include "Item.h"
 #include "FullColumn.h"
 #include "FullMatrix.h"
+#include "EulerParametersDot.h"
 
 namespace MbD {
 	class System;
@@ -17,7 +18,7 @@ namespace MbD {
 	public:
 		Part();
 		Part(const char* str);
-		void initialize();
+		void initialize() override;
 		void initializeLocally() override;
 		void initializeGlobally() override;
 		void setqX(FColDsptr x);
@@ -32,6 +33,18 @@ namespace MbD {
 		FColDsptr getqXddot();
 		void setqEddot(FColDsptr x);
 		FColDsptr getqEddot();
+		void qX(FColDsptr x);
+		FColDsptr qX();
+		void qE(std::shared_ptr<EulerParameters<double>> x);
+		std::shared_ptr<EulerParameters<double>> qE();
+		void qXdot(FColDsptr x);
+		FColDsptr qXdot();
+		void omeOpO(FColDsptr x);
+		FColDsptr omeOpO();
+		void qXddot(FColDsptr x);
+		FColDsptr qXddot();
+		void qEddot(FColDsptr x);
+		FColDsptr qEddot();
 		void setSystem(System& sys);
 		void asFixed();
 		void postInput() override;
@@ -52,6 +65,7 @@ namespace MbD {
 		void useEquationNumbers() override;
 		void setqsu(FColDsptr col) override;
 		void setqsulam(FColDsptr col) override;
+		void setqsudotlam(FColDsptr col) override;
 		void postPosICIteration() override;
 		void fillPosICError(FColDsptr col) override;
 		void fillPosICJacob(SpMatDsptr mat) override;
@@ -65,6 +79,21 @@ namespace MbD {
 		void fillPosKineError(FColDsptr col) override;
 		void fillPosKineJacob(SpMatDsptr mat) override;
 		void preVelIC() override;
+		void postVelIC() override;
+		void fillVelICError(FColDsptr col) override;
+		void fillVelICJacob(SpMatDsptr mat) override;
+		void preAccIC() override;
+		void calcp();
+		void calcpdot();
+		void calcmEdot();
+		void calcpTpE();
+		void calcppTpEpE();
+		void calcppTpEpEdot();
+		void calcmE();
+		void fillAccICIterError(FColDsptr col) override;
+		void fillAccICIterJacob(SpMatDsptr mat) override;
+		std::shared_ptr<EulerParametersDot<double>> qEdot();
+		void setqsuddotlam(FColDsptr qsudotlam) override;
 
 		int ipX = -1; 
 		int ipE = -1; 
@@ -76,7 +105,7 @@ namespace MbD {
 		FColDsptr pE;
 		FColDsptr pEdot;
 		std::shared_ptr<DiagonalMatrix<double>> mX;
-		std::shared_ptr<DiagonalMatrix<double>> mE;
+		FMatDsptr mE;
 		FMatDsptr mEdot;
 		FColDsptr pTpE;
 		FMatDsptr ppTpEpE;

MbDCode/PartFrame.cpp

@@ -55,12 +55,11 @@ FColDsptr PartFrame::getqE() {
 	return qE;
 }
 void PartFrame::setqXdot(FColDsptr x) {
-	//qXdot->copy(x);
+	qXdot = x;
 }
 
 FColDsptr PartFrame::getqXdot() {
-	//return qXdot;
-	return std::make_shared<FullColumn<double>>(3);
+	return qXdot;
 }
 
 void PartFrame::setomeOpO(FColDsptr omeOpO) {
@@ -200,6 +199,16 @@ FMatDsptr MbD::PartFrame::aAOp()
 	return qE->aA;
 }
 
+FMatDsptr MbD::PartFrame::aC()
+{
+	return qE->aC;
+}
+
+FMatDsptr MbD::PartFrame::aCdot()
+{
+	return qEdot->aCdot;
+}
+
 FColFMatDsptr MbD::PartFrame::pAOppE()
 {
 	return qE->pApE;
@@ -279,6 +288,15 @@ void MbD::PartFrame::setqsulam(FColDsptr col)
 	aGabsDo([&](std::shared_ptr<Constraint> con) { con->setqsulam(col); });
 }
 
+void MbD::PartFrame::setqsudotlam(FColDsptr col)
+{
+	qXdot->equalFullColumnAt(col, iqX);
+	qEdot->equalFullColumnAt(col, iqE);
+	markerFramesDo([&](std::shared_ptr<MarkerFrame> markerFrame) { markerFrame->setqsudotlam(col); });
+	aGeu->setqsudotlam(col);
+	aGabsDo([&](std::shared_ptr<Constraint> con) { con->setqsudotlam(col); });
+}
+
 void MbD::PartFrame::postPosICIteration()
 {
 	Item::postPosICIteration();
@@ -351,6 +369,62 @@ void MbD::PartFrame::preVelIC()
 	aGabsDo([](std::shared_ptr<Constraint> aGab) { aGab->preVelIC(); });
 }
 
+void MbD::PartFrame::postVelIC()
+{
+	qEdot->calcAdotBdotCdot();
+	qEdot->calcpAdotpE();
+	markerFramesDo([](std::shared_ptr<MarkerFrame> markerFrame) { markerFrame->postVelIC(); });
+	aGeu->postVelIC();
+	aGabsDo([](std::shared_ptr<Constraint> aGab) { aGab->postVelIC(); });
+}
+
+void MbD::PartFrame::fillVelICError(FColDsptr col)
+{
+	markerFramesDo([&](std::shared_ptr<MarkerFrame> markerFrame) { markerFrame->fillVelICError(col); });
+	aGeu->fillVelICError(col);
+	aGabsDo([&](std::shared_ptr<Constraint> con) { con->fillVelICError(col); });
+}
+
+void MbD::PartFrame::fillVelICJacob(SpMatDsptr mat)
+{
+	markerFramesDo([&](std::shared_ptr<MarkerFrame> markerFrame) { markerFrame->fillVelICJacob(mat); });
+	aGeu->fillVelICJacob(mat);
+	aGabsDo([&](std::shared_ptr<Constraint> con) { con->fillVelICJacob(mat); });
+}
+
+void MbD::PartFrame::preAccIC()
+{
+	qXddot = std::make_shared<FullColumn<double>>(3, 0.0);
+	qEddot = std::make_shared<FullColumn<double>>(4, 0.0);
+	Item::preAccIC();
+	markerFramesDo([](std::shared_ptr<MarkerFrame> markerFrame) { markerFrame->preAccIC(); });
+	aGeu->preAccIC();
+	aGabsDo([](std::shared_ptr<Constraint> aGab) { aGab->preAccIC(); });
+}
+
+void MbD::PartFrame::fillAccICIterError(FColDsptr col)
+{
+	markerFramesDo([&](std::shared_ptr<MarkerFrame> markerFrame) { markerFrame->fillAccICIterError(col); });
+	aGeu->fillAccICIterError(col);
+	aGabsDo([&](std::shared_ptr<Constraint> con) { con->fillAccICIterError(col); });
+}
+
+void MbD::PartFrame::fillAccICIterJacob(SpMatDsptr mat)
+{
+	markerFramesDo([&](std::shared_ptr<MarkerFrame> markerFrame) { markerFrame->fillAccICIterJacob(mat); });
+	aGeu->fillAccICIterJacob(mat);
+	aGabsDo([&](std::shared_ptr<Constraint> con) { con->fillAccICIterJacob(mat); });
+}
+
+void MbD::PartFrame::setqsuddotlam(FColDsptr qsudotlam)
+{
+	qXddot->equalFullColumnAt(qsudotlam, iqX);
+	qEddot->equalFullColumnAt(qsudotlam, iqE);
+	markerFramesDo([&](std::shared_ptr<MarkerFrame> markerFrame) { markerFrame->setqsuddotlam(qsudotlam); });
+	aGeu->setqsuddotlam(qsudotlam);
+	aGabsDo([&](std::shared_ptr<Constraint> con) { con->setqsuddotlam(qsudotlam); });
+}
+
 void PartFrame::asFixed()
 {
 	for (int i = 0; i < 6; i++) {
@@ -362,8 +436,8 @@ void PartFrame::asFixed()
 
 void MbD::PartFrame::postInput()
 {
-	//qXddot = std::make_shared<FullColumn<double>>(3, 0.0);
-	//qEddot = std::make_shared<FullColumn<double>>(4, 0.0);
+	qXddot = std::make_shared<FullColumn<double>>(3, 0.0);
+	qEddot = std::make_shared<FullColumn<double>>(4, 0.0);
 	Item::postInput();
 	markerFramesDo([](std::shared_ptr<MarkerFrame> markerFrame) { markerFrame->postInput(); });
 	aGeu->postInput();
@@ -374,6 +448,6 @@ void MbD::PartFrame::calcPostDynCorrectorIteration()
 {
 	qE->calcABC();
 	qE->calcpApE();
-	//qEdot->calcAdotBdotCdot();
-	//qEdot->calcpAdotpE();
+	qEdot->calcAdotBdotCdot();
+	qEdot->calcpAdotpE();
 }

MbDCode/PartFrame.h

@@ -24,7 +24,7 @@ namespace MbD {
 	public:
 		PartFrame();
 		PartFrame(const char* str);
-		void initialize();
+		void initialize() override;
 		void initializeLocally() override;
 		void initializeGlobally() override;
 		void asFixed();
@@ -57,6 +57,8 @@ namespace MbD {
 		void prePosKine() override;
 		FColDsptr rOpO();
 		FMatDsptr aAOp();
+		FMatDsptr aC();
+		FMatDsptr aCdot();
 		FColFMatDsptr pAOppE();
 		void fillEssenConstraints(std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> essenConstraints) override;
 		void fillRedundantConstraints(std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> redunConstraints) override;
@@ -69,6 +71,7 @@ namespace MbD {
 		void useEquationNumbers() override;
 		void setqsu(FColDsptr col) override;
 		void setqsulam(FColDsptr col) override;
+		void setqsudotlam(FColDsptr col) override;
 		void postPosICIteration() override;
 		void fillPosICError(FColDsptr col) override;
 		void fillPosICJacob(SpMatDsptr mat) override;
@@ -78,6 +81,13 @@ namespace MbD {
 		void storeDynState() override;
 		void fillPosKineError(FColDsptr col) override;
 		void preVelIC() override;
+		void postVelIC() override;
+		void fillVelICError(FColDsptr col) override;
+		void fillVelICJacob(SpMatDsptr mat) override;
+		void preAccIC() override;
+		void fillAccICIterError(FColDsptr col) override;
+		void fillAccICIterJacob(SpMatDsptr mat) override;
+		void setqsuddotlam(FColDsptr qsudotlam) override;
 
 		Part* part = nullptr; //Use raw pointer when pointing backwards.
 		int iqX = -1;
@@ -85,7 +95,7 @@ namespace MbD {
 		FColDsptr qX = std::make_shared<FullColumn<double>>(3);
 		std::shared_ptr<EulerParameters<double>> qE = CREATE<EulerParameters<double>>::With(4);
 		FColDsptr qXdot = std::make_shared<FullColumn<double>>(3);
-		std::shared_ptr<EulerParametersDot<double>> qEdot = std::make_shared<EulerParametersDot<double>>(4);
+		std::shared_ptr<EulerParametersDot<double>> qEdot = CREATE<EulerParametersDot<double>>::With(4);
 		FColDsptr qXddot = std::make_shared<FullColumn<double>>(3);
 		FColDsptr qEddot = std::make_shared<FullColumn<double>>(4);
 		std::shared_ptr<Constraint> aGeu;

MbDCode/PosNewtonRaphson.h

@@ -10,7 +10,7 @@ namespace MbD {
         void preRun() override;
         void incrementIterNo() override;
         void askSystemToUpdate() override;
-        virtual void postRun() override;
+        void postRun() override;
     };
 }
 

MbDCode/PrescribedMotion.h

@@ -10,7 +10,7 @@ namespace MbD {
     public:
         PrescribedMotion();
         PrescribedMotion(const char* str);
-        void initialize();
+        void initialize() override;
         void connectsItoJ(EndFrmcptr frmI, EndFrmcptr frmJ) override;
 
         Symsptr xBlk;

MbDCode/RedundantConstraint.cpp

@@ -59,6 +59,10 @@ void MbD::RedundantConstraint::setqsulam(FColDsptr col)
 {
 }
 
+void MbD::RedundantConstraint::setqsudotlam(FColDsptr col)
+{
+}
+
 void MbD::RedundantConstraint::fillPosICError(FColDsptr col)
 {
 }
@@ -74,3 +78,15 @@ void MbD::RedundantConstraint::fillPosKineJacob(SpMatDsptr mat)
 void MbD::RedundantConstraint::preVelIC()
 {
 }
+
+void MbD::RedundantConstraint::preAccIC()
+{
+}
+
+void MbD::RedundantConstraint::fillAccICIterError(FColDsptr col)
+{
+}
+
+void MbD::RedundantConstraint::setqsuddotlam(FColDsptr qsudotlam)
+{
+}

MbDCode/RedundantConstraint.h

@@ -20,10 +20,14 @@ namespace MbD {
 		void fillConstraints(std::shared_ptr<Constraint> sptr, std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> redunConstraints);
 		void fillRedundantConstraints(std::shared_ptr<Constraint> sptr, std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> allConstraints);
 		void setqsulam(FColDsptr col) override;
+		void setqsudotlam(FColDsptr col) override;
 		void fillPosICError(FColDsptr col) override;
 		void fillPosKineError(FColDsptr col) override;
 		void fillPosKineJacob(SpMatDsptr mat) override;
 		void preVelIC() override;
+		void preAccIC() override;
+		void fillAccICIterError(FColDsptr col) override;
+		void setqsuddotlam(FColDsptr qsudotlam) override;
 
 		std::shared_ptr<Constraint> constraint;
 	};

MbDCode/SingularMatrixError.h

@@ -9,7 +9,6 @@ namespace MbD {
 	class SingularMatrixError : virtual public std::runtime_error
 	{
 	protected:
-
 		std::shared_ptr<std::vector<int>> redundantEqnNos;
 
 	public:

MbDCode/SparseMatrix.h

@@ -27,6 +27,7 @@ namespace MbD {
 				this->push_back(row);
 			}
 		}
+		void atiput(int i, std::shared_ptr<SparseRow<T>> spRow);
 		void atijplusDiagonalMatrix(int i, int j, std::shared_ptr<DiagonalMatrix<double>> diagMat);
 		void atijminusDiagonalMatrix(int i, int j, std::shared_ptr<DiagonalMatrix<double>> diagMat);
 		double sumOfSquares() override;
@@ -34,19 +35,29 @@ namespace MbD {
 		void atijplusFullRow(int i, int j, std::shared_ptr<FullRow<T>> fullRow);
 		void atijplusFullColumn(int i, int j, std::shared_ptr<FullColumn<T>> fullCol);
 		void atijplusFullMatrix(int i, int j, std::shared_ptr<FullMatrix<T>> fullMat);
+		void atijminusFullMatrix(int i, int j, std::shared_ptr<FullMatrix<T>> fullMat);
 		void atijplusTransposeFullMatrix(int i, int j, std::shared_ptr<FullMatrix<T>> fullMat);
 		void atijplusFullMatrixtimes(int i, int j, std::shared_ptr<FullMatrix<T>> fullMat, T factor);
 		void atijplusNumber(int i, int j, double value);
 		void atijminusNumber(int i, int j, double value);
+		void atijput(int i, int j, T value);
 
 		virtual std::ostream& printOn(std::ostream& s) const;
 		friend std::ostream& operator<<(std::ostream& s, const SparseMatrix& spMat)
 		{
 			return spMat.printOn(s);
 		}
+		std::shared_ptr<FullColumn<T>> timesFullColumn(std::shared_ptr<FullColumn<T>> fullCol);
+
 	};
 	using SpMatDsptr = std::shared_ptr<SparseMatrix<double>>;
 
+	template<typename T>
+	inline void SparseMatrix<T>::atiput(int i, std::shared_ptr<SparseRow<T>> spRow)
+	{
+		this->at(i) = spRow;
+	}
+
 	template<typename T>
 	inline void SparseMatrix<T>::atijplusDiagonalMatrix(int i, int j, std::shared_ptr<DiagonalMatrix<double>> diagMat)
 	{
@@ -105,6 +116,14 @@ namespace MbD {
 		}
 	}
 	template<typename T>
+	inline void SparseMatrix<T>::atijminusFullMatrix(int i, int j, std::shared_ptr<FullMatrix<T>> fullMat)
+	{
+		for (int ii = 0; ii < fullMat->nrow(); ii++)
+		{
+			this->at(i + ii)->atiminusFullRow(j, fullMat->at(ii));
+		}
+	}
+	template<typename T>
 	inline void SparseMatrix<T>::atijplusTransposeFullMatrix(int i, int j, std::shared_ptr<FullMatrix<T>> fullMat)
 	{
 		for (int ii = 0; ii < fullMat->nrow(); ii++)
@@ -131,6 +150,11 @@ namespace MbD {
 		this->at(i)->atiminusNumber(j, value);
 	}
 	template<typename T>
+	inline void SparseMatrix<T>::atijput(int i, int j, T value)
+	{
+		this->at(i)->atiput(j, value);
+	}
+	template<typename T>
 	inline std::ostream& SparseMatrix<T>::printOn(std::ostream& s) const
 	{
 		s << "SpMat[" << std::endl;
@@ -141,4 +165,16 @@ namespace MbD {
 		s << "]" << std::endl;
 		return s;
 	}
+	template<typename T>
+	inline std::shared_ptr<FullColumn<T>> SparseMatrix<T>::timesFullColumn(std::shared_ptr<FullColumn<T>> fullCol)
+	{
+		//"a*b = a(i,j)b(j) sum j."
+		auto nrow = this->nrow();
+		auto answer = std::make_shared<FullColumn<T>>(nrow);
+		for (int i = 0; i < nrow; i++)
+		{
+			answer->at(i) = this->at(i)->timesFullColumn(fullCol);
+		}
+		return answer;
+	}
 }

MbDCode/SparseRow.h

@@ -17,7 +17,10 @@ namespace MbD {
 		std::shared_ptr<SparseRow<double>> timesconditionedWithTol(double scaling, double tol);
 		std::shared_ptr<SparseRow<double>> conditionedWithTol(double tol);
 		void atiplusFullRow(int j, std::shared_ptr<FullRow<T>> fullRow);
+		void atiminusFullRow(int j, std::shared_ptr<FullRow<T>> fullRow);
 		void atiplusFullRowtimes(int j, std::shared_ptr<FullRow<T>> fullRow, double factor);
+		T timesFullColumn(std::shared_ptr<FullColumn<T>> fullCol);
+
 	};
 	using SpRowDsptr = std::shared_ptr<SparseRow<double>>;
 	template<>
@@ -51,6 +54,14 @@ namespace MbD {
 		}
 	}
 	template<typename T>
+	inline void SparseRow<T>::atiminusFullRow(int j, std::shared_ptr<FullRow<T>> fullRow)
+	{
+		for (int jj = 0; jj < fullRow->size(); jj++)
+		{
+			(*this)[j + jj] -= fullRow->at(jj);
+		}
+	}
+	template<typename T>
 	inline void SparseRow<T>::atiplusFullRowtimes(int j, std::shared_ptr<FullRow<T>> fullRow, double factor)
 	{
 		for (int jj = 0; jj < fullRow->size(); jj++)
@@ -58,5 +69,14 @@ namespace MbD {
 			(*this)[j + jj] += fullRow->at(jj) * factor;
 		}
 	}
+	template<typename T>
+	inline T SparseRow<T>::timesFullColumn(std::shared_ptr<FullColumn<T>> fullCol)
+	{
+		T sum = 0.0;
+		for (auto const& keyValue : *this) {
+			sum += fullCol->at(keyValue.first) * keyValue.second;
+		}
+		return sum;
+	}
 }
 

MbDCode/SparseVector.h

@@ -28,6 +28,7 @@ namespace MbD {
 		double rootMeanSquare();
 		int numberOfElements();
 		double sumOfSquares();
+		void atiput(int i, T value);
 		void atiplusNumber(int i, double value);
 		void atiminusNumber(int i, double value);
 		void zeroSelf();
@@ -59,6 +60,11 @@ namespace MbD {
 		}
 		return sum;
 	}
+	template<typename T>
+	inline void SparseVector<T>::atiput(int i, T value)
+	{
+		(*this)[i] = value;
+	}
 	template<>
 	inline void SparseVector<double>::atiplusNumber(int i, double value)
 	{

MbDCode/System.cpp

@@ -80,7 +80,7 @@ void System::initializeGlobally()
 
 std::shared_ptr<std::vector<std::string>> MbD::System::discontinuitiesAtIC()
 {
-	return std::shared_ptr<std::vector<std::string>>();
+	return std::make_shared<std::vector<std::string>>();
 }
 
 void MbD::System::jointsMotionsDo(const std::function<void(std::shared_ptr<Joint>)>& f)

MbDCode/SystemSolver.cpp

@@ -14,6 +14,7 @@
 #include "PosICKineNewtonRaphson.h"
 #include "PosKineNewtonRaphson.h"
 #include "VelICSolver.h"
+#include "AccICNewtonRaphson.h"
 
 using namespace MbD;
 
@@ -81,7 +82,9 @@ void MbD::SystemSolver::runVelIC()
 
 void MbD::SystemSolver::runAccIC()
 {
-	assert(false);
+	icTypeSolver = CREATE<AccICNewtonRaphson>::With();
+	icTypeSolver->setSystem(this);
+	icTypeSolver->run();
 }
 
 bool MbD::SystemSolver::needToRedoPosIC()