piston.asmt file is running correctly

MbDCode/ASMTAnimationParameters.h

@@ -9,8 +9,9 @@ namespace MbD {
     public:
         void parseASMT(std::vector<std::string>& lines) override;
 
-		int nframe, icurrent, istart, iend, framesPerSecond;
-        bool isForward;
+		int nframe = 1000000, icurrent = 0, istart = 0, iend = 1000000, framesPerSecond = 30;
+        bool isForward = true;
+
 
     };
 }

MbDCode/ASMTAssembly.cpp

@@ -1,6 +1,8 @@
 #include <string>
 #include <cassert>
 #include <fstream>	
+#include <algorithm>
+#include <numeric>
 
 #include "ASMTAssembly.h"
 #include "CREATE.h"
@@ -8,16 +10,17 @@
 #include "ASMTCylindricalJoint.h"
 #include "ASMTRotationalMotion.h"
 #include "ASMTTranslationalMotion.h"
+#include "ASMTMarker.h"
+#include "Part.h"
 
 using namespace MbD;
 
 void MbD::ASMTAssembly::runFile(const char* chars)
 {
-	std::string str;
-	std::ifstream in(chars);
+	std::ifstream stream(chars);
 	std::string line;
 	std::vector<std::string> lines;
-	while (std::getline(in, line)) {
+	while (std::getline(stream, line)) {
 		lines.push_back(line);
 	}
 	assert(lines[0] == "freeCAD: 3D CAD with Motion Simulation  by  askoh.com");
@@ -27,10 +30,16 @@ void MbD::ASMTAssembly::runFile(const char* chars)
 		lines.erase(lines.begin());
 		auto assembly = CREATE<ASMTAssembly>::With();
 		assembly->parseASMT(lines);
+		assembly->runKINEMATIC();
 	}
 
 }
 
+ASMTAssembly* MbD::ASMTAssembly::root()
+{
+	return this;
+}
+
 void MbD::ASMTAssembly::parseASMT(std::vector<std::string>& lines)
 {
 	readNotes(lines);
@@ -39,13 +48,14 @@ void MbD::ASMTAssembly::parseASMT(std::vector<std::string>& lines)
 	readRotationMatrix(lines);
 	readVelocity3D(lines);
 	readOmega3D(lines);
+	initprincipalMassMarker();
 	readRefPoints(lines);
 	readRefCurves(lines);
 	readRefSurfaces(lines);
 	readParts(lines);
 	readKinematicIJs(lines);
 	readConstraintSets(lines);
-	readForceTorques(lines);
+	readForcesTorques(lines);
 	readConstantGravity(lines);
 	readSimulationParameters(lines);
 	readAnimationParameters(lines);
@@ -165,6 +175,7 @@ void MbD::ASMTAssembly::readMotions(std::vector<std::string>& lines)
 		motion->parseASMT(motionsLines);
 		motions->push_back(motion);
 		motion->owner = this;
+		motion->initMarkers();
 	}
 	lines.erase(lines.begin(), it);
 
@@ -183,19 +194,19 @@ void MbD::ASMTAssembly::readGeneralConstraintSets(std::vector<std::string>& line
 	lines.erase(lines.begin(), it);
 }
 
-void MbD::ASMTAssembly::readForceTorques(std::vector<std::string>& lines)
+void MbD::ASMTAssembly::readForcesTorques(std::vector<std::string>& lines)
 {
-	assert(lines[0] == "\tForceTorques");
+	assert(lines[0] == "\tForceTorques");	//Spelling is not consistent in asmt file.
 	lines.erase(lines.begin());
-	forceTorques = std::make_shared<std::vector<std::shared_ptr<ASMTForceTorque>>>();
+	forcesTorques = std::make_shared<std::vector<std::shared_ptr<ASMTForceTorque>>>();
 	auto it = std::find(lines.begin(), lines.end(), "\tConstantGravity");
-	std::vector<std::string> forceTorquesLines(lines.begin(), it);
-	while (!forceTorquesLines.empty()) {
-		if (forceTorquesLines[0] == "\t\tForceTorque") {
-			forceTorquesLines.erase(forceTorquesLines.begin());
+	std::vector<std::string> forcesTorquesLines(lines.begin(), it);
+	while (!forcesTorquesLines.empty()) {
+		if (forcesTorquesLines[0] == "\t\tForceTorque") {
+			forcesTorquesLines.erase(forcesTorquesLines.begin());
 			auto forceTorque = CREATE<ASMTForceTorque>::With();
-			forceTorque->parseASMT(forceTorquesLines);
-			forceTorques->push_back(forceTorque);
+			forceTorque->parseASMT(forcesTorquesLines);
+			forcesTorques->push_back(forceTorque);
 			forceTorque->owner = this;
 		}
 		else {
@@ -321,7 +332,7 @@ void MbD::ASMTAssembly::readPartSeries(std::vector<std::string>& lines)
 	auto it = std::find_if(parts->begin(), parts->end(), [&](const std::shared_ptr<ASMTPart>& prt) {
 		return prt->fullName("") == seriesName;
 		});
-	auto part = *it;
+	auto& part = *it;
 	part->readPartSeries(lines);
 }
 
@@ -336,7 +347,7 @@ void MbD::ASMTAssembly::readJointSeries(std::vector<std::string>& lines)
 	auto it = std::find_if(joints->begin(), joints->end(), [&](const std::shared_ptr<ASMTJoint>& jt) {
 		return jt->fullName("") == seriesName;
 		});
-	auto joint = *it;
+	auto& joint = *it;
 	joint->readJointSeries(lines);
 }
 
@@ -359,8 +370,275 @@ void MbD::ASMTAssembly::readMotionSeries(std::vector<std::string>& lines)
 	auto it = std::find_if(motions->begin(), motions->end(), [&](const std::shared_ptr<ASMTMotion>& jt) {
 		return jt->fullName("") == seriesName;
 		});
-	auto motion = *it;
+	auto& motion = *it;
 	motion->readMotionSeries(lines);
 }
 
+void MbD::ASMTAssembly::outputFor(AnalysisType type)
+{
+	assert(false);
+}
+
+void MbD::ASMTAssembly::logString(std::string& str)
+{
+	assert(false);
+}
+
+void MbD::ASMTAssembly::logString(double value)
+{
+	assert(false);
+}
+
+void MbD::ASMTAssembly::preMbDrun(std::shared_ptr<System> mbdSys)
+{
+	calcCharacteristicDimensions();
+	deleteMbD();
+	createMbD(mbdSys, mbdUnits);
+	std::static_pointer_cast<Part>(mbdObject)->asFixed();
+}
+
+void MbD::ASMTAssembly::postMbDrun()
+{
+	assert(false);
+}
+
+void MbD::ASMTAssembly::calcCharacteristicDimensions()
+{
+	auto unitTime = this->calcCharacteristicTime();
+	auto unitMass = this->calcCharacteristicMass();
+	auto unitLength = this->calcCharacteristicLength();
+	auto unitAngle = 1.0;
+	this->mbdUnits = std::make_shared<Units>(unitTime, unitMass, unitLength, unitAngle);
+}
+
+double MbD::ASMTAssembly::calcCharacteristicTime()
+{
+	return std::abs(simulationParameters->hout);
+}
+
+double MbD::ASMTAssembly::calcCharacteristicMass()
+{
+	auto n = parts->size();
+	double sumOfSquares = 0.0;
+	for (int i = 0; i < n; i++)
+	{
+		auto mass = parts->at(i)->principalMassMarker->mass;
+		sumOfSquares += mass * mass;
+	}
+	auto unitMass = std::sqrt(sumOfSquares / n);
+	if (unitMass <= 0) unitMass = 1.0;
+	return unitMass;
+}
+
+double MbD::ASMTAssembly::calcCharacteristicLength()
+{
+	auto markerMap = this->markerMap();
+	auto lengths = std::make_shared<std::vector<double>>();
+	auto connectorList = this->connectorList();
+	for (auto& connector : *connectorList) {
+		auto& mkrI = markerMap->at(connector->markerI);
+		lengths->push_back(mkrI->rpmp()->length());
+		auto& mkrJ = markerMap->at(connector->markerJ);
+		lengths->push_back(mkrJ->rpmp()->length());
+	}
+	auto n = lengths->size();
+	double sumOfSquares = std::accumulate(lengths->begin(), lengths->end(), 0.0, [](double sum, double l) { return sum + l * l; });
+	auto unitLength = std::sqrt(sumOfSquares / std::max((int)n, 1));
+	if (unitLength <= 0) unitLength = 1.0;
+	return unitLength;
+}
+
+std::shared_ptr<std::vector<std::shared_ptr<ASMTItemIJ>>> MbD::ASMTAssembly::connectorList()
+{
+	auto list = std::make_shared<std::vector<std::shared_ptr<ASMTItemIJ>>>();
+	list->insert(list->end(), joints->begin(), joints->end());
+	list->insert(list->end(), motions->begin(), motions->end());
+	list->insert(list->end(), kinematicIJs->begin(), kinematicIJs->end());
+	list->insert(list->end(), forcesTorques->begin(), forcesTorques->end());
+	return list;
+}
+
+std::shared_ptr<std::map<std::string, std::shared_ptr<ASMTMarker>>> MbD::ASMTAssembly::markerMap()
+{
+	auto answer = std::make_shared<std::map<std::string, std::shared_ptr<ASMTMarker>>>();
+	for (auto& refPoint : *refPoints) {
+		for (auto& marker : *refPoint->markers) {
+			answer->insert(std::make_pair(marker->fullName(""), marker));
+		}
+	}
+	for (auto& part : *parts) {
+		for (auto& refPoint : *part->refPoints) {
+			for (auto& marker : *refPoint->markers) {
+				answer->insert(std::make_pair(marker->fullName(""), marker));
+			}
+		}
+	}
+	return answer;
+}
+
+void MbD::ASMTAssembly::deleteMbD()
+{
+	ASMTSpatialContainer::deleteMbD();
+	constantGravity->deleteMbD();
+	asmtTime->deleteMbD();
+	for (auto& part : *parts) { part->deleteMbD(); }
+	for (auto& joint : *joints) { joint->deleteMbD(); }
+	for (auto& motion : *motions) { motion->deleteMbD(); }
+	for (auto& forceTorque : *forcesTorques) { forceTorque->deleteMbD(); }
+
+
+}
+
+void MbD::ASMTAssembly::createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits)
+{
+	ASMTSpatialContainer::createMbD(mbdSys, mbdUnits);
+	constantGravity->createMbD(mbdSys, mbdUnits);
+	asmtTime->createMbD(mbdSys, mbdUnits);
+	for (auto& part : *parts) { part->createMbD(mbdSys, mbdUnits); }
+	for (auto& joint : *joints) { joint->createMbD(mbdSys, mbdUnits); }
+	for (auto& motion : *motions) { motion->createMbD(mbdSys, mbdUnits); }
+	for (auto& forceTorque : *forcesTorques) { forceTorque->createMbD(mbdSys, mbdUnits); }
+
+	auto mbdSysSolver = mbdSys->systemSolver;
+	mbdSysSolver->errorTolPosKine = simulationParameters->errorTolPosKine;
+	mbdSysSolver->errorTolAccKine = simulationParameters->errorTolAccKine;
+	mbdSysSolver->iterMaxPosKine = simulationParameters->iterMaxPosKine;
+	mbdSysSolver->iterMaxAccKine = simulationParameters->iterMaxAccKine;
+	mbdSysSolver->tstart = simulationParameters->tstart / mbdUnits->time;
+	mbdSysSolver->tend = simulationParameters->tend / mbdUnits->time;
+	mbdSysSolver->hmin = simulationParameters->hmin / mbdUnits->time;
+	mbdSysSolver->hmax = simulationParameters->hmax / mbdUnits->time;
+	mbdSysSolver->hout = simulationParameters->hout / mbdUnits->time;
+	mbdSysSolver->corAbsTol = simulationParameters->corAbsTol;
+	mbdSysSolver->corRelTol = simulationParameters->corRelTol;
+	mbdSysSolver->intAbsTol = simulationParameters->intAbsTol;
+	mbdSysSolver->intRelTol = simulationParameters->intRelTol;
+	mbdSysSolver->iterMaxDyn = simulationParameters->iterMaxDyn;
+	mbdSysSolver->orderMax = simulationParameters->orderMax;
+	mbdSysSolver->translationLimit = simulationParameters->translationLimit / mbdUnits->length;
+	mbdSysSolver->rotationLimit = simulationParameters->rotationLimit;
+	animationParameters = nullptr;
+}
+
+void MbD::ASMTAssembly::runKINEMATIC()
+{
+	auto mbdSystem = CREATE<System>::With();
+	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->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();
+}
+
+std::shared_ptr<ASMTSpatialContainer> MbD::ASMTAssembly::spatialContainerAt(std::shared_ptr<ASMTAssembly> self, std::string& longname)
+{
+	if ((self->fullName("")) == longname) return self;
+	auto it = std::find_if(parts->begin(), parts->end(), [&](const std::shared_ptr<ASMTPart>& prt) {
+		return prt->fullName("") == longname;
+		});
+	auto& part = *it;
+	return part;
+}
+
+std::shared_ptr<ASMTMarker> MbD::ASMTAssembly::markerAt(std::string& longname)
+{
+	for (auto& refPoint : *refPoints) {
+		for (auto& marker : *refPoint->markers) {
+			if (marker->fullName("") == longname) return marker;
+		}
+	}
+	for (auto& part : *parts) {
+		for (auto& refPoint : *part->refPoints) {
+			for (auto& marker : *refPoint->markers) {
+				if (marker->fullName("") == longname) return marker;
+			}
+		}
+	}
+	return nullptr;
+}
+
+std::shared_ptr<ASMTJoint> MbD::ASMTAssembly::jointAt(std::string& longname)
+{
+	auto it = std::find_if(joints->begin(), joints->end(), [&](const std::shared_ptr<ASMTJoint>& jt) {
+		return jt->fullName("") == longname;
+		});
+	auto& joint = *it;
+	return joint;
+}
+
+std::shared_ptr<ASMTMotion> MbD::ASMTAssembly::motionAt(std::string& longname)
+{
+	auto it = std::find_if(motions->begin(), motions->end(), [&](const std::shared_ptr<ASMTMotion>& mt) {
+		return mt->fullName("") == longname;
+		});
+	auto& motion = *it;
+	return motion;
+}
+
+std::shared_ptr<ASMTForceTorque> MbD::ASMTAssembly::forceTorqueAt(std::string& longname)
+{
+	auto it = std::find_if(forcesTorques->begin(), forcesTorques->end(), [&](const std::shared_ptr<ASMTForceTorque>& mt) {
+		return mt->fullName("") == longname;
+		});
+	auto& forceTorque = *it;
+	return forceTorque;
+}
+
+FColDsptr MbD::ASMTAssembly::vOcmO()
+{
+	return std::make_shared<FullColumn<double>>(3, 0.0);
+}
+
+FColDsptr MbD::ASMTAssembly::omeOpO()
+{
+	return std::make_shared<FullColumn<double>>(3, 0.0);
+}
+
+std::shared_ptr<ASMTTime> MbD::ASMTAssembly::geoTime()
+{
+	return asmtTime;
+}
+
+void MbD::ASMTAssembly::updateFromMbD()
+{
+	ASMTSpatialContainer::updateFromMbD();
+	auto geoTime = asmtTime->getValue();
+	auto it = std::find_if(times->begin(), times->end(), [&](double t) {
+		return Numeric::equaltol(t, geoTime, 1.0e-9);
+		});
+	assert(it != times->end());
+	for (auto& part : *parts) part->updateFromMbD();
+	for (auto& joint : *joints) joint->updateFromMbD();
+	for (auto& motion : *motions) motion->updateFromMbD();
+	for (auto& forceTorque : *forcesTorques) forceTorque->updateFromMbD();
+}
+
+void MbD::ASMTAssembly::compareResults(AnalysisType type)
+{
+	ASMTSpatialContainer::compareResults(type);
+	for (auto& part : *parts) part->compareResults(type);
+	for (auto& joint : *joints) joint->compareResults(type);
+	for (auto& motion : *motions) motion->compareResults(type);
+	for (auto& forceTorque : *forcesTorques) forceTorque->compareResults(type);
+}
+
 

MbDCode/ASMTAssembly.h

@@ -16,6 +16,9 @@
 #include "FullMatrix.h"
 #include "ASMTJoint.h"
 #include "ASMTMotion.h"
+#include "Units.h"
+#include "ASMTTime.h"
+#include "SystemSolver.h"
 
 namespace MbD {
     class ASMTAssembly : public ASMTSpatialContainer
@@ -23,6 +26,7 @@ namespace MbD {
         //
     public:
         static void runFile(const char* chars);
+        ASMTAssembly* root() override;
         void parseASMT(std::vector<std::string>& lines) override;
         void readNotes(std::vector<std::string>& lines);
         void readParts(std::vector<std::string>& lines);
@@ -33,7 +37,7 @@ namespace MbD {
         void readJoints(std::vector<std::string>& lines);
         void readMotions(std::vector<std::string>& lines);
         void readGeneralConstraintSets(std::vector<std::string>& lines);
-        void readForceTorques(std::vector<std::string>& lines);
+        void readForcesTorques(std::vector<std::string>& lines);
         void readConstantGravity(std::vector<std::string>& lines);
         void readSimulationParameters(std::vector<std::string>& lines);
         void readAnimationParameters(std::vector<std::string>& lines);
@@ -47,17 +51,45 @@ namespace MbD {
         void readMotionSeriesMany(std::vector<std::string>& lines);
         void readMotionSeries(std::vector<std::string>& lines);
 
+        void outputFor(AnalysisType type);
+        void logString(std::string& str);
+        void logString(double value);
+        void preMbDrun(std::shared_ptr<System> mbdSys);
+        void postMbDrun();
+        void calcCharacteristicDimensions();
+        double calcCharacteristicTime();
+        double calcCharacteristicMass();
+        double calcCharacteristicLength();
+        std::shared_ptr<std::vector<std::shared_ptr<ASMTItemIJ>>> connectorList();
+        std::shared_ptr<std::map<std::string, std::shared_ptr<ASMTMarker>>>markerMap();
+        void deleteMbD();
+        void createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits) override;
+        void runKINEMATIC();
+        void initprincipalMassMarker();
+        std::shared_ptr<ASMTSpatialContainer> spatialContainerAt(std::shared_ptr<ASMTAssembly> self, std::string& longname);
+        std::shared_ptr<ASMTMarker> markerAt(std::string& longname);
+        std::shared_ptr<ASMTJoint> jointAt(std::string& longname);
+        std::shared_ptr<ASMTMotion> motionAt(std::string& longname);
+        std::shared_ptr<ASMTForceTorque> forceTorqueAt(std::string& longname);
+        FColDsptr vOcmO() override;
+        FColDsptr omeOpO() override;
+        std::shared_ptr<ASMTTime> geoTime();
+        void updateFromMbD() override;
+        void compareResults(AnalysisType type) override;
+
         std::string notes;
         std::shared_ptr<std::vector<std::shared_ptr<ASMTPart>>> parts;
         std::shared_ptr<std::vector<std::shared_ptr<ASMTKinematicIJ>>> kinematicIJs;
         std::shared_ptr<std::vector<std::shared_ptr<ASMTConstraintSet>>> constraintSets;
         std::shared_ptr<std::vector<std::shared_ptr<ASMTJoint>>> joints;
         std::shared_ptr<std::vector<std::shared_ptr<ASMTMotion>>> motions;
-        std::shared_ptr<std::vector<std::shared_ptr<ASMTForceTorque>>> forceTorques;
+        std::shared_ptr<std::vector<std::shared_ptr<ASMTForceTorque>>> forcesTorques;
         std::shared_ptr<ASMTConstantGravity> constantGravity;
         std::shared_ptr<ASMTSimulationParameters> simulationParameters;
         std::shared_ptr<ASMTAnimationParameters> animationParameters;
         std::shared_ptr<std::vector<double>> times;
+        std::shared_ptr<ASMTTime> asmtTime = std::make_shared<ASMTTime>();
+        std::shared_ptr<Units> mbdUnits;
 
     };
 }

MbDCode/ASMTConstantGravity.cpp

@@ -1,4 +1,7 @@
 #include "ASMTConstantGravity.h"
+#include "Units.h"
+#include "ConstantGravity.h"
+#include "System.h"
 
 using namespace MbD;
 
@@ -7,3 +10,11 @@ void MbD::ASMTConstantGravity::parseASMT(std::vector<std::string>& lines)
 	g = readColumnOfDoubles(lines[0]);
 	lines.erase(lines.begin());
 }
+
+void MbD::ASMTConstantGravity::createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits)
+{
+	auto mbdGravity = CREATE<ConstantGravity>::With();
+	mbdObject = mbdGravity;
+	mbdGravity->gXYZ = g->times(1.0 / mbdUnits->acceleration);
+	mbdSys->addForceTorque(mbdGravity);
+}

