Added many Joints. Parse asmt file.

MbDCode/ASMTAnimationParameters.cpp

@@ -0,0 +1,36 @@
+#include "ASMTAnimationParameters.h"
+
+using namespace MbD;
+
+void MbD::ASMTAnimationParameters::parseASMT(std::vector<std::string>& lines)
+{
+	//int nframe, icurrent, istart, iend, framesPerSecond;
+	//bool isForward;
+	size_t pos = lines[0].find_first_not_of("\t");
+	auto leadingTabs = lines[0].substr(0, pos);
+	assert(lines[0] == (leadingTabs + "nframe"));
+	lines.erase(lines.begin());
+	nframe = readInt(lines[0]);
+	lines.erase(lines.begin());
+	assert(lines[0] == (leadingTabs + "icurrent"));
+	lines.erase(lines.begin());
+	icurrent = readInt(lines[0]);
+	lines.erase(lines.begin());
+	assert(lines[0] == (leadingTabs + "istart"));
+	lines.erase(lines.begin());
+	istart = readInt(lines[0]);
+	lines.erase(lines.begin());
+	assert(lines[0] == (leadingTabs + "iend"));
+	lines.erase(lines.begin());
+	iend = readInt(lines[0]);
+	lines.erase(lines.begin());
+	assert(lines[0] == (leadingTabs + "isForward"));
+	lines.erase(lines.begin());
+	isForward = readBool(lines[0]);
+	lines.erase(lines.begin());
+	assert(lines[0] == (leadingTabs + "framesPerSecond"));
+	lines.erase(lines.begin());
+	framesPerSecond = readInt(lines[0]);
+	lines.erase(lines.begin());
+
+}

MbDCode/ASMTAnimationParameters.h

@@ -0,0 +1,17 @@
+#pragma once
+
+#include "ASMTItem.h"
+
+namespace MbD {
+    class ASMTAnimationParameters : public ASMTItem
+    {
+        //
+    public:
+        void parseASMT(std::vector<std::string>& lines) override;
+
+		int nframe, icurrent, istart, iend, framesPerSecond;
+        bool isForward;
+
+    };
+}
+

MbDCode/ASMTAssembly.cpp

@@ -0,0 +1,291 @@
+#include <string>
+#include <cassert>
+#include <fstream>	
+
+#include "ASMTAssembly.h"
+#include "CREATE.h"
+#include "ASMTRevoluteJoint.h"
+#include "ASMTCylindricalJoint.h"
+#include "ASMTRotationalMotion.h"
+#include "ASMTTranslationalMotion.h"
+
+using namespace MbD;
+
+void MbD::ASMTAssembly::runFile(const char* chars)
+{
+	std::string str;
+	std::ifstream in(chars);
+	std::string line;
+	std::vector<std::string> lines;
+	while (std::getline(in, line)) {
+		lines.push_back(line);
+	}
+	assert(lines[0] == "freeCAD: 3D CAD with Motion Simulation  by  askoh.com");
+	lines.erase(lines.begin());
+
+	if (lines[0] == "Assembly") {
+		lines.erase(lines.begin());
+		auto assembly = CREATE<ASMTAssembly>::With();
+		assembly->parseASMT(lines);
+	}
+
+}
+
+void MbD::ASMTAssembly::parseASMT(std::vector<std::string>& lines)
+{
+	size_t pos = lines[0].find_first_not_of("\t");
+	auto leadingTabs = lines[0].substr(0, pos);
+	while (!lines.empty()) {
+		if (lines[0] == (leadingTabs + "Notes")) {
+			lines.erase(lines.begin());
+			notes = lines[0];
+			lines.erase(lines.begin());
+		}
+		else if (lines[0] == (leadingTabs + "Name")) {
+			lines.erase(lines.begin());
+			name = lines[0];
+			lines.erase(lines.begin());
+		}
+		else if (lines[0] == (leadingTabs + "Position3D")) {
+			lines.erase(lines.begin());
+			std::istringstream iss(lines[0]);
+			position3D = std::make_shared<FullColumn<double>>();
+			double d;
+			while (iss >> d) {
+				position3D->push_back(d);
+			}
+			lines.erase(lines.begin());
+		}
+		else if (lines[0] == (leadingTabs + "RotationMatrix")) {
+			lines.erase(lines.begin());
+			rotationMatrix = std::make_shared<FullMatrix<double>>(3, 0);
+			for (int i = 0; i < 3; i++)
+			{
+				auto& row = rotationMatrix->at(i);
+				std::istringstream iss(lines[0]);
+				double d;
+				while (iss >> d) {
+					row->push_back(d);
+				}
+				lines.erase(lines.begin());
+			}
+		}
+		else if (lines[0] == (leadingTabs + "Velocity3D")) {
+			lines.erase(lines.begin());
+			std::istringstream iss(lines[0]);
+			velocity3D = std::make_shared<FullColumn<double>>();
+			double d;
+			while (iss >> d) {
+				velocity3D->push_back(d);
+			}
+			lines.erase(lines.begin());
+		}
+		else if (lines[0] == (leadingTabs + "Omega3D")) {
+			lines.erase(lines.begin());
+			std::istringstream iss(lines[0]);
+			omega3D = std::make_shared<FullColumn<double>>();
+			double d;
+			while (iss >> d) {
+				omega3D->push_back(d);
+			}
+			lines.erase(lines.begin());
+		}
+		else if (lines[0] == (leadingTabs + "RefPoints")) {
+			lines.erase(lines.begin());
+			refPoints = std::make_shared<std::vector<std::shared_ptr<ASMTRefPoint>>>();
+			auto it = std::find(lines.begin(), lines.end(), (leadingTabs + "RefCurves"));
+			std::vector<std::string> refPointsLines(lines.begin(), it);
+			while (!refPointsLines.empty()) {
+				if (refPointsLines[0] == (leadingTabs + "\tRefPoint")) {
+					refPointsLines.erase(refPointsLines.begin());
+					auto refPoint = CREATE<ASMTRefPoint>::With();
+					refPoint->parseASMT(refPointsLines);
+					refPoints->push_back(refPoint);
+					refPoint->owner = this;
+				}
+				else {
+					assert(false);
+				}
+			}
+			lines.erase(lines.begin(), it);
+		}
+		else if (lines[0] == (leadingTabs + "RefCurves")) {
+			lines.erase(lines.begin());
+			refCurves = std::make_shared<std::vector<std::shared_ptr<ASMTRefCurve>>>();
+			auto it = std::find(lines.begin(), lines.end(), (leadingTabs + "RefSurfaces"));
+			std::vector<std::string> refCurvesLines(lines.begin(), it);
+			while (!refCurvesLines.empty()) {
+				if (refCurvesLines[0] == (leadingTabs + "\tRefCurve")) {
+					refCurvesLines.erase(refCurvesLines.begin());
+					auto refCurve = CREATE<ASMTRefCurve>::With();
+					refCurve->parseASMT(refCurvesLines);
+					refCurves->push_back(refCurve);
+					refCurve->owner = this;
+				}
+				else {
+					assert(false);
+				}
+			}
+			lines.erase(lines.begin(), it);
+		}
+		else if (lines[0] == (leadingTabs + "RefSurfaces")) {
+			lines.erase(lines.begin());
+			refSurfaces = std::make_shared<std::vector<std::shared_ptr<ASMTRefSurface>>>();
+			auto it = std::find(lines.begin(), lines.end(), (leadingTabs + "Parts"));
+			std::vector<std::string> refSurfacesLines(lines.begin(), it);
+			while (!refSurfacesLines.empty()) {
+				if (refSurfacesLines[0] == (leadingTabs + "\tRefSurface")) {
+					refSurfacesLines.erase(refSurfacesLines.begin());
+					auto refSurface = CREATE<ASMTRefSurface>::With();
+					refSurface->parseASMT(refSurfacesLines);
+					refSurfaces->push_back(refSurface);
+					refSurface->owner = this;
+				}
+				else {
+					assert(false);
+				}
+			}
+			lines.erase(lines.begin(), it);
+		}
+		else if (lines[0] == (leadingTabs + "Parts")) {
+			lines.erase(lines.begin());
+			parts = std::make_shared<std::vector<std::shared_ptr<ASMTPart>>>();
+			auto it = std::find(lines.begin(), lines.end(), (leadingTabs + "KinematicIJs"));
+			std::vector<std::string> partsLines(lines.begin(), it);
+			while (!partsLines.empty()) {
+				if (partsLines[0] == (leadingTabs + "\tPart")) {
+					partsLines.erase(partsLines.begin());
+					auto part = CREATE<ASMTPart>::With();
+					part->parseASMT(partsLines);
+					parts->push_back(part);
+					part->owner = this;
+				}
+				else {
+					assert(false);
+				}
+			}
+			lines.erase(lines.begin(), it);
+		}
+		else if (lines[0] == (leadingTabs + "KinematicIJs")) {
+			lines.erase(lines.begin());
+			kinematicIJs = std::make_shared<std::vector<std::shared_ptr<ASMTKinematicIJ>>>();
+			auto it = std::find(lines.begin(), lines.end(), (leadingTabs + "ConstraintSets"));
+			std::vector<std::string> kinematicIJsLines(lines.begin(), it);
+			while (!kinematicIJsLines.empty()) {
+				if (kinematicIJsLines[0] == (leadingTabs + "\tKinematicIJ")) {
+					kinematicIJsLines.erase(kinematicIJsLines.begin());
+					auto kinematicIJ = CREATE<ASMTKinematicIJ>::With();
+					kinematicIJ->parseASMT(kinematicIJsLines);
+					kinematicIJs->push_back(kinematicIJ);
+					kinematicIJ->owner = this;
+				}
+				else {
+					assert(false);
+				}
+			}
+			lines.erase(lines.begin(), it);
+		}
+		else if (lines[0] == (leadingTabs + "ConstraintSets")) {
+			lines.erase(lines.begin());
+			assert(lines[0] == (leadingTabs + "\tJoints"));
+			lines.erase(lines.begin());
+			joints = std::make_shared<std::vector<std::shared_ptr<ASMTJoint>>>();
+			auto it = std::find(lines.begin(), lines.end(), (leadingTabs + "\tMotions"));
+			std::vector<std::string> jointsLines(lines.begin(), it);
+			while (!jointsLines.empty()) {
+				if (jointsLines[0] == (leadingTabs + "\t\tRevoluteJoint")) {
+					jointsLines.erase(jointsLines.begin());
+					auto joint = CREATE<ASMTRevoluteJoint>::With();
+					joint->parseASMT(jointsLines);
+					joints->push_back(joint);
+					joint->owner = this;
+				}
+				else if (jointsLines[0] == (leadingTabs + "\t\tCylindricalJoint")) {
+					jointsLines.erase(jointsLines.begin());
+					auto joint = CREATE<ASMTCylindricalJoint>::With();
+					joint->parseASMT(jointsLines);
+					joints->push_back(joint);
+					joint->owner = this;
+				}
+				else {
+					assert(false);
+				}
+			}
+			lines.erase(lines.begin(), it);
+			assert(lines[0] == (leadingTabs + "\tMotions"));
+			lines.erase(lines.begin());
+			motions = std::make_shared<std::vector<std::shared_ptr<ASMTMotion>>>();
+			it = std::find(lines.begin(), lines.end(), (leadingTabs + "\tGeneralConstraintSets"));
+			std::vector<std::string> motionsLines(lines.begin(), it);
+			while (!motionsLines.empty()) {
+				if (motionsLines[0] == (leadingTabs + "\t\tRotationalMotion")) {
+					motionsLines.erase(motionsLines.begin());
+					auto motion = CREATE<ASMTRotationalMotion>::With();
+					motion->parseASMT(motionsLines);
+					motions->push_back(motion);
+					motion->owner = this;
+				}
+				else if (motionsLines[0] == (leadingTabs + "\t\tTranslationalMotion")) {
+					motionsLines.erase(motionsLines.begin());
+					auto motion = CREATE<ASMTTranslationalMotion>::With();
+					motion->parseASMT(motionsLines);
+					motions->push_back(motion);
+					motion->owner = this;
+				}
+				else {
+					assert(false);
+				}
+			}
+			lines.erase(lines.begin(), it);
+			assert(lines[0] == (leadingTabs + "\tGeneralConstraintSets"));
+			lines.erase(lines.begin());
+			constraintSets = std::make_shared<std::vector<std::shared_ptr<ASMTConstraintSet>>>();
+			it = std::find(lines.begin(), lines.end(), (leadingTabs + "ForceTorques"));
+			std::vector<std::string> generalConstraintSetsLines(lines.begin(), it);
+			while (!generalConstraintSetsLines.empty()) {
+				assert(false);
+			}
+			lines.erase(lines.begin(), it);
+		}
+		else if (lines[0] == (leadingTabs + "ForceTorques")) {
+			lines.erase(lines.begin());
+			forceTorques = std::make_shared<std::vector<std::shared_ptr<ASMTForceTorque>>>();
+			auto it = std::find(lines.begin(), lines.end(), (leadingTabs + "ConstantGravity"));
+			std::vector<std::string> forceTorquesLines(lines.begin(), it);
+			while (!forceTorquesLines.empty()) {
+				if (forceTorquesLines[0] == (leadingTabs + "\tForceTorque")) {
+					forceTorquesLines.erase(forceTorquesLines.begin());
+					auto forceTorque = CREATE<ASMTForceTorque>::With();
+					forceTorque->parseASMT(forceTorquesLines);
+					forceTorques->push_back(forceTorque);
+					forceTorque->owner = this;
+				}
+				else {
+					assert(false);
+				}
+			}
+			lines.erase(lines.begin(), it);
+		}
+		else if (lines[0] == (leadingTabs + "ConstantGravity")) {
+			lines.erase(lines.begin());
+			constantGravity = CREATE<ASMTConstantGravity>::With();
+			constantGravity->parseASMT(lines);
+			constantGravity->owner = this;
+			}
+		else if (lines[0] == (leadingTabs + "SimulationParameters")) {
+			lines.erase(lines.begin());
+			simulationParameters = CREATE<ASMTSimulationParameters>::With();
+			simulationParameters->parseASMT(lines);
+			simulationParameters->owner = this;
+		}
+		else if (lines[0] == (leadingTabs + "AnimationParameters")) {
+			lines.erase(lines.begin());
+			animationParameters = CREATE<ASMTAnimationParameters>::With();
+			animationParameters->parseASMT(lines);
+			animationParameters->owner = this;
+		}
+		else {
+			assert(false);
+		}
+	}
+}

