Merge pull request #15 from Ondsel-Development/working

crank_slider.mbd MBDyn file working

OndselSolver/ASMTAssembly.cpp

@@ -43,6 +43,11 @@
 
 using namespace MbD;
 
+MbD::ASMTAssembly::ASMTAssembly() : ASMTSpatialContainer()
+{
+	times = std::make_shared<FullRow<double>>();
+}
+
 void MbD::ASMTAssembly::runSinglePendulumSuperSimplified()
 {
 	//In this version we skip declaration of variables that don't need as they use default values.
@@ -82,6 +87,64 @@ void MbD::ASMTAssembly::runSinglePendulumSuperSimplified()
 	assembly->runKINEMATIC();
 }
 
+void MbD::ASMTAssembly::runSinglePendulumSuperSimplified2()
+{
+	//In this version we skip declaration of variables that don't need as they use default values.
+	auto assembly = CREATE<ASMTAssembly>::With();
+	assembly->setName("OndselAssembly");
+
+	auto mkr = CREATE<ASMTMarker>::With();
+	mkr->setName("marker1");
+	assembly->addMarker(mkr);
+
+	auto part = CREATE<ASMTPart>::With();
+	part->setName("part1");
+	assembly->addPart(part);
+
+	auto marker1 = CREATE<ASMTMarker>::With();
+	marker1->setName("FixingMarker");
+	part->addMarker(marker1);
+
+	auto marker2 = CREATE<ASMTMarker>::With();
+	marker2->setName("marker2");
+	marker2->setPosition3D(20.0, 10.0, 0.0);
+	part->addMarker(marker2);
+
+	auto part2 = CREATE<ASMTPart>::With();
+	part2->setName("part2");
+	part2->setPosition3D(20.0, 10.0, 0.0);
+	assembly->addPart(part2);
+
+	auto marker3 = CREATE<ASMTMarker>::With();
+	marker3->setName("marker2");
+	marker3->setPosition3D(50.0, 10.0, 0.0);
+	part2->addMarker(marker3);
+
+	/*Ground joint*/
+	auto joint = CREATE<ASMTFixedJoint>::With();
+	joint->setName("Joint1");
+	joint->setMarkerI("/OndselAssembly/marker1");
+	joint->setMarkerJ("/OndselAssembly/part1/FixingMarker");
+	assembly->addJoint(joint);
+
+	auto joint2 = CREATE<ASMTRevoluteJoint>::With();
+	joint2->setName("Joint2");
+	joint2->setMarkerI("/OndselAssembly/part1/marker2");
+	joint2->setMarkerJ("/OndselAssembly/part2/marker2");
+	assembly->addJoint(joint2);
+
+	auto simulationParameters = CREATE<ASMTSimulationParameters>::With();
+	simulationParameters->settstart(0.0);
+	simulationParameters->settend(0.0);	//tstart == tend Initial Conditions only.
+	simulationParameters->sethmin(1.0e-9);
+	simulationParameters->sethmax(1.0);
+	simulationParameters->sethout(0.04);
+	simulationParameters->seterrorTol(1.0e-6);
+	assembly->setSimulationParameters(simulationParameters);
+
+	assembly->runKINEMATIC();
+}
+
 void MbD::ASMTAssembly::runSinglePendulumSimplified()
 {
 	auto assembly = CREATE<ASMTAssembly>::With();
@@ -96,7 +159,7 @@ void MbD::ASMTAssembly::runSinglePendulumSimplified()
 	assembly->setVelocity3D(0, 0, 0);
 	assembly->setOmega3D(0, 0, 0);
 
-	auto massMarker = CREATE<ASMTPrincipalMassMarker>::With();
+	auto massMarker = std::make_shared<ASMTPrincipalMassMarker>();
 	massMarker->setMass(0.0);
 	massMarker->setDensity(0.0);
 	massMarker->setMomentOfInertias(0, 0, 0);
@@ -110,7 +173,7 @@ void MbD::ASMTAssembly::runSinglePendulumSimplified()
 	auto mkr = CREATE<ASMTMarker>::With();
 	mkr->setName("Marker1");
 	mkr->setPosition3D(0, 0, 0);
-	massMarker->setRotationMatrix(
+	mkr->setRotationMatrix(
 		1, 0, 0,
 		0, 1, 0,
 		0, 0, 1);
@@ -127,7 +190,7 @@ void MbD::ASMTAssembly::runSinglePendulumSimplified()
 	part->setOmega3D(0, 0, 0);
 	assembly->addPart(part);
 
-	massMarker = CREATE<ASMTPrincipalMassMarker>::With();
+	massMarker = std::make_shared<ASMTPrincipalMassMarker>();
 	massMarker->setMass(0.2);
 	massMarker->setDensity(10.0);
 	massMarker->setMomentOfInertias(8.3333333333333e-4, 0.016833333333333, 0.017333333333333);
@@ -195,7 +258,7 @@ void MbD::ASMTAssembly::runSinglePendulum()
 	auto ome3D = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0 });
 	assembly->setOmega3D(ome3D);
 	//
-	auto massMarker = CREATE<ASMTPrincipalMassMarker>::With();
+	auto massMarker = std::make_shared<ASMTPrincipalMassMarker>();
 	massMarker->setMass(0.0);
 	massMarker->setDensity(0.0);
 	auto aJ = std::make_shared<DiagonalMatrix<double>>(ListD{ 0, 0, 0 });
@@ -220,7 +283,7 @@ void MbD::ASMTAssembly::runSinglePendulum()
 		{ 0, 1, 0 },
 		{ 0, 0, 1 }
 		});
-	massMarker->setRotationMatrix(rotMat);
+	mkr->setRotationMatrix(rotMat);
 	assembly->addMarker(mkr);
 	//
 	auto part = CREATE<ASMTPart>::With();
@@ -240,7 +303,7 @@ void MbD::ASMTAssembly::runSinglePendulum()
 	part->setOmega3D(ome3D);
 	assembly->addPart(part);
 	//
-	massMarker = CREATE<ASMTPrincipalMassMarker>::With();
+	massMarker = std::make_shared<ASMTPrincipalMassMarker>();
 	massMarker->setMass(0.2);
 	massMarker->setDensity(10.0);
 	aJ = std::make_shared<DiagonalMatrix<double>>(ListD{ 8.3333333333333e-4, 0.016833333333333, 0.017333333333333 });
@@ -729,6 +792,7 @@ void MbD::ASMTAssembly::calcCharacteristicDimensions()
 	auto unitLength = this->calcCharacteristicLength();
 	auto unitAngle = 1.0;
 	this->mbdUnits = std::make_shared<Units>(unitTime, unitMass, unitLength, unitAngle);
+	this->mbdUnits = std::make_shared<Units>(1.0, 1.0, 1.0, 1.0);	//for debug
 }
 
 double MbD::ASMTAssembly::calcCharacteristicTime()
@@ -861,31 +925,20 @@ void MbD::ASMTAssembly::solve()
 
 void MbD::ASMTAssembly::runKINEMATIC()
 {
-	auto mbdSystem = CREATE<System>::With();
+	auto mbdSystem = std::make_shared<System>();
 	mbdObject = mbdSystem;
 	mbdSystem->externalSystem->asmtAssembly = this;
 	mbdSystem->runKINEMATIC(mbdSystem);
 }
-//
-//void MbD::ASMTAssembly::initprincipalMassMarker()
-//{
-//	//Choose first refPoint as center of mass
-//	assert(!refPoints->empty());
-//	auto refPoint = refPoints->at(0);
-//	principalMassMarker = CREATE<ASMTPrincipalMassMarker>::With();
-//	principalMassMarker->position3D = refPoint->position3D;
-//	principalMassMarker->rotationMatrix = refPoint->rotationMatrix;
-//}
 
 void MbD::ASMTAssembly::initprincipalMassMarker()
 {
-	principalMassMarker = CREATE<ASMTPrincipalMassMarker>::With();
+	principalMassMarker = std::make_shared<ASMTPrincipalMassMarker>();
 	principalMassMarker->mass = 0.0;
 	principalMassMarker->density = 0.0;
 	principalMassMarker->momentOfInertias = std::make_shared<DiagonalMatrix<double>>(3, 0);
 	//principalMassMarker->position3D = std::make_shared<FullColumn<double>>(3, 0);
-	//principalMassMarker->rotationMatrix = std::make_shared<FullMatrix<double>>(3, 3);
-	//principalMassMarker->rotationMatrix->identity();
+	//principalMassMarker->rotationMatrix = FullMatrix<double>>::identitysptr(3);
 }
 
 std::shared_ptr<ASMTSpatialContainer> MbD::ASMTAssembly::spatialContainerAt(std::shared_ptr<ASMTAssembly> self, std::string& longname)