MbDCode/ASMTConstantGravity.h

@@ -1,13 +1,18 @@
 #pragma once
 
 #include "ASMTItem.h"
+//#include "Units.h"
 
 namespace MbD {
+    class System;
+    class Units;
+
     class ASMTConstantGravity : public ASMTItem
     {
         //
     public:
         void parseASMT(std::vector<std::string>& lines) override;
+        void createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits) override;
 
         FColDsptr g;
     };

MbDCode/ASMTConstraintSet.cpp

@@ -1,61 +1,63 @@
 #include "ASMTConstraintSet.h"
+#include "ASMTAssembly.h"
+#include "ASMTMarker.h"
 
 using namespace MbD;
 
-void MbD::ASMTConstraintSet::readMarkerI(std::vector<std::string>& lines)
+void MbD::ASMTConstraintSet::createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits)
 {
-	assert(lines[0].find("MarkerI") != std::string::npos);
-	lines.erase(lines.begin());
-	markerI = readString(lines[0]);
-	lines.erase(lines.begin());
+	//self dataSeries : OrderedCollection new.
+	//self discontinuities : OrderedCollection new.
+	auto mbdJt = this->mbdClassNew();
+	mbdObject = mbdJt;
+	mbdJt->name = fullName("");
+	auto mrkI = std::static_pointer_cast<EndFramec>(root()->markerAt(markerI)->mbdObject);
+	auto mrkJ = std::static_pointer_cast<EndFramec>(root()->markerAt(markerJ)->mbdObject);
+	mbdJt->connectsItoJ(mrkI, mrkJ);
+	mbdSys->addJoint(mbdJt);
 }
 
-void MbD::ASMTConstraintSet::readMarkerJ(std::vector<std::string>& lines)
+std::shared_ptr<Joint> MbD::ASMTConstraintSet::mbdClassNew()
 {
-	assert(lines[0].find("MarkerJ") != std::string::npos);
-	lines.erase(lines.begin());
-	markerJ = readString(lines[0]);
-	lines.erase(lines.begin());
+	assert(false);
+	return std::shared_ptr<Joint>();
 }
 
-void MbD::ASMTConstraintSet::readFXonIs(std::vector<std::string>& lines)
+void MbD::ASMTConstraintSet::updateFromMbD()
 {
-	std::string str = lines[0];
-	readDoublesInto(str, "FXonI", fxs);
-	lines.erase(lines.begin());
+	//"
+	//MbD returns aFIeO and aTIeO.
+	//GEO needs aFImO and aTImO.
+	//For Motion rImIeO is not zero and is changing.
+	//aFImO = aFIeO.
+	//aTImO = aTIeO + (rImIeO cross : aFIeO).
+	//"
+	auto mbdUnts = mbdUnits();
+	auto mbdJoint = std::static_pointer_cast<Joint>(mbdObject);
+	auto aFIeO = mbdJoint->aFX()->times(mbdUnts->force);
+	auto aTIeO = mbdJoint->aTX()->times(mbdUnts->torque);
+	auto rImIeO = mbdJoint->frmI->rmeO()->times(mbdUnts->length);
+	auto aFIO = aFIeO;
+	auto aTIO = aTIeO->plusFullColumn(rImIeO->cross(aFIeO));
+	fxs->push_back(aFIO->at(0));
+	fys->push_back(aFIO->at(1));
+	fzs->push_back(aFIO->at(2));
+	txs->push_back(aTIO->at(0));
+	tys->push_back(aTIO->at(1));
+	tzs->push_back(aTIO->at(2));
 }
 
-void MbD::ASMTConstraintSet::readFYonIs(std::vector<std::string>& lines)
+void MbD::ASMTConstraintSet::compareResults(AnalysisType type)
 {
-	std::string str = lines[0];
-	readDoublesInto(str, "FYonI", fys);
-	lines.erase(lines.begin());
-}
-
-void MbD::ASMTConstraintSet::readFZonIs(std::vector<std::string>& lines)
-{
-	std::string str = lines[0];
-	readDoublesInto(str, "FZonI", fzs);
-	lines.erase(lines.begin());
-}
-
-void MbD::ASMTConstraintSet::readTXonIs(std::vector<std::string>& lines)
-{
-	std::string str = lines[0];
-	readDoublesInto(str, "TXonI", txs);
-	lines.erase(lines.begin());
-}
-
-void MbD::ASMTConstraintSet::readTYonIs(std::vector<std::string>& lines)
-{
-	std::string str = lines[0];
-	readDoublesInto(str, "TYonI", tys);
-	lines.erase(lines.begin());
-}
-
-void MbD::ASMTConstraintSet::readTZonIs(std::vector<std::string>& lines)
-{
-	std::string str = lines[0];
-	readDoublesInto(str, "TZonI", tzs);
-	lines.erase(lines.begin());
+	auto mbdUnts = mbdUnits();
+	auto factor = 1.0e-6;
+	auto forceTol = mbdUnts->force * factor;
+	auto torqueTol = mbdUnts->torque * factor;
+	auto i = fxs->size() - 1;
+	//assert(Numeric::equaltol(fxs->at(i), infxs->at(i), forceTol));
+	//assert(Numeric::equaltol(fys->at(i), infys->at(i), forceTol));
+	//assert(Numeric::equaltol(fzs->at(i), infzs->at(i), forceTol));
+	//assert(Numeric::equaltol(txs->at(i), intxs->at(i), torqueTol));
+	//assert(Numeric::equaltol(tys->at(i), intys->at(i), torqueTol));
+	//assert(Numeric::equaltol(tzs->at(i), intzs->at(i), torqueTol));
 }

MbDCode/ASMTConstraintSet.h

@@ -1,23 +1,18 @@
 #pragma once
 
-#include "ASMTItem.h"
+#include "ASMTItemIJ.h"
 
 namespace MbD {
-    class ASMTConstraintSet : public ASMTItem
+    class Joint;
+
+    class ASMTConstraintSet : public ASMTItemIJ
     {
         //
     public:
-        void readMarkerI(std::vector<std::string>& lines);
-        void readMarkerJ(std::vector<std::string>& lines);
-        void readFXonIs(std::vector<std::string>& lines);
-        void readFYonIs(std::vector<std::string>& lines);
-        void readFZonIs(std::vector<std::string>& lines);
-        void readTXonIs(std::vector<std::string>& lines);
-        void readTYonIs(std::vector<std::string>& lines);
-        void readTZonIs(std::vector<std::string>& lines);
-
-        std::string markerI, markerJ;
-        FRowDsptr fxs, fys, fzs, txs, tys, tzs;
+        void createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits) override;
+        virtual std::shared_ptr<Joint> mbdClassNew();
+        void updateFromMbD() override;
+        void compareResults(AnalysisType type) override;
 
     };
 }

MbDCode/ASMTCylindricalJoint.cpp

@@ -2,3 +2,7 @@
 
 using namespace MbD;
 
+std::shared_ptr<Joint> MbD::ASMTCylindricalJoint::mbdClassNew()
+{
+    return CREATE<CylindricalJoint>::With();
+}

MbDCode/ASMTCylindricalJoint.h

@@ -1,12 +1,14 @@
 #pragma once
 
 #include "ASMTJoint.h"
+#include "CylindricalJoint.h"
 
 namespace MbD {
     class ASMTCylindricalJoint : public ASMTJoint
     {
         //
     public:
+        std::shared_ptr<Joint> mbdClassNew() override;
 
 
     };

MbDCode/ASMTForceTorque.cpp

@@ -1,3 +1,13 @@
 #include "ASMTForceTorque.h"
 
 using namespace MbD;
+
+void MbD::ASMTForceTorque::updateFromMbD()
+{
+	assert(false);
+}
+
+void MbD::ASMTForceTorque::compareResults(AnalysisType type)
+{
+	assert(false);
+}

MbDCode/ASMTForceTorque.h

@@ -1,12 +1,14 @@
 #pragma once
 
-#include "ASMTItem.h"
+#include "ASMTItemIJ.h"
 
 namespace MbD {
-    class ASMTForceTorque : public ASMTItem
+    class ASMTForceTorque : public ASMTItemIJ
     {
         //
     public:
+        void updateFromMbD() override;
+        void compareResults(AnalysisType type) override;
 
 
     };

MbDCode/ASMTItem.cpp

@@ -1,8 +1,20 @@
 #include "ASMTItem.h"
 #include "CREATE.h"
+#include "ASMTSpatialContainer.h"
+#include "ASMTAssembly.h"
 
 using namespace MbD;
 
+ASMTAssembly* MbD::ASMTItem::root()
+{
+	return owner->root();
+}
+
+std::shared_ptr<ASMTSpatialContainer> MbD::ASMTItem::part()
+{
+	return owner->part();
+}
+
 void MbD::ASMTItem::initialize()
 {
 }
@@ -62,6 +74,7 @@ bool MbD::ASMTItem::readBool(std::string& line)
 	}
 	else {
 		assert(false);
+		return false;
 	}
 }
 
@@ -98,3 +111,31 @@ void MbD::ASMTItem::readDoublesInto(std::string& str, std::string label, FRowDsp
 	str.erase(0, pos + label.length());
 	row = readRowOfDoubles(str);
 }
