Merge pull request #30 from Ondsel-Development/OutputASMT

Output asmt

OndselSolver/ASMTAnimationParameters.h

@@ -18,7 +18,7 @@ namespace MbD {
         void parseASMT(std::vector<std::string>& lines) override;
         void storeOnLevel(std::ofstream& os, int level) override;
 
-		int nframe = 1000000, icurrent = 0, istart = 0, iend = 1000000, framesPerSecond = 30;
+		int nframe = 1000000, icurrent = 1, istart = 1, iend = 1000000, framesPerSecond = 30;
         bool isForward = true;
 
 

OndselSolver/ASMTAssembly.cpp

@@ -940,7 +940,7 @@ void MbD::ASMTAssembly::createMbD(std::shared_ptr<System> mbdSys, std::shared_pt
 void MbD::ASMTAssembly::outputFile(std::string filename)
 {
 	std::ofstream os(filename);
-	os << std::setprecision(std::numeric_limits<double>::digits10 + 1);
+	os << std::setprecision(static_cast<std::streamsize>(std::numeric_limits<double>::digits10) + 1);
 	//	try {
 	os << "OndselSolver" << std::endl;
 	storeOnLevel(os, 0);
@@ -1144,7 +1144,7 @@ std::shared_ptr<ASMTPart> MbD::ASMTAssembly::partPartialNamed(std::string partia
 		auto fullName = prt->fullName("");
 		return fullName.find(partialName) != std::string::npos;
 		});
-	auto part = *it;
+	auto& part = *it;
 	return part;
 }
 
@@ -1212,6 +1212,7 @@ void MbD::ASMTAssembly::storeOnLevelGeneralConstraintSets(std::ofstream& os, int
 
 void MbD::ASMTAssembly::storeOnTimeSeries(std::ofstream& os)
 {
+	if (times->empty()) return;
 	os << "TimeSeries" << std::endl;
 	os << "Number\tInput\t";
 	for (int i = 1; i < times->size(); i++)

OndselSolver/ASMTAssembly.h

@@ -121,19 +121,19 @@ namespace MbD {
 		void storeOnLevelGeneralConstraintSets(std::ofstream& os, int level);
 		void storeOnTimeSeries(std::ofstream& os) override;
 
-		std::string notes;
+		std::string notes = "(Text string: '' runs: (Core.RunArray runs: #() values: #()))";
 		std::shared_ptr<std::vector<std::shared_ptr<ASMTPart>>> parts = std::make_shared<std::vector<std::shared_ptr<ASMTPart>>>();
 		std::shared_ptr<std::vector<std::shared_ptr<ASMTKinematicIJ>>> kinematicIJs = std::make_shared<std::vector<std::shared_ptr<ASMTKinematicIJ>>>();
 		std::shared_ptr<std::vector<std::shared_ptr<ASMTConstraintSet>>> constraintSets = std::make_shared<std::vector<std::shared_ptr<ASMTConstraintSet>>>();
 		std::shared_ptr<std::vector<std::shared_ptr<ASMTJoint>>> joints = std::make_shared<std::vector<std::shared_ptr<ASMTJoint>>>();
 		std::shared_ptr<std::vector<std::shared_ptr<ASMTMotion>>> motions = std::make_shared<std::vector<std::shared_ptr<ASMTMotion>>>();
 		std::shared_ptr<std::vector<std::shared_ptr<ASMTForceTorque>>> forcesTorques = std::make_shared<std::vector<std::shared_ptr<ASMTForceTorque>>>();
 		std::shared_ptr<ASMTConstantGravity> constantGravity = std::make_shared<ASMTConstantGravity>();
-		std::shared_ptr<ASMTSimulationParameters> simulationParameters;
-		std::shared_ptr<ASMTAnimationParameters> animationParameters;
-		std::shared_ptr<std::vector<double>> times;
+		std::shared_ptr<ASMTSimulationParameters> simulationParameters = std::make_shared<ASMTSimulationParameters>();
+		std::shared_ptr<ASMTAnimationParameters> animationParameters = std::make_shared<ASMTAnimationParameters>();
+		std::shared_ptr<std::vector<double>> times = std::make_shared<std::vector<double>>();
 		std::shared_ptr<ASMTTime> asmtTime = std::make_shared<ASMTTime>();
-		std::shared_ptr<Units> mbdUnits;
+		std::shared_ptr<Units> mbdUnits = std::make_shared<Units>();
 		MBDynSystem* mbdynItem = nullptr;
 	};
 }

OndselSolver/ASMTGeneralMotion.cpp

@@ -92,21 +92,21 @@ void MbD::ASMTGeneralMotion::createMbD(std::shared_ptr<System> mbdSys, std::shar
 	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 = Symbolic::times(geoX, sptrConstant(1.0 / mbdUnits->length));
 	geoX->createMbD(mbdSys, mbdUnits);
 	auto xBlk = geoX->simplified(geoX);
 
 	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 = Symbolic::times(geoY, sptrConstant(1.0 / mbdUnits->length));
 	geoY->createMbD(mbdSys, mbdUnits);
 	auto yBlk = geoY->simplified(geoY);
 
 	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));
+	geoZ = Symbolic::times(geoZ, sptrConstant(1.0 / mbdUnits->length));
 	geoZ->createMbD(mbdSys, mbdUnits);
 	auto zBlk = geoZ->simplified(geoZ);
 
@@ -117,21 +117,21 @@ void MbD::ASMTGeneralMotion::createMbD(std::shared_ptr<System> mbdSys, std::shar
 	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 = Symbolic::times(geoPhi, sptrConstant(1.0 / mbdUnits->angle));
 	geoPhi->createMbD(mbdSys, mbdUnits);
 	auto phiBlk = geoPhi->simplified(geoPhi);
 
 	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 = Symbolic::times(geoThe, sptrConstant(1.0 / mbdUnits->angle));
 	geoThe->createMbD(mbdSys, mbdUnits);
 	auto theBlk = geoThe->simplified(geoThe);
 
 	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));
