runOndselPiston, runPiston execute correctly

MbDCode/AbsConstraint.cpp

@@ -17,7 +17,7 @@ void AbsConstraint::initialize()
     Constraint::initialize();
 }
 
-void MbD::AbsConstraint::calcPostDynCorrectorIteration()
+void AbsConstraint::calcPostDynCorrectorIteration()
 {
     if (axis < 3) {
         aG = static_cast<PartFrame*>(owner)->qX->at(axis);
@@ -27,34 +27,34 @@ void MbD::AbsConstraint::calcPostDynCorrectorIteration()
     }
 }
 
-void MbD::AbsConstraint::useEquationNumbers()
+void AbsConstraint::useEquationNumbers()
 {
     iqXminusOnePlusAxis = static_cast<PartFrame*>(owner)->iqX + axis;
 }
 
-void MbD::AbsConstraint::fillPosICJacob(SpMatDsptr mat)
+void AbsConstraint::fillPosICJacob(SpMatDsptr mat)
 {
     mat->atijplusNumber(iG, iqXminusOnePlusAxis, 1.0);
     mat->atijplusNumber(iqXminusOnePlusAxis, iG, 1.0);
 }
 
-void MbD::AbsConstraint::fillPosICError(FColDsptr col)
+void AbsConstraint::fillPosICError(FColDsptr col)
 {
     Constraint::fillPosICError(col);
     col->at(iqXminusOnePlusAxis) += lam;
 }
 
-void MbD::AbsConstraint::fillPosKineJacob(SpMatDsptr mat)
+void AbsConstraint::fillPosKineJacob(SpMatDsptr mat)
 {
     mat->atijplusNumber(iG, iqXminusOnePlusAxis, 1.0);
 }
 
-void MbD::AbsConstraint::fillVelICJacob(SpMatDsptr mat)
+void AbsConstraint::fillVelICJacob(SpMatDsptr mat)
 {
     this->fillPosICJacob(mat);
 }
 
-void MbD::AbsConstraint::fillAccICIterError(FColDsptr col)
+void AbsConstraint::fillAccICIterError(FColDsptr col)
 {
     col->atiplusNumber(iqXminusOnePlusAxis, lam);
     auto partFrame = static_cast<PartFrame*>(owner);

MbDCode/AccICKineNewtonRaphson.cpp

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

MbDCode/AccICNewtonRaphson.cpp

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

MbDCode/AccKineNewtonRaphson.cpp

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

MbDCode/AccNewtonRaphson.cpp

@@ -5,12 +5,14 @@
 #include "SimulationStoppingError.h"
 #include <iostream>
 
-void MbD::AccNewtonRaphson::askSystemToUpdate()
+using namespace MbD;
+
+void AccNewtonRaphson::askSystemToUpdate()
 {
 	system->partsJointsMotionsForcesTorquesDo([&](std::shared_ptr<Item> item) { item->postAccICIteration(); });
 }
 
-void MbD::AccNewtonRaphson::assignEquationNumbers()
+void AccNewtonRaphson::assignEquationNumbers()
 {
 	auto parts = system->parts();
 	//auto contactEndFrames = system->contactEndFrames();
@@ -41,7 +43,7 @@ void MbD::AccNewtonRaphson::assignEquationNumbers()
 	n = nEqns;
 }
 
-void MbD::AccNewtonRaphson::fillPyPx()
+void AccNewtonRaphson::fillPyPx()
 {
 	pypx->zeroSelf();
 	system->partsJointsMotionsForcesTorquesDo([&](std::shared_ptr<Item> item) {
@@ -49,17 +51,17 @@ void MbD::AccNewtonRaphson::fillPyPx()
 		});
 }
 
-void MbD::AccNewtonRaphson::fillY()
+void 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;
+	std::cout << *y << std::endl;
 }
 
-void MbD::AccNewtonRaphson::incrementIterNo()
+void AccNewtonRaphson::incrementIterNo()
 {
 	if (iterNo >= iterMax)
 	{
@@ -82,13 +84,13 @@ void MbD::AccNewtonRaphson::incrementIterNo()
 	iterNo++;
 }
 
-void MbD::AccNewtonRaphson::initializeGlobally()
+void AccNewtonRaphson::initializeGlobally()
 {
 	SystemNewtonRaphson::initializeGlobally();
 	system->partsJointsMotionsDo([&](std::shared_ptr<Item> item) { item->fillqsuddotlam(x); });
 }
 
-void MbD::AccNewtonRaphson::logSingularMatrixMessage()
+void AccNewtonRaphson::logSingularMatrixMessage()
 {
 	std::string str = "MbD: Some parts with zero masses or moment of inertias have infinite accelerations.";
 	system->logString(str);
@@ -96,17 +98,17 @@ void MbD::AccNewtonRaphson::logSingularMatrixMessage()
 	system->logString(str);
 }
 
-void MbD::AccNewtonRaphson::passRootToSystem()
+void AccNewtonRaphson::passRootToSystem()
 {
 	system->partsJointsMotionsDo([&](std::shared_ptr<Item> item) { item->setqsuddotlam(x); });
 }
 
-void MbD::AccNewtonRaphson::postRun()
+void AccNewtonRaphson::postRun()
 {
 	system->partsJointsMotionsForcesTorquesDo([&](std::shared_ptr<Item> item) { item->postAccIC(); });
 }
 
-void MbD::AccNewtonRaphson::preRun()
+void AccNewtonRaphson::preRun()
 {
 	system->partsJointsMotionsForcesTorquesDo([&](std::shared_ptr<Item> item) { item->preAccIC(); });
 }

MbDCode/AnyPosICNewtonRaphson.cpp

@@ -5,13 +5,13 @@
 
 using namespace MbD;
 
-void MbD::AnyPosICNewtonRaphson::initialize()
+void AnyPosICNewtonRaphson::initialize()
 {
 	NewtonRaphson::initialize();
 	nSingularMatrixError = 0;
 }
 
-void MbD::AnyPosICNewtonRaphson::initializeGlobally()
+void AnyPosICNewtonRaphson::initializeGlobally()
 {
 	SystemNewtonRaphson::initializeGlobally();
 	system->partsJointsMotionsDo([&](std::shared_ptr<Item> item) {
@@ -21,14 +21,14 @@ void MbD::AnyPosICNewtonRaphson::initializeGlobally()
 		});
 }
 
-void MbD::AnyPosICNewtonRaphson::createVectorsAndMatrices()
+void AnyPosICNewtonRaphson::createVectorsAndMatrices()
 {
 	qsuOld = std::make_shared<FullColumn<double>>(nqsu);
 	qsuWeights = std::make_shared<DiagonalMatrix<double>>(nqsu);
 	SystemNewtonRaphson::createVectorsAndMatrices();
 }
 
-void MbD::AnyPosICNewtonRaphson::fillY()
+void AnyPosICNewtonRaphson::fillY()
 {
 	auto newMinusOld = qsuOld->negated();
 	newMinusOld->equalSelfPlusFullColumnAt(x, 0);
@@ -41,7 +41,7 @@ void MbD::AnyPosICNewtonRaphson::fillY()
 	//std::cout << *y << std::endl;
 }
 
-void MbD::AnyPosICNewtonRaphson::fillPyPx()
+void AnyPosICNewtonRaphson::fillPyPx()
 {
 	pypx->zeroSelf();
 	pypx->atijminusDiagonalMatrix(0, 0, qsuWeights);
@@ -52,7 +52,7 @@ void MbD::AnyPosICNewtonRaphson::fillPyPx()
 	//std::cout << *pypx << std::endl;
 }
 
-void MbD::AnyPosICNewtonRaphson::passRootToSystem()
+void AnyPosICNewtonRaphson::passRootToSystem()
 {
 	system->partsJointsMotionsDo([&](std::shared_ptr<Item> item) { item->setqsulam(x); });
 }

MbDCode/Array.h

@@ -1,7 +1,7 @@
 #pragma once
+#include <ostream>
 #include <vector>
 #include <memory>
-//#include <type_traits>
 #include <cmath>
 #include <cassert>
 
@@ -28,6 +28,17 @@ namespace MbD {
 		double maxMagnitudeOfVector();
 		void equalArrayAt(std::shared_ptr<Array<T>> array, int i);
 
+		virtual std::ostream& printOn(std::ostream& s) const {
+			std::string str = typeid(*this).name();
+			auto classname = str.substr(11, str.size() - 11);
+			s << classname << std::endl;
+			return s;
+		}
+		friend std::ostream& operator<<(std::ostream& s, const Array& array)
+		{
+			return array.printOn(s);
+		}
+
 	};
 	template<typename T>
 	inline void Array<T>::initialize()
@@ -99,7 +110,7 @@ namespace MbD {
 			this->at(ii) = array->at(i + ii);
 		}
 	}
-		using ListD = std::initializer_list<double>;
-		using ListListD = std::initializer_list<std::initializer_list<double>>;
-		using ListListPairD = std::initializer_list<std::initializer_list<std::initializer_list<double>>>;
+	using ListD = std::initializer_list<double>;
+	using ListListD = std::initializer_list<std::initializer_list<double>>;
+	using ListListPairD = std::initializer_list<std::initializer_list<std::initializer_list<double>>>;
 }

MbDCode/AtPointConstraintIJ.cpp

@@ -15,12 +15,12 @@ void AtPointConstraintIJ::initialize()
 	initriIeJeO();
 }
 
-void MbD::AtPointConstraintIJ::initializeLocally()
+void AtPointConstraintIJ::initializeLocally()
 {
 	riIeJeO->initializeLocally();
 }
 
-void MbD::AtPointConstraintIJ::initializeGlobally()
+void AtPointConstraintIJ::initializeGlobally()
 {
 	riIeJeO->initializeGlobally();
 }
@@ -30,41 +30,41 @@ void AtPointConstraintIJ::initriIeJeO()
 	riIeJeO = CREATE<DispCompIecJecO>::With(frmI, frmJ, axis);
 }
 
-void MbD::AtPointConstraintIJ::postInput()
+void AtPointConstraintIJ::postInput()
 {
 	riIeJeO->postInput();
 	Constraint::postInput();
 }
 
-void MbD::AtPointConstraintIJ::calcPostDynCorrectorIteration()
+void AtPointConstraintIJ::calcPostDynCorrectorIteration()
 {
 	aG = riIeJeO->riIeJeO - aConstant;
 }
 
-void MbD::AtPointConstraintIJ::prePosIC()
+void AtPointConstraintIJ::prePosIC()
 {
 	riIeJeO->prePosIC();
 	Constraint::prePosIC();
 }
 
-MbD::ConstraintType MbD::AtPointConstraintIJ::type()
+ConstraintType AtPointConstraintIJ::type()
 {
-	return MbD::displacement;
+	return displacement;
 }
 
-void MbD::AtPointConstraintIJ::postPosICIteration()
+void AtPointConstraintIJ::postPosICIteration()
 {
 	riIeJeO->postPosICIteration();
 	Item::postPosICIteration();
 }
 
-void MbD::AtPointConstraintIJ::preVelIC()
+void AtPointConstraintIJ::preVelIC()
 {
 	riIeJeO->preVelIC();
 	Item::preVelIC();
 }
 
