MBDyn new joints, sine, cosine

OndselSolver/ASMTGeneralMotion.cpp

@@ -69,7 +69,12 @@ void MbD::ASMTGeneralMotion::readRotationOrder(std::vector<std::string>& lines)
 {
 	assert(lines[0].find("RotationOrder") != std::string::npos);
 	lines.erase(lines.begin());
-	rotationOrder = readString(lines[0]);
+	std::istringstream iss(lines[0]);
+	rotationOrder = std::make_shared<std::vector<int>>();
+	int i;
+	while (iss >> i) {
+		rotationOrder->push_back(i);
+	}
 	lines.erase(lines.begin());
 }
 
@@ -137,15 +142,7 @@ void MbD::ASMTGeneralMotion::createMbD(std::shared_ptr<System> mbdSys, std::shar
 
 	auto xyzRotBlkList = std::initializer_list<Symsptr>{ phiBlk, theBlk, psiBlk };
 	auto fangIJJ = std::make_shared<EulerAngles<Symsptr>>(xyzRotBlkList);
-	std::istringstream iss(rotationOrder);
-	auto rotOrder = std::make_shared<FullColumn<int>>();
-	int order;
-	for (int i = 0; i < 3; i++)
-	{
-		iss >> order;
-		rotOrder->push_back(order);
-	}
-	fangIJJ->rotOrder = rotOrder;
+	fangIJJ->rotOrder = rotationOrder;
 	fullMotion->fangIJJ = fangIJJ;
 }
 

OndselSolver/ASMTGeneralMotion.h

@@ -24,8 +24,9 @@ namespace MbD {
         void storeOnLevel(std::ofstream& os, int level) override;
         void storeOnTimeSeries(std::ofstream& os) override;
 
-        std::shared_ptr<FullColumn<std::string>> rIJI, angIJJ;
-        std::string rotationOrder;
+        std::shared_ptr<FullColumn<std::string>> rIJI = std::make_shared<FullColumn<std::string>>(3);
+        std::shared_ptr<FullColumn<std::string>> angIJJ = std::make_shared<FullColumn<std::string>>(3);
+        std::shared_ptr<std::vector<int>> rotationOrder = std::make_shared<std::vector<int>>(std::initializer_list<int>{ 1, 2, 3 });
 
     };
 }

OndselSolver/Arguments.cpp

@@ -0,0 +1,21 @@
+/***************************************************************************
+ *   Copyright (c) 2023 Ondsel, Inc.                                       *
+ *                                                                         *
+ *   This file is part of OndselSolver.                                    *
+ *                                                                         *
+ *   See LICENSE file for details about copyright.                         *
+ ***************************************************************************/
+
+#include <algorithm>
+#include <iterator>
+
+#include "Arguments.h"
+
+using namespace MbD;
+
+
+void MbD::Arguments::arguments(Symsptr args)
+{
+	auto arguments = std::static_pointer_cast<Arguments>(args);
+	terms = arguments->terms;
+}

OndselSolver/Arguments.h

@@ -0,0 +1,19 @@
+/***************************************************************************
+ *   Copyright (c) 2023 Ondsel, Inc.                                       *
+ *                                                                         *
+ *   This file is part of OndselSolver.                                    *
+ *                                                                         *
+ *   See LICENSE file for details about copyright.                         *
+ ***************************************************************************/
+
+#pragma once
+
+#include "FunctionWithManyArgs.h"
+
+namespace MbD {
+	class Arguments : public FunctionWithManyArgs
+	{
+	public:
+		void arguments(Symsptr args) override;
+	};
+}

OndselSolver/Array.h