MbDCode/ASMTAssembly.h

@@ -0,0 +1,46 @@
+#pragma once
+#include <fstream>	
+
+#include "ASMTItem.h"
+#include "ASMTRefPoint.h"
+#include "ASMTRefCurve.h"
+#include "ASMTRefSurface.h"
+#include "ASMTPart.h"
+#include "ASMTKinematicIJ.h"
+#include "ASMTConstraintSet.h"
+#include "ASMTForceTorque.h"
+#include "ASMTConstantGravity.h"
+#include "ASMTSimulationParameters.h"
+#include "ASMTAnimationParameters.h"
+#include "FullColumn.h"
+#include "FullMatrix.h"
+#include "ASMTJoint.h"
+#include "ASMTMotion.h"
+
+namespace MbD {
+    class ASMTAssembly : public ASMTItem
+    {
+        //
+    public:
+        static void runFile(const char* chars);
+        void parseASMT(std::vector<std::string>& lines) override;
+
+        std::string notes, name;
+        FColDsptr position3D, velocity3D, omega3D;
+        FMatDsptr rotationMatrix;
+        std::shared_ptr<std::vector<std::shared_ptr<ASMTRefPoint>>> refPoints;
+        std::shared_ptr<std::vector<std::shared_ptr<ASMTRefCurve>>> refCurves;
+        std::shared_ptr<std::vector<std::shared_ptr<ASMTRefSurface>>> refSurfaces;
+        std::shared_ptr<std::vector<std::shared_ptr<ASMTPart>>> parts;
+        std::shared_ptr<std::vector<std::shared_ptr<ASMTKinematicIJ>>> kinematicIJs;
+        std::shared_ptr<std::vector<std::shared_ptr<ASMTConstraintSet>>> constraintSets;
+        std::shared_ptr<std::vector<std::shared_ptr<ASMTJoint>>> joints;
+        std::shared_ptr<std::vector<std::shared_ptr<ASMTMotion>>> motions;
+        std::shared_ptr<std::vector<std::shared_ptr<ASMTForceTorque>>> forceTorques;
+        std::shared_ptr<ASMTConstantGravity> constantGravity;
+        std::shared_ptr<ASMTSimulationParameters> simulationParameters;
+        std::shared_ptr<ASMTAnimationParameters> animationParameters;
+
+    };
+}
+

MbDCode/ASMTConstantGravity.cpp

@@ -0,0 +1,9 @@
+#include "ASMTConstantGravity.h"
+
+using namespace MbD;
+
+void MbD::ASMTConstantGravity::parseASMT(std::vector<std::string>& lines)
+{
+	g = readColumnOfDoubles(lines[0]);
+	lines.erase(lines.begin());
+}

MbDCode/ASMTConstantGravity.h

@@ -0,0 +1,15 @@
+#pragma once
+
+#include "ASMTItem.h"
+
+namespace MbD {
+    class ASMTConstantGravity : public ASMTItem
+    {
+        //
+    public:
+        void parseASMT(std::vector<std::string>& lines) override;
+
+        FColDsptr g;
+    };
+}
+

MbDCode/ASMTConstraintSet.cpp

@@ -0,0 +1,3 @@
+#include "ASMTConstraintSet.h"
+
+using namespace MbD;

MbDCode/ASMTConstraintSet.h

@@ -0,0 +1,14 @@
+#pragma once
+
+#include "ASMTItem.h"
+
+namespace MbD {
+    class ASMTConstraintSet : public ASMTItem
+    {
+        //
+    public:
+
+
+    };
+}
+

MbDCode/ASMTCylindricalJoint.cpp

@@ -0,0 +1,4 @@
+#include "ASMTCylindricalJoint.h"
+
+using namespace MbD;
+

MbDCode/ASMTCylindricalJoint.h

@@ -0,0 +1,14 @@
+#pragma once
+
+#include "ASMTJoint.h"
+
+namespace MbD {
+    class ASMTCylindricalJoint : public ASMTJoint
+    {
+        //
+    public:
+
+
+    };
+}
+

MbDCode/ASMTExtrusion.cpp

@@ -0,0 +1,3 @@
+#include "ASMTExtrusion.h"
+
+using namespace MbD;

MbDCode/ASMTExtrusion.h

@@ -0,0 +1,14 @@
+#pragma once
+
+#include "ASMTItem.h"
+
+namespace MbD {
+    class ASMTExtrusion : public ASMTItem
+    {
+        //
+    public:
+
+
+    };
+}
+

MbDCode/ASMTForceTorque.cpp

@@ -0,0 +1,3 @@
+#include "ASMTForceTorque.h"
+
+using namespace MbD;

MbDCode/ASMTForceTorque.h

@@ -0,0 +1,14 @@
+#pragma once
+
+#include "ASMTItem.h"
+
+namespace MbD {
+    class ASMTForceTorque : public ASMTItem
+    {
+        //
+    public:
+
+
+    };
+}
+

MbDCode/ASMTGeneralMotion.cpp

@@ -0,0 +1,7 @@
+#include "ASMTGeneralMotion.h"
+
+using namespace MbD;
+
+void MbD::ASMTGeneralMotion::parseASMT(std::vector<std::string>& lines)
+{
+}

MbDCode/ASMTGeneralMotion.h

@@ -0,0 +1,15 @@
+#pragma once
+
+#include "ASMTMotion.h"
+
+namespace MbD {
+    class ASMTGeneralMotion : public ASMTMotion
+    {
+        //
+    public:
+        void parseASMT(std::vector<std::string>& lines) override;
+
+
+    };
+}
+

MbDCode/ASMTItem.cpp

@@ -0,0 +1,66 @@
+#include "ASMTItem.h"
+#include "CREATE.h"
+
+using namespace MbD;
+
+void MbD::ASMTItem::initialize()
+{
+}
+
+void MbD::ASMTItem::parseASMT(std::vector<std::string>& lines)
+{
+	assert(false);
+}
+
+FRowDsptr MbD::ASMTItem::readRowOfDoubles(std::string line)
+{
+	std::istringstream iss(line);
+	auto readRowOfDoubles = std::make_shared<FullRow<double>>();
+	double d;
+	while (iss >> d) {
+		readRowOfDoubles->push_back(d);
+	}
+	return readRowOfDoubles;
+}
+
+FColDsptr MbD::ASMTItem::readColumnOfDoubles(std::string line)
+{
+	std::istringstream iss(line);
+	auto readColumnOfDoubles = std::make_shared<FullColumn<double>>();
+	double d;
+	while (iss >> d) {
+		readColumnOfDoubles->push_back(d);
+	}
+	return readColumnOfDoubles;
+}
+
+double MbD::ASMTItem::readDouble(std::string line)
+{
+	std::istringstream iss(line);
+	double d;
+	iss >> d;
+	return d;
+}
+
+int MbD::ASMTItem::readInt(std::string line)
+{
+	std::istringstream iss(line);
+	int i;
+	iss >> i;
+	return i;
+}
+
+bool MbD::ASMTItem::readBool(std::string line)
+{
+	if (line.find("true") != std::string::npos)
+	{
+		return true;
+	}
+	else 	if (line.find("false") != std::string::npos)
+	{
+		return false;
+	}
+	else {
+		assert(false);
+	}
+}

MbDCode/ASMTItem.h

@@ -0,0 +1,20 @@
+#pragma once
+#include "CREATE.h"
+
+namespace MbD {
+	class ASMTItem
+	{
+		//
+	public:
+		virtual void initialize();
+		virtual void parseASMT(std::vector<std::string>& lines);
+		FRowDsptr readRowOfDoubles(std::string line);
+		FColDsptr readColumnOfDoubles(std::string line);
+		double readDouble(std::string line);
+		int readInt(std::string line);
+		bool readBool(std::string line);
+
+		ASMTItem* owner;
+	};
+}
+

MbDCode/ASMTJoint.cpp

@@ -0,0 +1,21 @@
+#include "ASMTJoint.h"
+
+using namespace MbD;
+
+void MbD::ASMTJoint::parseASMT(std::vector<std::string>& lines)
+{
+	size_t pos = lines[0].find_first_not_of("\t");
+	auto leadingTabs = lines[0].substr(0, pos);
+	assert(lines[0] == (leadingTabs + "Name"));
+	lines.erase(lines.begin());
+	name = lines[0];
+	lines.erase(lines.begin());
+	assert(lines[0] == (leadingTabs + "MarkerI"));
+	lines.erase(lines.begin());
+	markerI = lines[0];
+	lines.erase(lines.begin());
+	assert(lines[0] == (leadingTabs + "MarkerJ"));
+	lines.erase(lines.begin());
+	markerJ = lines[0];
+	lines.erase(lines.begin());
+}

MbDCode/ASMTJoint.h

@@ -0,0 +1,15 @@
+#pragma once
+
+#include "ASMTConstraintSet.h"
+
+namespace MbD {
+    class ASMTJoint : public ASMTConstraintSet
+    {
+        //
+    public:
+        void parseASMT(std::vector<std::string>& lines) override;
+
+        std::string name, markerI, markerJ;
+
+    };
+}

MbDCode/ASMTKinematicIJ.cpp

@@ -0,0 +1,3 @@
+#include "ASMTKinematicIJ.h"
+
+using namespace MbD;

MbDCode/ASMTKinematicIJ.h

@@ -0,0 +1,14 @@
+#pragma once
+
+#include "ASMTItem.h"
+
+namespace MbD {
+    class ASMTKinematicIJ : public ASMTItem
+    {
+        //
+    public:
+
+
+    };
+}
+

MbDCode/ASMTMarker.cpp

@@ -0,0 +1,43 @@
+#include "ASMTMarker.h"
+
+using namespace MbD;
+
+void MbD::ASMTMarker::parseASMT(std::vector<std::string>& lines)
+{
+	size_t pos = lines[0].find_first_not_of("\t");
+	auto leadingTabs = lines[0].substr(0, pos);
+	while (!lines.empty()) {
+		if (lines[0] == (leadingTabs + "Name")) {
+			lines.erase(lines.begin());
+			name = lines[0];
+			lines.erase(lines.begin());
+		}
+		else if (lines[0] == (leadingTabs + "Position3D")) {
+			lines.erase(lines.begin());
+			std::istringstream iss(lines[0]);
+			position3D = std::make_shared<FullColumn<double>>();
+			double d;
+			while (iss >> d) {
+				position3D->push_back(d);
+			}
+			lines.erase(lines.begin());
+		}
+		else if (lines[0] == (leadingTabs + "RotationMatrix")) {
+			lines.erase(lines.begin());
+			rotationMatrix = std::make_shared<FullMatrix<double>>(3, 0);
+			for (int i = 0; i < 3; i++)
+			{
+				auto& row = rotationMatrix->at(i);
+				std::istringstream iss(lines[0]);
+				double d;
+				while (iss >> d) {
+					row->push_back(d);
+				}
+				lines.erase(lines.begin());
+			}
+		}
+		else {
+			assert(false);
+		}
+	}
+}