+	geoPsi = Symbolic::times(geoPsi, sptrConstant(1.0 / mbdUnits->angle));
 	geoPsi->createMbD(mbdSys, mbdUnits);
 	auto psiBlk = geoPsi->simplified(geoPsi);
 

OndselSolver/ASMTPointInPlaneJoint.cpp

@@ -25,11 +25,14 @@ void MbD::ASMTPointInPlaneJoint::parseASMT(std::vector<std::string>& lines)
 
 void MbD::ASMTPointInPlaneJoint::readOffset(std::vector<std::string>& lines)
 {
-	assert(lines[0].find("offset") != std::string::npos);
-	lines.erase(lines.begin());
-	offset = readDouble(lines[0]);
-	lines.erase(lines.begin());
-
+	if (lines[0].find("offset") == std::string::npos) {
+		offset = 0.0;
+	}
+	else {
+		lines.erase(lines.begin());
+		offset = readDouble(lines[0]);
+		lines.erase(lines.begin());
+	}
 }
 
 void MbD::ASMTPointInPlaneJoint::createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits)

OndselSolver/ASMTPrincipalMassMarker.cpp

@@ -12,6 +12,11 @@
 
 using namespace MbD;
 
+MbD::ASMTPrincipalMassMarker::ASMTPrincipalMassMarker()
+{
+	name = "MassMarker";
+}
+
 void MbD::ASMTPrincipalMassMarker::parseASMT(std::vector<std::string>& lines)
 {
 	size_t pos = lines[0].find_first_not_of("\t");

OndselSolver/ASMTPrincipalMassMarker.h

@@ -5,29 +5,30 @@
  *                                                                         *
  *   See LICENSE file for details about copyright.                         *
  ***************************************************************************/
- 
+
 #pragma once
 
 #include "ASMTSpatialItem.h"
 
 namespace MbD {
-    class EXPORT ASMTPrincipalMassMarker : public ASMTSpatialItem
-    {
-        //
-    public:
-        void parseASMT(std::vector<std::string>& lines) override;
-        void setMass(double mass);
-        void setDensity(double density);
-        void setMomentOfInertias(DiagMatDsptr momentOfInertias);
+	class EXPORT ASMTPrincipalMassMarker : public ASMTSpatialItem
+	{
+		//
+	public:
+		ASMTPrincipalMassMarker();
+		void parseASMT(std::vector<std::string>& lines) override;
+		void setMass(double mass);
+		void setDensity(double density);
+		void setMomentOfInertias(DiagMatDsptr momentOfInertias);
 
-        // Overloads to simplify syntax.
-        void setMomentOfInertias(double a, double b, double c);
-        void storeOnLevel(std::ofstream& os, int level) override;
+		// Overloads to simplify syntax.
+		void setMomentOfInertias(double a, double b, double c);
+		void storeOnLevel(std::ofstream& os, int level) override;
 
-        double mass = 0.0;
-        double density = 0.0;
-        DiagMatDsptr momentOfInertias = std::make_shared<DiagonalMatrix>(ListD{ 0.,0.,0. });
+		double mass = 1.0;
+		double density = 10.0;
+		DiagMatDsptr momentOfInertias = std::make_shared<DiagonalMatrix>(ListD{ 1.0, 2.0, 3.0 });
 
-    };
+	};
 }
 

OndselSolver/ASMTRotationalMotion.cpp

@@ -68,9 +68,13 @@ void MbD::ASMTRotationalMotion::createMbD(std::shared_ptr<System> mbdSys, std::s
 	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));