+
+void MbD::ASMTItem::deleteMbD()
+{
+	mbdObject = nullptr;
+}
+
+void MbD::ASMTItem::createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits)
+{
+	assert(false);
+}
+
+void MbD::ASMTItem::updateFromMbD()
+{
+	assert(false);
+}
+
+void MbD::ASMTItem::compareResults(AnalysisType type)
+{
+	assert(false);
+}
+
+std::shared_ptr<Units> MbD::ASMTItem::mbdUnits()
+{
+	if (owner) {
+		return owner->mbdUnits();
+	}
+	return static_cast<ASMTAssembly*>(this)->mbdUnits;
+}

MbDCode/ASMTItem.h

@@ -2,10 +2,17 @@
 #include "CREATE.h"
 
 namespace MbD {
+	class ASMTAssembly;
+	class Units;
+	class ASMTSpatialContainer;
+
 	class ASMTItem
 	{
 		//
 	public:
+		virtual ASMTAssembly* root();
+		virtual std::shared_ptr<ASMTSpatialContainer> part();
+
 		virtual void initialize();
 		virtual void parseASMT(std::vector<std::string>& lines);
 		FRowDsptr readRowOfDoubles(std::string& line);
@@ -15,11 +22,17 @@ namespace MbD {
 		bool readBool(std::string& line);
 		std::string readString(std::string& line);
 		void readName(std::vector<std::string>& lines);
-		std::string fullName(std::string partialName);
+		virtual std::string fullName(std::string partialName);
 		void readDoublesInto(std::string& str, std::string label, FRowDsptr& row);
+		virtual void deleteMbD();
+		virtual void createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits);
+		virtual void updateFromMbD();
+		virtual void compareResults(AnalysisType type);
+		std::shared_ptr<Units> mbdUnits();
 
 		std::string name;
 		ASMTItem* owner;
+		std::shared_ptr<Item> mbdObject;
 	};
 }
 

MbDCode/ASMTItemIJ.cpp

@@ -0,0 +1,69 @@
+#include "ASMTItemIJ.h"
+
+void MbD::ASMTItemIJ::initialize()
+{
+	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::readMarkerI(std::vector<std::string>& lines)
+{
+	assert(lines[0].find("MarkerI") != std::string::npos);
+	lines.erase(lines.begin());
+	markerI = readString(lines[0]);
+	lines.erase(lines.begin());
+}
+
+void MbD::ASMTItemIJ::readMarkerJ(std::vector<std::string>& lines)
+{
+	assert(lines[0].find("MarkerJ") != std::string::npos);
+	lines.erase(lines.begin());
+	markerJ = readString(lines[0]);
+	lines.erase(lines.begin());
+}
+
+void MbD::ASMTItemIJ::readFXonIs(std::vector<std::string>& lines)
+{
+	std::string str = lines[0];
+	readDoublesInto(str, "FXonI", infxs);
+	lines.erase(lines.begin());
+}
+
+void MbD::ASMTItemIJ::readFYonIs(std::vector<std::string>& lines)
+{
+	std::string str = lines[0];
+	readDoublesInto(str, "FYonI", infys);
+	lines.erase(lines.begin());
+}
+
+void MbD::ASMTItemIJ::readFZonIs(std::vector<std::string>& lines)
+{
+	std::string str = lines[0];
+	readDoublesInto(str, "FZonI", infzs);
+	lines.erase(lines.begin());
+}
+
+void MbD::ASMTItemIJ::readTXonIs(std::vector<std::string>& lines)
+{
+	std::string str = lines[0];
+	readDoublesInto(str, "TXonI", intxs);
+	lines.erase(lines.begin());
+}
+
+void MbD::ASMTItemIJ::readTYonIs(std::vector<std::string>& lines)
+{
+	std::string str = lines[0];
+	readDoublesInto(str, "TYonI", intys);
+	lines.erase(lines.begin());
+}
+
+void MbD::ASMTItemIJ::readTZonIs(std::vector<std::string>& lines)
+{
+	std::string str = lines[0];
+	readDoublesInto(str, "TZonI", intzs);
+	lines.erase(lines.begin());
+}

MbDCode/ASMTItemIJ.h

@@ -0,0 +1,26 @@
+#pragma once
+
+#include "ASMTItem.h"
+
+namespace MbD {
+    class ASMTItemIJ : public ASMTItem
+    {
+        //
+    public:
+        void initialize() override;
+        void readMarkerI(std::vector<std::string>& lines);
+        void readMarkerJ(std::vector<std::string>& lines);
+        void readFXonIs(std::vector<std::string>& lines);
+        void readFYonIs(std::vector<std::string>& lines);
+        void readFZonIs(std::vector<std::string>& lines);
+        void readTXonIs(std::vector<std::string>& lines);
+        void readTYonIs(std::vector<std::string>& lines);
+        void readTZonIs(std::vector<std::string>& lines);
+
+        std::string markerI, markerJ;
+        FRowDsptr fxs, fys, fzs, txs, tys, tzs;
+        FRowDsptr infxs, infys, infzs, intxs, intys, intzs;
+
+    };
+}
+

MbDCode/ASMTKinematicIJ.h

@@ -1,9 +1,9 @@
 #pragma once
 
-#include "ASMTItem.h"
+#include "ASMTItemIJ.h"
 
 namespace MbD {
-    class ASMTKinematicIJ : public ASMTItem
+    class ASMTKinematicIJ : public ASMTItemIJ
     {
         //
     public:

MbDCode/ASMTMarker.cpp

@@ -1,4 +1,9 @@
 #include "ASMTMarker.h"
+#include "ASMTRefItem.h"
+#include "ASMTPart.h"
+#include "Part.h"
+#include "PartFrame.h"
+#include "MarkerFrame.h"
 
 using namespace MbD;
 
@@ -8,3 +13,41 @@ void MbD::ASMTMarker::parseASMT(std::vector<std::string>& lines)
 	readPosition3D(lines);
 	readRotationMatrix(lines);
 }
+
+FColDsptr MbD::ASMTMarker::rpmp()
+{
+	//p is cm
+	auto refItem = static_cast<ASMTRefItem*>(owner);
+	auto& rPrefP = refItem->position3D;
+	auto& aAPref = refItem->rotationMatrix;
+	auto& rrefmref = position3D;
+	auto rPmP = rPrefP->plusFullColumn(aAPref->timesFullColumn(rrefmref));
+	auto& principalMassMarker = static_cast<ASMTPart*>(refItem->owner)->principalMassMarker;
+	auto& rPcmP = principalMassMarker->position3D;
+	auto& aAPcm = principalMassMarker->rotationMatrix;
+	auto rpmp = aAPcm->transposeTimesFullColumn(rPmP->minusFullColumn(rPcmP));
+	return rpmp;
+}
+
+FMatDsptr MbD::ASMTMarker::aApm()
+{
+	//p is cm
+	auto refItem = static_cast<ASMTRefItem*>(owner);
+	auto& aAPref = refItem->rotationMatrix;
+	auto& aArefm = rotationMatrix;
+	auto& principalMassMarker = static_cast<ASMTPart*>(refItem->owner)->principalMassMarker;
+	auto& aAPcm = principalMassMarker->rotationMatrix;
+	auto aApm = aAPcm->transposeTimesFullMatrix(aAPref->timesFullMatrix(aArefm));
+	return 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);
+	prt->partFrame->addMarkerFrame(mkr);
+
+	mkr->rpmp = rpmp()->times(1.0 / mbdUnits->length);
+	mkr->aApm = aApm();
+	mbdObject = mkr->endFrames->at(0);
+}

MbDCode/ASMTMarker.h

@@ -10,6 +10,9 @@ namespace MbD {
         //
     public:
         void parseASMT(std::vector<std::string>& lines) override;
+        FColDsptr rpmp();
+        FMatDsptr aApm();
+        void createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits) override;
 
     };
 }

MbDCode/ASMTMotion.cpp

@@ -19,3 +19,7 @@ void MbD::ASMTMotion::readMotionSeries(std::vector<std::string>& lines)
 	readTYonIs(lines);
 	readTZonIs(lines);
 }
+
+void MbD::ASMTMotion::initMarkers()
+{
+}

MbDCode/ASMTMotion.h

@@ -9,6 +9,7 @@ namespace MbD {
         //
     public:
         void readMotionSeries(std::vector<std::string>& lines);
+        virtual void initMarkers();
 
         std::shared_ptr<std::vector<std::shared_ptr<ForceTorqueData>>> motionSeries;
 

MbDCode/ASMTPart.cpp

@@ -80,3 +80,17 @@ void MbD::ASMTPart::readPartSeries(std::vector<std::string>& lines)
 	readAlphaYs(lines);
 	readAlphaZs(lines);
 }
+
+FColDsptr MbD::ASMTPart::vOcmO()
+{
+	auto& rOPO = position3D;
+	auto& vOPO = velocity3D;
+	auto& omeOPO = omega3D;
+	auto rPcmO = rOcmO()->minusFullColumn(rOPO);
+	return vOPO->plusFullColumn(omeOPO->cross(rPcmO));
+}
+
+FColDsptr MbD::ASMTPart::omeOpO()
+{
+	return omega3D;
+}

MbDCode/ASMTPart.h

@@ -16,8 +16,9 @@ namespace MbD {
         void readFeatureOrder(std::vector<std::string>& lines);
         void readPrincipalMassMarker(std::vector<std::string>& lines);
         void readPartSeries(std::vector<std::string>& lines);
+        FColDsptr vOcmO() override;
+        FColDsptr omeOpO() override;
 
-        std::shared_ptr<ASMTPrincipalMassMarker> principalMassMarker;
         //std::shared_ptr<std::vector<std::shared_ptr<ASMTFeature>>> featureOrder;
         std::shared_ptr<std::vector<std::shared_ptr<PosVelAccData>>> partSeries;
 

MbDCode/ASMTRefCurve.h

@@ -1,9 +1,9 @@
 #pragma once
 
-#include "ASMTItem.h"
+#include "ASMTRefItem.h"
 
 namespace MbD {
-    class ASMTRefCurve : public ASMTItem
+    class ASMTRefCurve : public ASMTRefItem
     {
         //
     public:

MbDCode/ASMTRefItem.cpp

@@ -0,0 +1,29 @@
+#include "ASMTRefItem.h"
+#include "CREATE.h"
+
+using namespace MbD;
+
+void MbD::ASMTRefItem::readMarkers(std::vector<std::string>& lines)
+{
+	assert(lines[0].find("Markers") != std::string::npos);
+	lines.erase(lines.begin());
+	markers = std::make_shared<std::vector<std::shared_ptr<ASMTMarker>>>();
+	auto it = std::find_if(lines.begin(), lines.end(), [](const std::string& s) {
+		return s.find("RefPoint") != std::string::npos;
+		});
+	std::vector<std::string> markersLines(lines.begin(), it);
+	while (!markersLines.empty()) {
+		readMarker(markersLines);
+	}
+	lines.erase(lines.begin(), it);
+}
+
+void MbD::ASMTRefItem::readMarker(std::vector<std::string>& lines)
+{
+	assert(lines[0].find("Marker") != std::string::npos);
+	lines.erase(lines.begin());
+	auto marker = CREATE<ASMTMarker>::With();
+	marker->parseASMT(lines);
+	markers->push_back(marker);
+	marker->owner = this;
+}

MbDCode/ASMTRefItem.h

@@ -0,0 +1,20 @@
+#pragma once
+
+#include "ASMTSpatialItem.h"
+#include "ASMTMarker.h"
+
+namespace MbD {
+
+    class ASMTRefItem : public ASMTSpatialItem
+    {
+        //
+    public:
+        void readMarkers(std::vector<std::string>& lines);
+        void readMarker(std::vector<std::string>& lines);
+
+        std::shared_ptr<std::vector<std::shared_ptr<ASMTMarker>>> markers;
+
+
+    };
+}
+

MbDCode/ASMTRefPoint.cpp

@@ -1,4 +1,5 @@
 #include "ASMTRefPoint.h"
+#include "ASMTMarker.h"
 #include "CREATE.h"
 
 using namespace MbD;
@@ -10,27 +11,14 @@ void MbD::ASMTRefPoint::parseASMT(std::vector<std::string>& lines)
 	readMarkers(lines);
 }
 
-void MbD::ASMTRefPoint::readMarkers(std::vector<std::string>& lines)
+std::string MbD::ASMTRefPoint::fullName(std::string partialName)
 {
-	assert(lines[0].find("Markers") != std::string::npos);
-	lines.erase(lines.begin());
-	markers = std::make_shared<std::vector<std::shared_ptr<ASMTMarker>>>();
-	auto it = std::find_if(lines.begin(), lines.end(), [](const std::string& s) {
-		return s.find("RefPoint") != std::string::npos;
-		});
-	std::vector<std::string> markersLines(lines.begin(), it);
-	while (!markersLines.empty()) {
-		readMarker(markersLines);
-	}
-	lines.erase(lines.begin(), it);
+	return owner->fullName(partialName);
 }
 
-void MbD::ASMTRefPoint::readMarker(std::vector<std::string>& lines)
+void MbD::ASMTRefPoint::createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits)
 {
-	assert(lines[0].find("Marker") != std::string::npos);
-	lines.erase(lines.begin());
-	auto marker = CREATE<ASMTMarker>::With();
-	marker->parseASMT(lines);
-	markers->push_back(marker);
-	marker->owner = this;
+	for (auto& marker : *markers) {
+		marker->createMbD(mbdSys, mbdUnits);
+	}
 }

MbDCode/ASMTRefPoint.h

@@ -1,20 +1,19 @@
 #pragma once
 
-#include "ASMTSpatialItem.h"
+#include "ASMTRefItem.h"
 #include <vector>
 #include <string>
-#include "ASMTMarker.h"
 
 namespace MbD {
-    class ASMTRefPoint : public ASMTSpatialItem
+    class ASMTRefPoint : public ASMTRefItem
     {
         //
     public:
         void parseASMT(std::vector<std::string>& lines) override;
-        void readMarkers(std::vector<std::string>& lines);
-        void readMarker(std::vector<std::string>& lines);
+        std::string fullName(std::string partialName) override;
+        void createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits) override;
 
-        std::shared_ptr<std::vector<std::shared_ptr<ASMTMarker>>> markers;
+    
     };
 }
 

MbDCode/ASMTRefSurface.h

@@ -1,9 +1,9 @@
 #pragma once
 