-void MbD::AtPointConstraintIJ::preAccIC()
+void AtPointConstraintIJ::preAccIC()
 {
 	riIeJeO->preAccIC();
 	Constraint::preAccIC();

MbDCode/AtPointConstraintIJ.h

@@ -17,7 +17,7 @@ namespace MbD {
         void postInput() override;
         void calcPostDynCorrectorIteration() override;
         void prePosIC() override;
-        MbD::ConstraintType type() override;
+        ConstraintType type() override;
         void postPosICIteration() override;
         void preVelIC() override;
         void preAccIC() override;

MbDCode/AtPointConstraintIqcJc.cpp

@@ -10,7 +10,7 @@ AtPointConstraintIqcJc::AtPointConstraintIqcJc(EndFrmcptr frmi, EndFrmcptr frmj,
 {
 }
 
-void MbD::AtPointConstraintIqcJc::initializeGlobally()
+void AtPointConstraintIqcJc::initializeGlobally()
 {
 	AtPointConstraintIJ::initializeGlobally();
 	ppGpEIpEI = (std::static_pointer_cast<DispCompIeqcJecO>(riIeJeO))->ppriIeJeOpEIpEI;
@@ -21,26 +21,26 @@ void AtPointConstraintIqcJc::initriIeJeO()
 	riIeJeO = CREATE<DispCompIeqcJecO>::With(frmI, frmJ, axis);
 }
 
-void MbD::AtPointConstraintIqcJc::calcPostDynCorrectorIteration()
+void AtPointConstraintIqcJc::calcPostDynCorrectorIteration()
 {
 	AtPointConstraintIJ::calcPostDynCorrectorIteration();
 	pGpEI = std::static_pointer_cast<DispCompIeqcJecO>(riIeJeO)->priIeJeOpEI;
 }
 
-void MbD::AtPointConstraintIqcJc::useEquationNumbers()
+void AtPointConstraintIqcJc::useEquationNumbers()
 {
 	iqXIminusOnePlusAxis = std::static_pointer_cast<EndFrameqc>(frmI)->iqX() + axis;
 	iqEI = std::static_pointer_cast<EndFrameqc>(frmI)->iqE();
 }
 
-void MbD::AtPointConstraintIqcJc::fillPosICError(FColDsptr col)
+void AtPointConstraintIqcJc::fillPosICError(FColDsptr col)
 {
 	Constraint::fillPosICError(col);
 	col->at(iqXIminusOnePlusAxis) -= lam;
 	col->atiplusFullVectortimes(iqEI, pGpEI, lam);
 }
 
-void MbD::AtPointConstraintIqcJc::fillPosICJacob(SpMatDsptr mat)
+void AtPointConstraintIqcJc::fillPosICJacob(SpMatDsptr mat)
 {
 	mat->atijplusNumber(iG, iqXIminusOnePlusAxis, -1.0);
 	mat->atijplusNumber(iqXIminusOnePlusAxis, iG, -1.0);
@@ -49,13 +49,13 @@ void MbD::AtPointConstraintIqcJc::fillPosICJacob(SpMatDsptr mat)
 	mat->atijplusFullMatrixtimes(iqEI, iqEI, ppGpEIpEI, lam);
 }
 
-void MbD::AtPointConstraintIqcJc::fillPosKineJacob(SpMatDsptr mat)
+void AtPointConstraintIqcJc::fillPosKineJacob(SpMatDsptr mat)
 {
 	mat->atijplusNumber(iG, iqXIminusOnePlusAxis, -1.0);
 	mat->atijplusFullRow(iG, iqEI, pGpEI);
 }
 
-void MbD::AtPointConstraintIqcJc::fillVelICJacob(SpMatDsptr mat)
+void AtPointConstraintIqcJc::fillVelICJacob(SpMatDsptr mat)
 {
 	mat->atijplusNumber(iG, iqXIminusOnePlusAxis, -1.0);
 	mat->atijplusNumber(iqXIminusOnePlusAxis, iG, -1.0);
@@ -63,7 +63,7 @@ void MbD::AtPointConstraintIqcJc::fillVelICJacob(SpMatDsptr mat)
 	mat->atijplusFullColumn(iqEI, iG, pGpEI->transpose());
 }
 
-void MbD::AtPointConstraintIqcJc::fillAccICIterError(FColDsptr col)
+void AtPointConstraintIqcJc::fillAccICIterError(FColDsptr col)
 {
 	col->atiminusNumber(iqXIminusOnePlusAxis, lam);
 	col->atiplusFullVectortimes(iqEI, pGpEI, lam);
@@ -74,3 +74,26 @@ void MbD::AtPointConstraintIqcJc::fillAccICIterError(FColDsptr col)
 	sum += qEdotI->transposeTimesFullColumn(ppGpEIpEI->timesFullColumn(qEdotI));
 	col->atiplusNumber(iG, sum);
 }
+
+void AtPointConstraintIqcJc::addToJointForceI(FColDsptr col)
+{
+	col->atiminusNumber(axis, lam);
+}
+
+void AtPointConstraintIqcJc::addToJointTorqueI(FColDsptr jointTorque)
+{
+	auto cForceT = std::make_shared<FullRow<double>>(3, 0.0);
+	cForceT->atiput(axis, -lam);
+	auto rIpIeIp = frmI->rpep();
+	auto pAOIppEI = frmI->pAOppE();
+	auto aBOIp = frmI->aBOp();
+	auto fpAOIppEIrIpIeIp = std::make_shared<FullColumn<double>>(4, 0.0);
+	for (int i = 0; i < 4; i++)
+	{
+		auto dum = cForceT->timesFullColumn(pAOIppEI->at(i)->timesFullColumn(rIpIeIp));
+		fpAOIppEIrIpIeIp->atiput(i, dum);
+	}
+	auto lampGpE = pGpEI->transpose()->times(lam);
+	auto c2Torque = aBOIp->timesFullColumn(lampGpE->minusFullColumn(fpAOIppEIrIpIeIp));
+	jointTorque->equalSelfPlusFullColumntimes(c2Torque, 0.5);
+}

MbDCode/AtPointConstraintIqcJc.h

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

MbDCode/AtPointConstraintIqcJqc.cpp

@@ -10,7 +10,7 @@ AtPointConstraintIqcJqc::AtPointConstraintIqcJqc(EndFrmcptr frmi, EndFrmcptr frm
 {
 }
 
-void MbD::AtPointConstraintIqcJqc::initializeGlobally()
+void AtPointConstraintIqcJqc::initializeGlobally()
 {
 	AtPointConstraintIqcJc::initializeGlobally();
 	ppGpEJpEJ = (std::static_pointer_cast<DispCompIeqcJeqcO>(riIeJeO))->ppriIeJeOpEJpEJ;
@@ -21,27 +21,27 @@ void AtPointConstraintIqcJqc::initriIeJeO()
 	riIeJeO = CREATE<DispCompIeqcJeqcO>::With(frmI, frmJ, axis);
 }
 
-void MbD::AtPointConstraintIqcJqc::calcPostDynCorrectorIteration()
+void AtPointConstraintIqcJqc::calcPostDynCorrectorIteration()
 {
 	AtPointConstraintIqcJc::calcPostDynCorrectorIteration();
 	pGpEJ = std::static_pointer_cast<DispCompIeqcJeqcO>(riIeJeO)->priIeJeOpEJ;
 }
 
-void MbD::AtPointConstraintIqcJqc::useEquationNumbers()
+void AtPointConstraintIqcJqc::useEquationNumbers()
 {
 	AtPointConstraintIqcJc::useEquationNumbers();
 	iqXJminusOnePlusAxis = std::static_pointer_cast<EndFrameqc>(frmJ)->iqX() + axis;
 	iqEJ = std::static_pointer_cast<EndFrameqc>(frmJ)->iqE();
 }
 
-void MbD::AtPointConstraintIqcJqc::fillPosICError(FColDsptr col)
+void AtPointConstraintIqcJqc::fillPosICError(FColDsptr col)
 {
 	AtPointConstraintIqcJc::fillPosICError(col);
 	col->at(iqXJminusOnePlusAxis) += lam;
 	col->atiplusFullVectortimes(iqEJ, pGpEJ, lam);
 }
 
-void MbD::AtPointConstraintIqcJqc::fillPosICJacob(SpMatDsptr mat)
+void AtPointConstraintIqcJqc::fillPosICJacob(SpMatDsptr mat)
 {
 	AtPointConstraintIqcJc::fillPosICJacob(mat);
 	mat->atijplusNumber(iG, iqXJminusOnePlusAxis, 1.0);
@@ -51,14 +51,14 @@ void MbD::AtPointConstraintIqcJqc::fillPosICJacob(SpMatDsptr mat)
 	mat->atijplusFullMatrixtimes(iqEJ, iqEJ, ppGpEJpEJ, lam);
 }
 
-void MbD::AtPointConstraintIqcJqc::fillPosKineJacob(SpMatDsptr mat)
+void AtPointConstraintIqcJqc::fillPosKineJacob(SpMatDsptr mat)
 {
 	AtPointConstraintIqcJc::fillPosKineJacob(mat);
 	mat->atijplusNumber(iG, iqXJminusOnePlusAxis, 1.0);
 	mat->atijplusFullRow(iG, iqEJ, pGpEJ);
 }
 
-void MbD::AtPointConstraintIqcJqc::fillVelICJacob(SpMatDsptr mat)
+void AtPointConstraintIqcJqc::fillVelICJacob(SpMatDsptr mat)
 {
 	AtPointConstraintIqcJc::fillVelICJacob(mat);
 	mat->atijplusNumber(iG, iqXJminusOnePlusAxis, 1.0);
@@ -67,7 +67,7 @@ void MbD::AtPointConstraintIqcJqc::fillVelICJacob(SpMatDsptr mat)
 	mat->atijplusFullColumn(iqEJ, iG, pGpEJ->transpose());
 }
 
-void MbD::AtPointConstraintIqcJqc::fillAccICIterError(FColDsptr col)
+void AtPointConstraintIqcJqc::fillAccICIterError(FColDsptr col)
 {
 	AtPointConstraintIqcJc::fillAccICIterError(col);
 	col->atiplusNumber(iqXJminusOnePlusAxis, lam);

MbDCode/AtPointConstraintIqctJqc.cpp

@@ -9,7 +9,7 @@ AtPointConstraintIqctJqc::AtPointConstraintIqctJqc(EndFrmcptr frmi, EndFrmcptr f
 {
 }
 
-void MbD::AtPointConstraintIqctJqc::initializeGlobally()
+void AtPointConstraintIqctJqc::initializeGlobally()
 {
 	riIeJeO->initializeGlobally();
 	ppGpEJpEJ = std::static_pointer_cast<DispCompIeqctJeqcO>(riIeJeO)->ppriIeJeOpEJpEJ;
@@ -20,7 +20,7 @@ void AtPointConstraintIqctJqc::initriIeJeO()
 	riIeJeO = CREATE<DispCompIeqctJeqcO>::With(frmI, frmJ, axis);
 }
 
-void MbD::AtPointConstraintIqctJqc::calcPostDynCorrectorIteration()
+void AtPointConstraintIqctJqc::calcPostDynCorrectorIteration()
 {
 	//"ppGpEIpEI is no longer constant."
 
@@ -28,23 +28,23 @@ void MbD::AtPointConstraintIqctJqc::calcPostDynCorrectorIteration()
 	AtPointConstraintIqcJqc::calcPostDynCorrectorIteration();
 }
 
-MbD::ConstraintType MbD::AtPointConstraintIqctJqc::type()
+ConstraintType AtPointConstraintIqctJqc::type()
 {
-	return MbD::essential;
+	return essential;
 }
 
-void MbD::AtPointConstraintIqctJqc::preVelIC()
+void AtPointConstraintIqctJqc::preVelIC()
 {
 	AtPointConstraintIJ::preVelIC();
 	pGpt = std::static_pointer_cast<DispCompIeqctJeqcO>(riIeJeO)->priIeJeOpt;
 }
 
-void MbD::AtPointConstraintIqctJqc::fillVelICError(FColDsptr col)
+void AtPointConstraintIqctJqc::fillVelICError(FColDsptr col)
 {
 	col->atiminusNumber(iG, pGpt);
 }
 
-void MbD::AtPointConstraintIqctJqc::fillAccICIterError(FColDsptr col)
+void AtPointConstraintIqctJqc::fillAccICIterError(FColDsptr col)
 {
 	AtPointConstraintIqcJqc::fillAccICIterError(col);
 	auto efrmIqc = std::static_pointer_cast<EndFrameqc>(frmI);
@@ -54,7 +54,7 @@ void MbD::AtPointConstraintIqctJqc::fillAccICIterError(FColDsptr col)
 	col->atiplusNumber(iG, sum);
 }
 
-void MbD::AtPointConstraintIqctJqc::preAccIC()
+void AtPointConstraintIqctJqc::preAccIC()
 {
 	AtPointConstraintIJ::preAccIC();
 	ppGpEIpt = std::static_pointer_cast<DispCompIeqctJeqcO>(riIeJeO)->ppriIeJeOpEIpt;

MbDCode/AtPointConstraintIqctJqc.h

@@ -11,7 +11,7 @@ namespace MbD {
         void initializeGlobally() override;
         void initriIeJeO() override;
         void calcPostDynCorrectorIteration() override;
-        MbD::ConstraintType type() override;
+        ConstraintType type() override;
         void preVelIC() override;
         void fillVelICError(FColDsptr col) override;
         void fillAccICIterError(FColDsptr col) override;

MbDCode/BasicIntegrator.cpp

@@ -5,30 +5,34 @@
 
 using namespace MbD;
 
-void MbD::BasicIntegrator::initializeLocally()
+void BasicIntegrator::initializeLocally()
 {
 	_continue = true;
 }
 
-void MbD::BasicIntegrator::iStep(int integer)
+void BasicIntegrator::iStep(int integer)
 {
 	istep = integer;
 	opBDF->setiStep(integer);
 }
 
-void MbD::BasicIntegrator::postFirstStep()
+void BasicIntegrator::postFirstStep()
+{
+	t = tnew;
+	system->postFirstStep();
+}
+
+void BasicIntegrator::postRun()
 {
 }
 
-void MbD::BasicIntegrator::postRun()
+void BasicIntegrator::postStep()
 {
+	t = tnew;
+	system->postStep();
 }
 
-void MbD::BasicIntegrator::postStep()
-{
-}
-
-void MbD::BasicIntegrator::initializeGlobally()
+void BasicIntegrator::initializeGlobally()
 {
 	//"Get info from system and prepare for start of simulation."
 	//"Integrator asks system for info. Not system setting integrator."
@@ -38,17 +42,17 @@ void MbD::BasicIntegrator::initializeGlobally()
 	this->orderMax = system->orderMax();
 }
 
-void MbD::BasicIntegrator::setSystem(Solver* sys)
+void BasicIntegrator::setSystem(Solver* sys)
 {
 	system = static_cast<IntegratorInterface*>(sys);
 }
 
-void MbD::BasicIntegrator::calcOperatorMatrix()
+void BasicIntegrator::calcOperatorMatrix()
 {
 	opBDF->calcOperatorMatrix();
 }
 
-void MbD::BasicIntegrator::incrementTime()
+void BasicIntegrator::incrementTime()
 {
 	tpast->insert(tpast->begin(), t);
 
@@ -62,12 +66,12 @@ void MbD::BasicIntegrator::incrementTime()
 	system->incrementTime(tnew);
 }
 
-void MbD::BasicIntegrator::incrementTry()
+void BasicIntegrator::incrementTry()
 {
 	assert(false);
 }
 
-void MbD::BasicIntegrator::initialize()
+void BasicIntegrator::initialize()
 {
 	Solver::initialize();
 	//statistics = IdentityDictionary new.
@@ -76,12 +80,12 @@ void MbD::BasicIntegrator::initialize()
 	opBDF->timeNodes = tpast;
 }
 
-void MbD::BasicIntegrator::logString(std::string& str)
+void BasicIntegrator::logString(std::string& str)
 {
 	system->logString(str);
 }
 
-void MbD::BasicIntegrator::run()
+void BasicIntegrator::run()
 {
 	this->preRun();
 	this->initializeLocally();
@@ -93,59 +97,59 @@ void MbD::BasicIntegrator::run()
 	this->postRun();
 }
 
-void MbD::BasicIntegrator::selectOrder()
+void BasicIntegrator::selectOrder()
 {
-	assert(false);
+	//"Increase order consecutively with step."
+	if (iTry == 1) orderNew = std::min(istep + 1, orderMax);
 }
 
-void MbD::BasicIntegrator::preFirstStep()
+void BasicIntegrator::preFirstStep()
 {
 	system->preFirstStep();
 }
 
-void MbD::BasicIntegrator::preRun()
+void BasicIntegrator::preRun()
 {
 }
 
-void MbD::BasicIntegrator::preStep()
+void BasicIntegrator::preStep()
 {
 	system->preStep();
 }
 
-void MbD::BasicIntegrator::reportStats()
+void BasicIntegrator::reportStats()
 {
-	assert(false);
 }
 
-void MbD::BasicIntegrator::firstStep()
-{
-	assert(false);
-}
-
-void MbD::BasicIntegrator::setorder(int o)
+void BasicIntegrator::setorder(int o)
 {
 	order = o;
 	opBDF->setorder(o);
 }
 
-void MbD::BasicIntegrator::settnew(double t)
+void BasicIntegrator::settnew(double t)
 {
 	tnew = t;
 	this->settime(t);
 }
 
-void MbD::BasicIntegrator::sett(double tt)
+void BasicIntegrator::sett(double tt)
 {
 	t = tt;
 	opBDF->settime(tt);
 }
 
-void MbD::BasicIntegrator::settime(double tt)
+void BasicIntegrator::settime(double tt)
 {
 	opBDF->settime(tt);
 }
 
-void MbD::BasicIntegrator::subsequentSteps()
+double BasicIntegrator::tprevious()
+{
+	return tpast->at(0);
+}
+
+void BasicIntegrator::subsequentSteps()
 {
 	while (_continue) { this->nextStep(); }
 }

MbDCode/BasicIntegrator.h

@@ -17,26 +17,25 @@ namespace MbD {
         void initialize() override;
         void initializeGlobally() override;
         void initializeLocally() override;
-        virtual void iStep(int i);
-        virtual void postFirstStep();
+        void iStep(int i) override;
+        void postFirstStep() override;
+        void postStep() override;
         void postRun() override;
-        virtual void postStep();
-        virtual void preFirstStep();
-        virtual void preRun() override;
-        virtual void preStep();
-        virtual void reportStats() override;
+        void preFirstStep() override;
+        void preRun() override;
+        void preStep() override;
+        void reportStats() override;
         void run() override;
-        virtual void selectOrder();
-        virtual void subsequentSteps();
+        void selectOrder() override;
+        void subsequentSteps() override;
         void setSystem(Solver* sys) override;
         void logString(std::string& str) override;
-        virtual void firstStep();
-        virtual void nextStep() = 0;
         
         virtual void setorder(int o);
         virtual void settnew(double t);
         virtual void sett(double t);
         void settime(double t);
+        double tprevious();
 
         IntegratorInterface* system;
         int istep = 0, iTry = 0, maxTry = 0;

MbDCode/BasicQuasiIntegrator.cpp

@@ -3,7 +3,7 @@
 
 using namespace MbD;
 
-void MbD::BasicQuasiIntegrator::firstStep()
+void BasicQuasiIntegrator::firstStep()
 {
 	istep = 0;
 	this->preFirstStep();
@@ -13,47 +13,34 @@ void MbD::BasicQuasiIntegrator::firstStep()
 	this->incrementTime();
 	this->runInitialConditionTypeSolution();
 	//this->reportTrialStepStats();
-
-	//while (this->isRedoingFirstStep())
-	//{
-	//	this->incrementTry();
-	//	orderNew = 1;
-	//	this->selectFirstStepSize();
-	//	this->changeTime();
-	//	this->runInitialConditionTypeSolution();
-	//	this->reportTrialStepStats();
-	//}
-	//this->postFirstStep();
+	this->postFirstStep();
 	//this->reportStepStats();
 }
 
-void MbD::BasicQuasiIntegrator::nextStep()
+bool BasicQuasiIntegrator::isRedoingFirstStep()
+{
+	return false;
+}
+
+void BasicQuasiIntegrator::nextStep()
 {
 	this->preStep();
 	iTry = 1;
 	this->selectOrder();
-	//this->selectStepSize();
-	//this->incrementTime();
-	//this->runInitialConditionTypeSolution();
+	this->selectStepSize();
+	this->incrementTime();
+	this->runInitialConditionTypeSolution();
 	//this->reportTrialStepStats();
-	//while (this->isRedoingStep()) {
-	//	this->incrementTry();
-	//	this->selectOrder();
-	//	this->selectStepSize();
-	//	this->changeTime();
-	//	this->runInitialConditionTypeSolution();
-	//	this->reportTrialStepStats();
-	//}
-	//this->postStep();
+	this->postStep();
 	//this->reportStepStats();
 }
 
-void MbD::BasicQuasiIntegrator::runInitialConditionTypeSolution()
+void BasicQuasiIntegrator::runInitialConditionTypeSolution()
 {
 	system->runInitialConditionTypeSolution();
 }
 
-void MbD::BasicQuasiIntegrator::selectFirstStepSize()
+void BasicQuasiIntegrator::selectFirstStepSize()
 {
 	if (iTry == 1) {
 		hnew = direction * (system->tout - t);
@@ -63,3 +50,14 @@ void MbD::BasicQuasiIntegrator::selectFirstStepSize()
 	}
 	hnew = system->suggestSmallerOrAcceptFirstStepSize(hnew);
 }
+
+void BasicQuasiIntegrator::selectStepSize()
+{
+	if (iTry == 1) {
+		hnew = direction * (system->tout - t);
+	}
+	else {
+		hnew = 0.25 * h;
+	}
+	hnew = system->suggestSmallerOrAcceptStepSize(hnew);
+}

MbDCode/BasicQuasiIntegrator.h

@@ -8,14 +8,13 @@ namespace MbD {
 		//
 	public:
         void firstStep();
-        //bool isRedoingFirstStep();
-        //bool isRedoingStep();
+        bool isRedoingFirstStep();
         void nextStep();
         //void reportStepStats();
         //void reportTrialStepStats();
         void runInitialConditionTypeSolution() override;
         void selectFirstStepSize();
-        //void selectStepSize();
+        void selectStepSize();
 	};
 }
 

MbDCode/CADSystem.cpp

@@ -0,0 +1,522 @@
+#include <string>
+#include <iostream>
+
+#include "CADSystem.h"
+#include "CREATE.h"
+#include "System.h"
+#include "Item.h"
+#include "Product.h"
+#include "Constant.h"
+#include "ZRotation.h"
+#include "RevoluteJoint.h"
+#include "CylindricalJoint.h"
+#include "SystemSolver.h"
+#include "Part.h"
+#include "MarkerFrame.h"
+#include "PartFrame.h"
+#include "Time.h"
+#include "StateData.h"
+
+using namespace MbD;
+//using namespace CAD;
+
+void CADSystem::outputFor(AnalysisType type)
+{
+	auto str = std::to_string(mbdSystem->mbdTimeValue());
+	this->logString(str);
+	mbdSystem->partsJointsMotionsForcesTorquesDo([](std::shared_ptr<Item> item) {
+		std::cout << std::endl;
+		std::cout << item->classname() << " " << item->getName() << std::endl;
+		auto data = item->stateData();
+		std::cout << *data << std::endl;
+		});
+}
+
+void CADSystem::logString(std::string& str)
+{
+	std::cout << str << std::endl;
+}
+
+void CADSystem::logString(double value)
+{
+}
+
+void CADSystem::runOndselPiston()
+{
+	//Piston with easy input numbers for exact port from Smalltalk
+	//GEOAssembly calcCharacteristicDimensions must set mbdUnits to unity.
+	std::cout << "runOndselPiston" << std::endl;
+	auto& TheSystem = mbdSystem;
+	TheSystem->clear();
+	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.0;
+	systemSolver->tend = 1.0;
+	systemSolver->hmin = 1.0e-9;
+	systemSolver->hmax = 1.0;
+	systemSolver->hout = 0.04;
+	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 = 1.0e10;
+	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->addPart(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->addPart(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->addPart(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->addJoint(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->addJoint(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->addJoint(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->addJoint(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->addJoint(rotMotion1);
+	//
+	TheSystem->runKINEMATIC();
+}
+
+void CADSystem::runPiston()
+{
+	std::cout << "runPiston" << std::endl;
+	auto& TheSystem = mbdSystem;
+	TheSystem->clear();
+	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, 2.8817526385684, 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 = 0.045210530089461;
+	crankPart1->aJ = std::make_shared<DiagonalMatrix<double>>(ListD{ 1.7381980042084e-4, 0.003511159968501, 0.0036154518487535 });
+	qX = std::make_shared<FullColumn<double>>(ListD{ 0.38423368514246, 2.6661567755108e-17, -0.048029210642807 });
+	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.096568457800423, 0 });
+	omeOpO = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0.25132741228718 });
+	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->addPart(crankPart1);
+	{
+		auto& partFrame = crankPart1->partFrame;
+		auto marker1 = CREATE<MarkerFrame>::With("/Assembly1/Part1/Marker1");
+		rpmp = std::make_shared<FullColumn<double>>(ListD{ -0.38423368514246, -2.6661567755108e-17, 0.048029210642807 });
+		marker1->setrpmp(rpmp);
+		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.38423368514246, -2.6661567755108e-17, 0.048029210642807 });
+		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 = 0.067815795134192;
+	conrodPart2->aJ = std::make_shared<DiagonalMatrix<double>>(ListD{ 2.6072970063126e-4, 0.011784982468533, 0.011941420288912 });
+	qX = std::make_shared<FullColumn<double>>(ListD{ 0.38423368514246, 0.49215295678475, 0.048029210642807 });
+	qE = std::make_shared<FullColumn<double>>(ListD{ 0.0, 0.0, 0.89871703427292, 0.43852900965351 });
+	conrodPart2->setqX(qX);
+	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 });
+	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->addPart(conrodPart2);
+	{
+		auto& partFrame = conrodPart2->partFrame;
+		auto marker1 = CREATE<MarkerFrame>::With("/Assembly1/Part2/Marker1");
+		rpmp = std::make_shared<FullColumn<double>>(ListD{ -0.6243797383565, 1.1997705489799e-16, -0.048029210642807 });
+		marker1->setrpmp(rpmp);
+		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}
+			});
+		marker1->setaApm(aApm);
+		partFrame->addMarkerFrame(marker1);
+		//
+		auto marker2 = CREATE<MarkerFrame>::With("/Assembly1/Part2/Marker2");
+		rpmp = std::make_shared<FullColumn<double>>(ListD{ 0.6243797383565, -2.1329254204087e-16, -0.048029210642807 });
+		marker2->setrpmp(rpmp);
+		aApm = std::make_shared<FullMatrix<double>>(ListListD{
+			{1.0, 2.4980018054066e-16, 2.2204460492503e-16},
+			{-2.4980018054066e-16, 1.0, 4.1633363423443e-17},
+			{-2.2204460492503e-16, -4.1633363423443e-17, 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.730132083368;
+	pistonPart3->aJ = std::make_shared<DiagonalMatrix<double>>(ListD{ 0.19449049546716, 0.23028116340971, 0.23028116340971 });
+	qX = std::make_shared<FullColumn<double>>(ListD{ -1.283972762056e-18, 1.4645980199976, -4.7652385308244e-17 });
+	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{ -6.3364526821409e-32, 0.19313691560085, -1.933731897626e-34 });
+	omeOpO = std::make_shared<FullColumn<double>>(ListD{ 1.670970041317e-34, 1.3045598281729e-34, 1.8896472173894e-50 });
+	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->addPart(pistonPart3);
+	{
+		auto& partFrame = pistonPart3->partFrame;
+		auto marker1 = CREATE<MarkerFrame>::With("/Assembly1/Part3/Marker1");
+		rpmp = std::make_shared<FullColumn<double>>(ListD{ -0.48029210642807, 7.6201599718927e-18, -2.816737703896e-17 });
+		marker1->setrpmp(rpmp);
+		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}
+			});
+		marker1->setaApm(aApm);
+		partFrame->addMarkerFrame(marker1);
+		//
+		auto marker2 = CREATE<MarkerFrame>::With("/Assembly1/Part3/Marker2");
+		rpmp = std::make_shared<FullColumn<double>>(ListD{ 0.48029210642807, 1.7618247880058e-17, 2.5155758471256e-17 });
+		marker2->setrpmp(rpmp);
+		aApm = std::make_shared<FullMatrix<double>>(ListListD{
+			{6.9388939039072e-18, -6.4146353042213e-50, 1.0},
+			{1.0, -6.9388939039072e-18, 6.9388939039072e-18},
+			{-6.9388939039072e-18, 1.0, -7.4837411882581e-50}
+			});
+		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->addJoint(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->addJoint(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->addJoint(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->addJoint(cylJoint4);
+
+	auto rotMotion1 = CREATE<ZRotation>::With("/Assembly1/Motion1");
+	rotMotion1->connectsItoJ(assembly1->partFrame->endFrame("/Assembly1/Marker2"), crankPart1->partFrame->endFrame("/Assembly1/Part1/Marker1"));
+	std::cout << "rotMotion1->getName() " << rotMotion1->getName() << std::endl;
+	auto omega = std::make_shared<Constant>(6.2831853071796);
+	auto timeScale = std::make_shared<Constant>(0.04);
+	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->addJoint(rotMotion1);
+	//
+	TheSystem->runKINEMATIC();
+}
+
+void CADSystem::postMbDrun()
+{
+}

MbDCode/CADSystem.h

@@ -0,0 +1,31 @@
+#pragma once
+#include<memory>
+
+#include "System.h"
+
+//using namespace MbD;
+
+//namespace CAD {
+namespace MbD {
+	//class System;
+
+	class CADSystem
+	{
+		//
+	public:
+		CADSystem() {
+			mbdSystem->externalSystem = this;
+		}
+
+		void outputFor(AnalysisType type);
+		void logString(std::string& str);
+		void logString(double value);
+		void runOndselPiston();
+		void runPiston();
+		void postMbDrun();
+
+		std::shared_ptr<System> mbdSystem = std::make_shared<System>();
+
+	};
+}
+

MbDCode/Constant.cpp

@@ -15,12 +15,12 @@ Symsptr Constant::differentiateWRT(Symsptr sptr, Symsptr var)
 	return std::make_shared<Constant>(0.0);
 }
 
-bool MbD::Constant::isConstant()
+bool Constant::isConstant()
 {
 	return true;
 }
 
-std::ostream& MbD::Constant::printOn(std::ostream& s) const
+std::ostream& Constant::printOn(std::ostream& s) const
 {
 	return s << this->value;
 }

MbDCode/Constraint.cpp

@@ -22,7 +22,7 @@ void Constraint::initialize()
 	name = std::to_string(nanoseconds);
 }
 
-void MbD::Constraint::postInput()
+void Constraint::postInput()
 {
 	lam = 0.0;
 	Item::postInput();
@@ -38,14 +38,14 @@ Item* Constraint::getOwner()
 	return owner;
 }
 
-void MbD::Constraint::prePosIC()
+void Constraint::prePosIC()
 {
 	lam = 0.0;
 	iG = -1;
 	Item::prePosIC();
 }
 
-void MbD::Constraint::prePosKine()
+void Constraint::prePosKine()
 {
 	//"Preserve lam calculated from AccIC for possible use by DynIntegrator if system is not kinematic."
 	auto lamOld = lam;
@@ -53,80 +53,80 @@ void MbD::Constraint::prePosKine()
 	lam = lamOld;
 }
 
-void MbD::Constraint::fillEssenConstraints(std::shared_ptr<Constraint> sptr, std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> essenConstraints)
+void Constraint::fillEssenConstraints(std::shared_ptr<Constraint> sptr, std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> essenConstraints)
 {
-	if (this->type() == MbD::essential) {
+	if (this->type() == essential) {
 		essenConstraints->push_back(sptr);
 	}
 }
-void MbD::Constraint::fillDispConstraints(std::shared_ptr<Constraint> sptr, std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> dispConstraints)
+void Constraint::fillDispConstraints(std::shared_ptr<Constraint> sptr, std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> dispConstraints)
 {
-	if (this->type() == MbD::displacement) {
+	if (this->type() == displacement) {
 		dispConstraints->push_back(sptr);
 	}
 }
-void MbD::Constraint::fillPerpenConstraints(std::shared_ptr<Constraint> sptr, std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> perpenConstraints)
+void Constraint::fillPerpenConstraints(std::shared_ptr<Constraint> sptr, std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> perpenConstraints)
 {
-	if (this->type() == MbD::perpendicular) {
+	if (this->type() == perpendicular) {
 		perpenConstraints->push_back(sptr);
 	}
 }
 
-void MbD::Constraint::fillRedundantConstraints(std::shared_ptr<Constraint> sptr, std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> redunConstraints)
+void Constraint::fillRedundantConstraints(std::shared_ptr<Constraint> sptr, std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> redunConstraints)
 {
-	if (this->type() == MbD::redundant) {
+	if (this->type() == redundant) {
 		redunConstraints->push_back(sptr);
 	}
 }
 
-void MbD::Constraint::fillConstraints(std::shared_ptr<Constraint> sptr, std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> allConstraints)
+void Constraint::fillConstraints(std::shared_ptr<Constraint> sptr, std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> allConstraints)
 {
 	allConstraints->push_back(sptr);
 }
 
-MbD::ConstraintType MbD::Constraint::type()
+ConstraintType Constraint::type()
 {
-	return MbD::essential;
+	return essential;
 }
 
-void MbD::Constraint::fillqsulam(FColDsptr col)
+void Constraint::fillqsulam(FColDsptr col)
 {
 	col->at(iG) = lam;
 }
 
-void MbD::Constraint::setqsulam(FColDsptr col)
+void Constraint::setqsulam(FColDsptr col)
 {
 	lam = col->at(iG);
 }
 
-void MbD::Constraint::setqsudotlam(FColDsptr col)
+void Constraint::setqsudotlam(FColDsptr col)
 {
 	lam = col->at(iG);
 }
 
-void MbD::Constraint::fillPosICError(FColDsptr col)
+void Constraint::fillPosICError(FColDsptr col)
 {
 	col->at(iG) += aG;
 }
 
-void MbD::Constraint::removeRedundantConstraints(std::shared_ptr<std::vector<int>> redundantEqnNos)
+void Constraint::removeRedundantConstraints(std::shared_ptr<std::vector<int>> redundantEqnNos)
 {
 	//My owner should handle this.
 	assert(false);
 }
 
-void MbD::Constraint::reactivateRedundantConstraints()
+void Constraint::reactivateRedundantConstraints()
 {
 	//My owner should handle this.
 	assert(false);
 }
 
-bool MbD::Constraint::isRedundant()
+bool Constraint::isRedundant()
 {
 	return false;
 }
 
-void MbD::Constraint::outputStates()
+void Constraint::outputStates()
 {
 	Item::outputStates();
 	std::stringstream ss;
@@ -137,29 +137,37 @@ void MbD::Constraint::outputStates()
 	this->logString(str);
 }
 
-void MbD::Constraint::preDyn()
+void Constraint::preDyn()
 {
 	mu = 0.0;
 }
 
-void MbD::Constraint::fillPosKineError(FColDsptr col)
+void Constraint::fillPosKineError(FColDsptr col)
 {
 	col->atiplusNumber(iG, aG);
 }
 
-void MbD::Constraint::preAccIC()
+void Constraint::preAccIC()
 {
 	lam = 0.0;
 	Item::preAccIC();
 }
 
-void MbD::Constraint::fillAccICIterJacob(SpMatDsptr mat)
+void Constraint::fillAccICIterJacob(SpMatDsptr mat)
 {
 	//"Same as velIC."
 	this->fillVelICJacob(mat);
 }
 
-void MbD::Constraint::setqsuddotlam(FColDsptr qsudotlam)
+void Constraint::setqsuddotlam(FColDsptr col)
+{
+	lam = col->at(iG);
+}
+
+void Constraint::addToJointForceI(FColDsptr col)
+{
+}
+
+void Constraint::addToJointTorqueI(FColDsptr col)
 {
-	lam = qsudotlam->at(iG);
 }

MbDCode/Constraint.h

@@ -22,7 +22,7 @@ namespace MbD {
 		virtual void fillPerpenConstraints(std::shared_ptr<Constraint> sptr, std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> perpenConstraints);
 		virtual void fillRedundantConstraints(std::shared_ptr<Constraint> sptr, std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> redunConstraints);
 		virtual void fillConstraints(std::shared_ptr<Constraint> sptr, std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> allConstraints);
-		virtual MbD::ConstraintType type();
+		virtual ConstraintType type();
 		void fillqsulam(FColDsptr col) override;
 		void setqsulam(FColDsptr col) override;
 		void setqsudotlam(FColDsptr col) override;
@@ -35,13 +35,14 @@ namespace MbD {
 		void fillPosKineError(FColDsptr col) override;
 		void preAccIC() override;
 		void fillAccICIterJacob(SpMatDsptr mat) override;
-		void setqsuddotlam(FColDsptr qsudotlam) override;
-
+		void setqsuddotlam(FColDsptr col) override;
+		virtual void addToJointForceI(FColDsptr col);
+		virtual void addToJointTorqueI(FColDsptr col);
+		
 		int iG = -1;
 		double aG = 0.0;		//Constraint function
 		double lam = 0.0;		//Lambda is Lagrange Multiplier
 		double mu = 0, lamDeriv = 0;
-		Item* owner;	//A Joint or PartFrame owns the constraint.  //Use raw pointer when pointing backwards.
 	};
 }
 

MbDCode/CylindricalJoint.cpp

@@ -12,7 +12,7 @@ CylindricalJoint::CylindricalJoint() {
 CylindricalJoint::CylindricalJoint(const char* str) : Joint(str) {
 }
 
-void MbD::CylindricalJoint::initializeGlobally()
+void CylindricalJoint::initializeGlobally()
 {
 	if (constraints->empty())
 	{
@@ -20,7 +20,7 @@ void MbD::CylindricalJoint::initializeGlobally()
 		addConstraint(CREATE<TranslationConstraintIJ>::ConstraintWith(frmI, frmJ, 1));
 		addConstraint(CREATE<DirectionCosineConstraintIJ>::ConstraintWith(frmI, frmJ, 2, 0));
 		addConstraint(CREATE<DirectionCosineConstraintIJ>::ConstraintWith(frmI, frmJ, 2, 1));
-		System::getInstance().hasChanged = true;
+		this->root()->hasChanged = true;
 	}
 	else {
 		Joint::initializeGlobally();

MbDCode/DataPosVelAcc.cpp

@@ -0,0 +1,17 @@
+#include <ostream>
+
+#include "DataPosVelAcc.h"
+
+using namespace MbD;
+
+std::ostream& DataPosVelAcc::printOn(std::ostream& s) const
+{
+    s << "refData = " << *refData << std::endl;
+    s << "rFfF = " << *rFfF << std::endl;
+    s << "vFfF = " << *vFfF << std::endl;
+    s << "omeFfF = " << *omeFfF << std::endl;
+    s << "aFfF = " << *aFfF << std::endl;
+    s << "alpFfF = " << *alpFfF << std::endl;
+    s << "aAFf = " << *aAFf;
+    return s;
+}

MbDCode/DataPosVelAcc.h

@@ -0,0 +1,17 @@
+#pragma once
+
+#include "StateData.h"
+
+namespace MbD {
+    class DataPosVelAcc : public StateData
+    {
+        //refData rFfF aAFf vFfF omeFfF aFfF alpFfF 
+    public:
+        std::ostream& printOn(std::ostream& s) const override;
+
+        std::shared_ptr<DataPosVelAcc> refData;
+        FColDsptr rFfF, vFfF, omeFfF, aFfF, alpFfF;
+        FMatDsptr aAFf;
+    };
+}
+

MbDCode/DiagonalMatrix.h

@@ -26,6 +26,8 @@ namespace MbD {
 		double sumOfSquares() override;
 		int numberOfElements() override;
 		void zeroSelf() override;
+		std::ostream& printOn(std::ostream& s) const override;
+
 	};
 	template<typename T>
 	inline void DiagonalMatrix<T>::atiputDiagonalMatrix(int i, std::shared_ptr<DiagonalMatrix<T>> diagMat)
@@ -94,5 +96,17 @@ namespace MbD {
 			this->at(i) = 0.0;
 		}
 	}
+	template<typename T>
+	inline std::ostream& DiagonalMatrix<T>::printOn(std::ostream& s) const
+	{
+		s << "DiagMat[";
+		s << this->at(0);
+		for (int i = 1; i < this->size(); i++)
+		{
+			s << ", " << this->at(i);
+		}
+		s << "]";
+		return s;
+	}
 }
 

MbDCode/DifferenceOperator.cpp

@@ -17,7 +17,7 @@ std::shared_ptr<FullRow<double>> DifferenceOperator::OneOverFactorials = []() {
 	return oneOverFactorials;
 }();
 
-void MbD::DifferenceOperator::calcOperatorMatrix()
+void DifferenceOperator::calcOperatorMatrix()
 {
 	//Compute operatorMatrix such that 
 	//value(time) : = (operatorMatrix at : 1) timesColumn : series.
@@ -32,28 +32,28 @@ void MbD::DifferenceOperator::calcOperatorMatrix()
 	}
 }
 
-void MbD::DifferenceOperator::initialize()
+void DifferenceOperator::initialize()
 {
 }
 
-void MbD::DifferenceOperator::setiStep(int i)
+void DifferenceOperator::setiStep(int i)
 {
 	iStep = i;
 }
 
-void MbD::DifferenceOperator::setorder(int o)
+void DifferenceOperator::setorder(int o)
 {
 	order = o;
 }
 
-void MbD::DifferenceOperator::instantiateTaylorMatrix()
+void DifferenceOperator::instantiateTaylorMatrix()
 {
 	if (taylorMatrix == nullptr || (taylorMatrix->nrow() != (order + 1))) {
 		taylorMatrix = std::make_shared<FullMatrix<double>>(order + 1, order + 1);
 	}
 }
 
-void MbD::DifferenceOperator::formTaylorRowwithTimeNodederivative(int i, int ii, int k)
+void DifferenceOperator::formTaylorRowwithTimeNodederivative(int i, int ii, int k)
 {
 	//| rowi hi hipower aij |
 	auto& rowi = taylorMatrix->at(i);
@@ -73,7 +73,7 @@ void MbD::DifferenceOperator::formTaylorRowwithTimeNodederivative(int i, int ii,
 	}
 }
 
-void MbD::DifferenceOperator::settime(double t)
+void DifferenceOperator::settime(double t)
 {
 	time = t;
 }

MbDCode/DirectionCosineConstraintIJ.cpp

@@ -16,12 +16,12 @@ void DirectionCosineConstraintIJ::initialize()
 	initaAijIeJe();
 }
 
-void MbD::DirectionCosineConstraintIJ::initializeLocally()
+void DirectionCosineConstraintIJ::initializeLocally()
 {
 	aAijIeJe->initializeLocally();
 }
 
-void MbD::DirectionCosineConstraintIJ::initializeGlobally()
+void DirectionCosineConstraintIJ::initializeGlobally()
 {
 	aAijIeJe->initializeGlobally();
 }
@@ -31,41 +31,41 @@ void DirectionCosineConstraintIJ::initaAijIeJe()
 	aAijIeJe = CREATE<DirectionCosineIecJec>::With(frmI, frmJ, axisI, axisJ);
 }
 
-void MbD::DirectionCosineConstraintIJ::postInput()
+void DirectionCosineConstraintIJ::postInput()
 {
 	aAijIeJe->postInput();
 	Constraint::postInput();
 }
 
-void MbD::DirectionCosineConstraintIJ::calcPostDynCorrectorIteration()
+void DirectionCosineConstraintIJ::calcPostDynCorrectorIteration()
 {
 	aG = aAijIeJe->aAijIeJe - aConstant;
 }
 
-void MbD::DirectionCosineConstraintIJ::prePosIC()
+void DirectionCosineConstraintIJ::prePosIC()
 {
 	aAijIeJe->prePosIC();
 	Constraint::prePosIC();
 }
 
-void MbD::DirectionCosineConstraintIJ::postPosICIteration()
+void DirectionCosineConstraintIJ::postPosICIteration()
 {
 	aAijIeJe->postPosICIteration();
 	Item::postPosICIteration();
 }
 
-MbD::ConstraintType MbD::DirectionCosineConstraintIJ::type()
+ConstraintType DirectionCosineConstraintIJ::type()
 {
-	return MbD::perpendicular;
+	return perpendicular;
 }
 
-void MbD::DirectionCosineConstraintIJ::preVelIC()
+void DirectionCosineConstraintIJ::preVelIC()
 {
 	aAijIeJe->preVelIC();
 	Item::preVelIC();
 }
 
-void MbD::DirectionCosineConstraintIJ::preAccIC()
+void DirectionCosineConstraintIJ::preAccIC()
 {
 	aAijIeJe->preAccIC();
 	Constraint::preAccIC();

MbDCode/DirectionCosineConstraintIJ.h

@@ -18,7 +18,7 @@ namespace MbD {
         void calcPostDynCorrectorIteration() override;
         void prePosIC() override;
         void postPosICIteration() override;
-        MbD::ConstraintType type() override;
+        ConstraintType type() override;
         void preVelIC() override;
         void preAccIC() override;
 

MbDCode/DirectionCosineConstraintIqcJc.cpp

@@ -15,7 +15,7 @@ void DirectionCosineConstraintIqcJc::initaAijIeJe()
 	aAijIeJe = CREATE<DirectionCosineIeqcJec>::With(frmI, frmJ, axisI, axisJ);
 }
 
-void MbD::DirectionCosineConstraintIqcJc::calcPostDynCorrectorIteration()
+void DirectionCosineConstraintIqcJc::calcPostDynCorrectorIteration()
 {
 	DirectionCosineConstraintIJ::calcPostDynCorrectorIteration();
 	auto aAijIeqJe = std::static_pointer_cast<DirectionCosineIeqcJec>(aAijIeJe);
@@ -23,36 +23,36 @@ void MbD::DirectionCosineConstraintIqcJc::calcPostDynCorrectorIteration()
 	ppGpEIpEI = aAijIeqJe->ppAijIeJepEIpEI;
 }
 
-void MbD::DirectionCosineConstraintIqcJc::useEquationNumbers()
+void DirectionCosineConstraintIqcJc::useEquationNumbers()
 {
 	iqEI = std::static_pointer_cast<EndFrameqc>(frmI)->iqE();
 }
 
-void MbD::DirectionCosineConstraintIqcJc::fillPosICError(FColDsptr col)
+void DirectionCosineConstraintIqcJc::fillPosICError(FColDsptr col)
 {
 	Constraint::fillPosICError(col);
 	col->atiplusFullVectortimes(iqEI, pGpEI, lam);
 }
 
-void MbD::DirectionCosineConstraintIqcJc::fillPosICJacob(SpMatDsptr mat)
+void DirectionCosineConstraintIqcJc::fillPosICJacob(SpMatDsptr mat)
 {
 	mat->atijplusFullRow(iG, iqEI, pGpEI);
 	mat->atijplusFullColumn(iqEI, iG, pGpEI->transpose());
 	mat->atijplusFullMatrixtimes(iqEI, iqEI, ppGpEIpEI, lam);
 }
 
-void MbD::DirectionCosineConstraintIqcJc::fillPosKineJacob(SpMatDsptr mat)
+void DirectionCosineConstraintIqcJc::fillPosKineJacob(SpMatDsptr mat)
 {
 	mat->atijplusFullRow(iG, iqEI, pGpEI);
 }
 
-void MbD::DirectionCosineConstraintIqcJc::fillVelICJacob(SpMatDsptr mat)
+void DirectionCosineConstraintIqcJc::fillVelICJacob(SpMatDsptr mat)
 {
 	mat->atijplusFullRow(iG, iqEI, pGpEI);
 	mat->atijplusFullColumn(iqEI, iG, pGpEI->transpose());
 }
 
-void MbD::DirectionCosineConstraintIqcJc::fillAccICIterError(FColDsptr col)
+void DirectionCosineConstraintIqcJc::fillAccICIterError(FColDsptr col)
 {
 	col->atiplusFullVector(iqEI, pGpEI->times(lam));
 	auto efrmIqc = std::static_pointer_cast<EndFrameqc>(frmI);
@@ -61,3 +61,15 @@ void MbD::DirectionCosineConstraintIqcJc::fillAccICIterError(FColDsptr col)
 	sum += qEdotI->transposeTimesFullColumn(ppGpEIpEI->timesFullColumn(qEdotI));
 	col->atiplusNumber(iG, sum);
 }
+
+void DirectionCosineConstraintIqcJc::addToJointForceI(FColDsptr col)
+{
+}
+
+void DirectionCosineConstraintIqcJc::addToJointTorqueI(FColDsptr jointTorque)
+{
+	auto aBOIp = frmI->aBOp();
+	auto lampGpE = pGpEI->transpose()->times(lam);
+	auto c2Torque = aBOIp->timesFullColumn(lampGpE);
+	jointTorque->equalSelfPlusFullColumntimes(c2Torque, 0.5);
+}

MbDCode/DirectionCosineConstraintIqcJc.h

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

MbDCode/DirectionCosineConstraintIqcJqc.cpp

@@ -15,7 +15,7 @@ void DirectionCosineConstraintIqcJqc::initaAijIeJe()
 	aAijIeJe = CREATE<DirectionCosineIeqcJeqc>::With(frmI, frmJ, axisI, axisJ);
 }
 
-void MbD::DirectionCosineConstraintIqcJqc::calcPostDynCorrectorIteration()
+void DirectionCosineConstraintIqcJqc::calcPostDynCorrectorIteration()
 {
 	DirectionCosineConstraintIqcJc::calcPostDynCorrectorIteration();
 	auto aAijIeqJqe = std::static_pointer_cast<DirectionCosineIeqcJeqc>(aAijIeJe);
@@ -24,19 +24,19 @@ void MbD::DirectionCosineConstraintIqcJqc::calcPostDynCorrectorIteration()
 	ppGpEJpEJ = aAijIeqJqe->ppAijIeJepEJpEJ;
 }
 
-void MbD::DirectionCosineConstraintIqcJqc::useEquationNumbers()
+void DirectionCosineConstraintIqcJqc::useEquationNumbers()
 {
 	DirectionCosineConstraintIqcJc::useEquationNumbers();
 	iqEJ = std::static_pointer_cast<EndFrameqc>(frmJ)->iqE();
 }
 
-void MbD::DirectionCosineConstraintIqcJqc::fillPosICError(FColDsptr col)
+void DirectionCosineConstraintIqcJqc::fillPosICError(FColDsptr col)
 {
 	DirectionCosineConstraintIqcJc::fillPosICError(col);
 	col->atiplusFullVectortimes(iqEJ, pGpEJ, lam);
 }
 
-void MbD::DirectionCosineConstraintIqcJqc::fillPosICJacob(SpMatDsptr mat)
+void DirectionCosineConstraintIqcJqc::fillPosICJacob(SpMatDsptr mat)
 {
 	DirectionCosineConstraintIqcJc::fillPosICJacob(mat);
 	mat->atijplusFullRow(iG, iqEJ, pGpEJ);
@@ -47,20 +47,20 @@ void MbD::DirectionCosineConstraintIqcJqc::fillPosICJacob(SpMatDsptr mat)
 	mat->atijplusFullMatrixtimes(iqEJ, iqEJ, ppGpEJpEJ, lam);
 }
 
-void MbD::DirectionCosineConstraintIqcJqc::fillPosKineJacob(SpMatDsptr mat)
+void DirectionCosineConstraintIqcJqc::fillPosKineJacob(SpMatDsptr mat)
 {
 	DirectionCosineConstraintIqcJc::fillPosKineJacob(mat);
 	mat->atijplusFullRow(iG, iqEJ, pGpEJ);
 }
 
-void MbD::DirectionCosineConstraintIqcJqc::fillVelICJacob(SpMatDsptr mat)
+void DirectionCosineConstraintIqcJqc::fillVelICJacob(SpMatDsptr mat)
 {
 	DirectionCosineConstraintIqcJc::fillVelICJacob(mat);
 	mat->atijplusFullRow(iG, iqEJ, pGpEJ);
 	mat->atijplusFullColumn(iqEJ, iG, pGpEJ->transpose());
 }
 
-void MbD::DirectionCosineConstraintIqcJqc::fillAccICIterError(FColDsptr col)
+void DirectionCosineConstraintIqcJqc::fillAccICIterError(FColDsptr col)
 {
 	DirectionCosineConstraintIqcJc::fillAccICIterError(col);
 	col->atiplusFullVectortimes(iqEJ, pGpEJ, lam);

MbDCode/DirectionCosineConstraintIqctJqc.cpp

@@ -14,23 +14,23 @@ void DirectionCosineConstraintIqctJqc::initaAijIeJe()
 	aAijIeJe = CREATE<DirectionCosineIeqctJeqc>::With(frmI, frmJ, axisI, axisJ);
 }
 
-MbD::ConstraintType MbD::DirectionCosineConstraintIqctJqc::type()
+ConstraintType DirectionCosineConstraintIqctJqc::type()
 {
-	return MbD::essential;
+	return essential;
 }
 
-void MbD::DirectionCosineConstraintIqctJqc::preVelIC()
+void DirectionCosineConstraintIqctJqc::preVelIC()
 {
 	DirectionCosineConstraintIJ::preVelIC();
 	pGpt = std::static_pointer_cast<DirectionCosineIeqctJeqc>(aAijIeJe)->pAijIeJept;
 }
 
-void MbD::DirectionCosineConstraintIqctJqc::fillVelICError(FColDsptr col)
+void DirectionCosineConstraintIqctJqc::fillVelICError(FColDsptr col)
 {
 	col->atiminusNumber(iG, pGpt);
 }
 
-void MbD::DirectionCosineConstraintIqctJqc::preAccIC()
+void DirectionCosineConstraintIqctJqc::preAccIC()
 {
 	DirectionCosineConstraintIJ::preAccIC();
 	ppGpEIpt = std::static_pointer_cast<DirectionCosineIeqctJeqc>(aAijIeJe)->ppAijIeJepEIpt;
@@ -38,7 +38,7 @@ void MbD::DirectionCosineConstraintIqctJqc::preAccIC()
 	ppGptpt = std::static_pointer_cast<DirectionCosineIeqctJeqc>(aAijIeJe)->ppAijIeJeptpt;
 }
 
-void MbD::DirectionCosineConstraintIqctJqc::fillAccICIterError(FColDsptr col)
+void DirectionCosineConstraintIqctJqc::fillAccICIterError(FColDsptr col)
 {
 	DirectionCosineConstraintIqcJqc::fillAccICIterError(col);
 	auto efrmIqc = std::static_pointer_cast<EndFrameqc>(frmI);

MbDCode/DirectionCosineConstraintIqctJqc.h

@@ -9,7 +9,7 @@ namespace MbD {
 	public:
 		DirectionCosineConstraintIqctJqc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi, int axisj);
 		void initaAijIeJe() override;
-		MbD::ConstraintType type() override;
+		ConstraintType type() override;
 		void preVelIC() override;
 		void fillVelICError(FColDsptr col) override;
 		void preAccIC() override;

MbDCode/DirectionCosineIecJec.cpp

@@ -15,38 +15,38 @@ DirectionCosineIecJec::DirectionCosineIecJec(EndFrmcptr frmi, EndFrmcptr frmj, i
 
 }
 
-void MbD::DirectionCosineIecJec::calcPostDynCorrectorIteration()
+void DirectionCosineIecJec::calcPostDynCorrectorIteration()
 {
 	aAjOIe = frmI->aAjOe(axisI);
 	aAjOJe = frmJ->aAjOe(axisJ);
 	aAijIeJe = aAjOIe->dot(aAjOJe);
 }
 
-FRowDsptr MbD::DirectionCosineIecJec::pvaluepXJ()
+FRowDsptr DirectionCosineIecJec::pvaluepXJ()
 {
 	assert(false);
 	return FRowDsptr();
 }
 
-FRowDsptr MbD::DirectionCosineIecJec::pvaluepEJ()
+FRowDsptr DirectionCosineIecJec::pvaluepEJ()
 {
 	assert(false);
 	return FRowDsptr();
 }
 
-FMatDsptr MbD::DirectionCosineIecJec::ppvaluepXJpEK()
+FMatDsptr DirectionCosineIecJec::ppvaluepXJpEK()
 {
 	assert(false);
 	return FMatDsptr();
 }
 
-FMatDsptr MbD::DirectionCosineIecJec::ppvaluepEJpEK()
+FMatDsptr DirectionCosineIecJec::ppvaluepEJpEK()
 {
 	assert(false);
 	return FMatDsptr();
 }
 
-FMatDsptr MbD::DirectionCosineIecJec::ppvaluepEJpEJ()
+FMatDsptr DirectionCosineIecJec::ppvaluepEJpEJ()
 {
 	assert(false);
 	return FMatDsptr();

MbDCode/DirectionCosineIeqcJec.cpp

@@ -19,12 +19,12 @@ void DirectionCosineIeqcJec::initialize()
 	ppAijIeJepEIpEI = std::make_shared<FullMatrix<double>>(4, 4);
 }
 
-void MbD::DirectionCosineIeqcJec::initializeGlobally()
+void DirectionCosineIeqcJec::initializeGlobally()
 {
 	ppAjOIepEIpEI = std::static_pointer_cast<EndFrameqc>(frmI)->ppAjOepEpE(axisI);
 }
 
-void MbD::DirectionCosineIeqcJec::calcPostDynCorrectorIteration()
+void DirectionCosineIeqcJec::calcPostDynCorrectorIteration()
 {
 	DirectionCosineIecJec::calcPostDynCorrectorIteration();
 	pAjOIepEIT = std::static_pointer_cast<EndFrameqc>(frmI)->pAjOepET(axisI);

MbDCode/DirectionCosineIeqcJeqc.cpp

@@ -20,13 +20,13 @@ void DirectionCosineIeqcJeqc::initialize()
 	ppAijIeJepEJpEJ = std::make_shared<FullMatrix<double>>(4, 4);
 }
 
-void MbD::DirectionCosineIeqcJeqc::initializeGlobally()
+void DirectionCosineIeqcJeqc::initializeGlobally()
 {
 	DirectionCosineIeqcJec::initializeGlobally();
 	ppAjOJepEJpEJ = std::static_pointer_cast<EndFrameqc>(frmJ)->ppAjOepEpE(axisJ);
 }
 
-void MbD::DirectionCosineIeqcJeqc::calcPostDynCorrectorIteration()
+void DirectionCosineIeqcJeqc::calcPostDynCorrectorIteration()
 {
 	DirectionCosineIeqcJec::calcPostDynCorrectorIteration();
 	pAjOJepEJT = std::static_pointer_cast<EndFrameqc>(frmJ)->pAjOepET(axisJ);
@@ -54,12 +54,12 @@ void MbD::DirectionCosineIeqcJeqc::calcPostDynCorrectorIteration()
 	ppAijIeJepEJpEJ->symLowerWithUpper();
 }
 
-FRowDsptr MbD::DirectionCosineIeqcJeqc::pvaluepEJ()
+FRowDsptr DirectionCosineIeqcJeqc::pvaluepEJ()
 {
 	return pAijIeJepEJ;
 }
 
-FMatDsptr MbD::DirectionCosineIeqcJeqc::ppvaluepEJpEJ()
+FMatDsptr DirectionCosineIeqcJeqc::ppvaluepEJpEJ()
 {
 	return ppAijIeJepEJpEJ;
 }

MbDCode/DirectionCosineIeqctJeqc.cpp

@@ -20,12 +20,12 @@ void DirectionCosineIeqctJeqc::initialize()
 	ppAijIeJepEJpt = std::make_shared<FullRow<double>>(4);
 }
 
-void MbD::DirectionCosineIeqctJeqc::initializeGlobally()
+void DirectionCosineIeqctJeqc::initializeGlobally()
 {
 	ppAjOJepEJpEJ = std::static_pointer_cast<EndFrameqc>(frmJ)->ppAjOepEpE(axisJ);
 }
 
-void MbD::DirectionCosineIeqctJeqc::calcPostDynCorrectorIteration()
+void DirectionCosineIeqctJeqc::calcPostDynCorrectorIteration()
 {
 	//"ppAjOIepEIpEI is not longer constant and must be set before any calculation."
 
@@ -33,19 +33,19 @@ void MbD::DirectionCosineIeqctJeqc::calcPostDynCorrectorIteration()
 	DirectionCosineIeqcJeqc::calcPostDynCorrectorIteration();
 }
 
-void MbD::DirectionCosineIeqctJeqc::preVelIC()
+void DirectionCosineIeqctJeqc::preVelIC()
 {
 	Item::preVelIC();
 	auto pAjOIept = std::static_pointer_cast<EndFrameqct>(frmI)->pAjOept(axisI);
 	pAijIeJept = pAjOIept->dot(aAjOJe);
 }
 
-double MbD::DirectionCosineIeqctJeqc::pvaluept()
+double DirectionCosineIeqctJeqc::pvaluept()
 {
 	return pAijIeJept;
 }
 
-void MbD::DirectionCosineIeqctJeqc::preAccIC()
+void DirectionCosineIeqctJeqc::preAccIC()
 {
 	//| ppAjOIepEITpt ppAjOIeptpt ppAjOIepEITpti pAjOIept |
 	Item::preAccIC();

MbDCode/DiscontinuityError.cpp

@@ -1,7 +1,3 @@
 #include "DiscontinuityError.h"
 
 using namespace MbD;
-
-MbD::DiscontinuityError::DiscontinuityError(const std::string& msg) : std::runtime_error(msg)
-{
-}

MbDCode/DiscontinuityError.h

@@ -2,14 +2,25 @@
 #include <stdexcept>
 #include <memory>
 #include <vector>
+#include "enum.h"
 
 namespace MbD {
 	class DiscontinuityError : virtual public std::runtime_error
 	{
+	protected:
+		std::shared_ptr<std::vector<DiscontinuityType>> discontinuityTypes;
 
 	public:
 		//DiscontinuityError();
-		explicit DiscontinuityError(const std::string& msg);
+		explicit
+			DiscontinuityError(const std::string& msg, std::shared_ptr<std::vector<DiscontinuityType>> disconTypes) :
+			std::runtime_error(msg), discontinuityTypes(disconTypes)
+		{
+		}
+		explicit DiscontinuityError(const std::string& msg) : std::runtime_error(msg)
+		{
+		}
+
 		virtual ~DiscontinuityError() noexcept {}
 	};
 }

MbDCode/DispCompIecJecKec.cpp

@@ -2,45 +2,45 @@
 
 using namespace MbD;
 
-MbD::DispCompIecJecKec::DispCompIecJecKec()
+DispCompIecJecKec::DispCompIecJecKec()
 {
 }
 
-MbD::DispCompIecJecKec::DispCompIecJecKec(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk): KinematicIeJe(frmi, frmj), efrmK(frmk), axisK(axisk)
+DispCompIecJecKec::DispCompIecJecKec(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk): KinematicIeJe(frmi, frmj), efrmK(frmk), axisK(axisk)
 {
 }
 
-FRowDsptr MbD::DispCompIecJecKec::pvaluepXJ()
+FRowDsptr DispCompIecJecKec::pvaluepXJ()
 {
     assert(false);
     return FRowDsptr();
 }
 
-FRowDsptr MbD::DispCompIecJecKec::pvaluepEJ()
+FRowDsptr DispCompIecJecKec::pvaluepEJ()
 {
     assert(false);
     return FRowDsptr();
 }
 
-FMatDsptr MbD::DispCompIecJecKec::ppvaluepXJpEK()
+FMatDsptr DispCompIecJecKec::ppvaluepXJpEK()
 {
     assert(false);
     return FMatDsptr();
 }
 
-FMatDsptr MbD::DispCompIecJecKec::ppvaluepEJpEK()
+FMatDsptr DispCompIecJecKec::ppvaluepEJpEK()
 {
     assert(false);
     return FMatDsptr();
 }
 
-FMatDsptr MbD::DispCompIecJecKec::ppvaluepEJpEJ()
+FMatDsptr DispCompIecJecKec::ppvaluepEJpEJ()
 {
     assert(false);
     return FMatDsptr();
 }
 
-double MbD::DispCompIecJecKec::value()
+double DispCompIecJecKec::value()
 {
     return riIeJeKe;
 }

MbDCode/DispCompIecJecKeqc.cpp

@@ -3,26 +3,26 @@
 
 using namespace MbD;
 
-MbD::DispCompIecJecKeqc::DispCompIecJecKeqc()
+DispCompIecJecKeqc::DispCompIecJecKeqc()
 {
 }
 
-MbD::DispCompIecJecKeqc::DispCompIecJecKeqc(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk) : DispCompIecJecKec(frmi, frmj, frmk, axisk)
+DispCompIecJecKeqc::DispCompIecJecKeqc(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk) : DispCompIecJecKec(frmi, frmj, frmk, axisk)
 {
 }
 
-void MbD::DispCompIecJecKeqc::initialize()
+void DispCompIecJecKeqc::initialize()
 {
     priIeJeKepEK = std::make_shared<FullRow<double>>(4);
     ppriIeJeKepEKpEK = std::make_shared<FullMatrix<double>>(4, 4);
 }
 
-void MbD::DispCompIecJecKeqc::initializeGlobally()
+void DispCompIecJecKeqc::initializeGlobally()
 {
     ppAjOKepEKpEK = std::static_pointer_cast<EndFrameqc>(efrmK)->ppAjOepEpE(axisK);
 }
 
-void MbD::DispCompIecJecKeqc::calcPostDynCorrectorIteration()
+void DispCompIecJecKeqc::calcPostDynCorrectorIteration()
 {
 	auto frmIqc = std::static_pointer_cast<EndFrameqc>(frmI);
 	auto frmJqc = std::static_pointer_cast<EndFrameqc>(frmJ);
@@ -47,7 +47,7 @@ void MbD::DispCompIecJecKeqc::calcPostDynCorrectorIteration()
 	}
 }
 
-FMatDsptr MbD::DispCompIecJecKeqc::ppvaluepEKpEK()
+FMatDsptr DispCompIecJecKeqc::ppvaluepEKpEK()
 {
     return ppriIeJeKepEKpEK;
 }

MbDCode/DispCompIecJecO.cpp

@@ -2,44 +2,44 @@
 
 using namespace MbD;
 
-MbD::DispCompIecJecO::DispCompIecJecO()
+DispCompIecJecO::DispCompIecJecO()
 {
 }
 
-MbD::DispCompIecJecO::DispCompIecJecO(EndFrmcptr frmi, EndFrmcptr frmj, int axis) : KinematicIeJe(frmi, frmj), axis(axis)
+DispCompIecJecO::DispCompIecJecO(EndFrmcptr frmi, EndFrmcptr frmj, int axis) : KinematicIeJe(frmi, frmj), axis(axis)
 {
 }
 
-void MbD::DispCompIecJecO::calcPostDynCorrectorIteration()
+void DispCompIecJecO::calcPostDynCorrectorIteration()
 {
 	riIeJeO = frmJ->riOeO(axis) - frmI->riOeO(axis);
 }
 
-FRowDsptr MbD::DispCompIecJecO::pvaluepXJ()
+FRowDsptr DispCompIecJecO::pvaluepXJ()
 {
 	assert(false);
 	return FRowDsptr();
 }
 
-FRowDsptr MbD::DispCompIecJecO::pvaluepEJ()
+FRowDsptr DispCompIecJecO::pvaluepEJ()
 {
 	assert(false);
 	return FRowDsptr();
 }
 
-FMatDsptr MbD::DispCompIecJecO::ppvaluepXJpEK()
+FMatDsptr DispCompIecJecO::ppvaluepXJpEK()
 {
 	assert(false);
 	return FMatDsptr();
 }
 
-FMatDsptr MbD::DispCompIecJecO::ppvaluepEJpEK()
+FMatDsptr DispCompIecJecO::ppvaluepEJpEK()
 {
 	assert(false);
 	return FMatDsptr();
 }
 
-FMatDsptr MbD::DispCompIecJecO::ppvaluepEJpEJ()
+FMatDsptr DispCompIecJecO::ppvaluepEJpEJ()
 {
 	assert(false);
 	return FMatDsptr();

MbDCode/DispCompIeqcJecKeqc.cpp

@@ -3,15 +3,15 @@
 
 using namespace MbD;
 
-MbD::DispCompIeqcJecKeqc::DispCompIeqcJecKeqc()
+DispCompIeqcJecKeqc::DispCompIeqcJecKeqc()
 {
 }
 
-MbD::DispCompIeqcJecKeqc::DispCompIeqcJecKeqc(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk) : DispCompIecJecKeqc(frmi, frmj, frmk, axisk)
+DispCompIeqcJecKeqc::DispCompIeqcJecKeqc(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk) : DispCompIecJecKeqc(frmi, frmj, frmk, axisk)
 {
 }
 
-void MbD::DispCompIeqcJecKeqc::initialize()
+void DispCompIeqcJecKeqc::initialize()
 {
 	DispCompIecJecKeqc::initialize();
 	priIeJeKepXI = std::make_shared<FullRow<double>>(3);
@@ -21,7 +21,7 @@ void MbD::DispCompIeqcJecKeqc::initialize()
 	ppriIeJeKepEIpEK = std::make_shared<FullMatrix<double>>(4, 4);
 }
 
-void MbD::DispCompIeqcJecKeqc::calcPostDynCorrectorIteration()
+void DispCompIeqcJecKeqc::calcPostDynCorrectorIteration()
 {
 	DispCompIecJecKeqc::calcPostDynCorrectorIteration();
 	auto frmIqc = std::static_pointer_cast<EndFrameqc>(frmI);
@@ -66,32 +66,32 @@ void MbD::DispCompIeqcJecKeqc::calcPostDynCorrectorIteration()
 	}
 }
 
-FRowDsptr MbD::DispCompIeqcJecKeqc::pvaluepXI()
+FRowDsptr DispCompIeqcJecKeqc::pvaluepXI()
 {
 	return priIeJeKepXI;
 }
 
-FRowDsptr MbD::DispCompIeqcJecKeqc::pvaluepEI()
+FRowDsptr DispCompIeqcJecKeqc::pvaluepEI()
 {
 	return priIeJeKepEI;
 }
 
-FRowDsptr MbD::DispCompIeqcJecKeqc::pvaluepEK()
+FRowDsptr DispCompIeqcJecKeqc::pvaluepEK()
 {
 	return priIeJeKepEK;
 }
 
-FMatDsptr MbD::DispCompIeqcJecKeqc::ppvaluepXIpEK()
+FMatDsptr DispCompIeqcJecKeqc::ppvaluepXIpEK()
 {
 	return ppriIeJeKepXIpEK;
 }
 
-FMatDsptr MbD::DispCompIeqcJecKeqc::ppvaluepEIpEK()
+FMatDsptr DispCompIeqcJecKeqc::ppvaluepEIpEK()
 {
 	return ppriIeJeKepEIpEK;
 }
 
-FMatDsptr MbD::DispCompIeqcJecKeqc::ppvaluepEIpEI()
+FMatDsptr DispCompIeqcJecKeqc::ppvaluepEIpEI()
 {
 	return ppriIeJeKepEIpEI;
 }

MbDCode/DispCompIeqcJecO.cpp

@@ -3,22 +3,22 @@
 
 using namespace MbD;
 
-MbD::DispCompIeqcJecO::DispCompIeqcJecO()
+DispCompIeqcJecO::DispCompIeqcJecO()
 {
 }
 
-MbD::DispCompIeqcJecO::DispCompIeqcJecO(EndFrmcptr frmi, EndFrmcptr frmj, int axis) : DispCompIecJecO(frmi, frmj, axis)
+DispCompIeqcJecO::DispCompIeqcJecO(EndFrmcptr frmi, EndFrmcptr frmj, int axis) : DispCompIecJecO(frmi, frmj, axis)
 {
 }
 
-void MbD::DispCompIeqcJecO::initializeGlobally()
+void DispCompIeqcJecO::initializeGlobally()
 {
 	priIeJeOpXI = std::make_shared<FullRow<double>>(3, 0.0);
 	priIeJeOpXI->at(axis) = -1.0;
 	ppriIeJeOpEIpEI = std::static_pointer_cast<EndFrameqc>(frmI)->ppriOeOpEpE(axis)->negated();
 }
 
-void MbD::DispCompIeqcJecO::calcPostDynCorrectorIteration()
+void DispCompIeqcJecO::calcPostDynCorrectorIteration()
 {
 	DispCompIecJecO::calcPostDynCorrectorIteration();
 	priIeJeOpEI = std::static_pointer_cast<EndFrameqc>(frmI)->priOeOpE(axis)->negated();

MbDCode/DispCompIeqcJeqcKeqc.cpp

@@ -3,15 +3,15 @@
 
 using namespace MbD;
 
-MbD::DispCompIeqcJeqcKeqc::DispCompIeqcJeqcKeqc()
+DispCompIeqcJeqcKeqc::DispCompIeqcJeqcKeqc()
 {
 }
 
-MbD::DispCompIeqcJeqcKeqc::DispCompIeqcJeqcKeqc(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk) : DispCompIeqcJecKeqc(frmi, frmj, frmk, axisk)
+DispCompIeqcJeqcKeqc::DispCompIeqcJeqcKeqc(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk) : DispCompIeqcJecKeqc(frmi, frmj, frmk, axisk)
 {
 }
 
-void MbD::DispCompIeqcJeqcKeqc::initialize()
+void DispCompIeqcJeqcKeqc::initialize()
 {
 	DispCompIeqcJecKeqc::initialize();
 	priIeJeKepXJ = std::make_shared<FullRow<double>>(3);
@@ -21,7 +21,7 @@ void MbD::DispCompIeqcJeqcKeqc::initialize()
 	ppriIeJeKepEJpEK = std::make_shared<FullMatrix<double>>(4, 4);
 }
 
-void MbD::DispCompIeqcJeqcKeqc::calcPostDynCorrectorIteration()
+void DispCompIeqcJeqcKeqc::calcPostDynCorrectorIteration()
 {
 	DispCompIeqcJecKeqc::calcPostDynCorrectorIteration();
 	auto frmJqc = std::static_pointer_cast<EndFrameqc>(frmJ);
@@ -66,27 +66,27 @@ void MbD::DispCompIeqcJeqcKeqc::calcPostDynCorrectorIteration()
 	}
 }
 
-FRowDsptr MbD::DispCompIeqcJeqcKeqc::pvaluepXJ()
+FRowDsptr DispCompIeqcJeqcKeqc::pvaluepXJ()
 {
 	return priIeJeKepXJ;
 }
 
-FRowDsptr MbD::DispCompIeqcJeqcKeqc::pvaluepEJ()
+FRowDsptr DispCompIeqcJeqcKeqc::pvaluepEJ()
 {
 	return priIeJeKepEJ;
 }
 
-FMatDsptr MbD::DispCompIeqcJeqcKeqc::ppvaluepXJpEK()
+FMatDsptr DispCompIeqcJeqcKeqc::ppvaluepXJpEK()
 {
 	return ppriIeJeKepXJpEK;
 }
 
-FMatDsptr MbD::DispCompIeqcJeqcKeqc::ppvaluepEJpEK()
+FMatDsptr DispCompIeqcJeqcKeqc::ppvaluepEJpEK()
 {
 	return ppriIeJeKepEJpEK;
 }
 
-FMatDsptr MbD::DispCompIeqcJeqcKeqc::ppvaluepEJpEJ()
+FMatDsptr DispCompIeqcJeqcKeqc::ppvaluepEJpEJ()
 {
 	return ppriIeJeKepEJpEJ;
 }

MbDCode/DispCompIeqcJeqcKeqct.cpp

@@ -4,15 +4,15 @@
 
 using namespace MbD;
 
-MbD::DispCompIeqcJeqcKeqct::DispCompIeqcJeqcKeqct()
+DispCompIeqcJeqcKeqct::DispCompIeqcJeqcKeqct()
 {
 }
 
-MbD::DispCompIeqcJeqcKeqct::DispCompIeqcJeqcKeqct(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk) : DispCompIeqcJeqcKeqc(frmi, frmj, frmk, axisk)
+DispCompIeqcJeqcKeqct::DispCompIeqcJeqcKeqct(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk) : DispCompIeqcJeqcKeqc(frmi, frmj, frmk, axisk)
 {
 }
 
-void MbD::DispCompIeqcJeqcKeqct::initialize()
+void DispCompIeqcJeqcKeqct::initialize()
 {
 	DispCompIeqcJeqcKeqc::initialize();
 	ppriIeJeKepXIpt = std::make_shared<FullRow<double>>(3);
@@ -22,7 +22,7 @@ void MbD::DispCompIeqcJeqcKeqct::initialize()
 	ppriIeJeKepEKpt = std::make_shared<FullRow<double>>(4);
 }
 
-void MbD::DispCompIeqcJeqcKeqct::calcPostDynCorrectorIteration()
+void DispCompIeqcJeqcKeqct::calcPostDynCorrectorIteration()
 {
 	//"ppAjOIepEKpEK is not longer constant and must be set before any calculation."
 	auto efrmKqc = std::static_pointer_cast<EndFrameqc>(efrmK);
@@ -30,49 +30,49 @@ void MbD::DispCompIeqcJeqcKeqct::calcPostDynCorrectorIteration()
 	DispCompIeqcJeqcKeqc::calcPostDynCorrectorIteration();
 }
 
-void MbD::DispCompIeqcJeqcKeqct::preVelIC()
+void DispCompIeqcJeqcKeqct::preVelIC()
 {
 	Item::preVelIC();
 	auto pAjOKept = std::static_pointer_cast<EndFrameqct>(efrmK)->pAjOept(axisK);
 	priIeJeKept = pAjOKept->dot(rIeJeO);
 }
 
-double MbD::DispCompIeqcJeqcKeqct::pvaluept()
+double DispCompIeqcJeqcKeqct::pvaluept()
 {
 	return priIeJeKept;
 }
 
-FRowDsptr MbD::DispCompIeqcJeqcKeqct::ppvaluepXIpt()
+FRowDsptr DispCompIeqcJeqcKeqct::ppvaluepXIpt()
 {
 	return ppriIeJeKepXIpt;
 }
 
-FRowDsptr MbD::DispCompIeqcJeqcKeqct::ppvaluepEIpt()
+FRowDsptr DispCompIeqcJeqcKeqct::ppvaluepEIpt()
 {
 	return ppriIeJeKepEIpt;
 }
 
-FRowDsptr MbD::DispCompIeqcJeqcKeqct::ppvaluepEKpt()
+FRowDsptr DispCompIeqcJeqcKeqct::ppvaluepEKpt()
 {
 	return ppriIeJeKepEKpt;
 }
 
-FRowDsptr MbD::DispCompIeqcJeqcKeqct::ppvaluepXJpt()
+FRowDsptr DispCompIeqcJeqcKeqct::ppvaluepXJpt()
 {
 	return ppriIeJeKepXJpt;
 }
 
-FRowDsptr MbD::DispCompIeqcJeqcKeqct::ppvaluepEJpt()
+FRowDsptr DispCompIeqcJeqcKeqct::ppvaluepEJpt()
 {
 	return ppriIeJeKepEJpt;
 }
 
-double MbD::DispCompIeqcJeqcKeqct::ppvalueptpt()
+double DispCompIeqcJeqcKeqct::ppvalueptpt()
 {
 	return ppriIeJeKeptpt;
 }
 
-void MbD::DispCompIeqcJeqcKeqct::preAccIC()
+void DispCompIeqcJeqcKeqct::preAccIC()
 {
 	Item::preAccIC();
 	auto pAjOKept = std::static_pointer_cast<EndFrameqct>(efrmK)->pAjOept(axisK);

MbDCode/DispCompIeqcJeqcO.cpp

@@ -3,15 +3,15 @@
 
 using namespace MbD;
 
-MbD::DispCompIeqcJeqcO::DispCompIeqcJeqcO()
+DispCompIeqcJeqcO::DispCompIeqcJeqcO()
 {
 }
 
-MbD::DispCompIeqcJeqcO::DispCompIeqcJeqcO(EndFrmcptr frmi, EndFrmcptr frmj, int axis) : DispCompIeqcJecO(frmi, frmj, axis)
+DispCompIeqcJeqcO::DispCompIeqcJeqcO(EndFrmcptr frmi, EndFrmcptr frmj, int axis) : DispCompIeqcJecO(frmi, frmj, axis)
 {
 }
 
-void MbD::DispCompIeqcJeqcO::initializeGlobally()
+void DispCompIeqcJeqcO::initializeGlobally()
 {
 	DispCompIeqcJecO::initializeGlobally();
 	priIeJeOpXJ = std::make_shared<FullRow<double>>(3, 0.0);
@@ -19,23 +19,23 @@ void MbD::DispCompIeqcJeqcO::initializeGlobally()
 	ppriIeJeOpEJpEJ = std::static_pointer_cast<EndFrameqc>(frmJ)->ppriOeOpEpE(axis);
 }
 
-void MbD::DispCompIeqcJeqcO::calcPostDynCorrectorIteration()
+void DispCompIeqcJeqcO::calcPostDynCorrectorIteration()
 {
 	DispCompIeqcJecO::calcPostDynCorrectorIteration();
 	priIeJeOpEJ = std::static_pointer_cast<EndFrameqc>(frmJ)->priOeOpE(axis);
 }
 
-FRowDsptr MbD::DispCompIeqcJeqcO::pvaluepXJ()
+FRowDsptr DispCompIeqcJeqcO::pvaluepXJ()
 {
 	return priIeJeOpXJ;
 }
 
-FRowDsptr MbD::DispCompIeqcJeqcO::pvaluepEJ()
+FRowDsptr DispCompIeqcJeqcO::pvaluepEJ()
 {
 	return priIeJeOpEJ;
 }
 
-FMatDsptr MbD::DispCompIeqcJeqcO::ppvaluepEJpEJ()
+FMatDsptr DispCompIeqcJeqcO::ppvaluepEJpEJ()
 {
 	return ppriIeJeOpEJpEJ;
 }

MbDCode/DispCompIeqctJeqcKeqct.cpp

@@ -3,22 +3,22 @@
 
 using namespace MbD;
 
-MbD::DispCompIeqctJeqcKeqct::DispCompIeqctJeqcKeqct()
+DispCompIeqctJeqcKeqct::DispCompIeqctJeqcKeqct()
 {
 }
 
-MbD::DispCompIeqctJeqcKeqct::DispCompIeqctJeqcKeqct(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk) : DispCompIeqcJeqcKeqct(frmi, frmj, frmk, axisk)
+DispCompIeqctJeqcKeqct::DispCompIeqctJeqcKeqct(EndFrmcptr frmi, EndFrmcptr frmj, EndFrmcptr frmk, int axisk) : DispCompIeqcJeqcKeqct(frmi, frmj, frmk, axisk)
 {
 }
 
-void MbD::DispCompIeqctJeqcKeqct::preVelIC()
+void DispCompIeqctJeqcKeqct::preVelIC()
 {
 	DispCompIeqcJeqcKeqct::preVelIC();
 	auto& mprIeJeOpt = std::static_pointer_cast<EndFrameqct>(frmI)->prOeOpt;
 	priIeJeKept -= aAjOKe->dot(mprIeJeOpt);
 }
 
-void MbD::DispCompIeqctJeqcKeqct::preAccIC()
+void DispCompIeqctJeqcKeqct::preAccIC()
 {
 	DispCompIeqcJeqcKeqct::preAccIC();
 	auto pAjOKept = std::static_pointer_cast<EndFrameqct>(efrmK)->pAjOept(axisK);

MbDCode/DispCompIeqctJeqcO.cpp

@@ -3,39 +3,39 @@
 
 using namespace MbD;
 
-MbD::DispCompIeqctJeqcO::DispCompIeqctJeqcO()
+DispCompIeqctJeqcO::DispCompIeqctJeqcO()
 {
 }
 
-MbD::DispCompIeqctJeqcO::DispCompIeqctJeqcO(EndFrmcptr frmi, EndFrmcptr frmj, int axis) : DispCompIeqcJeqcO(frmi, frmj, axis)
+DispCompIeqctJeqcO::DispCompIeqctJeqcO(EndFrmcptr frmi, EndFrmcptr frmj, int axis) : DispCompIeqcJeqcO(frmi, frmj, axis)
 {
 }
 
-void MbD::DispCompIeqctJeqcO::initializeGlobally()
+void DispCompIeqctJeqcO::initializeGlobally()
 {
 	//ToDo: Check why not using super classes.
 	ppriIeJeOpEJpEJ = std::static_pointer_cast<EndFrameqct>(frmJ)->ppriOeOpEpE(axis);
 }
 
-void MbD::DispCompIeqctJeqcO::calcPostDynCorrectorIteration()
+void DispCompIeqctJeqcO::calcPostDynCorrectorIteration()
 {
 	//"ppriIeJeOpEIpEI is not a constant now."
 	DispCompIeqcJeqcO::calcPostDynCorrectorIteration();
 	ppriIeJeOpEIpEI = std::static_pointer_cast<EndFrameqct>(frmI)->ppriOeOpEpE(axis)->negated();
 }
 
-void MbD::DispCompIeqctJeqcO::preVelIC()
+void DispCompIeqctJeqcO::preVelIC()
 {
 	Item::preVelIC();
 	priIeJeOpt = -(std::static_pointer_cast<EndFrameqct>(frmI)->priOeOpt(axis));
 }
 
-double MbD::DispCompIeqctJeqcO::pvaluept()
+double DispCompIeqctJeqcO::pvaluept()
 {
 	return priIeJeOpt;
 }
 
-void MbD::DispCompIeqctJeqcO::preAccIC()
+void DispCompIeqctJeqcO::preAccIC()
 {
 	Item::preAccIC();
 	ppriIeJeOpEIpt = (std::static_pointer_cast<EndFrameqct>(frmI)->ppriOeOpEpt(axis))->negated();

MbDCode/EndFramec.cpp

@@ -12,6 +12,11 @@ EndFramec::EndFramec() {
 EndFramec::EndFramec(const char* str) : CartesianFrame(str) {
 }
 
+System* EndFramec::root()
+{
+	return markerFrame->root();
+}
+
 void EndFramec::initialize()
 {
 }
@@ -35,18 +40,38 @@ void EndFramec::initEndFrameqct()
 	assert(false);
 }
 
-void MbD::EndFramec::calcPostDynCorrectorIteration()
+void EndFramec::calcPostDynCorrectorIteration()
 {
 	rOeO = markerFrame->rOmO;
 	aAOe = markerFrame->aAOm;
 }
 
-FColDsptr MbD::EndFramec::aAjOe(int j)
+FColDsptr EndFramec::aAjOe(int j)
 {
 	return aAOe->column(j);
 }
 
-double MbD::EndFramec::riOeO(int i)
+double EndFramec::riOeO(int i)
 {
 	return rOeO->at(i);
 }
+
+FColDsptr EndFramec::rmeO()
+{
+	return rOeO->minusFullColumn(markerFrame->rOmO);
+}
+
+FColDsptr EndFramec::rpep()
+{
+	return FColDsptr();
+}
+
+FColFMatDsptr EndFramec::pAOppE()
+{
+	return FColFMatDsptr();
+}
+
+FMatDsptr EndFramec::aBOp()
+{
+	return FMatDsptr();
+}

MbDCode/EndFramec.h

@@ -19,6 +19,7 @@ namespace MbD {
     public:
         EndFramec();
         EndFramec(const char* str);
+        System* root() override;
         void initialize() override;
         void setMarkerFrame(MarkerFrame* markerFrm);
         MarkerFrame* getMarkerFrame();
@@ -27,6 +28,10 @@ namespace MbD {
         void calcPostDynCorrectorIteration() override;
         FColDsptr aAjOe(int j);
         double riOeO(int i);
+        virtual FColDsptr rmeO();
+        virtual FColDsptr rpep();
+        virtual FColFMatDsptr pAOppE();
+        virtual FMatDsptr aBOp();
 
         MarkerFrame* markerFrame; //Use raw pointer when pointing backwards.
         FColDsptr rOeO = std::make_shared<FullColumn<double>>(3);

MbDCode/EndFrameqc.cpp

@@ -38,7 +38,7 @@ void EndFrameqc::initEndFrameqct()
 	endFrameqct->setMarkerFrame(markerFrame);
 }
 
-FMatFColDsptr MbD::EndFrameqc::ppAjOepEpE(int jj)
+FMatFColDsptr EndFrameqc::ppAjOepEpE(int jj)
 {
 	auto answer = std::make_shared<FullMatrix<std::shared_ptr<FullColumn<double>>>>(4, 4);
 	for (int i = 0; i < 4; i++) {
@@ -52,14 +52,14 @@ FMatFColDsptr MbD::EndFrameqc::ppAjOepEpE(int jj)
 	return answer;
 }
 
-void MbD::EndFrameqc::calcPostDynCorrectorIteration()
+void EndFrameqc::calcPostDynCorrectorIteration()
 {
 	EndFramec::calcPostDynCorrectorIteration();
 	prOeOpE = markerFrame->prOmOpE;
 	pAOepE = markerFrame->pAOmpE;
 }
 
-FMatDsptr MbD::EndFrameqc::pAjOepET(int axis)
+FMatDsptr EndFrameqc::pAjOepET(int axis)
 {
 	auto answer = std::make_shared<FullMatrix<double>>(4, 3);
 	for (int i = 0; i < 4; i++) {
@@ -73,7 +73,7 @@ FMatDsptr MbD::EndFrameqc::pAjOepET(int axis)
 	return answer;
 }
 
-FMatDsptr MbD::EndFrameqc::ppriOeOpEpE(int ii)
+FMatDsptr EndFrameqc::ppriOeOpEpE(int ii)
 {
 	auto answer = std::make_shared<FullMatrix<double>>(4, 4);
 	for (int i = 0; i < 4; i++) {
@@ -87,37 +87,52 @@ FMatDsptr MbD::EndFrameqc::ppriOeOpEpE(int ii)
 	return answer;
 }
 
-int MbD::EndFrameqc::iqX()
+int EndFrameqc::iqX()
 {
 	return markerFrame->iqX();
 }
 
-int MbD::EndFrameqc::iqE()
+int EndFrameqc::iqE()
 {
 	return markerFrame->iqE();
 }
 
-FRowDsptr MbD::EndFrameqc::priOeOpE(int i)
+FRowDsptr EndFrameqc::priOeOpE(int i)
 {
 	return prOeOpE->at(i);
 }
 
-FColDsptr MbD::EndFrameqc::qXdot()
+FColDsptr EndFrameqc::qXdot()
 {
 	return markerFrame->qXdot();
 }
 
-std::shared_ptr<EulerParametersDot<double>> MbD::EndFrameqc::qEdot()
+std::shared_ptr<EulerParametersDot<double>> EndFrameqc::qEdot()
 {
 	return markerFrame->qEdot();
 }
 
-FColDsptr MbD::EndFrameqc::qXddot()
+FColDsptr EndFrameqc::qXddot()
 {
 	return markerFrame->qXddot();
 }
 
-FColDsptr MbD::EndFrameqc::qEddot()
+FColDsptr EndFrameqc::qEddot()
 {
 	return markerFrame->qEddot();
 }
+
+FColDsptr EndFrameqc::rpep()
+{
+	return markerFrame->rpmp;
+}
+
+FColFMatDsptr EndFrameqc::pAOppE()
+{
+	return markerFrame->pAOppE();
+}
+
+FMatDsptr EndFrameqc::aBOp()
+{
+	return markerFrame->aBOp();
+}

MbDCode/EndFrameqc.h

@@ -28,6 +28,9 @@ namespace MbD {
         std::shared_ptr<EulerParametersDot<double>> qEdot();
         FColDsptr qXddot();
         FColDsptr qEddot();
+        FColDsptr rpep() override;
+        FColFMatDsptr pAOppE() override;
+        FMatDsptr aBOp() override;
 
         FMatDsptr prOeOpE;
         std::shared_ptr<FullMatrix<std::shared_ptr<FullColumn<double>>>> pprOeOpEpE;

MbDCode/EndFrameqct.cpp

@@ -60,7 +60,7 @@ void EndFrameqct::initializeGlobally()
 
 void EndFrameqct::initprmemptBlks()
 {
-	auto& mbdTime = System::getInstance().time;
+	auto& mbdTime = this->root()->time;
 	prmemptBlks = std::make_shared< FullColumn<Symsptr>>(3);
 	for (int i = 0; i < 3; i++) {
 		auto& disp = rmemBlks->at(i);
@@ -72,7 +72,7 @@ void EndFrameqct::initprmemptBlks()
 
 void EndFrameqct::initpprmemptptBlks()
 {
-	auto& mbdTime = System::getInstance().time;
+	auto& mbdTime = this->root()->time;
 	pprmemptptBlks = std::make_shared< FullColumn<Symsptr>>(3);
 	for (int i = 0; i < 3; i++) {
 		auto& vel = prmemptBlks->at(i);
@@ -84,7 +84,7 @@ void EndFrameqct::initpprmemptptBlks()
 
 void EndFrameqct::initpPhiThePsiptBlks()
 {
-	auto& mbdTime = System::getInstance().time;
+	auto& mbdTime = this->root()->time;
 	pPhiThePsiptBlks = std::make_shared< FullColumn<Symsptr>>(3);
 	for (int i = 0; i < 3; i++) {
 		auto& angle = phiThePsiBlks->at(i);
@@ -96,9 +96,9 @@ void EndFrameqct::initpPhiThePsiptBlks()
 	}
 }
 
-void MbD::EndFrameqct::initppPhiThePsiptptBlks()
+void EndFrameqct::initppPhiThePsiptptBlks()
 {
-	auto& mbdTime = System::getInstance().time;
+	auto& mbdTime = this->root()->time;
 	ppPhiThePsiptptBlks = std::make_shared< FullColumn<Symsptr>>(3);
 	for (int i = 0; i < 3; i++) {
 		auto& angleVel = pPhiThePsiptBlks->at(i);
@@ -108,14 +108,14 @@ void MbD::EndFrameqct::initppPhiThePsiptptBlks()
 	}
 }
 
-void MbD::EndFrameqct::postInput()
+void EndFrameqct::postInput()
 {
 	this->evalrmem();
 	this->evalAme();
 	Item::postInput();
 }
 
-void MbD::EndFrameqct::calcPostDynCorrectorIteration()
+void EndFrameqct::calcPostDynCorrectorIteration()
 {
 	auto& rOmO = markerFrame->rOmO;
 	auto& aAOm = markerFrame->aAOm;
@@ -143,15 +143,15 @@ void MbD::EndFrameqct::calcPostDynCorrectorIteration()
 	ppAOepEpE = EulerParameters<double>::ppApEpEtimesMatrix(aApe);
 }
 
-void MbD::EndFrameqct::prePosIC()
+void EndFrameqct::prePosIC()
 {
-	time = System::getInstance().mbdTimeValue();
+	time = this->root()->mbdTimeValue();
 	this->evalrmem();
 	this->evalAme();
 	EndFrameqc::prePosIC();
 }
 
-void MbD::EndFrameqct::evalrmem()
+void EndFrameqct::evalrmem()
 {
 	if (rmemBlks) {
 		for (int i = 0; i < 3; i++)
@@ -163,7 +163,7 @@ void MbD::EndFrameqct::evalrmem()
 	}
 }
 
-void MbD::EndFrameqct::evalAme()
+void EndFrameqct::evalAme()
 {
 	if (phiThePsiBlks) {
 		auto phiThePsi = CREATE<EulerAngleszxz<double>>::With();
@@ -178,9 +178,9 @@ void MbD::EndFrameqct::evalAme()
 	}
 }
 
-void MbD::EndFrameqct::preVelIC()
+void EndFrameqct::preVelIC()
 {
-	time = System::getInstance().mbdTimeValue();
+	time = this->root()->mbdTimeValue();
 	this->evalrmem();
 	this->evalAme();
 	Item::preVelIC();
@@ -191,16 +191,16 @@ void MbD::EndFrameqct::preVelIC()
 	pAOept = aAOm->timesFullMatrix(pAmept);
 }
 
-void MbD::EndFrameqct::postVelIC()
+void 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++)
+		for (int j = 0; j < 4; j++)
 		{
-				auto pprOeOpEptij = pAOmpE->at(j)->at(i)->dot(prmempt);
-				pprOeOpEpti->atiput(j, pprOeOpEptij);
+			auto pprOeOpEptij = pAOmpE->at(j)->at(i)->dot(prmempt);
+			pprOeOpEpti->atiput(j, pprOeOpEptij);
 		}
 	}
 	for (int i = 0; i < 4; i++)
@@ -209,12 +209,12 @@ void MbD::EndFrameqct::postVelIC()
 	}
 }
 
-FColDsptr MbD::EndFrameqct::pAjOept(int j)
+FColDsptr EndFrameqct::pAjOept(int j)
 {
 	return pAOept->column(j);
 }
 
-FMatDsptr MbD::EndFrameqct::ppAjOepETpt(int jj)
+FMatDsptr EndFrameqct::ppAjOepETpt(int jj)
 {
 	auto answer = std::make_shared<FullMatrix<double>>(4, 3);
 	for (int i = 0; i < 4; i++)
@@ -230,27 +230,27 @@ FMatDsptr MbD::EndFrameqct::ppAjOepETpt(int jj)
 	return answer;
 }
 
-FColDsptr MbD::EndFrameqct::ppAjOeptpt(int j)
+FColDsptr EndFrameqct::ppAjOeptpt(int j)
 {
 	return ppAOeptpt->column(j);
 }
 
-double MbD::EndFrameqct::priOeOpt(int i)
+double EndFrameqct::priOeOpt(int i)
 {
 	return prOeOpt->at(i);
 }
 
-FRowDsptr MbD::EndFrameqct::ppriOeOpEpt(int i)
+FRowDsptr EndFrameqct::ppriOeOpEpt(int i)
 {
 	return pprOeOpEpt->at(i);
 }
 
-double MbD::EndFrameqct::ppriOeOptpt(int i)
+double EndFrameqct::ppriOeOptpt(int i)
 {
 	return pprOeOptpt->at(i);
 }
 
-void MbD::EndFrameqct::evalprmempt()
+void EndFrameqct::evalprmempt()
 {
 	if (rmemBlks) {
 		for (int i = 0; i < 3; i++)
@@ -262,7 +262,7 @@ void MbD::EndFrameqct::evalprmempt()
 	}
 }
 
-void MbD::EndFrameqct::evalpAmept()
+void EndFrameqct::evalpAmept()
 {
 	if (phiThePsiBlks) {
 		auto phiThePsi = CREATE<EulerAngleszxz<double>>::With();
@@ -283,7 +283,7 @@ void MbD::EndFrameqct::evalpAmept()
 	}
 }
 
-void MbD::EndFrameqct::evalpprmemptpt()
+void EndFrameqct::evalpprmemptpt()
 {
 	if (rmemBlks) {
 		for (int i = 0; i < 3; i++)
@@ -295,7 +295,7 @@ void MbD::EndFrameqct::evalpprmemptpt()
 	}
 }
 
-void MbD::EndFrameqct::evalppAmeptpt()
+void EndFrameqct::evalppAmeptpt()
 {
 	if (phiThePsiBlks) {
 		auto phiThePsi = CREATE<EulerAngleszxz<double>>::With();
@@ -322,9 +322,22 @@ void MbD::EndFrameqct::evalppAmeptpt()
 	}
 }
 
-void MbD::EndFrameqct::preAccIC()
+FColDsptr EndFrameqct::rmeO()
 {
-	time = System::getInstance().mbdTimeValue();
+	return markerFrame->aAOm->timesFullColumn(rmem);
+}
+
+FColDsptr EndFrameqct::rpep()
+{
+	auto& rpmp = markerFrame->rpmp;
+	auto& aApm = markerFrame->aApm;
+	auto rpep = rpmp->plusFullColumn(aApm->timesFullColumn(rmem));
+	return rpep;
+}
+
+void EndFrameqct::preAccIC()
+{
+	time = this->root()->mbdTimeValue();
 	this->evalrmem();
 	this->evalAme();
 	Item::preVelIC();

MbDCode/EndFrameqct.h

@@ -37,7 +37,9 @@ namespace MbD {
 		void evalpAmept();
 		void evalpprmemptpt();
 		void evalppAmeptpt();
-		
+		FColDsptr rmeO() override;
+		FColDsptr rpep() override;
+
 		void preAccIC() override;
 
 		std::shared_ptr<FullColumn<Symsptr>> rmemBlks, prmemptBlks, pprmemptptBlks;

MbDCode/EulerConstraint.cpp

@@ -19,7 +19,7 @@ void EulerConstraint::initialize()
 	pGpE = std::make_shared<FullRow<double>>(4);
 }
 
-void MbD::EulerConstraint::calcPostDynCorrectorIteration()
+void EulerConstraint::calcPostDynCorrectorIteration()
 {
 	auto& qE = static_cast<PartFrame*>(owner)->qE;
 	aG = qE->sumOfSquares() - 1.0;
@@ -29,18 +29,18 @@ void MbD::EulerConstraint::calcPostDynCorrectorIteration()
 	}
 }
 
-void MbD::EulerConstraint::useEquationNumbers()
+void EulerConstraint::useEquationNumbers()
 {
 	iqE = static_cast<PartFrame*>(owner)->iqE;
 }
 
-void MbD::EulerConstraint::fillPosICError(FColDsptr col)
+void EulerConstraint::fillPosICError(FColDsptr col)
 {
 	Constraint::fillPosICError(col);
 	col->atiplusFullVectortimes(iqE, pGpE, lam);
 }
 
-void MbD::EulerConstraint::fillPosICJacob(SpMatDsptr mat)
+void EulerConstraint::fillPosICJacob(SpMatDsptr mat)
 {
 	//"ppGpEpE is a diag(2,2,2,2)."
 	mat->atijplusFullRow(iG, iqE, pGpE);
@@ -53,18 +53,18 @@ void MbD::EulerConstraint::fillPosICJacob(SpMatDsptr mat)
 	}
 }
 
-void MbD::EulerConstraint::fillPosKineJacob(SpMatDsptr mat)
+void EulerConstraint::fillPosKineJacob(SpMatDsptr mat)
 {
 	mat->atijplusFullRow(iG, iqE, pGpE);
 }
 
-void MbD::EulerConstraint::fillVelICJacob(SpMatDsptr mat)
+void EulerConstraint::fillVelICJacob(SpMatDsptr mat)
 {
 	mat->atijplusFullRow(iG, iqE, pGpE);
 	mat->atijplusFullColumn(iqE, iG, pGpE->transpose());
 }
 
-void MbD::EulerConstraint::fillAccICIterError(FColDsptr col)
+void EulerConstraint::fillAccICIterError(FColDsptr col)
 {
 	//"qdotT[ppGpqpq]*qdot."
 	//"qdotT[2 2 2 2 diag]*qdot."

MbDCode/EulerParametersDot.h

@@ -20,6 +20,7 @@ namespace MbD {
 		void calcAdotBdotCdot();
 		void calcpAdotpE();
 		FMatDsptr aB();
+		FColDsptr omeOpO();
 
 		std::shared_ptr<EulerParameters<T>> qE;
 		FMatDsptr aAdot, aBdot, aCdot;
@@ -170,6 +171,16 @@ namespace MbD {
 		return qE->aB;
 	}
 
+	template<typename T>
+	inline FColDsptr EulerParametersDot<T>::omeOpO()
+	{
+		auto aaa = this->aB();
+		auto bbb = aaa->timesFullColumn(this);
+		auto ccc = bbb->times(2.0);
+		return ccc;
+		//return this->aB->timesFullColumn(this)->times(2.0);
+	}
+
 	template<typename T>
 	inline void EulerParametersDot<T>::calc()
 	{

MbDCode/ForceTorqueData.cpp

@@ -0,0 +1,10 @@
+#include "ForceTorqueData.h"
+
+using namespace MbD;
+
+std::ostream& ForceTorqueData::printOn(std::ostream& s) const
+{
+    s << "aFIO = " << *aFIO << std::endl;
+    s << "aTIO = " << *aTIO << std::endl;
+    return s;
+}

MbDCode/ForceTorqueData.h

@@ -0,0 +1,15 @@
+#pragma once
+
+#include "StateData.h"
+
+namespace MbD {
+	class ForceTorqueData : public StateData
+	{
+		//aFIO aTIO
+	public:
+		std::ostream& printOn(std::ostream& s) const override;
+
+		FColDsptr aFIO, aTIO;
+	};
+}
+

MbDCode/FullColumn.h

@@ -30,13 +30,11 @@ namespace MbD {
 		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);
+		T transposeTimesFullColumn(const std::shared_ptr<FullColumn<T>> fullCol);		
+		void equalSelfPlusFullColumntimes(std::shared_ptr<FullColumn<T>> fullCol, T factor);
+		std::shared_ptr<FullColumn<T>> cross(std::shared_ptr<FullColumn<T>> fullCol);
 
-		virtual std::ostream& printOn(std::ostream& s) const;
-		friend std::ostream& operator<<(std::ostream& s, const FullColumn& fullCol)
-		{
-			return fullCol.printOn(s);
-		}
+		std::ostream& printOn(std::ostream& s) const override;
 	};
 	using FColDsptr = std::shared_ptr<FullColumn<double>>;
 	template<typename T>
@@ -63,7 +61,7 @@ namespace MbD {
 	inline std::shared_ptr<FullColumn<T>> FullColumn<T>::times(double a)
 	{
 		int n = (int) this->size();
-		auto answer = std::make_shared<FullColumn>(n);
+		auto answer = std::make_shared<FullColumn<T>>(n);
 		for (int i = 0; i < n; i++) {
 			answer->at(i) = this->at(i) * a;
 		}
@@ -148,9 +146,29 @@ namespace MbD {
 		return this->dot(fullCol);
 	}
 	template<typename T>
+	inline void FullColumn<T>::equalSelfPlusFullColumntimes(std::shared_ptr<FullColumn<T>> fullCol, T factor)
+	{
+		this->equalSelfPlusFullVectortimes(fullCol, factor);
+	}
+	template<typename T>
+	inline std::shared_ptr<FullColumn<T>> FullColumn<T>::cross(std::shared_ptr<FullColumn<T>> fullCol)
+	{
+			auto a0 = this->at(0);
+			auto a1 = this->at(1);
+			auto a2 = this->at(2);
+			auto b0 = fullCol->at(0);
+			auto b1 = fullCol->at(1);
+			auto b2 = fullCol->at(2);
+			auto answer = std::make_shared<FullColumn<T>>(3);
+			answer->atiput(0, a1 * b2 - (a2 * b1));
+			answer->atiput(1, a2 * b0 - (a0 * b2));
+			answer->atiput(2, a0 * b1 - (a1 * b0));
+			return answer;
+	}
+	template<typename T>
 	inline std::ostream& FullColumn<T>::printOn(std::ostream& s) const
 	{
-		s << "{";
+		s << "FullCol{";
 		s << this->at(0);
 		for (int i = 1; i < this->size(); i++)
 		{

MbDCode/FullMatrix.h

@@ -39,6 +39,7 @@ namespace MbD {
 		void identity();
 		std::shared_ptr<FullColumn<T>> column(int j);
 		std::shared_ptr<FullColumn<T>> timesFullColumn(std::shared_ptr<FullColumn<T>> fullCol);
+		std::shared_ptr<FullColumn<T>> timesFullColumn(FullColumn<T>* fullCol);
 		std::shared_ptr<FullMatrix<T>> timesFullMatrix(std::shared_ptr<FullMatrix<T>> fullMat);
 		std::shared_ptr<FullMatrix<T>> timesTransposeFullMatrix(std::shared_ptr<FullMatrix<T>> fullMat);
 		std::shared_ptr<FullMatrix<T>> times(double a);
@@ -58,6 +59,7 @@ namespace MbD {
 		std::shared_ptr<FullMatrix<T>> copy();
 		FullMatrix<T> operator+(const FullMatrix<T> fullMat);
 		std::shared_ptr<FullColumn<T>> transposeTimesFullColumn(const std::shared_ptr<FullColumn<T>> fullCol);
+		std::ostream& printOn(std::ostream& s) const override;
 
 	};
 	template<>
@@ -241,7 +243,23 @@ namespace MbD {
 		return fullCol->transpose()->timesFullMatrix(sptr)->transpose();
 	}
 	template<typename T>
+	inline std::ostream& FullMatrix<T>::printOn(std::ostream& s) const
+	{
+		s << "FullMat[" << std::endl;
+		for (int i = 0; i < this->size(); i++)
+		{
+			s << *(this->at(i)) << std::endl;
+		}
+		s << "]";
+		return s;
+	}
+	template<typename T>
 	inline std::shared_ptr<FullColumn<T>> FullMatrix<T>::timesFullColumn(std::shared_ptr<FullColumn<T>> fullCol)
+	{
+		return this->timesFullColumn(fullCol.get());
+	}
+	template<typename T>
+	inline std::shared_ptr<FullColumn<T>> FullMatrix<T>::timesFullColumn(FullColumn<T>* fullCol)
 	{
 		//"a*b = a(i,j)b(j) sum j."
 		auto nrow = this->nrow();

MbDCode/FullRow.h

@@ -22,19 +22,22 @@ namespace MbD {
 		std::shared_ptr<FullRow<T>> plusFullRow(std::shared_ptr<FullRow<T>> fullRow);
 		std::shared_ptr<FullRow<T>> minusFullRow(std::shared_ptr<FullRow<T>> fullRow);
 		T timesFullColumn(std::shared_ptr<FullColumn<T>> fullCol);
+		T timesFullColumn(FullColumn<T>* fullCol);
 		std::shared_ptr<FullRow<T>> timesFullMatrix(std::shared_ptr<FullMatrix<T>> fullMat);
 		std::shared_ptr<FullRow<T>> timesTransposeFullMatrix(std::shared_ptr<FullMatrix<T>> fullMat);
 		void equalSelfPlusFullRowTimes(std::shared_ptr<FullRow<T>> fullRow, double factor);
 		std::shared_ptr<FullColumn<T>> transpose();
 		std::shared_ptr<FullRow<T>> copy();
 		void atiplusFullRow(int j, std::shared_ptr<FullRow<T>> fullRow);
+		std::ostream& printOn(std::ostream& s) const override;
+
 	};
 
 	template<typename T>
 	inline std::shared_ptr<FullRow<T>> FullRow<T>::times(double a)
 	{
 		int n = (int) this->size();
-		auto answer = std::make_shared<FullRow>(n);
+		auto answer = std::make_shared<FullRow<T>>(n);
 		for (int i = 0; i < n; i++) {
 			answer->at(i) = this->at(i) * a;
 		}
@@ -67,6 +70,11 @@ namespace MbD {
 	}
 	template<typename T>
 	inline T FullRow<T>::timesFullColumn(std::shared_ptr<FullColumn<T>> fullCol)
+	{
+		return this->timesFullColumn(fullCol.get());
+	}
+	template<typename T>
+	inline T FullRow<T>::timesFullColumn(FullColumn<T>* fullCol)
 	{
 		auto answer = this->at(0) * fullCol->at(0);
 		for (int i = 1; i < this->size(); i++)
@@ -89,10 +97,7 @@ namespace MbD {
 	template<typename T>
 	inline void FullRow<T>::equalSelfPlusFullRowTimes(std::shared_ptr<FullRow<T>> fullRow, double factor)
 	{
-		for (int i = 0; i < this->size(); i++)
-		{
-			this->at(i) += fullRow->at(i) * factor;
-		}
+		this->equalSelfPlusFullVectortimes(fullRow, factor);
 	}
 	template<typename T>
 	inline std::shared_ptr<FullColumn<T>> FullRow<T>::transpose()
@@ -120,6 +125,18 @@ namespace MbD {
 		}
 	}
 	template<typename T>
+	inline std::ostream& FullRow<T>::printOn(std::ostream& s) const
+	{
+		s << "FullRow{";
+		s << this->at(0);
+		for (int i = 1; i < this->size(); i++)
+		{
+			s << ", " << this->at(i);
+		}
+		s << "}";
+		return s;
+	}
+	template<typename T>
 	inline std::shared_ptr<FullRow<T>> FullRow<T>::timesFullMatrix(std::shared_ptr<FullMatrix<T>> fullMat)
 	{
 		std::shared_ptr<FullRow<T>> answer = fullMat->at(0)->times(this->at(0));

MbDCode/FullVector.h

@@ -1,121 +1,157 @@
 #pragma once
+#include <ostream>
+
 #include "Array.h"
 
 namespace MbD {
-    template<typename T>
-    class FullVector : public Array<T>
-    {
-    public:
-        FullVector() {}
-        FullVector(std::vector<T> vec) : Array<T>(vec) {}
-        FullVector(int count) : Array<T>(count) {}
-        FullVector(int count, const T& value) : Array<T>(count, value) {}
-        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();
+	template<typename T>
+	class FullVector : public Array<T>
+	{
+	public:
+		FullVector() {}
+		FullVector(std::vector<T> vec) : Array<T>(vec) {}
+		FullVector(int count) : Array<T>(count) {}
+		FullVector(int count, const T& value) : Array<T>(count, value) {}
+		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<T>> fullVec);
+		void atiplusFullVectortimes(int i, std::shared_ptr<FullVector<T>> fullVec, T factor);
+		double maxMagnitude();
+		double length();
+		void equalSelfPlusFullVectortimes(std::shared_ptr<FullVector<T>> fullVec, T factor);
+		std::ostream& printOn(std::ostream& s) const override;
 
-    };
-    template<typename T>
-    inline double FullVector<T>::dot(std::shared_ptr<FullVector<T>> vec)
-    {
-        int n = (int) this->size();
-        double answer = 0.0;
-        for (int i = 0; i < n; i++) {
-            answer += this->at(i) * vec->at(i);
-        }
-        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;
-    }
-    template<typename T>
-    inline void FullVector<T>::atiminusNumber(int i, T value)
-    {
-        this->at(i) -= value;
-    }
-    template<>
-    inline double FullVector<double>::sumOfSquares()
-    {
-        double sum = 0.0;
-        for (int i = 0; i < this->size(); i++)
-        {
-            double element = this->at(i);
-            sum += element * element;
-        }
-        return sum;
-    }
-    template<typename T>
-    inline double FullVector<T>::sumOfSquares()
-    {
-        assert(false);
-        return 0.0;
-    }
-    template<typename T>
-    inline int FullVector<T>::numberOfElements()
-    {
-        return (int) this->size();
-    }
-    template<>
-    inline void FullVector<double>::zeroSelf()
-    {
-        for (int i = 0; i < this->size(); i++) {
-            this->at(i) = 0.0;
-        }
-    }
-    template<typename T>
-    inline void FullVector<T>::zeroSelf()
-    {
-        assert(false);
-    }
-    template<typename T>
-    inline void FullVector<T>::atitimes(int i, double factor)
-    {
-        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++)
-        {
-            auto i = i1 + ii;
-            this->at(i) += fullVec->at(ii) * factor;
-        }
-    }
-    template<>
-    inline double FullVector<double>::maxMagnitude()
-    {
-        auto answer = 0.0;
-        for (int i = 0; i < this->size(); i++)
-        {
-            auto mag = std::abs(this->at(i));
-            if (answer < mag) answer = mag;
-        }
-        return answer;
-    }
+	};
+	template<typename T>
+	inline double FullVector<T>::dot(std::shared_ptr<FullVector<T>> vec)
+	{
+		int n = (int)this->size();
+		double answer = 0.0;
+		for (int i = 0; i < n; i++) {
+			answer += this->at(i) * vec->at(i);
+		}
+		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;
+	}
+	template<typename T>
+	inline void FullVector<T>::atiminusNumber(int i, T value)
+	{
+		this->at(i) -= value;
+	}
+	template<>
+	inline double FullVector<double>::sumOfSquares()
+	{
+		double sum = 0.0;
+		for (int i = 0; i < this->size(); i++)
+		{
+			double element = this->at(i);
+			sum += element * element;
+		}
+		return sum;
+	}
+	template<typename T>
+	inline double FullVector<T>::sumOfSquares()
+	{
+		assert(false);
+		return 0.0;
+	}
+	template<typename T>
+	inline int FullVector<T>::numberOfElements()
+	{
+		return (int)this->size();
+	}
+	template<>
+	inline void FullVector<double>::zeroSelf()
+	{
+		for (int i = 0; i < this->size(); i++) {
+			this->at(i) = 0.0;
+		}
+	}
+	template<typename T>
+	inline void FullVector<T>::zeroSelf()
+	{
+		assert(false);
+	}
+	template<typename T>
+	inline void FullVector<T>::atitimes(int i, double factor)
+	{
+		this->at(i) *= factor;
+	}
+	template<typename T>
+	inline void FullVector<T>::atiplusFullVector(int i1, std::shared_ptr<FullVector<T>> 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<T>> fullVec, T factor)
+	{
+		for (int ii = 0; ii < fullVec->size(); ii++)
+		{
+			auto i = i1 + ii;
+			this->at(i) += fullVec->at(ii) * factor;
+		}
+	}
+	template<>
+	inline double FullVector<double>::maxMagnitude()
+	{
+		auto answer = 0.0;
+		for (int i = 0; i < this->size(); i++)
+		{
+			auto mag = std::abs(this->at(i));
+			if (answer < mag) answer = mag;
+		}
+		return answer;
+	}
+	template<>
+	inline double FullVector<double>::length()
+	{
+		auto ssq = 0.0;
+		for (int i = 0; i < this->size(); i++)
+		{
+			auto elem = this->at(i);
+			ssq += elem * elem;
+		}
+		return std::sqrt(ssq);
+	}
+	template<typename T>
+	inline void FullVector<T>::equalSelfPlusFullVectortimes(std::shared_ptr<FullVector<T>> fullVec, T factor)
+	{
+		for (int i = 0; i < this->size(); i++)
+		{
+			this->atiplusNumber(i, fullVec->at(i) * factor);
+		}
+	}
+	template<typename T>
+	inline std::ostream& FullVector<T>::printOn(std::ostream& s) const
+	{
+		s << "FullVec{";
+		s << this->at(0);
+		for (int i = 1; i < this->size(); i++)
+		{
+			s << ", " << this->at(i);
+		}
+		s << "}";
+		return s;
+	}
 }

MbDCode/FunctionWithManyArgs.cpp

@@ -3,7 +3,7 @@
 
 using namespace MbD;
 
-MbD::FunctionWithManyArgs::FunctionWithManyArgs()
+FunctionWithManyArgs::FunctionWithManyArgs()
 {
 	terms = std::make_shared<std::vector<Symsptr>>();
 }
@@ -29,7 +29,7 @@ FunctionWithManyArgs::FunctionWithManyArgs(std::shared_ptr<std::vector<Symsptr>>
 		terms->push_back(_terms->at(i));
 }
 
-std::shared_ptr<std::vector<Symsptr>> MbD::FunctionWithManyArgs::getTerms()
+std::shared_ptr<std::vector<Symsptr>> FunctionWithManyArgs::getTerms()
 {
 	return terms;
 }

MbDCode/GEFullMat.cpp

@@ -4,12 +4,12 @@
 
 using namespace MbD;
 
-void MbD::GEFullMat::forwardEliminateWithPivot(int p)
+void GEFullMat::forwardEliminateWithPivot(int p)
 {
 	assert(false);
 }
 
-void MbD::GEFullMat::backSubstituteIntoDU()
+void GEFullMat::backSubstituteIntoDU()
 {
 	answerX = std::make_shared<FullColumn<double>>(n);
 	answerX->at(n - 1) = rightHandSideB->at(m - 1) / matrixA->at(m - 1)->at(n - 1);
@@ -25,27 +25,27 @@ void MbD::GEFullMat::backSubstituteIntoDU()
 	}
 }
 
-void MbD::GEFullMat::postSolve()
+void GEFullMat::postSolve()
 {
 	assert(false);
 }
 
-void MbD::GEFullMat::preSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal)
+void GEFullMat::preSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal)
 {
 	assert(false);
 }
 
-void MbD::GEFullMat::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal)
+void GEFullMat::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal)
 {
 	assert(false);
 }
 
-double MbD::GEFullMat::getmatrixArowimaxMagnitude(int i)
+double GEFullMat::getmatrixArowimaxMagnitude(int i)
 {
 	return matrixA->at(i)->maxMagnitude();
 }
 
-FColDsptr MbD::GEFullMat::basicSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal)
+FColDsptr GEFullMat::basicSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal)
 {
 	this->preSolvewithsaveOriginal(fullMat, fullCol, saveOriginal);
 	for (int p = 0; p < m; p++)
@@ -58,7 +58,7 @@ FColDsptr MbD::GEFullMat::basicSolvewithsaveOriginal(FMatDsptr fullMat, FColDspt
 	return answerX;
 }
 
-FColDsptr MbD::GEFullMat::basicSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal)
+FColDsptr GEFullMat::basicSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal)
 {
 	assert(false);
 	return FColDsptr();

MbDCode/GEFullMatFullPv.cpp

@@ -5,7 +5,7 @@
 
 using namespace MbD;
 
-void MbD::GEFullMatFullPv::doPivoting(int p)
+void GEFullMatFullPv::doPivoting(int p)
 {
 	//"Do full pivoting."
 
@@ -45,7 +45,7 @@ void MbD::GEFullMatFullPv::doPivoting(int p)
 	if (max < singularPivotTolerance) throwSingularMatrixError("");
 }
 
-void MbD::GEFullMatFullPv::postSolve()
+void GEFullMatFullPv::postSolve()
 {
 	assert(false);
 }

MbDCode/GEFullMatParPv.cpp

@@ -5,7 +5,7 @@
 
 using namespace MbD;
 
-void MbD::GEFullMatParPv::doPivoting(int p)
+void GEFullMatParPv::doPivoting(int p)
 {
 	//"Use scalings. Do row pivoting."
 
@@ -36,12 +36,12 @@ void MbD::GEFullMatParPv::doPivoting(int p)
 	if (max < singularPivotTolerance) throwSingularMatrixError("");
 }
 
-void MbD::GEFullMatParPv::postSolve()
+void GEFullMatParPv::postSolve()
 {
 	assert(false);
 }
 
-void MbD::GEFullMatParPv::preSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal)
+void GEFullMatParPv::preSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal)
 {
 	assert(false);
 }

MbDCode/GESpMat.cpp

@@ -4,13 +4,13 @@
 
 using namespace MbD;
 
-FColDsptr MbD::GESpMat::solvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal)
+FColDsptr GESpMat::solvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal)
 {
 	this->timedSolvewithsaveOriginal(spMat, fullCol, saveOriginal);
 	return answerX;
 }
 
-FColDsptr MbD::GESpMat::basicSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal)
+FColDsptr GESpMat::basicSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal)
 {
 	this->preSolvewithsaveOriginal(spMat, fullCol, saveOriginal);
 	for (int p = 0; p < m; p++)
@@ -23,23 +23,23 @@ FColDsptr MbD::GESpMat::basicSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr f
 	return answerX;
 }
 
-FColDsptr MbD::GESpMat::basicSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal)
+FColDsptr GESpMat::basicSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal)
 {
 	assert(false);
 	return FColDsptr();
 }
 
-void MbD::GESpMat::preSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal)
+void GESpMat::preSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal)
 {
 	assert(false);
 }
 
-void MbD::GESpMat::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal)
+void GESpMat::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal)
 {
 	assert(false);
 }
 
-double MbD::GESpMat::getmatrixArowimaxMagnitude(int i)
+double GESpMat::getmatrixArowimaxMagnitude(int i)
 {
 	return matrixA->at(i)->maxMagnitude();
 }

MbDCode/GESpMatFullPv.cpp

@@ -5,7 +5,7 @@
 
 using namespace MbD;
 
-void MbD::GESpMatFullPv::doPivoting(int p)
+void GESpMatFullPv::doPivoting(int p)
 {
 	//"Used by Gauss Elimination only."
 	//"Do full pivoting."
@@ -53,7 +53,7 @@ void MbD::GESpMatFullPv::doPivoting(int p)
 	}
 	markowitzPivotColCount = (int)rowPositionsOfNonZerosInPivotColumn->size();
 }
-void MbD::GESpMatFullPv::forwardEliminateWithPivot(int p)
+void GESpMatFullPv::forwardEliminateWithPivot(int p)
 {
 	//app is pivot.
 	//i > p, j > p
@@ -92,7 +92,7 @@ void MbD::GESpMatFullPv::forwardEliminateWithPivot(int p)
 	}
 }
 
-void MbD::GESpMatFullPv::backSubstituteIntoDU()
+void GESpMatFullPv::backSubstituteIntoDU()
 {
 	//"Use colOrder to get DU in upper triangular with nonzero diagonals."
 	//"Formula given by Eqn. 9.26 and 9.27 in Chapra's text 2nd Edition."
@@ -124,12 +124,12 @@ void MbD::GESpMatFullPv::backSubstituteIntoDU()
 	}
 }
 
-void MbD::GESpMatFullPv::postSolve()
+void GESpMatFullPv::postSolve()
 {
 	assert(false);
 }
 
-void MbD::GESpMatFullPv::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal)
+void GESpMatFullPv::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal)
 {
 	//"A conditioned copy of spMat is solved."
 	if (m != spMat->nrow() || n != spMat->ncol()) {

MbDCode/GESpMatFullPvPosIC.cpp

@@ -7,7 +7,7 @@
 
 using namespace MbD;
 
-void MbD::GESpMatFullPvPosIC::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal)
+void GESpMatFullPvPosIC::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal)
 {
 	GESpMatFullPv::preSolvewithsaveOriginal(spMat, fullCol, saveOriginal);
 	if (system == nullptr) {
@@ -19,7 +19,7 @@ void MbD::GESpMatFullPvPosIC::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsp
 	pivotRowLimit = -1;
 }
 
-void MbD::GESpMatFullPvPosIC::doPivoting(int p)
+void GESpMatFullPvPosIC::doPivoting(int p)
 {
 	//"Used by Gauss Elimination only."
 	//"Swap rows but keep columns in place."

MbDCode/GESpMatParPv.cpp

@@ -4,7 +4,7 @@
 
 using namespace MbD;
 
-void MbD::GESpMatParPv::forwardEliminateWithPivot(int p)
+void GESpMatParPv::forwardEliminateWithPivot(int p)
 {
 	//"rightHandSideB may be multidimensional."
 
@@ -36,7 +36,7 @@ void MbD::GESpMatParPv::forwardEliminateWithPivot(int p)
 	}
 }
 
-void MbD::GESpMatParPv::backSubstituteIntoDU()
+void GESpMatParPv::backSubstituteIntoDU()
 {
 	//"DU is upper triangular with nonzero diagonals."
 
@@ -64,6 +64,6 @@ void MbD::GESpMatParPv::backSubstituteIntoDU()
 	}
 }
 
-void MbD::GESpMatParPv::postSolve()
+void GESpMatParPv::postSolve()
 {
 }

MbDCode/GESpMatParPvMarko.cpp

@@ -5,7 +5,7 @@
 
 using namespace MbD;
 
-void MbD::GESpMatParPvMarko::doPivoting(int p)
+void GESpMatParPvMarko::doPivoting(int p)
 {
 	//"Search from bottom to top."
 	//"Check for singular pivot."
@@ -67,7 +67,7 @@ void MbD::GESpMatParPvMarko::doPivoting(int p)
 	if (max < singularPivotTolerance) throwSingularMatrixError("");
 }
 
-void MbD::GESpMatParPvMarko::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal)
+void GESpMatParPvMarko::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal)
 {
 	//"Optimized for speed."
 	if (m != spMat->nrow() || n != spMat->ncol()) {

MbDCode/GESpMatParPvMarkoFast.cpp

@@ -6,7 +6,7 @@
 
 using namespace MbD;
 
-void MbD::GESpMatParPvMarkoFast::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal)
+void GESpMatParPvMarkoFast::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal)
 {
 	//assert(false);
 	//"Optimized for speed."
@@ -33,7 +33,7 @@ void MbD::GESpMatParPvMarkoFast::preSolvewithsaveOriginal(SpMatDsptr spMat, FCol
 	}
 }
 
