diff --git a/OndselSolver/ASMTAllowRotation.cpp b/OndselSolver/ASMTAllowRotation.cpp index 8f4c871..15a6646 100644 --- a/OndselSolver/ASMTAllowRotation.cpp +++ b/OndselSolver/ASMTAllowRotation.cpp @@ -50,7 +50,7 @@ void MbD::ASMTAllowRotation::initMarkers() } } -std::shared_ptr MbD::ASMTAllowRotation::mbdClassNew() +std::shared_ptr MbD::ASMTAllowRotation::mbdClassNew() { return AllowZRotation::With(); } diff --git a/OndselSolver/ASMTAllowRotation.h b/OndselSolver/ASMTAllowRotation.h index 16983d5..44885a8 100644 --- a/OndselSolver/ASMTAllowRotation.h +++ b/OndselSolver/ASMTAllowRotation.h @@ -19,7 +19,7 @@ namespace MbD { void parseASMT(std::vector& lines) override; void readMotionJoint(std::vector& lines); void initMarkers() override; - std::shared_ptr mbdClassNew() override; + std::shared_ptr mbdClassNew() override; void setMotionJoint(std::string motionJoint); void storeOnLevel(std::ofstream& os, size_t level) override; void storeOnTimeSeries(std::ofstream& os) override; diff --git a/OndselSolver/ASMTAngleJoint.cpp b/OndselSolver/ASMTAngleJoint.cpp index e546809..0deb313 100644 --- a/OndselSolver/ASMTAngleJoint.cpp +++ b/OndselSolver/ASMTAngleJoint.cpp @@ -12,7 +12,14 @@ using namespace MbD; -std::shared_ptr MbD::ASMTAngleJoint::mbdClassNew() +std::shared_ptr MbD::ASMTAngleJoint::With() +{ + auto asmt = std::make_shared(); + asmt->initialize(); + return asmt; +} + +std::shared_ptr MbD::ASMTAngleJoint::mbdClassNew() { return CREATE::With(); } diff --git a/OndselSolver/ASMTAngleJoint.h b/OndselSolver/ASMTAngleJoint.h index 3e57e9a..9e3fd7c 100644 --- a/OndselSolver/ASMTAngleJoint.h +++ b/OndselSolver/ASMTAngleJoint.h @@ -15,7 +15,8 @@ namespace MbD { { // public: - std::shared_ptr mbdClassNew() override; + static std::shared_ptr With(); + std::shared_ptr mbdClassNew() override; void parseASMT(std::vector& lines) override; void readTheIzJz(std::vector& lines); void createMbD(std::shared_ptr mbdSys, std::shared_ptr mbdUnits) override; diff --git a/OndselSolver/ASMTAnimationParameters.cpp b/OndselSolver/ASMTAnimationParameters.cpp index 625272f..a3f1d26 100644 --- a/OndselSolver/ASMTAnimationParameters.cpp +++ b/OndselSolver/ASMTAnimationParameters.cpp @@ -10,6 +10,13 @@ using namespace MbD; +std::shared_ptr MbD::ASMTAnimationParameters::With() +{ + auto asmt = std::make_shared(); + asmt->initialize(); + return asmt; +} + void MbD::ASMTAnimationParameters::parseASMT(std::vector& lines) { //size_t nframe, icurrent, istart, iend, framesPerSecond; diff --git a/OndselSolver/ASMTAnimationParameters.h b/OndselSolver/ASMTAnimationParameters.h index 03703a6..ffa5c54 100644 --- a/OndselSolver/ASMTAnimationParameters.h +++ b/OndselSolver/ASMTAnimationParameters.h @@ -15,6 +15,7 @@ namespace MbD { { // public: + static std::shared_ptr With(); void parseASMT(std::vector& lines) override; void storeOnLevel(std::ofstream& os, size_t level) override; diff --git a/OndselSolver/ASMTAssembly.cpp b/OndselSolver/ASMTAssembly.cpp index 840e6f8..541e108 100644 --- a/OndselSolver/ASMTAssembly.cpp +++ b/OndselSolver/ASMTAssembly.cpp @@ -58,6 +58,9 @@ #include "ExternalSystem.h" #include "SystemSolver.h" #include "ASMTRevRevJoint.h" +#include "ASMTLimit.h" +#include "ASMTRotationLimit.h" +#include "ASMTTranslationLimit.h" using namespace MbD; @@ -68,8 +71,9 @@ MbD::ASMTAssembly::ASMTAssembly() : ASMTSpatialContainer() std::shared_ptr MbD::ASMTAssembly::With() { - auto assembly = std::make_shared(); - return assembly; + auto asmt = std::make_shared(); + asmt->initialize(); + return asmt; } void MbD::ASMTAssembly::runSinglePendulumSuperSimplified() @@ -183,7 +187,7 @@ void MbD::ASMTAssembly::runSinglePendulumSimplified() assembly->setVelocity3D(0, 0, 0); assembly->setOmega3D(0, 0, 0); - auto massMarker = std::make_shared(); + auto massMarker = ASMTPrincipalMassMarker::With(); massMarker->setMass(0.0); massMarker->setDensity(0.0); massMarker->setMomentOfInertias(0, 0, 0); @@ -214,7 +218,7 @@ void MbD::ASMTAssembly::runSinglePendulumSimplified() part->setOmega3D(0, 0, 0); assembly->addPart(part); - massMarker = std::make_shared(); + massMarker = ASMTPrincipalMassMarker::With(); massMarker->setMass(0.2); massMarker->setDensity(10.0); massMarker->setMomentOfInertias(8.3333333333333e-4, 0.016833333333333, 0.017333333333333); @@ -282,7 +286,7 @@ void MbD::ASMTAssembly::runSinglePendulum() auto ome3D = std::make_shared>(ListD{ 0, 0, 0 }); assembly->setOmega3D(ome3D); // - auto massMarker = std::make_shared(); + auto massMarker = ASMTPrincipalMassMarker::With(); massMarker->setMass(0.0); massMarker->setDensity(0.0); auto aJ = std::make_shared>(ListD{ 0, 0, 0 }); @@ -327,7 +331,7 @@ void MbD::ASMTAssembly::runSinglePendulum() part->setOmega3D(ome3D); assembly->addPart(part); // - massMarker = std::make_shared(); + massMarker = ASMTPrincipalMassMarker::With(); massMarker->setMass(0.2); massMarker->setDensity(10.0); aJ = std::make_shared>(ListD{ 8.3333333333333e-4, 0.016833333333333, 0.017333333333333 }); @@ -439,7 +443,27 @@ void MbD::ASMTAssembly::runFile(const char* fileName) void MbD::ASMTAssembly::runDraggingTest() { + //auto assembly = ASMTAssembly::assemblyFromFile("../testapp/pistonWithLimits.asmt"); auto assembly = ASMTAssembly::assemblyFromFile("../testapp/dragCrankSlider.asmt"); + + auto limit1 = ASMTRotationLimit::With(); + limit1->setName("Limit1"); + limit1->setMarkerI("/Assembly1/Marker2"); + limit1->setMarkerJ("/Assembly1/Part1/Marker1"); + limit1->settype("=>"); + limit1->setlimit("30.0*pi/180.0"); + limit1->settol("1.0e-9"); + assembly->addLimit(limit1); + + auto limit2 = ASMTTranslationLimit::With(); + limit2->setName("Limit2"); + limit2->setMarkerI("/Assembly1/Part3/Marker2"); + limit2->setMarkerJ("/Assembly1/Marker1"); + limit2->settype("=<"); + limit2->setlimit("1.2"); + limit2->settol("1.0e-9"); + assembly->addLimit(limit2); + auto& dragPart = assembly->parts->at(0); auto dragParts = std::make_shared>>(); dragParts->push_back(dragPart); @@ -581,6 +605,7 @@ void MbD::ASMTAssembly::readConstraintSets(std::vector& lines) lines.erase(lines.begin()); readJoints(lines); readMotions(lines); + readLimits(lines); readGeneralConstraintSets(lines); } @@ -680,7 +705,10 @@ void MbD::ASMTAssembly::readMotions(std::vector& lines) assert(lines[0] == "\t\tMotions"); lines.erase(lines.begin()); motions->clear(); - auto it = std::find(lines.begin(), lines.end(), "\t\tGeneralConstraintSets"); + auto it = std::find(lines.begin(), lines.end(), "\t\tLimits"); + if (it == lines.end()) { + it = std::find(lines.begin(), lines.end(), "\t\tGeneralConstraintSets"); + } std::vector motionsLines(lines.begin(), it); std::shared_ptr motion; while (!motionsLines.empty()) { @@ -709,6 +737,33 @@ void MbD::ASMTAssembly::readMotions(std::vector& lines) } +void MbD::ASMTAssembly::readLimits(std::vector& lines) +{ + if (lines[0] != "\t\tLimits") return; + lines.erase(lines.begin()); + limits->clear(); + auto it = std::find(lines.begin(), lines.end(), "\t\tGeneralConstraintSets"); + std::vector limitsLines(lines.begin(), it); + std::shared_ptr limit; + while (!limitsLines.empty()) { + if (limitsLines[0] == "\t\t\tRotationLimit") { + limit = ASMTRotationLimit::With(); + } + else if (limitsLines[0] == "\t\t\tTranslationLimit") { + limit = ASMTTranslationLimit::With(); + } + else { + assert(false); + } + limitsLines.erase(limitsLines.begin()); + limit->parseASMT(limitsLines); + limits->push_back(limit); + limit->owner = this; + limit->initMarkers(); + } + lines.erase(lines.begin(), it); +} + void MbD::ASMTAssembly::readGeneralConstraintSets(std::vector& lines) const { assert(lines[0] == "\t\tGeneralConstraintSets"); @@ -1011,6 +1066,7 @@ void MbD::ASMTAssembly::deleteMbD() for (auto& part : *parts) { part->deleteMbD(); } for (auto& joint : *joints) { joint->deleteMbD(); } for (auto& motion : *motions) { motion->deleteMbD(); } + for (auto& limit : *limits) { limit->deleteMbD(); } for (auto& forceTorque : *forcesTorques) { forceTorque->deleteMbD(); } @@ -1026,9 +1082,11 @@ void MbD::ASMTAssembly::createMbD(std::shared_ptr mbdSys, std::shared_pt jointsMotions->insert(jointsMotions->end(), joints->begin(), joints->end()); jointsMotions->insert(jointsMotions->end(), motions->begin(), motions->end()); std::sort(jointsMotions->begin(), jointsMotions->end(), [](std::shared_ptr a, std::shared_ptr b) { return a->name < b->name; }); + std::sort(limits->begin(), limits->end(), [](std::shared_ptr a, std::shared_ptr b) { return a->name < b->name; }); std::sort(forcesTorques->begin(), forcesTorques->end(), [](std::shared_ptr a, std::shared_ptr b) { return a->name < b->name; }); for (auto& part : *parts) { part->createMbD(mbdSys, mbdUnits); } for (auto& joint : *jointsMotions) { joint->createMbD(mbdSys, mbdUnits); } + for (auto& limit : *limits) { limit->createMbD(mbdSys, mbdUnits); } for (auto& forceTorque : *forcesTorques) { forceTorque->createMbD(mbdSys, mbdUnits); } auto& mbdSysSolver = mbdSys->systemSolver; @@ -1138,7 +1196,7 @@ void MbD::ASMTAssembly::runKINEMATIC() void MbD::ASMTAssembly::initprincipalMassMarker() { - principalMassMarker = std::make_shared(); + principalMassMarker = ASMTPrincipalMassMarker::With(); principalMassMarker->mass = 0.0; principalMassMarker->density = 0.0; principalMassMarker->momentOfInertias = std::make_shared>(3, 0); @@ -1264,6 +1322,13 @@ void MbD::ASMTAssembly::addMotion(std::shared_ptr motion) motion->initMarkers(); } +void MbD::ASMTAssembly::addLimit(std::shared_ptr limit) +{ + limits->push_back(limit); + limit->owner = this; + limit->initMarkers(); +} + void MbD::ASMTAssembly::setConstantGravity(std::shared_ptr gravity) { constantGravity = gravity; diff --git a/OndselSolver/ASMTAssembly.h b/OndselSolver/ASMTAssembly.h index 60e2631..f867d7b 100644 --- a/OndselSolver/ASMTAssembly.h +++ b/OndselSolver/ASMTAssembly.h @@ -24,6 +24,7 @@ namespace MbD { class ASMTForceTorque; class ASMTJoint; class ASMTMotion; + class ASMTLimit; class SystemSolver; class ASMTItemIJ; class MBDynSystem; @@ -54,6 +55,7 @@ namespace MbD { void readConstraintSets(std::vector& lines); void readJoints(std::vector& lines); void readMotions(std::vector& lines); + void readLimits(std::vector& lines); void readGeneralConstraintSets(std::vector& lines) const; void readForcesTorques(std::vector& lines); void readConstantGravity(std::vector& lines); @@ -105,6 +107,7 @@ namespace MbD { void addPart(std::shared_ptr part); void addJoint(std::shared_ptr joint); void addMotion(std::shared_ptr motion); + void addLimit(std::shared_ptr limit); void setConstantGravity(std::shared_ptr constantGravity); void setSimulationParameters(std::shared_ptr simulationParameters); std::shared_ptr partNamed(std::string partName) const; @@ -127,12 +130,13 @@ namespace MbD { std::shared_ptr>> constraintSets = std::make_shared>>(); std::shared_ptr>> joints = std::make_shared>>(); std::shared_ptr>> motions = std::make_shared>>(); + std::shared_ptr>> limits = std::make_shared>>(); std::shared_ptr>> forcesTorques = std::make_shared>>(); - std::shared_ptr constantGravity = std::make_shared(); - std::shared_ptr simulationParameters = std::make_shared(); - std::shared_ptr animationParameters = std::make_shared(); + std::shared_ptr constantGravity = ASMTConstantGravity::With(); + std::shared_ptr simulationParameters = ASMTSimulationParameters::With(); + std::shared_ptr animationParameters = ASMTAnimationParameters::With(); std::shared_ptr> times = std::make_shared>(); - std::shared_ptr asmtTime = std::make_shared(); + std::shared_ptr asmtTime = ASMTTime::With(); std::shared_ptr mbdUnits = std::make_shared(); std::shared_ptr mbdSystem; MBDynSystem* mbdynItem = nullptr; diff --git a/OndselSolver/ASMTConstantGravity.cpp b/OndselSolver/ASMTConstantGravity.cpp index 8678f94..b75d2f1 100644 --- a/OndselSolver/ASMTConstantGravity.cpp +++ b/OndselSolver/ASMTConstantGravity.cpp @@ -15,6 +15,13 @@ using namespace MbD; +std::shared_ptr MbD::ASMTConstantGravity::With() +{ + auto asmt = std::make_shared(); + asmt->initialize(); + return asmt; +} + void MbD::ASMTConstantGravity::parseASMT(std::vector& lines) { g = readColumnOfDoubles(lines[0]); diff --git a/OndselSolver/ASMTConstantGravity.h b/OndselSolver/ASMTConstantGravity.h index 460231c..e58e023 100644 --- a/OndselSolver/ASMTConstantGravity.h +++ b/OndselSolver/ASMTConstantGravity.h @@ -19,6 +19,7 @@ namespace MbD { { // public: + static std::shared_ptr With(); void parseASMT(std::vector& lines) override; void createMbD(std::shared_ptr mbdSys, std::shared_ptr mbdUnits) override; FColDsptr getg(); diff --git a/OndselSolver/ASMTConstantVelocityJoint.cpp b/OndselSolver/ASMTConstantVelocityJoint.cpp index 1c00408..5dd26b4 100644 --- a/OndselSolver/ASMTConstantVelocityJoint.cpp +++ b/OndselSolver/ASMTConstantVelocityJoint.cpp @@ -12,7 +12,14 @@ using namespace MbD; -std::shared_ptr MbD::ASMTConstantVelocityJoint::mbdClassNew() +std::shared_ptr MbD::ASMTConstantVelocityJoint::With() +{ + auto asmt = std::make_shared(); + asmt->initialize(); + return asmt; +} + +std::shared_ptr MbD::ASMTConstantVelocityJoint::mbdClassNew() { return CREATE::With(); } diff --git a/OndselSolver/ASMTConstantVelocityJoint.h b/OndselSolver/ASMTConstantVelocityJoint.h index 5bd1bed..b6396ce 100644 --- a/OndselSolver/ASMTConstantVelocityJoint.h +++ b/OndselSolver/ASMTConstantVelocityJoint.h @@ -15,7 +15,8 @@ namespace MbD { { // public: - std::shared_ptr mbdClassNew() override; + static std::shared_ptr With(); + std::shared_ptr mbdClassNew() override; }; } diff --git a/OndselSolver/ASMTConstraintSet.cpp b/OndselSolver/ASMTConstraintSet.cpp index a5359d4..f83d730 100644 --- a/OndselSolver/ASMTConstraintSet.cpp +++ b/OndselSolver/ASMTConstraintSet.cpp @@ -14,25 +14,6 @@ using namespace MbD; -void MbD::ASMTConstraintSet::createMbD(std::shared_ptr mbdSys, std::shared_ptr) -{ - //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); -} - -std::shared_ptr MbD::ASMTConstraintSet::mbdClassNew() -{ - assert(false); - return std::shared_ptr(); -} - void MbD::ASMTConstraintSet::updateFromMbD() { //" diff --git a/OndselSolver/ASMTConstraintSet.h b/OndselSolver/ASMTConstraintSet.h index 88bc167..737164b 100644 --- a/OndselSolver/ASMTConstraintSet.h +++ b/OndselSolver/ASMTConstraintSet.h @@ -17,8 +17,6 @@ namespace MbD { { // public: - void createMbD(std::shared_ptr mbdSys, std::shared_ptr mbdUnits) override; - virtual std::shared_ptr mbdClassNew(); void updateFromMbD() override; void compareResults(AnalysisType type) override; void outputResults(AnalysisType type) override; diff --git a/OndselSolver/ASMTCylSphJoint.cpp b/OndselSolver/ASMTCylSphJoint.cpp index 8ceca31..d2f028b 100644 --- a/OndselSolver/ASMTCylSphJoint.cpp +++ b/OndselSolver/ASMTCylSphJoint.cpp @@ -12,7 +12,14 @@ using namespace MbD; -std::shared_ptr MbD::ASMTCylSphJoint::mbdClassNew() +std::shared_ptr MbD::ASMTCylSphJoint::With() +{ + auto asmt = std::make_shared(); + asmt->initialize(); + return asmt; +} + +std::shared_ptr MbD::ASMTCylSphJoint::mbdClassNew() { return CREATE::With(); } diff --git a/OndselSolver/ASMTCylSphJoint.h b/OndselSolver/ASMTCylSphJoint.h index 83280f4..b1d94c9 100644 --- a/OndselSolver/ASMTCylSphJoint.h +++ b/OndselSolver/ASMTCylSphJoint.h @@ -15,7 +15,8 @@ namespace MbD { { // public: - std::shared_ptr mbdClassNew() override; + static std::shared_ptr With(); + std::shared_ptr mbdClassNew() override; }; } diff --git a/OndselSolver/ASMTCylindricalJoint.cpp b/OndselSolver/ASMTCylindricalJoint.cpp index a3fc97d..defa275 100644 --- a/OndselSolver/ASMTCylindricalJoint.cpp +++ b/OndselSolver/ASMTCylindricalJoint.cpp @@ -12,7 +12,14 @@ using namespace MbD; -std::shared_ptr MbD::ASMTCylindricalJoint::mbdClassNew() +std::shared_ptr MbD::ASMTCylindricalJoint::With() +{ + auto asmt = std::make_shared(); + asmt->initialize(); + return asmt; +} + +std::shared_ptr MbD::ASMTCylindricalJoint::mbdClassNew() { return CREATE::With(); } diff --git a/OndselSolver/ASMTCylindricalJoint.h b/OndselSolver/ASMTCylindricalJoint.h index 3abe9c3..a1a6ccb 100644 --- a/OndselSolver/ASMTCylindricalJoint.h +++ b/OndselSolver/ASMTCylindricalJoint.h @@ -15,7 +15,8 @@ namespace MbD { { // public: - std::shared_ptr mbdClassNew() override; + static std::shared_ptr With(); + std::shared_ptr mbdClassNew() override; void storeOnTimeSeries(std::ofstream& os) override; }; diff --git a/OndselSolver/ASMTFixedJoint.cpp b/OndselSolver/ASMTFixedJoint.cpp index 9877b32..fe875d2 100644 --- a/OndselSolver/ASMTFixedJoint.cpp +++ b/OndselSolver/ASMTFixedJoint.cpp @@ -12,7 +12,14 @@ using namespace MbD; -std::shared_ptr MbD::ASMTFixedJoint::mbdClassNew() +std::shared_ptr MbD::ASMTFixedJoint::With() +{ + auto asmt = std::make_shared(); + asmt->initialize(); + return asmt; +} + +std::shared_ptr MbD::ASMTFixedJoint::mbdClassNew() { return CREATE::With(); } diff --git a/OndselSolver/ASMTFixedJoint.h b/OndselSolver/ASMTFixedJoint.h index 405ff8b..f30d206 100644 --- a/OndselSolver/ASMTFixedJoint.h +++ b/OndselSolver/ASMTFixedJoint.h @@ -15,7 +15,8 @@ namespace MbD { { // public: - std::shared_ptr mbdClassNew() override; + static std::shared_ptr With(); + std::shared_ptr mbdClassNew() override; void storeOnTimeSeries(std::ofstream& os) override; }; diff --git a/OndselSolver/ASMTGearJoint.cpp b/OndselSolver/ASMTGearJoint.cpp index 7fa1aee..00e1d99 100644 --- a/OndselSolver/ASMTGearJoint.cpp +++ b/OndselSolver/ASMTGearJoint.cpp @@ -12,7 +12,14 @@ using namespace MbD; -std::shared_ptr MbD::ASMTGearJoint::mbdClassNew() +std::shared_ptr MbD::ASMTGearJoint::With() +{ + auto asmt = std::make_shared(); + asmt->initialize(); + return asmt; +} + +std::shared_ptr MbD::ASMTGearJoint::mbdClassNew() { return CREATE::With(); } diff --git a/OndselSolver/ASMTGearJoint.h b/OndselSolver/ASMTGearJoint.h index 084ac93..89fb9ef 100644 --- a/OndselSolver/ASMTGearJoint.h +++ b/OndselSolver/ASMTGearJoint.h @@ -15,7 +15,8 @@ namespace MbD { { // public: - std::shared_ptr mbdClassNew() override; + static std::shared_ptr With(); + std::shared_ptr mbdClassNew() override; void parseASMT(std::vector& lines) override; void readRadiusI(std::vector& lines); void readRadiusJ(std::vector& lines); diff --git a/OndselSolver/ASMTGeneralMotion.cpp b/OndselSolver/ASMTGeneralMotion.cpp index c661af9..32d5515 100644 --- a/OndselSolver/ASMTGeneralMotion.cpp +++ b/OndselSolver/ASMTGeneralMotion.cpp @@ -19,6 +19,13 @@ using namespace MbD; +std::shared_ptr MbD::ASMTGeneralMotion::With() +{ + auto asmt = std::make_shared(); + asmt->initialize(); + return asmt; +} + void MbD::ASMTGeneralMotion::parseASMT(std::vector& lines) { readName(lines); @@ -78,7 +85,7 @@ void MbD::ASMTGeneralMotion::readRotationOrder(std::vector& lines) lines.erase(lines.begin()); } -std::shared_ptr MbD::ASMTGeneralMotion::mbdClassNew() +std::shared_ptr MbD::ASMTGeneralMotion::mbdClassNew() { return CREATE::With(); } @@ -96,21 +103,21 @@ void MbD::ASMTGeneralMotion::createMbD(std::shared_ptr mbdSys, std::shar //rIJI userFunc = std::make_shared(rIJI->at(0), 1.0); parser->parseUserFunction(userFunc); - auto geoX = parser->stack->top(); + auto& geoX = parser->stack->top(); geoX = Symbolic::times(geoX, sptrConstant(1.0 / mbdUnits->length)); geoX->createMbD(mbdSys, mbdUnits); auto xBlk = geoX->simplified(geoX); userFunc = std::make_shared(rIJI->at(1), 1.0); parser->parseUserFunction(userFunc); - auto geoY = parser->stack->top(); + auto& geoY = parser->stack->top(); geoY = Symbolic::times(geoY, sptrConstant(1.0 / mbdUnits->length)); geoY->createMbD(mbdSys, mbdUnits); auto yBlk = geoY->simplified(geoY); userFunc = std::make_shared(rIJI->at(2), 1.0); parser->parseUserFunction(userFunc); - auto geoZ = parser->stack->top(); + auto& geoZ = parser->stack->top(); geoZ = Symbolic::times(geoZ, sptrConstant(1.0 / mbdUnits->length)); geoZ->createMbD(mbdSys, mbdUnits); auto zBlk = geoZ->simplified(geoZ); @@ -121,21 +128,21 @@ void MbD::ASMTGeneralMotion::createMbD(std::shared_ptr mbdSys, std::shar //angIJJ userFunc = std::make_shared(angIJJ->at(0), 1.0); parser->parseUserFunction(userFunc); - auto geoPhi = parser->stack->top(); + auto& geoPhi = parser->stack->top(); geoPhi = Symbolic::times(geoPhi, sptrConstant(1.0 / mbdUnits->angle)); geoPhi->createMbD(mbdSys, mbdUnits); auto phiBlk = geoPhi->simplified(geoPhi); userFunc = std::make_shared(angIJJ->at(1), 1.0); parser->parseUserFunction(userFunc); - auto geoThe = parser->stack->top(); + auto& geoThe = parser->stack->top(); geoThe = Symbolic::times(geoThe, sptrConstant(1.0 / mbdUnits->angle)); geoThe->createMbD(mbdSys, mbdUnits); auto theBlk = geoThe->simplified(geoThe); userFunc = std::make_shared(angIJJ->at(2), 1.0); parser->parseUserFunction(userFunc); - auto geoPsi = parser->stack->top(); + auto& geoPsi = parser->stack->top(); geoPsi = Symbolic::times(geoPsi, sptrConstant(1.0 / mbdUnits->angle)); geoPsi->createMbD(mbdSys, mbdUnits); auto psiBlk = geoPsi->simplified(geoPsi); diff --git a/OndselSolver/ASMTGeneralMotion.h b/OndselSolver/ASMTGeneralMotion.h index da925aa..b3261da 100644 --- a/OndselSolver/ASMTGeneralMotion.h +++ b/OndselSolver/ASMTGeneralMotion.h @@ -15,11 +15,12 @@ namespace MbD { { // public: + static std::shared_ptr With(); void parseASMT(std::vector& lines) override; void readrIJI(std::vector& lines); void readangIJJ(std::vector& lines); void readRotationOrder(std::vector& lines); - std::shared_ptr mbdClassNew() override; + std::shared_ptr mbdClassNew() override; void createMbD(std::shared_ptr mbdSys, std::shared_ptr mbdUnits) override; void storeOnLevel(std::ofstream& os, size_t level) override; void storeOnTimeSeries(std::ofstream& os) override; diff --git a/OndselSolver/ASMTItem.cpp b/OndselSolver/ASMTItem.cpp index 8b075a3..93057e4 100644 --- a/OndselSolver/ASMTItem.cpp +++ b/OndselSolver/ASMTItem.cpp @@ -58,7 +58,7 @@ void MbD::ASMTItem::parseASMT(std::vector&) std::string MbD::ASMTItem::popOffTop(std::vector& args) { - auto str = args.at(0); //Must copy string + std::string str = args.at(0); //Must copy string args.erase(args.begin()); return str; } diff --git a/OndselSolver/ASMTItemIJ.cpp b/OndselSolver/ASMTItemIJ.cpp index 6b73621..6256dab 100644 --- a/OndselSolver/ASMTItemIJ.cpp +++ b/OndselSolver/ASMTItemIJ.cpp @@ -8,15 +8,21 @@ #include #include "ASMTItemIJ.h" +#include "ASMTAssembly.h" +#include "ASMTMarker.h" +#include "ItemIJ.h" + +using namespace MbD; MbD::ASMTItemIJ::ASMTItemIJ() { - fxs = std::make_shared>(); - fys = std::make_shared>(); - fzs = std::make_shared>(); - txs = std::make_shared>(); - tys = std::make_shared>(); - tzs = std::make_shared>(); + //Use initialize to be consistent with Smalltalk Twin + //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::initialize() @@ -144,3 +150,28 @@ void MbD::ASMTItemIJ::storeOnTimeSeries(std::ofstream& os) } os << std::endl; } + +void MbD::ASMTItemIJ::parseASMT(std::vector& lines) +{ + readName(lines); + readMarkerI(lines); + readMarkerJ(lines); +} + +void MbD::ASMTItemIJ::createMbD(std::shared_ptr mbdSys, std::shared_ptr mbdUnits) +{ + //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); +} + +std::shared_ptr MbD::ASMTItemIJ::mbdClassNew() +{ + assert(false); + return std::shared_ptr(); +} diff --git a/OndselSolver/ASMTItemIJ.h b/OndselSolver/ASMTItemIJ.h index c1118db..065acef 100644 --- a/OndselSolver/ASMTItemIJ.h +++ b/OndselSolver/ASMTItemIJ.h @@ -29,6 +29,9 @@ namespace MbD { void readTZonIs(std::vector& lines); void storeOnLevel(std::ofstream& os, size_t level) override; void storeOnTimeSeries(std::ofstream& os) override; + void parseASMT(std::vector& lines) override; + void createMbD(std::shared_ptr mbdSys, std::shared_ptr mbdUnits) override; + virtual std::shared_ptr mbdClassNew(); std::string markerI, markerJ; FRowDsptr fxs, fys, fzs, txs, tys, tzs; diff --git a/OndselSolver/ASMTJoint.cpp b/OndselSolver/ASMTJoint.cpp index 79e3372..063c0c1 100644 --- a/OndselSolver/ASMTJoint.cpp +++ b/OndselSolver/ASMTJoint.cpp @@ -8,14 +8,15 @@ #include #include "ASMTJoint.h" +#include "Joint.h" using namespace MbD; -void MbD::ASMTJoint::parseASMT(std::vector& lines) +std::shared_ptr MbD::ASMTJoint::With() { - readName(lines); - readMarkerI(lines); - readMarkerJ(lines); + auto asmt = std::make_shared(); + asmt->initialize(); + return asmt; } void MbD::ASMTJoint::readJointSeries(std::vector& lines) @@ -53,3 +54,10 @@ void MbD::ASMTJoint::storeOnTimeSeries(std::ofstream& os) os << label << "Series\t" << fullName("") << std::endl; ASMTItemIJ::storeOnTimeSeries(os); } + +void MbD::ASMTJoint::createMbD(std::shared_ptr mbdSys, std::shared_ptr mbdUnits) +{ + ASMTConstraintSet::createMbD(mbdSys, mbdUnits); + auto mbdJt = std::static_pointer_cast(mbdObject); + mbdSys->addJoint(mbdJt); +} diff --git a/OndselSolver/ASMTJoint.h b/OndselSolver/ASMTJoint.h index fc02378..eab37cc 100644 --- a/OndselSolver/ASMTJoint.h +++ b/OndselSolver/ASMTJoint.h @@ -17,10 +17,11 @@ namespace MbD { { // public: - void parseASMT(std::vector& lines) override; + static std::shared_ptr With(); void readJointSeries(std::vector& lines); void storeOnLevel(std::ofstream& os, size_t level) override; void storeOnTimeSeries(std::ofstream& os) override; + void createMbD(std::shared_ptr mbdSys, std::shared_ptr mbdUnits) override; std::shared_ptr>> jointSeries; diff --git a/OndselSolver/ASMTLimit.cpp b/OndselSolver/ASMTLimit.cpp new file mode 100644 index 0000000..efd8059 --- /dev/null +++ b/OndselSolver/ASMTLimit.cpp @@ -0,0 +1,103 @@ +#include "ASMTLimit.h" +#include "ASMTAssembly.h" +#include "ASMTJoint.h" +#include "SymbolicParser.h" +#include "BasicUserFunction.h" +#include "Constant.h" + +using namespace MbD; + +void MbD::ASMTLimit::initMarkers() +{ + if (motionJoint == "") { + assert(markerI != ""); + assert(markerJ != ""); + } + else { + auto jt = root()->jointAt(motionJoint); + markerI = jt->markerI; + markerJ = jt->markerJ; + } +} + +void MbD::ASMTLimit::storeOnLevel(std::ofstream& os, size_t level) +{ + assert(false); +} + +void MbD::ASMTLimit::readMotionJoint(std::vector& lines) +{ + assert(readStringOffTop(lines) == "MotionJoint"); + motionJoint = readStringOffTop(lines); +} + +void MbD::ASMTLimit::readLimit(std::vector& lines) +{ + assert(readStringOffTop(lines) == "Limit"); + limit = readStringOffTop(lines); +} + +void MbD::ASMTLimit::readType(std::vector& lines) +{ + assert(readStringOffTop(lines) == "Type"); + type = readStringOffTop(lines); +} + +void MbD::ASMTLimit::readTol(std::vector& lines) +{ + assert(readStringOffTop(lines) == "Tol"); + tol = readStringOffTop(lines); +} + +void MbD::ASMTLimit::parseASMT(std::vector& lines) +{ + ASMTConstraintSet::parseASMT(lines); + readMotionJoint(lines); + readLimit(lines); + readType(lines); + readTol(lines); +} + +void MbD::ASMTLimit::createMbD(std::shared_ptr mbdSys, std::shared_ptr mbdUnits) +{ + ASMTConstraintSet::createMbD(mbdSys, mbdUnits); + auto limitIJ = std::static_pointer_cast(mbdObject); + mbdSys->addLimit(limitIJ); + // + auto parser = std::make_shared(); + parser->owner = this; + std::shared_ptr userFunc; + // + userFunc = std::make_shared(limit, 1.0); + parser->parseUserFunction(userFunc); + auto& geolimit = parser->stack->top(); + geolimit = Symbolic::times(geolimit, sptrConstant(1.0 / mbdUnits->angle)); + geolimit->createMbD(mbdSys, mbdUnits); + geolimit = geolimit->simplified(geolimit); + limitIJ->limit = geolimit->getValue(); + // + limitIJ->type = type; + // + userFunc = std::make_shared(tol, 1.0); + parser->parseUserFunction(userFunc); + auto& geotol = parser->stack->top(); + geotol = Symbolic::times(geotol, sptrConstant(1.0 / mbdUnits->angle)); + geotol->createMbD(mbdSys, mbdUnits); + geotol = geotol->simplified(geotol); + limitIJ->tol = geotol->getValue(); +} + +void MbD::ASMTLimit::settype(std::string _type) +{ + type = _type; +} + +void MbD::ASMTLimit::setlimit(std::string _limit) +{ + limit = _limit; +} + +void MbD::ASMTLimit::settol(std::string _tol) +{ + tol = _tol; +} diff --git a/OndselSolver/ASMTLimit.h b/OndselSolver/ASMTLimit.h new file mode 100644 index 0000000..fc50c62 --- /dev/null +++ b/OndselSolver/ASMTLimit.h @@ -0,0 +1,35 @@ +/*************************************************************************** + * Copyright (c) 2023 Ondsel, Inc. * + * * + * This file is part of OndselSolver. * + * * + * See LICENSE file for details about copyright. * + ***************************************************************************/ + +#pragma once + +#include "ASMTConstraintSet.h" +#include "ForceTorqueData.h" + +namespace MbD { + class ASMTLimit : public ASMTConstraintSet + { + // + public: + virtual void initMarkers(); + void storeOnLevel(std::ofstream& os, size_t level) override; + void readMotionJoint(std::vector& lines); + void readLimit(std::vector& lines); + void readType(std::vector& lines); + void readTol(std::vector& lines); + void parseASMT(std::vector& lines) override; + void createMbD(std::shared_ptr mbdSys, std::shared_ptr mbdUnits) override; + void settype(std::string _type); + void setlimit(std::string _limit); + void settol(std::string _tol); + + std::string motionJoint, type, limit, tol; + + }; +} + diff --git a/OndselSolver/ASMTLineInPlaneJoint.cpp b/OndselSolver/ASMTLineInPlaneJoint.cpp index 865e2a4..e0d4311 100644 --- a/OndselSolver/ASMTLineInPlaneJoint.cpp +++ b/OndselSolver/ASMTLineInPlaneJoint.cpp @@ -12,7 +12,14 @@ using namespace MbD; -std::shared_ptr MbD::ASMTLineInPlaneJoint::mbdClassNew() +std::shared_ptr MbD::ASMTLineInPlaneJoint::With() +{ + auto asmt = std::make_shared(); + asmt->initialize(); + return asmt; +} + +std::shared_ptr MbD::ASMTLineInPlaneJoint::mbdClassNew() { return CREATE::With(); } diff --git a/OndselSolver/ASMTLineInPlaneJoint.h b/OndselSolver/ASMTLineInPlaneJoint.h index 3918c2e..87a7064 100644 --- a/OndselSolver/ASMTLineInPlaneJoint.h +++ b/OndselSolver/ASMTLineInPlaneJoint.h @@ -15,7 +15,8 @@ namespace MbD { { // public: - std::shared_ptr mbdClassNew() override; + static std::shared_ptr With(); + std::shared_ptr mbdClassNew() override; }; } diff --git a/OndselSolver/ASMTMarker.cpp b/OndselSolver/ASMTMarker.cpp index 720a902..b22e695 100644 --- a/OndselSolver/ASMTMarker.cpp +++ b/OndselSolver/ASMTMarker.cpp @@ -17,6 +17,13 @@ using namespace MbD; +std::shared_ptr MbD::ASMTMarker::With() +{ + auto asmt = std::make_shared(); + asmt->initialize(); + return asmt; +} + void ASMTMarker::parseASMT(std::vector& lines) { readName(lines); diff --git a/OndselSolver/ASMTMarker.h b/OndselSolver/ASMTMarker.h index 9677319..941c394 100644 --- a/OndselSolver/ASMTMarker.h +++ b/OndselSolver/ASMTMarker.h @@ -18,6 +18,7 @@ namespace MbD { { // public: + static std::shared_ptr With(); void parseASMT(std::vector& lines) override; FColDsptr rpmp(); FMatDsptr aApm(); diff --git a/OndselSolver/ASMTMotion.cpp b/OndselSolver/ASMTMotion.cpp index 8432ec7..79108c7 100644 --- a/OndselSolver/ASMTMotion.cpp +++ b/OndselSolver/ASMTMotion.cpp @@ -8,6 +8,7 @@ #include #include "ASMTMotion.h" +#include "Joint.h" using namespace MbD; @@ -42,3 +43,10 @@ void ASMTMotion::storeOnTimeSeries(std::ofstream&) { assert(false); } + +void MbD::ASMTMotion::createMbD(std::shared_ptr mbdSys, std::shared_ptr mbdUnits) +{ + ASMTConstraintSet::createMbD(mbdSys, mbdUnits); + auto mbdJt = std::static_pointer_cast(mbdObject); + mbdSys->addJoint(mbdJt); +} diff --git a/OndselSolver/ASMTMotion.h b/OndselSolver/ASMTMotion.h index f388ece..a131422 100644 --- a/OndselSolver/ASMTMotion.h +++ b/OndselSolver/ASMTMotion.h @@ -20,6 +20,7 @@ namespace MbD { virtual void initMarkers(); void storeOnLevel(std::ofstream& os, size_t level) override; void storeOnTimeSeries(std::ofstream& os) override; + void createMbD(std::shared_ptr mbdSys, std::shared_ptr mbdUnits) override; std::shared_ptr>> motionSeries; diff --git a/OndselSolver/ASMTNoRotationJoint.cpp b/OndselSolver/ASMTNoRotationJoint.cpp index c8d8dae..e7a69fb 100644 --- a/OndselSolver/ASMTNoRotationJoint.cpp +++ b/OndselSolver/ASMTNoRotationJoint.cpp @@ -12,7 +12,14 @@ using namespace MbD; -std::shared_ptr MbD::ASMTNoRotationJoint::mbdClassNew() +std::shared_ptr MbD::ASMTNoRotationJoint::With() +{ + auto asmt = std::make_shared(); + asmt->initialize(); + return asmt; +} + +std::shared_ptr MbD::ASMTNoRotationJoint::mbdClassNew() { return CREATE::With(); } diff --git a/OndselSolver/ASMTNoRotationJoint.h b/OndselSolver/ASMTNoRotationJoint.h index 4939153..f18078a 100644 --- a/OndselSolver/ASMTNoRotationJoint.h +++ b/OndselSolver/ASMTNoRotationJoint.h @@ -15,7 +15,8 @@ namespace MbD { { // public: - std::shared_ptr mbdClassNew() override; + static std::shared_ptr With(); + std::shared_ptr mbdClassNew() override; void storeOnTimeSeries(std::ofstream& os) override; }; diff --git a/OndselSolver/ASMTParallelAxesJoint.cpp b/OndselSolver/ASMTParallelAxesJoint.cpp index 0bb0628..103c5ec 100644 --- a/OndselSolver/ASMTParallelAxesJoint.cpp +++ b/OndselSolver/ASMTParallelAxesJoint.cpp @@ -12,7 +12,14 @@ using namespace MbD; -std::shared_ptr MbD::ASMTParallelAxesJoint::mbdClassNew() +std::shared_ptr MbD::ASMTParallelAxesJoint::With() +{ + auto asmt = std::make_shared(); + asmt->initialize(); + return asmt; +} + +std::shared_ptr MbD::ASMTParallelAxesJoint::mbdClassNew() { return CREATE::With(); } diff --git a/OndselSolver/ASMTParallelAxesJoint.h b/OndselSolver/ASMTParallelAxesJoint.h index 77576cb..c117dbf 100644 --- a/OndselSolver/ASMTParallelAxesJoint.h +++ b/OndselSolver/ASMTParallelAxesJoint.h @@ -15,7 +15,8 @@ namespace MbD { { // public: - std::shared_ptr mbdClassNew() override; + static std::shared_ptr With(); + std::shared_ptr mbdClassNew() override; }; } diff --git a/OndselSolver/ASMTPart.cpp b/OndselSolver/ASMTPart.cpp index be9a1fc..3e6346b 100644 --- a/OndselSolver/ASMTPart.cpp +++ b/OndselSolver/ASMTPart.cpp @@ -15,6 +15,13 @@ using namespace MbD; +std::shared_ptr MbD::ASMTPart::With() +{ + auto asmt = std::make_shared(); + asmt->initialize(); + return asmt; +} + void MbD::ASMTPart::parseASMT(std::vector& lines) { readName(lines); @@ -57,7 +64,7 @@ void MbD::ASMTPart::readPrincipalMassMarker(std::vector& lines) { assert(lines[0].find("PrincipalMassMarker") != std::string::npos); lines.erase(lines.begin()); - principalMassMarker = std::make_shared(); + principalMassMarker = ASMTPrincipalMassMarker::With(); principalMassMarker->parseASMT(lines); principalMassMarker->owner = this; } diff --git a/OndselSolver/ASMTPart.h b/OndselSolver/ASMTPart.h index fca1860..e3910da 100644 --- a/OndselSolver/ASMTPart.h +++ b/OndselSolver/ASMTPart.h @@ -17,6 +17,7 @@ namespace MbD { { // public: + static std::shared_ptr With(); void parseASMT(std::vector& lines) override; void readFeatureOrder(std::vector& lines); void readPrincipalMassMarker(std::vector& lines); diff --git a/OndselSolver/ASMTPerpendicularJoint.cpp b/OndselSolver/ASMTPerpendicularJoint.cpp index 64658a9..80974dd 100644 --- a/OndselSolver/ASMTPerpendicularJoint.cpp +++ b/OndselSolver/ASMTPerpendicularJoint.cpp @@ -12,7 +12,14 @@ using namespace MbD; -std::shared_ptr MbD::ASMTPerpendicularJoint::mbdClassNew() +std::shared_ptr MbD::ASMTPerpendicularJoint::With() +{ + auto asmt = std::make_shared(); + asmt->initialize(); + return asmt; +} + +std::shared_ptr MbD::ASMTPerpendicularJoint::mbdClassNew() { return CREATE::With(); } diff --git a/OndselSolver/ASMTPerpendicularJoint.h b/OndselSolver/ASMTPerpendicularJoint.h index 0fdfc56..0c6d99a 100644 --- a/OndselSolver/ASMTPerpendicularJoint.h +++ b/OndselSolver/ASMTPerpendicularJoint.h @@ -15,7 +15,8 @@ namespace MbD { { // public: - std::shared_ptr mbdClassNew() override; + static std::shared_ptr With(); + std::shared_ptr mbdClassNew() override; }; } diff --git a/OndselSolver/ASMTPlanarJoint.cpp b/OndselSolver/ASMTPlanarJoint.cpp index 7c904a1..555088c 100644 --- a/OndselSolver/ASMTPlanarJoint.cpp +++ b/OndselSolver/ASMTPlanarJoint.cpp @@ -12,7 +12,14 @@ using namespace MbD; -std::shared_ptr MbD::ASMTPlanarJoint::mbdClassNew() +std::shared_ptr MbD::ASMTPlanarJoint::With() +{ + auto asmt = std::make_shared(); + asmt->initialize(); + return asmt; +} + +std::shared_ptr MbD::ASMTPlanarJoint::mbdClassNew() { return CREATE::With(); } diff --git a/OndselSolver/ASMTPlanarJoint.h b/OndselSolver/ASMTPlanarJoint.h index 8be2a61..8ca4785 100644 --- a/OndselSolver/ASMTPlanarJoint.h +++ b/OndselSolver/ASMTPlanarJoint.h @@ -15,7 +15,8 @@ namespace MbD { { // public: - std::shared_ptr mbdClassNew() override; + static std::shared_ptr With(); + std::shared_ptr mbdClassNew() override; }; } diff --git a/OndselSolver/ASMTPointInLineJoint.cpp b/OndselSolver/ASMTPointInLineJoint.cpp index d1f7013..b38fcac 100644 --- a/OndselSolver/ASMTPointInLineJoint.cpp +++ b/OndselSolver/ASMTPointInLineJoint.cpp @@ -12,7 +12,14 @@ using namespace MbD; -std::shared_ptr MbD::ASMTPointInLineJoint::mbdClassNew() +std::shared_ptr MbD::ASMTPointInLineJoint::With() +{ + auto asmt = std::make_shared(); + asmt->initialize(); + return asmt; +} + +std::shared_ptr MbD::ASMTPointInLineJoint::mbdClassNew() { return CREATE::With(); } diff --git a/OndselSolver/ASMTPointInLineJoint.h b/OndselSolver/ASMTPointInLineJoint.h index 7abd0c2..6b019d7 100644 --- a/OndselSolver/ASMTPointInLineJoint.h +++ b/OndselSolver/ASMTPointInLineJoint.h @@ -15,7 +15,8 @@ namespace MbD { { // public: - std::shared_ptr mbdClassNew() override; + static std::shared_ptr With(); + std::shared_ptr mbdClassNew() override; void storeOnTimeSeries(std::ofstream& os) override; }; diff --git a/OndselSolver/ASMTPointInPlaneJoint.cpp b/OndselSolver/ASMTPointInPlaneJoint.cpp index 733055a..a21a730 100644 --- a/OndselSolver/ASMTPointInPlaneJoint.cpp +++ b/OndselSolver/ASMTPointInPlaneJoint.cpp @@ -12,7 +12,14 @@ using namespace MbD; -std::shared_ptr MbD::ASMTPointInPlaneJoint::mbdClassNew() +std::shared_ptr MbD::ASMTPointInPlaneJoint::With() +{ + auto asmt = std::make_shared(); + asmt->initialize(); + return asmt; +} + +std::shared_ptr MbD::ASMTPointInPlaneJoint::mbdClassNew() { return CREATE::With(); } diff --git a/OndselSolver/ASMTPointInPlaneJoint.h b/OndselSolver/ASMTPointInPlaneJoint.h index 6163d62..630333e 100644 --- a/OndselSolver/ASMTPointInPlaneJoint.h +++ b/OndselSolver/ASMTPointInPlaneJoint.h @@ -15,7 +15,8 @@ namespace MbD { { // public: - std::shared_ptr mbdClassNew() override; + static std::shared_ptr With(); + std::shared_ptr mbdClassNew() override; void storeOnTimeSeries(std::ofstream& os) override; }; diff --git a/OndselSolver/ASMTPrincipalMassMarker.cpp b/OndselSolver/ASMTPrincipalMassMarker.cpp index 77c206c..7f7b359 100644 --- a/OndselSolver/ASMTPrincipalMassMarker.cpp +++ b/OndselSolver/ASMTPrincipalMassMarker.cpp @@ -17,6 +17,13 @@ MbD::ASMTPrincipalMassMarker::ASMTPrincipalMassMarker() name = "MassMarker"; } +std::shared_ptr MbD::ASMTPrincipalMassMarker::With() +{ + auto asmt = std::make_shared(); + asmt->initialize(); + return asmt; +} + void MbD::ASMTPrincipalMassMarker::parseASMT(std::vector& lines) { auto pos = lines[0].find_first_not_of("\t"); diff --git a/OndselSolver/ASMTPrincipalMassMarker.h b/OndselSolver/ASMTPrincipalMassMarker.h index 0385e96..8c974e8 100644 --- a/OndselSolver/ASMTPrincipalMassMarker.h +++ b/OndselSolver/ASMTPrincipalMassMarker.h @@ -16,6 +16,7 @@ namespace MbD { // public: ASMTPrincipalMassMarker(); + static std::shared_ptr With(); void parseASMT(std::vector& lines) override; void setMass(double mass); void setDensity(double density); diff --git a/OndselSolver/ASMTRackPinionJoint.cpp b/OndselSolver/ASMTRackPinionJoint.cpp index d52accd..ae8b66f 100644 --- a/OndselSolver/ASMTRackPinionJoint.cpp +++ b/OndselSolver/ASMTRackPinionJoint.cpp @@ -12,7 +12,14 @@ using namespace MbD; -std::shared_ptr MbD::ASMTRackPinionJoint::mbdClassNew() +std::shared_ptr MbD::ASMTRackPinionJoint::With() +{ + auto asmt = std::make_shared(); + asmt->initialize(); + return asmt; +} + +std::shared_ptr MbD::ASMTRackPinionJoint::mbdClassNew() { return CREATE::With(); } diff --git a/OndselSolver/ASMTRackPinionJoint.h b/OndselSolver/ASMTRackPinionJoint.h index 6c2e0e7..f95cda1 100644 --- a/OndselSolver/ASMTRackPinionJoint.h +++ b/OndselSolver/ASMTRackPinionJoint.h @@ -15,7 +15,8 @@ namespace MbD { { // public: - std::shared_ptr mbdClassNew() override; + static std::shared_ptr With(); + std::shared_ptr mbdClassNew() override; void parseASMT(std::vector& lines) override; void readPitchRadius(std::vector& lines); void createMbD(std::shared_ptr mbdSys, std::shared_ptr mbdUnits) override; diff --git a/OndselSolver/ASMTRevCylJoint.cpp b/OndselSolver/ASMTRevCylJoint.cpp index 0369495..e72ddae 100644 --- a/OndselSolver/ASMTRevCylJoint.cpp +++ b/OndselSolver/ASMTRevCylJoint.cpp @@ -12,7 +12,14 @@ using namespace MbD; -std::shared_ptr MbD::ASMTRevCylJoint::mbdClassNew() +std::shared_ptr MbD::ASMTRevCylJoint::With() +{ + auto asmt = std::make_shared(); + asmt->initialize(); + return asmt; +} + +std::shared_ptr MbD::ASMTRevCylJoint::mbdClassNew() { return CREATE::With(); } diff --git a/OndselSolver/ASMTRevCylJoint.h b/OndselSolver/ASMTRevCylJoint.h index 15c5eb0..e084841 100644 --- a/OndselSolver/ASMTRevCylJoint.h +++ b/OndselSolver/ASMTRevCylJoint.h @@ -15,7 +15,8 @@ namespace MbD { { // public: - std::shared_ptr mbdClassNew() override; + static std::shared_ptr With(); + std::shared_ptr mbdClassNew() override; }; } diff --git a/OndselSolver/ASMTRevRevJoint.cpp b/OndselSolver/ASMTRevRevJoint.cpp index ebdc9f0..0b9984b 100644 --- a/OndselSolver/ASMTRevRevJoint.cpp +++ b/OndselSolver/ASMTRevRevJoint.cpp @@ -12,7 +12,14 @@ using namespace MbD; -std::shared_ptr MbD::ASMTRevRevJoint::mbdClassNew() +std::shared_ptr MbD::ASMTRevRevJoint::With() +{ + auto asmt = std::make_shared(); + asmt->initialize(); + return asmt; +} + +std::shared_ptr MbD::ASMTRevRevJoint::mbdClassNew() { return CREATE::With(); } diff --git a/OndselSolver/ASMTRevRevJoint.h b/OndselSolver/ASMTRevRevJoint.h index da2a091..c57c2f7 100644 --- a/OndselSolver/ASMTRevRevJoint.h +++ b/OndselSolver/ASMTRevRevJoint.h @@ -15,7 +15,8 @@ namespace MbD { { // public: - std::shared_ptr mbdClassNew() override; + static std::shared_ptr With(); + std::shared_ptr mbdClassNew() override; }; } diff --git a/OndselSolver/ASMTRevoluteJoint.cpp b/OndselSolver/ASMTRevoluteJoint.cpp index 4c3171b..87c3526 100644 --- a/OndselSolver/ASMTRevoluteJoint.cpp +++ b/OndselSolver/ASMTRevoluteJoint.cpp @@ -12,7 +12,14 @@ using namespace MbD; -std::shared_ptr MbD::ASMTRevoluteJoint::mbdClassNew() +std::shared_ptr MbD::ASMTRevoluteJoint::With() +{ + auto asmt = std::make_shared(); + asmt->initialize(); + return asmt; +} + +std::shared_ptr MbD::ASMTRevoluteJoint::mbdClassNew() { return CREATE::With(); } diff --git a/OndselSolver/ASMTRevoluteJoint.h b/OndselSolver/ASMTRevoluteJoint.h index 56969c7..95d58ae 100644 --- a/OndselSolver/ASMTRevoluteJoint.h +++ b/OndselSolver/ASMTRevoluteJoint.h @@ -15,7 +15,8 @@ namespace MbD { { // public: - std::shared_ptr mbdClassNew() override; + static std::shared_ptr With(); + std::shared_ptr mbdClassNew() override; //void storeOnTimeSeries(std::ofstream& os) override; }; diff --git a/OndselSolver/ASMTRotationLimit.cpp b/OndselSolver/ASMTRotationLimit.cpp new file mode 100644 index 0000000..5c4567f --- /dev/null +++ b/OndselSolver/ASMTRotationLimit.cpp @@ -0,0 +1,36 @@ +#include "ASMTRotationLimit.h" +#include "ASMTAssembly.h" +#include "SymbolicParser.h" +#include "BasicUserFunction.h" +#include "Constant.h" +#include "RotationLimitIJ.h" + +using namespace MbD; + +std::shared_ptr MbD::ASMTRotationLimit::With() +{ + auto rotationLimit = std::make_shared(); + rotationLimit->initialize(); + return rotationLimit; +} + +std::shared_ptr MbD::ASMTRotationLimit::mbdClassNew() +{ + return RotationLimitIJ::With(); +} + +void MbD::ASMTRotationLimit::storeOnLevel(std::ofstream& os, size_t level) +{ + storeOnLevelString(os, level, "RotationLimit"); + storeOnLevelString(os, level + 1, "Name"); + storeOnLevelString(os, level + 2, name); + ASMTItemIJ::storeOnLevel(os, level); + storeOnLevelString(os, level + 1, "MotionJoint"); + storeOnLevelString(os, level + 2, motionJoint); + storeOnLevelString(os, level + 1, "Limit"); + storeOnLevelString(os, level + 2, limit); + storeOnLevelString(os, level + 1, "Type"); + storeOnLevelString(os, level + 2, type); + storeOnLevelString(os, level + 1, "Tol"); + storeOnLevelString(os, level + 2, tol); +} diff --git a/OndselSolver/ASMTRotationLimit.h b/OndselSolver/ASMTRotationLimit.h new file mode 100644 index 0000000..3f763c0 --- /dev/null +++ b/OndselSolver/ASMTRotationLimit.h @@ -0,0 +1,23 @@ +/*************************************************************************** + * Copyright (c) 2023 Ondsel, Inc. * + * * + * This file is part of OndselSolver. * + * * + * See LICENSE file for details about copyright. * + ***************************************************************************/ + +#pragma once + +#include "ASMTLimit.h" + +namespace MbD { + class ASMTRotationLimit : public ASMTLimit + { + // + public: + static std::shared_ptr With(); + std::shared_ptr mbdClassNew() override; + void storeOnLevel(std::ofstream& os, size_t level) override; + + }; +} diff --git a/OndselSolver/ASMTRotationalMotion.cpp b/OndselSolver/ASMTRotationalMotion.cpp index fbb130a..89d25f4 100644 --- a/OndselSolver/ASMTRotationalMotion.cpp +++ b/OndselSolver/ASMTRotationalMotion.cpp @@ -18,6 +18,13 @@ using namespace MbD; +std::shared_ptr MbD::ASMTRotationalMotion::With() +{ + auto asmt = std::make_shared(); + asmt->initialize(); + return asmt; +} + void MbD::ASMTRotationalMotion::parseASMT(std::vector& lines) { readName(lines); @@ -67,7 +74,7 @@ void MbD::ASMTRotationalMotion::createMbD(std::shared_ptr mbdSys, std::s parser->variables->insert(std::make_pair("time", geoTime)); auto userFunc = std::make_shared(rotationZ, 1.0); parser->parseUserFunction(userFunc); - auto geoPhi = parser->stack->top(); + auto& geoPhi = parser->stack->top(); //std::cout << *geoPhi << std::endl; geoPhi = Symbolic::times(geoPhi, sptrConstant(1.0 / mbdUnits->angle)); geoPhi->createMbD(mbdSys, mbdUnits); @@ -77,7 +84,7 @@ void MbD::ASMTRotationalMotion::createMbD(std::shared_ptr mbdSys, std::s std::static_pointer_cast(mbdObject)->phiBlk = simple; } -std::shared_ptr MbD::ASMTRotationalMotion::mbdClassNew() +std::shared_ptr MbD::ASMTRotationalMotion::mbdClassNew() { return CREATE::With(); } diff --git a/OndselSolver/ASMTRotationalMotion.h b/OndselSolver/ASMTRotationalMotion.h index c9ecc6f..f8019a9 100644 --- a/OndselSolver/ASMTRotationalMotion.h +++ b/OndselSolver/ASMTRotationalMotion.h @@ -16,12 +16,13 @@ namespace MbD { { // public: + static std::shared_ptr With(); 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::shared_ptr mbdClassNew() override; void setMotionJoint(std::string motionJoint); void setRotationZ(std::string rotZ); void storeOnLevel(std::ofstream& os, size_t level) override; diff --git a/OndselSolver/ASMTScrewJoint.cpp b/OndselSolver/ASMTScrewJoint.cpp index 9499196..3fceb8e 100644 --- a/OndselSolver/ASMTScrewJoint.cpp +++ b/OndselSolver/ASMTScrewJoint.cpp @@ -12,7 +12,14 @@ using namespace MbD; -std::shared_ptr MbD::ASMTScrewJoint::mbdClassNew() +std::shared_ptr MbD::ASMTScrewJoint::With() +{ + auto asmt = std::make_shared(); + asmt->initialize(); + return asmt; +} + +std::shared_ptr MbD::ASMTScrewJoint::mbdClassNew() { return CREATE::With(); } diff --git a/OndselSolver/ASMTScrewJoint.h b/OndselSolver/ASMTScrewJoint.h index e60bced..d9634fc 100644 --- a/OndselSolver/ASMTScrewJoint.h +++ b/OndselSolver/ASMTScrewJoint.h @@ -15,7 +15,8 @@ namespace MbD { { // public: - std::shared_ptr mbdClassNew() override; + static std::shared_ptr With(); + std::shared_ptr mbdClassNew() override; void parseASMT(std::vector& lines) override; void readPitch(std::vector& lines); void createMbD(std::shared_ptr mbdSys, std::shared_ptr mbdUnits) override; diff --git a/OndselSolver/ASMTSimulationParameters.cpp b/OndselSolver/ASMTSimulationParameters.cpp index 7597ed5..1815c76 100644 --- a/OndselSolver/ASMTSimulationParameters.cpp +++ b/OndselSolver/ASMTSimulationParameters.cpp @@ -10,6 +10,13 @@ using namespace MbD; +std::shared_ptr MbD::ASMTSimulationParameters::With() +{ + auto asmt = std::make_shared(); + asmt->initialize(); + return asmt; +} + void MbD::ASMTSimulationParameters::parseASMT(std::vector& lines) { //tstart, tend, hmin, hmax, hout, errorTol; diff --git a/OndselSolver/ASMTSimulationParameters.h b/OndselSolver/ASMTSimulationParameters.h index 60c3410..70305f3 100644 --- a/OndselSolver/ASMTSimulationParameters.h +++ b/OndselSolver/ASMTSimulationParameters.h @@ -15,6 +15,7 @@ namespace MbD { { // public: + static std::shared_ptr With(); void parseASMT(std::vector& lines) override; void settstart(double tstart); void settend(double tend); diff --git a/OndselSolver/ASMTSpatialContainer.h b/OndselSolver/ASMTSpatialContainer.h index ca651d3..ac86254 100644 --- a/OndselSolver/ASMTSpatialContainer.h +++ b/OndselSolver/ASMTSpatialContainer.h @@ -99,7 +99,7 @@ namespace MbD { FRowDsptr axs, ays, azs, alpxs, alpys, alpzs; FRowDsptr invxs, invys, invzs, inomexs, inomeys, inomezs; FRowDsptr inaxs, inays, inazs, inalpxs, inalpys, inalpzs; - std::shared_ptr principalMassMarker = std::make_shared(); + std::shared_ptr principalMassMarker = ASMTPrincipalMassMarker::With(); }; } diff --git a/OndselSolver/ASMTSphSphJoint.cpp b/OndselSolver/ASMTSphSphJoint.cpp index f540be7..8dbdc3c 100644 --- a/OndselSolver/ASMTSphSphJoint.cpp +++ b/OndselSolver/ASMTSphSphJoint.cpp @@ -12,7 +12,14 @@ using namespace MbD; -std::shared_ptr MbD::ASMTSphSphJoint::mbdClassNew() +std::shared_ptr MbD::ASMTSphSphJoint::With() +{ + auto asmt = std::make_shared(); + asmt->initialize(); + return asmt; +} + +std::shared_ptr MbD::ASMTSphSphJoint::mbdClassNew() { return CREATE::With(); } diff --git a/OndselSolver/ASMTSphSphJoint.h b/OndselSolver/ASMTSphSphJoint.h index 92e43b8..5babbf5 100644 --- a/OndselSolver/ASMTSphSphJoint.h +++ b/OndselSolver/ASMTSphSphJoint.h @@ -15,7 +15,8 @@ namespace MbD { { // public: - std::shared_ptr mbdClassNew() override; + static std::shared_ptr With(); + std::shared_ptr mbdClassNew() override; }; } diff --git a/OndselSolver/ASMTSphericalJoint.cpp b/OndselSolver/ASMTSphericalJoint.cpp index 74ea09a..4763db4 100644 --- a/OndselSolver/ASMTSphericalJoint.cpp +++ b/OndselSolver/ASMTSphericalJoint.cpp @@ -12,7 +12,14 @@ using namespace MbD; -std::shared_ptr MbD::ASMTSphericalJoint::mbdClassNew() +std::shared_ptr MbD::ASMTSphericalJoint::With() +{ + auto asmt = std::make_shared(); + asmt->initialize(); + return asmt; +} + +std::shared_ptr MbD::ASMTSphericalJoint::mbdClassNew() { return CREATE::With(); } diff --git a/OndselSolver/ASMTSphericalJoint.h b/OndselSolver/ASMTSphericalJoint.h index 1d8b529..3bf50fe 100644 --- a/OndselSolver/ASMTSphericalJoint.h +++ b/OndselSolver/ASMTSphericalJoint.h @@ -15,7 +15,8 @@ namespace MbD { { // public: - std::shared_ptr mbdClassNew() override; + static std::shared_ptr With(); + std::shared_ptr mbdClassNew() override; void storeOnTimeSeries(std::ofstream& os) override; }; diff --git a/OndselSolver/ASMTTime.cpp b/OndselSolver/ASMTTime.cpp index 0a0ca93..aca06e7 100644 --- a/OndselSolver/ASMTTime.cpp +++ b/OndselSolver/ASMTTime.cpp @@ -15,6 +15,13 @@ using namespace MbD; +std::shared_ptr MbD::ASMTTime::With() +{ + auto asmt = std::make_shared(); + asmt->initialize(); + return asmt; +} + void MbD::ASMTTime::deleteMbD() { xx = nullptr; diff --git a/OndselSolver/ASMTTime.h b/OndselSolver/ASMTTime.h index 667e86e..c64cb7c 100644 --- a/OndselSolver/ASMTTime.h +++ b/OndselSolver/ASMTTime.h @@ -17,6 +17,7 @@ namespace MbD { { // public: + static std::shared_ptr With(); void deleteMbD(); void createMbD(std::shared_ptr mbdSys, std::shared_ptr mbdUnits) override; Symsptr expandUntil(Symsptr sptr, std::shared_ptr> set) override; diff --git a/OndselSolver/ASMTTranslationLimit.cpp b/OndselSolver/ASMTTranslationLimit.cpp new file mode 100644 index 0000000..1949e79 --- /dev/null +++ b/OndselSolver/ASMTTranslationLimit.cpp @@ -0,0 +1,35 @@ +#include "ASMTTranslationLimit.h" +#include "SymbolicParser.h" +#include "BasicUserFunction.h" +#include "Constant.h" +#include "TranslationLimitIJ.h" + +using namespace MbD; + +std::shared_ptr MbD::ASMTTranslationLimit::With() +{ + auto translationLimit = std::make_shared(); + translationLimit->initialize(); + return translationLimit; +} + +std::shared_ptr MbD::ASMTTranslationLimit::mbdClassNew() +{ + return TranslationLimitIJ::With(); +} + +void MbD::ASMTTranslationLimit::storeOnLevel(std::ofstream& os, size_t level) +{ + storeOnLevelString(os, level, "TranslationLimit"); + storeOnLevelString(os, level + 1, "Name"); + storeOnLevelString(os, level + 2, name); + ASMTItemIJ::storeOnLevel(os, level); + storeOnLevelString(os, level + 1, "MotionJoint"); + storeOnLevelString(os, level + 2, motionJoint); + storeOnLevelString(os, level + 1, "Limit"); + storeOnLevelString(os, level + 2, limit); + storeOnLevelString(os, level + 1, "Type"); + storeOnLevelString(os, level + 2, type); + storeOnLevelString(os, level + 1, "Tol"); + storeOnLevelString(os, level + 2, tol); +} diff --git a/OndselSolver/ASMTTranslationLimit.h b/OndselSolver/ASMTTranslationLimit.h new file mode 100644 index 0000000..bc43ef9 --- /dev/null +++ b/OndselSolver/ASMTTranslationLimit.h @@ -0,0 +1,23 @@ +/*************************************************************************** + * Copyright (c) 2023 Ondsel, Inc. * + * * + * This file is part of OndselSolver. * + * * + * See LICENSE file for details about copyright. * + ***************************************************************************/ + +#pragma once + +#include "ASMTLimit.h" + +namespace MbD { + class ASMTTranslationLimit : public ASMTLimit + { + // + public: + static std::shared_ptr With(); + std::shared_ptr mbdClassNew() override; + void storeOnLevel(std::ofstream& os, size_t level) override; + + }; +} diff --git a/OndselSolver/ASMTTranslationalJoint.cpp b/OndselSolver/ASMTTranslationalJoint.cpp index ef0886c..054f848 100644 --- a/OndselSolver/ASMTTranslationalJoint.cpp +++ b/OndselSolver/ASMTTranslationalJoint.cpp @@ -12,7 +12,14 @@ using namespace MbD; -std::shared_ptr MbD::ASMTTranslationalJoint::mbdClassNew() +std::shared_ptr MbD::ASMTTranslationalJoint::With() +{ + auto asmt = std::make_shared(); + asmt->initialize(); + return asmt; +} + +std::shared_ptr MbD::ASMTTranslationalJoint::mbdClassNew() { return CREATE::With(); } diff --git a/OndselSolver/ASMTTranslationalJoint.h b/OndselSolver/ASMTTranslationalJoint.h index 85272ff..fdf55e9 100644 --- a/OndselSolver/ASMTTranslationalJoint.h +++ b/OndselSolver/ASMTTranslationalJoint.h @@ -15,7 +15,8 @@ namespace MbD { { // public: - std::shared_ptr mbdClassNew() override; + static std::shared_ptr With(); + std::shared_ptr mbdClassNew() override; void storeOnTimeSeries(std::ofstream& os) override; diff --git a/OndselSolver/ASMTTranslationalMotion.cpp b/OndselSolver/ASMTTranslationalMotion.cpp index 4f3c52c..920b350 100644 --- a/OndselSolver/ASMTTranslationalMotion.cpp +++ b/OndselSolver/ASMTTranslationalMotion.cpp @@ -41,13 +41,13 @@ void MbD::ASMTTranslationalMotion::createMbD(std::shared_ptr mbdSys, std parser->variables->insert(std::make_pair("time", geoTime)); auto userFunc = std::make_shared(translationZ, 1.0); parser->parseUserFunction(userFunc); - auto zIJ = parser->stack->top(); + auto& zIJ = parser->stack->top(); zIJ = Symbolic::times(zIJ, sptrConstant(1.0 / mbdUnits->length)); zIJ->createMbD(mbdSys, mbdUnits); std::static_pointer_cast(mbdObject)->zBlk = zIJ->simplified(zIJ); } -std::shared_ptr MbD::ASMTTranslationalMotion::mbdClassNew() +std::shared_ptr MbD::ASMTTranslationalMotion::mbdClassNew() { return CREATE::With(); } diff --git a/OndselSolver/ASMTTranslationalMotion.h b/OndselSolver/ASMTTranslationalMotion.h index 9ea658a..8c907d4 100644 --- a/OndselSolver/ASMTTranslationalMotion.h +++ b/OndselSolver/ASMTTranslationalMotion.h @@ -18,7 +18,7 @@ namespace MbD { 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::shared_ptr mbdClassNew() override; void readMotionJoint(std::vector& lines); void readTranslationZ(std::vector& lines); void storeOnLevel(std::ofstream& os, size_t level) override; diff --git a/OndselSolver/ASMTUniversalJoint.cpp b/OndselSolver/ASMTUniversalJoint.cpp index 95685c1..13df1ef 100644 --- a/OndselSolver/ASMTUniversalJoint.cpp +++ b/OndselSolver/ASMTUniversalJoint.cpp @@ -12,7 +12,14 @@ using namespace MbD; -std::shared_ptr MbD::ASMTUniversalJoint::mbdClassNew() +std::shared_ptr MbD::ASMTUniversalJoint::With() +{ + auto asmt = std::make_shared(); + asmt->initialize(); + return asmt; +} + +std::shared_ptr MbD::ASMTUniversalJoint::mbdClassNew() { return CREATE::With(); } diff --git a/OndselSolver/ASMTUniversalJoint.h b/OndselSolver/ASMTUniversalJoint.h index d9e17fc..4e31f17 100644 --- a/OndselSolver/ASMTUniversalJoint.h +++ b/OndselSolver/ASMTUniversalJoint.h @@ -5,19 +5,20 @@ * * * See LICENSE file for details about copyright. * ***************************************************************************/ - + #pragma once #include "ASMTAtPointJoint.h" namespace MbD { - class ASMTUniversalJoint : public ASMTAtPointJoint - { - // - public: - std::shared_ptr mbdClassNew() override; - void storeOnTimeSeries(std::ofstream& os) override; + class ASMTUniversalJoint : public ASMTAtPointJoint + { + // + public: + static std::shared_ptr With(); + std::shared_ptr mbdClassNew() override; + void storeOnTimeSeries(std::ofstream& os) override; - }; + }; } diff --git a/OndselSolver/AccNewtonRaphson.cpp b/OndselSolver/AccNewtonRaphson.cpp index 9a8f2ff..683df36 100644 --- a/OndselSolver/AccNewtonRaphson.cpp +++ b/OndselSolver/AccNewtonRaphson.cpp @@ -20,7 +20,7 @@ using namespace MbD; void AccNewtonRaphson::askSystemToUpdate() { - system->partsJointsMotionsForcesTorquesDo([&](std::shared_ptr item) { item->postAccICIteration(); }); + system->partsJointsMotionsLimitsForcesTorquesDo([&](std::shared_ptr item) { item->postAccICIteration(); }); } void AccNewtonRaphson::assignEquationNumbers() @@ -57,7 +57,7 @@ void AccNewtonRaphson::assignEquationNumbers() void AccNewtonRaphson::fillPyPx() { pypx->zeroSelf(); - system->partsJointsMotionsForcesTorquesDo([&](std::shared_ptr item) { + system->partsJointsMotionsLimitsForcesTorquesDo([&](std::shared_ptr item) { item->fillAccICIterJacob(pypx); }); } @@ -65,7 +65,7 @@ void AccNewtonRaphson::fillPyPx() void AccNewtonRaphson::fillY() { y->zeroSelf(); - system->partsJointsMotionsForcesTorquesDo([&](std::shared_ptr item) { + system->partsJointsMotionsLimitsForcesTorquesDo([&](std::shared_ptr item) { item->fillAccICIterError(y); //std::cout << item->name << *y << std::endl; }); @@ -97,7 +97,7 @@ void AccNewtonRaphson::incrementIterNo() void AccNewtonRaphson::initializeGlobally() { SystemNewtonRaphson::initializeGlobally(); - system->partsJointsMotionsDo([&](std::shared_ptr item) { item->fillqsuddotlam(x); }); + system->partsJointsMotionsLimitsDo([&](std::shared_ptr item) { item->fillqsuddotlam(x); }); } void AccNewtonRaphson::logSingularMatrixMessage() @@ -110,17 +110,17 @@ void AccNewtonRaphson::logSingularMatrixMessage() void AccNewtonRaphson::passRootToSystem() { - system->partsJointsMotionsDo([&](std::shared_ptr item) { item->setqsuddotlam(x); }); + system->partsJointsMotionsLimitsDo([&](std::shared_ptr item) { item->setqsuddotlam(x); }); } void AccNewtonRaphson::postRun() { - system->partsJointsMotionsForcesTorquesDo([&](std::shared_ptr item) { item->postAccIC(); }); + system->partsJointsMotionsLimitsForcesTorquesDo([&](std::shared_ptr item) { item->postAccIC(); }); } void AccNewtonRaphson::preRun() { - system->partsJointsMotionsForcesTorquesDo([&](std::shared_ptr item) { item->preAccIC(); }); + system->partsJointsMotionsLimitsForcesTorquesDo([&](std::shared_ptr item) { item->preAccIC(); }); } void MbD::AccNewtonRaphson::handleSingularMatrix() diff --git a/OndselSolver/AngleZConstraintIJ.cpp b/OndselSolver/AngleZConstraintIJ.cpp new file mode 100644 index 0000000..7a65dfa --- /dev/null +++ b/OndselSolver/AngleZConstraintIJ.cpp @@ -0,0 +1,94 @@ +/*************************************************************************** + * Copyright (c) 2023 Ondsel, Inc. * + * * + * This file is part of OndselSolver. * + * * + * See LICENSE file for details about copyright. * + ***************************************************************************/ + +#include "AngleZConstraintIJ.h" +#include "AngleZConstraintIqcJqc.h" +#include "EndFrameqc.h" + +using namespace MbD; + +MbD::AngleZConstraintIJ::AngleZConstraintIJ(EndFrmsptr frmi, EndFrmsptr frmj) : ConstraintIJ(frmi, frmj) +{ +} + +std::shared_ptr MbD::AngleZConstraintIJ::With(EndFrmsptr frmi, EndFrmsptr frmj) +{ + assert(frmi->isEndFrameqc()); + assert(frmj->isEndFrameqc()); + auto angleZConIJ = std::make_shared(frmi, frmj); + angleZConIJ->initialize(); + return angleZConIJ; +} + +void MbD::AngleZConstraintIJ::calcPostDynCorrectorIteration() +{ + auto thez = thezIeJe->value(); + aG = thez - aConstant; +} + +void MbD::AngleZConstraintIJ::initthezIeJe() +{ + assert(false); +} + +void MbD::AngleZConstraintIJ::initialize() +{ + ConstraintIJ::initialize(); + this->initthezIeJe(); +} + +void MbD::AngleZConstraintIJ::initializeGlobally() +{ + thezIeJe->initializeGlobally(); +} + +void MbD::AngleZConstraintIJ::initializeLocally() +{ + thezIeJe->initializeLocally(); +} + +void MbD::AngleZConstraintIJ::postInput() +{ + assert(aConstant != std::numeric_limits::min()); + ConstraintIJ::postInput(); +} + +void MbD::AngleZConstraintIJ::postPosICIteration() +{ + thezIeJe->postPosICIteration(); + ConstraintIJ::postPosICIteration(); +} + +void MbD::AngleZConstraintIJ::preAccIC() +{ + thezIeJe->preAccIC(); + ConstraintIJ::preAccIC(); +} + +void MbD::AngleZConstraintIJ::prePosIC() +{ + thezIeJe->prePosIC(); + ConstraintIJ::prePosIC(); +} + +void MbD::AngleZConstraintIJ::preVelIC() +{ + thezIeJe->preVelIC(); + ConstraintIJ::preVelIC(); +} + +void MbD::AngleZConstraintIJ::simUpdateAll() +{ + thezIeJe->simUpdateAll(); + ConstraintIJ::simUpdateAll(); +} + +ConstraintType MbD::AngleZConstraintIJ::type() +{ + return essential; +} diff --git a/OndselSolver/AngleZConstraintIJ.h b/OndselSolver/AngleZConstraintIJ.h new file mode 100644 index 0000000..94c85b0 --- /dev/null +++ b/OndselSolver/AngleZConstraintIJ.h @@ -0,0 +1,38 @@ +/*************************************************************************** + * Copyright (c) 2023 Ondsel, Inc. * + * * + * This file is part of OndselSolver. * + * * + * See LICENSE file for details about copyright. * + ***************************************************************************/ + +#pragma once + +#include "ConstraintIJ.h" +#include "AngleZIecJec.h" + +namespace MbD { + class AngleZConstraintIJ : public ConstraintIJ + { + //thezIeJe + public: + AngleZConstraintIJ(EndFrmsptr frmi, EndFrmsptr frmj); + + static std::shared_ptr With(EndFrmsptr frmi, EndFrmsptr frmj); + + void calcPostDynCorrectorIteration() override; + virtual void initthezIeJe(); + void initialize() override; + void initializeGlobally() override; + void initializeLocally() override; + void postInput() override; + void postPosICIteration() override; + void preAccIC() override; + void prePosIC() override; + void preVelIC() override; + void simUpdateAll() override; + ConstraintType type() override; + + std::shared_ptr thezIeJe; + }; +} diff --git a/OndselSolver/AngleZConstraintIqcJc.cpp b/OndselSolver/AngleZConstraintIqcJc.cpp new file mode 100644 index 0000000..511b7ed --- /dev/null +++ b/OndselSolver/AngleZConstraintIqcJc.cpp @@ -0,0 +1,86 @@ +#include "AngleZConstraintIqcJc.h" +#include "AngleZIeqcJec.h" +#include "EndFrameqc.h" + +using namespace MbD; + +MbD::AngleZConstraintIqcJc::AngleZConstraintIqcJc(EndFrmsptr frmi, EndFrmsptr frmj) : AngleZConstraintIJ(frmi, frmj) +{ + pGpEI = std::make_shared>(4); + ppGpEIpEI = std::make_shared>(4, 4); +} + +void MbD::AngleZConstraintIqcJc::initthezIeJe() +{ + thezIeJe = std::make_shared(frmI, frmJ); +} + +void MbD::AngleZConstraintIqcJc::addToJointTorqueI(FColDsptr jointTorque) +{ + auto frmIeqc = std::static_pointer_cast(frmI); + auto rIpIeIp = frmIeqc->rpep(); + auto pAOIppEI = frmIeqc->pAOppE(); + auto aBOIp = frmIeqc->aBOp(); + auto fpAOIppEIrIpIeIp = std::make_shared>(4, 0.0); + auto lampGpE = pGpEI->transpose()->times(lam); + auto c2Torque = aBOIp->timesFullColumn(lampGpE->minusFullColumn(fpAOIppEIrIpIeIp)); + jointTorque->equalSelfPlusFullColumntimes(c2Torque, 0.5); +} + +void MbD::AngleZConstraintIqcJc::calc_pGpEI() +{ + pGpEI = thezIeJe->pvaluepEI(); +} + +void MbD::AngleZConstraintIqcJc::calc_ppGpEIpEI() +{ + ppGpEIpEI = thezIeJe->ppvaluepEIpEI(); +} + +void MbD::AngleZConstraintIqcJc::calcPostDynCorrectorIteration() +{ + AngleZConstraintIJ::calcPostDynCorrectorIteration(); + this->calc_pGpEI(); + this->calc_ppGpEIpEI(); +} + +void MbD::AngleZConstraintIqcJc::fillAccICIterError(FColDsptr col) +{ + col->atiplusFullVectortimes(iqEI, pGpEI, lam); + auto efrmIqc = std::static_pointer_cast(frmI); + auto qXdotI = efrmIqc->qXdot(); + auto qEdotI = efrmIqc->qEdot(); + auto sum = pGpEI->timesFullColumn(efrmIqc->qEddot()); + sum += qEdotI->transposeTimesFullColumn(ppGpEIpEI->timesFullColumn(qEdotI)); + col->atiplusNumber(iG, sum); +} + +void MbD::AngleZConstraintIqcJc::fillPosICError(FColDsptr col) +{ + AngleZConstraintIJ::fillPosICError(col); + col->atiplusFullVectortimes(iqEI, pGpEI, lam); +} + +void MbD::AngleZConstraintIqcJc::fillPosICJacob(SpMatDsptr mat) +{ + mat->atijplusFullRow(iG, iqEI, pGpEI); + mat->atijplusFullColumn(iqEI, iG, pGpEI->transpose()); + mat->atijplusFullMatrixtimes(iqEI, iqEI, ppGpEIpEI, lam); +} + +void MbD::AngleZConstraintIqcJc::fillPosKineJacob(SpMatDsptr mat) +{ + mat->atijplusFullRow(iG, iqEI, pGpEI); +} + +void MbD::AngleZConstraintIqcJc::fillVelICJacob(SpMatDsptr mat) +{ + mat->atijplusFullRow(iG, iqEI, pGpEI); + mat->atijplusFullColumn(iqEI, iG, pGpEI->transpose()); +} + +void MbD::AngleZConstraintIqcJc::useEquationNumbers() +{ + auto frmIeqc = std::static_pointer_cast(frmI); + iqEI = frmIeqc->iqE(); +} diff --git a/OndselSolver/AngleZConstraintIqcJc.h b/OndselSolver/AngleZConstraintIqcJc.h new file mode 100644 index 0000000..7b9cbd3 --- /dev/null +++ b/OndselSolver/AngleZConstraintIqcJc.h @@ -0,0 +1,37 @@ +/*************************************************************************** + * Copyright (c) 2023 Ondsel, Inc. * + * * + * This file is part of OndselSolver. * + * * + * See LICENSE file for details about copyright. * + ***************************************************************************/ + +#pragma once + +#include "AngleZConstraintIJ.h" + +namespace MbD { + class AngleZConstraintIqcJc : public AngleZConstraintIJ + { + //pGpEI ppGpEIpEI iqEI + public: + AngleZConstraintIqcJc(EndFrmsptr frmi, EndFrmsptr frmj); + + void initthezIeJe() override; + void addToJointTorqueI(FColDsptr col) override; + void calc_pGpEI(); + void calc_ppGpEIpEI(); + void calcPostDynCorrectorIteration() override; + void fillAccICIterError(FColDsptr col) override; + void fillPosICError(FColDsptr col) override; + void fillPosICJacob(SpMatDsptr mat) override; + void fillPosKineJacob(SpMatDsptr mat) override; + void fillVelICJacob(SpMatDsptr mat) override; + void useEquationNumbers() override; + + FRowDsptr pGpEI; + FMatDsptr ppGpEIpEI; + size_t iqEI = SIZE_MAX; + + }; +} diff --git a/OndselSolver/AngleZConstraintIqcJqc.cpp b/OndselSolver/AngleZConstraintIqcJqc.cpp new file mode 100644 index 0000000..8025682 --- /dev/null +++ b/OndselSolver/AngleZConstraintIqcJqc.cpp @@ -0,0 +1,93 @@ +#include "AngleZConstraintIqcJqc.h" +#include "AngleZIeqcJeqc.h" +#include "EndFrameqc.h" + +using namespace MbD; + +MbD::AngleZConstraintIqcJqc::AngleZConstraintIqcJqc(EndFrmsptr frmi, EndFrmsptr frmj) : AngleZConstraintIqcJc(frmi, frmj) +{ + pGpEJ = std::make_shared>(4); + ppGpEIpEJ = std::make_shared>(4, 4); + ppGpEJpEJ = std::make_shared>(4, 4); +} + +void MbD::AngleZConstraintIqcJqc::initthezIeJe() +{ + thezIeJe = std::make_shared(frmI, frmJ); +} + +void MbD::AngleZConstraintIqcJqc::calc_pGpEJ() +{ + pGpEJ = thezIeJe->pvaluepEJ(); +} + +void MbD::AngleZConstraintIqcJqc::calc_ppGpEIpEJ() +{ + ppGpEIpEJ = thezIeJe->ppvaluepEIpEJ(); +} + +void MbD::AngleZConstraintIqcJqc::calc_ppGpEJpEJ() +{ + ppGpEJpEJ = thezIeJe->ppvaluepEJpEJ(); +} + +void MbD::AngleZConstraintIqcJqc::calcPostDynCorrectorIteration() +{ + AngleZConstraintIqcJc::calcPostDynCorrectorIteration(); + this->calc_pGpEJ(); + this->calc_ppGpEIpEJ(); + this->calc_ppGpEJpEJ(); +} + +void MbD::AngleZConstraintIqcJqc::fillAccICIterError(FColDsptr col) +{ + AngleZConstraintIqcJc::fillAccICIterError(col); + col->atiplusFullVectortimes(iqEJ, pGpEJ, lam); + auto frmIeqc = std::static_pointer_cast(frmI); + auto frmJeqc = std::static_pointer_cast(frmJ); + auto qEdotI = frmIeqc->qEdot(); + auto qXdotJ = frmJeqc->qXdot(); + auto qEdotJ = frmJeqc->qEdot(); + double sum = 0.0; + sum += pGpEJ->timesFullColumn(frmJeqc->qEddot()); + sum += 2.0 * (qEdotI->transposeTimesFullColumn(ppGpEIpEJ->timesFullColumn(qEdotJ))); + sum += qEdotJ->transposeTimesFullColumn(ppGpEJpEJ->timesFullColumn(qEdotJ)); + col->atiplusNumber(iG, sum); +} + +void MbD::AngleZConstraintIqcJqc::fillPosICError(FColDsptr col) +{ + AngleZConstraintIqcJc::fillPosICError(col); + col->atiplusFullVectortimes(iqEJ, pGpEJ, lam); +} + +void MbD::AngleZConstraintIqcJqc::fillPosICJacob(SpMatDsptr mat) +{ + AngleZConstraintIqcJc::fillPosICJacob(mat); + mat->atijplusFullRow(iG, iqEJ, pGpEJ); + mat->atijplusFullColumn(iqEJ, iG, pGpEJ->transpose()); + auto ppGpEIpEJlam = ppGpEIpEJ->times(lam); + mat->atijplusFullMatrix(iqEI, iqEJ, ppGpEIpEJlam); + mat->atijplusTransposeFullMatrix(iqEJ, iqEI, ppGpEIpEJlam); + mat->atijplusFullMatrixtimes(iqEJ, iqEJ, ppGpEJpEJ, lam); +} + +void MbD::AngleZConstraintIqcJqc::fillPosKineJacob(SpMatDsptr mat) +{ + AngleZConstraintIqcJc::fillPosKineJacob(mat); + mat->atijplusFullRow(iG, iqEJ, pGpEJ); +} + +void MbD::AngleZConstraintIqcJqc::fillVelICJacob(SpMatDsptr mat) +{ + AngleZConstraintIqcJc::fillVelICJacob(mat); + mat->atijplusFullRow(iG, iqEJ, pGpEJ); + mat->atijplusFullColumn(iqEJ, iG, pGpEJ->transpose()); +} + +void MbD::AngleZConstraintIqcJqc::useEquationNumbers() +{ + AngleZConstraintIqcJc::useEquationNumbers(); + auto frmJeqc = std::static_pointer_cast(frmJ); + iqEJ = frmJeqc->iqE(); +} diff --git a/OndselSolver/AngleZConstraintIqcJqc.h b/OndselSolver/AngleZConstraintIqcJqc.h new file mode 100644 index 0000000..09ed7ea --- /dev/null +++ b/OndselSolver/AngleZConstraintIqcJqc.h @@ -0,0 +1,37 @@ +/*************************************************************************** + * Copyright (c) 2023 Ondsel, Inc. * + * * + * This file is part of OndselSolver. * + * * + * See LICENSE file for details about copyright. * + ***************************************************************************/ + +#pragma once + +#include "AngleZConstraintIqcJc.h" + +namespace MbD { + class AngleZConstraintIqcJqc : public AngleZConstraintIqcJc + { + //pGpEJ ppGpEIpEJ ppGpEJpEJ iqEJ + public: + AngleZConstraintIqcJqc(EndFrmsptr frmi, EndFrmsptr frmj); + + void initthezIeJe() override; + void calc_pGpEJ(); + void calc_ppGpEIpEJ(); + void calc_ppGpEJpEJ(); + void calcPostDynCorrectorIteration() override; + void fillAccICIterError(FColDsptr col) override; + void fillPosICError(FColDsptr col) override; + void fillPosICJacob(SpMatDsptr mat) override; + void fillPosKineJacob(SpMatDsptr mat) override; + void fillVelICJacob(SpMatDsptr mat) override; + void useEquationNumbers() override; + + FRowDsptr pGpEJ; + FMatDsptr ppGpEIpEJ, ppGpEJpEJ; + size_t iqEJ = SIZE_MAX; + }; +} + diff --git a/OndselSolver/AngleZIecJec.cpp b/OndselSolver/AngleZIecJec.cpp index 25bc4a1..8e6c583 100644 --- a/OndselSolver/AngleZIecJec.cpp +++ b/OndselSolver/AngleZIecJec.cpp @@ -91,7 +91,6 @@ void MbD::AngleZIecJec::prePosIC() { aA00IeJe->prePosIC(); aA10IeJe->prePosIC(); - assert(thez != std::numeric_limits::min()); KinematicIeJe::prePosIC(); } diff --git a/OndselSolver/AngleZIeqcJeqc.cpp b/OndselSolver/AngleZIeqcJeqc.cpp index 1328e57..f086bd6 100644 --- a/OndselSolver/AngleZIeqcJeqc.cpp +++ b/OndselSolver/AngleZIeqcJeqc.cpp @@ -23,6 +23,13 @@ MbD::AngleZIeqcJeqc::AngleZIeqcJeqc(EndFrmsptr frmi, EndFrmsptr frmj) : AngleZIe ppthezpEJpEJ = std::make_shared>(4, 4); } +std::shared_ptr MbD::AngleZIeqcJeqc::With(EndFrmsptr frmi, EndFrmsptr frmj) +{ + auto angleZIJ = std::make_shared(frmi, frmj); + angleZIJ->initialize(); + return angleZIJ; +} + void MbD::AngleZIeqcJeqc::calcPostDynCorrectorIteration() { AngleZIeqcJec::calcPostDynCorrectorIteration(); diff --git a/OndselSolver/AngleZIeqcJeqc.h b/OndselSolver/AngleZIeqcJeqc.h index fa73ac5..52feed7 100644 --- a/OndselSolver/AngleZIeqcJeqc.h +++ b/OndselSolver/AngleZIeqcJeqc.h @@ -17,7 +17,8 @@ namespace MbD { public: AngleZIeqcJeqc(); AngleZIeqcJeqc(EndFrmsptr frmi, EndFrmsptr frmj); - + static std::shared_ptr With(EndFrmsptr frmi, EndFrmsptr frmj); + void calcPostDynCorrectorIteration() override; void init_aAijIeJe() override; void initialize() override; diff --git a/OndselSolver/AnyPosICNewtonRaphson.cpp b/OndselSolver/AnyPosICNewtonRaphson.cpp index 824a0e1..a98475a 100644 --- a/OndselSolver/AnyPosICNewtonRaphson.cpp +++ b/OndselSolver/AnyPosICNewtonRaphson.cpp @@ -5,11 +5,14 @@ * * * See LICENSE file for details about copyright. * ***************************************************************************/ + +#include #include "AnyPosICNewtonRaphson.h" #include "SystemSolver.h" #include "Item.h" -#include +#include "Part.h" +#include "Constraint.h" using namespace MbD; @@ -22,7 +25,7 @@ void AnyPosICNewtonRaphson::initialize() void AnyPosICNewtonRaphson::initializeGlobally() { SystemNewtonRaphson::initializeGlobally(); - system->partsJointsMotionsDo([&](std::shared_ptr item) { + system->partsJointsMotionsLimitsDo([&](std::shared_ptr item) { item->fillqsu(qsuOld); item->fillqsuWeights(qsuWeights); item->fillqsulam(x); @@ -42,7 +45,7 @@ void AnyPosICNewtonRaphson::fillY() newMinusOld->equalSelfPlusFullColumnAt(x, 0); y->zeroSelf(); y->atiminusFullColumn(0, (qsuWeights->timesFullColumn(newMinusOld))); - system->partsJointsMotionsDo([&](std::shared_ptr item) { + system->partsJointsMotionsLimitsDo([&](std::shared_ptr item) { item->fillPosICError(y); //std::cout << item->name << *y << std::endl; //noop(); @@ -54,7 +57,7 @@ void AnyPosICNewtonRaphson::fillPyPx() { pypx->zeroSelf(); pypx->atijminusDiagonalMatrix(0, 0, qsuWeights); - system->partsJointsMotionsDo([&](std::shared_ptr item) { + system->partsJointsMotionsLimitsDo([&](std::shared_ptr item) { item->fillPosICJacob(pypx); //std::cout << *(pypx->at(3)) << std::endl; }); @@ -63,5 +66,42 @@ void AnyPosICNewtonRaphson::fillPyPx() void AnyPosICNewtonRaphson::passRootToSystem() { - system->partsJointsMotionsDo([&](std::shared_ptr item) { item->setqsulam(x); }); + system->partsJointsMotionsLimitsDo([&](std::shared_ptr item) { item->setqsulam(x); }); +} + +void MbD::AnyPosICNewtonRaphson::assignEquationNumbers() +{ + auto parts = system->parts(); + //auto contactEndFrames = system->contactEndFrames(); + //auto uHolders = system->uHolders(); + auto constraints = system->allConstraints(); + size_t eqnNo = 0; + for (auto& part : *parts) { + part->iqX(eqnNo); + eqnNo = eqnNo + 3; + part->iqE(eqnNo); + eqnNo = eqnNo + 4; + } + //for (auto& endFrm : *contactEndFrames) { + // endFrm->is(eqnNo); + // eqnNo = eqnNo + endFrm->sSize(); + //} + //for (auto& uHolder : *uHolders) { + // uHolder->iu(eqnNo); + // eqnNo += 1; + //} + auto nEqns = eqnNo; //C++ uses index 0. + nqsu = nEqns; + for (auto& con : *constraints) { + con->iG = eqnNo; + eqnNo += 1; + } + //auto lastEqnNo = eqnNo - 1; + nEqns = eqnNo; //C++ uses index 0. + n = nEqns; +} + +bool MbD::AnyPosICNewtonRaphson::isConverged() +{ + return dxNorms->at(iterNo) < dxTol || isConvergedToNumericalLimit(); } diff --git a/OndselSolver/AnyPosICNewtonRaphson.h b/OndselSolver/AnyPosICNewtonRaphson.h index 244963e..bdfcd8f 100644 --- a/OndselSolver/AnyPosICNewtonRaphson.h +++ b/OndselSolver/AnyPosICNewtonRaphson.h @@ -24,7 +24,8 @@ namespace MbD { void fillY() override; void fillPyPx() override; void passRootToSystem() override; - void assignEquationNumbers() override = 0; + void assignEquationNumbers() override; + bool isConverged() override; size_t nqsu = SIZE_MAX; FColDsptr qsuOld; diff --git a/OndselSolver/CADSystem.cpp b/OndselSolver/CADSystem.cpp index 3ea4e70..ce70dc8 100644 --- a/OndselSolver/CADSystem.cpp +++ b/OndselSolver/CADSystem.cpp @@ -32,7 +32,7 @@ void CADSystem::outputFor(AnalysisType) { auto str = std::to_string(mbdSystem->mbdTimeValue()); this->logString(str); - mbdSystem->partsJointsMotionsForcesTorquesDo([](std::shared_ptr item) { + mbdSystem->partsJointsMotionsLimitsForcesTorquesDo([](std::shared_ptr item) { std::cout << std::endl; std::cout << item->classname() << " " << item->name << std::endl; auto data = item->stateData(); diff --git a/OndselSolver/ConstraintSet.cpp b/OndselSolver/ConstraintSet.cpp new file mode 100644 index 0000000..c9c6f10 --- /dev/null +++ b/OndselSolver/ConstraintSet.cpp @@ -0,0 +1,185 @@ +#include +#include +#include + +#include "ConstraintSet.h" +#include "Constraint.h" + +using namespace MbD; + +MbD::ConstraintSet::ConstraintSet() +{ +} + +MbD::ConstraintSet::ConstraintSet(const char* str) : ItemIJ(str) +{ +} + +void MbD::ConstraintSet::constraintsDo(const std::function )>& f) +{ + std::for_each(constraints->begin(), constraints->end(), f); +} + +void MbD::ConstraintSet::initialize() +{ + constraints = std::make_shared>>(); +} + +void MbD::ConstraintSet::initializeGlobally() +{ + constraintsDo([](std::shared_ptr constraint) { constraint->initializeGlobally(); }); +} + +void MbD::ConstraintSet::initializeLocally() +{ + constraintsDo([](std::shared_ptr constraint) { constraint->initializeLocally(); }); +} + +void MbD::ConstraintSet::addConstraint(std::shared_ptr con) +{ + con->owner = this; + constraints->push_back(con); +} +void MbD::ConstraintSet::postInput() +{ + constraintsDo([](std::shared_ptr constraint) { constraint->postInput(); }); +} + +void MbD::ConstraintSet::prePosIC() +{ + constraintsDo([](std::shared_ptr constraint) { constraint->prePosIC(); }); +} + +void MbD::ConstraintSet::prePosKine() +{ + constraintsDo([](std::shared_ptr constraint) { constraint->prePosKine(); }); +} + +void MbD::ConstraintSet::fillEssenConstraints(std::shared_ptr>> essenConstraints) +{ + constraintsDo([&](std::shared_ptr con) { con->fillEssenConstraints(con, essenConstraints); }); +} + +void MbD::ConstraintSet::fillDispConstraints(std::shared_ptr>> dispConstraints) +{ + constraintsDo([&](std::shared_ptr con) { con->fillDispConstraints(con, dispConstraints); }); +} + +void MbD::ConstraintSet::fillPerpenConstraints(std::shared_ptr>> perpenConstraints) +{ + constraintsDo([&](std::shared_ptr con) { con->fillPerpenConstraints(con, perpenConstraints); }); +} + +void MbD::ConstraintSet::fillConstraints(std::shared_ptr>> allConstraints) +{ + constraintsDo([&](std::shared_ptr con) { con->fillConstraints(con, allConstraints); }); +} + +void MbD::ConstraintSet::fillqsulam(FColDsptr col) +{ + constraintsDo([&](std::shared_ptr con) { con->fillqsulam(col); }); +} + +void MbD::ConstraintSet::fillqsudot(FColDsptr col) +{ + constraintsDo([&](std::shared_ptr con) { con->fillqsudot(col); }); +} + +void MbD::ConstraintSet::fillqsudotWeights(DiagMatDsptr) +{ +} + +void MbD::ConstraintSet::useEquationNumbers() +{ + constraintsDo([](std::shared_ptr constraint) { constraint->useEquationNumbers(); }); +} + +void MbD::ConstraintSet::setqsulam(FColDsptr col) +{ + constraintsDo([&](std::shared_ptr con) { con->setqsulam(col); }); +} + +void MbD::ConstraintSet::setqsudotlam(FColDsptr col) +{ + constraintsDo([&](std::shared_ptr con) { con->setqsudotlam(col); }); +} + +void MbD::ConstraintSet::postPosICIteration() +{ + constraintsDo([](std::shared_ptr constraint) { constraint->postPosICIteration(); }); +} + +void MbD::ConstraintSet::fillPosICError(FColDsptr col) +{ + constraintsDo([&](std::shared_ptr con) { con->fillPosICError(col); }); +} + +void MbD::ConstraintSet::fillPosICJacob(SpMatDsptr mat) +{ + constraintsDo([&](std::shared_ptr con) { con->fillPosICJacob(mat); }); +} + +void MbD::ConstraintSet::postPosIC() +{ + constraintsDo([](std::shared_ptr constraint) { constraint->postPosIC(); }); +} + +void MbD::ConstraintSet::preDyn() +{ + constraintsDo([](std::shared_ptr constraint) { constraint->preDyn(); }); +} + +void MbD::ConstraintSet::fillPosKineError(FColDsptr col) +{ + constraintsDo([&](std::shared_ptr con) { con->fillPosKineError(col); }); +} + +void MbD::ConstraintSet::fillPosKineJacob(SpMatDsptr mat) +{ + constraintsDo([&](std::shared_ptr constraint) { constraint->fillPosKineJacob(mat); }); +} + +void MbD::ConstraintSet::fillqsuddotlam(FColDsptr col) +{ + constraintsDo([&](std::shared_ptr constraint) { constraint->fillqsuddotlam(col); }); +} + +void MbD::ConstraintSet::preVelIC() +{ + constraintsDo([](std::shared_ptr constraint) { constraint->preVelIC(); }); +} + +void MbD::ConstraintSet::fillVelICError(FColDsptr col) +{ + constraintsDo([&](std::shared_ptr con) { con->fillVelICError(col); }); +} + +void MbD::ConstraintSet::fillVelICJacob(SpMatDsptr mat) +{ + constraintsDo([&](std::shared_ptr constraint) { constraint->fillVelICJacob(mat); }); +} + +void MbD::ConstraintSet::preAccIC() +{ + constraintsDo([](std::shared_ptr constraint) { constraint->preAccIC(); }); +} + +void MbD::ConstraintSet::fillAccICIterError(FColDsptr col) +{ + constraintsDo([&](std::shared_ptr con) { con->fillAccICIterError(col); }); +} + +void MbD::ConstraintSet::fillAccICIterJacob(SpMatDsptr mat) +{ + constraintsDo([&](std::shared_ptr con) { con->fillAccICIterJacob(mat); }); +} + +void MbD::ConstraintSet::setqsuddotlam(FColDsptr col) +{ + constraintsDo([&](std::shared_ptr con) { con->setqsuddotlam(col); }); +} + +void MbD::ConstraintSet::postDynStep() +{ + constraintsDo([](std::shared_ptr constraint) { constraint->postDynStep(); }); +} diff --git a/OndselSolver/ConstraintSet.h b/OndselSolver/ConstraintSet.h new file mode 100644 index 0000000..777ddf6 --- /dev/null +++ b/OndselSolver/ConstraintSet.h @@ -0,0 +1,61 @@ +/*************************************************************************** + * Copyright (c) 2023 Ondsel, Inc. * + * * + * This file is part of OndselSolver. * + * * + * See LICENSE file for details about copyright. * + ***************************************************************************/ + +#pragma once + +#include + +#include "ItemIJ.h" + +namespace MbD { + class ConstraintSet : public ItemIJ + { + // + public: + ConstraintSet(); + ConstraintSet(const char* str); + void constraintsDo(const std::function )>& f); + void initialize() override; + void initializeGlobally() override; + void initializeLocally() override; + void addConstraint(std::shared_ptr con); + void fillAccICIterError(FColDsptr col) override; + void fillAccICIterJacob(SpMatDsptr mat) override; + void fillConstraints(std::shared_ptr>> allConstraints) override; + void fillDispConstraints(std::shared_ptr>> dispConstraints) override; + void fillEssenConstraints(std::shared_ptr>> essenConstraints) override; + void fillPerpenConstraints(std::shared_ptr>> perpenConstraints) override; + void fillPosICError(FColDsptr col) override; + void fillPosICJacob(SpMatDsptr mat) override; + void fillPosKineError(FColDsptr col) override; + void fillPosKineJacob(SpMatDsptr mat) override; + void fillqsuddotlam(FColDsptr col) override; + void fillqsulam(FColDsptr col) override; + void fillqsudot(FColDsptr col) override; + void fillqsudotWeights(DiagMatDsptr diagMat) override; + void fillVelICError(FColDsptr col) override; + void fillVelICJacob(SpMatDsptr mat) override; + void postDynStep() override; + void postInput() override; + void postPosIC() override; + void postPosICIteration() override; + void preAccIC() override; + void preDyn() override; + void prePosIC() override; + void prePosKine() override; + void preVelIC() override; + void setqsuddotlam(FColDsptr col) override; + void setqsudotlam(FColDsptr col) override; + void setqsulam(FColDsptr col) override; + void useEquationNumbers() override; + + + std::shared_ptr>> constraints; + + }; +} diff --git a/OndselSolver/Integral.cpp b/OndselSolver/Integral.cpp index 934e2e8..48c4e27 100644 --- a/OndselSolver/Integral.cpp +++ b/OndselSolver/Integral.cpp @@ -37,6 +37,11 @@ Symsptr MbD::Integral::simplifyUntil(Symsptr, std::shared_ptr> set) override; Symsptr simplifyUntil(Symsptr sptr, std::shared_ptr> set) override; + void setIntegrationConstant(double integConstant) override; std::ostream& printOn(std::ostream& s) const override; diff --git a/OndselSolver/Item.cpp b/OndselSolver/Item.cpp index 8f01dc0..67713bc 100644 --- a/OndselSolver/Item.cpp +++ b/OndselSolver/Item.cpp @@ -200,11 +200,6 @@ void Item::fillqsuWeights(DiagMatDsptr) { } -void MbD::Item::fillqsuWeightsSmall(FColDsptr) -{ - assert(false); -} - void Item::fillqsulam(FColDsptr) { } diff --git a/OndselSolver/Item.h b/OndselSolver/Item.h index fc13868..4ef7158 100644 --- a/OndselSolver/Item.h +++ b/OndselSolver/Item.h @@ -68,7 +68,6 @@ namespace MbD { virtual void fillqsudotWeights(DiagMatDsptr diagMat); virtual void fillqsulam(FColDsptr col); virtual void fillqsuWeights(DiagMatDsptr diagMat); - virtual void fillqsuWeightsSmall(FColDsptr col); virtual void fillRedundantConstraints(std::shared_ptr>> redunConstraints); virtual void fillStaticError(FColDsptr col); virtual void fillStaticJacob(FMatDsptr mat); diff --git a/OndselSolver/ItemIJ.cpp b/OndselSolver/ItemIJ.cpp new file mode 100644 index 0000000..ce5e8f2 --- /dev/null +++ b/OndselSolver/ItemIJ.cpp @@ -0,0 +1,21 @@ +#include "ItemIJ.h" + +using namespace MbD; + +MbD::ItemIJ::ItemIJ() +{ +} + +MbD::ItemIJ::ItemIJ(const char* str) : Item(str) +{ +} + +MbD::ItemIJ::ItemIJ(EndFrmsptr frmi, EndFrmsptr frmj) : frmI(frmi), frmJ(frmj) +{ +} + +void MbD::ItemIJ::connectsItoJ(EndFrmsptr frmi, EndFrmsptr frmj) +{ + frmI = frmi; + frmJ = frmj; +} diff --git a/OndselSolver/ItemIJ.h b/OndselSolver/ItemIJ.h new file mode 100644 index 0000000..9f3fd5d --- /dev/null +++ b/OndselSolver/ItemIJ.h @@ -0,0 +1,29 @@ +/*************************************************************************** + * Copyright (c) 2023 Ondsel, Inc. * + * * + * This file is part of OndselSolver. * + * * + * See LICENSE file for details about copyright. * + ***************************************************************************/ + +#pragma once + +#include "Item.h" + +namespace MbD { + class EndFramec; + using EndFrmsptr = std::shared_ptr; + + class ItemIJ : public Item + { + // + public: + ItemIJ(); + ItemIJ(const char* str); + ItemIJ(EndFrmsptr frmi, EndFrmsptr frmj); + virtual void connectsItoJ(EndFrmsptr frmI, EndFrmsptr frmJ); + + EndFrmsptr frmI, frmJ; + + }; +} diff --git a/OndselSolver/Joint.cpp b/OndselSolver/Joint.cpp index 735a8d5..786c195 100644 --- a/OndselSolver/Joint.cpp +++ b/OndselSolver/Joint.cpp @@ -6,7 +6,7 @@ * See LICENSE file for details about copyright. * ***************************************************************************/ -#include +#include #include #include @@ -26,21 +26,10 @@ Joint::Joint() { } -Joint::Joint(const char* str) : Item(str) { +Joint::Joint(const char* str) : ConstraintSet(str) { } -void Joint::initialize() -{ - constraints = std::make_shared>>(); -} - -void Joint::connectsItoJ(EndFrmsptr frmi, EndFrmsptr frmj) -{ - frmI = frmi; - frmJ = frmj; -} - void Joint::initializeLocally() { auto frmIqc = std::dynamic_pointer_cast(frmI); @@ -52,28 +41,6 @@ void Joint::initializeLocally() constraintsDo([](std::shared_ptr constraint) { constraint->initializeLocally(); }); } -void Joint::initializeGlobally() -{ - constraintsDo([](std::shared_ptr constraint) { constraint->initializeGlobally(); }); -} - -void Joint::constraintsDo(const std::function)>& f) -{ - std::for_each(constraints->begin(), constraints->end(), f); -} - -void Joint::postInput() -{ - constraintsDo([](std::shared_ptr constraint) { constraint->postInput(); }); - -} - -void Joint::addConstraint(std::shared_ptr con) -{ - con->owner = this; - constraints->push_back(con); -} - FColDsptr MbD::Joint::aFIeJtIe() { //"aFIeJtIe is joint force on end frame Ie expresses in Ie components." @@ -89,85 +56,11 @@ FColDsptr MbD::Joint::aFIeJtO() return aFIeJtO; } -void Joint::prePosIC() -{ - constraintsDo([](std::shared_ptr constraint) { constraint->prePosIC(); }); -} - -void Joint::prePosKine() -{ - constraintsDo([](std::shared_ptr constraint) { constraint->prePosKine(); }); -} - -void Joint::fillEssenConstraints(std::shared_ptr>> essenConstraints) -{ - constraintsDo([&](std::shared_ptr con) { con->fillEssenConstraints(con, essenConstraints); }); -} - -void Joint::fillDispConstraints(std::shared_ptr>> dispConstraints) -{ - constraintsDo([&](std::shared_ptr con) { con->fillDispConstraints(con, dispConstraints); }); -} - -void Joint::fillPerpenConstraints(std::shared_ptr>> perpenConstraints) -{ - constraintsDo([&](std::shared_ptr con) { con->fillPerpenConstraints(con, perpenConstraints); }); -} - void Joint::fillRedundantConstraints(std::shared_ptr>> redunConstraints) { constraintsDo([&](std::shared_ptr con) { con->fillRedundantConstraints(con, redunConstraints); }); } -void Joint::fillConstraints(std::shared_ptr>> allConstraints) -{ - constraintsDo([&](std::shared_ptr con) { con->fillConstraints(con, allConstraints); }); -} - -void Joint::fillqsulam(FColDsptr col) -{ - constraintsDo([&](std::shared_ptr con) { con->fillqsulam(col); }); -} - -void Joint::fillqsudot(FColDsptr col) -{ - constraintsDo([&](std::shared_ptr con) { con->fillqsudot(col); }); -} - -void Joint::fillqsudotWeights(DiagMatDsptr) -{ -} - -void Joint::useEquationNumbers() -{ - constraintsDo([](std::shared_ptr constraint) { constraint->useEquationNumbers(); }); -} - -void Joint::setqsulam(FColDsptr col) -{ - constraintsDo([&](std::shared_ptr con) { con->setqsulam(col); }); -} - -void Joint::setqsudotlam(FColDsptr col) -{ - constraintsDo([&](std::shared_ptr con) { con->setqsudotlam(col); }); -} - -void Joint::postPosICIteration() -{ - constraintsDo([](std::shared_ptr constraint) { constraint->postPosICIteration(); }); -} - -void Joint::fillPosICError(FColDsptr col) -{ - constraintsDo([&](std::shared_ptr con) { con->fillPosICError(col); }); -} - -void Joint::fillPosICJacob(SpMatDsptr mat) -{ - constraintsDo([&](std::shared_ptr con) { con->fillPosICJacob(mat); }); -} - void Joint::removeRedundantConstraints(std::shared_ptr> redundantEqnNos) { for (size_t i = 0; i < constraints->size(); i++) @@ -210,66 +103,6 @@ void Joint::constraintsReport() } } -void Joint::postPosIC() -{ - constraintsDo([](std::shared_ptr constraint) { constraint->postPosIC(); }); -} - -void Joint::preDyn() -{ - constraintsDo([](std::shared_ptr constraint) { constraint->preDyn(); }); -} - -void Joint::fillPosKineError(FColDsptr col) -{ - constraintsDo([&](std::shared_ptr con) { con->fillPosKineError(col); }); -} - -void Joint::fillPosKineJacob(SpMatDsptr mat) -{ - constraintsDo([&](std::shared_ptr constraint) { constraint->fillPosKineJacob(mat); }); -} - -void MbD::Joint::fillqsuddotlam(FColDsptr col) -{ - constraintsDo([&](std::shared_ptr constraint) { constraint->fillqsuddotlam(col); }); -} - -void Joint::preVelIC() -{ - constraintsDo([](std::shared_ptr constraint) { constraint->preVelIC(); }); -} - -void Joint::fillVelICError(FColDsptr col) -{ - constraintsDo([&](std::shared_ptr con) { con->fillVelICError(col); }); -} - -void Joint::fillVelICJacob(SpMatDsptr mat) -{ - constraintsDo([&](std::shared_ptr constraint) { constraint->fillVelICJacob(mat); }); -} - -void Joint::preAccIC() -{ - constraintsDo([](std::shared_ptr constraint) { constraint->preAccIC(); }); -} - -void Joint::fillAccICIterError(FColDsptr col) -{ - constraintsDo([&](std::shared_ptr con) { con->fillAccICIterError(col); }); -} - -void Joint::fillAccICIterJacob(SpMatDsptr mat) -{ - constraintsDo([&](std::shared_ptr con) { con->fillAccICIterJacob(mat); }); -} - -void Joint::setqsuddotlam(FColDsptr col) -{ - constraintsDo([&](std::shared_ptr con) { con->setqsuddotlam(col); }); -} - std::shared_ptr Joint::stateData() { //" @@ -328,8 +161,3 @@ FColDsptr Joint::jointTorqueI() constraintsDo([&](std::shared_ptr con) { con->addToJointTorqueI(jointTorque); }); return jointTorque; } - -void Joint::postDynStep() -{ - constraintsDo([](std::shared_ptr constraint) { constraint->postDynStep(); }); -} diff --git a/OndselSolver/Joint.h b/OndselSolver/Joint.h index 75ea760..b0b425c 100644 --- a/OndselSolver/Joint.h +++ b/OndselSolver/Joint.h @@ -12,72 +12,35 @@ #include #include -#include "Item.h" +#include "ConstraintSet.h" namespace MbD { class Constraint; class EndFramec; using EndFrmsptr = std::shared_ptr; - class Joint : public Item + class Joint : public ConstraintSet { //frmI frmJ constraints friction public: Joint(); Joint(const char* str); + virtual ~Joint() {} - void addConstraint(std::shared_ptr con); + void initializeLocally() override; FColDsptr aFIeJtIe(); FColDsptr aFIeJtO(); FColDsptr aFX(); FColDsptr aTIeJtIe(); FColDsptr aTIeJtO(); FColDsptr aTX(); - virtual void connectsItoJ(EndFrmsptr frmI, EndFrmsptr frmJ); - void constraintsDo(const std::function )>& f); void constraintsReport() override; - void fillAccICIterError(FColDsptr col) override; - void fillAccICIterJacob(SpMatDsptr mat) override; - void fillConstraints(std::shared_ptr>> allConstraints) override; - void fillDispConstraints(std::shared_ptr>> dispConstraints) override; - void fillEssenConstraints(std::shared_ptr>> essenConstraints) override; - void fillPerpenConstraints(std::shared_ptr>> perpenConstraints) override; - void fillPosICError(FColDsptr col) override; - void fillPosICJacob(SpMatDsptr mat) override; - void fillPosKineError(FColDsptr col) override; - void fillPosKineJacob(SpMatDsptr mat) override; - void fillqsuddotlam(FColDsptr col) override; - void fillqsulam(FColDsptr col) override; - void fillqsudot(FColDsptr col) override; - void fillqsudotWeights(DiagMatDsptr diagMat) override; void fillRedundantConstraints(std::shared_ptr>> redunConstraints) override; - void fillVelICError(FColDsptr col) override; - void fillVelICJacob(SpMatDsptr mat) override; - void initialize() override; - void initializeGlobally() override; - void initializeLocally() override; FColDsptr jointForceI(); FColDsptr jointTorqueI(); - void postDynStep() override; - void postInput() override; - void postPosIC() override; - void postPosICIteration() override; - void preAccIC() override; - void preDyn() override; - void prePosIC() override; - void prePosKine() override; - void preVelIC() override; void reactivateRedundantConstraints() override; void removeRedundantConstraints(std::shared_ptr> redundantEqnNos) override; - void setqsuddotlam(FColDsptr col) override; - void setqsudotlam(FColDsptr col) override; - void setqsulam(FColDsptr col) override; std::shared_ptr stateData() override; - void useEquationNumbers() override; - - EndFrmsptr frmI; - EndFrmsptr frmJ; - std::shared_ptr>> constraints; }; } diff --git a/OndselSolver/KinematicIeJe.cpp b/OndselSolver/KinematicIeJe.cpp index b278ec3..8f3f2e5 100644 --- a/OndselSolver/KinematicIeJe.cpp +++ b/OndselSolver/KinematicIeJe.cpp @@ -15,7 +15,7 @@ KinematicIeJe::KinematicIeJe() { } -KinematicIeJe::KinematicIeJe(EndFrmsptr frmi, EndFrmsptr frmj) : frmI(frmi), frmJ(frmj) +KinematicIeJe::KinematicIeJe(EndFrmsptr frmi, EndFrmsptr frmj) : ItemIJ(frmi, frmj) { } diff --git a/OndselSolver/KinematicIeJe.h b/OndselSolver/KinematicIeJe.h index ddaba65..a9bc44c 100644 --- a/OndselSolver/KinematicIeJe.h +++ b/OndselSolver/KinematicIeJe.h @@ -8,13 +8,13 @@ #pragma once -#include "Item.h" +#include "ItemIJ.h" namespace MbD { class EndFramec; using EndFrmsptr = std::shared_ptr; - class KinematicIeJe : public Item + class KinematicIeJe : public ItemIJ { //frmI frmJ public: @@ -84,7 +84,6 @@ namespace MbD { virtual FRowDsptr ppvaluepEKpt(); virtual double value(); - EndFrmsptr frmI, frmJ; }; } diff --git a/OndselSolver/LimitIJ.cpp b/OndselSolver/LimitIJ.cpp new file mode 100644 index 0000000..a7b85bc --- /dev/null +++ b/OndselSolver/LimitIJ.cpp @@ -0,0 +1,143 @@ +#include "LimitIJ.h" +#include "Constraint.h" + +using namespace MbD; + +MbD::LimitIJ::LimitIJ() +{ +} + +void MbD::LimitIJ::fillAccICIterError(FColDsptr col) +{ + assert(false); +} + +void MbD::LimitIJ::fillAccICIterJacob(SpMatDsptr mat) +{ + assert(false); +} + +bool MbD::LimitIJ::satisfied() const +{ + auto& constraint = constraints->front(); + if (type == "=<") { + return constraint->aG < tol; + } + else if (type == "=>") { + return constraint->aG > -tol; + } + assert(false); + return true; +} + +void MbD::LimitIJ::deactivate() +{ + active = false; +} + +void MbD::LimitIJ::activate() +{ + active = true; +} + +void MbD::LimitIJ::fillConstraints(std::shared_ptr>> allConstraints) +{ + if (active) { + ConstraintSet::fillConstraints(allConstraints); + } +} + +void MbD::LimitIJ::fillDispConstraints(std::shared_ptr>> dispConstraints) +{ + assert(false); +} + +void MbD::LimitIJ::fillEssenConstraints(std::shared_ptr>> essenConstraints) +{ + assert(false); +} + +void MbD::LimitIJ::fillPerpenConstraints(std::shared_ptr>> perpenConstraints) +{ + assert(false); +} + +void MbD::LimitIJ::fillPosICError(FColDsptr col) +{ + if (active) { + ConstraintSet::fillPosICError(col); + } +} + +void MbD::LimitIJ::fillPosICJacob(SpMatDsptr mat) +{ + if (active) { + ConstraintSet::fillPosICJacob(mat); + } +} + +void MbD::LimitIJ::fillPosKineError(FColDsptr col) +{ + assert(false); +} + +void MbD::LimitIJ::fillPosKineJacob(SpMatDsptr mat) +{ + assert(false); +} + +void MbD::LimitIJ::fillqsuddotlam(FColDsptr col) +{ + assert(false); +} + +void MbD::LimitIJ::fillqsulam(FColDsptr col) +{ + if (active) { + ConstraintSet::fillqsulam(col); + } +} + +void MbD::LimitIJ::fillqsudot(FColDsptr col) +{ + assert(false); +} + +void MbD::LimitIJ::fillqsudotWeights(DiagMatDsptr diagMat) +{ + assert(false); +} + +void MbD::LimitIJ::fillVelICError(FColDsptr col) +{ + assert(false); +} + +void MbD::LimitIJ::fillVelICJacob(SpMatDsptr mat) +{ + assert(false); +} + +void MbD::LimitIJ::setqsuddotlam(FColDsptr col) +{ + assert(false); +} + +void MbD::LimitIJ::setqsudotlam(FColDsptr col) +{ + assert(false); +} + +void MbD::LimitIJ::setqsulam(FColDsptr col) +{ + if (active) { + ConstraintSet::setqsulam(col); + } +} + +void MbD::LimitIJ::useEquationNumbers() +{ + if (active) { + ConstraintSet::useEquationNumbers(); + } +} diff --git a/OndselSolver/LimitIJ.h b/OndselSolver/LimitIJ.h new file mode 100644 index 0000000..e05883f --- /dev/null +++ b/OndselSolver/LimitIJ.h @@ -0,0 +1,49 @@ +/*************************************************************************** + * Copyright (c) 2023 Ondsel, Inc. * + * * + * This file is part of OndselSolver. * + * * + * See LICENSE file for details about copyright. * + ***************************************************************************/ + +#pragma once + +#include "ConstraintSet.h" + +namespace MbD { + class LimitIJ : public ConstraintSet + { + // + public: + LimitIJ(); + void fillAccICIterError(FColDsptr col) override; + void fillAccICIterJacob(SpMatDsptr mat) override; + void fillConstraints(std::shared_ptr>> allConstraints) override; + void fillDispConstraints(std::shared_ptr>> dispConstraints) override; + void fillEssenConstraints(std::shared_ptr>> essenConstraints) override; + void fillPerpenConstraints(std::shared_ptr>> perpenConstraints) override; + void fillPosICError(FColDsptr col) override; + void fillPosICJacob(SpMatDsptr mat) override; + void fillPosKineError(FColDsptr col) override; + void fillPosKineJacob(SpMatDsptr mat) override; + void fillqsuddotlam(FColDsptr col) override; + void fillqsulam(FColDsptr col) override; + void fillqsudot(FColDsptr col) override; + void fillqsudotWeights(DiagMatDsptr diagMat) override; + void fillVelICError(FColDsptr col) override; + void fillVelICJacob(SpMatDsptr mat) override; + void setqsuddotlam(FColDsptr col) override; + void setqsudotlam(FColDsptr col) override; + void setqsulam(FColDsptr col) override; + void useEquationNumbers() override; + + bool satisfied() const; + void deactivate(); + void activate(); + + double limit = std::numeric_limits::max(); + double tol = std::numeric_limits::max(); + std::string type; + bool active = false; + }; +} diff --git a/OndselSolver/MBDynAxialRotationJoint.cpp b/OndselSolver/MBDynAxialRotationJoint.cpp index 5795612..d9b954b 100644 --- a/OndselSolver/MBDynAxialRotationJoint.cpp +++ b/OndselSolver/MBDynAxialRotationJoint.cpp @@ -23,8 +23,8 @@ void MbD::MBDynAxialRotationJoint::createASMT() { MBDynJoint::createASMT(); auto asmtAsm = asmtAssembly(); - asmtMotion = std::make_shared(); - asmtMotion->setName(name.append("Motion")); + asmtMotion = ASMTRotationalMotion::With(); + asmtMotion->setName(name + "Motion"); asmtMotion->setMotionJoint(asmtItem->fullName("")); asmtMotion->setRotationZ(asmtFormulaIntegral()); asmtAsm->addMotion(asmtMotion); @@ -33,5 +33,7 @@ void MbD::MBDynAxialRotationJoint::createASMT() std::shared_ptr MbD::MBDynAxialRotationJoint::asmtClassNew() { - return std::make_shared(); + auto joint = ASMTRevoluteJoint::With(); + joint->initialize(); + return joint; } diff --git a/OndselSolver/MBDynBody.cpp b/OndselSolver/MBDynBody.cpp index 76182ed..05966c6 100644 --- a/OndselSolver/MBDynBody.cpp +++ b/OndselSolver/MBDynBody.cpp @@ -36,7 +36,7 @@ void MbD::MBDynBody::readMass(std::vector& args) parser->variables = mbdynVariables(); auto userFunc = std::make_shared(popOffTop(args), 1.0); parser->parseUserFunction(userFunc); - auto sym = parser->stack->top(); + auto& sym = parser->stack->top(); mass = sym->getValue(); } @@ -45,14 +45,14 @@ void MbD::MBDynBody::readInertiaMatrix(std::vector& args) auto parser = std::make_shared(); parser->variables = mbdynVariables(); aJmat = std::make_shared>(3, 3); - auto str = args.at(0); //Must copy string + std::string str = args.at(0); //Must copy string if (str.find("diag") != std::string::npos) { args.erase(args.begin()); for (size_t i = 0; i < 3; i++) { auto userFunc = std::make_shared(popOffTop(args), 1.0); parser->parseUserFunction(userFunc); - auto sym = parser->stack->top(); + auto& sym = parser->stack->top(); aJmat->at(i)->at(i) = sym->getValue(); } } @@ -67,7 +67,7 @@ void MbD::MBDynBody::readInertiaMatrix(std::vector& args) void MbD::MBDynBody::createASMT() { - auto asmtMassMarker = std::make_shared(); + auto asmtMassMarker = ASMTPrincipalMassMarker::With(); asmtItem = asmtMassMarker; asmtMassMarker->setMass(mass); if (aJmat->isDiagonalToWithin(1.0e-6)) { diff --git a/OndselSolver/MBDynClampJoint.cpp b/OndselSolver/MBDynClampJoint.cpp index 514a24b..fac6c87 100644 --- a/OndselSolver/MBDynClampJoint.cpp +++ b/OndselSolver/MBDynClampJoint.cpp @@ -46,5 +46,7 @@ void MbD::MBDynClampJoint::readMarkerJ(std::vector& args) std::shared_ptr MbD::MBDynClampJoint::asmtClassNew() { - return std::make_shared(); + auto joint = ASMTFixedJoint::With(); + joint->initialize(); + return joint; } diff --git a/OndselSolver/MBDynDriveHingeJoint.cpp b/OndselSolver/MBDynDriveHingeJoint.cpp index 06f10b9..a3aa68b 100644 --- a/OndselSolver/MBDynDriveHingeJoint.cpp +++ b/OndselSolver/MBDynDriveHingeJoint.cpp @@ -25,7 +25,7 @@ void MbD::MBDynDriveHingeJoint::createASMT() mkr1->createASMT(); if (mkr2) mkr2->createASMT(); auto asmtAsm = asmtAssembly(); - auto asmtMotion = std::make_shared(); + auto asmtMotion = ASMTRotationalMotion::With(); asmtItem = asmtMotion; asmtMotion->setName(name); asmtMotion->setMarkerI(mkr1->asmtItem->fullName("")); @@ -37,5 +37,6 @@ void MbD::MBDynDriveHingeJoint::createASMT() std::shared_ptr MbD::MBDynDriveHingeJoint::asmtClassNew() { - return std::make_shared(); + assert(false); + return ASMTJoint::With(); } diff --git a/OndselSolver/MBDynGravity.cpp b/OndselSolver/MBDynGravity.cpp index 0ee74b8..1fab0ae 100644 --- a/OndselSolver/MBDynGravity.cpp +++ b/OndselSolver/MBDynGravity.cpp @@ -35,7 +35,7 @@ void MbD::MBDynGravity::readFunction(std::vector&) void MbD::MBDynGravity::createASMT() { - auto asmtGravity = std::make_shared(); + auto asmtGravity = ASMTConstantGravity::With(); asmtGravity->setg(gvec); asmtAssembly()->setConstantGravity(asmtGravity); } diff --git a/OndselSolver/MBDynInLineJoint.cpp b/OndselSolver/MBDynInLineJoint.cpp index ef57b98..c19071f 100644 --- a/OndselSolver/MBDynInLineJoint.cpp +++ b/OndselSolver/MBDynInLineJoint.cpp @@ -23,5 +23,7 @@ void MbD::MBDynInLineJoint::createASMT() std::shared_ptr MbD::MBDynInLineJoint::asmtClassNew() { - return std::make_shared(); + auto joint = ASMTPointInLineJoint::With(); + joint->initialize(); + return joint; } diff --git a/OndselSolver/MBDynInPlaneJoint.cpp b/OndselSolver/MBDynInPlaneJoint.cpp index 9ada014..7af9224 100644 --- a/OndselSolver/MBDynInPlaneJoint.cpp +++ b/OndselSolver/MBDynInPlaneJoint.cpp @@ -23,5 +23,7 @@ void MbD::MBDynInPlaneJoint::createASMT() std::shared_ptr MbD::MBDynInPlaneJoint::asmtClassNew() { - return std::make_shared(); + auto joint = ASMTPointInPlaneJoint::With(); + joint->initialize(); + return joint; } diff --git a/OndselSolver/MBDynInitialValue.cpp b/OndselSolver/MBDynInitialValue.cpp index 7fab3da..5211b87 100644 --- a/OndselSolver/MBDynInitialValue.cpp +++ b/OndselSolver/MBDynInitialValue.cpp @@ -135,7 +135,7 @@ void MbD::MBDynInitialValue::readDerivativesCoefficient(std::vector void MbD::MBDynInitialValue::createASMT() { - auto simulationParameters = std::make_shared(); + auto simulationParameters = ASMTSimulationParameters::With(); asmtItem = simulationParameters; simulationParameters->settstart(initialTime); simulationParameters->settend(finalTime); //tstart == tend Initial Conditions only. diff --git a/OndselSolver/MBDynItem.cpp b/OndselSolver/MBDynItem.cpp index 939c651..b88563b 100644 --- a/OndselSolver/MBDynItem.cpp +++ b/OndselSolver/MBDynItem.cpp @@ -189,7 +189,7 @@ FColDsptr MbD::MBDynItem::readVector3(std::vector& args) auto parser = std::make_shared(); parser->variables = mbdynVariables(); auto rFfF = std::make_shared>(3); - auto str = args.at(0); //Must copy string + std::string str = args.at(0); //Must copy string if (str.find("null") != std::string::npos) { args.erase(args.begin()); } @@ -210,7 +210,7 @@ FColDsptr MbD::MBDynItem::readPosition(std::vector& args) { auto rOfO = std::make_shared>(3); if (args.empty()) return rOfO; - auto str = args.at(0); //Must copy string + std::string str = args.at(0); //Must copy string if (str.find("position") != std::string::npos) { args.erase(args.begin()); rOfO = readBasicPosition(args); @@ -249,7 +249,7 @@ FMatDsptr MbD::MBDynItem::readOrientation(std::vector& args) { auto aAOf = FullMatrix::identitysptr(3); if (args.empty()) return aAOf; - auto str = args.at(0); //Must copy string + std::string str = args.at(0); //Must copy string if (str.find("reference") != std::string::npos) { args.erase(args.begin()); auto refName = readStringOffTop(args); @@ -282,7 +282,7 @@ FMatDsptr MbD::MBDynItem::readBasicOrientation(std::vector& args) { auto parser = std::make_shared(); parser->variables = mbdynVariables(); - auto str = args.at(0); //Must copy string + std::string str = args.at(0); //Must copy string if (str.find("euler") != std::string::npos) { args.erase(args.begin()); auto euler = std::make_shared>(); @@ -421,7 +421,7 @@ FMatDsptr MbD::MBDynItem::readBasicOrientation(std::vector& args) std::string MbD::MBDynItem::popOffTop(std::vector& args) { - auto str = args.at(0); //Must copy string + std::string str = args.at(0); //Must copy string args.erase(args.begin()); return str; } diff --git a/OndselSolver/MBDynJoint.cpp b/OndselSolver/MBDynJoint.cpp index fe8b22f..f4b0f62 100644 --- a/OndselSolver/MBDynJoint.cpp +++ b/OndselSolver/MBDynJoint.cpp @@ -345,72 +345,9 @@ void MbD::MBDynJoint::createASMT() std::shared_ptr MbD::MBDynJoint::asmtClassNew() { assert(false); - return std::make_shared(); + return ASMTJoint::With(); } -//void MbD::MBDynJoint::createASMT() -//{ -// mkr1->createASMT(); -// if (mkr2) mkr2->createASMT(); -// std::shared_ptr asmtJoint; -// if (joint_type == "clamp") { -// auto asmtAsm = asmtAssembly(); -// asmtJoint = std::make_shared(); -// asmtJoint->setName(name); -// asmtJoint->setMarkerI(mkr1->asmtItem->fullName("")); -// asmtJoint->setMarkerJ(mkr2->asmtItem->fullName("")); -// asmtAsm->addJoint(asmtJoint); -// return; -// } -// if (joint_type == "axial rotation") { -// auto asmtAsm = asmtAssembly(); -// asmtJoint = std::make_shared(); -// asmtItem = asmtJoint; -// asmtJoint->setName(name); -// asmtJoint->setMarkerI(mkr1->asmtItem->fullName("")); -// asmtJoint->setMarkerJ(mkr2->asmtItem->fullName("")); -// asmtAsm->addJoint(asmtJoint); -// auto asmtMotion = std::make_shared(); -// asmtItem = asmtMotion; -// asmtMotion->setName(name.append("Motion")); -// asmtMotion->setMotionJoint(asmtJoint->fullName("")); -// asmtMotion->setRotationZ(asmtFormulaIntegral()); -// asmtAsm->addMotion(asmtMotion); -// return; -// } -// if (joint_type == "drive hinge") { -// auto asmtAsm = asmtAssembly(); -// auto asmtMotion = std::make_shared(); -// asmtItem = asmtMotion; -// asmtMotion->setName(name); -// asmtMotion->setMarkerI(mkr1->asmtItem->fullName("")); -// asmtMotion->setMarkerJ(mkr2->asmtItem->fullName("")); -// asmtMotion->setRotationZ(formula); -// asmtAsm->addMotion(asmtMotion); -// return; -// } -// if (joint_type == "revolute hinge") { -// asmtJoint = std::make_shared(); -// } -// else if (joint_type == "spherical hinge") { -// asmtJoint = std::make_shared(); -// } -// else if (joint_type == "in line") { -// asmtJoint = std::make_shared(); -// } -// else if (joint_type == "prismatic") { -// asmtJoint = std::make_shared(); -// } -// else { -// assert(false); -// } -// asmtItem = asmtJoint; -// asmtJoint->setName(name); -// asmtJoint->setMarkerI(mkr1->asmtItem->fullName("")); -// asmtJoint->setMarkerJ(mkr2->asmtItem->fullName("")); -// asmtAssembly()->addJoint(asmtJoint); -//} - std::string MbD::MBDynJoint::asmtFormula() { auto ss = std::stringstream(); diff --git a/OndselSolver/MBDynMarker.cpp b/OndselSolver/MBDynMarker.cpp index 6101bce..b7630c3 100644 --- a/OndselSolver/MBDynMarker.cpp +++ b/OndselSolver/MBDynMarker.cpp @@ -13,11 +13,11 @@ void MbD::MBDynMarker::parseMBDyn(std::vector& args) rPmP = std::make_shared>(3); aAPm = FullMatrix::identitysptr(3); if (args.empty()) return; - auto str = args.at(0); //Must copy string + std::string str = args.at(0); //Must copy string if (str.find("reference") != std::string::npos) { auto strucNode = std::static_pointer_cast(nodeAt(nodeStr)); - auto rOPO = strucNode->rOfO; - auto aAOP = strucNode->aAOf; + auto& rOPO = strucNode->rOfO; + auto& aAOP = strucNode->aAOf; auto rOmO = readPosition(args); auto aAOm = readOrientation(args); rPmP = aAOP->transposeTimesFullColumn(rOmO->minusFullColumn(rOPO)); @@ -55,7 +55,7 @@ void MbD::MBDynMarker::createASMT() { auto asmtAsm = asmtAssembly(); if (nodeStr == "Assembly") { - auto mkr = std::make_shared(); + auto mkr = ASMTMarker::With(); asmtItem = mkr; mkr->setName(asmtAsm->generateUniqueMarkerName()); mkr->setPosition3D(rPmP); @@ -64,7 +64,7 @@ void MbD::MBDynMarker::createASMT() } else { auto asmtPart = asmtAsm->partPartialNamed(nodeStr); - auto mkr = std::make_shared(); + auto mkr = ASMTMarker::With(); asmtItem = mkr; mkr->setName(asmtPart->generateUniqueMarkerName()); mkr->setPosition3D(rPmP); diff --git a/OndselSolver/MBDynPrismaticJoint.cpp b/OndselSolver/MBDynPrismaticJoint.cpp index 49371dd..be0a40c 100644 --- a/OndselSolver/MBDynPrismaticJoint.cpp +++ b/OndselSolver/MBDynPrismaticJoint.cpp @@ -23,5 +23,7 @@ void MbD::MBDynPrismaticJoint::createASMT() std::shared_ptr MbD::MBDynPrismaticJoint::asmtClassNew() { - return std::make_shared(); + auto joint = ASMTNoRotationJoint::With(); + joint->initialize(); + return joint; } diff --git a/OndselSolver/MBDynReference.cpp b/OndselSolver/MBDynReference.cpp index 419b12f..c35c77a 100644 --- a/OndselSolver/MBDynReference.cpp +++ b/OndselSolver/MBDynReference.cpp @@ -28,7 +28,7 @@ void MbD::MBDynReference::readVelocity(std::vector& args) auto parser = std::make_shared(); parser->variables = mbdynVariables(); vOfO = std::make_shared>(3); - auto str = args.at(0); //Must copy string + std::string str = args.at(0); //Must copy string if (str.find("null") != std::string::npos) { args.erase(args.begin()); return; @@ -38,7 +38,7 @@ void MbD::MBDynReference::readVelocity(std::vector& args) { auto userFunc = std::make_shared(popOffTop(args), 1.0); parser->parseUserFunction(userFunc); - auto sym = parser->stack->top(); + auto& sym = parser->stack->top(); vOfO->at(i) = sym->getValue(); } } @@ -49,7 +49,7 @@ void MbD::MBDynReference::readOmega(std::vector& args) auto parser = std::make_shared(); parser->variables = mbdynVariables(); omeOfO = std::make_shared>(3); - auto str = args.at(0); //Must copy string + std::string str = args.at(0); //Must copy string if (str.find("null") != std::string::npos) { args.erase(args.begin()); return; @@ -59,7 +59,7 @@ void MbD::MBDynReference::readOmega(std::vector& args) { auto userFunc = std::make_shared(popOffTop(args), 1.0); parser->parseUserFunction(userFunc); - auto sym = parser->stack->top(); + auto& sym = parser->stack->top(); omeOfO->at(i) = sym->getValue(); } } diff --git a/OndselSolver/MBDynRevoluteHingeJoint.cpp b/OndselSolver/MBDynRevoluteHingeJoint.cpp index 2e23a44..0cdc426 100644 --- a/OndselSolver/MBDynRevoluteHingeJoint.cpp +++ b/OndselSolver/MBDynRevoluteHingeJoint.cpp @@ -23,5 +23,7 @@ void MbD::MBDynRevoluteHingeJoint::createASMT() std::shared_ptr MbD::MBDynRevoluteHingeJoint::asmtClassNew() { - return std::make_shared(); + auto joint = ASMTRevoluteJoint::With(); + joint->initialize(); + return joint; } diff --git a/OndselSolver/MBDynRevolutePinJoint.cpp b/OndselSolver/MBDynRevolutePinJoint.cpp index ee00bb0..0e1d422 100644 --- a/OndselSolver/MBDynRevolutePinJoint.cpp +++ b/OndselSolver/MBDynRevolutePinJoint.cpp @@ -24,5 +24,5 @@ void MbD::MBDynRevolutePinJoint::createASMT() std::shared_ptr MbD::MBDynRevolutePinJoint::asmtClassNew() { assert(false); - return std::make_shared(); + return ASMTJoint::With(); } diff --git a/OndselSolver/MBDynSphericalHingeJoint.cpp b/OndselSolver/MBDynSphericalHingeJoint.cpp index 6fc1973..9fbb207 100644 --- a/OndselSolver/MBDynSphericalHingeJoint.cpp +++ b/OndselSolver/MBDynSphericalHingeJoint.cpp @@ -23,5 +23,7 @@ void MbD::MBDynSphericalHingeJoint::createASMT() std::shared_ptr MbD::MBDynSphericalHingeJoint::asmtClassNew() { - return std::make_shared(); + auto joint = ASMTSphericalJoint::With(); + joint->initialize(); + return joint; } diff --git a/OndselSolver/MBDynStructural.cpp b/OndselSolver/MBDynStructural.cpp index 64a0564..1731524 100644 --- a/OndselSolver/MBDynStructural.cpp +++ b/OndselSolver/MBDynStructural.cpp @@ -39,7 +39,7 @@ void MbD::MBDynStructural::readVelocity(std::vector& args) auto parser = std::make_shared(); parser->variables = mbdynVariables(); vOfO = std::make_shared>(3); - auto str = args.at(0); //Must copy string + std::string str = args.at(0); //Must copy string if (str.find("null") != std::string::npos) { args.erase(args.begin()); return; @@ -49,7 +49,7 @@ void MbD::MBDynStructural::readVelocity(std::vector& args) { auto userFunc = std::make_shared(popOffTop(args), 1.0); parser->parseUserFunction(userFunc); - auto sym = parser->stack->top(); + auto& sym = parser->stack->top(); vOfO->at(i) = sym->getValue(); } } @@ -60,7 +60,7 @@ void MbD::MBDynStructural::readOmega(std::vector& args) auto parser = std::make_shared(); parser->variables = mbdynVariables(); omeOfO = std::make_shared>(3); - auto str = args.at(0); //Must copy string + std::string str = args.at(0); //Must copy string if (str.find("null") != std::string::npos) { args.erase(args.begin()); return; @@ -70,7 +70,7 @@ void MbD::MBDynStructural::readOmega(std::vector& args) { auto userFunc = std::make_shared(popOffTop(args), 1.0); parser->parseUserFunction(userFunc); - auto sym = parser->stack->top(); + auto& sym = parser->stack->top(); omeOfO->at(i) = sym->getValue(); } } @@ -78,7 +78,7 @@ void MbD::MBDynStructural::readOmega(std::vector& args) void MbD::MBDynStructural::createASMT() { - auto asmtPart = std::make_shared(); + auto asmtPart = ASMTPart::With(); asmtItem = asmtPart; asmtPart->setName(name); asmtPart->setPosition3D(rOfO); diff --git a/OndselSolver/MBDynSystem.cpp b/OndselSolver/MBDynSystem.cpp index b4de09d..e4b6bdd 100644 --- a/OndselSolver/MBDynSystem.cpp +++ b/OndselSolver/MBDynSystem.cpp @@ -87,7 +87,7 @@ std::shared_ptr>> MbD::MBD void MbD::MBDynSystem::createASMT() { - auto asmtAsm = std::make_shared(); + auto asmtAsm = ASMTAssembly::With(); asmtAsm->mbdynItem = this; asmtItem = asmtAsm; asmtItem->setName("Assembly"); diff --git a/OndselSolver/MBDynTotalJoint.cpp b/OndselSolver/MBDynTotalJoint.cpp index 534f05f..703e722 100644 --- a/OndselSolver/MBDynTotalJoint.cpp +++ b/OndselSolver/MBDynTotalJoint.cpp @@ -109,7 +109,7 @@ void MbD::MBDynTotalJoint::createASMT() mkr1->createASMT(); if (mkr2) mkr2->createASMT(); auto asmtAsm = asmtAssembly(); - auto asmtMotion = std::make_shared(); + auto asmtMotion = ASMTGeneralMotion::With(); asmtItem = asmtMotion; asmtMotion->setName(name); asmtMotion->setMarkerI(mkr1->asmtItem->fullName("")); diff --git a/OndselSolver/OndselSolver.vcxproj b/OndselSolver/OndselSolver.vcxproj index de7b322..4b8053b 100644 --- a/OndselSolver/OndselSolver.vcxproj +++ b/OndselSolver/OndselSolver.vcxproj @@ -144,6 +144,9 @@ + + + @@ -172,6 +175,7 @@ + @@ -196,6 +200,7 @@ + @@ -205,6 +210,7 @@ + @@ -222,6 +228,7 @@ + @@ -243,6 +250,8 @@ + + @@ -275,6 +284,7 @@ + @@ -411,6 +421,7 @@ + @@ -448,6 +459,7 @@ + @@ -472,6 +484,9 @@ + + + @@ -500,6 +515,7 @@ + @@ -524,6 +540,7 @@ + @@ -533,6 +550,7 @@ + @@ -550,6 +568,7 @@ + @@ -571,6 +590,8 @@ + + @@ -603,6 +624,7 @@ + @@ -741,6 +763,7 @@ + @@ -778,6 +801,7 @@ + diff --git a/OndselSolver/OndselSolver.vcxproj.filters b/OndselSolver/OndselSolver.vcxproj.filters index 2582775..50bd51c 100644 --- a/OndselSolver/OndselSolver.vcxproj.filters +++ b/OndselSolver/OndselSolver.vcxproj.filters @@ -993,6 +993,36 @@ Source Files + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + @@ -1979,6 +2009,36 @@ Header Files + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + diff --git a/OndselSolver/PiecewiseFunction.cpp b/OndselSolver/PiecewiseFunction.cpp index aaf6500..81b691b 100644 --- a/OndselSolver/PiecewiseFunction.cpp +++ b/OndselSolver/PiecewiseFunction.cpp @@ -76,7 +76,7 @@ Symsptr MbD::PiecewiseFunction::integrateWRT(Symsptr var) auto fi = integrals->at(i)->getValue(x); auto fi1 = integrals->at(i + 1)->getValue(x); auto integConstant = fi - fi1; - integrals->at(i + 1)->integrationConstant(integConstant); + integrals->at(i + 1)->setIntegrationConstant(integConstant); noop(); } return std::make_shared(var, integrals, transitions); diff --git a/OndselSolver/Polynomial.cpp b/OndselSolver/Polynomial.cpp index 258b7d3..b1c7d87 100644 --- a/OndselSolver/Polynomial.cpp +++ b/OndselSolver/Polynomial.cpp @@ -101,7 +101,7 @@ double MbD::Polynomial::getValue() return answer; } -void MbD::Polynomial::integrationConstant(double integConstant) +void MbD::Polynomial::setIntegrationConstant(double integConstant) { auto coeff0 = coeffs->at(0); coeff0->setValue(coeff0->getValue() + integConstant); diff --git a/OndselSolver/Polynomial.h b/OndselSolver/Polynomial.h index e9c092e..90bad36 100644 --- a/OndselSolver/Polynomial.h +++ b/OndselSolver/Polynomial.h @@ -22,7 +22,7 @@ namespace MbD { Symsptr differentiateWRTx() override; Symsptr integrateWRT(Symsptr var) override; double getValue() override; - void integrationConstant(double integConstant) override; + void setIntegrationConstant(double integConstant) override; std::ostream& printOn(std::ostream& s) const override; diff --git a/OndselSolver/PosICDragLimitNewtonRaphson.cpp b/OndselSolver/PosICDragLimitNewtonRaphson.cpp new file mode 100644 index 0000000..46f66ef --- /dev/null +++ b/OndselSolver/PosICDragLimitNewtonRaphson.cpp @@ -0,0 +1,52 @@ +#include "PosICDragLimitNewtonRaphson.h" +#include "SystemSolver.h" +#include "SimulationStoppingError.h" +#include "Part.h" +#include "Constraint.h" + +using namespace MbD; + +std::shared_ptr MbD::PosICDragLimitNewtonRaphson::With() +{ + auto newtonRaphson = std::make_shared(); + newtonRaphson->initialize(); + return newtonRaphson; +} + +void MbD::PosICDragLimitNewtonRaphson::preRun() +{ + std::string str("MbD: Assembling system with limits. "); + system->logString(str); + system->partsJointsMotionsLimitsDo([&](std::shared_ptr item) { item->prePosIC(); }); + +} + +void MbD::PosICDragLimitNewtonRaphson::initializeGlobally() +{ + AnyPosICNewtonRaphson::initializeGlobally(); + iterMax = system->iterMaxPosKine; + dxTol = system->errorTolPosKine; +} + +void MbD::PosICDragLimitNewtonRaphson::setdragParts(std::shared_ptr>> dragParts) +{ + assert(false); +} + +void MbD::PosICDragLimitNewtonRaphson::run() +{ + preRun(); + system->deactivateLimits(); + if (system->limitsSatisfied()) return; + for (auto& limit : *system->limits()) { + limit->activate(); + preRun(); + initializeLocally(); + initializeGlobally(); + iterate(); + postRun(); + system->deactivateLimits(); + if (system->limitsSatisfied()) return; + } + throw SimulationStoppingError("Limits cannot be satisfiled."); +} diff --git a/OndselSolver/PosICDragLimitNewtonRaphson.h b/OndselSolver/PosICDragLimitNewtonRaphson.h new file mode 100644 index 0000000..219dd33 --- /dev/null +++ b/OndselSolver/PosICDragLimitNewtonRaphson.h @@ -0,0 +1,28 @@ +/*************************************************************************** + * Copyright (c) 2023 Ondsel, Inc. * + * * + * This file is part of OndselSolver. * + * * + * See LICENSE file for details about copyright. * + ***************************************************************************/ + +#pragma once + +#include "AnyPosICNewtonRaphson.h" + +namespace MbD { + class Part; + + class PosICDragLimitNewtonRaphson : public AnyPosICNewtonRaphson + { + //Kinematics with under constrained system + public: + static std::shared_ptr With(); + void preRun() override; + void initializeGlobally() override; + void setdragParts(std::shared_ptr>> dragParts); + void run() override; + + std::shared_ptr>> dragParts; + }; +} diff --git a/OndselSolver/PosICDragNewtonRaphson.cpp b/OndselSolver/PosICDragNewtonRaphson.cpp index 4e186b0..0c27bf8 100644 --- a/OndselSolver/PosICDragNewtonRaphson.cpp +++ b/OndselSolver/PosICDragNewtonRaphson.cpp @@ -38,43 +38,6 @@ void MbD::PosICDragNewtonRaphson::initializeGlobally() } } -void MbD::PosICDragNewtonRaphson::assignEquationNumbers() -{ - auto parts = system->parts(); - //auto contactEndFrames = system->contactEndFrames(); - //auto uHolders = system->uHolders(); - auto constraints = system->allConstraints(); - size_t eqnNo = 0; - for (auto& part : *parts) { - part->iqX(eqnNo); - eqnNo = eqnNo + 3; - part->iqE(eqnNo); - eqnNo = eqnNo + 4; - } - //for (auto& endFrm : *contactEndFrames) { - // endFrm->is(eqnNo); - // eqnNo = eqnNo + endFrm->sSize(); - //} - //for (auto& uHolder : *uHolders) { - // uHolder->iu(eqnNo); - // eqnNo += 1; - //} - auto nEqns = eqnNo; //C++ uses index 0. - nqsu = nEqns; - for (auto& con : *constraints) { - con->iG = eqnNo; - eqnNo += 1; - } - //auto lastEqnNo = eqnNo - 1; - nEqns = eqnNo; //C++ uses index 0. - n = nEqns; -} - -bool MbD::PosICDragNewtonRaphson::isConverged() -{ - return dxNorms->at(iterNo) < dxTol || isConvergedToNumericalLimit(); -} - void MbD::PosICDragNewtonRaphson::setdragParts(std::shared_ptr>> _dragParts) { dragParts = _dragParts; diff --git a/OndselSolver/PosICDragNewtonRaphson.h b/OndselSolver/PosICDragNewtonRaphson.h index 8e4debd..cf854c7 100644 --- a/OndselSolver/PosICDragNewtonRaphson.h +++ b/OndselSolver/PosICDragNewtonRaphson.h @@ -19,8 +19,6 @@ namespace MbD { public: static std::shared_ptr With(); void initializeGlobally() override; - void assignEquationNumbers() override; - bool isConverged() override; void setdragParts(std::shared_ptr>> dragParts); std::shared_ptr>> dragParts; diff --git a/OndselSolver/PosICKineNewtonRaphson.cpp b/OndselSolver/PosICKineNewtonRaphson.cpp index 28eebbb..c7c8569 100644 --- a/OndselSolver/PosICKineNewtonRaphson.cpp +++ b/OndselSolver/PosICKineNewtonRaphson.cpp @@ -20,45 +20,8 @@ void PosICKineNewtonRaphson::initializeGlobally() dxTol = system->errorTolPosKine; } -void PosICKineNewtonRaphson::assignEquationNumbers() -{ - auto parts = system->parts(); - //auto contactEndFrames = system->contactEndFrames(); - //auto uHolders = system->uHolders(); - auto constraints = system->allConstraints(); - size_t eqnNo = 0; - for (auto& part : *parts) { - part->iqX(eqnNo); - eqnNo = eqnNo + 3; - part->iqE(eqnNo); - eqnNo = eqnNo + 4; - } - //for (auto& endFrm : *contactEndFrames) { - // endFrm->is(eqnNo); - // eqnNo = eqnNo + endFrm->sSize(); - //} - //for (auto& uHolder : *uHolders) { - // uHolder->iu(eqnNo); - // eqnNo += 1; - //} - auto nEqns = eqnNo; //C++ uses index 0. - nqsu = nEqns; - for (auto& con : *constraints) { - con->iG = eqnNo; - eqnNo += 1; - } - //auto lastEqnNo = eqnNo - 1; - nEqns = eqnNo; //C++ uses index 0. - n = nEqns; -} - void MbD::PosICKineNewtonRaphson::preRun() { system->Solver::logString("MbD: Solving for quasi kinematic position."); PosNewtonRaphson::preRun(); } - -bool MbD::PosICKineNewtonRaphson::isConverged() -{ - return dxNorms->at(iterNo) < dxTol || isConvergedToNumericalLimit(); -} diff --git a/OndselSolver/PosICKineNewtonRaphson.h b/OndselSolver/PosICKineNewtonRaphson.h index 0769f1b..fa9d820 100644 --- a/OndselSolver/PosICKineNewtonRaphson.h +++ b/OndselSolver/PosICKineNewtonRaphson.h @@ -16,9 +16,7 @@ namespace MbD { //Kinematics with under constrained system public: void initializeGlobally() override; - void assignEquationNumbers() override; void preRun() override; - bool isConverged() override; }; } diff --git a/OndselSolver/PosICNewtonRaphson.cpp b/OndselSolver/PosICNewtonRaphson.cpp index 2c32760..d97779d 100644 --- a/OndselSolver/PosICNewtonRaphson.cpp +++ b/OndselSolver/PosICNewtonRaphson.cpp @@ -34,9 +34,9 @@ void PosICNewtonRaphson::run() } catch (SingularMatrixError ex) { auto redundantEqnNos = ex.getRedundantEqnNos(); - system->partsJointsMotionsDo([&](std::shared_ptr item) { item->removeRedundantConstraints(redundantEqnNos); }); - system->partsJointsMotionsDo([&](std::shared_ptr item) { item->constraintsReport(); }); - system->partsJointsMotionsDo([&](std::shared_ptr item) { item->setqsu(qsuOld); }); + system->partsJointsMotionsLimitsDo([&](std::shared_ptr item) { item->removeRedundantConstraints(redundantEqnNos); }); + system->partsJointsMotionsLimitsDo([&](std::shared_ptr item) { item->constraintsReport(); }); + system->partsJointsMotionsLimitsDo([&](std::shared_ptr item) { item->setqsu(qsuOld); }); } } } diff --git a/OndselSolver/PosKineNewtonRaphson.cpp b/OndselSolver/PosKineNewtonRaphson.cpp index 44f75c3..41e79f4 100644 --- a/OndselSolver/PosKineNewtonRaphson.cpp +++ b/OndselSolver/PosKineNewtonRaphson.cpp @@ -18,7 +18,7 @@ using namespace MbD; void PosKineNewtonRaphson::initializeGlobally() { SystemNewtonRaphson::initializeGlobally(); - system->partsJointsMotionsDo([&](std::shared_ptr item) { item->fillqsu(x); }); + system->partsJointsMotionsLimitsDo([&](std::shared_ptr item) { item->fillqsu(x); }); iterMax = system->iterMaxPosKine; dxTol = system->errorTolPosKine; } @@ -26,14 +26,14 @@ void PosKineNewtonRaphson::initializeGlobally() void PosKineNewtonRaphson::fillPyPx() { pypx->zeroSelf(); - system->partsJointsMotionsDo([&](std::shared_ptr item) { + system->partsJointsMotionsLimitsDo([&](std::shared_ptr item) { item->fillPosKineJacob(pypx); }); } void PosKineNewtonRaphson::passRootToSystem() { - system->partsJointsMotionsDo([&](std::shared_ptr item) { item->setqsu(x); }); + system->partsJointsMotionsLimitsDo([&](std::shared_ptr item) { item->setqsu(x); }); } void PosKineNewtonRaphson::assignEquationNumbers() @@ -75,13 +75,13 @@ void PosKineNewtonRaphson::preRun() { std::string str = "MbD: Solving for kinematic position."; system->logString(str); - system->partsJointsMotionsDo([](std::shared_ptr item) { item->prePosKine(); }); + system->partsJointsMotionsLimitsDo([](std::shared_ptr item) { item->prePosKine(); }); } void PosKineNewtonRaphson::fillY() { y->zeroSelf(); - system->partsJointsMotionsDo([&](std::shared_ptr item) { + system->partsJointsMotionsLimitsDo([&](std::shared_ptr item) { item->fillPosKineError(y); //std::cout << item->name << *y << std::endl; //noop(); diff --git a/OndselSolver/PosNewtonRaphson.cpp b/OndselSolver/PosNewtonRaphson.cpp index 0fa0dd8..60edc7e 100644 --- a/OndselSolver/PosNewtonRaphson.cpp +++ b/OndselSolver/PosNewtonRaphson.cpp @@ -17,7 +17,7 @@ using namespace MbD; void PosNewtonRaphson::preRun() { - system->partsJointsMotionsDo([&](std::shared_ptr item) { item->prePosIC(); }); + system->partsJointsMotionsLimitsDo([&](std::shared_ptr item) { item->prePosIC(); }); } void PosNewtonRaphson::incrementIterNo() @@ -44,10 +44,10 @@ void PosNewtonRaphson::incrementIterNo() void PosNewtonRaphson::askSystemToUpdate() { - system->partsJointsMotionsDo([&](std::shared_ptr item) { item->postPosICIteration(); }); + system->partsJointsMotionsLimitsDo([&](std::shared_ptr item) { item->postPosICIteration(); }); } void PosNewtonRaphson::postRun() { - system->partsJointsMotionsDo([&](std::shared_ptr item) { item->postPosIC(); }); + system->partsJointsMotionsLimitsDo([&](std::shared_ptr item) { item->postPosIC(); }); } diff --git a/OndselSolver/QuasiIntegrator.cpp b/OndselSolver/QuasiIntegrator.cpp index 86f7b95..9ec9480 100644 --- a/OndselSolver/QuasiIntegrator.cpp +++ b/OndselSolver/QuasiIntegrator.cpp @@ -24,7 +24,7 @@ using namespace MbD; void QuasiIntegrator::preRun() { - system->partsJointsMotionsForcesTorquesDo([](std::shared_ptr item) { item->preDyn(); }); + system->partsJointsMotionsLimitsForcesTorquesDo([](std::shared_ptr item) { item->preDyn(); }); } void QuasiIntegrator::initialize() @@ -76,12 +76,12 @@ void QuasiIntegrator::run() void QuasiIntegrator::preFirstStep() { - system->partsJointsMotionsForcesTorquesDo([](std::shared_ptr item) { item->preDynFirstStep(); }); + system->partsJointsMotionsLimitsForcesTorquesDo([](std::shared_ptr item) { item->preDynFirstStep(); }); } void QuasiIntegrator::postFirstStep() { - system->partsJointsMotionsForcesTorquesDo([](std::shared_ptr item) { item->postDynFirstStep(); }); + system->partsJointsMotionsLimitsForcesTorquesDo([](std::shared_ptr item) { item->postDynFirstStep(); }); if (integrator->istep > 0) { //"Noise make checking at the start unreliable." this->checkForDiscontinuity(); @@ -91,7 +91,7 @@ void QuasiIntegrator::postFirstStep() void QuasiIntegrator::preStep() { - system->partsJointsMotionsForcesTorquesDo([](std::shared_ptr item) { item->preDynStep(); }); + system->partsJointsMotionsLimitsForcesTorquesDo([](std::shared_ptr item) { item->preDynStep(); }); } void QuasiIntegrator::checkForDiscontinuity() @@ -110,7 +110,7 @@ void QuasiIntegrator::checkForDiscontinuity() epsilon = std::abs(t) * epsilon; tstartNew = ((direction * t) + epsilon) / direction; } - system->partsJointsMotionsForcesTorquesDo([&](std::shared_ptr item) { tstartNew = item->checkForDynDiscontinuityBetweenand(tprevious, tstartNew); }); + system->partsJointsMotionsLimitsForcesTorquesDo([&](std::shared_ptr item) { tstartNew = item->checkForDynDiscontinuityBetweenand(tprevious, tstartNew); }); if ((direction * tstartNew) > (direction * t)) { //"No discontinuity in step" return; @@ -122,7 +122,7 @@ void QuasiIntegrator::checkForDiscontinuity() system->tstart = tstartNew; system->toutFirst = tout; auto discontinuityTypes = std::make_shared>(); - system->partsJointsMotionsForcesTorquesDo([&](std::shared_ptr item) { item->discontinuityAtaddTypeTo(tstartNew, discontinuityTypes); }); + system->partsJointsMotionsLimitsForcesTorquesDo([&](std::shared_ptr item) { item->discontinuityAtaddTypeTo(tstartNew, discontinuityTypes); }); this->throwDiscontinuityError("", discontinuityTypes); } } @@ -130,7 +130,7 @@ void QuasiIntegrator::checkForDiscontinuity() double QuasiIntegrator::suggestSmallerOrAcceptFirstStepSize(double hnew) { auto hnew2 = hnew; - system->partsJointsMotionsForcesTorquesDo([&](std::shared_ptr item) { hnew2 = item->suggestSmallerOrAcceptDynFirstStepSize(hnew2); }); + system->partsJointsMotionsLimitsForcesTorquesDo([&](std::shared_ptr item) { hnew2 = item->suggestSmallerOrAcceptDynFirstStepSize(hnew2); }); if (hnew2 > hmax) { hnew2 = hmax; std::string str = "StM: Step size is at user specified maximum."; @@ -149,7 +149,7 @@ double QuasiIntegrator::suggestSmallerOrAcceptFirstStepSize(double hnew) double QuasiIntegrator::suggestSmallerOrAcceptStepSize(double hnew) { auto hnew2 = hnew; - system->partsJointsMotionsForcesTorquesDo([&](std::shared_ptr item) { hnew2 = item->suggestSmallerOrAcceptDynStepSize(hnew2); }); + system->partsJointsMotionsLimitsForcesTorquesDo([&](std::shared_ptr item) { hnew2 = item->suggestSmallerOrAcceptDynStepSize(hnew2); }); if (hnew2 > hmax) { hnew2 = hmax; this->Solver::logString("StM: Step size is at user specified maximum."); @@ -166,7 +166,7 @@ double QuasiIntegrator::suggestSmallerOrAcceptStepSize(double hnew) void QuasiIntegrator::incrementTime(double tnew) { - system->partsJointsMotionsForcesTorquesDo([](std::shared_ptr item) { item->storeDynState(); }); + system->partsJointsMotionsLimitsForcesTorquesDo([](std::shared_ptr item) { item->storeDynState(); }); IntegratorInterface::incrementTime(tnew); } @@ -198,7 +198,7 @@ void QuasiIntegrator::interpolateAt(double tArg) void QuasiIntegrator::postStep() { - system->partsJointsMotionsForcesTorquesDo([](std::shared_ptr item) { item->postDynStep(); }); + system->partsJointsMotionsLimitsForcesTorquesDo([](std::shared_ptr item) { item->postDynStep(); }); if (integrator->istep > 0) { //"Noise make checking at the start unreliable." @@ -209,5 +209,5 @@ void QuasiIntegrator::postStep() void QuasiIntegrator::postRun() { - system->partsJointsMotionsForcesTorquesDo([](std::shared_ptr item) { item->postDyn(); }); + system->partsJointsMotionsLimitsForcesTorquesDo([](std::shared_ptr item) { item->postDyn(); }); } diff --git a/OndselSolver/RackPinConstraintIqcJc.cpp b/OndselSolver/RackPinConstraintIqcJc.cpp index cc04cdf..a88ed8f 100644 --- a/OndselSolver/RackPinConstraintIqcJc.cpp +++ b/OndselSolver/RackPinConstraintIqcJc.cpp @@ -135,7 +135,7 @@ void MbD::RackPinConstraintIqcJc::fillVelICJacob(SpMatDsptr mat) void MbD::RackPinConstraintIqcJc::init_xthez() { xIeJeIe = CREATE::With(frmI, frmJ, 0); - thezIeJe = CREATE::With(frmJ, frmI); + thezIeJe = CREATE::With(frmI, frmJ); } void MbD::RackPinConstraintIqcJc::useEquationNumbers() diff --git a/OndselSolver/RackPinConstraintIqcJqc.cpp b/OndselSolver/RackPinConstraintIqcJqc.cpp index 8291ef5..3340794 100644 --- a/OndselSolver/RackPinConstraintIqcJqc.cpp +++ b/OndselSolver/RackPinConstraintIqcJqc.cpp @@ -35,7 +35,6 @@ void MbD::RackPinConstraintIqcJqc::initthezIeJe() void MbD::RackPinConstraintIqcJqc::calc_pGpEJ() { - pGpEJ = xIeJeIe->pvaluepEJ()->plusFullRow(thezIeJe->pvaluepEJ()->times(pitchRadius)); } void MbD::RackPinConstraintIqcJqc::calc_pGpXJ() @@ -131,7 +130,7 @@ void MbD::RackPinConstraintIqcJqc::fillVelICJacob(SpMatDsptr mat) void MbD::RackPinConstraintIqcJqc::init_xthez() { xIeJeIe = CREATE::With(frmI, frmJ, 0); - thezIeJe = CREATE::With(frmJ, frmI); + thezIeJe = CREATE::With(frmI, frmJ); } void MbD::RackPinConstraintIqcJqc::useEquationNumbers() diff --git a/OndselSolver/RotationLimitIJ.cpp b/OndselSolver/RotationLimitIJ.cpp new file mode 100644 index 0000000..36eba0a --- /dev/null +++ b/OndselSolver/RotationLimitIJ.cpp @@ -0,0 +1,25 @@ +#include "RotationLimitIJ.h" +#include "AngleZConstraintIJ.h" +#include "System.h" + +using namespace MbD; + +std::shared_ptr MbD::RotationLimitIJ::With() +{ + auto rotationLimit = std::make_shared(); + rotationLimit->initialize(); + return rotationLimit; +} + +void MbD::RotationLimitIJ::initializeGlobally() +{ + if (constraints->empty()) { + auto angleZConIJ = AngleZConstraintIJ::With(frmI, frmJ); + angleZConIJ->setConstant(limit); + addConstraint(angleZConIJ); + this->root()->hasChanged = true; + } + else { + LimitIJ::initializeGlobally(); + } +} diff --git a/OndselSolver/RotationLimitIJ.h b/OndselSolver/RotationLimitIJ.h new file mode 100644 index 0000000..614d7c9 --- /dev/null +++ b/OndselSolver/RotationLimitIJ.h @@ -0,0 +1,22 @@ +/*************************************************************************** + * Copyright (c) 2023 Ondsel, Inc. * + * * + * This file is part of OndselSolver. * + * * + * See LICENSE file for details about copyright. * + ***************************************************************************/ + +#pragma once + +#include "LimitIJ.h" + +namespace MbD { + class RotationLimitIJ : public LimitIJ + { + // + public: + static std::shared_ptr With(); + void initializeGlobally() override; + + }; +} diff --git a/OndselSolver/ScrewConstraintIqcJc.cpp b/OndselSolver/ScrewConstraintIqcJc.cpp index 6e2b15e..d5ae620 100644 --- a/OndselSolver/ScrewConstraintIqcJc.cpp +++ b/OndselSolver/ScrewConstraintIqcJc.cpp @@ -137,7 +137,7 @@ void MbD::ScrewConstraintIqcJc::fillVelICJacob(SpMatDsptr mat) void MbD::ScrewConstraintIqcJc::init_zthez() { zIeJeIe = CREATE::With(frmI, frmJ, 2); - thezIeJe = CREATE::With(frmJ, frmI); + thezIeJe = CREATE::With(frmI, frmJ); } void MbD::ScrewConstraintIqcJc::useEquationNumbers() diff --git a/OndselSolver/ScrewConstraintIqcJqc.cpp b/OndselSolver/ScrewConstraintIqcJqc.cpp index bdc6ab7..5564020 100644 --- a/OndselSolver/ScrewConstraintIqcJqc.cpp +++ b/OndselSolver/ScrewConstraintIqcJqc.cpp @@ -133,7 +133,7 @@ void MbD::ScrewConstraintIqcJqc::fillVelICJacob(SpMatDsptr mat) void MbD::ScrewConstraintIqcJqc::init_zthez() { zIeJeIe = CREATE::With(frmI, frmJ, 2); - thezIeJe = CREATE::With(frmJ, frmI); + thezIeJe = CREATE::With(frmI, frmJ); } void MbD::ScrewConstraintIqcJqc::useEquationNumbers() diff --git a/OndselSolver/SparseVector.h b/OndselSolver/SparseVector.h index 4c79431..814e6e9 100644 --- a/OndselSolver/SparseVector.h +++ b/OndselSolver/SparseVector.h @@ -11,6 +11,7 @@ #include #include #include +#include namespace MbD { template @@ -115,16 +116,20 @@ namespace MbD { template inline std::ostream& SparseVector::printOn(std::ostream& s) const { - s << "{"; + + std::stringstream ss; + ss << std::setprecision(std::numeric_limits::max_digits10); + ss << "{" << std::endl; auto index = 0; for (const auto& keyValue : *this) { - if (index > 0) s << ", "; - s << keyValue.first; - s << "->"; - s << keyValue.second; + if (index > 0) ss << ", " << std::endl; + ss << keyValue.first; + ss << "->"; + ss << keyValue.second; index++; } - s << "}"; + ss << std::endl << "}"; + s << ss.str(); return s; } } diff --git a/OndselSolver/StableBackwardDifference.cpp b/OndselSolver/StableBackwardDifference.cpp index ad98b99..fc38f2b 100644 --- a/OndselSolver/StableBackwardDifference.cpp +++ b/OndselSolver/StableBackwardDifference.cpp @@ -59,7 +59,7 @@ void StableBackwardDifference::formTaylorRowwithTimeNodederivative(size_t i, siz //| rowi hi hipower aij | auto& rowi = taylorMatrix->at(i); if (k > 0) { - for (size_t j = 0; j < k - 2; j++) + for (int j = 0; j < (int)k - 2; j++) //Use int because of subtraction { rowi->at(j) = 0.0; } diff --git a/OndselSolver/Sum.cpp b/OndselSolver/Sum.cpp index 3a33cc7..85520a2 100644 --- a/OndselSolver/Sum.cpp +++ b/OndselSolver/Sum.cpp @@ -12,6 +12,7 @@ #include "Sum.h" #include "Constant.h" #include +#include "Integral.h" using namespace MbD; @@ -102,21 +103,18 @@ Symsptr Sum::simplifyUntil(Symsptr, std::shared_ptr> newTerms->push_back(newTerm); } } + if (newTerms->size() == 0 && constant == 0.0) { + return sptrConstant(0.0); + } if (constant != 0.0) { newTerms->insert(newTerms->begin(), sptrConstant(constant)); } - auto newSize = newTerms->size(); - if (newSize == 0) { - return sptrConstant(0.0); - } - else if (newSize == 1) { + if (newTerms->size() == 1) { return newTerms->at(0); } - else { - auto answer = std::make_shared(); - answer->terms = newTerms; - return answer; - } + auto answer = std::make_shared(); + answer->terms = newTerms; + return answer; } bool Sum::isSum() @@ -139,19 +137,21 @@ Symsptr MbD::Sum::clonesptr() Symsptr MbD::Sum::differentiateWRT(Symsptr var) { auto derivatives = std::make_shared>(); - std::transform(terms->begin(), - terms->end(), - std::back_inserter(*derivatives), - [var](Symsptr term) { return term->differentiateWRT(var); } - ); + for (const auto& term : *terms) { + auto deriv = term->differentiateWRT(var); + derivatives->push_back(deriv); + } auto answer = std::make_shared(); answer->terms = derivatives; - return answer; + return answer->simplified(); } Symsptr MbD::Sum::integrateWRT(Symsptr var) { auto simple = simplified(); + auto answer = std::make_shared(); + answer->xx = var; + answer->integrand = simple;; if (simple->isSum()) { auto newTerms = simple->getTerms(); auto integrals = std::make_shared>(); @@ -160,13 +160,14 @@ Symsptr MbD::Sum::integrateWRT(Symsptr var) std::back_inserter(*integrals), [var](Symsptr term) { return term->integrateWRT(var); } ); - auto answer = std::make_shared(); - answer->terms = integrals; - return answer; + auto sum = std::make_shared(); + sum->terms = integrals; + answer->expression = sum->simplified(); } else { - return simple->integrateWRT(var); + answer->expression = simple->integrateWRT(var); } + return answer; } std::ostream& Sum::printOn(std::ostream& s) const diff --git a/OndselSolver/Symbolic.cpp b/OndselSolver/Symbolic.cpp index 8fcc0bf..75bcbb6 100644 --- a/OndselSolver/Symbolic.cpp +++ b/OndselSolver/Symbolic.cpp @@ -217,7 +217,7 @@ bool MbD::Symbolic::isVariable() return false; } -void MbD::Symbolic::integrationConstant(double) +void MbD::Symbolic::setIntegrationConstant(double) { assert(false); } diff --git a/OndselSolver/Symbolic.h b/OndselSolver/Symbolic.h index f1b91c5..68f77f4 100644 --- a/OndselSolver/Symbolic.h +++ b/OndselSolver/Symbolic.h @@ -54,7 +54,7 @@ namespace MbD { virtual Symsptr clonesptr(); std::shared_ptr sptrConstant(double value); virtual bool isVariable(); - virtual void integrationConstant(double integConstant); + virtual void setIntegrationConstant(double integConstant); virtual std::ostream& printOn(std::ostream& s) const; friend std::ostream& operator<<(std::ostream& s, const Symbolic& sym) diff --git a/OndselSolver/System.cpp b/OndselSolver/System.cpp index 52f8951..b87e7d2 100644 --- a/OndselSolver/System.cpp +++ b/OndselSolver/System.cpp @@ -25,6 +25,7 @@ System::System() { time = std::make_shared