+	std::cout << *geoPhi << std::endl;
+	geoPhi = Symbolic::times(geoPhi, sptrConstant(1.0 / mbdUnits->angle));
 	geoPhi->createMbD(mbdSys, mbdUnits);
-	std::static_pointer_cast<ZRotation>(mbdObject)->phiBlk = geoPhi->simplified(geoPhi);
+	std::cout << *geoPhi << std::endl;
+	auto simple = geoPhi->simplified(geoPhi);
+	std::cout << *simple << std::endl;
+	std::static_pointer_cast<ZRotation>(mbdObject)->phiBlk = simple;
 }
 
 std::shared_ptr<Joint> MbD::ASMTRotationalMotion::mbdClassNew()

OndselSolver/ASMTSpatialContainer.cpp

@@ -550,13 +550,25 @@ void MbD::ASMTSpatialContainer::setOmega3D(double a, double b, double c)
 void MbD::ASMTSpatialContainer::storeOnLevelVelocity(std::ofstream& os, int level)
 {
 	storeOnLevelString(os, level, "Velocity3D");
-	storeOnLevelArray(os, level + 1, *velocity3D);
+	if (vxs == nullptr || vxs->empty()) {
+		storeOnLevelArray(os, level + 1, *velocity3D);
+	}
+	else {
+		auto array = getVelocity3D(0);
+		storeOnLevelArray(os, level + 1, *array);
+	}
 }
 
 void MbD::ASMTSpatialContainer::storeOnLevelOmega(std::ofstream& os, int level)
 {
 	storeOnLevelString(os, level, "Omega3D");
-	storeOnLevelArray(os, level + 1, *omega3D);
+	if (omexs == nullptr || omexs->empty()) {
+		storeOnLevelArray(os, level + 1, *omega3D);
+	}
+	else {
+		auto array = getOmega3D(0);
+		storeOnLevelArray(os, level + 1, *array);
+	}
 }
 
 void MbD::ASMTSpatialContainer::storeOnLevelRefPoints(std::ofstream& os, int level)
@@ -697,3 +709,21 @@ void MbD::ASMTSpatialContainer::storeOnTimeSeries(std::ofstream& os)
 	}
 	os << std::endl;
 }
+
+FColDsptr MbD::ASMTSpatialContainer::getVelocity3D(size_t i)
+{
+	auto vec3 = std::make_shared<FullColumn<double>>(3);
+	vec3->atiput(0, vxs->at(i));
+	vec3->atiput(1, vys->at(i));
+	vec3->atiput(2, vzs->at(i));
+	return vec3;
+}
+
+FColDsptr MbD::ASMTSpatialContainer::getOmega3D(size_t i)
+{
+	auto vec3 = std::make_shared<FullColumn<double>>(3);
+	vec3->atiput(0, omexs->at(i));
+	vec3->atiput(1, omeys->at(i));
+	vec3->atiput(2, omezs->at(i));
+	return vec3;
+}

OndselSolver/ASMTSpatialContainer.h