@@ -33,6 +33,7 @@ namespace MbD {
 		Array(std::initializer_list<T> list) : std::vector<T>{ list } {}
 		virtual ~Array() {}
 		virtual void initialize();
+		static bool equaltol(double x, double xx, double tol);
 		void copyFrom(std::shared_ptr<Array<T>> x);
 		virtual void zeroSelf();
 		virtual double sumOfSquares() = 0;
@@ -63,6 +64,11 @@ namespace MbD {
 	inline void Array<T>::initialize()
 	{
 	}
+	template<>
+	inline bool Array<double>::equaltol(double x, double xx, double tol)
+	{
+		return std::abs(x - xx) < tol;
+	}
 	template<typename T>
 	inline void Array<T>::copyFrom(std::shared_ptr<Array<T>> x)
 	{

OndselSolver/EulerAngles.h

@@ -32,7 +32,7 @@ namespace MbD {
 		std::shared_ptr<EulerAnglesDot<T>> differentiateWRT(T var);
 		void setRotOrder(int i, int j, int k);
 
-		std::shared_ptr<FullColumn<int>> rotOrder;
+		std::shared_ptr<std::vector<int>> rotOrder;
 		FColFMatDsptr cA;
 		FMatDsptr aA;
 
@@ -106,7 +106,7 @@ namespace MbD {
 	template<typename T>
 	inline void EulerAngles<T>::setRotOrder(int i, int j, int k)
 	{
-		rotOrder = std::make_shared<FullColumn<int>>(3);
+		rotOrder = std::make_shared<std::vector<int>>(3);
 		rotOrder->at(0) = i;
 		rotOrder->at(1) = j;
 		rotOrder->at(2) = k;

OndselSolver/FullMatrix.h

@@ -104,6 +104,7 @@ namespace MbD {
 		FColsptr<T> bryantAngles();
 		bool isDiagonal();
 		bool isDiagonalToWithin(double ratio);
+		bool equaltol(FMatsptr<T> mat, double ratio);
 		std::shared_ptr<DiagonalMatrix<T>> asDiagonalMatrix();
 		void conditionSelfWithTol(double tol);
 
@@ -703,6 +704,22 @@ namespace MbD {
 		}
 	}
 	template<typename T>
+	inline bool FullMatrix<T>::equaltol(FMatsptr<T> mat2, double tol)
+	{
+		if (this->size() != mat2->size()) return false;
+		for (size_t i = 0; i < this->size(); i++)
+		{
+			auto& rowi = this->at(i);
+			auto& rowi2 = mat2->at(i);
+			if (rowi->size() != rowi2->size()) return false;
+			for (size_t j = 0; j < rowi->size(); j++)
+			{
+				if (!Array<double>::equaltol((double)rowi->at(j), (double)rowi2->at(j), tol)) return false;
+			}
+		}
+		return true;
+	}
+	template<typename T>
 	inline std::shared_ptr<DiagonalMatrix<T>> FullMatrix<T>::asDiagonalMatrix()
 	{
 		int nrow = this->nrow();

OndselSolver/FunctionX.cpp

@@ -9,6 +9,7 @@
 #include "FunctionX.h"
 #include "Constant.h"
 #include "Sum.h"
+#include "Arguments.h"
 
 using namespace MbD;
 
@@ -18,10 +19,9 @@ MbD::FunctionX::FunctionX(Symsptr arg) : xx(arg)
 
 void MbD::FunctionX::arguments(Symsptr args)
 {
-	//args is a Sum with "terms" containing the actual arguments
-	auto sum = std::static_pointer_cast<Sum>(args);
-	assert(sum->terms->size() == 1);
-	xx = sum->terms->front();
+	auto arguments = std::static_pointer_cast<Arguments>(args);
+	assert(arguments->terms->size() == 1);
+	xx = arguments->terms->front();
 }
 
 Symsptr MbD::FunctionX::copyWith(Symsptr)

OndselSolver/Functions.cpp

@@ -0,0 +1,2 @@
+#include "Functions.h"
+

OndselSolver/Functions.h

@@ -0,0 +1,19 @@
+/***************************************************************************
+ *   Copyright (c) 2023 Ondsel, Inc.                                       *
+ *                                                                         *
+ *   This file is part of OndselSolver.                                    *
+ *                                                                         *
+ *   See LICENSE file for details about copyright.                         *
+ ***************************************************************************/
+
+#pragma once
+
+#include "Arguments.h"
+
+namespace MbD {
+	class Functions : public Arguments
+	{
+	public:
+
+	};
+}

OndselSolver/MBDynAxialRotationJoint.cpp

@@ -0,0 +1,37 @@
+/***************************************************************************
+ *   Copyright (c) 2023 Ondsel, Inc.                                       *
+ *                                                                         *
+ *   This file is part of OndselSolver.                                    *
+ *                                                                         *
+ *   See LICENSE file for details about copyright.                         *
+ ***************************************************************************/
+
+#include "MBDynAxialRotationJoint.h"
+#include "ASMTAssembly.h"
+#include "ASMTRevoluteJoint.h"
+#include "ASMTRotationalMotion.h"
+
+using namespace MbD;
+
+void MbD::MBDynAxialRotationJoint::parseMBDyn(std::string line)
+{
+	MBDynJoint::parseMBDyn(line);
+	readFunction(arguments);
+}
+
+void MbD::MBDynAxialRotationJoint::createASMT()
+{
+	MBDynJoint::createASMT();
+	auto asmtAsm = asmtAssembly();
+	asmtMotion = std::make_shared<ASMTRotationalMotion>();
+	asmtMotion->setName(name.append("Motion"));
+	asmtMotion->setMotionJoint(asmtItem->fullName(""));
+	asmtMotion->setRotationZ(asmtFormulaIntegral());
+	asmtAsm->addMotion(asmtMotion);
+	return;
+}
+
+std::shared_ptr<ASMTJoint> MbD::MBDynAxialRotationJoint::asmtClassNew()
+{
+	return std::make_shared<ASMTRevoluteJoint>();
+}

OndselSolver/MBDynAxialRotationJoint.h

@@ -0,0 +1,26 @@
+/***************************************************************************
+ *   Copyright (c) 2023 Ondsel, Inc.                                       *
+ *                                                                         *
+ *   This file is part of OndselSolver.                                    *
+ *                                                                         *
+ *   See LICENSE file for details about copyright.                         *
+ ***************************************************************************/
+
+#pragma once
+#include "MBDynJoint.h"
+
+namespace MbD {
+    class ASMTJoint;
+    class ASMTRotationalMotion;
+
+    class MBDynAxialRotationJoint : public MBDynJoint
+    {
+    public:
+        void parseMBDyn(std::string line) override;
+        void createASMT() override;
+        std::shared_ptr<ASMTJoint> asmtClassNew() override;
+
+        std::shared_ptr<ASMTRotationalMotion> asmtMotion;
+    };
+
+}

OndselSolver/MBDynBody.cpp

@@ -16,31 +16,15 @@ void MbD::MBDynBody::initialize()
 void MbD::MBDynBody::parseMBDyn(std::string line)
 {
 	bodyString = line;
-	size_t previousPos = 0;
-	auto pos = line.find(":");
-	auto front = line.substr(previousPos, pos - previousPos);
-	assert(front.find("body") != std::string::npos);
-	auto arguments = std::vector<std::string>();
-	std::string argument;
-	while (true) {
-		previousPos = pos;
-		pos = line.find(",", pos + 1);
-		if (pos != std::string::npos) {
-			argument = line.substr(previousPos + 1, pos - previousPos - 1);
-			arguments.push_back(argument);
-		}
-		else {
-			argument = line.substr(previousPos + 1);
-			arguments.push_back(argument);
-			break;
-		}
-	}
-	auto iss = std::istringstream(arguments.at(0));
-	iss >> name;
-	arguments.erase(arguments.begin());
-	iss = std::istringstream(arguments.at(0));
-	iss >> nodeName;
-	arguments.erase(arguments.begin());
+	arguments = collectArgumentsFor("body", line);
+	name = readStringOffTop(arguments);
+	nodeName = readStringOffTop(arguments);
+	//auto iss = std::istringstream(arguments.at(0));
+	//iss >> name;
+	//arguments.erase(arguments.begin());
+	//iss = std::istringstream(arguments.at(0));
+	//iss >> nodeName;
+	//arguments.erase(arguments.begin());
 	readMass(arguments);
 	rPcmP = readPosition(arguments);
 	readInertiaMatrix(arguments);

OndselSolver/MBDynClampJoint.cpp

@@ -0,0 +1,50 @@
+/***************************************************************************
+ *   Copyright (c) 2023 Ondsel, Inc.                                       *
+ *                                                                         *
+ *   This file is part of OndselSolver.                                    *
+ *                                                                         *
+ *   See LICENSE file for details about copyright.                         *
+ ***************************************************************************/
+
+#include "MBDynClampJoint.h"
+#include "ASMTAssembly.h"
+#include "ASMTFixedJoint.h"
+
+using namespace MbD;
+
+void MbD::MBDynClampJoint::parseMBDyn(std::string statement)
+{
+	MBDynJoint::parseMBDyn(statement);
+	assert(joint_type == "clamp");
+	return;
+}
+
+void MbD::MBDynClampJoint::createASMT()
+{
+	MBDynJoint::createASMT();
+}
+
+void MbD::MBDynClampJoint::readMarkerI(std::vector<std::string>& args)
+{
+	//mkr1 should be on assembly which doesn't exist in MBDyn
+	//mkr2 is on the node
+	mkr1 = std::make_shared<MBDynMarker>();
+	mkr1->owner = this;
+	mkr1->nodeStr = "Assembly";
+	mkr1->rPmP = std::make_shared<FullColumn<double>>(3);
+	mkr1->aAPm = FullMatrix<double>::identitysptr(3);
+}
+
+void MbD::MBDynClampJoint::readMarkerJ(std::vector<std::string>& args)
+{
+	if (args.empty()) return;
+	mkr2 = std::make_shared<MBDynMarker>();
+	mkr2->owner = this;
+	mkr2->nodeStr = readStringOffTop(args);
+	mkr2->parseMBDynClamp(args);
+}
+
+std::shared_ptr<ASMTJoint> MbD::MBDynClampJoint::asmtClassNew()
+{
+	return std::make_shared<ASMTFixedJoint>();
+}

OndselSolver/MBDynClampJoint.h

@@ -0,0 +1,24 @@
+/***************************************************************************
+ *   Copyright (c) 2023 Ondsel, Inc.                                       *
+ *                                                                         *
+ *   This file is part of OndselSolver.                                    *
+ *                                                                         *
+ *   See LICENSE file for details about copyright.                         *
+ ***************************************************************************/
+
+#pragma once
+#include "MBDynJoint.h"
+
+namespace MbD {
+    class ASMTJoint;
+    
+    class MBDynClampJoint : public MBDynJoint
+    {
+    public:
+        void parseMBDyn(std::string line) override;
+        void createASMT() override;
+        void readMarkerI(std::vector<std::string>& args);
+        void readMarkerJ(std::vector<std::string>& args);
+        std::shared_ptr<ASMTJoint> asmtClassNew() override;
+    };
+}

OndselSolver/MBDynDrive.cpp

@@ -7,25 +7,7 @@ using namespace MbD;
 void MbD::MBDynDrive::parseMBDyn(std::string line)
 {
 	driveString = line;
-	size_t previousPos = 0;
-	auto pos = line.find(":");
-	auto front = line.substr(previousPos, pos - previousPos);
-	assert(front.find("drive caller") != std::string::npos);
-	auto arguments = std::vector<std::string>();
-	std::string argument;
-	while (true) {
-		previousPos = pos;
-		pos = line.find(",", pos + 1);
-		if (pos != std::string::npos) {
-			argument = line.substr(previousPos + 1, pos - previousPos - 1);
-			arguments.push_back(argument);
-		}
-		else {
-			argument = line.substr(previousPos + 1);
-			arguments.push_back(argument);
-			break;
-		}
-	}
+	arguments = collectArgumentsFor("drive caller", line);
 	auto iss = std::istringstream(arguments.at(0));
 	iss >> name;
 	arguments.erase(arguments.begin());
@@ -41,11 +23,8 @@ void MbD::MBDynDrive::parseMBDyn(std::string line)
 void MbD::MBDynDrive::readFunction(std::vector<std::string>& args)
 {
 	if (args.empty()) return;
-	std::string str;
-	auto iss = std::istringstream(args.at(0));
-	iss >> str;
-	if (str.find("ramp") != std::string::npos) {
-		args.erase(args.begin());
+	std::string str = readStringOffTop(args);
+	if (str == "ramp") {
 		std::string slope, initValue, initTime, finalTime;
 		slope = popOffTop(args);
 		initTime = popOffTop(args);
@@ -67,6 +46,100 @@ void MbD::MBDynDrive::readFunction(std::vector<std::string>& args)
 			<< initValue << " + " << slope << "*(" << finalTime << " - " << initTime << "))";
 		formula = ss.str();
 	}
+	else if (str == "cosine") {
+		std::string initial_time, angular_velocity, amplitude, number_of_cycles, initial_value;
+		initial_time = popOffTop(args);
+		angular_velocity = popOffTop(args);
+		amplitude = popOffTop(args);
+		number_of_cycles = popOffTop(args);
+		initial_value = popOffTop(args);
+		initial_time.erase(remove_if(initial_time.begin(), initial_time.end(), isspace), initial_time.end());
+		angular_velocity.erase(remove_if(angular_velocity.begin(), angular_velocity.end(), isspace), angular_velocity.end());
+		amplitude.erase(remove_if(amplitude.begin(), amplitude.end(), isspace), amplitude.end());
+		number_of_cycles.erase(remove_if(number_of_cycles.begin(), number_of_cycles.end(), isspace), number_of_cycles.end());
+		initial_value.erase(remove_if(initial_value.begin(), initial_value.end(), isspace), initial_value.end());
+		//f(t) = initial_value + amplitude * (1 ? cos (angular_velocity * (t ? initial_time)))
+
+		double nCycle;
+		if (number_of_cycles.find("forever") != std::string::npos) {
+			nCycle = 1.0e9;
+		}
+		else if (number_of_cycles.find("one") != std::string::npos) {
+			nCycle = 1.0;
+		}
+		else if (number_of_cycles.find("half") != std::string::npos) {
+			nCycle = 0.5;
+		}
+		else {
+			nCycle = stod(number_of_cycles);
+		}
+		double x0 = stod(initial_time);
+		double y0 = stod(initial_value);
+		double omega = stod(angular_velocity);
+		double amp = stod(amplitude);
+		double x1 = x0 + (2 * OS_M_PI * nCycle / omega);
+		double y1 = y0 + amp * (1.0 - std::cos(omega * (x1 - x0)));
+		double f1 = y0;
+		auto ss = std::stringstream();
+		ss << "(" << y0 << " + " << amp << "*(1.0 - cos(" << omega << "*(time - " << x0 << "))))";
+		std::string f2 = ss.str();
+		double f3 = y1;
+		double t1 = x0;
+		double t2 = x1;
+		ss = std::stringstream();
+		ss << "piecewise(time, functions(" << f1 << ", " << f2 << ", " << f3 << "), transitions(" << t1 << ", " << t2 << "))";
+		formula = ss.str();
+	}
+	else if (str == "sine") {
+		std::string initial_time, angular_velocity, amplitude, number_of_cycles, initial_value;
+		initial_time = popOffTop(args);
+		angular_velocity = popOffTop(args);
+		amplitude = popOffTop(args);
+		number_of_cycles = popOffTop(args);
+		initial_value = popOffTop(args);
+		initial_time.erase(remove_if(initial_time.begin(), initial_time.end(), isspace), initial_time.end());
+		angular_velocity.erase(remove_if(angular_velocity.begin(), angular_velocity.end(), isspace), angular_velocity.end());
+		amplitude.erase(remove_if(amplitude.begin(), amplitude.end(), isspace), amplitude.end());
+		number_of_cycles.erase(remove_if(number_of_cycles.begin(), number_of_cycles.end(), isspace), number_of_cycles.end());
+		initial_value.erase(remove_if(initial_value.begin(), initial_value.end(), isspace), initial_value.end());
+		//f(t) = initial_value + amplitude · sin (angular_velocity · (t − initial_time))
+
+		double nCycle;
+		if (number_of_cycles.find("forever") != std::string::npos) {
+			nCycle = 1.0e9;
+		}
+		else if (number_of_cycles.find("one") != std::string::npos) {
+			nCycle = 0.5;	//Different from cosine
+		}
+		else if (number_of_cycles.find("half") != std::string::npos) {
+			nCycle = 0.25;	//Different from cosine
+		}
+		else {
+			nCycle = stod(number_of_cycles);
+			if (nCycle < 0.0) {
+				nCycle -= 0.75;
+			}
+			else if (nCycle > 0.0) {
+				nCycle -= 0.5;
+			}
+		}
+		double x0 = stod(initial_time);
+		double y0 = stod(initial_value);
+		double omega = stod(angular_velocity);
+		double amp = stod(amplitude);
+		double x1 = x0 + (2 * OS_M_PI * nCycle / omega);
+		double y1 = y0 + amp * std::sin(omega * (x1 - x0));
+		double f1 = y0;
+		auto ss = std::stringstream();
+		ss << "(" << y0 << " + " << amp << "*sin(" << omega << "*(time - " << x0 << ")))";
+		std::string f2 = ss.str();
+		double f3 = y1;
+		double t1 = x0;
+		double t2 = x1;
+		ss = std::stringstream();
+		ss << "piecewise(time, functions(" << f1 << ", " << f2 << ", " << f3 << "), transitions(" << t1 << ", " << t2 << "))";
+		formula = ss.str();
+	}
 	else {
 		assert(false);
 	}

OndselSolver/MBDynDriveHingeJoint.cpp

@@ -0,0 +1,41 @@
+/***************************************************************************
+ *   Copyright (c) 2023 Ondsel, Inc.                                       *
+ *                                                                         *
+ *   This file is part of OndselSolver.                                    *
+ *                                                                         *
+ *   See LICENSE file for details about copyright.                         *
+ ***************************************************************************/
+
+#include "MBDynDriveHingeJoint.h"
+#include "ASMTMarker.h"
+#include "ASMTJoint.h"
+#include "ASMTRotationalMotion.h"
+#include "ASMTAssembly.h"
+
+using namespace MbD;
+
+void MbD::MBDynDriveHingeJoint::parseMBDyn(std::string line)
+{
+	MBDynJoint::parseMBDyn(line);
+	readFunction(arguments);
+}
+
+void MbD::MBDynDriveHingeJoint::createASMT()
+{
+	mkr1->createASMT();
+	if (mkr2) mkr2->createASMT();
+	auto asmtAsm = asmtAssembly();
+	auto asmtMotion = std::make_shared<ASMTRotationalMotion>();
+	asmtItem = asmtMotion;
+	asmtMotion->setName(name);
+	asmtMotion->setMarkerI(mkr1->asmtItem->fullName(""));
+	asmtMotion->setMarkerJ(mkr2->asmtItem->fullName(""));
+	asmtMotion->setRotationZ(formula);
+	asmtAsm->addMotion(asmtMotion);
+	return;
+}
+
+std::shared_ptr<ASMTJoint> MbD::MBDynDriveHingeJoint::asmtClassNew()
+{
+	return std::make_shared<ASMTJoint>();
+}

OndselSolver/MBDynDriveHingeJoint.h

@@ -0,0 +1,23 @@
+/***************************************************************************
+ *   Copyright (c) 2023 Ondsel, Inc.                                       *
+ *                                                                         *
+ *   This file is part of OndselSolver.                                    *
+ *                                                                         *
+ *   See LICENSE file for details about copyright.                         *
+ ***************************************************************************/
+
+#pragma once
+#include "MBDynJoint.h"
+
+namespace MbD {
+    class ASMTJoint;
+
+    class MBDynDriveHingeJoint : public MBDynJoint
+    {
+    public:
+        void parseMBDyn(std::string line) override;
+        void createASMT() override;
+        std::shared_ptr<ASMTJoint> asmtClassNew() override;
+    };
+
+}

OndselSolver/MBDynGravity.cpp

@@ -9,25 +9,7 @@ using namespace MbD;
 void MbD::MBDynGravity::parseMBDyn(std::string line)
 {
 	gravityString = line;
-	size_t previousPos = 0;
-	auto pos = line.find(":");
-	auto front = line.substr(previousPos, pos - previousPos);
-	assert(front.find("gravity") != std::string::npos);
-	auto arguments = std::vector<std::string>();
-	std::string argument;
-	while (true) {
-		previousPos = pos;
-		pos = line.find(",", pos + 1);
-		if (pos != std::string::npos) {
-			argument = line.substr(previousPos + 1, pos - previousPos - 1);
-			arguments.push_back(argument);
-		}
-		else {
-			argument = line.substr(previousPos + 1);
-			arguments.push_back(argument);
-			break;
-		}
-	}
+	arguments = collectArgumentsFor("gravity", line);
 	assert(arguments.at(0).find("uniform") != std::string::npos);
 	arguments.erase(arguments.begin());
 	gvec = readPosition(arguments);

OndselSolver/MBDynInLineJoint.cpp

@@ -0,0 +1,27 @@
+/***************************************************************************
+ *   Copyright (c) 2023 Ondsel, Inc.                                       *
+ *                                                                         *
+ *   This file is part of OndselSolver.                                    *
+ *                                                                         *
+ *   See LICENSE file for details about copyright.                         *
+ ***************************************************************************/
+
+#include "MBDynInLineJoint.h"
+#include "ASMTPointInLineJoint.h"
+
+using namespace MbD;
+
+void MbD::MBDynInLineJoint::parseMBDyn(std::string line)
+{
+	MBDynJoint::parseMBDyn(line);
+}
+
+void MbD::MBDynInLineJoint::createASMT()
+{
+	MBDynJoint::createASMT();
+}
+
+std::shared_ptr<ASMTJoint> MbD::MBDynInLineJoint::asmtClassNew()
+{
+	return std::make_shared<ASMTPointInLineJoint>();
+}

OndselSolver/MBDynInLineJoint.h

@@ -0,0 +1,22 @@
+/***************************************************************************
+ *   Copyright (c) 2023 Ondsel, Inc.                                       *
+ *                                                                         *
+ *   This file is part of OndselSolver.                                    *
+ *                                                                         *
+ *   See LICENSE file for details about copyright.                         *
+ ***************************************************************************/
+
+#pragma once
+#include "MBDynJoint.h"
+
+namespace MbD {
+	class ASMTJoint;
+
+	class MBDynInLineJoint : public MBDynJoint
+	{
+	public:
+		void parseMBDyn(std::string line) override;
+		void createASMT() override;
+		std::shared_ptr<ASMTJoint> asmtClassNew() override;
+	};
+}

OndselSolver/MBDynInPlaneJoint.cpp

@@ -0,0 +1,27 @@
+/***************************************************************************
+ *   Copyright (c) 2023 Ondsel, Inc.                                       *
+ *                                                                         *
+ *   This file is part of OndselSolver.                                    *
+ *                                                                         *
+ *   See LICENSE file for details about copyright.                         *
+ ***************************************************************************/
+
+#include "MBDynInPlaneJoint.h"
+#include "ASMTPointInPlaneJoint.h"
+
+using namespace MbD;
+
+void MbD::MBDynInPlaneJoint::parseMBDyn(std::string line)
+{
+	MBDynJoint::parseMBDyn(line);
+}
+
+void MbD::MBDynInPlaneJoint::createASMT()
+{
+	MBDynJoint::createASMT();
+}
+
+std::shared_ptr<ASMTJoint> MbD::MBDynInPlaneJoint::asmtClassNew()
+{
+	return std::make_shared<ASMTPointInPlaneJoint>();
+}

OndselSolver/MBDynInPlaneJoint.h

@@ -0,0 +1,24 @@
+/***************************************************************************
+ *   Copyright (c) 2023 Ondsel, Inc.                                       *
+ *                                                                         *
+ *   This file is part of OndselSolver.                                    *
+ *                                                                         *
+ *   See LICENSE file for details about copyright.                         *
+ ***************************************************************************/
+
+#pragma once
+#include "MBDynJoint.h"
+
+namespace MbD {
+    class ASMTJoint;
+
+    class MBDynInPlaneJoint : public MBDynJoint
+    {
+    public:
+        void parseMBDyn(std::string line) override;
+        void createASMT() override;
+        std::shared_ptr<ASMTJoint> asmtClassNew() override;
+
+    };
+}
+

OndselSolver/MBDynItem.cpp

@@ -263,7 +263,10 @@ FMatDsptr MbD::MBDynItem::readOrientation(std::vector<std::string>& args)
 		aAOf = readOrientation(args);
 	}
 	else if (str.find("position") != std::string::npos) {
-		//Do nothing
+		if (str.find("orientation") != std::string::npos) {
+			args.erase(args.begin());
+			aAOf = readOrientation(args);
+		}
 	}
 	else if (str.find("orientation") != std::string::npos) {
 		args.erase(args.begin());
@@ -283,7 +286,7 @@ FMatDsptr MbD::MBDynItem::readBasicOrientation(std::vector<std::string>& args)
 	if (str.find("euler") != std::string::npos) {
 		args.erase(args.begin());
 		auto euler = std::make_shared<EulerAngles<Symsptr>>();
-		euler->rotOrder = std::make_shared<FullColumn<int>>(std::initializer_list<int>{ 1, 2, 3 });
+		euler->rotOrder = std::make_shared<std::vector<int>>(std::initializer_list<int>{ 1, 2, 3 });
 		for (int i = 0; i < 3; i++)
 		{
 			auto userFunc = std::make_shared<BasicUserFunction>(popOffTop(args), 1.0);
@@ -432,6 +435,26 @@ std::string MbD::MBDynItem::readStringOffTop(std::vector<std::string>& args)
 	return str;
 }
 
+void MbD::MBDynItem::readName(std::vector<std::string>& args)
+{
+	name = readStringOffTop(args);
+}
+
+std::string MbD::MBDynItem::readJointTypeOffTop(std::vector<std::string>& args)
+{
+	auto ss = std::stringstream();
+	auto iss = std::istringstream(popOffTop(args));
+	std::string str;
+	iss >> str;
+	ss << str;
+	while (!iss.eof()) {
+		ss << ' ';
+		iss >> str;
+		ss << str;
+	}
+	return ss.str();
+}
+
 std::string MbD::MBDynItem::readToken(std::string& line)
 {
 	auto iss = std::istringstream(line);

OndselSolver/MBDynItem.h

@@ -34,7 +34,7 @@ namespace MbD {
 		virtual void parseMBDyn(std::string line);
 		static std::vector<std::string> collectArgumentsFor(std::string title, std::string& statement);
 		std::vector<std::string>::iterator findLineWith(std::vector<std::string>& lines, std::vector<std::string>& tokens);
-		bool lineHasTokens(const std::string& line, std::vector<std::string>& tokens);
+		static bool lineHasTokens(const std::string& line, std::vector<std::string>& tokens);
 		virtual std::shared_ptr<std::vector<std::shared_ptr<MBDynNode>>> mbdynNodes();
 		virtual std::shared_ptr<std::vector<std::shared_ptr<MBDynBody>>> mbdynBodies();
 		virtual std::shared_ptr<std::vector<std::shared_ptr<MBDynJoint>>> mbdynJoints();
@@ -57,11 +57,14 @@ namespace MbD {
 		FMatDsptr readBasicOrientation(std::vector<std::string>& args);
 		std::string popOffTop(std::vector<std::string>& args);
 		std::string readStringOffTop(std::vector<std::string>& args);
+		void readName(std::vector<std::string>& args);
+		std::string readJointTypeOffTop(std::vector<std::string>& args);
 		std::string readToken(std::string& line);
 
 		std::string name;
 		MBDynItem* owner = nullptr;
 		std::shared_ptr<ASMTItem> asmtItem;
+		std::vector<std::string> arguments;
 
 
 	};

OndselSolver/MBDynJoint.cpp

@@ -1,8 +1,17 @@
+/***************************************************************************
+ *   Copyright (c) 2023 Ondsel, Inc.                                       *
+ *                                                                         *
+ *   This file is part of OndselSolver.                                    *
+ *                                                                         *
+ *   See LICENSE file for details about copyright.                         *
+ ***************************************************************************/
+
 #include <regex>
 
 #include "MBDynJoint.h"
 #include "ASMTMarker.h"
 #include "ASMTPart.h"
+#include "ASMTJoint.h"
 #include "ASMTAssembly.h"
 #include "ASMTRevoluteJoint.h"
 #include "ASMTRotationalMotion.h"
@@ -10,9 +19,67 @@
 #include "ASMTNoRotationJoint.h"
 #include "ASMTFixedJoint.h"
 #include "ASMTSphericalJoint.h"
+#include "MBDynTotalJoint.h"
+#include "MBDynClampJoint.h"
+#include "MBDynAxialRotationJoint.h"
+#include "MBDynDriveHingeJoint.h"
+#include "MBDynInLineJoint.h"
+#include "MBDynInPlaneJoint.h"
+#include "MBDynPrismaticJoint.h"
+#include "MBDynRevoluteHingeJoint.h"
+#include "MBDynRevolutePinJoint.h"
+#include "MBDynSphericalHingeJoint.h"
 
 using namespace MbD;
 
+std::shared_ptr<MBDynJoint> MbD::MBDynJoint::newJoint(std::string statement)
+{
+	//std::shared_ptr<MBDynJoint> joint;
+	//std::vector<std::string> tokens{ "total", "joint" };
+	std::vector<std::string> tokens;
+	tokens = { "axial", "rotation" };
+	if (lineHasTokens(statement, tokens)) {
+		return std::make_shared<MBDynAxialRotationJoint>();
+	}
+	tokens = { "clamp" };
+	if (lineHasTokens(statement, tokens)) {
+		return std::make_shared<MBDynClampJoint>();
+	}
+	tokens = { "drive", "hinge" };
+	if (lineHasTokens(statement, tokens)) {
+		return std::make_shared<MBDynDriveHingeJoint>();
+	}
+	tokens = { "in", "line" };
+	if (lineHasTokens(statement, tokens)) {
+		return std::make_shared<MBDynInLineJoint>();
+	}
+	tokens = { "in", "plane" };
+	if (lineHasTokens(statement, tokens)) {
+		return std::make_shared<MBDynInPlaneJoint>();
+	}
+	tokens = { "prismatic" };
+	if (lineHasTokens(statement, tokens)) {
+		return std::make_shared<MBDynPrismaticJoint>();
+	}
+	tokens = { "revolute", "hinge" };
+	if (lineHasTokens(statement, tokens)) {
+		return std::make_shared<MBDynRevoluteHingeJoint>();
+	}
+	tokens = { "revolute", "pin" };
+	if (lineHasTokens(statement, tokens)) {
+		return std::make_shared<MBDynRevolutePinJoint>();
+	}
+	tokens = { "spherical", "hinge" };
+	if (lineHasTokens(statement, tokens)) {
+		return std::make_shared<MBDynSphericalHingeJoint>();
+	}
+	tokens = { "total", "joint" };
+	if (lineHasTokens(statement, tokens)) {
+		return std::make_shared<MBDynTotalJoint>();
+	}
+	return std::make_shared<MBDynJoint>();
+}
+
 void MbD::MBDynJoint::initialize()
 {
 }
@@ -20,120 +87,131 @@ void MbD::MBDynJoint::initialize()
 void MbD::MBDynJoint::parseMBDyn(std::string line)
 {
 	jointString = line;
-	auto arguments = collectArgumentsFor("joint", line);
-	//size_t previousPos = 0;
-	//auto pos = line.find(":");
-	//auto front = line.substr(previousPos, pos - previousPos);
-	//assert(front.find("joint") != std::string::npos);
-	//auto arguments = std::vector<std::string>();
-	//std::string argument;
-	//while (true) {
-	//	previousPos = pos;
-	//	pos = line.find(",", pos + 1);
-	//	if (pos != std::string::npos) {
-	//		argument = line.substr(previousPos + 1, pos - previousPos - 1);
-	//		arguments.push_back(argument);
-	//	}
-	//	else {
-	//		argument = line.substr(previousPos + 1);
-	//		arguments.push_back(argument);
-	//		break;
-	//	}
-	//}
-	auto ss = std::stringstream();
-	auto iss = std::istringstream(arguments.at(0));
-	iss >> name;
-	arguments.erase(arguments.begin());
-	iss = std::istringstream(arguments.at(0));
-	iss >> joint_type;
-	if (joint_type == "axial") {
-		ss << joint_type;
-		iss >> joint_type;
-		if (joint_type == "rotation") {
-			ss << " " << joint_type;
-			joint_type = ss.str();
-		}
-		else {
-			assert(false);
-		}
-		arguments.erase(arguments.begin());
-		readMarkerI(arguments);
-		readMarkerJ(arguments);
-		readFunction(arguments);
-		return;
-	}
-	else if (joint_type == "revolute") {
-		ss << joint_type;
-		iss >> joint_type;
-		if (joint_type == "hinge") {
-			ss << " " << joint_type;
-			joint_type = ss.str();
-		}
-		else {
-			assert(false);
-		}
-	}
-	else if (joint_type == "spherical") {
-		ss << joint_type;
-		iss >> joint_type;
-		if (joint_type == "hinge") {
-			ss << " " << joint_type;
-			joint_type = ss.str();
-		}
-		else {
-			assert(false);
-		}
-	}
-	else if (joint_type == "drive") {
-		ss << joint_type;
-		iss >> joint_type;
-		if (joint_type == "hinge") {
-			ss << " " << joint_type;
-			joint_type = ss.str();
-		}
-		else {
-			assert(false);
-		}
-		arguments.erase(arguments.begin());
-		readMarkerI(arguments);
-		readMarkerJ(arguments);
-		readFunction(arguments);
-		return;
-	}
-	else if (joint_type == "in") {
-		ss << joint_type;
-		iss >> joint_type;
-		if (joint_type == "line") {
-			ss << " " << joint_type;
-			joint_type = ss.str();
-		}
-		else {
-			assert(false);
-		}
-	}
-	else if (joint_type == "clamp") {
-		//mkr1 should be on assembly which doesn't exist in MBDyn
-		//mkr2 is on the node
-		arguments.erase(arguments.begin());
-		mkr1 = std::make_shared<MBDynMarker>();
-		mkr1->owner = this;
-		mkr1->nodeStr = "Assembly";
-		mkr1->rPmP = std::make_shared<FullColumn<double>>(3);
-		mkr1->aAPm = FullMatrix<double>::identitysptr(3);
-		readClampMarkerJ(arguments);
-		return;
-	}
-	else if (joint_type == "prismatic") {
-		noop();
-	}
-	else {
-		assert(false);
-	}
-	arguments.erase(arguments.begin());
+	arguments = collectArgumentsFor("joint", line);
+	readName(arguments);
+	readJointType(arguments);
 	readMarkerI(arguments);
 	readMarkerJ(arguments);
+
 }
 
+void MbD::MBDynJoint::readJointType(std::vector<std::string>& args)
+{
+	joint_type = readJointTypeOffTop(args);
+}
+
+//void MbD::MBDynJoint::parseMBDyn0(std::string line)
+//{
+//	jointString = line;
+//	arguments = collectArgumentsFor("joint", line);
+//	auto ss = std::stringstream();
+//	name = readStringOffTop(arguments);
+//	auto iss = std::istringstream(arguments.at(0));
+//	iss >> joint_type;
+//	if (joint_type == "axial") {
+//		ss << joint_type;
+//		iss >> joint_type;
+//		if (joint_type == "rotation") {
+//			ss << " " << joint_type;
+//			joint_type = ss.str();
+//		}
+//		else {
+//			assert(false);
+//		}
+//		arguments.erase(arguments.begin());
+//		readMarkerI(arguments);
+//		readMarkerJ(arguments);
+//		readFunction(arguments);
+//		return;
+//	}
+//	else if (joint_type == "revolute") {
+//		ss << joint_type;
+//		iss >> joint_type;
+//		if (joint_type == "hinge") {
+//			ss << " " << joint_type;
+//			joint_type = ss.str();
+//		}
+//		else {
+//			assert(false);
+//		}
+//	}
+//	else if (joint_type == "spherical") {
+//		ss << joint_type;
+//		iss >> joint_type;
+//		if (joint_type == "hinge") {
+//			ss << " " << joint_type;
+//			joint_type = ss.str();
+//		}
+//		else {
+//			assert(false);
+//		}
+//	}
+//	else if (joint_type == "drive") {
+//		ss << joint_type;
+//		iss >> joint_type;
+//		if (joint_type == "hinge") {
+//			ss << " " << joint_type;
+//			joint_type = ss.str();
+//		}
+//		else {
+//			assert(false);
+//		}
+//		arguments.erase(arguments.begin());
+//		readMarkerI(arguments);
+//		readMarkerJ(arguments);
+//		readFunction(arguments);
+//		return;
+//	}
+//	else if (joint_type == "in") {
+//		ss << joint_type;
+//		iss >> joint_type;
+//		if (joint_type == "line") {
+//			ss << " " << joint_type;
+//			joint_type = ss.str();
+//		}
+//		else {
+//			assert(false);
+//		}
+//	}
+//	else if (joint_type == "total") {
+//		ss << joint_type;
+//		iss >> joint_type;
+//		if (joint_type == "joint") {
+//			ss << " " << joint_type;
+//			joint_type = ss.str();
+//		}
+//		else {
+//			assert(false);
+//		}
+//		arguments.erase(arguments.begin());
+//		readTotalJointMarkerI(arguments);
+//		readTotalJointMarkerJ(arguments);
+//		readTotalJointFunction(arguments);
+//		return;
+//	}
+//	else if (joint_type == "clamp") {
+//		//mkr1 should be on assembly which doesn't exist in MBDyn
+//		//mkr2 is on the node
+//		arguments.erase(arguments.begin());
+//		mkr1 = std::make_shared<MBDynMarker>();
+//		mkr1->owner = this;
+//		mkr1->nodeStr = "Assembly";
+//		mkr1->rPmP = std::make_shared<FullColumn<double>>(3);
+//		mkr1->aAPm = FullMatrix<double>::identitysptr(3);
+//		readClampMarkerJ(arguments);
+//		return;
+//	}
+//	else if (joint_type == "prismatic") {
+//		noop();
+//	}
+//	else {
+//		assert(false);
+//	}
+//	arguments.erase(arguments.begin());
+//	readMarkerI(arguments);
+//	readMarkerJ(arguments);
+//}
+
 void MbD::MBDynJoint::readMarkerI(std::vector<std::string>& args)
 {
 	mkr1 = std::make_shared<MBDynMarker>();
@@ -161,6 +239,33 @@ void MbD::MBDynJoint::readMarkerJ(std::vector<std::string>& args)
 	mkr2->parseMBDyn(args);
 }
 
+void MbD::MBDynJoint::readTotalJointMarkerI(std::vector<std::string>& args)
+{
+	mkr1 = std::make_shared<MBDynMarker>();
+	mkr1->owner = this;
+	mkr1->nodeStr = readStringOffTop(args);
+	auto _nodeNames = nodeNames();
+	std::string nodeName;
+	auto it = std::find_if(args.begin(), args.end(), [&](const std::string& s) {
+		auto iss = std::istringstream(s);
+		iss >> nodeName;
+		if (std::find(_nodeNames.begin(), _nodeNames.end(), nodeName) != _nodeNames.end()) return true;
+		return false;
+		});
+	auto markerArgs = std::vector<std::string>(args.begin(), it);
+	args.erase(args.begin(), it);
+	mkr1->parseMBDynTotalJointMarker(markerArgs);
+}
+
+void MbD::MBDynJoint::readTotalJointMarkerJ(std::vector<std::string>& args)
+{
+	if (args.empty()) return;
+	mkr2 = std::make_shared<MBDynMarker>();
+	mkr2->owner = this;
+	mkr2->nodeStr = readStringOffTop(args);
+	mkr2->parseMBDynTotalJointMarker(args);
+}
+
 void MbD::MBDynJoint::readClampMarkerJ(std::vector<std::string>& args)
 {
 	if (args.empty()) return;
@@ -206,69 +311,106 @@ void MbD::MBDynJoint::readFunction(std::vector<std::string>& args)
 	}
 }
 
+void MbD::MBDynJoint::readTotalJointFunction(std::vector<std::string>& args)
+{
+	std::vector<std::string> tokens{ "position", "constraint" };
+	assert(lineHasTokens(args[0], tokens));
+	args.erase(args.begin());
+	assert(readStringOffTop(args) == "active");
+	assert(readStringOffTop(args) == "active");
+	assert(readStringOffTop(args) == "active");
+	assert(readStringOffTop(args) == "null");
+	std::vector<std::string> tokens1{ "orientation", "constraint" };
+	assert(lineHasTokens(args[0], tokens1));
+	args.erase(args.begin());
+	assert(readStringOffTop(args) == "active");
+	assert(readStringOffTop(args) == "active");
+	assert(readStringOffTop(args) == "rotation");
+	readFunction(args);
+}
+
 void MbD::MBDynJoint::createASMT()
 {
 	mkr1->createASMT();
 	if (mkr2) mkr2->createASMT();
-	std::shared_ptr<ASMTJoint> asmtJoint;
-	if (joint_type == "clamp") {
-		auto asmtAsm = asmtAssembly();
-		asmtJoint = std::make_shared<ASMTFixedJoint>();
-		asmtJoint->setName(name);
-		asmtJoint->setMarkerI(mkr1->asmtItem->fullName(""));
-		asmtJoint->setMarkerJ(mkr2->asmtItem->fullName(""));
-		asmtAsm->addJoint(asmtJoint);
-		return;
-	}
-	if (joint_type == "axial rotation") {
-		auto asmtAsm = asmtAssembly();
-		asmtJoint = std::make_shared<ASMTRevoluteJoint>();
-		asmtItem = asmtJoint;
-		asmtJoint->setName(name);
-		asmtJoint->setMarkerI(mkr1->asmtItem->fullName(""));
-		asmtJoint->setMarkerJ(mkr2->asmtItem->fullName(""));
-		asmtAsm->addJoint(asmtJoint);
-		auto asmtMotion = std::make_shared<ASMTRotationalMotion>();
-		asmtItem = asmtMotion;
-		asmtMotion->setName(name.append("Motion"));
-		asmtMotion->setMotionJoint(asmtJoint->fullName(""));
-		asmtMotion->setRotationZ(asmtFormulaIntegral());
-		asmtAsm->addMotion(asmtMotion);
-		return;
-	}
-	if (joint_type == "drive hinge") {
-		auto asmtAsm = asmtAssembly();
-		auto asmtMotion = std::make_shared<ASMTRotationalMotion>();
-		asmtItem = asmtMotion;
-		asmtMotion->setName(name);
-		asmtMotion->setMarkerI(mkr1->asmtItem->fullName(""));
-		asmtMotion->setMarkerJ(mkr2->asmtItem->fullName(""));
-		asmtMotion->setRotationZ(formula);
-		asmtAsm->addMotion(asmtMotion);
-		return;
-	}
-	if (joint_type == "revolute hinge") {
-		asmtJoint = std::make_shared<ASMTRevoluteJoint>();
-	}
-	else if (joint_type == "spherical hinge") {
-		asmtJoint = std::make_shared<ASMTSphericalJoint>();
-	}
-	else if (joint_type == "in line") {
-		asmtJoint = std::make_shared<ASMTPointInLineJoint>();
-	}
-	else if (joint_type == "prismatic") {
-		asmtJoint = std::make_shared<ASMTNoRotationJoint>();
-	}
-	else {
-		assert(false);
-	}
+	auto asmtAsm = asmtAssembly();
+	auto asmtJoint = asmtClassNew();
 	asmtItem = asmtJoint;
 	asmtJoint->setName(name);
 	asmtJoint->setMarkerI(mkr1->asmtItem->fullName(""));
 	asmtJoint->setMarkerJ(mkr2->asmtItem->fullName(""));
-	asmtAssembly()->addJoint(asmtJoint);
+	asmtAsm->addJoint(asmtJoint);
 }
 
+std::shared_ptr<ASMTJoint> MbD::MBDynJoint::asmtClassNew()
+{
+	assert(false);
+	return std::make_shared<ASMTJoint>();
+}
+
+//void MbD::MBDynJoint::createASMT()
+//{
+//	mkr1->createASMT();
+//	if (mkr2) mkr2->createASMT();
+//	std::shared_ptr<ASMTJoint> asmtJoint;
+//	if (joint_type == "clamp") {
+//		auto asmtAsm = asmtAssembly();
+//		asmtJoint = std::make_shared<ASMTFixedJoint>();
+//		asmtJoint->setName(name);
+//		asmtJoint->setMarkerI(mkr1->asmtItem->fullName(""));
+//		asmtJoint->setMarkerJ(mkr2->asmtItem->fullName(""));
+//		asmtAsm->addJoint(asmtJoint);
+//		return;
+//	}
+//	if (joint_type == "axial rotation") {
+//		auto asmtAsm = asmtAssembly();
+//		asmtJoint = std::make_shared<ASMTRevoluteJoint>();
+//		asmtItem = asmtJoint;
+//		asmtJoint->setName(name);
+//		asmtJoint->setMarkerI(mkr1->asmtItem->fullName(""));
+//		asmtJoint->setMarkerJ(mkr2->asmtItem->fullName(""));
+//		asmtAsm->addJoint(asmtJoint);
+//		auto asmtMotion = std::make_shared<ASMTRotationalMotion>();
+//		asmtItem = asmtMotion;
+//		asmtMotion->setName(name.append("Motion"));
+//		asmtMotion->setMotionJoint(asmtJoint->fullName(""));
+//		asmtMotion->setRotationZ(asmtFormulaIntegral());
+//		asmtAsm->addMotion(asmtMotion);
+//		return;
+//	}
+//	if (joint_type == "drive hinge") {
+//		auto asmtAsm = asmtAssembly();
+//		auto asmtMotion = std::make_shared<ASMTRotationalMotion>();
+//		asmtItem = asmtMotion;
+//		asmtMotion->setName(name);
+//		asmtMotion->setMarkerI(mkr1->asmtItem->fullName(""));
+//		asmtMotion->setMarkerJ(mkr2->asmtItem->fullName(""));
+//		asmtMotion->setRotationZ(formula);
+//		asmtAsm->addMotion(asmtMotion);
+//		return;
+//	}
+//	if (joint_type == "revolute hinge") {
+//		asmtJoint = std::make_shared<ASMTRevoluteJoint>();
+//	}
+//	else if (joint_type == "spherical hinge") {
+//		asmtJoint = std::make_shared<ASMTSphericalJoint>();
+//	}
+//	else if (joint_type == "in line") {
+//		asmtJoint = std::make_shared<ASMTPointInLineJoint>();
+//	}
+//	else if (joint_type == "prismatic") {
+//		asmtJoint = std::make_shared<ASMTNoRotationJoint>();
+//	}
+//	else {
+//		assert(false);
+//	}
+//	asmtItem = asmtJoint;
+//	asmtJoint->setName(name);
+//	asmtJoint->setMarkerI(mkr1->asmtItem->fullName(""));
+//	asmtJoint->setMarkerJ(mkr2->asmtItem->fullName(""));
+//	asmtAssembly()->addJoint(asmtJoint);
+//}
+
 std::string MbD::MBDynJoint::asmtFormula()
 {
 	auto ss = std::stringstream();
@@ -296,6 +438,33 @@ std::string MbD::MBDynJoint::asmtFormula()
 	return ss.str();
 }
 
+std::string MbD::MBDynJoint::asmtFormula(std::string mbdynFormula)
+{
+	auto ss = std::stringstream();
+	std::string drivestr = "model::drive";
+	size_t previousPos = 0;
+	auto pos = mbdynFormula.find(drivestr);
+	ss << mbdynFormula.substr(previousPos, pos - previousPos);
+	while (pos != std::string::npos) {
+		previousPos = pos;
+		pos = mbdynFormula.find('(', pos + 1);
+		previousPos = pos;
+		pos = mbdynFormula.find(',', pos + 1);
+		auto driveName = mbdynFormula.substr(previousPos + 1, pos - previousPos - 1);
+		driveName = readToken(driveName);
+		previousPos = pos;
+		pos = mbdynFormula.find(')', pos + 1);
+		auto varName = mbdynFormula.substr(previousPos + 1, pos - previousPos - 1);
+		varName = readToken(varName);
+		//Insert drive mbdynFormula
+		ss << formulaFromDrive(driveName, varName);
+		previousPos = pos;
+		pos = mbdynFormula.find(drivestr, pos + 1);
+		ss << mbdynFormula.substr(previousPos + 1, pos - previousPos);
+	}
+	return ss.str();
+}
+
 std::string MbD::MBDynJoint::asmtFormulaIntegral()
 {
 	auto ss = std::stringstream();

OndselSolver/MBDynJoint.h

@@ -11,20 +11,29 @@
 #include "MBDynMarker.h"
 
 namespace MbD {
+    class ASMTJoint;
+
     class MBDynJoint : public MBDynElement
     {
     public:
+        static std::shared_ptr<MBDynJoint> newJoint(std::string line);
         void initialize() override;
         void parseMBDyn(std::string line) override;
-        void readMarkerI(std::vector<std::string>& args);
-        void readMarkerJ(std::vector<std::string>& args);
+        virtual void readJointType(std::vector<std::string>& args);
+        virtual void readMarkerI(std::vector<std::string>& args);
+        virtual void readMarkerJ(std::vector<std::string>& args);
+        void readTotalJointMarkerI(std::vector<std::string>& args);
+        void readTotalJointMarkerJ(std::vector<std::string>& args);
         void readClampMarkerJ(std::vector<std::string>& args);
-        void readFunction(std::vector<std::string>& args);
+        virtual void readFunction(std::vector<std::string>& args);
+        void readTotalJointFunction(std::vector<std::string>& args);
         void createASMT() override;
+        virtual std::shared_ptr<ASMTJoint> asmtClassNew();
         std::string asmtFormula();
+        std::string asmtFormula(std::string mbdynFormula);
         std::string asmtFormulaIntegral();
 
-        std::string jointString, joint_type, node_1_label, node_2_label;
+        std::string jointString, joint_type;
         std::shared_ptr<MBDynMarker> mkr1, mkr2;
         std::string formula;
     };

OndselSolver/MBDynMarker.cpp

@@ -32,6 +32,13 @@ void MbD::MBDynMarker::parseMBDyn(std::vector<std::string>& args)
 	}
 }
 
+void MbD::MBDynMarker::parseMBDynTotalJointMarker(std::vector<std::string>& args)
+{
+	parseMBDyn(args);
+	aAPm2 = readOrientation(args);
+	assert(aAPm->equaltol(aAPm2, 1.0e-9));
+}
+
 void MbD::MBDynMarker::parseMBDynClamp(std::vector<std::string>& args)
 {
 	//rOmO = rOPO + aAOP*rPmP

OndselSolver/MBDynMarker.h

@@ -15,11 +15,12 @@ namespace MbD {
 	{
 	public:
 		void parseMBDyn(std::vector<std::string>& args) override;
+		void parseMBDynTotalJointMarker(std::vector<std::string>& args);
 		void parseMBDynClamp(std::vector<std::string>& args);
 		void createASMT() override;
 
 		std::string nodeStr;
 		FColDsptr rPmP; //part to marker
-		FMatDsptr aAPm;
+		FMatDsptr aAPm, aAPm2;
 	};
 }

OndselSolver/MBDynPrismaticJoint.cpp

@@ -0,0 +1,27 @@
+/***************************************************************************
+ *   Copyright (c) 2023 Ondsel, Inc.                                       *
+ *                                                                         *
+ *   This file is part of OndselSolver.                                    *
+ *                                                                         *
+ *   See LICENSE file for details about copyright.                         *
+ ***************************************************************************/
+
+#include "MBDynPrismaticJoint.h"
+#include "ASMTNoRotationJoint.h"
+
+using namespace MbD;
+
+void MbD::MBDynPrismaticJoint::parseMBDyn(std::string line)
+{
+	MBDynJoint::parseMBDyn(line);
+}
+
+void MbD::MBDynPrismaticJoint::createASMT()
+{
+	MBDynJoint::createASMT();
+}
+
+std::shared_ptr<ASMTJoint> MbD::MBDynPrismaticJoint::asmtClassNew()
+{
+	return std::make_shared<ASMTNoRotationJoint>();
+}

OndselSolver/MBDynPrismaticJoint.h

@@ -0,0 +1,23 @@
+/***************************************************************************
+ *   Copyright (c) 2023 Ondsel, Inc.                                       *
+ *                                                                         *
+ *   This file is part of OndselSolver.                                    *
+ *                                                                         *
+ *   See LICENSE file for details about copyright.                         *
+ ***************************************************************************/
+
+#pragma once
+#include "MBDynJoint.h"
+
+namespace MbD {
+    class ASMTJoint;
+
+    class MBDynPrismaticJoint : public MBDynJoint
+    {
+    public:
+        void parseMBDyn(std::string line) override;
+        void createASMT() override;
+        std::shared_ptr<ASMTJoint> asmtClassNew() override;
+
+    };
+}

OndselSolver/MBDynReference.cpp

@@ -12,25 +12,7 @@ void MbD::MBDynReference::initialize()
 void MbD::MBDynReference::parseMBDyn(std::string line)
 {
 	refString = line;
-	size_t previousPos = 0;
-	auto pos = line.find(":");
-	auto front = line.substr(previousPos, pos - previousPos);
-	assert(front.find("reference") != std::string::npos);
-	auto arguments = std::vector<std::string>();
-	std::string argument;
-	while (true) {
-		previousPos = pos;
-		pos = line.find(",", pos + 1);
-		if (pos != std::string::npos) {
-			argument = line.substr(previousPos + 1, pos - previousPos - 1);
-			arguments.push_back(argument);
-		}
-		else {
-			argument = line.substr(previousPos + 1);
-			arguments.push_back(argument);
-			break;
-		}
-	}
+	arguments = collectArgumentsFor("reference", line);
 	std::istringstream iss(arguments.at(0));
 	iss >> name;
 	arguments.erase(arguments.begin());

OndselSolver/MBDynRevoluteHingeJoint.cpp

@@ -0,0 +1,27 @@
+/***************************************************************************
+ *   Copyright (c) 2023 Ondsel, Inc.                                       *
+ *                                                                         *
+ *   This file is part of OndselSolver.                                    *
+ *                                                                         *
+ *   See LICENSE file for details about copyright.                         *
+ ***************************************************************************/
+
+#include "MBDynRevoluteHingeJoint.h"
+#include "ASMTRevoluteJoint.h"
+
+using namespace MbD;
+
+void MbD::MBDynRevoluteHingeJoint::parseMBDyn(std::string line)
+{
+	MBDynJoint::parseMBDyn(line);
+}
+
+void MbD::MBDynRevoluteHingeJoint::createASMT()
+{
+	MBDynJoint::createASMT();
+}
+
+std::shared_ptr<ASMTJoint> MbD::MBDynRevoluteHingeJoint::asmtClassNew()
+{
+	return std::make_shared<ASMTRevoluteJoint>();
+}

OndselSolver/MBDynRevoluteHingeJoint.h

@@ -0,0 +1,23 @@
+/***************************************************************************
+ *   Copyright (c) 2023 Ondsel, Inc.                                       *
+ *                                                                         *
+ *   This file is part of OndselSolver.                                    *
+ *                                                                         *
+ *   See LICENSE file for details about copyright.                         *
+ ***************************************************************************/
+
+#pragma once
+#include "MBDynJoint.h"
+
+namespace MbD {
+    class ASMTJoint;
+
+    class MBDynRevoluteHingeJoint : public MBDynJoint
+    {
+    public:
+        void parseMBDyn(std::string line) override;
+        void createASMT() override;
+        std::shared_ptr<ASMTJoint> asmtClassNew() override;
+
+    };
+}

OndselSolver/MBDynRevolutePinJoint.cpp

@@ -0,0 +1,28 @@
+/***************************************************************************
+ *   Copyright (c) 2023 Ondsel, Inc.                                       *
+ *                                                                         *
+ *   This file is part of OndselSolver.                                    *
+ *                                                                         *
+ *   See LICENSE file for details about copyright.                         *
+ ***************************************************************************/
+
+#include "MBDynRevolutePinJoint.h"
+#include "ASMTJoint.h"
+
+using namespace MbD;
+
+void MbD::MBDynRevolutePinJoint::parseMBDyn(std::string line)
+{
+	MBDynJoint::parseMBDyn(line);
+}
+
+void MbD::MBDynRevolutePinJoint::createASMT()
+{
+	MBDynJoint::createASMT();
+}
+
+std::shared_ptr<ASMTJoint> MbD::MBDynRevolutePinJoint::asmtClassNew()
+{
+	assert(false);
+	return std::make_shared<ASMTJoint>();
+}

OndselSolver/MBDynRevolutePinJoint.h

@@ -0,0 +1,26 @@
+/***************************************************************************
+ *   Copyright (c) 2023 Ondsel, Inc.                                       *
+ *                                                                         *
+ *   This file is part of OndselSolver.                                    *
+ *                                                                         *
+ *   See LICENSE file for details about copyright.                         *
+ ***************************************************************************/
+
+#pragma once
+#include "MBDynJoint.h"
+
+namespace MbD {
+    class ASMTJoint;
+
+    class MBDynRevolutePinJoint : public MBDynJoint
+    {
+        //Note: this is equivalent to a revolute hinge (see Section 8.12.38) when one node is grounded.
+
+    public:
+        void parseMBDyn(std::string line) override;
+        void createASMT() override;
+        std::shared_ptr<ASMTJoint> asmtClassNew() override;
+
+    };
+}
+

OndselSolver/MBDynSphericalHingeJoint.cpp

@@ -0,0 +1,27 @@
+/***************************************************************************
+ *   Copyright (c) 2023 Ondsel, Inc.                                       *
+ *                                                                         *
+ *   This file is part of OndselSolver.                                    *
+ *                                                                         *
+ *   See LICENSE file for details about copyright.                         *
+ ***************************************************************************/
+
+#include "MBDynSphericalHingeJoint.h"
+#include "ASMTSphericalJoint.h"
+
+using namespace MbD;
+
+void MbD::MBDynSphericalHingeJoint::parseMBDyn(std::string line)
+{
+	MBDynJoint::parseMBDyn(line);
+}
+
+void MbD::MBDynSphericalHingeJoint::createASMT()
+{
+	MBDynJoint::createASMT();
+}
+
+std::shared_ptr<ASMTJoint> MbD::MBDynSphericalHingeJoint::asmtClassNew()
+{
+	return std::make_shared<ASMTSphericalJoint>();
+}

OndselSolver/MBDynSphericalHingeJoint.h

@@ -0,0 +1,23 @@
+/***************************************************************************
+ *   Copyright (c) 2023 Ondsel, Inc.                                       *
+ *                                                                         *
+ *   This file is part of OndselSolver.                                    *
+ *                                                                         *
+ *   See LICENSE file for details about copyright.                         *
+ ***************************************************************************/
+
+#pragma once
+#include "MBDynJoint.h"
+
+namespace MbD {
+    class ASMTJoint;
+
+    class MBDynSphericalHingeJoint : public MBDynJoint
+    {
+    public:
+        void parseMBDyn(std::string line) override;
+        void createASMT() override;
+        std::shared_ptr<ASMTJoint> asmtClassNew() override;
+
+    };
+}

OndselSolver/MBDynStructural.cpp

@@ -20,25 +20,7 @@ MbD::MBDynStructural::MBDynStructural()
 void MbD::MBDynStructural::parseMBDyn(std::string line)
 {
 	strucString = line;
-	size_t previousPos = 0;
-	auto pos = line.find(":");
-	auto front = line.substr(previousPos, pos - previousPos);
-	assert(front.find("structural") != std::string::npos);
-	auto arguments = std::vector<std::string>();
-	std::string argument;
-	while (true) {
-		previousPos = pos;
-		pos = line.find(",", pos + 1);
-		if (pos != std::string::npos) {
-			argument = line.substr(previousPos + 1, pos - previousPos - 1);
-			arguments.push_back(argument);
-		}
-		else {
-			argument = line.substr(previousPos + 1);
-			arguments.push_back(argument);
-			break;
-		}
-	}
+	arguments = collectArgumentsFor("structural", line);
 	std::istringstream iss(arguments.at(0));
 	iss >> name;
 	arguments.erase(arguments.begin());

OndselSolver/MBDynSystem.cpp

@@ -346,7 +346,8 @@ void MbD::MBDynSystem::parseMBDynElements(std::vector<std::string>& lines)
 		}
 		it = findLineWith(lines, jointTokens);
 		if (it != lines.end()) {
-			auto joint = std::make_shared<MBDynJoint>();
+			//auto joint = std::make_shared<MBDynJoint>();
+			auto joint = MBDynJoint::newJoint(*it);
 			joint->owner = this;
 			joint->parseMBDyn(*it);
 			joints->push_back(joint);

OndselSolver/MBDynTotalJoint.cpp

@@ -0,0 +1,123 @@
+/***************************************************************************
+ *   Copyright (c) 2023 Ondsel, Inc.                                       *
+ *                                                                         *
+ *   This file is part of OndselSolver.                                    *
+ *                                                                         *
+ *   See LICENSE file for details about copyright.                         *
+ ***************************************************************************/
+
+#include <regex>
+
+#include "MBDynTotalJoint.h"
+#include "ASMTAssembly.h"
+#include "ASMTJoint.h"
+#include "ASMTGeneralMotion.h"
+
+using namespace MbD;
+
+void MbD::MBDynTotalJoint::parseMBDyn(std::string statement)
+{
+	MBDynJoint::parseMBDyn(statement);
+	readPositionConstraints(arguments);
+	readOrientationConstraints(arguments);
+	return;
+}
+
+void MbD::MBDynTotalJoint::readMarkerI(std::vector<std::string>& args)
+{
+	mkr1 = std::make_shared<MBDynMarker>();
+	mkr1->owner = this;
+	mkr1->nodeStr = readStringOffTop(args);
+	auto _nodeNames = nodeNames();
+	std::string nodeName;
+	auto it = std::find_if(args.begin(), args.end(), [&](const std::string& s) {
+		auto iss = std::istringstream(s);
+		iss >> nodeName;
+		if (std::find(_nodeNames.begin(), _nodeNames.end(), nodeName) != _nodeNames.end()) return true;
+		return false;
+		});
+	auto markerArgs = std::vector<std::string>(args.begin(), it);
+	args.erase(args.begin(), it);
+	mkr1->parseMBDynTotalJointMarker(markerArgs);
+}
+
+void MbD::MBDynTotalJoint::readMarkerJ(std::vector<std::string>& args)
+{
+	if (args.empty()) return;
+	mkr2 = std::make_shared<MBDynMarker>();
+	mkr2->owner = this;
+	mkr2->nodeStr = readStringOffTop(args);
+	mkr2->parseMBDynTotalJointMarker(args);
+}
+
+void MbD::MBDynTotalJoint::readPositionConstraints(std::vector<std::string>& args)
+{
+	std::vector<std::string> tokens{ "position", "constraint" };
+	assert(lineHasTokens(popOffTop(args), tokens));
+	positionConstraints = std::vector<std::string>();
+	positionConstraints.push_back(readStringOffTop(args));
+	positionConstraints.push_back(readStringOffTop(args));
+	positionConstraints.push_back(readStringOffTop(args));
+	readPositionFormulas(args);
+}
+
+void MbD::MBDynTotalJoint::readOrientationConstraints(std::vector<std::string>& args)
+{
+	std::vector<std::string> tokens{ "orientation", "constraint" };
+	assert(lineHasTokens(popOffTop(args), tokens));
+	orientationConstraints = std::vector<std::string>();
+	orientationConstraints.push_back(readStringOffTop(args));
+	orientationConstraints.push_back(readStringOffTop(args));
+	orientationConstraints.push_back(readStringOffTop(args));
+	readOrientationFormulas(args);
+}
+
+void MbD::MBDynTotalJoint::readPositionFormulas(std::vector<std::string>& args)
+{
+	std::string str = readStringOffTop(args);
+	if (str == "null") return;
+	assert(false);
+}
+
+void MbD::MBDynTotalJoint::readOrientationFormulas(std::vector<std::string>& args)
+{
+	std::string str = readStringOffTop(args);
+	if (str == "null") { return; }
+	else if (str == "single") {
+		auto vec3 = readVector3(args);
+		assert(vec3->at(0) == 0 && vec3->at(1) == 0 && vec3->at(2) == 1);
+		assert(readStringOffTop(args) == "string");
+		formula = popOffTop(args);
+		formula = std::regex_replace(formula, std::regex("\""), "");
+		orientationFormulas = std::vector<std::string>();
+		for (auto& status : orientationConstraints) {
+			if (status == "active") {
+				orientationFormulas.push_back("");
+			}
+			else if (status == "rotation") {
+				orientationFormulas.push_back(formula);
+			}
+			else { assert(false); }
+		}
+		return;
+	}
+	assert(false);
+}
+
+void MbD::MBDynTotalJoint::createASMT()
+{
+	mkr1->createASMT();
+	if (mkr2) mkr2->createASMT();
+	auto asmtAsm = asmtAssembly();
+	auto asmtMotion = std::make_shared<ASMTGeneralMotion>();
+	asmtItem = asmtMotion;
+	asmtMotion->setName(name);
+	asmtMotion->setMarkerI(mkr1->asmtItem->fullName(""));
+	asmtMotion->setMarkerJ(mkr2->asmtItem->fullName(""));
+	asmtAsm->addMotion(asmtMotion);
+	for (int i = 0; i < 3; i++)
+	{
+		asmtMotion->rIJI->atiput(i, asmtFormula(positionFormulas.at(i)));
+		asmtMotion->angIJJ->atiput(i, asmtFormula(orientationFormulas.at(i)));
+	}
+}

OndselSolver/MBDynTotalJoint.h

@@ -0,0 +1,30 @@
+/***************************************************************************
+ *   Copyright (c) 2023 Ondsel, Inc.                                       *
+ *                                                                         *
+ *   This file is part of OndselSolver.                                    *
+ *                                                                         *
+ *   See LICENSE file for details about copyright.                         *
+ ***************************************************************************/
+
+#pragma once
+#include "MBDynJoint.h"
+
+namespace MbD {
+    class MBDynTotalJoint : public MBDynJoint
+    {
+    public:
+        void parseMBDyn(std::string line) override;
+        void readMarkerI(std::vector<std::string>& args);
+        void readMarkerJ(std::vector<std::string>& args);
+        void readPositionConstraints(std::vector<std::string>& args);
+        void readOrientationConstraints(std::vector<std::string>& args);
+        void readPositionFormulas(std::vector<std::string>& args);
+        void readOrientationFormulas(std::vector<std::string>& args);
+        void createASMT() override;
+
+        std::vector<std::string> positionConstraints = std::vector<std::string>(3);
+        std::vector<std::string> orientationConstraints = std::vector<std::string>(3);
+        std::vector<std::string> positionFormulas = std::vector<std::string>(3);
+        std::vector<std::string> orientationFormulas = std::vector<std::string>(3);
+    };
+}

OndselSolver/OndselSolver.vcxproj

@@ -150,6 +150,7 @@
     <ClCompile Include="ArcSine.cpp" />
     <ClCompile Include="ArcTan.cpp" />
     <ClCompile Include="ArcTan2.cpp" />
+    <ClCompile Include="Arguments.cpp" />
     <ClCompile Include="Array.cpp" />
     <ClCompile Include="ASMTAngleJoint.cpp" />
     <ClCompile Include="ASMTAnimationParameters.cpp" />
@@ -233,6 +234,7 @@
     <ClCompile Include="Exponential.cpp" />
     <ClCompile Include="ExternalSystem.cpp" />
     <ClCompile Include="FunctionFromData.cpp" />
+    <ClCompile Include="Functions.cpp" />
     <ClCompile Include="FunctionXcParameter.cpp" />
     <ClCompile Include="FunctionXY.cpp" />
     <ClCompile Include="GeneralSpline.cpp" />
@@ -240,21 +242,31 @@
     <ClCompile Include="Ln.cpp" />
     <ClCompile Include="Log10.cpp" />
     <ClCompile Include="LogN.cpp" />
+    <ClCompile Include="MBDynAxialRotationJoint.cpp" />
     <ClCompile Include="MBDynBlock.cpp" />
     <ClCompile Include="MBDynBody.cpp" />
+    <ClCompile Include="MBDynClampJoint.cpp" />
     <ClCompile Include="MBDynControlData.cpp" />
     <ClCompile Include="MBDynData.cpp" />
     <ClCompile Include="MBDynDrive.cpp" />
+    <ClCompile Include="MBDynDriveHingeJoint.cpp" />
     <ClCompile Include="MBDynElement.cpp" />
     <ClCompile Include="MBDynGravity.cpp" />
     <ClCompile Include="MBDynInitialValue.cpp" />
+    <ClCompile Include="MBDynInLineJoint.cpp" />
+    <ClCompile Include="MBDynInPlaneJoint.cpp" />
     <ClCompile Include="MBDynItem.cpp" />
     <ClCompile Include="MBDynJoint.cpp" />
     <ClCompile Include="MBDynMarker.cpp" />
     <ClCompile Include="MBDynNode.cpp" />
+    <ClCompile Include="MBDynPrismaticJoint.cpp" />
     <ClCompile Include="MBDynReference.cpp" />
+    <ClCompile Include="MBDynRevoluteHingeJoint.cpp" />
+    <ClCompile Include="MBDynRevolutePinJoint.cpp" />
+    <ClCompile Include="MBDynSphericalHingeJoint.cpp" />
     <ClCompile Include="MBDynStructural.cpp" />
     <ClCompile Include="MBDynSystem.cpp" />
+    <ClCompile Include="MBDynTotalJoint.cpp" />
     <ClCompile Include="MomentOfInertiaSolver.cpp" />
     <ClCompile Include="Negative.cpp" />
     <ClCompile Include="PiecewiseFunction.cpp" />
@@ -423,6 +435,7 @@
     <ClCompile Include="Time.cpp" />
     <ClCompile Include="TooManyTriesError.cpp" />
     <ClCompile Include="TooSmallStepSizeError.cpp" />
+    <ClCompile Include="Transitions.cpp" />
     <ClCompile Include="Translation.cpp" />
     <ClCompile Include="TranslationalJoint.cpp" />
     <ClCompile Include="TranslationConstraintIJ.cpp" />
@@ -459,6 +472,7 @@
     <ClInclude Include="ArcSine.h" />
     <ClInclude Include="ArcTan.h" />
     <ClInclude Include="ArcTan2.h" />
+    <ClInclude Include="Arguments.h" />
     <ClInclude Include="Array.h" />
     <ClInclude Include="ASMTAngleJoint.h" />
     <ClInclude Include="ASMTAnimationParameters.h" />
@@ -542,6 +556,7 @@
     <ClInclude Include="Exponential.h" />
     <ClInclude Include="ExternalSystem.h" />
     <ClInclude Include="FunctionFromData.h" />
+    <ClInclude Include="Functions.h" />
     <ClInclude Include="FunctionXcParameter.h" />
     <ClInclude Include="FunctionXY.h" />
     <ClInclude Include="GeneralSpline.h" />
@@ -549,21 +564,31 @@
     <ClInclude Include="Ln.h" />
     <ClInclude Include="Log10.h" />
     <ClInclude Include="LogN.h" />
+    <ClInclude Include="MBDynAxialRotationJoint.h" />
     <ClInclude Include="MBDynBlock.h" />
     <ClInclude Include="MBDynBody.h" />
+    <ClInclude Include="MBDynClampJoint.h" />
     <ClInclude Include="MBDynControlData.h" />
     <ClInclude Include="MBDynData.h" />
     <ClInclude Include="MBDynDrive.h" />
+    <ClInclude Include="MBDynDriveHingeJoint.h" />
     <ClInclude Include="MBDynElement.h" />
     <ClInclude Include="MBDynGravity.h" />
     <ClInclude Include="MBDynInitialValue.h" />
+    <ClInclude Include="MBDynInLineJoint.h" />
+    <ClInclude Include="MBDynInPlaneJoint.h" />
     <ClInclude Include="MBDynItem.h" />
     <ClInclude Include="MBDynJoint.h" />
     <ClInclude Include="MBDynMarker.h" />
     <ClInclude Include="MBDynNode.h" />
+    <ClInclude Include="MBDynPrismaticJoint.h" />
     <ClInclude Include="MBDynReference.h" />
+    <ClInclude Include="MBDynRevoluteHingeJoint.h" />
+    <ClInclude Include="MBDynRevolutePinJoint.h" />
+    <ClInclude Include="MBDynSphericalHingeJoint.h" />
     <ClInclude Include="MBDynStructural.h" />
     <ClInclude Include="MBDynSystem.h" />
+    <ClInclude Include="MBDynTotalJoint.h" />
     <ClInclude Include="MomentOfInertiaSolver.h" />
     <ClInclude Include="Negative.h" />
     <ClInclude Include="PiecewiseFunction.h" />
@@ -734,6 +759,7 @@
     <ClInclude Include="Time.h" />
     <ClInclude Include="TooManyTriesError.h" />
     <ClInclude Include="TooSmallStepSizeError.h" />
+    <ClInclude Include="Transitions.h" />
     <ClInclude Include="Translation.h" />
     <ClInclude Include="TranslationalJoint.h" />
     <ClInclude Include="TranslationConstraintIJ.h" />
@@ -772,6 +798,8 @@
     <None Include="..\testapp\Gears.asmt" />
     <None Include="..\testapp\gyro.asmt" />
     <None Include="..\testapp\InitialConditions.mbd" />
+    <None Include="..\testapp\MBDynCase(Cosine-half drive).mbd" />
+    <None Include="..\testapp\MBDynCase(Sine-forever drive).mbd" />
     <None Include="..\testapp\MBDynCase.mbd" />
     <None Include="..\testapp\MBDynCase2.mbd" />
     <None Include="..\testapp\MBDynCase2orig.mbd" />
@@ -779,6 +807,7 @@
     <None Include="..\testapp\MBDynCase4orig.mbd" />
     <None Include="..\testapp\MBDynCase5orig.mbd" />
     <None Include="..\testapp\MBDynCase6orig.mbd" />
+    <None Include="..\testapp\MBDynCaseBird.mbd" />
     <None Include="..\testapp\MBDynCaseDebug1.mbd" />
     <None Include="..\testapp\MBDynCaseDebug2.mbd" />
     <None Include="..\testapp\piston.asmt" />
@@ -793,6 +822,8 @@
     <Media Include="..\testapp\CrankSlider2.mov" />
     <Media Include="..\testapp\InitialConditionsMBDyn.mov" />
     <Media Include="..\testapp\InitialConditionsOndselSolver.mov" />
+    <Media Include="..\testapp\MBDynCase(Cosine-half drive).mov" />
+    <Media Include="..\testapp\MBDynCase(Sine-forever drive).mov" />
     <Media Include="..\testapp\MBDynCase.mov" />
     <Media Include="..\testapp\MBDynCase2.mov" />
     <Media Include="..\testapp\SphericalHinge.mov" />

OndselSolver/OndselSolver.vcxproj.filters

@@ -936,6 +936,45 @@
     <ClCompile Include="ASMTCompoundJoint.cpp">
       <Filter>Source Files</Filter>
     </ClCompile>
+    <ClCompile Include="MBDynAxialRotationJoint.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="MBDynClampJoint.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="MBDynDriveHingeJoint.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="MBDynInLineJoint.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="MBDynInPlaneJoint.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="MBDynPrismaticJoint.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="MBDynRevoluteHingeJoint.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="MBDynRevolutePinJoint.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="MBDynSphericalHingeJoint.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="MBDynTotalJoint.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="Functions.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="Transitions.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="Arguments.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
   </ItemGroup>
   <ItemGroup>
     <ClInclude Include="Array.h">
@@ -1865,6 +1904,45 @@
     <ClInclude Include="ASMTCompoundJoint.h">
       <Filter>Header Files</Filter>
     </ClInclude>
+    <ClInclude Include="MBDynAxialRotationJoint.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="MBDynClampJoint.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="MBDynDriveHingeJoint.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="MBDynInLineJoint.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="MBDynInPlaneJoint.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="MBDynPrismaticJoint.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="MBDynRevoluteHingeJoint.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="MBDynRevolutePinJoint.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="MBDynSphericalHingeJoint.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="MBDynTotalJoint.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="Functions.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="Transitions.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="Arguments.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
   </ItemGroup>
   <ItemGroup>
     <ResourceCompile Include="OndselSolver.rc">
@@ -1903,6 +1981,9 @@
     <None Include="..\testapp\MBDynCase3orig.mbd" />
     <None Include="..\testapp\MBDynCase5orig.mbd" />
     <None Include="..\testapp\MBDynCase6orig.mbd" />
+    <None Include="..\testapp\MBDynCase(Cosine-half drive).mbd" />
+    <None Include="..\testapp\MBDynCase(Sine-forever drive).mbd" />
+    <None Include="..\testapp\MBDynCaseBird.mbd" />
   </ItemGroup>
   <ItemGroup>
     <Media Include="..\testapp\CrankSlider2.mov" />
@@ -1911,6 +1992,8 @@
     <Media Include="..\testapp\MBDynCase.mov" />
     <Media Include="..\testapp\MBDynCase2.mov" />
     <Media Include="..\testapp\SphericalHinge.mov" />
+    <Media Include="..\testapp\MBDynCase(Cosine-half drive).mov" />
+    <Media Include="..\testapp\MBDynCase(Sine-forever drive).mov" />
   </ItemGroup>
   <ItemGroup>
     <Text Include="..\CMakeLists.txt" />

OndselSolver/PiecewiseFunction.cpp

@@ -94,6 +94,14 @@ double MbD::PiecewiseFunction::getValue()
 	return functions->back()->getValue();
 }
 
+void MbD::PiecewiseFunction::arguments(Symsptr args)
+{
+	auto arguments = args->getTerms();
+	xx = arguments->at(0);
+	functions = arguments->at(1)->getTerms();
+	transitions = arguments->at(2)->getTerms();
+}
+
 std::ostream& MbD::PiecewiseFunction::printOn(std::ostream& s) const
 {
 	s << "PiecewiseFunction(" << *xx << ", " << std::endl;

OndselSolver/PiecewiseFunction.h

@@ -26,6 +26,7 @@ namespace MbD {
         Symsptr differentiateWRTx() override;
         Symsptr integrateWRT(Symsptr var) override;
         double getValue() override;
+        void arguments(Symsptr args) override;
 
         std::ostream& printOn(std::ostream& s) const override;
 

OndselSolver/SymbolicParser.cpp

@@ -27,6 +27,9 @@
 #include "ArcSine.h"
 #include "Integral.h"
 #include "RampStepFunction.h"
+#include "Arguments.h"
+#include "Functions.h"
+#include "Transitions.h"
 
 MbD::SymbolicParser::SymbolicParser()
 {
@@ -283,6 +286,11 @@ bool MbD::SymbolicParser::symfunction()
 
 bool MbD::SymbolicParser::expression()
 {
+	if (token == "" && tokenType == "end") {
+		auto symNum = std::make_shared<Constant>(0.0);
+		stack->push(symNum);
+		return true;
+	}
 	auto sum = std::make_shared<Sum>();
 	stack->push(sum);
 	if (plusTerm() || minusTerm() || plainTerm()) {
@@ -369,6 +377,15 @@ bool MbD::SymbolicParser::intrinsic()
 	else if (peekForTypevalue("word", "rampstep")) {
 		symfunc = std::make_shared<RampStepFunction>();
 	}
+	else if (peekForTypevalue("word", "piecewise")) {
+		symfunc = std::make_shared<PiecewiseFunction>();
+	}
+	else if (peekForTypevalue("word", "functions")) {
+		symfunc = std::make_shared<Functions>();
+	}
+	else if (peekForTypevalue("word", "transitions")) {
+		symfunc = std::make_shared<Transitions>();
+	}
 	if (symfunc != nullptr) {
 		stack->push(symfunc);
 		if (peekForTypeNoPush("(")) {
@@ -493,7 +510,7 @@ void MbD::SymbolicParser::combineStackTo(int pos)
 		args->push_back(arg);
 	}
 	std::reverse(args->begin(), args->end());
-	auto sum = std::make_shared<Sum>();
+	auto sum = std::make_shared<Arguments>();
 	sum->terms = args;
 	stack->push(sum);
 }

OndselSolver/Transitions.cpp

@@ -0,0 +1,2 @@
+#include "Transitions.h"
+

OndselSolver/Transitions.h

@@ -0,0 +1,21 @@
+/***************************************************************************
+ *   Copyright (c) 2023 Ondsel, Inc.                                       *
+ *                                                                         *
+ *   This file is part of OndselSolver.                                    *
+ *                                                                         *
+ *   See LICENSE file for details about copyright.                         *
+ ***************************************************************************/
+
+#pragma once
+
+#include "Arguments.h"
+
+namespace MbD {
+	class Transitions : public Arguments
+	{
+	public:
+
+	};
+}
+
+

testapp/MBDynCase(Cosine-half drive)debug1.asmt

@@ -0,0 +1,200 @@
+OndselSolver
+Assembly
+	Notes
+		(Text string: '' runs: (Core.RunArray runs: #() values: #()))
+	Name
+		Assembly
+	Position3D
+		0	0	0	
+	RotationMatrix
+		1	0	0	
+		0	1	0	
+		0	0	1	
+	Velocity3D
+		0	0	0	
+	Omega3D
+		0	0	0	
+	RefPoints
+		RefPoint
+			Position3D
+				0	0	0	
+			RotationMatrix
+				1	0	0	
+				0	1	0	
+				0	0	1	
+			Markers
+				Marker
+					Name
+						Marker0
+					Position3D
+						0	0	0	
+					RotationMatrix
+						1	0	0	
+						0	1	0	
+						0	0	1	
+	RefCurves
+	RefSurfaces
+	Parts
+		Part
+			Name
+				structural_node_1
+			Position3D
+				-0.1322131193924678	0.04215394533365132	0.2000000152587891	
+			RotationMatrix
+				1	0	0	
+				0	1	0	
+				0	0	1	
+			Velocity3D
+				0	0	0	
+			Omega3D
+				0	0	0	
+			FeatureOrder
+			PrincipalMassMarker
+				Name
+					MassMarker
+				Position3D
+					0	0	0	
+				RotationMatrix
+					1	0	0	
+					0	1	0	
+					0	0	1	
+				Mass
+					1
+				MomentOfInertias
+					1	2	3	
+				Density
+					10
+			RefPoints
+				RefPoint
+					Position3D
+						0	0	0	
+					RotationMatrix
+						1	0	0	
+						0	1	0	
+						0	0	1	
+					Markers
+						Marker
+							Name
+								Marker0
+							Position3D
+								0.1322131193924678	-0.04215394533365132	-0.2000000152587891	
+							RotationMatrix
+								1	0	0	
+								0	1	0	
+								0	0	1	
+				RefPoint
+					Position3D
+						0	0	0	
+					RotationMatrix
+						1	0	0	
+						0	1	0	
+						0	0	1	
+					Markers
+						Marker
+							Name
+								Marker1
+							Position3D
+								0.2032130965042842	0.117846068852255	-2.842170943040401e-17	
+							RotationMatrix
+								1	-0	0	
+								0	1	2.471452993948174e-16	
+								-0	-2.471452993948174e-16	1	
+			RefCurves
+			RefSurfaces
+		Part
+			Name
+				structural_node_2
+			Position3D
+				0.4257698614077033	0.1732822793381788	0.302175885912376	
+			RotationMatrix
+				-0.8995965927692124	0.4234982246408653	-0.1066546951805642	
+				-0.4367218454325635	-0.8723574602808865	0.2196963614042295	
+				1.387778780781446e-17	0.2442165334663419	0.969720725148062	
+			Velocity3D
+				0	0	0	
+			Omega3D
+				0	0	0	
+			FeatureOrder
+			PrincipalMassMarker
+				Name
+					MassMarker
+				Position3D
+					-0.003947705364974695	0.01042661386112648	-1.13686837721616e-16	
+				RotationMatrix
+					1	0	0	
+					0	1	0	
+					0	0	1	
+				Mass
+					12.63583350443436
+				MomentOfInertias
+					0.08708924866282601	0.085370944710882	0.06459712318937701	
+				Density
+					10
+			RefPoints
+				RefPoint
+					Position3D
+						0	0	0	
+					RotationMatrix
+						1	0	0	
+						0	1	0	
+						0	0	1	
+					Markers
+						Marker
+							Name
+								Marker0
+							Position3D
+								0	0	0	
+							RotationMatrix
+								1	0	0	
+								0	1	0	
+								0	0	1	
+			RefCurves
+			RefSurfaces
+	KinematicIJs
+	ConstraintSets
+		Joints
+			FixedJoint
+				Name
+					joint_1
+				MarkerI
+					/Assembly/Marker0
+				MarkerJ
+					/Assembly/structural_node_1/Marker0
+		Motions
+			GeneralMotion
+				Name
+					joint_2
+				MarkerI
+					/Assembly/structural_node_1/Marker1
+				MarkerJ
+					/Assembly/structural_node_2/Marker0
+		GeneralConstraintSets
+	ForceTorques
+	ConstantGravity
+		0	0	0	
+	SimulationParameters
+		tstart
+			0
+		tend
+			4
+		hmin
+			1e-09
+		hmax
+			1
+		hout
+			0.01
+		errorTol
+			1e-06
+	AnimationParameters
+		nframe
+			1000000
+		icurrent
+			1
+		istart
+			1
+		iend
+			1000000
+		isForward
+			true
+		framesPerSecond
+			30

testapp/MBDynCase(Sine-forever drive)debug1.asmt

@@ -0,0 +1,200 @@
+OndselSolver
+Assembly
+	Notes
+		(Text string: '' runs: (Core.RunArray runs: #() values: #()))
+	Name
+		Assembly
+	Position3D
+		0	0	0	
+	RotationMatrix
+		1	0	0	
+		0	1	0	
+		0	0	1	
+	Velocity3D
+		0	0	0	
+	Omega3D
+		0	0	0	
+	RefPoints
+		RefPoint
+			Position3D
+				0	0	0	
+			RotationMatrix
+				1	0	0	
+				0	1	0	
+				0	0	1	
+			Markers
+				Marker
+					Name
+						Marker0
+					Position3D
+						0	0	0	
+					RotationMatrix
+						1	0	0	
+						0	1	0	
+						0	0	1	
+	RefCurves
+	RefSurfaces
+	Parts
+		Part
+			Name
+				structural_node_1
+			Position3D
+				-0.1322131193924678	0.04215394533365132	0.2000000152587891	
+			RotationMatrix
+				1	0	0	
+				0	1	0	
+				0	0	1	
+			Velocity3D
+				0	0	0	
+			Omega3D
+				0	0	0	
+			FeatureOrder
+			PrincipalMassMarker
+				Name
+					MassMarker
+				Position3D
+					0	0	0	
+				RotationMatrix
+					1	0	0	
+					0	1	0	
+					0	0	1	
+				Mass
+					1
+				MomentOfInertias
+					1	2	3	
+				Density
+					10
+			RefPoints
+				RefPoint
+					Position3D
+						0	0	0	
+					RotationMatrix
+						1	0	0	
+						0	1	0	
+						0	0	1	
+					Markers
+						Marker
+							Name
+								Marker0
+							Position3D
+								0.1322131193924678	-0.04215394533365132	-0.2000000152587891	
+							RotationMatrix
+								1	0	0	
+								0	1	0	
+								0	0	1	
+				RefPoint
+					Position3D
+						0	0	0	
+					RotationMatrix
+						1	0	0	
+						0	1	0	
+						0	0	1	
+					Markers
+						Marker
+							Name
+								Marker1
+							Position3D
+								0.2032130965042842	0.117846068852255	-2.842170943040401e-17	
+							RotationMatrix
+								1	-0	0	
+								0	1	2.471452993948174e-16	
+								-0	-2.471452993948174e-16	1	
+			RefCurves
+			RefSurfaces
+		Part
+			Name
+				structural_node_2
+			Position3D
+				0.4257698614077033	0.1732822793381788	0.302175885912376	
+			RotationMatrix
+				-0.8995965927692124	0.4234982246408653	-0.1066546951805642	
+				-0.4367218454325635	-0.8723574602808865	0.2196963614042295	
+				1.387778780781446e-17	0.2442165334663419	0.969720725148062	
+			Velocity3D
+				0	0	0	
+			Omega3D
+				0	0	0	
+			FeatureOrder
+			PrincipalMassMarker
+				Name
+					MassMarker
+				Position3D
+					-0.003947705364974695	0.01042661386112648	-1.13686837721616e-16	
+				RotationMatrix
+					1	0	0	
+					0	1	0	
+					0	0	1	
+				Mass
+					12.63583350443436
+				MomentOfInertias
+					0.08708924866282601	0.085370944710882	0.06459712318937701	
+				Density
+					10
+			RefPoints
+				RefPoint
+					Position3D
+						0	0	0	
+					RotationMatrix
+						1	0	0	
+						0	1	0	
+						0	0	1	
+					Markers
+						Marker
+							Name
+								Marker0
+							Position3D
+								0	0	0	
+							RotationMatrix
+								1	0	0	
+								0	1	0	
+								0	0	1	
+			RefCurves
+			RefSurfaces
+	KinematicIJs
+	ConstraintSets
+		Joints
+			FixedJoint
+				Name
+					joint_1
+				MarkerI
+					/Assembly/Marker0
+				MarkerJ
+					/Assembly/structural_node_1/Marker0
+		Motions
+			GeneralMotion
+				Name
+					joint_2
+				MarkerI
+					/Assembly/structural_node_1/Marker1
+				MarkerJ
+					/Assembly/structural_node_2/Marker0
+		GeneralConstraintSets
+	ForceTorques
+	ConstantGravity
+		0	0	0	
+	SimulationParameters
+		tstart
+			0
+		tend
+			4
+		hmin
+			1e-09
+		hmax
+			1
+		hout
+			0.01
+		errorTol
+			1e-06
+	AnimationParameters
+		nframe
+			1000000
+		icurrent
+			1
+		istart
+			1
+		iend
+			1000000
+		isForward
+			true
+		framesPerSecond
+			30

testapp/MBDynSliderCrankTotalJoint.mbd

@@ -0,0 +1,317 @@
+#-----------------------------------------------------------------------------
+ # [Data Block]
+
+ begin: data;
+     problem: initial value;
+ end: data;
+
+ #-----------------------------------------------------------------------------
+ # [Problem Block]
+
+ begin: initial value;
+     initial time: 0.0;
+     final time: 4.0;
+     time step: 0.01;
+     max iterations: 100;
+     tolerance: 1e-06;
+     derivatives tolerance: 0.0001;
+     derivatives max iterations: 100;
+     derivatives coefficient: auto;
+ end: initial value;
+
+ #-----------------------------------------------------------------------------
+ # [Control Data Block]
+
+ begin: control data;
+     max iterations: 1000;
+     default orientation: orientation matrix;
+     omega rotates: no;
+     print: none;
+     initial stiffness: 1.0, 1.0;
+     structural nodes: 4;
+     rigid bodies: 3;
+     joints: 6;
+ end: control data;
+
+ #-----------------------------------------------------------------------------
+ # [Design Variables]
+
+     #Generic bodies
+
+         #body: 2
+             set: integer body_2 = 2; #body label
+             set: real mass_2 = 12.635833504434364; #mass [kg]
+             set: real volume_2 = 0.0015994725954980207; #volume [m^3]
+
+         #body: 3
+             set: integer body_3 = 3; #body label
+             set: real mass_3 = 4.761616622776476; #mass [kg]
+             set: real volume_3 = 0.0006027362813641108; #volume [m^3]
+
+         #body: 4
+             set: integer body_4 = 4; #body label
+             set: real mass_4 = 2.8656033319778023; #mass [kg]
+             set: real volume_4 = 0.00036273459898453187; #volume [m^3]
+
+     #Nodes
+
+         #node: 1
+             set: integer structural_node_1 = 1; #node label
+
+         #node: 2
+             set: integer structural_node_2 = 2; #node label
+
+         #node: 3
+             set: integer structural_node_3 = 3; #node label
+
+         #node: 4
+             set: integer structural_node_4 = 4; #node label
+
+     #Joints
+
+         #joint: 1
+             set: integer joint_1 = 1; #joint label
+
+         #joint: 2_3
+             set: integer joint_2 = 2; #joint label
+
+             set: integer joint_3 = 3; #joint label
+
+         #joint: 4_5
+             set: integer joint_4 = 4; #joint label
+
+             set: integer joint_5 = 5; #joint label
+
+         #joint: 6_7_8
+             set: integer joint_6 = 6; #joint label
+
+             set: integer joint_7 = 7; #joint label
+
+             set: integer joint_8 = 8; #joint label
+
+         #joint: 9
+             set: integer joint_9 = 9; #joint label
+
+     #Nodes: initial conditions
+
+         #node: 1
+             set: real Px_1 = -0.419; #X component of the absolute position [m]
+             set: real Py_1 = 0.09000000999999999; #Y component of the absolute position [m]
+             set: real Pz_1 = 0.2; #Z component of the absolute position [m]
+
+             set: real Vx_1 = 0.0; #X component of the absolute velocity [m/s]
+             set: real Vy_1 = 0.0; #Y component of the absolute velocity [m/s]
+             set: real Vz_1 = 0.0; #Z component of the absolute velocity [m/s]
+
+             set: real Wx_1 = 0.0; #X component of the absolute angular velocity [rad/s]
+             set: real Wy_1 = 0.0; #Y component of the absolute angular velocity [rad/s]
+             set: real Wz_1 = 0.0; #Z component of the absolute angular velocity [rad/s]
+
+         #node: 2
+             set: real Px_2 = 0.07099999999999991; #X component of the absolute position [m]
+             set: real Py_2 = 0.16; #Y component of the absolute position [m]
+             set: real Pz_2 = 0.19999999999999996; #Z component of the absolute position [m]
+
+             set: real Vx_2 = 0.0; #X component of the absolute velocity [m/s]
+             set: real Vy_2 = 0.0; #Y component of the absolute velocity [m/s]
+             set: real Vz_2 = 0.0; #Z component of the absolute velocity [m/s]
+
+             set: real Wx_2 = 0.0; #X component of the absolute angular velocity [rad/s]
+             set: real Wy_2 = 0.0; #Y component of the absolute angular velocity [rad/s]
+             set: real Wz_2 = 0.0; #Z component of the absolute angular velocity [rad/s]
+
+         #node: 3
+             set: real Px_3 = -0.017337619999999984; #X component of the absolute position [m]
+             set: real Py_3 = 0.20686650000000006; #Y component of the absolute position [m]
+             set: real Pz_3 = 0.2; #Z component of the absolute position [m]
+
+             set: real Vx_3 = 0.0; #X component of the absolute velocity [m/s]
+             set: real Vy_3 = 0.0; #Y component of the absolute velocity [m/s]
+             set: real Vz_3 = 0.0; #Z component of the absolute velocity [m/s]
+
+             set: real Wx_3 = 0.0; #X component of the absolute angular velocity [rad/s]
+             set: real Wy_3 = 0.0; #Y component of the absolute angular velocity [rad/s]
+             set: real Wz_3 = 0.0; #Z component of the absolute angular velocity [rad/s]
+
+         #node: 4
+             set: real Px_4 = -0.29363859999999997; #X component of the absolute position [m]
+             set: real Py_4 = 0.09000000999999998; #Y component of the absolute position [m]
+             set: real Pz_4 = 0.19999999999999982; #Z component of the absolute position [m]
+
+             set: real Vx_4 = 0.0; #X component of the absolute velocity [m/s]
+             set: real Vy_4 = 0.0; #Y component of the absolute velocity [m/s]
+             set: real Vz_4 = 0.0; #Z component of the absolute velocity [m/s]
+
+             set: real Wx_4 = 0.0; #X component of the absolute angular velocity [rad/s]
+             set: real Wy_4 = 0.0; #Y component of the absolute angular velocity [rad/s]
+             set: real Wz_4 = 0.0; #Z component of the absolute angular velocity [rad/s]
+
+ #-----------------------------------------------------------------------------
+ # [Intermediate Variables]
+
+     #Moments of inertia and relative center of mass
+
+         #body 2: 
+             set: real Ixx_2 = 0.08708924866282601; #moment of inertia [kg*m^2]
+             set: real Iyy_2 = 0.085370944710882; #moment of inertia [kg*m^2]
+             set: real Izz_2 = 0.064597123189377; #moment of inertia [kg*m^2]
+
+             set: real Rx_2 = 0.23369760691597463; #X component of the relative center of mass [m]
+             set: real Ry_2 = -0.017056058774585096; #Y component of the relative center of mass [m]
+             set: real Rz_2 = 0.02412068691121374; #Z component of the relative center of mass [m]
+
+         #body 3: 
+             set: real Ixx_3 = 0.052187573112937; #moment of inertia [kg*m^2]
+             set: real Iyy_3 = 0.049571280557501; #moment of inertia [kg*m^2]
+             set: real Izz_3 = 0.0035529543779500004; #moment of inertia [kg*m^2]
+
+             set: real Rx_3 = 0.10067231525120601; #X component of the relative center of mass [m]
+             set: real Ry_3 = 0.1919853054998222; #Y component of the relative center of mass [m]
+             set: real Rz_3 = 0.04071350992502829; #Z component of the relative center of mass [m]
+
+         #body 4: 
+             set: real Ixx_4 = 0.010133521085753; #moment of inertia [kg*m^2]
+             set: real Iyy_4 = 0.006853402672398001; #moment of inertia [kg*m^2]
+             set: real Izz_4 = 0.00669113151275; #moment of inertia [kg*m^2]
+
+             set: real Rx_4 = -0.0330388768314067; #X component of the relative center of mass [m]
+             set: real Ry_4 = -0.010904342272327113; #Y component of the relative center of mass [m]
+             set: real Rz_4 = -0.26627346364210613; #Z component of the relative center of mass [m]
+
+ #-----------------------------------------------------------------------------
+ # [Nodes Block]
+
+ begin: nodes;
+
+     structural: structural_node_1,
+                 static,
+                 Px_1, Py_1, Pz_1, #<absolute_position> [m]
+                 3, 1.0, -2.220446049250313e-16, 2.220446049250313e-16, 2, -2.220446049250313e-16, 0.0, 1.0, #<absolute_orientation_matrix>
+                 Vx_1, Vy_1, Vz_1, #<absolute_velocity> [m/s]
+                 Wx_1, Wy_1, Wz_1; #<absolute_angular_velocity> [rad/s]
+
+     structural: structural_node_2,
+                 dynamic,
+                 Px_2, Py_2, Pz_2, #<absolute_position> [m]
+                 3, 3.0728199502890935e-08, -8.312621051592148e-09, 0.9999999999999994, 2, 9.860761315262648e-32, 1.0, 8.312621051592151e-09, #<absolute_orientation_matrix>
+                 Vx_2, Vy_2, Vz_2, #<absolute_velocity> [m/s]
+                 Wx_2, Wy_2, Wz_2; #<absolute_angular_velocity> [rad/s]
+
+     structural: structural_node_3,
+                 dynamic,
+                 Px_3, Py_3, Pz_3, #<absolute_position> [m]
+                 3, 1.2241820623870575e-07, -7.110463684512068e-09, 0.9999999999999925, 2, -0.9983174164285165, 0.057985653876428844, 1.2262453226214997e-07, #<absolute_orientation_matrix>
+                 Vx_3, Vy_3, Vz_3, #<absolute_velocity> [m/s]
+                 Wx_3, Wy_3, Wz_3; #<absolute_angular_velocity> [rad/s]
+
+     structural: structural_node_4,
+                 dynamic,
+                 Px_4, Py_4, Pz_4, #<absolute_position> [m]
+                 3, 1.0, -2.220446049250313e-16, 2.220446049250313e-16, 2, -2.220446049250313e-16, 0.0, 1.0, #<absolute_orientation_matrix>
+                 Vx_4, Vy_4, Vz_4, #<absolute_velocity> [m/s]
+                 Wx_4, Wy_4, Wz_4; #<absolute_angular_velocity> [rad/s]
+
+ end: nodes;
+
+ #-----------------------------------------------------------------------------
+ # [Elements Block]
+
+ begin: elements;
+
+     #-----------------------------------------------------------------------------
+     # [Bodies]
+
+         body: body_2,
+               structural_node_2, #<node_label> 
+               mass_2, #<mass> [kg]
+               Rx_2, Ry_2, Rz_2, #<relative_center_of_mass> [m]
+               diag, Ixx_2, Iyy_2, Izz_2, #<inertia matrix> [kg*m^2]
+               orientation, 3, -0.12000003011363553, -0.1699999918536315, 0.9799999977257611, 2, 0.8099999999999996, -0.5899999999999999, 2.9794288017781016e-08; 
+
+         body: body_3,
+               structural_node_3, #<node_label> 
+               mass_3, #<mass> [kg]
+               Rx_3, Ry_3, Rz_3, #<relative_center_of_mass> [m]
+               diag, Ixx_3, Iyy_3, Izz_3, #<inertia matrix> [kg*m^2]
+               orientation, 3, 0.05682594079890069, -0.9783510423487942, 0.21000011996984053, 2, 0.9977375598897598, 0.06796883662443129, 0.06999999166535376; 
+
+         body: body_4,
+               structural_node_4, #<node_label> 
+               mass_4, #<mass> [kg]
+               Rx_4, Ry_4, Rz_4, #<relative_center_of_mass> [m]
+               diag, Ixx_4, Iyy_4, Izz_4, #<inertia matrix> [kg*m^2]
+               orientation, 3, -0.010000000000000047, 0.98, -0.20999999999999977, 2, 0.98, -0.029999999999999954, -0.21000000000000021; 
+
+     #-----------------------------------------------------------------------------
+     # [Joints]
+
+     joint: joint_1,
+         clamp,
+         structural_node_1, #<node_label>
+             -0.419, 0.09000000999999999, 0.2, #<absolute_pin_position> [m]
+             3, 1.0, -2.220446049250313e-16, 2.220446049250313e-16, 2, -2.220446049250313e-16, 0.0, 1.0; #<absolute_orientation_matrix> 
+
+     joint: joint_2,
+         total joint,
+         structural_node_1, #<node_1_label>
+             position, 0.07000000000000012, -5.684341886080802e-17, 0.48999999999999994, #<relative_position_1> [m]
+             position orientation, 3, 3.885780586188048e-16, 1.0, 1.1102230246251565e-16, 2, 1.0, -4.440892098500626e-16, -1.6653345369377348e-16, #<relative_pin_orientation_matrix_1>
+             rotation orientation, 3, 3.885780586188048e-16, 1.0, 1.1102230246251565e-16, 2, 1.0, -4.440892098500626e-16, -1.6653345369377348e-16, #<relative_pin_orientation_matrix_1>
+         structural_node_2, #<node_2_label>
+             position, 8.526512829121202e-17, 9.999999974752426e-09, 2.842170943040401e-17, #<relative_position_2> [m]
+             position orientation, 3, -3.0728199502890935e-08, 8.312621298737451e-09, 0.9999999999999994, 2, 2.554318856595847e-16, 1.0, -8.312621298737448e-09, #<relative_pin_orientation_matrix_2>
+             rotation orientation, 3, -3.0728199502890935e-08, 8.312621298737451e-09, 0.9999999999999994, 2, 2.554318856595847e-16, 1.0, -8.312621298737448e-09, #<relative_pin_orientation_matrix_2>
+         position constraint,
+             active, active, active,
+             null,
+         orientation constraint,
+             active, active, angular velocity,
+             single, 0., 0., 1., string, "0.0+model::drive(1, Time)"; #<angle> [rad]
+
+     joint: joint_4,
+         total joint,
+         structural_node_2, #<node_1_label>
+             position, -0.08833762115250354, 0.0468664559052288, 2.842170943040401e-17, #<relative_position_1> [m]
+             position orientation, 3, -3.7239980926641352e-16, 1.6936012837021416e-16, 1.0, 2, 5.204131742837832e-24, 1.0, -1.6936012837021416e-16, #<relative_pin_orientation_matrix_1>
+             rotation orientation, 3, -3.7239980926641352e-16, 1.6936012837021416e-16, 1.0, 2, 5.204131742837832e-24, 1.0, -1.6936012837021416e-16, #<relative_pin_orientation_matrix_1>
+         structural_node_3, #<node_2_label>
+             position, -4.408740682038115e-08, -1.4062993436425586e-09, 3.1040392514114503e-09, #<relative_position_2> [m]
+             position orientation, 3, -6.51683948387302e-09, 9.146602313741393e-08, 0.9999999999999958, 2, 0.9983174164285239, 0.057985653876430066, 1.2021571977199034e-09, #<relative_pin_orientation_matrix_2>
+             rotation orientation, 3, -6.51683948387302e-09, 9.146602313741393e-08, 0.9999999999999958, 2, 0.9983174164285239, 0.057985653876430066, 1.2021571977199034e-09, #<relative_pin_orientation_matrix_2>
+         position constraint,
+             active, active, active,
+             null,
+         orientation constraint,
+             active, active, inactive,
+             null;
+
+     joint: joint_6,
+         in line,
+         structural_node_1, #<node_1_label>
+             0.0, 0.0, 0.0, #<relative_line_position> [m]
+             3, 0.0, 0.0, 1.0, 2, 0.0, 1.0, 0.0, #<relative_orientation>
+         structural_node_4, #<node_2_label>
+             offset, 0.0, 0.0, 0.0; #<relative_offset> [m]
+
+     joint: joint_7,
+         prismatic,
+         structural_node_1, #<node_1_label>
+             orientation, 3, 0.0, 0.0, 1.0, 2, 0.0, 1.0, 0.0, #relative_orientation_matrix_1>
+         structural_node_4, #<node_2_label>
+             orientation, 3, 0, 0, 1, 2, 0, 1, 0; #relative_orientation_matrix_2>
+
+     joint: joint_9,
+         in line,
+         structural_node_3, #<node_1_label>
+             -0.13269133782709974, 0.26905948945475705, 1.4177195929807734e-15, #<relative_line_position> [m]
+             3, 1.3390417087237118e-16, -2.3053784001404156e-15, 1.0, 2, -1.0919852799773366e-09, 1.0, 2.3053784002866372e-15, #<relative_orientation>
+         structural_node_4, #<node_2_label>
+             offset, 0.0, 0.0, 0.0; #<relative_offset> [m]
+
+     #-----------------------------------------------------------------------------
+     # [Drive callers]
+
+     drive caller: 1, name,"drive:1", cosine, 0.25, 3.1416, 10.0, half, 0.0;
+
+ end: elements;
+

testapp/OndselSolver.cpp

@@ -26,6 +26,8 @@ void sharedptrTest();
 
 int main()
 {
+	//MBDynSystem::runFile("../testapp/MBDynCase(Cosine-half drive).mbd");
+	//MBDynSystem::runFile("../testapp/MBDynCase(Sine-forever drive).mbd");
 	MBDynSystem::runFile("../testapp/MBDynCase9orig.mbd");	//SimulationStoppingError
 	MBDynSystem::runFile("../testapp/MBDynCase8orig.mbd");	//Incompatible geometry at t=3.15
 	MBDynSystem::runFile("../testapp/MBDynCase5orig.mbd");	//Test Product::integrateWRT