@@ -961,6 +1014,7 @@ void MbD::ASMTAssembly::updateFromMbD()
 {
 	ASMTSpatialContainer::updateFromMbD();
 	times->push_back(asmtTime->getValue());
+	std::cout << "Time = " << asmtTime->getValue() << std::endl;
 	for (auto& part : *parts) part->updateFromMbD();
 	for (auto& joint : *joints) joint->updateFromMbD();
 	for (auto& motion : *motions) motion->updateFromMbD();
@@ -1016,4 +1070,23 @@ void MbD::ASMTAssembly::setSimulationParameters(std::shared_ptr<ASMTSimulationPa
 	parameters->owner = this;
 }
 
+std::shared_ptr<ASMTPart> MbD::ASMTAssembly::partNamed(std::string partName)
+{
+	auto it = std::find_if(parts->begin(), parts->end(), [&](const std::shared_ptr<ASMTPart>& prt) {
+		return prt->fullName("") == partName;
+		});
+	auto& part = *it;
+	return part;
+}
+
+std::shared_ptr<ASMTPart> MbD::ASMTAssembly::partPartialNamed(std::string partialName)
+{
+	auto it = std::find_if(parts->begin(), parts->end(), [&](const std::shared_ptr<ASMTPart>& prt) {
+		auto fullName = prt->fullName("");
+		return fullName.find(partialName) != std::string::npos;
+		});
+	auto part = *it;
+	return part;
+}
+
 

OndselSolver/ASMTAssembly.h

@@ -12,6 +12,7 @@
 #include "ASMTSpatialContainer.h"
 #include "FullColumn.h"
 #include "FullMatrix.h"
+#include "MBDynSystem.h"
 
 namespace MbD {
 	class ASMTRefPoint;
@@ -35,7 +36,9 @@ namespace MbD {
 	{
 		//
 	public:
+		ASMTAssembly();
 		static void runSinglePendulumSuperSimplified();
+		static void runSinglePendulumSuperSimplified2();
 		static void runSinglePendulumSimplified();
 		static void runSinglePendulum();
 		static void runFile(const char* chars);
@@ -101,6 +104,8 @@ namespace MbD {
 		void addMotion(std::shared_ptr<ASMTMotion> motion);
 		void setConstantGravity(std::shared_ptr<ASMTConstantGravity> constantGravity);
 		void setSimulationParameters(std::shared_ptr<ASMTSimulationParameters> simulationParameters);
+		std::shared_ptr<ASMTPart> partNamed(std::string partName);
+		std::shared_ptr<ASMTPart> partPartialNamed(std::string partialName);
 
 		std::string notes;
 		std::shared_ptr<std::vector<std::shared_ptr<ASMTPart>>> parts = std::make_shared<std::vector<std::shared_ptr<ASMTPart>>>();
@@ -115,7 +120,7 @@ namespace MbD {
 		std::shared_ptr<std::vector<double>> times;
 		std::shared_ptr<ASMTTime> asmtTime = std::make_shared<ASMTTime>();
 		std::shared_ptr<Units> mbdUnits;
-
+		MBDynSystem* mbdynItem = nullptr;
 	};
 }
 

OndselSolver/ASMTFixedJoint.h

@@ -16,7 +16,7 @@ namespace MbD {
     {
         //
     public:
-        virtual std::shared_ptr<Joint> mbdClassNew() override;
+        std::shared_ptr<Joint> mbdClassNew() override;
 
     };
 }

OndselSolver/ASMTGeneralMotion.cpp

@@ -30,7 +30,7 @@ void MbD::ASMTGeneralMotion::parseASMT(std::vector<std::string>& lines)
 
 void MbD::ASMTGeneralMotion::readrIJI(std::vector<std::string>& lines)
 {
-	rIJI = std::make_shared<std::vector<std::string>>(3);
+	rIJI = std::make_shared<FullColumn<std::string>>(3);
 
 	assert(lines[0].find("rIJI1") != std::string::npos);
 	lines.erase(lines.begin());
@@ -48,7 +48,7 @@ void MbD::ASMTGeneralMotion::readrIJI(std::vector<std::string>& lines)
 
 void MbD::ASMTGeneralMotion::readangIJJ(std::vector<std::string>& lines)
 {
-	angIJJ = std::make_shared<std::vector<std::string>>(3);
+	angIJJ = std::make_shared<FullColumn<std::string>>(3);
 
 	assert(lines[0].find("angIJJ1") != std::string::npos);
 	lines.erase(lines.begin());
@@ -80,7 +80,7 @@ std::shared_ptr<Joint> MbD::ASMTGeneralMotion::mbdClassNew()
 void MbD::ASMTGeneralMotion::createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits)
 {
 	ASMTMotion::createMbD(mbdSys, mbdUnits);
-	auto parser = CREATE<SymbolicParser>::With();
+	auto parser = std::make_shared<SymbolicParser>();
 	parser->owner = this;
 	auto geoTime = owner->root()->geoTime();
 	parser->variables->insert(std::make_pair("time", geoTime));
@@ -88,21 +88,21 @@ void MbD::ASMTGeneralMotion::createMbD(std::shared_ptr<System> mbdSys, std::shar
 	auto fullMotion = std::static_pointer_cast<FullMotion>(mbdObject);
 
 	//rIJI
-	userFunc = CREATE<BasicUserFunction>::With(rIJI->at(0), 1.0);
+	userFunc = std::make_shared<BasicUserFunction>(rIJI->at(0), 1.0);
 	parser->parseUserFunction(userFunc);
 	auto geoX = parser->stack->top();
 	geoX = Symbolic::times(geoX, std::make_shared<Constant>(1.0 / mbdUnits->length));
 	geoX->createMbD(mbdSys, mbdUnits);
 	auto xBlk = geoX->simplified(geoX);
 
-	userFunc = CREATE<BasicUserFunction>::With(rIJI->at(1), 1.0);
+	userFunc = std::make_shared<BasicUserFunction>(rIJI->at(1), 1.0);
 	parser->parseUserFunction(userFunc);
 	auto geoY = parser->stack->top();
 	geoY = Symbolic::times(geoY, std::make_shared<Constant>(1.0 / mbdUnits->length));
 	geoY->createMbD(mbdSys, mbdUnits);
 	auto yBlk = geoY->simplified(geoY);
 
-	userFunc = CREATE<BasicUserFunction>::With(rIJI->at(2), 1.0);
+	userFunc = std::make_shared<BasicUserFunction>(rIJI->at(2), 1.0);
 	parser->parseUserFunction(userFunc);
 	auto geoZ = parser->stack->top();
 	geoZ = Symbolic::times(geoZ, std::make_shared<Constant>(1.0 / mbdUnits->length));
@@ -113,21 +113,21 @@ void MbD::ASMTGeneralMotion::createMbD(std::shared_ptr<System> mbdSys, std::shar
 	fullMotion->frIJI = std::make_shared<FullColumn<Symsptr>>(xyzBlkList);
 
 	//angIJJ
-	userFunc = CREATE<BasicUserFunction>::With(angIJJ->at(0), 1.0);
+	userFunc = std::make_shared<BasicUserFunction>(angIJJ->at(0), 1.0);
 	parser->parseUserFunction(userFunc);
 	auto geoPhi = parser->stack->top();
 	geoPhi = Symbolic::times(geoPhi, std::make_shared<Constant>(1.0 / mbdUnits->angle));
 	geoPhi->createMbD(mbdSys, mbdUnits);
 	auto phiBlk = geoPhi->simplified(geoPhi);
 
-	userFunc = CREATE<BasicUserFunction>::With(angIJJ->at(1), 1.0);
+	userFunc = std::make_shared<BasicUserFunction>(angIJJ->at(1), 1.0);
 	parser->parseUserFunction(userFunc);
 	auto geoThe = parser->stack->top();
 	geoThe = Symbolic::times(geoThe, std::make_shared<Constant>(1.0 / mbdUnits->angle));
 	geoThe->createMbD(mbdSys, mbdUnits);
 	auto theBlk = geoThe->simplified(geoThe);
 
-	userFunc = CREATE<BasicUserFunction>::With(angIJJ->at(2), 1.0);
+	userFunc = std::make_shared<BasicUserFunction>(angIJJ->at(2), 1.0);
 	parser->parseUserFunction(userFunc);
 	auto geoPsi = parser->stack->top();
 	geoPsi = Symbolic::times(geoPsi, std::make_shared<Constant>(1.0 / mbdUnits->angle));

OndselSolver/ASMTGeneralMotion.h

@@ -22,7 +22,7 @@ namespace MbD {
         std::shared_ptr<Joint> mbdClassNew() override;
         void createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits) override;
 
-        std::shared_ptr<std::vector<std::string>> rIJI, angIJJ;
+        std::shared_ptr<FullColumn<std::string>> rIJI, angIJJ;
         std::string rotationOrder;
 
     };

OndselSolver/ASMTItem.cpp

@@ -19,7 +19,12 @@ ASMTAssembly* MbD::ASMTItem::root()
 	return owner->root();
 }
 
-std::shared_ptr<ASMTSpatialContainer> MbD::ASMTItem::part()
+ASMTSpatialContainer* MbD::ASMTItem::partOrAssembly()
+{
+	return owner->partOrAssembly();
+}
+
+ASMTPart* MbD::ASMTItem::part()
 {
 	return owner->part();
 }
@@ -28,6 +33,11 @@ void MbD::ASMTItem::initialize()
 {
 }
 
+void MbD::ASMTItem::noop()
+{
+	//No Operations
+}
+
 void MbD::ASMTItem::setName(std::string str)
 {
 	name = str;

OndselSolver/ASMTItem.h

@@ -14,15 +14,18 @@ namespace MbD {
 	class ASMTAssembly;
 	class Units;
 	class ASMTSpatialContainer;
+	class ASMTPart;
 
 	class EXPORT ASMTItem
 	{
 		//
 	public:
 		virtual ASMTAssembly* root();
-		virtual std::shared_ptr<ASMTSpatialContainer> part();
+		virtual ASMTSpatialContainer* partOrAssembly();
+		virtual ASMTPart* part();
 
 		virtual void initialize();
+		void noop();
 		void setName(std::string str);
 		virtual void parseASMT(std::vector<std::string>& lines);
 		FRowDsptr readRowOfDoubles(std::string& line);
@@ -43,7 +46,7 @@ namespace MbD {
 		std::shared_ptr<Constant> sptrConstant(double value);
 
 		std::string name;
-		ASMTItem* owner;
+		ASMTItem* owner = nullptr;
 		std::shared_ptr<Item> mbdObject;
 	};
 }

OndselSolver/ASMTItemIJ.cpp

@@ -8,6 +8,16 @@
  
 #include "ASMTItemIJ.h"
 
+MbD::ASMTItemIJ::ASMTItemIJ()
+{
+	fxs = std::make_shared<FullRow<double>>();
+	fys = std::make_shared<FullRow<double>>();
+	fzs = std::make_shared<FullRow<double>>();
+	txs = std::make_shared<FullRow<double>>();
+	tys = std::make_shared<FullRow<double>>();
+	tzs = std::make_shared<FullRow<double>>();
+}
+
 void MbD::ASMTItemIJ::initialize()
 {
 	fxs = std::make_shared<FullRow<double>>();

OndselSolver/ASMTItemIJ.h

@@ -15,6 +15,7 @@ namespace MbD {
     {
         //
     public:
+        ASMTItemIJ();
         void initialize() override;
         void setMarkerI(std::string mkrI);
         void setMarkerJ(std::string mkrJ);

OndselSolver/ASMTMarker.cpp

@@ -53,7 +53,7 @@ FMatDsptr MbD::ASMTMarker::aApm()
 void MbD::ASMTMarker::createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits)
 {
 	auto mkr = CREATE<MarkerFrame>::With(name.c_str());
-	auto prt = std::static_pointer_cast<Part>(part()->mbdObject);
+	auto prt = std::static_pointer_cast<Part>(partOrAssembly()->mbdObject);
 	prt->partFrame->addMarkerFrame(mkr);
 
 	mkr->rpmp = rpmp()->times(1.0 / mbdUnits->length);

OndselSolver/ASMTMarker.h

@@ -11,6 +11,7 @@
 #include "ASMTSpatialItem.h"
 #include "FullColumn.h"
 #include "FullMatrix.h"
+#include "ASMTPart.h"
 
 namespace MbD {
     class EXPORT ASMTMarker : public ASMTSpatialItem

OndselSolver/ASMTNoRotationJoint.cpp

@@ -0,0 +1,17 @@
+/***************************************************************************
+ *   Copyright (c) 2023 Ondsel, Inc.                                       *
+ *                                                                         *
+ *   This file is part of OndselSolver.                                    *
+ *                                                                         *
+ *   See LICENSE file for details about copyright.                         *
+ ***************************************************************************/
+
+#include "ASMTNoRotationJoint.h"
+#include "NoRotationJoint.h"
+
+using namespace MbD;
+
+std::shared_ptr<Joint> MbD::ASMTNoRotationJoint::mbdClassNew()
+{
+    return CREATE<NoRotationJoint>::With();
+}

OndselSolver/ASMTNoRotationJoint.h

@@ -7,17 +7,15 @@
  ***************************************************************************/
 
 #pragma once
-#include "MBDynItem.h"
+
+#include "ASMTJoint.h"
 
 namespace MbD {
-    class MBDynNode;
-
-    class MBDynNodes : public MBDynItem
+    class EXPORT ASMTNoRotationJoint : public ASMTJoint
     {
+        //
     public:
-        void initialize() override;
-        void parseMBDyn(std::vector<std::string>& lines) override;
+        std::shared_ptr<Joint> mbdClassNew() override;
 
-        std::shared_ptr<std::vector<std::shared_ptr<MBDynNode>>> nodes;
     };
 }

OndselSolver/ASMTPart.cpp

@@ -9,6 +9,7 @@
 #include "ASMTPart.h"
 #include "CREATE.h"
 #include "ASMTPrincipalMassMarker.h"
+#include "Part.h"
 
 
 using namespace MbD;
@@ -55,7 +56,7 @@ void MbD::ASMTPart::readPrincipalMassMarker(std::vector<std::string>& lines)
 {
 	assert(lines[0].find("PrincipalMassMarker") != std::string::npos);
 	lines.erase(lines.begin());
-	principalMassMarker = CREATE<ASMTPrincipalMassMarker>::With();
+	principalMassMarker = std::make_shared<ASMTPrincipalMassMarker>();
 	principalMassMarker->parseASMT(lines);
 	principalMassMarker->owner = this;
 }
@@ -104,3 +105,14 @@ FColDsptr MbD::ASMTPart::omeOpO()
 {
 	return omega3D;
 }
+
+ASMTPart* MbD::ASMTPart::part()
+{
+	return this;
+}
+
+void MbD::ASMTPart::createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits)
+{
+	ASMTSpatialContainer::createMbD(mbdSys, mbdUnits);
+	if (isFixed) std::static_pointer_cast<Part>(mbdObject)->asFixed();
+}

OndselSolver/ASMTPart.h

@@ -23,9 +23,12 @@ namespace MbD {
         void readPartSeries(std::vector<std::string>& lines);
         FColDsptr vOcmO() override;
         FColDsptr omeOpO() override;
+        ASMTPart* part() override;
+        void createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits) override;
 
         //std::shared_ptr<std::vector<std::shared_ptr<ASMTFeature>>> featureOrder;
         std::shared_ptr<std::vector<std::shared_ptr<PosVelAccData>>> partSeries;
+        bool isFixed = false;
 
     };
 }

OndselSolver/ASMTPointInLineJoint.cpp

@@ -0,0 +1,17 @@
+/***************************************************************************
+ *   Copyright (c) 2023 Ondsel, Inc.                                       *
+ *                                                                         *
+ *   This file is part of OndselSolver.                                    *
+ *                                                                         *
+ *   See LICENSE file for details about copyright.                         *
+ ***************************************************************************/
+
+#include "ASMTPointInLineJoint.h"
+#include "PointInLineJoint.h"
+
+using namespace MbD;
+
+std::shared_ptr<Joint> MbD::ASMTPointInLineJoint::mbdClassNew()
+{
+    return CREATE<PointInLineJoint>::With();
+}

OndselSolver/ASMTPointInLineJoint.h

@@ -7,17 +7,15 @@
  ***************************************************************************/
 
 #pragma once
-#include "MBDynBlock.h"
+
+#include "ASMTJoint.h"
 
 namespace MbD {
-	class MBDynElement;
+    class EXPORT ASMTPointInLineJoint : public ASMTJoint
+    {
+        //
+    public:
+        std::shared_ptr<Joint> mbdClassNew() override;
 
-	class MBDynElements : public MBDynBlock
-	{
-	public:
-		void initialize() override;
-		void parseMBDyn(std::vector<std::string>& lines) override;
-
-		std::shared_ptr<std::vector<std::shared_ptr<MBDynElement>>> elements;
-	};
+    };
 }

OndselSolver/ASMTPointInPlaneJoint.h

@@ -15,7 +15,7 @@ namespace MbD {
     {
         //
     public:
-        virtual std::shared_ptr<Joint> mbdClassNew() override;
+        std::shared_ptr<Joint> mbdClassNew() override;
         void parseASMT(std::vector<std::string>& lines) override;
         void readOffset(std::vector<std::string>& lines);
         void createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits) override;

OndselSolver/ASMTRevoluteJoint.h

@@ -16,7 +16,7 @@ namespace MbD {
     {
         //
     public:
-        virtual std::shared_ptr<Joint> mbdClassNew() override;
+        std::shared_ptr<Joint> mbdClassNew() override;
 
     };
 }

OndselSolver/ASMTRotationalMotion.cpp

@@ -5,7 +5,7 @@
  *                                                                         *
  *   See LICENSE file for details about copyright.                         *
  ***************************************************************************/
- 
+
 #include "ASMTRotationalMotion.h"
 #include "ASMTAssembly.h"
 #include "SymbolicParser.h"
@@ -42,19 +42,25 @@ void MbD::ASMTRotationalMotion::readRotationZ(std::vector<std::string>& lines)
 
 void MbD::ASMTRotationalMotion::initMarkers()
 {
-	auto jt = root()->jointAt(motionJoint);
-	markerI = jt->markerI;
-	markerJ = jt->markerJ;
+	if (motionJoint == "") {
+		assert(markerI != "");
+		assert(markerJ != "");
+	}
+	else {
+		auto jt = root()->jointAt(motionJoint);
+		markerI = jt->markerI;
+		markerJ = jt->markerJ;
+	}
 }
 
 void MbD::ASMTRotationalMotion::createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits)
 {
 	ASMTMotion::createMbD(mbdSys, mbdUnits);
-	auto parser = CREATE<SymbolicParser>::With();
+	auto parser = std::make_shared<SymbolicParser>();
 	parser->owner = this;
 	auto geoTime = owner->root()->geoTime();
 	parser->variables->insert(std::make_pair("time", geoTime));
-	auto userFunc = CREATE<BasicUserFunction>::With(rotationZ, 1.0);
+	auto userFunc = std::make_shared<BasicUserFunction>(rotationZ, 1.0);
 	parser->parseUserFunction(userFunc);
 	auto geoPhi = parser->stack->top();
 	geoPhi = Symbolic::times(geoPhi, std::make_shared<Constant>(1.0 / mbdUnits->angle));

OndselSolver/ASMTSimulationParameters.cpp

@@ -72,3 +72,9 @@ void MbD::ASMTSimulationParameters::seterrorTol(double tol)
 {
 	errorTol = tol;
 }
+
+void MbD::ASMTSimulationParameters::setmaxIter(int maxIter)
+{
+	iterMaxPosKine = maxIter;
+	iterMaxAccKine = maxIter;
+}

OndselSolver/ASMTSimulationParameters.h

@@ -22,6 +22,7 @@ namespace MbD {
 		void sethmax(double hmax);
 		void sethout(double hout);
 		void seterrorTol(double errorTol);
+		void setmaxIter(int maxIter);
 
 		double tstart = 0.0, tend = 1.0, hmin = 1.0e-9, hmax = 1.0e9, hout = 0.1, errorTol = 1.0e-6;
 		double errorTolPosKine = 1.0e-6, errorTolAccKine = 1.0e-6, corAbsTol = 1.0e-6, corRelTol = 1.0e-6;

OndselSolver/ASMTSpatialContainer.cpp

@@ -13,11 +13,38 @@
 #include "ASMTRefPoint.h"
 #include "ASMTRefCurve.h"
 #include "ASMTRefSurface.h"
+#include <algorithm>
 //#include "ASMTPrincipalMassMarker.h"
 
 
 using namespace MbD;
 
+MbD::ASMTSpatialContainer::ASMTSpatialContainer()
+{
+	refPoints = std::make_shared<std::vector<std::shared_ptr<ASMTRefPoint>>>();
+	refCurves = std::make_shared<std::vector<std::shared_ptr<ASMTRefCurve>>>();
+	refSurfaces = std::make_shared<std::vector<std::shared_ptr<ASMTRefSurface>>>();
+
+	xs = std::make_shared<FullRow<double>>();
+	ys = std::make_shared<FullRow<double>>();
+	zs = std::make_shared<FullRow<double>>();
+	bryxs = std::make_shared<FullRow<double>>();
+	bryys = std::make_shared<FullRow<double>>();
+	bryzs = std::make_shared<FullRow<double>>();
+	vxs = std::make_shared<FullRow<double>>();
+	vys = std::make_shared<FullRow<double>>();
+	vzs = std::make_shared<FullRow<double>>();
+	omexs = std::make_shared<FullRow<double>>();
+	omeys = std::make_shared<FullRow<double>>();
+	omezs = std::make_shared<FullRow<double>>();
+	axs = std::make_shared<FullRow<double>>();
+	ays = std::make_shared<FullRow<double>>();
+	azs = std::make_shared<FullRow<double>>();
+	alpxs = std::make_shared<FullRow<double>>();
+	alpys = std::make_shared<FullRow<double>>();
+	alpzs = std::make_shared<FullRow<double>>();
+}
+
 void MbD::ASMTSpatialContainer::initialize()
 {
 	refPoints = std::make_shared<std::vector<std::shared_ptr<ASMTRefPoint>>>();
@@ -294,9 +321,9 @@ FColDsptr MbD::ASMTSpatialContainer::omeOpO()
 	return FColDsptr();
 }
 
-std::shared_ptr<ASMTSpatialContainer> MbD::ASMTSpatialContainer::part()
+ASMTSpatialContainer* MbD::ASMTSpatialContainer::partOrAssembly()
 {
-	return std::make_shared<ASMTSpatialContainer>(*this);
+	return this;
 }
 
 void MbD::ASMTSpatialContainer::updateFromMbD()
@@ -433,3 +460,32 @@ void MbD::ASMTSpatialContainer::addMarker(std::shared_ptr<ASMTMarker> marker)
 	addRefPoint(refPoint);
 	refPoint->addMarker(marker);
 }
+
+std::string MbD::ASMTSpatialContainer::generateUniqueMarkerName()
+{
+	auto aItemList = markerList();
+	auto markerNames = std::vector<std::string>();
+	for (auto& mkr : *aItemList) {
+		markerNames.push_back(mkr->name);
+	}
+	std::stringstream ss;
+	auto count = 0;
+	while (true) {
+		ss.str("");
+		ss << "Marker";
+		ss << count;
+		if (std::find(markerNames.begin(), markerNames.end(), ss.str()) == markerNames.end()) break;
+		count++;
+	}
+	return ss.str();
+}
+
+std::shared_ptr<std::vector<std::shared_ptr<ASMTMarker>>> MbD::ASMTSpatialContainer::markerList()
+{
+	auto markers = std::make_shared<std::vector<std::shared_ptr<ASMTMarker>>>();
+	for (auto& refPoint : *refPoints) {
+		auto refmarkers = refPoint->markers;
+		markers->insert(markers->end(), refmarkers->begin(), refmarkers->end());
+	}
+	return markers;
+}

OndselSolver/ASMTSpatialContainer.h

@@ -33,6 +33,7 @@ namespace MbD {
     {
         //
     public:
+        ASMTSpatialContainer();
         void initialize() override;
         void setPrincipalMassMarker(std::shared_ptr<ASMTPrincipalMassMarker> aJ);
         void readRefPoints(std::vector<std::string>& lines);
@@ -64,12 +65,14 @@ namespace MbD {
         std::shared_ptr<EulerParameters<double>> qEp();
         virtual FColDsptr vOcmO();
         virtual FColDsptr omeOpO();
-        std::shared_ptr<ASMTSpatialContainer> part() override;
+        ASMTSpatialContainer* partOrAssembly() override;
         void updateFromMbD() override;
         void compareResults(AnalysisType type) override;
         void outputResults(AnalysisType type) override;
         void addRefPoint(std::shared_ptr<ASMTRefPoint> refPoint);
         void addMarker(std::shared_ptr<ASMTMarker> marker);
+        std::string generateUniqueMarkerName();
+        std::shared_ptr<std::vector<std::shared_ptr<ASMTMarker>>> markerList();
 
         std::shared_ptr<std::vector<std::shared_ptr<ASMTRefPoint>>> refPoints;
         std::shared_ptr<std::vector<std::shared_ptr<ASMTRefCurve>>> refCurves;

OndselSolver/ASMTSphericalJoint.h

@@ -16,7 +16,7 @@ namespace MbD {
     {
         //
     public:
-        virtual std::shared_ptr<Joint> mbdClassNew() override;
+        std::shared_ptr<Joint> mbdClassNew() override;
 
     };
 }

OndselSolver/ASMTTranslationalMotion.cpp

@@ -49,11 +49,11 @@ void MbD::ASMTTranslationalMotion::initMarkers()
 void MbD::ASMTTranslationalMotion::createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits)
 {
 	ASMTMotion::createMbD(mbdSys, mbdUnits);
-	auto parser = CREATE<SymbolicParser>::With();
+	auto parser = std::make_shared<SymbolicParser>();
 	parser->owner = this;
 	auto geoTime = owner->root()->geoTime();
 	parser->variables->insert(std::make_pair("time", geoTime));
-	auto userFunc = CREATE<BasicUserFunction>::With(translationZ, 1.0);
+	auto userFunc = std::make_shared<BasicUserFunction>(translationZ, 1.0);
 	parser->parseUserFunction(userFunc);
 	auto zIJ = parser->stack->top();
 	zIJ = Symbolic::times(zIJ, std::make_shared<Constant>(1.0 / mbdUnits->length));

OndselSolver/ASMTUniversalJoint.h

@@ -15,7 +15,7 @@ namespace MbD {
     {
         //
     public:
-        virtual std::shared_ptr<Joint> mbdClassNew() override;
+        std::shared_ptr<Joint> mbdClassNew() override;
 
     };
 }

OndselSolver/AccNewtonRaphson.cpp

@@ -6,12 +6,15 @@
  *   See LICENSE file for details about copyright.                         *
  ***************************************************************************/
  
+#include <iostream>
+
 #include "AccNewtonRaphson.h"
 #include "SystemSolver.h"
 #include "Part.h"
 #include "Constraint.h"
 #include "SimulationStoppingError.h"
-#include <iostream>
+#include "GESpMatParPvPrecise.h"
+#include "CREATE.h"
 
 using namespace MbD;
 
@@ -120,3 +123,22 @@ void AccNewtonRaphson::preRun()
 {
 	system->partsJointsMotionsForcesTorquesDo([&](std::shared_ptr<Item> item) { item->preAccIC(); });
 }
+
+void MbD::AccNewtonRaphson::handleSingularMatrix()
+{
+	std::string str = typeid(*matrixSolver).name();
+	if (str.find("GESpMatParPvMarkoFast") != std::string::npos) {
+		matrixSolver = CREATE<GESpMatParPvPrecise>::With();
+		this->solveEquations();
+	}
+	else {
+		str = typeid(*matrixSolver).name();
+		if (str.find("GESpMatParPvPrecise") != std::string::npos) {
+			this->logSingularMatrixMessage();
+			matrixSolver->throwSingularMatrixError("AccAccNewtonRaphson");
+		}
+		else {
+			assert(false);
+		}
+	}
+}

OndselSolver/AccNewtonRaphson.h

@@ -25,6 +25,7 @@ namespace MbD {
         void passRootToSystem();
         void postRun() override;
         void preRun() override;
+        void handleSingularMatrix() override;
 
 
     };

OndselSolver/AnyPosICNewtonRaphson.cpp

@@ -44,9 +44,10 @@ void AnyPosICNewtonRaphson::fillY()
 	y->atiminusFullColumn(0, (qsuWeights->timesFullColumn(newMinusOld)));
 	system->partsJointsMotionsDo([&](std::shared_ptr<Item> item) {
 		item->fillPosICError(y);
-		//std::cout << *y << std::endl;
+		//std::cout << item->name << *y << std::endl;
+		//noop();
 		});
-	//std::cout << *y << std::endl;
+	//std::cout << "Final" << *y << std::endl;
 }
 
 void AnyPosICNewtonRaphson::fillPyPx()

OndselSolver/Array.h

@@ -41,12 +41,9 @@ namespace MbD {
 		virtual double maxMagnitude() = 0;
 		double maxMagnitudeOfVector();
 		void equalArrayAt(std::shared_ptr<Array<T>> array, int i);
-		//virtual void normalizeSelf();
-		//virtual void conditionSelf();
-		//virtual void conditionSelfWithTol(double tol);
 		virtual void atiput(int i, T value);
-		//double length();
 		void magnifySelf(T factor);
+		void negateSelf();
 		void atitimes(int i, double factor);
 
 		virtual std::ostream& printOn(std::ostream& s) const {
@@ -99,7 +96,7 @@ namespace MbD {
 	//template<>
 	//inline double Array<double>::maxMagnitude()
 	//{
-	//	auto max = 0.0;
+	//	double max = 0.0;
 	//	for (int i = 0; i < this->size(); i++)
 	//	{
 	//		auto element = this->at(i);
@@ -111,10 +108,10 @@ namespace MbD {
 	template<typename T>
 	inline double Array<T>::maxMagnitudeOfVector()
 	{
-		auto answer = 0.0;
+		double answer = 0.0;
 		for (int i = 0; i < this->size(); i++)
 		{
-			auto mag = std::abs(this->at(i));
+			double mag = std::abs(this->at(i));
 			if (answer < mag) answer = mag;
 		}
 		return answer;
@@ -130,15 +127,15 @@ namespace MbD {
 	//template<>
 	//inline void Array<double>::normalizeSelf()
 	//{
-	//	auto length = this->length();
+	//	double length = this->length();
 	//	if (length == 0.0) throw std::runtime_error("Cannot normalize a null vector.");
 	//	this->magnifySelf(1.0 / length);
 	//}
 	//template<>
 	//inline void Array<double>::conditionSelf()
 	//{
-	//	constexpr auto epsilon = std::numeric_limits<double>::epsilon();
-	//	auto tol = maxMagnitude() * epsilon;
+	//	constexpr double epsilon = std::numeric_limits<double>::epsilon();
+	//	double tol = maxMagnitude() * epsilon;
 	//	conditionSelfWithTol(tol);
 	//}
 	//template<>
@@ -146,7 +143,7 @@ namespace MbD {
 	//{
 	//	for (int i = 0; i < this->size(); i++)
 	//	{
-	//		auto element = this->at(i);
+	//		double element = this->at(i);
 	//		if (element < 0.0) element = -element;
 	//		if (element < tol) this->atiput(i, 0.0);
 	//	}
@@ -159,10 +156,10 @@ namespace MbD {
 	//template<>
 	//inline double Array<double>::length()
 	//{
-	//	auto ssq = 0.0;
+	//	double ssq = 0.0;
 	//	for (int i = 0; i < this->size(); i++)
 	//	{
-	//		auto elem = this->at(i);
+	//		double elem = this->at(i);
 	//		ssq += elem * elem;
 	//	}
 	//	return std::sqrt(ssq);
@@ -176,6 +173,11 @@ namespace MbD {
 		}
 	}
 	template<typename T>
+	inline void Array<T>::negateSelf()
+	{
+		magnifySelf(-1);
+	}
+	template<typename T>
 	inline void Array<T>::atitimes(int i, double factor)
 	{
 		this->at(i) *= factor;

OndselSolver/BasicIntegrator.h

@@ -49,7 +49,7 @@ namespace MbD {
         IntegratorInterface* system;
         int istep = 0, iTry = 0, maxTry = 0;
         std::shared_ptr<std::vector<double>> tpast;
-        double t = 0, tnew = 0, h = 0, hnew = 0;
+        double t = 0.0, tnew = 0.0, h = 0, hnew = 0.0;
         int order = 0, orderNew = 0, orderMax = 0;
         std::shared_ptr<DifferenceOperator> opBDF;
         bool _continue = false;

OndselSolver/BasicUserFunction.cpp

@@ -14,6 +14,7 @@ using namespace MbD;
 
 MbD::BasicUserFunction::BasicUserFunction(const std::string& expression, double myUnt) : funcText(expression), myUnit(myUnt)
 {
+	units = std::make_shared<Units>();
 }
 
 void MbD::BasicUserFunction::initialize()

OndselSolver/CADSystem.cpp

@@ -610,7 +610,7 @@ void CADSystem::runPiston()
 	systemSolver->errorTolAccKine = 1.0e-6;
 	systemSolver->iterMaxPosKine = 25;
 	systemSolver->iterMaxAccKine = 25;
-	systemSolver->tstart = 0;
+	systemSolver->tstart = 0.0;
 	systemSolver->tend = 25.0;
 	systemSolver->hmin = 2.5e-8;
 	systemSolver->hmax = 25.0;

OndselSolver/ClassDiagram1.cd

@@ -0,0 +1,11 @@
+<?xml version="1.0" encoding="utf-8"?>
+<ClassDiagram MajorVersion="1" MinorVersion="1">
+  <Class Name="MbD::Solver" Collapsed="true">
+    <Position X="0.5" Y="0.5" Width="1.5" />
+    <TypeIdentifier>
+      <HashCode>AKAAAgAAAAAAgAAAEAEAAIABAAAAAAAACIAAIAAAAAI=</HashCode>
+      <FileName>Solver.h</FileName>
+    </TypeIdentifier>
+  </Class>
+  <Font Name="Segoe UI" Size="9" />
+</ClassDiagram>

OndselSolver/ConstVelConstraintIqcJc.cpp

@@ -53,7 +53,7 @@ void MbD::ConstVelConstraintIqcJc::fillAccICIterError(FColDsptr col)
 	col->atiplusFullVectortimes(iqEI, pGpEI, lam);
 	auto efrmIqc = std::static_pointer_cast<EndFrameqc>(frmI);
 	auto qEdotI = efrmIqc->qEdot();
-	auto sum = 0.0;
+	double sum = 0.0;
 	sum += pGpEI->timesFullColumn(efrmIqc->qEddot());
 	sum += qEdotI->transposeTimesFullColumn(ppGpEIpEI->timesFullColumn(qEdotI));
 	col->atiplusNumber(iG, sum);

OndselSolver/ConstVelConstraintIqcJqc.cpp

@@ -67,7 +67,7 @@ void MbD::ConstVelConstraintIqcJqc::fillAccICIterError(FColDsptr col)
 	auto qEdotI = efrmIqc->qEdot();
 	auto efrmJqc = std::static_pointer_cast<EndFrameqc>(frmJ);
 	auto qEdotJ = efrmJqc->qEdot();
-	auto sum = 0.0;
+	double sum = 0.0;
 	sum += pGpEJ->timesFullColumn(efrmJqc->qEddot());
 	sum += 2.0 * qEdotI->transposeTimesFullColumn(ppGpEIpEJ->timesFullColumn(qEdotJ));
 	sum += qEdotJ->transposeTimesFullColumn(ppGpEJpEJ->timesFullColumn(qEdotJ));

OndselSolver/Constraint.h

@@ -51,7 +51,7 @@ namespace MbD {
 		int iG = -1;
 		double aG = 0.0;		//Constraint function
 		double lam = 0.0;		//Lambda is Lagrange Multiplier
-		double mu = 0, lamDeriv = 0;
+		double mu = 0.0, lamDeriv = 0.0;
 	};
 }
 

OndselSolver/CylindricalJoint.cpp

@@ -32,6 +32,6 @@ void CylindricalJoint::initializeGlobally()
 		this->root()->hasChanged = true;
 	}
 	else {
-		InLineJoint::initializeGlobally();
+		Joint::initializeGlobally();
 	}
 }

OndselSolver/DiagonalMatrix.h

@@ -5,7 +5,7 @@
  *                                                                         *
  *   See LICENSE file for details about copyright.                         *
  ***************************************************************************/
- 
+
 #pragma once
 
 #include "Array.h"
@@ -24,10 +24,11 @@ namespace MbD {
 	{
 		//
 	public:
+		DiagonalMatrix() : Array<T>() {}
 		DiagonalMatrix(int count) : Array<T>(count) {}
 		DiagonalMatrix(int count, const T& value) : Array<T>(count, value) {}
 		DiagonalMatrix(std::initializer_list<T> list) : Array<T>{ list } {}
-		void atiputDiagonalMatrix(int i, std::shared_ptr < DiagonalMatrix<T>> diagMat);
+		void atiputDiagonalMatrix(int i, std::shared_ptr<DiagonalMatrix<T>> diagMat);
 		DiagMatsptr<T> times(T factor);
 		FColsptr<T> timesFullColumn(FColsptr<T> fullCol);
 		FMatsptr<T> timesFullMatrix(FMatsptr<T> fullMat);
@@ -45,14 +46,6 @@ namespace MbD {
 		std::ostream& printOn(std::ostream& s) const override;
 
 	};
-	template<typename T>
-	inline void DiagonalMatrix<T>::atiputDiagonalMatrix(int i, DiagMatsptr<T> diagMat)
-	{
-		for (int ii = 0; ii < diagMat->size(); ii++)
-		{
-			this->at(i + ii) = diagMat->at(ii);
-		}
-	}
 	template<>
 	inline DiagMatDsptr DiagonalMatrix<double>::times(double factor)
 	{
@@ -65,6 +58,14 @@ namespace MbD {
 		return answer;
 	}
 	template<typename T>
+	inline void DiagonalMatrix<T>::atiputDiagonalMatrix(int i, std::shared_ptr<DiagonalMatrix<T>> diagMat)
+	{
+		for (int ii = 0; ii < diagMat->size(); ii++)
+		{
+			this->at(i + ii) = diagMat->at(ii);
+		}
+	}
+	template<typename T>
 	inline DiagMatsptr<T> DiagonalMatrix<T>::times(T factor)
 	{
 		assert(false);
@@ -120,10 +121,10 @@ namespace MbD {
 	template<>
 	inline double DiagonalMatrix<double>::maxMagnitude()
 	{
-		auto max = 0.0;
+		double max = 0.0;
 		for (int i = 0; i < this->size(); i++)
 		{
-			auto element = this->at(i);
+			double element = this->at(i);
 			if (element < 0.0) element = -element;
 			if (max < element) max = element;
 		}

OndselSolver/DifferenceOperator.h

@@ -28,7 +28,7 @@ namespace MbD {
 
 		int iStep = 0, order = 0;
 		FMatDsptr taylorMatrix, operatorMatrix;
-		double time = 0;
+		double time = 0.0;
 		std::shared_ptr<std::vector<double>> timeNodes;
 		static FRowDsptr OneOverFactorials;
 	};

OndselSolver/EigenDecomposition.cpp

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

OndselSolver/EigenDecomposition.h

@@ -7,15 +7,16 @@
  ***************************************************************************/
 
 #pragma once
-#include "MBDynItem.h"
+
+#include "MatrixSolver.h"
 
 namespace MbD {
-	class MBDynLabels : public MBDynItem
+	class EigenDecomposition : public MatrixSolver
 	{
-	public:
-		void initialize() override;
-		void parseMBDyn(std::vector<std::string>& lines) override;
 
-		std::shared_ptr<std::map<std::string, int>> labels;
+		//
+	public:
+
 	};
 }
+

OndselSolver/EndFramec.h

@@ -44,7 +44,7 @@ namespace MbD {
 
 		MarkerFrame* markerFrame; //Use raw pointer when pointing backwards.
 		FColDsptr rOeO = std::make_shared<FullColumn<double>>(3);
-		FMatDsptr aAOe = std::make_shared<FullMatrix<double>>(3, 3);
+		FMatDsptr aAOe = FullMatrix<double>::identitysptr(3);
 	};
 	//using EndFrmsptr = std::shared_ptr<EndFramec>;
 }

OndselSolver/EndFrameqct.cpp

@@ -31,7 +31,7 @@ void EndFrameqct::initialize()
 	rmem = std::make_shared<FullColumn<double>>(3);
 	prmempt = std::make_shared<FullColumn<double>>(3);
 	pprmemptpt = std::make_shared<FullColumn<double>>(3);
-	aAme = std::make_shared<FullMatrix<double>>(3, 3);
+	aAme = FullMatrix<double>::identitysptr(3);
 	pAmept = std::make_shared<FullMatrix<double>>(3, 3);
 	ppAmeptpt = std::make_shared<FullMatrix<double>>(3, 3);
 	pprOeOpEpt = std::make_shared<FullMatrix<double>>(3, 4);

OndselSolver/EulerAngles.h

@@ -30,6 +30,7 @@ namespace MbD {
 		void initialize() override;
 		void calc() override;
 		std::shared_ptr<EulerAnglesDot<T>> differentiateWRT(T var);
+		void setRotOrder(int i, int j, int k);
 
 		std::shared_ptr<FullColumn<int>> rotOrder;
 		FColFMatDsptr cA;
@@ -64,6 +65,29 @@ namespace MbD {
 		}
 		aA = cA->at(0)->timesFullMatrix(cA->at(1)->timesFullMatrix(cA->at(2)));
 	}
+	template<>
+	inline void EulerAngles<double>::calc()
+	{
+		cA = std::make_shared<FullColumn<FMatDsptr>>(3);
+		for (int i = 0; i < 3; i++)
+		{
+			auto axis = rotOrder->at(i);
+			auto angle = this->at(i);
+			if (axis == 1) {
+				cA->atiput(i, FullMatrix<double>::rotatex(angle));
+			}
+			else if (axis == 2) {
+				cA->atiput(i, FullMatrix<double>::rotatey(angle));
+			}
+			else if (axis == 3) {
+				cA->atiput(i, FullMatrix<double>::rotatez(angle));
+			}
+			else {
+				throw std::runtime_error("Euler angle rotation order must be any permutation of 1,2,3 without consecutive repeats.");
+			}
+		}
+		aA = cA->at(0)->timesFullMatrix(cA->at(1)->timesFullMatrix(cA->at(2)));
+	}
 	template<typename T>
 	inline void EulerAngles<T>::calc()
 	{
@@ -79,5 +103,13 @@ namespace MbD {
 		derivatives->aEulerAngles = this;
 		return derivatives;
 	}
+	template<typename T>
+	inline void EulerAngles<T>::setRotOrder(int i, int j, int k)
+	{
+		rotOrder = std::make_shared<FullColumn<int>>(3);
+		rotOrder->at(0) = i;
+		rotOrder->at(1) = j;
+		rotOrder->at(2) = k;
+	}
 }
 

OndselSolver/EulerAngleszxz.h

@@ -29,12 +29,9 @@ namespace MbD {
 	template<typename T>
 	inline void EulerAngleszxz<T>::initialize()
 	{
-		phiA = std::make_shared<FullMatrix<double>>(3, 3);
-		phiA->identity();
-		theA = std::make_shared<FullMatrix<double>>(3, 3);
-		theA->identity();
-		psiA = std::make_shared<FullMatrix<double>>(3, 3);
-		psiA->identity();
+		phiA = FullMatrix<T>::identitysptr(3);
+		theA = FullMatrix<T>::identitysptr(3);
+		psiA = FullMatrix<T>::identitysptr(3);
 	}
 	template<typename T>
 	inline void EulerAngleszxz<T>::calc()

OndselSolver/EulerAngleszxzDDot.h

@@ -40,7 +40,7 @@ namespace MbD {
     {
 		//| zero phiThePsi phi sphi cphi phidot phiddot cphiddot sphiddot the sthe cthe thedot theddot ctheddot stheddot 
 		// psi spsi cpsi psidot psiddot cpsiddot spsiddot phiA theA psiA phiAdot theAdot psiAdot |
-		auto zero = 0.0;
+		double zero = 0.0;
 		auto& phiThePsi = phiThePsiDot->phiThePsi;
 		auto& phi = phiThePsi->at(0);
 		auto sphi = std::sin(phi);

OndselSolver/EulerParameters.h

@@ -28,6 +28,17 @@ namespace MbD {
 		EulerParameters(int count) : EulerArray<T>(count) {}
 		EulerParameters(int count, const T& value) : EulerArray<T>(count, value) {}
 		EulerParameters(std::initializer_list<T> list) : EulerArray<T>{ list } {}
+		EulerParameters(FColDsptr axis, double theta) : EulerArray<T>(4) {
+			auto halfTheta = theta / 2.0;
+			auto sinHalfTheta = std::sin(halfTheta);
+			auto cosHalfTheta = std::cos(halfTheta);
+			axis->normalizeSelf();
+			this->atiputFullColumn(0, axis->times(sinHalfTheta));
+			this->atiput(3, cosHalfTheta);
+			this->conditionSelf();
+			this->initialize();
+			this->calc();
+		}
 
 		static std::shared_ptr<FullMatrix<FColsptr<T>>> ppApEpEtimesColumn(FColDsptr col);
 		static FMatDsptr pCpEtimesColumn(FColDsptr col);
@@ -201,7 +212,7 @@ namespace MbD {
 	template<>
 	inline void EulerParameters<double>::initialize()
 	{
-		aA = std::make_shared<FullMatrix<double>>(3, 3);
+		aA = FullMatrix<double>::identitysptr(3);
 		aB = std::make_shared<FullMatrix<double>>(3, 4);
 		aC = std::make_shared<FullMatrix<double>>(3, 4);
 		pApE = std::make_shared<FullColumn<FMatDsptr>>(4);

OndselSolver/FullMatrix.h

@@ -5,13 +5,14 @@
  *                                                                         *
  *   See LICENSE file for details about copyright.                         *
  ***************************************************************************/
- 
+
 #pragma once
 
 #include "corecrt_math_defines.h"
 #include <memory>
 
 #include "RowTypeMatrix.h"
+#include "FullColumn.h"
 
 namespace MbD {
 	template<typename T>
@@ -26,6 +27,9 @@ namespace MbD {
 	class FullRow;
 	template<typename T>
 	class EulerParameters;
+	template<typename T>
+	class DiagonalMatrix;
+
 	using FMatFColDsptr = std::shared_ptr<FullMatrix<FColDsptr>>;
 	using FMatFMatDsptr = std::shared_ptr<FullMatrix<FMatDsptr>>;
 	using FColFMatDsptr = std::shared_ptr<FullColumn<FMatDsptr>>;
@@ -66,6 +70,8 @@ namespace MbD {
 		static FMatsptr<T> rotatexrotDotrotDDot(T angle, T angleDot, T angleDDot);
 		static FMatsptr<T> rotateyrotDotrotDDot(T angle, T angleDot, T angleDDot);
 		static FMatsptr<T> rotatezrotDotrotDDot(T angle, T angleDot, T angleDDot);
+		static FMatsptr<T> identitysptr(int n);
+		static FMatsptr<T> tildeMatrix(FColDsptr col);
 		void identity();
 		FColsptr<T> column(int j);
 		FColsptr<T> timesFullColumn(FColsptr<T> fullCol);
@@ -95,6 +101,10 @@ namespace MbD {
 		T trace();
 		double maxMagnitude() override;
 		FColsptr<T> bryantAngles();
+		bool isDiagonal();
+		bool isDiagonalToWithin(double ratio);
+		std::shared_ptr<DiagonalMatrix<T>> asDiagonalMatrix();
+		void conditionSelfWithTol(double tol);
 
 		std::ostream& printOn(std::ostream& s) const override;
 	};
@@ -296,13 +306,40 @@ namespace MbD {
 		row2->atiput(2, 0.0);
 		return rotMat;
 	}
-    template<>
-    inline void FullMatrix<double>::zeroSelf()
-    {
-        for (int i = 0; i < this->size(); i++) {
-            this->at(i)->zeroSelf();
-        }
-    }
+	template<typename T>
+	inline FMatsptr<T> FullMatrix<T>::identitysptr(int n)
+	{
+		auto mat = std::make_shared<FullMatrix<T>>(n, n);
+		mat->identity();
+		return mat;
+	}
+	template<typename T>
+	inline FMatsptr<T> FullMatrix<T>::tildeMatrix(FColDsptr col)
+	{
+		//"tildeMatrix is skew symmetric matrix related to angular velocity and cross product."
+		if (col->size() != 3) throw std::runtime_error("Column is not of dimension 3");
+		auto tilde = std::make_shared<FullMatrix<double>>(3, 3);
+		auto c0 = col->at(0);
+		auto c1 = col->at(1);
+		auto c2 = col->at(2);
+		tilde->atijput(0, 0, 0.0);
+		tilde->atijput(1, 1, 0.0);
+		tilde->atijput(2, 2, 0.0);
+		tilde->atijput(1, 2, -c0);
+		tilde->atijput(0, 2, c1);
+		tilde->atijput(0, 1, -c2);
+		tilde->atijput(1, 0, c2);
+		tilde->atijput(2, 0, -c1);
+		tilde->atijput(2, 1, c0);
+		return tilde;
+	}
+	template<>
+	inline void FullMatrix<double>::zeroSelf()
+	{
+		for (int i = 0; i < this->size(); i++) {
+			this->at(i)->zeroSelf();
+		}
+	}
 	template<>
 	inline void FullMatrix<double>::identity() {
 		this->zeroSelf();
@@ -582,10 +619,10 @@ namespace MbD {
 	template<typename T>
 	inline double FullMatrix<T>::maxMagnitude()
 	{
-		auto max = 0.0;
+		double max = 0.0;
 		for (int i = 0; i < this->size(); i++)
 		{
-			auto element = this->at(i)->maxMagnitude();
+			double element = this->at(i)->maxMagnitude();
 			if (max < element) max = element;
 		}
 		return max;
@@ -628,6 +665,61 @@ namespace MbD {
 		return answer;
 	}
 	template<typename T>
+	inline bool FullMatrix<T>::isDiagonal()
+	{
+		auto m = this->nrow();
+		auto n = this->ncol();
+		if (m != n) return false;
+		for (int i = 0; i < m; i++)
+		{
+			auto rowi = this->at(i);
+			for (int j = 0; j < n; j++)
+			{
+				if (i != j && rowi->at(j) != 0) return false;
+			}
+		}
+		return true;
+	}
+	template<typename T>
+	inline bool FullMatrix<T>::isDiagonalToWithin(double ratio)
+	{
+		double maxMag = this->maxMagnitude();
+		auto tol = ratio * maxMag;
+		auto nrow = this->nrow();
+		if (nrow == this->ncol()) {
+			for (int i = 0; i < 3; i++)
+			{
+				for (int j = i + 1; j < 3; j++)
+				{
+					if (std::abs(this->at(i)->at(j)) > tol) return false;
+					if (std::abs(this->at(j)->at(i)) > tol) return false;
+				}
+			}
+			return true;
+		}
+		else {
+			return false;
+		}
+	}
+	template<typename T>
+	inline std::shared_ptr<DiagonalMatrix<T>> FullMatrix<T>::asDiagonalMatrix()
+	{
+		int nrow = this->nrow();
+		auto diagMat = std::make_shared<DiagonalMatrix<T>>(nrow);
+		for (int i = 0; i < nrow; i++)
+		{
+			diagMat->atiput(i, this->at(i)->at(i));
+		}
+		return diagMat;
+	}
+	template<typename T>
+	inline void FullMatrix<T>::conditionSelfWithTol(double tol)
+	{
+		for (auto row : *this) {
+			row->conditionSelfWithTol(tol);
+		}
+	}
+	template<typename T>
 	inline FColsptr<T> FullMatrix<T>::timesFullColumn(FColsptr<T> fullCol)
 	{
 		return this->timesFullColumn(fullCol.get());

OndselSolver/FullVector.h

@@ -5,7 +5,7 @@
  *                                                                         *
  *   See LICENSE file for details about copyright.                         *
  ***************************************************************************/
- 
+
 #pragma once
 
 #include <ostream>
@@ -37,7 +37,7 @@ namespace MbD {
 		void normalizeSelf();
 		double length();
 		virtual void conditionSelf();
-		virtual void conditionSelfWithTol(double tol);
+		void conditionSelfWithTol(double tol);
 
 		std::ostream& printOn(std::ostream& s) const override;
 
@@ -125,10 +125,10 @@ namespace MbD {
 	template<>
 	inline double FullVector<double>::maxMagnitude()
 	{
-		auto max = 0.0;
+		double max = 0.0;
 		for (int i = 0; i < this->size(); i++)
 		{
-			auto element = this->at(i);
+			double element = this->at(i);
 			if (element < 0.0) element = -element;
 			if (max < element) max = element;
 		}
@@ -143,17 +143,17 @@ namespace MbD {
 	template<>
 	inline void FullVector<double>::normalizeSelf()
 	{
-		auto length = this->length();
+		double length = this->length();
 		if (length == 0.0) throw std::runtime_error("Cannot normalize a null vector.");
 		this->magnifySelf(1.0 / length);
 	}
 	template<typename T>
 	inline double FullVector<T>::length()
 	{
-		auto ssq = 0.0;
+		double ssq = 0.0;
 		for (int i = 0; i < this->size(); i++)
 		{
-			auto elem = this->at(i);
+			double elem = this->at(i);
 			ssq += elem * elem;
 		}
 		return std::sqrt(ssq);
@@ -161,8 +161,8 @@ namespace MbD {
 	template<typename T>
 	inline void FullVector<T>::conditionSelf()
 	{
-		constexpr auto epsilon = std::numeric_limits<double>::epsilon();
-		auto tol = this->maxMagnitude() * epsilon;
+		constexpr double epsilon = std::numeric_limits<double>::epsilon();
+		double tol = this->maxMagnitude() * epsilon;
 		this->conditionSelfWithTol(tol);
 	}
 	template<>
@@ -170,7 +170,7 @@ namespace MbD {
 	{
 		for (int i = 0; i < this->size(); i++)
 		{
-			auto element = this->at(i);
+			double element = this->at(i);
 			if (element < 0.0) element = -element;
 			if (element < tol) this->atiput(i, 0.0);
 		}

OndselSolver/GEFullMatParPv.cpp

@@ -19,7 +19,7 @@ void GEFullMatParPv::doPivoting(int p)
 
 		//| app max rowPivot aip mag |
 	auto app = matrixA->at(p)->at(p);
-	auto max = app * rowScalings->at(p);
+	double max = app * rowScalings->at(p);
 	if (max < 0.0) max = -max;
 	auto rowPivot = p;
 	for (int i = p + 1; i < m; i++)

OndselSolver/GESpMatFullPv.cpp

@@ -20,7 +20,7 @@ void GESpMatFullPv::doPivoting(int p)
 	//"Swap rows but keep columns in place."
 	//"The elements below the diagonal are removed column by column."
 
-	auto max = 0.0;
+	double max = 0.0;
 	auto pivotRow = p;
 	auto pivotCol = p;
 	for (int j = p; j < n; j++)
@@ -169,7 +169,7 @@ void GESpMatFullPv::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol
 	for (int i = 0; i < m; i++)
 	{
 		auto& spRowi = spMat->at(i);
-		auto maxRowMagnitude = spRowi->maxMagnitude();
+		double maxRowMagnitude = spRowi->maxMagnitude();
 		if (maxRowMagnitude == 0) {
 			throwSingularMatrixError("");
 		}

OndselSolver/GESpMatFullPvPosIC.cpp

@@ -33,7 +33,7 @@ void GESpMatFullPvPosIC::doPivoting(int p)
 	//"Swap rows but keep columns in place."
 	//"The elements below the diagonal are removed column by column."
 
-	auto max = 0.0;
+	double max = 0.0;
 	auto pivotRow = p;
 	auto pivotCol = p;
 	for (int j = p; j < n; j++)

OndselSolver/GESpMatParPvMarko.cpp

@@ -94,7 +94,7 @@ void GESpMatParPvMarko::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr ful
 	for (int i = 0; i < m; i++)
 	{
 		auto& spRowi = spMat->at(i);
-		auto maxRowMagnitude = spRowi->maxMagnitude();
+		double maxRowMagnitude = spRowi->maxMagnitude();
 		if (maxRowMagnitude == 0) {
 			throwSingularMatrixError("");
 		}

OndselSolver/GESpMatParPvMarkoFast.cpp

@@ -33,7 +33,7 @@ void GESpMatParPvMarkoFast::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr
 	for (int i = 0; i < m; i++)
 	{
 		auto& spRowi = spMat->at(i);
-		auto maxRowMagnitude = spRowi->maxMagnitude();
+		double maxRowMagnitude = spRowi->maxMagnitude();
 		if (maxRowMagnitude == 0) throwSingularMatrixError("");
 		auto scaling = 1.0 / maxRowMagnitude;
 		matrixA->at(i) = spRowi->timesconditionedWithTol(scaling, singularPivotTolerance);

OndselSolver/GESpMatParPvPrecise.cpp

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

OndselSolver/GeneralSpline.cpp

@@ -69,23 +69,23 @@ void MbD::GeneralSpline::computeDerivatives()
 	auto matrix = std::make_shared<SparseMatrix<double>>(np, np);
 	auto bvector = std::make_shared<FullColumn<double>>(np, 0.0);
 	auto hs = std::make_shared<FullColumn<double>>(n - 1);
-	auto hmax = 0.0;
+	double hmax = 0.0;
 	for (int i = 0; i < n - 1; i++)
 	{
-		auto h = xs->at((size_t)i + 1) - xs->at(i);
+		double h = xs->at((size_t)i + 1) - xs->at(i);
 		hmax = std::max(hmax, std::abs(h));
 		hs->atiput(i, h);
 	}
 	for (int i = 0; i < n - 1; i++)
 	{
-		auto offset = i * p;
-		auto hbar = hs->at(i) / hmax;
+		double offset = i * p;
+		double hbar = hs->at(i) / hmax;
 		for (int j = 1; j < p; j++)
 		{
 			matrix->atijput(offset + j, offset + j - 1, 1.0);
 			matrix->atijput(offset + j, offset + j - 1 + p, -1.0);
 		}
-		auto dum = 1.0;
+		double dum = 1.0;
 		for (int j = 0; j < p; j++)
 		{
 			dum = dum * hbar / (j + 1);
@@ -148,7 +148,7 @@ double MbD::GeneralSpline::derivativeAt(int n, double xxx)
 	if (n > degree) return 0.0;
 	calcIndexAndDeltaFor(xxx);
 	auto& derivsi = derivs->at(index);
-	auto sum = 0.0;
+	double sum = 0.0;
 	for (int j = degree; j >= n + 1; j--)
 	{
 		sum = (sum + derivsi->at((size_t)j - 1)) * delta / (j - n);
@@ -169,16 +169,16 @@ void MbD::GeneralSpline::calcIndexAndDeltaFor(double xxx)
 
 void MbD::GeneralSpline::calcCyclicIndexAndDelta()
 {
-	auto xFirst = xs->front();
-	auto xLast = xs->back();
+	double xFirst = xs->front();
+	double xLast = xs->back();
 	xvalue = std::fmod(xvalue - xFirst, xLast - xFirst) + xFirst;
 	calcIndexAndDelta();
 }
 
 void MbD::GeneralSpline::calcNonCyclicIndexAndDelta()
 {
-	auto xFirst = xs->front();
-	auto xLast = xs->back();
+	double xFirst = xs->front();
+	double xLast = xs->back();
 	if (xvalue <= xFirst) {
 		index = 0;
 		delta = xvalue - xFirst;
@@ -230,7 +230,7 @@ double MbD::GeneralSpline::y(double xxx)
 
 	calcIndexAndDeltaFor(xxx);
 	auto& derivsi = derivs->at(index);
-	auto sum = 0.0;
+	double sum = 0.0;
 	for (int j = degree; j >= 1; j--)
 	{
 		sum = (sum + derivsi->at((size_t)j - 1)) * delta / j;

OndselSolver/InLineJoint.h

@@ -13,10 +13,11 @@
 namespace MbD {
 	class InLineJoint : public Joint
 	{
-		//
+		//Abstract class. Create subclasses only.
 	public:
 		InLineJoint();
 		InLineJoint(const char* str);
+		virtual void initializeGlobally() = 0;	//To prevent instantiation of this class
 
 		void createInLineConstraints();
 

OndselSolver/InPlaneJoint.h

@@ -17,7 +17,8 @@ namespace MbD {
 	public:
 		InPlaneJoint();
 		InPlaneJoint(const char* str);
-		
+		virtual void initializeGlobally() = 0;	//To prevent instantiation of this class
+
 		void createInPlaneConstraint();
 
 		double offset;

OndselSolver/IntegratorInterface.h

@@ -36,7 +36,7 @@ namespace MbD {
         virtual void interpolateAt(double t);
 
         SystemSolver* system;
-        double tout = 0, hout = 0, hmin = 0, hmax = 0, tstart = 0, tend = 0;
+        double tout = 0.0, hout = 0.0, hmin = 0.0, hmax = 0.0, tstart = 0.0, tend = 0.0;
         std::shared_ptr<BasicQuasiIntegrator> integrator;
     };
 }

OndselSolver/Item.cpp

@@ -28,6 +28,11 @@ System* Item::root()
 	return owner->root();
 }
 
+void MbD::Item::noop()
+{
+	//No Operations
+}
+
 void Item::initialize()
 {
 }

OndselSolver/Item.h

@@ -29,6 +29,7 @@ namespace MbD {
 		Item();
 		Item(const char* str);
 		virtual System* root();
+		void noop();
 
 		virtual void calcPostDynCorrectorIteration();
 		virtual void checkForCollisionDiscontinuityBetweenand(double impulsePrevious, double impulse);

OndselSolver/LDUFullMatParPv.cpp

@@ -18,7 +18,7 @@ void LDUFullMatParPv::doPivoting(int p)
 
 	//| app max rowPivot aip mag |
 	auto app = matrixA->at(p)->at(p);
-	auto max = app * rowScalings->at(p);
+	double max = app * rowScalings->at(p);
 	if (max < 0.0) max = -max;
 	auto rowPivot = p;
 	for (int i = p + 1; i < m; i++)

OndselSolver/LineInPlaneJoint.cpp

@@ -29,6 +29,6 @@ void MbD::LineInPlaneJoint::initializeGlobally()
 		this->root()->hasChanged = true;
 	}
 	else {
-		InPlaneJoint::initializeGlobally();
+		Joint::initializeGlobally();
 	}
 }

OndselSolver/MBDynBody.cpp

@@ -1,9 +1,11 @@
 #include "MBDynBody.h"
-#include "MBDynReferences.h"
 #include "MBDynReference.h"
+#include "MBDynStructural.h"
 #include "SymbolicParser.h"
 #include "BasicUserFunction.h"
-#include "MBDynVariables.h"
+#include "ASMTPart.h"
+#include "ASMTAssembly.h"
+#include "MomentOfInertiaSolver.h"
 
 using namespace MbD;
 
@@ -18,31 +20,91 @@ void MbD::MBDynBody::parseMBDyn(std::string line)
 	auto pos = line.find(":");
 	auto front = line.substr(previousPos, pos - previousPos);
 	assert(front.find("body") != std::string::npos);
-	auto arguments = std::make_shared<std::vector<std::string>>();
+	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);
+			arguments.push_back(argument);
 		}
 		else {
 			argument = line.substr(previousPos + 1);
-			arguments->push_back(argument);
+			arguments.push_back(argument);
 			break;
 		}
 	}
-	auto iss = std::istringstream(arguments->at(0));
+	auto iss = std::istringstream(arguments.at(0));
 	iss >> name;
-	arguments->erase(arguments->begin());
-	iss = std::istringstream(arguments->at(0));
-	iss >> node;
-	arguments->erase(arguments->begin());
-	rOfO = readPosition(arguments);
+	arguments.erase(arguments.begin());
+	iss = std::istringstream(arguments.at(0));
+	iss >> nodeName;
+	arguments.erase(arguments.begin());
+	readMass(arguments);
+	rPcmP = readPosition(arguments);
 	readInertiaMatrix(arguments);
 }
 
-void MbD::MBDynBody::readInertiaMatrix(std::shared_ptr<std::vector<std::string>>& args)
+void MbD::MBDynBody::readMass(std::vector<std::string>& args)
 {
+	auto parser = std::make_shared<SymbolicParser>();
+	parser->variables = mbdynVariables();
+	auto userFunc = std::make_shared<BasicUserFunction>(popOffTop(args), 1.0);
+	parser->parseUserFunction(userFunc);
+	auto sym = parser->stack->top();
+	mass = sym->getValue();
+}
+
+void MbD::MBDynBody::readInertiaMatrix(std::vector<std::string>& args)
+{
+	auto parser = std::make_shared<SymbolicParser>();
+	parser->variables = mbdynVariables();
+	aJmat = std::make_shared<FullMatrix<double>>(3, 3);
+	auto& str = args.at(0);
+	if (str.find("diag") != std::string::npos) {
+		args.erase(args.begin());
+		for (int i = 0; i < 3; i++)
+		{
+			auto userFunc = std::make_shared<BasicUserFunction>(popOffTop(args), 1.0);
+			parser->parseUserFunction(userFunc);
+			auto sym = parser->stack->top();
+			aJmat->at(i)->at(i) = sym->getValue();
+		}
+	}
+	else if (str.find("eye") != std::string::npos) {
+		args.erase(args.begin());
+		aJmat->identity();
+	}
+	else {
+		aJmat = readBasicOrientation(args);
+	}
+}
+
+void MbD::MBDynBody::createASMT()
+{
+	auto asmtMassMarker = std::make_shared<ASMTPrincipalMassMarker>();
+	asmtItem = asmtMassMarker;
+	asmtMassMarker->setMass(mass);
+	if (aJmat->isDiagonalToWithin(1.0e-6)) {
+		asmtMassMarker->setMomentOfInertias(aJmat->asDiagonalMatrix());
+		asmtMassMarker->setPosition3D(rPcmP);
+		asmtMassMarker->setRotationMatrix(FullMatrix<double>::identitysptr(3));
+		auto asmtPart = asmtAssembly()->partPartialNamed(nodeName);
+		asmtPart->setPrincipalMassMarker(asmtMassMarker);
+	}
+	else {
+		auto solver = std::make_shared<MomentOfInertiaSolver>();
+		solver->setm(mass);
+		solver->setJPP(aJmat);
+		solver->setrPoP(rPcmP);
+		solver->setAPo(FullMatrix<double>::identitysptr(3));
+		solver->setrPcmP(rPcmP);
+		solver->calc();
+		asmtMassMarker->setMomentOfInertias(solver->aJpp);
+		asmtMassMarker->setPosition3D(rPcmP);
+		asmtMassMarker->setRotationMatrix(solver->aAPp);
+		auto asmtPart = asmtAssembly()->partPartialNamed(nodeName);
+		asmtPart->setPrincipalMassMarker(asmtMassMarker);
+	}
 }

OndselSolver/MBDynBody.h

@@ -15,12 +15,14 @@ namespace MbD {
     public:
         void initialize() override;
         void parseMBDyn(std::string line);
-        void readInertiaMatrix(std::shared_ptr<std::vector<std::string>>& args);
+        void readMass(std::vector<std::string>& args);
+        void readInertiaMatrix(std::vector<std::string>& args);
+        void createASMT() override;
 
-        std::string bodyString, name, node;
+        std::string bodyString, name, nodeName;
         double mass;
-        FColDsptr rOfO;
-        FMatDsptr aAOf;
+        FColDsptr rPcmP;
+        FMatDsptr aJmat;
 
     };
 }

OndselSolver/MBDynCase.mbd

@@ -0,0 +1,349 @@
+###################################################################################################################### 
+ # MBDyn is a free and open-source general purpose multi-body dynamics software. See https://www.mbdyn.org/ for details. 
+ # This input file was automatically generated by the FreeCAD "MBD workbench". 
+ # To learn more about MBDyn input files, you can visit the website: https://www.sky-engin.jp/en/MBDynTutorial/index.html 
+ # Details about the structure of input files can be studied in the input file manuals: https://www.mbdyn.org/?Software_Download 
+ # Although MBDyn has already reached a mature stage and is used by several industries, 
+ # the FreeCAD dynamics workbench is still under development, and it is likely to have bugs. 
+ # Please be aware of this fact before you use this input file for any critical application. 
+ # If you think you have found a bug or have any suggestion, please send your comments to Jose Egas: 
+ # josegegas@gmail.com 
+ ###################################################################################################################### 
+
+ #-----------------------------------------------------------------------------
+ # [Data Block]
+
+ begin: data;
+     problem: initial value;
+ end: data;
+
+ #-----------------------------------------------------------------------------
+ # [Problem Block]
+
+ begin: initial value;
+     initial time: 0.0;
+     final time: 8.0;
+     time step: 0.1;
+     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: euler321;
+     omega rotates: no;
+     print: none;
+     initial stiffness: 1.0, 1.0;
+     structural nodes: 4;
+     rigid bodies: 3;
+     joints: 7;
+ end: control data;
+
+ #-----------------------------------------------------------------------------
+ # [Design Variables]
+
+     #Generic bodies
+
+         #body: 2
+             set: integer body_2 = 2; #body label
+             set: real mass_2 = 1.4486361883511716; #mass [kg]
+             set: real volume_2 = 0.0001833716694115407; #volume [m^3]
+
+         #body: 3
+             set: integer body_3 = 3; #body label
+             set: real mass_3 = 2.85294865570677; #mass [kg]
+             set: real volume_3 = 0.00036113274122870505; #volume [m^3]
+
+         #body: 4
+             set: integer body_4 = 4; #body label
+             set: real mass_4 = 10.859427202622147; #mass [kg]
+             set: real volume_4 = 0.0013746110383066007; #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
+             set: integer joint_2 = 2; #joint label
+
+         #joint: 3
+             set: integer joint_3 = 3; #joint label
+
+         #joint: 4
+             set: integer joint_4 = 4; #joint label
+
+         #joint: 5
+             set: integer joint_5 = 5; #joint label
+
+         #joint: 6
+             set: integer joint_6 = 6; #joint label
+
+         #joint: 7
+             set: integer joint_7 = 7; #joint label
+
+     #Nodes: initial conditions
+
+         #node: 1
+             set: real Px_1 = -0.121; #X component of the absolute position [m]
+             set: real Py_1 = -8.796847998598882e-19; #Y component of the absolute position [m]
+             set: real Pz_1 = -0.08; #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.015; #X component of the absolute position [m]
+             set: real Py_2 = 0.092; #Y component of the absolute position [m]
+             set: real Pz_2 = 0.008; #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.088; #X component of the absolute position [m]
+             set: real Py_3 = -0.055; #Y component of the absolute position [m]
+             set: real Pz_3 = 0.05; #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.3200000010688326; #X component of the absolute position [m]
+             set: real Py_4 = 1.4796688528733327e-10; #Y component of the absolute position [m]
+             set: real Pz_4 = 0.04999999225971574; #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.0012769356301204219; #moment of inertia [kg*m^2]
+             set: real Ixy_2 = 4.67e-20; #moment of inertia [kg*m^2]
+             set: real Ixz_2 = -0.0009495625871945231; #moment of inertia [kg*m^2]
+
+             set: real Iyx_2 = 4.67e-20; #moment of inertia [kg*m^2]
+             set: real Iyy_2 = 0.0028717510150978666; #moment of inertia [kg*m^2]
+             set: real Iyz_2 = 4.4900000000000004e-20; #moment of inertia [kg*m^2]
+
+             set: real Izx_2 = -0.0009495625871945231; #moment of inertia [kg*m^2]
+             set: real Izy_2 = 4.4900000000000004e-20; #moment of inertia [kg*m^2]
+             set: real Izz_2 = 0.002296471669735477; #moment of inertia [kg*m^2]
+
+             set: real Rx_2 = -0.03260146715730948; #X component of the relative center of mass [m]
+             set: real Ry_2 = 0.0; #Y component of the relative center of mass [m]
+             set: real Rz_2 = -0.028286762255221056; #Z component of the relative center of mass [m]
+
+         #body 3: 
+             set: real Ixx_3 = 0.0019563103180210077; #moment of inertia [kg*m^2]
+             set: real Ixy_3 = -7.580852e-16; #moment of inertia [kg*m^2]
+             set: real Ixz_3 = 5.386829000000001e-16; #moment of inertia [kg*m^2]
+
+             set: real Iyx_3 = -7.580852e-16; #moment of inertia [kg*m^2]
+             set: real Iyy_3 = 0.03371514809951082; #moment of inertia [kg*m^2]
+             set: real Iyz_3 = -4.95494e-17; #moment of inertia [kg*m^2]
+
+             set: real Izx_3 = 5.386829000000001e-16; #moment of inertia [kg*m^2]
+             set: real Izy_3 = -4.95494e-17; #moment of inertia [kg*m^2]
+             set: real Izz_3 = 0.03383792198797204; #moment of inertia [kg*m^2]
+
+             set: real Rx_3 = 7.87281351222191e-15; #X component of the relative center of mass [m]
+             set: real Ry_3 = 1.0302869668521452e-15; #Y component of the relative center of mass [m]
+             set: real Rz_3 = -0.024999999999998045; #Z component of the relative center of mass [m]
+
+         #body 4: 
+             set: real Ixx_4 = 0.07706742098795094; #moment of inertia [kg*m^2]
+             set: real Ixy_4 = 8.01291908309e-10; #moment of inertia [kg*m^2]
+             set: real Ixz_4 = -1.2135214817691901e-08; #moment of inertia [kg*m^2]
+
+             set: real Iyx_4 = 8.01291908309e-10; #moment of inertia [kg*m^2]
+             set: real Iyy_4 = 0.06635181579849883; #moment of inertia [kg*m^2]
+             set: real Iyz_4 = -3.32678908792e-11; #moment of inertia [kg*m^2]
+
+             set: real Izx_4 = -1.2135214817691901e-08; #moment of inertia [kg*m^2]
+             set: real Izy_4 = -3.32678908792e-11; #moment of inertia [kg*m^2]
+             set: real Izz_4 = 0.06179235045624136; #moment of inertia [kg*m^2]
+
+             set: real Rx_4 = 0.045580834634119355; #X component of the relative center of mass [m]
+             set: real Ry_4 = 2.0299354041266675e-10; #Y component of the relative center of mass [m]
+             set: real Rz_4 = -1.2562251448855477e-08; #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, 0.0, 0.0, 1.0, 2, 0.0, 1.0, 0.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, 0.0, 0.0, 1.0, 2, 0.0, 1.0, 0.0, #<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, 0.0, 0.0, 1.0, 2, 0.0, 1.0, 0.0, #<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, 0.0, 0.0, 1.0, 2, 0.0, 1.0, 0.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]
+
+         #IMPORTANT NOTE: FreeCAD provides the inertia matrix in the global reference frame,
+         #while by default, MBDyn assumes it to be in the reference frame of the node,
+         #thus, the matrix of inertia must be rotated, using the inverse of the placement
+         #matrix of the node. This is done using the inertial keyword and the following
+         #orientation matrix.
+
+         body: body_2,
+               structural_node_2, #<node_label> 
+               mass_2, #<mass> [kg]
+               Rx_2, Ry_2, Rz_2, #<relative_center_of_mass> [m]
+               Ixx_2, Ixy_2, Ixz_2, #<inertia_matrix> [kg*m^2]
+               Iyx_2, Iyy_2, Iyz_2,
+               Izx_2, Izy_2, Izz_2,
+               orientation, 3, 0.0, 0.0, 1.0, 2, 0.0, 1.0, 0.0; 
+
+         body: body_3,
+               structural_node_3, #<node_label> 
+               mass_3, #<mass> [kg]
+               Rx_3, Ry_3, Rz_3, #<relative_center_of_mass> [m]
+               Ixx_3, Ixy_3, Ixz_3, #<inertia_matrix> [kg*m^2]
+               Iyx_3, Iyy_3, Iyz_3,
+               Izx_3, Izy_3, Izz_3,
+               orientation, 3, 0.0, 0.0, 1.0, 2, 0.0, 1.0, 0.0; 
+
+         body: body_4,
+               structural_node_4, #<node_label> 
+               mass_4, #<mass> [kg]
+               Rx_4, Ry_4, Rz_4, #<relative_center_of_mass> [m]
+               Ixx_4, Ixy_4, Ixz_4, #<inertia_matrix> [kg*m^2]
+               Iyx_4, Iyy_4, Iyz_4,
+               Izx_4, Izy_4, Izz_4,
+               orientation, 3, 0.0, 0.0, 1.0, 2, 0.0, 1.0, 0.0; 
+
+     #-----------------------------------------------------------------------------
+     # [Joints]
+
+     joint: joint_1,
+         clamp,
+         structural_node_1, #<node_label>
+             -0.121, -8.796847998598882e-19, -0.08, #<absolute_pin_position> [m]
+             3, 0.0, 0.0, 1.0, 2, 0.0, 1.0, 0.0; #<absolute_orientation_matrix> 
+
+     joint: joint_2,
+         spherical hinge,
+         structural_node_1, #<node_1_label>
+         0.0, 0.0, 0.0, #<relative_pin_position_1> [m]
+         structural_node_2, #<node_2_label>
+         -0.07, 0.0, -0.055; #<relative_pin_position_2> [m]
+
+     joint: joint_3,
+            revolute hinge,
+     structural_node_2, #<node_1_label>
+     0.0, 0.0, 0.0, #<relative_position_1> [m]
+     orientation, 3, 0.0, 0.0, 1.0, 2, 0.0, 1.0, 0.0, #<relative_pin_orientation_matrix_1>
+     structural_node_3, #<node_2_label>
+                -0.14, 0.0, -0.024999999999999998, #<relative_position_2> [m]
+                orientation, 3, 0.0, 0.0, 1.0, 2, guess; #<relative_pin_orientation_matrix_2>
+
+     joint: joint_4,
+         drive hinge,
+         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_2, #<node_2_label>
+         orientation, 3, 0.0, 0.0, 1.0, 2, 0.0, 1.0, 0.0, #<relative_orientation_matrix_2>
+         single, 0., 0., 1., #<hinge_orientation>
+         string, "0"; #<hinge_orientation> [rad]
+
+     joint: joint_5,
+         in line,
+         structural_node_4, #<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_3, #<node_2_label>
+             offset, 0.14, 0.0, -0.024999999999999998; #<relative_offset> [m]
+
+     joint: joint_6,
+         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, 0.0, 1.0, 2, 0.0, 1.0, 0.0; #relative_orientation_matrix_2>
+
+     joint: joint_7,
+         in line,
+         structural_node_1, #<node_1_label>
+             0.0, 8.796847998604521e-19, 0.08, #<relative_line_position> [m]
+             3, 1.0, -2.220446049250313e-16, 2.220446049250313e-16, 2, -2.220446049250313e-16, 0.0, 1.0, #<relative_orientation>
+         structural_node_4, #<node_2_label>
+             offset, 0.0, 0.0, 0.0; #<relative_offset> [m]
+
+ end: elements;
+

OndselSolver/MBDynCase.mov

@@ -0,0 +1,324 @@
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749178 -0.09687939551894122 -3.252606517456513e-19 0.9999846242181696 -0.005545387925678226 0.09688088513829464 0.005519302327492587 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051885 0.9952948074630237 -0.001621407113303408 -0.09687939551894119 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.09688683977859862 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894111 2.168404344971009e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981437 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894122 -6.505213034913027e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894111 0 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894122 -4.336808689942018e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894111 2.168404344971009e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894122 -6.505213034913027e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894111 0 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894122 -4.336808689942018e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894111 2.168404344971009e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894122 -6.505213034913027e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894111 0 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894122 -4.336808689942018e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894111 2.168404344971009e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894122 -6.505213034913027e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894111 0 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894122 -4.336808689942018e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894111 2.168404344971009e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894122 -6.505213034913027e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894111 0 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894122 -4.336808689942018e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894111 2.168404344971009e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894122 -6.505213034913027e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894111 0 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894122 -4.336808689942018e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894111 2.168404344971009e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894122 -6.505213034913027e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894111 0 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894122 -4.336808689942018e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894111 2.168404344971009e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894122 -6.505213034913027e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894111 0 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894122 -4.336808689942018e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894111 2.168404344971009e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894122 -6.505213034913027e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894111 0 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894122 -4.336808689942018e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894111 2.168404344971009e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894122 -6.505213034913027e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894111 0 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894122 -4.336808689942018e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894111 2.168404344971009e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894122 -6.505213034913027e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894111 0 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894122 -4.336808689942018e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894111 2.168404344971009e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894122 -6.505213034913027e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894111 0 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894122 -4.336808689942018e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894111 2.168404344971009e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894122 -6.505213034913027e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894111 0 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894122 -4.336808689942018e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894111 2.168404344971009e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894122 -6.505213034913027e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894111 0 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894122 -4.336808689942018e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894111 2.168404344971009e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894122 -6.505213034913027e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894111 0 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894122 -4.336808689942018e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894111 2.168404344971009e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894122 -6.505213034913027e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894111 0 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894122 -4.336808689942018e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894111 2.168404344971009e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894122 -6.505213034913027e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894111 0 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894122 -4.336808689942018e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894111 2.168404344971009e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894122 -6.505213034913027e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894111 0 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894122 -4.336808689942018e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894111 2.168404344971009e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894122 -6.505213034913027e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894111 0 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894122 -4.336808689942018e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894111 2.168404344971009e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123014 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894122 -6.505213034913027e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749175 -0.09687939551894111 0 0.9999846242181696 -0.005545387925678225 0.09688088513829453 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 
+1 0.121 8.796847998598882e-19 0.08 1 0 0 0 1 0 0 0 1 -0 -0 0 -0 0 0 
+2 0.1853423520676719 -0.0003049963359123015 0.1415220993435272 0.9952959831601956 -0.0005372420906749176 -0.09687939551894122 -4.336808689942018e-19 0.9999846242181696 -0.005545387925678225 0.09688088513829464 0.005519302327492586 0.995280679706302 0 -0 -0 0 0 0 
+3 0.3222616402245218 -0.0002911328660981436 0.1799682739051886 0.9952948074630237 -0.001621407113303408 -0.09687939551894117 0.001089272628257954 0.9999840309514423 -0.005545387925678226 0.0968868397785986 0.005413767734014727 0.995280679706302 0 0 0 0 0 0 
+4 0.4640248981573186 -7.44074583944898e-17 0.1600000000000001 1 0 0 0 1 0 0 0 1 0 0 -0 0 0 0 

OndselSolver/MBDynControlData.cpp

@@ -8,42 +8,126 @@ void MbD::MBDynControlData::initialize()
 
 void MbD::MBDynControlData::parseMBDyn(std::vector<std::string>& lines)
 {
+	readMaxIterations(lines);
+	readDefaultOrientation(lines);
+	readOmegaRotates(lines);
+	readPrint(lines);
+	readInitialStiffness(lines);
 	readStructuralNodes(lines);
 	readRigidBodies(lines);
 	readJoints(lines);
+	assert(lines.size() == 2);
+}
+
+void MbD::MBDynControlData::readMaxIterations(std::vector<std::string>& lines)
+{
+	//max iterations: 1000;
+	std::vector<std::string> tokens{ "max", "iterations:" };
+	auto it = findLineWith(lines, tokens);
+	if (it == lines.end()) return;
+	std::istringstream iss(*it);
+	std::string str;
+	iss >> str;
+	iss >> str;
+	iss >> maxIterations;
+	lines.erase(it);
+}
+
+void MbD::MBDynControlData::readDefaultOrientation(std::vector<std::string>& lines)
+{
+	//default orientation: euler321;
+	std::vector<std::string> tokens{ "default", "orientation:" };
+	auto it = findLineWith(lines, tokens);
+	if (it == lines.end()) return;
+	std::istringstream iss(*it);
+	std::string str;
+	iss >> str;
+	iss >> str;
+	iss >> defaultOrientation;
+	lines.erase(it);
+}
+
+void MbD::MBDynControlData::readOmegaRotates(std::vector<std::string>& lines)
+{
+	//omega rotates: no;
+	std::vector<std::string> tokens{ "omega", "rotates:" };
+	auto it = findLineWith(lines, tokens);
+	if (it == lines.end()) return;
+	std::istringstream iss(*it);
+	std::string str;
+	iss >> str;
+	iss >> str;
+	iss >> omegaRotates;
+	lines.erase(it);
+}
+
+void MbD::MBDynControlData::readPrint(std::vector<std::string>& lines)
+{
+	//print: none;
+	std::vector<std::string> tokens{ "print:" };
+	auto it = findLineWith(lines, tokens);
+	if (it == lines.end()) return;
+	std::istringstream iss(*it);
+	std::string str;
+	iss >> str;
+	iss >> str;
+	iss >> print;
+	lines.erase(it);
+}
+
+void MbD::MBDynControlData::readInitialStiffness(std::vector<std::string>& lines)
+{
+	//initial stiffness: 1.0, 1.0;
+	std::vector<std::string> tokens{ "initial", "stiffness:" };
+	auto it = findLineWith(lines, tokens);
+	if (it == lines.end()) return;
+	std::istringstream iss(*it);
+	std::string str;
+	iss >> str;
+	iss >> str;
+	iss >> initialStiffness;
+	iss >> str;
+	initialStiffness.append(str);
+	lines.erase(it);
 }
 
 void MbD::MBDynControlData::readStructuralNodes(std::vector<std::string>& lines)
-{	
+{
 	//structural nodes: 4;
-	std::vector<std::string> tokens{"structural", "nodes:"};
+	std::vector<std::string> tokens{ "structural", "nodes:" };
 	auto it = findLineWith(lines, tokens);
+	if (it == lines.end()) return;
 	std::istringstream iss(*it);
 	std::string str;
 	iss >> str;
 	iss >> str;
 	iss >> structuralNodes;
+	lines.erase(it);
 }
 
 void MbD::MBDynControlData::readRigidBodies(std::vector<std::string>& lines)
 {
 	//rigid bodies:     3;
-	std::vector<std::string> tokens{"rigid", "bodies:"};
+	std::vector<std::string> tokens{ "rigid", "bodies:" };
 	auto it = findLineWith(lines, tokens);
+	if (it == lines.end()) return;
 	std::istringstream iss(*it);
 	std::string str;
 	iss >> str;
 	iss >> str;
 	iss >> rigidBodies;
+	lines.erase(it);
 }
 
 void MbD::MBDynControlData::readJoints(std::vector<std::string>& lines)
 {
 	//joints:           6;
-	std::vector<std::string> tokens{"joints:"};
+	std::vector<std::string> tokens{ "joints:" };
 	auto it = findLineWith(lines, tokens);
+	if (it == lines.end()) return;
 	std::istringstream iss(*it);
 	std::string str;
 	iss >> str;
 	iss >> joints;
+	lines.erase(it);
 }

OndselSolver/MBDynControlData.h

@@ -15,10 +15,20 @@ namespace MbD {
 	public:
 		void initialize() override;
 		void parseMBDyn(std::vector<std::string>& lines) override;
+		void readMaxIterations(std::vector<std::string>& lines);
+		void readDefaultOrientation(std::vector<std::string>& lines);
+		void readOmegaRotates(std::vector<std::string>& lines);
+		void readPrint(std::vector<std::string>& lines);
+		void readInitialStiffness(std::vector<std::string>& lines);
 		void readStructuralNodes(std::vector<std::string>& lines);
 		void readRigidBodies(std::vector<std::string>& lines);
 		void readJoints(std::vector<std::string>& lines);
 
+		int maxIterations = 1000;
+		std::string defaultOrientation = "euler321";
+		std::string omegaRotates = "no";
+		std::string print = "none";
+		std::string initialStiffness = "1.0, 1.0";
 		int structuralNodes, rigidBodies, joints;
 	};
 }

OndselSolver/MBDynData.cpp

@@ -9,7 +9,7 @@ void MbD::MBDynData::initialize()
 void MbD::MBDynData::parseMBDyn(std::vector<std::string>& lines)
 {
 	assert(lines.size() == 3);
-	std::vector<std::string> tokens{"problem:", "initial", "value;"};
+	std::vector<std::string> tokens{ "problem:", "initial", "value" };
 	auto problemit = findLineWith(lines, tokens);
 	assert(problemit != lines.end());
 }

OndselSolver/MBDynElements.cpp

@@ -1,37 +0,0 @@
-#include "MBDynElements.h"
-#include "MBDynBody.h"
-
-using namespace MbD;
-
-void MbD::MBDynElements::initialize()
-{
-}
-
-void MbD::MBDynElements::parseMBDyn(std::vector<std::string>& lines)
-{
-	elements = std::make_shared<std::vector<std::shared_ptr<MBDynElement>>>();
-	std::vector<std::string> bodyToken{ "body:" };
-	std::vector<std::string> jointToken{ "joint:" };
-	std::vector<std::string>::iterator it;
-	while (true) {
-		it = findLineWith(lines, bodyToken);
-		if (it != lines.end()) {
-			auto body = std::make_shared<MBDynBody>();
-			body->owner = this;
-			body->parseMBDyn(*it);
-			elements->push_back(body);
-			lines.erase(it);
-			continue;
-		}
-		it = findLineWith(lines, jointToken);
-		if (it != lines.end()) {
-			auto body = std::make_shared<MBDynBody>();
-			body->owner = this;
-			body->parseMBDyn(*it);
-			elements->push_back(body);
-			lines.erase(it);
-			continue;
-		}
-		break;
-	}
-}

OndselSolver/MBDynInitialValue.cpp

@@ -1,4 +1,6 @@
 #include "MBDynInitialValue.h"
+#include "ASMTSimulationParameters.h"
+#include "ASMTAssembly.h"
 
 using namespace MbD;
 
@@ -13,63 +15,134 @@ void MbD::MBDynInitialValue::parseMBDyn(std::vector<std::string>& lines)
 	readTimeStep(lines);
 	readMaxIterations(lines);
 	readTolerance(lines);
+	readDerivativesTolerance(lines);
+	readDerivativesMaxIterations(lines);
+	readDerivativesCoefficient(lines);
+	assert(lines.size() == 2);
 }
 
 void MbD::MBDynInitialValue::readInitialTime(std::vector<std::string>& lines)
 {
 	//initial time:   0.;
-	std::vector<std::string> tokens{"initial", "time:"};
+	std::vector<std::string> tokens{ "initial", "time:" };
 	auto it = findLineWith(lines, tokens);
+	if (it == lines.end()) return;
 	std::istringstream iss(*it);
 	std::string str;
 	iss >> str;
 	iss >> str;
-	iss >> initialTime;	
+	iss >> initialTime;
+	lines.erase(it);
 }
 
 void MbD::MBDynInitialValue::readFinalTime(std::vector<std::string>& lines)
 {
 	//final time:     5.;
-	std::vector<std::string> tokens{"final", "time:"};
+	std::vector<std::string> tokens{ "final", "time:" };
 	auto it = findLineWith(lines, tokens);
+	if (it == lines.end()) return;
 	std::istringstream iss(*it);
 	std::string str;
 	iss >> str;
 	iss >> str;
 	iss >> finalTime;
+	lines.erase(it);
 }
 
 void MbD::MBDynInitialValue::readTimeStep(std::vector<std::string>& lines)
 {
 	//time step:      1.e-2;
-	std::vector<std::string> tokens{"time", "step:"};
+	std::vector<std::string> tokens{ "time", "step:" };
 	auto it = findLineWith(lines, tokens);
+	if (it == lines.end()) return;
 	std::istringstream iss(*it);
 	std::string str;
 	iss >> str;
 	iss >> str;
 	iss >> timeStep;
+	lines.erase(it);
 }
 
 void MbD::MBDynInitialValue::readMaxIterations(std::vector<std::string>& lines)
 {
 	//max iterations: 10;
-	std::vector<std::string> tokens{"max", "iterations:"};
+	std::vector<std::string> tokens{ "max", "iterations:" };
 	auto it = findLineWith(lines, tokens);
+	if (it == lines.end()) return;
 	std::istringstream iss(*it);
 	std::string str;
 	iss >> str;
 	iss >> str;
 	iss >> maxIterations;
+	lines.erase(it);
 }
 
 void MbD::MBDynInitialValue::readTolerance(std::vector<std::string>& lines)
 {
 	//tolerance:      1.e-6;
-	std::vector<std::string> tokens{"tolerance:"};
+	std::vector<std::string> tokens{ "tolerance:" };
 	auto it = findLineWith(lines, tokens);
+	if (it == lines.end()) return;
 	std::istringstream iss(*it);
 	std::string str;
 	iss >> str;
 	iss >> tolerance;
+	lines.erase(it);
+}
+
+void MbD::MBDynInitialValue::readDerivativesTolerance(std::vector<std::string>& lines)
+{
+	//derivatives tolerance: 0.0001;
+	std::vector<std::string> tokens{ "derivatives", "tolerance:" };
+	auto it = findLineWith(lines, tokens);
+	if (it == lines.end()) return;
+	std::istringstream iss(*it);
+	std::string str;
+	iss >> str;
+	iss >> str;
+	iss >> derivativesTolerance;
+	lines.erase(it);
+}
+
+void MbD::MBDynInitialValue::readDerivativesMaxIterations(std::vector<std::string>& lines)
+{
+	//derivatives max iterations: 100;
+	std::vector<std::string> tokens{ "derivatives", "max", "iterations:" };
+	auto it = findLineWith(lines, tokens);
+	if (it == lines.end()) return;
+	std::istringstream iss(*it);
+	std::string str;
+	iss >> str;
+	iss >> str;
+	iss >> str;
+	iss >> derivativesMaxIterations;
+	lines.erase(it);
+}
+
+void MbD::MBDynInitialValue::readDerivativesCoefficient(std::vector<std::string>& lines)
+{
+	//derivatives coefficient: auto;
+	std::vector<std::string> tokens{ "derivatives", "coefficient:" };
+	auto it = findLineWith(lines, tokens);
+	if (it == lines.end()) return;
+	std::istringstream iss(*it);
+	std::string str;
+	iss >> str;
+	iss >> str;
+	iss >> derivativesCoefficient;
+	lines.erase(it);
+}
+
+void MbD::MBDynInitialValue::createASMT()
+{
+	auto simulationParameters = std::make_shared<ASMTSimulationParameters>();
+	asmtItem = simulationParameters;
+	simulationParameters->settstart(initialTime);
+	simulationParameters->settend(finalTime);	//tstart == tend Initial Conditions only.
+	simulationParameters->sethmin(1.0e-9);
+	simulationParameters->sethmax(1.0);
+	simulationParameters->sethout(timeStep);
+	simulationParameters->seterrorTol(tolerance);
+	simulationParameters->setmaxIter(maxIterations);
+	asmtAssembly()->setSimulationParameters(simulationParameters);
 }

OndselSolver/MBDynInitialValue.h

@@ -20,8 +20,15 @@ namespace MbD {
 		void readTimeStep(std::vector<std::string>& lines);
 		void readMaxIterations(std::vector<std::string>& lines);
 		void readTolerance(std::vector<std::string>& lines);
+		void readDerivativesTolerance(std::vector<std::string>& lines);
+		void readDerivativesMaxIterations(std::vector<std::string>& lines);
+		void readDerivativesCoefficient(std::vector<std::string>& lines);
+		void createASMT() override;
 
-		double initialTime, finalTime, timeStep, tolerance;
-		int maxIterations;
+		double initialTime = 0.0, finalTime = 5.0, timeStep = 1.0e-2, tolerance = 1.0e-6;
+		int maxIterations = 10;
+		double derivativesTolerance = 1.0e-4;
+		int derivativesMaxIterations = 100;
+		std::string derivativesCoefficient = "auto";
 	};
 }

OndselSolver/MBDynItem.cpp

@@ -1,12 +1,13 @@
 #include "MBDynItem.h"
 #include "MBDynSystem.h"
-#include "MBDynVariables.h"
 #include "SymbolicParser.h"
 #include "BasicUserFunction.h"
 #include "EulerAngles.h"
 #include "Constant.h"
-#include "MBDynReferences.h"
 #include "MBDynReference.h"
+#include "FullMatrix.h"
+#include "ASMTItem.h"
+#include "MBDynBody.h"
 
 
 using namespace MbD;
@@ -21,6 +22,11 @@ void MbD::MBDynItem::initialize()
 	assert(false);
 }
 
+void MbD::MBDynItem::noop()
+{
+	//No Operations
+}
+
 void MbD::MBDynItem::parseMBDyn(std::vector<std::string>& lines)
 {
 	assert(false);
@@ -45,108 +51,288 @@ bool MbD::MBDynItem::lineHasTokens(const std::string& line, std::vector<std::str
 	return true;
 }
 
-std::shared_ptr<MBDynVariables> MbD::MBDynItem::mbdynVariables()
+std::shared_ptr<std::vector<std::shared_ptr<MBDynNode>>> MbD::MBDynItem::mbdynNodes()
+{
+	return owner->mbdynNodes();
+}
+
+std::vector<std::string> MbD::MBDynItem::nodeNames()
+{
+	return owner->nodeNames();
+}
+
+std::shared_ptr<std::map<std::string, Symsptr>> MbD::MBDynItem::mbdynVariables()
 {
 	return owner->mbdynVariables();
 }
 
-std::shared_ptr<MBDynReferences> MbD::MBDynItem::mbdynReferences()
+std::shared_ptr<std::map<std::string, std::shared_ptr<MBDynReference>>> MbD::MBDynItem::mbdynReferences()
 {
 	return owner->mbdynReferences();
 }
 
-FColDsptr MbD::MBDynItem::readPosition(std::shared_ptr<std::vector<std::string>>& args)
+void MbD::MBDynItem::createASMT()
+{
+	assert(false);
+}
+
+std::shared_ptr<MBDynNode> MbD::MBDynItem::nodeAt(std::string nodeName)
+{
+	return owner->nodeAt(nodeName);
+}
+
+int MbD::MBDynItem::nodeidAt(std::string nodeName)
+{
+	return owner->nodeidAt(nodeName);
+}
+
+std::shared_ptr<MBDynBody> MbD::MBDynItem::bodyWithNode(std::string nodeName)
+{
+	return owner->bodyWithNode(nodeName);
+}
+
+std::shared_ptr<ASMTAssembly> MbD::MBDynItem::asmtAssembly()
+{
+	return owner->asmtAssembly();
+}
+
+FColDsptr MbD::MBDynItem::readVector3(std::vector<std::string>& args)
+{
+	auto parser = std::make_shared<SymbolicParser>();
+	parser->variables = mbdynVariables();
+	auto rFfF = std::make_shared<FullColumn<double>>(3);
+	auto& str = args.at(0);
+	if (str.find("null") != std::string::npos) {
+		args.erase(args.begin());
+	}
+	else {
+		for (int i = 0; i < 3; i++)
+		{
+			auto userFunc = std::make_shared<BasicUserFunction>(popOffTop(args), 1.0);
+			parser->parseUserFunction(userFunc);
+			auto sym = parser->stack->top();
+			rFfF->at(i) = sym->getValue();
+		}
+
+	}
+	return rFfF;
+}
+
+FColDsptr MbD::MBDynItem::readPosition(std::vector<std::string>& args)
 {
 	auto rOfO = std::make_shared<FullColumn<double>>(3);
-	auto str = args->at(0);
-	if (str.find("reference") != std::string::npos) {
-		args->erase(args->begin());
-		std::string refName;
-		std::istringstream iss(args->at(0));
-		iss >> refName;
-		auto ref = mbdynReferences()->references->at(refName);
-		args->erase(args->begin());
+	if (args.empty()) return rOfO;
+	auto& str = args.at(0);
+	if (str.find("orientation") != std::string::npos) {
+		//Do nothing
+	}
+	else if (str.find("reference") != std::string::npos) {
+		args.erase(args.begin());
+		auto refName = readStringOffTop(args);
+		auto ref = mbdynReferences()->at(refName);
 		auto rFfF = readBasicPosition(args);
 		auto rOFO = ref->rOfO;
 		auto aAOF = ref->aAOf;
 		rOfO = rOFO->plusFullColumn(aAOF->timesFullColumn(rFfF));
 	}
+	else if (str.find("offset") != std::string::npos) {
+		args.erase(args.begin());
+		rOfO = readBasicPosition(args);
+	}
+	else if (str.find("null") != std::string::npos) {
+		args.erase(args.begin());
+	}
 	else {
 		rOfO = readBasicPosition(args);
 	}
 	return rOfO;
 }
 
-FColDsptr MbD::MBDynItem::readBasicPosition(std::shared_ptr<std::vector<std::string>>& args)
+FColDsptr MbD::MBDynItem::readBasicPosition(std::vector<std::string>& args)
 {
-	auto parser = CREATE<SymbolicParser>::With();
-	parser->variables = mbdynVariables()->variables;
-	auto rFfF = std::make_shared<FullColumn<double>>(3);
-	std::string str;
-	for (int i = 0; i < 3; i++)
-	{
-		str = args->at(0);
-		args->erase(args->begin());
-		auto userFunc = CREATE<BasicUserFunction>::With(str, 1.0);
-		parser->parseUserFunction(userFunc);
-		auto sym = parser->stack->top();
-		rFfF->at(i) = sym->getValue();
-	}
-	return rFfF;
+	return readVector3(args);
 }
 
-FMatDsptr MbD::MBDynItem::readOrientation(std::shared_ptr<std::vector<std::string>>& args)
+FMatDsptr MbD::MBDynItem::readOrientation(std::vector<std::string>& args)
 {
-	auto aAOf = std::make_shared<FullMatrix<double>>(3, 3);
-	auto str = args->at(0);
+	auto aAOf = FullMatrix<double>::identitysptr(3);
+	if (args.empty()) return aAOf;
+	auto& str = args.at(0);
 	if (str.find("reference") != std::string::npos) {
-		args->erase(args->begin());
-		std::string refName;
-		std::istringstream iss(args->at(0));
-		iss >> refName;
-		auto ref = mbdynReferences()->references->at(refName);
-		args->erase(args->begin());
+		args.erase(args.begin());
+		auto refName = readStringOffTop(args);
+		auto ref = mbdynReferences()->at(refName);
 		auto aAFf = readBasicOrientation(args);
 		auto aAOF = ref->aAOf;
 		aAOf = aAOF->timesFullMatrix(aAFf);
 	}
+	else if (str.find("hinge") != std::string::npos) {
+		args.erase(args.begin());
+		aAOf = readOrientation(args);
+	}
+	else if (str.find("orientation") != std::string::npos) {
+		args.erase(args.begin());
+		aAOf = readOrientation(args);
+	}
 	else {
 		aAOf = readBasicOrientation(args);
 	}
 	return aAOf;
 }
 
-FMatDsptr MbD::MBDynItem::readBasicOrientation(std::shared_ptr<std::vector<std::string>>& args)
+FMatDsptr MbD::MBDynItem::readBasicOrientation(std::vector<std::string>& args)
 {
-	auto parser = CREATE<SymbolicParser>::With();
-	parser->variables = mbdynVariables()->variables;
-	auto euler = std::make_shared<EulerAngles<Symsptr>>();
-	auto str = args->at(0);
+	auto parser = std::make_shared<SymbolicParser>();
+	parser->variables = mbdynVariables();
+	auto& str = args.at(0);
 	if (str.find("euler") != std::string::npos) {
-		args->erase(args->begin());
+		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 });
 		for (int i = 0; i < 3; i++)
 		{
-			str = args->at(0);
-			args->erase(args->begin());
-			auto userFunc = CREATE<BasicUserFunction>::With(str, 1.0);
+			auto userFunc = std::make_shared<BasicUserFunction>(popOffTop(args), 1.0);
 			parser->parseUserFunction(userFunc);
 			auto sym = parser->stack->top();
 			euler->at(i) = sym;
 		}
+		euler->calc();
+		auto aAFf = euler->aA;
+		return aAFf;
 	}
-	else if (str.find("eye") != std::string::npos) {
-		args->erase(args->begin());
-		euler->rotOrder = std::make_shared<FullColumn<int>>(std::initializer_list<int>{ 1, 2, 3 });
-		for (int i = 0; i < 3; i++)
+	if (str.find("eye") != std::string::npos) {
+		args.erase(args.begin());
+		auto aAFf = FullMatrix<double>::identitysptr(3);
+		return aAFf;
+	}
+	auto iss = std::istringstream(str);
+	int integer;
+	iss >> integer;
+	if (integer == 1) {
+		args.erase(args.begin());
+		FColDsptr vecX, vecY, vecZ, vec;
+		vecX = readPosition(args);
+		vecX->normalizeSelf();
+		auto axis = stoi(popOffTop(args));
+		str = args.at(0);
+		if (str.find("guess") != std::string::npos) {
+			args.erase(args.begin());
+			double min = std::numeric_limits<double>::max();
+			double max = -1.0;
+			int imin, imax;
+			for (int i = 0; i < 3; i++)
+			{
+				auto mag = std::abs(vecX->at(i));
+				if (mag > max) {
+					imax = i;
+					max = mag;
+				}
+				if (mag < min) {
+					imin = i;
+					min = mag;
+				}
+			}
+			vec = std::make_shared<FullColumn<double>>(3);
+			vec->at(imin) = 1.0;
+			vec->at(imax) = -vecX->at(imin) / vecX->at(imax);
+		}
+		else {
+			vec = readPosition(args);
+		}
+		vec->normalizeSelf();
+		if (axis == 2) {
+			vecZ = vecX->cross(vec);
+			vecY = vecZ->cross(vecX);
+		}
+		else if (axis == 3) {
+			vecY = vec->cross(vecX);
+			vecZ = vecX->cross(vecY);
+		}
+		else {
+			assert(false);
+		}
+		auto aAFf = FullMatrix<double>::identitysptr(3);
+		aAFf->atijputFullColumn(0, 0, vecX);
+		aAFf->atijputFullColumn(0, 1, vecY);
+		aAFf->atijputFullColumn(0, 2, vecZ);
+		return aAFf;
+	}
+	if (integer == 3) {
+		args.erase(args.begin());
+		FColDsptr vecX, vecY, vecZ, vec;
+		vecZ = readPosition(args);
+		vecZ->normalizeSelf();
+		auto axis = stoi(popOffTop(args));
+		str = args.at(0);
+		if (str.find("guess") != std::string::npos) {
+			args.erase(args.begin());
+			double min = std::numeric_limits<double>::max();
+			double max = -1.0;
+			int imin, imax;
+			for (int i = 0; i < 3; i++)
+			{
+				auto mag = std::abs(vecZ->at(i));
+				if (mag > max) {
+					imax = i;
+					max = mag;
+				}
+				if (mag < min) {
+					imin = i;
+					min = mag;
+				}
+			}
+			vec = std::make_shared<FullColumn<double>>(3);
+			vec->at(imin) = 1.0;
+			vec->at(imax) = -vecZ->at(imin) / vecZ->at(imax);
+		}
+		else {
+			vec = readPosition(args);
+		}
+		vec->normalizeSelf();
+		if (axis == 2) {
+			vecX = vec->cross(vecZ);
+			vecY = vecZ->cross(vecX);
+		}
+		else if (axis == 1) {
+			vecY = vecZ->cross(vec);
+			vecX = vecY->cross(vecZ);
+		}
+		else {
+			assert(false);
+		}
+		auto aAFf = FullMatrix<double>::identitysptr(3);
+		aAFf->atijputFullColumn(0, 0, vecX);
+		aAFf->atijputFullColumn(0, 1, vecY);
+		aAFf->atijputFullColumn(0, 2, vecZ);
+		return aAFf;
+	}
+	auto aAFf = FullMatrix<double>::identitysptr(3);
+	for (int i = 0; i < 3; i++)
+	{
+		auto rowi = aAFf->at(i);
+		for (int j = 0; j < 3; j++)
 		{
-			euler->at(i) = std::make_shared<Constant>(0.0);
+			auto userFunc = std::make_shared<BasicUserFunction>(popOffTop(args), 1.0);
+			parser->parseUserFunction(userFunc);
+			auto sym = parser->stack->top();
+			rowi->at(j) = sym->getValue();
 		}
 	}
-	else {
-		assert(false);
-	}
-	euler->calc();
-	auto aAFf = euler->aA;
 	return aAFf;
 }
+
+std::string MbD::MBDynItem::popOffTop(std::vector<std::string>& args)
+{
+	auto str = args.at(0);
+	args.erase(args.begin());
+	return str;
+}
+
+std::string MbD::MBDynItem::readStringOffTop(std::vector<std::string>& args)
+{
+	auto iss = std::istringstream(args.at(0));
+	args.erase(args.begin());
+	std::string str;
+	iss >> str;
+	return str;
+}

OndselSolver/MBDynItem.h

@@ -11,8 +11,12 @@
 
 namespace MbD {
 	class MBDynSystem;
-	class MBDynVariables;
-	class MBDynReferences;
+	class MBDynVariable;
+	class MBDynReference;
+	class MBDynNode;
+	class ASMTItem;
+	class MBDynBody;
+	class ASMTAssembly;
 
 	class MBDynItem
 	{
@@ -21,20 +25,32 @@ namespace MbD {
 		virtual MBDynSystem* root();
 
 		virtual void initialize();
+		void noop();
 		//void setName(std::string str);
 		virtual void parseMBDyn(std::vector<std::string>& lines);
 		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);
-		virtual std::shared_ptr<MBDynVariables> mbdynVariables();
-		virtual std::shared_ptr<MBDynReferences> mbdynReferences();
-		FColDsptr readPosition(std::shared_ptr<std::vector<std::string>>& args);
-		FColDsptr readBasicPosition(std::shared_ptr<std::vector<std::string>>& args);
-		FMatDsptr readOrientation(std::shared_ptr<std::vector<std::string>>& args);
-		FMatDsptr readBasicOrientation(std::shared_ptr<std::vector<std::string>>& args);
+		virtual std::shared_ptr<std::vector<std::shared_ptr<MBDynNode>>> mbdynNodes();
+		virtual std::vector<std::string> nodeNames();
+		virtual std::shared_ptr<std::map<std::string, Symsptr>> mbdynVariables();
+		virtual std::shared_ptr<std::map<std::string, std::shared_ptr<MBDynReference>>> mbdynReferences();
+		virtual void createASMT();
+		virtual std::shared_ptr<MBDynNode> nodeAt(std::string nodeName);
+		virtual int nodeidAt(std::string nodeName);
+		virtual std::shared_ptr<MBDynBody> bodyWithNode(std::string nodeName);
+		virtual std::shared_ptr<ASMTAssembly> asmtAssembly();
+
+		FColDsptr readVector3(std::vector<std::string>& args);
+		FColDsptr readPosition(std::vector<std::string>& args);
+		FColDsptr readBasicPosition(std::vector<std::string>& args);
+		FMatDsptr readOrientation(std::vector<std::string>& args);
+		FMatDsptr readBasicOrientation(std::vector<std::string>& args);
+		std::string popOffTop(std::vector<std::string>& args);
+		std::string readStringOffTop(std::vector<std::string>& args);
 
 		std::string name;
 		MBDynItem* owner;
-		std::shared_ptr<Item> mbdObject;
+		std::shared_ptr<ASMTItem> asmtItem;
 
 
 	};

OndselSolver/MBDynJoint.cpp

@@ -1,4 +1,15 @@
+#include <regex>
+
 #include "MBDynJoint.h"
+#include "ASMTMarker.h"
+#include "ASMTPart.h"
+#include "ASMTAssembly.h"
+#include "ASMTRevoluteJoint.h"
+#include "ASMTRotationalMotion.h"
+#include "ASMTPointInLineJoint.h"
+#include "ASMTNoRotationJoint.h"
+#include "ASMTFixedJoint.h"
+#include "ASMTSphericalJoint.h"
 
 using namespace MbD;
 
@@ -6,7 +17,238 @@ void MbD::MBDynJoint::initialize()
 {
 }
 
-void MbD::MBDynJoint::parseMBDyn(std::vector<std::string>& lines)
+void MbD::MBDynJoint::parseMBDyn(std::string line)
 {
-	assert(false);
+	jointString = 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);
+		readMarkerJ(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>();
+	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->parseMBDyn(markerArgs);
+}
+
+void MbD::MBDynJoint::readMarkerJ(std::vector<std::string>& args)
+{
+	if (args.empty()) return;
+	mkr2 = std::make_shared<MBDynMarker>();
+	mkr2->owner = this;
+	mkr2->nodeStr = readStringOffTop(args);
+	mkr2->parseMBDyn(args);
+}
+
+void MbD::MBDynJoint::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 slope, initValue, initTime, finalTime;
+		slope = popOffTop(args);
+		initTime = popOffTop(args);
+		finalTime = popOffTop(args);
+		initValue = popOffTop(args);
+		//f = slope*(time - t0) + f0
+		auto ss = std::stringstream();
+		ss << slope << "*(time - " << initTime << ") + " << initValue;
+		formula = ss.str();
+	} else if (str.find("single") != std::string::npos) {
+		args.erase(args.begin());
+		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("\""), "");
+	}
+	else {
+		assert(false);
+	}
+}
+
+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(formula);
+		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);
 }

OndselSolver/MBDynJoint.h

@@ -8,13 +8,21 @@
 
 #pragma once
 #include "MBDynElement.h"
+#include "MBDynMarker.h"
 
 namespace MbD {
     class MBDynJoint : public MBDynElement
     {
     public:
         void initialize() override;
-        void parseMBDyn(std::vector<std::string>& lines) override;
+        void parseMBDyn(std::string line);
+        void readMarkerI(std::vector<std::string>& args);
+        void readMarkerJ(std::vector<std::string>& args);
+        void readFunction(std::vector<std::string>& args);
+        void createASMT() override;
 
+        std::string jointString, joint_type, node_1_label, node_2_label;
+        std::shared_ptr<MBDynMarker> mkr1, mkr2;
+        std::string formula;
     };
 }

OndselSolver/MBDynLabels.cpp

@@ -1,31 +0,0 @@
-#include "MBDynLabels.h"
-
-using namespace MbD;
-
-void MbD::MBDynLabels::initialize()
-{
-}
-
-void MbD::MBDynLabels::parseMBDyn(std::vector<std::string>& lines)
-{
-	labels = std::make_shared<std::map<std::string, int>>();
-	std::string str, label;
-	int intValue;
-	std::vector<std::string> tokens{"set:", "integer"};
-	while (true) {
-		auto it = findLineWith(lines, tokens);
-		if (it != lines.end()) {
-			std::istringstream iss(*it);
-			iss >> str;
-			iss >> str;
-			iss >> label;
-			iss >> str;
-			iss >> intValue;
-			labels->insert(std::make_pair(label, intValue));
-			lines.erase(it);
-		}
-		else {
-			break;
-		}
-	}
-}

OndselSolver/MBDynMarker.cpp

@@ -0,0 +1,55 @@
+#include "MBDynMarker.h"
+#include "MBDynStructural.h"
+#include "ASMTMarker.h"
+#include "ASMTPart.h"
+#include "ASMTAssembly.h"
+#include "MBDynBody.h"
+#include "ASMTSpatialContainer.h"
+
+using namespace MbD;
+
+void MbD::MBDynMarker::parseMBDyn(std::vector<std::string>& args)
+{
+	rPmP = std::make_shared<FullColumn<double>>(3);
+	aAPm = FullMatrix<double>::identitysptr(3);
+	if (args.empty()) return;
+	auto& str = args.at(0);
+	if (str.find("reference") != std::string::npos) {
+		auto strucNode = std::static_pointer_cast<MBDynStructural>(nodeAt(nodeStr));
+		auto rOPO = strucNode->rOfO;
+		auto aAOP = strucNode->aAOf;
+		auto rOmO = readPosition(args);
+		auto aAOm = readOrientation(args);
+		rPmP = aAOP->transposeTimesFullColumn(rOmO->minusFullColumn(rOPO));
+		aAPm = aAOP->transposeTimesFullMatrix(aAOm);
+	}
+	else if (str.find("offset") != std::string::npos) {
+		rPmP = readPosition(args);
+	}
+	else {
+		rPmP = readPosition(args);
+		aAPm = readOrientation(args);
+	}
+}
+
+void MbD::MBDynMarker::createASMT()
+{
+	auto asmtAsm = asmtAssembly();
+	if (nodeStr == "Assembly") {
+		auto mkr = std::make_shared<ASMTMarker>();
+		asmtItem = mkr;
+		mkr->setName(asmtAsm->generateUniqueMarkerName());
+		mkr->setPosition3D(rPmP);
+		mkr->setRotationMatrix(aAPm);
+		asmtAsm->addMarker(mkr);
+	}
+	else {
+		auto asmtPart = asmtAsm->partPartialNamed(nodeStr);
+		auto mkr = std::make_shared<ASMTMarker>();
+		asmtItem = mkr;
+		mkr->setName(asmtPart->generateUniqueMarkerName());
+		mkr->setPosition3D(rPmP);
+		mkr->setRotationMatrix(aAPm);
+		asmtPart->addMarker(mkr);
+	}
+}

OndselSolver/MBDynMarker.h

@@ -10,12 +10,15 @@
 #include "MBDynItem.h"
 
 namespace MbD {
-	class MBDynVariables : public MBDynItem
+
+	class MBDynMarker : public MBDynItem
 	{
 	public:
-		void initialize() override;
-		void parseMBDyn(std::vector<std::string>& lines) override;
+		void parseMBDyn(std::vector<std::string>& args);
+		void createASMT() override;
 
-		std::shared_ptr<std::map<std::string, Symsptr>> variables;
+		std::string nodeStr;
+		FColDsptr rPmP; //part to marker
+		FMatDsptr aAPm;
 	};
 }

OndselSolver/MBDynNode.cpp

@@ -1,4 +1,6 @@
 #include "MBDynNode.h"
+#include "ASMTPart.h"
+#include "EulerAngles.h"
 
 using namespace MbD;
 
@@ -10,3 +12,36 @@ void MbD::MBDynNode::parseMBDyn(std::vector<std::string>& lines)
 {
 	assert(false);
 }
+
+void MbD::MBDynNode::outputLine(int i, std::ostream& os)
+{
+	auto id = nodeidAt(name);
+	auto asmtPart = std::static_pointer_cast<ASMTPart>(asmtItem);
+	auto x = asmtPart->xs->at(i);
+	auto y = asmtPart->ys->at(i);
+	auto z = asmtPart->zs->at(i);
+	auto bryantAngles = std::make_shared<EulerAngles<double>>();
+	bryantAngles->setRotOrder(1, 2, 3);
+	bryantAngles->at(0) = asmtPart->bryxs->at(i);
+	bryantAngles->at(1) = asmtPart->bryys->at(i);
+	bryantAngles->at(2) = asmtPart->bryzs->at(i);
+	bryantAngles->calc();
+	auto aA = bryantAngles->aA;
+	auto vx = asmtPart->vxs->at(i);
+	auto vy = asmtPart->vys->at(i);
+	auto vz = asmtPart->vzs->at(i);
+	auto omex = asmtPart->omexs->at(i);
+	auto omey = asmtPart->omeys->at(i);
+	auto omez = asmtPart->omezs->at(i);
+	os << id << " ";
+	os << x << " " << y << " " << z << " ";
+	auto row = aA->at(0);
+	os << row->at(0) << " " << row->at(1) << " " << row->at(2) << " ";
+	row = aA->at(1);
+	os << row->at(0) << " " << row->at(1) << " " << row->at(2) << " ";
+	row = aA->at(2);
+	os << row->at(0) << " " << row->at(1) << " " << row->at(2) << " ";
+	os << vx << " " << vy << " " << vz << " ";
+	os << omex << " " << omey << " " << omez << " ";
+	os << std::endl;
+}

OndselSolver/MBDynNode.h

@@ -15,6 +15,7 @@ namespace MbD {
     public:
         void initialize() override;
         void parseMBDyn(std::vector<std::string>& lines) override;
+        void outputLine(int i, std::ostream& os);
 
     };
 }

OndselSolver/MBDynNodes.cpp

@@ -1,28 +0,0 @@
-#include "MBDynNodes.h"
-#include "MBDynNode.h"
-#include "MBDynStructural.h"
-
-using namespace MbD;
-
-void MbD::MBDynNodes::initialize()
-{
-}
-
-void MbD::MBDynNodes::parseMBDyn(std::vector<std::string>& lines)
-{
-	nodes = std::make_shared<std::vector<std::shared_ptr<MBDynNode>>>();
-	std::vector<std::string> tokens{ "structural:" };
-	while (true) {
-		auto it = findLineWith(lines, tokens);
-		if (it != lines.end()) {
-			auto structural = std::make_shared<MBDynStructural>();
-			structural->owner = this;
-			structural->parseMBDyn(*it);
-			nodes->push_back(structural);
-			lines.erase(it);
-		}
-		else {
-			break;
-		}
-	}
-}

OndselSolver/MBDynReference.cpp

@@ -1,5 +1,4 @@
 #include "MBDynReference.h"
-#include "MBDynVariables.h"
 #include "SymbolicParser.h"
 #include "BasicUserFunction.h"
 #include "EulerAngles.h"
@@ -17,24 +16,24 @@ void MbD::MBDynReference::parseMBDyn(std::string line)
 	auto pos = line.find(":");
 	auto front = line.substr(previousPos, pos - previousPos);
 	assert(front.find("reference") != std::string::npos);
-	auto arguments = std::make_shared<std::vector<std::string>>();
+	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);
+			arguments.push_back(argument);
 		}
 		else {
 			argument = line.substr(previousPos + 1);
-			arguments->push_back(argument);
+			arguments.push_back(argument);
 			break;
 		}
 	}
-	std::istringstream iss(arguments->at(0));
+	std::istringstream iss(arguments.at(0));
 	iss >> name;
-	arguments->erase(arguments->begin());
+	arguments.erase(arguments.begin());
 
 	rOfO = readPosition(arguments);
 	aAOf = readOrientation(arguments);
@@ -42,22 +41,20 @@ void MbD::MBDynReference::parseMBDyn(std::string line)
 	readOmega(arguments);
 }
 
-void MbD::MBDynReference::readVelocity(std::shared_ptr<std::vector<std::string>>& args)
+void MbD::MBDynReference::readVelocity(std::vector<std::string>& args)
 {
-	auto parser = CREATE<SymbolicParser>::With();
-	parser->variables = mbdynVariables()->variables;
+	auto parser = std::make_shared<SymbolicParser>();
+	parser->variables = mbdynVariables();
 	vOfO = std::make_shared<FullColumn<double>>(3);
-	auto str = args->at(0);
+	auto& str = args.at(0);
 	if (str.find("null") != std::string::npos) {
-		args->erase(args->begin());
+		args.erase(args.begin());
 		return;
 	}
 	else {
 		for (int i = 0; i < 3; i++)
 		{
-			str = args->at(0);
-			args->erase(args->begin());
-			auto userFunc = CREATE<BasicUserFunction>::With(str, 1.0);
+			auto userFunc = std::make_shared<BasicUserFunction>(popOffTop(args), 1.0);
 			parser->parseUserFunction(userFunc);
 			auto sym = parser->stack->top();
 			vOfO->at(i) = sym->getValue();
@@ -65,22 +62,20 @@ void MbD::MBDynReference::readVelocity(std::shared_ptr<std::vector<std::string>>
 	}
 }
 
-void MbD::MBDynReference::readOmega(std::shared_ptr<std::vector<std::string>>& args)
+void MbD::MBDynReference::readOmega(std::vector<std::string>& args)
 {
-	auto parser = CREATE<SymbolicParser>::With();
-	parser->variables = mbdynVariables()->variables;
+	auto parser = std::make_shared<SymbolicParser>();
+	parser->variables = mbdynVariables();
 	omeOfO = std::make_shared<FullColumn<double>>(3);
-	auto str = args->at(0);
+	auto& str = args.at(0);
 	if (str.find("null") != std::string::npos) {
-		args->erase(args->begin());
+		args.erase(args.begin());
 		return;
 	}
 	else {
 		for (int i = 0; i < 3; i++)
 		{
-			str = args->at(0);
-			args->erase(args->begin());
-			auto userFunc = CREATE<BasicUserFunction>::With(str, 1.0);
+			auto userFunc = std::make_shared<BasicUserFunction>(popOffTop(args), 1.0);
 			parser->parseUserFunction(userFunc);
 			auto sym = parser->stack->top();
 			omeOfO->at(i) = sym->getValue();

OndselSolver/MBDynReference.h

@@ -16,8 +16,8 @@ namespace MbD {
 	public:
 		void initialize() override;
 		void parseMBDyn(std::string line);
-		void readVelocity(std::shared_ptr<std::vector<std::string>>& args);
-		void readOmega(std::shared_ptr<std::vector<std::string>>& args);
+		void readVelocity(std::vector<std::string>& args);
+		void readOmega(std::vector<std::string>& args);
 
 		std::string refString, name;
 		FColDsptr rOfO, vOfO, omeOfO;

OndselSolver/MBDynReferences.cpp

@@ -1,29 +0,0 @@
-#include "MBDynReferences.h"
-#include "MBDynReference.h"
-
-using namespace MbD;
-
-void MbD::MBDynReferences::initialize()
-{
-}
-
-void MbD::MBDynReferences::parseMBDyn(std::vector<std::string>& lines)
-{
-	references = std::make_shared<std::map<std::string, std::shared_ptr<MBDynReference>>>();
-	std::string str, refName;
-	double doubleValue;
-	std::vector<std::string> tokens{"reference:"};
-	while (true) {
-		auto it = findLineWith(lines, tokens);
-		if (it != lines.end()) {
-			auto reference = std::make_shared<MBDynReference>();
-			reference->owner = this;
-			reference->parseMBDyn(*it);
-			references->insert(std::make_pair(reference->name, reference));
-			lines.erase(it);
-		}
-		else {
-			break;
-		}
-	}
-}

OndselSolver/MBDynReferences.h

@@ -1,24 +0,0 @@
-/***************************************************************************
- *   Copyright (c) 2023 Ondsel, Inc.                                       *
- *                                                                         *
- *   This file is part of OndselSolver.                                    *
- *                                                                         *
- *   See LICENSE file for details about copyright.                         *
- ***************************************************************************/
-
-#pragma once
-#include "MBDynItem.h"
-
-namespace MbD {
-    class MBDynReference;
-
-    class MBDynReferences : public MBDynItem
-    {
-    public:
-        void initialize() override;
-        void parseMBDyn(std::vector<std::string>& lines) override;
-
-        std::shared_ptr<std::map<std::string, std::shared_ptr<MBDynReference>>> references;
-
-    };
-}

OndselSolver/MBDynStructural.cpp

@@ -1,16 +1,20 @@
 #include "MBDynStructural.h"
-#include "MBDynVariables.h"
 #include "SymbolicParser.h"
 #include "BasicUserFunction.h"
 #include "EulerAngles.h"
 #include "Constant.h"
-#include "MBDynReferences.h"
 #include "MBDynReference.h"
+#include "ASMTPart.h"
+#include "ASMTAssembly.h"
 
 using namespace MbD;
 
-void MbD::MBDynStructural::initialize()
+MbD::MBDynStructural::MBDynStructural()
 {
+	rOfO = std::make_shared<FullColumn<double>>(3);
+	aAOf = FullMatrix<double>::identitysptr(3);
+	vOfO = std::make_shared<FullColumn<double>>(3);
+	omeOfO = std::make_shared<FullColumn<double>>(3);
 }
 
 void MbD::MBDynStructural::parseMBDyn(std::string line)
@@ -20,27 +24,27 @@ void MbD::MBDynStructural::parseMBDyn(std::string line)
 	auto pos = line.find(":");
 	auto front = line.substr(previousPos, pos - previousPos);
 	assert(front.find("structural") != std::string::npos);
-	auto arguments = std::make_shared<std::vector<std::string>>();
+	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);
+			arguments.push_back(argument);
 		}
 		else {
 			argument = line.substr(previousPos + 1);
-			arguments->push_back(argument);
+			arguments.push_back(argument);
 			break;
 		}
 	}
-	std::istringstream iss(arguments->at(0));
+	std::istringstream iss(arguments.at(0));
 	iss >> name;
-	arguments->erase(arguments->begin());
-	iss = std::istringstream(arguments->at(0));
+	arguments.erase(arguments.begin());
+	iss = std::istringstream(arguments.at(0));
 	iss >> type;
-	arguments->erase(arguments->begin());
+	arguments.erase(arguments.begin());
 
 	rOfO = readPosition(arguments);
 	aAOf = readOrientation(arguments);
@@ -48,22 +52,20 @@ void MbD::MBDynStructural::parseMBDyn(std::string line)
 	readOmega(arguments);
 }
 
-void MbD::MBDynStructural::readVelocity(std::shared_ptr<std::vector<std::string>>& args)
+void MbD::MBDynStructural::readVelocity(std::vector<std::string>& args)
 {
-	auto parser = CREATE<SymbolicParser>::With();
-	parser->variables = mbdynVariables()->variables;
+	auto parser = std::make_shared<SymbolicParser>();
+	parser->variables = mbdynVariables();
 	vOfO = std::make_shared<FullColumn<double>>(3);
-	auto str = args->at(0);
+	auto& str = args.at(0);
 	if (str.find("null") != std::string::npos) {
-		args->erase(args->begin());
+		args.erase(args.begin());
 		return;
 	}
 	else {
 		for (int i = 0; i < 3; i++)
 		{
-			str = args->at(0);
-			args->erase(args->begin());
-			auto userFunc = CREATE<BasicUserFunction>::With(str, 1.0);
+			auto userFunc = std::make_shared<BasicUserFunction>(popOffTop(args), 1.0);
 			parser->parseUserFunction(userFunc);
 			auto sym = parser->stack->top();
 			vOfO->at(i) = sym->getValue();
@@ -71,25 +73,35 @@ void MbD::MBDynStructural::readVelocity(std::shared_ptr<std::vector<std::string>
 	}
 }
 
-void MbD::MBDynStructural::readOmega(std::shared_ptr<std::vector<std::string>>& args)
+void MbD::MBDynStructural::readOmega(std::vector<std::string>& args)
 {
-	auto parser = CREATE<SymbolicParser>::With();
-	parser->variables = mbdynVariables()->variables;
+	auto parser = std::make_shared<SymbolicParser>();
+	parser->variables = mbdynVariables();
 	omeOfO = std::make_shared<FullColumn<double>>(3);
-	auto str = args->at(0);
+	auto& str = args.at(0);
 	if (str.find("null") != std::string::npos) {
-		args->erase(args->begin());
+		args.erase(args.begin());
 		return;
 	}
 	else {
 		for (int i = 0; i < 3; i++)
 		{
-			str = args->at(0);
-			args->erase(args->begin());
-			auto userFunc = CREATE<BasicUserFunction>::With(str, 1.0);
+			auto userFunc = std::make_shared<BasicUserFunction>(popOffTop(args), 1.0);
 			parser->parseUserFunction(userFunc);
 			auto sym = parser->stack->top();
 			omeOfO->at(i) = sym->getValue();
 		}
 	}
 }
+
+void MbD::MBDynStructural::createASMT()
+{
+	auto asmtPart = std::make_shared<ASMTPart>();
+	asmtItem = asmtPart;
+	asmtPart->setName(name);
+	asmtPart->setPosition3D(rOfO);
+	asmtPart->setRotationMatrix(aAOf);
+	asmtPart->setVelocity3D(vOfO);
+	asmtPart->setOmega3D(omeOfO);
+	asmtAssembly()->addPart(asmtPart);
+}

OndselSolver/MBDynStructural.h

@@ -13,12 +13,13 @@ namespace MbD {
     class MBDynStructural : public MBDynNode
     {
     public:
-        void initialize() override;
+        MBDynStructural();
         void parseMBDyn(std::string line);
-        void readVelocity(std::shared_ptr<std::vector<std::string>>& args);
-        void readOmega(std::shared_ptr<std::vector<std::string>>& args);
+        void readVelocity(std::vector<std::string>& args);
+        void readOmega(std::vector<std::string>& args);
+        void createASMT() override;
 
-        std::string strucString, name, type;
+        std::string strucString, type;
         FColDsptr rOfO, vOfO, omeOfO;
         FMatDsptr aAOf;
     };

OndselSolver/MBDynSystem.cpp

@@ -10,11 +10,16 @@
 #include "MBDynInitialValue.h"
 #include "MBDynData.h"
 #include "MBDynControlData.h"
-#include "MBDynLabels.h"
-#include "MBDynVariables.h"
-#include "MBDynReferences.h"
-#include "MBDynNodes.h"
-#include "MBDynElements.h"
+#include "ASMTAssembly.h"
+#include "ASMTConstantGravity.h"
+#include "ASMTTime.h"
+#include "MBDynBody.h"
+#include "MBDynStructural.h"
+#include "SymbolicParser.h"
+#include "BasicUserFunction.h"
+#include "MBDynJoint.h"
+#include "MBDynReference.h"
+#include <iomanip>
 
 using namespace MbD;
 
@@ -48,19 +53,95 @@ void MbD::MBDynSystem::parseMBDyn(std::vector<std::string>& lines)
 
 }
 
-std::shared_ptr<MBDynVariables> MbD::MBDynSystem::mbdynVariables()
+std::shared_ptr<std::vector<std::shared_ptr<MBDynNode>>> MbD::MBDynSystem::mbdynNodes()
+{
+	return nodes;
+}
+
+std::shared_ptr<std::map<std::string, Symsptr>> MbD::MBDynSystem::mbdynVariables()
 {
 	return variables;
 }
 
-std::shared_ptr<MBDynReferences> MbD::MBDynSystem::mbdynReferences()
+std::shared_ptr<std::map<std::string, std::shared_ptr<MBDynReference>>> MbD::MBDynSystem::mbdynReferences()
 {
 	return references;
 }
 
+void MbD::MBDynSystem::createASMT()
+{
+	auto asmtAsm = std::make_shared<ASMTAssembly>();
+	asmtAsm->mbdynItem = this;
+	asmtItem = asmtAsm;
+	asmtItem->setName("Assembly");
+	initialValue->createASMT();
+	for (auto& node : *nodes) node->createASMT();
+	for (auto& element : *elements) element->createASMT();
+}
+
+std::shared_ptr<MBDynNode> MbD::MBDynSystem::nodeAt(std::string nodeName)
+{
+	for (auto& node : *nodes) {
+		if (node->name == nodeName) return node;
+	}
+	return nullptr;
+}
+
+int MbD::MBDynSystem::nodeidAt(std::string nodeName)
+{
+	return labels->at(nodeName);
+}
+
+std::shared_ptr<MBDynBody> MbD::MBDynSystem::bodyWithNode(std::string nodeName)
+{
+	for (auto& element : *elements) {
+		std::string str = typeid(element).name();
+		if (str.find("MBDynBody") != std::string::npos) {
+			auto body = std::static_pointer_cast<MBDynBody>(element);
+			if (body->nodeName == nodeName) return body;
+		}
+	}
+	return nullptr;
+}
+
+std::shared_ptr<ASMTAssembly> MbD::MBDynSystem::asmtAssembly()
+{
+	return std::static_pointer_cast<ASMTAssembly>(asmtItem);
+}
+
+std::vector<std::string> MbD::MBDynSystem::nodeNames()
+{
+	auto nodeNames = std::vector<std::string>();
+	for (auto& node : *nodes) {
+		nodeNames.push_back(node->name);
+	}
+	return nodeNames;
+}
+
 void MbD::MBDynSystem::runKINEMATIC()
 {
-	assert(false);
+	createASMT();
+	std::static_pointer_cast<ASMTAssembly>(asmtItem)->runKINEMATIC();
+	outputFiles();
+}
+
+void MbD::MBDynSystem::outputFiles()
+{
+	auto movFile = filename.substr(0, filename.find_last_of('.')) + ".mov";
+	auto asmtAsm = asmtAssembly();
+	auto asmtTimes = asmtAsm->times;
+	auto asmtParts = asmtAsm->parts;
+	auto asmtJoints = asmtAsm->joints;
+	auto asmtMotions = asmtAsm->motions;
+	std::ofstream os(movFile);
+	os << std::setprecision(std::numeric_limits<double>::digits10 + 1);
+	for (int i = 1; i < asmtTimes->size(); i++)
+	{
+		for (auto& node : *nodes) {
+			node->outputLine(i, os);
+		}
+	}
+	os.close();
 }
 
 void MbD::MBDynSystem::setFilename(std::string str)
@@ -70,87 +151,75 @@ void MbD::MBDynSystem::setFilename(std::string str)
 
 void MbD::MBDynSystem::readDataBlock(std::vector<std::string>& lines)
 {
-	std::vector<std::string> tokens{"begin:", "data;"};
+	std::vector<std::string> tokens{ "begin:", "data" };
 	auto beginit = findLineWith(lines, tokens);
-	std::vector<std::string> tokens1{"end:", "data;"};
+	std::vector<std::string> tokens1{ "end:", "data" };
 	auto endit = findLineWith(lines, tokens1);
 	std::vector<std::string> blocklines = { beginit, endit + 1 };
-	dataBlk = std::make_shared<MBDynData>();
-	dataBlk->owner = this;
-	dataBlk->parseMBDyn(blocklines);
+	parseMBDynData(blocklines);
 	lines.erase(beginit, endit + 1);
 }
 
 void MbD::MBDynSystem::readInitialValueBlock(std::vector<std::string>& lines)
 {
-	std::vector<std::string> tokens{"begin:", "initial", "value"};
+	std::vector<std::string> tokens{ "begin:", "initial", "value" };
 	auto beginit = findLineWith(lines, tokens);
-	std::vector<std::string> tokens1{"end:", "initial", "value"};
+	std::vector<std::string> tokens1{ "end:", "initial", "value" };
 	auto endit = findLineWith(lines, tokens1);
 	std::vector<std::string> blocklines = { beginit, endit + 1 };
-	initialValueBlk = std::make_shared<MBDynInitialValue>();
-	initialValueBlk->owner = this;
-	initialValueBlk->parseMBDyn(blocklines);
+	initialValue = std::make_shared<MBDynInitialValue>();
+	initialValue->owner = this;
+	initialValue->parseMBDyn(blocklines);
 	lines.erase(beginit, endit + 1);
 }
 
 void MbD::MBDynSystem::readControlDataBlock(std::vector<std::string>& lines)
 {
-	std::vector<std::string> tokens{"begin:", "control", "data"};
+	std::vector<std::string> tokens{ "begin:", "control", "data" };
 	auto beginit = findLineWith(lines, tokens);
-	std::vector<std::string> tokens1{"end:", "control", "data"};
+	std::vector<std::string> tokens1{ "end:", "control", "data" };
 	auto endit = findLineWith(lines, tokens1);
 	std::vector<std::string> blocklines = { beginit, endit + 1 };
-	controlDataBlk = std::make_shared<MBDynControlData>();
-	controlDataBlk->owner = this;
-	controlDataBlk->parseMBDyn(blocklines);
+	controlData = std::make_shared<MBDynControlData>();
+	controlData->owner = this;
+	controlData->parseMBDyn(blocklines);
 	lines.erase(beginit, endit + 1);
 }
 
 void MbD::MBDynSystem::readLabels(std::vector<std::string>& lines)
 {
-	labels = std::make_shared<MBDynLabels>();
-	labels->owner = this;
-	labels->parseMBDyn(lines);
+	parseMBDynLabels(lines);
 }
 
 void MbD::MBDynSystem::readVariables(std::vector<std::string>& lines)
 {
-	variables = std::make_shared<MBDynVariables>();
-	variables->owner = this;
-	variables->parseMBDyn(lines);
+	parseMBDynVariables(lines);
 }
 
 void MbD::MBDynSystem::readReferences(std::vector<std::string>& lines)
 {
-	references = std::make_shared<MBDynReferences>();
-	references->owner = this;
-	references->parseMBDyn(lines);
+	parseMBDynReferences(lines);
 }
 
 void MbD::MBDynSystem::readNodesBlock(std::vector<std::string>& lines)
 {
-	std::vector<std::string> tokens{ "begin:", "nodes;" };
+	std::vector<std::string> tokens{ "begin:", "nodes" };
 	auto beginit = findLineWith(lines, tokens);
-	std::vector<std::string> tokens1{ "end:", "nodes;" };
+	std::vector<std::string> tokens1{ "end:", "nodes" };
 	auto endit = findLineWith(lines, tokens1);
 	std::vector<std::string> blocklines = { beginit, endit + 1 };
-	nodesBlk = std::make_shared<MBDynNodes>();
-	nodesBlk->owner = this;
-	nodesBlk->parseMBDyn(blocklines);
+	parseMBDynNodes(blocklines);
 	lines.erase(beginit, endit + 1);
 }
 
 void MbD::MBDynSystem::readElementsBlock(std::vector<std::string>& lines)
 {
-	std::vector<std::string> tokens{ "begin:", "elements;" };
+	std::vector<std::string> tokens{ "begin:", "elements" };
 	auto beginit = findLineWith(lines, tokens);
-	std::vector<std::string> tokens1{ "end:", "elements;" };
+	std::vector<std::string> tokens1{ "end:", "elements" };
 	auto endit = findLineWith(lines, tokens1);
 	std::vector<std::string> blocklines = { beginit, endit + 1 };
-	elementsBlk = std::make_shared<MBDynElements>();
-	elementsBlk->owner = this;
-	elementsBlk->parseMBDyn(blocklines);
+	parseMBDynElements(blocklines);
 	lines.erase(beginit, endit + 1);
 }
 
@@ -181,7 +250,7 @@ std::vector<std::string> MbD::MBDynSystem::collectStatements(std::vector<std::st
 			lines.erase(lines.begin());
 			auto i = line.find(';');
 			if (i != std::string::npos) {
-				ss << line.substr(0, i + 1);
+				ss << line.substr(0, i);
 				if (line.size() > i + 1) {
 					auto remainder = line.substr(i + 1);
 					auto it = std::find_if(remainder.begin(), remainder.end(), [](unsigned char ch) { return !std::isspace(ch); });
@@ -201,3 +270,134 @@ std::vector<std::string> MbD::MBDynSystem::collectStatements(std::vector<std::st
 void MbD::MBDynSystem::initialize()
 {
 }
+
+void MbD::MBDynSystem::parseMBDynData(std::vector<std::string>& lines)
+{
+	assert(lines.size() == 3);
+	std::vector<std::string> tokens{ "problem:", "initial", "value" };
+	auto problemit = findLineWith(lines, tokens);
+	assert(problemit != lines.end());
+	data = *problemit;
+}
+
+void MbD::MBDynSystem::parseMBDynNodes(std::vector<std::string>& lines)
+{
+	nodes = std::make_shared<std::vector<std::shared_ptr<MBDynNode>>>();
+	std::vector<std::string> tokens{ "structural:" };
+	while (true) {
+		auto it = findLineWith(lines, tokens);
+		if (it != lines.end()) {
+			auto structural = std::make_shared<MBDynStructural>();
+			structural->owner = this;
+			structural->parseMBDyn(*it);
+			nodes->push_back(structural);
+			lines.erase(it);
+		}
+		else {
+			break;
+		}
+	}
+}
+
+void MbD::MBDynSystem::parseMBDynElements(std::vector<std::string>& lines)
+{
+	elements = std::make_shared<std::vector<std::shared_ptr<MBDynElement>>>();
+	std::vector<std::string> bodyToken{ "body:" };
+	std::vector<std::string> jointToken{ "joint:" };
+	std::vector<std::string>::iterator it;
+	while (true) {
+		it = findLineWith(lines, bodyToken);
+		if (it != lines.end()) {
+			auto body = std::make_shared<MBDynBody>();
+			body->owner = this;
+			body->parseMBDyn(*it);
+			elements->push_back(body);
+			lines.erase(it);
+			continue;
+		}
+		it = findLineWith(lines, jointToken);
+		if (it != lines.end()) {
+			auto joint = std::make_shared<MBDynJoint>();
+			joint->owner = this;
+			joint->parseMBDyn(*it);
+			elements->push_back(joint);
+			lines.erase(it);
+			continue;
+		}
+		break;
+	}
+}
+
+void MbD::MBDynSystem::parseMBDynVariables(std::vector<std::string>& lines)
+{
+	variables = std::make_shared<std::map<std::string, Symsptr>>();
+	std::string str, variable;
+	double doubleValue;
+	std::vector<std::string> tokens{ "set:", "real" };
+	while (true) {
+		auto it = findLineWith(lines, tokens);
+		if (it != lines.end()) {
+			std::istringstream iss(*it);
+			iss >> str;
+			iss >> str;
+			iss >> variable;
+			iss >> str;
+			iss >> str;
+			auto parser = std::make_shared<SymbolicParser>();
+			parser->variables = variables;
+			auto userFunc = std::make_shared<BasicUserFunction>(str, 1.0);
+			parser->parseUserFunction(userFunc);
+			auto sym = parser->stack->top();
+			variables->insert(std::make_pair(variable, sym));
+			lines.erase(it);
+		}
+		else {
+			break;
+		}
+	}
+}
+
+void MbD::MBDynSystem::parseMBDynLabels(std::vector<std::string>& lines)
+{
+	labels = std::make_shared<std::map<std::string, int>>();
+	std::string str, label;
+	int intValue;
+	std::vector<std::string> tokens{ "set:", "integer" };
+	while (true) {
+		auto it = findLineWith(lines, tokens);
+		if (it != lines.end()) {
+			std::istringstream iss(*it);
+			iss >> str;
+			iss >> str;
+			iss >> label;
+			iss >> str;
+			iss >> intValue;
+			labels->insert(std::make_pair(label, intValue));
+			lines.erase(it);
+		}
+		else {
+			break;
+		}
+	}
+}
+
+void MbD::MBDynSystem::parseMBDynReferences(std::vector<std::string>& lines)
+{
+	references = std::make_shared<std::map<std::string, std::shared_ptr<MBDynReference>>>();
+	std::string str, refName;
+	double doubleValue;
+	std::vector<std::string> tokens{ "reference:" };
+	while (true) {
+		auto it = findLineWith(lines, tokens);
+		if (it != lines.end()) {
+			auto reference = std::make_shared<MBDynReference>();
+			reference->owner = this;
+			reference->parseMBDyn(*it);
+			references->insert(std::make_pair(reference->name, reference));
+			lines.erase(it);
+		}
+		else {
+			break;
+		}
+	}
+}

OndselSolver/MBDynSystem.h

@@ -16,11 +16,11 @@ namespace MbD {
     class MBDynData;
     class MBDynInitialValue;
     class MBDynControlData;
-    class MBDynNodes;
-    class MBDynElements;
-    class MBDynVariables;
-    class MBDynLabels;
-    class MBDynReferences;
+    class MBDynNode;
+    class MBDynElement;
+    class MBDynVariable;
+    class MBDynLabel;
+    class MBDynReference;
 
     class MBDynSystem : public MBDynItem
     {
@@ -30,10 +30,24 @@ namespace MbD {
         static std::vector<std::string> collectStatements(std::vector<std::string>& lines);
         void initialize() override;
         void parseMBDyn(std::vector<std::string>& lines) override;
-        std::shared_ptr<MBDynVariables> mbdynVariables() override;
-        std::shared_ptr<MBDynReferences> mbdynReferences() override;
+        void parseMBDynData(std::vector<std::string>& lines);
+        void parseMBDynNodes(std::vector<std::string>& lines);
+        void parseMBDynElements(std::vector<std::string>& lines);
+        void parseMBDynVariables(std::vector<std::string>& lines);
+        void parseMBDynLabels(std::vector<std::string>& lines);
+        void parseMBDynReferences(std::vector<std::string>& lines);
+        std::shared_ptr<std::vector<std::shared_ptr<MBDynNode>>> mbdynNodes() override;
+        std::shared_ptr<std::map<std::string, Symsptr>> mbdynVariables() override;
+        std::shared_ptr<std::map<std::string, std::shared_ptr<MBDynReference>>> mbdynReferences() override;
+        void createASMT() override;
+        std::shared_ptr<MBDynNode> nodeAt(std::string nodeName) override;
+        int nodeidAt(std::string nodeName) override;
+        std::shared_ptr<MBDynBody> bodyWithNode(std::string nodeName) override;
+        std::shared_ptr<ASMTAssembly> asmtAssembly() override;
+        std::vector<std::string> nodeNames() override;
 
         void runKINEMATIC();
+        void outputFiles();
         void setFilename(std::string filename);
         void readDataBlock(std::vector<std::string>& lines);
         void readInitialValueBlock(std::vector<std::string>& lines);
@@ -45,14 +59,14 @@ namespace MbD {
         void readElementsBlock(std::vector<std::string>& lines);
 
         std::string filename = "";
-        std::shared_ptr<MBDynData> dataBlk;
-        std::shared_ptr<MBDynInitialValue> initialValueBlk;
-        std::shared_ptr<MBDynControlData> controlDataBlk;
-        std::shared_ptr<MBDynNodes> nodesBlk;
-        std::shared_ptr<MBDynElements> elementsBlk;
-        std::shared_ptr<MBDynVariables> variables;
-        std::shared_ptr<MBDynLabels> labels;
-        std::shared_ptr<MBDynReferences> references;
+        std::string data;
+        std::shared_ptr<MBDynInitialValue> initialValue;
+        std::shared_ptr<MBDynControlData> controlData;
+        std::shared_ptr<std::vector<std::shared_ptr<MBDynNode>>> nodes;
+        std::shared_ptr<std::vector<std::shared_ptr<MBDynElement>>> elements;
+        std::shared_ptr<std::map<std::string, Symsptr>> variables;
+        std::shared_ptr<std::map<std::string, int>> labels;
+        std::shared_ptr<std::map<std::string, std::shared_ptr<MBDynReference>>> references;
 
     };
 }

OndselSolver/MBDynVariables.cpp

@@ -1,38 +0,0 @@
-#include "MBDynVariables.h"
-#include "SymbolicParser.h"
-#include "BasicUserFunction.h"
-
-using namespace MbD;
-
-void MbD::MBDynVariables::initialize()
-{
-}
-
-void MbD::MBDynVariables::parseMBDyn(std::vector<std::string>& lines)
-{
-	variables = std::make_shared<std::map<std::string, Symsptr>>();
-	std::string str, variable;
-	double doubleValue;
-	std::vector<std::string> tokens{"set:", "real"};
-	while (true) {
-		auto it = findLineWith(lines, tokens);
-		if (it != lines.end()) {
-			std::istringstream iss(*it);
-			iss >> str;
-			iss >> str;
-			iss >> variable;
-			iss >> str;
-			iss >> str;
-			auto parser = CREATE<SymbolicParser>::With();
-			parser->variables = variables;
-			auto userFunc = CREATE<BasicUserFunction>::With(str, 1.0);
-			parser->parseUserFunction(userFunc);
-			auto sym = parser->stack->top();
-			variables->insert(std::make_pair(variable, sym));
-			lines.erase(it);
-		}
-		else {
-			break;
-		}
-	}
-}

OndselSolver/MarkerFrame.h

@@ -72,9 +72,9 @@ namespace MbD {
 
 		PartFrame* partFrame; //Use raw pointer when pointing backwards.
 		FColDsptr rpmp = std::make_shared<FullColumn<double>>(3);
-		FMatDsptr aApm = std::make_shared<FullMatrix<double>>(3, 3);
+		FMatDsptr aApm = FullMatrix<double>::identitysptr(3);
 		FColDsptr rOmO = std::make_shared<FullColumn<double>>(3);
-		FMatDsptr aAOm = std::make_shared<FullMatrix<double>>(3, 3);
+		FMatDsptr aAOm = FullMatrix<double>::identitysptr(3);
 		FMatDsptr prOmOpE;
 		FColFMatDsptr pAOmpE;
 		FMatFColDsptr pprOmOpEpE;

OndselSolver/MatrixSolver.cpp

@@ -64,7 +64,7 @@ void MatrixSolver::findScalingsForRowRange(int begin, int end)
 	rowScalings = std::make_shared<FullColumn<double>>(m);
 	for (int i = begin; i < end; i++)
 	{
-		auto maxRowMagnitude = this->getmatrixArowimaxMagnitude(i);
+		double maxRowMagnitude = this->getmatrixArowimaxMagnitude(i);
 		if (maxRowMagnitude == 0.0) throwSingularMatrixError("");
 		rowScalings->at(i) = 1.0 / maxRowMagnitude;
 	}

OndselSolver/MomentOfInertiaSolver.cpp

@@ -0,0 +1,401 @@
+#include "MomentOfInertiaSolver.h"
+#include "EulerParameters.h"
+#include <iostream>
+#include <algorithm>
+
+using namespace MbD;
+
+void MbD::MomentOfInertiaSolver::example1()
+{
+	auto aJpp = std::make_shared<FullMatrix<double>>(ListListD{
+		{ 1, 0, 0 },
+		{ 0, 2, 0 },
+		{ 0, 0, 3 }
+		});
+
+	auto rpPp = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 1 });
+	auto rotAxis = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 1 });
+	auto aApP = std::make_shared<EulerParameters<double>>(rotAxis, M_PI*10/180)->aA;
+	auto solver = std::make_shared<MomentOfInertiaSolver>();
+	solver->setm(4.0);
+	solver->setJPP(aJpp);
+	solver->setrPoP(rpPp);
+	solver->setAPo(aApP);
+	auto rPcmP = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0 });
+	solver->setrPcmP(rPcmP);
+	solver->calc();
+	auto aJPP = solver->getJoo();
+	std::cout << *solver->getJoo() << std::endl;
+	std::cout << *solver->getJpp() << std::endl;
+	std::cout << *solver->getAPp() << std::endl;
+	solver->setm(4.0);
+	solver->setJPP(aJPP);
+	auto rPoP = aApP->transposeTimesFullColumn(rpPp->negated());
+	solver->setrPoP(rPoP);
+	auto aAPo = aApP->transpose();
+	solver->setAPo(aAPo);
+	solver->setrPcmP(rPoP);
+	solver->calc();
+	std::cout << *solver->getJoo() << std::endl;
+	std::cout << *solver->getJpp() << std::endl;
+	std::cout << *solver->getAPp() << std::endl;
+}
+
+void MbD::MomentOfInertiaSolver::doFullPivoting(int p)
+{
+	double max = 0.0;
+	auto pivotRow = p;
+	auto pivotCol = p;
+	for (int i = p; i < 3; i++)
+	{
+		auto rowi = aJcmPcopy->at(i);
+		for (int j = p; j < 3; j++)
+		{
+			auto aij = rowi->at(j);
+			if (aij != 0.0) {
+				auto mag = std::abs(aij);
+				if (mag > max) {
+					max = mag;
+					pivotRow = i;
+					pivotCol = j;
+				}
+			}
+		}
+
+	}
+	if (p != pivotRow) aJcmPcopy->swapElems(p, pivotRow);
+	if (p != pivotCol) {
+		for (auto& row : *aJcmPcopy) {
+			row->swapElems(p, pivotCol);
+		}
+		colOrder->swapElems(p, pivotCol);
+	}
+}
+
+void MbD::MomentOfInertiaSolver::forwardEliminateWithPivot(int p)
+{
+	auto rowp = aJcmPcopy->at(p);
+	auto app = rowp->at(p);
+	for (int i = p + 1; i < 3; i++)
+	{
+		auto rowi = aJcmPcopy->at(i);
+		auto aip = rowi->at(p);
+		if (aip != 0) {
+			rowi->atiput(p, 0.0);
+			auto factor = aip / app;
+			for (int j = p + 1; j < 3; j++)
+			{
+				rowi->atiminusNumber(j, factor * rowp->at(j));
+			}
+		}
+	}
+}
+
+void MbD::MomentOfInertiaSolver::backSubstituteIntoDU()
+{
+}
+
+void MbD::MomentOfInertiaSolver::postSolve()
+{
+}
+
+FColDsptr MbD::MomentOfInertiaSolver::basicSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal)
+{
+	return FColDsptr();
+}
+
+FColDsptr MbD::MomentOfInertiaSolver::basicSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal)
+{
+	return FColDsptr();
+}
+
+void MbD::MomentOfInertiaSolver::preSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal)
+{
+}
+
+void MbD::MomentOfInertiaSolver::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal)
+{
+}
+
+double MbD::MomentOfInertiaSolver::getmatrixArowimaxMagnitude(int i)
+{
+	return 0.0;
+}
+
+void MbD::MomentOfInertiaSolver::doPivoting(int p)
+{
+}
+
+void MbD::MomentOfInertiaSolver::setm(double mass)
+{
+	m = mass;
+}
+
+void MbD::MomentOfInertiaSolver::setJPP(FMatDsptr mat)
+{
+	aJPP = mat;
+}
+
+void MbD::MomentOfInertiaSolver::setrPoP(FColDsptr col)
+{
+	rPoP = col;
+}
+
+void MbD::MomentOfInertiaSolver::setAPo(FMatDsptr mat)
+{
+	aAPo = mat;
+}
+
+void MbD::MomentOfInertiaSolver::setrPcmP(FColDsptr col)
+{
+	rPcmP = col;
+}
+
+FMatDsptr MbD::MomentOfInertiaSolver::getJoo()
+{
+	return aJoo;
+}
+
+DiagMatDsptr MbD::MomentOfInertiaSolver::getJpp()
+{
+	return aJpp;
+}
+
+FMatDsptr MbD::MomentOfInertiaSolver::getAPp()
+{
+	return aAPp;
+}
+
+void MbD::MomentOfInertiaSolver::calc()
+{
+	calcJoo();
+	calcJpp();
+	calcAPp();
+}
+
+void MbD::MomentOfInertiaSolver::calcJoo()
+{
+	//"aJoo = aAPoT*[aJPP + mass*(rPoPTilde*rPoPTilde + rPoPTilde*rocmPTilde + rocmPTilde*rPoPTilde)]*aAPo"
+
+	if (!rPoP) {
+		rPoP = rPcmP;
+		aAPo = FullMatrix<double>::identitysptr(3);
+	}
+	auto rocmPtilde = FullMatrix<double>::tildeMatrix(rPcmP->minusFullColumn(rPoP));
+	auto rPoPtilde = FullMatrix<double>::tildeMatrix(rPoP);
+	auto term1 = aJPP;
+	auto term21 = rPoPtilde->timesFullMatrix(rPoPtilde);
+	auto term22 = rPoPtilde->timesFullMatrix(rocmPtilde);
+	auto term23 = term22->transpose();
+	auto term2 = term21->plusFullMatrix(term22)->plusFullMatrix(term23)->times(m);
+	aJoo = aAPo->transposeTimesFullMatrix(term1->plusFullMatrix(term2))->timesFullMatrix(aAPo);
+	aJoo->symLowerWithUpper();
+	aJoo->conditionSelfWithTol(aJoo->maxMagnitude() * 1.0e-6);
+}
+
+void MbD::MomentOfInertiaSolver::calcJpp()
+{
+	//"aJcmP = aJPP + mass*(rPcmPTilde*rPcmPTilde)"
+
+	auto rPcmPtilde = FullMatrix<double>::tildeMatrix(rPcmP);
+	aJcmP = aJPP->plusFullMatrix(rPcmPtilde->timesFullMatrix(rPcmPtilde)->times(m));
+	aJcmP->symLowerWithUpper();
+	aJcmP->conditionSelfWithTol(aJcmP->maxMagnitude() * 1.0e-6);
+	if (aJcmP->isDiagonal()) {
+		calcJppFromDiagJcmP();
+	}
+	else {
+		calcJppFromFullJcmP();
+	}
+}
+
+void MbD::MomentOfInertiaSolver::calcAPp()
+{
+	FColDsptr eigenvector1, eigenvector2, eigenvector0;
+	auto lam0 = aJpp->at(0);
+	auto lam1 = aJpp->at(1);
+	auto lam2 = aJpp->at(2);
+	if (lam0 == lam1) {
+		if (lam1 == lam2) {
+			aAPp = FullMatrix<double>::identitysptr(3);
+		}
+		else {
+			eigenvector1 = eigenvectorFor(lam1);
+			eigenvector2 = eigenvectorFor(lam2);
+			eigenvector1->normalizeSelf();
+			eigenvector2->normalizeSelf();
+			if (eigenvector1->at(1) < 0.0) eigenvector1->negateSelf();
+			if (eigenvector2->at(2) < 0.0) eigenvector2->negateSelf();
+			eigenvector0 = eigenvector1->cross(eigenvector2);
+		}
+	}
+	else {
+		eigenvector0 = eigenvectorFor(lam0);
+		eigenvector1 = eigenvectorFor(lam1);
+		eigenvector0->normalizeSelf();
+		eigenvector1->normalizeSelf();
+		if (eigenvector0->at(0) < 0.0) eigenvector0->negateSelf();
+		if (eigenvector1->at(1) < 0.0) eigenvector1->negateSelf();
+		eigenvector2 = eigenvector0->cross(eigenvector1);
+	}
+	aAPp = std::make_shared<FullMatrix<double>>(3, 3);
+	aAPp->atijputFullColumn(0, 0, eigenvector0);
+	aAPp->atijputFullColumn(0, 1, eigenvector1);
+	aAPp->atijputFullColumn(0, 2, eigenvector2);
+}
+
+FColDsptr MbD::MomentOfInertiaSolver::eigenvectorFor(double lam)
+{
+	//"[aJcmP] - lam[I]."
+
+	double e0, e1, e2;
+	aJcmPcopy = aJcmP->copy();
+	colOrder = std::make_shared<FullRow<int>>(3);
+	auto eigenvector = std::make_shared<FullColumn<double>>(3);
+	for (int i = 0; i < 3; i++)
+	{
+		colOrder->atiput(i, i);
+		aJcmPcopy->atijminusNumber(i, i, lam);
+	}
+	for (int p = 0; p < 3; p++)
+	{
+		doFullPivoting(p);
+		forwardEliminateWithPivot(p);
+	}
+
+	auto row0 = aJcmPcopy->at(0);
+	auto row1 = aJcmPcopy->at(1);
+	auto row2 = aJcmPcopy->at(2);
+	auto norm0 = row0->length();
+	auto aaaa = row2->length();
+	if ((row2->length() / norm0) > 1.0e-5) throw std::runtime_error("3rd row should be very small.");
+	if ((row1->length() / norm0) > 1.0e-5) {
+		e2 = 1.0;
+		e1 = e2 * -row1->at(2) / row1->at(1);
+		e0 = -(e1 * row0->at(1) + e2 * row0->at(2)) / row0->at(0);
+	}
+	else {
+		//"Repeated roots."
+		e2 = 1.0;
+		e1 = 0.0;
+		e0 = -(e1 * row0->at(1) + e2 * row0->at(2)) / row0->at(0);
+	}
+
+
+	auto dum = std::make_shared<FullColumn<double>>(ListD{ e0, e1, e2 });
+	for (int i = 0; i < 3; i++)
+	{
+		eigenvector->atiput(colOrder->at(i), dum->at(i));
+	}
+	return eigenvector;
+}
+
+void MbD::MomentOfInertiaSolver::calcJppFromDiagJcmP()
+{
+	//"Eigenvalues are orders from smallest to largest."
+
+	double average;
+	auto sortedJ = std::make_shared<DiagonalMatrix<double>>();
+	sortedJ->push_back(aJcmP->at(0)->at(0));
+	sortedJ->push_back(aJcmP->at(1)->at(1));
+	sortedJ->push_back(aJcmP->at(2)->at(2));
+	std::sort(sortedJ->begin(), sortedJ->end(), [](double a, double b) { return a < b; });
+	auto lam0 = sortedJ->at(0);
+	auto lam1 = sortedJ->at(1);
+	auto lam2 = sortedJ->at(2);
+	if (lam1 == 0 || std::abs((lam0 / lam1) - 1.0) < 1.0e-6) {
+		if (lam2 == 0 || std::abs((lam1 / lam2) - 1.0) < 1.0e-6) {
+			//"All are equal."
+			average = (lam0 + lam1 + lam2) / 3.0;
+			lam0 = average;
+			lam1 = average;
+			lam2 = average;
+		}
+		else {
+			//"lam0 = lam1"
+			average = (lam0 + lam1) / 2.0;
+			lam0 = average;
+			lam1 = average;
+		}
+	}
+	else {
+		if (std::abs(lam1 / lam2 - 1.0) < 1.0e-6) {
+			//"lam1 = lam2"
+			average = (lam1 + lam2) / 2.0;
+			lam1 = average;
+			lam2 = average;
+		}
+	}
+	aJpp = std::make_shared<DiagonalMatrix<double>>(ListD{ lam0, lam1, lam2 });
+}
+
+void MbD::MomentOfInertiaSolver::calcJppFromFullJcmP()
+{
+	//"Eigenvalues are orders from smallest to largest."
+	//"Rounding error can be significant."
+	//"e.g. (diag 100.0d 100.0d 1.0d) gives only 8 digit accuracy."
+
+	double average;
+	auto a00 = aJcmP->at(0)->at(0);
+	auto a01 = aJcmP->at(0)->at(1);
+	auto a02 = aJcmP->at(0)->at(2);
+	auto a11 = aJcmP->at(1)->at(1);
+	auto a12 = aJcmP->at(1)->at(2);
+	auto a22 = aJcmP->at(2)->at(2);
+	auto a = -(a00 + a11 + a22);
+	auto b = a00 * a11 + (a00 * a22) + (a11 * a22) - (a01 * a01) - (a02 * a02) - (a12 * a12);
+	auto c = a00 * a12 * a12 + (a11 * a02 * a02) + (a22 * a01 * a01) - (a00 * a11 * a22) - (2.0 * a01 * a02 * a12);
+	auto aDiv3 = a / 3.0;
+	auto p = (b / 3.0) - (aDiv3 * aDiv3);
+	auto q = (c + (2.0 * aDiv3 * aDiv3 * aDiv3) - (aDiv3 * b)) / 2.0;
+	auto phiDiv3 = modifiedArcCos(-q / std::sqrt(-p * p * p)) / 3.0;
+	auto twoSqrtMinusp = 2.0 * std::sqrt(-p);
+	auto piDiv3 = M_PI / 3.0;
+	auto sortedJ = std::make_shared<DiagonalMatrix<double>>();
+	sortedJ->push_back(twoSqrtMinusp * std::cos(phiDiv3));
+	sortedJ->push_back(twoSqrtMinusp * -std::cos(phiDiv3 + piDiv3));
+	sortedJ->push_back(twoSqrtMinusp * -std::cos(phiDiv3 - piDiv3));
+	std::sort(sortedJ->begin(), sortedJ->end(), [](double a, double b) { return a < b; });
+	auto lam0 = sortedJ->at(0) - aDiv3;
+	auto lam1 = sortedJ->at(1) - aDiv3;
+	auto lam2 = sortedJ->at(2) - aDiv3;
+	if (lam1 == 0 || std::abs((lam0 / lam1) - 1.0) < 1.0e-6) {
+		if (lam2 == 0 || std::abs((lam1 / lam2) - 1.0) < 1.0e-6) {
+			//"All are equal."
+			average = (lam0 + lam1 + lam2) / 3.0;
+			lam0 = average;
+			lam1 = average;
+			lam2 = average;
+		}
+		else {
+			//"lam0 = lam1"
+			average = (lam0 + lam1) / 2.0;
+			lam0 = average;
+			lam1 = average;
+		}
+	}
+	else {
+		if (std::abs((lam1 / lam2) - 1.0) < 1.0e-6) {
+			//"lam1 = lam2"
+			average = (lam1 + lam2) / 2.0;
+			lam1 = average;
+			lam2 = average;
+		}
+	}
+	aJpp = std::make_shared<DiagonalMatrix<double>>(ListD{ lam0, lam1, lam2 });
+}
+
+double MbD::MomentOfInertiaSolver::modifiedArcCos(double val)
+{
+	if (val > 1.0) {
+		return 0.0;
+	}
+	else {
+		if (val < -1.0) {
+			return M_PI;
+		}
+		else {
+			return std::acos(val);
+		}
+	}
+}

OndselSolver/MomentOfInertiaSolver.h

@@ -0,0 +1,57 @@
+/***************************************************************************
+ *   Copyright (c) 2023 Ondsel, Inc.                                       *
+ *                                                                         *
+ *   This file is part of OndselSolver.                                    *
+ *                                                                         *
+ *   See LICENSE file for details about copyright.                         *
+ ***************************************************************************/
+
+#pragma once
+
+#include "EigenDecomposition.h"
+
+namespace MbD {
+    class MomentOfInertiaSolver : public EigenDecomposition
+    {
+        //See document 9100moment.fodt
+        //m aJPP aJoo rPoP aAPo aJcmP aJcmPcopy rPcmP aJpp aAPp colOrder 
+        //aJoo == aJpp when rPoP == rPcmP and aAPo == aAPp
+    public:
+        static void example1();
+        void doFullPivoting(int p);
+        void forwardEliminateWithPivot(int p) override;
+        void backSubstituteIntoDU() override;
+        void postSolve() override;
+        FColDsptr basicSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal) override;
+        FColDsptr basicSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) override;
+        void preSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal) override;
+        void preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) override;
+        double getmatrixArowimaxMagnitude(int i) override;
+        void doPivoting(int p) override;
+
+        void setm(double mass);
+        void setJPP(FMatDsptr mat);
+        void setrPoP(FColDsptr col);
+        void setAPo(FMatDsptr mat);
+        void setrPcmP(FColDsptr col);
+        FMatDsptr getJoo();
+        DiagMatDsptr getJpp();
+        FMatDsptr getAPp();
+        void calc();
+        void calcJoo();
+        void calcJpp();
+        void calcAPp();
+        FColDsptr eigenvectorFor(double lam);
+        void calcJppFromDiagJcmP();
+        void calcJppFromFullJcmP();
+        double modifiedArcCos(double val);
+
+
+        double m;
+        FMatDsptr aJPP, aJoo, aAPo, aJcmP, aJcmPcopy, aAPp;
+        FColDsptr rPoP, rPcmP;
+        DiagMatDsptr aJpp;
+        std::shared_ptr<FullRow<int>> colOrder;
+
+    };
+}

OndselSolver/OndselSolver.cpp

@@ -5,12 +5,12 @@
  *                                                                         *
  *   See LICENSE file for details about copyright.                         *
  ***************************************************************************/
- 
-/*********************************************************************
- * @file  MbDCode.cpp
- *
- * @brief Program to assemble a piston crank system.
- *********************************************************************/
+
+ /*********************************************************************
+  * @file  MbDCode.cpp
+  *
+  * @brief Program to assemble a piston crank system.
+  *********************************************************************/
 
 #include <filesystem>
 #include "CADSystem.h"
@@ -18,16 +18,19 @@
 #include "GESpMatParPvPrecise.h"
 #include "ASMTAssembly.h"
 #include "MBDynSystem.h"
+#include "MomentOfInertiaSolver.h"
 
 using namespace MbD;
 void runSpMat();
 
 int main()
 {
-	//MBDynSystem::runFile("crank_slider.mbd");		//To be completed
+	MBDynSystem::runFile("MBDynCase.mbd");		//To be completed
+	MBDynSystem::runFile("crank_slider.mbd");		//To be completed
 	//ASMTAssembly::runSinglePendulumSuperSimplified();	//Mass is missing
+	//ASMTAssembly::runSinglePendulumSuperSimplified2();	//DOF has infinite acceleration due to zero mass and inertias
 	ASMTAssembly::runSinglePendulumSimplified();
-	ASMTAssembly::runSinglePendulum();	
+	ASMTAssembly::runSinglePendulum();
 	ASMTAssembly::runFile("piston.asmt");
 	ASMTAssembly::runFile("00backhoe.asmt");
 	//ASMTAssembly::runFile("circular.asmt");	//Needs checking
@@ -43,6 +46,7 @@ int main()
 	//cadSystem->runOndselPiston();		//For debugging
 	cadSystem->runPiston();
 	runSpMat();
+	MomentOfInertiaSolver::example1();
 }
 
 void runSpMat() {

OndselSolver/OndselSolver.vcxproj

@@ -159,12 +159,14 @@
     <ClCompile Include="ASMTFixedJoint.cpp" />
     <ClCompile Include="ASMTForceTorque.cpp" />
     <ClCompile Include="ASMTGeneralMotion.cpp" />
+    <ClCompile Include="ASMTPointInLineJoint.cpp" />
     <ClCompile Include="ASMTItem.cpp" />
     <ClCompile Include="ASMTItemIJ.cpp" />
     <ClCompile Include="ASMTJoint.cpp" />
     <ClCompile Include="ASMTKinematicIJ.cpp" />
     <ClCompile Include="ASMTMarker.cpp" />
     <ClCompile Include="ASMTMotion.cpp" />
+    <ClCompile Include="ASMTNoRotationJoint.cpp" />
     <ClCompile Include="ASMTPart.cpp" />
     <ClCompile Include="ASMTPointInPlaneJoint.cpp" />
     <ClCompile Include="ASMTPrincipalMassMarker.cpp" />
@@ -206,6 +208,7 @@
     <ClCompile Include="CylindricalJoint.cpp" />
     <ClCompile Include="CylSphJoint.cpp" />
     <ClCompile Include="DifferentiatedGeneralSpline.cpp" />
+    <ClCompile Include="EigenDecomposition.cpp" />
     <ClCompile Include="EndFrameqct2.cpp" />
     <ClCompile Include="EulerAngles.cpp" />
     <ClCompile Include="EulerAnglesDDot.cpp" />
@@ -224,18 +227,15 @@
     <ClCompile Include="MBDynControlData.cpp" />
     <ClCompile Include="MBDynData.cpp" />
     <ClCompile Include="MBDynElement.cpp" />
-    <ClCompile Include="MBDynElements.cpp" />
     <ClCompile Include="MBDynInitialValue.cpp" />
     <ClCompile Include="MBDynItem.cpp" />
     <ClCompile Include="MBDynJoint.cpp" />
-    <ClCompile Include="MBDynLabels.cpp" />
+    <ClCompile Include="MBDynMarker.cpp" />
     <ClCompile Include="MBDynNode.cpp" />
-    <ClCompile Include="MBDynNodes.cpp" />
     <ClCompile Include="MBDynReference.cpp" />
-    <ClCompile Include="MBDynReferences.cpp" />
     <ClCompile Include="MBDynStructural.cpp" />
     <ClCompile Include="MBDynSystem.cpp" />
-    <ClCompile Include="MBDynVariables.cpp" />
+    <ClCompile Include="MomentOfInertiaSolver.cpp" />
     <ClCompile Include="Negative.cpp" />
     <ClCompile Include="PosVelAccData.cpp" />
     <ClCompile Include="DiagonalMatrix.cpp" />
@@ -445,12 +445,14 @@
     <ClInclude Include="ASMTFixedJoint.h" />
     <ClInclude Include="ASMTForceTorque.h" />
     <ClInclude Include="ASMTGeneralMotion.h" />
+    <ClInclude Include="ASMTPointInLineJoint.h" />
     <ClInclude Include="ASMTItem.h" />
     <ClInclude Include="ASMTItemIJ.h" />
     <ClInclude Include="ASMTJoint.h" />
     <ClInclude Include="ASMTKinematicIJ.h" />
     <ClInclude Include="ASMTMarker.h" />
     <ClInclude Include="ASMTMotion.h" />
+    <ClInclude Include="ASMTNoRotationJoint.h" />
     <ClInclude Include="ASMTPart.h" />
     <ClInclude Include="ASMTPointInPlaneJoint.h" />
     <ClInclude Include="ASMTPrincipalMassMarker.h" />
@@ -492,6 +494,7 @@
     <ClInclude Include="CylindricalJoint.h" />
     <ClInclude Include="CylSphJoint.h" />
     <ClInclude Include="DifferentiatedGeneralSpline.h" />
+    <ClInclude Include="EigenDecomposition.h" />
     <ClInclude Include="EndFrameqct2.h" />
     <ClInclude Include="EulerAngles.h" />
     <ClInclude Include="EulerAnglesDDot.h" />
@@ -510,18 +513,15 @@
     <ClInclude Include="MBDynControlData.h" />
     <ClInclude Include="MBDynData.h" />
     <ClInclude Include="MBDynElement.h" />
-    <ClInclude Include="MBDynElements.h" />
     <ClInclude Include="MBDynInitialValue.h" />
     <ClInclude Include="MBDynItem.h" />
     <ClInclude Include="MBDynJoint.h" />
-    <ClInclude Include="MBDynLabels.h" />
+    <ClInclude Include="MBDynMarker.h" />
     <ClInclude Include="MBDynNode.h" />
-    <ClInclude Include="MBDynNodes.h" />
     <ClInclude Include="MBDynReference.h" />
-    <ClInclude Include="MBDynReferences.h" />
     <ClInclude Include="MBDynStructural.h" />
     <ClInclude Include="MBDynSystem.h" />
-    <ClInclude Include="MBDynVariables.h" />
+    <ClInclude Include="MomentOfInertiaSolver.h" />
     <ClInclude Include="Negative.h" />
     <ClInclude Include="PosVelAccData.h" />
     <ClInclude Include="DiagonalMatrix.h" />
@@ -715,6 +715,7 @@
     <None Include="circular.asmt" />
     <None Include="cirpendu.asmt" />
     <None Include="ClassDiagram.cd" />
+    <None Include="ClassDiagram1.cd" />
     <None Include="crank_slider.mbd" />
     <None Include="engine1.asmt" />
     <None Include="fourbar.asmt" />