diff --git a/OndselSolver/ASMTSpatialContainer.cpp b/OndselSolver/ASMTSpatialContainer.cpp index 7492d91..c71e156 100644 --- a/OndselSolver/ASMTSpatialContainer.cpp +++ b/OndselSolver/ASMTSpatialContainer.cpp @@ -301,6 +301,31 @@ void MbD::ASMTSpatialContainer::updateMbDFromPosition3D(FColDsptr vec) mbdPart->qX(rOcmO()->times(1.0 / mbdUnits->length)); } +void MbD::ASMTSpatialContainer::updateMbDFromPosition3D(double a, double b, double c) +{ + auto pos3D = std::make_shared>(ListD{ a, b, c }); + updateMbDFromPosition3D(pos3D); +} + +void MbD::ASMTSpatialContainer::updateMbDFromRotationMatrix(FMatDsptr mat) +{ + rotationMatrix = mat; + auto mbdPart = std::static_pointer_cast(mbdObject); + auto mbdUnits = this->mbdUnits(); + mbdPart->qX(rOcmO()->times(1.0 / mbdUnits->length)); + mbdPart->qE(qEp()); +} + +void MbD::ASMTSpatialContainer::updateMbDFromRotationMatrix(double v11, double v12, double v13, double v21, double v22, double v23, double v31, double v32, double v33) +{ + auto rotMat = std::make_shared>(ListListD{ + { v11, v12, v13 }, + { v21, v22, v23 }, + { v31, v32, v33 } + }); + updateMbDFromRotationMatrix(rotMat); +} + FColDsptr MbD::ASMTSpatialContainer::rOcmO() { auto& rOPO = position3D; diff --git a/OndselSolver/ASMTSpatialContainer.h b/OndselSolver/ASMTSpatialContainer.h index ac86254..b0452ad 100644 --- a/OndselSolver/ASMTSpatialContainer.h +++ b/OndselSolver/ASMTSpatialContainer.h @@ -5,13 +5,13 @@ * * * See LICENSE file for details about copyright. * ***************************************************************************/ - + #pragma once #include "ASMTSpatialItem.h" -//#include "ASMTRefPoint.h" -//#include "ASMTRefCurve.h" -//#include "ASMTRefSurface.h" + //#include "ASMTRefPoint.h" + //#include "ASMTRefCurve.h" + //#include "ASMTRefSurface.h" #include "ASMTPrincipalMassMarker.h" //#include "Units.h" //#include "ASMTPart.h" @@ -19,88 +19,93 @@ //#include "ASMTMotion.h" namespace MbD { - class ASMTRefPoint; - class ASMTRefCurve; - class ASMTRefSurface; - class ASMTPrincipalMassMarker; - class Units; - class ASMTPart; - class ASMTJoint; - class ASMTMotion; - class ASMTMarker; + class ASMTRefPoint; + class ASMTRefCurve; + class ASMTRefSurface; + class ASMTPrincipalMassMarker; + class Units; + class ASMTPart; + class ASMTJoint; + class ASMTMotion; + class ASMTMarker; - class ASMTSpatialContainer : public ASMTSpatialItem - { - // - public: - ASMTSpatialContainer(); - void initialize() override; - void setPrincipalMassMarker(std::shared_ptr aJ); - void readRefPoints(std::vector& lines); - void readRefPoint(std::vector& lines); - void readRefCurves(std::vector& lines); - void readRefCurve(std::vector& lines); - void readRefSurfaces(std::vector& lines); - void readRefSurface(std::vector& lines); - void readXs(std::vector& lines); - void readYs(std::vector& lines); - void readZs(std::vector& lines); - void readBryantxs(std::vector& lines); - void readBryantys(std::vector& lines); - void readBryantzs(std::vector& lines); - void readVXs(std::vector& lines); - void readVYs(std::vector& lines); - void readVZs(std::vector& lines); - void readOmegaXs(std::vector& lines); - void readOmegaYs(std::vector& lines); - void readOmegaZs(std::vector& lines); - void readAXs(std::vector& lines); - void readAYs(std::vector& lines); - void readAZs(std::vector& lines); - void readAlphaXs(std::vector& lines); - void readAlphaYs(std::vector& lines); - void readAlphaZs(std::vector& lines); - void createMbD(std::shared_ptr mbdSys, std::shared_ptr mbdUnits) override; - void updateMbDFromPosition3D(FColDsptr position3D); - FColDsptr rOcmO(); - std::shared_ptr> qEp(); - virtual FColDsptr vOcmO(); - virtual FColDsptr omeOpO(); - ASMTSpatialContainer* partOrAssembly() override; - void updateFromMbD() override; - void compareResults(AnalysisType type) override; - void outputResults(AnalysisType type) override; - void addRefPoint(std::shared_ptr refPoint); - void addMarker(std::shared_ptr marker); - std::string generateUniqueMarkerName(); - std::shared_ptr>> markerList(); - void setVelocity3D(FColDsptr velocity3D); - void setOmega3D(FColDsptr omega3D); - void readVelocity3D(std::vector& lines); - void readOmega3D(std::vector& lines); - void setVelocity3D(double a, double b, double c); - void setOmega3D(double a, double b, double c); - void storeOnLevel(std::ofstream& os, size_t level) override; - void storeOnLevelVelocity(std::ofstream& os, size_t level); - void storeOnLevelOmega(std::ofstream& os, size_t level); - void storeOnLevelRefPoints(std::ofstream& os, size_t level); - void storeOnLevelRefCurves(std::ofstream& os, size_t level); - void storeOnLevelRefSurfaces(std::ofstream& os, size_t level); - void storeOnTimeSeries(std::ofstream& os) override; - FColDsptr getVelocity3D(size_t i); - FColDsptr getOmega3D(size_t i); + class ASMTSpatialContainer : public ASMTSpatialItem + { + // + public: + ASMTSpatialContainer(); + void initialize() override; + void setPrincipalMassMarker(std::shared_ptr aJ); + void readRefPoints(std::vector& lines); + void readRefPoint(std::vector& lines); + void readRefCurves(std::vector& lines); + void readRefCurve(std::vector& lines); + void readRefSurfaces(std::vector& lines); + void readRefSurface(std::vector& lines); + void readXs(std::vector& lines); + void readYs(std::vector& lines); + void readZs(std::vector& lines); + void readBryantxs(std::vector& lines); + void readBryantys(std::vector& lines); + void readBryantzs(std::vector& lines); + void readVXs(std::vector& lines); + void readVYs(std::vector& lines); + void readVZs(std::vector& lines); + void readOmegaXs(std::vector& lines); + void readOmegaYs(std::vector& lines); + void readOmegaZs(std::vector& lines); + void readAXs(std::vector& lines); + void readAYs(std::vector& lines); + void readAZs(std::vector& lines); + void readAlphaXs(std::vector& lines); + void readAlphaYs(std::vector& lines); + void readAlphaZs(std::vector& lines); + void createMbD(std::shared_ptr mbdSys, std::shared_ptr mbdUnits) override; + void updateMbDFromPosition3D(FColDsptr position3D); + void updateMbDFromPosition3D(double a, double b, double c); + void updateMbDFromRotationMatrix(FMatDsptr mat); + void updateMbDFromRotationMatrix(double v11, double v12, double v13, + double v21, double v22, double v23, + double v31, double v32, double v33); + FColDsptr rOcmO(); + std::shared_ptr> qEp(); + virtual FColDsptr vOcmO(); + virtual FColDsptr omeOpO(); + ASMTSpatialContainer* partOrAssembly() override; + void updateFromMbD() override; + void compareResults(AnalysisType type) override; + void outputResults(AnalysisType type) override; + void addRefPoint(std::shared_ptr refPoint); + void addMarker(std::shared_ptr marker); + std::string generateUniqueMarkerName(); + std::shared_ptr>> markerList(); + void setVelocity3D(FColDsptr velocity3D); + void setOmega3D(FColDsptr omega3D); + void readVelocity3D(std::vector& lines); + void readOmega3D(std::vector& lines); + void setVelocity3D(double a, double b, double c); + void setOmega3D(double a, double b, double c); + void storeOnLevel(std::ofstream& os, size_t level) override; + void storeOnLevelVelocity(std::ofstream& os, size_t level); + void storeOnLevelOmega(std::ofstream& os, size_t level); + void storeOnLevelRefPoints(std::ofstream& os, size_t level); + void storeOnLevelRefCurves(std::ofstream& os, size_t level); + void storeOnLevelRefSurfaces(std::ofstream& os, size_t level); + void storeOnTimeSeries(std::ofstream& os) override; + FColDsptr getVelocity3D(size_t i); + FColDsptr getOmega3D(size_t i); - FColDsptr velocity3D = std::make_shared>(3); - FColDsptr omega3D = std::make_shared>(3); - std::shared_ptr>> refPoints; - std::shared_ptr>> refCurves; - std::shared_ptr>> refSurfaces; - FRowDsptr vxs, vys, vzs, omexs, omeys, omezs; - 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 = ASMTPrincipalMassMarker::With(); + FColDsptr velocity3D = std::make_shared>(3); + FColDsptr omega3D = std::make_shared>(3); + std::shared_ptr>> refPoints; + std::shared_ptr>> refCurves; + std::shared_ptr>> refSurfaces; + FRowDsptr vxs, vys, vzs, omexs, omeys, omezs; + 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 = ASMTPrincipalMassMarker::With(); - }; + }; } diff --git a/OndselSolver/PosICDragLimitNewtonRaphson.cpp b/OndselSolver/PosICDragLimitNewtonRaphson.cpp index 46f66ef..6321914 100644 --- a/OndselSolver/PosICDragLimitNewtonRaphson.cpp +++ b/OndselSolver/PosICDragLimitNewtonRaphson.cpp @@ -3,6 +3,7 @@ #include "SimulationStoppingError.h" #include "Part.h" #include "Constraint.h" +#include using namespace MbD; @@ -38,7 +39,11 @@ void MbD::PosICDragLimitNewtonRaphson::run() preRun(); system->deactivateLimits(); if (system->limitsSatisfied()) return; - for (auto& limit : *system->limits()) { + auto limits = system->limits(); + std::partition(limits->begin(), limits->end(), [](auto limit) { return !limit->satisfied(); }); + //Violated limits are in front. + for (auto it = limits->begin(); it != limits->end(); it++) { + auto limit = *it; limit->activate(); preRun(); initializeLocally(); diff --git a/OndselSolver/ScrewConstraintIJ.cpp b/OndselSolver/ScrewConstraintIJ.cpp index 996253f..04e7617 100644 --- a/OndselSolver/ScrewConstraintIJ.cpp +++ b/OndselSolver/ScrewConstraintIJ.cpp @@ -72,7 +72,9 @@ void MbD::ScrewConstraintIJ::postInput() { zIeJeIe->postInput(); thezIeJe->postInput(); - aConstant = (2.0 * OS_M_PI * zIeJeIe->value()) - (thezIeJe->value() * pitch); + if (aConstant == std::numeric_limits::min()) { + aConstant = (2.0 * OS_M_PI * zIeJeIe->value()) - (thezIeJe->value() * pitch); + } ConstraintIJ::postInput(); }