MbDCode/ASMTMarker.h

@@ -0,0 +1,20 @@
+#pragma once
+
+#include "ASMTItem.h"
+#include "FullColumn.h"
+#include "FullMatrix.h"
+
+namespace MbD {
+    class ASMTMarker : public ASMTItem
+    {
+        //
+    public:
+        void parseASMT(std::vector<std::string>& lines) override;
+
+        std::string name;
+        FColDsptr position3D;
+        FMatDsptr rotationMatrix;
+
+    };
+}
+

MbDCode/ASMTMotion.cpp

@@ -0,0 +1,3 @@
+#include "ASMTMotion.h"
+
+using namespace MbD;

MbDCode/ASMTMotion.h

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

MbDCode/ASMTPart.cpp

@@ -0,0 +1,127 @@
+#include "ASMTPart.h"
+#include "CREATE.h"
+
+using namespace MbD;
+
+void MbD::ASMTPart::parseASMT(std::vector<std::string>& lines)
+{
+	size_t pos = lines[0].find_first_not_of("\t");
+	auto leadingTabs = lines[0].substr(0, pos);
+	while (!lines.empty()) {
+		if (lines[0] == (leadingTabs + "Name")) {
+			lines.erase(lines.begin());
+			name = lines[0];
+			lines.erase(lines.begin());
+		}
+		else if (lines[0] == (leadingTabs + "Position3D")) {
+			lines.erase(lines.begin());
+			position3D = readColumnOfDoubles(lines[0]);
+			lines.erase(lines.begin());
+		}
+		else if (lines[0] == (leadingTabs + "RotationMatrix")) {
+			lines.erase(lines.begin());
+			rotationMatrix = std::make_shared<FullMatrix<double>>(3);
+			for (int i = 0; i < 3; i++)
+			{
+				auto row = readRowOfDoubles(lines[0]);
+				rotationMatrix->atiput(i, row);
+				lines.erase(lines.begin());
+			}
+		}
+		else if (lines[0] == (leadingTabs + "Velocity3D")) {
+			lines.erase(lines.begin());
+			velocity3D = readColumnOfDoubles(lines[0]);
+			lines.erase(lines.begin());
+		}
+		else if (lines[0] == (leadingTabs + "Omega3D")) {
+			lines.erase(lines.begin());
+			omega3D = readColumnOfDoubles(lines[0]);
+			lines.erase(lines.begin());
+		}
+		else if (lines[0] == (leadingTabs + "FeatureOrder")) {
+			lines.erase(lines.begin());
+			//featureOrder = std::make_shared<std::vector<std::shared_ptr<ASMTRefPoint>>>();
+			auto it = std::find(lines.begin(), lines.end(), (leadingTabs + "PrincipalMassMarker"));
+			//std::vector<std::string> featureOrderLines(lines.begin(), it);
+			//while (!featureOrderLines.empty()) {
+			//	if (featureOrderLines[0] == (leadingTabs + "\tExtrusion")) {
+			//		featureOrderLines.erase(featureOrderLines.begin());
+			//		auto extrusion = CREATE<ASMTExtrusion>::With();
+			//		extrusion->parseASMT(featureOrderLines);
+			//		featureOrder->push_back(extrusion);
+			//		extrusion->owner = this;
+			//	}
+			//	else {
+			//		assert(false);
+			//	}
+			//}
+			lines.erase(lines.begin(), it);
+		}
+		else if (lines[0] == (leadingTabs + "PrincipalMassMarker")) {
+			lines.erase(lines.begin());
+			principalMassMarker = CREATE<ASMTPrincipalMassMarker>::With();
+			principalMassMarker->parseASMT(lines);
+			principalMassMarker->owner = this;
+		}
+		else if (lines[0] == (leadingTabs + "RefPoints")) {
+			lines.erase(lines.begin());
+			refPoints = std::make_shared<std::vector<std::shared_ptr<ASMTRefPoint>>>();
+			auto it = std::find(lines.begin(), lines.end(), (leadingTabs + "RefCurves"));
+			std::vector<std::string> refPointsLines(lines.begin(), it);
+			while (!refPointsLines.empty()) {
+				if (refPointsLines[0] == (leadingTabs + "\tRefPoint")) {
+					refPointsLines.erase(refPointsLines.begin());
+					auto refPoint = CREATE<ASMTRefPoint>::With();
+					refPoint->parseASMT(refPointsLines);
+					refPoints->push_back(refPoint);
+					refPoint->owner = this;
+				}
+				else {
+					assert(false);
+				}
+			}
+			lines.erase(lines.begin(), it);
+		}
+		else if (lines[0] == (leadingTabs + "RefCurves")) {
+			lines.erase(lines.begin());
+			refCurves = std::make_shared<std::vector<std::shared_ptr<ASMTRefCurve>>>();
+			auto it = std::find(lines.begin(), lines.end(), (leadingTabs + "RefSurfaces"));
+			std::vector<std::string> refCurvesLines(lines.begin(), it);
+			while (!refCurvesLines.empty()) {
+				if (refCurvesLines[0] == (leadingTabs + "\tRefCurve")) {
+					refCurvesLines.erase(refCurvesLines.begin());
+					auto refCurve = CREATE<ASMTRefCurve>::With();
+					refCurve->parseASMT(refCurvesLines);
+					refCurves->push_back(refCurve);
+					refCurve->owner = this;
+				}
+				else {
+					assert(false);
+				}
+			}
+			lines.erase(lines.begin(), it);
+		}
+		else if (lines[0] == (leadingTabs + "RefSurfaces")) {
+			lines.erase(lines.begin());
+			refSurfaces = std::make_shared<std::vector<std::shared_ptr<ASMTRefSurface>>>();
+			auto it = std::find(lines.begin(), lines.end(), (leadingTabs.substr(0, leadingTabs.size() - 1) + "Part"));
+			std::vector<std::string> refSurfacesLines(lines.begin(), it);
+			while (!refSurfacesLines.empty()) {
+				if (refSurfacesLines[0] == (leadingTabs + "\tRefSurface")) {
+					refSurfacesLines.erase(refSurfacesLines.begin());
+					auto refSurface = CREATE<ASMTRefSurface>::With();
+					refSurface->parseASMT(refSurfacesLines);
+					refSurfaces->push_back(refSurface);
+					refSurface->owner = this;
+				}
+				else {
+					assert(false);
+				}
+			}
+			lines.erase(lines.begin(), it);
+		}
+		else {
+			return;
+		}
+	}
+}

MbDCode/ASMTPart.h

@@ -0,0 +1,27 @@
+#pragma once
+
+#include "ASMTItem.h"
+#include "ASMTRefPoint.h"
+#include "ASMTRefCurve.h"
+#include "ASMTRefSurface.h"
+#include "ASMTPrincipalMassMarker.h"
+
+namespace MbD {
+    class ASMTPart : public ASMTItem
+    {
+        //
+    public:
+        void parseASMT(std::vector<std::string>& lines) override;
+
+        std::string name;
+        FColDsptr position3D, velocity3D, omega3D;
+        FMatDsptr rotationMatrix;
+        std::shared_ptr<ASMTPrincipalMassMarker> principalMassMarker;
+        //std::shared_ptr<std::vector<std::shared_ptr<ASMTFeature>>> featureOrder;
+        std::shared_ptr<std::vector<std::shared_ptr<ASMTRefPoint>>> refPoints;
+        std::shared_ptr<std::vector<std::shared_ptr<ASMTRefCurve>>> refCurves;
+        std::shared_ptr<std::vector<std::shared_ptr<ASMTRefSurface>>> refSurfaces;
+
+    };
+}
+

MbDCode/ASMTPrincipalMassMarker.cpp

@@ -0,0 +1,45 @@
+#include "ASMTPrincipalMassMarker.h"
+#include <cassert>
+#include "FullMatrix.h"
+
+using namespace MbD;
+
+void MbD::ASMTPrincipalMassMarker::parseASMT(std::vector<std::string>& lines)
+{
+	size_t pos = lines[0].find_first_not_of("\t");
+	auto leadingTabs = lines[0].substr(0, pos);
+	assert(lines[0] == (leadingTabs + "Name"));
+	lines.erase(lines.begin());
+	name = lines[0];
+	lines.erase(lines.begin());
+	assert(lines[0] == (leadingTabs + "Position3D"));
+	lines.erase(lines.begin());
+	position3D = readColumnOfDoubles(lines[0]);
+	lines.erase(lines.begin());
+	assert(lines[0] == (leadingTabs + "RotationMatrix"));
+	lines.erase(lines.begin());
+	rotationMatrix = std::make_shared<FullMatrix<double>>(3);
+	for (int i = 0; i < 3; i++)
+	{
+		auto row = readRowOfDoubles(lines[0]);
+		rotationMatrix->atiput(i, row);
+		lines.erase(lines.begin());
+	}
+	assert(lines[0] == (leadingTabs + "Mass"));
+	lines.erase(lines.begin());
+	mass = readDouble(lines[0]);
+	lines.erase(lines.begin());
+	assert(lines[0] == (leadingTabs + "MomentOfInertias"));
+	lines.erase(lines.begin());
+	momentOfInertias = std::make_shared<DiagonalMatrix<double>>(3);
+	auto row = readRowOfDoubles(lines[0]);
+	lines.erase(lines.begin());
+	for (int i = 0; i < 3; i++)
+	{
+		momentOfInertias->atiput(i, row->at(i));
+	}
+	assert(lines[0] == (leadingTabs + "Density"));
+	lines.erase(lines.begin());
+	density = readDouble(lines[0]);
+	lines.erase(lines.begin());
+}

MbDCode/ASMTPrincipalMassMarker.h

@@ -0,0 +1,20 @@
+#pragma once
+
+#include "ASMTItem.h"
+
+namespace MbD {
+    class ASMTPrincipalMassMarker : public ASMTItem
+    {
+        //
+    public:
+        void parseASMT(std::vector<std::string>& lines) override;
+
+		std::string name;
+		FColDsptr position3D;
+		FMatDsptr rotationMatrix;
+		double mass, density;
+        DiagMatDsptr momentOfInertias;
+
+    };
+}
+

MbDCode/ASMTRefCurve.cpp

@@ -0,0 +1,9 @@
+#include "ASMTRefCurve.h"
+#include "CREATE.h"
+
+using namespace MbD;
+
+void MbD::ASMTRefCurve::parseASMT(std::vector<std::string>& lines)
+{
+	assert(false);
+}

MbDCode/ASMTRefCurve.h

@@ -0,0 +1,15 @@
+#pragma once
+
+#include "ASMTItem.h"
+
+namespace MbD {
+    class ASMTRefCurve : public ASMTItem
+    {
+        //
+    public:
+        void parseASMT(std::vector<std::string>& lines) override;
+
+
+    };
+}
+

MbDCode/ASMTRefPoint.cpp

@@ -0,0 +1,58 @@
+#include "ASMTRefPoint.h"
+#include "CREATE.h"
+
+using namespace MbD;
+
+void MbD::ASMTRefPoint::parseASMT(std::vector<std::string>& lines)
+{
+	size_t pos = lines[0].find_first_not_of("\t");
+	auto leadingTabs = lines[0].substr(0, pos);
+	while (!lines.empty()) {
+		if (lines[0] == (leadingTabs + "Position3D")) {
+			lines.erase(lines.begin());
+			std::istringstream iss(lines[0]);
+			position3D = std::make_shared<FullColumn<double>>();
+			double d;
+			while (iss >> d) {
+				position3D->push_back(d);
+			}
+			lines.erase(lines.begin());
+		}
+		else if (lines[0] == (leadingTabs + "RotationMatrix")) {
+			lines.erase(lines.begin());
+			rotationMatrix = std::make_shared<FullMatrix<double>>(3, 0);
+			for (int i = 0; i < 3; i++)
+			{
+				auto& row = rotationMatrix->at(i);
+				std::istringstream iss(lines[0]);
+				double d;
+				while (iss >> d) {
+					row->push_back(d);
+				}
+				lines.erase(lines.begin());
+			}
+		}
+		else if (lines[0] == (leadingTabs + "Markers")) {
+			lines.erase(lines.begin());
+			markers = std::make_shared<std::vector<std::shared_ptr<ASMTMarker>>>();
+			auto it = std::find(lines.begin(), lines.end(), (leadingTabs.substr(0, leadingTabs.size() - 1) + "RefPoint"));
+			std::vector<std::string> markersLines(lines.begin(), it);
+			while (!markersLines.empty()) {
+				if (markersLines[0] == (leadingTabs + "\tMarker")) {
+					markersLines.erase(markersLines.begin());
+					auto marker = CREATE<ASMTMarker>::With();
+					marker->parseASMT(markersLines);
+					markers->push_back(marker);
+					marker->owner = this;
+				}
+				else {
+					return;
+				}
+			}
+			lines.erase(lines.begin(), it);
+		}
+		else {
+			return;
+		}
+	}
+}