-#include "ASMTItem.h"
+#include "ASMTRefItem.h"
 
 namespace MbD {
-    class ASMTRefSurface : public ASMTItem
+    class ASMTRefSurface : public ASMTRefItem
     {
         //
     public:

MbDCode/ASMTRevoluteJoint.cpp

@@ -1,3 +1,8 @@
 #include "ASMTRevoluteJoint.h"
 
 using namespace MbD;
+
+std::shared_ptr<Joint> MbD::ASMTRevoluteJoint::mbdClassNew()
+{
+    return CREATE<RevoluteJoint>::With();
+}

MbDCode/ASMTRevoluteJoint.h

@@ -1,13 +1,14 @@
 #pragma once
 
 #include "ASMTJoint.h"
+#include "RevoluteJoint.h"
 
 namespace MbD {
     class ASMTRevoluteJoint : public ASMTJoint
     {
         //
     public:
-
+        virtual std::shared_ptr<Joint> mbdClassNew() override;
 
     };
 }

MbDCode/ASMTRotationalMotion.cpp

@@ -1,4 +1,9 @@
 #include "ASMTRotationalMotion.h"
+#include "ASMTAssembly.h"
+#include "SymbolicParser.h"
+#include "BasicUserFunction.h"
+#include "CREATE.h"
+#include "Constant.h"
 
 using namespace MbD;
 
@@ -24,3 +29,30 @@ void MbD::ASMTRotationalMotion::readRotationZ(std::vector<std::string>& lines)
 	rotationZ = readString(lines[0]);
 	lines.erase(lines.begin());
 }
+
+void MbD::ASMTRotationalMotion::initMarkers()
+{
+	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();
+	parser->owner = this;
+	auto geoTime = owner->root()->geoTime();
+	parser->variables->insert(std::make_pair("time", geoTime));
+	auto userFunc = CREATE<BasicUserFunction>::With(rotationZ, 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);
+	std::static_pointer_cast<ZRotation>(mbdObject)->phiBlk = geoPhi->simplified(geoPhi);
+}
+
+std::shared_ptr<Joint> MbD::ASMTRotationalMotion::mbdClassNew()
+{
+	return CREATE<ZRotation>::With();
+}

MbDCode/ASMTRotationalMotion.h

@@ -1,6 +1,7 @@
 #pragma once
 
 #include "ASMTMotion.h"
+#include "ZRotation.h"
 
 namespace MbD {
     class ASMTRotationalMotion : public ASMTMotion
@@ -10,6 +11,9 @@ namespace MbD {
         void parseASMT(std::vector<std::string>& lines) override;
         void readMotionJoint(std::vector<std::string>& lines);
         void readRotationZ(std::vector<std::string>& lines);
+        void initMarkers() override;
+        void createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits) override;
+        std::shared_ptr<Joint> mbdClassNew() override;
 
         std::string motionJoint, rotationZ;
     };

MbDCode/ASMTSimulationParameters.h

@@ -3,14 +3,16 @@
 #include "ASMTItem.h"
 
 namespace MbD {
-    class ASMTSimulationParameters : public ASMTItem
-    {
-        //
-    public:
-        void parseASMT(std::vector<std::string>& lines) override;
+	class ASMTSimulationParameters : public ASMTItem
+	{
+		//
+	public:
+		void parseASMT(std::vector<std::string>& lines) override;
 
-        double tstart, tend, hmin, hmax, hout, errorTol;
-
-    };
+		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;
+		double intAbsTol = 1.0e-6, intRelTol = 1.0e-6, translationLimit = 1.0e9, rotationLimit = 1.0e9;
+		int iterMaxPosKine = 25, iterMaxAccKine = 25, iterMaxDyn = 4, orderMax = 5;
+	};
 }
 

MbDCode/ASMTSpatialContainer.cpp

@@ -1,4 +1,31 @@
 #include "ASMTSpatialContainer.h"
+#include "Units.h"
+#include "Part.h"
+#include "System.h"
+
+using namespace MbD;
+
+void MbD::ASMTSpatialContainer::initialize()
+{
+	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::readRefPoints(std::vector<std::string>& lines)
 {
@@ -68,125 +95,254 @@ void MbD::ASMTSpatialContainer::readRefSurface(std::vector<std::string>& lines)
 void MbD::ASMTSpatialContainer::readXs(std::vector<std::string>& lines)
 {
 	std::string str = lines[0];
-	readDoublesInto(str, "X", xs);
+	readDoublesInto(str, "X", inxs);
 	lines.erase(lines.begin());
 }
 
 void MbD::ASMTSpatialContainer::readYs(std::vector<std::string>& lines)
 {
 	std::string str = lines[0];
-	readDoublesInto(str, "Y", ys);
+	readDoublesInto(str, "Y", inys);
 	lines.erase(lines.begin());
 }
 
 void MbD::ASMTSpatialContainer::readZs(std::vector<std::string>& lines)
 {
 	std::string str = lines[0];
-	readDoublesInto(str, "Z", zs);
+	readDoublesInto(str, "Z", inzs);
 	lines.erase(lines.begin());
 }
 
 void MbD::ASMTSpatialContainer::readBryantxs(std::vector<std::string>& lines)
 {
 	std::string str = lines[0];
-	readDoublesInto(str, "Bryantx", bryxs);
+	readDoublesInto(str, "Bryantx", inbryxs);
 	lines.erase(lines.begin());
 }
 
 void MbD::ASMTSpatialContainer::readBryantys(std::vector<std::string>& lines)
 {
 	std::string str = lines[0];
-	readDoublesInto(str, "Bryanty", bryys);
+	readDoublesInto(str, "Bryanty", inbryys);
 	lines.erase(lines.begin());
 }
 
 void MbD::ASMTSpatialContainer::readBryantzs(std::vector<std::string>& lines)
 {
 	std::string str = lines[0];
-	readDoublesInto(str, "Bryantz", bryzs);
+	readDoublesInto(str, "Bryantz", inbryzs);
 	lines.erase(lines.begin());
 }
 
 void MbD::ASMTSpatialContainer::readVXs(std::vector<std::string>& lines)
 {
 	std::string str = lines[0];
-	readDoublesInto(str, "VX", vxs);
+	readDoublesInto(str, "VX", invxs);
 	lines.erase(lines.begin());
 }
 
 void MbD::ASMTSpatialContainer::readVYs(std::vector<std::string>& lines)
 {
 	std::string str = lines[0];
-	readDoublesInto(str, "VY", vys);
+	readDoublesInto(str, "VY", invys);
 	lines.erase(lines.begin());
 }
 
 void MbD::ASMTSpatialContainer::readVZs(std::vector<std::string>& lines)
 {
 	std::string str = lines[0];
-	readDoublesInto(str, "VZ", vzs);
+	readDoublesInto(str, "VZ", invzs);
 	lines.erase(lines.begin());
 }
 
 void MbD::ASMTSpatialContainer::readOmegaXs(std::vector<std::string>& lines)
 {
 	std::string str = lines[0];
-	readDoublesInto(str, "OmegaX", omexs);
+	readDoublesInto(str, "OmegaX", inomexs);
 	lines.erase(lines.begin());
 }
 
 void MbD::ASMTSpatialContainer::readOmegaYs(std::vector<std::string>& lines)
 {
 	std::string str = lines[0];
-	readDoublesInto(str, "OmegaY", omeys);
+	readDoublesInto(str, "OmegaY", inomeys);
 	lines.erase(lines.begin());
 }
 
 void MbD::ASMTSpatialContainer::readOmegaZs(std::vector<std::string>& lines)
 {
 	std::string str = lines[0];
-	readDoublesInto(str, "OmegaZ", omezs);
+	readDoublesInto(str, "OmegaZ", inomezs);
 	lines.erase(lines.begin());
 }
 
 void MbD::ASMTSpatialContainer::readAXs(std::vector<std::string>& lines)
 {
 	std::string str = lines[0];
-	readDoublesInto(str, "AX", axs);
+	readDoublesInto(str, "AX", inaxs);
 	lines.erase(lines.begin());
 }
 
 void MbD::ASMTSpatialContainer::readAYs(std::vector<std::string>& lines)
 {
 	std::string str = lines[0];
-	readDoublesInto(str, "AY", ays);
+	readDoublesInto(str, "AY", inays);
 	lines.erase(lines.begin());
 }
 
 void MbD::ASMTSpatialContainer::readAZs(std::vector<std::string>& lines)
 {
 	std::string str = lines[0];
-	readDoublesInto(str, "AZ", azs);
+	readDoublesInto(str, "AZ", inazs);
 	lines.erase(lines.begin());
 }
 
 void MbD::ASMTSpatialContainer::readAlphaXs(std::vector<std::string>& lines)
 {
 	std::string str = lines[0];
-	readDoublesInto(str, "AlphaX", alpxs);
+	readDoublesInto(str, "AlphaX", inalpxs);
 	lines.erase(lines.begin());
 }
 
 void MbD::ASMTSpatialContainer::readAlphaYs(std::vector<std::string>& lines)
 {
 	std::string str = lines[0];
-	readDoublesInto(str, "AlphaY", alpys);
+	readDoublesInto(str, "AlphaY", inalpys);
 	lines.erase(lines.begin());
 }
 
 void MbD::ASMTSpatialContainer::readAlphaZs(std::vector<std::string>& lines)
 {
 	std::string str = lines[0];
-	readDoublesInto(str, "AlphaZ", alpzs);
+	readDoublesInto(str, "AlphaZ", inalpzs);
 	lines.erase(lines.begin());
 }
+
+void MbD::ASMTSpatialContainer::createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits)
+{
+	auto mbdPart = CREATE<Part>::With();
+	mbdObject = mbdPart;
+	mbdPart->name = fullName("");
+	mbdPart->m = principalMassMarker->mass / mbdUnits->mass;
+	mbdPart->aJ = principalMassMarker->momentOfInertias->times(1.0 / mbdUnits->aJ);
+	mbdPart->qX(rOcmO()->times(1.0 / mbdUnits->length));
+	mbdPart->qE(qEp());
+	mbdPart->qXdot(vOcmO()->times(1.0 / mbdUnits->velocity));
+	mbdPart->omeOpO(omeOpO()->times(1.0 / mbdUnits->omega));
+	mbdPart->qXddot(std::make_shared<FullColumn<double>>(3, 0));
+	mbdPart->qEddot(std::make_shared<FullColumn<double>>(4, 0));
+	mbdSys->addPart(mbdPart);
+	for (auto& refPoint : *refPoints) {
+		refPoint->createMbD(mbdSys, mbdUnits);
+	}
+	for (auto& refCurve : *refCurves) {
+		refCurve->createMbD(mbdSys, mbdUnits);
+	}
+	for (auto& refSurface : *refSurfaces) {
+		refSurface->createMbD(mbdSys, mbdUnits);
+	}
+}
+
+FColDsptr MbD::ASMTSpatialContainer::rOcmO()
+{
+	auto& rOPO = position3D;
+	auto& aAOP = rotationMatrix;
+	auto& rPcmP = principalMassMarker->position3D;
+	auto rOcmO = rOPO->plusFullColumn(aAOP->timesFullColumn(rPcmP));
+	return rOcmO;
+}
+
+std::shared_ptr<EulerParameters<double>> MbD::ASMTSpatialContainer::qEp()
+{
+	auto& aAOP = rotationMatrix;
+	auto& aAPcm = principalMassMarker->rotationMatrix;
+	auto aAOcm = aAOP->timesFullMatrix(aAPcm);
+	return aAOcm->asEulerParameters();
+}
+
+FColDsptr MbD::ASMTSpatialContainer::vOcmO()
+{
+	assert(false);
+	return FColDsptr();
+}
+
+FColDsptr MbD::ASMTSpatialContainer::omeOpO()
+{
+	assert(false);
+	return FColDsptr();
+}
+
+std::shared_ptr<ASMTSpatialContainer> MbD::ASMTSpatialContainer::part()
+{
+	return std::make_shared<ASMTSpatialContainer>(*this);
+}
+
+void MbD::ASMTSpatialContainer::updateFromMbD()
+{
+	auto mbdUnts = mbdUnits();
+	auto mbdPart = std::static_pointer_cast<Part>(mbdObject);
+	auto rOcmO = mbdPart->qX()->times(mbdUnts->length);
+	auto aAOp = mbdPart->aAOp();
+	auto vOcmO = mbdPart->qXdot()->times(mbdUnts->velocity);
+	auto omeOPO = mbdPart->omeOpO()->times(mbdUnts->omega);
+	auto aOcmO = mbdPart->qXddot()->times(mbdUnts->acceleration);
+	auto alpOPO = mbdPart->alpOpO()->times(mbdUnts->alpha);
+	auto& rPcmP = principalMassMarker->position3D;
+	auto& aAPp = principalMassMarker->rotationMatrix;
+	auto aAOP = aAOp->timesTransposeFullMatrix(aAPp);
+	auto rPcmO = aAOP->timesFullColumn(rPcmP);
+	auto rOPO = rOcmO->minusFullColumn(rPcmO);
+	auto vOPO = vOcmO->minusFullColumn(omeOPO->cross(rPcmO));
+	auto aOPO = aOcmO->minusFullColumn(alpOPO->cross(rPcmO))->minusFullColumn(omeOPO->cross(omeOPO->cross(rPcmO)));
+	xs->push_back(rOPO->at(0));
+	ys->push_back(rOPO->at(1));
+	zs->push_back(rOPO->at(2));
+	auto bryantAngles = aAOP->bryantAngles();
+	bryxs->push_back(bryantAngles->at(0));
+	bryys->push_back(bryantAngles->at(1));
+	bryzs->push_back(bryantAngles->at(2));
+	vxs->push_back(vOPO->at(0));
+	vys->push_back(vOPO->at(1));
+	vzs->push_back(vOPO->at(2));
+	omexs->push_back(omeOPO->at(0));
+	omeys->push_back(omeOPO->at(1));
+	omezs->push_back(omeOPO->at(2));
+	axs->push_back(aOPO->at(0));
+	ays->push_back(aOPO->at(1));
+	azs->push_back(aOPO->at(2));
+	alpxs->push_back(alpOPO->at(0));
+	alpys->push_back(alpOPO->at(1));
+	alpzs->push_back(alpOPO->at(2));
+}
+
+void MbD::ASMTSpatialContainer::compareResults(AnalysisType type)
+{
+	auto mbdUnts = mbdUnits();
+	auto factor = 1.0e-6;
+	auto lengthTol = mbdUnts->length * factor;
+	auto angleTol = mbdUnts->angle * factor;
+	auto velocityTol = mbdUnts->velocity * factor;
+	auto omegaTol = mbdUnts->omega * factor;
+	auto accelerationTol = mbdUnts->acceleration * factor;
+	auto alphaTol = mbdUnts->alpha * factor;
+	auto i = xs->size() - 1;
+	assert(Numeric::equaltol(xs->at(i), inxs->at(i), lengthTol));
+	assert(Numeric::equaltol(ys->at(i), inys->at(i), lengthTol));
+	assert(Numeric::equaltol(zs->at(i), inzs->at(i), lengthTol));
+	assert(Numeric::equaltol(bryxs->at(i), inbryxs->at(i), lengthTol));
+	assert(Numeric::equaltol(bryys->at(i), inbryys->at(i), lengthTol));
+	assert(Numeric::equaltol(bryzs->at(i), inbryzs->at(i), lengthTol));
+	assert(Numeric::equaltol(vxs->at(i), invxs->at(i), lengthTol));
+	assert(Numeric::equaltol(vys->at(i), invys->at(i), lengthTol));
+	assert(Numeric::equaltol(vzs->at(i), invzs->at(i), lengthTol));
+	assert(Numeric::equaltol(omexs->at(i), inomexs->at(i), lengthTol));
+	assert(Numeric::equaltol(omeys->at(i), inomeys->at(i), lengthTol));
+	assert(Numeric::equaltol(omezs->at(i), inomezs->at(i), lengthTol));
+	if (type == INPUT) return;
+	assert(Numeric::equaltol(axs->at(i), inaxs->at(i), lengthTol));
+	assert(Numeric::equaltol(ays->at(i), inays->at(i), lengthTol));
+	assert(Numeric::equaltol(azs->at(i), inazs->at(i), lengthTol));
+	assert(Numeric::equaltol(alpxs->at(i), inalpxs->at(i), lengthTol));
+	assert(Numeric::equaltol(alpys->at(i), inalpys->at(i), lengthTol));
+	assert(Numeric::equaltol(alpzs->at(i), inalpzs->at(i), lengthTol));
+}

MbDCode/ASMTSpatialContainer.h

@@ -4,12 +4,15 @@
 #include "ASMTRefPoint.h"
 #include "ASMTRefCurve.h"
 #include "ASMTRefSurface.h"
+#include "ASMTPrincipalMassMarker.h"
+#include "Units.h"
 
 namespace MbD {
     class ASMTSpatialContainer : public ASMTSpatialItem
     {
         //
     public:
+        void initialize() override;
         void readRefPoints(std::vector<std::string>& lines);
         void readRefPoint(std::vector<std::string>& lines);
         void readRefCurves(std::vector<std::string>& lines);
@@ -34,6 +37,14 @@ namespace MbD {
         void readAlphaXs(std::vector<std::string>& lines);
         void readAlphaYs(std::vector<std::string>& lines);
         void readAlphaZs(std::vector<std::string>& lines);
+        void createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits) override;
+        FColDsptr rOcmO();
+        std::shared_ptr<EulerParameters<double>> qEp();
+        virtual FColDsptr vOcmO();
+        virtual FColDsptr omeOpO();
+        std::shared_ptr<ASMTSpatialContainer> part() override;
+        void updateFromMbD() override;
+        void compareResults(AnalysisType type) override;
 
         std::shared_ptr<std::vector<std::shared_ptr<ASMTRefPoint>>> refPoints;
         std::shared_ptr<std::vector<std::shared_ptr<ASMTRefCurve>>> refCurves;
@@ -41,6 +52,10 @@ namespace MbD {
         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;
 
     };
 }

MbDCode/ASMTTime.cpp

@@ -0,0 +1,23 @@
+#include <cassert>
+
+#include "ASMTTime.h"
+#include "Time.h"
+#include "Constant.h"
+#include "Product.h"
+
+using namespace MbD;
+
+void MbD::ASMTTime::deleteMbD()
+{
+	xx = nullptr;
+	expression = nullptr;
+}
+
+void MbD::ASMTTime::createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits)
+{
+	auto mbdTime = mbdSys->time;
+	if (xx == mbdTime) return;
+	auto timeScale = std::make_shared<Constant>(mbdUnits->time);
+	auto geoTime = std::make_shared<Product>(timeScale, mbdTime);
+	this->xexpression(mbdTime, geoTime->simplified(geoTime));
+}

MbDCode/ASMTTime.h

@@ -0,0 +1,17 @@
+#pragma once
+
+#include "ExpressionX.h"
+#include "System.h"
+#include "Units.h"
+
+namespace MbD {
+    class ASMTTime : public ExpressionX
+    {
+        //
+    public:
+        void deleteMbD();
+        void createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits);
+
+    };
+}
+

MbDCode/ASMTTranslationalMotion.cpp

@@ -1,4 +1,5 @@
 #include "ASMTTranslationalMotion.h"
+#include "ASMTAssembly.h"
 
 using namespace MbD;
 
@@ -19,3 +20,35 @@ void MbD::ASMTTranslationalMotion::parseASMT(std::vector<std::string>& lines)
 	translationZ = lines[0];
 	lines.erase(lines.begin());
 }
