64 lines
2.1 KiB
C++
64 lines
2.1 KiB
C++
#include "ASMTConstraintSet.h"
|
|
#include "ASMTAssembly.h"
|
|
#include "ASMTMarker.h"
|
|
|
|
using namespace MbD;
|
|
|
|
void MbD::ASMTConstraintSet::createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits)
|
|
{
|
|
//self dataSeries : OrderedCollection new.
|
|
//self discontinuities : OrderedCollection new.
|
|
auto mbdJt = this->mbdClassNew();
|
|
mbdObject = mbdJt;
|
|
mbdJt->name = fullName("");
|
|
auto mrkI = std::static_pointer_cast<EndFramec>(root()->markerAt(markerI)->mbdObject);
|
|
auto mrkJ = std::static_pointer_cast<EndFramec>(root()->markerAt(markerJ)->mbdObject);
|
|
mbdJt->connectsItoJ(mrkI, mrkJ);
|
|
mbdSys->addJoint(mbdJt);
|
|
}
|
|
|
|
std::shared_ptr<Joint> MbD::ASMTConstraintSet::mbdClassNew()
|
|
{
|
|
assert(false);
|
|
return std::shared_ptr<Joint>();
|
|
}
|
|
|
|
void MbD::ASMTConstraintSet::updateFromMbD()
|
|
{
|
|
//"
|
|
//MbD returns aFIeO and aTIeO.
|
|
//GEO needs aFImO and aTImO.
|
|
//For Motion rImIeO is not zero and is changing.
|
|
//aFImO = aFIeO.
|
|
//aTImO = aTIeO + (rImIeO cross : aFIeO).
|
|
//"
|
|
auto mbdUnts = mbdUnits();
|
|
auto mbdJoint = std::static_pointer_cast<Joint>(mbdObject);
|
|
auto aFIeO = mbdJoint->aFX()->times(mbdUnts->force);
|
|
auto aTIeO = mbdJoint->aTX()->times(mbdUnts->torque);
|
|
auto rImIeO = mbdJoint->frmI->rmeO()->times(mbdUnts->length);
|
|
auto aFIO = aFIeO;
|
|
auto aTIO = aTIeO->plusFullColumn(rImIeO->cross(aFIeO));
|
|
fxs->push_back(aFIO->at(0));
|
|
fys->push_back(aFIO->at(1));
|
|
fzs->push_back(aFIO->at(2));
|
|
txs->push_back(aTIO->at(0));
|
|
tys->push_back(aTIO->at(1));
|
|
tzs->push_back(aTIO->at(2));
|
|
}
|
|
|
|
void MbD::ASMTConstraintSet::compareResults(AnalysisType type)
|
|
{
|
|
auto mbdUnts = mbdUnits();
|
|
auto factor = 1.0e-6;
|
|
auto forceTol = mbdUnts->force * factor;
|
|
auto torqueTol = mbdUnts->torque * factor;
|
|
auto i = fxs->size() - 1;
|
|
//assert(Numeric::equaltol(fxs->at(i), infxs->at(i), forceTol));
|
|
//assert(Numeric::equaltol(fys->at(i), infys->at(i), forceTol));
|
|
//assert(Numeric::equaltol(fzs->at(i), infzs->at(i), forceTol));
|
|
//assert(Numeric::equaltol(txs->at(i), intxs->at(i), torqueTol));
|
|
//assert(Numeric::equaltol(tys->at(i), intys->at(i), torqueTol));
|
|
//assert(Numeric::equaltol(tzs->at(i), intzs->at(i), torqueTol));
|
|
}
|