MbDCode/ASMTRefPoint.h

@@ -0,0 +1,20 @@
+#pragma once
+
+#include "ASMTItem.h"
+#include <vector>
+#include <string>
+#include "ASMTMarker.h"
+
+namespace MbD {
+    class ASMTRefPoint : public ASMTItem
+    {
+        //
+    public:
+        void parseASMT(std::vector<std::string>& lines) override;
+
+        FColDsptr position3D;
+        FMatDsptr rotationMatrix;
+        std::shared_ptr<std::vector<std::shared_ptr<ASMTMarker>>> markers;
+    };
+}
+

MbDCode/ASMTRefSurface.cpp

@@ -0,0 +1,9 @@
+#include "ASMTRefSurface.h"
+#include "CREATE.h"
+
+using namespace MbD;
+
+void MbD::ASMTRefSurface::parseASMT(std::vector<std::string>& lines)
+{
+	assert(false);
+}

MbDCode/ASMTRefSurface.h

@@ -0,0 +1,15 @@
+#pragma once
+
+#include "ASMTItem.h"
+
+namespace MbD {
+    class ASMTRefSurface : public ASMTItem
+    {
+        //
+    public:
+        void parseASMT(std::vector<std::string>& lines) override;
+
+
+    };
+}
+

MbDCode/ASMTRevoluteJoint.cpp

@@ -0,0 +1,3 @@
+#include "ASMTRevoluteJoint.h"
+
+using namespace MbD;

MbDCode/ASMTRevoluteJoint.h

@@ -0,0 +1,14 @@
+#pragma once
+
+#include "ASMTJoint.h"
+
+namespace MbD {
+    class ASMTRevoluteJoint : public ASMTJoint
+    {
+        //
+    public:
+
+
+    };
+}
+

MbDCode/ASMTRotationalMotion.cpp

@@ -0,0 +1,21 @@
+#include "ASMTRotationalMotion.h"
+
+using namespace MbD;
+
+void MbD::ASMTRotationalMotion::parseASMT(std::vector<std::string>& lines)
+{
+	size_t pos = lines[0].find_first_not_of("\t");
+	auto leadingTabs = lines[0].substr(0, pos);
+	assert(lines[0] == (leadingTabs + "Name"));
+	lines.erase(lines.begin());
+	name = lines[0];
+	lines.erase(lines.begin());
+	assert(lines[0] == (leadingTabs + "MotionJoint"));
+	lines.erase(lines.begin());
+	motionJoint = lines[0];
+	lines.erase(lines.begin());
+	assert(lines[0] == (leadingTabs + "RotationZ"));
+	lines.erase(lines.begin());
+	rotationZ = lines[0];
+	lines.erase(lines.begin());
+}

MbDCode/ASMTRotationalMotion.h

@@ -0,0 +1,15 @@
+#pragma once
+
+#include "ASMTMotion.h"
+
+namespace MbD {
+    class ASMTRotationalMotion : public ASMTMotion
+    {
+        //
+    public:
+        void parseASMT(std::vector<std::string>& lines) override;
+
+        std::string name, motionJoint, rotationZ;
+    };
+}
+

MbDCode/ASMTSimulationParameters.cpp

@@ -0,0 +1,36 @@
+#include "ASMTSimulationParameters.h"
+
+using namespace MbD;
+
+void MbD::ASMTSimulationParameters::parseASMT(std::vector<std::string>& lines)
+{
+	//tstart, tend, hmin, hmax, hout, errorTol;
+
+	size_t pos = lines[0].find_first_not_of("\t");
+	auto leadingTabs = lines[0].substr(0, pos);
+	assert(lines[0] == (leadingTabs + "tstart"));
+	lines.erase(lines.begin());
+	tstart = readDouble(lines[0]);
+	lines.erase(lines.begin());
+	assert(lines[0] == (leadingTabs + "tend"));
+	lines.erase(lines.begin());
+	tend = readDouble(lines[0]);
+	lines.erase(lines.begin());
+	assert(lines[0] == (leadingTabs + "hmin"));
+	lines.erase(lines.begin());
+	hmin = readDouble(lines[0]);
+	lines.erase(lines.begin());
+	assert(lines[0] == (leadingTabs + "hmax"));
+	lines.erase(lines.begin());
+	hmax = readDouble(lines[0]);
+	lines.erase(lines.begin());
+	assert(lines[0] == (leadingTabs + "hout"));
+	lines.erase(lines.begin());
+	hout = readDouble(lines[0]);
+	lines.erase(lines.begin());
+	assert(lines[0] == (leadingTabs + "errorTol"));
+	lines.erase(lines.begin());
+	errorTol = readDouble(lines[0]);
+	lines.erase(lines.begin());
+
+}

MbDCode/ASMTSimulationParameters.h

@@ -0,0 +1,16 @@
+#pragma once
+
+#include "ASMTItem.h"
+
+namespace MbD {
+    class ASMTSimulationParameters : public ASMTItem
+    {
+        //
+    public:
+        void parseASMT(std::vector<std::string>& lines) override;
+
+        double tstart, tend, hmin, hmax, hout, errorTol;
+
+    };
+}
+

MbDCode/ASMTTranslationalMotion.cpp

@@ -0,0 +1,21 @@
+#include "ASMTTranslationalMotion.h"
+
+using namespace MbD;
+
+void MbD::ASMTTranslationalMotion::parseASMT(std::vector<std::string>& lines)
+{
+	size_t pos = lines[0].find_first_not_of("\t");
+	auto leadingTabs = lines[0].substr(0, pos);
+	assert(lines[0] == (leadingTabs + "Name"));
+	lines.erase(lines.begin());
+	name = lines[0];
+	lines.erase(lines.begin());
+	assert(lines[0] == (leadingTabs + "MotionJoint"));
+	lines.erase(lines.begin());
+	motionJoint = lines[0];
+	lines.erase(lines.begin());
+	assert(lines[0] == (leadingTabs + "TranslationZ"));
+	lines.erase(lines.begin());
+	translationZ = lines[0];
+	lines.erase(lines.begin());
+}

MbDCode/ASMTTranslationalMotion.h

@@ -0,0 +1,16 @@
+#pragma once
+
+#include "ASMTMotion.h"
+
+namespace MbD {
+    class ASMTTranslationalMotion : public ASMTMotion
+    {
+        //
+    public:
+        void parseASMT(std::vector<std::string>& lines) override;
+
+        std::string name, motionJoint, translationZ;
+
+    };
+}
+

MbDCode/AbsConstraint.h

@@ -1,4 +1,5 @@
 #pragma once