-void MbD::GESpMatParPvMarkoFast::doPivoting(int p)
+void GESpMatParPvMarkoFast::doPivoting(int p)
 {
 	//"Search from bottom to top."
 	//"Optimized for speed. No check for singular pivot."

MbDCode/GESpMatParPvPrecise.cpp

@@ -5,7 +5,7 @@
 
 using namespace MbD;
 
-void MbD::GESpMatParPvPrecise::doPivoting(int p)
+void GESpMatParPvPrecise::doPivoting(int p)
 {
 	//"Search from bottom to top."
 	//"Use scaling vector and partial pivoting with actual swapping of rows."
@@ -66,7 +66,7 @@ void MbD::GESpMatParPvPrecise::doPivoting(int p)
 	if (max < singularPivotTolerance) throwSingularMatrixError("");
 }
 
-void MbD::GESpMatParPvPrecise::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal)
+void GESpMatParPvPrecise::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal)
 {
 	//assert(false);
 	//"A conditioned copy of aMatrix is solved."

MbDCode/ICKineIntegrator.cpp

@@ -1,9 +1,42 @@
 #include "ICKineIntegrator.h"
 #include "SystemSolver.h"
 
-void MbD::ICKineIntegrator::runInitialConditionTypeSolution()
+using namespace MbD;
+
+void ICKineIntegrator::runInitialConditionTypeSolution()
 {
 	system->runPosICKine();
 	system->runVelICKine();
 	system->runAccICKine();
 }
+
+void ICKineIntegrator::iStep(int i)
+{
+	assert(false);
+}
+
+void ICKineIntegrator::selectOrder()
+{
+	assert(false);
+}
+
+void ICKineIntegrator::preRun()
+{
+	system->Solver::logString("MbD: Starting quasi kinematic analysis.");
+	QuasiIntegrator::preRun();
+}
+
+void ICKineIntegrator::firstStep()
+{
+	assert(false);
+}
+
+void ICKineIntegrator::subsequentSteps()
+{
+	assert(false);
+}
+
+void MbD::ICKineIntegrator::nextStep()
+{
+	assert(false);
+}

MbDCode/ICKineIntegrator.h

@@ -6,7 +6,13 @@ namespace MbD {
     {
         //
     public:
+        void preRun() override;
+        void firstStep() override;
+        void subsequentSteps() override;
+        void nextStep() override;
         void runInitialConditionTypeSolution() override;
+        void iStep(int i) override;
+        void selectOrder() override;
 
     };
 }

MbDCode/IndependentVariable.cpp

@@ -3,11 +3,11 @@
 
 using namespace MbD;
 
-MbD::IndependentVariable::IndependentVariable()
+IndependentVariable::IndependentVariable()
 {
 }
 
-Symsptr MbD::IndependentVariable::differentiateWRT(Symsptr sptr, Symsptr var)
+Symsptr IndependentVariable::differentiateWRT(Symsptr sptr, Symsptr var)
 {
 	if (this == var.get()) {
 		return std::make_shared<Constant>(1.0);

MbDCode/Integrator.cpp

@@ -2,6 +2,6 @@
 
 using namespace MbD;
 
-void MbD::Integrator::setSystem(Solver* sys)
+void Integrator::setSystem(Solver* sys)
 {
 }

MbDCode/Integrator.h

@@ -10,7 +10,16 @@ namespace MbD {
         //system direction 
     public:
         void setSystem(Solver* sys) override;
+        virtual void firstStep() = 0;
+        virtual void preFirstStep() = 0;
+        virtual void postFirstStep() = 0;
+        virtual void subsequentSteps() = 0;
+        virtual void nextStep() = 0;
+        virtual void preStep() = 0;
+        virtual void postStep() = 0;
         virtual void runInitialConditionTypeSolution() = 0;
+        virtual void iStep(int i) = 0;
+        virtual void selectOrder() = 0;
 
         double direction = 1;
     };

MbDCode/IntegratorInterface.cpp

@@ -6,28 +6,28 @@
 
 using namespace MbD;
 
-void MbD::IntegratorInterface::initializeGlobally()
+void IntegratorInterface::initializeGlobally()
 {
-tstart = system->startTime();
-hout = system->outputStepSize();
-hmax  = system->maxStepSize();
-hmin  = system->minStepSize();
-tout  = system->firstOutputTime();
-tend  = system->endTime();
-direction = (tstart < tend) ? 1.0 : -1.0;
+	tstart = system->startTime();
+	hout = system->outputStepSize();
+	hmax = system->maxStepSize();
+	hmin = system->minStepSize();
+	tout = system->firstOutputTime();
+	tend = system->endTime();
+	direction = (tstart < tend) ? 1.0 : -1.0;
 }
 
-void MbD::IntegratorInterface::setSystem(Solver* sys)
+void IntegratorInterface::setSystem(Solver* sys)
 {
 	system = static_cast<SystemSolver*>(sys);
 }
 
-void MbD::IntegratorInterface::logString(std::string& str)
+void IntegratorInterface::logString(std::string& str)
 {
 	system->logString(str);
 }
 
-void MbD::IntegratorInterface::run()
+void IntegratorInterface::run()
 {
 	this->preRun();
 	this->initializeLocally();
@@ -40,12 +40,37 @@ void MbD::IntegratorInterface::run()
 	this->postRun();
 }
 
-int MbD::IntegratorInterface::orderMax()
+int IntegratorInterface::orderMax()
 {
 	return system->orderMax;
 }
 
-void MbD::IntegratorInterface::incrementTime(double tnew)
+void IntegratorInterface::incrementTime(double tnew)
 {
 	system->settime(tnew);
 }
+
+void IntegratorInterface::postFirstStep()
+{
+	assert(false);	//Not used.
+	//system->postFirstStep();
+	//if (integrator->istep > 0) {
+	//	//"Noise make checking at the start unreliable."
+	//	this->checkForDiscontinuity();
+	//}
+	//this->checkForOutputThrough(integrator->t);
+}
+
+void IntegratorInterface::interpolateAt(double tArg)
+{
+	//"Interpolate for system state at tArg and leave system in that state."
+	assert(false);
+	//auto yout = integrator->yDerivat(0, tArg);
+	//auto ydotout = integrator->yDerivat(1, tArg);
+	//auto yddotout = integrator->yDerivat(2, tArg);
+	//system->time(tArg);
+	//system->y(yout);
+	//system->ydot(ydotout);
+	//system->yddot(yddotout);
+	//system->simUpdateAll();
+}

MbDCode/IntegratorInterface.h

@@ -13,15 +13,20 @@ namespace MbD {
 
         void initializeGlobally() override;
         virtual void preRun() = 0;
+        virtual void checkForDiscontinuity() = 0;
+        
         void setSystem(Solver* sys) override;
         void logString(std::string& str) override;
         void run() override;
         int orderMax();
         virtual void incrementTime(double tnew);
 
-        virtual void preFirstStep() = 0;
-        virtual void preStep() = 0;
+        void postFirstStep() override;
         virtual double suggestSmallerOrAcceptFirstStepSize(double hnew) = 0;
+        virtual double suggestSmallerOrAcceptStepSize(double hnew) = 0;
+        virtual void checkForOutputThrough(double t) = 0;
+        virtual void interpolateAt(double t);
+
         SystemSolver* system;
         double tout = 0, hout = 0, hmin = 0, hmax = 0, tstart = 0, tend = 0;
         std::shared_ptr<BasicQuasiIntegrator> integrator;

MbDCode/Item.cpp

@@ -11,10 +11,15 @@ using namespace MbD;
 Item::Item() {
 }
 
-Item::Item(const char* str) : name(str) 
+Item::Item(const char* str) : name(str)
 {
 }
 
+System* Item::root()
+{
+	return owner->root();
+}
+
 void Item::initialize()
 {
 }
@@ -29,6 +34,14 @@ const std::string& Item::getName() const
 	return name;
 }
 
+std::ostream& Item::printOn(std::ostream& s) const
+{
+	std::string str = typeid(*this).name();
+	auto classname = str.substr(11, str.size() - 11);
+	s << classname << std::endl;
+	return s;
+}
+
 void Item::initializeLocally()
 {
 }
@@ -37,62 +50,62 @@ void Item::initializeGlobally()
 {
 }
 
-void MbD::Item::postInput()
+void Item::postInput()
 {
 	//Called once after input
 	calcPostDynCorrectorIteration();
 }
 
-void MbD::Item::calcPostDynCorrectorIteration()
+void Item::calcPostDynCorrectorIteration()
 {
 }
 
-void MbD::Item::removeRedundantConstraints(std::shared_ptr<std::vector<int>> redunEqnNos)
+void Item::removeRedundantConstraints(std::shared_ptr<std::vector<int>> redunEqnNos)
 {
 }
 
-void MbD::Item::reactivateRedundantConstraints()
+void Item::reactivateRedundantConstraints()
 {
 }
 
-void MbD::Item::fillPosKineError(FColDsptr col)
+void Item::fillPosKineError(FColDsptr col)
 {
 }
 
-void MbD::Item::fillPosKineJacob(SpMatDsptr mat)
+void Item::fillPosKineJacob(SpMatDsptr mat)
 {
 }
 
-void MbD::Item::fillEssenConstraints(std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> essenConstraints)
+void Item::fillEssenConstraints(std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> essenConstraints)
 {
 }
 
-void MbD::Item::fillRedundantConstraints(std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> redunConstraints)
+void Item::fillRedundantConstraints(std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> redunConstraints)
 {
 }
 
-void MbD::Item::fillConstraints(std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> allConstraints)
+void Item::fillConstraints(std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> allConstraints)
 {
 	assert(false);
 }
 
-void MbD::Item::fillqsu(FColDsptr col)
+void Item::fillqsu(FColDsptr col)
 {
 }
 
-void MbD::Item::fillqsuWeights(std::shared_ptr<DiagonalMatrix<double>> diagMat)
+void Item::fillqsuWeights(std::shared_ptr<DiagonalMatrix<double>> diagMat)
 {
 }
 
-void MbD::Item::fillqsulam(FColDsptr col)
+void Item::fillqsulam(FColDsptr col)
 {
 }
 
-void MbD::Item::setqsulam(FColDsptr col)
+void Item::setqsulam(FColDsptr col)
 {
 }
 
-void MbD::Item::outputStates()
+void Item::outputStates()
 {
 	std::stringstream ss;
 	ss << classname() << " " << name;
@@ -100,39 +113,73 @@ void MbD::Item::outputStates()
 	this->logString(str);
 }
 
-void MbD::Item::preDyn()
+void Item::preDyn()
 {
+	//"Assume positions, velocities and accelerations are valid."
+	//"Called once before solving for dynamic solution."
+	//"Update all variable dependent instance variables needed for runDYNAMICS even if they 
+	//have been calculated in postPosIC, postVelIC and postAccIC."
+	//"Calculate p, pdot."
+	//"Default is do nothing."
 }
 
-std::string MbD::Item::classname()
+void Item::postDyn()
+{
+	//"Assume runDYNAMICS ended successfully."
+	//"Called once at the end of runDYNAMICS."
+	//"Update all instance variables dependent on p,q,s,u,mu,pdot,qdot,sdot,udot,mudot (lam) 
+	//regardless of whether they are needed."
+	//"This is a subset of update."
+	//"Default is do nothing."
+}
+
+std::string Item::classname()
 {
 	std::string str = typeid(*this).name();
 	auto answer = str.substr(11, str.size() - 11);
 	return answer;
 }
 
-void MbD::Item::preDynFirstStep()
+void Item::preDynFirstStep()
 {
 	//"Called before the start of the first step in the dynamic solution."
 	this->preDynStep();
 }
 
-void MbD::Item::preDynStep()
+void Item::postDynFirstStep()
+{
+	this->postDynStep();
+}
+
+void Item::preDynStep()
 {
 }
 
-void MbD::Item::storeDynState()
+void Item::postDynStep()
+{
+	//"Called after the end of a complete step in the dynamic solution."
+	//"Update info before checking for discontinuities."
+	//"Default is do nothing."
+}
+
+void Item::storeDynState()
 {
 }
 
-double MbD::Item::suggestSmallerOrAcceptDynFirstStepSize(double hnew)
+double Item::suggestSmallerOrAcceptDynFirstStepSize(double hnew)
 {
 	//"Default is return hnew."
 	//"Best to do nothing so as not to disrupt the starting algorithm."
 	return hnew;
 }
 
-void MbD::Item::preVelIC()
+double Item::suggestSmallerOrAcceptDynStepSize(double hnew)
+{
+	//"Default is return hnew."
+	return hnew;
+}
+
+void Item::preVelIC()
 {
 	//"Assume positions are valid."
 	//"Called once before solving for velocity initial conditions."
@@ -143,77 +190,105 @@ void MbD::Item::preVelIC()
 	this->calcPostDynCorrectorIteration();
 }
 
-void MbD::Item::postVelIC()
+void Item::postVelIC()
 {
 }
 
-void MbD::Item::fillqsudot(FColDsptr col)
+void Item::fillqsudot(FColDsptr col)
 {
 }
 
-void MbD::Item::fillqsudotWeights(std::shared_ptr<DiagonalMatrix<double>> diagMat)
+void Item::fillqsudotWeights(std::shared_ptr<DiagonalMatrix<double>> diagMat)
 {
 }
 
-void MbD::Item::fillVelICError(FColDsptr col)
+void Item::fillVelICError(FColDsptr col)
 {
 }
 
-void MbD::Item::fillVelICJacob(SpMatDsptr mat)
+void Item::fillVelICJacob(SpMatDsptr mat)
 {
 }
 
-void MbD::Item::setqsudotlam(FColDsptr qsudotlam)
+void Item::setqsudotlam(FColDsptr col)
 {
 }
 
-void MbD::Item::preAccIC()
+void Item::preAccIC()
 {
 	this->calcPostDynCorrectorIteration();
 }
 
-void MbD::Item::postAccIC()
+void Item::postAccIC()
 {
 }
 
-void MbD::Item::postAccICIteration()
+void Item::postAccICIteration()
 {
 }
 
-void MbD::Item::fillqsuddotlam(FColDsptr col)
+void Item::fillqsuddotlam(FColDsptr col)
 {
 }
 
-void MbD::Item::fillAccICIterError(FColDsptr col)
+void Item::fillAccICIterError(FColDsptr col)
 {
 }
 
-void MbD::Item::fillAccICIterJacob(SpMatDsptr mat)
+void Item::fillAccICIterJacob(SpMatDsptr mat)
 {
 }
 
-void MbD::Item::setqsuddotlam(FColDsptr qsudotlam)
+void Item::setqsudot(FColDsptr col)
 {
 }
 
-void MbD::Item::constraintsReport()
+void Item::setqsuddotlam(FColDsptr col)
 {
 }
 
-void MbD::Item::setqsu(FColDsptr qsuOld)
+std::shared_ptr<StateData> Item::stateData()
+{
+	assert(false);
+	return std::shared_ptr<StateData>();
+}
+
+void Item::discontinuityAtaddTypeTo(double t, std::shared_ptr<std::vector<DiscontinuityType>> disconTypes)
 {
 }
 
-void MbD::Item::useEquationNumbers()
+double Item::checkForDynDiscontinuityBetween(double tprev, double t)
+{
+	//"Check for discontinuity in the last step defined by the interval (tprevious,t]."
+	//"Default is assume no discontinuity and return t."
+
+	return t;
+}
+
+void Item::constraintsReport()
 {
 }
 
-void MbD::Item::logString(std::string& str)
+void Item::setqsu(FColDsptr qsuOld)
 {
-	System::getInstance().logString(str);
 }
 
-void MbD::Item::prePosIC()
+void Item::useEquationNumbers()
+{
+}
+
+void Item::logString(std::string& str)
+{
+	this->root()->logString(str);
+}
+
+void Item::logString(const char* chars)
+{
+	std::string str = chars;
+	this->logString(str);
+}
+
+void Item::prePosIC()
 {
 	//"Called once before solving for position initial conditions."
 	//"Update all variable dependent instance variables needed for posIC."
@@ -222,28 +297,28 @@ void MbD::Item::prePosIC()
 	calcPostDynCorrectorIteration();
 }
 
-void MbD::Item::prePosKine()
+void Item::prePosKine()
 {
 	this->prePosIC();
 }
 
-void MbD::Item::postPosIC()
+void Item::postPosIC()
 {
 }
 
-void MbD::Item::postPosICIteration()
+void Item::postPosICIteration()
 {
 	this->calcPostDynCorrectorIteration();
 }
 
-void MbD::Item::fillPosICError(FColDsptr col)
+void Item::fillPosICError(FColDsptr col)
 {
 }
 
-void MbD::Item::fillPosICJacob(FMatDsptr mat)
+void Item::fillPosICJacob(FMatDsptr mat)
 {
 }
 
-void MbD::Item::fillPosICJacob(SpMatDsptr mat)
+void Item::fillPosICJacob(SpMatDsptr mat)
 {
 }

MbDCode/Item.h

@@ -6,10 +6,12 @@
 #include "FullMatrix.h"
 #include "DiagonalMatrix.h"
 #include "SparseMatrix.h"
+#include "enum.h"
 
 namespace MbD {
-
+	class System;
 	class Constraint;
+	class StateData;
 
 	class Item
 	{
@@ -17,6 +19,7 @@ namespace MbD {
 	public:
 		Item();
 		Item(const char* str);
+		virtual System* root();
 		virtual void initialize();
 		virtual void initializeLocally();
 		virtual void initializeGlobally();
@@ -26,6 +29,7 @@ namespace MbD {
 		virtual void setqsu(FColDsptr qsuOld);
 		virtual void useEquationNumbers();
 		virtual void logString(std::string& str);
+		void logString(const char* chars);
 
 		virtual void prePosIC();
 		virtual void prePosKine();
@@ -49,18 +53,24 @@ namespace MbD {
 		virtual void setqsulam(FColDsptr col);
 		virtual void outputStates();
 		virtual void preDyn();
+		virtual void postDyn();
 		virtual std::string classname();
 		virtual void preDynFirstStep();
+		virtual void postDynFirstStep();
 		virtual void preDynStep();
+		virtual void postDynStep();
 		virtual void storeDynState();
 		virtual double suggestSmallerOrAcceptDynFirstStepSize(double hnew);
+		virtual double suggestSmallerOrAcceptDynStepSize(double hnew);
 		virtual void preVelIC();
 		virtual void postVelIC();
 		virtual void fillqsudot(FColDsptr col);
 		virtual void fillqsudotWeights(std::shared_ptr<DiagonalMatrix<double>> diagMat);
 		virtual void fillVelICError(FColDsptr col);
 		virtual void fillVelICJacob(SpMatDsptr mat);
-		virtual void setqsudotlam(FColDsptr qsudotlam);
+		virtual void setqsudotlam(FColDsptr col);
+		virtual void setqsudot(FColDsptr col);
+		virtual void setqsuddotlam(FColDsptr col);
 
 		virtual void preAccIC();
 		virtual void postAccIC();
@@ -68,12 +78,27 @@ namespace MbD {
 		virtual void fillqsuddotlam(FColDsptr col);
 		virtual void fillAccICIterError(FColDsptr col);
 		virtual void fillAccICIterJacob(SpMatDsptr mat);
-		virtual void setqsuddotlam(FColDsptr qsudotlam);
+		virtual std::shared_ptr<StateData> stateData();
+		virtual void discontinuityAtaddTypeTo(double t, std::shared_ptr<std::vector<DiscontinuityType>> disconTypes);
+		virtual double checkForDynDiscontinuityBetween(double tprev, double t);
 
 		void setName(std::string& str);
 		const std::string& getName() const;
 
+		virtual std::ostream& printOn(std::ostream& s) const;
+		friend std::ostream& operator<<(std::ostream& s, const Item& item)
+		{
+			if (&item) {
+				return item.printOn(s);
+			}
+			else {
+				s << "NULL";
+			}
+			return s;
+		}
+
 		std::string name;
+		Item* owner = nullptr;	//Use raw pointer when pointing backwards.
 	};
 }
 

MbDCode/Joint.cpp

@@ -9,6 +9,7 @@
 #include "CREATE.h"
 #include "RedundantConstraint.h"
 #include "MarkerFrame.h"
+#include "ForceTorqueData.h"
 
 using namespace MbD;
 
@@ -47,103 +48,103 @@ void Joint::initializeGlobally()
 	constraintsDo([](std::shared_ptr<Constraint> constraint) { constraint->initializeGlobally(); });
 }
 
-void MbD::Joint::constraintsDo(const std::function<void(std::shared_ptr<Constraint>)>& f)
+void Joint::constraintsDo(const std::function<void(std::shared_ptr<Constraint>)>& f)
 {
 	std::for_each(constraints->begin(), constraints->end(), f);
 }
 
-void MbD::Joint::postInput()
+void Joint::postInput()
 {
 	constraintsDo([](std::shared_ptr<Constraint> constraint) { constraint->postInput(); });
 
 }
 
-void MbD::Joint::addConstraint(std::shared_ptr<Constraint> con)
+void Joint::addConstraint(std::shared_ptr<Constraint> con)
 {
 	con->setOwner(this);
 	constraints->push_back(con);
 }
 
-void MbD::Joint::prePosIC()
+void Joint::prePosIC()
 {
 	constraintsDo([](std::shared_ptr<Constraint> constraint) { constraint->prePosIC(); });
 }
 
-void MbD::Joint::prePosKine()
+void Joint::prePosKine()
 {
 	constraintsDo([](std::shared_ptr<Constraint> constraint) { constraint->prePosKine(); });
 }
 
-void MbD::Joint::fillEssenConstraints(std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> essenConstraints)
+void Joint::fillEssenConstraints(std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> essenConstraints)
 {
 	constraintsDo([&](std::shared_ptr<Constraint> con) { con->fillEssenConstraints(con, essenConstraints); });
 }
 
-void MbD::Joint::fillDispConstraints(std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> dispConstraints)
+void Joint::fillDispConstraints(std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> dispConstraints)
 {
 	constraintsDo([&](std::shared_ptr<Constraint> con) { con->fillDispConstraints(con, dispConstraints); });
 }
 
-void MbD::Joint::fillPerpenConstraints(std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> perpenConstraints)
+void Joint::fillPerpenConstraints(std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> perpenConstraints)
 {
 	constraintsDo([&](std::shared_ptr<Constraint> con) { con->fillPerpenConstraints(con, perpenConstraints); });
 }
 
-void MbD::Joint::fillRedundantConstraints(std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> redunConstraints)
+void Joint::fillRedundantConstraints(std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> redunConstraints)
 {
 	constraintsDo([&](std::shared_ptr<Constraint> con) { con->fillRedundantConstraints(con, redunConstraints); });
 }
 
-void MbD::Joint::fillConstraints(std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> allConstraints)
+void Joint::fillConstraints(std::shared_ptr<std::vector<std::shared_ptr<Constraint>>> allConstraints)
 {
 	constraintsDo([&](std::shared_ptr<Constraint> con) { con->fillConstraints(con, allConstraints); });
 }
 
-void MbD::Joint::fillqsulam(FColDsptr col)
+void Joint::fillqsulam(FColDsptr col)
 {
 	constraintsDo([&](std::shared_ptr<Constraint> con) { con->fillqsulam(col); });
 }
 
-void MbD::Joint::fillqsudot(FColDsptr col)
+void Joint::fillqsudot(FColDsptr col)
 {
 	constraintsDo([&](std::shared_ptr<Constraint> con) { con->fillqsudot(col); });
 }
 
-void MbD::Joint::fillqsudotWeights(std::shared_ptr<DiagonalMatrix<double>> diagMat)
+void Joint::fillqsudotWeights(std::shared_ptr<DiagonalMatrix<double>> diagMat)
 {
 }
 
-void MbD::Joint::useEquationNumbers()
+void Joint::useEquationNumbers()
 {
 	constraintsDo([](std::shared_ptr<Constraint> constraint) { constraint->useEquationNumbers(); });
 }
 
-void MbD::Joint::setqsulam(FColDsptr col)
+void Joint::setqsulam(FColDsptr col)
 {
 	constraintsDo([&](std::shared_ptr<Constraint> con) { con->setqsulam(col); });
 }
 
-void MbD::Joint::setqsudotlam(FColDsptr col)
+void Joint::setqsudotlam(FColDsptr col)
 {
 	constraintsDo([&](std::shared_ptr<Constraint> con) { con->setqsudotlam(col); });
 }
 
-void MbD::Joint::postPosICIteration()
+void Joint::postPosICIteration()
 {
 	constraintsDo([](std::shared_ptr<Constraint> constraint) { constraint->postPosICIteration(); });
 }
 
-void MbD::Joint::fillPosICError(FColDsptr col)
+void Joint::fillPosICError(FColDsptr col)
 {
 	constraintsDo([&](std::shared_ptr<Constraint> con) { con->fillPosICError(col); });
 }
 
-void MbD::Joint::fillPosICJacob(SpMatDsptr mat)
+void Joint::fillPosICJacob(SpMatDsptr mat)
 {
 	constraintsDo([&](std::shared_ptr<Constraint> con) { con->fillPosICJacob(mat); });
 }
 
-void MbD::Joint::removeRedundantConstraints(std::shared_ptr<std::vector<int>> redundantEqnNos)
+void Joint::removeRedundantConstraints(std::shared_ptr<std::vector<int>> redundantEqnNos)
 {
 	for (size_t i = 0; i < constraints->size(); i++)
 	{
@@ -156,7 +157,7 @@ void MbD::Joint::removeRedundantConstraints(std::shared_ptr<std::vector<int>> re
 	}
 }
 
-void MbD::Joint::reactivateRedundantConstraints()
+void Joint::reactivateRedundantConstraints()
 {
 	for (size_t i = 0; i < constraints->size(); i++)
 	{
@@ -167,7 +168,7 @@ void MbD::Joint::reactivateRedundantConstraints()
 	}
 }
 
-void MbD::Joint::constraintsReport()
+void Joint::constraintsReport()
 {
 	auto redunCons = std::make_shared<std::vector<std::shared_ptr<Constraint>>>();
 	constraintsDo([&](std::shared_ptr<Constraint> con) {
@@ -185,12 +186,12 @@ void MbD::Joint::constraintsReport()
 	}
 }
 
-void MbD::Joint::postPosIC()
+void Joint::postPosIC()
 {
 	constraintsDo([](std::shared_ptr<Constraint> constraint) { constraint->postPosIC(); });
 }
 
-void MbD::Joint::outputStates()
+void Joint::outputStates()
 {
 	Item::outputStates();
 	std::stringstream ss;
@@ -202,52 +203,102 @@ void MbD::Joint::outputStates()
 	constraintsDo([](std::shared_ptr<Constraint> constraint) { constraint->outputStates(); });
 }
 
-void MbD::Joint::preDyn()
+void Joint::preDyn()
 {
 	constraintsDo([](std::shared_ptr<Constraint> constraint) { constraint->preDyn(); });
 }
 
-void MbD::Joint::fillPosKineError(FColDsptr col)
+void Joint::fillPosKineError(FColDsptr col)
 {
 	constraintsDo([&](std::shared_ptr<Constraint> con) { con->fillPosKineError(col); });
 }
 
-void MbD::Joint::fillPosKineJacob(SpMatDsptr mat)
+void Joint::fillPosKineJacob(SpMatDsptr mat)
 {
 	constraintsDo([&](std::shared_ptr<Constraint> constraint) { constraint->fillPosKineJacob(mat); });
 }
 
-void MbD::Joint::preVelIC()
+void Joint::preVelIC()
 {
 	constraintsDo([](std::shared_ptr<Constraint> constraint) { constraint->preVelIC(); });
 }
 
-void MbD::Joint::fillVelICError(FColDsptr col)
+void Joint::fillVelICError(FColDsptr col)
 {
 	constraintsDo([&](std::shared_ptr<Constraint> con) { con->fillVelICError(col); });
 }
 
-void MbD::Joint::fillVelICJacob(SpMatDsptr mat)
+void Joint::fillVelICJacob(SpMatDsptr mat)
 {
 	constraintsDo([&](std::shared_ptr<Constraint> constraint) { constraint->fillVelICJacob(mat); });
 }
 
-void MbD::Joint::preAccIC()
+void Joint::preAccIC()
 {
 	constraintsDo([](std::shared_ptr<Constraint> constraint) { constraint->preAccIC(); });
 }
 
-void MbD::Joint::fillAccICIterError(FColDsptr col)
+void Joint::fillAccICIterError(FColDsptr col)
 {
 	constraintsDo([&](std::shared_ptr<Constraint> con) { con->fillAccICIterError(col); });
 }
 
-void MbD::Joint::fillAccICIterJacob(SpMatDsptr mat)
+void Joint::fillAccICIterJacob(SpMatDsptr mat)
 {
 	constraintsDo([&](std::shared_ptr<Constraint> con) { con->fillAccICIterJacob(mat); });
 }
 
-void MbD::Joint::setqsuddotlam(FColDsptr qsudotlam)
+void Joint::setqsuddotlam(FColDsptr col)
 {
-	constraintsDo([&](std::shared_ptr<Constraint> con) { con->setqsuddotlam(qsudotlam); });
+	constraintsDo([&](std::shared_ptr<Constraint> con) { con->setqsuddotlam(col); });
+}
+
+std::shared_ptr<StateData> Joint::stateData()
+{
+	//"
+	//MbD returns aFIeO and aTIeO.
+	//GEO needs aFImO and aTImO.
+	//For Motion rImIeO is not zero and is changing.
+	//aFImO : = aFIeO.
+	//aTImO : = aTIeO + (rImIeO cross : aFIeO).
+	//"
+
+	auto answer = std::make_shared<ForceTorqueData>();
+	auto aFIeO = this->aFX();
+	auto aTIeO = this->aTX();
+	auto rImIeO = this->frmI->rmeO();
+	answer->aFIO = aFIeO;
+	answer->aTIO = aTIeO->plusFullColumn(rImIeO->cross(aFIeO));
+	return answer;
+}
+
+FColDsptr Joint::aFX()
+{
+	return this->jointForceI();
+}
+
+FColDsptr Joint::jointForceI()
+{
+	//"jointForceI is force on MbD marker I."
+	auto jointForce = std::make_shared <FullColumn<double>>(3);
+	constraintsDo([&](std::shared_ptr<Constraint> con) { con->addToJointForceI(jointForce); });
+	return jointForce;
+}
+
+FColDsptr Joint::aTX()
+{
+	return this->jointTorqueI();
+}
+
+FColDsptr Joint::jointTorqueI()
+{
+	//"jointTorqueI is torque on MbD marker I."
+	auto jointTorque = std::make_shared <FullColumn<double>>(3);
+	constraintsDo([&](std::shared_ptr<Constraint> con) { con->addToJointTorqueI(jointTorque); });
+	return jointTorque;
+}
+
+void Joint::postDynStep()
+{
+	constraintsDo([](std::shared_ptr<Constraint> constraint) { constraint->postDynStep(); });
 }

MbDCode/Joint.h

@@ -52,7 +52,13 @@ namespace MbD {
         void preAccIC() override;
         void fillAccICIterError(FColDsptr col) override;
         void fillAccICIterJacob(SpMatDsptr mat) override;
-        void setqsuddotlam(FColDsptr qsudotlam) override;
+        void setqsuddotlam(FColDsptr col) override;
+        std::shared_ptr<StateData> stateData() override;
+        FColDsptr aFX();
+        FColDsptr jointForceI();
+        FColDsptr aTX();
+        FColDsptr jointTorqueI();
+        void postDynStep() override;
 
         EndFrmcptr frmI;
         EndFrmcptr frmJ;

MbDCode/KineIntegrator.cpp

@@ -2,19 +2,44 @@
 
 #include "KineIntegrator.h"
 #include "SystemSolver.h"
+#include "Solver.h"
 
 using namespace MbD;
 
-void MbD::KineIntegrator::preRun()
+void KineIntegrator::preRun()
 {
-	std::string str = "MbD: Starting kinematic analysis.";
-	system->logString(str);
+	system->Solver::logString("MbD: Starting kinematic analysis.");
 	QuasiIntegrator::preRun();
 }
 
-void MbD::KineIntegrator::runInitialConditionTypeSolution()
+void KineIntegrator::firstStep()
+{
+	assert(false);
+}
+
+void KineIntegrator::subsequentSteps()
+{
+	assert(false);
+}
+
+void KineIntegrator::nextStep()
+{
+	assert(false);
+}
+
+void KineIntegrator::runInitialConditionTypeSolution()
 {
 	system->runPosKine();
 	system->runVelKine();
 	system->runAccKine();
 }
+
+void KineIntegrator::iStep(int i)
+{
+	assert(false);
+}
+
+void KineIntegrator::selectOrder()
+{
+	assert(false);
+}

MbDCode/KineIntegrator.h

@@ -8,7 +8,12 @@ namespace MbD {
         //
     public:
         void preRun() override;
+        void firstStep() override;
+        void subsequentSteps() override;
+        void nextStep() override;
         void runInitialConditionTypeSolution() override;
+        void iStep(int i) override;
+        void selectOrder() override;
     };
 }
 

MbDCode/LDUFullMat.cpp

@@ -2,30 +2,30 @@
 
 using namespace MbD;
 
-FColDsptr MbD::LDUFullMat::basicSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal)
+FColDsptr LDUFullMat::basicSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal)
 {
 	this->decomposesaveOriginal(fullMat, saveOriginal);
 	FColDsptr answer = this->forAndBackSubsaveOriginal(fullCol, saveOriginal);
 	return answer;
 }
 
-FColDsptr MbD::LDUFullMat::basicSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal)
+FColDsptr LDUFullMat::basicSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal)
 {
 	assert(false);
 	return FColDsptr();
 }
 
-void MbD::LDUFullMat::preSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal)
+void LDUFullMat::preSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal)
 {
 	assert(false);
 }
 
-void MbD::LDUFullMat::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal)
+void LDUFullMat::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal)
 {
 	assert(false);
 }
 