+
+void MbD::ASMTTranslationalMotion::initMarkers()
+{
+	auto jt = root()->jointAt(motionJoint);
+	markerI = jt->markerI;
+	markerJ = jt->markerJ;
+}
+
+void MbD::ASMTTranslationalMotion::createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits)
+{
+	//ASMTMotion::createMbD(mbdSys, mbdUnits);
+	//	zFunc : = zIJ isNil
+	//	ifTrue : [StMConstant with : 0]
+	//	ifFalse : [zIJ isUserFunction
+	//	ifTrue :
+	//[parser:= self functionParser.
+	//	stack : = parser
+	//	parseUserFunction : zIJ
+	//	notifying : nil
+	//	ifFail : nil.
+	//	func : = stack last.
+	//	func]
+	//ifFalse: [zIJ] ] .
+	//	zFunc : = (zFunc / self mbdUnits length) createMbD simplified.
+	//	mbdZTranslation : = self mbdObject.
+	//	mbdZTranslation zBlk : zFunc
+}
+
+std::shared_ptr<Joint> MbD::ASMTTranslationalMotion::mbdClassNew()
+{
+	return CREATE<ZTranslation>::With();
+}

MbDCode/ASMTTranslationalMotion.h

@@ -1,6 +1,7 @@
 #pragma once
 
 #include "ASMTMotion.h"
+#include "ZTranslation.h"
 
 namespace MbD {
     class ASMTTranslationalMotion : public ASMTMotion
@@ -8,6 +9,9 @@ namespace MbD {
         //
     public:
         void parseASMT(std::vector<std::string>& lines) override;
+        void initMarkers() override;
+        void createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits) override;
+        std::shared_ptr<Joint> mbdClassNew() override;
 
         std::string motionJoint, translationZ;
 

MbDCode/Abs.cpp

@@ -0,0 +1,6 @@
+#include "Abs.h"
+
+double MbD::Abs::getValue()
+{
+    return std::abs(xx->getValue());
+}

MbDCode/Abs.h

@@ -0,0 +1,15 @@
+#pragma once
+
+#include "FunctionX.h"
+
+namespace MbD {
+    class Abs : public FunctionX
+    {
+        //
+    public:
+        double getValue() override;
+
+
+    };
+}
+

MbDCode/AbsConstraint.cpp

@@ -15,10 +15,10 @@ AbsConstraint::AbsConstraint(int i)
 void AbsConstraint::calcPostDynCorrectorIteration()
 {
     if (axis < 3) {
-        aG = static_cast<PartFrame*>(owner)->qX->at(axis);
+        aG = static_cast<PartFrame*>(owner)->qX->at((size_t)axis);
     }
     else {
-        aG = static_cast<PartFrame*>(owner)->qE->at(axis - 3);
+        aG = static_cast<PartFrame*>(owner)->qE->at((size_t)axis - 3);
     }
 }
 
@@ -55,10 +55,10 @@ void AbsConstraint::fillAccICIterError(FColDsptr col)
     auto partFrame = static_cast<PartFrame*>(owner);
         double sum;
         if (axis < 3) {
-            sum = partFrame->qXddot->at(axis);
+            sum = partFrame->qXddot->at((size_t)axis);
         }
         else {
-            sum = partFrame->qEddot->at(axis - 3);
+            sum = partFrame->qEddot->at((size_t)axis - 3);
         }
         col->atiplusNumber(iG, sum);
 }

MbDCode/ArcTan.cpp

@@ -0,0 +1,6 @@
+#include "ArcTan.h"
+
+double MbD::ArcTan::getValue()
+{
+    return std::atan(xx->getValue());
+}

MbDCode/ArcTan.h

@@ -0,0 +1,15 @@
+#pragma once
+
+#include "FunctionX.h"
+
+namespace MbD {
+    class ArcTan : public FunctionX
+    {
+        //
+    public:
+        double getValue() override;
+
+
+    };
+}
+

MbDCode/Array.h

@@ -25,9 +25,16 @@ namespace MbD {
 		double rootMeanSquare();
 		virtual int numberOfElements();
 		void swapElems(int i, int ii);
-		//double maxMagnitude();
+		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 atitimes(int i, double factor);
 
 		virtual std::ostream& printOn(std::ostream& s) const {
 			std::string str = typeid(*this).name();
@@ -76,21 +83,17 @@ namespace MbD {
 		this->at(i) = this->at(ii);
 		this->at(ii) = temp;
 	}
-	//template<typename T>
-	//inline double Array<T>::maxMagnitude()
+	//template<>
+	//inline double Array<double>::maxMagnitude()
 	//{
-	//	if (std::is_arithmetic<T>::value) {
-	//		return this->maxMagnitudeOfVector();
-	//	}
-	//	else {
-	//		auto answer = 0.0;
-	//		for (int i = 0; i < this->size(); i++)
-	//		{
-	//			auto mag = this->at(i)->maxMagnitude();
-	//			if (answer < mag) answer = mag;
-	//		}
-	//		return answer;
+	//	auto max = 0.0;
+	//	for (int i = 0; i < this->size(); i++)
+	//	{
+	//		auto element = this->at(i);
+	//		if (element < 0.0) element = -element;
+	//		if (max < element) max = element;
 	//	}
+	//	return max;
 	//}
 	template<typename T>
 	inline double Array<T>::maxMagnitudeOfVector()
@@ -111,6 +114,59 @@ namespace MbD {
 			this->at(ii) = array->at(i + ii);
 		}
 	}
+	//template<>
+	//inline void Array<double>::normalizeSelf()
+	//{
+	//	auto 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;
+	//	conditionSelfWithTol(tol);
+	//}
+	//template<>
+	//inline void Array<double>::conditionSelfWithTol(double tol)
+	//{
+	//	for (int i = 0; i < this->size(); i++)
+	//	{
+	//		auto element = this->at(i);
+	//		if (element < 0.0) element = -element;
+	//		if (element < tol) this->atiput(i, 0.0);
+	//	}
+	//}
+	template<typename T>
+	inline void Array<T>::atiput(int i, T value)
+	{
+		this->at(i) = value;
+	}
+	//template<>
+	//inline double Array<double>::length()
+	//{
+	//	auto ssq = 0.0;
+	//	for (int i = 0; i < this->size(); i++)
+	//	{
+	//		auto elem = this->at(i);
+	//		ssq += elem * elem;
+	//	}
+	//	return std::sqrt(ssq);
+	//}
+	template<typename T>
+	inline void Array<T>::magnifySelf(T factor)
+	{
+		for (int i = 0; i < this->size(); i++)
+		{
+			this->atitimes(i, factor);
+		}
+	}
+	template<typename T>
+	inline void Array<T>::atitimes(int i, double factor)
+	{
+		this->at(i) *= factor;
+	}
 	using ListD = std::initializer_list<double>;
 	using ListListD = std::initializer_list<std::initializer_list<double>>;
 	using ListListPairD = std::initializer_list<std::initializer_list<std::initializer_list<double>>>;

MbDCode/BasicUserFunction.cpp

@@ -1,3 +1,14 @@
 #include "BasicUserFunction.h"
+#include "CREATE.h"
+#include "Units.h"
 
 using namespace MbD;
+
+MbD::BasicUserFunction::BasicUserFunction(const std::string& expression, double myUnt) : funcText(expression), myUnit(myUnt)
+{
+}
+
+void MbD::BasicUserFunction::initialize()
+{
+	units = CREATE<Units>::With();
+}

MbDCode/BasicUserFunction.h

@@ -5,12 +5,18 @@
 #include "UserFunction.h"
 
 namespace MbD {
+    class Units;
+
     class BasicUserFunction : public UserFunction
     {
         //funcText myUnit units 
     public:
+        BasicUserFunction(const std::string& expression, double myUnt);
+        void initialize();
+            
         std::string funcText;
         double myUnit;
+        std::shared_ptr<Units> units;
     };
 }
 

MbDCode/CADSystem.cpp

@@ -277,7 +277,7 @@ void CADSystem::runOndselPiston()
 	std::cout << "rotMotion1->phiBlk " << *(rotMotion1->phiBlk) << std::endl;
 	TheSystem->addJoint(rotMotion1);
 	//
-	TheSystem->runKINEMATIC();
+	TheSystem->runKINEMATIC(TheSystem);
 }
 
 void CADSystem::runPiston()
@@ -514,9 +514,17 @@ void CADSystem::runPiston()
 	std::cout << "rotMotion1->phiBlk " << *(rotMotion1->phiBlk) << std::endl;
 	TheSystem->addJoint(rotMotion1);
 	//
-	TheSystem->runKINEMATIC();
+	TheSystem->runKINEMATIC(TheSystem);
+}
+
+void MbD::CADSystem::preMbDrun(std::shared_ptr<System> mbdSys)
+{
 }
 
 void CADSystem::postMbDrun()
 {
 }
+
+void MbD::CADSystem::updateFromMbD()
+{
+}

MbDCode/CADSystem.h

@@ -15,6 +15,7 @@ namespace MbD {
 		//
 	public:
 		CADSystem() {
+			mbdSystem->initialize();
 			mbdSystem->externalSystem->cadSystem = this;
 		}
 
@@ -23,7 +24,9 @@ namespace MbD {
 		void logString(double value);
 		void runOndselPiston();
 		void runPiston();
+		void preMbDrun(std::shared_ptr<System> mbdSys);
 		void postMbDrun();
+		void updateFromMbD();
 
 		std::shared_ptr<System> mbdSystem = std::make_shared<System>();
 

MbDCode/CREATE.h

@@ -1,4 +1,4 @@
-//This header file causes wierd problems in Visual Studio when included in subclasses of std::vector or std::map.
+//This header file causes wierd problems in Visual Studio when included in subclasses of std::vector or std::map. Why?
 
 #pragma once
 
@@ -20,6 +20,11 @@ namespace MbD {
 			inst->initialize();
 			return inst;
 		}
+		static std::shared_ptr<T> With(const std::string& expr, double unit) {
+			auto inst = std::make_shared<T>(expr, unit);
+			inst->initialize();
+			return inst;
+		}
 		static std::shared_ptr<T> With() {
 			auto inst = std::make_shared<T>();
 			inst->initialize();

MbDCode/Constant.cpp

@@ -1,4 +1,6 @@
 #include "Constant.h"
+#include "System.h"
+#include "Units.h"
 
 using namespace MbD;
 
@@ -10,7 +12,7 @@ Constant::Constant(double val) : Variable(val)
 {
 }
 
-Symsptr Constant::differentiateWRT(Symsptr sptr, Symsptr var)
+Symsptr MbD::Constant::differentiateWRT(Symsptr var)
 {
 	return std::make_shared<Constant>(0.0);
 }
@@ -20,6 +22,36 @@ bool Constant::isConstant()
 	return true;
 }
 
+Symsptr MbD::Constant::expandUntil(std::shared_ptr<std::unordered_set<Symsptr>> set)
+{
+	return clonesptr();
+}
+
+Symsptr MbD::Constant::clonesptr()
+{
+	return std::make_shared<Constant>(*this);
+}
+
+bool MbD::Constant::isZero()
+{
+	return value == 0.0;
+}
+
+bool MbD::Constant::isOne()
+{
+	return value == 1.0;
+}
+
+void MbD::Constant::createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits)
+{
+	return;
+}
+
+double MbD::Constant::getValue()
+{
+	return value;
+}
+
 std::ostream& Constant::printOn(std::ostream& s) const
 {
 	return s << this->value;

MbDCode/Constant.h

@@ -8,8 +8,15 @@ namespace MbD {
     public:
         Constant();
         Constant(double val);
-        Symsptr differentiateWRT(Symsptr sptr, Symsptr var) override;
+        Symsptr differentiateWRT(Symsptr var) override;
         bool isConstant() override;
+        Symsptr expandUntil(std::shared_ptr<std::unordered_set<Symsptr>> set) override;
+        Symsptr clonesptr() override;
+        bool isZero() override;
+        bool isOne() override;
+        void createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits) override;
+        double getValue() override;
+
         std::ostream& printOn(std::ostream& s) const override;
     };
 }