+
 #include "Constraint.h"
 namespace MbD {
 	class AbsConstraint : public Constraint

MbDCode/AngleJoint.cpp

@@ -17,11 +17,9 @@ void MbD::AngleJoint::initializeGlobally()
 {
 	if (constraints->empty())
 	{
-		addConstraint(CREATE<AtPointConstraintIJ>::ConstraintWith(frmI, frmJ, 0));
-		addConstraint(CREATE<AtPointConstraintIJ>::ConstraintWith(frmI, frmJ, 1));
-		addConstraint(CREATE<AtPointConstraintIJ>::ConstraintWith(frmI, frmJ, 2));
-		addConstraint(CREATE<DirectionCosineConstraintIJ>::ConstraintWith(frmI, frmJ, 2, 0));
-		addConstraint(CREATE<DirectionCosineConstraintIJ>::ConstraintWith(frmI, frmJ, 2, 1));
+		auto dirCosIzJz = CREATE<DirectionCosineConstraintIJ>::ConstraintWith(frmI, frmJ, 2, 2);
+		dirCosIzJz->setConstant(std::cos(theIzJz));
+		addConstraint(dirCosIzJz);
 		this->root()->hasChanged = true;
 	}
 	else {

MbDCode/AngleZIecJec.cpp

@@ -5,6 +5,14 @@
 
 using namespace MbD;
 
+MbD::AngleZIecJec::AngleZIecJec()
+{
+}
+
+MbD::AngleZIecJec::AngleZIecJec(EndFrmcptr frmi, EndFrmcptr frmj) : KinematicIeJe(frmi, frmj)
+{
+}
+
 void MbD::AngleZIecJec::calcPostDynCorrectorIteration()
 {
 	auto cthez = aA00IeJe->value();

MbDCode/AngleZIecJec.h

@@ -8,6 +8,9 @@ namespace MbD {
     {
         //thez aA00IeJe aA10IeJe cosOverSSq sinOverSSq twoCosSinOverSSqSq dSqOverSSqSq 
     public:
+        AngleZIecJec();
+        AngleZIecJec(EndFrmcptr frmi, EndFrmcptr frmj);
+        
         void calcPostDynCorrectorIteration() override;
         virtual void init_aAijIeJe() = 0;
         void initialize() override;

MbDCode/AngleZIeqcJec.cpp

@@ -3,6 +3,14 @@
 
 using namespace MbD;
 
+MbD::AngleZIeqcJec::AngleZIeqcJec()
+{
+}
+
+MbD::AngleZIeqcJec::AngleZIeqcJec(EndFrmcptr frmi, EndFrmcptr frmj) : AngleZIecJec(frmi, frmj)
+{
+}
+
 void MbD::AngleZIeqcJec::calcPostDynCorrectorIteration()
 {
 	AngleZIecJec::calcPostDynCorrectorIteration();

MbDCode/AngleZIeqcJec.h

@@ -7,6 +7,9 @@ namespace MbD {
 	{
 		//pthezpEI ppthezpEIpEI pcthezpEI psthezpEI 
 	public:
+		AngleZIeqcJec();
+		AngleZIeqcJec(EndFrmcptr frmi, EndFrmcptr frmj);
+
 		void calcPostDynCorrectorIteration() override;
 		void init_aAijIeJe() override;
 		void initialize() override;

MbDCode/AngleZIeqcJeqc.cpp

@@ -4,6 +4,14 @@
 
 using namespace MbD;
 
+MbD::AngleZIeqcJeqc::AngleZIeqcJeqc()
+{
+}
+
+MbD::AngleZIeqcJeqc::AngleZIeqcJeqc(EndFrmcptr frmi, EndFrmcptr frmj) : AngleZIeqcJec(frmi, frmj)
+{
+}
+
 void MbD::AngleZIeqcJeqc::calcPostDynCorrectorIteration()
 {
 	AngleZIeqcJec::calcPostDynCorrectorIteration();

MbDCode/AngleZIeqcJeqc.h

@@ -7,6 +7,9 @@ namespace MbD {
     {
         //pthezpEJ ppthezpEIpEJ ppthezpEJpEJ 
     public:
+        AngleZIeqcJeqc();
+        AngleZIeqcJeqc(EndFrmcptr frmi, EndFrmcptr frmj);
+        
         void calcPostDynCorrectorIteration() override;
         void init_aAijIeJe() override;
         void initialize() override;

MbDCode/Array.h

@@ -1,4 +1,5 @@
 #pragma once
+
 #include <ostream>
 #include <vector>
 #include <memory>

MbDCode/AtPointJoint.cpp

@@ -0,0 +1,20 @@
+#include "AtPointJoint.h"
+#include "System.h"
+#include "CREATE.h"
+
+using namespace MbD;
+
+MbD::AtPointJoint::AtPointJoint()
+{
+}
+
+MbD::AtPointJoint::AtPointJoint(const char* str) : Joint(str)
+{
+}
+
+void MbD::AtPointJoint::createAtPointConstraints()
+{
+	addConstraint(CREATE<AtPointConstraintIJ>::ConstraintWith(frmI, frmJ, 0));
+	addConstraint(CREATE<AtPointConstraintIJ>::ConstraintWith(frmI, frmJ, 1));
+	addConstraint(CREATE<AtPointConstraintIJ>::ConstraintWith(frmI, frmJ, 2));
+}

MbDCode/AtPointJoint.h

@@ -0,0 +1,18 @@
+#pragma once
+
+#include "Joint.h"
+
+namespace MbD {
+    class AtPointJoint : public Joint
+    {
+        //
+    public:
+        AtPointJoint();
+        AtPointJoint(const char* str);
+
+        void createAtPointConstraints();
+
+
+    };
+}
+

MbDCode/BasicIntegrator.h

@@ -1,4 +1,5 @@
 #pragma once
+
 #include <vector>
 
 #include "Integrator.h"

MbDCode/BasicUserFunction.h

@@ -1,4 +1,5 @@
 #pragma once
+
 #include <string>
 
 #include "UserFunction.h"

MbDCode/CADSystem.h

@@ -1,4 +1,5 @@
 #pragma once
+
 #include<memory>
 
 #include "System.h"

MbDCode/CREATE.h

@@ -1,6 +1,7 @@
 //This header file causes wierd problems in Visual Studio when included in subclasses of std::vector or std::map.
 
 #pragma once
+
 #include <memory>
 #include "EndFrameqct.h"
 #include "AtPointConstraintIqctJqc.h"

MbDCode/CartesianFrame.h

@@ -1,4 +1,5 @@
 #pragma once
+
 #include "Item.h"
 
 namespace MbD {

MbDCode/ClassDiagram.cd

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="utf-8"?>
+<ClassDiagram /> 

MbDCode/CompoundJoint.cpp

@@ -0,0 +1,11 @@
+#include "CompoundJoint.h"
+
+using namespace MbD;
+
+MbD::CompoundJoint::CompoundJoint()
+{
+}
+
+MbD::CompoundJoint::CompoundJoint(const char* str) : Joint(str)
+{
+}

MbDCode/CompoundJoint.h

@@ -0,0 +1,17 @@
+#pragma once
+
+#include "Joint.h"
+
+namespace MbD {
+	class CompoundJoint : public Joint
+	{
+		//distanceIJ
+	public:
+		CompoundJoint();
+		CompoundJoint(const char* str);
+
+		double distanceIJ = 0.0;
+	};
+}
+
+

MbDCode/ConstVelConstraintIJ.cpp

@@ -12,6 +12,16 @@ void ConstVelConstraintIJ::calcPostDynCorrectorIteration()
 	aG = aA01IeJe->aAijIeJe + aA10IeJe->aAijIeJe - aConstant;
 }
 
+void MbD::ConstVelConstraintIJ::initA01IeJe()
+{
+	assert(false);
+}
+
+void MbD::ConstVelConstraintIJ::initA10IeJe()
+{
+	assert(false);
+}
+
 void ConstVelConstraintIJ::initialize()
 {
 	this->initA01IeJe();
@@ -37,11 +47,6 @@ void ConstVelConstraintIJ::postInput()
 	Constraint::postInput();
 }
 
-ConstraintType ConstVelConstraintIJ::type()
-{
-	return ConstraintType();
-}
-
 void ConstVelConstraintIJ::postPosICIteration()
 {
 	aA01IeJe->postPosICIteration();

MbDCode/ConstVelConstraintIJ.h

@@ -12,8 +12,8 @@ namespace MbD {
         ConstVelConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj);
 
         void calcPostDynCorrectorIteration() override;
-        virtual void initA01IeJe() = 0;
-        virtual void initA10IeJe() = 0;
+        virtual void initA01IeJe();
+        virtual void initA10IeJe();
         void initialize() override;
         void initializeGlobally() override;
         void initializeLocally() override;
@@ -23,7 +23,6 @@ namespace MbD {
         void prePosIC() override;
         void preVelIC() override;
         void simUpdateAll() override;
-        ConstraintType type() override;
 
         std::shared_ptr<DirectionCosineIecJec> aA01IeJe, aA10IeJe;
     };

MbDCode/ConstantVelocityJoint.cpp

@@ -0,0 +1,37 @@
+#include "ConstantVelocityJoint.h"
+#include "System.h"
+#include "AtPointConstraintIJ.h"
+#include "CREATE.h"
+#include "ConstVelConstraintIJ.h"
+
+using namespace MbD;
+
+MbD::ConstantVelocityJoint::ConstantVelocityJoint()
+{
+}
+
+MbD::ConstantVelocityJoint::ConstantVelocityJoint(const char* str) : AtPointJoint(str)
+{
+}
+
+void MbD::ConstantVelocityJoint::initializeGlobally()
+{
+	if (constraints->empty())
+	{
+		createAtPointConstraints();
+		addConstraint(CREATE<ConstVelConstraintIJ>::With(frmI, frmJ));
+		this->root()->hasChanged = true;
+	}
+	else {
+		Joint::initializeGlobally();
+	}
+}
+
+void MbD::ConstantVelocityJoint::connectsItoJ(EndFrmcptr frmIe, EndFrmcptr frmJe)
+{
+	//"Subsequent prescribed motions may make frmIe, frmJe become prescribed end frames."
+	//"Use newCopyEndFrameqc to prevent efrms from becoming EndFrameqct."
+
+	frmI = frmIe->newCopyEndFrameqc();
+	frmJ = frmJe->newCopyEndFrameqc();
+}

MbDCode/ConstantVelocityJoint.h

@@ -0,0 +1,18 @@
+#pragma once
+
+#include "AtPointJoint.h"
+
+namespace MbD {
+    class ConstantVelocityJoint : public AtPointJoint
+    {
+        //
+    public:
+        ConstantVelocityJoint();
+        ConstantVelocityJoint(const char* str);
+        void initializeGlobally() override;
+        void connectsItoJ(EndFrmcptr frmI, EndFrmcptr frmJ) override;
+
+
+    };
+}
+

MbDCode/Constraint.cpp

@@ -105,6 +105,10 @@ void Constraint::removeRedundantConstraints(std::shared_ptr<std::vector<int>> re
 	assert(false);
 }
 
+void MbD::Constraint::setConstant(double value)
+{
+}
+
 void Constraint::reactivateRedundantConstraints()
 {
 	//My owner should handle this.

MbDCode/Constraint.h

@@ -1,4 +1,5 @@
 #pragma once
+
 #include <memory>
 
 #include "enum.h"
@@ -33,6 +34,7 @@ namespace MbD {
 		void prePosKine() override;
 		void reactivateRedundantConstraints() override;
 		void removeRedundantConstraints(std::shared_ptr<std::vector<int>> redundantEqnNos) override;
+		virtual void setConstant(double value);
 		void setqsudotlam(FColDsptr col) override;
 		void setqsuddotlam(FColDsptr col) override;
 		void setqsulam(FColDsptr col) override;

MbDCode/ConstraintIJ.cpp

@@ -12,3 +12,8 @@ void ConstraintIJ::initialize()
 	Constraint::initialize();
 	aConstant = 0.0;
 }
+
+void MbD::ConstraintIJ::setConstant(double value)
+{
+	aConstant = value;
+}

MbDCode/ConstraintIJ.h

@@ -11,6 +11,7 @@ namespace MbD {
         ConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj);
 
         void initialize() override;
+        void setConstant(double value) override;
 
         EndFrmcptr frmI, frmJ;
         double aConstant;

MbDCode/CylSphJoint.cpp

@@ -0,0 +1,28 @@
+#include "CylSphJoint.h"
+#include "CREATE.h"
+#include "DistancexyConstraintIJ.h"
+#include "System.h"
+
+using namespace MbD;
+
+MbD::CylSphJoint::CylSphJoint()
+{
+}
+
+MbD::CylSphJoint::CylSphJoint(const char* str) : CompoundJoint(str)
+{
+}
+
+void MbD::CylSphJoint::initializeGlobally()
+{
+	if (constraints->empty())
+	{
+		auto distxyIJ = CREATE<DistancexyConstraintIJ>::With(frmI, frmJ);
+		distxyIJ->setConstant(distanceIJ);
+		addConstraint(distxyIJ);
+		this->root()->hasChanged = true;
+	}
+	else {
+		CompoundJoint::initializeGlobally();
+	}
+}

MbDCode/CylSphJoint.h

@@ -0,0 +1,16 @@
+#pragma once
+
+#include "CompoundJoint.h"
+
+namespace MbD {
+	class CylSphJoint : public CompoundJoint
+	{
+		//
+	public:
+		CylSphJoint();
+		CylSphJoint(const char* str);
+		void initializeGlobally() override;
+
+
+	};
+}

MbDCode/CylindricalJoint.cpp

@@ -6,23 +6,24 @@
 
 using namespace MbD;
 
-CylindricalJoint::CylindricalJoint() {
+CylindricalJoint::CylindricalJoint() 
+{
 }
 
-CylindricalJoint::CylindricalJoint(const char* str) : Joint(str) {
+CylindricalJoint::CylindricalJoint(const char* str) : InLineJoint(str) 
+{
 }
 
 void CylindricalJoint::initializeGlobally()
 {
 	if (constraints->empty())
 	{
-		addConstraint(CREATE<TranslationConstraintIJ>::ConstraintWith(frmI, frmJ, 0));
-		addConstraint(CREATE<TranslationConstraintIJ>::ConstraintWith(frmI, frmJ, 1));
+		createInLineConstraints();
 		addConstraint(CREATE<DirectionCosineConstraintIJ>::ConstraintWith(frmI, frmJ, 2, 0));
 		addConstraint(CREATE<DirectionCosineConstraintIJ>::ConstraintWith(frmI, frmJ, 2, 1));
 		this->root()->hasChanged = true;
 	}
 	else {
-		Joint::initializeGlobally();
+		InLineJoint::initializeGlobally();
 	}
 }

MbDCode/CylindricalJoint.h

@@ -1,8 +1,9 @@
 #pragma once
-#include "Joint.h"
+
+#include "InLineJoint.h"
 
 namespace MbD {
-    class CylindricalJoint : public Joint
+    class CylindricalJoint : public InLineJoint
     {
         //frmI frmJ constraints friction 
     public:

MbDCode/DiagonalMatrix.h

@@ -13,6 +13,7 @@ namespace MbD {
 		DiagonalMatrix(int count) : Array<T>(count) {}
 		DiagonalMatrix(int count, const T& value) : Array<T>(count, value) {}
 		DiagonalMatrix(std::initializer_list<T> list) : Array<T>{ list } {}
+		void atiput(int i, T value);
 		void atiputDiagonalMatrix(int i, std::shared_ptr < DiagonalMatrix<T>> diagMat);
 		std::shared_ptr<DiagonalMatrix<T>> times(T factor);
 		std::shared_ptr<FullColumn<T>> timesFullColumn(std::shared_ptr<FullColumn<T>> fullCol);
@@ -30,6 +31,11 @@ namespace MbD {
 
 	};
 	template<typename T>
+	inline void DiagonalMatrix<T>::atiput(int i, T value)
+	{
+		this->at(i) = value;
+	}
+	template<typename T>
 	inline void DiagonalMatrix<T>::atiputDiagonalMatrix(int i, std::shared_ptr<DiagonalMatrix<T>> diagMat)
 	{
 		for (int ii = 0; ii < diagMat->size(); ii++)
@@ -108,5 +114,6 @@ namespace MbD {
 		s << "]";
 		return s;
 	}
+	using DiagMatDsptr = std::shared_ptr<DiagonalMatrix<double>>;
 }
 

MbDCode/DirectionCosineIecJec.h

@@ -1,4 +1,5 @@
 #pragma once
+
 #include <memory>
 
 #include "KinematicIeJe.h"

MbDCode/DiscontinuityError.h

@@ -1,4 +1,5 @@
 #pragma once
+
 #include <stdexcept>
 #include <memory>
 #include <vector>

MbDCode/DispCompIecJecIe.cpp

@@ -9,3 +9,20 @@ MbD::DispCompIecJecIe::DispCompIecJecIe()
 MbD::DispCompIecJecIe::DispCompIecJecIe(EndFrmcptr frmi, EndFrmcptr frmj, int axis) : KinematicIeJe(frmi, frmj), axis(axis)
 {
 }
+
+void MbD::DispCompIecJecIe::calc_value()
+{
+	aAjOIe = frmI->aAjOe(axis);
+	rIeJeO = frmJ->rOeO->minusFullColumn(frmI->rOeO);
+	riIeJeIe = aAjOIe->dot(rIeJeO);
+}
+
+void MbD::DispCompIecJecIe::calcPostDynCorrectorIteration()
+{
+	calc_value();
+}
+
+double MbD::DispCompIecJecIe::value()
+{
+	return riIeJeIe;
+}

MbDCode/DispCompIecJecIe.h

@@ -10,6 +10,10 @@ namespace MbD {
         DispCompIecJecIe();
         DispCompIecJecIe(EndFrmcptr frmi, EndFrmcptr frmj, int axis);
 
+        void calc_value() override;
+        void calcPostDynCorrectorIteration() override;
+        double value() override;
+
         int axis;
         double riIeJeIe;
         FColDsptr aAjOIe, rIeJeO;

MbDCode/DispCompIeqcJecIe.cpp

@@ -1,4 +1,7 @@
 #include "DispCompIeqcJecIe.h"
+#include "EndFrameqc.h"
+
+using namespace MbD;
 
 MbD::DispCompIeqcJecIe::DispCompIeqcJecIe()
 {
@@ -7,3 +10,101 @@ MbD::DispCompIeqcJecIe::DispCompIeqcJecIe()
 MbD::DispCompIeqcJecIe::DispCompIeqcJecIe(EndFrmcptr frmi, EndFrmcptr frmj, int axis) : DispCompIecJecIe(frmi, frmj, axis)
 {
 }
+
+void MbD::DispCompIeqcJecIe::calc_ppvaluepEIpEI()
+{
+	auto frmIeqc = std::static_pointer_cast<EndFrameqc>(frmI);
+	auto mprIeJeOpEIT = frmIeqc->prOeOpE->transpose();
+	auto mpprIeJeOpEIpEI = frmIeqc->pprOeOpEpE;
+	for (int i = 0; i < 4; i++)
+	{
+		auto ppAjOIepEIipEI = ppAjOIepEIpEI->at(i);
+		auto mpprIeJeOpEIipEI = mpprIeJeOpEIpEI->at(i);
+		auto ppriIeJeIepEIipEI = ppriIeJeIepEIpEI->at(i);
+		for (int j = i; j < 4; j++)
+		{
+			auto term1 = ppAjOIepEIipEI->at(j)->dot(rIeJeO);
+			auto mterm2 = pAjOIepEIT->at(i)->dot(mprIeJeOpEIT->at(j));
+			auto mterm3 = (i == j) ? mterm2 : pAjOIepEIT->at(j)->dot(mprIeJeOpEIT->at(i));
+			auto mterm4 = aAjOIe->dot(mpprIeJeOpEIipEI->at(j));
+			ppriIeJeIepEIipEI->atiput(j, term1 - mterm2 - mterm3 - mterm4);
+		}
+	}
+	ppriIeJeIepEIpEI->symLowerWithUpper();
+}
+
+void MbD::DispCompIeqcJecIe::calc_ppvaluepXIpEI()
+{
+	for (int i = 0; i < 3; i++)
+	{
+		auto ppriIeJeIepXIipEI = ppriIeJeIepXIpEI->at(i);
+		for (int j = 0; j < 4; j++)
+		{
+			ppriIeJeIepXIipEI->atiput(j, -pAjOIepEIT->at(j)->at(i));
+		}
+	}
+}
+
+void MbD::DispCompIeqcJecIe::calc_pvaluepEI()
+{
+	auto frmIeqc = std::static_pointer_cast<EndFrameqc>(frmI);
+	pAjOIepEIT = frmIeqc->pAjOepET(axis);
+	auto mprIeJeOpEIT = frmIeqc->prOeOpE->transpose();
+	for (int i = 0; i < 4; i++)
+	{
+		priIeJeIepEI->atiput(i, pAjOIepEIT->at(i)->dot(rIeJeO) - aAjOIe->dot(mprIeJeOpEIT->at(i)));
+	}
+}
+
+void MbD::DispCompIeqcJecIe::calc_pvaluepXI()
+{
+	for (int i = 0; i < 3; i++)
+	{
+		priIeJeIepXI->atiput(i, -aAjOIe->at(i));
+	}
+}
+
+void MbD::DispCompIeqcJecIe::calcPostDynCorrectorIteration()
+{
+	//Must maintain order of calc_xxx.
+	DispCompIecJecIe::calcPostDynCorrectorIteration();
+	calc_pvaluepXI();
+	calc_pvaluepEI();
+	calc_ppvaluepXIpEI();
+	calc_ppvaluepEIpEI();
+}
+
+void MbD::DispCompIeqcJecIe::initialize()
+{
+	DispCompIecJecIe::initialize();
+	priIeJeIepXI = std::make_shared<FullRow<double>>(3);
+	priIeJeIepEI = std::make_shared<FullRow<double>>(4);
+	ppriIeJeIepXIpEI = std::make_shared<FullMatrix<double>>(3, 4);
+	ppriIeJeIepEIpEI = std::make_shared<FullMatrix<double>>(4, 4);
+}
+
+void MbD::DispCompIeqcJecIe::initializeGlobally()
+{
+	auto frmIeqc = std::static_pointer_cast<EndFrameqc>(frmI);
+	ppAjOIepEIpEI = frmIeqc->ppAjOepEpE(axis);
+}
+
+FMatDsptr MbD::DispCompIeqcJecIe::ppvaluepEIpEI()
+{
+	return ppriIeJeIepEIpEI;
+}
+
+FMatDsptr MbD::DispCompIeqcJecIe::ppvaluepXIpEI()
+{
+	return ppriIeJeIepXIpEI;
+}
+
+FRowDsptr MbD::DispCompIeqcJecIe::pvaluepEI()
+{
+	return priIeJeIepEI;
+}
+
+FRowDsptr MbD::DispCompIeqcJecIe::pvaluepXI()
+{
+	return priIeJeIepXI;
+}

MbDCode/DispCompIeqcJecIe.h

@@ -10,6 +10,18 @@ namespace MbD {
         DispCompIeqcJecIe();
         DispCompIeqcJecIe(EndFrmcptr frmi, EndFrmcptr frmj, int axis);
 
+        void calc_ppvaluepEIpEI() override;
+        void calc_ppvaluepXIpEI() override;
+        void calc_pvaluepEI() override;
+        void calc_pvaluepXI() override;
+        void calcPostDynCorrectorIteration() override;
+        void initialize() override;
+        void initializeGlobally() override;
+        FMatDsptr ppvaluepEIpEI() override;
+        FMatDsptr ppvaluepXIpEI() override;
+        FRowDsptr pvaluepEI() override;
+        FRowDsptr pvaluepXI() override;
+
         FRowDsptr priIeJeIepXI, priIeJeIepEI;
         FMatDsptr ppriIeJeIepXIpEI, ppriIeJeIepEIpEI, pAjOIepEIT;
         FMatFColDsptr ppAjOIepEIpEI;

MbDCode/DispCompIeqcJeqcIe.cpp

@@ -1,4 +1,7 @@
 #include "DispCompIeqcJeqcIe.h"
+#include "EndFrameqc.h"
+
+using namespace MbD;
 
 MbD::DispCompIeqcJeqcIe::DispCompIeqcJeqcIe()
 {
@@ -7,3 +10,97 @@ MbD::DispCompIeqcJeqcIe::DispCompIeqcJeqcIe()
 MbD::DispCompIeqcJeqcIe::DispCompIeqcJeqcIe(EndFrmcptr frmi, EndFrmcptr frmj, int axis) : DispCompIeqcJecIe(frmi, frmj, axis)
 {
 }
+
+void MbD::DispCompIeqcJeqcIe::calc_ppvaluepEIpEJ()
+{
+	auto frmJeqc = std::static_pointer_cast<EndFrameqc>(frmJ);
+	auto& prIeJeOpEJ = frmJeqc->prOeOpE;
+	ppriIeJeIepEIpEJ = pAjOIepEIT->timesFullMatrix(prIeJeOpEJ);
+}
+
+void MbD::DispCompIeqcJeqcIe::calc_ppvaluepEIpXJ()
+{
+	ppriIeJeIepEIpXJ = pAjOIepEIT;
+}
+
+void MbD::DispCompIeqcJeqcIe::calc_ppvaluepEJpEJ()
+{
+	auto frmJeqc = std::static_pointer_cast<EndFrameqc>(frmJ);
+	auto pprIeJeOpEJpEJ = frmJeqc->pprOeOpEpE;
+	for (int i = 0; i < 4; i++)
+	{
+		auto pprIeJeOpEJipEJ = pprIeJeOpEJpEJ->at(i);
+		auto ppriIeJeIepEJipEJ = ppriIeJeIepEJpEJ->at(i);
+		for (int j = i; j < 4; j++)
+		{
+			auto term1 = aAjOIe->dot(pprIeJeOpEJipEJ->at(j));
+			ppriIeJeIepEJipEJ->atiput(j, term1);
+		}
+	}
+	ppriIeJeIepEJpEJ->symLowerWithUpper();
+}
+
+void MbD::DispCompIeqcJeqcIe::calc_pvaluepEJ()
+{
+	auto frmJeqc = std::static_pointer_cast<EndFrameqc>(frmJ);
+	auto prIeJeOpEJT = frmJeqc->prOeOpE->transpose();
+	for (int i = 0; i < 4; i++)
+	{
+		priIeJeIepEJ->atiput(i, aAjOIe->dot(prIeJeOpEJT->at(i)));
+	}
+}
+
+void MbD::DispCompIeqcJeqcIe::calc_pvaluepXJ()
+{
+	for (int i = 0; i < 3; i++)
+	{
+		priIeJeIepXJ->atiput(i, aAjOIe->at(i));
+	}
+}
+
+void MbD::DispCompIeqcJeqcIe::calcPostDynCorrectorIteration()
+{
+	//Must maintain order of calc_xxx.
+	DispCompIeqcJecIe::calcPostDynCorrectorIteration();
+	calc_pvaluepXJ();
+	calc_pvaluepEJ();
+	calc_ppvaluepEIpXJ();
+	calc_ppvaluepEIpEJ();
+	calc_ppvaluepEJpEJ();
+
+}
+
+void MbD::DispCompIeqcJeqcIe::initialize()
+{
+	DispCompIeqcJecIe::initialize();
+	priIeJeIepXJ = std::make_shared<FullRow<double>>(3);
+	priIeJeIepEJ = std::make_shared<FullRow<double>>(4);
+	ppriIeJeIepEIpXJ = std::make_shared<FullMatrix<double>>(4, 3);
+	ppriIeJeIepEIpEJ = std::make_shared<FullMatrix<double>>(4, 4);
+	ppriIeJeIepEJpEJ = std::make_shared<FullMatrix<double>>(4, 4);
+}
+
+FMatDsptr MbD::DispCompIeqcJeqcIe::ppvaluepEIpEJ()
+{
+	return ppriIeJeIepEIpEJ;
+}
+
+FMatDsptr MbD::DispCompIeqcJeqcIe::ppvaluepEIpXJ()
+{
+	return ppriIeJeIepEIpXJ;
+}
+
+FMatDsptr MbD::DispCompIeqcJeqcIe::ppvaluepEJpEJ()
+{
+	return ppriIeJeIepEJpEJ;
+}
+
+FRowDsptr MbD::DispCompIeqcJeqcIe::pvaluepEJ()
+{
+	return priIeJeIepEJ;
+}
+
+FRowDsptr MbD::DispCompIeqcJeqcIe::pvaluepXJ()
+{
+	return priIeJeIepXJ;
+}

MbDCode/DispCompIeqcJeqcIe.h

@@ -10,6 +10,19 @@ namespace MbD {
         DispCompIeqcJeqcIe();
         DispCompIeqcJeqcIe(EndFrmcptr frmi, EndFrmcptr frmj, int axis);
 
+        void calc_ppvaluepEIpEJ() override;
+        void calc_ppvaluepEIpXJ() override;
+        void calc_ppvaluepEJpEJ() override;
+        void calc_pvaluepEJ() override;
+        void calc_pvaluepXJ() override;
+        void calcPostDynCorrectorIteration() override;
+        void initialize() override;
+        FMatDsptr ppvaluepEIpEJ() override;
+        FMatDsptr ppvaluepEIpXJ() override;
+        FMatDsptr ppvaluepEJpEJ() override;
+        FRowDsptr pvaluepEJ() override;
+        FRowDsptr pvaluepXJ() override;
+
         FRowDsptr priIeJeIepXJ, priIeJeIepEJ;
         FMatDsptr ppriIeJeIepEIpXJ, ppriIeJeIepEIpEJ, ppriIeJeIepEJpEJ;
     };

MbDCode/DispCompIeqctJeqcIe.cpp

@@ -1,4 +1,7 @@
 #include "DispCompIeqctJeqcIe.h"
+#include "EndFrameqct.h"
+
+using namespace MbD;
 
 MbD::DispCompIeqctJeqcIe::DispCompIeqctJeqcIe()
 {
@@ -7,3 +10,140 @@ MbD::DispCompIeqctJeqcIe::DispCompIeqctJeqcIe()
 MbD::DispCompIeqctJeqcIe::DispCompIeqctJeqcIe(EndFrmcptr frmi, EndFrmcptr frmj, int axis) : DispCompIeqcJeqcIe(frmi, frmj, axis)
 {
 }
+
+void MbD::DispCompIeqctJeqcIe::calc_ppvaluepEIpt()
+{
+	auto frmIeqct = std::static_pointer_cast<EndFrameqct>(frmI);
+	auto pAjOIept = frmIeqct->pAjOept(axis);
+	auto ppAjOIepEITpt = frmIeqct->ppAjOepETpt(axis);
+	auto mprIeJeOpEIT = frmIeqct->prOeOpE->transpose();
+	auto mpprIeJeOpEITpt = frmIeqct->pprOeOpEpt->transpose();
+	auto mprIeJeOpt = frmIeqct->prOeOpt;
+	for (int i = 0; i < 4; i++)
+	{
+		auto ppAjOIepEITpti = ppAjOIepEITpt->at(i);
+		auto pAjOIepEITi = pAjOIepEIT->at(i);
+		auto mprIeJeOpEITi = mprIeJeOpEIT->at(i);
+		auto mpprIeJeOpEITpti = mpprIeJeOpEITpt->at(i);
+		auto ppriIeJeIepEIpti = ppAjOIepEITpti->dot(rIeJeO) - pAjOIepEITi->dot(mprIeJeOpt) - pAjOIept->dot(mprIeJeOpEITi) - aAjOIe->dot(mpprIeJeOpEITpti);
+		ppriIeJeIepEIpt->atiput(i, ppriIeJeIepEIpti);
+	}
+}
+
+void MbD::DispCompIeqctJeqcIe::calc_ppvaluepEJpt()
+{
+	auto frmIeqct = std::static_pointer_cast<EndFrameqct>(frmI);
+	auto frmJeqct = std::static_pointer_cast<EndFrameqct>(frmJ);
+	auto pAjOIept = frmIeqct->pAjOept(axis);
+	auto prIeJeOpEJT = frmJeqct->prOeOpE->transpose();
+	for (int i = 0; i < 4; i++)
+	{
+		ppriIeJeIepEJpt->atiput(i, pAjOIept->dot(prIeJeOpEJT->at(i)));
+	}
+}
+
+void MbD::DispCompIeqctJeqcIe::calc_ppvalueptpt()
+{
+	auto frmIeqct = std::static_pointer_cast<EndFrameqct>(frmI);
+	auto pAjOIept = frmIeqct->pAjOept(axis);
+	auto ppAjOIeptpt = frmIeqct->ppAjOeptpt(axis);
+	auto mprIeJeOpt = frmIeqct->prOeOpt;
+	auto mpprIeJeOptpt = frmIeqct->pprOeOptpt;
+	ppriIeJeIeptpt = ppAjOIeptpt->dot(rIeJeO) - pAjOIept->dot(mprIeJeOpt) - pAjOIept->dot(mprIeJeOpt) - aAjOIe->dot(mpprIeJeOptpt);
+}
+
+void MbD::DispCompIeqctJeqcIe::calc_ppvaluepXIpt()
+{
+	auto frmIeqct = std::static_pointer_cast<EndFrameqct>(frmI);
+	auto pAjOIept = frmIeqct->pAjOept(axis);
+	for (int i = 0; i < 3; i++)
+	{
+		ppriIeJeIepXIpt->atiput(i, -pAjOIept->at(i));
+	}
+}
+
+void MbD::DispCompIeqctJeqcIe::calc_ppvaluepXJpt()
+{
+	auto frmIeqct = std::static_pointer_cast<EndFrameqct>(frmI);
+	auto pAjOIept = frmIeqct->pAjOept(axis);
+	for (int i = 0; i < 3; i++)
+	{
+		ppriIeJeIepXJpt->atiput(i, pAjOIept->at(i));
+	}
+}
+
+void MbD::DispCompIeqctJeqcIe::calc_pvaluept()
+{
+	auto frmIeqct = std::static_pointer_cast<EndFrameqct>(frmI);
+	auto pAjOIept = frmIeqct->pAjOept(axis);
+	auto mprIeJeOpt = frmIeqct->prOeOpt;
+	priIeJeIept = pAjOIept->dot(rIeJeO) - aAjOIe->dot(mprIeJeOpt);
+}
+
+void MbD::DispCompIeqctJeqcIe::calcPostDynCorrectorIteration()
+{
+	//"ppAjOIepEIpEI is not longer constant and must be set before any calculation."
+	auto frmIeqct = std::static_pointer_cast<EndFrameqct>(frmI);
+	ppAjOIepEIpEI = frmIeqct->ppAjOepEpE(axis);
+	DispCompIeqcJeqcIe::calcPostDynCorrectorIteration();
+}
+
+void MbD::DispCompIeqctJeqcIe::initialize()
+{
+	DispCompIeqcJeqcIe::initialize();
+	ppriIeJeIepXIpt = std::make_shared<FullRow<double>>(3);
+	ppriIeJeIepEIpt = std::make_shared<FullRow<double>>(4);
+	ppriIeJeIepXJpt = std::make_shared<FullRow<double>>(3);
+	ppriIeJeIepEJpt = std::make_shared<FullRow<double>>(4);
+}
+
+void MbD::DispCompIeqctJeqcIe::initializeGlobally()
+{
+	//"Do nothing."
+}
+
+void MbD::DispCompIeqctJeqcIe::preAccIC()
+{
+	DispCompIeqcJeqcIe::preAccIC();
+	calc_ppvaluepXIpt();
+	calc_ppvaluepEIpt();
+	calc_ppvaluepXJpt();
+	calc_ppvaluepEJpt();
+	calc_ppvalueptpt();
+}
+
+void MbD::DispCompIeqctJeqcIe::preVelIC()
+{
+	DispCompIeqcJeqcIe::preVelIC();
+	calc_pvaluept();
+}
+
+FRowDsptr MbD::DispCompIeqctJeqcIe::ppvaluepEIpt()
+{
+	return ppriIeJeIepEIpt;
+}
+
+FRowDsptr MbD::DispCompIeqctJeqcIe::ppvaluepEJpt()
+{
+	return ppriIeJeIepEJpt;
+}
+
+double MbD::DispCompIeqctJeqcIe::ppvalueptpt()
+{
+	return ppriIeJeIeptpt;
+}
+
+FRowDsptr MbD::DispCompIeqctJeqcIe::ppvaluepXIpt()
+{
+	return ppriIeJeIepXIpt;
+}
+
+FRowDsptr MbD::DispCompIeqctJeqcIe::ppvaluepXJpt()
+{
+	return ppriIeJeIepXJpt;
+}
+
+double MbD::DispCompIeqctJeqcIe::pvaluept()
+{
+	return priIeJeIept;
+}

MbDCode/DispCompIeqctJeqcIe.h

@@ -10,6 +10,24 @@ namespace MbD {
 		DispCompIeqctJeqcIe();
 		DispCompIeqctJeqcIe(EndFrmcptr frmi, EndFrmcptr frmj, int axis);
 
+		void calc_ppvaluepEIpt() override;
+		void calc_ppvaluepEJpt() override;
+		void calc_ppvalueptpt() override;
+		void calc_ppvaluepXIpt() override;
+		void calc_ppvaluepXJpt() override;
+		void calc_pvaluept() override;
+		void calcPostDynCorrectorIteration() override;
+		void initialize() override;
+		void initializeGlobally() override;
+		void preAccIC() override;
+		void preVelIC() override;
+		FRowDsptr ppvaluepEIpt() override;
+		FRowDsptr ppvaluepEJpt() override;
+		double ppvalueptpt() override;
+		FRowDsptr ppvaluepXIpt() override;
+		FRowDsptr ppvaluepXJpt() override;
+		double pvaluept() override;
+
 		double priIeJeIept, ppriIeJeIeptpt;
 		FRowDsptr ppriIeJeIepXIpt, ppriIeJeIepEIpt, ppriIeJeIepXJpt, ppriIeJeIepEJpt;
 	};

MbDCode/DistIecJec.cpp

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

MbDCode/DistanceConstraintIJ.cpp

@@ -11,6 +11,11 @@ void MbD::DistanceConstraintIJ::calcPostDynCorrectorIteration()
 aG = distIeJe->value() - aConstant;
 }
 
+void MbD::DistanceConstraintIJ::init_distIeJe()
+{
+	assert(false);
+}
+
 void MbD::DistanceConstraintIJ::initialize()
 {
 	ConstraintIJ::initialize();

MbDCode/DistanceConstraintIJ.h

@@ -11,7 +11,7 @@ namespace MbD {
         DistanceConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj);
 
         void calcPostDynCorrectorIteration() override;
-        virtual void init_distIeJe() = 0;
+        virtual void init_distIeJe();
         void initialize() override;
         void initializeGlobally() override;
         void initializeLocally() override;

MbDCode/DistanceConstraintIqcJc.cpp

@@ -3,6 +3,8 @@
 #include "CREATE.h"
 #include "DistIeqcJec.h"
 
+using namespace MbD;
+
 MbD::DistanceConstraintIqcJc::DistanceConstraintIqcJc(EndFrmcptr frmi, EndFrmcptr frmj) : DistanceConstraintIJ(frmi, frmj)
 {
 }

MbDCode/DistanceConstraintIqcJqc.cpp

@@ -3,6 +3,8 @@
 #include "CREATE.h"
 #include "DistIeqcJeqc.h"
 
+using namespace MbD;
+
 MbD::DistanceConstraintIqcJqc::DistanceConstraintIqcJqc(EndFrmcptr frmi, EndFrmcptr frmj) : DistanceConstraintIqcJc(frmi, frmj)
 {
 }

MbDCode/DistanceConstraintIqctJqc.cpp

@@ -1,6 +1,13 @@
 #include "DistanceConstraintIqctJqc.h"
 
+using namespace MbD;
+
 MbD::DistanceConstraintIqctJqc::DistanceConstraintIqctJqc(EndFrmcptr frmi, EndFrmcptr frmj) : DistanceConstraintIqcJqc(frmi, frmj)
 {
 	assert(false);
 }
+
+ConstraintType MbD::DistanceConstraintIqctJqc::type()
+{
+	return essential;
+}

MbDCode/DistanceConstraintIqctJqc.h

@@ -8,6 +8,7 @@ namespace MbD {
         //pGpt ppGpXIpt ppGpEIpt ppGpXJpt ppGpEJpt ppGptpt 
     public:
         DistanceConstraintIqctJqc(EndFrmcptr frmi, EndFrmcptr frmj);
+        ConstraintType type() override;
 
         double pGpt, ppGptpt;
         FRowDsptr ppGpXIpt, ppGpEIpt, ppGpXJpt, ppGpEJpt;

MbDCode/DistancexyConstraintIJ.cpp

@@ -13,6 +13,11 @@ void MbD::DistancexyConstraintIJ::calcPostDynCorrectorIteration()
 	aG = x * x + (y * y) - (aConstant * aConstant);
 }
 
+void MbD::DistancexyConstraintIJ::init_xyIeJeIe()
+{
+	assert(false);
+}
+
 void MbD::DistancexyConstraintIJ::initialize()
 {
 	ConstraintIJ::initialize();
@@ -75,5 +80,5 @@ void MbD::DistancexyConstraintIJ::simUpdateAll()
 
 ConstraintType MbD::DistancexyConstraintIJ::type()
 {
-	return ConstraintType::displacement;
+	return displacement;
 }

MbDCode/DistancexyConstraintIJ.h

@@ -11,7 +11,7 @@ namespace MbD {
         DistancexyConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj);
 
         void calcPostDynCorrectorIteration() override;
-        virtual void init_xyIeJeIe() = 0;
+        virtual void init_xyIeJeIe();
         void initialize() override;
         void initializeGlobally() override;
         void initializeLocally() override;
@@ -26,6 +26,7 @@ namespace MbD {
 
 
         std::shared_ptr<DispCompIecJecIe> xIeJeIe, yIeJeIe;
+        //ToDo: Use DistxyIecJec instead of xIeJeIe, yIeJeIe
 
     };
 }

MbDCode/DistancexyConstraintIqcJc.h

@@ -17,12 +17,12 @@ namespace MbD {
 		void calc_ppGpXIpEI();
 		void calc_ppGpXIpXI();
 		void calcPostDynCorrectorIteration() override;
-		void init_xyIeJeIe() override;
 		void fillAccICIterError(FColDsptr col) override;
 		void fillPosICError(FColDsptr col) override;
 		void fillPosICJacob(SpMatDsptr mat) override;
 		void fillPosKineJacob(SpMatDsptr mat) override;
 		void fillVelICJacob(SpMatDsptr mat) override;
+		void init_xyIeJeIe() override;
 		void useEquationNumbers() override;
 
 

MbDCode/DistxyIecJec.cpp

@@ -0,0 +1,88 @@
+#include "DistxyIecJec.h"
+
+using namespace MbD;
+
+MbD::DistxyIecJec::DistxyIecJec()
+{
+}
+
+MbD::DistxyIecJec::DistxyIecJec(EndFrmcptr frmi, EndFrmcptr frmj) : KinematicIeJe(frmi, frmj)
+{
+}
+
+void MbD::DistxyIecJec::calcPostDynCorrectorIteration()
+{
+	auto x = xIeJeIe->value();
+	auto y = yIeJeIe->value();
+	distxy = std::sqrt(x * x + (y * y));
+}
+
+void MbD::DistxyIecJec::initialize()
+{
+	KinematicIeJe::initialize();
+	this->init_xyIeJeIe();
+}
+
+void MbD::DistxyIecJec::initializeGlobally()
+{
+	xIeJeIe->initializeGlobally();
+	yIeJeIe->initializeGlobally();
+}
+
+void MbD::DistxyIecJec::initializeLocally()
+{
+	xIeJeIe->initializeLocally();
+	yIeJeIe->initializeLocally();
+}
+
+void MbD::DistxyIecJec::init_xyIeJeIe()
+{
+	assert(false);
+}
+
+void MbD::DistxyIecJec::postInput()
+{
+	xIeJeIe->postInput();
+	yIeJeIe->postInput();
+	KinematicIeJe::postInput();
+}
+
+void MbD::DistxyIecJec::postPosICIteration()
+{
+	xIeJeIe->postPosICIteration();
+	yIeJeIe->postPosICIteration();
+	KinematicIeJe::postPosICIteration();
+}
+
+void MbD::DistxyIecJec::preAccIC()
+{
+	xIeJeIe->preAccIC();
+	yIeJeIe->preAccIC();
+	KinematicIeJe::preAccIC();
+}
+
+void MbD::DistxyIecJec::prePosIC()
+{
+	xIeJeIe->prePosIC();
+	yIeJeIe->prePosIC();
+	KinematicIeJe::prePosIC();
+}
+
+void MbD::DistxyIecJec::preVelIC()
+{
+	xIeJeIe->preVelIC();
+	yIeJeIe->preVelIC();
+	KinematicIeJe::preVelIC();
+}
+
+void MbD::DistxyIecJec::simUpdateAll()
+{
+	xIeJeIe->simUpdateAll();
+	yIeJeIe->simUpdateAll();
+	KinematicIeJe::simUpdateAll();
+}
+
+double MbD::DistxyIecJec::value()
+{
+	return distxy;
+}

MbDCode/DistxyIecJec.h

@@ -0,0 +1,32 @@
+#pragma once
+
+#include "KinematicIeJe.h"
+#include "DispCompIecJecIe.h"
+
+namespace MbD {
+	class DistxyIecJec : public KinematicIeJe
+	{
+		//distxy xIeJeIe yIeJeIe
+	public:
+        DistxyIecJec();
+        DistxyIecJec(EndFrmcptr frmi, EndFrmcptr frmj);
+
+        void calcPostDynCorrectorIteration() override;
+        void initialize() override;
+        void initializeGlobally() override;
+        void initializeLocally() override;
+        virtual void init_xyIeJeIe();
+        void postInput() override;
+        void postPosICIteration() override;
+        void preAccIC() override;
+        void prePosIC() override;
+        void preVelIC() override;
+        void simUpdateAll() override;
+        double value() override;
+
+        double distxy;
+        std::shared_ptr<DispCompIecJecIe> xIeJeIe, yIeJeIe;
+    
+    };
+}
+

MbDCode/DistxyIeqcJec.cpp

@@ -0,0 +1,184 @@
+#include "DistxyIeqcJec.h"
+#include "CREATE.h"
+#include "DispCompIeqcJecIe.h"
+
+using namespace MbD;
+
+MbD::DistxyIeqcJec::DistxyIeqcJec()
+{
+}
+
+MbD::DistxyIeqcJec::DistxyIeqcJec(EndFrmcptr frmi, EndFrmcptr frmj) : DistxyIecJec(frmi, frmj)
+{
+}
+
+void MbD::DistxyIeqcJec::calc_ppdistxypEIpEI()
+{
+	auto x = xIeJeIe->value();
+	auto y = yIeJeIe->value();
+	auto pxpEI = xIeJeIe->pvaluepEI();
+	auto pypEI = yIeJeIe->pvaluepEI();
+	auto ppxpEIpEI = xIeJeIe->ppvaluepEIpEI();
+	auto ppypEIpEI = yIeJeIe->ppvaluepEIpEI();
+	for (int i = 0; i < 4; i++)
+	{
+		auto ppdistxypEIpEIi = ppdistxypEIpEI->at(i);
+		auto pdistxypEIi = pdistxypEI->at(i);
+		auto ppxpEIpEIi = ppxpEIpEI->at(i);
+		auto ppypEIpEIi = ppypEIpEI->at(i);
+		auto pxpEIi = pxpEI->at(i);
+		auto pypEIi = pypEI->at(i);
+		for (int j = i; j < 4; j++)
+		{
+			auto pdistxypEIj = pdistxypEI->at(j);
+			auto pxpEIj = pxpEI->at(j);
+			auto pypEIj = pypEI->at(j);
+			auto term1 = -pdistxypEIi * pdistxypEIj;
+			auto term2 = ppxpEIpEIi->at(j) * x + ppypEIpEIi->at(j) * y;
+			auto term3 = pxpEIi * pxpEIj + pypEIi * pypEIj;
+			auto ppdistxypEIpEIij = (term1 + term2 + term3) / distxy;
+			ppdistxypEIpEIi->atiput(j, ppdistxypEIpEIij);
+			if (i < j) ppdistxypEIpEI->atijput(j, i, ppdistxypEIpEIij);
+		}
+	}
+}
+
+void MbD::DistxyIeqcJec::calc_ppdistxypXIpEI()
+{
+	auto x = xIeJeIe->value();
+	auto y = yIeJeIe->value();
+	auto pxpXI = xIeJeIe->pvaluepXI();
+	auto pypXI = yIeJeIe->pvaluepXI();
+	auto pxpEI = xIeJeIe->pvaluepEI();
+	auto pypEI = yIeJeIe->pvaluepEI();
+	auto ppxpXIpEI = xIeJeIe->ppvaluepXIpEI();
+	auto ppypXIpEI = yIeJeIe->ppvaluepXIpEI();
+	for (int i = 0; i < 3; i++)
+	{
+		auto ppdistxypXIpEIi = ppdistxypXIpEI->at(i);
+		auto pdistxypXIi = pdistxypXI->at(i);
+		auto ppxpXIpEIi = ppxpXIpEI->at(i);
+		auto ppypXIpEIi = ppypXIpEI->at(i);
+		auto pxpXIi = pxpXI->at(i);
+		auto pypXIi = pypXI->at(i);
+		for (int j = 0; j < 4; j++)
+		{
+			auto pdistxypEIj = pdistxypEI->at(j);
+			auto pxpEIj = pxpEI->at(j);
+			auto pypEIj = pypEI->at(j);
+			auto term1 = -pdistxypXIi * pdistxypEIj;
+			auto term2 = ppxpXIpEIi->at(j) * x + ppypXIpEIi->at(j) * y;
+			auto term3 = pxpXIi * pxpEIj + pypXIi * pypEIj;
+			auto ppdistxypXIpEIij = (term1 + term2 + term3) / distxy;
+			ppdistxypXIpEIi->atiput(j, ppdistxypXIpEIij);
+		}
+	}
+}
+
+void MbD::DistxyIeqcJec::calc_ppdistxypXIpXI()
+{
+	auto x = xIeJeIe->value();
+	auto y = yIeJeIe->value();
+	auto pxpXI = xIeJeIe->pvaluepXI();
+	auto pypXI = yIeJeIe->pvaluepXI();
+	auto ppxpXIpXI = xIeJeIe->ppvaluepXIpXI();
+	auto ppypXIpXI = yIeJeIe->ppvaluepXIpXI();
+	for (int i = 0; i < 3; i++)
+	{
+		auto ppdistxypXIpXIi = ppdistxypXIpXI->at(i);
+		auto pdistxypXIi = pdistxypXI->at(i);
+		auto ppxpXIpXIi = ppxpXIpXI->at(i);
+		auto ppypXIpXIi = ppypXIpXI->at(i);
+		auto pxpXIi = pxpXI->at(i);
+		auto pypXIi = pypXI->at(i);
+		for (int j = i; j < 3; j++)
+		{
+			auto pdistxypXIj = pdistxypXI->at(j);
+			auto pxpXIj = pxpXI->at(j);
+			auto pypXIj = pypXI->at(j);
+			auto term1 = -pdistxypXIi * pdistxypXIj;
+			auto term2 = ppxpXIpXIi->at(j) * x + ppypXIpXIi->at(j) * y;
+			auto term3 = pxpXIi * pxpXIj + pypXIi * pypXIj;
+			auto ppdistxypXIpXIij = (term1 + term2 + term3) / distxy;
+			ppdistxypXIpXIi->atiput(j, ppdistxypXIpXIij);
+			if (i < j) ppdistxypXIpXI->atijput(j, i, ppdistxypXIpXIij);
+		}
+	}
+}
+
+void MbD::DistxyIeqcJec::calc_pdistxypEI()
+{
+	auto x = xIeJeIe->value();
+	auto y = yIeJeIe->value();
+	auto pxpEI = xIeJeIe->pvaluepEI();
+	auto pypEI = yIeJeIe->pvaluepEI();
+	for (int i = 0; i < 4; i++)
+	{
+		auto term = pxpEI->at(i) * x + pypEI->at(i) * y;
+		pdistxypEI->atiput(i, term / distxy);
+	}
+}
+
+void MbD::DistxyIeqcJec::calc_pdistxypXI()
+{
+	auto x = xIeJeIe->value();
+	auto y = yIeJeIe->value();
+	auto pxpXI = xIeJeIe->pvaluepXI();
+	auto pypXI = yIeJeIe->pvaluepXI();
+	for (int i = 0; i < 3; i++)
+	{
+		auto term = pxpXI->at(i) * x + pypXI->at(i) * y;
+		pdistxypXI->atiput(i, term / distxy);
+	}
+}
+
+void MbD::DistxyIeqcJec::calcPostDynCorrectorIteration()
+{
+	DistxyIeqcJec::calcPostDynCorrectorIteration();
+	this->calc_pdistxypXI();
+	this->calc_pdistxypEI();
+	this->calc_ppdistxypXIpXI();
+	this->calc_ppdistxypXIpEI();
+	this->calc_ppdistxypEIpEI();
+}
+
+void MbD::DistxyIeqcJec::init_xyIeJeIe()
+{
+	xIeJeIe = CREATE<DispCompIeqcJecIe>::With(frmI, frmJ, 0);
+	yIeJeIe = CREATE<DispCompIeqcJecIe>::With(frmI, frmJ, 1);
+}
+
+void MbD::DistxyIeqcJec::initialize()
+{
+	DistxyIecJec::initialize();
+	pdistxypXI = std::make_shared<FullRow<double>>(3);
+	pdistxypEI = std::make_shared<FullRow<double>>(4);
+	ppdistxypXIpXI = std::make_shared<FullMatrix<double>>(3, 3);
+	ppdistxypXIpEI = std::make_shared<FullMatrix<double>>(3, 4);
+	ppdistxypEIpEI = std::make_shared<FullMatrix<double>>(4, 4);
+}
+
+FMatDsptr MbD::DistxyIeqcJec::ppvaluepEIpEI()
+{
+	return ppdistxypEIpEI;
+}
+
+FMatDsptr MbD::DistxyIeqcJec::ppvaluepXIpEI()
+{
+	return ppdistxypXIpEI;
+}
+
+FMatDsptr MbD::DistxyIeqcJec::ppvaluepXIpXI()
+{
+	return ppdistxypXIpXI;
+}
+
+FRowDsptr MbD::DistxyIeqcJec::pvaluepEI()
+{
+	return pdistxypEI;
+}
+
+FRowDsptr MbD::DistxyIeqcJec::pvaluepXI()
+{
+	return pdistxypXI;
+}

MbDCode/DistxyIeqcJec.h

@@ -0,0 +1,32 @@
+#pragma once
+
+#include "DistxyIecJec.h"
+
+namespace MbD {
+	class DistxyIeqcJec : public DistxyIecJec
+	{
+		//pdistxypXI pdistxypEI ppdistxypXIpXI ppdistxypXIpEI ppdistxypEIpEI
+
+	public:
+		DistxyIeqcJec();
+		DistxyIeqcJec(EndFrmcptr frmi, EndFrmcptr frmj);
+
+		void calc_ppdistxypEIpEI();
+		void calc_ppdistxypXIpEI();
+		void calc_ppdistxypXIpXI();
+		void calc_pdistxypEI();
+		void calc_pdistxypXI();
+		void calcPostDynCorrectorIteration() override;
+		void init_xyIeJeIe() override;
+		void initialize() override;
+		FMatDsptr ppvaluepEIpEI() override;
+		FMatDsptr ppvaluepXIpEI() override;
+		FMatDsptr ppvaluepXIpXI() override;
+		FRowDsptr pvaluepEI() override;
+		FRowDsptr pvaluepXI() override;
+
+		FRowDsptr pdistxypXI, pdistxypEI;
+		FMatDsptr ppdistxypXIpXI, ppdistxypXIpEI, ppdistxypEIpEI;
+	};
+}
+