@@ -86,16 +86,16 @@ namespace MbD {
         void storeOnLevelRefCurves(std::ofstream& os, int level);
         void storeOnLevelRefSurfaces(std::ofstream& os, int level);
         void storeOnTimeSeries(std::ofstream& os) override;
+        FColDsptr getVelocity3D(size_t i);
+        FColDsptr getOmega3D(size_t i);
 
         FColDsptr velocity3D = std::make_shared<FullColumn<double>>(3);
         FColDsptr omega3D = std::make_shared<FullColumn<double>>(3);
         std::shared_ptr<std::vector<std::shared_ptr<ASMTRefPoint>>> refPoints;
         std::shared_ptr<std::vector<std::shared_ptr<ASMTRefCurve>>> refCurves;
         std::shared_ptr<std::vector<std::shared_ptr<ASMTRefSurface>>> refSurfaces;
-        FRowDsptr xs, ys, zs, bryxs, bryys, bryzs;
         FRowDsptr vxs, vys, vzs, omexs, omeys, omezs;
         FRowDsptr axs, ays, azs, alpxs, alpys, alpzs;
-        FRowDsptr inxs, inys, inzs, inbryxs, inbryys, inbryzs;
         FRowDsptr invxs, invys, invzs, inomexs, inomeys, inomezs;
         FRowDsptr inaxs, inays, inazs, inalpxs, inalpys, inalpzs;
         std::shared_ptr<ASMTPrincipalMassMarker> principalMassMarker = std::make_shared<ASMTPrincipalMassMarker>();

OndselSolver/ASMTSpatialItem.cpp

@@ -10,6 +10,7 @@
 #include "Units.h"
 #include "Part.h"
 #include "ASMTSpatialContainer.h"
+#include "EulerAngles.h"
 
 using namespace MbD;
 
@@ -102,14 +103,50 @@ void MbD::ASMTSpatialItem::storeOnLevel(std::ofstream& os, int level)
 void MbD::ASMTSpatialItem::storeOnLevelPosition(std::ofstream& os, int level)
 {
 	storeOnLevelString(os, level, "Position3D");
-	storeOnLevelArray(os, level + 1, *position3D);
+	if (xs == nullptr || xs->empty()) {
+		storeOnLevelArray(os, level + 1, *position3D);
+	}
+	else {
+		auto array = getPosition3D(0);
+		storeOnLevelArray(os, level + 1, *array);
+	}
 }
 
 void MbD::ASMTSpatialItem::storeOnLevelRotationMatrix(std::ofstream& os, int level)
 {
 	storeOnLevelString(os, level, "RotationMatrix");
-	for (int i = 0; i < 3; i++)
-	{
-		storeOnLevelArray(os, level + 1, *rotationMatrix->at(i));
+	if (xs == nullptr || xs->empty()) {
+		for (int i = 0; i < 3; i++)
+		{
+			storeOnLevelArray(os, level + 1, *rotationMatrix->at(i));
+		}
+	}
+	else {
+		auto rotMat = getRotationMatrix(0);
+		for (int i = 0; i < 3; i++)
+		{
+			storeOnLevelArray(os, level + 1, *rotMat->at(i));
+		}
 	}
+
+}
+
+FColDsptr MbD::ASMTSpatialItem::getPosition3D(size_t i)
+{
+	auto vec3 = std::make_shared<FullColumn<double>>(3);
+	vec3->atiput(0, xs->at(i));
+	vec3->atiput(1, ys->at(i));
+	vec3->atiput(2, zs->at(i));
+	return vec3;
+}
+
+FMatDsptr MbD::ASMTSpatialItem::getRotationMatrix(size_t i)
+{
+	auto bryantAngles = std::make_shared<EulerAngles<double>>();
+	bryantAngles->setRotOrder(1, 2, 3);
+	bryantAngles->at(0) = bryxs->at(i);
+	bryantAngles->at(1) = bryys->at(i);
+	bryantAngles->at(2) = bryzs->at(i);
+	bryantAngles->calc();
+	return bryantAngles->aA;
 }

OndselSolver/ASMTSpatialItem.h

@@ -31,13 +31,18 @@ namespace MbD {
 		void storeOnLevel(std::ofstream& os, int level) override;
 		void storeOnLevelPosition(std::ofstream& os, int level);
 		void storeOnLevelRotationMatrix(std::ofstream& os, int level);
+		FColDsptr getPosition3D(size_t i);
+		FMatDsptr getRotationMatrix(size_t i);
 
 		FColDsptr position3D = std::make_shared<FullColumn<double>>(3);
 		FMatDsptr rotationMatrix = std::make_shared<FullMatrixDouble>(ListListD{
 				{ 1, 0, 0 },
 				{ 0, 1, 0 },
 				{ 0, 0, 1 }
 			});
+		FRowDsptr xs, ys, zs, bryxs, bryys, bryzs;
+		FRowDsptr inxs, inys, inzs, inbryxs, inbryys, inbryzs;
+
 	};
 }
 

OndselSolver/ASMTTime.cpp

@@ -25,7 +25,27 @@ void MbD::ASMTTime::createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Un
 {
 	auto mbdTime = mbdSys->time;
 	if (xx == mbdTime) return;
-	auto timeScale = std::make_shared<Constant>(mbdUnits->time);
+	auto timeScale = sptrConstant(mbdUnits->time);
 	auto geoTime = std::make_shared<Product>(timeScale, mbdTime);
 	this->xexpression(mbdTime, geoTime->simplified(geoTime));
 }
+
+Symsptr MbD::ASMTTime::expandUntil(Symsptr sptr, std::shared_ptr<std::unordered_set<Symsptr>> set)
+{
+	return sptr;
+}
+
+Symsptr MbD::ASMTTime::simplifyUntil(Symsptr sptr, std::shared_ptr<std::unordered_set<Symsptr>> set)
+{
+	return sptr;
+}
+
+bool MbD::ASMTTime::isVariable()
+{
+	return true;
+}
+
+void MbD::ASMTTime::setValue(double val)
+{
+	xx->setValue(val);
+}

OndselSolver/ASMTTime.h

@@ -19,6 +19,10 @@ namespace MbD {
     public:
         void deleteMbD();
         void createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits);
+        Symsptr expandUntil(Symsptr sptr, std::shared_ptr<std::unordered_set<Symsptr>> set) override;
+        Symsptr simplifyUntil(Symsptr sptr, std::shared_ptr<std::unordered_set<Symsptr>> set) override;
+        bool isVariable() override;
+        void setValue(double val) override;
 
     };
 }