MbDCode/ConstantGravity.cpp

@@ -0,0 +1,9 @@
+#include "ConstantGravity.h"
+#include "System.h"
+
+using namespace MbD;
+
+void MbD::ConstantGravity::submitToSystem(std::shared_ptr<Item> self)
+{
+	root()->forcesTorques->push_back(std::static_pointer_cast<ForceTorqueItem>(self));
+}

MbDCode/ConstantGravity.h

@@ -0,0 +1,15 @@
+#pragma once
+
+#include "ForceTorqueItem.h"
+
+namespace MbD {
+    class ConstantGravity : public ForceTorqueItem
+    {
+        //
+    public:
+        void submitToSystem(std::shared_ptr<Item> self) override;
+
+        FColDsptr gXYZ;
+    };
+}
+

MbDCode/Constraint.h.bak

@@ -1,24 +0,0 @@
-#pragma once
-#include <memory>
-
-#include "Item.h"
-//#include "PartFrame.h"
-
-namespace MbD {
-	class Constraint : public Item
-	{
-		//iG aG lam mu lamDeriv owner 
-	public:
-		Constraint();
-		Constraint(const char* str);
-		void initialize();
-		void setOwner(Item* x);
-		Item* getOwner();
-		
-		int iG;
-		double aG;  //Constraint function
-		double lam; //Lambda is Lagrange Multiplier
-		Item* owner; //A Joint or PartFrame owns the constraint
-	};
-}
-

MbDCode/CylindricalJoint.cpp.bak

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

MbDCode/CylindricalJoint.h.bak

@@ -1,10 +0,0 @@
-#pragma once
-#include "Joint.h"
-
-namespace MbD {
-    class CylindricalJoint : public Joint
-    {
-    public:
-    };
-}
-

MbDCode/DiagonalMatrix.h

@@ -13,29 +13,25 @@ namespace MbD {
 		DiagonalMatrix(int count) : Array<T>(count) {}
 		DiagonalMatrix(int count, const T& value) : Array<T>(count, value) {}
 		DiagonalMatrix(std::initializer_list<T> list) : Array<T>{ list } {}
-		void atiput(int i, T value);
 		void atiputDiagonalMatrix(int i, std::shared_ptr < DiagonalMatrix<T>> diagMat);
 		std::shared_ptr<DiagonalMatrix<T>> times(T factor);
 		std::shared_ptr<FullColumn<T>> timesFullColumn(std::shared_ptr<FullColumn<T>> fullCol);
 		std::shared_ptr<FullMatrix<T>> timesFullMatrix(std::shared_ptr<FullMatrix<T>> fullMat);
 		int nrow() {
-			return (int) this->size();
+			return (int)this->size();
 		}
 		int ncol() {
-			return (int) this->size();
+			return (int)this->size();
 		}
 		double sumOfSquares() override;
 		int numberOfElements() override;
 		void zeroSelf() override;
+		double maxMagnitude() override;
+
 		std::ostream& printOn(std::ostream& s) const override;
 
 	};
 	template<typename T>
