339 lines
8.0 KiB
C++
339 lines
8.0 KiB
C++
#include "MBDynItem.h"
|
|
#include "MBDynSystem.h"
|
|
#include "SymbolicParser.h"
|
|
#include "BasicUserFunction.h"
|
|
#include "EulerAngles.h"
|
|
#include "Constant.h"
|
|
#include "MBDynReference.h"
|
|
#include "FullMatrix.h"
|
|
#include "ASMTItem.h"
|
|
#include "MBDynBody.h"
|
|
|
|
|
|
using namespace MbD;
|
|
|
|
MBDynSystem* MbD::MBDynItem::root()
|
|
{
|
|
return nullptr;
|
|
}
|
|
|
|
void MbD::MBDynItem::initialize()
|
|
{
|
|
assert(false);
|
|
}
|
|
|
|
void MbD::MBDynItem::noop()
|
|
{
|
|
//No Operations
|
|
}
|
|
|
|
void MbD::MBDynItem::parseMBDyn(std::vector<std::string>& lines)
|
|
{
|
|
assert(false);
|
|
}
|
|
|
|
std::vector<std::string>::iterator MbD::MBDynItem::findLineWith(std::vector<std::string>& lines, std::vector<std::string>& tokens)
|
|
{
|
|
auto it = std::find_if(lines.begin(), lines.end(), [&](const std::string& line) {
|
|
return lineHasTokens(line, tokens);
|
|
});
|
|
return it;
|
|
}
|
|
|
|
bool MbD::MBDynItem::lineHasTokens(const std::string& line, std::vector<std::string>& tokens)
|
|
{
|
|
size_t index = 0;
|
|
for (auto& token : tokens) {
|
|
index = line.find(token, index);
|
|
if (index == std::string::npos) return false;
|
|
index++;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
std::shared_ptr<std::vector<std::shared_ptr<MBDynNode>>> MbD::MBDynItem::mbdynNodes()
|
|
{
|
|
return owner->mbdynNodes();
|
|
}
|
|
|
|
std::vector<std::string> MbD::MBDynItem::nodeNames()
|
|
{
|
|
return owner->nodeNames();
|
|
}
|
|
|
|
std::shared_ptr<std::map<std::string, Symsptr>> MbD::MBDynItem::mbdynVariables()
|
|
{
|
|
return owner->mbdynVariables();
|
|
}
|
|
|
|
std::shared_ptr<std::map<std::string, std::shared_ptr<MBDynReference>>> MbD::MBDynItem::mbdynReferences()
|
|
{
|
|
return owner->mbdynReferences();
|
|
}
|
|
|
|
void MbD::MBDynItem::createASMT()
|
|
{
|
|
assert(false);
|
|
}
|
|
|
|
std::shared_ptr<MBDynNode> MbD::MBDynItem::nodeAt(std::string nodeName)
|
|
{
|
|
return owner->nodeAt(nodeName);
|
|
}
|
|
|
|
int MbD::MBDynItem::nodeidAt(std::string nodeName)
|
|
{
|
|
return owner->nodeidAt(nodeName);
|
|
}
|
|
|
|
std::shared_ptr<MBDynBody> MbD::MBDynItem::bodyWithNode(std::string nodeName)
|
|
{
|
|
return owner->bodyWithNode(nodeName);
|
|
}
|
|
|
|
std::shared_ptr<ASMTAssembly> MbD::MBDynItem::asmtAssembly()
|
|
{
|
|
return owner->asmtAssembly();
|
|
}
|
|
|
|
FColDsptr MbD::MBDynItem::readVector3(std::vector<std::string>& args)
|
|
{
|
|
auto parser = std::make_shared<SymbolicParser>();
|
|
parser->variables = mbdynVariables();
|
|
auto rFfF = std::make_shared<FullColumn<double>>(3);
|
|
auto& str = args.at(0);
|
|
if (str.find("null") != std::string::npos) {
|
|
args.erase(args.begin());
|
|
}
|
|
else {
|
|
for (int i = 0; i < 3; i++)
|
|
{
|
|
auto userFunc = std::make_shared<BasicUserFunction>(popOffTop(args), 1.0);
|
|
parser->parseUserFunction(userFunc);
|
|
auto sym = parser->stack->top();
|
|
rFfF->at(i) = sym->getValue();
|
|
}
|
|
|
|
}
|
|
return rFfF;
|
|
}
|
|
|
|
FColDsptr MbD::MBDynItem::readPosition(std::vector<std::string>& args)
|
|
{
|
|
auto rOfO = std::make_shared<FullColumn<double>>(3);
|
|
if (args.empty()) return rOfO;
|
|
auto& str = args.at(0);
|
|
if (str.find("orientation") != std::string::npos) {
|
|
//Do nothing
|
|
}
|
|
else if (str.find("reference") != std::string::npos) {
|
|
args.erase(args.begin());
|
|
auto refName = readStringOffTop(args);
|
|
auto ref = mbdynReferences()->at(refName);
|
|
auto rFfF = readBasicPosition(args);
|
|
auto rOFO = ref->rOfO;
|
|
auto aAOF = ref->aAOf;
|
|
rOfO = rOFO->plusFullColumn(aAOF->timesFullColumn(rFfF));
|
|
}
|
|
else if (str.find("offset") != std::string::npos) {
|
|
args.erase(args.begin());
|
|
rOfO = readBasicPosition(args);
|
|
}
|
|
else if (str.find("null") != std::string::npos) {
|
|
args.erase(args.begin());
|
|
}
|
|
else {
|
|
rOfO = readBasicPosition(args);
|
|
}
|
|
return rOfO;
|
|
}
|
|
|
|
FColDsptr MbD::MBDynItem::readBasicPosition(std::vector<std::string>& args)
|
|
{
|
|
return readVector3(args);
|
|
}
|
|
|
|
FMatDsptr MbD::MBDynItem::readOrientation(std::vector<std::string>& args)
|
|
{
|
|
auto aAOf = FullMatrixDouble::identitysptr(3);
|
|
if (args.empty()) return aAOf;
|
|
auto& str = args.at(0);
|
|
if (str.find("reference") != std::string::npos) {
|
|
args.erase(args.begin());
|
|
auto refName = readStringOffTop(args);
|
|
auto ref = mbdynReferences()->at(refName);
|
|
auto aAFf = readBasicOrientation(args);
|
|
auto aAOF = ref->aAOf;
|
|
aAOf = toFMDsptr(aAOF->timesFullMatrix(aAFf));
|
|
}
|
|
else if (str.find("hinge") != std::string::npos) {
|
|
args.erase(args.begin());
|
|
aAOf = readOrientation(args);
|
|
}
|
|
else if (str.find("orientation") != std::string::npos) {
|
|
args.erase(args.begin());
|
|
aAOf = readOrientation(args);
|
|
}
|
|
else {
|
|
aAOf = readBasicOrientation(args);
|
|
}
|
|
return aAOf;
|
|
}
|
|
|
|
FMatDsptr MbD::MBDynItem::readBasicOrientation(std::vector<std::string>& args)
|
|
{
|
|
auto parser = std::make_shared<SymbolicParser>();
|
|
parser->variables = mbdynVariables();
|
|
auto& str = args.at(0);
|
|
if (str.find("euler") != std::string::npos) {
|
|
args.erase(args.begin());
|
|
auto euler = std::make_shared<EulerAngles<Symsptr>>();
|
|
euler->rotOrder = std::make_shared<FullColumn<int>>(std::initializer_list<int>{ 1, 2, 3 });
|
|
for (int i = 0; i < 3; i++)
|
|
{
|
|
auto userFunc = std::make_shared<BasicUserFunction>(popOffTop(args), 1.0);
|
|
parser->parseUserFunction(userFunc);
|
|
auto sym = parser->stack->top();
|
|
euler->at(i) = sym;
|
|
}
|
|
euler->calc();
|
|
auto aAFf = euler->aA;
|
|
return aAFf;
|
|
}
|
|
if (str.find("eye") != std::string::npos) {
|
|
args.erase(args.begin());
|
|
auto aAFf = FullMatrixDouble::identitysptr(3);
|
|
return aAFf;
|
|
}
|
|
auto iss = std::istringstream(str);
|
|
int integer;
|
|
iss >> integer;
|
|
if (integer == 1) {
|
|
args.erase(args.begin());
|
|
FColDsptr vecX, vecY, vecZ, vec;
|
|
vecX = readPosition(args);
|
|
vecX->normalizeSelf();
|
|
auto axis = stoi(popOffTop(args));
|
|
str = args.at(0);
|
|
if (str.find("guess") != std::string::npos) {
|
|
args.erase(args.begin());
|
|
double min = std::numeric_limits<double>::max();
|
|
double max = -1.0;
|
|
int imin, imax;
|
|
for (int i = 0; i < 3; i++)
|
|
{
|
|
auto mag = std::abs(vecX->at(i));
|
|
if (mag > max) {
|
|
imax = i;
|
|
max = mag;
|
|
}
|
|
if (mag < min) {
|
|
imin = i;
|
|
min = mag;
|
|
}
|
|
}
|
|
vec = std::make_shared<FullColumn<double>>(3);
|
|
vec->at(imin) = 1.0;
|
|
vec->at(imax) = -vecX->at(imin) / vecX->at(imax);
|
|
}
|
|
else {
|
|
vec = readPosition(args);
|
|
}
|
|
vec->normalizeSelf();
|
|
if (axis == 2) {
|
|
vecZ = vecX->cross(vec);
|
|
vecY = vecZ->cross(vecX);
|
|
}
|
|
else if (axis == 3) {
|
|
vecY = vec->cross(vecX);
|
|
vecZ = vecX->cross(vecY);
|
|
}
|
|
else {
|
|
assert(false);
|
|
}
|
|
auto aAFf = FullMatrixDouble::identitysptr(3);
|
|
aAFf->atijputFullColumn(0, 0, vecX);
|
|
aAFf->atijputFullColumn(0, 1, vecY);
|
|
aAFf->atijputFullColumn(0, 2, vecZ);
|
|
return aAFf;
|
|
}
|
|
if (integer == 3) {
|
|
args.erase(args.begin());
|
|
FColDsptr vecX, vecY, vecZ, vec;
|
|
vecZ = readPosition(args);
|
|
vecZ->normalizeSelf();
|
|
auto axis = stoi(popOffTop(args));
|
|
str = args.at(0);
|
|
if (str.find("guess") != std::string::npos) {
|
|
args.erase(args.begin());
|
|
double min = std::numeric_limits<double>::max();
|
|
double max = -1.0;
|
|
int imin, imax;
|
|
for (int i = 0; i < 3; i++)
|
|
{
|
|
auto mag = std::abs(vecZ->at(i));
|
|
if (mag > max) {
|
|
imax = i;
|
|
max = mag;
|
|
}
|
|
if (mag < min) {
|
|
imin = i;
|
|
min = mag;
|
|
}
|
|
}
|
|
vec = std::make_shared<FullColumn<double>>(3);
|
|
vec->at(imin) = 1.0;
|
|
vec->at(imax) = -vecZ->at(imin) / vecZ->at(imax);
|
|
}
|
|
else {
|
|
vec = readPosition(args);
|
|
}
|
|
vec->normalizeSelf();
|
|
if (axis == 2) {
|
|
vecX = vec->cross(vecZ);
|
|
vecY = vecZ->cross(vecX);
|
|
}
|
|
else if (axis == 1) {
|
|
vecY = vecZ->cross(vec);
|
|
vecX = vecY->cross(vecZ);
|
|
}
|
|
else {
|
|
assert(false);
|
|
}
|
|
auto aAFf = FullMatrixDouble::identitysptr(3);
|
|
aAFf->atijputFullColumn(0, 0, vecX);
|
|
aAFf->atijputFullColumn(0, 1, vecY);
|
|
aAFf->atijputFullColumn(0, 2, vecZ);
|
|
return aAFf;
|
|
}
|
|
auto aAFf = FullMatrixDouble::identitysptr(3);
|
|
for (int i = 0; i < 3; i++)
|
|
{
|
|
auto rowi = aAFf->at(i);
|
|
for (int j = 0; j < 3; j++)
|
|
{
|
|
auto userFunc = std::make_shared<BasicUserFunction>(popOffTop(args), 1.0);
|
|
parser->parseUserFunction(userFunc);
|
|
auto sym = parser->stack->top();
|
|
rowi->at(j) = sym->getValue();
|
|
}
|
|
}
|
|
return aAFf;
|
|
}
|
|
|
|
std::string MbD::MBDynItem::popOffTop(std::vector<std::string>& args)
|
|
{
|
|
auto str = args.at(0);
|
|
args.erase(args.begin());
|
|
return str;
|
|
}
|
|
|
|
std::string MbD::MBDynItem::readStringOffTop(std::vector<std::string>& args)
|
|
{
|
|
auto iss = std::istringstream(args.at(0));
|
|
args.erase(args.begin());
|
|
std::string str;
|
|
iss >> str;
|
|
return str;
|
|
}
|