From 225966dceddacfad09b3151705e8b0f6b825485c Mon Sep 17 00:00:00 2001 From: Aik-Siong Koh Date: Wed, 19 Jul 2023 09:41:17 -0600 Subject: [PATCH 1/4] ExternalSystem class --- MbDCode/ASMTAssembly.cpp | 563 ++++++++++-------- MbDCode/ASMTAssembly.h | 34 +- MbDCode/ASMTConstraintSet.cpp | 58 ++ MbDCode/ASMTConstraintSet.h | 10 + MbDCode/ASMTItem.cpp | 44 +- MbDCode/ASMTItem.h | 15 +- MbDCode/ASMTJoint.cpp | 35 +- MbDCode/ASMTJoint.h | 4 +- MbDCode/ASMTMarker.cpp | 39 +- MbDCode/ASMTMarker.h | 8 +- MbDCode/ASMTMotion.cpp | 18 + MbDCode/ASMTMotion.h | 3 + MbDCode/ASMTPart.cpp | 193 +++--- MbDCode/ASMTPart.h | 15 +- MbDCode/ASMTPrincipalMassMarker.h | 1 - MbDCode/ASMTRefPoint.cpp | 78 +-- MbDCode/ASMTRefPoint.h | 8 +- MbDCode/ASMTRotationalMotion.cpp | 29 +- MbDCode/ASMTRotationalMotion.h | 4 +- MbDCode/ASMTSpatialContainer.cpp | 192 ++++++ MbDCode/ASMTSpatialContainer.h | 47 ++ MbDCode/ASMTSpatialItem.cpp | 59 ++ MbDCode/ASMTSpatialItem.h | 20 + MbDCode/ASMTTranslationalMotion.h | 2 +- MbDCode/CADSystem.h | 2 +- MbDCode/ExternalSystem.cpp | 31 + MbDCode/ExternalSystem.h | 32 + MbDCode/MbDCode.cpp | 4 +- MbDCode/MbDCode.vcxproj | 10 +- MbDCode/MbDCode.vcxproj.filters | 22 +- MbDCode/Part.cpp | 4 +- .../{DataPosVelAcc.cpp => PosVelAccData.cpp} | 4 +- MbDCode/{DataPosVelAcc.h => PosVelAccData.h} | 4 +- MbDCode/System.cpp | 4 +- MbDCode/System.h | 5 +- 35 files changed, 1069 insertions(+), 532 deletions(-) create mode 100644 MbDCode/ASMTSpatialContainer.cpp create mode 100644 MbDCode/ASMTSpatialContainer.h create mode 100644 MbDCode/ASMTSpatialItem.cpp create mode 100644 MbDCode/ASMTSpatialItem.h create mode 100644 MbDCode/ExternalSystem.cpp create mode 100644 MbDCode/ExternalSystem.h rename MbDCode/{DataPosVelAcc.cpp => PosVelAccData.cpp} (80%) rename MbDCode/{DataPosVelAcc.h => PosVelAccData.h} (75%) diff --git a/MbDCode/ASMTAssembly.cpp b/MbDCode/ASMTAssembly.cpp index 304d873..be25d1c 100644 --- a/MbDCode/ASMTAssembly.cpp +++ b/MbDCode/ASMTAssembly.cpp @@ -33,259 +33,334 @@ void MbD::ASMTAssembly::runFile(const char* chars) void MbD::ASMTAssembly::parseASMT(std::vector& lines) { - size_t pos = lines[0].find_first_not_of("\t"); - auto leadingTabs = lines[0].substr(0, pos); - while (!lines.empty()) { - if (lines[0] == (leadingTabs + "Notes")) { - lines.erase(lines.begin()); - notes = lines[0]; - lines.erase(lines.begin()); + readNotes(lines); + readName(lines); + readPosition3D(lines); + readRotationMatrix(lines); + readVelocity3D(lines); + readOmega3D(lines); + readRefPoints(lines); + readRefCurves(lines); + readRefSurfaces(lines); + readParts(lines); + readKinematicIJs(lines); + readConstraintSets(lines); + readForceTorques(lines); + readConstantGravity(lines); + readSimulationParameters(lines); + readAnimationParameters(lines); + readTimeSeries(lines); + readAssemblySeries(lines); + readPartSeriesMany(lines); + readJointSeriesMany(lines); + readMotionSeriesMany(lines); +} + +void MbD::ASMTAssembly::readNotes(std::vector& lines) +{ + assert(lines[0] == "\tNotes"); + lines.erase(lines.begin()); + notes = readString(lines[0]); + lines.erase(lines.begin()); +} + +void MbD::ASMTAssembly::readParts(std::vector& lines) +{ + assert(lines[0] == "\tParts"); + lines.erase(lines.begin()); + parts = std::make_shared>>(); + auto it = std::find(lines.begin(), lines.end(), "\tKinematicIJs"); + std::vector partsLines(lines.begin(), it); + while (!partsLines.empty()) { + readPart(partsLines); + } + lines.erase(lines.begin(), it); + +} + +void MbD::ASMTAssembly::readPart(std::vector& lines) +{ + assert(lines[0] == "\t\tPart"); + lines.erase(lines.begin()); + auto part = CREATE::With(); + part->parseASMT(lines); + parts->push_back(part); + part->owner = this; +} + +void MbD::ASMTAssembly::readKinematicIJs(std::vector& lines) +{ + assert(lines[0] == "\tKinematicIJs"); + lines.erase(lines.begin()); + kinematicIJs = std::make_shared>>(); + auto it = std::find(lines.begin(), lines.end(), "\tConstraintSets"); + std::vector kinematicIJsLines(lines.begin(), it); + while (!kinematicIJsLines.empty()) { + readKinematicIJ(kinematicIJsLines); + } + lines.erase(lines.begin(), it); + +} + +void MbD::ASMTAssembly::readKinematicIJ(std::vector& lines) +{ + assert(false); +} + +void MbD::ASMTAssembly::readConstraintSets(std::vector& lines) +{ + assert(lines[0] == "\tConstraintSets"); + lines.erase(lines.begin()); + readJoints(lines); + readMotions(lines); + readGeneralConstraintSets(lines); +} + +void MbD::ASMTAssembly::readJoints(std::vector& lines) +{ + assert(lines[0] == "\t\tJoints"); + lines.erase(lines.begin()); + joints = std::make_shared>>(); + auto it = std::find(lines.begin(), lines.end(), "\t\tMotions"); + std::vector jointsLines(lines.begin(), it); + std::shared_ptr joint; + while (!jointsLines.empty()) { + if (jointsLines[0] == "\t\t\tRevoluteJoint") { + joint = CREATE::With(); } - else if (lines[0] == (leadingTabs + "Name")) { - lines.erase(lines.begin()); - name = lines[0]; - lines.erase(lines.begin()); + else if (jointsLines[0] == "\t\t\tCylindricalJoint") { + joint = CREATE::With(); } - else if (lines[0] == (leadingTabs + "Position3D")) { - lines.erase(lines.begin()); - std::istringstream iss(lines[0]); - position3D = std::make_shared>(); - double d; - while (iss >> d) { - position3D->push_back(d); - } - lines.erase(lines.begin()); + else { + assert(false); } - else if (lines[0] == (leadingTabs + "RotationMatrix")) { - lines.erase(lines.begin()); - rotationMatrix = std::make_shared>(3, 0); - for (int i = 0; i < 3; i++) - { - auto& row = rotationMatrix->at(i); - std::istringstream iss(lines[0]); - double d; - while (iss >> d) { - row->push_back(d); - } - lines.erase(lines.begin()); - } + jointsLines.erase(jointsLines.begin()); + joint->parseASMT(jointsLines); + joints->push_back(joint); + joint->owner = this; + } + lines.erase(lines.begin(), it); + +} + +void MbD::ASMTAssembly::readMotions(std::vector& lines) +{ + assert(lines[0] == "\t\tMotions"); + lines.erase(lines.begin()); + motions = std::make_shared>>(); + auto it = std::find(lines.begin(), lines.end(), "\t\tGeneralConstraintSets"); + std::vector motionsLines(lines.begin(), it); + std::shared_ptr motion; + while (!motionsLines.empty()) { + if (motionsLines[0] == "\t\t\tRotationalMotion") { + motion = CREATE::With(); } - else if (lines[0] == (leadingTabs + "Velocity3D")) { - lines.erase(lines.begin()); - std::istringstream iss(lines[0]); - velocity3D = std::make_shared>(); - double d; - while (iss >> d) { - velocity3D->push_back(d); - } - lines.erase(lines.begin()); + else if (motionsLines[0] == "\t\t\tTranslationalMotion") { + motion = CREATE::With(); } - else if (lines[0] == (leadingTabs + "Omega3D")) { - lines.erase(lines.begin()); - std::istringstream iss(lines[0]); - omega3D = std::make_shared>(); - double d; - while (iss >> d) { - omega3D->push_back(d); - } - lines.erase(lines.begin()); + else { + assert(false); } - else if (lines[0] == (leadingTabs + "RefPoints")) { - lines.erase(lines.begin()); - refPoints = std::make_shared>>(); - auto it = std::find(lines.begin(), lines.end(), (leadingTabs + "RefCurves")); - std::vector refPointsLines(lines.begin(), it); - while (!refPointsLines.empty()) { - if (refPointsLines[0] == (leadingTabs + "\tRefPoint")) { - refPointsLines.erase(refPointsLines.begin()); - auto refPoint = CREATE::With(); - refPoint->parseASMT(refPointsLines); - refPoints->push_back(refPoint); - refPoint->owner = this; - } - else { - assert(false); - } - } - lines.erase(lines.begin(), it); - } - else if (lines[0] == (leadingTabs + "RefCurves")) { - lines.erase(lines.begin()); - refCurves = std::make_shared>>(); - auto it = std::find(lines.begin(), lines.end(), (leadingTabs + "RefSurfaces")); - std::vector refCurvesLines(lines.begin(), it); - while (!refCurvesLines.empty()) { - if (refCurvesLines[0] == (leadingTabs + "\tRefCurve")) { - refCurvesLines.erase(refCurvesLines.begin()); - auto refCurve = CREATE::With(); - refCurve->parseASMT(refCurvesLines); - refCurves->push_back(refCurve); - refCurve->owner = this; - } - else { - assert(false); - } - } - lines.erase(lines.begin(), it); - } - else if (lines[0] == (leadingTabs + "RefSurfaces")) { - lines.erase(lines.begin()); - refSurfaces = std::make_shared>>(); - auto it = std::find(lines.begin(), lines.end(), (leadingTabs + "Parts")); - std::vector refSurfacesLines(lines.begin(), it); - while (!refSurfacesLines.empty()) { - if (refSurfacesLines[0] == (leadingTabs + "\tRefSurface")) { - refSurfacesLines.erase(refSurfacesLines.begin()); - auto refSurface = CREATE::With(); - refSurface->parseASMT(refSurfacesLines); - refSurfaces->push_back(refSurface); - refSurface->owner = this; - } - else { - assert(false); - } - } - lines.erase(lines.begin(), it); - } - else if (lines[0] == (leadingTabs + "Parts")) { - lines.erase(lines.begin()); - parts = std::make_shared>>(); - auto it = std::find(lines.begin(), lines.end(), (leadingTabs + "KinematicIJs")); - std::vector partsLines(lines.begin(), it); - while (!partsLines.empty()) { - if (partsLines[0] == (leadingTabs + "\tPart")) { - partsLines.erase(partsLines.begin()); - auto part = CREATE::With(); - part->parseASMT(partsLines); - parts->push_back(part); - part->owner = this; - } - else { - assert(false); - } - } - lines.erase(lines.begin(), it); - } - else if (lines[0] == (leadingTabs + "KinematicIJs")) { - lines.erase(lines.begin()); - kinematicIJs = std::make_shared>>(); - auto it = std::find(lines.begin(), lines.end(), (leadingTabs + "ConstraintSets")); - std::vector kinematicIJsLines(lines.begin(), it); - while (!kinematicIJsLines.empty()) { - if (kinematicIJsLines[0] == (leadingTabs + "\tKinematicIJ")) { - kinematicIJsLines.erase(kinematicIJsLines.begin()); - auto kinematicIJ = CREATE::With(); - kinematicIJ->parseASMT(kinematicIJsLines); - kinematicIJs->push_back(kinematicIJ); - kinematicIJ->owner = this; - } - else { - assert(false); - } - } - lines.erase(lines.begin(), it); - } - else if (lines[0] == (leadingTabs + "ConstraintSets")) { - lines.erase(lines.begin()); - assert(lines[0] == (leadingTabs + "\tJoints")); - lines.erase(lines.begin()); - joints = std::make_shared>>(); - auto it = std::find(lines.begin(), lines.end(), (leadingTabs + "\tMotions")); - std::vector jointsLines(lines.begin(), it); - while (!jointsLines.empty()) { - if (jointsLines[0] == (leadingTabs + "\t\tRevoluteJoint")) { - jointsLines.erase(jointsLines.begin()); - auto joint = CREATE::With(); - joint->parseASMT(jointsLines); - joints->push_back(joint); - joint->owner = this; - } - else if (jointsLines[0] == (leadingTabs + "\t\tCylindricalJoint")) { - jointsLines.erase(jointsLines.begin()); - auto joint = CREATE::With(); - joint->parseASMT(jointsLines); - joints->push_back(joint); - joint->owner = this; - } - else { - assert(false); - } - } - lines.erase(lines.begin(), it); - assert(lines[0] == (leadingTabs + "\tMotions")); - lines.erase(lines.begin()); - motions = std::make_shared>>(); - it = std::find(lines.begin(), lines.end(), (leadingTabs + "\tGeneralConstraintSets")); - std::vector motionsLines(lines.begin(), it); - while (!motionsLines.empty()) { - if (motionsLines[0] == (leadingTabs + "\t\tRotationalMotion")) { - motionsLines.erase(motionsLines.begin()); - auto motion = CREATE::With(); - motion->parseASMT(motionsLines); - motions->push_back(motion); - motion->owner = this; - } - else if (motionsLines[0] == (leadingTabs + "\t\tTranslationalMotion")) { - motionsLines.erase(motionsLines.begin()); - auto motion = CREATE::With(); - motion->parseASMT(motionsLines); - motions->push_back(motion); - motion->owner = this; - } - else { - assert(false); - } - } - lines.erase(lines.begin(), it); - assert(lines[0] == (leadingTabs + "\tGeneralConstraintSets")); - lines.erase(lines.begin()); - constraintSets = std::make_shared>>(); - it = std::find(lines.begin(), lines.end(), (leadingTabs + "ForceTorques")); - std::vector generalConstraintSetsLines(lines.begin(), it); - while (!generalConstraintSetsLines.empty()) { - assert(false); - } - lines.erase(lines.begin(), it); - } - else if (lines[0] == (leadingTabs + "ForceTorques")) { - lines.erase(lines.begin()); - forceTorques = std::make_shared>>(); - auto it = std::find(lines.begin(), lines.end(), (leadingTabs + "ConstantGravity")); - std::vector forceTorquesLines(lines.begin(), it); - while (!forceTorquesLines.empty()) { - if (forceTorquesLines[0] == (leadingTabs + "\tForceTorque")) { - forceTorquesLines.erase(forceTorquesLines.begin()); - auto forceTorque = CREATE::With(); - forceTorque->parseASMT(forceTorquesLines); - forceTorques->push_back(forceTorque); - forceTorque->owner = this; - } - else { - assert(false); - } - } - lines.erase(lines.begin(), it); - } - else if (lines[0] == (leadingTabs + "ConstantGravity")) { - lines.erase(lines.begin()); - constantGravity = CREATE::With(); - constantGravity->parseASMT(lines); - constantGravity->owner = this; - } - else if (lines[0] == (leadingTabs + "SimulationParameters")) { - lines.erase(lines.begin()); - simulationParameters = CREATE::With(); - simulationParameters->parseASMT(lines); - simulationParameters->owner = this; - } - else if (lines[0] == (leadingTabs + "AnimationParameters")) { - lines.erase(lines.begin()); - animationParameters = CREATE::With(); - animationParameters->parseASMT(lines); - animationParameters->owner = this; + motionsLines.erase(motionsLines.begin()); + motion->parseASMT(motionsLines); + motions->push_back(motion); + motion->owner = this; + } + lines.erase(lines.begin(), it); + +} + +void MbD::ASMTAssembly::readGeneralConstraintSets(std::vector& lines) +{ + assert(lines[0] == "\t\tGeneralConstraintSets"); + lines.erase(lines.begin()); + constraintSets = std::make_shared>>(); + auto it = std::find(lines.begin(), lines.end(), "\tForceTorques"); + std::vector generalConstraintSetsLines(lines.begin(), it); + while (!generalConstraintSetsLines.empty()) { + assert(false); + } + lines.erase(lines.begin(), it); +} + +void MbD::ASMTAssembly::readForceTorques(std::vector& lines) +{ + assert(lines[0] == "\tForceTorques"); + lines.erase(lines.begin()); + forceTorques = std::make_shared>>(); + auto it = std::find(lines.begin(), lines.end(), "\tConstantGravity"); + std::vector forceTorquesLines(lines.begin(), it); + while (!forceTorquesLines.empty()) { + if (forceTorquesLines[0] == "\t\tForceTorque") { + forceTorquesLines.erase(forceTorquesLines.begin()); + auto forceTorque = CREATE::With(); + forceTorque->parseASMT(forceTorquesLines); + forceTorques->push_back(forceTorque); + forceTorque->owner = this; } else { assert(false); } } + lines.erase(lines.begin(), it); } + +void MbD::ASMTAssembly::readConstantGravity(std::vector& lines) +{ + assert(lines[0] == "\tConstantGravity"); + lines.erase(lines.begin()); + constantGravity = CREATE::With(); + constantGravity->parseASMT(lines); + constantGravity->owner = this; +} + +void MbD::ASMTAssembly::readSimulationParameters(std::vector& lines) +{ + assert(lines[0] == "\tSimulationParameters"); + lines.erase(lines.begin()); + simulationParameters = CREATE::With(); + simulationParameters->parseASMT(lines); + simulationParameters->owner = this; +} + +void MbD::ASMTAssembly::readAnimationParameters(std::vector& lines) +{ + assert(lines[0] == "\tAnimationParameters"); + lines.erase(lines.begin()); + animationParameters = CREATE::With(); + animationParameters->parseASMT(lines); + animationParameters->owner = this; +} + +void MbD::ASMTAssembly::readTimeSeries(std::vector& lines) +{ + assert(lines[0] == "TimeSeries"); + lines.erase(lines.begin()); + assert(lines[0].find("Number\tInput") != std::string::npos); + lines.erase(lines.begin()); + readTimes(lines); +} + +void MbD::ASMTAssembly::readTimes(std::vector& lines) +{ + std::string str = lines[0]; + std::string substr = "Time\tInput"; + auto pos = str.find(substr); + assert(pos != std::string::npos); + str.erase(0, pos + substr.length()); + times = readRowOfDoubles(str); + times->insert(times->begin(), times->at(0)); //The first element is the input state. + lines.erase(lines.begin()); +} + +void MbD::ASMTAssembly::readPartSeriesMany(std::vector& lines) +{ + assert(lines[0].find("PartSeries") != std::string::npos); + auto it = std::find_if(lines.begin(), lines.end(), [](const std::string& s) { + return s.find("JointSeries") != std::string::npos; + }); + std::vector partSeriesLines(lines.begin(), it); + while (!partSeriesLines.empty()) { + readPartSeries(partSeriesLines); + } + lines.erase(lines.begin(), it); +} + +void MbD::ASMTAssembly::readJointSeriesMany(std::vector& lines) +{ + assert(lines[0].find("JointSeries") != std::string::npos); + auto it = std::find_if(lines.begin(), lines.end(), [](const std::string& s) { + return s.find("MotionSeries") != std::string::npos; + }); + std::vector jointSeriesLines(lines.begin(), it); + while (!jointSeriesLines.empty()) { + readJointSeries(jointSeriesLines); + } + lines.erase(lines.begin(), it); +} + +void MbD::ASMTAssembly::readAssemblySeries(std::vector& lines) +{ + std::string str = lines[0]; + std::string substr = "AssemblySeries"; + auto pos = str.find(substr); + assert(pos != std::string::npos); + str.erase(0, pos + substr.length()); + auto seriesName = readString(str); + assert(fullName("") == seriesName); + lines.erase(lines.begin()); + //xs, ys, zs, bryxs, bryys, bryzs + readXs(lines); + readYs(lines); + readZs(lines); + readBryantxs(lines); + readBryantys(lines); + readBryantzs(lines); + readVXs(lines); + readVYs(lines); + readVZs(lines); + readOmegaXs(lines); + readOmegaYs(lines); + readOmegaZs(lines); + readAXs(lines); + readAYs(lines); + readAZs(lines); + readAlphaXs(lines); + readAlphaYs(lines); + readAlphaZs(lines); +} + +void MbD::ASMTAssembly::readPartSeries(std::vector& lines) +{ + std::string str = lines[0]; + std::string substr = "PartSeries"; + auto pos = str.find(substr); + assert(pos != std::string::npos); + str.erase(0, pos + substr.length()); + auto seriesName = readString(str); + auto it = std::find_if(parts->begin(), parts->end(), [&](const std::shared_ptr& prt) { + return prt->fullName("") == seriesName; + }); + auto part = *it; + part->readPartSeries(lines); +} + +void MbD::ASMTAssembly::readJointSeries(std::vector& lines) +{ + std::string str = lines[0]; + std::string substr = "JointSeries"; + auto pos = str.find(substr); + assert(pos != std::string::npos); + str.erase(0, pos + substr.length()); + auto seriesName = readString(str); + auto it = std::find_if(joints->begin(), joints->end(), [&](const std::shared_ptr& jt) { + return jt->fullName("") == seriesName; + }); + auto joint = *it; + joint->readJointSeries(lines); +} + +void MbD::ASMTAssembly::readMotionSeriesMany(std::vector& lines) +{ + assert(lines[0].find("MotionSeries") != std::string::npos); + while (!lines.empty()) { + readMotionSeries(lines); + } +} + +void MbD::ASMTAssembly::readMotionSeries(std::vector& lines) +{ + std::string str = lines[0]; + std::string substr = "MotionSeries"; + auto pos = str.find(substr); + assert(pos != std::string::npos); + str.erase(0, pos + substr.length()); + auto seriesName = readString(str); + auto it = std::find_if(motions->begin(), motions->end(), [&](const std::shared_ptr& jt) { + return jt->fullName("") == seriesName; + }); + auto motion = *it; + motion->readMotionSeries(lines); +} + + diff --git a/MbDCode/ASMTAssembly.h b/MbDCode/ASMTAssembly.h index 05fe34d..799b16f 100644 --- a/MbDCode/ASMTAssembly.h +++ b/MbDCode/ASMTAssembly.h @@ -1,7 +1,7 @@ #pragma once #include -#include "ASMTItem.h" +#include "ASMTSpatialContainer.h" #include "ASMTRefPoint.h" #include "ASMTRefCurve.h" #include "ASMTRefSurface.h" @@ -18,19 +18,36 @@ #include "ASMTMotion.h" namespace MbD { - class ASMTAssembly : public ASMTItem + class ASMTAssembly : public ASMTSpatialContainer { // public: static void runFile(const char* chars); void parseASMT(std::vector& lines) override; + void readNotes(std::vector& lines); + void readParts(std::vector& lines); + void readPart(std::vector& lines); + void readKinematicIJs(std::vector& lines); + void readKinematicIJ(std::vector& lines); + void readConstraintSets(std::vector& lines); + void readJoints(std::vector& lines); + void readMotions(std::vector& lines); + void readGeneralConstraintSets(std::vector& lines); + void readForceTorques(std::vector& lines); + void readConstantGravity(std::vector& lines); + void readSimulationParameters(std::vector& lines); + void readAnimationParameters(std::vector& lines); + void readTimeSeries(std::vector& lines); + void readTimes(std::vector& lines); + void readAssemblySeries(std::vector& lines); + void readPartSeriesMany(std::vector& lines); + void readPartSeries(std::vector& lines); + void readJointSeriesMany(std::vector& lines); + void readJointSeries(std::vector& lines); + void readMotionSeriesMany(std::vector& lines); + void readMotionSeries(std::vector& lines); - std::string notes, name; - FColDsptr position3D, velocity3D, omega3D; - FMatDsptr rotationMatrix; - std::shared_ptr>> refPoints; - std::shared_ptr>> refCurves; - std::shared_ptr>> refSurfaces; + std::string notes; std::shared_ptr>> parts; std::shared_ptr>> kinematicIJs; std::shared_ptr>> constraintSets; @@ -40,6 +57,7 @@ namespace MbD { std::shared_ptr constantGravity; std::shared_ptr simulationParameters; std::shared_ptr animationParameters; + std::shared_ptr> times; }; } diff --git a/MbDCode/ASMTConstraintSet.cpp b/MbDCode/ASMTConstraintSet.cpp index 7d115bc..7972675 100644 --- a/MbDCode/ASMTConstraintSet.cpp +++ b/MbDCode/ASMTConstraintSet.cpp @@ -1,3 +1,61 @@ #include "ASMTConstraintSet.h" using namespace MbD; + +void MbD::ASMTConstraintSet::readMarkerI(std::vector& lines) +{ + assert(lines[0].find("MarkerI") != std::string::npos); + lines.erase(lines.begin()); + markerI = readString(lines[0]); + lines.erase(lines.begin()); +} + +void MbD::ASMTConstraintSet::readMarkerJ(std::vector& lines) +{ + assert(lines[0].find("MarkerJ") != std::string::npos); + lines.erase(lines.begin()); + markerJ = readString(lines[0]); + lines.erase(lines.begin()); +} + +void MbD::ASMTConstraintSet::readFXonIs(std::vector& lines) +{ + std::string str = lines[0]; + readDoublesInto(str, "FXonI", fxs); + lines.erase(lines.begin()); +} + +void MbD::ASMTConstraintSet::readFYonIs(std::vector& lines) +{ + std::string str = lines[0]; + readDoublesInto(str, "FYonI", fys); + lines.erase(lines.begin()); +} + +void MbD::ASMTConstraintSet::readFZonIs(std::vector& lines) +{ + std::string str = lines[0]; + readDoublesInto(str, "FZonI", fzs); + lines.erase(lines.begin()); +} + +void MbD::ASMTConstraintSet::readTXonIs(std::vector& lines) +{ + std::string str = lines[0]; + readDoublesInto(str, "TXonI", txs); + lines.erase(lines.begin()); +} + +void MbD::ASMTConstraintSet::readTYonIs(std::vector& lines) +{ + std::string str = lines[0]; + readDoublesInto(str, "TYonI", tys); + lines.erase(lines.begin()); +} + +void MbD::ASMTConstraintSet::readTZonIs(std::vector& lines) +{ + std::string str = lines[0]; + readDoublesInto(str, "TZonI", tzs); + lines.erase(lines.begin()); +} diff --git a/MbDCode/ASMTConstraintSet.h b/MbDCode/ASMTConstraintSet.h index 22b74f8..28db835 100644 --- a/MbDCode/ASMTConstraintSet.h +++ b/MbDCode/ASMTConstraintSet.h @@ -7,7 +7,17 @@ namespace MbD { { // public: + void readMarkerI(std::vector& lines); + void readMarkerJ(std::vector& lines); + void readFXonIs(std::vector& lines); + void readFYonIs(std::vector& lines); + void readFZonIs(std::vector& lines); + void readTXonIs(std::vector& lines); + void readTYonIs(std::vector& lines); + void readTZonIs(std::vector& lines); + std::string markerI, markerJ; + FRowDsptr fxs, fys, fzs, txs, tys, tzs; }; } diff --git a/MbDCode/ASMTItem.cpp b/MbDCode/ASMTItem.cpp index dcb5908..d5b39ce 100644 --- a/MbDCode/ASMTItem.cpp +++ b/MbDCode/ASMTItem.cpp @@ -12,7 +12,7 @@ void MbD::ASMTItem::parseASMT(std::vector& lines) assert(false); } -FRowDsptr MbD::ASMTItem::readRowOfDoubles(std::string line) +FRowDsptr MbD::ASMTItem::readRowOfDoubles(std::string& line) { std::istringstream iss(line); auto readRowOfDoubles = std::make_shared>(); @@ -23,7 +23,7 @@ FRowDsptr MbD::ASMTItem::readRowOfDoubles(std::string line) return readRowOfDoubles; } -FColDsptr MbD::ASMTItem::readColumnOfDoubles(std::string line) +FColDsptr MbD::ASMTItem::readColumnOfDoubles(std::string& line) { std::istringstream iss(line); auto readColumnOfDoubles = std::make_shared>(); @@ -34,7 +34,7 @@ FColDsptr MbD::ASMTItem::readColumnOfDoubles(std::string line) return readColumnOfDoubles; } -double MbD::ASMTItem::readDouble(std::string line) +double MbD::ASMTItem::readDouble(std::string& line) { std::istringstream iss(line); double d; @@ -42,7 +42,7 @@ double MbD::ASMTItem::readDouble(std::string line) return d; } -int MbD::ASMTItem::readInt(std::string line) +int MbD::ASMTItem::readInt(std::string& line) { std::istringstream iss(line); int i; @@ -50,7 +50,7 @@ int MbD::ASMTItem::readInt(std::string line) return i; } -bool MbD::ASMTItem::readBool(std::string line) +bool MbD::ASMTItem::readBool(std::string& line) { if (line.find("true") != std::string::npos) { @@ -64,3 +64,37 @@ bool MbD::ASMTItem::readBool(std::string line) assert(false); } } + +std::string MbD::ASMTItem::readString(std::string& line) +{ + std::string str = line; + str.erase(str.begin(), std::find_if(str.begin(), str.end(), [](unsigned char ch) { return !std::isspace(ch); })); + return str; +} + +void MbD::ASMTItem::readName(std::vector& lines) +{ + assert(lines[0].find("Name") != std::string::npos); + lines.erase(lines.begin()); + name = readString(lines[0]); + lines.erase(lines.begin()); +} + +std::string MbD::ASMTItem::fullName(std::string partialName) +{ + std::string longerName = "/" + name + partialName; + if (owner == nullptr) { + return longerName; + } + else { + return owner->fullName(longerName); + } +} + +void MbD::ASMTItem::readDoublesInto(std::string& str, std::string label, FRowDsptr& row) +{ + auto pos = str.find(label); + assert(pos != std::string::npos); + str.erase(0, pos + label.length()); + row = readRowOfDoubles(str); +} diff --git a/MbDCode/ASMTItem.h b/MbDCode/ASMTItem.h index 2fcbad0..546181d 100644 --- a/MbDCode/ASMTItem.h +++ b/MbDCode/ASMTItem.h @@ -8,12 +8,17 @@ namespace MbD { public: virtual void initialize(); virtual void parseASMT(std::vector& lines); - FRowDsptr readRowOfDoubles(std::string line); - FColDsptr readColumnOfDoubles(std::string line); - double readDouble(std::string line); - int readInt(std::string line); - bool readBool(std::string line); + FRowDsptr readRowOfDoubles(std::string& line); + FColDsptr readColumnOfDoubles(std::string& line); + double readDouble(std::string& line); + int readInt(std::string& line); + bool readBool(std::string& line); + std::string readString(std::string& line); + void readName(std::vector& lines); + std::string fullName(std::string partialName); + void readDoublesInto(std::string& str, std::string label, FRowDsptr& row); + std::string name; ASMTItem* owner; }; } diff --git a/MbDCode/ASMTJoint.cpp b/MbDCode/ASMTJoint.cpp index 316700c..d061f10 100644 --- a/MbDCode/ASMTJoint.cpp +++ b/MbDCode/ASMTJoint.cpp @@ -4,18 +4,25 @@ using namespace MbD; void MbD::ASMTJoint::parseASMT(std::vector& lines) { - size_t pos = lines[0].find_first_not_of("\t"); - auto leadingTabs = lines[0].substr(0, pos); - assert(lines[0] == (leadingTabs + "Name")); - lines.erase(lines.begin()); - name = lines[0]; - lines.erase(lines.begin()); - assert(lines[0] == (leadingTabs + "MarkerI")); - lines.erase(lines.begin()); - markerI = lines[0]; - lines.erase(lines.begin()); - assert(lines[0] == (leadingTabs + "MarkerJ")); - lines.erase(lines.begin()); - markerJ = lines[0]; - lines.erase(lines.begin()); + readName(lines); + readMarkerI(lines); + readMarkerJ(lines); +} + +void MbD::ASMTJoint::readJointSeries(std::vector& lines) +{ + std::string str = lines[0]; + std::string substr = "JointSeries"; + auto pos = str.find(substr); + assert(pos != std::string::npos); + str.erase(0, pos + substr.length()); + auto seriesName = readString(str); + assert(fullName("") == seriesName); + lines.erase(lines.begin()); + readFXonIs(lines); + readFYonIs(lines); + readFZonIs(lines); + readTXonIs(lines); + readTYonIs(lines); + readTZonIs(lines); } diff --git a/MbDCode/ASMTJoint.h b/MbDCode/ASMTJoint.h index efb1784..418c2af 100644 --- a/MbDCode/ASMTJoint.h +++ b/MbDCode/ASMTJoint.h @@ -1,6 +1,7 @@ #pragma once #include "ASMTConstraintSet.h" +#include "ForceTorqueData.h" namespace MbD { class ASMTJoint : public ASMTConstraintSet @@ -8,8 +9,9 @@ namespace MbD { // public: void parseASMT(std::vector& lines) override; + void readJointSeries(std::vector& lines); - std::string name, markerI, markerJ; + std::shared_ptr>> jointSeries; }; } diff --git a/MbDCode/ASMTMarker.cpp b/MbDCode/ASMTMarker.cpp index 372fc59..6e76a6a 100644 --- a/MbDCode/ASMTMarker.cpp +++ b/MbDCode/ASMTMarker.cpp @@ -4,40 +4,7 @@ using namespace MbD; void MbD::ASMTMarker::parseASMT(std::vector& lines) { - size_t pos = lines[0].find_first_not_of("\t"); - auto leadingTabs = lines[0].substr(0, pos); - while (!lines.empty()) { - if (lines[0] == (leadingTabs + "Name")) { - lines.erase(lines.begin()); - name = lines[0]; - lines.erase(lines.begin()); - } - else if (lines[0] == (leadingTabs + "Position3D")) { - lines.erase(lines.begin()); - std::istringstream iss(lines[0]); - position3D = std::make_shared>(); - double d; - while (iss >> d) { - position3D->push_back(d); - } - lines.erase(lines.begin()); - } - else if (lines[0] == (leadingTabs + "RotationMatrix")) { - lines.erase(lines.begin()); - rotationMatrix = std::make_shared>(3, 0); - for (int i = 0; i < 3; i++) - { - auto& row = rotationMatrix->at(i); - std::istringstream iss(lines[0]); - double d; - while (iss >> d) { - row->push_back(d); - } - lines.erase(lines.begin()); - } - } - else { - assert(false); - } - } + readName(lines); + readPosition3D(lines); + readRotationMatrix(lines); } diff --git a/MbDCode/ASMTMarker.h b/MbDCode/ASMTMarker.h index 0969a90..6dcc715 100644 --- a/MbDCode/ASMTMarker.h +++ b/MbDCode/ASMTMarker.h @@ -1,20 +1,16 @@ #pragma once -#include "ASMTItem.h" +#include "ASMTSpatialItem.h" #include "FullColumn.h" #include "FullMatrix.h" namespace MbD { - class ASMTMarker : public ASMTItem + class ASMTMarker : public ASMTSpatialItem { // public: void parseASMT(std::vector& lines) override; - std::string name; - FColDsptr position3D; - FMatDsptr rotationMatrix; - }; } diff --git a/MbDCode/ASMTMotion.cpp b/MbDCode/ASMTMotion.cpp index c60fa7e..491da2f 100644 --- a/MbDCode/ASMTMotion.cpp +++ b/MbDCode/ASMTMotion.cpp @@ -1,3 +1,21 @@ #include "ASMTMotion.h" using namespace MbD; + +void MbD::ASMTMotion::readMotionSeries(std::vector& lines) +{ + std::string str = lines[0]; + std::string substr = "MotionSeries"; + auto pos = str.find(substr); + assert(pos != std::string::npos); + str.erase(0, pos + substr.length()); + auto seriesName = readString(str); + assert(fullName("") == seriesName); + lines.erase(lines.begin()); + readFXonIs(lines); + readFYonIs(lines); + readFZonIs(lines); + readTXonIs(lines); + readTYonIs(lines); + readTZonIs(lines); +} diff --git a/MbDCode/ASMTMotion.h b/MbDCode/ASMTMotion.h index a1c9cd2..70ec845 100644 --- a/MbDCode/ASMTMotion.h +++ b/MbDCode/ASMTMotion.h @@ -1,13 +1,16 @@ #pragma once #include "ASMTConstraintSet.h" +#include "ForceTorqueData.h" namespace MbD { class ASMTMotion : public ASMTConstraintSet { // public: + void readMotionSeries(std::vector& lines); + std::shared_ptr>> motionSeries; }; } diff --git a/MbDCode/ASMTPart.cpp b/MbDCode/ASMTPart.cpp index ee58a52..4e3e276 100644 --- a/MbDCode/ASMTPart.cpp +++ b/MbDCode/ASMTPart.cpp @@ -5,123 +5,78 @@ using namespace MbD; void MbD::ASMTPart::parseASMT(std::vector& lines) { - size_t pos = lines[0].find_first_not_of("\t"); - auto leadingTabs = lines[0].substr(0, pos); - while (!lines.empty()) { - if (lines[0] == (leadingTabs + "Name")) { - lines.erase(lines.begin()); - name = lines[0]; - lines.erase(lines.begin()); - } - else if (lines[0] == (leadingTabs + "Position3D")) { - lines.erase(lines.begin()); - position3D = readColumnOfDoubles(lines[0]); - lines.erase(lines.begin()); - } - else if (lines[0] == (leadingTabs + "RotationMatrix")) { - lines.erase(lines.begin()); - rotationMatrix = std::make_shared>(3); - for (int i = 0; i < 3; i++) - { - auto row = readRowOfDoubles(lines[0]); - rotationMatrix->atiput(i, row); - lines.erase(lines.begin()); - } - } - else if (lines[0] == (leadingTabs + "Velocity3D")) { - lines.erase(lines.begin()); - velocity3D = readColumnOfDoubles(lines[0]); - lines.erase(lines.begin()); - } - else if (lines[0] == (leadingTabs + "Omega3D")) { - lines.erase(lines.begin()); - omega3D = readColumnOfDoubles(lines[0]); - lines.erase(lines.begin()); - } - else if (lines[0] == (leadingTabs + "FeatureOrder")) { - lines.erase(lines.begin()); - //featureOrder = std::make_shared>>(); - auto it = std::find(lines.begin(), lines.end(), (leadingTabs + "PrincipalMassMarker")); - //std::vector featureOrderLines(lines.begin(), it); - //while (!featureOrderLines.empty()) { - // if (featureOrderLines[0] == (leadingTabs + "\tExtrusion")) { - // featureOrderLines.erase(featureOrderLines.begin()); - // auto extrusion = CREATE::With(); - // extrusion->parseASMT(featureOrderLines); - // featureOrder->push_back(extrusion); - // extrusion->owner = this; - // } - // else { - // assert(false); - // } - //} - lines.erase(lines.begin(), it); - } - else if (lines[0] == (leadingTabs + "PrincipalMassMarker")) { - lines.erase(lines.begin()); - principalMassMarker = CREATE::With(); - principalMassMarker->parseASMT(lines); - principalMassMarker->owner = this; - } - else if (lines[0] == (leadingTabs + "RefPoints")) { - lines.erase(lines.begin()); - refPoints = std::make_shared>>(); - auto it = std::find(lines.begin(), lines.end(), (leadingTabs + "RefCurves")); - std::vector refPointsLines(lines.begin(), it); - while (!refPointsLines.empty()) { - if (refPointsLines[0] == (leadingTabs + "\tRefPoint")) { - refPointsLines.erase(refPointsLines.begin()); - auto refPoint = CREATE::With(); - refPoint->parseASMT(refPointsLines); - refPoints->push_back(refPoint); - refPoint->owner = this; - } - else { - assert(false); - } - } - lines.erase(lines.begin(), it); - } - else if (lines[0] == (leadingTabs + "RefCurves")) { - lines.erase(lines.begin()); - refCurves = std::make_shared>>(); - auto it = std::find(lines.begin(), lines.end(), (leadingTabs + "RefSurfaces")); - std::vector refCurvesLines(lines.begin(), it); - while (!refCurvesLines.empty()) { - if (refCurvesLines[0] == (leadingTabs + "\tRefCurve")) { - refCurvesLines.erase(refCurvesLines.begin()); - auto refCurve = CREATE::With(); - refCurve->parseASMT(refCurvesLines); - refCurves->push_back(refCurve); - refCurve->owner = this; - } - else { - assert(false); - } - } - lines.erase(lines.begin(), it); - } - else if (lines[0] == (leadingTabs + "RefSurfaces")) { - lines.erase(lines.begin()); - refSurfaces = std::make_shared>>(); - auto it = std::find(lines.begin(), lines.end(), (leadingTabs.substr(0, leadingTabs.size() - 1) + "Part")); - std::vector refSurfacesLines(lines.begin(), it); - while (!refSurfacesLines.empty()) { - if (refSurfacesLines[0] == (leadingTabs + "\tRefSurface")) { - refSurfacesLines.erase(refSurfacesLines.begin()); - auto refSurface = CREATE::With(); - refSurface->parseASMT(refSurfacesLines); - refSurfaces->push_back(refSurface); - refSurface->owner = this; - } - else { - assert(false); - } - } - lines.erase(lines.begin(), it); - } - else { - return; - } - } + readName(lines); + readPosition3D(lines); + readRotationMatrix(lines); + readVelocity3D(lines); + readOmega3D(lines); + readFeatureOrder(lines); + readPrincipalMassMarker(lines); + readRefPoints(lines); + readRefCurves(lines); + readRefSurfaces(lines); +} + +void MbD::ASMTPart::readFeatureOrder(std::vector& lines) +{ + assert(lines[0].find("FeatureOrder") != std::string::npos); + lines.erase(lines.begin()); + //featureOrder = std::make_shared>>(); + auto it = std::find_if(lines.begin(), lines.end(), [](const std::string& s) { + return s.find("PrincipalMassMarker") != std::string::npos; + }); + //std::vector featureOrderLines(lines.begin(), it); + //while (!featureOrderLines.empty()) { + // if (featureOrderLines[0] == (leadingTabs + "\tExtrusion")) { + // featureOrderLines.erase(featureOrderLines.begin()); + // auto extrusion = CREATE::With(); + // extrusion->parseASMT(featureOrderLines); + // featureOrder->push_back(extrusion); + // extrusion->owner = this; + // } + // else { + // assert(false); + // } + //} + lines.erase(lines.begin(), it); +} + +void MbD::ASMTPart::readPrincipalMassMarker(std::vector& lines) +{ + assert(lines[0].find("PrincipalMassMarker") != std::string::npos); + lines.erase(lines.begin()); + principalMassMarker = CREATE::With(); + principalMassMarker->parseASMT(lines); + principalMassMarker->owner = this; +} + +void MbD::ASMTPart::readPartSeries(std::vector& lines) +{ + std::string str = lines[0]; + std::string substr = "PartSeries"; + auto pos = str.find(substr); + assert(pos != std::string::npos); + str.erase(0, pos + substr.length()); + auto seriesName = readString(str); + assert(fullName("") == seriesName); + lines.erase(lines.begin()); + //xs, ys, zs, bryxs, bryys, bryzs + readXs(lines); + readYs(lines); + readZs(lines); + readBryantxs(lines); + readBryantys(lines); + readBryantzs(lines); + readVXs(lines); + readVYs(lines); + readVZs(lines); + readOmegaXs(lines); + readOmegaYs(lines); + readOmegaZs(lines); + readAXs(lines); + readAYs(lines); + readAZs(lines); + readAlphaXs(lines); + readAlphaYs(lines); + readAlphaZs(lines); } diff --git a/MbDCode/ASMTPart.h b/MbDCode/ASMTPart.h index cab2c96..12940d5 100644 --- a/MbDCode/ASMTPart.h +++ b/MbDCode/ASMTPart.h @@ -1,26 +1,25 @@ #pragma once -#include "ASMTItem.h" +#include "ASMTSpatialContainer.h" #include "ASMTRefPoint.h" #include "ASMTRefCurve.h" #include "ASMTRefSurface.h" #include "ASMTPrincipalMassMarker.h" +#include "PosVelAccData.h" namespace MbD { - class ASMTPart : public ASMTItem + class ASMTPart : public ASMTSpatialContainer { // public: void parseASMT(std::vector& lines) override; + void readFeatureOrder(std::vector& lines); + void readPrincipalMassMarker(std::vector& lines); + void readPartSeries(std::vector& lines); - std::string name; - FColDsptr position3D, velocity3D, omega3D; - FMatDsptr rotationMatrix; std::shared_ptr principalMassMarker; //std::shared_ptr>> featureOrder; - std::shared_ptr>> refPoints; - std::shared_ptr>> refCurves; - std::shared_ptr>> refSurfaces; + std::shared_ptr>> partSeries; }; } diff --git a/MbDCode/ASMTPrincipalMassMarker.h b/MbDCode/ASMTPrincipalMassMarker.h index 7cab1bc..1bc0b42 100644 --- a/MbDCode/ASMTPrincipalMassMarker.h +++ b/MbDCode/ASMTPrincipalMassMarker.h @@ -9,7 +9,6 @@ namespace MbD { public: void parseASMT(std::vector& lines) override; - std::string name; FColDsptr position3D; FMatDsptr rotationMatrix; double mass, density; diff --git a/MbDCode/ASMTRefPoint.cpp b/MbDCode/ASMTRefPoint.cpp index 50d74eb..bd1b5d3 100644 --- a/MbDCode/ASMTRefPoint.cpp +++ b/MbDCode/ASMTRefPoint.cpp @@ -5,54 +5,32 @@ using namespace MbD; void MbD::ASMTRefPoint::parseASMT(std::vector& lines) { - size_t pos = lines[0].find_first_not_of("\t"); - auto leadingTabs = lines[0].substr(0, pos); - while (!lines.empty()) { - if (lines[0] == (leadingTabs + "Position3D")) { - lines.erase(lines.begin()); - std::istringstream iss(lines[0]); - position3D = std::make_shared>(); - double d; - while (iss >> d) { - position3D->push_back(d); - } - lines.erase(lines.begin()); - } - else if (lines[0] == (leadingTabs + "RotationMatrix")) { - lines.erase(lines.begin()); - rotationMatrix = std::make_shared>(3, 0); - for (int i = 0; i < 3; i++) - { - auto& row = rotationMatrix->at(i); - std::istringstream iss(lines[0]); - double d; - while (iss >> d) { - row->push_back(d); - } - lines.erase(lines.begin()); - } - } - else if (lines[0] == (leadingTabs + "Markers")) { - lines.erase(lines.begin()); - markers = std::make_shared>>(); - auto it = std::find(lines.begin(), lines.end(), (leadingTabs.substr(0, leadingTabs.size() - 1) + "RefPoint")); - std::vector markersLines(lines.begin(), it); - while (!markersLines.empty()) { - if (markersLines[0] == (leadingTabs + "\tMarker")) { - markersLines.erase(markersLines.begin()); - auto marker = CREATE::With(); - marker->parseASMT(markersLines); - markers->push_back(marker); - marker->owner = this; - } - else { - return; - } - } - lines.erase(lines.begin(), it); - } - else { - return; - } - } + readPosition3D(lines); + readRotationMatrix(lines); + readMarkers(lines); +} + +void MbD::ASMTRefPoint::readMarkers(std::vector& lines) +{ + assert(lines[0].find("Markers") != std::string::npos); + lines.erase(lines.begin()); + markers = std::make_shared>>(); + auto it = std::find_if(lines.begin(), lines.end(), [](const std::string& s) { + return s.find("RefPoint") != std::string::npos; + }); + std::vector markersLines(lines.begin(), it); + while (!markersLines.empty()) { + readMarker(markersLines); + } + lines.erase(lines.begin(), it); +} + +void MbD::ASMTRefPoint::readMarker(std::vector& lines) +{ + assert(lines[0].find("Marker") != std::string::npos); + lines.erase(lines.begin()); + auto marker = CREATE::With(); + marker->parseASMT(lines); + markers->push_back(marker); + marker->owner = this; } diff --git a/MbDCode/ASMTRefPoint.h b/MbDCode/ASMTRefPoint.h index cdb816f..9de3c9e 100644 --- a/MbDCode/ASMTRefPoint.h +++ b/MbDCode/ASMTRefPoint.h @@ -1,19 +1,19 @@ #pragma once -#include "ASMTItem.h" +#include "ASMTSpatialItem.h" #include #include #include "ASMTMarker.h" namespace MbD { - class ASMTRefPoint : public ASMTItem + class ASMTRefPoint : public ASMTSpatialItem { // public: void parseASMT(std::vector& lines) override; + void readMarkers(std::vector& lines); + void readMarker(std::vector& lines); - FColDsptr position3D; - FMatDsptr rotationMatrix; std::shared_ptr>> markers; }; } diff --git a/MbDCode/ASMTRotationalMotion.cpp b/MbDCode/ASMTRotationalMotion.cpp index dc5aef0..ff4a4b4 100644 --- a/MbDCode/ASMTRotationalMotion.cpp +++ b/MbDCode/ASMTRotationalMotion.cpp @@ -4,18 +4,23 @@ using namespace MbD; void MbD::ASMTRotationalMotion::parseASMT(std::vector& lines) { - size_t pos = lines[0].find_first_not_of("\t"); - auto leadingTabs = lines[0].substr(0, pos); - assert(lines[0] == (leadingTabs + "Name")); + readName(lines); + readMotionJoint(lines); + readRotationZ(lines); +} + +void MbD::ASMTRotationalMotion::readMotionJoint(std::vector& lines) +{ + assert(lines[0].find("MotionJoint") != std::string::npos); lines.erase(lines.begin()); - name = lines[0]; - lines.erase(lines.begin()); - assert(lines[0] == (leadingTabs + "MotionJoint")); - lines.erase(lines.begin()); - motionJoint = lines[0]; - lines.erase(lines.begin()); - assert(lines[0] == (leadingTabs + "RotationZ")); - lines.erase(lines.begin()); - rotationZ = lines[0]; + motionJoint = readString(lines[0]); + lines.erase(lines.begin()); +} + +void MbD::ASMTRotationalMotion::readRotationZ(std::vector& lines) +{ + assert(lines[0].find("RotationZ") != std::string::npos); + lines.erase(lines.begin()); + rotationZ = readString(lines[0]); lines.erase(lines.begin()); } diff --git a/MbDCode/ASMTRotationalMotion.h b/MbDCode/ASMTRotationalMotion.h index 669c9b9..88d4cf8 100644 --- a/MbDCode/ASMTRotationalMotion.h +++ b/MbDCode/ASMTRotationalMotion.h @@ -8,8 +8,10 @@ namespace MbD { // public: void parseASMT(std::vector& lines) override; + void readMotionJoint(std::vector& lines); + void readRotationZ(std::vector& lines); - std::string name, motionJoint, rotationZ; + std::string motionJoint, rotationZ; }; } diff --git a/MbDCode/ASMTSpatialContainer.cpp b/MbDCode/ASMTSpatialContainer.cpp new file mode 100644 index 0000000..b1cd641 --- /dev/null +++ b/MbDCode/ASMTSpatialContainer.cpp @@ -0,0 +1,192 @@ +#include "ASMTSpatialContainer.h" + +void MbD::ASMTSpatialContainer::readRefPoints(std::vector& lines) +{ + assert(lines[0].find("RefPoints") != std::string::npos); + lines.erase(lines.begin()); + refPoints = std::make_shared>>(); + auto it = std::find_if(lines.begin(), lines.end(), [](const std::string& s) { + return s.find("RefCurves") != std::string::npos; + }); + std::vector refPointsLines(lines.begin(), it); + while (!refPointsLines.empty()) { + readRefPoint(refPointsLines); + } + lines.erase(lines.begin(), it); +} + +void MbD::ASMTSpatialContainer::readRefPoint(std::vector& lines) +{ + assert(lines[0].find("RefPoint") != std::string::npos); + lines.erase(lines.begin()); + auto refPoint = CREATE::With(); + refPoint->parseASMT(lines); + refPoints->push_back(refPoint); + refPoint->owner = this; +} + +void MbD::ASMTSpatialContainer::readRefCurves(std::vector& lines) +{ + assert(lines[0].find("RefCurves") != std::string::npos); + lines.erase(lines.begin()); + refCurves = std::make_shared>>(); + auto it = std::find_if(lines.begin(), lines.end(), [](const std::string& s) { + return s.find("RefSurfaces") != std::string::npos; + }); + std::vector refCurvesLines(lines.begin(), it); + while (!refCurvesLines.empty()) { + readRefCurve(refCurvesLines); + } + lines.erase(lines.begin(), it); +} + +void MbD::ASMTSpatialContainer::readRefCurve(std::vector& lines) +{ + assert(false); +} + +void MbD::ASMTSpatialContainer::readRefSurfaces(std::vector& lines) +{ + assert(lines[0].find("RefSurfaces") != std::string::npos); + lines.erase(lines.begin()); + refSurfaces = std::make_shared>>(); + auto it = std::find_if(lines.begin(), lines.end(), [](const std::string& s) { + return s.find("Part") != std::string::npos; + }); + std::vector refSurfacesLines(lines.begin(), it); + while (!refSurfacesLines.empty()) { + readRefSurface(refSurfacesLines); + } + lines.erase(lines.begin(), it); +} + +void MbD::ASMTSpatialContainer::readRefSurface(std::vector& lines) +{ + assert(false); +} + +void MbD::ASMTSpatialContainer::readXs(std::vector& lines) +{ + std::string str = lines[0]; + readDoublesInto(str, "X", xs); + lines.erase(lines.begin()); +} + +void MbD::ASMTSpatialContainer::readYs(std::vector& lines) +{ + std::string str = lines[0]; + readDoublesInto(str, "Y", ys); + lines.erase(lines.begin()); +} + +void MbD::ASMTSpatialContainer::readZs(std::vector& lines) +{ + std::string str = lines[0]; + readDoublesInto(str, "Z", zs); + lines.erase(lines.begin()); +} + +void MbD::ASMTSpatialContainer::readBryantxs(std::vector& lines) +{ + std::string str = lines[0]; + readDoublesInto(str, "Bryantx", bryxs); + lines.erase(lines.begin()); +} + +void MbD::ASMTSpatialContainer::readBryantys(std::vector& lines) +{ + std::string str = lines[0]; + readDoublesInto(str, "Bryanty", bryys); + lines.erase(lines.begin()); +} + +void MbD::ASMTSpatialContainer::readBryantzs(std::vector& lines) +{ + std::string str = lines[0]; + readDoublesInto(str, "Bryantz", bryzs); + lines.erase(lines.begin()); +} + +void MbD::ASMTSpatialContainer::readVXs(std::vector& lines) +{ + std::string str = lines[0]; + readDoublesInto(str, "VX", vxs); + lines.erase(lines.begin()); +} + +void MbD::ASMTSpatialContainer::readVYs(std::vector& lines) +{ + std::string str = lines[0]; + readDoublesInto(str, "VY", vys); + lines.erase(lines.begin()); +} + +void MbD::ASMTSpatialContainer::readVZs(std::vector& lines) +{ + std::string str = lines[0]; + readDoublesInto(str, "VZ", vzs); + lines.erase(lines.begin()); +} + +void MbD::ASMTSpatialContainer::readOmegaXs(std::vector& lines) +{ + std::string str = lines[0]; + readDoublesInto(str, "OmegaX", omexs); + lines.erase(lines.begin()); +} + +void MbD::ASMTSpatialContainer::readOmegaYs(std::vector& lines) +{ + std::string str = lines[0]; + readDoublesInto(str, "OmegaY", omeys); + lines.erase(lines.begin()); +} + +void MbD::ASMTSpatialContainer::readOmegaZs(std::vector& lines) +{ + std::string str = lines[0]; + readDoublesInto(str, "OmegaZ", omezs); + lines.erase(lines.begin()); +} + +void MbD::ASMTSpatialContainer::readAXs(std::vector& lines) +{ + std::string str = lines[0]; + readDoublesInto(str, "AX", axs); + lines.erase(lines.begin()); +} + +void MbD::ASMTSpatialContainer::readAYs(std::vector& lines) +{ + std::string str = lines[0]; + readDoublesInto(str, "AY", ays); + lines.erase(lines.begin()); +} + +void MbD::ASMTSpatialContainer::readAZs(std::vector& lines) +{ + std::string str = lines[0]; + readDoublesInto(str, "AZ", azs); + lines.erase(lines.begin()); +} + +void MbD::ASMTSpatialContainer::readAlphaXs(std::vector& lines) +{ + std::string str = lines[0]; + readDoublesInto(str, "AlphaX", alpxs); + lines.erase(lines.begin()); +} + +void MbD::ASMTSpatialContainer::readAlphaYs(std::vector& lines) +{ + std::string str = lines[0]; + readDoublesInto(str, "AlphaY", alpys); + lines.erase(lines.begin()); +} + +void MbD::ASMTSpatialContainer::readAlphaZs(std::vector& lines) +{ + std::string str = lines[0]; + readDoublesInto(str, "AlphaZ", alpzs); + lines.erase(lines.begin()); +} diff --git a/MbDCode/ASMTSpatialContainer.h b/MbDCode/ASMTSpatialContainer.h new file mode 100644 index 0000000..8af5521 --- /dev/null +++ b/MbDCode/ASMTSpatialContainer.h @@ -0,0 +1,47 @@ +#pragma once + +#include "ASMTSpatialItem.h" +#include "ASMTRefPoint.h" +#include "ASMTRefCurve.h" +#include "ASMTRefSurface.h" + +namespace MbD { + class ASMTSpatialContainer : public ASMTSpatialItem + { + // + public: + 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); + + std::shared_ptr>> refPoints; + std::shared_ptr>> refCurves; + std::shared_ptr>> refSurfaces; + FRowDsptr xs, ys, zs, bryxs, bryys, bryzs; + FRowDsptr vxs, vys, vzs, omexs, omeys, omezs; + FRowDsptr axs, ays, azs, alpxs, alpys, alpzs; + + }; +} + diff --git a/MbDCode/ASMTSpatialItem.cpp b/MbDCode/ASMTSpatialItem.cpp new file mode 100644 index 0000000..7a888a6 --- /dev/null +++ b/MbDCode/ASMTSpatialItem.cpp @@ -0,0 +1,59 @@ +#include "ASMTSpatialItem.h" + +using namespace MbD; + +void MbD::ASMTSpatialItem::readPosition3D(std::vector& lines) +{ + assert(lines[0].find("Position3D") != std::string::npos); + lines.erase(lines.begin()); + std::istringstream iss(lines[0]); + position3D = std::make_shared>(); + double d; + while (iss >> d) { + position3D->push_back(d); + } + lines.erase(lines.begin()); +} + +void MbD::ASMTSpatialItem::readRotationMatrix(std::vector& lines) +{ + assert(lines[0].find("RotationMatrix") != std::string::npos); + lines.erase(lines.begin()); + rotationMatrix = std::make_shared>(3, 0); + for (int i = 0; i < 3; i++) + { + auto& row = rotationMatrix->at(i); + std::istringstream iss(lines[0]); + double d; + while (iss >> d) { + row->push_back(d); + } + lines.erase(lines.begin()); + } +} + +void MbD::ASMTSpatialItem::readVelocity3D(std::vector& lines) +{ + assert(lines[0].find("Velocity3D") != std::string::npos); + lines.erase(lines.begin()); + std::istringstream iss(lines[0]); + velocity3D = std::make_shared>(); + double d; + while (iss >> d) { + velocity3D->push_back(d); + } + lines.erase(lines.begin()); +} + +void MbD::ASMTSpatialItem::readOmega3D(std::vector& lines) +{ + assert(lines[0].find("Omega3D") != std::string::npos); + lines.erase(lines.begin()); + std::istringstream iss(lines[0]); + omega3D = std::make_shared>(); + double d; + while (iss >> d) { + omega3D->push_back(d); + } + lines.erase(lines.begin()); +} diff --git a/MbDCode/ASMTSpatialItem.h b/MbDCode/ASMTSpatialItem.h new file mode 100644 index 0000000..61e8faa --- /dev/null +++ b/MbDCode/ASMTSpatialItem.h @@ -0,0 +1,20 @@ +#pragma once + +#include "ASMTItem.h" + +namespace MbD { + class ASMTSpatialItem : public ASMTItem + { + // + public: + void readPosition3D(std::vector& lines); + void readRotationMatrix(std::vector& lines); + void readVelocity3D(std::vector& lines); + void readOmega3D(std::vector& lines); + + FColDsptr position3D, velocity3D, omega3D; + FMatDsptr rotationMatrix; + + }; +} + diff --git a/MbDCode/ASMTTranslationalMotion.h b/MbDCode/ASMTTranslationalMotion.h index 38c58c9..359fc82 100644 --- a/MbDCode/ASMTTranslationalMotion.h +++ b/MbDCode/ASMTTranslationalMotion.h @@ -9,7 +9,7 @@ namespace MbD { public: void parseASMT(std::vector& lines) override; - std::string name, motionJoint, translationZ; + std::string motionJoint, translationZ; }; } diff --git a/MbDCode/CADSystem.h b/MbDCode/CADSystem.h index 519d9c4..65ba431 100644 --- a/MbDCode/CADSystem.h +++ b/MbDCode/CADSystem.h @@ -15,7 +15,7 @@ namespace MbD { // public: CADSystem() { - mbdSystem->externalSystem = this; + mbdSystem->externalSystem->cadSystem = this; } void outputFor(AnalysisType type); diff --git a/MbDCode/ExternalSystem.cpp b/MbDCode/ExternalSystem.cpp new file mode 100644 index 0000000..52310e9 --- /dev/null +++ b/MbDCode/ExternalSystem.cpp @@ -0,0 +1,31 @@ +#include "ExternalSystem.h" + +using namespace MbD; + +void MbD::ExternalSystem::preMbDrun() +{ +} + +void MbD::ExternalSystem::outputFor(AnalysisType type) +{ +} + +void MbD::ExternalSystem::logString(std::string& str) +{ +} + +void MbD::ExternalSystem::logString(double value) +{ +} + +void MbD::ExternalSystem::runOndselPiston() +{ +} + +void MbD::ExternalSystem::runPiston() +{ +} + +void MbD::ExternalSystem::postMbDrun() +{ +} diff --git a/MbDCode/ExternalSystem.h b/MbDCode/ExternalSystem.h new file mode 100644 index 0000000..c04b449 --- /dev/null +++ b/MbDCode/ExternalSystem.h @@ -0,0 +1,32 @@ +#pragma once + +#include +#include "enum.h" +#include + +//#include "CADSystem.h" +//#include "ASMTAssembly.h" + +namespace MbD { + class CADSystem; + class ASMTAssembly; + + class ExternalSystem + { + // + public: + void preMbDrun(); + void outputFor(AnalysisType type); + void logString(std::string& str); + void logString(double value); + void runOndselPiston(); + void runPiston(); + void postMbDrun(); + + + CADSystem* cadSystem; + ASMTAssembly* asmtAssembly; + + }; +} + diff --git a/MbDCode/MbDCode.cpp b/MbDCode/MbDCode.cpp index c85995d..8c4b15f 100644 --- a/MbDCode/MbDCode.cpp +++ b/MbDCode/MbDCode.cpp @@ -23,8 +23,8 @@ int main() auto externalSys = std::make_shared(); externalSys->runOndselPiston(); - externalSys->runPiston(); - runSpMat(); + //externalSys->runPiston(); + //runSpMat(); } void runSpMat() { diff --git a/MbDCode/MbDCode.vcxproj b/MbDCode/MbDCode.vcxproj index a2ea094..246bb09 100644 --- a/MbDCode/MbDCode.vcxproj +++ b/MbDCode/MbDCode.vcxproj @@ -159,6 +159,8 @@ + + @@ -181,7 +183,8 @@ - + + @@ -390,6 +393,8 @@ + + @@ -412,7 +417,8 @@ - + + diff --git a/MbDCode/MbDCode.vcxproj.filters b/MbDCode/MbDCode.vcxproj.filters index cb64dd4..eb3bfb4 100644 --- a/MbDCode/MbDCode.vcxproj.filters +++ b/MbDCode/MbDCode.vcxproj.filters @@ -429,7 +429,7 @@ Source Files - + Source Files @@ -702,6 +702,15 @@ Source Files + + Source Files + + + Source Files + + + Source Files + @@ -1127,7 +1136,7 @@ Header Files - + Header Files @@ -1400,6 +1409,15 @@ Header Files + + Header Files + + + Header Files + + + Header Files + diff --git a/MbDCode/Part.cpp b/MbDCode/Part.cpp index 5b018ee..6531741 100644 --- a/MbDCode/Part.cpp +++ b/MbDCode/Part.cpp @@ -4,7 +4,7 @@ #include "CREATE.h" #include "DiagonalMatrix.h" #include "EulerParameters.h" -#include "DataPosVelAcc.h" +#include "PosVelAccData.h" using namespace MbD; @@ -538,7 +538,7 @@ std::shared_ptr Part::stateData() auto omeOpO = this->omeOpO(); auto aOpO = this->qXddot(); auto alpOpO = this->alpOpO(); - auto answer = std::make_shared(); + auto answer = std::make_shared(); answer->rFfF = rOpO; answer->aAFf = aAOp; answer->vFfF = vOpO; diff --git a/MbDCode/DataPosVelAcc.cpp b/MbDCode/PosVelAccData.cpp similarity index 80% rename from MbDCode/DataPosVelAcc.cpp rename to MbDCode/PosVelAccData.cpp index 7fda007..506ba1f 100644 --- a/MbDCode/DataPosVelAcc.cpp +++ b/MbDCode/PosVelAccData.cpp @@ -1,10 +1,10 @@ #include -#include "DataPosVelAcc.h" +#include "PosVelAccData.h" using namespace MbD; -std::ostream& DataPosVelAcc::printOn(std::ostream& s) const +std::ostream& PosVelAccData::printOn(std::ostream& s) const { s << "refData = " << *refData << std::endl; s << "rFfF = " << *rFfF << std::endl; diff --git a/MbDCode/DataPosVelAcc.h b/MbDCode/PosVelAccData.h similarity index 75% rename from MbDCode/DataPosVelAcc.h rename to MbDCode/PosVelAccData.h index b60dab0..7f7534f 100644 --- a/MbDCode/DataPosVelAcc.h +++ b/MbDCode/PosVelAccData.h @@ -3,13 +3,13 @@ #include "StateData.h" namespace MbD { - class DataPosVelAcc : public StateData + class PosVelAccData : public StateData { //refData rFfF aAFf vFfF omeFfF aFfF alpFfF public: std::ostream& printOn(std::ostream& s) const override; - std::shared_ptr refData; + std::shared_ptr refData; FColDsptr rFfF, vFfF, omeFfF, aFfF, alpFfF; FMatDsptr aAFf; }; diff --git a/MbDCode/System.cpp b/MbDCode/System.cpp index b94fb1e..c4fb277 100644 --- a/MbDCode/System.cpp +++ b/MbDCode/System.cpp @@ -7,7 +7,7 @@ #include "SystemSolver.h" #include "Time.h" #include "CREATE.h" -#include "CADSystem.h" +#include "ExternalSystem.h" using namespace MbD; @@ -26,6 +26,7 @@ System* MbD::System::root() void System::initialize() { + externalSystem = std::make_shared(); time = CREATE @@ -1418,6 +1460,48 @@ Header Files + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + @@ -1426,5 +1510,6 @@ + \ No newline at end of file diff --git a/MbDCode/Numeric.cpp b/MbDCode/Numeric.cpp index 15403df..29a60d9 100644 --- a/MbDCode/Numeric.cpp +++ b/MbDCode/Numeric.cpp @@ -42,3 +42,8 @@ double MbD::Numeric::arcTan0to2piYoverX(double y, double x) } } } + +bool MbD::Numeric::equaltol(double x, double xx, double tol) +{ + return std::abs(x - xx) < tol; +} diff --git a/MbDCode/Numeric.h b/MbDCode/Numeric.h index b384477..6583bfe 100644 --- a/MbDCode/Numeric.h +++ b/MbDCode/Numeric.h @@ -8,6 +8,7 @@ namespace MbD { // public: static double arcTan0to2piYoverX(double y, double x); + static bool equaltol(double x, double xx, double tol); }; diff --git a/MbDCode/Part.cpp b/MbDCode/Part.cpp index 6531741..b7ae4c8 100644 --- a/MbDCode/Part.cpp +++ b/MbDCode/Part.cpp @@ -559,3 +559,8 @@ void Part::postDynStep() { partFrame->postDynStep(); } + +void MbD::Part::submitToSystem(std::shared_ptr self) +{ + root()->parts->push_back(std::static_pointer_cast(self)); +} diff --git a/MbDCode/Part.cpp.bak b/MbDCode/Part.cpp.bak deleted file mode 100644 index 21342a0..0000000 --- a/MbDCode/Part.cpp.bak +++ /dev/null @@ -1,38 +0,0 @@ -#include "Part.h" -#include "PartFrame.h" -#include "FullColumn.h" - -using namespace MbD; - -Part::Part() { - partFrame = std::make_shared(this); -} - -void Part::setqX(FullColDptr x) { - partFrame.get()->setqX(x); -} - -FullColDptr Part::getqX() { - return partFrame.get()->getqX(); -} - -void Part::setqE(FullColDptr x) { - partFrame.get()->setqE(x); -} - -FullColDptr Part::getqE() { - return partFrame.get()->getqE(); -} - -void Part::setSystem(System& sys) -{ - //May be needed in the future -} - -void Part::initializeLocally() -{ -} - -void Part::initializeGlobally() -{ -} diff --git a/MbDCode/Part.h b/MbDCode/Part.h index 68ff87e..1f40a61 100644 --- a/MbDCode/Part.h +++ b/MbDCode/Part.h @@ -102,6 +102,7 @@ namespace MbD { std::shared_ptr stateData() override; double suggestSmallerOrAcceptDynStepSize(double hnew) override; void postDynStep() override; + void submitToSystem(std::shared_ptr self) override; System* system; //Use raw pointer when pointing backwards. int ipX = -1; diff --git a/MbDCode/Part.h.bak b/MbDCode/Part.h.bak deleted file mode 100644 index a8d66f3..0000000 --- a/MbDCode/Part.h.bak +++ /dev/null @@ -1,29 +0,0 @@ -#pragma once -#include - -#include "Item.h" -#include "System.h" -#include "PartFrame.h" -#include "FullColumn.h" - -namespace MbD { - class System; - class PartFrame; - - class Part : public Item - { - //ToDo: ipX ipE m aJ partFrame pX pXdot pE pEdot mX mE mEdot pTpE ppTpEpE ppTpEpEdot - public: - Part(); - void setqX(FullColDptr x); - FullColDptr getqX(); - void setqE(FullColDptr x); - FullColDptr getqE(); - void setSystem(System& sys); - void initializeLocally(); - void initializeGlobally(); - - std::shared_ptr partFrame; - }; -} - diff --git a/MbDCode/PartFrame.cpp.bak b/MbDCode/PartFrame.cpp.bak deleted file mode 100644 index 31419b1..0000000 --- a/MbDCode/PartFrame.cpp.bak +++ /dev/null @@ -1,52 +0,0 @@ -#include "Part.h" -#include "PartFrame.h" -#include "EulerConstraint.h" -#include "AbsConstraint.h" -#include "MarkerFrame.h" - -using namespace MbD; - -PartFrame::PartFrame() -{ - aGeu = std::make_shared("EulerCon"); - aGeu->setOwner(this); - aGabs = std::make_unique>>(); - markerFrames = std::make_unique>>(); -} -MbD::PartFrame::PartFrame(const char* str) : CartesianFrame(str) -{ -} -void MbD::PartFrame::initialize() -{ -} -void PartFrame::setqX(FullColDptr x) { - qX->copy(x); -} -FullColDptr PartFrame::getqX() { - return qX; -} -void PartFrame::setqE(FullColDptr x) { - qE->copy(x); -} -FullColDptr PartFrame::getqE() { - return qE; -} -void PartFrame::setPart(Part* x) { - part = x; -} -Part* PartFrame::getPart() { - return part; -} - -void PartFrame::addMarkerFrame(std::shared_ptr markerFrame) -{ - markerFrames->push_back(markerFrame); -} - -std::shared_ptr PartFrame::endFrame(std::string name) -{ - auto match = std::find_if(markerFrames->begin(), markerFrames->end(), [&](std::shared_ptr mkr) {return mkr->endFrames->at(0)->getName() == name; }); - //auto match = std::find_if(markerFrames->begin(), markerFrames->end(), [&](std::shared_ptr mkr) {return mkr->getName() == name; }); - - return (*match)->endFrames->at(0); -} diff --git a/MbDCode/Power.cpp b/MbDCode/Power.cpp new file mode 100644 index 0000000..91bca5f --- /dev/null +++ b/MbDCode/Power.cpp @@ -0,0 +1,38 @@ +#include "Power.h" +#include "Constant.h" +#include "Ln.h" + +using namespace MbD; + +MbD::Power::Power() +{ +} + +MbD::Power::Power(Symsptr bse, Symsptr ex) : FunctionXY(bse, ex) +{ +} + +Symsptr MbD::Power::differentiateWRTx() +{ + auto yminus1 = Symbolic::sum(y, std::make_shared(-1.0)); + auto power = Symbolic::raisedTo(x, yminus1); + auto deriv = Symbolic::times(y, power); + return deriv->simplified(deriv); +} + +Symsptr MbD::Power::differentiateWRTy() +{ + auto lnterm = std::make_shared(x); + auto deriv = Symbolic::times(clonesptr(), lnterm); + return deriv->simplified(); +} + +Symsptr MbD::Power::simplifyUntil(Symsptr sptr, std::shared_ptr> set) +{ + return Symsptr(); +} + +double MbD::Power::getValue() +{ + return std::pow(x->getValue(), y->getValue()); +} diff --git a/MbDCode/Power.h b/MbDCode/Power.h new file mode 100644 index 0000000..1685812 --- /dev/null +++ b/MbDCode/Power.h @@ -0,0 +1,20 @@ +#pragma once + +#include "FunctionXY.h" + +namespace MbD { + class Power : public FunctionXY + { + // + public: + Power(); + Power(Symsptr base, Symsptr exp); + Symsptr differentiateWRTx(); + Symsptr differentiateWRTy(); + + Symsptr simplifyUntil(Symsptr sptr, std::shared_ptr> set) override; + double getValue() override; + + }; +} + diff --git a/MbDCode/PrescribedMotion.cpp b/MbDCode/PrescribedMotion.cpp index 682ebc1..4540e3b 100644 --- a/MbDCode/PrescribedMotion.cpp +++ b/MbDCode/PrescribedMotion.cpp @@ -36,5 +36,5 @@ void MbD::PrescribedMotion::initMotions() void PrescribedMotion::connectsItoJ(EndFrmcptr frmi, EndFrmcptr frmj) { Joint::connectsItoJ(frmi, frmj); - frmI->initEndFrameqct(); + std::static_pointer_cast(frmI)->initEndFrameqct(); } diff --git a/MbDCode/PrescribedMotion.cpp.bak b/MbDCode/PrescribedMotion.cpp.bak deleted file mode 100644 index 77bdb3f..0000000 --- a/MbDCode/PrescribedMotion.cpp.bak +++ /dev/null @@ -1,19 +0,0 @@ -#include -#include -#include - -#include "PrescribedMotion.h" -#include "EndFrameqct.h" - -using namespace MbD; - -void PrescribedMotion::connectsItoJ(std::shared_ptr frmi, std::shared_ptr frmj) -{ - Joint::connectsItoJ(frmi, frmj); - if (typeid(frmI).name() != "EndFrameqct") { - std::shared_ptr newFrmI; - newFrmI = std::make_shared(); - std::swap(frmI, newFrmI); - assert(typeid(frmI).name() != "EndFrameqct"); - } -} diff --git a/MbDCode/PrescribedMotion.h b/MbDCode/PrescribedMotion.h index eefe604..9541e42 100644 --- a/MbDCode/PrescribedMotion.h +++ b/MbDCode/PrescribedMotion.h @@ -1,9 +1,11 @@ #pragma once +#include #include "Joint.h" -#include "Symbolic.h" +//#include "Symbolic.h" namespace MbD { + class Symbolic; class PrescribedMotion : public Joint { @@ -16,12 +18,19 @@ namespace MbD { void initialize() override; virtual void initMotions(); - Symsptr xBlk; - Symsptr yBlk; - Symsptr zBlk; - Symsptr phiBlk; - Symsptr theBlk; - Symsptr psiBlk; + //Why the following fails? + //Symsptr xBlk; + //Symsptr yBlk; + //Symsptr zBlk; + //Symsptr phiBlk; + //Symsptr theBlk; + //Symsptr psiBlk; + std::shared_ptr xBlk; + std::shared_ptr yBlk; + std::shared_ptr zBlk; + std::shared_ptr phiBlk; + std::shared_ptr theBlk; + std::shared_ptr psiBlk; }; } diff --git a/MbDCode/PrescribedMotion.h.bak b/MbDCode/PrescribedMotion.h.bak deleted file mode 100644 index 8ce12b4..0000000 --- a/MbDCode/PrescribedMotion.h.bak +++ /dev/null @@ -1,11 +0,0 @@ -#pragma once -#include "Joint.h" - -namespace MbD { - class PrescribedMotion : public Joint - { - public: - void connectsItoJ(std::shared_ptr frmI, std::shared_ptr frmJ) override; - }; -} - diff --git a/MbDCode/Product.cpp b/MbDCode/Product.cpp index 3de8546..dc713e8 100644 --- a/MbDCode/Product.cpp +++ b/MbDCode/Product.cpp @@ -7,7 +7,7 @@ using namespace MbD; -Symsptr Product::differentiateWRT(Symsptr sptr, Symsptr var) +Symsptr MbD::Product::differentiateWRT(Symsptr var) { //"Apply chain rule of differentiation." // "(xyz)' := x'yz + xy'z + xyz'." @@ -16,7 +16,7 @@ Symsptr Product::differentiateWRT(Symsptr sptr, Symsptr var) std::transform(terms->begin(), terms->end(), std::back_inserter(*derivatives), - [var](Symsptr term) { return term->differentiateWRT(term, var); } + [var](Symsptr term) { return term->differentiateWRT(var); } ); auto derivativeTerms = std::make_shared>(); for (int i = 0; i < terms->size(); i++) @@ -59,9 +59,9 @@ Symsptr Product::expandUntil(Symsptr sptr, std::shared_ptrterms = productTerms; auto sumOfProductsOfSums = std::make_shared(std::make_shared(1)); for (const auto& term : *sumTerms) { - sumOfProductsOfSums = std::static_pointer_cast(sumOfProductsOfSums->timesSum(sumOfProductsOfSums, term)); + sumOfProductsOfSums = std::static_pointer_cast(Symbolic::times(sumOfProductsOfSums, term)); } - return factor->timesSum(factor, sumOfProductsOfSums); + return Symbolic::times(factor, sumOfProductsOfSums); } Symsptr Product::simplifyUntil(Symsptr sptr, std::shared_ptr> set) @@ -103,39 +103,6 @@ Symsptr Product::simplifyUntil(Symsptr sptr, std::shared_ptr(); - auto sumTERMs = aSum->getTerms(); - for (const auto& sumTERM : *sumTERMs) { - Symsptr termTERM; - if (sumTERM->isProduct()) { - termTERM = sptr->timesProduct(sptr, sumTERM); - } - else { - termTERM = sptr->timesFunction(sptr, sumTERM); - } - answer->terms->push_back(termTERM); - } - return answer; -} - -Symsptr Product::timesProduct(Symsptr sptr, Symsptr product) -{ - auto answer = std::make_shared(sptr->getTerms()); - auto& answerTerms = answer->terms; - auto productTerms = product->getTerms(); - answerTerms->insert(answerTerms->end(), productTerms->begin(), productTerms->end()); - return answer; -} - -Symsptr Product::timesFunction(Symsptr sptr, Symsptr function) -{ - auto answer = std::make_shared(sptr->getTerms()); - answer->terms->push_back(function); - return answer; -} - std::ostream& Product::printOn(std::ostream& s) const { s << "("; @@ -159,3 +126,8 @@ double Product::getValue() for (int i = 0; i < terms->size(); i++) answer *= terms->at(i)->getValue(); return answer; } + +Symsptr MbD::Product::clonesptr() +{ + return std::make_shared(*this); +} diff --git a/MbDCode/Product.h b/MbDCode/Product.h index 12a3806..c12ee76 100644 --- a/MbDCode/Product.h +++ b/MbDCode/Product.h @@ -2,6 +2,8 @@ #include "FunctionWithManyArgs.h" #include "Symbolic.h" +#include "System.h" +#include "Units.h" namespace MbD { @@ -13,15 +15,15 @@ namespace MbD { Product(Symsptr term, Symsptr term1) : FunctionWithManyArgs(term, term1) {} Product(Symsptr term, Symsptr term1, Symsptr term2) : FunctionWithManyArgs(term, term1, term2) {} Product(std::shared_ptr> _terms) : FunctionWithManyArgs(_terms) {} - Symsptr differentiateWRT(Symsptr sptr, Symsptr var) override; + Symsptr differentiateWRT(Symsptr var) override; Symsptr expandUntil(Symsptr sptr, std::shared_ptr> set) override; Symsptr simplifyUntil(Symsptr sptr, std::shared_ptr> set) override; - Symsptr timesSum(Symsptr sptr, Symsptr sum) override; - Symsptr timesProduct(Symsptr sptr, Symsptr product) override; - Symsptr timesFunction(Symsptr sptr, Symsptr function) override; - std::ostream& printOn(std::ostream& s) const override; bool isProduct() override; double getValue() override; + Symsptr clonesptr() override; + + std::ostream& printOn(std::ostream& s) const override; + }; } diff --git a/MbDCode/RevoluteJoint.cpp.bak b/MbDCode/RevoluteJoint.cpp.bak deleted file mode 100644 index 1e728ca..0000000 --- a/MbDCode/RevoluteJoint.cpp.bak +++ /dev/null @@ -1 +0,0 @@ -#include "RevoluteJoint.h" diff --git a/MbDCode/RevoluteJoint.h.bak b/MbDCode/RevoluteJoint.h.bak deleted file mode 100644 index e6b94ac..0000000 --- a/MbDCode/RevoluteJoint.h.bak +++ /dev/null @@ -1,8 +0,0 @@ -#pragma once -#include "Joint.h" -namespace MbD { - class RevoluteJoint : public Joint - { - }; -} - diff --git a/MbDCode/RowTypeMatrix.h b/MbDCode/RowTypeMatrix.h index a9192eb..3476d4b 100644 --- a/MbDCode/RowTypeMatrix.h +++ b/MbDCode/RowTypeMatrix.h @@ -14,13 +14,15 @@ namespace MbD { RowTypeMatrix(std::initializer_list list) : Array{ list } {} void copyFrom(std::shared_ptr> x); virtual void zeroSelf() = 0; + //double maxMagnitude() override; + int numberOfElements() override; + int nrow() { return (int) this->size(); } int ncol() { return this->at(0)->numberOfElements(); } - int numberOfElements() override; }; template @@ -30,6 +32,17 @@ namespace MbD { this->at(i)->copyFrom(x->at(i)); } } + //template + //inline double RowTypeMatrix::maxMagnitude() + //{ + // auto max = 0.0; + // for (int i = 0; i < this->size(); i++) + // { + // auto element = this->at(i)->maxMagnitude(); + // if (max < element) max = element; + // } + // return max; + //} template inline int RowTypeMatrix::numberOfElements() { diff --git a/MbDCode/Sin.cpp b/MbDCode/Sin.cpp new file mode 100644 index 0000000..45b02ef --- /dev/null +++ b/MbDCode/Sin.cpp @@ -0,0 +1,6 @@ +#include "Sin.h" + +double MbD::Sin::getValue() +{ + return std::sin(xx->getValue()); +} diff --git a/MbDCode/Sin.h b/MbDCode/Sin.h new file mode 100644 index 0000000..21314c1 --- /dev/null +++ b/MbDCode/Sin.h @@ -0,0 +1,15 @@ +#pragma once + +#include "FunctionX.h" + +namespace MbD { + class Sin : public FunctionX + { + // + public: + double getValue() override; + + + }; +} + diff --git a/MbDCode/SparseMatrix.h b/MbDCode/SparseMatrix.h index 7402750..e80097d 100644 --- a/MbDCode/SparseMatrix.h +++ b/MbDCode/SparseMatrix.h @@ -7,6 +7,11 @@ #include "DiagonalMatrix.h" namespace MbD { + //template + //class SparseRow; + //template + //class DiagonalMatrix; + template class SparseMatrix : public RowTypeMatrix>> { @@ -42,9 +47,10 @@ namespace MbD { void atijplusNumber(int i, int j, double value); void atijminusNumber(int i, int j, double value); void atijput(int i, int j, T value); + double maxMagnitude() override; + std::shared_ptr> timesFullColumn(std::shared_ptr> fullCol); std::ostream& printOn(std::ostream& s) const override; - std::shared_ptr> timesFullColumn(std::shared_ptr> fullCol); }; using SpMatDsptr = std::shared_ptr>; @@ -152,6 +158,17 @@ namespace MbD { this->at(i)->atiput(j, value); } template + inline double SparseMatrix::maxMagnitude() + { + auto max = 0.0; + for (int i = 0; i < this->size(); i++) + { + auto element = this->at(i)->maxMagnitude(); + if (max < element) max = element; + } + return max; + } + template inline std::ostream& SparseMatrix::printOn(std::ostream& s) const { s << "SpMat[" << std::endl; diff --git a/MbDCode/Sum.cpp b/MbDCode/Sum.cpp index 7933ab3..8779167 100644 --- a/MbDCode/Sum.cpp +++ b/MbDCode/Sum.cpp @@ -1,35 +1,46 @@ +#include +#include + #include "Sum.h" #include "Constant.h" using namespace MbD; -Symsptr Sum::timesSum(Symsptr sptr, Symsptr aSum) +Symsptr MbD::Sum::parseExpression(std::string& expression) { - auto answer = std::make_shared(); - auto sumTERMs = aSum->getTerms(); - for (const auto& term : *terms) { - for (const auto& sumTERM : *sumTERMs) { - Symsptr termTERM; - if (sumTERM->isProduct()) { - termTERM = term->timesProduct(term, sumTERM); - } - else { - termTERM = term->timesFunction(term, sumTERM); - } - answer->terms->push_back(termTERM); - } + std::istringstream iss(expression); + auto sum = std::make_shared(); + sum->parse(iss); + return sum->simplified(sum); +} + +void MbD::Sum::parse(std::istringstream& iss) +{ + iss >> std::ws; + char c = iss.peek(); + if (c == '+') { + parsePlusTerm(iss); + } + else if (c == '-') { + parseMinusTerm(iss); + } + else { + parseTerm(iss); } - return answer; } -Symsptr Sum::timesProduct(Symsptr sptr, Symsptr product) +void MbD::Sum::parseTerm(std::istringstream& iss) { - return product->timesSum(product, sptr); } -Symsptr Sum::timesFunction(Symsptr sptr, Symsptr function) +void MbD::Sum::parsePlusTerm(std::istringstream& iss) +{ + iss.get(); + +} + +void MbD::Sum::parseMinusTerm(std::istringstream& iss) { - return function->timesSum(function, sptr); } Symsptr Sum::expandUntil(Symsptr sptr, std::shared_ptr> set) @@ -112,6 +123,11 @@ double Sum::getValue() return answer; } +Symsptr MbD::Sum::clonesptr() +{ + return std::make_shared(*this); +} + std::ostream& Sum::printOn(std::ostream& s) const { s << "("; diff --git a/MbDCode/Sum.h b/MbDCode/Sum.h index 8c0e4a7..a45e724 100644 --- a/MbDCode/Sum.h +++ b/MbDCode/Sum.h @@ -6,19 +6,25 @@ namespace MbD { class Sum : public FunctionWithManyArgs { public: + static Symsptr parseExpression(std::string& expression); + void parse(std::istringstream& iss); + void parseTerm(std::istringstream& iss); + void parsePlusTerm(std::istringstream& iss); + void parseMinusTerm(std::istringstream& iss); + Sum() : FunctionWithManyArgs() {} Sum(Symsptr term) : FunctionWithManyArgs(term) {} Sum(Symsptr term, Symsptr term1) : FunctionWithManyArgs(term, term1) {} Sum(Symsptr term, Symsptr term1, Symsptr term2) : FunctionWithManyArgs(term, term1, term2) {} Sum(std::shared_ptr> _terms) : FunctionWithManyArgs(_terms) {} - Symsptr timesSum(Symsptr sptr, Symsptr sum) override; - Symsptr timesProduct(Symsptr sptr, Symsptr product) override; - Symsptr timesFunction(Symsptr sptr, Symsptr function) override; Symsptr expandUntil(Symsptr sptr, std::shared_ptr> set) override; Symsptr simplifyUntil(Symsptr sptr, std::shared_ptr> set) override; bool isSum() override; double getValue() override; + Symsptr clonesptr() override; + std::ostream& printOn(std::ostream& s) const override; + }; } diff --git a/MbDCode/Symbolic.cpp b/MbDCode/Symbolic.cpp index f862d85..c46e7db 100644 --- a/MbDCode/Symbolic.cpp +++ b/MbDCode/Symbolic.cpp @@ -6,6 +6,7 @@ #include "Constant.h" #include "Product.h" #include "Sum.h" +#include "Power.h" using namespace MbD; @@ -13,16 +14,82 @@ Symbolic::Symbolic() { } +Symsptr MbD::Symbolic::times(Symsptr arg, Symsptr arg1) +{ + if (arg->isProduct()) { + if (arg1->isProduct()) { + auto newTerms = arg->getTerms(); + auto arg1Terms = arg1->getTerms(); + newTerms->insert(newTerms->end(), arg1Terms->begin(), arg1Terms->end()); + return std::make_shared(newTerms); + } + else { + auto newTerms = arg->getTerms(); + newTerms->insert(newTerms->end(), arg1); + return std::make_shared(newTerms); + } + } + else { + if (arg1->isProduct()) { + auto newTerms = arg1->getTerms(); + newTerms->insert(newTerms->begin(), arg); + return std::make_shared(newTerms); + } + else { + return std::make_shared(arg, arg1); + } + } +} + +Symsptr MbD::Symbolic::sum(Symsptr arg, Symsptr arg1) +{ + if (arg->isSum()) { + if (arg1->isSum()) { + auto newTerms = arg->getTerms(); + auto arg1Terms = arg1->getTerms(); + newTerms->insert(newTerms->end(), arg1Terms->begin(), arg1Terms->end()); + return std::make_shared(newTerms); + } + else { + auto newTerms = arg->getTerms(); + newTerms->insert(newTerms->end(), arg1); + return std::make_shared(newTerms); + } + } + else { + if (arg1->isSum()) { + auto newTerms = arg1->getTerms(); + newTerms->insert(newTerms->begin(), arg); + return std::make_shared(newTerms); + } + else { + return std::make_shared(arg, arg1); + } + } +} + void Symbolic::initialize() { } -Symsptr Symbolic::differentiateWRT(Symsptr sptr, Symsptr var) +Symsptr MbD::Symbolic::differentiateWRT(Symsptr var) { assert(false); return Symsptr(); } +Symsptr MbD::Symbolic::simplified() +{ + //std::cout << "sptr " << *sptr << std::endl; + auto set = std::make_shared>(); + auto expanded = this->expandUntil(set); + //std::cout << "expanded " << *expanded << std::endl; + auto set1 = std::make_shared>(); + auto simplified = expanded->simplifyUntil(expanded, set1); + //std::cout << "simplified " << *simplified << std::endl; + return simplified; +} + Symsptr Symbolic::simplified(Symsptr sptr) { //std::cout << "sptr " << *sptr << std::endl; @@ -35,6 +102,12 @@ Symsptr Symbolic::simplified(Symsptr sptr) return simplified; } +Symsptr MbD::Symbolic::expandUntil(std::shared_ptr> set) +{ + assert(false); + return clonesptr(); +} + Symsptr Symbolic::expandUntil(Symsptr sptr, std::shared_ptr> set) { return sptr; @@ -45,24 +118,14 @@ Symsptr Symbolic::simplifyUntil(Symsptr sptr, std::shared_ptr(sptr); - return product->timesSum(product, sum); + return false; } -Symsptr Symbolic::timesProduct(Symsptr sptr, Symsptr product) +bool MbD::Symbolic::isOne() { - auto answer = std::make_shared(product->getTerms()); - auto& answerTerms = answer->terms; - answerTerms->insert(answerTerms->begin(), sptr); - return answer; -} - -Symsptr Symbolic::timesFunction(Symsptr sptr, Symsptr function) -{ - auto answer = std::make_shared(sptr, function); - return answer; + return false; } bool Symbolic::isSum() @@ -87,11 +150,40 @@ std::ostream& Symbolic::printOn(std::ostream& s) const std::shared_ptr> Symbolic::getTerms() { + assert(false); return std::make_shared>(); } +void MbD::Symbolic::addTerm(Symsptr trm) +{ + getTerms()->push_back(trm); +} + double Symbolic::getValue() { assert(false); return 0.0; } + +void MbD::Symbolic::createMbD(std::shared_ptr mbdSys, std::shared_ptr mbdUnits) +{ + assert(false); + return; +} + +Symsptr MbD::Symbolic::clonesptr() +{ + //Return shallow copy of *this wrapped in shared_ptr + assert(false); + return std::make_shared(*this); +} + +void MbD::Symbolic::arguments(Symsptr args) +{ + assert(false); +} + +Symsptr MbD::Symbolic::raisedTo(Symsptr x, Symsptr y) +{ + return std::make_shared(x, y); +} diff --git a/MbDCode/Symbolic.h b/MbDCode/Symbolic.h index f2370ea..c71c3ce 100644 --- a/MbDCode/Symbolic.h +++ b/MbDCode/Symbolic.h @@ -6,6 +6,8 @@ #include #include "Math.h" +#include "System.h" +#include "Units.h" namespace MbD { @@ -13,22 +15,29 @@ namespace MbD { { public: Symbolic(); + static std::shared_ptr times(std::shared_ptr arg, std::shared_ptr arg1); + static std::shared_ptr sum(std::shared_ptr arg, std::shared_ptr arg1); + static std::shared_ptr raisedTo(std::shared_ptr x, std::shared_ptr y); + virtual void initialize(); - virtual std::shared_ptr differentiateWRT(std::shared_ptr sptr, std::shared_ptr var); + virtual std::shared_ptr differentiateWRT(std::shared_ptr var); + virtual std::shared_ptr simplified(); virtual std::shared_ptr simplified(std::shared_ptr sptr); + virtual std::shared_ptr expandUntil(std::shared_ptr>> set); virtual std::shared_ptr expandUntil(std::shared_ptr sptr, std::shared_ptr>> set); virtual std::shared_ptr simplifyUntil(std::shared_ptr sptr, std::shared_ptr>> set); - virtual std::shared_ptr timesSum(std::shared_ptr sptr, std::shared_ptr sum); - virtual std::shared_ptr timesProduct(std::shared_ptr sptr, std::shared_ptr product); - virtual std::shared_ptr timesFunction(std::shared_ptr sptr, std::shared_ptr function); + virtual bool isZero(); + virtual bool isOne(); virtual bool isSum(); virtual bool isProduct(); virtual bool isConstant(); - virtual std::ostream& printOn(std::ostream& s) const; - virtual std::shared_ptr>> getTerms(); + virtual void addTerm(std::shared_ptr trm); virtual double getValue(); - + virtual void createMbD(std::shared_ptr mbdSys, std::shared_ptr mbdUnits); + virtual std::shared_ptr clonesptr(); + virtual void arguments(std::shared_ptr args); + virtual std::ostream& printOn(std::ostream& s) const; friend std::ostream& operator<<(std::ostream& s, const Symbolic& sym) { return sym.printOn(s); diff --git a/MbDCode/SymbolicParser.cpp b/MbDCode/SymbolicParser.cpp new file mode 100644 index 0000000..9a4349b --- /dev/null +++ b/MbDCode/SymbolicParser.cpp @@ -0,0 +1,431 @@ +#include +#include +#include + +#include "SymbolicParser.h" +#include "BasicUserFunction.h" +#include "Constant.h" +#include "Sum.h" +#include "Product.h" +#include "CREATE.h" +#include "Power.h" +#include "Abs.h" +#include "ArcTan.h" + +void MbD::SymbolicParser::initialize() +{ + variables = std::make_shared>(); + stack = std::make_shared>(); + buffer = std::make_shared(); + +} + +void MbD::SymbolicParser::parseUserFunction(Symsptr userFunc) +{ + auto usrFunc = std::static_pointer_cast(userFunc); + units = usrFunc->units; + this->parseString(usrFunc->funcText); + Symsptr func = stack->top(); + stack->pop(); + stack->push(Symbolic::times(func, std::make_shared(usrFunc->myUnit))); +} + +void MbD::SymbolicParser::parseString(std::string expr) +{ + buffer->clear(); + while (!stack->empty()) { + stack->pop(); + } + source = std::make_shared(expr); + hereChar = source->get(); + prevEnd = -1; + scanToken(); + expression(); + if (tokenType != "end") expected("Nothing more"); + if (stack->size() != 1) notify("Stack size error, compiler bug!"); +} + +bool MbD::SymbolicParser::plusTerm() +{ + if (peekForTypeNoPush("+")) { + if (plainTerm()) return true; + expected("plainTerm"); + } + return false; +} + +bool MbD::SymbolicParser::minusTerm() +{ + if (peekForTypeNoPush("-")) { + if (plainTerm()) return true; + expected("plainTerm"); + } + return false; +} + +bool MbD::SymbolicParser::plainTerm() +{ + if (term()) { + auto trm = stack->top(); + stack->pop(); + auto sum = stack->top(); + sum->addTerm(trm); + return true; + } + return false; +} + +bool MbD::SymbolicParser::term() +{ + auto product = std::make_shared(); + stack->push(product); + if (plainFunction()) { + while (timesFunction() || divideByFunction()) {} + auto term = stack->top(); + if (term->isProduct()) { + if (term->isOne()) { + stack->pop(); + } + else if (term->getTerms()->size() == 1) { + stack->pop(); + stack->push(term->getTerms()->front()); + } + } + else { + notify("SymbolicParser error"); + } + return true; + } + return false; +} + +bool MbD::SymbolicParser::plainFunction() +{ + if (symfunction()) { + auto trm = stack->top(); + stack->pop(); + auto product = stack->top(); + product->addTerm(trm); + return true; + } + return false; +} + +bool MbD::SymbolicParser::timesFunction() +{ + if (peekForTypeNoPush("*")) { + if (plainFunction()) return true; + expected("plainFunction"); + } + return false; +} + +bool MbD::SymbolicParser::divideByFunction() +{ + if (peekForTypeNoPush("/")) { + if (plainFunction()) return true; + expected("plainFunction"); + } + return false; +} + +bool MbD::SymbolicParser::peekForTypeNoPush(std::string c) +{ + //"Test to see if tokenType matches aType. If so, advance to the next token, leaving the stack unchanged" + + if (tokenType == c) { + scanToken(); + return true; + } + return false; +} + +std::string MbD::SymbolicParser::scanToken() +{ + prevEnd = (int)source->tellg(); + prevEnd--; + while (std::isspace(hereChar)) { + hereChar = source->get(); + } + if (hereChar == EOF) { + mark = prevEnd + 1; + tokenType = "end"; + return token = ""; + } + mark = (int)source->tellg(); + if (std::isalpha(hereChar)) { + xLetter(); + } + else if (std::isdigit(hereChar)) { + xDigit(); + } + else if (hereChar == '\"') { + xDoubleQuote(); + } + else { + token = std::string(1, hereChar); + tokenType = token; + hereChar = source->get(); + } + return token; +} + +void MbD::SymbolicParser::xLetter() +{ + buffer->str(""); + buffer->clear(); + *buffer << hereChar; + while (true) { + hereChar = source->get(); + if (hereChar == EOF) break; + if (!std::isalnum(hereChar)) break; + *buffer << hereChar; + } + tokenType = "word"; + token = buffer->str(); +} + +void MbD::SymbolicParser::xDigit() +{ + tokenType = "number"; + if (hereChar != EOF) { + auto pos = source->tellg(); + std::streamoff offset = -1; + pos += offset; + source->seekg(pos); + } + double mantissa = 0.0; + int exponent = 0; + *source >> mantissa; + hereChar = source->peek(); + if ((hereChar == 'd') || (hereChar == 'e')) { + hereChar = source->get(); + char peekChar = source->peek(); + if (std::isdigit(peekChar) || (peekChar == '+') || (peekChar == '-')) { + *source >> exponent; + } + } + token = ""; + tokenNum = mantissa * std::pow(10.0, exponent); + hereChar = source->get(); +} + +void MbD::SymbolicParser::xDoubleQuote() +{ + tokenType = "comment"; + if (hereChar != EOF) { + auto pos = source->tellg(); + std::streamoff offset = -1; + pos += offset; + source->seekg(pos); + } + *source >> std::quoted(token); + hereChar = source->get(); +} + +bool MbD::SymbolicParser::symfunction() +{ + if (expressionInParentheses() || constant() || namedFunction() || variable()) { + raisedTo(); + return true; + } + else { + notify("Unrecognized symbol ->"); + } + return false; +} + +bool MbD::SymbolicParser::expression() +{ + auto sum = std::make_shared(); + stack->push(sum); + if (plusTerm() || minusTerm() || plainTerm()) { + while (plusTerm() || minusTerm()) {} + auto term = stack->top(); + if (term->isSum()) { + auto sum1 = std::static_pointer_cast(term); + if (sum1->isZero()) { + stack->pop(); + } + else if (sum1->terms->size() == 1) { + stack->pop(); + stack->push(sum1->terms->front()); + } + } + else { + notify("Compiler error!"); + } + } + return false; +} + +bool MbD::SymbolicParser::expressionInParentheses() +{ + if (peekForTypeNoPush("(")) { + if (expression()) { + if (peekForTypeNoPush(")")) { + return true; + } + else { + expected(")"); + } + } + else { + expected("expression"); + } + } + return false; +} + +bool MbD::SymbolicParser::constant() +{ + if (signedNumber()) { + return true; + } + if (peekForTypevalue("word", "pi")) { + auto symconst = std::make_shared(M_PI); + stack->push(symconst); + return true; + } + return false; +} + +bool MbD::SymbolicParser::namedFunction() +{ + return intrinsic(); +} + +bool MbD::SymbolicParser::intrinsic() +{ + Symsptr symfunc = nullptr; + if (peekForTypevalue("word", "abs")) { + symfunc = std::make_shared(); + } + else if (peekForTypevalue("word", "arctan")) { + symfunc = std::make_shared(); + } + if (symfunc != nullptr) { + stack->push(symfunc); + if (peekForTypeNoPush("(")) { + auto startsize = stack->size(); + while (expression() && peekForTypeNoPush(",")) {} + if (stack->size() > startsize) { + combineStackTo((int)startsize); + if (peekForTypeNoPush(")")) { + auto args = stack->top(); + stack->pop(); + auto func = stack->top(); + func->arguments(args); + stack->pop(); + return true; + } + expected(")"); + } + expected("expression"); + } + expected("("); + } + return false; +} + +bool MbD::SymbolicParser::variable() +{ + if ((tokenType == "word") && (variables->count(token) == 1)) { + auto& var = variables->at(token); + stack->push(var); + scanToken(); + return true; + } + return false; +} + +bool MbD::SymbolicParser::raisedTo() +{ + if (peekForTypeNoPush("^")) { + if (symfunction()) { + auto exp = stack->top(); + stack->pop(); + auto base = stack->top(); + stack->pop(); + auto pow = std::make_shared(base, exp); + stack->push(pow); + return true; + } + expected("function"); + } + return false; +} + +bool MbD::SymbolicParser::expected(std::string msg) +{ + return false; +} + +bool MbD::SymbolicParser::signedNumber() +{ + if (tokenType == "number") { + auto symNum = std::make_shared(tokenNum); + stack->push(symNum); + scanToken(); + return true; + } + if ((token == "+") && (hereChar != EOF) && (std::isdigit(hereChar) || (hereChar == '.'))) { + //"no intervening delimiters" + scanToken(); + auto symNum = std::make_shared(tokenNum); + stack->push(symNum); + scanToken(); + return true; + } + if ((token == "-") && (hereChar != EOF) && (std::isdigit(hereChar) || (hereChar == '.'))) { + //"no intervening delimiters" + scanToken(); + auto symNum = std::make_shared(-tokenNum); + stack->push(symNum); + scanToken(); + return true; + } + return false; +} + +bool MbD::SymbolicParser::peekForTypevalue(std::string type, std::string symbol) +{ + if ((tokenType == type) && (token == symbol)) { + scanToken(); + return true; + } + return false; +} + +void MbD::SymbolicParser::notify(std::string msg) +{ + notifyat(msg, mark); +} + +void MbD::SymbolicParser::notifyat(std::string msg, int mrk) +{ + //"Temporarily reset source in order to get full contents" + auto p = source->tellg(); + source->seekg(0); + auto contents = source->str(); + source->seekg(p); + assert(false); + //SyntaxErrorException new + //targetClass : class; + //messageText: aString; + //source: contents; + //position: position; + //raiseSignal +} + +void MbD::SymbolicParser::combineStackTo(int pos) +{ + auto args = std::make_shared(); + while (stack->size() > pos) { + auto arg = stack->top(); + stack->pop(); + args->addTerm(arg); + } + stack->push(args); +} + diff --git a/MbDCode/SymbolicParser.h b/MbDCode/SymbolicParser.h new file mode 100644 index 0000000..a03370b --- /dev/null +++ b/MbDCode/SymbolicParser.h @@ -0,0 +1,59 @@ +#pragma once + +#include +#include +#include + +#include "Symbolic.h" +#include "ASMTItem.h" +#include "ASMTItemIJ.h" + +namespace MbD { + class SymbolicParser + { + // + public: + void initialize(); + void parseUserFunction(Symsptr userFunc); + void parseString(std::string expr); + bool plusTerm(); + bool minusTerm(); + bool plainTerm(); + bool term(); + bool plainFunction(); + bool timesFunction(); + bool divideByFunction(); + bool peekForTypeNoPush(std::string c); + std::string scanToken(); + void xLetter(); + void xDigit(); + void xDoubleQuote(); + bool symfunction(); + bool expression(); + bool expressionInParentheses(); + bool constant(); + bool namedFunction(); + bool intrinsic(); + bool variable(); + bool raisedTo(); + bool expected(std::string msg); + bool signedNumber(); + bool peekForTypevalue(std::string type, std::string symbol); + void notify(std::string msg); + void notifyat(std::string msg, int mrk); + void combineStackTo(int pos); + + ASMTItem* owner = nullptr; + std::shared_ptr> variables; + std::shared_ptr> geoIJs; + std::shared_ptr units; + int mark = -1, prevEnd = -1; + char hereChar = '\0'; + std::string token, tokenType; + double tokenNum = -1.0e100; + std::shared_ptr source; + std::shared_ptr buffer; + std::shared_ptr> stack; + }; +} + diff --git a/MbDCode/SyntaxError.cpp b/MbDCode/SyntaxError.cpp new file mode 100644 index 0000000..8f1d043 --- /dev/null +++ b/MbDCode/SyntaxError.cpp @@ -0,0 +1,7 @@ +#include "SyntaxError.h" + +using namespace MbD; + +SyntaxError::SyntaxError(const std::string& msg) : std::runtime_error(msg) +{ +} diff --git a/MbDCode/SyntaxError.h b/MbDCode/SyntaxError.h new file mode 100644 index 0000000..c4ed6ed --- /dev/null +++ b/MbDCode/SyntaxError.h @@ -0,0 +1,16 @@ +#pragma once + +#include +#include +#include + +namespace MbD { + class SyntaxError : virtual public std::runtime_error + { + + public: + //SyntaxError(); + explicit SyntaxError(const std::string& msg); + virtual ~SyntaxError() noexcept {} + }; +} diff --git a/MbDCode/System.cpp b/MbDCode/System.cpp index c4fb277..9d154f8 100644 --- a/MbDCode/System.cpp +++ b/MbDCode/System.cpp @@ -12,11 +12,9 @@ using namespace MbD; System::System() { - initialize(); } System::System(const char* str) : Item(str) { - initialize(); } System* MbD::System::root() @@ -52,31 +50,29 @@ void MbD::System::addMotion(std::shared_ptr motion) jointsMotions->push_back(motion); } -void System::runKINEMATIC() +void MbD::System::addForceTorque(std::shared_ptr forTor) { - externalSystem->preMbDrun(); + forTor->owner = this; + forcesTorques->push_back(forTor); +} + +void System::runKINEMATIC(std::shared_ptr self) +{ + externalSystem->preMbDrun(self); while (true) { initializeLocally(); initializeGlobally(); if (!hasChanged) break; } - partsJointsMotionsForcesTorquesDo([&](std::shared_ptr item) { item->postInput(); }); - outputInput(); + partsJointsMotionsForcesTorquesDo([](std::shared_ptr item) { item->postInput(); }); + externalSystem->outputFor(INPUT); systemSolver->runAllIC(); externalSystem->outputFor(INITIALCONDITION); systemSolver->runBasicKinematic(); externalSystem->postMbDrun(); } -void System::outputInput() -{ -} - -void System::outputTimeSeries() -{ -} - void System::initializeLocally() { hasChanged = false; diff --git a/MbDCode/System.cpp.bak b/MbDCode/System.cpp.bak deleted file mode 100644 index b7aae47..0000000 --- a/MbDCode/System.cpp.bak +++ /dev/null @@ -1,63 +0,0 @@ -#include - -#include "System.h" - -using namespace MbD; - -System::System() { - time = std::make_unique