-	inline void DiagonalMatrix<T>::atiput(int i, T value)
-	{
-		this->at(i) = value;
-	}
-	template<typename T>
 	inline void DiagonalMatrix<T>::atiputDiagonalMatrix(int i, std::shared_ptr<DiagonalMatrix<T>> diagMat)
 	{
 		for (int ii = 0; ii < diagMat->size(); ii++)
@@ -59,7 +55,7 @@ namespace MbD {
 	{
 		//"a*b = a(i,j)b(j) sum j."
 
-		auto nrow = (int) this->size();
+		auto nrow = (int)this->size();
 		auto answer = std::make_shared<FullColumn<T>>(nrow);
 		for (int i = 0; i < nrow; i++)
 		{
@@ -92,7 +88,7 @@ namespace MbD {
 	template<typename T>
 	inline int DiagonalMatrix<T>::numberOfElements()
 	{
-		auto n = (int) this->size();
+		auto n = (int)this->size();
 		return n * n;
 	}
 	template<>
@@ -102,6 +98,24 @@ namespace MbD {
 			this->at(i) = 0.0;
 		}
 	}
+	template<>
+	inline double DiagonalMatrix<double>::maxMagnitude()
+	{
+		auto max = 0.0;
+		for (int i = 0; i < this->size(); i++)
+		{
+			auto element = this->at(i);
+			if (element < 0.0) element = -element;
+			if (max < element) max = element;
+		}
+		return max;
+	}
+	template<typename T>
+	inline double DiagonalMatrix<T>::maxMagnitude()
+	{
+		assert(false);
+		return 0.0;
+	}
 	template<typename T>
 	inline std::ostream& DiagonalMatrix<T>::printOn(std::ostream& s) const
 	{

MbDCode/DistxyIeqcJec.cpp

@@ -22,12 +22,12 @@ void MbD::DistxyIeqcJec::calc_ppdistxypEIpEI()
 	auto ppypEIpEI = yIeJeIe->ppvaluepEIpEI();
 	for (int i = 0; i < 4; i++)
 	{
-		auto ppdistxypEIpEIi = ppdistxypEIpEI->at(i);
-		auto pdistxypEIi = pdistxypEI->at(i);
-		auto ppxpEIpEIi = ppxpEIpEI->at(i);
-		auto ppypEIpEIi = ppypEIpEI->at(i);
-		auto pxpEIi = pxpEI->at(i);
-		auto pypEIi = pypEI->at(i);
+		auto& ppdistxypEIpEIi = ppdistxypEIpEI->at(i);
+		auto& pdistxypEIi = pdistxypEI->at(i);
+		auto& ppxpEIpEIi = ppxpEIpEI->at(i);
+		auto& ppypEIpEIi = ppypEIpEI->at(i);
+		auto& pxpEIi = pxpEI->at(i);
+		auto& pypEIi = pypEI->at(i);
 		for (int j = i; j < 4; j++)
 		{
 			auto pdistxypEIj = pdistxypEI->at(j);
@@ -55,17 +55,17 @@ void MbD::DistxyIeqcJec::calc_ppdistxypXIpEI()
 	auto ppypXIpEI = yIeJeIe->ppvaluepXIpEI();
 	for (int i = 0; i < 3; i++)
 	{
-		auto ppdistxypXIpEIi = ppdistxypXIpEI->at(i);
-		auto pdistxypXIi = pdistxypXI->at(i);
-		auto ppxpXIpEIi = ppxpXIpEI->at(i);
-		auto ppypXIpEIi = ppypXIpEI->at(i);
-		auto pxpXIi = pxpXI->at(i);
-		auto pypXIi = pypXI->at(i);
+		auto& ppdistxypXIpEIi = ppdistxypXIpEI->at(i);
+		auto& pdistxypXIi = pdistxypXI->at(i);
+		auto& ppxpXIpEIi = ppxpXIpEI->at(i);
+		auto& ppypXIpEIi = ppypXIpEI->at(i);
+		auto& pxpXIi = pxpXI->at(i);
+		auto& pypXIi = pypXI->at(i);
 		for (int j = 0; j < 4; j++)
 		{
-			auto pdistxypEIj = pdistxypEI->at(j);
-			auto pxpEIj = pxpEI->at(j);
-			auto pypEIj = pypEI->at(j);
+			auto& pdistxypEIj = pdistxypEI->at(j);
+			auto& pxpEIj = pxpEI->at(j);
+			auto& pypEIj = pypEI->at(j);
 			auto term1 = -pdistxypXIi * pdistxypEIj;
 			auto term2 = ppxpXIpEIi->at(j) * x + ppypXIpEIi->at(j) * y;
 			auto term3 = pxpXIi * pxpEIj + pypXIi * pypEIj;
@@ -85,12 +85,12 @@ void MbD::DistxyIeqcJec::calc_ppdistxypXIpXI()
 	auto ppypXIpXI = yIeJeIe->ppvaluepXIpXI();
 	for (int i = 0; i < 3; i++)
 	{
-		auto ppdistxypXIpXIi = ppdistxypXIpXI->at(i);
-		auto pdistxypXIi = pdistxypXI->at(i);
-		auto ppxpXIpXIi = ppxpXIpXI->at(i);
-		auto ppypXIpXIi = ppypXIpXI->at(i);
-		auto pxpXIi = pxpXI->at(i);
-		auto pypXIi = pypXI->at(i);
+		auto& ppdistxypXIpXIi = ppdistxypXIpXI->at(i);
+		auto& pdistxypXIi = pdistxypXI->at(i);
+		auto& ppxpXIpXIi = ppxpXIpXI->at(i);
+		auto& ppypXIpXIi = ppypXIpXI->at(i);
+		auto& pxpXIi = pxpXI->at(i);
+		auto& pypXIi = pypXI->at(i);
 		for (int j = i; j < 3; j++)
 		{
 			auto pdistxypXIj = pdistxypXI->at(j);
@@ -134,7 +134,7 @@ void MbD::DistxyIeqcJec::calc_pdistxypXI()
 
 void MbD::DistxyIeqcJec::calcPostDynCorrectorIteration()
 {
-	DistxyIeqcJec::calcPostDynCorrectorIteration();
+	DistxyIecJec::calcPostDynCorrectorIteration();
 	this->calc_pdistxypXI();
 	this->calc_pdistxypEI();
 	this->calc_ppdistxypXIpXI();

MbDCode/EndFramec.cpp.bak

@@ -1,12 +0,0 @@
-#include "EndFramec.h"
-
-using namespace MbD;
-
-EndFramec::EndFramec()
-{
-}
-
-void EndFramec::setMarkerFrame(std::shared_ptr<MarkerFrame> markerFrm)
-{
-	markerFrame = markerFrm;
-}

MbDCode/EndFramec.h.bak

@@ -1,22 +0,0 @@
-#pragma once
-#include "CartesianFrame.h"
-#include "MarkerFrame.h"
-#include "FullColumn.h"
-#include "FullMatrix.h"
-
-namespace MbD {
-    class MarkerFrame;
-
-    class EndFramec : public CartesianFrame
-    {
-        //markerFrame rOeO aAOe 
-    public:
-        EndFramec();
-        void setMarkerFrame(std::shared_ptr<MarkerFrame> markerFrm);
-
-        std::weak_ptr<MarkerFrame> markerFrame;
-        FullColDptr rOeO = std::make_shared<FullColumn<double>>(3);
-        FullMatDptr aAOe = std::make_shared<FullMatrix<double>>(3, 3);
-    };
-}
-

MbDCode/EndFrameqc.cpp.bak

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

MbDCode/EndFrameqc.h.bak

@@ -1,23 +0,0 @@
-#pragma once
-#include "EndFramec.h"
-#include "FullColumn.h"
-#include "FullMatrix.h"
-
-namespace MbD {
-    class EndFramec;
-
-    class EndFrameqc : public EndFramec
-    {
-        //prOeOpE pprOeOpEpE pAOepE ppAOepEpE
-    public:
-        EndFrameqc();
-        FullMatDptr prOeOpE = std::make_shared<FullMatrix<double>>(3, 4);
-        //std::shared_ptr<FullMatrix<std::shared_ptr<FullColumn<double>>>> pprOeOpEpE = std::make_shared<FullMatrix<std::shared_ptr<FullColumn<double>>>>(3, 4);
-        std::shared_ptr<FullColumn<std::shared_ptr<FullMatrix<double>>>> pAOepE = std::make_shared<FullColumn<std::shared_ptr<FullMatrix<double>>>>(4);
-        //std::shared_ptr<FullMatrix<std::shared_ptr<FullMatrix<double>>>> ppAOepEpE = std::make_shared<FullMatrix<std::shared_ptr<FullMatrix<double>>>>(4, 4);
-        //FullMatrix<FullColumn<double>>* pprOeOpEpE1 = new FullMatrix<FullColumn<double>>(3, 4);
-        //FullColumn<FullMatrix<double>>* pAOepE1 = new FullColumn<FullMatrix<double>>(4);
-        //FullMatrix<FullMatrix<double>>* ppAOepEpE1 = new FullMatrix<FullMatrix<double>>(4, 4);
-    };
-}
-

MbDCode/EndFrameqct.cpp

@@ -64,7 +64,7 @@ void EndFrameqct::initprmemptBlks()
 	prmemptBlks = std::make_shared< FullColumn<Symsptr>>(3);
 	for (int i = 0; i < 3; i++) {
 		auto& disp = rmemBlks->at(i);
-		auto var = disp->differentiateWRT(disp, mbdTime);
+		auto var = disp->differentiateWRT(mbdTime);
 		auto vel = var->simplified(var);
 		prmemptBlks->at(i) = vel;
 	}
@@ -76,7 +76,7 @@ void EndFrameqct::initpprmemptptBlks()
 	pprmemptptBlks = std::make_shared< FullColumn<Symsptr>>(3);
 	for (int i = 0; i < 3; i++) {
 		auto& vel = prmemptBlks->at(i);
-		auto var = vel->differentiateWRT(vel, mbdTime);
+		auto var = vel->differentiateWRT(mbdTime);
 		auto acc = var->simplified(var);
 		pprmemptptBlks->at(i) = acc;
 	}
@@ -88,7 +88,7 @@ void EndFrameqct::initpPhiThePsiptBlks()
 	pPhiThePsiptBlks = std::make_shared< FullColumn<Symsptr>>(3);
 	for (int i = 0; i < 3; i++) {
 		auto& angle = phiThePsiBlks->at(i);
-		auto var = angle->differentiateWRT(angle, mbdTime);
+		auto var = angle->differentiateWRT(mbdTime);
 		//std::cout << "var " << *var << std::endl;
 		auto vel = var->simplified(var);
 		//std::cout << "vel " << *vel << std::endl;
@@ -102,7 +102,7 @@ void EndFrameqct::initppPhiThePsiptptBlks()
 	ppPhiThePsiptptBlks = std::make_shared< FullColumn<Symsptr>>(3);
 	for (int i = 0; i < 3; i++) {
 		auto& angleVel = pPhiThePsiptBlks->at(i);
-		auto var = angleVel->differentiateWRT(angleVel, mbdTime);
+		auto var = angleVel->differentiateWRT(mbdTime);
 		auto angleAcc = var->simplified(var);
 		ppPhiThePsiptptBlks->at(i) = angleAcc;
 	}

MbDCode/EulerAngles.cpp

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

MbDCode/EulerAngles.h

@@ -0,0 +1,16 @@
+#pragma once
+
+#include "EulerArray.h"
+
+namespace MbD {
+    template<typename T>
+    class EulerAngles : public EulerArray<T>
+    {
+        //rotOrder cA aA 
+        //Used for user input.
+    public:
+
+
+    };
+}
+

MbDCode/EulerAngleszxz.h

@@ -10,6 +10,7 @@ namespace MbD {
 	class EulerAngleszxz : public EulerArray<T>
 	{
 		//phiA theA psiA aA 
+		//Used by EndFrameqct
 	public:
 		EulerAngleszxz() : EulerArray<T>(3) {}
 		void initialize() override;

MbDCode/EulerConstraint.cpp.bak

@@ -1,12 +0,0 @@
-#include "EulerConstraint.h"
-#include "Constraint.h"
-
-using namespace MbD;
-
-EulerConstraint::EulerConstraint()
-{
-}
-
-EulerConstraint::EulerConstraint(Item* item) : Constraint::Constraint(item)
-{
-}

MbDCode/EulerConstraint.h.bak

@@ -1,20 +0,0 @@
-#pragma once
-#include <memory>
-#include <vector>
-
-#include "Constraint.h"
-#include "FullRow.h"
-
-namespace MbD {
-	class EulerConstraint : public Constraint
-	{
-		//pGpE iqE 
-	public:
-		EulerConstraint();
-		EulerConstraint(Item* item);
-
-		FullRowDptr pGpE = std::make_shared<FullRow<double>>(4);    //partial derivative of G wrt pE
-		int iqE = -1;
-	};
-}
-

MbDCode/EulerParameters.h

@@ -26,6 +26,7 @@ namespace MbD {
 		void calc() override;
 		void calcABC();
 		void calcpApE();
+		void conditionSelf() override;
 
 		FMatDsptr aA;
 		FMatDsptr aB;
@@ -316,5 +317,11 @@ namespace MbD {
 		pAipEk->at(1) = a2E0;
 		pAipEk->at(2) = a2E3;
 	}
+	template<typename T>
+	inline void EulerParameters<T>::conditionSelf()
+	{
+		EulerArray<T>::conditionSelf();
+		this->normalizeSelf();
+	}
 }
 

MbDCode/EulerParametersDot.h

@@ -4,6 +4,7 @@
 #include "FullColumn.h"
 #include "FullMatrix.h"
 #include "EulerParameters.h"
+//#include "CREATE.h" //Cannot use CREATE.h in subclasses of std::vector. Why?
 
 namespace MbD {
 
@@ -32,8 +33,9 @@ namespace MbD {
 	template<typename T>
 	inline std::shared_ptr<EulerParametersDot<T>> EulerParametersDot<T>::FromqEOpAndOmegaOpO(std::shared_ptr<EulerParameters<T>> qEOp, FColDsptr omeOpO)
 	{
+		//auto answer = CREATE<EulerParametersDot<T>>::With(4);	//Cannot use CREATE.h in subclasses of std::vector. Why?
 		auto answer = std::make_shared<EulerParametersDot<T>>(4);
-		answer->initialize();	//Cannot use CREATE.h in subclasses of std::vector. Why?
+		answer->initialize();	
 		qEOp->calcABC();
 		auto aB = qEOp->aB;
 		answer->equalFullColumn(aB->transposeTimesFullColumn(omeOpO->times(0.5)));

MbDCode/ExpressionX.cpp

@@ -1,3 +1,26 @@
 #include "ExpressionX.h"
 
 using namespace MbD;
+
+void MbD::ExpressionX::xexpression(Symsptr arg, Symsptr func)
+{
+	//"
+	//Future modification :
+	//Check that func is a function of arg.
+	//No need for self to be dependent of arg since self is dependent of func which is indirectly
+	//dependent of of arg.
+	//"
+
+	xx = arg;
+	expression = func;
+}
+
+Symsptr MbD::ExpressionX::differentiateWRT(Symsptr var)
+{
+	return expression->differentiateWRT(var);
+}
+
+double MbD::ExpressionX::getValue()
+{
+	return expression->getValue();
+}

MbDCode/ExpressionX.h

@@ -5,6 +5,14 @@
 namespace MbD {
     class ExpressionX : public FunctionX
     {
+        //
+    public:
+
+        void xexpression(Symsptr arg, Symsptr func);
+        Symsptr differentiateWRT(Symsptr var) override;
+        double getValue() override;
+
+        Symsptr expression = std::make_shared<Symbolic>();
     };
 }
 

MbDCode/ExternalSystem.cpp

@@ -1,13 +1,35 @@
 #include "ExternalSystem.h"
+#include "CADSystem.h"
+#include "ASMTAssembly.h"
+#include "System.h"
 
 using namespace MbD;
 
-void MbD::ExternalSystem::preMbDrun()
+void MbD::ExternalSystem::preMbDrun(std::shared_ptr<System> mbdSys)
 {
+	if (cadSystem) {
+		cadSystem->preMbDrun(mbdSys);
+	}
+	else if (asmtAssembly) {
+		asmtAssembly->preMbDrun(mbdSys);
+	}
+	else {
+		assert(false);
+	}
 }
 
 void MbD::ExternalSystem::outputFor(AnalysisType type)
 {
+	if (cadSystem) {
+		cadSystem->updateFromMbD();
+	}
+	else if (asmtAssembly) {
+		asmtAssembly->updateFromMbD();
+		asmtAssembly->compareResults(type);
+	}
+	else {
+		assert(false);
+	}
 }
 
 void MbD::ExternalSystem::logString(std::string& str)

MbDCode/ExternalSystem.h

@@ -10,12 +10,13 @@
 namespace MbD {
 	class CADSystem;
 	class ASMTAssembly;
+	class System;
 
 	class ExternalSystem
 	{
 		//
 	public:
-		void preMbDrun();
+		void preMbDrun(std::shared_ptr<System> mbdSys);
 		void outputFor(AnalysisType type);
 		void logString(std::string& str);
 		void logString(double value);

MbDCode/FullMatrix.cpp.bak

@@ -1,5 +0,0 @@
-#include "FullMatrix.h"
-#include "FullRow.h"
-
-using namespace MbD;
-

MbDCode/FullMatrix.h

@@ -1,14 +1,18 @@
 #pragma once
 
+#include <corecrt_math_defines.h>
 #include <memory>
 
 #include "RowTypeMatrix.h"
+//#include "CREATE.h" //Use forward declaration. //Cannot use CREATE.h in subclasses of std::vector. Why?
 
 namespace MbD {
 	template<typename T>
 	class FullColumn;
 	template<typename T>
 	class FullRow;
+	template<typename T>
+	class EulerParameters;
 
 	template<typename T>
 	class FullMatrix : public RowTypeMatrix<std::shared_ptr<FullRow<T>>>
@@ -62,8 +66,12 @@ namespace MbD {
 		FullMatrix<T> operator+(const FullMatrix<T> fullMat);
 		std::shared_ptr<FullColumn<T>> transposeTimesFullColumn(const std::shared_ptr<FullColumn<T>> fullCol);
 		void magnifySelf(T factor);
-		std::ostream& printOn(std::ostream& s) const override;
+		std::shared_ptr<EulerParameters<T>> asEulerParameters();
+		T trace();
+		double maxMagnitude() override;
+		std::shared_ptr<FullColumn<T>> bryantAngles();
 
+		std::ostream& printOn(std::ostream& s) const override;
 	};
 	template<>
 	inline void FullMatrix<double>::identity() {
@@ -274,6 +282,124 @@ namespace MbD {
 		return s;
 	}
 	template<typename T>
+	inline std::shared_ptr<EulerParameters<T>> FullMatrix<T>::asEulerParameters()
+	{
+		//"Given [A], compute Euler parameter."
+
+		auto traceA = this->trace();
+		T dum = 0.0;
+		T dumSq = 0.0;
+		//auto qE = CREATE<EulerParameters<double>>::With(4); //Cannot use CREATE.h in subclasses of std::vector. Why?
+		auto qE = std::make_shared<EulerParameters<T>>(4);
+		qE->initialize();
+		auto OneMinusTraceDivFour = (1.0 - traceA) / 4.0;
+		for (int i = 0; i < 3; i++)
+		{
+			dumSq = this->at(i)->at(i) / 2.0 + OneMinusTraceDivFour;
+			dum = (dumSq > 0.0) ? std::sqrt(dumSq) : 0.0;
+			qE->atiput(i, dum);
+		}
+		dumSq = (1.0 + traceA) / 4.0;
+		dum = (dumSq > 0.0) ? std::sqrt(dumSq) : 0.0;
+		qE->atiput(3, dum);
+		T max = 0.0;
+		int maxE = -1;
+		for (int i = 0; i < 4; i++)
+		{
+			auto num = qE->at(i);
+			if (max < num) {
+				max = num;
+				maxE = i;
+			}
+		}
+
+		if (maxE == 0) {
+			auto FourE = 4.0 * qE->at(0);
+			qE->atiput(1, (this->at(0)->at(1) + this->at(1)->at(0)) / FourE);
+			qE->atiput(2, (this->at(0)->at(2) + this->at(2)->at(0)) / FourE);
+			qE->atiput(3, (this->at(2)->at(1) - this->at(1)->at(2)) / FourE);
+		}
+		else if (maxE == 1) {
+			auto FourE = 4.0 * qE->at(1);
+			qE->atiput(0, (this->at(0)->at(1) + this->at(1)->at(0)) / FourE);
+			qE->atiput(2, (this->at(1)->at(2) + this->at(2)->at(1)) / FourE);
+			qE->atiput(3, (this->at(0)->at(2) - this->at(2)->at(0)) / FourE);
+		}
+		else if (maxE == 2) {
+			auto FourE = 4.0 * qE->at(2);
+			qE->atiput(0, (this->at(0)->at(2) + this->at(2)->at(0)) / FourE);
+			qE->atiput(1, (this->at(1)->at(2) + this->at(2)->at(1)) / FourE);
+			qE->atiput(3, (this->at(1)->at(0) - this->at(0)->at(1)) / FourE);
+		}
+		else if (maxE == 3) {
+			auto FourE = 4.0 * qE->at(3);
+			qE->atiput(0, (this->at(2)->at(1) - this->at(1)->at(2)) / FourE);
+			qE->atiput(1, (this->at(0)->at(2) - this->at(2)->at(0)) / FourE);
+			qE->atiput(2, (this->at(1)->at(0) - this->at(0)->at(1)) / FourE);
+		}
+		qE->conditionSelf();
+		qE->calc();
+		return qE;
+	}
+	template<typename T>
+	inline T FullMatrix<T>::trace()
+	{
+		T trace = 0.0;
+		for (int i = 0; i < this->size(); i++)
+		{
+			trace += this->at(i)->at(i);
+		}
+		return trace;
+	}
+	template<typename T>
+	inline double FullMatrix<T>::maxMagnitude()
+	{
+		auto max = 0.0;
+		for (int i = 0; i < this->size(); i++)
+		{
+			auto element = this->at(i)->maxMagnitude();
+			if (max < element) max = element;
+		}
+		return max;
+	}
+	template<typename T>
+	inline std::shared_ptr<FullColumn<T>> FullMatrix<T>::bryantAngles()
+	{
+		auto answer = std::make_shared<FullColumn<T>>(3);
+		auto sthe1y = this->at(0)->at(2);
+		T the0x, the1y, the2z, cthe0x, sthe0x, y, x;
+		if (std::abs(sthe1y) > 0.9999) {
+			if (sthe1y > 0.0) {
+				the0x = std::atan2(this->at(1)->at(0), this->at(1)->at(1));
+				the1y = M_PI / 2.0;
+				the2z = 0.0;
+			}
+			else {
+				the0x = std::atan2(this->at(2)->at(1), this->at(2)->at(0));
+				the1y = M_PI / -2.0;
+				the2z = 0.0;
+			}
+		}
+		else {
+			the0x = std::atan2(-this->at(1)->at(2), this->at(2)->at(2));
+			cthe0x = std::cos(the0x);
+			sthe0x = std::sin(the0x);
+			y = sthe1y;
+			if (std::abs(cthe0x) > std::abs(sthe0x)) {
+				x = this->at(2)->at(2) / cthe0x;
+			}
+			else {
+				x = this->at(1)->at(2) / -sthe0x;
+			}
+			the1y = std::atan2(y, x);
+			the2z = std::atan2(-this->at(0)->at(1), this->at(0)->at(0));
+		}
+		answer->atiput(0, the0x);
+		answer->atiput(1, the1y);
+		answer->atiput(2, the2z);
+		return answer;
+	}
+	template<typename T>
 	inline std::shared_ptr<FullColumn<T>> FullMatrix<T>::timesFullColumn(std::shared_ptr<FullColumn<T>> fullCol)
 	{
 		return this->timesFullColumn(fullCol.get());

MbDCode/FullMatrix.h.bak

@@ -1,29 +0,0 @@
-#pragma once
-#include <memory>
-
-#include "RowTypeMatrix.h"
-#include "FullColumn.h"
-#include "FullRow.h"
-
-namespace MbD {
-	//class FullColumn<double>;
-
-	template <typename T>
-	class FullMatrix : public RowTypeMatrix<std::shared_ptr<FullRow<T>>>
-	{
-	public:
-		FullMatrix() {}
-		FullMatrix(int m, int n) {
-			for (int i = 0; i < m; i++) {
-				auto row = std::make_shared<FullRow<T>>(n);
-				this->push_back(row);
-			}
-		}
-		FullMatrix(std::initializer_list<std::initializer_list<T>> list) : RowTypeMatrix<FullRow<T>>{ list } {}
-	};
-
-	typedef std::shared_ptr<FullMatrix<double>> FullMatDptr;
-	//typedef std::shared_ptr<FullMatrix<std::shared_ptr<FullColumn<double>>>> FullMatFCptr;
-	//typedef std::shared_ptr<FullMatrix<std::shared_ptr<FullMatrix<double>>>> FullMatFMptr;
-}
-

MbDCode/FullVector.h

@@ -16,19 +16,20 @@ namespace MbD {
 		FullVector(std::vector<T>::iterator begin, std::vector<T>::iterator end) : Array<T>(begin, end) {}
 		FullVector(std::initializer_list<T> list) : Array<T>{ list } {}
 		double dot(std::shared_ptr<FullVector<T>> vec);
-		void atiput(int i, T value);
 		void atiplusNumber(int i, T value);
 		void atiminusNumber(int i, T value);
 		double sumOfSquares() override;
 		int numberOfElements() override;
 		void zeroSelf() override;
-		void atitimes(int i, double factor);
 		void atiplusFullVector(int i, std::shared_ptr<FullVector<T>> fullVec);
 		void atiplusFullVectortimes(int i, std::shared_ptr<FullVector<T>> fullVec, T factor);
-		double maxMagnitude();
-		double length();
 		void equalSelfPlusFullVectortimes(std::shared_ptr<FullVector<T>> fullVec, T factor);
-		void magnifySelf(T factor);
+		double maxMagnitude() override;
+		void normalizeSelf();
+		double length();
+		virtual void conditionSelf();
+		virtual void conditionSelfWithTol(double tol);
+
 		std::ostream& printOn(std::ostream& s) const override;
 
 	};
@@ -43,11 +44,6 @@ namespace MbD {
 		return answer;
 	}
 	template<typename T>
-	inline void FullVector<T>::atiput(int i, T value)
-	{
-		this->at(i) = value;
-	}
-	template<typename T>
 	inline void FullVector<T>::atiplusNumber(int i, T value)
 	{
 		this->at(i) += value;
@@ -92,11 +88,6 @@ namespace MbD {
 		assert(false);
 	}
 	template<typename T>
-	inline void FullVector<T>::atitimes(int i, double factor)
-	{
-		this->at(i) *= factor;
-	}
-	template<typename T>
 	inline void FullVector<T>::atiplusFullVector(int i1, std::shared_ptr<FullVector<T>> fullVec)
 	{
 		for (int ii = 0; ii < fullVec->size(); ii++)
@@ -114,19 +105,41 @@ namespace MbD {
 			this->at(i) += fullVec->at(ii) * factor;
 		}
 	}
+	template<typename T>
+	inline void FullVector<T>::equalSelfPlusFullVectortimes(std::shared_ptr<FullVector<T>> fullVec, T factor)
+	{
+		for (int i = 0; i < this->size(); i++)
+		{
+			this->atiplusNumber(i, fullVec->at(i) * factor);
+		}
+	}
 	template<>
 	inline double FullVector<double>::maxMagnitude()
 	{
-		auto answer = 0.0;
+		auto max = 0.0;
 		for (int i = 0; i < this->size(); i++)
 		{
-			auto mag = std::abs(this->at(i));
-			if (answer < mag) answer = mag;
+			auto element = this->at(i);
+			if (element < 0.0) element = -element;
+			if (max < element) max = element;
 		}
-		return answer;
+		return max;
+	}
+	template<typename T>
+	inline double FullVector<T>::maxMagnitude()
+	{
+		assert(false);
+		return 0.0;
 	}
 	template<>
-	inline double FullVector<double>::length()
+	inline void FullVector<double>::normalizeSelf()
+	{
+		auto 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;
 		for (int i = 0; i < this->size(); i++)
@@ -137,20 +150,27 @@ namespace MbD {
 		return std::sqrt(ssq);
 	}
 	template<typename T>
-	inline void FullVector<T>::equalSelfPlusFullVectortimes(std::shared_ptr<FullVector<T>> fullVec, T factor)
+	inline void FullVector<T>::conditionSelf()
+	{
+		constexpr auto epsilon = std::numeric_limits<double>::epsilon();
+		auto tol = this->maxMagnitude() * epsilon;
+		this->conditionSelfWithTol(tol);
+	}
+	template<>
+	inline void FullVector<double>::conditionSelfWithTol(double tol)
 	{
 		for (int i = 0; i < this->size(); i++)
 		{
-			this->atiplusNumber(i, fullVec->at(i) * factor);
+			auto element = this->at(i);
+			if (element < 0.0) element = -element;
+			if (element < tol) this->atiput(i, 0.0);
 		}
 	}
 	template<typename T>
-	inline void FullVector<T>::magnifySelf(T factor)
+	inline void FullVector<T>::conditionSelfWithTol(double tol)
 	{
-		for (int i = 0; i < this->size(); i++)
-		{
-			this->atitimes(i, factor);
-		}
+		assert(false);
+		return;
 	}
 	template<typename T>
 	inline std::ostream& FullVector<T>::printOn(std::ostream& s) const

MbDCode/FunctionWithManyArgs.cpp

@@ -34,3 +34,8 @@ std::shared_ptr<std::vector<Symsptr>> FunctionWithManyArgs::getTerms()
 	return terms;
 }
 
+void MbD::FunctionWithManyArgs::createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits)
+{
+	for (auto& term : *terms) term->createMbD(mbdSys, mbdUnits);
+}
+

MbDCode/FunctionWithManyArgs.h

@@ -2,6 +2,8 @@
 
 #include "Function.h"
 #include "Symbolic.h"
+#include "System.h"
+#include "Units.h"
 
 namespace MbD {
 
@@ -15,6 +17,7 @@ namespace MbD {
         FunctionWithManyArgs(Symsptr term, Symsptr term1, Symsptr term2);
         FunctionWithManyArgs(std::shared_ptr<std::vector<Symsptr>> _terms);
         std::shared_ptr<std::vector<Symsptr>> getTerms() override;
+        void createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits) override;
 
         std::shared_ptr<std::vector<Symsptr>> terms;
     };

MbDCode/FunctionX.cpp

@@ -1,3 +1,22 @@
 #include "FunctionX.h"
+#include "Constant.h"
 
 using namespace MbD;
+
+MbD::FunctionX::FunctionX(Symsptr arg) : xx(arg)
+{
+}
+
+Symsptr MbD::FunctionX::differentiateWRT(Symsptr var)
+{
+	if (this == var.get()) return std::make_shared<Constant>(1.0);
+	auto dfdx = differentiateWRTx();
+	auto dxdvar = xx->differentiateWRT(var);
+	return Symbolic::times(dfdx, dxdvar);
+}
+
+Symsptr MbD::FunctionX::differentiateWRTx()
+{
+	assert(false);
+	return Symsptr();
+}

MbDCode/FunctionX.h

@@ -5,6 +5,17 @@
 namespace MbD {
 	class FunctionX : public Function
 	{
+		//
+	public:
+		FunctionX() = default;
+		FunctionX(Symsptr arg);
+		Symsptr differentiateWRT(Symsptr var) override;
+		virtual Symsptr differentiateWRTx();
+
+		Symsptr xx;
+
+
+
 	};
 }
 

MbDCode/FunctionXY.cpp

@@ -0,0 +1,9 @@
+#include "FunctionXY.h"
+
+MbD::FunctionXY::FunctionXY()
+{
+}
+
+MbD::FunctionXY::FunctionXY(Symsptr base, Symsptr exp) : x(base), y(exp)
+{
+}

MbDCode/FunctionXY.h

@@ -0,0 +1,17 @@
+#pragma once
+
+#include "Function.h"
+
+namespace MbD {
+    class FunctionXY : public Function
+    {
+        //x y 
+    public:
+        FunctionXY();
+        FunctionXY(Symsptr base, Symsptr exp);
+
+        Symsptr x, y;
+
+    };
+}
+

MbDCode/GESpMatParPvMarko.cpp

@@ -62,7 +62,7 @@ void GESpMatParPvMarko::doPivoting(int p)
 		matrixA->swapElems(p, rowPivoti);
 		rightHandSideB->swapElems(p, rowPivoti);
 		rowScalings->swapElems(p, rowPivoti);
-		if (aip != std::numeric_limits<double>::min()) rowPositionsOfNonZerosInPivotColumn->at(markowitzPivotColCount - 1) = rowPivoti;
+		if (aip != std::numeric_limits<double>::min()) rowPositionsOfNonZerosInPivotColumn->at((size_t)markowitzPivotColCount - 1) = rowPivoti;
 	}
 	if (max < singularPivotTolerance) throwSingularMatrixError("");
 }

MbDCode/IndependentVariable.cpp

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

MbDCode/IndependentVariable.h

@@ -7,7 +7,7 @@ namespace MbD {
     {
     public:
         IndependentVariable();
-        Symsptr differentiateWRT(Symsptr sptr, Symsptr var) override;
+        Symsptr differentiateWRT(Symsptr var) override;
     };
 }
 

MbDCode/Item.cpp

@@ -298,7 +298,7 @@ void Item::storeDynState()
 {
 }
 
-void MbD::Item::submitToSystem()
+void MbD::Item::submitToSystem(std::shared_ptr<Item> self)
 {
 	assert(false);
 }

MbDCode/Item.h

@@ -133,7 +133,7 @@ namespace MbD {
 		virtual std::shared_ptr<StateData> stateData();
 		virtual void storeCollisionState();
 		virtual void storeDynState();
-		virtual void submitToSystem();
+		virtual void submitToSystem(std::shared_ptr<Item> self);
 		virtual double suggestSmallerOrAcceptCollisionFirstStepSize(double hnew);
 		virtual double suggestSmallerOrAcceptCollisionStepSize(double hnew);
 		virtual double suggestSmallerOrAcceptDynFirstStepSize(double hnew);

MbDCode/Item.h.bak

@@ -1,16 +0,0 @@
-#pragma once
-#include <string>
-
-namespace MbD {
-	class Item
-	{
-	public:
-		Item() {}
-		Item(std::string str) : name(str) {}
-		void setName(std::string& str);
-		const std::string& getName() const;
-	private:
-		std::string name;
-	};
-}
-

MbDCode/Joint.cpp

@@ -10,6 +10,7 @@
 #include "RedundantConstraint.h"
 #include "MarkerFrame.h"
 #include "ForceTorqueData.h"
+#include "System.h"
 
 using namespace MbD;
 
@@ -134,6 +135,11 @@ void Joint::useEquationNumbers()
 	constraintsDo([](std::shared_ptr<Constraint> constraint) { constraint->useEquationNumbers(); });
 }
 
+void MbD::Joint::submitToSystem(std::shared_ptr<Item> self)
+{
+	root()->jointsMotions->push_back(std::static_pointer_cast<Joint>(self));
+}
+
 void Joint::setqsulam(FColDsptr col)
 {
 	constraintsDo([&](std::shared_ptr<Constraint> con) { con->setqsulam(col); });

MbDCode/Joint.cpp.bak

@@ -1,22 +0,0 @@
-#include "Joint.h"
-
-using namespace MbD;
-
-Joint::Joint()
-{
-
-}
-
-void Joint::connectsItoJ(std::shared_ptr<EndFramec> frmi, std::shared_ptr<EndFramec> frmj)
-{
-	frmI = frmi;
-	frmJ = frmj;
-}
-
-void Joint::initializeLocally()
-{
-}
-
-void Joint::initializeGlobally()
-{
-}

MbDCode/Joint.h

@@ -65,6 +65,7 @@ namespace MbD {
 		void setqsulam(FColDsptr col) override;
 		std::shared_ptr<StateData> stateData() override;
 		void useEquationNumbers() override;
+		void submitToSystem(std::shared_ptr<Item> self) override;
 
 		EndFrmcptr frmI;
 		EndFrmcptr frmJ;

MbDCode/Joint.h.bak

@@ -1,28 +0,0 @@
-#pragma once
-#include <memory>
-#include <vector>
-
-#include "Item.h"
-#include "EndFramec.h"
-#include "Constraint.h"
-
-namespace MbD {
-    class EndFramec;
-    class Constraint;
-
-    class Joint : public Item
-    {
-        //frmI frmJ constraints friction 
-    public:
-        Joint();
-        virtual void connectsItoJ(std::shared_ptr<EndFramec> frmI, std::shared_ptr<EndFramec> frmJ);
-        void initializeLocally();
-        void initializeGlobally();
-
-        std::shared_ptr<EndFramec> frmI;
-        std::shared_ptr<EndFramec> frmJ;
-        std::unique_ptr<std::vector<std::shared_ptr<Constraint>>> constraints;
-
-    };
-}
-

MbDCode/LDUSpMatParPvPrecise.cpp

@@ -58,7 +58,7 @@ void LDUSpMatParPvPrecise::doPivoting(int p)
 		rowScalings->swapElems(p, rowPivoti);
 		rowOrder->swapElems(p, rowPivoti);
 		matrixL->swapElems(p, rowPivoti);
-		if (aip != std::numeric_limits<double>::min()) rowPositionsOfNonZerosInPivotColumn->at(markowitzPivotColCount - 1) = rowPivoti;
+		if (aip != std::numeric_limits<double>::min()) rowPositionsOfNonZerosInPivotColumn->at((size_t)markowitzPivotColCount - 1) = rowPivoti;
 	}
 	pivotValues->at(p) = max;
 	if (max < singularPivotTolerance) throwSingularMatrixError("");

MbDCode/Ln.cpp

@@ -0,0 +1,12 @@
+#include "Ln.h"
+
+using namespace MbD;
+
+MbD::Ln::Ln(Symsptr arg) : FunctionX(arg)
+{
+}
+
+double MbD::Ln::getValue()
+{
+    return std::log(xx->getValue());
+}

MbDCode/Ln.h

@@ -0,0 +1,18 @@
+#pragma once
+
+#include "FunctionX.h"
+
+namespace MbD {
+    class Ln : public FunctionX
+    {
+        //
+    public:
+        Ln() = default;
+        Ln(Symsptr arg);
+        double getValue() override;
+
+
+
+    };
+}
+

MbDCode/MarkerFrame.cpp.bak

@@ -1,37 +0,0 @@
-#include "PartFrame.h"
-#include "MarkerFrame.h"
-#include "EndFramec.h"
-#include "EndFrameqc.h"
-
-using namespace MbD;
-
-MarkerFrame::MarkerFrame()
-{
-	auto endFrm = std::make_shared<EndFrameqc>();
-	std::string str = "EndFrame1";
-	endFrm->setName(str);
-	this->addEndFrame(endFrm);
-}
-
-void MarkerFrame::setPartFrame(std::shared_ptr<PartFrame> partFrm)
-{
-	partFrame = partFrm;
-}
-
-std::shared_ptr<PartFrame> MarkerFrame::getPartFrame() {
-	return partFrame.lock();
-}
-
-void MarkerFrame::setrpmp(FullColDptr x)
-{
-	rpmp->copy(x);
-}
-
-void MarkerFrame::setaApm(FullMatDptr x)
-{
-	aApm->copy(x);
-}
-void MarkerFrame::addEndFrame(std::shared_ptr<EndFramec> endFrm)
-{
-	endFrames->push_back(endFrm);
-}

MbDCode/MarkerFrame.h.bak

@@ -1,36 +0,0 @@
-#pragma once
-#include <memory>
-
-#include "CartesianFrame.h"
-#include "PartFrame.h"
-#include "FullColumn.h"
-#include "FullMatrix.h"
-#include "EndFramec.h"
-
-namespace MbD {
-	class PartFrame;
-	class EndFramec;
-
-	class MarkerFrame : public CartesianFrame
-	{
-		//partFrame rpmp aApm rOmO aAOm prOmOpE pAOmpE pprOmOpEpE ppAOmpEpE endFrames 
-	public:
-		MarkerFrame();
-		void setPartFrame(std::shared_ptr<PartFrame> partFrm);
-		std::shared_ptr<PartFrame> getPartFrame();
-		void setrpmp(FullColDptr x);
-		void setaApm(FullMatDptr x);
-		void addEndFrame(std::shared_ptr<EndFramec> x);
-
-		std::weak_ptr<PartFrame> partFrame;
-		FullColDptr rpmp = std::make_shared<FullColumn<double>>(3);
-		FullMatDptr aApm = std::make_shared<FullMatrix<double>>(3, 3);
-		FullColDptr rOmO = std::make_shared<FullColumn<double>>(3);
-		FullMatDptr aAOm = std::make_shared<FullMatrix<double>>(3, 3);
-		FullMatDptr prOmOpE = std::make_shared<FullMatrix<double>>(3, 4);
-		FullColumn<FullMatrix<double>>* pAOmpE = new FullColumn<FullMatrix<double>>(4);
-		std::unique_ptr<std::vector<std::shared_ptr<EndFramec>>> endFrames;
-
-	};
-}
-