Skip to content

Commit

Permalink
updateMbDFromRotationMatrix
Browse files Browse the repository at this point in the history
  • Loading branch information
@aiksiongkoh @PaddleStroke
aiksiongkoh authored and PaddleStroke committed Apr 7, 2024
1 parent 994f90d commit abd5d4a
Show file tree
Hide file tree
Showing 4 changed files with 123 additions and 86 deletions.
25 changes: 25 additions & 0 deletions OndselSolver/ASMTSpatialContainer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<FullColumn<double>>(ListD{ a, b, c });
updateMbDFromPosition3D(pos3D);
}

void MbD::ASMTSpatialContainer::updateMbDFromRotationMatrix(FMatDsptr mat)
{
rotationMatrix = mat;
auto mbdPart = std::static_pointer_cast<Part>(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<FullMatrix<double>>(ListListD{
{ v11, v12, v13 },
{ v21, v22, v23 },
{ v31, v32, v33 }
});
updateMbDFromRotationMatrix(rotMat);
}

FColDsptr MbD::ASMTSpatialContainer::rOcmO()
{
auto& rOPO = position3D;
Expand Down
173 changes: 89 additions & 84 deletions OndselSolver/ASMTSpatialContainer.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,102 +5,107 @@
* *
* 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"
//#include "ASMTJoint.h"
//#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<ASMTPrincipalMassMarker> aJ);
void readRefPoints(std::vector<std::string>& lines);
void readRefPoint(std::vector<std::string>& lines);
void readRefCurves(std::vector<std::string>& lines);
void readRefCurve(std::vector<std::string>& lines);
void readRefSurfaces(std::vector<std::string>& lines);
void readRefSurface(std::vector<std::string>& lines);
void readXs(std::vector<std::string>& lines);
void readYs(std::vector<std::string>& lines);
void readZs(std::vector<std::string>& lines);
void readBryantxs(std::vector<std::string>& lines);
void readBryantys(std::vector<std::string>& lines);
void readBryantzs(std::vector<std::string>& lines);
void readVXs(std::vector<std::string>& lines);
void readVYs(std::vector<std::string>& lines);
void readVZs(std::vector<std::string>& lines);
void readOmegaXs(std::vector<std::string>& lines);
void readOmegaYs(std::vector<std::string>& lines);
void readOmegaZs(std::vector<std::string>& lines);
void readAXs(std::vector<std::string>& lines);
void readAYs(std::vector<std::string>& lines);
void readAZs(std::vector<std::string>& lines);
void readAlphaXs(std::vector<std::string>& lines);
void readAlphaYs(std::vector<std::string>& lines);
void readAlphaZs(std::vector<std::string>& lines);
void createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits) override;
void updateMbDFromPosition3D(FColDsptr position3D);
FColDsptr rOcmO();
std::shared_ptr<EulerParameters<double>> 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<ASMTRefPoint> refPoint);
void addMarker(std::shared_ptr<ASMTMarker> marker);
std::string generateUniqueMarkerName();
std::shared_ptr<std::vector<std::shared_ptr<ASMTMarker>>> markerList();
void setVelocity3D(FColDsptr velocity3D);
void setOmega3D(FColDsptr omega3D);
void readVelocity3D(std::vector<std::string>& lines);
void readOmega3D(std::vector<std::string>& 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<ASMTPrincipalMassMarker> aJ);
void readRefPoints(std::vector<std::string>& lines);
void readRefPoint(std::vector<std::string>& lines);
void readRefCurves(std::vector<std::string>& lines);
void readRefCurve(std::vector<std::string>& lines);
void readRefSurfaces(std::vector<std::string>& lines);
void readRefSurface(std::vector<std::string>& lines);
void readXs(std::vector<std::string>& lines);
void readYs(std::vector<std::string>& lines);
void readZs(std::vector<std::string>& lines);
void readBryantxs(std::vector<std::string>& lines);
void readBryantys(std::vector<std::string>& lines);
void readBryantzs(std::vector<std::string>& lines);
void readVXs(std::vector<std::string>& lines);
void readVYs(std::vector<std::string>& lines);
void readVZs(std::vector<std::string>& lines);
void readOmegaXs(std::vector<std::string>& lines);
void readOmegaYs(std::vector<std::string>& lines);
void readOmegaZs(std::vector<std::string>& lines);
void readAXs(std::vector<std::string>& lines);
void readAYs(std::vector<std::string>& lines);
void readAZs(std::vector<std::string>& lines);
void readAlphaXs(std::vector<std::string>& lines);
void readAlphaYs(std::vector<std::string>& lines);
void readAlphaZs(std::vector<std::string>& lines);
void createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits) override;
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<EulerParameters<double>> 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<ASMTRefPoint> refPoint);
void addMarker(std::shared_ptr<ASMTMarker> marker);
std::string generateUniqueMarkerName();
std::shared_ptr<std::vector<std::shared_ptr<ASMTMarker>>> markerList();
void setVelocity3D(FColDsptr velocity3D);
void setOmega3D(FColDsptr omega3D);
void readVelocity3D(std::vector<std::string>& lines);
void readOmega3D(std::vector<std::string>& 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<FullColumn<double>>(3);
FColDsptr omega3D = std::make_shared<FullColumn<double>>(3);
std::shared_ptr<std::vector<std::shared_ptr<ASMTRefPoint>>> refPoints;
std::shared_ptr<std::vector<std::shared_ptr<ASMTRefCurve>>> refCurves;
std::shared_ptr<std::vector<std::shared_ptr<ASMTRefSurface>>> refSurfaces;
FRowDsptr 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<ASMTPrincipalMassMarker> principalMassMarker = ASMTPrincipalMassMarker::With();
FColDsptr velocity3D = std::make_shared<FullColumn<double>>(3);
FColDsptr omega3D = std::make_shared<FullColumn<double>>(3);
std::shared_ptr<std::vector<std::shared_ptr<ASMTRefPoint>>> refPoints;
std::shared_ptr<std::vector<std::shared_ptr<ASMTRefCurve>>> refCurves;
std::shared_ptr<std::vector<std::shared_ptr<ASMTRefSurface>>> refSurfaces;
FRowDsptr 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<ASMTPrincipalMassMarker> principalMassMarker = ASMTPrincipalMassMarker::With();

};
};
}

7 changes: 6 additions & 1 deletion OndselSolver/PosICDragLimitNewtonRaphson.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@
#include "SimulationStoppingError.h"
#include "Part.h"
#include "Constraint.h"
#include <algorithm>

using namespace MbD;

Expand Down Expand Up @@ -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();
Expand Down
4 changes: 3 additions & 1 deletion OndselSolver/ScrewConstraintIJ.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<double>::min()) {
aConstant = (2.0 * OS_M_PI * zIeJeIe->value()) - (thezIeJe->value() * pitch);
}
ConstraintIJ::postInput();
}

Expand Down

0 comments on commit abd5d4a

Please sign in to comment.