-void MbD::LDUFullMat::forwardEliminateWithPivot(int p)
+void LDUFullMat::forwardEliminateWithPivot(int p)
 {
 	//"Save factors in lower triangle for LU decomposition."
 
@@ -47,12 +47,12 @@ void MbD::LDUFullMat::forwardEliminateWithPivot(int p)
 	}
 }
 
-void MbD::LDUFullMat::postSolve()
+void LDUFullMat::postSolve()
 {
 	assert(false);
 }
 
-void MbD::LDUFullMat::preSolvesaveOriginal(FMatDsptr fullMat, bool saveOriginal)
+void LDUFullMat::preSolvesaveOriginal(FMatDsptr fullMat, bool saveOriginal)
 {
 	if (saveOriginal) {
 		matrixA = fullMat->copy();
@@ -87,7 +87,7 @@ void MbD::LDUFullMat::preSolvesaveOriginal(FMatDsptr fullMat, bool saveOriginal)
 	this->findScalingsForRowRange(0, m);
 }
 
-void MbD::LDUFullMat::decomposesaveOriginal(FMatDsptr fullMat, bool saveOriginal)
+void LDUFullMat::decomposesaveOriginal(FMatDsptr fullMat, bool saveOriginal)
 {
 	this->preSolvesaveOriginal(fullMat, saveOriginal);
 	for (int p = 0; p < m; p++)
@@ -97,12 +97,12 @@ void MbD::LDUFullMat::decomposesaveOriginal(FMatDsptr fullMat, bool saveOriginal
 	}
 }
 
-void MbD::LDUFullMat::decomposesaveOriginal(SpMatDsptr spMat, bool saveOriginal)
+void LDUFullMat::decomposesaveOriginal(SpMatDsptr spMat, bool saveOriginal)
 {
 	assert(false);
 }
 
-FMatDsptr MbD::LDUFullMat::inversesaveOriginal(FMatDsptr fullMat, bool saveOriginal)
+FMatDsptr LDUFullMat::inversesaveOriginal(FMatDsptr fullMat, bool saveOriginal)
 {
 	//"ForAndBackSub be optimized for the identity matrix."
 
@@ -119,12 +119,12 @@ FMatDsptr MbD::LDUFullMat::inversesaveOriginal(FMatDsptr fullMat, bool saveOrigi
 	return matrixAinverse;
 }
 
-double MbD::LDUFullMat::getmatrixArowimaxMagnitude(int i)
+double LDUFullMat::getmatrixArowimaxMagnitude(int i)
 {
 	return matrixA->at(i)->maxMagnitude();
 }
 
-void MbD::LDUFullMat::forwardSubstituteIntoL()
+void LDUFullMat::forwardSubstituteIntoL()
 {
 	//"L is lower triangular with nonzero and ones in diagonal."
 	auto vectorc = std::make_shared<FullColumn<double>>(n);
@@ -141,17 +141,17 @@ void MbD::LDUFullMat::forwardSubstituteIntoL()
 	rightHandSideB = vectorc;
 }
 
-void MbD::LDUFullMat::backSubstituteIntoDU()
+void LDUFullMat::backSubstituteIntoDU()
 {
 	//"DU is upper triangular with nonzero and arbitrary diagonals."
 
 		//| rowi sum |
 	answerX = std::make_shared<FullColumn<double>>(n);
 	answerX->at(n - 1) = rightHandSideB->at(m - 1) / matrixA->at(m - 1)->at(n - 1);
-	for (int i = n-2; i >= 0; i--)
+	for (int i = n - 2; i >= 0; i--)
 	{
 		auto rowi = matrixA->at(i);
-		double sum = answerX->at(n) * rowi->at(n);
+		double sum = answerX->at(n - 1) * rowi->at(n - 1);
 		for (int j = i + 1; j < n - 1; j++)
 		{
 			sum += answerX->at(j) * rowi->at(j);

MbDCode/LDUFullMatParPv.cpp

@@ -4,7 +4,7 @@
 
 using namespace MbD;
 
-void MbD::LDUFullMatParPv::doPivoting(int p)
+void LDUFullMatParPv::doPivoting(int p)
 {
 	//"Use scalings. Do row pivoting."
 

MbDCode/LDUSpMat.cpp

@@ -3,35 +3,35 @@
 
 using namespace MbD;
 
-FColDsptr MbD::LDUSpMat::basicSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal)
+FColDsptr LDUSpMat::basicSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal)
 {
 	this->decomposesaveOriginal(spMat, saveOriginal);
 	FColDsptr answer = this->forAndBackSubsaveOriginal(fullCol, saveOriginal);
 	return answer;
 }
 
-void MbD::LDUSpMat::decomposesaveOriginal(FMatDsptr fullMat, bool saveOriginal)
+void LDUSpMat::decomposesaveOriginal(FMatDsptr fullMat, bool saveOriginal)
 {
 	assert(false);
 }
 
-void MbD::LDUSpMat::decomposesaveOriginal(SpMatDsptr spMat, bool saveOriginal)
+void LDUSpMat::decomposesaveOriginal(SpMatDsptr spMat, bool saveOriginal)
 {
 	assert(false);
 }
 
-FColDsptr MbD::LDUSpMat::forAndBackSubsaveOriginal(FColDsptr fullCol, bool saveOriginal)
+FColDsptr LDUSpMat::forAndBackSubsaveOriginal(FColDsptr fullCol, bool saveOriginal)
 {
 	assert(false);
 	return FColDsptr();
 }
 
-double MbD::LDUSpMat::getmatrixArowimaxMagnitude(int i)
+double LDUSpMat::getmatrixArowimaxMagnitude(int i)
 {
 	return matrixA->at(i)->maxMagnitude();
 }
 
-void MbD::LDUSpMat::forwardSubstituteIntoL()
+void LDUSpMat::forwardSubstituteIntoL()
 {
 	//"L is lower triangular with nonzero and ones in diagonal."
 	auto vectorc = std::make_shared<FullColumn<double>>(n);
@@ -50,7 +50,7 @@ void MbD::LDUSpMat::forwardSubstituteIntoL()
 	rightHandSideB = vectorc;
 }
 
-void MbD::LDUSpMat::backSubstituteIntoDU()
+void LDUSpMat::backSubstituteIntoDU()
 {
 	//"DU is upper triangular with nonzero diagonals."
 

MbDCode/LDUSpMatParPvMarko.cpp

@@ -1,7 +1,9 @@
 #include "LDUSpMatParPvMarko.h"
 #include "SingularMatrixError.h"
 
-void MbD::LDUSpMatParPvMarko::doPivoting(int p)
+using namespace MbD;
+
+void LDUSpMatParPvMarko::doPivoting(int p)
 {
 	//"Search from bottom to top."
 	//"Check for singular pivot."

MbDCode/LDUSpMatParPvPrecise.cpp

@@ -1,7 +1,9 @@
 #include "LDUSpMatParPvPrecise.h"
 #include "SingularMatrixError.h"
 
-void MbD::LDUSpMatParPvPrecise::doPivoting(int p)
+using namespace MbD;
+
+void LDUSpMatParPvPrecise::doPivoting(int p)
 {
 	//"Search from bottom to top."
 	//"Use scaling vector and partial pivoting with actual swapping of rows."

MbDCode/MarkerFrame.cpp

@@ -16,6 +16,11 @@ MarkerFrame::MarkerFrame()
 MarkerFrame::MarkerFrame(const char* str) : CartesianFrame(str) {
 }
 
+System* MarkerFrame::root()
+{
+	return partFrame->root();
+}
+
 void MarkerFrame::initialize()
 {
 	prOmOpE = std::make_shared<FullMatrix<double>>(3, 4);
@@ -46,13 +51,13 @@ void MarkerFrame::initializeGlobally()
 	endFramesDo([](std::shared_ptr<EndFramec> endFrame) { endFrame->initializeGlobally(); });
 }
 
-void MbD::MarkerFrame::postInput()
+void MarkerFrame::postInput()
 {
 	Item::postInput();
 	endFramesDo([](std::shared_ptr<EndFramec> endFrame) { endFrame->postInput(); });
 }
 
-void MbD::MarkerFrame::calcPostDynCorrectorIteration()
+void MarkerFrame::calcPostDynCorrectorIteration()
 {
 	auto rOpO = partFrame->rOpO();
 	auto aAOp = partFrame->aAOp();
@@ -67,128 +72,148 @@ void MbD::MarkerFrame::calcPostDynCorrectorIteration()
 	}
 }
 
-void MbD::MarkerFrame::prePosIC()
+void MarkerFrame::prePosIC()
 {
 	Item::prePosIC();
 	endFramesDo([](std::shared_ptr<EndFramec> endFrame) { endFrame->prePosIC(); });
 }
 
-int MbD::MarkerFrame::iqX()
+int MarkerFrame::iqX()
 {
 	return partFrame->iqX;
 }
 
-int MbD::MarkerFrame::iqE()
+int MarkerFrame::iqE()
 {
 	return partFrame->iqE;
 }
 
-void MbD::MarkerFrame::endFramesDo(const std::function<void(std::shared_ptr<EndFramec>)>& f)
+void MarkerFrame::endFramesDo(const std::function<void(std::shared_ptr<EndFramec>)>& f)
 {
 	std::for_each(endFrames->begin(), endFrames->end(), f);
 }
 
-void MbD::MarkerFrame::fillqsu(FColDsptr col)
+void MarkerFrame::fillqsu(FColDsptr col)
 {
 	endFramesDo([&](const std::shared_ptr<EndFramec>& endFrame) { endFrame->fillqsu(col); });
 }
 
-void MbD::MarkerFrame::fillqsuWeights(std::shared_ptr<DiagonalMatrix<double>> diagMat)
+void MarkerFrame::fillqsuWeights(std::shared_ptr<DiagonalMatrix<double>> diagMat)
 {
 	endFramesDo([&](const std::shared_ptr<EndFramec>& endFrame) { endFrame->fillqsuWeights(diagMat); });
 }
 
-void MbD::MarkerFrame::fillqsulam(FColDsptr col)
+void MarkerFrame::fillqsulam(FColDsptr col)
 {
 	endFramesDo([&](const std::shared_ptr<EndFramec>& endFrame) { endFrame->fillqsulam(col); });
 }
 
-void MbD::MarkerFrame::fillqsudot(FColDsptr col)
+void MarkerFrame::fillqsudot(FColDsptr col)
 {
 	endFramesDo([&](const std::shared_ptr<EndFramec>& endFrame) { endFrame->fillqsudot(col); });
 }
 
-void MbD::MarkerFrame::fillqsudotWeights(std::shared_ptr<DiagonalMatrix<double>> diagMat)
+void MarkerFrame::fillqsudotWeights(std::shared_ptr<DiagonalMatrix<double>> diagMat)
 {
 	endFramesDo([&](const std::shared_ptr<EndFramec>& endFrame) { endFrame->fillqsudotWeights(diagMat); });
 }
 
-void MbD::MarkerFrame::setqsu(FColDsptr col)
+void MarkerFrame::setqsu(FColDsptr col)
 {
 	endFramesDo([&](const std::shared_ptr<EndFramec>& endFrame) { endFrame->setqsu(col); });
 }
 
-void MbD::MarkerFrame::setqsulam(FColDsptr col)
+void MarkerFrame::setqsulam(FColDsptr col)
 {
 	endFramesDo([&](const std::shared_ptr<EndFramec>& endFrame) { endFrame->setqsulam(col); });
 }
 
-void MbD::MarkerFrame::setqsudotlam(FColDsptr col)
+void MarkerFrame::setqsudot(FColDsptr col)
+{
+	endFramesDo([&](const std::shared_ptr<EndFramec>& endFrame) { endFrame->setqsudot(col); });
+}
+
+void MarkerFrame::setqsudotlam(FColDsptr col)
 {
 	endFramesDo([&](const std::shared_ptr<EndFramec>& endFrame) { endFrame->setqsudotlam(col); });
 }
 
-void MbD::MarkerFrame::postPosICIteration()
+void MarkerFrame::postPosICIteration()
 {
 	Item::postPosICIteration();
 	endFramesDo([](std::shared_ptr<EndFramec> endFrame) { endFrame->postPosICIteration(); });
 }
 
-void MbD::MarkerFrame::postPosIC()
+void MarkerFrame::postPosIC()
 {
 	endFramesDo([](std::shared_ptr<EndFramec> endFrame) { endFrame->postPosIC(); });
 }
 
-void MbD::MarkerFrame::preDyn()
+void MarkerFrame::preDyn()
 {
 	endFramesDo([](std::shared_ptr<EndFramec> endFrame) { endFrame->preDyn(); });
 }
 
-void MbD::MarkerFrame::storeDynState()
+void MarkerFrame::storeDynState()
 {
 	endFramesDo([](std::shared_ptr<EndFramec> endFrame) { endFrame->storeDynState(); });
 }
 
-void MbD::MarkerFrame::preVelIC()
+void MarkerFrame::preVelIC()
 {
 	Item::preVelIC();
 	endFramesDo([](std::shared_ptr<EndFramec> endFrame) { endFrame->preVelIC(); });
 }
 
-void MbD::MarkerFrame::postVelIC()
+void MarkerFrame::postVelIC()
 {
 	endFramesDo([](std::shared_ptr<EndFramec> endFrame) { endFrame->postVelIC(); });
 }
 
-void MbD::MarkerFrame::preAccIC()
+void MarkerFrame::preAccIC()
 {
 	Item::preAccIC();
 	endFramesDo([](std::shared_ptr<EndFramec> endFrame) { endFrame->preAccIC(); });
 }
 
-FColDsptr MbD::MarkerFrame::qXdot()
+FColDsptr MarkerFrame::qXdot()
 {
 	return partFrame->qXdot;
 }
 
-std::shared_ptr<EulerParametersDot<double>> MbD::MarkerFrame::qEdot()
+std::shared_ptr<EulerParametersDot<double>> MarkerFrame::qEdot()
 {
 	return partFrame->qEdot;
 }
 
-FColDsptr MbD::MarkerFrame::qXddot()
+FColDsptr MarkerFrame::qXddot()
 {
 	return partFrame->qXddot;
 }
 
-FColDsptr MbD::MarkerFrame::qEddot()
+FColDsptr MarkerFrame::qEddot()
 {
 	return partFrame->qEddot;
 }
 
-void MbD::MarkerFrame::setqsuddotlam(FColDsptr qsudotlam)
+void MarkerFrame::setqsuddotlam(FColDsptr col)
 {
-	endFramesDo([&](const std::shared_ptr<EndFramec>& endFrame) { endFrame->setqsuddotlam(qsudotlam); });
+	endFramesDo([&](const std::shared_ptr<EndFramec>& endFrame) { endFrame->setqsuddotlam(col); });
+}
+
+FColFMatDsptr MarkerFrame::pAOppE()
+{
+	return partFrame->pAOppE();
+}
+
+FMatDsptr MarkerFrame::aBOp()
+{
+	return partFrame->aBOp();
+}
+
+void MarkerFrame::postDynStep()
+{
+	endFramesDo([](std::shared_ptr<EndFramec> endFrame) { endFrame->postDynStep(); });
 }
 
 void MarkerFrame::setPartFrame(PartFrame* partFrm)

MbDCode/MarkerFrame.h

@@ -21,7 +21,8 @@ namespace MbD {
 	public:
 		MarkerFrame();
         MarkerFrame(const char* str);
-        void initialize() override;
+		System* root() override;
+		void initialize() override;
 		void setPartFrame(PartFrame* partFrm);
 		PartFrame* getPartFrame();
 		void setrpmp(FColDsptr x);
@@ -42,7 +43,9 @@ namespace MbD {
 		void fillqsudotWeights(std::shared_ptr<DiagonalMatrix<double>> diagMat) override;
 		void setqsu(FColDsptr col) override;
 		void setqsulam(FColDsptr col) override;
+		void setqsudot(FColDsptr col) override;
 		void setqsudotlam(FColDsptr col) override;
+		void setqsuddotlam(FColDsptr col) override;
 		void postPosICIteration() override;
 		void postPosIC() override;
 		void preDyn() override;
@@ -54,7 +57,9 @@ namespace MbD {
 		std::shared_ptr<EulerParametersDot<double>> qEdot();
 		FColDsptr qXddot();
 		FColDsptr qEddot();
-		void setqsuddotlam(FColDsptr qsudotlam) override;
+		FColFMatDsptr pAOppE();
+		FMatDsptr aBOp();
+		void postDynStep() override;
 
 		PartFrame* partFrame; //Use raw pointer when pointing backwards.
 		FColDsptr rpmp = std::make_shared<FullColumn<double>>(3);

MbDCode/MatrixDecomposition.cpp

@@ -2,7 +2,7 @@
 
 using namespace MbD;
 
-void MbD::MatrixDecomposition::applyRowOrderOnRightHandSideB()
+void MatrixDecomposition::applyRowOrderOnRightHandSideB()
 {
 	FColDsptr answer = std::make_shared<FullColumn<double>>(m);
 	for (int i = 0; i < m; i++)

MbDCode/MatrixLDU.cpp

@@ -2,7 +2,7 @@
 
 using namespace MbD;
 
-FColDsptr MbD::MatrixLDU::forAndBackSubsaveOriginal(FColDsptr fullCol, bool saveOriginal)
+FColDsptr MatrixLDU::forAndBackSubsaveOriginal(FColDsptr fullCol, bool saveOriginal)
 {
 	if (saveOriginal) {
 		rightHandSideB = fullCol->copy();

MbDCode/MatrixSolver.cpp

@@ -10,29 +10,29 @@
 
 using namespace MbD;
 
-void MbD::MatrixSolver::initialize()
+void MatrixSolver::initialize()
 {
 	Solver::initialize();
 	singularPivotTolerance = 4 * std::numeric_limits<double>::epsilon();
 }
 
-void MbD::MatrixSolver::setSystem(Solver* sys)
+void MatrixSolver::setSystem(Solver* sys)
 {
 }
 
-FColDsptr MbD::MatrixSolver::solvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal)
+FColDsptr MatrixSolver::solvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal)
 {
 	this->timedSolvewithsaveOriginal(spMat, fullCol, saveOriginal);
 	return answerX;
 }
 
-FColDsptr MbD::MatrixSolver::solvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal)
+FColDsptr MatrixSolver::solvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal)
 {
 	this->timedSolvewithsaveOriginal(fullMat, fullCol, saveOriginal);
 	return answerX;
 }
 
-FColDsptr MbD::MatrixSolver::timedSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal)
+FColDsptr MatrixSolver::timedSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal)
 {
 	auto start = std::chrono::steady_clock::now();
 
@@ -45,12 +45,12 @@ FColDsptr MbD::MatrixSolver::timedSolvewithsaveOriginal(SpMatDsptr spMat, FColDs
 	return answerX;
 }
 
-FColDsptr MbD::MatrixSolver::timedSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal)
+FColDsptr MatrixSolver::timedSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal)
 {
 	return FColDsptr();
 }
 
-void MbD::MatrixSolver::findScalingsForRowRange(int begin, int end)
+void MatrixSolver::findScalingsForRowRange(int begin, int end)
 {
 	//"Row element * scaling <= 1.0."
 	rowScalings = std::make_shared<FullColumn<double>>(m);
@@ -62,12 +62,12 @@ void MbD::MatrixSolver::findScalingsForRowRange(int begin, int end)
 	}
 }
 
-void MbD::MatrixSolver::throwSingularMatrixError(const char* chars)
+void MatrixSolver::throwSingularMatrixError(const char* chars)
 {
 	throw SingularMatrixError(chars);
 }
 
-void MbD::MatrixSolver::throwSingularMatrixError(const char* chars, std::shared_ptr<FullColumn<int>> redunEqnNos)
+void MatrixSolver::throwSingularMatrixError(const char* chars, std::shared_ptr<FullColumn<int>> redunEqnNos)
 {
 	throw SingularMatrixError(chars, redunEqnNos);
 }

MbDCode/MaximumIterationError.cpp

@@ -2,6 +2,6 @@
 
 using namespace MbD;
 
-MbD::MaximumIterationError::MaximumIterationError(const std::string& msg) : std::runtime_error(msg)
+MaximumIterationError::MaximumIterationError(const std::string& msg) : std::runtime_error(msg)
 {
 }