diff --git a/MbDCode/ASMTAnimationParameters.h b/MbDCode/ASMTAnimationParameters.h index ac32e2c..73c9b35 100644 --- a/MbDCode/ASMTAnimationParameters.h +++ b/MbDCode/ASMTAnimationParameters.h @@ -9,8 +9,9 @@ namespace MbD { public: void parseASMT(std::vector& lines) override; - int nframe, icurrent, istart, iend, framesPerSecond; - bool isForward; + int nframe = 1000000, icurrent = 0, istart = 0, iend = 1000000, framesPerSecond = 30; + bool isForward = true; + }; } diff --git a/MbDCode/ASMTAssembly.cpp b/MbDCode/ASMTAssembly.cpp index be25d1c..7b46d2f 100644 --- a/MbDCode/ASMTAssembly.cpp +++ b/MbDCode/ASMTAssembly.cpp @@ -1,6 +1,8 @@ #include #include #include +#include +#include #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 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::With(); assembly->parseASMT(lines); + assembly->runKINEMATIC(); } } +ASMTAssembly* MbD::ASMTAssembly::root() +{ + return this; +} + void MbD::ASMTAssembly::parseASMT(std::vector& lines) { readNotes(lines); @@ -39,13 +48,14 @@ void MbD::ASMTAssembly::parseASMT(std::vector& 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& 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& line lines.erase(lines.begin(), it); } -void MbD::ASMTAssembly::readForceTorques(std::vector& lines) +void MbD::ASMTAssembly::readForcesTorques(std::vector& lines) { - assert(lines[0] == "\tForceTorques"); + assert(lines[0] == "\tForceTorques"); //Spelling is not consistent in asmt file. lines.erase(lines.begin()); - forceTorques = std::make_shared>>(); + forcesTorques = std::make_shared>>(); auto it = std::find(lines.begin(), lines.end(), "\tConstantGravity"); - std::vector forceTorquesLines(lines.begin(), it); - while (!forceTorquesLines.empty()) { - if (forceTorquesLines[0] == "\t\tForceTorque") { - forceTorquesLines.erase(forceTorquesLines.begin()); + std::vector forcesTorquesLines(lines.begin(), it); + while (!forcesTorquesLines.empty()) { + if (forcesTorquesLines[0] == "\t\tForceTorque") { + forcesTorquesLines.erase(forcesTorquesLines.begin()); auto forceTorque = CREATE::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& lines) auto it = std::find_if(parts->begin(), parts->end(), [&](const std::shared_ptr& prt) { return prt->fullName("") == seriesName; }); - auto part = *it; + auto& part = *it; part->readPartSeries(lines); } @@ -336,7 +347,7 @@ void MbD::ASMTAssembly::readJointSeries(std::vector& lines) auto it = std::find_if(joints->begin(), joints->end(), [&](const std::shared_ptr& jt) { return jt->fullName("") == seriesName; }); - auto joint = *it; + auto& joint = *it; joint->readJointSeries(lines); } @@ -359,8 +370,275 @@ void MbD::ASMTAssembly::readMotionSeries(std::vector& lines) auto it = std::find_if(motions->begin(), motions->end(), [&](const std::shared_ptr& 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 mbdSys) +{ + calcCharacteristicDimensions(); + deleteMbD(); + createMbD(mbdSys, mbdUnits); + std::static_pointer_cast(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(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>(); + 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>> MbD::ASMTAssembly::connectorList() +{ + auto list = std::make_shared>>(); + 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>> MbD::ASMTAssembly::markerMap() +{ + auto answer = std::make_shared>>(); + 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 mbdSys, std::shared_ptr 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::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::With(); +// principalMassMarker->position3D = refPoint->position3D; +// principalMassMarker->rotationMatrix = refPoint->rotationMatrix; +//} + +void MbD::ASMTAssembly::initprincipalMassMarker() +{ + principalMassMarker = CREATE::With(); + principalMassMarker->mass = 0.0; + principalMassMarker->density = 0.0; + principalMassMarker->momentOfInertias = std::make_shared>(3, 0); + principalMassMarker->position3D = std::make_shared>(3, 0); + principalMassMarker->rotationMatrix = std::make_shared>(3, 3); + principalMassMarker->rotationMatrix->identity(); +} + +std::shared_ptr MbD::ASMTAssembly::spatialContainerAt(std::shared_ptr self, std::string& longname) +{ + if ((self->fullName("")) == longname) return self; + auto it = std::find_if(parts->begin(), parts->end(), [&](const std::shared_ptr& prt) { + return prt->fullName("") == longname; + }); + auto& part = *it; + return part; +} + +std::shared_ptr 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 MbD::ASMTAssembly::jointAt(std::string& longname) +{ + auto it = std::find_if(joints->begin(), joints->end(), [&](const std::shared_ptr& jt) { + return jt->fullName("") == longname; + }); + auto& joint = *it; + return joint; +} + +std::shared_ptr MbD::ASMTAssembly::motionAt(std::string& longname) +{ + auto it = std::find_if(motions->begin(), motions->end(), [&](const std::shared_ptr& mt) { + return mt->fullName("") == longname; + }); + auto& motion = *it; + return motion; +} + +std::shared_ptr MbD::ASMTAssembly::forceTorqueAt(std::string& longname) +{ + auto it = std::find_if(forcesTorques->begin(), forcesTorques->end(), [&](const std::shared_ptr& mt) { + return mt->fullName("") == longname; + }); + auto& forceTorque = *it; + return forceTorque; +} + +FColDsptr MbD::ASMTAssembly::vOcmO() +{ + return std::make_shared>(3, 0.0); +} + +FColDsptr MbD::ASMTAssembly::omeOpO() +{ + return std::make_shared>(3, 0.0); +} + +std::shared_ptr 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); +} + diff --git a/MbDCode/ASMTAssembly.h b/MbDCode/ASMTAssembly.h index 799b16f..f7bb5ac 100644 --- a/MbDCode/ASMTAssembly.h +++ b/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& lines) override; void readNotes(std::vector& lines); void readParts(std::vector& lines); @@ -33,7 +37,7 @@ namespace MbD { void readJoints(std::vector& lines); void readMotions(std::vector& lines); void readGeneralConstraintSets(std::vector& lines); - void readForceTorques(std::vector& lines); + void readForcesTorques(std::vector& lines); void readConstantGravity(std::vector& lines); void readSimulationParameters(std::vector& lines); void readAnimationParameters(std::vector& lines); @@ -47,17 +51,45 @@ namespace MbD { void readMotionSeriesMany(std::vector& lines); void readMotionSeries(std::vector& lines); + void outputFor(AnalysisType type); + void logString(std::string& str); + void logString(double value); + void preMbDrun(std::shared_ptr mbdSys); + void postMbDrun(); + void calcCharacteristicDimensions(); + double calcCharacteristicTime(); + double calcCharacteristicMass(); + double calcCharacteristicLength(); + std::shared_ptr>> connectorList(); + std::shared_ptr>>markerMap(); + void deleteMbD(); + void createMbD(std::shared_ptr mbdSys, std::shared_ptr mbdUnits) override; + void runKINEMATIC(); + void initprincipalMassMarker(); + std::shared_ptr spatialContainerAt(std::shared_ptr self, std::string& longname); + std::shared_ptr markerAt(std::string& longname); + std::shared_ptr jointAt(std::string& longname); + std::shared_ptr motionAt(std::string& longname); + std::shared_ptr forceTorqueAt(std::string& longname); + FColDsptr vOcmO() override; + FColDsptr omeOpO() override; + std::shared_ptr geoTime(); + void updateFromMbD() override; + void compareResults(AnalysisType type) override; + std::string notes; std::shared_ptr>> parts; std::shared_ptr>> kinematicIJs; std::shared_ptr>> constraintSets; std::shared_ptr>> joints; std::shared_ptr>> motions; - std::shared_ptr>> forceTorques; + std::shared_ptr>> forcesTorques; std::shared_ptr constantGravity; std::shared_ptr simulationParameters; std::shared_ptr animationParameters; std::shared_ptr> times; + std::shared_ptr asmtTime = std::make_shared(); + std::shared_ptr mbdUnits; }; } diff --git a/MbDCode/ASMTConstantGravity.cpp b/MbDCode/ASMTConstantGravity.cpp index 655a0c3..90f7597 100644 --- a/MbDCode/ASMTConstantGravity.cpp +++ b/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& lines) g = readColumnOfDoubles(lines[0]); lines.erase(lines.begin()); } + +void MbD::ASMTConstantGravity::createMbD(std::shared_ptr mbdSys, std::shared_ptr mbdUnits) +{ + auto mbdGravity = CREATE::With(); + mbdObject = mbdGravity; + mbdGravity->gXYZ = g->times(1.0 / mbdUnits->acceleration); + mbdSys->addForceTorque(mbdGravity); +} diff --git a/MbDCode/ASMTConstantGravity.h b/MbDCode/ASMTConstantGravity.h index fc3cbfd..f9ee162 100644 --- a/MbDCode/ASMTConstantGravity.h +++ b/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& lines) override; + void createMbD(std::shared_ptr mbdSys, std::shared_ptr mbdUnits) override; FColDsptr g; }; diff --git a/MbDCode/ASMTConstraintSet.cpp b/MbDCode/ASMTConstraintSet.cpp index 7972675..f26ff88 100644 --- a/MbDCode/ASMTConstraintSet.cpp +++ b/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& lines) +void MbD::ASMTConstraintSet::createMbD(std::shared_ptr mbdSys, std::shared_ptr 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(root()->markerAt(markerI)->mbdObject); + auto mrkJ = std::static_pointer_cast(root()->markerAt(markerJ)->mbdObject); + mbdJt->connectsItoJ(mrkI, mrkJ); + mbdSys->addJoint(mbdJt); } -void MbD::ASMTConstraintSet::readMarkerJ(std::vector& lines) +std::shared_ptr 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(); } -void MbD::ASMTConstraintSet::readFXonIs(std::vector& 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(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& 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& lines) -{ - std::string str = lines[0]; - readDoublesInto(str, "FZonI", fzs); - lines.erase(lines.begin()); -} - -void MbD::ASMTConstraintSet::readTXonIs(std::vector& lines) -{ - std::string str = lines[0]; - readDoublesInto(str, "TXonI", txs); - lines.erase(lines.begin()); -} - -void MbD::ASMTConstraintSet::readTYonIs(std::vector& lines) -{ - std::string str = lines[0]; - readDoublesInto(str, "TYonI", tys); - lines.erase(lines.begin()); -} - -void MbD::ASMTConstraintSet::readTZonIs(std::vector& 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)); } diff --git a/MbDCode/ASMTConstraintSet.h b/MbDCode/ASMTConstraintSet.h index 28db835..e7ddb17 100644 --- a/MbDCode/ASMTConstraintSet.h +++ b/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& lines); - void readMarkerJ(std::vector& lines); - void readFXonIs(std::vector& lines); - void readFYonIs(std::vector& lines); - void readFZonIs(std::vector& lines); - void readTXonIs(std::vector& lines); - void readTYonIs(std::vector& lines); - void readTZonIs(std::vector& lines); - - std::string markerI, markerJ; - FRowDsptr fxs, fys, fzs, txs, tys, tzs; + void createMbD(std::shared_ptr mbdSys, std::shared_ptr mbdUnits) override; + virtual std::shared_ptr mbdClassNew(); + void updateFromMbD() override; + void compareResults(AnalysisType type) override; }; } diff --git a/MbDCode/ASMTCylindricalJoint.cpp b/MbDCode/ASMTCylindricalJoint.cpp index 98a9b31..ffe8829 100644 --- a/MbDCode/ASMTCylindricalJoint.cpp +++ b/MbDCode/ASMTCylindricalJoint.cpp @@ -2,3 +2,7 @@ using namespace MbD; +std::shared_ptr MbD::ASMTCylindricalJoint::mbdClassNew() +{ + return CREATE::With(); +} diff --git a/MbDCode/ASMTCylindricalJoint.h b/MbDCode/ASMTCylindricalJoint.h index 00e37e2..6eaea1e 100644 --- a/MbDCode/ASMTCylindricalJoint.h +++ b/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 mbdClassNew() override; }; diff --git a/MbDCode/ASMTForceTorque.cpp b/MbDCode/ASMTForceTorque.cpp index 385f360..dc2711a 100644 --- a/MbDCode/ASMTForceTorque.cpp +++ b/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); +} diff --git a/MbDCode/ASMTForceTorque.h b/MbDCode/ASMTForceTorque.h index d4dead2..1e43298 100644 --- a/MbDCode/ASMTForceTorque.h +++ b/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; }; diff --git a/MbDCode/ASMTItem.cpp b/MbDCode/ASMTItem.cpp index d5b39ce..1192f4f 100644 --- a/MbDCode/ASMTItem.cpp +++ b/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 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 mbdSys, std::shared_ptr mbdUnits) +{ + assert(false); +} + +void MbD::ASMTItem::updateFromMbD() +{ + assert(false); +} + +void MbD::ASMTItem::compareResults(AnalysisType type) +{ + assert(false); +} + +std::shared_ptr MbD::ASMTItem::mbdUnits() +{ + if (owner) { + return owner->mbdUnits(); + } + return static_cast(this)->mbdUnits; +} diff --git a/MbDCode/ASMTItem.h b/MbDCode/ASMTItem.h index 546181d..daec0db 100644 --- a/MbDCode/ASMTItem.h +++ b/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 part(); + virtual void initialize(); virtual void parseASMT(std::vector& 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& 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 mbdSys, std::shared_ptr mbdUnits); + virtual void updateFromMbD(); + virtual void compareResults(AnalysisType type); + std::shared_ptr mbdUnits(); std::string name; ASMTItem* owner; + std::shared_ptr mbdObject; }; } diff --git a/MbDCode/ASMTItemIJ.cpp b/MbDCode/ASMTItemIJ.cpp new file mode 100644 index 0000000..bb6984e --- /dev/null +++ b/MbDCode/ASMTItemIJ.cpp @@ -0,0 +1,69 @@ +#include "ASMTItemIJ.h" + +void MbD::ASMTItemIJ::initialize() +{ + fxs = std::make_shared>(); + fys = std::make_shared>(); + fzs = std::make_shared>(); + txs = std::make_shared>(); + tys = std::make_shared>(); + tzs = std::make_shared>(); +} + +void MbD::ASMTItemIJ::readMarkerI(std::vector& 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& 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& lines) +{ + std::string str = lines[0]; + readDoublesInto(str, "FXonI", infxs); + lines.erase(lines.begin()); +} + +void MbD::ASMTItemIJ::readFYonIs(std::vector& lines) +{ + std::string str = lines[0]; + readDoublesInto(str, "FYonI", infys); + lines.erase(lines.begin()); +} + +void MbD::ASMTItemIJ::readFZonIs(std::vector& lines) +{ + std::string str = lines[0]; + readDoublesInto(str, "FZonI", infzs); + lines.erase(lines.begin()); +} + +void MbD::ASMTItemIJ::readTXonIs(std::vector& lines) +{ + std::string str = lines[0]; + readDoublesInto(str, "TXonI", intxs); + lines.erase(lines.begin()); +} + +void MbD::ASMTItemIJ::readTYonIs(std::vector& lines) +{ + std::string str = lines[0]; + readDoublesInto(str, "TYonI", intys); + lines.erase(lines.begin()); +} + +void MbD::ASMTItemIJ::readTZonIs(std::vector& lines) +{ + std::string str = lines[0]; + readDoublesInto(str, "TZonI", intzs); + lines.erase(lines.begin()); +} diff --git a/MbDCode/ASMTItemIJ.h b/MbDCode/ASMTItemIJ.h new file mode 100644 index 0000000..2fc12d0 --- /dev/null +++ b/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& lines); + void readMarkerJ(std::vector& lines); + void readFXonIs(std::vector& lines); + void readFYonIs(std::vector& lines); + void readFZonIs(std::vector& lines); + void readTXonIs(std::vector& lines); + void readTYonIs(std::vector& lines); + void readTZonIs(std::vector& lines); + + std::string markerI, markerJ; + FRowDsptr fxs, fys, fzs, txs, tys, tzs; + FRowDsptr infxs, infys, infzs, intxs, intys, intzs; + + }; +} + diff --git a/MbDCode/ASMTKinematicIJ.h b/MbDCode/ASMTKinematicIJ.h index 0a86631..733af47 100644 --- a/MbDCode/ASMTKinematicIJ.h +++ b/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: diff --git a/MbDCode/ASMTMarker.cpp b/MbDCode/ASMTMarker.cpp index 6e76a6a..76e6528 100644 --- a/MbDCode/ASMTMarker.cpp +++ b/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& lines) readPosition3D(lines); readRotationMatrix(lines); } + +FColDsptr MbD::ASMTMarker::rpmp() +{ + //p is cm + auto refItem = static_cast(owner); + auto& rPrefP = refItem->position3D; + auto& aAPref = refItem->rotationMatrix; + auto& rrefmref = position3D; + auto rPmP = rPrefP->plusFullColumn(aAPref->timesFullColumn(rrefmref)); + auto& principalMassMarker = static_cast(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(owner); + auto& aAPref = refItem->rotationMatrix; + auto& aArefm = rotationMatrix; + auto& principalMassMarker = static_cast(refItem->owner)->principalMassMarker; + auto& aAPcm = principalMassMarker->rotationMatrix; + auto aApm = aAPcm->transposeTimesFullMatrix(aAPref->timesFullMatrix(aArefm)); + return aApm; +} + +void MbD::ASMTMarker::createMbD(std::shared_ptr mbdSys, std::shared_ptr mbdUnits) +{ + auto mkr = CREATE::With(name.c_str()); + auto prt = std::static_pointer_cast(part()->mbdObject); + prt->partFrame->addMarkerFrame(mkr); + + mkr->rpmp = rpmp()->times(1.0 / mbdUnits->length); + mkr->aApm = aApm(); + mbdObject = mkr->endFrames->at(0); +} diff --git a/MbDCode/ASMTMarker.h b/MbDCode/ASMTMarker.h index 6dcc715..090c889 100644 --- a/MbDCode/ASMTMarker.h +++ b/MbDCode/ASMTMarker.h @@ -10,6 +10,9 @@ namespace MbD { // public: void parseASMT(std::vector& lines) override; + FColDsptr rpmp(); + FMatDsptr aApm(); + void createMbD(std::shared_ptr mbdSys, std::shared_ptr mbdUnits) override; }; } diff --git a/MbDCode/ASMTMotion.cpp b/MbDCode/ASMTMotion.cpp index 491da2f..f09cb8d 100644 --- a/MbDCode/ASMTMotion.cpp +++ b/MbDCode/ASMTMotion.cpp @@ -19,3 +19,7 @@ void MbD::ASMTMotion::readMotionSeries(std::vector& lines) readTYonIs(lines); readTZonIs(lines); } + +void MbD::ASMTMotion::initMarkers() +{ +} diff --git a/MbDCode/ASMTMotion.h b/MbDCode/ASMTMotion.h index 70ec845..4e7570f 100644 --- a/MbDCode/ASMTMotion.h +++ b/MbDCode/ASMTMotion.h @@ -9,6 +9,7 @@ namespace MbD { // public: void readMotionSeries(std::vector& lines); + virtual void initMarkers(); std::shared_ptr>> motionSeries; diff --git a/MbDCode/ASMTPart.cpp b/MbDCode/ASMTPart.cpp index 4e3e276..f6f477b 100644 --- a/MbDCode/ASMTPart.cpp +++ b/MbDCode/ASMTPart.cpp @@ -80,3 +80,17 @@ void MbD::ASMTPart::readPartSeries(std::vector& 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; +} diff --git a/MbDCode/ASMTPart.h b/MbDCode/ASMTPart.h index 12940d5..d9f2064 100644 --- a/MbDCode/ASMTPart.h +++ b/MbDCode/ASMTPart.h @@ -16,8 +16,9 @@ namespace MbD { void readFeatureOrder(std::vector& lines); void readPrincipalMassMarker(std::vector& lines); void readPartSeries(std::vector& lines); + FColDsptr vOcmO() override; + FColDsptr omeOpO() override; - std::shared_ptr principalMassMarker; //std::shared_ptr>> featureOrder; std::shared_ptr>> partSeries; diff --git a/MbDCode/ASMTRefCurve.h b/MbDCode/ASMTRefCurve.h index 45db368..922a895 100644 --- a/MbDCode/ASMTRefCurve.h +++ b/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: diff --git a/MbDCode/ASMTRefItem.cpp b/MbDCode/ASMTRefItem.cpp new file mode 100644 index 0000000..a9fee50 --- /dev/null +++ b/MbDCode/ASMTRefItem.cpp @@ -0,0 +1,29 @@ +#include "ASMTRefItem.h" +#include "CREATE.h" + +using namespace MbD; + +void MbD::ASMTRefItem::readMarkers(std::vector& lines) +{ + assert(lines[0].find("Markers") != std::string::npos); + lines.erase(lines.begin()); + markers = std::make_shared>>(); + auto it = std::find_if(lines.begin(), lines.end(), [](const std::string& s) { + return s.find("RefPoint") != std::string::npos; + }); + std::vector markersLines(lines.begin(), it); + while (!markersLines.empty()) { + readMarker(markersLines); + } + lines.erase(lines.begin(), it); +} + +void MbD::ASMTRefItem::readMarker(std::vector& lines) +{ + assert(lines[0].find("Marker") != std::string::npos); + lines.erase(lines.begin()); + auto marker = CREATE::With(); + marker->parseASMT(lines); + markers->push_back(marker); + marker->owner = this; +} diff --git a/MbDCode/ASMTRefItem.h b/MbDCode/ASMTRefItem.h new file mode 100644 index 0000000..540fc2d --- /dev/null +++ b/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& lines); + void readMarker(std::vector& lines); + + std::shared_ptr>> markers; + + + }; +} + diff --git a/MbDCode/ASMTRefPoint.cpp b/MbDCode/ASMTRefPoint.cpp index bd1b5d3..59144bb 100644 --- a/MbDCode/ASMTRefPoint.cpp +++ b/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& lines) readMarkers(lines); } -void MbD::ASMTRefPoint::readMarkers(std::vector& 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>>(); - auto it = std::find_if(lines.begin(), lines.end(), [](const std::string& s) { - return s.find("RefPoint") != std::string::npos; - }); - std::vector markersLines(lines.begin(), it); - while (!markersLines.empty()) { - readMarker(markersLines); - } - lines.erase(lines.begin(), it); + return owner->fullName(partialName); } -void MbD::ASMTRefPoint::readMarker(std::vector& lines) +void MbD::ASMTRefPoint::createMbD(std::shared_ptr mbdSys, std::shared_ptr mbdUnits) { - assert(lines[0].find("Marker") != std::string::npos); - lines.erase(lines.begin()); - auto marker = CREATE::With(); - marker->parseASMT(lines); - markers->push_back(marker); - marker->owner = this; + for (auto& marker : *markers) { + marker->createMbD(mbdSys, mbdUnits); + } } diff --git a/MbDCode/ASMTRefPoint.h b/MbDCode/ASMTRefPoint.h index 9de3c9e..388f74a 100644 --- a/MbDCode/ASMTRefPoint.h +++ b/MbDCode/ASMTRefPoint.h @@ -1,20 +1,19 @@ #pragma once -#include "ASMTSpatialItem.h" +#include "ASMTRefItem.h" #include #include -#include "ASMTMarker.h" namespace MbD { - class ASMTRefPoint : public ASMTSpatialItem + class ASMTRefPoint : public ASMTRefItem { // public: void parseASMT(std::vector& lines) override; - void readMarkers(std::vector& lines); - void readMarker(std::vector& lines); + std::string fullName(std::string partialName) override; + void createMbD(std::shared_ptr mbdSys, std::shared_ptr mbdUnits) override; - std::shared_ptr>> markers; + }; } diff --git a/MbDCode/ASMTRefSurface.h b/MbDCode/ASMTRefSurface.h index 0b5085e..7fe16e4 100644 --- a/MbDCode/ASMTRefSurface.h +++ b/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: diff --git a/MbDCode/ASMTRevoluteJoint.cpp b/MbDCode/ASMTRevoluteJoint.cpp index ce18b81..2f47f26 100644 --- a/MbDCode/ASMTRevoluteJoint.cpp +++ b/MbDCode/ASMTRevoluteJoint.cpp @@ -1,3 +1,8 @@ #include "ASMTRevoluteJoint.h" using namespace MbD; + +std::shared_ptr MbD::ASMTRevoluteJoint::mbdClassNew() +{ + return CREATE::With(); +} diff --git a/MbDCode/ASMTRevoluteJoint.h b/MbDCode/ASMTRevoluteJoint.h index af61164..b20aaf1 100644 --- a/MbDCode/ASMTRevoluteJoint.h +++ b/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 mbdClassNew() override; }; } diff --git a/MbDCode/ASMTRotationalMotion.cpp b/MbDCode/ASMTRotationalMotion.cpp index ff4a4b4..e94dff7 100644 --- a/MbDCode/ASMTRotationalMotion.cpp +++ b/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& 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 mbdSys, std::shared_ptr mbdUnits) +{ + ASMTMotion::createMbD(mbdSys, mbdUnits); + auto parser = CREATE::With(); + parser->owner = this; + auto geoTime = owner->root()->geoTime(); + parser->variables->insert(std::make_pair("time", geoTime)); + auto userFunc = CREATE::With(rotationZ, 1.0); + parser->parseUserFunction(userFunc); + auto geoPhi = parser->stack->top(); + geoPhi = Symbolic::times(geoPhi, std::make_shared(1.0 / mbdUnits->angle)); + geoPhi->createMbD(mbdSys, mbdUnits); + std::static_pointer_cast(mbdObject)->phiBlk = geoPhi->simplified(geoPhi); +} + +std::shared_ptr MbD::ASMTRotationalMotion::mbdClassNew() +{ + return CREATE::With(); +} diff --git a/MbDCode/ASMTRotationalMotion.h b/MbDCode/ASMTRotationalMotion.h index 88d4cf8..7a65e83 100644 --- a/MbDCode/ASMTRotationalMotion.h +++ b/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& lines) override; void readMotionJoint(std::vector& lines); void readRotationZ(std::vector& lines); + void initMarkers() override; + void createMbD(std::shared_ptr mbdSys, std::shared_ptr mbdUnits) override; + std::shared_ptr mbdClassNew() override; std::string motionJoint, rotationZ; }; diff --git a/MbDCode/ASMTSimulationParameters.h b/MbDCode/ASMTSimulationParameters.h index 3b6f333..e96a11d 100644 --- a/MbDCode/ASMTSimulationParameters.h +++ b/MbDCode/ASMTSimulationParameters.h @@ -3,14 +3,16 @@ #include "ASMTItem.h" namespace MbD { - class ASMTSimulationParameters : public ASMTItem - { - // - public: - void parseASMT(std::vector& lines) override; + class ASMTSimulationParameters : public ASMTItem + { + // + public: + void parseASMT(std::vector& 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; + }; } diff --git a/MbDCode/ASMTSpatialContainer.cpp b/MbDCode/ASMTSpatialContainer.cpp index b1cd641..3e98cdf 100644 --- a/MbDCode/ASMTSpatialContainer.cpp +++ b/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>(); + ys = std::make_shared>(); + zs = std::make_shared>(); + bryxs = std::make_shared>(); + bryys = std::make_shared>(); + bryzs = std::make_shared>(); + vxs = std::make_shared>(); + vys = std::make_shared>(); + vzs = std::make_shared>(); + omexs = std::make_shared>(); + omeys = std::make_shared>(); + omezs = std::make_shared>(); + axs = std::make_shared>(); + ays = std::make_shared>(); + azs = std::make_shared>(); + alpxs = std::make_shared>(); + alpys = std::make_shared>(); + alpzs = std::make_shared>(); +} void MbD::ASMTSpatialContainer::readRefPoints(std::vector& lines) { @@ -68,125 +95,254 @@ void MbD::ASMTSpatialContainer::readRefSurface(std::vector& lines) void MbD::ASMTSpatialContainer::readXs(std::vector& lines) { std::string str = lines[0]; - readDoublesInto(str, "X", xs); + readDoublesInto(str, "X", inxs); lines.erase(lines.begin()); } void MbD::ASMTSpatialContainer::readYs(std::vector& lines) { std::string str = lines[0]; - readDoublesInto(str, "Y", ys); + readDoublesInto(str, "Y", inys); lines.erase(lines.begin()); } void MbD::ASMTSpatialContainer::readZs(std::vector& lines) { std::string str = lines[0]; - readDoublesInto(str, "Z", zs); + readDoublesInto(str, "Z", inzs); lines.erase(lines.begin()); } void MbD::ASMTSpatialContainer::readBryantxs(std::vector& lines) { std::string str = lines[0]; - readDoublesInto(str, "Bryantx", bryxs); + readDoublesInto(str, "Bryantx", inbryxs); lines.erase(lines.begin()); } void MbD::ASMTSpatialContainer::readBryantys(std::vector& lines) { std::string str = lines[0]; - readDoublesInto(str, "Bryanty", bryys); + readDoublesInto(str, "Bryanty", inbryys); lines.erase(lines.begin()); } void MbD::ASMTSpatialContainer::readBryantzs(std::vector& lines) { std::string str = lines[0]; - readDoublesInto(str, "Bryantz", bryzs); + readDoublesInto(str, "Bryantz", inbryzs); lines.erase(lines.begin()); } void MbD::ASMTSpatialContainer::readVXs(std::vector& lines) { std::string str = lines[0]; - readDoublesInto(str, "VX", vxs); + readDoublesInto(str, "VX", invxs); lines.erase(lines.begin()); } void MbD::ASMTSpatialContainer::readVYs(std::vector& lines) { std::string str = lines[0]; - readDoublesInto(str, "VY", vys); + readDoublesInto(str, "VY", invys); lines.erase(lines.begin()); } void MbD::ASMTSpatialContainer::readVZs(std::vector& lines) { std::string str = lines[0]; - readDoublesInto(str, "VZ", vzs); + readDoublesInto(str, "VZ", invzs); lines.erase(lines.begin()); } void MbD::ASMTSpatialContainer::readOmegaXs(std::vector& lines) { std::string str = lines[0]; - readDoublesInto(str, "OmegaX", omexs); + readDoublesInto(str, "OmegaX", inomexs); lines.erase(lines.begin()); } void MbD::ASMTSpatialContainer::readOmegaYs(std::vector& lines) { std::string str = lines[0]; - readDoublesInto(str, "OmegaY", omeys); + readDoublesInto(str, "OmegaY", inomeys); lines.erase(lines.begin()); } void MbD::ASMTSpatialContainer::readOmegaZs(std::vector& lines) { std::string str = lines[0]; - readDoublesInto(str, "OmegaZ", omezs); + readDoublesInto(str, "OmegaZ", inomezs); lines.erase(lines.begin()); } void MbD::ASMTSpatialContainer::readAXs(std::vector& lines) { std::string str = lines[0]; - readDoublesInto(str, "AX", axs); + readDoublesInto(str, "AX", inaxs); lines.erase(lines.begin()); } void MbD::ASMTSpatialContainer::readAYs(std::vector& lines) { std::string str = lines[0]; - readDoublesInto(str, "AY", ays); + readDoublesInto(str, "AY", inays); lines.erase(lines.begin()); } void MbD::ASMTSpatialContainer::readAZs(std::vector& lines) { std::string str = lines[0]; - readDoublesInto(str, "AZ", azs); + readDoublesInto(str, "AZ", inazs); lines.erase(lines.begin()); } void MbD::ASMTSpatialContainer::readAlphaXs(std::vector& lines) { std::string str = lines[0]; - readDoublesInto(str, "AlphaX", alpxs); + readDoublesInto(str, "AlphaX", inalpxs); lines.erase(lines.begin()); } void MbD::ASMTSpatialContainer::readAlphaYs(std::vector& lines) { std::string str = lines[0]; - readDoublesInto(str, "AlphaY", alpys); + readDoublesInto(str, "AlphaY", inalpys); lines.erase(lines.begin()); } void MbD::ASMTSpatialContainer::readAlphaZs(std::vector& 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 mbdSys, std::shared_ptr mbdUnits) +{ + auto mbdPart = CREATE::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>(3, 0)); + mbdPart->qEddot(std::make_shared>(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> 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 MbD::ASMTSpatialContainer::part() +{ + return std::make_shared(*this); +} + +void MbD::ASMTSpatialContainer::updateFromMbD() +{ + auto mbdUnts = mbdUnits(); + auto mbdPart = std::static_pointer_cast(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)); +} diff --git a/MbDCode/ASMTSpatialContainer.h b/MbDCode/ASMTSpatialContainer.h index 8af5521..9175fa9 100644 --- a/MbDCode/ASMTSpatialContainer.h +++ b/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& lines); void readRefPoint(std::vector& lines); void readRefCurves(std::vector& lines); @@ -34,6 +37,14 @@ namespace MbD { void readAlphaXs(std::vector& lines); void readAlphaYs(std::vector& lines); void readAlphaZs(std::vector& lines); + void createMbD(std::shared_ptr mbdSys, std::shared_ptr mbdUnits) override; + FColDsptr rOcmO(); + std::shared_ptr> qEp(); + virtual FColDsptr vOcmO(); + virtual FColDsptr omeOpO(); + std::shared_ptr part() override; + void updateFromMbD() override; + void compareResults(AnalysisType type) override; std::shared_ptr>> refPoints; std::shared_ptr>> 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 principalMassMarker; }; } diff --git a/MbDCode/ASMTTime.cpp b/MbDCode/ASMTTime.cpp new file mode 100644 index 0000000..312b3de --- /dev/null +++ b/MbDCode/ASMTTime.cpp @@ -0,0 +1,23 @@ +#include + +#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 mbdSys, std::shared_ptr mbdUnits) +{ + auto mbdTime = mbdSys->time; + if (xx == mbdTime) return; + auto timeScale = std::make_shared(mbdUnits->time); + auto geoTime = std::make_shared(timeScale, mbdTime); + this->xexpression(mbdTime, geoTime->simplified(geoTime)); +} diff --git a/MbDCode/ASMTTime.h b/MbDCode/ASMTTime.h new file mode 100644 index 0000000..d319f0e --- /dev/null +++ b/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 mbdSys, std::shared_ptr mbdUnits); + + }; +} + diff --git a/MbDCode/ASMTTranslationalMotion.cpp b/MbDCode/ASMTTranslationalMotion.cpp index e3d6a9b..94efd45 100644 --- a/MbDCode/ASMTTranslationalMotion.cpp +++ b/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& 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 mbdSys, std::shared_ptr 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 MbD::ASMTTranslationalMotion::mbdClassNew() +{ + return CREATE::With(); +} diff --git a/MbDCode/ASMTTranslationalMotion.h b/MbDCode/ASMTTranslationalMotion.h index 359fc82..3347489 100644 --- a/MbDCode/ASMTTranslationalMotion.h +++ b/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& lines) override; + void initMarkers() override; + void createMbD(std::shared_ptr mbdSys, std::shared_ptr mbdUnits) override; + std::shared_ptr mbdClassNew() override; std::string motionJoint, translationZ; diff --git a/MbDCode/Abs.cpp b/MbDCode/Abs.cpp new file mode 100644 index 0000000..baa92b8 --- /dev/null +++ b/MbDCode/Abs.cpp @@ -0,0 +1,6 @@ +#include "Abs.h" + +double MbD::Abs::getValue() +{ + return std::abs(xx->getValue()); +} diff --git a/MbDCode/Abs.h b/MbDCode/Abs.h new file mode 100644 index 0000000..af6fc3f --- /dev/null +++ b/MbDCode/Abs.h @@ -0,0 +1,15 @@ +#pragma once + +#include "FunctionX.h" + +namespace MbD { + class Abs : public FunctionX + { + // + public: + double getValue() override; + + + }; +} + diff --git a/MbDCode/AbsConstraint.cpp b/MbDCode/AbsConstraint.cpp index de4f8be..0b858b2 100644 --- a/MbDCode/AbsConstraint.cpp +++ b/MbDCode/AbsConstraint.cpp @@ -15,10 +15,10 @@ AbsConstraint::AbsConstraint(int i) void AbsConstraint::calcPostDynCorrectorIteration() { if (axis < 3) { - aG = static_cast(owner)->qX->at(axis); + aG = static_cast(owner)->qX->at((size_t)axis); } else { - aG = static_cast(owner)->qE->at(axis - 3); + aG = static_cast(owner)->qE->at((size_t)axis - 3); } } @@ -55,10 +55,10 @@ void AbsConstraint::fillAccICIterError(FColDsptr col) auto partFrame = static_cast(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); } diff --git a/MbDCode/ArcTan.cpp b/MbDCode/ArcTan.cpp new file mode 100644 index 0000000..372b10e --- /dev/null +++ b/MbDCode/ArcTan.cpp @@ -0,0 +1,6 @@ +#include "ArcTan.h" + +double MbD::ArcTan::getValue() +{ + return std::atan(xx->getValue()); +} diff --git a/MbDCode/ArcTan.h b/MbDCode/ArcTan.h new file mode 100644 index 0000000..7595e96 --- /dev/null +++ b/MbDCode/ArcTan.h @@ -0,0 +1,15 @@ +#pragma once + +#include "FunctionX.h" + +namespace MbD { + class ArcTan : public FunctionX + { + // + public: + double getValue() override; + + + }; +} + diff --git a/MbDCode/Array.h b/MbDCode/Array.h index 4cf7126..d836537 100644 --- a/MbDCode/Array.h +++ b/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, 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 - //inline double Array::maxMagnitude() + //template<> + //inline double Array::maxMagnitude() //{ - // if (std::is_arithmetic::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 inline double Array::maxMagnitudeOfVector() @@ -111,6 +114,59 @@ namespace MbD { this->at(ii) = array->at(i + ii); } } + //template<> + //inline void Array::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::conditionSelf() + //{ + // constexpr auto epsilon = std::numeric_limits::epsilon(); + // auto tol = maxMagnitude() * epsilon; + // conditionSelfWithTol(tol); + //} + //template<> + //inline void Array::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 + inline void Array::atiput(int i, T value) + { + this->at(i) = value; + } + //template<> + //inline double Array::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 + inline void Array::magnifySelf(T factor) + { + for (int i = 0; i < this->size(); i++) + { + this->atitimes(i, factor); + } + } + template + inline void Array::atitimes(int i, double factor) + { + this->at(i) *= factor; + } using ListD = std::initializer_list; using ListListD = std::initializer_list>; using ListListPairD = std::initializer_list>>; diff --git a/MbDCode/BasicUserFunction.cpp b/MbDCode/BasicUserFunction.cpp index 3f4254d..a0e9978 100644 --- a/MbDCode/BasicUserFunction.cpp +++ b/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::With(); +} diff --git a/MbDCode/BasicUserFunction.h b/MbDCode/BasicUserFunction.h index 77ba97f..387475c 100644 --- a/MbDCode/BasicUserFunction.h +++ b/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; }; } diff --git a/MbDCode/CADSystem.cpp b/MbDCode/CADSystem.cpp index 3216561..5d30ad5 100644 --- a/MbDCode/CADSystem.cpp +++ b/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 mbdSys) +{ } void CADSystem::postMbDrun() { } + +void MbD::CADSystem::updateFromMbD() +{ +} diff --git a/MbDCode/CADSystem.h b/MbDCode/CADSystem.h index 65ba431..f43e17a 100644 --- a/MbDCode/CADSystem.h +++ b/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 mbdSys); void postMbDrun(); + void updateFromMbD(); std::shared_ptr mbdSystem = std::make_shared(); diff --git a/MbDCode/CREATE.h b/MbDCode/CREATE.h index fdf9811..a4e1490 100644 --- a/MbDCode/CREATE.h +++ b/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 With(const std::string& expr, double unit) { + auto inst = std::make_shared(expr, unit); + inst->initialize(); + return inst; + } static std::shared_ptr With() { auto inst = std::make_shared(); inst->initialize(); diff --git a/MbDCode/Constant.cpp b/MbDCode/Constant.cpp index d11c445..8f0153f 100644 --- a/MbDCode/Constant.cpp +++ b/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(0.0); } @@ -20,6 +22,36 @@ bool Constant::isConstant() return true; } +Symsptr MbD::Constant::expandUntil(std::shared_ptr> set) +{ + return clonesptr(); +} + +Symsptr MbD::Constant::clonesptr() +{ + return std::make_shared(*this); +} + +bool MbD::Constant::isZero() +{ + return value == 0.0; +} + +bool MbD::Constant::isOne() +{ + return value == 1.0; +} + +void MbD::Constant::createMbD(std::shared_ptr mbdSys, std::shared_ptr mbdUnits) +{ + return; +} + +double MbD::Constant::getValue() +{ + return value; +} + std::ostream& Constant::printOn(std::ostream& s) const { return s << this->value; diff --git a/MbDCode/Constant.h b/MbDCode/Constant.h index bd007ba..b5e7a53 100644 --- a/MbDCode/Constant.h +++ b/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> set) override; + Symsptr clonesptr() override; + bool isZero() override; + bool isOne() override; + void createMbD(std::shared_ptr mbdSys, std::shared_ptr mbdUnits) override; + double getValue() override; + std::ostream& printOn(std::ostream& s) const override; }; } diff --git a/MbDCode/ConstantGravity.cpp b/MbDCode/ConstantGravity.cpp new file mode 100644 index 0000000..d8e4718 --- /dev/null +++ b/MbDCode/ConstantGravity.cpp @@ -0,0 +1,9 @@ +#include "ConstantGravity.h" +#include "System.h" + +using namespace MbD; + +void MbD::ConstantGravity::submitToSystem(std::shared_ptr self) +{ + root()->forcesTorques->push_back(std::static_pointer_cast(self)); +} diff --git a/MbDCode/ConstantGravity.h b/MbDCode/ConstantGravity.h new file mode 100644 index 0000000..501f869 --- /dev/null +++ b/MbDCode/ConstantGravity.h @@ -0,0 +1,15 @@ +#pragma once + +#include "ForceTorqueItem.h" + +namespace MbD { + class ConstantGravity : public ForceTorqueItem + { + // + public: + void submitToSystem(std::shared_ptr self) override; + + FColDsptr gXYZ; + }; +} + diff --git a/MbDCode/Constraint.h.bak b/MbDCode/Constraint.h.bak deleted file mode 100644 index e0911ed..0000000 --- a/MbDCode/Constraint.h.bak +++ /dev/null @@ -1,24 +0,0 @@ -#pragma once -#include - -#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 - }; -} - diff --git a/MbDCode/CylindricalJoint.cpp.bak b/MbDCode/CylindricalJoint.cpp.bak deleted file mode 100644 index ae20cdd..0000000 --- a/MbDCode/CylindricalJoint.cpp.bak +++ /dev/null @@ -1 +0,0 @@ -#include "CylindricalJoint.h" diff --git a/MbDCode/CylindricalJoint.h.bak b/MbDCode/CylindricalJoint.h.bak deleted file mode 100644 index 0280eb7..0000000 --- a/MbDCode/CylindricalJoint.h.bak +++ /dev/null @@ -1,10 +0,0 @@ -#pragma once -#include "Joint.h" - -namespace MbD { - class CylindricalJoint : public Joint - { - public: - }; -} - diff --git a/MbDCode/DiagonalMatrix.h b/MbDCode/DiagonalMatrix.h index 82a7820..918b6a4 100644 --- a/MbDCode/DiagonalMatrix.h +++ b/MbDCode/DiagonalMatrix.h @@ -13,29 +13,25 @@ namespace MbD { DiagonalMatrix(int count) : Array(count) {} DiagonalMatrix(int count, const T& value) : Array(count, value) {} DiagonalMatrix(std::initializer_list list) : Array{ list } {} - void atiput(int i, T value); void atiputDiagonalMatrix(int i, std::shared_ptr < DiagonalMatrix> diagMat); std::shared_ptr> times(T factor); std::shared_ptr> timesFullColumn(std::shared_ptr> fullCol); std::shared_ptr> timesFullMatrix(std::shared_ptr> 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 - inline void DiagonalMatrix::atiput(int i, T value) - { - this->at(i) = value; - } - template inline void DiagonalMatrix::atiputDiagonalMatrix(int i, std::shared_ptr> 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>(nrow); for (int i = 0; i < nrow; i++) { @@ -92,7 +88,7 @@ namespace MbD { template inline int DiagonalMatrix::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::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 + inline double DiagonalMatrix::maxMagnitude() + { + assert(false); + return 0.0; + } template inline std::ostream& DiagonalMatrix::printOn(std::ostream& s) const { diff --git a/MbDCode/DistxyIeqcJec.cpp b/MbDCode/DistxyIeqcJec.cpp index f680b33..a0f2a3d 100644 --- a/MbDCode/DistxyIeqcJec.cpp +++ b/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(); diff --git a/MbDCode/EndFramec.cpp.bak b/MbDCode/EndFramec.cpp.bak deleted file mode 100644 index ac59cbc..0000000 --- a/MbDCode/EndFramec.cpp.bak +++ /dev/null @@ -1,12 +0,0 @@ -#include "EndFramec.h" - -using namespace MbD; - -EndFramec::EndFramec() -{ -} - -void EndFramec::setMarkerFrame(std::shared_ptr markerFrm) -{ - markerFrame = markerFrm; -} diff --git a/MbDCode/EndFramec.h.bak b/MbDCode/EndFramec.h.bak deleted file mode 100644 index efe2e32..0000000 --- a/MbDCode/EndFramec.h.bak +++ /dev/null @@ -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 markerFrm); - - std::weak_ptr markerFrame; - FullColDptr rOeO = std::make_shared>(3); - FullMatDptr aAOe = std::make_shared>(3, 3); - }; -} - diff --git a/MbDCode/EndFrameqc.cpp.bak b/MbDCode/EndFrameqc.cpp.bak deleted file mode 100644 index b84300c..0000000 --- a/MbDCode/EndFrameqc.cpp.bak +++ /dev/null @@ -1,7 +0,0 @@ -#include "EndFrameqc.h" - -using namespace MbD; - -EndFrameqc::EndFrameqc() : EndFramec() -{ -} diff --git a/MbDCode/EndFrameqc.h.bak b/MbDCode/EndFrameqc.h.bak deleted file mode 100644 index d4d41e0..0000000 --- a/MbDCode/EndFrameqc.h.bak +++ /dev/null @@ -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>(3, 4); - //std::shared_ptr>>> pprOeOpEpE = std::make_shared>>>(3, 4); - std::shared_ptr>>> pAOepE = std::make_shared>>>(4); - //std::shared_ptr>>> ppAOepEpE = std::make_shared>>>(4, 4); - //FullMatrix>* pprOeOpEpE1 = new FullMatrix>(3, 4); - //FullColumn>* pAOepE1 = new FullColumn>(4); - //FullMatrix>* ppAOepEpE1 = new FullMatrix>(4, 4); - }; -} - diff --git a/MbDCode/EndFrameqct.cpp b/MbDCode/EndFrameqct.cpp index 3cfc402..cb35f1b 100644 --- a/MbDCode/EndFrameqct.cpp +++ b/MbDCode/EndFrameqct.cpp @@ -64,7 +64,7 @@ void EndFrameqct::initprmemptBlks() prmemptBlks = std::make_shared< FullColumn>(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>(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>(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>(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; } diff --git a/MbDCode/EulerAngles.cpp b/MbDCode/EulerAngles.cpp new file mode 100644 index 0000000..f946a98 --- /dev/null +++ b/MbDCode/EulerAngles.cpp @@ -0,0 +1 @@ +#include "EulerAngles.h" diff --git a/MbDCode/EulerAngles.h b/MbDCode/EulerAngles.h new file mode 100644 index 0000000..576bdc8 --- /dev/null +++ b/MbDCode/EulerAngles.h @@ -0,0 +1,16 @@ +#pragma once + +#include "EulerArray.h" + +namespace MbD { + template + class EulerAngles : public EulerArray + { + //rotOrder cA aA + //Used for user input. + public: + + + }; +} + diff --git a/MbDCode/EulerAngleszxz.h b/MbDCode/EulerAngleszxz.h index 17a8830..66ffad9 100644 --- a/MbDCode/EulerAngleszxz.h +++ b/MbDCode/EulerAngleszxz.h @@ -10,6 +10,7 @@ namespace MbD { class EulerAngleszxz : public EulerArray { //phiA theA psiA aA + //Used by EndFrameqct public: EulerAngleszxz() : EulerArray(3) {} void initialize() override; diff --git a/MbDCode/EulerConstraint.cpp.bak b/MbDCode/EulerConstraint.cpp.bak deleted file mode 100644 index f08225b..0000000 --- a/MbDCode/EulerConstraint.cpp.bak +++ /dev/null @@ -1,12 +0,0 @@ -#include "EulerConstraint.h" -#include "Constraint.h" - -using namespace MbD; - -EulerConstraint::EulerConstraint() -{ -} - -EulerConstraint::EulerConstraint(Item* item) : Constraint::Constraint(item) -{ -} diff --git a/MbDCode/EulerConstraint.h.bak b/MbDCode/EulerConstraint.h.bak deleted file mode 100644 index c40dbf8..0000000 --- a/MbDCode/EulerConstraint.h.bak +++ /dev/null @@ -1,20 +0,0 @@ -#pragma once -#include -#include - -#include "Constraint.h" -#include "FullRow.h" - -namespace MbD { - class EulerConstraint : public Constraint - { - //pGpE iqE - public: - EulerConstraint(); - EulerConstraint(Item* item); - - FullRowDptr pGpE = std::make_shared>(4); //partial derivative of G wrt pE - int iqE = -1; - }; -} - diff --git a/MbDCode/EulerParameters.h b/MbDCode/EulerParameters.h index fb56a27..57ddf1b 100644 --- a/MbDCode/EulerParameters.h +++ b/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 + inline void EulerParameters::conditionSelf() + { + EulerArray::conditionSelf(); + this->normalizeSelf(); + } } diff --git a/MbDCode/EulerParametersDot.h b/MbDCode/EulerParametersDot.h index 3499c74..2edde3a 100644 --- a/MbDCode/EulerParametersDot.h +++ b/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 inline std::shared_ptr> EulerParametersDot::FromqEOpAndOmegaOpO(std::shared_ptr> qEOp, FColDsptr omeOpO) { + //auto answer = CREATE>::With(4); //Cannot use CREATE.h in subclasses of std::vector. Why? auto answer = std::make_shared>(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))); diff --git a/MbDCode/ExpressionX.cpp b/MbDCode/ExpressionX.cpp index b3214a9..9ee2e33 100644 --- a/MbDCode/ExpressionX.cpp +++ b/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(); +} diff --git a/MbDCode/ExpressionX.h b/MbDCode/ExpressionX.h index 0a7e4d0..5bc43f5 100644 --- a/MbDCode/ExpressionX.h +++ b/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(); }; } diff --git a/MbDCode/ExternalSystem.cpp b/MbDCode/ExternalSystem.cpp index 52310e9..c4b2f57 100644 --- a/MbDCode/ExternalSystem.cpp +++ b/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 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) diff --git a/MbDCode/ExternalSystem.h b/MbDCode/ExternalSystem.h index c04b449..4e422a1 100644 --- a/MbDCode/ExternalSystem.h +++ b/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 mbdSys); void outputFor(AnalysisType type); void logString(std::string& str); void logString(double value); diff --git a/MbDCode/FullMatrix.cpp.bak b/MbDCode/FullMatrix.cpp.bak deleted file mode 100644 index e74852e..0000000 --- a/MbDCode/FullMatrix.cpp.bak +++ /dev/null @@ -1,5 +0,0 @@ -#include "FullMatrix.h" -#include "FullRow.h" - -using namespace MbD; - diff --git a/MbDCode/FullMatrix.h b/MbDCode/FullMatrix.h index 89e6400..d2d7696 100644 --- a/MbDCode/FullMatrix.h +++ b/MbDCode/FullMatrix.h @@ -1,14 +1,18 @@ #pragma once +#include #include #include "RowTypeMatrix.h" +//#include "CREATE.h" //Use forward declaration. //Cannot use CREATE.h in subclasses of std::vector. Why? namespace MbD { template class FullColumn; template class FullRow; + template + class EulerParameters; template class FullMatrix : public RowTypeMatrix>> @@ -62,8 +66,12 @@ namespace MbD { FullMatrix operator+(const FullMatrix fullMat); std::shared_ptr> transposeTimesFullColumn(const std::shared_ptr> fullCol); void magnifySelf(T factor); - std::ostream& printOn(std::ostream& s) const override; + std::shared_ptr> asEulerParameters(); + T trace(); + double maxMagnitude() override; + std::shared_ptr> bryantAngles(); + std::ostream& printOn(std::ostream& s) const override; }; template<> inline void FullMatrix::identity() { @@ -274,6 +282,124 @@ namespace MbD { return s; } template + inline std::shared_ptr> FullMatrix::asEulerParameters() + { + //"Given [A], compute Euler parameter." + + auto traceA = this->trace(); + T dum = 0.0; + T dumSq = 0.0; + //auto qE = CREATE>::With(4); //Cannot use CREATE.h in subclasses of std::vector. Why? + auto qE = std::make_shared>(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 + inline T FullMatrix::trace() + { + T trace = 0.0; + for (int i = 0; i < this->size(); i++) + { + trace += this->at(i)->at(i); + } + return trace; + } + template + inline double FullMatrix::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 + inline std::shared_ptr> FullMatrix::bryantAngles() + { + auto answer = std::make_shared>(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 inline std::shared_ptr> FullMatrix::timesFullColumn(std::shared_ptr> fullCol) { return this->timesFullColumn(fullCol.get()); diff --git a/MbDCode/FullMatrix.h.bak b/MbDCode/FullMatrix.h.bak deleted file mode 100644 index fbfdec3..0000000 --- a/MbDCode/FullMatrix.h.bak +++ /dev/null @@ -1,29 +0,0 @@ -#pragma once -#include - -#include "RowTypeMatrix.h" -#include "FullColumn.h" -#include "FullRow.h" - -namespace MbD { - //class FullColumn; - - template - class FullMatrix : public RowTypeMatrix>> - { - public: - FullMatrix() {} - FullMatrix(int m, int n) { - for (int i = 0; i < m; i++) { - auto row = std::make_shared>(n); - this->push_back(row); - } - } - FullMatrix(std::initializer_list> list) : RowTypeMatrix>{ list } {} - }; - - typedef std::shared_ptr> FullMatDptr; - //typedef std::shared_ptr>>> FullMatFCptr; - //typedef std::shared_ptr>>> FullMatFMptr; -} - diff --git a/MbDCode/FullVector.h b/MbDCode/FullVector.h index 9b4e7c3..987fe73 100644 --- a/MbDCode/FullVector.h +++ b/MbDCode/FullVector.h @@ -16,19 +16,20 @@ namespace MbD { FullVector(std::vector::iterator begin, std::vector::iterator end) : Array(begin, end) {} FullVector(std::initializer_list list) : Array{ list } {} double dot(std::shared_ptr> 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> fullVec); void atiplusFullVectortimes(int i, std::shared_ptr> fullVec, T factor); - double maxMagnitude(); - double length(); void equalSelfPlusFullVectortimes(std::shared_ptr> 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 - inline void FullVector::atiput(int i, T value) - { - this->at(i) = value; - } - template inline void FullVector::atiplusNumber(int i, T value) { this->at(i) += value; @@ -92,11 +88,6 @@ namespace MbD { assert(false); } template - inline void FullVector::atitimes(int i, double factor) - { - this->at(i) *= factor; - } - template inline void FullVector::atiplusFullVector(int i1, std::shared_ptr> fullVec) { for (int ii = 0; ii < fullVec->size(); ii++) @@ -114,19 +105,41 @@ namespace MbD { this->at(i) += fullVec->at(ii) * factor; } } + template + inline void FullVector::equalSelfPlusFullVectortimes(std::shared_ptr> fullVec, T factor) + { + for (int i = 0; i < this->size(); i++) + { + this->atiplusNumber(i, fullVec->at(i) * factor); + } + } template<> inline double FullVector::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 + inline double FullVector::maxMagnitude() + { + assert(false); + return 0.0; } template<> - inline double FullVector::length() + inline void FullVector::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 double FullVector::length() { auto ssq = 0.0; for (int i = 0; i < this->size(); i++) @@ -137,20 +150,27 @@ namespace MbD { return std::sqrt(ssq); } template - inline void FullVector::equalSelfPlusFullVectortimes(std::shared_ptr> fullVec, T factor) + inline void FullVector::conditionSelf() + { + constexpr auto epsilon = std::numeric_limits::epsilon(); + auto tol = this->maxMagnitude() * epsilon; + this->conditionSelfWithTol(tol); + } + template<> + inline void FullVector::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 - inline void FullVector::magnifySelf(T factor) + inline void FullVector::conditionSelfWithTol(double tol) { - for (int i = 0; i < this->size(); i++) - { - this->atitimes(i, factor); - } + assert(false); + return; } template inline std::ostream& FullVector::printOn(std::ostream& s) const diff --git a/MbDCode/FunctionWithManyArgs.cpp b/MbDCode/FunctionWithManyArgs.cpp index 1044375..845c398 100644 --- a/MbDCode/FunctionWithManyArgs.cpp +++ b/MbDCode/FunctionWithManyArgs.cpp @@ -34,3 +34,8 @@ std::shared_ptr> FunctionWithManyArgs::getTerms() return terms; } +void MbD::FunctionWithManyArgs::createMbD(std::shared_ptr mbdSys, std::shared_ptr mbdUnits) +{ + for (auto& term : *terms) term->createMbD(mbdSys, mbdUnits); +} + diff --git a/MbDCode/FunctionWithManyArgs.h b/MbDCode/FunctionWithManyArgs.h index cf214c7..a53cafb 100644 --- a/MbDCode/FunctionWithManyArgs.h +++ b/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> _terms); std::shared_ptr> getTerms() override; + void createMbD(std::shared_ptr mbdSys, std::shared_ptr mbdUnits) override; std::shared_ptr> terms; }; diff --git a/MbDCode/FunctionX.cpp b/MbDCode/FunctionX.cpp index 4ce0b79..584a8da 100644 --- a/MbDCode/FunctionX.cpp +++ b/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(1.0); + auto dfdx = differentiateWRTx(); + auto dxdvar = xx->differentiateWRT(var); + return Symbolic::times(dfdx, dxdvar); +} + +Symsptr MbD::FunctionX::differentiateWRTx() +{ + assert(false); + return Symsptr(); +} diff --git a/MbDCode/FunctionX.h b/MbDCode/FunctionX.h index 239d397..7186fec 100644 --- a/MbDCode/FunctionX.h +++ b/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; + + + }; } diff --git a/MbDCode/FunctionXY.cpp b/MbDCode/FunctionXY.cpp new file mode 100644 index 0000000..895fa2c --- /dev/null +++ b/MbDCode/FunctionXY.cpp @@ -0,0 +1,9 @@ +#include "FunctionXY.h" + +MbD::FunctionXY::FunctionXY() +{ +} + +MbD::FunctionXY::FunctionXY(Symsptr base, Symsptr exp) : x(base), y(exp) +{ +} diff --git a/MbDCode/FunctionXY.h b/MbDCode/FunctionXY.h new file mode 100644 index 0000000..35bef31 --- /dev/null +++ b/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; + + }; +} + diff --git a/MbDCode/GESpMatParPvMarko.cpp b/MbDCode/GESpMatParPvMarko.cpp index 5a33c55..b36be0a 100644 --- a/MbDCode/GESpMatParPvMarko.cpp +++ b/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::min()) rowPositionsOfNonZerosInPivotColumn->at(markowitzPivotColCount - 1) = rowPivoti; + if (aip != std::numeric_limits::min()) rowPositionsOfNonZerosInPivotColumn->at((size_t)markowitzPivotColCount - 1) = rowPivoti; } if (max < singularPivotTolerance) throwSingularMatrixError(""); } diff --git a/MbDCode/IndependentVariable.cpp b/MbDCode/IndependentVariable.cpp index 2b200b9..2286333 100644 --- a/MbDCode/IndependentVariable.cpp +++ b/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(1.0); diff --git a/MbDCode/IndependentVariable.h b/MbDCode/IndependentVariable.h index 64e6f66..3690a79 100644 --- a/MbDCode/IndependentVariable.h +++ b/MbDCode/IndependentVariable.h @@ -7,7 +7,7 @@ namespace MbD { { public: IndependentVariable(); - Symsptr differentiateWRT(Symsptr sptr, Symsptr var) override; + Symsptr differentiateWRT(Symsptr var) override; }; } diff --git a/MbDCode/Item.cpp b/MbDCode/Item.cpp index f7e6fab..5bf368b 100644 --- a/MbDCode/Item.cpp +++ b/MbDCode/Item.cpp @@ -298,7 +298,7 @@ void Item::storeDynState() { } -void MbD::Item::submitToSystem() +void MbD::Item::submitToSystem(std::shared_ptr self) { assert(false); } diff --git a/MbDCode/Item.h b/MbDCode/Item.h index bfd0476..eddcd42 100644 --- a/MbDCode/Item.h +++ b/MbDCode/Item.h @@ -133,7 +133,7 @@ namespace MbD { virtual std::shared_ptr stateData(); virtual void storeCollisionState(); virtual void storeDynState(); - virtual void submitToSystem(); + virtual void submitToSystem(std::shared_ptr self); virtual double suggestSmallerOrAcceptCollisionFirstStepSize(double hnew); virtual double suggestSmallerOrAcceptCollisionStepSize(double hnew); virtual double suggestSmallerOrAcceptDynFirstStepSize(double hnew); diff --git a/MbDCode/Item.h.bak b/MbDCode/Item.h.bak deleted file mode 100644 index ef363d9..0000000 --- a/MbDCode/Item.h.bak +++ /dev/null @@ -1,16 +0,0 @@ -#pragma once -#include - -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; - }; -} - diff --git a/MbDCode/Joint.cpp b/MbDCode/Joint.cpp index 1e8064e..ab6f938 100644 --- a/MbDCode/Joint.cpp +++ b/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->useEquationNumbers(); }); } +void MbD::Joint::submitToSystem(std::shared_ptr self) +{ + root()->jointsMotions->push_back(std::static_pointer_cast(self)); +} + void Joint::setqsulam(FColDsptr col) { constraintsDo([&](std::shared_ptr con) { con->setqsulam(col); }); diff --git a/MbDCode/Joint.cpp.bak b/MbDCode/Joint.cpp.bak deleted file mode 100644 index c438f46..0000000 --- a/MbDCode/Joint.cpp.bak +++ /dev/null @@ -1,22 +0,0 @@ -#include "Joint.h" - -using namespace MbD; - -Joint::Joint() -{ - -} - -void Joint::connectsItoJ(std::shared_ptr frmi, std::shared_ptr frmj) -{ - frmI = frmi; - frmJ = frmj; -} - -void Joint::initializeLocally() -{ -} - -void Joint::initializeGlobally() -{ -} diff --git a/MbDCode/Joint.h b/MbDCode/Joint.h index 6af27bf..f0a4e66 100644 --- a/MbDCode/Joint.h +++ b/MbDCode/Joint.h @@ -65,6 +65,7 @@ namespace MbD { void setqsulam(FColDsptr col) override; std::shared_ptr stateData() override; void useEquationNumbers() override; + void submitToSystem(std::shared_ptr self) override; EndFrmcptr frmI; EndFrmcptr frmJ; diff --git a/MbDCode/Joint.h.bak b/MbDCode/Joint.h.bak deleted file mode 100644 index 82d84ab..0000000 --- a/MbDCode/Joint.h.bak +++ /dev/null @@ -1,28 +0,0 @@ -#pragma once -#include -#include - -#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 frmI, std::shared_ptr frmJ); - void initializeLocally(); - void initializeGlobally(); - - std::shared_ptr frmI; - std::shared_ptr frmJ; - std::unique_ptr>> constraints; - - }; -} - diff --git a/MbDCode/LDUSpMatParPvPrecise.cpp b/MbDCode/LDUSpMatParPvPrecise.cpp index 8c1fa7f..b7ea379 100644 --- a/MbDCode/LDUSpMatParPvPrecise.cpp +++ b/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::min()) rowPositionsOfNonZerosInPivotColumn->at(markowitzPivotColCount - 1) = rowPivoti; + if (aip != std::numeric_limits::min()) rowPositionsOfNonZerosInPivotColumn->at((size_t)markowitzPivotColCount - 1) = rowPivoti; } pivotValues->at(p) = max; if (max < singularPivotTolerance) throwSingularMatrixError(""); diff --git a/MbDCode/Ln.cpp b/MbDCode/Ln.cpp new file mode 100644 index 0000000..f97d080 --- /dev/null +++ b/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()); +} diff --git a/MbDCode/Ln.h b/MbDCode/Ln.h new file mode 100644 index 0000000..4ea66ce --- /dev/null +++ b/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; + + + + }; +} + diff --git a/MbDCode/MarkerFrame.cpp.bak b/MbDCode/MarkerFrame.cpp.bak deleted file mode 100644 index 13d3ccc..0000000 --- a/MbDCode/MarkerFrame.cpp.bak +++ /dev/null @@ -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(); - std::string str = "EndFrame1"; - endFrm->setName(str); - this->addEndFrame(endFrm); -} - -void MarkerFrame::setPartFrame(std::shared_ptr partFrm) -{ - partFrame = partFrm; -} - -std::shared_ptr 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 endFrm) -{ - endFrames->push_back(endFrm); -} diff --git a/MbDCode/MarkerFrame.h.bak b/MbDCode/MarkerFrame.h.bak deleted file mode 100644 index 5b4f013..0000000 --- a/MbDCode/MarkerFrame.h.bak +++ /dev/null @@ -1,36 +0,0 @@ -#pragma once -#include - -#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 partFrm); - std::shared_ptr getPartFrame(); - void setrpmp(FullColDptr x); - void setaApm(FullMatDptr x); - void addEndFrame(std::shared_ptr x); - - std::weak_ptr partFrame; - FullColDptr rpmp = std::make_shared>(3); - FullMatDptr aApm = std::make_shared>(3, 3); - FullColDptr rOmO = std::make_shared>(3); - FullMatDptr aAOm = std::make_shared>(3, 3); - FullMatDptr prOmOpE = std::make_shared>(3, 4); - FullColumn>* pAOmpE = new FullColumn>(4); - std::unique_ptr>> endFrames; - - }; -} - diff --git a/MbDCode/MbDCode.cpp b/MbDCode/MbDCode.cpp index 8c4b15f..9b82041 100644 --- a/MbDCode/MbDCode.cpp +++ b/MbDCode/MbDCode.cpp @@ -19,12 +19,12 @@ void runSpMat(); int main() { - //ASMTAssembly::runFile("C:\\Users\\askoh\\OneDrive\\askoh\\visualworks\\vw8.1\\askoh\\64bit\\CADSM\\current\\asm\\00piston.asmt"); + ASMTAssembly::runFile("piston.asmt"); - auto externalSys = std::make_shared(); - externalSys->runOndselPiston(); - //externalSys->runPiston(); - //runSpMat(); + auto cadSystem = std::make_shared(); + //cadSystem->runOndselPiston(); + cadSystem->runPiston(); + runSpMat(); } void runSpMat() { diff --git a/MbDCode/MbDCode.cpp.bak b/MbDCode/MbDCode.cpp.bak deleted file mode 100644 index 5031e6f..0000000 --- a/MbDCode/MbDCode.cpp.bak +++ /dev/null @@ -1,240 +0,0 @@ -/********************************************************************* - * @file MbDCode.cpp - * - * @brief Program to assemble a piston crank system. - *********************************************************************/ - -#include -#include "System.h" -#include "FullColumn.h" -#include "FullMatrix.h" -#include "Part.h" -#include "Joint.h" -#include "CylindricalJoint.h" -#include "RevoluteJoint.h" -#include "ZRotation.h" -#include "EndFrameqc.h" -#include "MbDCode.h" - -using namespace MbD; - -int main() -{ - std::cout << "Hello World!\n"; - //System& TheSystem = System::getInstance(); - System& TheSystem = System::getInstance("TheSystem"); - std::cout << "TheSystem.getName() " << TheSystem.getName() << std::endl; - std::string str; - FullColDptr qX, qE; - FullColDptr rpmp; - FullMatDptr aApm; - FullRowDptr fullRow; - auto row = std::make_shared>(ListD{ 0.0, 0.0, 0.0, 1.0 }); - fullRow = std::make_shared>(4); - fullRow->copy(row); - // - auto assembly1 = std::make_shared(); - str = "Assembly1"; - assembly1->setName(str); - std::cout << "assembly1->getName() " << assembly1->getName() << std::endl; - qX = std::make_shared>(ListD{ 0, 0, 0 }); - qE = std::make_shared>(ListD{ 0, 0, 0, 1 }); - assembly1->setqX(qX); - assembly1->setqE(qE); - std::cout << "assembly1->getqX() " << assembly1->getqX()->toString() << std::endl; - std::cout << "assembly1->getqE() " << assembly1->getqE()->toString() << std::endl; - TheSystem.addPart(assembly1); - { - auto partFrame = assembly1->partFrame; - auto marker1 = std::make_shared(); - str = "Marker1"; - marker1->setName(str); - rpmp = std::make_shared>(ListD{ 0.38423366582893, 6.8384291794733e-9, -0.048029210642807 }); - marker1->setrpmp(rpmp); - aApm = std::make_shared>(ListListD{ - { 1, 0, 0 }, - { 0, 0, 1 }, - { 0, -1, 0 } - }); - marker1->setaApm(aApm); - auto endFrame1 = std::make_shared(); - endFrame1->setMarkerFrame(marker1); - marker1->addEndFrame(endFrame1); - marker1->setPartFrame(partFrame); - partFrame->addMarkerFrame(marker1); - // - auto marker2 = std::make_shared(); - str = "Marker2"; - marker2->setName(str); - rpmp = std::make_shared>(ListD{ 0.0, 0.0, 0.0 }); - marker2->setrpmp(rpmp); - aApm = std::make_shared>(ListListD{ - { 1, 0, 0 }, - { 0, 1, 0 }, - { 0, 0, 1 } - }); - marker2->setaApm(aApm); - auto endFrame2 = std::make_shared(); - endFrame2->setMarkerFrame(marker2); - marker2->addEndFrame(endFrame2); - marker2->setPartFrame(partFrame); - partFrame->addMarkerFrame(marker2); - } - // - auto part1 = std::make_shared(); - str = "Part1"; - part1->setName(str); - qX = std::make_shared>(ListD{ 0.38423366582893, 6.8384291794733e-9, -0.048029210642807 }); - qE = std::make_shared>(ListD{ 0.0, 0.0, 1.4248456266393e-10, 1.0 }); - part1->setqX(qX); - part1->setqE(qE); - TheSystem.parts->push_back(part1); - { - auto partFrame = part1->partFrame; - auto marker1 = std::make_shared(); - str = "Marker1"; - marker1->setName(str); - rpmp = std::make_shared>(ListD{ -0.38423368514246, -2.6661567755108e-17, 0.048029210642807 }); - marker1->setrpmp(rpmp); - aApm = std::make_shared>(ListListD{ - { 1, 0, 0 }, - { 0, 1, 0 }, - { 0, 0, 1 } - }); - marker1->setaApm(aApm); - auto endFrame1 = std::make_shared(); - endFrame1->setMarkerFrame(marker1); - marker1->addEndFrame(endFrame1); - marker1->setPartFrame(partFrame); - partFrame->addMarkerFrame(marker1); - // - auto marker2 = std::make_shared(); - str = "Marker2"; - marker2->setName(str); - rpmp = std::make_shared>(ListD{ 0.38423368514246, -2.6661567755108e-17, 0.048029210642807 }); - marker2->setrpmp(rpmp); - aApm = std::make_shared>(ListListD{ - { 1, 0, 0 }, - { 0, 1, 0 }, - { 0, 0, 1 } - }); - marker2->setaApm(aApm); - auto endFrame2 = std::make_shared(); - endFrame2->setMarkerFrame(marker2); - marker2->addEndFrame(endFrame2); - marker2->setPartFrame(partFrame); - partFrame->addMarkerFrame(marker2); - } - // - auto part2 = std::make_shared(); - str = "Part2"; - part2->setName(str); - qX = std::make_shared>(ListD{ 0.38423366582893, 0.49215308269277, 0.048029210642807 }); - qE = std::make_shared>(ListD{ 0.0, 0.0, 0.89871701272344, 0.4385290538168 }); - part2->setqX(qX); - part2->setqE(qE); - TheSystem.parts->push_back(part2); - { - auto partFrame = part2->partFrame; - auto marker1 = std::make_shared(); - str = "Marker1"; - marker1->setName(str); - rpmp = std::make_shared>(ListD{ -0.6243797383565, 1.1997705489799e-16, -0.048029210642807 }); - marker1->setrpmp(rpmp); - aApm = std::make_shared>(ListListD{ - {1.0, 2.7755575615629e-16, 0.0}, - {-2.7755575615629e-16, 1.0, 0.0}, - {0.0, 0.0, 1.0} - }); - marker1->setaApm(aApm); - auto endFrame1 = std::make_shared(); - endFrame1->setMarkerFrame(marker1); - marker1->addEndFrame(endFrame1); - marker1->setPartFrame(partFrame); - partFrame->addMarkerFrame(marker1); - // - auto marker2 = std::make_shared(); - str = "Marker2"; - marker2->setName(str); - rpmp = std::make_shared>(ListD{ 0.6243797383565, -2.1329254204087e-16, -0.048029210642807 }); - marker2->setrpmp(rpmp); - aApm = std::make_shared>(ListListD{ - {1.0, 2.4980018054066e-16, 2.2204460492503e-16}, - {-2.4980018054066e-16, 1.0, 4.1633363423443e-17}, - {-2.2204460492503e-16, -4.1633363423443e-17, 1.0} - }); - marker2->setaApm(aApm); - auto endFrame2 = std::make_shared(); - endFrame2->setMarkerFrame(marker2); - marker2->addEndFrame(endFrame2); - marker2->setPartFrame(partFrame); - partFrame->addMarkerFrame(marker2); - } - // - auto part3 = std::make_shared(); - str = "Part3"; - part3->setName(str); - qX = std::make_shared>(ListD{ -1.284772285311e-18, 1.4645982581368, -4.788228906425e-17 }); - qE = std::make_shared>(ListD{ 0.70710678118655, 0.70710678118655, 0.0, 0.0 }); - part3->setqX(qX); - part3->setqE(qE); - TheSystem.parts->push_back(part3); - { - auto partFrame = part3->partFrame; - auto marker1 = std::make_shared(); - str = "Marker1"; - marker1->setName(str); - rpmp = std::make_shared>(ListD{ -0.48029210642807, 7.6201599718927e-18, -2.816737703896e-17 }); - marker1->setrpmp(rpmp); - aApm = std::make_shared>(ListListD{ - {9.2444637330587e-33, 1.0, 2.2204460492503e-16}, - {1.0, -9.2444637330587e-33, -1.0785207688569e-32}, - {-1.0785207688569e-32, 2.2204460492503e-16, -1.0} - }); - marker1->setaApm(aApm); - auto endFrame1 = std::make_shared(); - endFrame1->setMarkerFrame(marker1); - marker1->addEndFrame(endFrame1); - marker1->setPartFrame(partFrame); - partFrame->addMarkerFrame(marker1); - // - auto marker2 = std::make_shared(); - str = "Marker2"; - marker2->setName(str); - rpmp = std::make_shared>(ListD{ 0.48029210642807, 1.7618247880058e-17, 2.5155758471256e-17 }); - marker2->setrpmp(rpmp); - aApm = std::make_shared>(ListListD{ - {6.9388939039072e-18, -6.4146353042213e-50, 1.0}, - {1.0, -6.9388939039072e-18, 6.9388939039072e-18}, - {-6.9388939039072e-18, 1.0, -7.4837411882581e-50} - }); - marker2->setaApm(aApm); - auto endFrame2 = std::make_shared(); - endFrame2->setMarkerFrame(marker2); - marker2->addEndFrame(endFrame2); - marker2->setPartFrame(partFrame); - partFrame->addMarkerFrame(marker2); - } - // - auto cylJoint4 = std::make_shared(); - cylJoint4->connectsItoJ(part3->partFrame->endFrame("Marker2"), assembly1->partFrame->endFrame("Marker1")); - TheSystem.jointsMotions->push_back(cylJoint4); - - auto revJoint3 = std::make_shared(); - revJoint3->connectsItoJ(part2->partFrame->endFrame("Marker2"), part3->partFrame->endFrame("Marker1")); - TheSystem.jointsMotions->push_back(revJoint3); - - auto revJoint2 = std::make_shared(); - revJoint2->connectsItoJ(part1->partFrame->endFrame("Marker2"), part2->partFrame->endFrame("Marker1")); - TheSystem.jointsMotions->push_back(revJoint2); - - auto revJoint1 = std::make_shared(); - revJoint1->connectsItoJ(assembly1->partFrame->endFrame("Marker2"), part1->partFrame->endFrame("Marker1")); - TheSystem.jointsMotions->push_back(revJoint1); - - auto rotMotion1 = std::make_shared(); - rotMotion1->connectsItoJ(assembly1->partFrame->endFrame("Marker2"), part1->partFrame->endFrame("Marker1")); - TheSystem.jointsMotions->push_back(rotMotion1); - // - TheSystem.runKINEMATICS(); -} \ No newline at end of file diff --git a/MbDCode/MbDCode.vcxproj b/MbDCode/MbDCode.vcxproj index 246bb09..842ace8 100644 --- a/MbDCode/MbDCode.vcxproj +++ b/MbDCode/MbDCode.vcxproj @@ -127,6 +127,7 @@ + @@ -137,6 +138,7 @@ + @@ -147,6 +149,7 @@ + @@ -154,6 +157,7 @@ + @@ -161,6 +165,7 @@ + @@ -174,6 +179,7 @@ + @@ -183,7 +189,10 @@ + + + @@ -306,6 +315,7 @@ + @@ -323,6 +333,7 @@ + @@ -336,6 +347,8 @@ + + @@ -348,6 +361,7 @@ + @@ -361,6 +375,7 @@ + @@ -371,6 +386,7 @@ + @@ -381,6 +397,7 @@ + @@ -388,6 +405,7 @@ + @@ -395,6 +413,7 @@ + @@ -408,6 +427,7 @@ + @@ -417,7 +437,10 @@ + + + @@ -541,6 +564,7 @@ + @@ -559,6 +583,7 @@ + @@ -572,6 +597,8 @@ + + @@ -585,6 +612,7 @@ + @@ -602,6 +630,7 @@ + diff --git a/MbDCode/MbDCode.vcxproj.filters b/MbDCode/MbDCode.vcxproj.filters index eb3bfb4..52d8352 100644 --- a/MbDCode/MbDCode.vcxproj.filters +++ b/MbDCode/MbDCode.vcxproj.filters @@ -711,6 +711,48 @@ Source Files + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + @@ -1418,6 +1460,48 @@ Header Files + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + @@ -1426,5 +1510,6 @@ + \ No newline at end of file diff --git a/MbDCode/Numeric.cpp b/MbDCode/Numeric.cpp index 15403df..29a60d9 100644 --- a/MbDCode/Numeric.cpp +++ b/MbDCode/Numeric.cpp @@ -42,3 +42,8 @@ double MbD::Numeric::arcTan0to2piYoverX(double y, double x) } } } + +bool MbD::Numeric::equaltol(double x, double xx, double tol) +{ + return std::abs(x - xx) < tol; +} diff --git a/MbDCode/Numeric.h b/MbDCode/Numeric.h index b384477..6583bfe 100644 --- a/MbDCode/Numeric.h +++ b/MbDCode/Numeric.h @@ -8,6 +8,7 @@ namespace MbD { // public: static double arcTan0to2piYoverX(double y, double x); + static bool equaltol(double x, double xx, double tol); }; diff --git a/MbDCode/Part.cpp b/MbDCode/Part.cpp index 6531741..b7ae4c8 100644 --- a/MbDCode/Part.cpp +++ b/MbDCode/Part.cpp @@ -559,3 +559,8 @@ void Part::postDynStep() { partFrame->postDynStep(); } + +void MbD::Part::submitToSystem(std::shared_ptr self) +{ + root()->parts->push_back(std::static_pointer_cast(self)); +} diff --git a/MbDCode/Part.cpp.bak b/MbDCode/Part.cpp.bak deleted file mode 100644 index 21342a0..0000000 --- a/MbDCode/Part.cpp.bak +++ /dev/null @@ -1,38 +0,0 @@ -#include "Part.h" -#include "PartFrame.h" -#include "FullColumn.h" - -using namespace MbD; - -Part::Part() { - partFrame = std::make_shared(this); -} - -void Part::setqX(FullColDptr x) { - partFrame.get()->setqX(x); -} - -FullColDptr Part::getqX() { - return partFrame.get()->getqX(); -} - -void Part::setqE(FullColDptr x) { - partFrame.get()->setqE(x); -} - -FullColDptr Part::getqE() { - return partFrame.get()->getqE(); -} - -void Part::setSystem(System& sys) -{ - //May be needed in the future -} - -void Part::initializeLocally() -{ -} - -void Part::initializeGlobally() -{ -} diff --git a/MbDCode/Part.h b/MbDCode/Part.h index 68ff87e..1f40a61 100644 --- a/MbDCode/Part.h +++ b/MbDCode/Part.h @@ -102,6 +102,7 @@ namespace MbD { std::shared_ptr stateData() override; double suggestSmallerOrAcceptDynStepSize(double hnew) override; void postDynStep() override; + void submitToSystem(std::shared_ptr self) override; System* system; //Use raw pointer when pointing backwards. int ipX = -1; diff --git a/MbDCode/Part.h.bak b/MbDCode/Part.h.bak deleted file mode 100644 index a8d66f3..0000000 --- a/MbDCode/Part.h.bak +++ /dev/null @@ -1,29 +0,0 @@ -#pragma once -#include - -#include "Item.h" -#include "System.h" -#include "PartFrame.h" -#include "FullColumn.h" - -namespace MbD { - class System; - class PartFrame; - - class Part : public Item - { - //ToDo: ipX ipE m aJ partFrame pX pXdot pE pEdot mX mE mEdot pTpE ppTpEpE ppTpEpEdot - public: - Part(); - void setqX(FullColDptr x); - FullColDptr getqX(); - void setqE(FullColDptr x); - FullColDptr getqE(); - void setSystem(System& sys); - void initializeLocally(); - void initializeGlobally(); - - std::shared_ptr partFrame; - }; -} - diff --git a/MbDCode/PartFrame.cpp.bak b/MbDCode/PartFrame.cpp.bak deleted file mode 100644 index 31419b1..0000000 --- a/MbDCode/PartFrame.cpp.bak +++ /dev/null @@ -1,52 +0,0 @@ -#include "Part.h" -#include "PartFrame.h" -#include "EulerConstraint.h" -#include "AbsConstraint.h" -#include "MarkerFrame.h" - -using namespace MbD; - -PartFrame::PartFrame() -{ - aGeu = std::make_shared("EulerCon"); - aGeu->setOwner(this); - aGabs = std::make_unique>>(); - markerFrames = std::make_unique>>(); -} -MbD::PartFrame::PartFrame(const char* str) : CartesianFrame(str) -{ -} -void MbD::PartFrame::initialize() -{ -} -void PartFrame::setqX(FullColDptr x) { - qX->copy(x); -} -FullColDptr PartFrame::getqX() { - return qX; -} -void PartFrame::setqE(FullColDptr x) { - qE->copy(x); -} -FullColDptr PartFrame::getqE() { - return qE; -} -void PartFrame::setPart(Part* x) { - part = x; -} -Part* PartFrame::getPart() { - return part; -} - -void PartFrame::addMarkerFrame(std::shared_ptr markerFrame) -{ - markerFrames->push_back(markerFrame); -} - -std::shared_ptr PartFrame::endFrame(std::string name) -{ - auto match = std::find_if(markerFrames->begin(), markerFrames->end(), [&](std::shared_ptr mkr) {return mkr->endFrames->at(0)->getName() == name; }); - //auto match = std::find_if(markerFrames->begin(), markerFrames->end(), [&](std::shared_ptr mkr) {return mkr->getName() == name; }); - - return (*match)->endFrames->at(0); -} diff --git a/MbDCode/Power.cpp b/MbDCode/Power.cpp new file mode 100644 index 0000000..91bca5f --- /dev/null +++ b/MbDCode/Power.cpp @@ -0,0 +1,38 @@ +#include "Power.h" +#include "Constant.h" +#include "Ln.h" + +using namespace MbD; + +MbD::Power::Power() +{ +} + +MbD::Power::Power(Symsptr bse, Symsptr ex) : FunctionXY(bse, ex) +{ +} + +Symsptr MbD::Power::differentiateWRTx() +{ + auto yminus1 = Symbolic::sum(y, std::make_shared(-1.0)); + auto power = Symbolic::raisedTo(x, yminus1); + auto deriv = Symbolic::times(y, power); + return deriv->simplified(deriv); +} + +Symsptr MbD::Power::differentiateWRTy() +{ + auto lnterm = std::make_shared(x); + auto deriv = Symbolic::times(clonesptr(), lnterm); + return deriv->simplified(); +} + +Symsptr MbD::Power::simplifyUntil(Symsptr sptr, std::shared_ptr> set) +{ + return Symsptr(); +} + +double MbD::Power::getValue() +{ + return std::pow(x->getValue(), y->getValue()); +} diff --git a/MbDCode/Power.h b/MbDCode/Power.h new file mode 100644 index 0000000..1685812 --- /dev/null +++ b/MbDCode/Power.h @@ -0,0 +1,20 @@ +#pragma once + +#include "FunctionXY.h" + +namespace MbD { + class Power : public FunctionXY + { + // + public: + Power(); + Power(Symsptr base, Symsptr exp); + Symsptr differentiateWRTx(); + Symsptr differentiateWRTy(); + + Symsptr simplifyUntil(Symsptr sptr, std::shared_ptr> set) override; + double getValue() override; + + }; +} + diff --git a/MbDCode/PrescribedMotion.cpp b/MbDCode/PrescribedMotion.cpp index 682ebc1..4540e3b 100644 --- a/MbDCode/PrescribedMotion.cpp +++ b/MbDCode/PrescribedMotion.cpp @@ -36,5 +36,5 @@ void MbD::PrescribedMotion::initMotions() void PrescribedMotion::connectsItoJ(EndFrmcptr frmi, EndFrmcptr frmj) { Joint::connectsItoJ(frmi, frmj); - frmI->initEndFrameqct(); + std::static_pointer_cast(frmI)->initEndFrameqct(); } diff --git a/MbDCode/PrescribedMotion.cpp.bak b/MbDCode/PrescribedMotion.cpp.bak deleted file mode 100644 index 77bdb3f..0000000 --- a/MbDCode/PrescribedMotion.cpp.bak +++ /dev/null @@ -1,19 +0,0 @@ -#include -#include -#include - -#include "PrescribedMotion.h" -#include "EndFrameqct.h" - -using namespace MbD; - -void PrescribedMotion::connectsItoJ(std::shared_ptr frmi, std::shared_ptr frmj) -{ - Joint::connectsItoJ(frmi, frmj); - if (typeid(frmI).name() != "EndFrameqct") { - std::shared_ptr newFrmI; - newFrmI = std::make_shared(); - std::swap(frmI, newFrmI); - assert(typeid(frmI).name() != "EndFrameqct"); - } -} diff --git a/MbDCode/PrescribedMotion.h b/MbDCode/PrescribedMotion.h index eefe604..9541e42 100644 --- a/MbDCode/PrescribedMotion.h +++ b/MbDCode/PrescribedMotion.h @@ -1,9 +1,11 @@ #pragma once +#include #include "Joint.h" -#include "Symbolic.h" +//#include "Symbolic.h" namespace MbD { + class Symbolic; class PrescribedMotion : public Joint { @@ -16,12 +18,19 @@ namespace MbD { void initialize() override; virtual void initMotions(); - Symsptr xBlk; - Symsptr yBlk; - Symsptr zBlk; - Symsptr phiBlk; - Symsptr theBlk; - Symsptr psiBlk; + //Why the following fails? + //Symsptr xBlk; + //Symsptr yBlk; + //Symsptr zBlk; + //Symsptr phiBlk; + //Symsptr theBlk; + //Symsptr psiBlk; + std::shared_ptr xBlk; + std::shared_ptr yBlk; + std::shared_ptr zBlk; + std::shared_ptr phiBlk; + std::shared_ptr theBlk; + std::shared_ptr psiBlk; }; } diff --git a/MbDCode/PrescribedMotion.h.bak b/MbDCode/PrescribedMotion.h.bak deleted file mode 100644 index 8ce12b4..0000000 --- a/MbDCode/PrescribedMotion.h.bak +++ /dev/null @@ -1,11 +0,0 @@ -#pragma once -#include "Joint.h" - -namespace MbD { - class PrescribedMotion : public Joint - { - public: - void connectsItoJ(std::shared_ptr frmI, std::shared_ptr frmJ) override; - }; -} - diff --git a/MbDCode/Product.cpp b/MbDCode/Product.cpp index 3de8546..dc713e8 100644 --- a/MbDCode/Product.cpp +++ b/MbDCode/Product.cpp @@ -7,7 +7,7 @@ using namespace MbD; -Symsptr Product::differentiateWRT(Symsptr sptr, Symsptr var) +Symsptr MbD::Product::differentiateWRT(Symsptr var) { //"Apply chain rule of differentiation." // "(xyz)' := x'yz + xy'z + xyz'." @@ -16,7 +16,7 @@ Symsptr Product::differentiateWRT(Symsptr sptr, Symsptr var) std::transform(terms->begin(), terms->end(), std::back_inserter(*derivatives), - [var](Symsptr term) { return term->differentiateWRT(term, var); } + [var](Symsptr term) { return term->differentiateWRT(var); } ); auto derivativeTerms = std::make_shared>(); for (int i = 0; i < terms->size(); i++) @@ -59,9 +59,9 @@ Symsptr Product::expandUntil(Symsptr sptr, std::shared_ptrterms = productTerms; auto sumOfProductsOfSums = std::make_shared(std::make_shared(1)); for (const auto& term : *sumTerms) { - sumOfProductsOfSums = std::static_pointer_cast(sumOfProductsOfSums->timesSum(sumOfProductsOfSums, term)); + sumOfProductsOfSums = std::static_pointer_cast(Symbolic::times(sumOfProductsOfSums, term)); } - return factor->timesSum(factor, sumOfProductsOfSums); + return Symbolic::times(factor, sumOfProductsOfSums); } Symsptr Product::simplifyUntil(Symsptr sptr, std::shared_ptr> set) @@ -103,39 +103,6 @@ Symsptr Product::simplifyUntil(Symsptr sptr, std::shared_ptr(); - auto sumTERMs = aSum->getTerms(); - for (const auto& sumTERM : *sumTERMs) { - Symsptr termTERM; - if (sumTERM->isProduct()) { - termTERM = sptr->timesProduct(sptr, sumTERM); - } - else { - termTERM = sptr->timesFunction(sptr, sumTERM); - } - answer->terms->push_back(termTERM); - } - return answer; -} - -Symsptr Product::timesProduct(Symsptr sptr, Symsptr product) -{ - auto answer = std::make_shared(sptr->getTerms()); - auto& answerTerms = answer->terms; - auto productTerms = product->getTerms(); - answerTerms->insert(answerTerms->end(), productTerms->begin(), productTerms->end()); - return answer; -} - -Symsptr Product::timesFunction(Symsptr sptr, Symsptr function) -{ - auto answer = std::make_shared(sptr->getTerms()); - answer->terms->push_back(function); - return answer; -} - std::ostream& Product::printOn(std::ostream& s) const { s << "("; @@ -159,3 +126,8 @@ double Product::getValue() for (int i = 0; i < terms->size(); i++) answer *= terms->at(i)->getValue(); return answer; } + +Symsptr MbD::Product::clonesptr() +{ + return std::make_shared(*this); +} diff --git a/MbDCode/Product.h b/MbDCode/Product.h index 12a3806..c12ee76 100644 --- a/MbDCode/Product.h +++ b/MbDCode/Product.h @@ -2,6 +2,8 @@ #include "FunctionWithManyArgs.h" #include "Symbolic.h" +#include "System.h" +#include "Units.h" namespace MbD { @@ -13,15 +15,15 @@ namespace MbD { Product(Symsptr term, Symsptr term1) : FunctionWithManyArgs(term, term1) {} Product(Symsptr term, Symsptr term1, Symsptr term2) : FunctionWithManyArgs(term, term1, term2) {} Product(std::shared_ptr> _terms) : FunctionWithManyArgs(_terms) {} - Symsptr differentiateWRT(Symsptr sptr, Symsptr var) override; + Symsptr differentiateWRT(Symsptr var) override; Symsptr expandUntil(Symsptr sptr, std::shared_ptr> set) override; Symsptr simplifyUntil(Symsptr sptr, std::shared_ptr> set) override; - Symsptr timesSum(Symsptr sptr, Symsptr sum) override; - Symsptr timesProduct(Symsptr sptr, Symsptr product) override; - Symsptr timesFunction(Symsptr sptr, Symsptr function) override; - std::ostream& printOn(std::ostream& s) const override; bool isProduct() override; double getValue() override; + Symsptr clonesptr() override; + + std::ostream& printOn(std::ostream& s) const override; + }; } diff --git a/MbDCode/RevoluteJoint.cpp.bak b/MbDCode/RevoluteJoint.cpp.bak deleted file mode 100644 index 1e728ca..0000000 --- a/MbDCode/RevoluteJoint.cpp.bak +++ /dev/null @@ -1 +0,0 @@ -#include "RevoluteJoint.h" diff --git a/MbDCode/RevoluteJoint.h.bak b/MbDCode/RevoluteJoint.h.bak deleted file mode 100644 index e6b94ac..0000000 --- a/MbDCode/RevoluteJoint.h.bak +++ /dev/null @@ -1,8 +0,0 @@ -#pragma once -#include "Joint.h" -namespace MbD { - class RevoluteJoint : public Joint - { - }; -} - diff --git a/MbDCode/RowTypeMatrix.h b/MbDCode/RowTypeMatrix.h index a9192eb..3476d4b 100644 --- a/MbDCode/RowTypeMatrix.h +++ b/MbDCode/RowTypeMatrix.h @@ -14,13 +14,15 @@ namespace MbD { RowTypeMatrix(std::initializer_list list) : Array{ list } {} void copyFrom(std::shared_ptr> x); virtual void zeroSelf() = 0; + //double maxMagnitude() override; + int numberOfElements() override; + int nrow() { return (int) this->size(); } int ncol() { return this->at(0)->numberOfElements(); } - int numberOfElements() override; }; template @@ -30,6 +32,17 @@ namespace MbD { this->at(i)->copyFrom(x->at(i)); } } + //template + //inline double RowTypeMatrix::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 inline int RowTypeMatrix::numberOfElements() { diff --git a/MbDCode/Sin.cpp b/MbDCode/Sin.cpp new file mode 100644 index 0000000..45b02ef --- /dev/null +++ b/MbDCode/Sin.cpp @@ -0,0 +1,6 @@ +#include "Sin.h" + +double MbD::Sin::getValue() +{ + return std::sin(xx->getValue()); +} diff --git a/MbDCode/Sin.h b/MbDCode/Sin.h new file mode 100644 index 0000000..21314c1 --- /dev/null +++ b/MbDCode/Sin.h @@ -0,0 +1,15 @@ +#pragma once + +#include "FunctionX.h" + +namespace MbD { + class Sin : public FunctionX + { + // + public: + double getValue() override; + + + }; +} + diff --git a/MbDCode/SparseMatrix.h b/MbDCode/SparseMatrix.h index 7402750..e80097d 100644 --- a/MbDCode/SparseMatrix.h +++ b/MbDCode/SparseMatrix.h @@ -7,6 +7,11 @@ #include "DiagonalMatrix.h" namespace MbD { + //template + //class SparseRow; + //template + //class DiagonalMatrix; + template class SparseMatrix : public RowTypeMatrix>> { @@ -42,9 +47,10 @@ namespace MbD { void atijplusNumber(int i, int j, double value); void atijminusNumber(int i, int j, double value); void atijput(int i, int j, T value); + double maxMagnitude() override; + std::shared_ptr> timesFullColumn(std::shared_ptr> fullCol); std::ostream& printOn(std::ostream& s) const override; - std::shared_ptr> timesFullColumn(std::shared_ptr> fullCol); }; using SpMatDsptr = std::shared_ptr>; @@ -152,6 +158,17 @@ namespace MbD { this->at(i)->atiput(j, value); } template + inline double SparseMatrix::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 inline std::ostream& SparseMatrix::printOn(std::ostream& s) const { s << "SpMat[" << std::endl; diff --git a/MbDCode/Sum.cpp b/MbDCode/Sum.cpp index 7933ab3..8779167 100644 --- a/MbDCode/Sum.cpp +++ b/MbDCode/Sum.cpp @@ -1,35 +1,46 @@ +#include +#include + #include "Sum.h" #include "Constant.h" using namespace MbD; -Symsptr Sum::timesSum(Symsptr sptr, Symsptr aSum) +Symsptr MbD::Sum::parseExpression(std::string& expression) { - auto answer = std::make_shared(); - auto sumTERMs = aSum->getTerms(); - for (const auto& term : *terms) { - for (const auto& sumTERM : *sumTERMs) { - Symsptr termTERM; - if (sumTERM->isProduct()) { - termTERM = term->timesProduct(term, sumTERM); - } - else { - termTERM = term->timesFunction(term, sumTERM); - } - answer->terms->push_back(termTERM); - } + std::istringstream iss(expression); + auto sum = std::make_shared(); + sum->parse(iss); + return sum->simplified(sum); +} + +void MbD::Sum::parse(std::istringstream& iss) +{ + iss >> std::ws; + char c = iss.peek(); + if (c == '+') { + parsePlusTerm(iss); + } + else if (c == '-') { + parseMinusTerm(iss); + } + else { + parseTerm(iss); } - return answer; } -Symsptr Sum::timesProduct(Symsptr sptr, Symsptr product) +void MbD::Sum::parseTerm(std::istringstream& iss) { - return product->timesSum(product, sptr); } -Symsptr Sum::timesFunction(Symsptr sptr, Symsptr function) +void MbD::Sum::parsePlusTerm(std::istringstream& iss) +{ + iss.get(); + +} + +void MbD::Sum::parseMinusTerm(std::istringstream& iss) { - return function->timesSum(function, sptr); } Symsptr Sum::expandUntil(Symsptr sptr, std::shared_ptr> set) @@ -112,6 +123,11 @@ double Sum::getValue() return answer; } +Symsptr MbD::Sum::clonesptr() +{ + return std::make_shared(*this); +} + std::ostream& Sum::printOn(std::ostream& s) const { s << "("; diff --git a/MbDCode/Sum.h b/MbDCode/Sum.h index 8c0e4a7..a45e724 100644 --- a/MbDCode/Sum.h +++ b/MbDCode/Sum.h @@ -6,19 +6,25 @@ namespace MbD { class Sum : public FunctionWithManyArgs { public: + static Symsptr parseExpression(std::string& expression); + void parse(std::istringstream& iss); + void parseTerm(std::istringstream& iss); + void parsePlusTerm(std::istringstream& iss); + void parseMinusTerm(std::istringstream& iss); + Sum() : FunctionWithManyArgs() {} Sum(Symsptr term) : FunctionWithManyArgs(term) {} Sum(Symsptr term, Symsptr term1) : FunctionWithManyArgs(term, term1) {} Sum(Symsptr term, Symsptr term1, Symsptr term2) : FunctionWithManyArgs(term, term1, term2) {} Sum(std::shared_ptr> _terms) : FunctionWithManyArgs(_terms) {} - Symsptr timesSum(Symsptr sptr, Symsptr sum) override; - Symsptr timesProduct(Symsptr sptr, Symsptr product) override; - Symsptr timesFunction(Symsptr sptr, Symsptr function) override; Symsptr expandUntil(Symsptr sptr, std::shared_ptr> set) override; Symsptr simplifyUntil(Symsptr sptr, std::shared_ptr> set) override; bool isSum() override; double getValue() override; + Symsptr clonesptr() override; + std::ostream& printOn(std::ostream& s) const override; + }; } diff --git a/MbDCode/Symbolic.cpp b/MbDCode/Symbolic.cpp index f862d85..c46e7db 100644 --- a/MbDCode/Symbolic.cpp +++ b/MbDCode/Symbolic.cpp @@ -6,6 +6,7 @@ #include "Constant.h" #include "Product.h" #include "Sum.h" +#include "Power.h" using namespace MbD; @@ -13,16 +14,82 @@ Symbolic::Symbolic() { } +Symsptr MbD::Symbolic::times(Symsptr arg, Symsptr arg1) +{ + if (arg->isProduct()) { + if (arg1->isProduct()) { + auto newTerms = arg->getTerms(); + auto arg1Terms = arg1->getTerms(); + newTerms->insert(newTerms->end(), arg1Terms->begin(), arg1Terms->end()); + return std::make_shared(newTerms); + } + else { + auto newTerms = arg->getTerms(); + newTerms->insert(newTerms->end(), arg1); + return std::make_shared(newTerms); + } + } + else { + if (arg1->isProduct()) { + auto newTerms = arg1->getTerms(); + newTerms->insert(newTerms->begin(), arg); + return std::make_shared(newTerms); + } + else { + return std::make_shared(arg, arg1); + } + } +} + +Symsptr MbD::Symbolic::sum(Symsptr arg, Symsptr arg1) +{ + if (arg->isSum()) { + if (arg1->isSum()) { + auto newTerms = arg->getTerms(); + auto arg1Terms = arg1->getTerms(); + newTerms->insert(newTerms->end(), arg1Terms->begin(), arg1Terms->end()); + return std::make_shared(newTerms); + } + else { + auto newTerms = arg->getTerms(); + newTerms->insert(newTerms->end(), arg1); + return std::make_shared(newTerms); + } + } + else { + if (arg1->isSum()) { + auto newTerms = arg1->getTerms(); + newTerms->insert(newTerms->begin(), arg); + return std::make_shared(newTerms); + } + else { + return std::make_shared(arg, arg1); + } + } +} + void Symbolic::initialize() { } -Symsptr Symbolic::differentiateWRT(Symsptr sptr, Symsptr var) +Symsptr MbD::Symbolic::differentiateWRT(Symsptr var) { assert(false); return Symsptr(); } +Symsptr MbD::Symbolic::simplified() +{ + //std::cout << "sptr " << *sptr << std::endl; + auto set = std::make_shared>(); + auto expanded = this->expandUntil(set); + //std::cout << "expanded " << *expanded << std::endl; + auto set1 = std::make_shared>(); + auto simplified = expanded->simplifyUntil(expanded, set1); + //std::cout << "simplified " << *simplified << std::endl; + return simplified; +} + Symsptr Symbolic::simplified(Symsptr sptr) { //std::cout << "sptr " << *sptr << std::endl; @@ -35,6 +102,12 @@ Symsptr Symbolic::simplified(Symsptr sptr) return simplified; } +Symsptr MbD::Symbolic::expandUntil(std::shared_ptr> set) +{ + assert(false); + return clonesptr(); +} + Symsptr Symbolic::expandUntil(Symsptr sptr, std::shared_ptr> set) { return sptr; @@ -45,24 +118,14 @@ Symsptr Symbolic::simplifyUntil(Symsptr sptr, std::shared_ptr(sptr); - return product->timesSum(product, sum); + return false; } -Symsptr Symbolic::timesProduct(Symsptr sptr, Symsptr product) +bool MbD::Symbolic::isOne() { - auto answer = std::make_shared(product->getTerms()); - auto& answerTerms = answer->terms; - answerTerms->insert(answerTerms->begin(), sptr); - return answer; -} - -Symsptr Symbolic::timesFunction(Symsptr sptr, Symsptr function) -{ - auto answer = std::make_shared(sptr, function); - return answer; + return false; } bool Symbolic::isSum() @@ -87,11 +150,40 @@ std::ostream& Symbolic::printOn(std::ostream& s) const std::shared_ptr> Symbolic::getTerms() { + assert(false); return std::make_shared>(); } +void MbD::Symbolic::addTerm(Symsptr trm) +{ + getTerms()->push_back(trm); +} + double Symbolic::getValue() { assert(false); return 0.0; } + +void MbD::Symbolic::createMbD(std::shared_ptr mbdSys, std::shared_ptr mbdUnits) +{ + assert(false); + return; +} + +Symsptr MbD::Symbolic::clonesptr() +{ + //Return shallow copy of *this wrapped in shared_ptr + assert(false); + return std::make_shared(*this); +} + +void MbD::Symbolic::arguments(Symsptr args) +{ + assert(false); +} + +Symsptr MbD::Symbolic::raisedTo(Symsptr x, Symsptr y) +{ + return std::make_shared(x, y); +} diff --git a/MbDCode/Symbolic.h b/MbDCode/Symbolic.h index f2370ea..c71c3ce 100644 --- a/MbDCode/Symbolic.h +++ b/MbDCode/Symbolic.h @@ -6,6 +6,8 @@ #include #include "Math.h" +#include "System.h" +#include "Units.h" namespace MbD { @@ -13,22 +15,29 @@ namespace MbD { { public: Symbolic(); + static std::shared_ptr times(std::shared_ptr arg, std::shared_ptr arg1); + static std::shared_ptr sum(std::shared_ptr arg, std::shared_ptr arg1); + static std::shared_ptr raisedTo(std::shared_ptr x, std::shared_ptr y); + virtual void initialize(); - virtual std::shared_ptr differentiateWRT(std::shared_ptr sptr, std::shared_ptr var); + virtual std::shared_ptr differentiateWRT(std::shared_ptr var); + virtual std::shared_ptr simplified(); virtual std::shared_ptr simplified(std::shared_ptr sptr); + virtual std::shared_ptr expandUntil(std::shared_ptr>> set); virtual std::shared_ptr expandUntil(std::shared_ptr sptr, std::shared_ptr>> set); virtual std::shared_ptr simplifyUntil(std::shared_ptr sptr, std::shared_ptr>> set); - virtual std::shared_ptr timesSum(std::shared_ptr sptr, std::shared_ptr sum); - virtual std::shared_ptr timesProduct(std::shared_ptr sptr, std::shared_ptr product); - virtual std::shared_ptr timesFunction(std::shared_ptr sptr, std::shared_ptr function); + virtual bool isZero(); + virtual bool isOne(); virtual bool isSum(); virtual bool isProduct(); virtual bool isConstant(); - virtual std::ostream& printOn(std::ostream& s) const; - virtual std::shared_ptr>> getTerms(); + virtual void addTerm(std::shared_ptr trm); virtual double getValue(); - + virtual void createMbD(std::shared_ptr mbdSys, std::shared_ptr mbdUnits); + virtual std::shared_ptr clonesptr(); + virtual void arguments(std::shared_ptr args); + virtual std::ostream& printOn(std::ostream& s) const; friend std::ostream& operator<<(std::ostream& s, const Symbolic& sym) { return sym.printOn(s); diff --git a/MbDCode/SymbolicParser.cpp b/MbDCode/SymbolicParser.cpp new file mode 100644 index 0000000..9a4349b --- /dev/null +++ b/MbDCode/SymbolicParser.cpp @@ -0,0 +1,431 @@ +#include +#include +#include + +#include "SymbolicParser.h" +#include "BasicUserFunction.h" +#include "Constant.h" +#include "Sum.h" +#include "Product.h" +#include "CREATE.h" +#include "Power.h" +#include "Abs.h" +#include "ArcTan.h" + +void MbD::SymbolicParser::initialize() +{ + variables = std::make_shared>(); + stack = std::make_shared>(); + buffer = std::make_shared(); + +} + +void MbD::SymbolicParser::parseUserFunction(Symsptr userFunc) +{ + auto usrFunc = std::static_pointer_cast(userFunc); + units = usrFunc->units; + this->parseString(usrFunc->funcText); + Symsptr func = stack->top(); + stack->pop(); + stack->push(Symbolic::times(func, std::make_shared(usrFunc->myUnit))); +} + +void MbD::SymbolicParser::parseString(std::string expr) +{ + buffer->clear(); + while (!stack->empty()) { + stack->pop(); + } + source = std::make_shared(expr); + hereChar = source->get(); + prevEnd = -1; + scanToken(); + expression(); + if (tokenType != "end") expected("Nothing more"); + if (stack->size() != 1) notify("Stack size error, compiler bug!"); +} + +bool MbD::SymbolicParser::plusTerm() +{ + if (peekForTypeNoPush("+")) { + if (plainTerm()) return true; + expected("plainTerm"); + } + return false; +} + +bool MbD::SymbolicParser::minusTerm() +{ + if (peekForTypeNoPush("-")) { + if (plainTerm()) return true; + expected("plainTerm"); + } + return false; +} + +bool MbD::SymbolicParser::plainTerm() +{ + if (term()) { + auto trm = stack->top(); + stack->pop(); + auto sum = stack->top(); + sum->addTerm(trm); + return true; + } + return false; +} + +bool MbD::SymbolicParser::term() +{ + auto product = std::make_shared(); + stack->push(product); + if (plainFunction()) { + while (timesFunction() || divideByFunction()) {} + auto term = stack->top(); + if (term->isProduct()) { + if (term->isOne()) { + stack->pop(); + } + else if (term->getTerms()->size() == 1) { + stack->pop(); + stack->push(term->getTerms()->front()); + } + } + else { + notify("SymbolicParser error"); + } + return true; + } + return false; +} + +bool MbD::SymbolicParser::plainFunction() +{ + if (symfunction()) { + auto trm = stack->top(); + stack->pop(); + auto product = stack->top(); + product->addTerm(trm); + return true; + } + return false; +} + +bool MbD::SymbolicParser::timesFunction() +{ + if (peekForTypeNoPush("*")) { + if (plainFunction()) return true; + expected("plainFunction"); + } + return false; +} + +bool MbD::SymbolicParser::divideByFunction() +{ + if (peekForTypeNoPush("/")) { + if (plainFunction()) return true; + expected("plainFunction"); + } + return false; +} + +bool MbD::SymbolicParser::peekForTypeNoPush(std::string c) +{ + //"Test to see if tokenType matches aType. If so, advance to the next token, leaving the stack unchanged" + + if (tokenType == c) { + scanToken(); + return true; + } + return false; +} + +std::string MbD::SymbolicParser::scanToken() +{ + prevEnd = (int)source->tellg(); + prevEnd--; + while (std::isspace(hereChar)) { + hereChar = source->get(); + } + if (hereChar == EOF) { + mark = prevEnd + 1; + tokenType = "end"; + return token = ""; + } + mark = (int)source->tellg(); + if (std::isalpha(hereChar)) { + xLetter(); + } + else if (std::isdigit(hereChar)) { + xDigit(); + } + else if (hereChar == '\"') { + xDoubleQuote(); + } + else { + token = std::string(1, hereChar); + tokenType = token; + hereChar = source->get(); + } + return token; +} + +void MbD::SymbolicParser::xLetter() +{ + buffer->str(""); + buffer->clear(); + *buffer << hereChar; + while (true) { + hereChar = source->get(); + if (hereChar == EOF) break; + if (!std::isalnum(hereChar)) break; + *buffer << hereChar; + } + tokenType = "word"; + token = buffer->str(); +} + +void MbD::SymbolicParser::xDigit() +{ + tokenType = "number"; + if (hereChar != EOF) { + auto pos = source->tellg(); + std::streamoff offset = -1; + pos += offset; + source->seekg(pos); + } + double mantissa = 0.0; + int exponent = 0; + *source >> mantissa; + hereChar = source->peek(); + if ((hereChar == 'd') || (hereChar == 'e')) { + hereChar = source->get(); + char peekChar = source->peek(); + if (std::isdigit(peekChar) || (peekChar == '+') || (peekChar == '-')) { + *source >> exponent; + } + } + token = ""; + tokenNum = mantissa * std::pow(10.0, exponent); + hereChar = source->get(); +} + +void MbD::SymbolicParser::xDoubleQuote() +{ + tokenType = "comment"; + if (hereChar != EOF) { + auto pos = source->tellg(); + std::streamoff offset = -1; + pos += offset; + source->seekg(pos); + } + *source >> std::quoted(token); + hereChar = source->get(); +} + +bool MbD::SymbolicParser::symfunction() +{ + if (expressionInParentheses() || constant() || namedFunction() || variable()) { + raisedTo(); + return true; + } + else { + notify("Unrecognized symbol ->"); + } + return false; +} + +bool MbD::SymbolicParser::expression() +{ + auto sum = std::make_shared(); + stack->push(sum); + if (plusTerm() || minusTerm() || plainTerm()) { + while (plusTerm() || minusTerm()) {} + auto term = stack->top(); + if (term->isSum()) { + auto sum1 = std::static_pointer_cast(term); + if (sum1->isZero()) { + stack->pop(); + } + else if (sum1->terms->size() == 1) { + stack->pop(); + stack->push(sum1->terms->front()); + } + } + else { + notify("Compiler error!"); + } + } + return false; +} + +bool MbD::SymbolicParser::expressionInParentheses() +{ + if (peekForTypeNoPush("(")) { + if (expression()) { + if (peekForTypeNoPush(")")) { + return true; + } + else { + expected(")"); + } + } + else { + expected("expression"); + } + } + return false; +} + +bool MbD::SymbolicParser::constant() +{ + if (signedNumber()) { + return true; + } + if (peekForTypevalue("word", "pi")) { + auto symconst = std::make_shared(M_PI); + stack->push(symconst); + return true; + } + return false; +} + +bool MbD::SymbolicParser::namedFunction() +{ + return intrinsic(); +} + +bool MbD::SymbolicParser::intrinsic() +{ + Symsptr symfunc = nullptr; + if (peekForTypevalue("word", "abs")) { + symfunc = std::make_shared(); + } + else if (peekForTypevalue("word", "arctan")) { + symfunc = std::make_shared(); + } + if (symfunc != nullptr) { + stack->push(symfunc); + if (peekForTypeNoPush("(")) { + auto startsize = stack->size(); + while (expression() && peekForTypeNoPush(",")) {} + if (stack->size() > startsize) { + combineStackTo((int)startsize); + if (peekForTypeNoPush(")")) { + auto args = stack->top(); + stack->pop(); + auto func = stack->top(); + func->arguments(args); + stack->pop(); + return true; + } + expected(")"); + } + expected("expression"); + } + expected("("); + } + return false; +} + +bool MbD::SymbolicParser::variable() +{ + if ((tokenType == "word") && (variables->count(token) == 1)) { + auto& var = variables->at(token); + stack->push(var); + scanToken(); + return true; + } + return false; +} + +bool MbD::SymbolicParser::raisedTo() +{ + if (peekForTypeNoPush("^")) { + if (symfunction()) { + auto exp = stack->top(); + stack->pop(); + auto base = stack->top(); + stack->pop(); + auto pow = std::make_shared(base, exp); + stack->push(pow); + return true; + } + expected("function"); + } + return false; +} + +bool MbD::SymbolicParser::expected(std::string msg) +{ + return false; +} + +bool MbD::SymbolicParser::signedNumber() +{ + if (tokenType == "number") { + auto symNum = std::make_shared(tokenNum); + stack->push(symNum); + scanToken(); + return true; + } + if ((token == "+") && (hereChar != EOF) && (std::isdigit(hereChar) || (hereChar == '.'))) { + //"no intervening delimiters" + scanToken(); + auto symNum = std::make_shared(tokenNum); + stack->push(symNum); + scanToken(); + return true; + } + if ((token == "-") && (hereChar != EOF) && (std::isdigit(hereChar) || (hereChar == '.'))) { + //"no intervening delimiters" + scanToken(); + auto symNum = std::make_shared(-tokenNum); + stack->push(symNum); + scanToken(); + return true; + } + return false; +} + +bool MbD::SymbolicParser::peekForTypevalue(std::string type, std::string symbol) +{ + if ((tokenType == type) && (token == symbol)) { + scanToken(); + return true; + } + return false; +} + +void MbD::SymbolicParser::notify(std::string msg) +{ + notifyat(msg, mark); +} + +void MbD::SymbolicParser::notifyat(std::string msg, int mrk) +{ + //"Temporarily reset source in order to get full contents" + auto p = source->tellg(); + source->seekg(0); + auto contents = source->str(); + source->seekg(p); + assert(false); + //SyntaxErrorException new + //targetClass : class; + //messageText: aString; + //source: contents; + //position: position; + //raiseSignal +} + +void MbD::SymbolicParser::combineStackTo(int pos) +{ + auto args = std::make_shared(); + while (stack->size() > pos) { + auto arg = stack->top(); + stack->pop(); + args->addTerm(arg); + } + stack->push(args); +} + diff --git a/MbDCode/SymbolicParser.h b/MbDCode/SymbolicParser.h new file mode 100644 index 0000000..a03370b --- /dev/null +++ b/MbDCode/SymbolicParser.h @@ -0,0 +1,59 @@ +#pragma once + +#include +#include +#include + +#include "Symbolic.h" +#include "ASMTItem.h" +#include "ASMTItemIJ.h" + +namespace MbD { + class SymbolicParser + { + // + public: + void initialize(); + void parseUserFunction(Symsptr userFunc); + void parseString(std::string expr); + bool plusTerm(); + bool minusTerm(); + bool plainTerm(); + bool term(); + bool plainFunction(); + bool timesFunction(); + bool divideByFunction(); + bool peekForTypeNoPush(std::string c); + std::string scanToken(); + void xLetter(); + void xDigit(); + void xDoubleQuote(); + bool symfunction(); + bool expression(); + bool expressionInParentheses(); + bool constant(); + bool namedFunction(); + bool intrinsic(); + bool variable(); + bool raisedTo(); + bool expected(std::string msg); + bool signedNumber(); + bool peekForTypevalue(std::string type, std::string symbol); + void notify(std::string msg); + void notifyat(std::string msg, int mrk); + void combineStackTo(int pos); + + ASMTItem* owner = nullptr; + std::shared_ptr> variables; + std::shared_ptr> geoIJs; + std::shared_ptr units; + int mark = -1, prevEnd = -1; + char hereChar = '\0'; + std::string token, tokenType; + double tokenNum = -1.0e100; + std::shared_ptr source; + std::shared_ptr buffer; + std::shared_ptr> stack; + }; +} + diff --git a/MbDCode/SyntaxError.cpp b/MbDCode/SyntaxError.cpp new file mode 100644 index 0000000..8f1d043 --- /dev/null +++ b/MbDCode/SyntaxError.cpp @@ -0,0 +1,7 @@ +#include "SyntaxError.h" + +using namespace MbD; + +SyntaxError::SyntaxError(const std::string& msg) : std::runtime_error(msg) +{ +} diff --git a/MbDCode/SyntaxError.h b/MbDCode/SyntaxError.h new file mode 100644 index 0000000..c4ed6ed --- /dev/null +++ b/MbDCode/SyntaxError.h @@ -0,0 +1,16 @@ +#pragma once + +#include +#include +#include + +namespace MbD { + class SyntaxError : virtual public std::runtime_error + { + + public: + //SyntaxError(); + explicit SyntaxError(const std::string& msg); + virtual ~SyntaxError() noexcept {} + }; +} diff --git a/MbDCode/System.cpp b/MbDCode/System.cpp index c4fb277..9d154f8 100644 --- a/MbDCode/System.cpp +++ b/MbDCode/System.cpp @@ -12,11 +12,9 @@ using namespace MbD; System::System() { - initialize(); } System::System(const char* str) : Item(str) { - initialize(); } System* MbD::System::root() @@ -52,31 +50,29 @@ void MbD::System::addMotion(std::shared_ptr motion) jointsMotions->push_back(motion); } -void System::runKINEMATIC() +void MbD::System::addForceTorque(std::shared_ptr forTor) { - externalSystem->preMbDrun(); + forTor->owner = this; + forcesTorques->push_back(forTor); +} + +void System::runKINEMATIC(std::shared_ptr self) +{ + externalSystem->preMbDrun(self); while (true) { initializeLocally(); initializeGlobally(); if (!hasChanged) break; } - partsJointsMotionsForcesTorquesDo([&](std::shared_ptr item) { item->postInput(); }); - outputInput(); + partsJointsMotionsForcesTorquesDo([](std::shared_ptr item) { item->postInput(); }); + externalSystem->outputFor(INPUT); systemSolver->runAllIC(); externalSystem->outputFor(INITIALCONDITION); systemSolver->runBasicKinematic(); externalSystem->postMbDrun(); } -void System::outputInput() -{ -} - -void System::outputTimeSeries() -{ -} - void System::initializeLocally() { hasChanged = false; diff --git a/MbDCode/System.cpp.bak b/MbDCode/System.cpp.bak deleted file mode 100644 index b7aae47..0000000 --- a/MbDCode/System.cpp.bak +++ /dev/null @@ -1,63 +0,0 @@ -#include - -#include "System.h" - -using namespace MbD; - -System::System() { - time = std::make_unique